Reactive robot motion deadlock avoiding method and device

Abstract

The invention belongs to the technical field related to robot motion control, and discloses a reaction type robot motion deadlock avoiding method and device, and the method comprises the steps: (1) building a collision avoidance constraint used for guaranteeing that a robot does not collide with an environment object based on a control barrier function; a deformable control Lyapunov function is utilized to construct a convergence constraint for ensuring the robot to move to a target; (2) establishing a robot collision avoidance motion optimization model by taking minimum deviation between the robot and model control input as an objective function based on collision avoidance constraint, convergence constraint conditions and a robot dynamic model; and (3) detecting the motion deadlock risk of the robot in real time based on the robot collision avoidance motion optimization model, and regulating and controlling the shape parameters of the deformable control Lyapunov function based on the obtained deadlock type so as to avoid the motion deadlock of the robot. According to the method, deadlock is avoided by regulating and controlling the deformable control Lyapunov function, the method can depend on a prior environment model or a fixed rule, and generalization is high.

Description

Technical Field

[0001] The present invention belongs to the technical field related to robot motion control, and more specifically, relates to a reactive robot motion deadlock avoidance method and device. Background Art

[0002] With the rapid development of robotics technology, mobile robots have been widely used in warehousing and logistics, service distribution, industrial manufacturing, and home services. In these dynamic, unstructured and complex environments, robots need to perceive the environment in real time and efficiently complete autonomous obstacle avoidance movements, which places extremely high demands on the robustness and real-time performance of motion control algorithms. At present, mainstream obstacle avoidance methods (such as dynamic window method, potential field method, path planning based on reinforcement learning, etc.) mostly use reactive control strategies, which can quickly generate local motion instructions based on sensor information. However, in scenes with dense obstacles or when multiple robots are working together, the robot may fall into a "motion deadlock" state due to local environmental constraints, which manifests as repeated oscillations, looping in place, or long-term stagnation, resulting in mission failure or even system paralysis.

[0003] In the existing technology, the solutions to the deadlock problem still have significant deficiencies. Some methods identify deadlocks by detecting the path repeatability of the robot's motion trajectory, but such static rules are prone to misjudgment or omission under the interference of dynamic obstacles; other methods rely on global path replanning or preset environmental maps, which are difficult to adapt to unknown environments or real-time changing scenarios, and have high computational complexity. In addition, traditional deadlock avoidance strategies (such as random perturbations and backtracking paths) lack targeted analysis of the causes of deadlocks, which may cause the robot to fall into the same state again after escaping from the deadlock, or cause additional motion energy consumption and time costs. In complex and changeable practical applications, existing methods often face the following challenges:

[0004] Deadlock detection hysteresis: Methods based on post-event trajectory analysis have difficulty in capturing deadlock triggering conditions in a timely manner, resulting in delayed response;

[0005] Poor environmental adaptability: relying on prior environmental models or fixed rules, it is difficult to cope with sudden interference from dynamic obstacles or complex geometric constraints;

[0006] Insufficient strategy generalization ability: Different deadlock types such as symmetric deadlock and local minimum are not distinguished, and the obstacle avoidance strategy is single and has limited flexibility;

[0007] High computing resource usage: Global replanning or high-dimensional state search leads to reduced real-time performance of the algorithm, making it difficult to deploy on embedded systems. Summary of the invention

[0008] In view of the above defects or improvement requirements of the prior art, the present invention provides a method and device for avoiding reactive robot motion deadlocks, aiming to solve the problems of current robot deadlock detection lag and poor environmental adaptability.

[0009] To achieve the above object, according to one aspect of the present invention, a method for avoiding reactive robot motion deadlocks is provided. The method includes the following steps:

[0010] (1) Construct a number of control barrier functions based on the state of each robot itself and environmental object information, and build a collision avoidance constraint for ensuring that the robot does not collide with environmental objects based on the obtained control barrier functions; at the same time, construct a deformable control Lyapunov function based on the state information of each robot itself and the task target, and use the deformable control Lyapunov function to construct a convergence constraint for ensuring that the robot moves to the target;

[0011] (2) Based on the collision avoidance constraint and the convergence constraint conditions, combined with the robot dynamics model, establish a robot collision avoidance motion optimization model with the minimum deviation between the robot and the model control input as the objective function;

[0012] (3) Based on the robot collision avoidance motion optimization model, real-time detect the risk of robot motion deadlocks, and based on the obtained deadlock type, adjust the shape parameters of the deformable control Lyapunov function, thereby achieving the avoidance of robot motion deadlocks.

[0013] Further, a collision avoidance constraint is constructed using the signed distance function between the robot and the obstacle.

[0014] Further, the deformable control Lyapunov function is in the form of a quadratic form of the control error e.

[0015] Further, the expression of the deformable control Lyapunov function after parameters is V(e) = e T QSQ T e, where Q is a rotation matrix representing the rotation state of the control Lyapunov function; S is a scaling matrix representing the scaling ratio in each direction of the control Lyapunov function.

[0016] Further, the shape of the deformable control Lyapunov function can be controlled by the generalized angular velocity ω and the scaling rate and its shape dynamics is expressed as

[0017] Further, the expression of the robot collision avoidance motion optimization model is:

[0018]

[0019] s.t.L f V(x)+Lg V(x)u + γ(V(x)) ≤ δ

[0020] L f h i (x) + L g h i (x)u + α i (h i (x)) ≥ 0, i ∈ {1, 2, …, M}

[0021] In the formula, the motion model of the robot is characterized as x represents the robot control state, u represents the motion control input of the robot, f represents the system transfer matrix, g represents the system input matrix, represents the model control input of the robot, V(x) represents the deformable control Lyapunov function, h i (x) represents the CBF function obtained by the robot and the surrounding obstacle o i through SDF calculation, γ, α i are respectively class functions and extended class functions, δ is a slack variable, p is a constant coefficient, L f V, L f h i , L g V, L g h i are respectively the simplified representations of the expressions .

[0022] Furthermore, based on the parameters of the robot collision avoidance motion optimization model, the current convergence force and safety force vectors of the robot are calculated, and the deadlock risk of the robot is evaluated based on the convex cone relationship formed by the convergence force and the safety force.

[0023] Furthermore, if there is no activated control barrier function, that is, the convex cone does not exist, it is considered a completely deadlock-free state; if there is an activated control barrier function and the negative direction of the convergence force is outside the convex cone, it is considered a limited deadlock-free state; if the convex cone covers the entire robot state space, it is a strong deadlock state; if the convex cone covers part of the robot state space, it is a weak deadlock state.

[0024] Furthermore, after detecting deadlock, deadlock avoidance is achieved by regulating the deformable control Lyapunov function. Specifically, when the robot is in a weak deadlock state, the shape parameter of the deformable control Lyapunov function is adjusted instantaneously; when the robot is in a limited deadlock-free state and a completely deadlock-free state, the shape parameter of the deformable control Lyapunov function is adjusted continuously.

[0025] The present invention also provides a reactive robot motion deadlock avoidance system, which includes a memory and a processor. The memory stores a computer program, and when the processor executes the computer program, it executes the reactive robot motion deadlock avoidance method as described above.

[0026] The present invention also provides a computer-readable storage medium, which stores machine-executable instructions. When the machine-executable instructions are called and executed by a processor, the machine-executable instructions cause the processor to implement the reactive robot motion deadlock avoidance method as described above.

[0027] Generally speaking, compared with the prior art by the above technical solutions conceived by the present invention, the reactive robot motion deadlock avoidance method and device provided by the present invention mainly have the following beneficial effects:

[0028] 1. The present invention avoids deadlocks by regulating the deformable control Lyapunov function, does not rely on a priori environment models or fixed rules, and has strong generalization ability.

[0029] 2. Since the present invention uses the relationship between the convergence force and the safety force to evaluate the deadlock risk, it can retrieve all possible deadlock situations during the robot's motion and can timely capture the deadlock trigger conditions.

[0030] 3. Since the computational complexity required for the robot's motion obstacle avoidance, deadlock detection, and avoidance is at most to solve a quadratic optimization problem, the computational complexity is low, the computational efficiency is high, and it is easy to be deployed in an embedded system.

[0031] 4. When the robot is in a weak deadlock state, instantaneously adjust the shape parameters of the deformable control Lyapunov function; when the robot is in a restricted deadlock-free state and a completely deadlock-free state, continuously adjust the shape parameters of the deformable control Lyapunov function. Among them, instantaneously adjusting the shape parameters of the control Lyapunov function can quickly avoid the detected deadlock from occurring and reduce the risk of task failure and even system paralysis; while continuously adjusting the shape parameters is to prevent the robot from entering a deadlock and ensure the stable and healthy operation of the robot. BRIEF DESCRIPTION OF THE DRAWINGS

[0032] Figure 1 is a flowchart of a reactive robot motion deadlock avoidance method provided by the present invention;

[0033] Figure 2 is a flowchart of online deadlock risk assessment provided by the present invention;

[0034] Figure 3 is a flowchart of the robot avoiding deadlocks in a weak deadlock state provided by the present invention. DETAILED DESCRIPTION OF THE EMBODIMENTS

[0035] In order to make the objectives, technical solutions and advantages of the present invention more clearly understood, the present invention will be further described in detail below with reference to the accompanying drawings and embodiments. It should be understood that the specific embodiments described herein are only used to explain the present invention and are not used to limit the present invention. In addition, the technical features involved in the various embodiments of the present invention described below can be combined with each other as long as they do not conflict with each other.

[0036] The present invention provides a method for avoiding reactive robot motion deadlocks. The method can comprehensively capture the risk of deadlock occurrence based on the current state of the robot and the surrounding environmental constraints, and improve the response speed; it realizes the avoidance and prevention of deadlocks by modulating the parameters of the robot controller, without relying on prior environmental models and rules; at the same time, the method provided by the present invention only needs to solve a simple quadratic optimization equation and can achieve millisecond-level operations on the robot's embedded computing board. At the same time, the present invention realizes real-time evaluation of the motion deadlock risk based on local environmental information and an efficient reactive deadlock avoidance strategy, avoids the robot from getting stuck due to local environmental constraints, ensures that the robot can move safely to the target position in a complex and dynamic environment, and improves the motion flexibility of the robot.

[0037] The method is a reactive robot deadlock detection and avoidance method based on a Control Barrier Function (CBF) and a Deformable Control Lyapunov Function (DCLF). It evaluates the future deadlock risk through the relationship between the stabilizing force and the collision avoidance repulsive force in robot control, and eliminates the deadlock risk by changing the shape of the DCLF, solving the technical problem that the robot gets stuck due to local environmental constraints in a complex dynamic environment.

[0038] In one embodiment, the robot's on-board computing unit runs the following program at a certain frequency: First, obtain the positions of external obstacles through on-board sensors / external sensors, including but not limited to lidar, depth cameras, motion capture cameras, etc.; construct an optimization model for the robot's collision avoidance motion based on the control barrier function and the deformable control Lyapunov function; then calculate the current convergence force and safety force vectors of the robot according to the parameters of the collision avoidance motion optimization model, and evaluate the deadlock risk of the robot based on the convex cone relationship formed by the convergence force and the safety force; then, based on the determined deadlock type, adjust the shape parameters of the deformable control Lyapunov function; finally, solve the optimization model for the robot's collision avoidance motion to achieve deadlock avoidance control. In the proposed method, the collision avoidance motion of the robot is realized through the collision avoidance motion optimization model, and the robot is made to escape from the deadlock condition by adjusting the shape parameters of the deformable control Lyapunov function, achieving the purpose of avoiding deadlocks during the safe motion process.

[0039] Please refer toFigure 1 , Figure 2 and Figure 3 , the method mainly includes the following steps:

[0040] Step 1: Based on the self - state of each robot and the information of environmental objects, construct several control barrier functions, and based on the obtained control barrier functions, construct a collision - avoidance constraint to ensure that the robot does not collide with environmental objects.

[0041] Each control barrier function is used to construct a safety constraint (i.e., collision - avoidance constraint) to ensure that the robot does not collide with environmental objects. Preferably, the signed distance function (SDF) between the robot and the obstacle is selected to construct the collision - avoidance constraint.

[0042] Step 2: Based on the self - state information of each robot and the task objective, construct a deformable control Lyapunov function, and use the deformable control Lyapunov function to construct a convergence constraint to ensure that the robot moves to the target.

[0043] The deformable control Lyapunov function is a quadratic form of the control error e, and its parameterized expression is V(e)=e T QSQ T e, where Q is a rotation matrix representing the rotation state of the control Lyapunov function; S is a scaling matrix representing the scaling ratio in each direction of the control Lyapunov function.

[0044] The shape of the deformable control Lyapunov function can be controlled by the generalized angular velocity ω and the scaling rate , and its shape dynamics is expressed as where the control error can be the robot point - stabilization error, trajectory - tracking error, etc., which can be characterized by the current state and the target state.

[0045] Step 3: Based on the collision - avoidance constraint and the convergence constraint conditions, combined with the robot dynamics model, with the minimum deviation of the robot from the model control input as the objective function, establish a robot collision - avoidance motion optimization model.

[0046] The expression of the robot collision - avoidance motion optimization model is:[[]]

[0047]

[0048] s.t.L f V(x)+L g V(x)u + γ(V(x))≤δ

[0049] L f h i (x)+L g h i(x)u + α i (h i (x)) ≥ 0, i ∈ {1, 2, …, M}

[0050] where the motion model of the robot is characterized as x represents the control state of the robot, u represents the motion control input of the robot, f represents the system transfer matrix, g represents the system input matrix, represents the model control input of the robot, which can be the input result of the upper controller or the instruction issued by a human operator. V(x) represents the deformable control Lyapunov function, h i (x) represents the CBF function obtained by the robot through SDF calculation with the surrounding obstacle o i γ, α i are respectively type functions and extended type functions, L f V, L f h i , L g V, L g h i are respectively the simplified representations of the expression , where δ is a slack variable and p is a constant coefficient.

[0051] Step 4: Based on the robot collision avoidance motion optimization model, real-time detect the risk of the robot having motion deadlocks, and based on the obtained deadlock types, regulate the shape parameters of the deformable control Lyapunov function, thereby avoiding the robot motion deadlocks.

[0052] Based on the parameters of the robot collision avoidance motion optimization model, calculate the current convergence force and safety force vector of the robot, and evaluate the deadlock risk of the robot based on the convex cone relationship formed by the convergence force and the safety force.

[0053] Among them, the deadlock risks are divided into four categories: completely deadlock-free, restricted deadlock-free, weak deadlock, and strong deadlock.

[0054] The specific steps of Step 4 are as follows:

[0055] S41: Calculate the convergence force of the robot jointly represented by the control Lyapunov function and the model control input where G = gg T is a concise notation.

[0056] S42: Calculate the activated control barrier function and the robot safety maintenance force characterized by it The activated control barrier function is the collision avoidance constraint for which the equation holds in the robot collision avoidance motion optimization model, and can be conservatively calculated using the formula

[0057] ​S43, according to the negative vector -F of the convergence force V and the convex cone formed by all activated safety maintenance forces to determine the deadlock risk. The expression of the convex cone is:

[0058]

[0059] where λ i can be any non - negative number.

[0060] The specific method for determining deadlock is as follows: If there is no activated control barrier function, that is, the convex cone does not exist, it is considered a completely deadlock - free state; if there is an activated control barrier function and the negative direction of the convergence force is outside the convex cone, it is considered a restricted deadlock - free state; if the convex cone covers the entire robot state space, it is a strong deadlock state; if the convex cone covers part of the robot state space, it is a weak deadlock state.

[0061] When the robot is in a weak deadlock state, instantaneously adjust the shape parameter of the deformable control Lyapunov function. Specifically, it includes the following steps: Denote a transformation of the convergence force F V through an equation into an intermediate vector (also called the convergence force):

[0062] First, obtain the boundary of the convex cone. Preferably, the boundary of the convex cone is obtained by the following algorithm:

[0063]

[0064] Secondly, screen the boundary of the convex cone. The screening method is as follows: (1) Denote all boundaries of the convex cone as reference boundaries (2) Remove the boundaries that are not in the positive half - plane of the robot error vector, that is

[0065] After that, according to the remaining boundaries, extend from the convergence force in the direction of the minimum distance to the selected boundary according to a certain safety threshold to obtain a set of alternative convergence forces. For all elements of the set of alternative convex cone boundaries perform the following operations: (1) Calculate the distance vector from the convergence force q2 to the boundary where represents the projection of the q2 vector on the boundary ; (2) Extend according to the distance vector to obtain the alternative convergence force vector where D0 is a positive number representing the safety distance, and n(δ) represents the direction vector of the distance vector. (3) Scale the alternative convergence force vector q'2←max{-μq1 T e / q'2e,1}·q'2, where q1 is a part of the convergence force,

[0066] Next, obtain the vector closest to the original convergence force vector from all alternative convergence force vectors, that is:

[0067]

[0068] wherein, represents the set of alternative convergence force vectors.

[0069] Finally, calculate the gradient vector of the new DCLF Preferably, solve the parameter Q of the corresponding DCLF through an optimization equation * , S * :

[0070]

[0071] When the robot is in a restricted deadlock-free and completely deadlock-free state, establish a deformable control Lyapunov function shape optimization model, and continuously adjust the shape parameters of the deformable control Lyapunov function. The specific steps are as follows:

[0072] First, calculate the rotational speed and scaling ratio for restoring the shape of the deformable control Lyapunov function to the initial state

[0073] Second, calculate the distance vector δ from the robot's convergence force to the convex cone boundary d ,

[0074]

[0075] and the minimum distance D = ||δ d ||.

[0076] Furthermore, establish a virtual CBF constraint for maintaining the distance from the robot's convergence force to the convex cone boundary:

[0077]

[0078] This CBF constraint is only activated when the system is in a restricted deadlock-free state. This CBF constraint prevents the robot from entering a deadlock by keeping the robot's convergence force outside the convex cone.

[0079] After that, establish a deformable control Lyapunov function shape optimization model and solve the deformation rate of the deformable control Lyapunov function

[0080]

[0081] Finally, update the shape of the deformable control Lyapunov function according to the time step.

[0082] When the robot is in a strong deadlock state, keep the shape of the control Lyapunov function unchanged.

[0083] Solve the robot collision avoidance motion optimization model to control the robot's motion. The robot collision avoidance motion optimization model is a quadratic optimization equation containing a deformable control Lyapunov function, which can achieve ms-level operations in the robot's on-board computing unit.

[0084] The above steps are executed in the robot's local computing unit at a certain control frequency and can be divided into three modules: a robot collision avoidance control module for generating the robot's motion speed to prevent the distance between the robot and the obstacle from exceeding the safety limit; a robot deadlock detection module for real-time judging the deadlock risk of the robot based on the robot's own state and environmental constraints; and a robot deadlock avoidance module for generating actions to avoid deadlocks for the robot according to different deadlock states. Through the cooperation of the three modules, the safe and deadlock-free motion of the robot is realized.

[0085] The present invention also provides a reactive robot motion deadlock avoidance system, which includes a memory and a processor. The memory stores a computer program, and when the processor executes the computer program, it executes the reactive robot motion deadlock avoidance method as described above.

[0086] The present invention also provides a computer-readable storage medium, which stores machine-executable instructions. When the machine-executable instructions are called and executed by the processor, the machine-executable instructions cause the processor to implement the reactive robot motion deadlock avoidance method as described above.

[0087] It is easy for those skilled in the art to understand that the above is only a preferred embodiment of the present invention and is not intended to limit the present invention. Any modifications, equivalent replacements, and improvements made within the spirit and principle of the present invention shall be included within the protection scope of the present invention.

Claims

1. A reactive robot motion deadlock avoidance method, characterized in that: The method comprises the following steps: (1) Based on each robot's own state and environmental object information, several control barrier functions are constructed, and based on the obtained control barrier functions, collision avoidance constraints are constructed to ensure that the robot does not collide with environmental objects; at the same time, a deformable control Lyapunov function is constructed based on each robot's own state information and task objectives, and the deformable control Lyapunov function is used to construct convergence constraints to ensure that the robot moves to the target; (2) Based on the collision avoidance constraints and convergence constraints, combined with the robot dynamics model, the robot collision avoidance motion optimization model is established with the minimum deviation between the robot and the model control input as the objective function; (3) Based on the robot's collision avoidance motion optimization model, the risk of motion deadlock of the robot is detected in real time. Based on the obtained deadlock type, the shape parameters of the deformable control Lyapunov function are adjusted to avoid the robot's motion deadlock.

2. The reactive robot motion deadlock avoidance method according to claim 1, characterized in that: The collision avoidance constraint is constructed using the signed distance function between the robot and the obstacle.

3. The reactive robot motion deadlock avoidance method according to claim 1, characterized in that: The deformable control Lyapunov function is a quadratic form of the control error e.

4. The reactive robot motion deadlock avoidance method as claimed in claim 2, characterized in that: The expression of the deformable control Lyapunov function after parameterization is V(e)=e T QSQ T e, where Q is a rotation matrix that represents the rotation state of the control Lyapunov function; S is a scaling matrix that represents the scaling ratio of the Lyapunov function.

5. The reactive robot motion deadlock avoidance method as claimed in claim 4, characterized in that: The shape of the deformable control Lyapunov function can be controlled by the generalized angular velocity ω and the scaling factor To control, its shape dynamics is expressed as 6. The reactive robot motion deadlock avoidance method according to claim 1, characterized in that: The expression of the robot collision avoidance motion optimization model is: s.t.L f V(x)+L g V(x)u+γ(V(x))≤δ L f h i (x)+L g h i (x)u+α i (h i (x))≥0,i∈{1,2,…,M} In the formula, the robot's motion model is represented as x represents the robot control state, u represents the robot motion control input, f represents the system transfer matrix, g represents the system input matrix, u represents the robot's model control input, V(x) represents the deformable control Lyapunov function, h i (x) represents the robot and the surrounding obstacles o i CBF function,γ,α calculated by SDF i They are Class functions and extensions class function, δ is the slack variable, p is a constant coefficient, L f V,L f h i ,L g V,L g h i The expressions are Simplified representation of g.

7. The method for avoiding deadlock in reactive robot motion according to any one of claims 1 to 6, characterized in that: The current convergence force and safety force vector of the robot are calculated based on the parameters of the robot's collision avoidance motion optimization model, and the deadlock risk of the robot is evaluated based on the convex cone relationship formed by the convergence force and the safety force.

8. The reactive robot motion deadlock avoidance method as claimed in claim 7, characterized in that: If there is no activated control barrier function, that is, the convex cone does not exist, it is considered to be a completely deadlock-free state; if there is an activated control barrier function and the negative direction of the convergence force is outside the convex cone, it is considered to be a restricted deadlock-free state; if the convex cone covers the entire robot state space, it is a strong deadlock state; if the convex cone covers part of the robot state space, it is a weak deadlock state; When the robot is in a weak deadlock state, the shape parameters of the deformable control Lyapunov function are adjusted instantaneously; when the robot is in a restricted deadlock-free state and a completely deadlock-free state, the shape parameters of the deformable control Lyapunov function are adjusted continuously.

9. A reactive robot motion deadlock avoidance system, characterized in that: The system includes a memory and a processor, wherein the memory stores a computer program, and the processor executes the reactive robot motion deadlock avoidance method according to any one of claims 1 to 8 when executing the computer program.

10. A computer-readable storage medium, characterized in that: The computer-readable storage medium stores machine-executable instructions, and when the machine-executable instructions are called and executed by the processor, the machine-executable instructions prompt the processor to implement the reactive robot motion deadlock avoidance method according to any one of claims 1-8.

Images