Dynamic obstacle avoidance path planning control method for redundant mechanical arm

Abstract

The invention provides a dynamic obstacle avoidance path planning control method for a redundant mechanical arm, which adopts a self-adaptive artificial potential field dynamic obstacle avoidance algorithm based on a second-order dynamic control barrier function. The problems that a traditional artificial potential field method cannot reach a target, is prone to falling into a local minimum value and cannot effectively deal with a dynamic obstacle are solved. The efficient and safe obstacle avoidance of the mechanical arm in a dynamic complex environment is realized by constructing a self-adaptive velocity repulsion field and a virtual obstacle model, combining a high-order dynamic control barrier function as a safety constraint and utilizing a quadratic programming optimization algorithm. Experimental results show that the method can effectively guide the mechanical arm to escape from the local minimum point, the target point can be reached, meanwhile, the safe distance between the mechanical arm and a dynamic obstacle is guaranteed, the movement efficiency and track quality of the mechanical arm are remarkably improved, and the real-time performance and safety requirements of a dynamic obstacle avoidance task are met.

Description

Technical Field

[0001] This invention relates to the field of path planning optimization methods, and in particular to a dynamic obstacle avoidance path planning and control method for a redundant robotic arm. Background Technology

[0002] Robotics technology is experiencing unprecedented rapid development, with its applications expanding from controlled, structured industrial environments to open, unstructured, and complex scenarios, encompassing collaborative manufacturing, medical assistance, home services, and field operations. This evolutionary trend poses significant challenges to the environmental perception, decision-making, planning, and real-time control capabilities of robotic arms. The core challenge lies in their ability to safely and efficiently handle highly dynamic, human-robot coexisting unstructured scenarios. Unlike structured environments, unstructured environments lack precise prior models, contain unpredictable moving entities with significant uncertainties in their trajectories, requiring robotic arms to possess real-time, robust online replanning capabilities.

[0003] Common path planning algorithms, such as RRT and its improved versions, are essentially offline planning algorithms, planning only based on the current obstacle position and lacking deep prediction of future obstacle trajectories. Unlike the global tree expansion mechanism of offline planning algorithms such as RRT, the Artificial Potential Field (APF) method directly transforms the position changes of dynamic obstacles into control force vectors by constructing a real-time updated repulsive-gravitational field model. It generates continuous and smooth obstacle avoidance trajectories based on local gradient descent, and its O(1) computational complexity ensures the real-time nature of millisecond-level obstacle avoidance decisions. However, the APF method has the disadvantages of unreachable targets and easy trapping in local minima. Reinforcement learning algorithms can learn action strategies that maximize cumulative rewards through trial-and-error interaction between the robot and the environment, thereby achieving adaptive obstacle avoidance. However, this method is highly dependent on a carefully designed reward function, and its policy learning process usually requires a lot of time. Model predictive control (MPC) is also widely used in motion planning for dynamic obstacle avoidance of robotic arms, but the setting of prediction time-domain parameters has a key technical constraint on the obstacle avoidance performance of the system. Specifically, when the prediction time domain is set too short, the control system cannot fully predict the trajectory of dynamic obstacles, leading to delayed obstacle avoidance decisions and technical problems such as emergency avoidance maneuvers. Conversely, when the prediction time domain is set too long, the dimensionality of the optimization problem increases exponentially, resulting in excessive computational resource consumption and prolonged control cycles, making it difficult to meet the real-time response performance requirements of the control system in dynamic environments. Considering that the control barrier function (CBF) can ensure the forward invariance of the safety set, some studies use the CBF to ensure robot safety; however, the design of the CBF is only for static obstacle avoidance.

[0004] In summary, the existing technology mainly has the following problems: Poor real-time performance of path planning: Traditional offline algorithms such as RRT lack the ability to predict and respond quickly to dynamic obstacles, making it difficult to meet the real-time obstacle avoidance requirements in unstructured environments.

[0005] Obstacle avoidance strategies are prone to getting stuck in local minima or failing: Artificial potential field methods suffer from the problems of unreachable targets and local minima, while reinforcement learning relies on a lot of training time and a suitable reward function.

[0006] Incomplete safety controls: Control barrier functions are mostly applied to static obstacle scenarios, lacking high-order modeling and effective control methods for dynamic obstacles. Summary of the Invention

[0007] To address the problems of poor real-time performance in existing path planning, susceptibility to local minima or failure in obstacle avoidance strategies, and incomplete safety control, this invention proposes a dynamic obstacle avoidance path planning and control method for redundant robotic arms. Based on a second-order dynamic control barrier function, an adaptive artificial potential field is constructed to achieve real-time guidance and dynamic obstacle avoidance constraints. This effectively solves the trajectory planning failure problems caused by dynamic obstacles, such as local minima convergence and target unreachability, when redundant robotic arms perform complex tasks in unstructured scenarios. Furthermore, the algorithm uses a quadratic programming optimization algorithm and a control barrier function to ensure the safety of the robotic arm, thereby further correcting the reference path output by the real-time motion planner. This invention can significantly improve the motion efficiency and trajectory quality of robotic arms in dynamic and complex environments. The core design of the method is as follows: In the design of the real-time motion planner, an adaptive virtual repulsive field with a local minima escape mechanism and an adaptive velocity repulsive field with dynamic obstacle avoidance are constructed to generate a collision-free guidance path. The guidance path output by the real-time motion planner is used as the reference trajectory, and a dynamic control barrier function is introduced as a hard safety constraint to strictly guarantee the avoidance of dynamic obstacles. The dynamic obstacle avoidance method proposed in this invention integrates the real-time obstacle avoidance capability of APF (Automatic Obstacle Avoidance Function) and the safety assurance capability of CBF (Closed-Loop Safety Function), achieving both real-time performance and safety of the closed-loop system in dynamic environments. The technical solution is as follows: A dynamic obstacle avoidance control method for a redundant robotic arm, comprising: Construct the gravitational field of the target and the repulsive field of the static obstacle; An adaptive velocity repulsion field is constructed based on the velocity vector of the dynamic obstacle and the relative position of the dynamic obstacle and the robotic arm. Based on the previous The trajectory points at each moment determine whether the robotic arm is trapped in a local minimum. When the robotic arm is trapped in a local minimum, virtual obstacles are introduced to construct an adaptive virtual repulsion field. The adaptive virtual repulsion field includes a three-stage escape mechanism: Retreat Phase: When the preset retreat conditions are met, a repulsive force is applied along the virtual obstacle pointing towards the robotic arm. ; Detour Phase: When the preset detour conditions are met, a repulsive force is applied in the direction perpendicular to the robotic arm within the repulsive-attraction plane. ; Forward Phase: When the preset forward conditions are met, a repulsive force is applied along the direction the robotic arm points towards the target. ; The current moment is generated based on the target gravitational field, the static obstacle repulsive field, the adaptive velocity repulsive field, and the adaptive virtual repulsive field. Expected trajectory and nominal control quantity ; The safety distance constraint between the robotic arm and the obstacle is constructed based on the second-order control barrier function, and the constraint control quantity is solved. ; To minimize the nominal control quantity With constraint control quantity A quadratic programming model is established with the deviation as the objective and the second-order control barrier function as the constraint to solve for the optimal control quantity. And generate the optimal trajectory .

[0008] Furthermore, the construction of the adaptive velocity repulsion field based on the velocity vector of the dynamic obstacle and the relative position of the dynamic obstacle and the robotic arm includes: Calculate the first A dynamic obstacle points towards the end of the robotic arm. relative position vector ; Calculate the relative position vector and velocity vector The angle between ; According to the included angle Generate dynamic obstacle repulsion force : in Relative position vector and velocity vector In the plane formed, perpendicular to the velocity vector unit vector, For dynamic obstacles The basic repulsive force on the robotic arm, The parameter variable is used to adjust the magnitude of the dynamic repulsive force.

[0009] Furthermore, the three-stage escape mechanism specifically includes: Rollback phase: When the preset rollback conditions are met At that time, a repulsive force is applied along the virtual obstacle pointing in the direction of the robotic arm. Until the detour conditions are met. ; Detour Phase: When the preset detour conditions are met At that time, a repulsive force is applied in the direction perpendicular to the robotic arm within the repulsive-attractive plane. Until the virtual obstacle is connected to the end of the robotic arm. The line connecting to the target point The angle between ; Forward Phase: When the preset forward conditions are met At that time, a repulsive force is applied along the direction the robotic arm points towards the target. ; in, This is the position of the robotic arm's end effector. For the location of virtual obstacles, The target point location, It is the vector sum of the repulsive forces of static and dynamic obstacles.

[0010] Furthermore, the aforementioned based on the previous Determining whether the robotic arm is trapped in a local minimum based on the trajectory points at each moment includes: Before recording trajectory points at each moment ; Calculate this The center point of each point ; Calculate the Distance between points maximum value ; Will With the set maximum threshold If a comparison is made, If the value is positive, the robotic arm will fall into a local minimum; otherwise, it will not fall into a local minimum.

[0011] Furthermore, the center position of the virtual obstacle satisfy: in, , The center point of the static obstacle closest to the robotic arm.

[0012] Furthermore, the safety distance constraint between the robotic arm and the obstacle, constructed based on the second-order control barrier function, is as follows: in, It is a second-order control barrier function. and For Li Dao, count the operators. and They represent Regarding vector fields and Li Daoshu, Weight parameters are used to adjust the convergence speed and stability of the system.

[0013] Furthermore, the minimization of the nominal control quantity With constraint control quantity The formula for calculating the deviation is as follows: in To control the input feasible region, It is a weighted matrix.

[0014] Furthermore, the quadratic programming model is as follows: in, , For all constraints obtained for all obstacles The matrix obtained by stacking: .

[0015] According to another aspect of the present invention, a dynamic obstacle avoidance control system for a redundant robotic arm is proposed, comprising: Environmental perception module: Real-time acquisition of velocity vectors and position coordinates of dynamic obstacles; Potential field calculation module: performs calculations of target gravitational field, static obstacle repulsive field, adaptive velocity repulsive field, and adaptive virtual repulsive field; Local Minimum Detection Module: Determines whether the robotic arm is trapped in a local minimum based on historical trajectory points; Quadratic planner: Generates optimal control input based on nominal control input and second-order control barrier function constraints; Motion control module: Drives the robotic arm to execute a safe trajectory.

[0016] The beneficial effects of this invention are as follows: This invention constructs a dynamic obstacle avoidance algorithm for redundant robotic arms based on a second-order control barrier function and an adaptive artificial potential field method. The algorithm includes introducing a vector adjustment factor pointing to the target point to improve target inaccessibility, introducing velocity repulsion to avoid dynamic obstacles, introducing virtual obstacles to push the robotic arm away from local minima, and designing an adaptive virtual repulsion field. The process of escaping local minima is divided into three stages, enabling the robotic arm to escape local minimum points more quickly. The reference trajectory is obtained in real time using the adaptive artificial potential field. Furthermore, based on the dynamic obstacle avoidance requirements of the robotic arm, a high-order dynamic control barrier function is constructed, and safety constraints are derived. A quadratic programming optimization algorithm is used to approach the reference trajectory as closely as possible while ensuring that safety constraints are not violated, thus simultaneously guaranteeing the safety and real-time performance of the robotic arm's dynamic obstacle avoidance. Attached Figure Description Figure 1 This is a schematic diagram of the traditional artificial potential field method model; Figure 2 This is a schematic diagram of an adaptive velocity repulsion field model according to an embodiment of the present invention; Figure 3 This is a schematic diagram of an adaptive virtual repulsive field model according to an embodiment of the present invention; Figure 4 A flowchart illustrating the adaptive virtual repulsive field escaping a local minimum according to an embodiment of the present invention; Figure 5 The diagram shows the trajectories of the basic virtual repulsive field and the adaptive virtual repulsive field escaping local minima. Figure 5 (a) is a trajectory diagram of the basic virtual repulsive field escaping a local minimum. Figure 5 (b) is a trajectory diagram of the adaptive virtual repulsive field escaping a local minimum; Figure 6 The graph shows the time-varying distance between the robotic arm and the obstacle without control barrier function constraints. Figure 7 The graph shows the time-varying distance between the robotic arm and the obstacle under control barrier function constraints. Figure 8 This is a dynamic obstacle avoidance trajectory diagram of different obstacle avoidance algorithms in complex scenarios, where... Figure 8 (a) is a dynamic obstacle avoidance trajectory diagram using the traditional artificial potential field method. Figure 8 (b) shows the dynamic obstacle avoidance trajectory of the adaptive artificial potential field method without a control barrier function. Figure 8 (c) is the dynamic obstacle avoidance trajectory diagram of the adaptive artificial potential field method with control barrier function. Detailed Implementation

[0017] Example 1 An adaptive artificial potential field dynamic obstacle avoidance algorithm based on a high-order dynamic control barrier function includes the following steps: Step 1: Construct the target gravitational field and the static obstacle repulsive field to obtain the first... A static obstacle repulsive force and the gravitational pull of the target point .

[0018] Step 2: Real-time data collection The velocity vector of a dynamic obstacle and the position coordinates of the dynamic obstacle relative to the coordinate system of the robotic arm base The adaptive velocity repulsion field is constructed, and the specific implementation process includes: Calculate the first A dynamic obstacle points towards the end of the robotic arm. relative position vector ; The relative position vector and velocity vector The angle between ; According to the included angle Generate dynamic obstacle repulsion force .

[0019] Step 3: Before recording trajectory points at each moment Calculate this The center point of each point ,calculate Each point is far from the center point. maximum value , and the set maximum threshold By comparing the values, we can determine whether the object is trapped in a local minimum. If the current robotic arm is trapped in a local minimum, then it is trapped in a local minimum; otherwise, it is not trapped in a local minimum.

[0020] Step 4: If it was determined in Step 3 that the robotic arm is trapped in a local minimum, then based on the distance from the end effector of the robotic arm... Nearest obstacle location and the location of the target point Create virtual obstacles Construct an adaptive virtual repulsive field and calculate the repulsive force of virtual obstacles. .

[0021] Step 5: If it is determined in Step 3 that the robotic arm is trapped in a local minimum, a three-stage escape strategy is executed based on the adaptive virtual repulsion field, including a retreat phase, a detour phase, and a forward phase, where: Rollback phase: When the conditions are met At that time, a unit vector is generated along the direction of the virtual repulsive force. Obtain the adaptive virtual repulsion force during the rollback phase. Continuous monitoring conditions Until satisfied .

[0022] Detour phase: when conditions are met At that time, calculate the repulsive force of the actual obstacle. and target gravity Plane On the plane Find the gravitational force perpendicular to the target. unit vector Obtain adaptive virtual repulsion during the detour phase. Continuously monitor the connection between the virtual obstacle and the robotic arm's end effector. The line connecting to the target point The angle between The size, until .

[0023] Advance phase: When conditions are met At that time, a unit vector is generated along the direction of the virtual repulsive force. Obtain the adaptive virtual repulsion force during the rollback phase. .

[0024] Step 6: Construct the desired trajectory based on the target gravitational field, static obstacle repulsive field, adaptive velocity repulsive field, and adaptive virtual repulsive field. Utilize the static obstacle repulsive force obtained in Step 1. Target point gravity and the dynamic obstacle repulsion force calculated in step 2 The resultant force on the end effector of the robotic arm is obtained through vector synthesis. The desired trajectory for the next moment is generated using the direction of this resultant force. .

[0025] Step 7: Based on the desired trajectory generated in Step 6 The desired velocity of the robotic arm's end effector at the next moment is calculated using numerical differentiation. and expected acceleration ,Will As the nominal control quantity for the end effector motion of a robotic arm.

[0026] Step 8: To meet the obstacle avoidance requirements of the robotic arm, address the obstacles... Design a hierarchical constraint mechanism. First, define the control barrier function. This characterizes the relationship between the system state and obstacle safety. A second-order control barrier function is constructed. Based on the system dynamics characteristics, the safety constraints are derived. Calculate the constraint control quantity This constraint ensures that the system state is always within the safe set, avoiding collisions with obstacles.

[0027] Step 9: Construct a constrained optimization problem to balance nominal control and safety requirements. Set the optimization objective to minimize the constraint control quantity. With nominal control quantity The deviation is expressed in the form of a weighted norm. ,in To control the input feasible region, The matrix is ​​weighted. The second-order control barrier function constraint derived in step 8 is added to ensure safe obstacle avoidance. The optimal control input for the next time step is obtained through a quadratic planner. The optimal trajectory at the next moment is obtained by integrating the system motion model. This enables real-time, dynamic obstacle avoidance for safety.

[0028] In another embodiment of the present invention, the specific process of establishing the artificial gravitational field and repulsive field model in step 1 is as follows: The artificial potential field method, a classic path planning algorithm, possesses the significant advantage of simple computational logic. This method relies on the potential energy field theory in physics, abstracting the robotic arm's end effector as a point mass model, defining the target location as an attractive potential field source, and simultaneously setting obstacles within the robotic arm's workspace as potential field sources generating repulsive forces. For example... Figure 1 In the artificial potential field model shown, the end effector of the robotic arm is abstracted as... Point, obstacle pair Generate repulsive force Target point Generate gravity By constructing a synthetic potential field function that includes the superposition of gravitational and repulsive potential fields, and utilizing the potential field gradient guidance mechanism, collision-free path planning for a robotic arm in the workspace is achieved. This method transforms the path search problem into a problem of force-induced motion of a particle in a virtual force field through mathematical modeling of the potential energy field, providing an intuitive physical modeling approach for dynamic obstacle avoidance and path optimization of robotic arms.

[0029] The artificial potential energy function in three-dimensional space can be defined as: (1) in Let be the gravitational potential energy function generated at the target point relative to the end effector of the robotic arm. Let be the potential energy function of the combined repulsive force exerted by all obstacles in space on the end effector of the robotic arm. According to classical field theory, the negative gradient direction of the potential field corresponds to the direction of the conservative force acting on the end effector of the robotic arm. Therefore, the force acting on the robotic arm in space is: (2) in . Let the target point be the gravitational force vector generated by the robotic arm's end effector. This is the repulsive force vector generated by the obstacle on the end of the robotic arm.

[0030] Assume the coordinates of the target point are The coordinates of the robotic arm's end effector in space are: Let the distance between the target point and the end effector of the robotic arm be denoted as . Constructing an artificial gravitational potential energy function: (3) In the formula Let be a gravitational constant greater than 0. Then the gravitational force exerted by the target point on the robotic arm is: (4) As the distance between the robotic arm and the target point decreases, the gravitational force it experiences also increases with distance. It decreases linearly.

[0031] Assuming a static obstacle The coordinates in space are Record obstacles The distance to the end effector of the robotic arm is .

[0032] Constructing an artificial repulsive field: (5) in The threshold value for the influence range of the obstacle repulsion force satisfies... ; It is a repulsive constant greater than 0.

[0033] Formula (5) is the repulsive potential field of the traditional artificial potential field method. It can be seen that in , When it is constant, Only by The repulsive force is determined by the distance to the obstacle, and it is inversely proportional to the distance to the obstacle; that is, the closer the distance is to the obstacle, the greater the repulsive force. However, this method has a drawback: when the obstacle... Distance from target point less than When, that is, the target point is on the obstacle Within the range of the repulsive force, when the robotic arm reaches the target point... The value is 0, which, according to formula (4), means that the gravitational force experienced at this time is 0. However, The force is not zero. According to formula (5), the repulsive force at this time is not zero. So even if the end of the robotic arm reaches the target point at this time, the force is not zero, and it will still leave the target point, which will lead to the problem of the target being unreachable.

[0034] Therefore, the repulsive potential field is improved: (6) When the target point is reached When the value is 0, the repulsive force received is zero. The value is 0. This solves the problem of the aforementioned goal being unattainable.

[0035] From formula (6), we obtain the first... The repulsive force of a static obstacle is: (7) in and for: (8) (9) The repulsive force exerted by all static obstacles on the end effector of the robotic arm is: (10) In the formula It is a set of static obstacles.

[0036] The robotic arm experiences a repulsive force from the obstacle in the direction of the spherical obstacle's center towards the end of the robotic arm, and an attractive force from the target point in the direction of the target point. The net force on the end of the robotic arm is: (11) In another embodiment of the present invention, the specific process of constructing the adaptive velocity repulsion field in step 2 is as follows: By collecting data in real time The velocity vector of a dynamic obstacle and its position coordinates relative to the coordinate system of the robotic arm base. ; Calculate the first The relative position vector of a dynamic obstacle pointing to the end effector of the robotic arm. The relative position vector and velocity vector The angle between According to the included angle Generate dynamic obstacle repulsion force .

[0037] (12) (13) in For vectors and velocity vector In the plane formed, perpendicular to the velocity vector The unit vector represents the velocity repulsion vector. The direction. It is a scalar representing the velocity repulsion vector. The size of the modulus. For example... Figure 2 As shown, The direction is The direction, when At that time, that is This represents the velocity vector of the dynamic obstacle. The vector pointing to the end of the robotic arm The included angle is less than At this time, dynamic obstacles There is a possibility of collision with the robotic arm, and the smaller the angle, the greater the possibility of collision. If the angle is 0, meaning the robotic arm is on the obstacle's trajectory, a collision is certain. If the angle is greater than... If the dynamic obstacle is moving away from the robotic arm, then the speed repulsion setting is canceled.

[0038] In yet another embodiment of the present invention, the specific process of constructing the virtual obstacle model described in step 4 is as follows: Before recording trajectory points at each moment Calculate this The center point of each point Calculate this The distance between the points and the center point maximum value , and the set maximum threshold By comparing the values, we can determine whether the object is trapped in a local minimum.

[0039] like This represents the front of the robotic arm. If the robot arm hovers near a certain point, it is trapped in a local minimum; otherwise, it is not. If it is trapped in a local minimum, a virtual obstacle is added at a certain point, causing the robot arm to experience a virtual repulsive force that helps it escape the local minimum. The coordinates of the center point of the virtual obstacle are designed below: To the end of the robotic arm With the target point The center point of the selected virtual obstacle satisfies: (14) (15) in The center point of the static obstacle closest to the robotic arm.

[0040] The above formula has already calculated the position of the virtual obstacle. Next, we design the virtual repulsion function of the virtual obstacle. The initial intention of designing the virtual obstacle is to allow the robotic arm's end effector to escape a local minimum after being subjected to a virtual repulsion force. The trajectory for escaping the local minimum is designed in three stages: backward movement, detour, and forward movement, as follows... Figure 3 As shown. To shorten the escape trajectory, a corresponding virtual repulsive force is designed for each stage, i.e., a piecewise function for the virtual repulsive force is designed: (16) The direction vector of the virtual repulsive force in the first stage The direction is equal to The direction, i.e., by virtual obstacles Pointing to the end of the robotic arm The direction vector of the virtual repulsive force in the second stage. satisfy: (17) (18) in Actual repulsive force With gravity The plane The normal vector; For plane Internal perpendicular to gravity The unit vector. The direction vector of the virtual repulsive force in the third stage. The direction is equal to The direction, that is, from the end of the robotic arm Point to target point .

[0041] in satisfy: (19) for and The included angle.

[0042] In yet another embodiment of the present invention, step 5 involves generating the desired trajectory for the next moment. The specific process; The static obstacle repulsion force obtained from the established adaptive artificial potential field model Target point gravity and repulsive force of dynamic obstacles and the repulsive force of virtual obstacles To obtain the resultant force on the end effector of the robotic arm. : (20) in It is a set of obstacles, including static and dynamic obstacles. Therefore, the expected trajectory at the next moment is... .

[0043] In yet another embodiment of the invention, step 6 involves calculating the desired control quantity. The specific process is as follows: The desired velocity of the robotic arm's end effector at the current moment is calculated using numerical differentiation. velocity at the previous moment : (twenty one) (twenty two) in This refers to the robot's control cycle. It's necessary to achieve the desired movement speed. Design nominal control input That is, the robot's expected acceleration: (twenty three) In another embodiment of the present invention, the specific process of constructing the second-order control barrier function in step 8 is as follows: Set the position of the robotic arm's end effector as follows: The speed is acceleration is The state of the robotic arm can then be set to... ,have and satisfy The motion of the robotic arm's end effector can be written as: (twenty four) Let U, Then the equation of motion (24) can be written in the form of a linear system: (25) The robotic arm can only avoid obstacles if the distance between its end effector and the obstacle is greater than a safe distance. : (26) in and These are the robotic arm end effector and the obstacle. The radius of the spherical bounding box, This is the minimum distance set. To meet the obstacle avoidance requirements of the robotic arm, this distance is used to address obstacles. Design No. Control barrier functions : (27) in It is the first The location of the center of the spherical bounding box of each obstacle. Since the position of the obstacles is dynamic, It is a time-varying function. For the sake of simplicity, in the following formula derivation, we will use... This represents the control barrier function.

[0044] safe collection Defined as a control barrier function The upper level set: (28) Referring to the definition of the control barrier function, the dynamic control barrier function D-CBF can be defined as: for all have And there is an extension. function , so that: (29) It can be expressed in the form of Lie derivative: (30) Calculate the first-order Lie derivative: , We can obtain: (31) (32) because This causes the control input to have no effect on safety constraints. A second-order dynamic control barrier function needs to be designed. : (33) in Belongs to extension Function, let Therefore: (34) Therefore, the second-order Lie derivative can be calculated: (35) (36) For robotic arm affine systems The constraint conditions for the second-order control barrier function can be obtained as follows: (37) in Belongs to extension Function, let In another embodiment of the present invention, the objective of optimization in step 9 is to minimize the deviation between the actual control quantity and the nominal acceleration. The specific process is as follows: Using weighted norm ,in To control the input feasible region, This is a weighted matrix. All constraints are obtained for all obstacles. Construct a stacked matrix: (38) Therefore, the inequality constraints obtained by controlling the barrier function can be written as follows: .

[0045] Therefore, a quadratic planner can be constructed: (39) Solving (39) yields the optimal control quantity at the next time step. The optimal velocity at the next moment is obtained by integrating the system motion model. Thus, the optimal trajectory at the next moment can be obtained. This enables real-time, dynamic obstacle avoidance for safety.

[0046] Example 2 The effectiveness of the method of the present invention was verified under a simple, common motion scenario. The speed of the dynamic obstacle was uniform linear motion. The flowchart of the method of the present invention based on the adaptive virtual repulsive field to escape the local minimum is as follows. Figure 4 As shown. Figure 5 (a) shows the path of the robotic arm using the basic virtual repulsion field method. It can be seen that the robotic arm is trapped in a local minimum and cannot escape. Using the adaptive virtual repulsion function of this invention to guide the robotic arm out of the local minimum, we can obtain... Figure 5 (b) shows the obstacle avoidance path. Table 1 provides a comparison of path length, planning time, and jitter between the basic virtual repulsion field and the adaptive virtual repulsion field proposed in this invention, where trajectory jitter is used to qualitatively measure the smoothing effect.

[0047] Table 1 Path Comparison As can be seen from Table 1, Figure 5 ( a The trajectory length in () is 2.9711m. Figure 5 ( bThe trajectory length in the model is 1.5305m, and the planning time of the adaptive artificial potential field method is much shorter than that of the traditional artificial potential field method. A comparison reveals that by introducing an adaptive virtual repulsion force, the adaptive artificial potential field method enables the robotic arm to escape local minima faster, and the planned path is shorter, with less jitter, and the algorithm exhibits better trajectory smoothing. The planned trajectory is superior to the path planned by the basic virtual repulsion function.

[0048] Meanwhile, the algorithm's dynamic obstacle avoidance performance was further verified in more complex scenarios. In these complex scenarios, dynamic obstacles no longer move at a constant velocity in a straight line. Figure 6 The curves showing the change in distance between the robotic arm and the obstacle over time under the constraint of no control barrier function are presented. Figure 7 The graph showing the distance between the robotic arm and the obstacle over time under control barrier function constraints is presented. It can be seen that after applying the control barrier function constraints, the distance between the robotic arm and the dynamic obstacle remains within a safe range, further ensuring the safety of the robotic arm.

[0049] Figure 8 The dynamic obstacle avoidance performance of different obstacle avoidance algorithms in complex scenarios is presented, including the traditional artificial potential field method obstacle avoidance performance. Figure 8 (a) Obstacle avoidance effect of adaptive artificial potential field method without control barrier function ( Figure 8 (b) and the obstacle avoidance effect of the adaptive artificial potential field method based on the control barrier function ( Figure 8 (c) Experimental results show that the traditional artificial potential field method cannot escape local minima and will get stuck in local minima, leading to planning failure. The adaptive artificial potential field method without a control barrier function will collide with dynamic obstacles with strong randomness, resulting in failure. The adaptive artificial potential field method based on a control barrier function can achieve good dynamic obstacle avoidance in complex scenarios, even in scenarios with highly maneuverable dynamic obstacles.

[0050] Therefore, the adaptive artificial potential field dynamic obstacle avoidance method based on the second-order dynamic control barrier function adopted in this invention achieves target point reachability and successful escape from local minimum points through the adaptive artificial potential field, and meets the real-time planning requirements while ensuring the safety of the robotic arm in the dynamic obstacle avoidance task.

[0051] This invention may have other embodiments. Without departing from the spirit and essence of this invention, those skilled in the art can make various corresponding changes and modifications according to this invention, but these corresponding changes and modifications should all fall within the protection scope of the appended claims.

Claims

1. A dynamic obstacle avoidance control method for a redundant robot arm, characterized by, The method comprises the following steps: constructing a target attractive force field and a static obstacle repulsive force field; constructing an adaptive velocity repulsive force field based on a velocity vector of a dynamic obstacle, a relative position between the dynamic obstacle and the robot arm; Based on the previous The trajectory points at each moment determine whether the robotic arm is trapped in a local minimum. When the robotic arm is trapped in a local minimum, virtual obstacles are introduced to construct an adaptive virtual repulsion field. The adaptive virtual repulsion field includes a three-stage escape mechanism: Retreat phase: when preset retreat conditions are met, repulsive force is applied along the virtual barrier pointing to the direction of the robot arm ; A detour phase: when a preset detour condition is met, repulsive force is applied in a direction perpendicular to the mechanical arm in the repulsive-attractive force plane ; Forward stage: when a preset forward condition is met, repulsive force is applied along the direction in which the mechanical arm points to the target direction ; Based on the target gravitational field, static obstacle repulsive field, adaptive speed repulsive field, adaptive virtual repulsive field, the desired trajectory and the nominal control quantity at the current time are generated; A safety distance constraint between the robot arm and the obstacle is constructed based on a second-order control barrier function, and a constraint control quantity is solved ; to minimize the deviation of the nominal control variable from the constraint control variable a quadratic programming model is established with the second-order control barrier function as the constraint, and the optimal control variable is solved and the optimal trajectory is generated .

2. The method of claim 1, wherein, the adaptive velocity repulsive force field is constructed based on the velocity vector of the dynamic obstacle and the relative position between the dynamic obstacle and the robot arm; Computing the relative position vector of a dynamic obstacle pointing to the end position of a robot arm ;​​ calculating an angle between the relative position vector and a dynamic obstacle velocity vector ;​ According to the included angle Generating dynamic obstacle repulsion : wherein is the relative position vector and velocity vector consists of the plane in which the relative position vector is perpendicular to the velocity vector is a dynamic obstacle a basic repulsive force to the robot arm, is a parameter variable that adjusts the size of the dynamic repulsive force.

3. The method of claim 1, wherein, the three-stage escape mechanism specifically comprises: Retreat phase: when preset retreat condition is met , repulsive force is applied along the direction of the virtual barrier pointing to the mechanical arm , until the detour condition is met ; The bypass stage: when the preset bypass condition is met , the repulsive force is applied in the vertical mechanical arm direction in the repulsive-attractive force plane , until the included angle between the line connecting the virtual obstacle and the end of the mechanical arm and the line connecting the target point is ; Forward stage: when preset forward conditions are met , repulsive force is applied along the mechanical arm pointing to the target direction ; wherein, is a mechanical arm end position, is a virtual obstacle position, is a target point position, is a vector sum of a static obstacle repulsion and a dynamic obstacle repulsion.

4. The method of claim 1, wherein, The trajectory point based on the previous moment determines whether the robot arm falls into a local minimum value, comprising: Pre-recorded Trajectory points at a time ; Calculate the center point of this point ; The maximum value of the distance of the points ;​​ The maximum threshold value set is compared with, if , the robot arm falls into a local minimum, otherwise it does not.

5. The method of claim 1, wherein, The center position of the virtual obstacle satisfies: wherein, , is the center point of the closest static obstacle to the robot arm.

6. The method of claim 1, wherein, the safety distance constraint between the robot arm and the obstacle is constructed based on the second-order control barrier function as: in, It is a second-order control barrier function. and For Li Dao, count the operators. and They represent Regarding vector fields and Li Daoshu, Weight parameters are used to adjust the convergence speed and stability of the system.

7. The method of claim 1, wherein, The minimization nominal control quantity The constraint control quantity The deviation calculation formula is as follows: wherein is a control input feasible region, is a weighting matrix.

8. The method of claim 1, wherein, the quadratic programming model is: wherein, , is the matrix resulting from stacking all constraints obtained for all obstacles ​ 。 9. A dynamic obstacle avoidance control system for a redundant robot arm implementing the method of claim 1, characterized by, The method comprises the following steps: an environment perception module: collecting a dynamic obstacle velocity vector and position coordinates in real time; a potential field calculation module: performing target attractive force field, static obstacle repulsive force field, adaptive velocity repulsive force field and adaptive virtual repulsive force field calculation; a local minimum detection module: judging whether the robot arm is trapped in a local minimum value based on historical trajectory points; a quadratic programming device: generating an optimal control quantity based on a nominal control quantity and a second-order control barrier function constraint; a motion control module: driving the robot arm to execute a safe trajectory.

Images