A method for safe motion planning of a dual-redundant robotic arm

Abstract

This invention belongs to the field of robot safety control technology and discloses a method for safe motion planning of a dual-redundant robotic arm. A linear mapping relationship is established between the joint spatial velocities of the redundant robotic arm and the velocity of the end effector. End effector velocity constraints, safety distance constraints, and joint amplitude constraints are established, and a multi-objective joint constraint model is established by integrating a differential kinematics model with the three types of constraints. The optimal decision variables satisfy the Cartesian-Kuhn-Tucker conditions. The original expression of the Cartesian-Kuhn-Tucker conditions is clarified, and a nonlinear complementary function is introduced to reconstruct the Cartesian-Kuhn-Tucker conditions. Based on the improved Cartesian-Kuhn-Tucker conditions, a vector error function is defined, and an adaptive zeroing neural network is designed. The vector error function is iteratively solved using the adaptive zeroing neural network to obtain the optimal decision variables of the multi-objective joint constraint model. The optimal decision variables are then used as the joint motion control commands for the robotic arm and output, completing the robotic arm motion planning.

Description

Technical Field

[0001] This invention relates to the field of robot safety control technology, and in particular to a method for safe motion planning of a dual-redundant robotic arm. Background Technology

[0002] In recent years, robotics technology has developed rapidly and has been widely applied in manufacturing, healthcare, service, and emergency response. A redundant robotic arm refers to a robot system with more degrees of freedom than the minimum required to complete a specific task; redundancy is typically defined as having more than 6 degrees of freedom. Compared to traditional single-redundant robotic arms, dual-redundant robotic arm systems offer greater workspace and flexibility, demonstrating significant advantages in complex tasks such as precision assembly and collaborative handling. However, this flexibility also brings new control and planning challenges, especially in collaborative operations where the two arms need to maintain a predetermined relative posture or synchronized grasping, increasing the complexity of inverse kinematics calculations and task coordination. Furthermore, the redundancy and overlapping workspace of dual-arm robotic systems also present significant obstacle avoidance challenges.

[0003] Inverse kinematics is a fundamental problem in the control of redundant manipulators, its core being the calculation of the corresponding joint angle configuration based on the desired pose of the end effector. Traditional methods typically use the pseudo-inverse of the Jacobian matrix to obtain the minimum norm solution corresponding to the end effector pose. However, with the increase in degrees of freedom, the pseudo-inverse method requires frequent calculation of the matrix pseudo-inverse, resulting in high computational complexity and time consumption. More importantly, the pseudo-inverse method is difficult to directly incorporate inequality constraints such as joint limits or collision avoidance, thus failing to handle the requirements of multiple secondary tasks simultaneously. Existing research has shown that transforming the inverse kinematics problem of redundant manipulators into a constrained optimization problem and solving for the optimal decision variables through optimization algorithms is an effective way to achieve multi-objective coordinated control. Based on this, this invention proposes a collision avoidance motion planning method based on neural networks. By unifying the differential kinematic model of the redundant manipulator with multiple types of constraints into the optimization framework, and utilizing the parallel computing and online solution capabilities of neural networks, safe motion planning control of a dual-redundant manipulator is achieved. Summary of the Invention

[0004] The purpose of this invention is to provide a safe motion planning method for dual-redundant robotic arms, which solves the problem of redundancy decomposition, controls the simultaneous operation of two robotic arms, and avoids collisions between the two robotic arms.

[0005] The technical solution of the present invention is as follows: A method for safe motion planning of a dual-redundant robotic arm includes the following steps:

[0006] Based on the series operation of homogeneous coordinate transformation matrices between adjacent links of the robotic arm, the pose of the end effector of the dual-redundant robotic arm in the base coordinate system is calculated; combined with the joint space velocity of the robotic arm, a differential kinematic model is established; the differential kinematic model is used to characterize the linear mapping relationship between the joint space velocity of the robotic arm and the velocity of the end effector.

[0007] Based on the differential kinematics model, position negative feedback correction is introduced to construct the end effector velocity constraint; based on the shortest distance calculation results of the link at the geometric level, a safety distance constraint is constructed; and combined with the mechanical limit parameters of the robotic arm joint, a joint amplitude limit constraint is constructed.

[0008] By integrating the differential kinematics model, end effector velocity constraints, safety distance constraints, and joint amplitude constraints, a multi-objective joint constraint model is established; the optimal decision variables of the multi-objective joint constraint model satisfy the Caro-Kuhn-Tucker conditions; a nonlinear complementary function is introduced to improve the Caro-Kuhn-Tucker conditions, resulting in the improved Caro-Kuhn-Tucker conditions;

[0009] Based on the improved Caro-Kuhn-Tucker condition, a vector error function is defined, and an adaptive zero-return neural network is designed. The vector error function is iteratively solved by the adaptive zero-return neural network to obtain the optimal decision variables of the multi-objective joint constraint model. The optimal decision variables are then used as joint motion control commands for the robotic arm and output to complete the motion planning of the robotic arm.

[0010] The linear mapping relationship between the joint space velocity of the robotic arm and the velocity of the end effector is as follows:

[0011] Each arm of the dual-redundant robotic arm is discretized into several link elements. Based on the DH parameter table, the link length, link twist angle, joint offset, and joint angle parameters between adjacent link elements are determined, and a homogeneous coordinate transformation matrix between adjacent link elements is constructed. , used to characterize the The link is relative to the first The pose transformation relationship of each link. Number the link. For the first The angle of each joint;

[0012] Following the link sequence from the base coordinate system to the coordinate system of the dual-redundant robotic arm end effector, the homogeneous coordinate transformation matrices of adjacent link units are multiplied in series to obtain the expression for the total homogeneous coordinate transformation matrix of the dual-redundant robotic arm end effector relative to the base coordinate system: ;

[0013] in, Represents the rotation matrix. Represents the end effector position vector; This represents the concatenated product of homogeneous transformation matrices in the forward kinematics of a redundant robotic arm.

[0014] The pose of the dual-redundant robotic arm's end effector in Cartesian space is solved using forward kinematics, and its kinematic equations are expressed as follows: ;in, This is the end-effector state vector of the dual-redundant robotic arm. For Cartesian space dimensions, This represents the joint angle variable of the robotic arm. Represents the number of joints. This represents a nonlinear mapping relationship from joint space to state space.

[0015] Based on the total homogeneous coordinate transformation matrix Construct the Jacobian matrix of the robotic arm by considering the differential relationships of the joint angles. ;

[0016] Combined with joint space velocity A differential kinematic model is established; the mathematical expression of the differential kinematic model is: It is used to characterize the linear mapping relationship between the joint space velocity of a robotic arm and the velocity of the end effector.

[0017] The process for constructing the end effector speed constraint is as follows:

[0018] A closed-loop correction mechanism driven by position deviation is used to dynamically adjust the speed of the end effector. The expression for the end effector speed with position negative feedback correction is as follows: ; It is a position feedback gain coefficient. This represents the desired position vector of the end effector. This represents the actual position vector of the end effector. This represents the end effector velocity vector after position negative feedback correction, used to drive the end effector motion; This represents the initial velocity vector of the end effector;

[0019] The end effector velocity vector, corrected by position negative feedback, is used as the upper limit of the constraint to construct the end effector velocity constraint condition, ensuring that the end effector velocity always tracks the desired velocity and suppressing the accumulation of position tracking deviation; the end effector velocity constraint is as follows: In the formula, the Jacobian matrix formed by the left and right robotic arms is: , Let Jacobian matrix be the left robotic arm. The left robotic arm joint angle vector. Let Jacobian matrix be the right robotic arm. This is the joint angle vector of the right robotic arm.

[0020] The position deviation-driven closed-loop correction mechanism specifically involves: using the desired position vector of the end effector... With respect to the actual position vector of the end effector The deviation is the driving signal, which is fed back through the gain coefficient. After amplitude adjustment of the drive signal, it is superimposed with the initial velocity vector of the end effector to form the end effector velocity vector after position negative feedback correction. When the actual position vector of the end effector Deviation from the desired position vector of the end effector At that time, the position deviation signal vector Through feedback gain coefficient It generates a positive or negative correction component; the correction component acts on the initial velocity vector of the end effector. The end effector velocity vector after position negative feedback correction Adjust in the direction that cancels out the positional deviation, thereby driving the actual position vector of the end effector. To the desired position vector of the end effector near;

[0021] Position deviation signal vector The end effector velocity vector after position negative feedback correction There is a positive correlation; when the end effector position lags, Position deviation signal vector , correction component For positive, When the end effector position is ahead, Positional deviation The correction component is negative. .

[0022] The process for constructing the safety distance constraints is as follows:

[0023] The left and right robotic arms are respectively represented as connected link units. Using the shortest distance algorithm for spatial line segments, the real-time shortest distance between all links of the left and right robotic arms is obtained in any state. ; The real-time distance between any links of the two arms; setting layered safety distance thresholds, including critical thresholds. and safe activation threshold ;

[0024] The initial hard constraint for the safety distance is constructed, and its mathematical expression is as follows: In the formula, the spatial distance vector of the critical point is... , It is the vector from the critical point D of the robotic arm link in the direction of P. It is the vector from the critical point P of the robotic arm link in the direction of D. The diagonal Jacobian matrix representing the Jacobian matrices of the critical points located on the links of the dual-redundant robotic arm. Critical point Jacobian matrix, Critical point Jacobian matrix, operators It is Hadama multiplication. It is the zero vector;

[0025] Introducing an adaptive transition function with collision avoidance constraints This is used to output the collision avoidance response weights. The collision avoidance constraint adaptive transition function is a piecewise function, and its expression is:

[0026]

[0027] Define the composite constraint matrix and constraint threshold terms Combining the composite constraint matrix, constraint threshold term, and collision avoidance constraint adaptive transition function, a safe distance constraint is constructed, the expression of which is: ;

[0028] in, , , This is a function that takes the maximum value and is used to limit the upper limit of the constraint threshold term.

[0029] The process of constructing the joint limiting constraint is as follows:

[0030] Obtain the mechanical limit parameters of each joint of the robotic arm; the mechanical limit parameters include the mechanical limit range of the joint angles. and the maximum permissible angular velocity range of joint machinery , It is the upper limit of the joint angle. It is the lower limit of the joint angle. It is the upper limit of joint space velocity. It is the lower limit of joint space velocity;

[0031] Preliminary joint limiting constraints are constructed based on the aforementioned mechanical limiting parameters: , Introducing gain coefficient Mapping the difference between the joint angle limit and the real-time angle to a velocity constraint boundary yields: In the formula, This is the lower bound vector of the velocity constraint after angle transformation. This is the upper limit vector of the velocity constraint after angle transformation;

[0032] By fusing the upper and lower bound vectors of the velocity constraint after angle transformation with the original joint angular velocity mechanical constraint, a joint amplitude limiting constraint is constructed: the lower bound of the joint amplitude limiting constraint is defined. Joint amplitude limiting upper limit The mathematical expression for the joint limiting constraint is: .

[0033] The differential kinematics model forms the kinematic basis, with end-effector velocity constraints, safety distance constraints, and joint amplitude limiting constraints as the constraints. The joint space velocity vector is used as the optimal decision variable in the multi-objective joint constraint model; the objective function is a weighted optimization objective function of joint motion energy consumption and trajectory tracking deviation. ;

[0034] Based on the objective function and constraints, a multi-objective joint constraint model is constructed, expressed as follows:

[0035]

[0036] in, It is a positive definite matrix. This is a weighted vector for trajectory tracking deviation. This is the inequality constraint coefficient matrix. The vector represents the threshold vector for inequality constraints; the multi-objective joint constraint model is a nonlinear programming problem, and the optimization vector... Satisfy the Caro-Kun-Tucker conditions;

[0037] The Caro-Kun-Tucker condition expression is as follows:

[0038]

[0039] in, and It is to optimize the Lagrange multipliers. These are the decision variables for the optimal solution of a multi-objective joint constraint model; towards complementary relaxation conditions. Introduce a smooth, nonlinear complementary function, expressed as: , satisfy , It is a disturbance term. These are variables in a nonlinear complementary function;

[0040] The complementary relaxation condition is transformed into a continuous and smooth nonlinear equation; based on this nonlinear equation, the Carosch-Kuhn-Tucker condition is reconstructed, and the discrete constraint is eliminated to obtain the mathematical expression of the improved Carosch-Kuhn-Tucker condition:

[0041] .

[0042] Based on the improved Caro-Kun-Tucker condition, a vector is defined. Constructing nonlinear equations The nonlinear equation is an equivalent transformation of the improved Caro-Kuhn-Tucker condition, and its solution corresponds to the optimal decision variables of the multi-objective joint constraint model; the specific expression of the nonlinear equation is: ;

[0043] in, It is a matrix in a nonlinear equation. It is the identity matrix; It is a vector in a nonlinear equation; , , , , These are small constants used to avoid the denominator being zero. For complementary functions, nonlinear terms, and They are Lagrange multipliers;

[0044] Define the vector error function as: The vector error function is used to monitor the solution process of the nonlinear equation, and its zero-state... Equivalent to nonlinear equations The conditions for satisfying the conditions correspond to the optimization vector of the multi-objective joint constraint model. ;

[0045] The derivative expression of the vector error function is: In the formula, It is the convergence gain, used to adjust the error convergence speed; It is an array of activation functions used to enhance the nonlinear fitting ability and convergence stability of the network;

[0046] Expanding the derivative expression of the vector error function and performing a directive yields the initial dynamic equations of the adaptive zero-return neural network: ;

[0047] in, It is about Time derivative, It is about Time derivative, It is about Time derivative, yes Time derivative, and It is a diagonal matrix. For diagonalization operations, Represents the Hadama division;

[0048] By rearranging the initial dynamic equations, a standardized adaptive zero-return neural network is obtained: ;

[0049] in, The variable parameter coefficient matrix for a standardized adaptive zero-return neural network; For a standardized time-varying weight matrix of an adaptive return-to-zero neural network; This is the constant term vector of a standardized adaptive zero-return neural network; and It is the derivative with respect to time;

[0050] The optimal decision variables are solved using a standardized adaptive zero-return neural network. First, the state vector of the adaptive zero-return neural network is initialized. Convergence gain coefficient Variable parameter coefficient matrix Time-varying weight matrix ; Vector error function The standardized adaptive return-to-zero neural network is then connected, initiating its adaptive iterative process. As the iteration progresses, the standardized adaptive return-to-zero neural network dynamically adjusts the variable parameter coefficient matrix. and time-varying weight matrix , so that the vector error function asymptotically converges to the zero vector; when , The iteration stops at the preset convergence accuracy threshold, and the optimized vector output by the standardized adaptive zeroing neural network is then determined. In this context, the component corresponding to the joint space velocity is the optimal decision variable in the multi-objective joint constraint model. ;

[0051] The optimal decision variables obtained by solving The mapping is performed as real-time motion control commands for each joint of the dual-redundant robotic arm. The mapping process is achieved through signal conversion by the joint servo controller, which converts the optimal solution of the joint space velocity into a voltage signal that can be recognized by the servo motor. The real-time motion control commands are synchronously output to the servo actuators of each joint of the dual-redundant robotic arm, driving the robotic arm to move smoothly along the planned trajectory and completing the robot's safe motion planning.

[0052] The beneficial effects of this invention are as follows: By introducing a closed-loop correction mechanism driven by position deviation, the accumulation of trajectory tracking errors is effectively suppressed, improving the accuracy and stability of the end effector's motion. Simultaneously, the safety distance constraint constructed based on the adaptive transition function of collision avoidance constraints gradually strengthens as the two arms approach each other, and automatically degenerates into an inactive state within the safe region, thereby avoiding problems such as joint space velocity jumps, numerical oscillations, and control instability caused by abrupt changes in distance constraints. This progressive constraint mechanism ensures system safety while balancing the continuity of motion planning and the smoothness of the control process.

[0053] To address the insufficient adaptability of traditional methods such as single-objective programming and pseudo-inverse solving under multiple constraints, this invention integrates differential kinematics models and three types of constraints to construct a multi-objective joint constraint model with joint motion energy consumption and trajectory tracking deviation as weighted indices, thereby enhancing the matching between the objective function and the constraints. Furthermore, by introducing a nonlinear complementary function to smooth the Caro-Kuhn-Tucker conditions, the originally discrete complementary relaxation conditions are transformed into continuously differentiable smooth equations. This effectively overcomes the problems of slow convergence and numerical oscillations that traditional Caro-Kuhn-Tucker conditions often encounter when dealing with inequality constraints, providing theoretical support for the efficient and stable solution of optimal decision variables.

[0054] Compared with the traditional original-dual neural network solution method, the adaptive zero-return neural network based on the smooth function in this invention performs better in terms of real-time performance and numerical stability, and can better adapt to the online optimization and collision avoidance control requirements of dual redundant robotic arms under complex and multi-constraint conditions. Attached Figure Description

[0055] Figure 1 A flowchart for a safe motion planning method for a dual-redundant robotic arm;

[0056] Figure 2 This is a diagram illustrating the minimum distance based on geometric meaning.

[0057] Figure 3 Here is a block diagram of an adaptive zero-return neural network structure;

[0058] Figure 4 This is a diagram illustrating the collision avoidance effect of a dual-redundant robotic arm.

[0059] Figure 5 This is a comparison chart of the effects of safety distance constraints. Detailed Implementation

[0060] The present application will now be described in detail with reference to the accompanying drawings and specific embodiments.

[0061] In an embodiment, such as Figure 1As shown, a method for safe motion planning of a dual-redundant robotic arm includes the following steps:

[0062] Step 1: Calculate the pose of the end effector of the dual-redundant robotic arm in the base coordinate system based on the series operation of homogeneous coordinate transformation matrices between adjacent links of the robotic arm; establish a differential kinematics model by combining the joint space velocity of the robotic arm; the differential kinematics model is used to characterize the linear mapping relationship between the joint space velocity of the robotic arm and the velocity of the end effector.

[0063] Specifically:

[0064] Each arm of the dual-redundant robotic arm is discretized into several link elements. Based on the DH parameter table, the link length, link twist angle, joint offset, and joint angle parameters between adjacent link elements are determined, and a homogeneous coordinate transformation matrix between adjacent link elements is constructed. , used to characterize the The link is relative to the first The pose transformation relationship of each link. Number the link. For the first The angle of each joint;

[0065] Following the link sequence from the base coordinate system to the coordinate system of the dual-redundant robotic arm end effector, the homogeneous coordinate transformation matrices of adjacent link units are multiplied in series to obtain the expression for the total homogeneous coordinate transformation matrix of the dual-redundant robotic arm end effector relative to the base coordinate system: ;

[0066] in, Represents the rotation matrix. Represents the end effector position vector; This represents the concatenated product of homogeneous transformation matrices in the forward kinematics of a redundant robotic arm.

[0067] The pose of the dual-redundant robotic arm's end effector in Cartesian space is solved using forward kinematics, and its kinematic equations are expressed as follows: ;in, This is the end-effector state vector of the dual-redundant robotic arm. For Cartesian space dimensions, This represents the joint angle variable of the robotic arm. Represents the number of joints. This represents a nonlinear mapping relationship from joint space to state space.

[0068] Based on the total homogeneous coordinate transformation matrix Construct the Jacobian matrix of the robotic arm by considering the differential relationships of the joint angles. Combined with joint space velocity A differential kinematic model is established; the mathematical expression of the differential kinematic model is: It is used to characterize the linear mapping relationship between the joint space velocity of a robotic arm and the velocity of the end effector.

[0069] Step 2: Based on the differential kinematics model, position negative feedback correction is introduced to construct the end effector velocity constraint; based on the geometric shortest distance calculation results of the link and the adaptive transition function of the collision avoidance constraint, a safety distance constraint is constructed; combined with the mechanical limit parameters of the robotic arm joint, a joint amplitude limit constraint is constructed.

[0070] The speed constraints of the end effector with position negative feedback correction are as follows;

[0071] A closed-loop correction mechanism driven by position deviation is used to dynamically adjust the speed of the robotic arm's end effector; the expression for the end effector speed with position negative feedback correction is: ; It is a position feedback gain coefficient. This represents the desired position vector of the end effector. This represents the actual position vector of the end effector. This represents the velocity vector of the end effector after position negative feedback correction, used to drive the motion of the end effector;

[0072] The end effector velocity vector, corrected by position negative feedback, is used as the upper limit of the constraint to construct the end effector velocity constraint condition, ensuring that the end effector velocity always tracks the desired velocity and suppressing the accumulation of position tracking deviation; the end effector velocity constraint is as follows: In the formula, the Jacobian matrix formed by the left and right robotic arms is: , Let Jacobian matrix be the left robotic arm. The left robotic arm joint angle vector. Let Jacobian matrix be the right robotic arm. This is the joint angle vector of the right robotic arm.

[0073] The position deviation-driven closed-loop correction mechanism is specifically as follows: using the desired position vector of the end effector... With respect to the actual position vector of the end effector The deviation is the driving signal, which is fed back through the gain coefficient. After amplitude adjustment of the drive signal, it is superimposed with the initial velocity vector of the end effector to form the end effector velocity vector after position negative feedback correction. When the actual position vector of the end effector Deviation from the desired position vector of the end effector At that time, the position deviation signal vector Through feedback gain coefficient It generates a positive or negative correction component, which acts on the initial velocity vector of the end effector. The end effector velocity vector after position negative feedback correction Adjust in the direction that cancels out the positional deviation, thereby driving the end effector to its actual position. To the desired position of the end effector By moving closer together, dynamic convergence of positional errors can be achieved;

[0074] Position deviation signal vector The end effector velocity vector after position negative feedback correction There is a positive correlation; when the end effector position lags, Position deviation signal vector , correction component For positive, When the end effector position is ahead, Positional deviation The correction component is negative. .

[0075] The specific safety distance constraints are as follows;

[0076] In collaborative tasks performed by two robotic arms, to ensure that the two robotic arms do not collide during operation, one robotic arm is treated as an obstacle to the other robotic arm, and the minimum distance between the obstacle and the robotic arm is measured.

[0077] The calculation of the shortest distance between any two links based on geometric principles is as follows;

[0078] Suppose there is a point O outside link AB, such as Figure 2 As shown. Point It is the intersection point on the perpendicular line passing through point O of link AB. Because of the minimum distance... The location of point D is a key factor in the algorithm and should be considered and calculated first. From Figure 2 From this, we can obtain, Further simplified to: .

[0079] Among them let ,according to Consider the following three cases depending on the location of the point.

[0080] 1) If point Located on the left side of link AB, i.e. The minimum distance from point O to link AB is The critical point P is the left end point of link AB.

[0081] 2) If point Located on link AB, i.e. The minimum distance from point O to link AB is Critical point It is a point .

[0082] 3) If point Located on the right side of link AB, i.e. The minimum distance from point O to link AB is The critical point D is the right end point B of the connecting rod AB.

[0083] For the left arm link of the dual-redundant robotic arm, define... , Left robotic arm The positions of the two ends of the first link in Cartesian space, and its first link... The central axis of each link is equivalently defined as a line vector. Similarly, for the right arm link of the dual-redundant robotic arm, define... , For the right robotic arm The positions of the two ends of the first link in Cartesian space, the position of the second link... The central axis of each link is equivalently defined as a line vector. ; and They are line vectors and The critical point on the surface; the shortest distance between links is determined geometrically. , express critical point on arrive The shortest distance; express Points on arrive The shortest distance;

[0084] The left and right robotic arms are represented as interconnected link units. Based on a geometric shortest distance algorithm, the real-time shortest distance between all links of the left and right robotic arms in any state is obtained. ; The real-time distance between any links of the two arms; setting layered safety distance thresholds, including critical thresholds. and safe activation threshold ;

[0085] The initial hard constraint for the safety distance is constructed, and its mathematical expression is as follows: In the formula, the spatial distance vector of the critical point is... , It is the vector from the critical point D of the robotic arm link in the direction of P. It is the vector from the critical point P of the robotic arm link in the direction of D. The diagonal Jacobian matrix representing the Jacobian matrices of the critical points located on the links of the dual-redundant robotic arm. Critical point Jacobian matrix, Critical point Jacobian matrix, operators It is Hadama multiplication. It is the zero vector;

[0086] Introducing an adaptive transition function with collision avoidance constraints This is used to output collision avoidance response weights to achieve a smooth transition of constraints. The adaptive transition function for collision avoidance constraints is a piecewise function, expressed as:

[0087]

[0088] The output characteristic of the collision avoidance constraint adaptive transition function is: when When the collision avoidance response weight is 0, there is a sufficient safety distance between the two robotic arms, and they are in an absolutely safe state. The collision avoidance constraint is not activated, and the robotic arms run normally according to the original planned trajectory. At that time, the collision avoidance response weight follows The decrease smoothly increases from 0 to 1, indicating that the dual robotic arms are gradually entering the collision warning and transition phase. Within this range, collision avoidance constraints intervene gradually, suppressing potential collision-causing motion components through weighted adjustment of joint space velocities, thereby achieving smooth collision avoidance without disrupting trajectory continuity. When the collision avoidance response weight is 1, it means that the two robotic arms have reached the lower limit of collision safety and the collision avoidance constraint is fully effective.

[0089] Define the composite constraint matrix and constraint threshold terms Combining the composite constraint matrix, constraint threshold term, and collision avoidance constraint adaptive transition function, the final safe distance constraint is constructed, and its expression is: ;

[0090] in, , , This is a function that maximizes the value of the constraint threshold term. The constraint utilizes the smoothing weights output by the transition function to continuously modulate the collision avoidance constraint strength, causing the originally abrupt hard constraint to gradually take effect across the time and distance dimensions.

[0091] The joint amplitude limiting constraints are as follows;

[0092] Obtain the mechanical limit parameters of each joint of the robotic arm; the mechanical limit parameters include the mechanical limit range of the joint angles. and the maximum permissible angular velocity range of joint machinery , It is the upper limit of the joint angle. It is the lower limit of the joint angle. It is the upper limit of joint space velocity. It is the lower limit of joint space velocity;

[0093] Preliminary joint limiting constraints are constructed based on the aforementioned mechanical limiting parameters: , Introducing gain coefficient Mapping the difference between the joint angle limit and the real-time angle to a velocity constraint boundary yields: In the formula, This is the lower bound vector of the velocity constraint after angle transformation. This is the upper limit vector of the velocity constraint after angle transformation;

[0094] By fusing the upper and lower bound vectors of the velocity constraint after angle transformation with the original joint angular velocity mechanical constraint, a joint amplitude limiting constraint is constructed: the lower bound of the joint amplitude limiting constraint is defined. Joint amplitude limiting upper limit The mathematical expression for the joint limiting constraint is: .

[0095] Step 3: Integrate the differential kinematics model, end effector velocity constraint, safety distance constraint, and joint amplitude limiting constraint to establish a multi-objective joint constraint model; the optimal decision variables of the multi-objective joint constraint model satisfy the Caro-Kuhn-Tucker condition; introduce a nonlinear complementary function to improve the Caro-Kuhn-Tucker condition, resulting in the improved Caro-Kuhn-Tucker condition;

[0096] The differential kinematics model forms the kinematic basis, with end-effector velocity constraints, safety distance constraints, and joint amplitude limiting constraints as the constraints. The joint space velocity vector is used as the optimal decision variable in the multi-objective joint constraint model; the objective function is a weighted optimization objective function of joint motion energy consumption and trajectory tracking deviation. Then, the original form of the Caro-Kuhn-Tucker condition is clarified, and then the Caro-Kuhn-Tucker condition is improved by using a nonlinear complementary function.

[0097] Based on the objective function and constraints, a multi-objective joint constraint model is constructed, expressed as follows:

[0098]

[0099] in, It is a positive definite matrix. This is a weighted vector for trajectory tracking deviation. This is the inequality constraint coefficient matrix. The vector represents the threshold vector for inequality constraints; the multi-objective joint constraint model is a nonlinear programming problem, and the optimization vector... Satisfy the Caro-Kun-Tucker conditions;

[0100] The Caro-Kun-Tucker condition expression is as follows:

[0101]

[0102] in, and It is to optimize the Lagrange multipliers. These are the decision variables for the optimal solution of a multi-objective joint constraint model; where complementary relaxation conditions are... The discrete nonlinear characteristics of the material lead to problems such as slow convergence speed and iterative oscillation in traditional numerical solution methods, making it difficult to adapt to the needs of real-time motion planning.

[0103] To address the difficulty in solving the complementary relaxation conditions in the Caro-Kuhn-Tucker conditions, a smooth, nonlinear complementary function expression is introduced: , satisfy , It is a disturbance term. These are variables in a nonlinear complementary function;

[0104] The complementary relaxation condition is transformed into a continuous and smooth nonlinear equation; based on this nonlinear equation, the Carosch-Kuhn-Tucker condition is reconstructed, and the discrete constraint is eliminated to obtain the mathematical expression of the improved Carosch-Kuhn-Tucker condition:

[0105]

[0106] The improved Caro-Kuhn-Tucker condition retains its original optimality determination characteristics and also possesses continuous and smooth mathematical properties. It can directly adapt to the iterative solution requirements of intelligent methods such as neural networks, thereby improving the efficiency and real-time performance of solving optimal decision variables.

[0107] Step 4: Based on the improved Caro-Kuhn-Tucker condition, define the vector error function and design an adaptive zero-return neural network, such as... Figure 3 As shown, the vector error function is iteratively solved by the adaptive zero-return neural network to obtain the optimal decision variables of the multi-objective joint constraint model. The optimal decision variables are then used as joint motion control commands for the robotic arm and output to complete the motion planning of the robotic arm.

[0108] Based on the improved Caro-Kun-Tucker condition, a vector is defined. Constructing nonlinear equations The nonlinear equation is an equivalent transformation of the improved Caro-Kuhn-Tucker condition, and its solution corresponds to the optimal decision variables of the multi-objective joint constraint model; the specific expression of the nonlinear equation is: ;

[0109] in, It is a matrix in a nonlinear equation. It is the identity matrix; It is a vector in a nonlinear equation; , , , , These are small constants used to avoid the denominator being zero. For complementary functions, nonlinear terms, and They are Lagrange multipliers;

[0110] Define the vector error function as: The vector error function is used to monitor the solution process of the nonlinear equation, and its zero-state... Equivalent to nonlinear equations The conditions for satisfying the conditions correspond to the optimization vector of the multi-objective joint constraint model. ;

[0111] The derivative expression of the vector error function is: In the formula, It is the convergence gain, used to adjust the error convergence speed; It is an array of activation functions used to enhance the nonlinear fitting ability and convergence stability of the network;

[0112] Expanding the derivative expression of the vector error function and performing a directive yields the initial dynamic equations of the adaptive zero-return neural network: ;

[0113] in, It is about Time derivative, It is about Time derivative, It is about Time derivative, yes Time derivative, and It is a diagonal matrix. For diagonalization operations, Represents the Hadama division;

[0114] By rearranging the initial dynamic equations, a standardized adaptive zero-return neural network is obtained: ;

[0115] in, The variable parameter coefficient matrix for a standardized adaptive zero-return neural network; For a standardized time-varying weight matrix of an adaptive return-to-zero neural network; This is the constant term vector of a standardized adaptive zero-return neural network; and It is the derivative with respect to time;

[0116] The optimal decision variables are solved using a standardized adaptive zero-return neural network. First, the state vector of the adaptive zero-return neural network is initialized. Convergence gain coefficient Variable parameter coefficient matrix Time-varying weight matrix ; Vector error function The standardized adaptive return-to-zero neural network is then connected, initiating its adaptive iterative process. As the iteration progresses, the standardized adaptive return-to-zero neural network dynamically adjusts the variable parameter coefficient matrix. and time-varying weight matrix , so that the vector error function asymptotically converges to the zero vector; when , The iteration stops at the preset convergence accuracy threshold, and the optimized vector output by the standardized adaptive zeroing neural network is then determined. In this context, the component corresponding to the joint space velocity is the optimal decision variable in the multi-objective joint constraint model. ;

[0117] The optimal decision variables obtained by solving The mapping is performed as real-time motion control commands for each joint of the dual-redundant robotic arm. The mapping process is achieved through signal conversion by the joint servo controller, which converts the optimal solution of the joint spatial velocity into a voltage signal that the servo motor can recognize. The real-time motion control commands are synchronously output to the servo actuators of each joint of the dual-redundant robotic arm, driving the robotic arm to move smoothly along the planned trajectory and completing the robot's safe motion planning.

[0118] In the specific implementation process, Given the end-point tracking task period is The critical threshold and the safe activation threshold are respectively set to In addition, the origin of the base coordinate system of the left and right redundant robotic arms is fixed at [value missing]. The initial joint angles of the dual-redundant robotic arm are preset to... , The end-point tracking trajectory consists of two intersecting circles, and its specific expression is as follows:

[0119]

[0120] in, Represents the radius of the circular trajectory. This represents the angle of inclination of the circular trajectory in Cartesian space. These represent the initial points of the left (right) robotic arm ends, respectively.

[0121] like Figure 4 The diagram shows the critical point between the two arms at a certain moment. The proposed safe motion planning method ensures accurate tracking of the end effector trajectory while achieving mutual collision avoidance between the two redundant robotic arms. Figure 4 The global minimum distance at any time in .like Figure 5 As shown, without applying the content of this invention, the shortest distance between the arms in the time period of 1.2s to 2.8s is less than [a certain value]. This indicates that the two arms collided during this time, affecting the completion of the task; after applying the content of this invention, the global minimum distance between the two arms during the entire cooperative operation process of the collision avoidance mechanism based on geometric distance is This means that, based on the proposed dual-arm safe movement planning scheme, the global minimum distance during its movement is always greater than the set critical threshold, meaning that if a collision does not occur, the dual arms are always within a safe distance range.

Claims

1. A dual-redundant robot safety motion planning method, characterized by, Includes the following steps: Based on the series operation of homogeneous coordinate transformation matrices between adjacent links of the robotic arm, the pose of the end effector of the dual-redundant robotic arm in the base coordinate system is calculated; combined with the joint space velocity of the robotic arm, a differential kinematic model is established; the differential kinematic model is used to characterize the linear mapping relationship between the joint space velocity of the robotic arm and the velocity of the end effector. Based on the differential kinematics model, position negative feedback correction is introduced to construct the end effector velocity constraint; based on the shortest distance calculation results of the link at the geometric level, a safety distance constraint is constructed; and combined with the mechanical limit parameters of the robotic arm joint, a joint amplitude limit constraint is constructed. By integrating the differential kinematics model, end effector velocity constraints, safety distance constraints, and joint amplitude constraints, a multi-objective joint constraint model is established; the optimal decision variables of the multi-objective joint constraint model satisfy the Caro-Kuhn-Tucker conditions; a nonlinear complementary function is introduced to improve the Caro-Kuhn-Tucker conditions, resulting in the improved Caro-Kuhn-Tucker conditions; Based on the improved Caro-Kuhn-Tucker condition, a vector error function is defined, and an adaptive zero-return neural network is designed. The vector error function is iteratively solved by the adaptive zero-return neural network to obtain the optimal decision variables of the multi-objective joint constraint model. The optimal decision variables are then used as joint motion control commands for the robotic arm and output to complete the motion planning of the robotic arm.

2. The dual redundant robot safety motion planning method of claim 1, wherein, The linear mapping relationship between the joint space velocity of the robotic arm and the velocity of the end effector is as follows: Each of the dual-redundant robot arms is discretized into a plurality of link units, link lengths, link torsion angles, joint offsets and joint angle parameters between adjacent link units are determined according to a D-H parameter table, and a homogeneous coordinate transformation matrix between adjacent link units is constructed , for representing a pose transformation relationship of an th link relative to an th link, is a link number, is an angle of an th joint; Following the link sequence from the base coordinate system to the coordinate system of the dual-redundant robotic arm end effector, the homogeneous coordinate transformation matrices of adjacent link units are multiplied in series to obtain the expression for the total homogeneous coordinate transformation matrix of the dual-redundant robotic arm end effector relative to the base coordinate system: ; in, Represents the rotation matrix. Represents the end effector position vector; This represents the concatenated product of homogeneous transformation matrices in the forward kinematics of a redundant robotic arm. The pose of the dual-redundant robotic arm's end effector in Cartesian space is solved using forward kinematics, and its kinematic equations are expressed as follows: ;in, This is the end-effector state vector of the dual-redundant robotic arm. For Cartesian space dimensions, This represents the joint angle variable of the robotic arm. Represents the number of joints. This represents a nonlinear mapping relationship from joint space to state space. Based on the total homogeneous coordinate transformation matrix Construct the Jacobian matrix of the robotic arm by considering the differential relationships of the joint angles. ; Combined with joint space velocity A differential kinematic model is established; the mathematical expression of the differential kinematic model is: It is used to characterize the linear mapping relationship between the joint space velocity of a robotic arm and the velocity of the end effector.

3. The method for safe motion planning of a dual-redundant robotic arm according to claim 1, characterized in that, The process for constructing the end effector speed constraint is as follows: A closed-loop correction mechanism driven by position deviation is used to dynamically adjust the speed of the end effector. The expression for the end effector speed with position negative feedback correction is as follows: ; It is a position feedback gain coefficient. This represents the desired position vector of the end effector. This represents the actual position vector of the end effector. This represents the end effector velocity vector after position negative feedback correction, used to drive the end effector motion; This represents the initial velocity vector of the end effector; The end effector velocity vector, corrected by position negative feedback, is used as the upper limit of the constraint to construct the end effector velocity constraint condition, ensuring that the end effector velocity always tracks the desired velocity and suppressing the accumulation of position tracking deviation; the end effector velocity constraint is as follows: In the formula, the Jacobian matrix formed by the left and right robotic arms is: , Let Jacobian matrix be the left robotic arm. The left robotic arm joint angle vector. Let Jacobian matrix be the right robotic arm. This is the joint angle vector of the right robotic arm.

4. The method for safe motion planning of a dual-redundant robotic arm according to claim 3, characterized in that, The position deviation-driven closed-loop correction mechanism specifically involves: using the desired position vector of the end effector... With respect to the actual position vector of the end effector The deviation is the driving signal, which is fed back through the gain coefficient. After amplitude adjustment of the drive signal, it is superimposed with the initial velocity vector of the end effector to form the end effector velocity vector after position negative feedback correction. When the actual position vector of the end effector Deviation from the desired position vector of the end effector At that time, the position deviation signal vector Through feedback gain coefficient It generates a positive or negative correction component; the correction component acts on the initial velocity vector of the end effector. The end effector velocity vector after position negative feedback correction Adjust in the direction that cancels out the positional deviation, thereby driving the actual position vector of the end effector. To the desired position vector of the end effector near; Position deviation signal vector The end effector velocity vector after position negative feedback correction There is a positive correlation; when the end effector position lags, Position deviation signal vector , correction component For positive, ; When the end effector position is ahead Positional deviation The correction component is negative. .

5. The method for safe motion planning of a dual-redundant robotic arm according to claim 1, characterized in that, The process for constructing the safety distance constraints is as follows: The left and right robotic arms are respectively represented as connected link units. Using the shortest distance algorithm for spatial line segments, the real-time shortest distance between all links of the left and right robotic arms is obtained in any state. ; The real-time distance between any links of the two arms; setting layered safety distance thresholds, including critical thresholds. and safe activation threshold ; The initial hard constraint for the safety distance is constructed, and its mathematical expression is as follows: In the formula, the spatial distance vector of the critical point is... , It is the vector from the critical point D of the robotic arm link in the direction of P. It is the vector from the critical point P of the robotic arm link in the direction of D. The diagonal Jacobian matrix representing the Jacobian matrices of the critical points located on the links of the dual-redundant robotic arm. Critical point Jacobian matrix, Critical point Jacobian matrix, operators It is Hadama multiplication. It is the zero vector; Introducing an adaptive transition function with collision avoidance constraints This is used to output the collision avoidance response weights. The collision avoidance constraint adaptive transition function is a piecewise function, and its expression is: Define the composite constraint matrix and constraint threshold terms Combining the composite constraint matrix, constraint threshold term, and collision avoidance constraint adaptive transition function, a safe distance constraint is constructed, the expression of which is: ; in, , , This is a function that takes the maximum value and is used to limit the upper limit of the constraint threshold term.

6. The method for safe motion planning of a dual-redundant robotic arm according to claim 1, characterized in that, The process of constructing the joint limiting constraint is as follows: Obtain the mechanical limit parameters of each joint of the robotic arm; the mechanical limit parameters include the mechanical limit range of the joint angles. and the maximum permissible angular velocity range of joint machinery , It is the upper limit of the joint angle. It is the lower limit of the joint angle. It is the upper limit of joint space velocity. It is the lower limit of joint space velocity; Preliminary joint limiting constraints are constructed based on the aforementioned mechanical limiting parameters: , Introducing gain coefficient Mapping the difference between the joint angle limit and the real-time angle to a velocity constraint boundary yields: In the formula, This is the lower bound vector of the velocity constraint after angle transformation. This is the upper limit vector of the velocity constraint after angle transformation; By fusing the upper and lower bound vectors of the velocity constraint after angle transformation with the original joint angular velocity mechanical constraint, a joint amplitude limiting constraint is constructed: the lower bound of the joint amplitude limiting constraint is defined. Joint amplitude limiting upper limit The mathematical expression for the joint limiting constraint is: .

7. The method for safe motion planning of a dual-redundant robotic arm according to claim 1, characterized in that, The differential kinematics model forms the kinematic basis, with end-effector velocity constraints, safety distance constraints, and joint amplitude limiting constraints as the constraints. The joint space velocity vector is used as the optimal decision variable in the multi-objective joint constraint model; the objective function is a weighted optimization objective function of joint motion energy consumption and trajectory tracking deviation. ; Based on the objective function and constraints, a multi-objective joint constraint model is constructed, expressed as follows: in, It is a positive definite matrix. This is a weighted vector for trajectory tracking deviation. This is the inequality constraint coefficient matrix. The vector represents the threshold vector for inequality constraints; the multi-objective joint constraint model is a nonlinear programming problem, and the optimization vector... Satisfy the Caro-Kun-Tucker conditions; The Caro-Kun-Tucker condition expression is as follows: in, and It is to optimize the Lagrange multipliers. These are the decision variables for the optimal solution of a multi-objective joint constraint model; towards complementary relaxation conditions. Introduce a smooth, nonlinear complementary function, expressed as: , satisfy , It is a disturbance term. These are variables in a nonlinear complementary function; The complementary relaxation condition is transformed into a continuous and smooth nonlinear equation; based on this nonlinear equation, the Carosch-Kuhn-Tucker condition is reconstructed, and the discrete constraint is eliminated to obtain the mathematical expression of the improved Carosch-Kuhn-Tucker condition: 。 8. The method for safe motion planning of a dual-redundant robotic arm according to claim 7, characterized in that, Based on the improved Caro-Kun-Tucker condition, a vector is defined. Constructing nonlinear equations The nonlinear equation is an equivalent transformation of the improved Caro-Kuhn-Tucker condition, and its solution corresponds to the optimal decision variables of the multi-objective joint constraint model; the specific expression of the nonlinear equation is: ; in, It is a matrix in a nonlinear equation. It is the identity matrix; It is a vector in a nonlinear equation; , , , , These are small constants used to avoid the denominator being zero. For complementary functions, nonlinear terms, and They are Lagrange multipliers; Define the vector error function as: The vector error function is used to monitor the solution process of the nonlinear equation, and its zero-state... Equivalent to nonlinear equations The conditions for satisfying the conditions correspond to the optimization vector of the multi-objective joint constraint model. ; The derivative expression of the vector error function is: In the formula, It is the convergence gain, used to adjust the error convergence speed; It is an array of activation functions used to enhance the nonlinear fitting ability and convergence stability of the network; Expanding the derivative expression of the vector error function and performing a directive yields the initial dynamic equations of the adaptive zero-return neural network: ; in, It is about Time derivative, It is about Time derivative, It is about Time derivative, yes Time derivative, and It is a diagonal matrix. For diagonalization operations, Represents the Hadama division; By rearranging the initial dynamic equations, a standardized adaptive zero-return neural network is obtained: ; in, The variable parameter coefficient matrix for a standardized adaptive zero-return neural network; For a standardized time-varying weight matrix of an adaptive return-to-zero neural network; This is the constant term vector of a standardized adaptive zero-return neural network; and It is the derivative with respect to time; The optimal decision variables are solved using a standardized adaptive zero-return neural network. First, the state vector of the adaptive zero-return neural network is initialized. Convergence gain coefficient Variable parameter coefficient matrix Time-varying weight matrix ; Vector error function The standardized adaptive return-to-zero neural network is then connected, initiating its adaptive iterative process. As the iteration progresses, the standardized adaptive return-to-zero neural network dynamically adjusts the variable parameter coefficient matrix. and time-varying weight matrix , so that the vector error function asymptotically converges to the zero vector; when , The iteration stops at the preset convergence accuracy threshold, and the optimized vector output by the standardized adaptive zeroing neural network is then determined. In this context, the component corresponding to the joint space velocity is the optimal decision variable in the multi-objective joint constraint model. ; The optimal decision variables obtained by solving The mapping is performed as real-time motion control commands for each joint of the dual-redundant robotic arm. The mapping process is achieved through signal conversion by the joint servo controller, which converts the optimal solution of the joint space velocity into a voltage signal that can be recognized by the servo motor. The real-time motion control commands are synchronously output to the servo actuators of each joint of the dual-redundant robotic arm, driving the robotic arm to move smoothly along the planned trajectory and completing the robot's safe motion planning.

Images