Cooperative collision avoidance and deadlock resolution method for multiple mobile robots

Abstract

The invention relates to the field of robot deadlock escape technologies, in particular to a multi-mobile-robot cooperative collision avoidance and deadlock resolution method, which comprises the following steps: collecting sensed obstacle information and current state information of mobile robots; constructing a mobile robot model, and converting the mobile robot model into a corresponding affine control form; building an obstacle avoidance constraint of the mobile robot according to the current state information of the mobile robot and the obstacle information; determining a target function; constructing a QP optimization formula based on the obstacle avoidance constraint of the mobile robot, the objective function and the physical constraint of the control quantity; the QP optimization formula is solved to obtain the optimal control quantity of the mobile robot, the optimal control quantity is used for controlling the mobile robot to move, a deadlock resolution method is used as an auxiliary speed vector to be added to a trajectory tracking method, a deadlock detection method is associated with a Lagrange product, the mobile robot can escape deadlock in a reasonable path, and the trajectory tracking accuracy is improved. And the energy consumption is reduced.

Description

Technical Field

[0001] This application relates to the field of robot deadlock escape technology, and in particular to a method for multi-mobile robot collaborative collision avoidance and deadlock resolution. Background Art

[0002] In recent years, the collision avoidance control method based on the control barrier function, i.e., CBF, has been widely used in multi-mobile robot collaborative collision avoidance control. This method realizes multi-objective control under the background of quadratic programming, i.e., QP, and provides strict safety guarantees at the same time. However, for this type of method, when the target position of the mobile robot is collinear with the obstacle, the mobile robot will fall into a deadlock state.

[0003] In terms of deadlock detection, some studies handle deadlock conflicts after the deadlock occurs, such as the two articles "Safety barrier certificates for collisions-free multirobot systems" and "Simultaneous position and orientation planning of nonholonomic multi-robot systems: A dynamic vector field approach". Although effective, this kind of handling will cause the mobile robot to have a nearly zero speed and a slow reaction speed.

[0004] In terms of deadlock resolution, some studies escape from the deadlock by swapping the positions of the mobile robots, such as the article "The before, during, and after of multi-robot deadlock". However, using this strategy, the mobile robot will inevitably stagnate for a period of time.

[0005] There are also some studies that break the equilibrium state by perturbing the QP controller, but few studies consider the rationality of the deadlock escape direction. Using the right-hand rule is not always reasonable, and for collision avoidance with large detours, the energy consumption of the mobile robot is relatively large. Summary of the Invention

[0006] In order to enable the mobile robot to smoothly resolve the deadlock while having a fast reaction speed and moving along a reasonable path to reduce the energy consumption of the mobile robot, this application provides a method for multi-mobile robot collaborative collision avoidance and deadlock resolution.

[0007] The method for multi-mobile robot collaborative collision avoidance and deadlock resolution provided by this application adopts the following technical solutions.

[0008] A method for cooperative collision avoidance and deadlock resolution of multiple mobile robots, specifically including the following steps.

[0009] Step 1: Collect the perceived obstacle information and the current state information of the mobile robot.

[0010] Step 2: Construct a mobile robot model and convert the mobile robot model into the corresponding affine control form.

[0011] Step 3: Construct an obstacle avoidance constraint for the mobile robot according to the current state information and obstacle information of the mobile robot.

[0012] Step 4: Determine the objective function.

[0013] Step 5: Based on the obstacle avoidance constraint, objective function and physical constraint of the control quantity of the mobile robot, construct a QP optimization formula.

[0014] Step 6: Solve the QP optimization formula to obtain the optimal control quantity of the mobile robot, and use the optimal control quantity to control the movement of the mobile robot.

[0015] In Step 5, the deadlock resolution method is attached to the trajectory tracking method as an auxiliary velocity vector, and the deadlock detection method is associated with the Lagrange multiplier.

[0016] By adopting the above technical solution, trajectory tracking and safety control are achieved simultaneously, the deadlock is quickly escaped, and the mobile robot turns with the minimum attitude angle, avoiding large detours, while ensuring a relatively fast response speed of the mobile robot and minimizing energy consumption as much as possible.

[0017] Optionally, in Step 4, the objective function is to minimize the deviation between the actual path and the desired trajectory of the mobile robot, then the trajectory tracking strategy of the mobile robot is: Trajectory tracking strategy:

[0018] ;

[0019] ;

[0020] Wherein, is the desired acceleration of the mobile robot , is the desired velocity of the mobile robot , is the desired position of the mobile robot , is the current position of the mobile robot , and are trajectory tracking control parameters, is the control input, To adjust the mobile robot from the amplitude of deviation from the desired trajectory, for the mobile robot is the rotation matrix, A i for the mobile robot is the Jacobian matrix, is the first derivative.

[0021] By adopting the above technical solution, the deadlock resolution technology is attached to the trajectory tracking strategy.

[0022] Optionally, the piecewise function is:

[0023] ;

[0024] wherein, is the deadlock detection condition, represents the Lagrange multiplier factor related to the mobile robot .

[0025] By adopting the above technical solution, the deadlock detection method is associated with the Lagrange multiplier.

[0026] Optionally, the is specifically:

[0027] ;

[0028] wherein, , determines the direction of deviation of the mobile robot from the desired trajectory.

[0029] By adopting the above technical solution, the direction of deviation of the mobile robot from the desired trajectory can be adjusted automatically, reducing the inconvenience of manual intervention.

[0030] Optionally, the piecewise function is:

[0031] ;

[0032] ;

[0033] ;

[0034] ;

[0035] wherein, represents the sign function, , , is related to the mobile robot The set of all adjacent mobile robots, denotes the mobile robot and the adjacent mobile robot the phase angle between them, denotes the mobile robot the abscissa, denotes the mobile robot the ordinate, denotes the mobile robot the abscissa, denotes the mobile robot the ordinate.

[0036] By adopting the above technical solution, the mobile robot can always avoid obstacles with the minimum attitude angle, avoid large detours and save energy.

[0037] Optionally, the QP optimization formula is:

[0038] ;

[0039] where, is the minimum value of the control input, is the maximum value of the control input, is the obstacle avoidance constraint.

[0040] By adopting the above technical solution, the control input is restricted within a reasonable range, which can prevent the mobile robot from having excessive behaviors.

[0041] Optionally, the affine control form of the mobile robot in step two is:

[0042] ;

[0043] where, is the wheel speed of the mobile robot , is the right wheel speed of the mobile robot is the left wheel speed of the mobile robot is the control input, is the mobile robot the Jacobian matrix, is the first derivative of is the mobile robot the current position, is the first derivative of is the mobile robot​​​ The speed at the current moment is the first derivative of

[0044] By adopting the above technical solution, the design of the controller is simplified, the control accuracy and response speed are improved, and the stability during the movement of the mobile robot is enhanced.

[0045] Optionally, the safety set between the mobile robot and the adjacent mobile robot is as follows:

[0046] ;

[0047] ;

[0048] ;

[0049] ;

[0050] wherein, is the relative distance between the mobile robot and the adjacent mobile robot , is the current position of the mobile robot , is the current position of the mobile robot , is the relative speed between the mobile robot and the adjacent mobile robot , is the speed at the current moment of the mobile robot , is the speed at the current moment of the mobile robot , is the Euclidean distance, is the safety threshold, is the control parameter.

[0051] By adopting the above technical solution, there is no need to introduce a braking distance as in the traditional method, which helps to improve the reaction speed of the mobile robot and can avoid collisions between mobile robots in some cases.

[0052] Optionally, the obstacle avoidance constraint of the mobile robot is as follows:

[0053] ;

[0054] ;

[0055] ;

[0056] Among them, is the control parameter.

[0057] By adopting the above technical solution, an obstacle avoidance constraint is constructed between adjacent mobile robots.

[0058] In summary, the present application at least includes the following beneficial effects.

[0059] 1. Attach the deadlock resolution method to the trajectory tracking method as an auxiliary velocity vector. The detection method is related to the Lagrange product. While achieving trajectory tracking and safety control simultaneously, it quickly escapes from the deadlock and the mobile robot turns with the minimum attitude angle, avoiding large detours;

[0060] 2. Each mobile robot separately solves a QP optimization formula, avoiding centralized calculation and greatly improving the solution speed;

[0061] 3. Adjust so that the direction in which the mobile robot deviates from the desired trajectory can be self-adjusted, reducing the inconvenience of manual intervention;

[0062] 4. The uniquely designed safety set does not require introducing a braking distance as in traditional methods, which helps to improve the reaction speed of the mobile robot. BRIEF DESCRIPTION OF THE DRAWINGS

[0063] Figure 1 is a flowchart of a method for multi-mobile robot cooperative collision avoidance and deadlock resolution of the present application. DETAILED DESCRIPTION OF THE EMBODIMENTS

[0064] The following further describes the present application in detail with reference to the accompanying drawings.

[0065] The embodiments of the present application disclose a method for multi-mobile robot cooperative collision avoidance and deadlock resolution. Referring to Figure 1 , it specifically includes the following steps.

[0066] Step 1: Collect the perceived obstacle information and the current state information of the mobile robot.

[0067] In this embodiment, for the mobile robot , all other adjacent mobile robots perceived at its current moment are regarded as obstacles, and . The current state information of the mobile robot includes the position information, that is, , is the abscissa of the mobile robot at the current moment, is the mobile robot at the current moment The ordinate; speed information, i.e., ; orientation angle information, i.e., .

[0068] Adjacent to the mobile robot The mobile robot , , is the set of all mobile robots adjacent to the mobile robot , and its current state information includes position information, i.e., , is the abscissa of the mobile robot at the current moment, is the ordinate of the mobile robot at the current moment; speed information, i.e., ; orientation angle information, i.e., .

[0069] Step 2: Construct a mobile robot model and convert the mobile robot model into a corresponding affine control form.

[0070] The kinematic model of the mobile robot is:

[0071] ;

[0072] where, represents the wheel radius of the mobile robot , represents the wheel axle spacing of the mobile robot , represents the right wheel speed of the mobile robot , represents the left wheel speed of the mobile robot , is the first derivative of is the first derivative of is the first derivative of.

[0073] Using the feedback linearization technique, the above model is converted into the following affine control form:

[0074] ;

[0075] where, is the wheel speed of the mobile robot , , is the control input for controlling the mobile robot The moving input signal or instruction, For the mobile robot The Jacobian matrix of, Is The first derivative of, Is The first derivative of, Is The first derivative of.

[0076] The specific form is as follows:

[0077] ;

[0078] Wherein, For the mobile robot The distance from the center of the rear axle to the center of mass.

[0079] Step 3: Construct an obstacle avoidance constraint for the mobile robot according to the current state information and obstacle information of the mobile robot. The obstacle avoidance constraint is used to restrict the mobile robot from colliding with obstacles.

[0080] The mobile robot And the adjacent mobile robot The safety set between is as follows:

[0081] ;

[0082] ;

[0083] ;

[0084] ;

[0085] Wherein, For the mobile robot And the adjacent mobile robot The relative distance between, For the mobile robot The current position of, For the mobile robot The current position of, For the mobile robot And the adjacent mobile robot The relative speed between, For the mobile robot The speed at the current moment of, For the mobile robot The speed at the current moment of, Is the Euclidean distance, Is the safety threshold, Is the control parameter.

[0086] To achieve , based on the annihilating CBF, the following can be obtained:

[0087] ;

[0088] wherein, is the first derivative of, and is the control parameter.

[0089] Therefore, the obstacle avoidance constraint between the mobile robot and the adjacent mobile robot is:

[0090] ;

[0091] By evenly distributing the collision avoidance responsibility, the following two conditional formulas are obtained.

[0092] ;

[0093] ;

[0094] Then the following definitions are made. Let:

[0095] ;

[0096] ;

[0097] Then for the mobile robot , the obstacle avoidance constraint can be expressed in the following form:

[0098] ;

[0099] Step Four: Determine the objective function, with the minimum deviation between the actual path of the mobile robot and the desired trajectory as the objective function.

[0100] Step Five: Based on the obstacle avoidance constraint, objective function, and physical constraint of the control quantity of the mobile robot, construct a QP optimization formula.

[0101] The trajectory tracking strategy of a general mobile robot is:

[0102] ;

[0103] In this application, however, the deadlock resolution technique is attached to the trajectory tracking strategy, and the above formula is modified to:

[0104] ;

[0105] ;

[0106] Among them, is the desired acceleration of the mobile robot , is the desired velocity of the mobile robot , is the desired position of the mobile robot , is the current position of the mobile robot , and are trajectory tracking control parameters and their values need to be greater than zero, is the amplitude for adjusting the deviation of the mobile robot from the desired trajectory, is the rotation matrix of the mobile robot .

[0107] And, the piecewise function of

[0108] ;

[0109] Among them, is the deadlock detection condition, represents the Lagrange multiplier factor related to the mobile robot .

[0110] And, specifically:

[0111] ;

[0112] Among them, , determines the direction of the deviation of the mobile robot from the desired trajectory. When is equal to -1, the mobile robot deflects to the right; when is equal to 1, the mobile robot deflects to the left.

[0113] Among them, the piecewise function of

[0114] ;

[0115] ;

[0116] ;

[0117] ;

[0118] Among them, represents the sign function, , represents the mobile robot and the adjacent mobile robot the phase angle between them, represents the abscissa of the mobile robot ; represents the ordinate of the mobile robot ; represents the abscissa of the mobile robot ; represents the ordinate of the mobile robot ;

[0119] Finally, the QP optimization formula integrating trajectory tracking, obstacle avoidance, and physical constraints of control quantities is:

[0120] ;

[0121] wherein, is the minimum value of the control input, is the maximum value of the control input, represents the physical constraint of the control quantity.

[0122] Step Six: Solve the QP optimization formula to obtain the optimal control quantity of the mobile robot, and control the movement of the mobile robot with the optimal control quantity.

[0123] The implementation principle of a multi-mobile robot cooperative collision avoidance and deadlock resolution method in an embodiment of this application is: attaching the deadlock resolution method to the trajectory tracking method as an auxiliary velocity vector, the detection method is related to the Lagrange product, while achieving trajectory tracking and safety control, quickly escaping from deadlock and the mobile robot turning with the minimum attitude angle, avoiding large detours.

[0124] The above are all preferred embodiments of this application. Without limiting the protection scope of this application accordingly, therefore: All equivalent changes made according to the structure, shape, and principle of this application should be covered within the protection scope of this application.

Claims

1. A method for multi-mobile robot collaborative collision avoidance and deadlock resolution, characterized in that: Specifically, it includes the following steps: Step 1: Collect the perceived obstacle information and the current state information of the mobile robot; Step 2: Construct a mobile robot model and convert the mobile robot model into a corresponding affine control form; Step 3: Construct an obstacle avoidance constraint for the mobile robot according to the current state information and obstacle information of the mobile robot; Step 4: Determine the objective function; Step 5: Based on the obstacle avoidance constraint, objective function and physical constraint of the control quantity of the mobile robot, construct a QP optimization formula; Step 6: Solve the QP optimization formula to obtain the optimal control quantity of the mobile robot, and use the optimal control quantity to control the movement of the mobile robot; In Step 5, the deadlock resolution method is attached to the trajectory tracking method as an auxiliary velocity vector, and the deadlock detection method is associated with the Lagrange product.

2. A method for multi-mobile robot collaborative collision avoidance and deadlock resolution according to claim 1, characterized in that: In the fourth step, taking the minimum deviation between the actual path of the mobile robot and the desired trajectory as the objective function, the mobile robot The trajectory tracking strategy is as follows: ； ； wherein, is the desired acceleration of the mobile robot ; is the desired velocity of the mobile robot ; is the desired position of the mobile robot ; is the current position of the mobile robot ; and are trajectory tracking control parameters ; is the control input ; is the amplitude for adjusting the deviation of the mobile robot from the desired trajectory i ; is the Jacobian matrix of the mobile robot ; is the first derivative of 3. A multi-mobile robot collaborative collision avoidance and deadlock resolution method according to claim 2, characterized in that: The said piecewise function is as follows: ； Among them, is the deadlock detection condition, represents the Lagrange multiplier factor related to the mobile robot.

4. A method for multi-mobile robot collaborative collision avoidance and deadlock resolution according to claim 2, characterized in that: The said Specifically: ； Among them, , determines the direction in which the mobile robot deviates from the desired trajectory.

5. A multi-mobile robot collaborative collision avoidance and deadlock resolution method according to claim 4, characterized in that: The said piecewise function is as follows: ； ； ； ； Among them, represents the sign function, , , is the set of all mobile robots adjacent to the mobile robot . represents the phase angle between the mobile robot and the adjacent mobile robot . represents the abscissa of the mobile robot . represents the ordinate of the mobile robot . represents the abscissa of the mobile robot . represents the ordinate of the mobile robot .

6. A method for multi-mobile robot collaborative collision avoidance and deadlock resolution according to claim 5, characterized in that: The QP optimization formula is as follows: ； Among them, is the minimum value of the control input, is the maximum value of the control input, is the obstacle avoidance constraint.

7. A method for multi-mobile robot collaborative collision avoidance and deadlock resolution according to claim 1, characterized in that: The mobile robot in the second step has an affine control form as follows: ； Among them, is the wheel speed of the mobile robot . , is the right wheel speed of the mobile robot . is the left wheel speed of the mobile robot . is the control input, is the Jacobian matrix of the mobile robot . is the first derivative of is the current position of the mobile robot . is the first derivative of is the current moment speed of the mobile robot , is the first derivative of 8. A multi-mobile robot collaborative collision avoidance and deadlock resolution method according to claim 7, characterized in that: The mobile robot in step three and the adjacent mobile robot The safety set between them is as follows: ； ； ； ； Wherein, is the mobile robot and the adjacent mobile robot the relative distance between them, is the mobile robot the current position of, is the mobile robot the current position of, is the mobile robot and the adjacent mobile robot the relative speed between them, is the mobile robot the speed at the current moment, is the mobile robot the speed at the current moment, is the Euclidean distance, is the safety threshold, is the control parameter.

9. A multi-mobile robot collaborative collision avoidance and deadlock resolution method according to claim 8, characterized in that: The mobile robot has the following obstacle avoidance constraints: ； ； ； Among them, is a control parameter.

Images