WO2018192284A1

WO2018192284A1 - Sonar-based robot tracking method and system

Info

Publication number: WO2018192284A1
Application number: PCT/CN2018/073825
Authority: WO
Inventors: 王振华
Original assignee: 南京阿凡达机器人科技有限公司
Priority date: 2017-04-22
Filing date: 2018-01-23
Publication date: 2018-10-25
Also published as: CN107422725B; CN107422725A

Abstract

A sonar-based robot tracking method and system, comprising: providing a sonar transceiver on a tracking target as a first routing node; and mounting two sonar transceivers on a robot, with one serving as a coordinator and the other one serving as a second routing node. A sonar transceiver is used to realize tracking target recognition and location, and obstacle ranging, thereby realizing multiple uses of a single sensor, so that not only is the product function realized, but the manufacturing cost is also reduced. Furthermore, information about an obstacle in the external environment can be quickly detected. The structure is simple, positioning and tracking are accurate, and the application value is high.

Description

Sonar-based robot tracking method and system

The present application claims the priority of the Japanese Patent Application Serial No. No. No. No. No. No. No. No. No

Technical field

The invention belongs to the field of tracking, positioning, navigation and obstacle avoidance of mobile robots, in particular to a sonar-based robot tracking method and system.

Background technique

In order for the robot to serve a specific target object and help people complete the task, such a robot should be required to have the functions of target recognition and autonomous navigation. Currently, robots with target recognition and autonomous navigation generally recognize targets through a "machine vision system" and assist in performing autonomous navigation functions with other sensors.

"Machine Vision" simulates a person's visual function, acquires target information, and processes the target information. According to different visual sensor data, it is divided into "monocular vision" and "binocular vision". The expression of the target of the "monocular vision system" is mainly reflected in the brightness and darkness of the two-dimensional image, texture, surface contour, shadow, etc., which loses the depth information of the imaged point during the imaging process, which is a serious disadvantage of this type of system. . In addition, the "single-eye vision" has a limited range of perception, and reducing the focal length to increase the wide angle often requires an increase in image distortion cost, which is another disadvantage of such systems. "Binocular vision" conforms to the structure of biological vision and has advantages in acquiring three-dimensional information, but the structure of binocular imaging is complex and the algorithm is immature. Moreover, "machine vision" can be realized by extracting target image data through 3D modeling and a large number of complex algorithms. The requirements for the hardware platform are high and the cost is high.

At present, the implementation of "machine vision" often uses a PC to perform a large number of algorithm calculations, and the embedded "machine vision" application is still in the research stage. Even if there is an application, the effect is not ideal, and it is easy to lose the target when tracking.

Summary of the invention

The invention aims to propose a sonar-based robot tracking method and system, which can realize the positioning, tracking and obstacle avoidance functions of the mobile robot based on sonar, realize multiple functions with low cost and simple structure, and avoid loss. Depth information and image distortion of the imaging point, and the positioning tracking is accurate.

In order to achieve the above object, the present invention adopts the following technical solutions:

The present disclosure provides a sonar-based robot tracking method in which a sonar transceiver as a coordinator is installed on a robot, and a sonar transceiver provided as a first routing node on the tracking target includes the following steps:

1: in the process of tracking the tracking target, when the coordinator receives the target recognition positioning instruction issued by the motion controller of the robot, the coordinator acquires the target recognition and positioning according to the sound wave emitted by the first routing node. Data, and transmitting the target identification positioning data to the motion controller;

2: when the coordinator receives the ranging instruction issued by the motion controller, the coordinator acquires ranging data, and sends the ranging data to the motion controller;

3: The motion controller identifies the positioning data and the ranging data according to the target, obtains a tracking state, and calculates a following path according to the tracking state;

4: The motion controller sends a corresponding motion instruction sequence to the servo controller of the robot according to the following path, and the servo controller drives the driving device on the robot to implement a following action.

Further, another sonar transceiver is installed on the robot as the second routing node; the step 1 includes the following steps:

11: when the coordinator receives the target identification positioning instruction, the coordinator broadcasts a first frame command sequence;

12: The first routing node receives the first frame command sequence, generates a second frame command sequence including a delay time, and broadcasts and transmits the second frame command sequence, and then transmits according to the delay time delay. Sound wave

After receiving the second frame command sequence, the coordinator obtains a first time of receiving the sound wave emitted by the first routing node, and calculates the first distance data according to the first time;

After receiving the first frame command sequence and the second frame command sequence, the second routing node acquires a second time of receiving the sound wave emitted by the first routing node, and calculates according to the second time. Obtaining second distance data, and sending the second distance data to the coordinator;

15: The coordinator transmits the first distance data and the second distance data as the target identification positioning data to a motion controller.

Further, the step 14 includes the following steps: 141: after receiving the first frame command sequence and the second frame command sequence, the second routing node starts timing according to the delay time delay; 142: When receiving the sound wave emitted by the first routing node, the second routing node stops timing to obtain a second time; 143: the second routing node is configured according to the second time and a preset distance formula. Calculating the second distance data; 144: the second routing node sends the second distance data to the coordinator.

Further, the step 2: when the coordinator receives the ranging instruction issued by the motion controller, the coordinator acquires the ranging data, and sends the ranging data to the motion controller, including the following step:

21: When the coordinator receives the ranging instruction sent by the motion controller, the coordinator sends a fifth frame command sequence to the second routing node;

22: When the second routing node receives the fifth frame command sequence, the second routing node transmits an acoustic wave, and obtains a third time of receiving an echo signal of the acoustic wave sent by the second routing node. ;

23: The second routing node calculates third distance data according to the third time and a preset ranging formula, and sends the third distance data to the coordinator;

24: After the coordinator receives the third distance data, the coordinator transmits an acoustic wave, and obtains a fourth time of receiving an echo signal of the acoustic wave sent by the coordinator;

25: The coordinator calculates the fourth distance data according to the fourth time and the preset ranging formula;

26: The coordinator transmits the third distance data and the fourth distance data as the ranging data to a motion controller.

Further, the step 3: the motion controller identifies the positioning data according to the target and the ranging data, obtains a tracking state, and calculates a following path according to the tracking state, including the following steps: 31: the motion The controller identifies the positioning data and the ranging data according to the target, and determines whether there is an obstacle between the tracking targets; 32: if there is no obstacle, the following path is obtained without obstacle avoidance; 33: if present Obstacle, the follow-up path of obstacle avoidance is obtained according to the preset obstacle avoidance method.

Further, the step 33: if there is an obstacle, obtaining the following path of the obstacle avoidance according to the preset obstacle avoidance method includes the following steps: 331: when the distance to the obstacle is less than the preset distance, the stop is adopted Follow path; 332: When the obstacle is not less than the preset distance, the following path of the slow steering is obtained according to the ranging data.

The present disclosure also provides a sonar-based robot tracking system, including: tracking a target and a robot;

The tracking target includes: a sonar transceiver as a first routing node;

The robot includes: a sonar transceiver as a coordinator, a motion controller, a servo controller, and a driving device;

The coordinator, in the process of tracking the tracking target, when receiving the target recognition positioning instruction issued by the motion controller, the coordinator acquires target recognition according to the sound wave emitted by the first routing node Positioning data and transmitting the target identification positioning data to the motion controller;

And, when receiving the ranging instruction issued by the motion controller, the coordinator acquires ranging data, and sends the ranging data to the motion controller;

The motion controller is configured to identify the positioning data according to the target and the ranging data, obtain a tracking status, and calculate a following path according to the tracking status; and send a corresponding action instruction sequence according to the following path Giving the servo controller;

The servo controller is configured to drive the driving device to implement a following action according to the received sequence of the motion instruction.

Further, the robot further includes: another sonar transceiver as a second routing node; the coordinator is configured to receive the target issued by the motion controller during tracking of the tracking target When the positioning command is identified, the coordinator acquires the target identification positioning data according to the sound wave transmitted by the first routing node, and sends the target identification positioning data to the motion controller, including:

The coordinator, when the target recognition positioning instruction is received, the coordinator broadcasts a first frame command sequence;

The first routing node is configured to receive the first frame command sequence, generate a second frame command sequence including a delay time, and broadcast and transmit the second frame command sequence, according to the delay time delay Acoustic waves are emitted afterwards;

The coordinator is configured to: after receiving the second frame command sequence, acquire a first time of receiving the sound wave emitted by the first routing node, and calculate the first distance data according to the first time;

The second routing node, after receiving the first frame command sequence and the second frame command sequence, acquiring a second time of receiving the sound wave emitted by the first routing node, and according to the Calculating, by the second time, the second distance data, and sending the second distance data to the coordinator;

The coordinator transmits the first distance data and the second distance data as the target identification positioning data to a motion controller.

Further, the second routing node is configured to: after receiving the first frame command sequence and the second frame command sequence, acquire a second time of receiving the sound wave emitted by the first routing node, and according to The second time data is calculated by the second time, and the sending the second distance data to the coordinator includes: the second routing node receiving the first frame command sequence and the second frame command After the sequence, according to the delay time delay, start timing; and, when receiving the sound wave emitted by the first routing node, the second routing node stops timing, obtaining a second time; Calculating the second distance data by using the second time and the preset distance formula; and transmitting the second distance data to the coordinator.

Further, the coordinator is configured to: when receiving the ranging instruction issued by the motion controller, the coordinator acquires ranging data, and sends the ranging data to the motion controller, including:

The coordinator, when receiving the ranging instruction sent by the motion controller, the coordinator sends a fifth frame command sequence to the second routing node;

The second routing node is configured to: when receiving the sequence of the fifth frame command, the second routing node transmits an acoustic wave, and obtains a third of an echo signal of the acoustic wave sent by the second routing node. And calculating, according to the third time and the preset ranging formula, the third distance data, and sending the third distance data to the coordinator;

The coordinator, configured to: after receiving the third distance data, the coordinator emits an acoustic wave, and obtains a fourth time of receiving an echo signal of the acoustic wave sent by the coordinator; and, according to the And calculating a fourth distance data by using the fourth time and the preset ranging formula; and transmitting the third distance data and the fourth distance data as the ranging data to the motion controller.

Further, the motion controller is configured to obtain a tracking state according to the target identification positioning data and the ranging data, and calculate a following path according to the tracking state, where the motion controller is configured to The target identifies the positioning data and the ranging data, and determines whether there is an obstacle between the tracking targets; if there is no obstacle, the following path is obtained without obstacle avoidance; if there is an obstacle, according to the preset The obstacle avoidance method obtains the following path of obstacle avoidance.

Further, if there is an obstacle, the motion controller obtains an obstacle avoidance follow path according to the preset obstacle avoidance method, and the motion controller is configured to: when the distance to the obstacle is less than a preset distance And adopting a following path for stopping the advance; and, when the obstacle is not less than the preset distance, obtaining a following path of the slow steering according to the ranging data.

The sonar-based robot tracking method and system of the present disclosure converts the recognition of the tracking target into the recognition of a specific signal, and the identification of the specific signal is much easier and much simpler. This greatly improves the recognition rate and the accuracy of the positioning, and the sonar transceiver takes into account the ranging function. The sonar transceiver can not only identify and locate the target, but also measure the distance of the obstacle, and realize multiple uses of a single sensor, which not only realizes the product function but also reduces the manufacturing cost. At the same time, it can quickly detect the obstacle information in the external environment, and the detection has no blind zone. The parameters measured by various sensors are complementary and practical, and can provide the most realistic external environmental parameters for the robot control center in real time, so that the robot control center can be real-time. Optimized travel routes and timely and fast obstacle avoidance.

DRAWINGS

1 is a schematic diagram of a sonar-based robot tracking system according to the present invention;

2 is a schematic diagram of a sonar positioning target of the present invention;

3 is a flow chart of robot obstacle avoidance control according to the present invention;

4 is a flow chart of the robot following target control according to the present invention.

Figure 5 is a schematic diagram of ultrasonic ranging proposed by the present invention

detailed description

A sonar-based robot tracking method of the present invention will be described in detail below with reference to the accompanying drawings. It should be noted that the “first”, “second” and the like in this paper are only for distinguishing related terms, and do not indicate the level of importance.

As shown in FIG. 1, a sonar-based robot tracking method according to the present invention employs an (automatic follow-to-go) robot by a motion controller, at least two sonar transceivers, a servo controller, and a driving device (for example, a machine). The transmission module is composed of, of course, some modules such as auxiliary sensors commonly used by robots. Targeting, path planning and real-time obstacle avoidance are performed by the motion controller based on the data detected by the sonar transceiver.

In this embodiment, the method of the present disclosure uses a plurality of sonar sensors to locate the ranging. In order to locate the exact position of the target, the error is minimized, and the distribution distance of the two sonar transceivers on the robot is as large as possible. The straight line distance is greater than 15 cm (because the dead zone of the sonar receiver is typically around 15 cm) and the two sonar transceivers are distorted in the vertical direction. At the same time, the installation height of the two sonar transceivers on the robot is preferably 60cm-80cm from the ground (because the sonar transceiver on the robot receives the signal of the sonar transceiver that is tracking the target wearing on the belt, the waist of the person is on the ground. The height is about 60cm---80cm. In order to facilitate the tracking of the target wear, a small sonar transceiver is generally selected. In addition, other auxiliary sensors should be installed in the detection dead zone of the sonar sensor, so as to minimize the detection dead zone of the entire robot. The robot can fully and accurately sense the obstacles in the external environment during the following process, so as to better realize the obstacle function.

A sonar-based robot tracking method of the present disclosure is mounted with a sonar transceiver as a coordinator, and a sonar transceiver provided as a first routing node on the tracking target, the method comprising the following steps:

1: in the process of tracking the tracking target, when the coordinator receives the target recognition positioning instruction issued by the motion controller of the robot, the coordinator acquires the target according to the sound wave emitted by the first routing node The positioning data is identified and the target identification positioning data is transmitted to the motion controller.

Specifically, in the process of tracking the tracking target by the robot, the motion controller of the robot sends a target recognition positioning instruction to the coordinator according to a certain frequency (for example: once every 2 seconds), and the motion controller can receive from the coordinator. The target identifies the positioning data, and the motion controller determines the orientation of the tracking target according to the target recognition positioning data sent back by the coordinator and calculates the distance between the robot and the target.

2: When the coordinator receives the ranging instruction issued by the motion controller of the robot, the coordinator acquires ranging data and transmits the ranging data to the motion controller.

Specifically, after receiving the target identification positioning data sent by the coordinator, the motion controller sends a ranging instruction to the coordinator, in order to measure whether there is an obstacle in the path of the tracking target. After receiving the ranging data sent by the coordinator, the motion controller can be conveniently determined whether there is an obstacle between the robot and the target, and if there is an obstacle, the distance and orientation between the robot and the obstacle are calculated.

Specifically, the motion controller determines the tracking state according to the target identification positioning data and the ranging data (for example, whether there is an obstacle between the robot and the tracking target, if present, the direction of the obstacle, the distance of the robot to the obstacle, etc.), Then follow the tracking path to calculate the following path.

During the process of following the tracking target, the robot continuously sends the target recognition positioning command and the ranging instruction to obtain the position and distance of the tracking target and the obstacle information in the walking environment, thereby continuously updating the walking path of the robot in real time. The purpose of positioning, path planning, autonomous following and dynamic obstacle avoidance.

In another embodiment of the present disclosure, in addition to the same as above, another sonar transceiver is installed on the robot as the second routing node; Step 1: in the process of tracking the tracking target And when the coordinator receives the target recognition positioning instruction issued by the motion controller of the robot, the coordinator acquires the target identification positioning data according to the sound wave transmitted by the first routing node, and sends the target identification positioning data. The motion controller includes the following steps:

12: When the first routing node receives the first frame command sequence, generates a second frame command sequence including a delay time, and broadcasts and sends a second frame command sequence, and then transmits according to the delay time delay. Sound wave

After receiving the second frame command sequence, the coordinator obtains the first time of receiving the sound wave transmitted by the first routing node, and calculates the first distance data according to the first time;

After receiving the first frame command sequence and the second frame command sequence, the second routing node acquires the second time of receiving the sound wave emitted by the first routing node, and calculates the second distance data according to the second time. Transmitting the second distance data to the coordinator;

Specifically, as shown in FIG. 2, the specific process of the coordinator obtaining the target identification positioning data is:

1) When the coordinator receives the target recognition positioning instruction sent by the motion controller, it will use the coordinator identification number ID0 as the first bit data, and the time synchronization instruction issued by the motion controller (the time synchronization instruction refers to the motion controller) How long does it take to obtain the target identification positioning data, if the time synchronization command is 1.5 seconds, it is desirable to obtain the target identification positioning data in 1.5 seconds) and the first routing node is required to transmit the acoustic wave command as the subsequent bit data; the first bit data and The subsequent bit data constitutes a first frame command sequence, and the wireless signal transmitting module on the coordinator transmits the first frame command sequence in the form of a broadcast command. The wireless signal transmitting module of this embodiment selects the Zigbee module. It should be noted that other types of wireless transmitting modules (for example, Wi-Fi modules) can also implement the functions of the Zigbee module in this embodiment, and can be used as the Zigbee module. replace. The coordinator then waits for a confirmation command sent by the first routing node.

2) After receiving the first frame command sequence sent by the coordinator, the first routing node uses the identity identification number ID1 of the first routing node as the first bit data, and the first bit data ID1 and the confirmation instruction (confirm) The instruction may include a delay time, the delay time may be set according to the time synchronization instruction, or may be set according to an empirical default) to form a second frame command sequence, and the wireless signal transmitting module on the first routing node will execute the second frame command sequence. It is sent in the form of a broadcast command. After the transmission is completed, a series of sound waves are transmitted according to the delay time delay. After the sound wave is sent, the first routing node stops transmitting and enters the waiting command state.

3) After receiving the confirmation command in the second frame command sequence, the coordinator parses the delay time, for example: 15 milliseconds, then starts timing after 15 milliseconds delay, and waits to receive the sound wave signal transmitted by the first routing node. When the coordinator receives the acoustic signal transmitted by the first routing node, it stops timing, and the coordinator reads out the first time t1 of the timing, and then calculates the robot to the tracking target according to the “transit time detection ranging method”. A distance data S1 temporarily stores the first distance data S1 into the data buffer.

4) After receiving the first frame command sequence sent by the coordinator and the second frame command sequence sent by the first routing node, the second routing node starts timing after delaying according to the delay time in the second frame command sequence. And waiting to receive the acoustic signal transmitted by the first routing node. After the second routing node receives the acoustic signal transmitted by the first routing node, the timing is stopped, and the second routing node reads the second time t2 of the timing, and then calculates according to the “transit time detection ranging method”. The robot goes to the second distance data S2 of the tracking target, and temporarily stores the second distance data S2 into the data buffer.

After the second routing node calculates the second distance data S2, the second routing node ID2 is used as the first bit data, and the second distance data S2 is the second bit to form the third frame command sequence. The wireless signal transmitting module on the routing node sends a third frame command sequence to the coordinator.

6) After receiving the third frame command sequence sent by the second routing node, the coordinator parses the second distance data S2 measured by the second routing node from the third frame data. The coordinator uses the coordinator identification number ID0 as the first bit data, the first distance data S1 as the second bit data, and the second distance data S2 as the third bit data, and the three kinds of data are sequentially composed of the fourth frame command sequence. Give the robot's motion controller. The first distance data S1 and the second distance data S2 constitute target recognition positioning data.

It should be noted that the examples of the time and frequency in each embodiment are only for convenience of description, and the actual use needs to be set according to actual conditions.

Preferably, step 14 specifically includes the following steps:

141: The second routing node starts timing according to the delay time delay;

142: When receiving the sound wave emitted by the first routing node, the second routing node stops timing and obtains the second time;

143: The second routing node calculates the second distance data according to the second time and the preset distance formula.

144: The second routing node sends the second distance data to the coordinator.

Specifically, when the motion controller sends the target identification positioning instruction, the time synchronization instruction is sent to the coordinator, and the coordinator also adds the time synchronization instruction when broadcasting the first frame command sequence, when the first routing node receives the In the first frame command sequence, in addition to the broadcast receiving the sound wave command, a delay time is set in the second frame command sequence according to the time synchronization command, so that the received coordinator and the second routing node have the same delay. After the time has elapsed, the first routing node also delays transmitting the sound wave after the same time, ensuring that each sonar sounder emits sound waves and starts time synchronization, thereby ensuring the accuracy of the first distance data and the second distance data. Sex. It should be noted that the time taken by the coordinator and the second routing node to receive the second frame command sequence is very short and can be ignored.

The principle of "transit time detection ranging method" is to obtain the distance by measuring the time elapsed by the ultrasonic echo when the speed of sound is known. The schematic diagram is shown in Figure 5. First, the sonar transceiver emits a series of sound wave pulses into the air, and the sound waves are reflected back after encountering the object to be measured. It is known that the speed of sound is C. If the time difference between the arrival time of the first echo and the transmitted sound wave pulse can be measured, the distance S between the sonar transceiver and the measured object can be calculated by using formula (1):

The measured distance d between the sonar transceiver and the measured object is calculated using equation (2):

In equation (2), h is the distance between the transmitting end and the receiving end, and S is the distance between the sonar transceiver and the object to be measured.

When S

h, d≈S, in the actual use of the sonar transceiver, generally use the sensor to send and receive the same body, so h = 0, then the formula (2) is reduced to the formula (3):

Knowing the speed of sound C=344m/s (at 20°C), equation (3) is further simplified to equation (4):

S=172*t (4)

When the first distance data and the second distance data are measured, since the distance data is calculated according to the time of the received sound wave sent by the first routing node, the first time and the second time do not include receiving the reflected echo. Time, so the preset distance formula should be set to: S=344*t.

The first distance data and the second distance data are calculated according to a preset distance formula.

In another embodiment of the present disclosure, in addition to the same as above, step 2: when the coordinator receives the ranging instruction issued by the motion controller, the coordinator acquires the ranging data and sends the ranging data to The motion controller includes the following steps:

21: When the coordinator receives the ranging instruction sent by the motion controller, the coordinator sends the fifth frame command sequence to the second routing node;

22: When the second routing node receives the fifth frame command sequence, the second routing node transmits the sound wave, and obtains a third time of receiving the echo signal of the sound wave sent by the second routing node;

23: The second routing node calculates the third distance data according to the third time and the preset ranging formula, and sends the third distance data to the coordinator;

24: when the coordinator receives the third distance data, the coordinator transmits the sound wave, and obtains a fourth time of receiving the echo signal of the sound wave sent by the coordinator;

26: The coordinator transmits the third distance data and the fourth distance data as ranging data to the motion controller.

Specifically, the ranging instruction is to sense whether there is an obstacle between the robot and the tracking target. The process by which the robot senses obstacles while tracking the tracking target is:

1) When receiving the ranging instruction sent by the motion controller, the coordinator uses the coordinator identification number ID0 as the first bit data, and the coordinator combines the first bit data and the ranging command into a fifth frame command sequence, and coordinates The wireless signal transmitting module on the device sends the fifth frame command sequence to the second routing node, and then the coordinator waits for the measurement result of the second routing node.

2) After receiving the fifth frame command sequence sent by the coordinator, the second routing node first sends a string of sound waves, and after the sound wave is sent, the timer is turned on and waits for receiving the echo signal, when the second routing node After receiving the echo signal, the timing is stopped and the second routing time is read by the second routing node, and then the third distance data S3 of the robot to the obstacle is calculated according to the “transit time detection ranging method”, and the second The routing node takes the identity number ID2 as the first bit data, the ranging command as the second bit data, and the third distance data S3 as the third bit data to form the sixth frame command sequence, and the wireless signal transmitting module on the second routing node The sixth frame command sequence is sent to the coordinator. The second routing node then stops working and enters the wait command state.

3) After receiving the sixth frame command sequence of the second routing node, the coordinator parses out the third distance data S3 temporarily stored in the data buffer, and then the coordinator sends a string of sound waves, and after the sound wave is sent, the timer is turned on and waits for reception. The echo signal; when the coordinator receives the echo signal, stops the timing and reads out the fourth time t4 of the timing, and then calculates the fourth distance data S4 of the robot to the obstacle according to the "transit time detection ranging method".

4) The coordinator sends the coordinator identification number ID0 as the first bit data, the third distance data S3, and the fourth distance data S4 to form a seventh frame command sequence to the motion controller. The third distance data and the fourth distance data are composed of ranging data.

When calculating the third distance data and the fourth distance data, it is timed according to the echo of the sound wave emitted by the first routing node or the coordinator itself, and therefore, the preset ranging formula is set to: S=172*t.

In another embodiment of the present disclosure, in addition to the same as above, step 3: the motion controller identifies the positioning data and the ranging data according to the target, obtains the tracking state, and calculates the following path according to the tracking state, including the following steps:

31: The motion controller determines whether there is an obstacle between the distance tracking targets according to the target identification positioning data and the ranging data (that is, whether there is an obstacle on the straight line of the robot to the tracking target);

32: If there is no obstacle, the following path is obtained without obstacle avoidance;

33: If there is an obstacle, the following path of the obstacle avoidance is obtained according to the preset obstacle avoidance method.

Specifically, according to the ranging data and the target identification positioning data, it can be determined whether there is an obstacle between the robot and the tracking target. Of course, it may happen that the obstacle indicated by the third distance data and the fourth distance data is the tracking target itself, and the orientation and distance of the obstacle obtained according to the ranging data and the tracking target obtained according to the target identification positioning data are needed. Azimuth and distance are used for judgment.

If there is no obstacle between the robot and the tracking target, the full speed can be used as the following path without obstacle avoidance; if there is an obstacle in the path between the robot and the tracking target, it is necessary to obtain the following path of the obstacle avoidance according to the preset obstacle avoidance method. .

Preferably, if there is an obstacle, obtaining the following path of the obstacle avoidance according to the preset obstacle avoidance method comprises the following steps:

331: when the distance to the obstacle is less than a preset distance, adopt a follow path that stops advancing;

332: When the obstacle is not less than a preset distance, obtaining a following path of the slow steering according to the ranging data.

Specifically, considering that if the obstacle is close, and the robot can no longer perform steering obstacle avoidance, it needs to stop and avoid hitting the obstacle. Therefore, a preset distance is set, for example, 1 meter, and when the distance from the robot to the obstacle is less than 1 meter based on the third distance data and the fourth distance data, it is stopped.

When the obstacle is located on the route forwarded by the robot following the tracking target, the robot will confirm which direction is relatively empty according to the third distance data and the fourth distance data (for example, if the third distance data is smaller than the fourth distance data, the fourth The direction corresponding to the distance data is relatively empty, so which direction is turned to follow the tracking target.

For example, the third distance data is smaller than the fourth distance data, indicating that the orientation corresponding to the fourth distance data is relatively empty, and the direction corresponding to the fourth distance data can be turned, thereby achieving the purpose of following the tracking target.

Slow speed is to prevent the robot from hitting obstacles during the steering process, so let it slow down. Slow and full speed can be set by the engineer according to experience and use, for example: full speed is set to 2 m / s, slow speed is set to 1 m / sec.

In another embodiment, as shown in FIG. 3, the robot obstacle avoidance control process:

Determine whether there is an obstacle in front, if there is no obstacle, advance at full speed; if there is an obstacle ahead, determine the distance of the obstacle, if the obstacle is close (the distance from the robot to the obstacle is less than the preset distance), stop the advance, if the obstacle ahead The distance of the object is far (the distance from the robot to the obstacle is not less than the preset distance), and the speed is forward; the left and right space of the obstacle is judged. If the left side is wider, turn left, if the right side is wider, turn right;

As shown in Figure 4, the robot follows the target control flow:

Detect whether there is an object. If there is no object, do not plan the target path. If the object is detected, identify whether the object is a tracking target or an obstacle. If it is a tracking target, track the tracking target and follow the tracking target. When observing the obstacles, avoid obstacles according to the obstacle avoidance process; if it is an obstacle rather than tracking the target, determine whether the obstacle is in front, if the obstacle is not in front, do not follow the path planning, if the obstacle is in front, avoid obstacles according to the obstacle avoidance process At this time, the target path planning is not performed.

In another embodiment of the present disclosure, a sonar-based robot tracking system includes: tracking a target and a robot;

The tracking target includes: a sonar transceiver as the first routing node;

The robot includes:

a sonar transceiver, motion controller, servo controller and drive as a coordinator;

The coordinator, in the process of tracking the tracking target, when receiving the target recognition positioning instruction issued by the motion controller, the coordinator acquires the sound wave according to the first routing node The target identifies the positioning data, and sends the target identification positioning data to the motion controller;

Specifically, in the process of tracking the tracking target by the robot, the motion controller of the robot sends a target recognition positioning instruction to the coordinator according to a certain frequency (for example: once every 1.5 seconds), and the motion controller can receive from the coordinator. The target identifies the positioning data, and the motion controller determines the orientation of the tracking target according to the target recognition positioning data sent back by the coordinator and calculates the distance between the robot and the target.

After the motion controller receives the target identification positioning data sent by the coordinator, it sends a ranging instruction to the coordinator to measure whether there is an obstacle in the path of the tracking target. After receiving the ranging data sent by the coordinator, the motion controller can be conveniently determined whether there is an obstacle between the robot and the target, and if there is an obstacle, the distance and orientation between the robot and the obstacle are calculated.

The motion controller determines the tracking state according to the target identification positioning data and the ranging data (for example, whether there is an obstacle between the robot and the tracking target, if present, the direction of the obstacle, the distance of the robot to the obstacle, etc.), and then according to the tracking The state calculates the following path.

In another embodiment of the present disclosure, the robot further includes: another sonar transceiver as a second routing node, except for the same as above;

a coordinator, in the process of tracking the tracking target, when receiving the target recognition positioning instruction issued by the motion controller, the coordinator acquires the target identification positioning data according to the sound wave emitted by the first routing node And transmitting the target identification positioning data to the motion controller includes:

The coordinator is configured to: when the target identification positioning instruction is received, the coordinator broadcasts a first frame command sequence;

The first routing node is configured to: after receiving the first frame command sequence, generate a second frame command sequence including a delay time, and broadcast and transmit the second frame command sequence, and then transmit according to the delay time delay Sound wave

1) When the coordinator receives the target recognition positioning instruction sent by the motion controller, the coordinator identification number ID0 is used as the first bit data, and the time synchronization instruction issued by the motion controller and the requesting first routing node to transmit the acoustic wave command are Subsequent bit data; the first bit data and the subsequent bit data form a first frame command sequence, and the wireless signal transmitting module on the coordinator transmits the first frame command sequence in the form of a broadcast command. The wireless signal transmitting module of this embodiment selects the Zigbee module. It should be noted that other types of wireless transmitting modules (for example, Wi-Fi modules) can also implement the functions of the Zigbee module in this embodiment, and can be used as the Zigbee module. replace. The coordinator then waits for a confirmation command sent by the first routing node.

2) After receiving the first frame command sequence sent by the coordinator, the first routing node uses the identity identification number ID1 of the first routing node as the first bit data, and the first bit data ID1 and the confirmation instruction (confirm) The instruction may include a delay time, the delay time may be set according to the time synchronization instruction, or may be set according to an empirical default) to form a second frame command sequence, and the wireless signal transmitting module on the first routing node will execute the second frame command sequence. It is sent in the form of a broadcast command. After the transmission is completed, a series of sound waves are transmitted according to the delay time delay. After the sound wave is sent, the first routing node stops transmitting and enters the wait command state.

3) After receiving the confirmation command in the second frame command sequence, the coordinator parses the delay time, for example: 18 milliseconds, then starts timing after 18 milliseconds delay, and waits to receive the sound wave signal transmitted by the first routing node. When the coordinator receives the acoustic signal transmitted by the first routing node, it stops timing, and the coordinator reads out the first time t1 of the timing, and then calculates the robot to the tracking target according to the “transit time detection ranging method”. A distance data S1 temporarily stores the first distance data S1 into the data buffer.

Preferably, the second routing node is configured to: after receiving the first frame command sequence and the second frame command sequence, acquire a second time of receiving the sound wave emitted by the first routing node, and according to And calculating, by the second time, the second distance data, and sending the second distance data to the coordinator includes:

After receiving the first frame command sequence and the second frame command sequence, the second routing node starts timing according to the delay time delay;

And, when receiving the sound wave emitted by the first routing node, the second routing node stops timing, and obtains a second time;

And calculating, according to the second time and the preset distance formula, the second distance data;

And sending the second distance data to the coordinator.

In another embodiment of the present disclosure, in addition to the same as the above, the coordinator is configured to acquire the ranging data when receiving the ranging instruction issued by the motion controller, and The sending of the ranging data to the motion controller includes:

In another embodiment of the present disclosure, in addition to the same as the above, the motion controller is configured to identify the positioning data according to the target and the ranging data, obtain a tracking status, and calculate according to the tracking status. Follow paths include:

The motion controller is configured to determine, according to the target identification positioning data and the ranging data, whether there is an obstacle between the tracking targets (ie, determining whether there is an obstacle on a straight line of the robot to the tracking target) If there is no obstacle, the following path is obtained without obstacle avoidance; if there is an obstacle, the following path of the obstacle avoidance is obtained according to the preset obstacle avoidance method.

Preferably, the motion controller, if there is an obstacle, the following obstacle path obtained according to the preset obstacle avoidance method includes:

The motion controller is configured to adopt a follow path that stops advancing when a distance to the obstacle is less than a preset distance;

And, when the obstacle is not less than the preset distance, the following path of the slow steering is obtained according to the ranging data.

When the obstacle is located on the route that the robot advances to follow the tracking target, the robot will confirm which direction is relatively empty based on the third distance data and the fourth distance data, thereby turning to which direction to follow the tracking target. For specific examples, refer to the corresponding method embodiments, which are not described herein.

Slow speed is to prevent the robot from hitting obstacles during the steering process, so let it slow down. Slow and full speed can be set by the engineer according to experience and use, for example: full speed is set to 3 m / s, slow speed is set to 1 m / sec.

The tracking system of the present disclosure realizes various uses of the sonar transceiver, which not only realizes the product function but also reduces the manufacturing cost, and can quickly and quickly pull the information of the obstacle and the tracking target in the external environment. A number of functions are realized at a lower cost and a simple structure, and the positioning and tracking are accurate and the application value is high.

While the invention has been described above in the preferred embodiments, it is not intended to limit the invention. A person skilled in the art can make various changes and modifications without departing from the spirit and scope of the invention. Therefore, the scope of the invention is defined by the scope of the appended claims.

Claims

A sonar-based robot tracking method is provided with a sonar transceiver as a coordinator, and a sonar transceiver as a first routing node is provided on the tracking target, which comprises the following steps:

1: in the process of tracking the tracking target, when the coordinator receives the target recognition positioning instruction issued by the motion controller of the robot, the coordinator acquires the target according to the sound wave emitted by the first routing node Identifying positioning data and transmitting the target identification positioning data to a motion controller;

2: when the coordinator receives the ranging instruction issued by the motion controller, the coordinator acquires ranging data, and sends the ranging data to the motion controller;

3: The motion controller identifies the positioning data and the ranging data according to the target, obtains a tracking state, and calculates a following path according to the tracking state;

4: The motion controller sends a corresponding motion instruction sequence to the servo controller of the robot according to the following path, and the servo controller drives the driving device on the robot to implement a following action.
The sonar-based robot tracking method according to claim 1, wherein another sonar transceiver is further mounted on the robot as a second routing node; and the step 1 includes the following steps:

11: when the coordinator receives the target identification positioning instruction, the coordinator broadcasts a first frame command sequence;

12: The first routing node receives the first frame command sequence, generates a second frame command sequence including a delay time, and broadcasts the second frame command sequence after broadcasting, according to the delay time delay After emitting sound waves;

After receiving the second frame command sequence, the coordinator obtains a first time of receiving the sound wave emitted by the first routing node, and calculates first distance data according to the first time;

After receiving the first frame command sequence and the second frame command sequence, the second routing node acquires a second time of receiving the sound wave emitted by the first routing node, and calculates according to the second time. Obtaining second distance data, and sending the second distance data to the coordinator;

15: The coordinator transmits the first distance data and the second distance data as the target identification positioning data to a motion controller.
The sonar-based robot tracking method according to claim 2, wherein the step 14 specifically comprises the following steps:

141: After receiving the first frame command sequence and the second frame command sequence, the second routing node starts timing according to the delay time delay;

142: When receiving the sound wave emitted by the first routing node, the second routing node stops timing, and obtains a second time;

143: The second routing node calculates the second distance data according to the second time and a preset distance formula.

144: The second routing node sends the second distance data to the coordinator.
The sonar-based robot tracking method according to claim 2, wherein the step 2: when the coordinator receives the ranging instruction issued by the motion controller, the coordinator acquires the ranging Data and transmitting the ranging data to the motion controller includes the following steps:

21: When the coordinator receives the ranging instruction sent by the motion controller, the coordinator sends a fifth frame command sequence to the second routing node;

22: When the second routing node receives the fifth frame command sequence, the second routing node transmits an acoustic wave, and obtains a third time of receiving an echo signal of the acoustic wave sent by the second routing node. ;

23: The second routing node calculates third distance data according to the third time and a preset ranging formula, and sends the third distance data to the coordinator;

24: after the coordinator receives the third distance data, the coordinator transmits an acoustic wave, and obtains a fourth time of receiving an echo signal of the acoustic wave sent by the coordinator;

25: The coordinator calculates the fourth distance data according to the fourth time and the preset ranging formula;

26: The coordinator transmits the third distance data and the fourth distance data as the ranging data to a motion controller.
The sonar-based robot tracking method according to claim 1, wherein the step 3: the motion controller obtains a tracking state according to the target identification positioning data and the ranging data, and according to the The tracking state calculates the following path including the following steps:

31: The motion controller determines, according to the target identification positioning data and the ranging data, whether there is an obstacle between the tracking targets;

32: If there is no obstacle, the following path is obtained without obstacle avoidance;

33: If there is an obstacle, the following path of the obstacle avoidance is obtained according to the preset obstacle avoidance method.
The sonar-based robot tracking method according to claim 5, wherein the step 33: if there is an obstacle, obtaining the following path of the obstacle avoidance according to the preset obstacle avoidance method comprises the following steps:

331: when the distance to the obstacle is less than a preset distance, adopt a follow path that stops advancing;

332: When the obstacle is not less than a preset distance, obtain a following path of the slow steering according to the ranging data.
A sonar-based robot tracking system, comprising: tracking a target and a robot;

The tracking target includes: a sonar transceiver as a first routing node;

The robot includes: a sonar transceiver as a coordinator, a motion controller, a servo controller, and a driving device;

The coordinator, in the process of tracking the tracking target, when receiving the target recognition positioning instruction issued by the motion controller, the coordinator acquires the sound wave according to the first routing node The target identifies the positioning data, and sends the target identification positioning data to the motion controller;

And, when receiving the ranging instruction issued by the motion controller, the coordinator acquires ranging data, and sends the ranging data to the motion controller;

The motion controller is configured to identify the positioning data according to the target and the ranging data, obtain a tracking status, and calculate a following path according to the tracking status; and send a corresponding action instruction sequence according to the following path Giving the servo controller;

The servo controller is configured to drive the driving device to implement a following action according to the received sequence of the motion instruction.
The sonar-based robot tracking system according to claim 7, wherein:

The robot further includes: another sonar transceiver as a second routing node;

The coordinator, in the process of tracking the tracking target, when receiving the target recognition positioning instruction issued by the motion controller, the coordinator acquires target recognition according to the sound wave emitted by the first routing node Locating the data and transmitting the target identification positioning data to the motion controller includes:

The coordinator, when the target recognition positioning instruction is received, the coordinator broadcasts a first frame command sequence;

The first routing node is configured to receive the first frame command sequence, generate a second frame command sequence including a delay time, and broadcast and transmit the second frame command sequence, according to the delay time delay Acoustic waves are emitted afterwards;

The coordinator is configured to: after receiving the second frame command sequence, acquire a first time of receiving the sound wave emitted by the first routing node, and calculate the first distance data according to the first time;

The second routing node, after receiving the first frame command sequence and the second frame command sequence, acquiring a second time of receiving the sound wave emitted by the first routing node, and according to the Calculating, by the second time, the second distance data, and sending the second distance data to the coordinator;

The coordinator transmits the first distance data and the second distance data as the target identification positioning data to a motion controller.
The sonar-based robot tracking system according to claim 8, wherein the second routing node is configured to receive and receive the first frame command sequence and the second frame command sequence a second time of the sound wave transmitted by the first routing node, and calculating second distance data according to the second time, and sending the second distance data to the coordinator includes:

After receiving the first frame command sequence and the second frame command sequence, the second routing node starts timing according to the delay time delay;

And, when receiving the sound wave emitted by the first routing node, the second routing node stops timing, and obtains a second time;

And calculating, according to the second time and the preset distance formula, the second distance data;

And transmitting the second distance data to the coordinator.
The sonar-based robot tracking system according to claim 8, wherein the coordinator is configured to acquire ranging data when receiving a ranging instruction issued by the motion controller, And transmitting the ranging data to the motion controller comprises:

The coordinator, when receiving the ranging instruction sent by the motion controller, the coordinator sends a fifth frame command sequence to the second routing node;

The second routing node is configured to: when receiving the sequence of the fifth frame command, the second routing node transmits an acoustic wave, and obtains a third of an echo signal of the acoustic wave sent by the second routing node. And calculating, according to the third time and the preset ranging formula, the third distance data, and sending the third distance data to the coordinator;

The coordinator, configured to: after receiving the third distance data, the coordinator emits an acoustic wave, and obtains a fourth time of receiving an echo signal of the acoustic wave sent by the coordinator; and, according to the And calculating a fourth distance data by using the fourth time and the preset ranging formula; and transmitting the third distance data and the fourth distance data as the ranging data to the motion controller.
The sonar-based robot tracking system according to claim 7, wherein the motion controller is configured to obtain a tracking state according to the target identification positioning data and the ranging data, and according to the tracking The state calculates the following path including:

The motion controller is configured to determine, according to the target identification positioning data and the ranging data, whether there is an obstacle between the tracking targets; if there is no obstacle, obtain a following path that does not need to avoid obstacles; If there is an obstacle, the following path of the obstacle avoidance is obtained according to the preset obstacle avoidance method.
The sonar-based robot tracking system according to claim 11, wherein the motion controller, if there is an obstacle, obtains an obstacle avoidance follow path according to the preset obstacle avoidance method, including:

The motion controller is configured to adopt a follow path that stops advancing when a distance to the obstacle is less than a preset distance;

And, when the obstacle is not less than the preset distance, the following path of the slow steering is obtained according to the ranging data.