A relative positioning method for partner robots based on multi-information fusion

Abstract

The present invention discloses a relative positioning method of a partner robot with multi-information fusion. In order to overcome the problem that each single partner robot positioning technology has its own drawbacks; the present invention comprises the following steps: S1: the current robot uses a millimeter wave radar to detect the surrounding environment and obtains a point cloud formed by a reflected signal; S2: the current robot communicates and measures distance with the partner robot through UWB to obtain the real-name distance measurement result of the partner robot; S3: the current robot and the partner robot obtain their respective current positions through dead reckoning, and the current robot receives the current position information of the partner robot to obtain the estimated result of the relative position of the two; S4: the current robot obtains the relative position of the partner robot according to the point cloud, the real-name distance measurement result and the relative position estimation result at the same time. By combining the millimeter wave radar and UWB, point-to-point positioning between partner robots can be achieved, and the influence of interference can be reduced.

Description

Technical Field

[0001] The present invention relates to the field of robot positioning, and in particular to a relative positioning method of a partner robot based on the combination of millimeter wave radar and UWB. Background Art

[0002] In a multi-robot system, the relative positioning between robots is the basis for realizing multi-robot collaborative operation. As a high-precision positioning technology, Ultra Wide Band (UWB) technology is also widely used in multi-robot relative positioning.

[0003] The partner positioning method based on UWB usually takes advantage of the fact that the UWB system can complete point-to-point precise distance measurement. For two-dimensional space, when there are four robots in the system, the relative distance between the robots can be determined with the help of UWB ranging. Based on this principle, a variety of positioning methods such as TOF and TDOA have been derived. However, these methods must ensure that more than four robots in the system can complete mutual distance measurement. If some robots cannot complete the distance measurement due to the long distance or occlusion, the algorithm fails. Similar methods include deploying UWB base stations in the environment to assist in positioning.

[0004] For example, a "multi-robot collaborative positioning method integrating odometer and UWB" disclosed in Chinese patent literature, with publication number CN112833876B, includes the following steps: S1: using the odometer and UWB data acquisition module to respectively collect the robot's posture information and the distance information between multiple groups of UWB nodes; S2: based on the collected distance information between multiple groups of UWB nodes, multi-robot collaborative positioning is achieved through a nonlinear optimization algorithm; S3: based on the multi-robot positioning information after nonlinear optimization, the information provided by the robot odometer is integrated to construct a posture graph and optimize it to achieve precise positioning of multi-robot collaboration.

[0005] The dead reckoning method is another method for relative positioning between partner robots. The idea of ​​this method is to first determine the initial relative position of the two robots through initialization, then estimate the movement of each robot through dead reckoning, and finally update the relative position between the robots. The challenge faced by this type of method is that the IMU used for dead reckoning has cumulative errors, which will gradually increase over time.

[0006] The detection-based method is another method for relative positioning of the partner robot. For example, microwave radar, laser radar, machine vision and other methods are used to actively identify the partner robot from the environment and determine the relative position of the partner robot. However, the problem with this method is that it is easy to mistake interference targets in the environment for partner robots, and it is impossible to distinguish different partner robots.

[0007] Each single partner robot positioning technology has its own drawbacks. Traditionally, using UWB for partner robot positioning requires more than four robots to be able to sense each other, while relative positioning technologies based on active detection such as microwave radar make it difficult to distinguish between partner robots and interference objects. Summary of the invention

[0008] The present invention mainly solves the problem that the existing single partner robot positioning technology has its own disadvantages; it provides a multi-information fusion partner robot relative positioning method, which realizes point-to-point positioning between partner robots by combining millimeter wave radar and UWB, and can reduce the influence of interference.

[0009] The above technical problems of the present invention are mainly solved by the following technical solutions:

[0010] A multi-information fusion partner robot relative positioning method comprises the following steps:

[0011] S1: The current robot uses millimeter-wave radar to detect the surrounding environment and obtain the point cloud formed by the reflected signal;

[0012] S2: The current robot communicates and measures distance with the partner robot through UWB to obtain the real-name distance measurement result of the partner robot;

[0013] S3: The current robot and the partner robot obtain their respective current positions through dead reckoning, and the current robot receives the current position information of the partner robot to obtain an estimated result of the relative positions of the two robots;

[0014] S4: The current robot obtains the relative position of the partner robot based on the point cloud, real-name distance measurement results and relative position estimation results at the same time.

[0015] This solution combines millimeter wave radar with UWB technology, which enables current robots to locate partner robots without relying on auxiliary robots or additional base stations, and achieve point-to-point relative positioning. It allows multiple robots to be arranged in a single line formation and position each other in the form of relay, so that the robot at the end can obtain the position of the robot at the head end, so as to explore narrow spaces such as lanes and caves, which helps to expand the collaborative operation application of multi-robot systems.

[0016] Preferably, the current robot and the partner robot are both equipped with a relative positioning module;

[0017] The relative positioning module includes a millimeter wave radar for detecting the surrounding environment, a corner reflector for enhancing the reflection intensity of the radar signal, a UWB ranging module for relative distance measurement between robots, an inertial sensor for dead reckoning, and a radio communication module.

[0018] The millimeter wave radar-UWB-IMU is integrated into an independent collaborative positioning module, which can be easily installed on different robots and is easy to use. It enables the multi-robot system to achieve one-to-one partner positioning without relying on additional base stations or multiple (more than 2) robots to complete positioning. On the one hand, it improves the robustness of the system, and on the other hand, it also allows the multi-robot system to go deep into narrow spaces such as caves and lanes in a chain formation to conduct regional exploration.

[0019] Preferably, all points detected by the millimeter wave radar are screened to retain only points in the area where the partner robot is potentially present;

[0020] D f ={d|ll ε <d<l+l ε , d∈D}

[0021] Where D is the set of all points detected by the millimeter wave radar;

[0022] l is the distance to the partner robot detected by UWB;

[0023] l ε is the general empirical error of UWB ranging;

[0024] D f is the filtered subset;

[0025] The points retained after screening are clustered using the DBSCAN algorithm; the positions represented by the clustering results are the potential positions of the partner robot;

[0026] According to the dead reckoning information, the clustering results are screened iteratively for multiple times to finally determine the position of the partner robot.

[0027] The purpose of this solution is to reduce the amount of calculation, thereby speeding up the positioning speed. It can effectively locate in complex environments, especially those with a large number of interference sources.

[0028] Preferably, the information of several positioning cycles is continuously collected to obtain a set of potential positions of the partner robot at the beginning and end of each positioning cycle, and all potential motion trajectories of the partner robot relative to the current robot in several iterations are constructed;

[0029] In each iteration, the partner robot’s moving distance is calculated based on the dead reckoning results of the two robots;

[0030] Based on the deviation between the moving distance of each segment of the motion trajectory and the actual moving distance of the partner robot, each potential motion trajectory of the partner robot is scored, and the trajectory with the smallest deviation is taken as the actual motion trajectory of the partner robot; based on this, the position of the partner robot in several consecutive positioning cycles can be obtained, as well as the relative position of the partner robot relative to the current robot at the current moment;

[0031] Combine the dead reckoning results of the current robot and the partner robot to obtain the reference direction deviation angle of the IMUs of the two robots.

[0032] Preferably, in each cycle, the following operations are performed:

[0033] <1> Calculate the partner robot's current position p in the current robot's carrier coordinate system last The coordinates of

[0034] <2> Calculate the clustering result set C p Each element in p last The distance of the closest partner robot in this cycle is l neighbor The elements of are used as the estimated values ​​of the current relative position of the partner robot and recorded as

[0035] <3> renew

[0036] <4> Update the relative position of the partner robot based on dead reckoning to obtain the estimated value of the partner robot's current relative position, which is recorded as

[0037] After initialization is completed, the calculation is performed to obtain the relative position of the partner robot.

[0038] As a preference, calculate and When the deviation is greater than a certain threshold, it is judged that there is an obstacle between the robots. As a result of relative positioning;

[0039] However, when judging that there are no obstacles between robots, As the relative positioning result, at the same time, the deviation θ between the IMU reference directions is updated using the relative positioning result error .

[0040] When there is an obstacle between the two robots, the positioning results based on UWB and millimeter-wave radar become inaccurate. However, if you rely solely on dead reckoning, there will be cumulative errors in the positioning results. Therefore, the positioning results based on UWB and millimeter-wave radar are monitored using the dead reckoning results in a short period of time to determine whether there are obstacles between the robots. At the same time, the positioning results of UWB and millimeter-wave radar are used to correct the cumulative errors of dead reckoning.

[0041] The beneficial effects of the present invention are:

[0042] 1. Combining millimeter-wave radar with UWB technology can enable current robots to locate partner robots without relying on auxiliary robots or additional base stations, achieving point-to-point relative positioning.

[0043] 2. It allows multiple robots to be arranged in a single line formation and to locate each other in a relay manner, so that the robot at the end can obtain the position of the robot at the head end, thereby exploring narrow spaces such as tunnels and caves, which helps to expand the collaborative operation application of multi-robot systems.

[0044] 3. Using millimeter-wave radar to detect the partner robot can ensure that the positioning effect is not affected by ambient light. At the same time, the UWB ranging results are combined to filter the location of the partner robot from the detection results of the millimeter-wave radar, and the identity of each partner robot can be identified.

[0045] 4. Integrate millimeter-wave radar-UWB-IMU into an independent collaborative positioning module, which can be easily installed on different robots and is easy to use. BRIEF DESCRIPTION OF THE DRAWINGS

[0046] Figure 1 It is a flow chart of the relative positioning method of the partner robot of the present invention. DETAILED DESCRIPTION

[0047] The technical solution of the present invention is further specifically described below through embodiments and in conjunction with the accompanying drawings.

[0048] Example:

[0049] A multi-information fusion partner robot relative positioning method of this embodiment is applied to a multi-robot system.

[0050] The scenario demonstrated in this implementation is to use a multi-robot system consisting of two robots to explore the target area. Among the two robots, the first robot is equipped with a sampling device, and the second robot is equipped with a detection sensor. When the second robot finds a suspected target, the first robot will approach and take samples. To achieve this goal, the first robot must be able to obtain the position of the second robot relative to itself at any time, so as to keep following the second robot and realize the collaborative operation of this dual-robot system.

[0051] To achieve this goal, each robot is equipped with a partner robot relative positioning module, each of which includes millimeter wave radar, corner reflector, ultra-wideband (UWB) ranging module, inertial sensor (IMU), radio communication equipment and embedded computer.

[0052] The millimeter-wave radar is used to detect the surrounding environment and identify all surrounding targets including the partner robot.

[0053] Corner reflectors are used to enhance the reflection strength of radar signals, thereby ensuring that when detected by the partner robot’s millimeter-wave radar, the reflected signal is strong enough and will not be drowned out by background noise.

[0054] The UWB ranging module is used to achieve relative ranging between partner robots and is the key to distinguishing partner robots from interference targets.

[0055] IMU is used to perform dead reckoning, that is, to obtain the amount of movement of the robot between two positionings. This information helps to improve the accuracy of relative positioning.

[0056] The radio communication device is used to send its own dead reckoning results to the partner robot as a basis for its relative positioning.

[0057] The embedded computer of the relative positioning module is used to complete the calculations required for relative positioning.

[0058] Based on the above relative positioning module, the first robot obtains the relative position of the second robot according to the following process. Similarly, the second robot can also follow similar steps to obtain the relative position of the first robot. The first robot and the second robot are partner robots.

[0059] like Figure 1 As shown, a multi-information fusion partner robot relative positioning method of this embodiment includes the following steps: S1: The current robot uses a millimeter wave radar to detect the surrounding environment and obtains a point cloud formed by a reflection signal.

[0060] In this embodiment, at the moment when the two robots are just deployed, the first robot can know the existence of the second robot through radio communication, but does not know the initial position of the second robot. Similarly, since the initial states of the two robots are different, the reference directions of the IMUs carried by each are also different, and at this time, the connection between the two robots needs to be established through initialization.

[0061] The first robot uses a millimeter-wave radar to scan the surrounding environment and obtains a point cloud composed of reflection signals of all surrounding targets. These points contain reflection signals of the second robot, but the first robot cannot recognize them.

[0062] S2: The current robot communicates and measures distance with the partner robot via UWB to obtain the real-name distance measurement result of the partner robot.

[0063] The current robot uses the UWB module to communicate and measure distance with the partner robot carrying the UWB module, thereby obtaining an accurate distance measurement result of the partner robot, which is called the real-name distance measurement result. The first robot obtains the relative distance with the second robot through UWB distance measurement. This distance measurement result is relatively accurate, and the error is usually at the decimeter level.

[0064] Based on the ranging results and after considering the ranging error, the collected point cloud is screened and only the points whose distance to the first robot is close to the ranging result are retained. It can be considered that the reflection signal of the second robot is among these retained points.

[0065] Assume that the set of all points detected by the millimeter wave radar is D, the distance to the partner robot detected by UWB is l, and the general empirical error of UWB ranging is l ε In this embodiment, l ε The value is 0.3 meters.

[0066] Filter the points in set D to obtain its subset D f :

[0067] D f ={d|ll ε <d<l+l ε , d∈D}

[0068] That is, for all the points detected by the millimeter-wave radar, only the points in the annular area where the partner robot may exist are retained. The purpose of this operation is to reduce the amount of calculation, thereby speeding up the positioning speed. For complex environments, especially those with a large number of interference sources, it can effectively locate.

[0069] The points retained after screening are clustered using the DBSCAN algorithm, and the position represented by the clustering result is the potential position of the second robot; there may be multiple clustering results at this time.

[0070] For set D f The points in the cluster are clustered, and each cluster result is considered to be a target, that is, the possible location of the partner robot. This embodiment uses the DBSCAN algorithm for clustering, and it is recommended to set the parameters as follows: the cluster radius is 0.2 meters, and the number of cluster points is 5.

[0071] The clustering results form a set C p , which is the set of possible positions of the partner robot. p Each element in represents a position point, which is expressed as the midpoint of the polar coordinate system with the millimeter-wave radar carried by the current robot as the pole and the direction of the millimeter-wave radar as the polar axis, expressed as:

[0072]

[0073] Among them, p i For set C p The position of the i-th element in ;

[0074] It is the polar diameter of the position point represented by the i-th element in the polar coordinate system, that is, the distance between this point and the millimeter-wave radar.

[0075] It is the polar angle of the position point represented by the i-th element in the polar coordinate system, that is, the angle between the line connecting this point and the millimeter-wave radar and the polar axis of the polar coordinate system.

[0076] n is the set C p The number of elements in .

[0077] The default orientation of the millimeter-wave radar is consistent with the reference direction of the IMU.

[0078] Then, with the help of dead reckoning information, the clustering results are screened and the position of the second robot is finally determined. This process is achieved through multiple iterations.

[0079] S3: The current robot and the partner robot obtain their respective current positions through dead reckoning, and the current robot receives the current position information of the partner robot to obtain an estimated result of the relative positions of the two.

[0080] In this embodiment, the clustering results are screened with the help of dead reckoning information to finally determine the position of the second robot. This process is implemented through multiple iterations.

[0081] The basic idea is to continuously collect information from several positioning cycles, obtain a set of potential positions of the second robot at the beginning and end of each positioning cycle, and finally construct all possible motion trajectories of the second robot (relative to the first robot) in several iterations.

[0082] At the same time, in each iteration, the moving distance of the second robot is calculated based on the dead reckoning results of the two robots. Each possible motion trajectory of the second robot is scored, and the scoring criteria are the deviation between the moving distance of each segment of the motion trajectory and the actual moving distance of the second robot. The trajectory with the smallest deviation is taken as the actual motion trajectory of the second robot. Based on this, the position of the second robot in several consecutive positioning cycles can be obtained, as well as the relative position of the second robot relative to the first robot at the current moment. Then, combined with the dead reckoning results of the first robot and the second robot, the reference direction deviation angle of the IMU of the two robots can be obtained.

[0083] Both the current robot and the partner robot to be positioned are equipped with IMU modules, which can perform dead reckoning on their respective motion conditions.

[0084] Assuming the robot positioning cycle is ΔT, then at T 0 Time to T 0 +ΔT, the current robot motion (L self ,θ self ), the partner robot can obtain its motion through dead reckoning as (L neighbor ,θ neighbor ).

[0085] Among them, L self is the movement distance of the current robot within ΔT time;

[0086] θ self is the current movement direction of the robot within ΔT time.

[0087] L neighbor is the movement distance of the partner robot within ΔT time;

[0088] θ neighbor is the movement direction of the partner robot within ΔT time.

[0089] Since the IMU reference directions of the two robots are different, the two angles θ self and θ neighbor is relative to different coordinate systems. The deviation angle of these two coordinate systems is the deviation angle of the reference direction of the IMU carried by the two robots, defined as θ error , but this value is unknown. In each beat, the partner robot will change L neighbor and θ neighbor Send to the current robot.

[0090] After the two robots are started, they first enter the initialization phase.

[0091] For two consecutive positioning cycles T0 and T 0 +ΔT, the current robot will get a set of two cluster points and At the same time, the current robot will obtain the distance l moved by the partner robot in this cycle neigkbor .

[0092] Hypothesis set An element in is the position of the partner robot at the beginning of this cycle; An element in is the position of the partner robot at the end of this cycle. With the help of the dead reckoning result of the current robot's own movement in this cycle, the movement distance l of the partner robot in this cycle can be obtained based on the above assumptions. move .

[0093] Applying this assumption to all points in the above two sets, we get the set K:

[0094]

[0095] in, For collection An element of 0 At this moment, a suspected location of the companion robot.

[0096] For collection An element of T at the end of the period 0 At time +ΔT, a suspected position of the partner robot.

[0097] dis(·) is a function that finds the distance between two locations.

[0098] According to l move With l neighbor The absolute value of the difference between the two points is used to sort the elements in the above set in ascending order. The sorting result indicates the credibility of the corresponding point being the position of the partner robot. That is, the higher the sorting, the more likely the position of the two points in the element is that the partner robot is at T. 0 Moment and T O +ΔT time. move -l neighbor The value of | is used as the score, defined as

[0099] The above process is continuously performed for several cycles. In this embodiment, three cycles are performed. In each cycle, a set K is obtained, which are K 1 , K 2 , K 3 The sequence of motion points is constructed as follows:

[0100] From the set K 1 Take any element In the set K 2 Satisfaction An element of Then in the set K 3 Satisfaction An element of The end point of this sequence is Its rating is:

[0101]

[0102] Construct all sequences according to the above method, and take the end point of the sequence with the lowest score as the current position of the partner robot, recorded as p last .

[0103] Based on p last The second to last point p′ in this sequence can be used to obtain the deviation angle of the IMU reference values ​​of the two robots, that is, θ error .

[0104] S4: The current robot obtains the relative position of the partner robot based on the point cloud, real-name distance measurement results and relative position estimation results at the same time.

[0105] After initialization, the algorithm enters the normal operation process. In each cycle, the following operations are performed:

[0106] <1> Calculate p in the current robot's carrier coordinate system last Coordinates:

[0107]

[0108] in, To p last The result of transformation into the robot carrier coordinate system.

[0109] <2> Calculate C p Each element in p last The distance closest to l neighbor The element of is taken as the estimated value of the current relative position of the partner robot and recorded as

[0110] <3> renew

[0111] <4> Update the relative position of the partner robot based on dead reckoning to obtain the estimated value of the partner robot's current relative position, which is recorded as

[0112] Positioning result monitoring and cumulative error elimination:

[0113] When there is an obstacle between the two robots, the positioning results based on UWB and millimeter-wave radar become inaccurate. However, if we rely solely on dead reckoning, there will be cumulative errors in the positioning results. Therefore, we use the dead reckoning results in a short period of time to monitor the positioning results based on UWB and millimeter-wave radar to determine whether there are obstacles between the robots. At the same time, we use the positioning results of UWB and millimeter-wave radar to correct the cumulative errors of dead reckoning, specifically:

[0114] At each beat, calculate and When the deviation is greater than a certain threshold, such as 0.5 meters, it is considered that there is an obstacle between the robots, and the positioning results based on millimeter-wave radar and UWB are no longer accurate. At this time, the result of dead reckoning is used. As a result of relative positioning, the initialization state is entered at the same time.

[0115] If it is considered that there are no obstacles between the robots, As the relative positioning result, that is, the position of the partner robot relative to the current robot. At the same time, the relative positioning result is used to update the deviation θ between the IMU reference directions error .

[0116] After the initialization is completed, the relative position of the second robot with respect to the first robot at the current moment and the reference direction deviation angles of the IMUs carried by the first robot and the second robot can be obtained.

[0117] In each subsequent positioning cycle, a set of potential positions of the second robot can be obtained based on the information fed back by the millimeter wave radar and UWB. Combined with the relative position of the second robot at the end of the previous cycle, the movement distance of the second robot in this cycle represented by this set of potential positions is calculated. According to the actual movement distance of the second robot provided by dead reckoning, the most suitable one is selected from this set of potential positions as an estimated value of the relative position of the second robot

[0118] At the same time, based on the dead reckoning results of the first robot and the second robot, another estimated value of the relative position of the second robot can be obtained.

[0119] When there is an obstacle blocking the robots, the detection of the millimeter wave radar fails, and the distance measurement result of the UWB will also have a large deviation. Based on the above two position estimates, it can be determined whether there is an obstacle blocking the two robots, so as to select the more accurate one as the positioning result.

[0120] when and When the deviation between the robots is small, it can be considered that there is no obstacle blocking the robots. Since the detection accuracy of millimeter-wave radar is high, and dead reckoning has cumulative errors, the results based on millimeter-wave radar detection are used. Output as the relative positioning result of the second robot.

[0121] At the same time, according to the relative positioning results of the second robot in two consecutive shots and the dead reckoning results of the first and second robots, the IMU reference direction deviations of the two robots are recalculated to eliminate the cumulative error.

[0122] when and When the deviation is large, it is considered that there is an obstacle blocking the robots. At this time, the positioning results based on millimeter wave radar and UWB are invalid, and the positioning results based on dead reckoning are used. Output as the relative positioning result of the second robot.

[0123] At the same time, the positioning module re-enters the initialization process. When the initialization is completed and there is still occlusion between the two robots, the deviation between the initialization result and the dead reckoning result will be greater than the threshold, so the module will return to the initialization process. Until the obstacle between the two robots disappears, the deviation between the initialization result and the dead reckoning result will be less than the threshold, and the system will return to the normal positioning stage.

[0124] It should be understood that the embodiments are only used to illustrate the present invention and are not used to limit the scope of the present invention. In addition, it should be understood that after reading the content taught by the present invention, those skilled in the art can make various changes or modifications to the present invention, and these equivalent forms also fall within the scope limited by the appended claims of the application.

Claims

1. A relative positioning method of partner robots based on multi-information fusion, It is characterized in that The following steps are involved: S1: The current robot uses millimeter-wave radar to detect the surrounding environment and obtain the point cloud formed by the reflected signal; S2: The current robot communicates and measures distance with the partner robot through UWB to obtain the real-name distance measurement result of the partner robot; S3: The current robot and the partner robot obtain their respective current positions through dead reckoning, and the current robot receives the current position information of the partner robot; the point cloud is screened and clustered according to the ranging result and error to determine the potential position of the partner robot; information of multiple positioning cycles is collected, and a potential motion trajectory of the partner robot is constructed according to the potential position set, and a score is given according to the deviation between the trajectory moving distance and the actual moving distance, and the trajectory with the smallest deviation is selected to determine the position of the partner robot and the relative position estimation result; S4: The current robot obtains the relative position of the partner robot based on the point cloud, real-name distance measurement results and relative position estimation results at the same time.

2. A relative positioning method for a partner robot based on multi-information fusion according to claim 1, It is characterized in that The current robot and the partner robot are both equipped with a relative positioning module; The relative positioning module includes a millimeter wave radar for detecting the surrounding environment, a corner reflector for enhancing the reflection intensity of the radar signal, a UWB ranging module for relative distance measurement between robots, an inertial sensor for dead reckoning, and a radio communication module.

3. A relative positioning method for a partner robot based on multi-information fusion according to claim 1 or 2, It is characterized in that Filter all points detected by the millimeter-wave radar and only keep those in the area where the partner robot is potentially present; D f =[d|ll ε <d<l+l ε ,d∈D} Where D is the set of all points detected by the millimeter wave radar; l is the distance to the partner robot detected by UWB; l ε is the general empirical error of UWB ranging; D f is the filtered subset; The points retained after screening are clustered using the DBSCAN algorithm; the positions represented by the clustering results are the potential positions of the partner robot; According to the dead reckoning information, the clustering results are screened iteratively for multiple times to finally determine the position of the partner robot.

4. A relative positioning method for a partner robot based on multi-information fusion according to claim 3, It is characterized in that Continuously collect information from several positioning cycles to obtain a set of potential positions of the partner robot at the beginning and end of each positioning cycle, and construct all potential motion trajectories of the partner robot relative to the current robot in several iterations; In each iteration, the partner robot’s moving distance is calculated based on the dead reckoning results of the two robots; Based on the deviation between the moving distance of each segment of the motion trajectory and the actual moving distance of the partner robot, each potential motion trajectory of the partner robot is scored, and the trajectory with the smallest deviation is taken as the actual motion trajectory of the partner robot; based on this, the position of the partner robot in several consecutive positioning cycles can be obtained, as well as the relative position of the partner robot relative to the current robot at the current moment; Combine the dead reckoning results of the current robot and the partner robot to obtain the reference direction deviation angle of the IMUs of the two robots.

5. A relative positioning method for a partner robot based on multi-information fusion according to claim 3, It is characterized in that In each cycle, the following operations are performed: <1> Calculate the partner robot's current position p in the current robot's carrier coordinate system last The coordinates of <2> Calculate the clustering result set C p Each element in p last The distance of the closest partner robot in this cycle is l neighbor The elements of are used as the estimated values ​​of the current relative position of the partner robot and recorded as ; <3> renew <4> Update the relative position of the partner robot based on dead reckoning to obtain the estimated value of the partner robot's current relative position, which is recorded as .

6. A relative positioning method of a partner robot based on multi-information fusion according to claim 5, It is characterized in that calculate and When the deviation is greater than a certain threshold, it is judged that there is an obstacle between the robots. As a result of relative positioning; However, when judging that there are no obstacles between robots, As the relative positioning result, at the same time, the deviation θ between the IMU reference directions is updated using the relative positioning result error .

Images