CN113084797B - A Dynamic Cooperative Control Method of Dual-Arm Redundant Manipulators Based on Task Decomposition - Google Patents

Abstract

The invention discloses a dynamic cooperative control method of double-arm redundant mechanical arms based on task decomposition, which comprises the following steps: giving a target task of two arms; selecting a plurality of characteristic points for the two arms; establishing a kinematic equation of a double-arm system, and solving the position and the speed of a control point of a double arm; establishing a self-obstacle avoidance task of two arms through a control point, establishing a joint speed general solution equation of the redundant mechanical arm, and adding a dynamic decision factor and the self-obstacle avoidance task of the two arms; until both arms complete the target task. The dynamic decision control method for redundant double-arm kinematics task decomposition is realized, and different control strategies can be adopted according to the real-time motion states of double arms, so that the double arms can complete given target tasks on the basis of ensuring safety.

Description

A Dynamic Cooperative Control Method of Dual-Arm Redundant Manipulators Based on Task Decomposition

technical field

The invention belongs to the technical field of decision-making control of a dual-arm robot, and relates to a dynamic cooperative control method of dual-arm redundant mechanical arms based on task decomposition.

Background technique

With the development of science and technology and the progress of industrial production, it is often difficult for a single manipulator system to meet the automation and intelligence requirements of robots. The dual redundant manipulator system is used as a research tool for robotics due to its high adaptability, strong collaboration and high flexibility. One of the key points. Because of the problem of overlapping operating spaces in the dual-arm system, the research on dual-arm coordination and collision avoidance methods is the focus of the dual-arm system.

At present, there are many methods and strategies for the coordinated control of the arms. The focus of many studies is to keep the arms at a reasonable safe distance during the motion control process; or to find a collision-free path in the feasible space. To solve the problem of self-obstacle avoidance with both arms, there are currently the following solutions:

(1) Plan a collision-free path from the start point to the end point in the configuration space;

(2) Normalize the initial planned path, establish a coordination space, and change the speed on the path point to make the arms avoid each other before collision;

(3) The distance between the arms is used as the limiting condition for motion control to maintain a safe distance between the arms;

(4) A potential field is established for the arms, and there is mutual repulsion between the arms, which guides the arms to maintain a relatively safe distance.

However, in the above method, the multi-degree-of-freedom dual-arm system has high-dimensional calculation problems in configuration space and coordination space. The multi-degree-of-freedom manipulator system has high computational complexity, and it is difficult to meet the timeliness requirements of real-time control; the distance function of the restricted arms only considers the relative distance between the arms, and does not take the relative speed as a consideration. When running at high speed, it is difficult to avoid the special situation of impending collision at high speed. Wu Changzheng and others proposed a method of self-collision detection of manipulators based on space vector geometric distance in "Self-collision detection and motion planning of dual-arm robots", and through the improved artificial potential field method, the linear repulsion is used as the algorithm The child performs self-obstacle avoidance planning. Methods such as constructing the potential field construct the corresponding field function by integrating the connecting rod of the manipulator, and use the field function as the basis for the control of the manipulator. On the one hand, the construction field in three-dimensional space increases the computational complexity; on the other hand, the potential field method has the problem of causing the manipulator to fall into a local minimum, and it is difficult to complete the task in the end. In view of the above problems, on the one hand, this method uses the relative speed between the two arms as one of the control basis, which can deal with the dangerous situation of approaching high speed between the two arms; on the other hand, it directly constructs the obstacle avoidance task in the workspace, avoiding the Or the transformation of other spaces, which improves the planning efficiency and enhances the real-time performance of the manipulator control.

Contents of the invention

The purpose of the present invention is to overcome the above-mentioned shortcomings of the prior art, and provide a dynamic cooperative control method for dual-arm redundant manipulators based on task decomposition.

The present invention is realized through at least one of the following technical solutions.

A method for dynamic cooperative control of dual-arm redundant manipulators based on task decomposition, comprising the following steps:

Step 1, given the respective target tasks of both arms;

Step 2. Set multiple feature points on the arms to indicate the spatial position of the arms;

Step 3. Establish the kinematic equation of the dual-arm system, solve the spatial position of the feature point and determine the spatial position of the control point accordingly, and obtain the spatial velocity of the dual-arm control point;

Step 4. Determine the self-obstacle avoidance task of both arms, establish the joint velocity general solution equation of the redundant mechanical arm, and add dynamic decision factors and the self-obstacle avoidance task of both arms;

Step 5. Determine whether the arms have completed the target task, if not, go to step 3.

Preferably, step 1 is specifically: given the target position of the end of the manipulator in Cartesian space:

[x _d ,y _d ,z _d ]

Among them, x _d , y _d , z _d are the three-dimensional coordinate values in the coordinate system of the base of the manipulator, respectively.

Preferably, step 2 is specifically: setting multiple feature points at the joints and connecting rods of the arms, setting feature points at the centers of each joint of the mechanical arm, and setting the feature points at the centers of the first three connecting rods of the mechanical arm Feature points.

Preferably, determining the spatial position of the control point is specifically as follows:

Taking the base coordinate system of a certain side arm as the reference coordinate system, the positions of the 2n feature points of the double arm are obtained through the positive kinematic equation, and P _ri (i=1,2,...,n) represents the i-th point of the side arm The position of the feature point, n represents the number of feature points, and P _lj (j=1,2,...,n) represents the position of the jth feature point of the other side arm:

P _ri =f _ri (q _r )=(x _ri ,y _ri ,z _ri )

f _ri (·) is the forward kinematics formula at point P _ri , q _r is the joint angle vector of the side arm, (x _ri , y _ri , z _ri ) is the three-dimensional space coordinate value of P _ri ;

P _li ＝f _li (q _l )+[0,L,0]=(x _li ,y _li ,z _li )

f _li (·) is the forward kinematics formula at point P _li , q _l is the joint angle vector of the other arm, (x _li , y _li , z _li ) is the coordinate value of P _li ; calculate the characteristics of both arms The distance between points, use d _ij to represent the distance between the i-th feature point of one side arm and the j-th feature point of the other side arm:

d _ij =‖P _ri -P _lj ‖

Maintain a distance of n×n as D, and select the two feature points with the shortest distance between the feature points of the arms as the control points of the arms, use P _r to represent the control point position of one arm, and use P _l to represent the other side The control point position of the arm.

Preferably, in step 3, the space velocity of the dual-arm control point is obtained through the Jacobian matrix.

Preferably, to obtain the space velocity of the dual-arm control point is specifically:

Through the joint velocity vector of both arms and the Jacobian matrix at the control point, the Cartesian space velocity at the control point is obtained, v _r represents the control point velocity of one arm, and v _l represents the control point velocity of the other arm :

和

分别为双臂此时的关节速度向量。Among them, J _r and J _l are the Jacobian matrix of P _r and P _l respectively,

and

are the joint velocity vectors of both arms at this time.

Preferably, the step 4 is specifically:

Determine the relative position vector P _a of two control points:

P _a =P _r -P _l

Set the retreat speed as v _b , expand the speed of the distance between the control points of the arms, the direction of the speed is the same as the direction of the relative position vector, and the magnitude of the speed changes with the distance of the control points:

In the formula, d represents the spatial distance between the control points of the two arms, γ _b controls the speed of the obstacle avoidance speed changing with d, and σ _b is the control threshold at which the obstacle avoidance speed starts to work;

Determine the relative velocity vector v _a of two control points:

v _a =v _l -v _r

The angle between the relative position vector and the relative velocity vector

Set the dodge speed v _d , when one side arm continues to approach, the corresponding control point of the other side arm produces a sideways dodge speed, its direction is orthogonal to the relative position vector and relative speed vector, and its magnitude varies with the distance from the control point Variety:

In the formula, γ _d controls the speed at which the dynamic decision-making factor changes with d, and γ _d is the control threshold parameter at which the dynamic decision-making factor starts to take effect;

Add the retreat speed and dodge speed to get the dodge speed v _h :

v _h =v _b +v _d

Map evasion velocities to joint space via the Jacobian matrix of the control points

为控制点的雅可比矩阵的广义逆矩阵；In the formula

is the generalized inverse matrix of the Jacobian matrix of the control points;

For redundant manipulators, assuming that its degree of freedom is m, its i-th joint angle is q _i (i=1, 2, ..., m), the corresponding joint angle vector is q, and the joint speed required to complete the target trajectory y _d The general solution is:

为目标轨迹的微分函数。In the formula, J is the Jacobian matrix corresponding to the target trajectory y _d , J ⁺ is the generalized inverse matrix of J, I _m is the m-order unit matrix, k is an arbitrary m-dimensional space vector,

is the differential function of the target trajectory.

Preferably, the added dynamic decision factor α is:

Among them, the dynamic decision factor α is:

In the formula, d represents the spatial distance between the control points of the two arms, γ controls the speed at which the dynamic decision-making factor changes with d, and σ is the control threshold at which the dynamic decision-making factor begins to work;

The angular velocity of each axis joint when the robot arm realizes the self-obstacle avoidance action is taken as the sub-motion of the robot arm movement:

Preferably, if the error between the current position of the robotic arm and the target position is within the threshold, it means that both arms have completed the target task. If the error between the current position of the robotic arm and the target position exceeds the threshold, return to step 3.

Preferably, the step 5 is specifically:

Obtain the current position and attitude of the robotic arm through the forward kinematics equation:

P _e =f _e (q)=[x _e ,y _e ,z _e ]

Among them, P _e is the position of the end of the manipulator, f _e ( ) is the forward kinematics formula of the end of the manipulator, q is the current joint angle vector of the manipulator, [x _e , y _e , z _e ] is the value of P _e Space coordinate value;

Given an error threshold ∈, if the error between the current position of the manipulator and the target position is within the threshold:

Control ends, if the error between the current position of the robot arm and the target position exceeds the threshold:

Go back to step 3.

Compared with the prior art, the present invention has the following beneficial effects:

The space position of the manipulator is represented by multiple feature points, which simplifies the space expression of the multi-link structure. Considering the relative distance and relative speed of the control points of the dual arms, the self-obstacle avoidance task of the dual arms is established. Using dynamic decision-making factors, the arms can implement different control strategies in different situations.

Description of drawings

Fig. 1 is the mechanical arm configuration diagram of this example;

Fig. 2 is a schematic diagram of the feature point selection of the arms in this example;

3 is a schematic diagram of the simulation results of the angle positions of the joints of the right arm when using the dynamic decision-making control method of task decomposition in Example 1;

4 is a schematic diagram of the simulation results of the angle positions of the joints of the left arm when using the dynamic decision-making control method of task decomposition in Example 1;

5 is a schematic diagram of the simulation results of the angle positions of the right arm joints when the dynamic decision-making control method of task decomposition is not used in Example 1;

6 is a schematic diagram of the simulation results of the angle positions of the left arm joints when the dynamic decision-making control method of task decomposition is not used in Example 1;

Fig. 7 is a schematic diagram of the shortest distance of both arms when using the dynamic decision-making control method of task decomposition in Example 1;

Fig. 8 is a schematic diagram of the shortest distance of both arms when the dynamic decision-making control method of task decomposition is not used in Example 1;

Fig. 9 is a schematic diagram of the position of the end of the right arm when using the dynamic decision-making control method of task decomposition in Example 1;

Figure 10 is a schematic diagram of the position of the arms of Example 2;

Figure 11 is a schematic diagram of the solid line of the position of the arms when using the dynamic decision-making control method of task decomposition in Example 2;

Figure 12 is a schematic diagram of the dotted line of the position of the arms when the dynamic decision-making control method of task decomposition is not used in Example 2;

Figure 13 is a schematic diagram of the position of the arms when the dynamic decision-making control method of task decomposition is not used in Example 2;

Figure 14 is a schematic diagram of the position of the ends of the arms when the dynamic decision-making control method of task decomposition is not used in Example 2;

Figure 15 is a diagram of the position of the ends of the arms when using the dynamic decision-making control method of task decomposition in Example 2;

16 is a schematic diagram of the position of the ends of the arms when using the dynamic decision-making control method of task decomposition in Example 2;

17 is a schematic diagram of the position of the ends of the arms when using the dynamic decision-making control method of task decomposition in Example 3;

Figure 18 is a schematic diagram of the position of the ends of the arms when using the dynamic decision-making control method of task decomposition in Example 3;

Figure 19 is a schematic diagram of the position of the ends of the arms when the dynamic decision-making control method of task decomposition is not used in Example 3;

Figure 20 is a schematic diagram of the position of the ends of the arms when the dynamic decision-making control method of task decomposition is not used in Example 3;

Figure 21 is a schematic diagram of the position of the ends of the arms when the dynamic decision-making control method of task decomposition is not used in Example 3;

Figure 22 is a schematic diagram of the position of the ends of the arms when using the dynamic decision-making control method of task decomposition in Example 3;

Figure 23 is a schematic diagram of the position of the ends of the arms when using the dynamic decision-making control method of task decomposition in Example 3;

Figure 24 is a schematic diagram of the position of the ends of the arms when using the dynamic decision-making control method of task decomposition in Example 4;

Figure 25 is a schematic diagram of the position of the ends of the arms when using the dynamic decision-making control method of task decomposition in Example 4;

Figure 26 is a schematic diagram of the position of the ends of the arms when the dynamic decision-making control method of task decomposition is not used in Example 4;

Figure 27 is a schematic diagram of the position of the ends of the arms when the dynamic decision-making control method of task decomposition is not used in Example 4;

Figure 28 is a schematic diagram of the position of the ends of the arms when the dynamic decision-making control method of task decomposition is not used in Example 4;

Figure 29 is a schematic diagram of the position of the ends of the arms when using the dynamic decision-making control method of task decomposition in Example 4;

Fig. 30 is a schematic diagram of the position of the ends of the arms when the dynamic decision-making control method of task decomposition is used in Example 4.

Detailed ways

The embodiments of the present invention will be clearly and completely described below with reference to the accompanying drawings in the embodiments of the present invention.

As shown in Figures 1 and 2, a method for dynamic cooperative control of dual-arm redundant manipulators based on task decomposition in this embodiment includes the following steps:

Step 1. Given the respective target tasks of both arms, that is, the target position of the end of the given manipulator in Cartesian space:

[x _d ,y _d ,z _d ]

Among them, x _d , y _d , and z _d are the three-dimensional coordinate values in the coordinate system of the base of the manipulator, respectively.

Step 2. Set multiple feature points at the joints and connecting rods of the arms, set feature points at the center of each joint of the robot arm, and set feature points at the center of the longer connecting rod. The feature points approximately represent the spatial position of the dual arm, and multiple feature points serve as the spatial expression of the dual arm system.

Step 3. Establish the kinematic equation of the dual-arm system, solve the spatial position of the feature point and determine the spatial position of the control point, and obtain the spatial velocity of the dual-arm control point through the Jacobian matrix, specifically:

Taking the base coordinate system of the right arm as the reference coordinate system, the position of each feature point of the double arm is obtained through the positive kinematic equation:

P _ri = f _ri (q _r ) = (x _ri , y _ri , z _ri )

Among them, (x _ri , y _ri , z _ri ) is the three-dimensional space coordinate value of point P _ri , f _ri ( ) is the positive kinematics formula at point P _ri , q _r is the joint vector of the right arm, P _ri ( i=1, 2,...,n) represents the position of the i-th feature point on the right arm, n represents the number of feature points, and P _lj (j=1, 2,...,n) represents the j-th feature point on the left arm s position:

P _li =f _li (q _l )+[0, L, 0]=(x _li , y _li , z _li )

Among them, f _li (·) is the forward kinematics formula at point P _li , q _l is the joint vector of the left arm, (x _li , y _li , z _li ) is the three-dimensional space coordinate value of P _li .

Calculate the distance between each feature point of both arms, and use d _ij to represent the distance between the i-th feature point of the right arm and the j-th feature point of the left arm:

d _ij =‖P _ri -P _lj ‖

Maintain a distance of n×n as D, and select the two feature points with the shortest distance between the feature points of the arms as the control points of the arms, use P _r to represent the control point position of the right arm, and use P _l to represent the control point of the left arm point location.

The Cartesian space velocity at the control point can be obtained by the joint velocity vector of the arms at this time and the Jacobian matrix at the control point:

和

分别为右臂与左臂此时的关节速度向量，v_r代表右臂的控制点速度，v_l代表左臂的控制点速度。Among them, J _r and J _l are the Jacobian matrix of P _r and P _l respectively,

and

are the joint velocity vectors of the right arm and the left arm at this time, v _r represents the control point velocity of the right arm, and v _l represents the control point velocity of the left arm.

Step 4. Determine the self-obstacle avoidance task of both arms, establish the joint velocity general solution equation of the redundant manipulator, and add the dynamic decision-making factor and the self-obstacle avoidance task of both arms, specifically:

Determine the relative position vector P _a of two control points:

P _a =P _r -P _l

Set the retreat speed v _b : the speed of expanding the distance between the control points of both arms. Its direction is the same as the relative position vector, and its magnitude varies with the control point distance:

In the formula, d represents the spatial distance between the control points of the two arms, γ _b controls the speed of the obstacle avoidance speed changing with d, and σ _b is the control threshold at which the obstacle avoidance speed starts to work;

Determine the relative velocity vector v _a of two control points:

v _a =v _l -v _r

The angle between the relative position vector and the relative velocity vector

Set dodge speed v _d : Dodge sideways to the approaching left arm control point. Its direction is orthogonal to the relative position vector and relative velocity vector, and its magnitude varies with the distance from the control point:

In the formula, d represents the spatial distance between the control points of the two arms, γ _b controls the speed of the obstacle avoidance speed changing with d, and σ _b is the control threshold at which the obstacle avoidance speed starts to work.

Add the retreat speed and dodge speed to get the dodge speed v _h :

v _h =v _b +v _d

Map evasion velocities to joint space via the Jacobian matrix of the control points

为控制点的雅可比矩阵的广义逆矩阵。In the formula

is the generalized inverse of the Jacobian matrix for the control points.

For redundant manipulators, assuming that its degree of freedom is m, its i-th joint angle is q _i (i=1, 2,..., m), the corresponding joint vector is q, and the required joint speed to complete the target trajectory y _d is General interpretation is

为目标轨迹的微分函数。In the formula, J is the Jacobian matrix corresponding to the y _d target trajectory, J ⁺ is the generalized inverse matrix of J, I _m is the m-order unit matrix, k is an arbitrary m-dimensional space vector,

is the differential function of the target trajectory.

Add dynamic decision factor α:

Among them, the dynamic decision factor α is:

The angular velocity of each axis joint when the robot arm realizes the self-obstacle avoidance action is taken as the sub-motion of the robot arm movement.

Step 5. Determine whether the arms have completed the target task, if not, go to step 3.

Obtain the current position and attitude of the robotic arm through the forward kinematics equation:

P _e = f _e (q) = [x _e , y _e , z _e ]

Among them, P _e is the position of the end of the manipulator, f _e ( ) is the forward kinematics formula of the end of the manipulator, q is the current joint vector of the manipulator, [x _e , y _e , z _e ] is the three _- dimensional Space coordinate value.

Given an error threshold ∈, if the error between the current position of the robot arm and the target position is within a certain threshold:

The control method ends. If the error between the current position of the robot arm and the target position exceeds the threshold:

Go back to step 3.

Method of the present invention is applied in concrete example 1 below:

1) Given a target location:

2) Left arm target position: [0.300; -0.300; 0.000],

3) The current position of the left arm:

4) q _l [0.1; -1.27; -1.574; -0.3045; 2.854; 0.0] → [-0.4930; -0.8244; -1.1924; -0.3680; 3.1993; 0.0]

5) Right arm target position: [0.500; 0.200; -0.250],

6) The current position of the right arm:

7) q _r [0.538; -0.883; -0.8633; -0.13; 2.643; 0] → [-0.1959; -0.4492; -1.8690;

The simulation results are shown in Fig. 3, Fig. 4, the solid line in Fig. 7, Fig. 8 and Fig. 9. It can be seen from the simulation results that when the dynamic decision-making control method of redundant dual-arm kinematic task decomposition is used, the While ensuring a safe distance, both ends have reached the target position, and the target task is completed on the basis of ensuring safety. As shown by the dotted lines in Figure 5, Figure 6, and Figure 7, when the dynamic decision-making control method of kinematic task decomposition of redundant arms is not used, the arms have already collided at the beginning of the movement, and the task cannot be continued, and the movement fails .

Method of the present invention is applied in concrete example 2 below:

1) Given a target location:

2) Left arm target position: [0.350; -0.260; 0.050],

3) The current position of the left arm:

4) q _l [0.1; -1.27; -1.574; -0.3045; 2.854; 0.0] → [-0.3791; -1.1397; -1.4941; -0.3544; 2.8478; 0.0]

5) Right arm target position: [0.500; 0.250; -0.200],

6) The current position of the right arm:

7) q _r [0.538; -0.883; -0.8633; -0.13; 2.643; 0] → [-0.0275; -0.6141; -1.8763;

The simulation results are shown in Fig. 10, Fig. 11, the solid line in Fig. 14, Fig. 15 and Fig. 16. From the simulation results, it can be seen that when using the dynamic decision-making control method of redundant dual-arm kinematic task decomposition, the dual-arm While ensuring a safe distance, both ends have reached the target position, and the target task is completed on the basis of ensuring safety. As shown by the dotted lines in Figure 12, Figure 13, and Figure 14, when the dynamic decision-making control method of redundant dual-arm kinematics task decomposition is not used, the arms have already collided in the early stage of motion, and cannot continue to complete the task, and the motion fails .

Method of the present invention is applied in concrete example 3 below:

1) Given a target location:

2) Left arm target position: [0.320; -0.280; 0.025],

3) The current position of the left arm:

4) q _l [0.1; -1.27; -1.574; -0.3045; 2.854; 0.0] → [-0.4866; -1.2512; -1.7589; -0.5077; 2.6847; 0.0]

5) Right arm target position: [0.450; 0.230; -0.200],

6) The current position of the right arm:

7) q _r [0.538; -0.883; -0.8633; -0.13; 2.643; 0] → [-0.1263; -0.6679; -2.1252;

The simulation results are shown in Fig. 17, Fig. 18, the solid line in Fig. 21, Fig. 22 and Fig. 23. From the simulation results, it can be seen that when the dynamic decision-making control method of redundant dual-arm kinematic task decomposition is used, the dual-arm While ensuring a safe distance, both ends have reached the target position, and the target task is completed on the basis of ensuring safety. As shown by the dotted lines in Figure 19, Figure 20, and Figure 21, when the dynamic decision-making control method of redundant dual-arm kinematic task decomposition is not used, the arms have already collided in the early stage of motion, and cannot continue to complete the task, and the motion fails .

Method of the present invention is applied in concrete example 4 below:

1) Given a target location:

2) Left arm target position: [0.320; -0.260; -0.050],

3) The current position of the left arm:

4) q _l [0.1; -1.27; -1.574; -0.3045; 2.854; 0.0] → [-0.3855; -0.9143; -1.5747; -0.6605; 3.0188; 0.0]

5) Right arm target position: [0.450; 0.220; -0.250],

6) The current position of the right arm:

7) q _r [0.538; -0.883; -0.8633; -0.13; 2.643; 0] → [-0.0956; -0.4515; -2.0497; -1.5670;

The simulation results are shown in Figure 24, Figure 25, the solid line in Figure 28, Figure 29 and Figure 30. From the simulation results, it can be seen that when using the dynamic decision-making control method of redundant dual-arm kinematic task decomposition, the dual-arm While ensuring a safe distance, both ends have reached the target position, and the target task is completed on the basis of ensuring safety. As shown by the dotted lines in Figure 26, Figure 27, and Figure 28, when the dynamic decision-making control method of redundant dual-arm kinematic task decomposition is not used, the arms have already collided in the early stage of motion, and cannot continue to complete the task, and the motion fails .

The above content is only to illustrate the technical ideas of the present invention, and cannot limit the protection scope of the present invention. Any changes made on the basis of the technical solutions according to the technical ideas proposed in the present invention shall fall within the scope of the claims of the present invention. within the scope of protection.

Claims (9)

1. A dual-arm redundant mechanical arm dynamic cooperative control method based on task decomposition, is characterized in that, comprises the following steps: Step 1, given the respective target tasks of both arms; Step 2. Set multiple feature points on the arms to indicate the spatial position of the arms; Step 3. Establish the kinematic equation of the dual-arm system, solve the spatial position of the feature point and determine the spatial position of the control point accordingly, and obtain the spatial velocity of the dual-arm control point; Step 4. Determine the self-obstacle avoidance task of both arms, establish the joint velocity general solution equation of the redundant manipulator, and add dynamic decision factors and the self-obstacle avoidance task of both arms; step 4 is specifically: Determine the relative position vector P _a of two control points: P _a =P _r -P _l In the formula, P _r represents the control point position of one arm, and P _l represents the control point position of the other arm Set the retreat speed as v _b , expand the speed of the distance between the control points of the arms, the direction of the speed is the same as the direction of the relative position vector, and the magnitude of the speed changes with the distance of the control points:

In the formula, d represents the spatial distance between the control points of the two arms, γ _b controls the speed of the obstacle avoidance speed changing with d, and σ _b is the control threshold at which the obstacle avoidance speed starts to work; Determine the relative velocity vector v _a of two control points: v _a =v _l -v _r In the formula, v _r represents the control point velocity of one arm, and v _l represents the control point velocity of the other arm; The angle between the relative position vector and the relative velocity vector

Set the dodge speed v _d , when one side arm continues to approach, the corresponding control point of the other side arm produces a sideways dodge speed, its direction is orthogonal to the relative position vector and relative speed vector, and its magnitude varies with the distance from the control point Variety:

In the formula, d represents the spatial distance between the control points of the two arms, γ _d controls the speed at which the obstacle avoidance speed changes with d, and σ _d is the control threshold at which the obstacle avoidance speed begins to work; Add the retreat speed and dodge speed to get the dodge speed v _h : v _h =v _b +v _d Map evasion velocities to joint space via the Jacobian matrix of the control points

In the formula

is the generalized inverse matrix of the Jacobian matrix of the control points; For a redundant robotic arm, assuming that its degree of freedom is m, its i-th joint angle is q _i (i=1,2,…,m), the corresponding joint angle vector is q, and the joint speed required to complete the target trajectory y _d The general solution is:

In the formula, J is the Jacobian matrix corresponding to the target trajectory y _d , J ⁺ is the generalized inverse matrix of J, I _m is the m-order unit matrix, k is an arbitrary m-dimensional space vector,

is the differential function of the target trajectory; Step 5. Determine whether the arms have completed the target task, if not, go to step 3. 2. The dual-arm redundant manipulator dynamic cooperative control method based on task decomposition according to claim 1, wherein step 1 is specifically: given the target position of the end of the manipulator in Cartesian space: [x _d ,y _d ,z _d ] Among them, x _d , y _d , z _d are the three-dimensional coordinate values in the coordinate system of the base of the manipulator, respectively. 3. The dynamic cooperative control method of dual-arm redundant manipulators based on task decomposition according to claim 2, wherein step 2 is specifically: setting a plurality of feature points at the joints and connecting rods of both arms, and Set feature points at the centers of each joint of the manipulator, and set feature points at the centers of the first three links of the manipulator. 4. the dual-arm redundant manipulator dynamic cooperative control method based on task decomposition according to claim 3, is characterized in that, the spatial position of determining control point is specifically as follows: Taking the base coordinate system of a certain side arm as the reference coordinate system, the positions of the 2n feature points of the double arm are obtained through the positive kinematic equation, and P _ri (i=1,2,...,n) represents the i-th point of the side arm The position of the feature point, n represents the number of feature points, and P _lj (j=1,2,...,n) represents the position of the jth feature point of the other side arm: P _ri =f _ri (q _r )=(x _ri ,y _ri ,z _ri ) f _ri (·) is the forward kinematics formula at point P _ri , q _r is the joint angle vector of the side arm, (x _ri , y _ri , z _ri ) is the three-dimensional space coordinate value of P _ri ; P _li ＝f _li (q _l )+[0,L,0]=(x _li ,y _li ,z _li ) f _li (·) is the forward kinematics formula at point P _li , q _l is the joint angle vector of the other arm, (x _li , y _li , z _li ) is the coordinate value of P _li ; calculate the characteristics of both arms The distance between points, use d _ij to represent the distance between the i-th feature point of one side arm and the j-th feature point of the other side arm: d _ij =‖P _ri -P _lj ‖ Maintain a distance of n×n as D, and select the two feature points with the shortest distance between the feature points of the arms as the control points of the arms, use P _r to represent the control point position of one arm, and use P _l to represent the other side The control point position of the arm. 5. The dynamic cooperative control method of dual-arm redundant manipulators based on task decomposition according to claim 4, characterized in that, in step 3, the space velocity of the dual-arm control points is obtained through the Jacobian matrix. 6. The dynamic cooperative control method of dual-arm redundant manipulators based on task decomposition according to claim 5, wherein obtaining the spatial velocity of the dual-arm control points is specifically: Through the joint velocity vector of both arms and the Jacobian matrix at the control point, the Cartesian space velocity at the control point is obtained, v _r represents the control point velocity of one arm, and v _l represents the control point velocity of the other arm :

Among them, J _r and J _l are the Jacobian matrix of P _r and P _l respectively,

and

are the joint velocity vectors of both arms at this time. 7. The dynamic cooperative control method of dual-arm redundant manipulators based on task decomposition according to claim 6, wherein the dynamic decision factor α added is:

Among them, the dynamic decision factor α is:

In the formula, d represents the spatial distance between the control points of the two arms, γ controls the speed at which the dynamic decision-making factor changes with d, and σ is the control threshold at which the dynamic decision-making factor begins to work; The angular velocity of each axis joint when the robot arm realizes the self-obstacle avoidance action is taken as the sub-motion of the robot arm movement:

8. The dynamic cooperative control method of dual-arm redundant manipulators based on task decomposition according to claim 7, wherein the error between the current position of the manipulator and the target position is within a threshold, indicating that both arms have completed the target task, If the error between the current position of the robot arm and the target position exceeds the threshold, return to step 3. 9. The dual-arm redundant mechanical arm dynamic cooperative control method based on task decomposition according to claim 8, wherein the step 5 is specifically: Obtain the current position and attitude of the robotic arm through the forward kinematics equation: P _e =f _e (q)=[x _e ,y _e ,z _e ] Among them, P _e is the position of the end of the manipulator, f _e ( ) is the forward kinematics formula of the end of the manipulator, q is the current joint angle vector of the manipulator, [x _e , y _e , z _e ] is the value of P _e Space coordinate value; Given an error threshold ∈, if the error between the current position of the manipulator and the target position is within the threshold:

Control ends, if the error between the current position of the robot arm and the target position exceeds the threshold:

Go back to step 3.

Images