Positioning part
Hardware composition
The core component of the device is a system composed of a single-chip microcomputer, including an ultrasonic transmitter, an ultrasonic receiver, a receiving central control module, and a transmitting central control module. The system and computer together form a positioning system with a computer as the core. The system structure diagram is shown as in Fig. 1.
How the system works
The conceived working method of the system is to use a computer to control the positioning process and implement it in sequence: first, the computer sends a start measurement command to the transmitter and waits for a delay; another step is to send ultrasound to the transmitter and notify the receiver to start timing; after the computer delay ends , Read the distance data from the receiving end, calculate the coordinates according to the arrangement of the receiving points, and publish the coordinates and display as needed.
The system is mainly implemented by two core programs. One is the microcontroller control program, and the other is the coordinate calculation program. After the single-chip microcomputer obtains the distance between each ultrasonic receiver and the ultrasonic transmitter, the information can be transmitted to the computer, and the relative position coordinates of the object can be calculated through the mathematical model and algorithm introduced above.
Temperature changes will cause changes in the speed of sound. Due to the small space size of the experimental device, it is necessary to consider the impact of temperature on measurement accuracy. There is a sound speed correction program in the device. The real-time temperature of the current environment is obtained through the temperature sensor on the single-chip microcomputer, and the sound speed at the current temperature is calculated through the sound speed correction formula to achieve the purpose of correcting the sound speed. Finally, the real-time display of the coordinates (visualization program), the use of computer language to display the obtained position coordinates in real time, and construct a visual display mode, so that the coordinate position of the object has a high readability.
System interface composition
This interface consists of four parts. The first is the real-time display interface of XY relative coordinates at the upper right, the real-time display interface of XZ relative coordinates at the lower right, and the real-time display interface of YZ relative coordinates at the lower left. The coordinate value display area on the side.
The experimental device of the system consists of the spatial ultrasonic positioning system device and the experimental support, as shown in Figure 3. Four ultrasonic receivers are respectively installed on the four corners of the top of the bracket. The ultrasonic transmitter (positioned object) is installed on a sliding rod that can move freely on the plane. The shelf can also move in the vertical direction, so that it can be Free movement of positioning objects in space.
The space rectangular coordinate system is established with a corner at the bottom as the origin, and a scale is pasted. When measuring, move the plane slide bar and the vertical moving device to place the ultrasonic transmitter (positioned object) at a certain position in the space, and the X, Y, and Z axis coordinates displayed on the scale will be positioned The actual value of the object. The X, Y, and Z axis coordinate values given by the program are the measured values of the object to be positioned. By comparing them with the actual values, the positioning accuracy of the spatial ultrasonic positioning system device can be known.
Navigation section
This part is realized by ultrasonic positioning, which belongs to the category of "absolute positioning", even if a large error occurs, it will not affect the positioning accuracy of the next point. However, ultrasonic positioning also has its limitations: the main reason is that the positioning speed is slightly slower, and the data refresh rate per second is 3-10 times, and 10 times is close to the limit. The more reliable rate should be 3-5 times per second. If you go too fast, large errors will occur. To improve the accuracy, it may be necessary to use dead-reckoning method to make up the gap between two positioning data. Ultrasonic positioning information is just used as the relative position correction of dead-reckoning.
Current and target location information distribution design
Based on the implementation of the Processing test program, it is relatively simple to send the current position and target position on the positioning test page. The current position information distribution is relatively simple. After each positioning is completed, the coordinates are sent to the car. You only need to design a data frame to send the positioning coordinate information Pack with collection time. The target position is generated by clicking the mouse on the positioning area. In order to realize the distribution of "multi-target position information", a button is designed: clear, the purpose is to clear the previously entered target position and start from the beginning. Normal operation, without clicking the mouse once, generate a target position, design an independent frame, and send the target position after sending the current position information.
The target position frame contains two information: first: whether it is the first point. Second: the X and Y coordinates of the target position. In this way, you can know what to do when receiving. If it is the first point, clear the previously stored target position and point to the first element of the target position array. If it is a subsequent target, add the target position at the end of the array.
Corresponding behavior design
With the above control terminal design, the behavior design is as follows:
First: add relevant command analysis to the wireless receiving task to get the position of the car and the target position. Second: Add a task to complete the journey to the target location.
The way of traveling to the target position is initially designed to consist of two behaviors: Behavior 1: Turning, pointing the walking direction to the target position. Behavior 2: Walk straight, toward the goal. When the distance between the target position and the position of the trolley is less than 50mm, it is regarded as reaching the target.
The control logic is: After receiving the first target position, immediately calculate the distance to me, and start when it is greater than 50mm. If I do not get the current direction, the first action is to walk in a straight line, otherwise, turn according to the difference between the target direction and my direction. Each time a new position information is received, it is judged whether the distance from the position saved in the previous calculation of the direction is more than 50mm, and the direction is calculated once to determine whether it is necessary to turn, and the steering is executed according to the result or continues straight ahead. The location information and target information are transmitted through the mailbox, and the walking task is constantly inquiring whether there is new coordinate information.