Seven-degree-of-freedom mechanical arm trajectory planning method based on null space optimization

Abstract

The invention discloses a seven-degree-of-freedom mechanical arm trajectory planning method based on null space optimization, relates to the technical field of mechanical arm intelligent control, and aims to solve the problems that an existing mechanical arm trajectory planning method easily causes sudden posture change and affects trajectory continuity and motion stability when Euler angle velocity is adopted for posture planning. According to the method, the Jacobian matrix is adopted for operation without depending on a complex inverse kinematics calculation process, and the method can be suitable for redundant mechanical arms of any configuration; according to the method, the tail end trajectory is planned in a pose matrix mode, joint layer optimization is achieved through a null space formed by redundant freedom degrees, pose sudden change easily caused when the Euler angle velocity is adopted for pose planning is avoided, and the continuity and motion stability of trajectory planning of the mechanical arm are improved.

Description

TECHNICAL FIELD

[0001] The application relates to the technical field of intelligent control of mechanical arms, in particular to a seven-degree-of-freedom mechanical arm trajectory planning method based on zero space optimization. BACKGROUND

[0002] With the rapid development of automation technology, mechanical arms are widely used in industrial production, space operation, medical assistance and other fields. In view of the increasingly complex operation requirements, mechanical arms with redundant degrees of freedom (the number of degrees of freedom is greater than 6) gradually become the mainstream configuration. The increase of redundant degrees of freedom can significantly improve the operability and task adaptability of the mechanical arm, but at the same time, it also brings new trajectory planning problems.

[0003] Taking a seven-degree-of-freedom mechanical arm as an example, the motion mapping from Cartesian space to joint space has non-uniqueness, and only part of the specific configuration can obtain a position-level inverse kinematics analytical solution. Such analytical solution is usually complex to deduce, and needs to additionally introduce an arm type angle parameter, or degenerate the problem into a six-degree-of-freedom inverse solution by locking a certain joint angle, thereby limiting the play of the redundant degrees of freedom.

[0004] The existing trajectory planning method of the redundant mechanical arm mostly adopts a velocity-level planning method, that is, by setting the end motion velocity, and using the Jacobian matrix to map it into a joint velocity instruction to drive the motion of the mechanical arm. However, when the Euler angle angular velocity is used for attitude planning, the method is easy to cause attitude mutation, affecting the trajectory continuity and motion stability. SUMMARY

[0005] The purpose of the present application is to solve the problem that the existing mechanical arm trajectory planning method is easy to cause attitude mutation when the Euler angle angular velocity is used for attitude planning, affecting the trajectory continuity and motion stability, and to provide a seven-degree-of-freedom mechanical arm trajectory planning method based on zero space optimization.

[0006] The technical scheme adopted by the present application to solve the above technical problem is:

[0007] A seven-degree-of-freedom mechanical arm trajectory planning method based on zero space optimization, comprising the following steps:

[0008] Step 1: setting the desired motion trajectory of the mechanical arm end effector , the desired motion trajectory of the mechanical arm end effector contains the desired attitude and the desired position ;

[0009] Step 2: based on the kinematic model parameters and joint angles of the mechanical arm , obtaining the actual end pose matrix and Jacobian matrix of the mechanical arm;

[0010] Step 3: Obtain the end-effector linear velocity vector and the end-effector angular velocity vector using the actual end-effector pose matrix of the robot arm, the desired pose and the desired position ;

[0011] Step 4: Obtain the pseudo-inverse of the Jacobian matrix , and combine the end-effector linear velocity vector and the end-effector angular velocity vector to obtain the joint velocity ;

[0012] Step 5: Obtain the null-space optimized velocity , and use the null-space optimized velocity and the joint velocity to obtain the joint planning velocity ;

[0013] Step 6: Integrate the joint planning velocity to obtain the joint planning position ;

[0014] Step 7: Drive the robot arm motion based on the joint planning velocity and the joint planning position to complete the trajectory planning.

[0015] Further, the desired motion trajectory of the end-effector of the robot arm is represented as:

[0016] ,

[0017] where is the desired pose, is the desired position.

[0018] Further, the actual end-effector pose matrix of the robot arm is represented as:

[0019] ,

[0020] The Jacobian matrix is represented as:

[0021] ,

[0022] where is the current end-effector pose matrix of the robot arm, is the current end-effector position of the robot arm, is the current position vector of the robot arm, is the kinematic model parameter of the robot arm, is the forward kinematics function of the robot arm, A function of the Jacobian matrix of the robot arm.

[0023] Further, the specific steps of step 3 are:

[0024] Step 31: using the desired position and the current end position of the robot arm , the end position error is obtained, which is expressed as:

[0025] ;

[0026] Step 32: using the desired attitude and the current end attitude matrix of the robot arm , the attitude error matrix is obtained, which is expressed as:

[0027] ;

[0028] Step 33: performing matrix logarithm operation on the attitude error matrix to obtain the skew-symmetric matrix , which is expressed as:

[0029] ,

[0030] wherein is the matrix logarithm function;

[0031] Step 34: using the skew-symmetric matrix , the end angle error is obtained, which is expressed as:

[0032] ,

[0033] wherein is the skew-symmetric matrix vectorization function;

[0034] Step 35: using the end angle error , the end linear velocity vector and the end angular velocity vector are generated, which are expressed as:

[0035] ,

[0036] ,

[0037] wherein and are the set linear velocity gain matrix and angular velocity gain matrix, respectively.

[0038] Further, the pseudo-inverse is expressed as:

[0039] ,

[0040] wherein, is the transpose of the current Jacobian matrix of the robot arm.

[0041] Further, the joint velocity is expressed as:

[0042] .

[0043] Further, the null-space optimized velocity is obtained by the following steps:

[0044] Step 51: solving the null-space projection matrix is expressed as:

[0045] ,

[0046] wherein, is the identity matrix;

[0047] Step 52: generating the null-space optimized term with joint weight matrix and calculating its gradient vector is expressed as:

[0048] ,

[0049] ,

[0050] wherein, is the initial angle vector of the joints, is the diagonal positive definite weight matrix;

[0051] Step 53: using the null-space projection matrix and the gradient vector to obtain the null-space optimized velocity is expressed as:

[0052] ,

[0053] wherein, is the proportional coefficient of the optimized term.

[0054] Further, the joint planning velocity is expressed as:

[0055] .

[0056] Further, the joint planning position is expressed as:

[0057] .

[0058] Further, the specific steps of step 7 are:

[0059] The driving torque required for generating the servo joint planning position and joint planning speed of the mechanical arm joint controller is generated, the movement of the mechanical arm is driven, and the trajectory planning is completed.

[0060] The beneficial effects of the present application are:

[0061] The present application adopts the Jacobian matrix for operation and does not depend on the complex inverse kinematics calculation process, and can be applied to any configuration of the redundant mechanical arm; the method adopts the pose matrix mode to plan the end trajectory, and utilizes the null space formed by the redundant degrees of freedom to realize joint layer optimization, avoids the attitude mutation easily caused when the Euler angle angular velocity is used for attitude planning, and improves the continuity and motion stability of the mechanical arm trajectory planning. BRIEF DESCRIPTION OF DRAWINGS

[0062] Figure 1 It is a trajectory planning method flow chart;

[0063] Figure 2 It is an initial configuration diagram in the process of executing the inverted "8" trajectory planning by the mechanical arm end;

[0064] Figure 3 It is a "topmost point" configuration diagram in the process of executing the inverted "8" trajectory planning by the mechanical arm end;

[0065] Figure 4 It is a "bottommost point" configuration diagram in the process of executing the inverted "8" trajectory planning by the mechanical arm end;

[0066] Figure 5 It is a "leftmost point" configuration diagram in the process of executing the inverted "8" trajectory planning by the mechanical arm end;

[0067] Figure 6 It is a "crossing midpoint" configuration diagram in the process of executing the inverted "8" trajectory planning by the mechanical arm end;

[0068] Figure 7 It is a final configuration diagram in the process of executing the inverted "8" trajectory planning by the mechanical arm end;

[0069] Figure 8 It is a mechanical arm joint 1 planning position schematic diagram;

[0070] Figure 9 It is a mechanical arm joint 2 planning position schematic diagram;

[0071] Figure 10 It is a mechanical arm joint 3 planning position schematic diagram;

[0072] Figure 11 Position planning schematic for mechanical arm joint 4;

[0073] Figure 12 Position planning schematic for mechanical arm joint 5;

[0074] Figure 13 Position planning schematic for mechanical arm joint 6;

[0075] Figure 14 Position planning schematic for mechanical arm joint 7;

[0076] Figure 15 Velocity planning schematic for mechanical arm joint 1;

[0077] Figure 16 Velocity planning schematic for mechanical arm joint 2;

[0078] Figure 17 Velocity planning schematic for mechanical arm joint 3;

[0079] Figure 18 Velocity planning schematic for mechanical arm joint 4;

[0080] Figure 19 Velocity planning schematic for mechanical arm joint 5;

[0081] Figure 20 Velocity planning schematic for mechanical arm joint 6;

[0082] Figure 21 Velocity planning schematic for mechanical arm joint 7. DETAILED DESCRIPTION

[0083] It should be particularly noted that the various embodiments disclosed in the present application can be combined with each other without conflict.

[0084] Embodiment one: according to the task requirement setting, the step one of the embodiment sets the desired motion trajectory of the end effector of the mechanical arm , the trajectory includes a desired position vector and a desired attitude matrix:

[0085] ,

[0086] In the formula, is the planned time-varying desired end pose matrix, is the desired attitude described in the form of a rotation matrix, is the desired position.

[0087] Step two, based on the kinematic model parameters and joint angles of the mechanical arm , the actual end pose matrix and the Jacobian matrix of the mechanical arm are calculated.

[0088] ,

[0089] ,

[0090] where, is the current end-effector pose matrix of the robot arm, is the current end-effector position of the robot arm, is the current Jacobian matrix of the robot arm, is the current position vector of the robot arm, is the kinematic model parameter of the robot arm, is the forward kinematics function of the robot arm, is the Jacobian matrix function of the robot arm.

[0091] Step three, using and , get the planned end-effector linear velocity vector and end-effector angular velocity vector

[0092] Step three one, calculate the end-effector position error:

[0093] ,

[0094] Step three two, calculate the end-effector pose error, get the pose error matrix :

[0095] ,

[0096] Perform matrix logarithm operation on the pose error matrix , get the corresponding skew-symmetric matrix :

[0097] ,

[0098] where, is the matrix logarithm function.

[0099] From the skew-symmetric matrix , get the end-effector angle error:

[0100] ,

[0101] where, is the skew-symmetric matrix vectorization function.

[0102] Step three three, generate the desired end-effector linear velocity vector and angular velocity vector :

[0103] ,

[0104] ,

[0105] wherein, and are the set linear and angular velocity gain matrices, respectively.

[0106] Step four, calculate the pseudo-inverse of the current Jacobian matrix of the robot arm and solve the joint velocity :

[0107] ,

[0108] ,

[0109] wherein, is the transpose of the current Jacobian matrix of the robot arm.

[0110] Step five, establish a null-space optimization index and solve the null-space optimization velocity.

[0111] Step five one, solve the null-space projection matrix :

[0112] ,

[0113] wherein, is the identity matrix.

[0114] Step five two, generate the null-space optimization term with joint weight matrix and calculate its gradient vector :

[0115] ,

[0116] ,

[0117] wherein, is the initial angle vector of the joint, is a diagonal positive definite weight matrix.

[0118] Step five three, calculate the null-space optimization velocity :

[0119] ,

[0120] wherein, is the proportional coefficient of the optimization term.

[0121] Step six, synthesize the joint planning velocity and integrate to obtain the joint planning position .

[0122] ,

[0123] ,

[0124] Step seven, generate the driving torque required by the servo joint planning position and joint planning speed from the joint controller of the mechanical arm, drive the movement of the mechanical arm, complete the trajectory planning.

[0125] The mechanical arm system is composed of a seven-degree-of-freedom mechanical arm and an end effector.

[0126] Step one, set the desired motion trajectory of the end effector of the mechanical arm as an inverted "8" character, as shown in Figures 2 to 7 .

[0127] ,

[0128] Step two, calculate the actual end pose matrix and Jacobian matrix of the mechanical arm based on the kinematic model parameters of the mechanical arm and the current joint angular velocity:

[0129] ,

[0130] ,

[0131] Step three, generate the planned end linear velocity vector and end angular velocity vector according to the desired end pose matrix and the actual pose matrix of the mechanical arm:

[0132] The difference between the desired position vector of the end effector and its actual position vector can be defined as the end position error vector, that is:

[0133] ,

[0134] The rotational difference from the current pose to the desired pose is recorded as the end pose error, and the pose error matrix is obtained:

[0135] ,

[0136] The matrix logarithm operation is performed on the pose error matrix to obtain the corresponding skew-symmetric matrix :

[0137] ,

[0138] From the skew-symmetric matrix , the end angular error is obtained:

[0139] ,

[0140] The linear velocity gain matrix and angular velocity gain matrix generate desired end-effector linear velocity vector and angular velocity vector :

[0141] ,

[0142] ,

[0143] ,

[0144] ,

[0145] Step four, calculate the pseudo-inverse of the current Jacobian matrix of the robot arm and solve joint velocity from desired end-effector linear and angular velocities :

[0146] ,

[0147] ,

[0148] Step five, establish null-space optimization index, solve null-space optimization velocity:

[0149] solve null-space projection matrix :

[0150] ,

[0151] generate null-space optimization term with joint weight matrix and calculate its gradient vector :

[0152] ,

[0153] ,

[0154] ,

[0155] calculate null-space optimization velocity :

[0156]

[0157] ,

[0158] Step six, synthesize joint planning velocity and integrate to get joint planning position.

[0159] ,

[0160] ,

[0161] Step seven, generate the driving torque required by the servo joint controller for the planned position and velocity of the joint, drive the movement of the robot arm, and complete the trajectory planning.

[0162] To verify the effectiveness of the technical method, the following is combined with the attached Figure 1 to the attached Figure 21 Further description of the technical solutions of the present application, using computer simulation, the implementation steps are shown in the attached Figure 1 .

[0163] Embodiment:

[0164] Build a computer simulation platform, start the servo, the specific implementation steps are shown in the attached Figure 1 , in each planning period as follows:

[0165] Step 1: According to the task requirements, set the expected motion trajectory of the robot arm end effector , including the expected position vector and the expected attitude matrix ;

[0166] Step 2: Based on the kinematic model parameters of the robot arm and the current joint angular velocity, calculate the actual end pose matrix and the Jacobian matrix ;

[0167] Step 3: Calculate the end position error and the end angle error using the pose matrix;

[0168] Step 4: Set the linear velocity gain matrix and the angular velocity gain matrix , generate the planned end linear velocity vector and the end angular velocity vector ;

[0169] ,

[0170] ,

[0171] Step 5: Calculate the pseudo-inverse of the current Jacobian matrix of the robot arm , and solve the joint speed ;

[0172] ,

[0173] ,

[0174] Step 6: Solve the null space projection matrix ;

[0175] ,

[0176] Step 7: Generate null space optimization term with joint weight matrix , and calculate its gradient vector ;

[0177] ,

[0178] ,

[0179] Step 8: Calculate null space optimization velocity :

[0180] ,

[0181] Step 9: Synthesize joint planning velocity, and integrate to get joint planning position.

[0182] ,

[0183] ,

[0184] Step 10: Generate servo joint planning position and joint planning velocity required driving torque by the joint controller of the robot arm, drive the movement of the robot arm.

[0185] Step 11: Determine whether it is in place, if it has been servoed to the right place, end the planning, otherwise return to step 2 and continue planning.

[0186] The purpose of the present application is to solve the problems that the speed level trajectory planning method cannot guarantee the end-to-end accuracy and the attitude angular velocity planning is difficult in the existing redundant robot arm, and the position level trajectory planning method relies on the inverse kinematics with high complexity, resulting in low calculation efficiency and limited applicability. A seven-degree-of-freedom robot arm trajectory planning method based on null space optimization is proposed. The method plans the end trajectory in the form of pose matrix, and realizes joint layer optimization by using the null space formed by the redundant degrees of freedom, to ensure high precision end-to-end and improve the controllability and stability of the overall trajectory.

[0187] It should be noted that the specific embodiments are only an explanation and description of the technical solutions of the present application, and cannot limit the protection scope. Any partial change made according to the claims and description of the present application shall fall within the protection scope of the present application.

Claims

1. A seven-degree-of-freedom manipulator trajectory planning method based on null space optimization, characterized by The method comprises the following steps: Step 1: Set a desired motion trajectory for the end effector of the robot arm , the desired motion trajectory for the end effector of the robot arm comprises a desired pose and a desired position ; Step 2: obtaining the actual end pose matrix and Jacobian matrix of the robot arm based on the kinematic model parameters and joint angles of the robot arm , obtaining the actual end pose matrix and Jacobian matrix of the robot arm based on the kinematic model parameters and joint angles of the robot arm Step 3: Obtain the end linear velocity vector and the end angular velocity vector using the actual end pose matrix of the robot arm, the desired pose and the desired position . Step 4: Obtain the pseudo-inverse of the Jacobian matrix and combine the end-line velocity vector with the end-angle velocity vector to obtain the joint velocity ; Step 5: Obtain null-space optimized velocity and utilize null-space optimized velocity and joint velocity to obtain joint planned velocity ; Step 6: Plan the joint velocity Integrate to get the joint planned position ; Step 7: Plan velocity based on joint and joint planned position , drive the robot motion to complete the trajectory planning.

2. The trajectory planning method for a seven-degree-of-freedom manipulator based on null space optimization according to claim 1, characterized in that Desired motion trajectory of the robot arm end effector is represented as: ， wherein is a desired pose, is a desired position.

3. The zero-space optimization based trajectory planning method for a 7-DOF manipulator according to claim 2, wherein The actual end position matrix of the mechanical arm is represented as: ， The Jacobian matrix is represented as: ， wherein, is the current end pose matrix of the robot arm, is the current end position of the robot arm, is the current position vector of the robot arm, is the kinematic model parameter of the robot arm, is the forward kinematics function of the robot arm, is the Jacobian matrix function of the robot arm.

4. The zero-space optimization based trajectory planning method for a 7-DOF manipulator according to claim 3, wherein The specific steps of step 3 are: Step 31 : Utilize desired position and current end position of the robot arm resulting in an end position error is expressed as: ； Step 32: Utilize desired pose and the current end pose matrix of the robot arm , resulting in a pose error matrix , expressed as: ； Step 33: Perform a matrix logarithm operation on the attitude error matrix to obtain an anti-symmetric matrix denoted as: ， wherein is the matrix logarithm function; Step 34: Utilizing an anti-symmetric matrix , to obtain end angle error , is expressed as: ， wherein is an anti-symmetric matrix vectorization function; Step 35: Utilizing end angle error , generating end linear velocity vector and end angular velocity vector , is expressed as: ， ， wherein with are a set linear velocity gain matrix and angular velocity gain matrix, respectively.

5. The zero-space optimization based trajectory planning method for a seven degree-of-freedom robotic arm according to claim 4, wherein The pseudo-inverse is represented as: ， wherein, is the transpose of the Jacobian matrix of the robot arm at the current time.

6. The zero-space optimization based seven degree-of-freedom manipulator trajectory planning method of claim 5, wherein The joint velocity is represented as: 。 7. The zero-space optimization based seven degree-of-freedom manipulator trajectory planning method of claim 6, wherein The acquiring null space optimizes speed The specific steps are: Step 51: Solve null space projection matrix is denoted as: ， wherein is the identity matrix; Step 52: Generate null-space optimization term with joint weight matrix and compute its gradient vector denoted as: ， ， wherein, is an initial angle vector of the joint, is a diagonal positive definite weight matrix; Step 53: Utilizing the null space projection matrix and the gradient vector to obtain the null space optimized velocity is expressed as: ， wherein is a proportionality factor for the optimization term.

8. The zero-space optimization based seven degree-of-freedom manipulator trajectory planning method of claim 7, wherein The joint planning velocity is represented as: 。 9. The zero-space optimization based seven degree-of-freedom manipulator trajectory planning method of claim 8, wherein The joint planning position is represented as: 。 10. The zero-space optimization based seven degree-of-freedom manipulator trajectory planning method of claim 9, wherein The specific steps of step 7 are: The driving torque required for generating the servo joint planning position and joint planning speed by the joint controller of the mechanical arm drives the movement of the mechanical arm and completes the trajectory planning.

Images