CN111813121B - Multi-robot formation obstacle avoidance method based on distance-angle priority - Google Patents

Abstract

The invention provides a multi-mobile robot formation obstacle avoidance method based on distance-angle priority, which comprises the following steps of (1) establishing a formation database according to the number of robots; (2) forming a distance-angle priority obstacle avoidance strategy; analyzing the distance between the single-side obstacle and the formation, judging whether the obstacle avoidance is needed, judging which formation is used for obstacle avoidance according to the distance-angle priority if the obstacle avoidance is needed, and calling the formation database in the step (1) to prepare for obstacle avoidance; (3) Designing a motion controller, and starting obstacle avoidance according to the decision-making obstacle avoidance formation; and (4) after obstacle avoidance is finished, rapidly recovering the formation. According to the invention, a formation database is established, the distances between the single-side barriers and the double-side barriers and the robot formation are analyzed, a shortest path obstacle avoidance strategy is obtained, and the motion control rate is designed by using a backstepping method according to the relative pose and the kinematic equation of the follower and the virtual navigator, so that the robot can pass through the barriers and quickly recover the formation while finishing stable formation.

Description

Multi-robot formation obstacle avoidance method based on distance-angle priority

technical field

The invention belongs to the technical field of robots, and in particular relates to a multi-mobile robot formation obstacle avoidance method based on distance-angle priority.

Background technique

At present, multi-mobile robots are widely used in production and life, such as substation inspections, unattended warehouses, automated factory cargo transportation, handling, etc. Through the cooperation between robots, investment costs can be effectively saved and work efficiency can be improved. In the process of multi-machine coordination to complete the task, static or dynamic obstacles are usually encountered, so effective obstacle avoidance is necessary, otherwise the task will fail. At present, there are two commonly used obstacle avoidance methods, one is formation split obstacle avoidance, such as grid method, visualization method, fuzzy logic method, RTT algorithm, genetic algorithm, etc., and the other is formation overall obstacle avoidance, such as manual Potential field method, model predictive control method, fixed formation transformation method and behavior-based method, etc. The overall obstacle avoidance of the formation can maintain the original control within the formation, and online obstacle avoidance can be performed by dynamically adjusting the formation. Compared with split obstacle avoidance, it is more flexible, integrated and reliable, and can effectively avoid split obstacle avoidance There are problems of followers falling behind and collisions between robots, but if there is no effective coordination and communication between multiple machines, the advantages of multi-machine cooperative obstacle avoidance will not be brought into play, causing serious economic losses. For this reason, the Chinese patent "A Fuzzy Formation and Obstacle Avoidance Control Method for Multi-mobile Robot System" (application date: 2013.09.06; application number: 201310402941.3; publication date: 2013.12.18; patent number: CN 103455033 A) was published A method based on fuzzy control that informs all following robots to avoid obstacles in the form of broadcasting by the leading robot, and the follower switches formations according to the stored information of different formations and the information broadcast by the leading robot, but the fuzzy rules are complicated and artificial. The accuracy is difficult to guarantee, and the problem of formation recovery after obstacle avoidance and collision between robots during obstacle avoidance is not considered. The Chinese patent "A method for formulating a multi-UAV formation flight obstacle avoidance control strategy" (application date: 2018.10.29; application number: 201811272488.8; publication date: 2019.03.29; patent number: CN 109542115 A) discloses a repulsion-based The field function formation obstacle avoidance method and the path re-planning strategy after obstacle avoidance ensure real-time obstacle avoidance within the aircraft during multi-aircraft formation missions, but the larger the repulsive force field, the faster the response speed and movement speed of the UAV leaving the trigger area The faster it is, the greater the overshoot of formation change and the lower the stability of the control system.

Aiming at the problems of collision between robots and large amount of overshoot in formation transformation during robot obstacle avoidance, the present invention proposes a distance-angle priority obstacle avoidance strategy. Fully consider the robot's own safety distance and communication range, design a priority obstacle avoidance strategy based on distance and angle, and design a motion controller, so that multiple robots can safely and stably complete obstacle avoidance while improving obstacle avoidance efficiency and making the obstacle avoidance path the shortest .

Contents of the invention

The purpose of the present invention is to provide a multi-robot formation obstacle avoidance method based on distance-angle priority for a two-dimensional plane, so that multiple robots can effectively avoid obstacles online in a complex environment, and at the same time improve work efficiency, making the robot formation overall The total motion path for obstacle avoidance is the shortest.

In order to reduce the excessive dependence of the follower on the leader and avoid the problem of tracking error accumulation caused by the chain structure, a virtual leader-follower structure is adopted. By establishing the formation database, the distance-angle priority strategy is designed by the robot's safe distance, and the obstacle avoidance and collision avoidance are planned, and the motion controller is designed according to the relative pose error state equation of the leader and the follower, so that the robot group can complete the obstacle avoidance Then restore the initial formation and continue to move steadily.

A multi-mobile robot formation obstacle avoidance method based on distance-angle priority, including the following steps:

(1) Establish formation database according to the number of robots;

(2) Form a distance-angle priority obstacle avoidance strategy; analyze the distance between single and bilateral obstacles and the formation to determine whether obstacle avoidance is required, and if necessary, determine which formation to avoid based on the distance-angle priority , and call the formation database in step (1) to prepare for obstacle avoidance;

(3) Design motion controller, start obstacle avoidance according to the obstacle avoidance formation of decision-making in step (2);

(4) After the obstacle avoidance is completed, the last robot passes the obstacle and quickly recovers the formation.

In the establishment of the formation database described in step (1), there are i robots participating in the formation, the virtual leader is denoted as L, its position coordinates are denoted as [x _l y _l θ _l ] ^T , and the followers are denoted as F _i , according to The number of robots is set in the corresponding formation database.

The method of forming the distance-angle priority obstacle avoidance strategy in step (2) is as follows: set the robot’s own safety distance as the radius r _s , then the minimum safe distance for two robots moving side by side is 4r _s , and there are three robot initial states Advance horizontally in a triangular formation;

A unilateral obstacle avoidance strategy

When the virtual pilot robot detects a unilateral obstacle ahead:

(a1) Determine whether obstacle avoidance is required, if d ₀ ≤asinα+ _rs , then obstacle avoidance is required, if d ₀ >asinα+ _rs , then obstacle avoidance is not required;

(a2) Determine the direction of the obstacle, which is located on the upper or lower side of the forward direction. If it is on the upper side, the robot system will maintain the initial formation and turn right to avoid obstacles. If it is on the lower side, the robot system will maintain the initial formation to the left. Turn forward to avoid obstacles;

(a3) When the formation as a whole passes through the obstacle, the formation converges to the horizontal line of the original track and continues to move forward;

B bilateral obstacle avoidance strategy

When the virtual pilot robot detects that there are bilateral obstacles ahead,

(b1) First judge whether obstacle avoidance is required, if d ₀ ≤ _{d l} , then obstacle avoidance is required, if d ₀ >d _l , then obstacle avoidance is not required, and the original formation movement is continued, where d _l = 2asinα+2r _s ;

(b2) If obstacle avoidance is required, determine which formation to avoid the obstacle with;

If d ₀ <2r _s , follow the one-sided obstacle avoidance scheme to avoid obstacles;

If 2r _s <d ₀ <4r _s , it is necessary to change the line formation to pass the obstacle;

If d ₀ >4r _s , there is no need to change the formation, only change the angle α, and pass in a triangular formation like a branch; that is, the value of a remains unchanged, and α is reduced. Small, in order to shorten the obstacle avoidance time, the team forms a proportional reduction to pass the obstacle;

By calculating the single-sided and double-sided obstacles that may appear in the general environment, the distance between the robot's own safety distance and the overall formation and the obstacle is calculated to obtain the distance-angle priority, that is, first judge whether it is necessary to avoid obstacles, and then give priority to it Judging by the distance whether it is necessary to change the formation, and finally determine whether it is necessary to change the angle and which formation to pass; according to the above strategy analysis, the shortest path is the solution with the least consumption;

Step (3) motion controller design, including:

Let the pose of the virtual leader be q _d ＝[x _d y _d θ _d ], u _d ＝[v _d w _d ], and the pose of the following robot be q _i ＝[x _i y _i θ _i ], u _i ＝[v _i w _i ], follow the relative pose equation of robot i and virtual leader:

Among them, Δx=x _{d -xi} , Δy=y _d -y _i ;

The kinematic equation of the robot:

Where x, y, and θ are the position coordinates and heading angles of the two-dimensional coordinate system, respectively, are the derivatives of x, y, and θ, respectively.

Design control rate of linear velocity v and angular velocity w by backstepping method:

v＝v _d cos(p _θid )+k ₁ p _θid (4)

w＝w _d +k ₂ v _d p _yid +k ₃ v _r sin(p _θid ) (5)

Where k ₁ , k ₂ , k ₃ are constants greater than 0, v _d , w _d are ideal linear velocity and angular velocity respectively;

Through the analysis of the relative pose of the follower and the virtual leader, combined with the kinematic equations, the control rate of the linear velocity and angular velocity is obtained, and the actual pose of the robot is adjusted through the control rate to make it infinitely close to the ideal pose to realize the high-precision formation of the robot.

Step (4) fast formation recovery algorithm, including:

In order to improve the working efficiency of multi-robots, when the robot formation as a whole passes through obstacles, it is necessary to restore the formation in time and continue to move forward along the original trajectory. After avoiding obstacles, the virtual leader's pose is p(t)=[x _l y _l θ _l ] ^T , let the formation fast recovery function be:

p(t)＝(p ₀ -p _∞ )e ^-μl +p _∞ (1)

Among them, m>0, p ₀ is the initial value of the virtual leader's position state after obstacle avoidance, p _∞ is the steady-state value of p(t), that is, the position state after the formation is restored, and p(t) is indexed Decrease quickly to the value of p _∞ , the formation can be quickly restored to the formation in exponential form.

The present invention aims at the problems of low robot formation obstacle avoidance efficiency, large formation overshoot, and low control precision. By establishing a formation database, the present invention analyzes the distance between unilateral obstacles and bilateral obstacles and robot formations, and obtains The shortest path obstacle avoidance strategy. Finally, according to the relative pose and kinematic equations of the follower and the virtual leader, use the backstepping method to design the motion control rate, so that the robot can safely and efficiently pass through obstacles and quickly recover the formation while completing a stable formation. .

Description of drawings

Fig. 1 is the schematic diagram of embodiment inline formation position;

Fig. 2 is the schematic diagram of embodiment triangular formation position;

Fig. 3 is a schematic diagram of an embodiment avoiding upper obstacles;

Fig. 4 is a schematic diagram of an embodiment avoiding lower obstacles;

Fig. 5 is the schematic diagram of avoiding obstacles in the triangular formation of tree branch;

Fig. 6 is a schematic flow chart of the present invention.

Detailed ways

The specific technical solutions of the present invention are described in conjunction with the examples.

As shown in Figure 6, the multi-mobile robot formation obstacle avoidance method based on distance-angle priority includes the following steps:

(1) Establish formation database according to the number of robots;

(2) Analyze the distance between the single and bilateral obstacles and the formation to determine whether obstacle avoidance is required, and if so, determine which formation to avoid based on the distance-angle priority, and call the team in step (1) Shape database for obstacle avoidance preparation;

(3) Design motion controller, start obstacle avoidance according to the obstacle avoidance formation of decision-making in step (2);

(4) After the obstacle avoidance is completed, the last robot passes the obstacle and quickly recovers the formation.

The described establishment formation database of step (1):

There are i robots participating in the formation, the virtual leader is denoted as L, its position coordinates are denoted as [ _x ly _l θ _l ] ^T , and the followers are denoted as F _i , and the corresponding formation database is set according to the number of robots. The commonly used The formations are: a font as shown in Figure 1, a triangle as shown in Figure 2, a herringbone, a rectangle, a regular polygon, a circle, etc.

Taking three robots as an example, the formation is expressed in matrix form as shown in Table 1 and Table 2:

Table 1 The position coordinates of each follower in the inline formation

Table 2 The position coordinates of each follower in the triangle formation

in And α<β, a> _2rs and a<C _l , where C _l is the maximum communication distance, β is the maximum communication angle, and rs _is the radius of the safe distance of the robot itself.

For the convenience of understanding, the specific expression takes three robots as an example, and other numbers of robots are also applicable, just calculate the distance and angle according to the current formation.

The method for forming distance-angle priority obstacle avoidance strategy in step (2) is:

Assuming that the robot's own safety distance is the radius r _s , the minimum safe distance for two robots moving side by side is 4r _s , and the initial state of three robots is a triangular formation moving along the horizontal direction, as shown in Figure 2.

A. Unilateral obstacle avoidance strategy

When the virtual pilot robot detects a unilateral obstacle ahead:

(a1) Determine whether obstacle avoidance is required, if d ₀ ≤asinα+ _rs , then obstacle avoidance is required, if d ₀ >asinα+ _rs , then obstacle avoidance is not required;

(a2) Determine the direction of the obstacle, which is located on the upper or lower side of the forward direction, as shown in Figure 3. If it is the upper side, the robot system maintains the initial formation and turns right to move forward to avoid obstacles, as shown in Figure 4. If it is the lower side, Then the robot system maintains the initial formation and turns left to avoid obstacles;

(a3) When the formation as a whole passes through the obstacle, the formation converges to the horizontal line of the original track and continues to move forward;

B. Bilateral obstacle avoidance strategy

When the virtual pilot robot detects that there are bilateral obstacles ahead,

(b1) First judge whether obstacle avoidance is required, if d ₀ ≤ _{d l} , then obstacle avoidance is required, if d ₀ >d _l , then obstacle avoidance is not required, and the original formation movement is continued, where d _l = 2asinα+2r _s ;

(b2) If obstacle avoidance is required, determine which formation to avoid the obstacle with;

If d ₀ <2r _s , follow the one-sided obstacle avoidance scheme to avoid obstacles;

If 2r _s <d ₀ <4r _s , it is necessary to change the line formation to pass the obstacle;

If d ₀ >4r _s , there is no need to change the formation, only change the angle α, and pass in a triangular formation like a tree branch, as shown in Figure 5; that is, the value of a remains unchanged, and α is reduced. If the obstacle moves along the robot The along-surface obstacle distance in the direction is small, in order to shorten the obstacle avoidance time, the team forms a proportional reduction to pass the obstacle.

By calculating the single-sided and double-sided obstacles that may appear in the general environment, the distance between the robot's own safety distance and the overall formation and the obstacle is calculated to obtain the distance-angle priority, that is, first judge whether it is necessary to avoid obstacles, and then give priority to it The distance judges whether it is necessary to change the formation, and finally decides whether it is necessary to change the angle and which formation to pass. According to the above strategy analysis, the shortest path is the solution with the least consumption.

Step (3) motion controller design:

Let the pose of the virtual leader be q _d ＝[x _d y _d θ _d ], u _d ＝[v _d w _d ], and the pose of the following robot be q _i ＝[x _i y _i θ _i ], u _i ＝[v _i w _i ], we have to follow the relative pose equation of robot i and virtual navigator:

Wherein, Δx=x _{d -xi} , Δy=y _d -y _i .

The kinematic equation of the robot:

Where x, y, and θ are the position coordinates and heading angles of the two-dimensional coordinate system, respectively, are the derivatives of x, y, and θ, respectively.

Design control rate of linear velocity v and angular velocity w by backstepping method:

v＝v _d cos(p _θid )+k ₁ p _θid (4)

w＝w _d +k ₂ v _d p _yid +k ₃ v _r sin(p _θid ) (5)

Where k ₁ , k ₂ , k ₃ are constants greater than 0, v _d , w _d are ideal linear velocity and angular velocity, respectively.

Through the analysis of the relative pose of the follower and the virtual leader, combined with the kinematic equations, the control rate of the linear velocity and angular velocity is obtained, and the actual pose of the robot is adjusted through the control rate to make it infinitely close to the ideal pose to realize the high-precision formation of the robot.

Step (4) formation fast recovery algorithm:

In order to improve the working efficiency of multi-robots, when the robot formation as a whole passes through the obstacle, it needs to restore the formation in time, continue to move forward along the original trajectory, and set the virtual leader's pose after obstacle avoidance as p(t)=[x _l y _l θ _l ] ^T , let the formation fast recovery function be:

p(t)＝(p ₀ -p_ゥ)e ^-mt +p (1)

Among them, m>0, p ₀ is the initial value of the virtual leader's position state after obstacle avoidance, p _∞ is the steady-state value of p(t), that is, the position state value after the formation is restored, and p(t) is indexed Decrease quickly to the value of p _∞ , the formation can be quickly restored to the formation in exponential form.

Claims (1)

1. The multi-mobile robot formation obstacle avoidance method based on distance-angle priority is characterized in that, comprising the following steps: (1) Establish the formation database according to the number of robots; in step (1) to establish the formation database, there are i robots participating in the formation, the virtual leader is expressed as L, and its position coordinates are expressed as [x _l y _l θ _l ] ^T , the follower is represented as F _i , and the corresponding formation database is set according to the number of robots; (2) Form a distance-angle priority obstacle avoidance strategy; analyze the distance between single and bilateral obstacles and the formation to determine whether obstacle avoidance is required, and if necessary, determine which formation to avoid based on the distance-angle priority , and call the formation database in step (1) to prepare for obstacle avoidance; in step (2), the method of forming a distance-angle priority obstacle avoidance strategy is as follows: set the robot’s own safe distance as the radius r _s , then the two robots The minimum safety distance for side-by-side movement is 4r _s ; A unilateral obstacle avoidance strategy When the virtual pilot robot detects a unilateral obstacle ahead: (a1) Determine whether obstacle avoidance is required, if d ₀ ≤ asinα+ _rs , then obstacle avoidance is required, if d ₀ >asinα+ _rs , then obstacle avoidance is not required; (a2) Determine the direction of the obstacle, which is located on the upper or lower side of the forward direction. If it is on the upper side, the robot system will maintain the initial formation and turn right to avoid obstacles. If it is on the lower side, the robot system will maintain the initial formation to the left. Turn forward to avoid obstacles; (a3) When the formation as a whole passes through the obstacle, the formation converges to the horizontal line of the original track and continues to move forward; B bilateral obstacle avoidance strategy When the virtual pilot robot detects that there are bilateral obstacles ahead, (b1) First judge whether to avoid obstacles, if d ₀ ≤ _{d l} , then need to avoid obstacles, if d ₀ >d _l , then do not need to avoid obstacles, and continue to maintain the original formation movement, where d _l = 2asinα+2r _s ; (b2) If obstacle avoidance is required, determine which formation to avoid the obstacle with; If d ₀ <2r _s , follow the one-sided obstacle avoidance scheme to avoid obstacles; If 2r _s <d ₀ <4r _s , it is necessary to change the formation to pass the obstacle; If d ₀ >4r _s , there is no need to change the formation, only change the angle, and pass in a tree-like triangular formation; that is, the value of a remains unchanged, and α is reduced; if the obstacle distance along the robot's moving direction is small , in order to shorten the obstacle avoidance time, the team forms a proportional reduction to pass the obstacle; By calculating the single-sided and double-sided obstacles that may appear in the general environment, the distance between the robot's own safety distance and the overall formation and the obstacle is calculated to obtain the distance-angle priority, that is, first judge whether it is necessary to avoid obstacles, and then give priority to it Judging by the distance whether it is necessary to change the formation, and finally determine whether it is necessary to change the angle and which formation to pass; according to the above strategy analysis, the shortest path is the solution with the least consumption; (3) Design motion controller, start obstacle avoidance according to the obstacle avoidance formation of decision-making in step (2); Step (3) motion controller design, include: Let the pose of the virtual leader be q _d ＝[x _d y _d θ _d ], u _d ＝[v _d w _d ], and the pose of the following robot be q _i ＝[x _i y _i θ _i ], u _i ＝[v _i w _i ], follow the relative pose equation of robot i and virtual leader: Among them, Δx=x _{d -xi} , Δy=y _d -y _i ; The kinematic equation of the robot: Among them, x, y, and θ are the position coordinates and heading angles of the two-dimensional coordinate system, respectively. are the derivatives of x, y, and θ respectively; Design control rate of linear velocity v and angular velocity w by backstepping method: v＝v _d cos(p _θid )+k ₁ p _θid (4) w＝w _d +k ₂ v _d p _yid +k ₃ v _r sin(p _θid ) (5) Where k ₁ , k ₂ , k ₃ are constants greater than 0, v _d , w _d are ideal linear velocity and angular velocity respectively; Through the analysis of the relative pose of the follower and the virtual leader, combined with the kinematic equations, the control rate of linear velocity and angular velocity is obtained, and the actual pose of the robot is adjusted through the control rate to make it infinitely close to the ideal pose, so as to realize the high-precision formation of robots; (4) After the obstacle avoidance is completed, the last robot passes the obstacle and quickly recovers the formation; step (4) the formation rapid recovery algorithm, including: In order to improve the work efficiency of multi-robots, when the robot formation as a whole passes through obstacles, it is necessary to restore the formation in time and continue to move forward along the original trajectory. Set the virtual leader's pose after obstacle avoidance to [x _l y _l θ _l ] ^T , and set The quick formation recovery function is: p(t)＝(p ₀ -p _∞ )e ^-mt +p _∞ (1) Among them, m>0, p ₀ is the initial value of the virtual leader's position state after obstacle avoidance, p _∞ is the steady-state value of p(t), that is, the position state value after the formation is restored, and p(t) is indexed Decrease quickly to the value of p _∞ , the formation can be quickly restored to the formation in exponential form.