Seven-degree-of-freedom robot trajectory inverse kinematics control method based on dynamic programming algorithm

Abstract

The application relates to the field of robot control and aims to provide a seven-degree-of-freedom mechanical arm trajectory inverse motion control method based on a dynamic programming algorithm. The method is realized by communication between an upper computer and a seven-degree-of-freedom mechanical arm through an Ethernet communication line to control the motion trajectory of the mechanical arm; the method comprises the following steps: acquiring motion parameters in real time by using a connecting rod side torque sensor built in each joint of the mechanical arm; the upper computer receives data from each sensor and calculates a spatial trajectory according to the motion parameters contained in the data; the upper computer adjusts the motor operation parameters built in each joint in real time according to the real-time motion parameters of each joint and the calculated spatial trajectory to control the actual motion direction of the seven-degree-of-freedom mechanical arm. The application can make the mechanical arm work along a predetermined route without errors; the path solution of the mechanical arm is not affected by the initial position of the mechanical arm, and the joint angles solved by reciprocating motion are consistent.

Description

Technical Field

[0001] This invention relates to a method for controlling the inverse motion of a seven-degree-of-freedom robotic arm trajectory based on a dynamic programming algorithm, belonging to the field of robot control. Background Technology

[0002] In many robotic applications, such as ultrasonic scanning robots, massage robots, and polishing robots, the robots need to move precisely along pre-planned paths, which are typically planned directly in Cartesian space. To enable a robotic arm to move along a pre-planned Cartesian path, it is necessary to determine the joint angles corresponding to the Cartesian poses of each point on the path. This involves solving the inverse kinematics problem of the robotic arm.

[0003] Traditional 6-DOF robotic arms are easily limited by singularities and joint range, making it difficult for them to achieve certain specific poses on a given path. Therefore, 7-DOF robotic arms with redundant degrees of freedom can better complete the intended task, but also pose challenges to inverse kinematics solutions.

[0004] Existing inverse kinematics control methods for robotic arms, particularly those based on the Jacobian null space and velocity-level inverse kinematics control, only consider the current position and the target position of the robotic arm at the next moment, neglecting the potential impact of the current joint angles on subsequent motion. Each joint of the robotic arm has limitations on angle and velocity. If the inverse kinematic solution corresponding to the next pose cannot simultaneously satisfy these limitations, the robotic arm cannot reach the next given pose, leading to motion termination. Furthermore, the velocity limitations prevent the joint angles at the next moment from differing significantly from those at the current moment. Therefore, the current joint angles restrict the selection of joint angles at subsequent points. Without pre-planning the complete motion of the joints along the entire Cartesian path, the robotic arm may fail to complete the entire path due to limitations in joint angles and angular velocities. Secondly, the path starting point calculated in this control method is influenced by the current joint angles of the robotic arm, making it impossible to predict the motion outcome and increasing the risk of robotic arm movement. Additionally, considering only local path points for a given loss function cannot yield the globally optimal solution to the optimization problem. On the other hand, algorithms based on position-level inverse kinematics generally only consider the starting and ending points of the path and cannot control the path points in the middle of the motion.

[0005] Therefore, it is necessary to propose new control methods to address the above-mentioned technical deficiencies. Summary of the Invention

[0006] The technical problem this invention aims to solve is to overcome the shortcomings of existing technologies and provide a method for inverse motion control of a seven-degree-of-freedom robotic arm based on dynamic programming algorithms. This method can control a seven-degree-of-freedom robotic arm to move uninterruptedly along a pre-given Cartesian space trajectory.

[0007] To solve the technical problem, the solution of the present invention is:

[0008] A method for inverse motion control of a seven-degree-of-freedom robotic arm based on dynamic programming algorithm is provided. This method involves communication between a host computer and the seven-degree-of-freedom robotic arm via an Ethernet communication line to control the robotic arm's motion trajectory. Specifically, it includes the following steps:

[0009] (1) Using the link side torque sensor built into each joint of the robotic arm, motion parameters are acquired in real time, including: torque, angle, angular velocity, angular acceleration and force data of each joint;

[0010] (2) The host computer receives data from each sensor and calculates the spatial trajectory based on the motion parameters contained in the data;

[0011] (2.1) Based on the parameterized solution of the seventh joint angle of the fixed manipulator, the analytical solution of the inverse kinematics of the seven-degree-of-freedom manipulator is obtained;

[0012] (2.2) For the path planned by the robotic arm in Cartesian space, sample at fixed time intervals to discretize the path;

[0013] (2.3) The value of the seventh joint angle of the robotic arm is limited to several discrete fixed values. The joint angle parameters of the seventh joint at each sampling point in the path are used as inputs, and the constraints of joint angle, speed and acceleration are used as constraints. The sum of squares of the joint angle differences between adjacent sampling points is used as the optimization index to establish the optimal control problem.

[0014] (2.4) The optimal joint angle parameters were solved based on the dynamic programming algorithm, and the spatial trajectories of 7 joints considering the constraints of joint angle, velocity and acceleration were obtained.

[0015] (3) The host computer adjusts the motor running parameters built into each joint in real time according to the real-time motion parameters of each joint and the calculated spatial trajectory, and controls the actual motion direction of the seven-degree-of-freedom robotic arm.

[0016] Compared with the prior art, the beneficial effects of the present invention are:

[0017] 1. This invention achieves path control for a 7-DOF redundant robotic arm, enabling the robotic arm to move along a given Cartesian path. The method of this invention controls the joint angles, joint velocities, and joint accelerations of the robotic arm, allowing it to work along a predetermined route without errors.

[0018] 2. This invention employs a dynamic programming algorithm to solve the inverse kinematics of the robotic arm path, determining the selection of the inverse solution based on the path points of the entire path. Compared to traditional inverse kinematics solution algorithms that only consider the start and end points, such as patent CN112091979A, the algorithm in this paper controls and plans each point along the robotic arm's motion path during the solution process. It selects the joint angle of the current path point based on the path points of the entire Cartesian path, preventing the robotic arm's joint angles, velocities, and accelerations from exceeding the limits in subsequent movements, thus preventing the movement from stopping.

[0019] 3. Compared with the Jacobian velocity-level inverse kinematics solution method, the present invention solves the path of the robotic arm without being affected by the initial position of the robotic arm, and the joint angles obtained by reciprocating motion are consistent, thus ensuring the consistency of the solution during repeated motion. Attached Figure Description

[0020] Figure 1 This is a schematic diagram of a seven-degree-of-freedom robotic arm motion control system based on dynamic programming algorithm according to the present invention.

[0021] Figure 2 This is a schematic diagram of the process of the present invention.

[0022] Figure 3 This shows the changes in joint angle control values ​​for different joints.

[0023] Figure 1 In the attached diagram, the labels are: 1 host computer, 2 robotic arm, and 3 Ethernet communication cable. Detailed Implementation

[0024] The specific implementation of the present invention will now be described in detail with reference to the accompanying drawings.

[0025] First, it should be noted that this invention relates to computer control technology. The implementation of this invention involves the application of software programs; the control method described in this invention is based on the execution of these software programs to achieve corresponding calculations and control. The applicant believes that, after carefully reading the application documents and accurately understanding the implementation principles and objectives of this invention, and in conjunction with existing known technologies, those skilled in the art can fully utilize their software programming skills to implement this invention.

[0026] The robotic arm control method of this invention is used to achieve motion trajectory control of a seven-degree-of-freedom robotic arm. The hardware of the system includes: a host computer 1 with a built-in real-time operation platform (software program), and a seven-degree-of-freedom robotic arm capable of multi-joint rotation (commonly available products can be used in this invention). The host computer 1 is connected to the robotic arm 2 via an Ethernet communication cable 3. The robotic arm manufacturer usually provides a standard control interface as specified, making the connection very simple and convenient.

[0027] Based on the control algorithm of the operating platform in the host computer 1 and the Ethernet communication with the robotic arm, this invention can realize the control of the robotic arm's motion trajectory, specifically including the following steps:

[0028] (1) Using the link side torque sensor built into each joint of the robotic arm, motion parameters are acquired in real time, including: torque, angle, angular velocity, angular acceleration and force data of each joint;

[0029] (2) The host computer receives data from each sensor and calculates the spatial trajectory based on the motion parameters contained in the data;

[0030] (2.1) Based on the parameterized solution of the seventh joint angle of the fixed robotic arm, the analytical solution of the inverse kinematics of the seven-degree-of-freedom robotic arm is obtained; specifically including:

[0031] (2.1.1) For a seven-degree-of-freedom robotic arm, let q i Let be the angle value of the i-th joint angle, i = 1, 2, ..., 7; fix the joint q7 value as a constant u. In this case, the robotic arm is considered a six-degree-of-freedom robotic arm, and the forward kinematics of the robotic arm can be expressed as:

[0032] f u (x)=T (1)

[0033] Where, f u Let x be the positive kinematics function of the six-degree-of-freedom robotic arm, x be the joint angles excluding q7, and T be the desired pose, which is a constant during the solution process.

[0034] (2.1.2) Based on the joint angle parameterization method, the analytical solution of the inverse kinematics is obtained, and the corresponding position-level inverse kinematics is expressed as:

[0035] x = f u -1 (T) (2)

[0036] Where, f u Let x be the positive kinematics function of the six-degree-of-freedom manipulator, x be the joint angles excluding q7, and T be the desired pose; x and u form a set of manipulator joint angles q corresponding to pose T.

[0037] (2.2) For the path planned by the robotic arm in Cartesian space, sample at fixed time intervals to discretize the path; specifically including:

[0038] During path discretization, the Cartesian path is sampled according to the communication frequency of the robotic arm, with the sampling interval denoted as t0; the end-effector pose corresponding to the sampling point is denoted as T. i Let i = 0, 1, ..., n, where n is the number of sampling points minus 1; when performing discretization calculations, it is necessary to calculate all T. i Inverse kinematics solution q i This ensures that the angles, speeds, and accelerations of the robotic arm's joints do not exceed the limitations of the robotic arm.

[0039] (2.3) The value of the seventh joint angle of the robotic arm is limited to several discrete fixed values. The joint angle parameters of the seventh joint at each sampling point in the path are used as inputs, and the constraints on joint angle, velocity, and acceleration are used as limiting conditions. The sum of the squares of the joint angle differences between adjacent sampling points is used as the optimization index to establish the optimal control problem; specifically including:

[0040] (2.3.1) Set the range of q7 [q min,7 ,q max,7 Divide the matrix into m-1 equal parts, where q min,7 The minimum value of q7, q max,7 Let q be the maximum value of 7, and m be a fixed constant; this will include q min,7 and q max,7 The m equally divided points are denoted as a1,...,a1 in ascending order. m , representing all possible values ​​of the joint angle q7;

[0041] (2.3.2) Let the seventh joint angle of the n sampling points be denoted as q. i,7 As input parameters, i = 1, 2, ..., n; the optimized loss function L(q) n The sum of squares of the differences in joint angles between adjacent sampling points is:

[0042]

[0043] Where q i =(q i,1 ,q i,2 ,q i,3 ,q i,4 ,q i,5 ,q i,6 ,q i,7 ,) represents the joint angle corresponding to the pose of the i-th sampling point; q i,c , where is the joint angle of the c-th joint corresponding to the i-th sampling point, and c = 1, 2, ..., 7;

[0044] (2.3.3) The constraints of each joint angle are used as constraints in the optimization problem:

[0045] First, calculate the velocity and acceleration at each joint angle:

[0046]

[0047]

[0048] in, Let be the angular velocity of each joint at the i-th sampling point. Let be the angular acceleration of each joint at the i-th sampling point;

[0049] The constraints that the optimization problem needs to satisfy are:

[0050] q min，c ≤q i，c ≤q max，c， i=0, 1, ..., n; c=1, 2, ..., 7

[0051]

[0052]

[0053] Where, q min，c q is the minimum value of the angle of the c-th joint. max，c This represents the maximum value of the angle of the c-th joint. The maximum speed of the c-th joint is... Let be the maximum acceleration of the c-th joint.

[0054] (2.4) The optimal joint angle parameters were solved using a dynamic programming algorithm, resulting in the spatial trajectories of seven joints considering constraints on joint angles, velocities, and accelerations. The solution process specifically included:

[0055] (2.4.1) For all i, j, set the end pose T i The j-th possible value of the seventh joint angle is a j Substitute the inverse kinematics function The solution obtained is denoted as Where i = 1, 2, ..., n, j = 1, 2, ..., m;

[0056] (2.4.2) Let L(q) i Let ) be the loss function for the first i steps of the path, i.e.:

[0057]

[0058] remember At that time, L(q) i The minimum value of ) is remember At that time, L(q) i The minimum value of ) is remember At that time, L(q) i The minimum value of ) is

[0059] (2.4.3) Calculate all

[0060] First, consider i=1, and iterate through all j and k; according to Calculate the velocity and the acceleration from 0 velocity to the current velocity; if it does not exceed the limit, then:

[0061]

[0062] If the limit is exceeded, then It is infinite;

[0063] (2.4.4) Recursively calculate all For i, iterate through all j, k, l, according to Calculate the velocity and acceleration at time i. If either of them exceeds the limit, then... If it is infinity, otherwise:

[0064]

[0065] Subsequently The minimum value is obtained in the middle, which is

[0066] (2.4.5) In The minimum value obtained is the minimum value of the loss function under the constraints; then according to The condition for obtaining the minimum value yields the inverse kinematic solution of the robot arm path. Where, j n ,j n-1 In order to make Take the minimum values ​​j, k; j i In order to make Take the minimum value of l, 1≤i≤n-2.

[0067] (3) The host computer adjusts the operating parameters of the motors built into each joint in real time based on the real-time motion parameters of each joint and the calculated spatial trajectory, thereby controlling the actual motion direction of the seven-degree-of-freedom robotic arm; specifically including:

[0068] (3.1) The host computer reads the readings of the built-in sensors of each joint of the robotic arm in real time at a fixed communication frequency to obtain the real-time angle and force data of each joint of the robotic arm.

[0069] (3.2) Based on the spatial trajectory of the robotic arm joints calculated in step (2), the expected joint angle of the robotic arm at the next sampling time is obtained;

[0070] (3.3) Calculate the expected angular velocity of each joint based on the current joint angle and the expected joint angle at the next sampling time.

[0071] (3.4) The host computer controls the torque of each joint motor, driving each joint to reach the desired angular velocity, thereby enabling the robotic arm to move along the path in the spatial trajectory.

[0072] A specific application example:

[0073] In practical use, the user inputs the preset Cartesian trajectory of the robotic arm movement into the host computer 1. The host computer 1 calculates the corresponding inverse kinematics solution based on the given Cartesian trajectory, converting it into the change trajectory of the joints in the robotic arm 2. Subsequently, the host computer 1 communicates and controls the robotic arm 2 in real time via Ethernet communication cable 3, using the sensors built into each joint of the robotic arm 2 to make the robotic arm 2 move along the given path.

[0074] Taking a 7-DOF robotic arm manufactured by Franka Emika GmbH in Germany as an example, the motion control system of a 7-DOF robotic arm based on dynamic programming algorithm of the present invention is tested: Given a robotic arm motion trajectory lasting 10 seconds, the homogeneous transformation matrix of the robotic arm pose in the robotic arm base coordinate system at time t (0(s)≤t≤10(s)) is:

[0075]

[0076] in

[0077] The inverse kinematic solution of the path is calculated using the control method described in this invention, and the result is as follows: Figure 3 As shown.

[0078] The host computer reads the data from the joint sensors in real time and controls the Franka Emika robotic arm in real time according to the desired joint angle and angular velocity values. The robotic arm can successfully run from the starting point to the end point, thus verifying the technical effect of the system of the present invention.

[0079] The dynamic programming method employed in this invention comprehensively considers the entire path, effectively optimizing joint angles along the path and making the robotic arm's movement smoother and more stable. Before the robotic arm begins its movement, the system plans a complete motion trajectory, ensuring that the robotic arm can reach each given path point within the constraints of joint angles and speeds, effectively preventing interruptions in movement.

Claims

1. A method for inverse motion control of a seven-degree-of-freedom robotic arm based on dynamic programming algorithm, wherein the host computer communicates with the seven-degree-of-freedom robotic arm via an Ethernet communication line to control the motion trajectory of the robotic arm; Specifically, the following steps are included: (1) Using the link side torque sensor built into each joint of the robotic arm, the motion parameters are acquired in real time, including: data on the torque, angle, angular velocity, angular acceleration and force of each joint; (2) The host computer receives data from each sensor and calculates the spatial trajectory based on the motion parameters contained in the data; (2.1) Based on the parameterized solution of the seventh joint angle of the fixed manipulator, the analytical solution of the inverse kinematics of the seven-degree-of-freedom manipulator is obtained; (2.2) For the path planned by the robotic arm in Cartesian space, sample at fixed time intervals to discretize the path; (2.3) The value of the seventh joint angle of the robotic arm is limited to several discrete fixed values, and the joint angle parameters of the seventh joint at each sampling point in the path are used as inputs. The constraints on joint angle, velocity, and acceleration are used as limiting conditions, and the sum of squares of the joint angle differences between adjacent sampling points is used as the optimization index to establish the optimal control problem; including: (2.3.1) Divide the range of angle values ​​of the seventh joint angle into equal parts, and take the multiple points of equal division containing the minimum and maximum angle values ​​as all possible values ​​of the seventh joint angle; (2.3.2) The seventh joint angle of n sampling points is taken as the parameter input, and the optimized loss function is the sum of squares of the differences between the joint angles of adjacent sampling points; (2.3.3) The constraints of each joint angle are used as constraints in the optimization problem; (2.4) Based on the dynamic programming algorithm, the optimal joint angle parameters were solved, resulting in seven joint spatial trajectories considering joint angle, velocity, and acceleration constraints; including: (2.4.1) Substitute the end pose and the possible values ​​of the seventh joint angle into the inverse kinematics function to find the joint angle parameter solution; (2.4.2) Define the path loss function and the minimum value of the function under different joint angle solutions; (2.4.3) Calculate the minimum value: Iterate through the solution combinations to verify whether the velocity and acceleration exceed the limits. If they do not exceed the limits, calculate according to the rules; if they exceed the limits, set them to infinity. (2.4.4) Recursively calculate the minimum value of subsequent steps, verify whether the velocity and acceleration exceed the limits, and take the corresponding minimum value as the result of the current step; (2.4.5) Take the optimal loss value in the last step and derive the inverse kinematics solution of the robot arm in reverse; (3) The host computer adjusts the motor running parameters built into each joint in real time according to the real-time motion parameters of each joint and the calculated spatial trajectory, and controls the actual motion direction of the seven-degree-of-freedom robotic arm.

2. The method according to claim 1, characterized in that, Step (2.1) includes: (2.1.1) For a seven-degree-of-freedom robotic arm, let q i Let be the angle value of the i-th joint angle, i=1,2,...,7; fix the joint q7 value as a constant u. Then, consider the robotic arm as a six-degree-of-freedom robotic arm. The forward kinematics of the robotic arm can be expressed as: f u (x)=T (1) Among them, f u Let x be the positive kinematics function of the six-degree-of-freedom robotic arm, x be the joint angles excluding q7, and T be the desired pose, which is a constant during the solution process. (2.1.2) Based on the joint angle parameterization method, the analytical solution of the inverse kinematics is obtained, and the corresponding position-level inverse kinematics is expressed as: x=f u -1 (T) (2) Among them, f u Let x be the positive kinematics function of the six-degree-of-freedom manipulator, x be the joint angles excluding q7, and T be the desired pose; x and u form a set of manipulator joint angles q corresponding to pose T.

3. The method according to claim 1, characterized in that, Step (2.2) includes: During path discretization, the Cartesian path is sampled according to the communication frequency of the robotic arm, with the sampling interval denoted as t0; the end-effector pose corresponding to the sampling point is denoted as T. i Let i = 0, 1, ..., n, where n is the number of sampling points minus 1; when performing discretization calculations, it is necessary to calculate all T. i Inverse kinematics solution q i This ensures that the angles, speeds, and accelerations of the robotic arm's joints do not exceed the limitations of the robotic arm.

4. The method according to claim 1, characterized in that, The specific steps (2.3) are as follows: (2.3.1) Set the range of q7 [q min,7 ,q max,7 Divide the matrix into m-1 equal parts, where q min,7 The minimum value of q7, q max,7 Let q be the maximum value of 7, and m be a fixed constant; this will include q min,7 and q max,7 The m equally divided points are denoted as a1,...,a1 in ascending order. m , representing all possible values ​​of the joint angle q7; (2.3.2) Let the seventh joint angle of the n sampling points be denoted as q. i,7 As input parameters, i = 1, 2, ..., n; the optimized loss function L(q) n The sum of squares of the differences in joint angles between adjacent sampling points is: 2 2 (3) in, =(q i,1, q i,2, q i,3, q i,4, q i,5, q i,6, q i,7, ) represents the joint angle corresponding to the pose of the i-th sampling point; q i,c , where is the joint angle of the c-th joint corresponding to the i-th sampling point, c=1,2,...,7; (2.3.3) The constraints of each joint angle are used as constraints in the optimization problem: First, calculate the velocity and acceleration at each joint angle: ； （4） in, =( , , , , , , ) represents the angular velocity of each joint at the i-th sampling point. =( , , , , , , ) represents the angular acceleration of each joint at the i-th sampling point; t0 represents the Cartesian path sampling interval determined based on the communication frequency of the robotic arm. The constraints that the optimization problem needs to satisfy are: ； ； （5） in, This represents the minimum value of the angle of the c-th joint. This represents the maximum value of the angle of the c-th joint. The maximum speed of the c-th joint is... Let be the maximum acceleration of the c-th joint.

5. The method according to claim 1, characterized in that, In step (2.4), the process of solving for the optimal joint angle parameters based on the dynamic programming algorithm is as follows: (2.4.1) For all i, j, set the end pose T i The j-th possible value of the seventh joint angle is a j Substitute the inverse kinematics function -1 The solution obtained is denoted as Where i = 1, 2, ..., n, j = 1, 2, ..., m; (2.4.2) Note The loss function for the path up to step i, i.e.: 2 2 (6) Let = , the minimum of ; let = , = , the minimum of ; let = , = , = , the minimum of ; (2.4.3) Calculate all of ; First, consider i=1, and iterate through all j and k; according to Calculate the velocity and the acceleration from 0 velocity to the current velocity; if it does not exceed the limit, then: |2 2 (7) If the limit is exceeded, then It is infinitely large; (2.4.4) Recursively calculate all For i, iterate through all j, k, l, according to , , Calculate the velocity and acceleration at time i. If either of them exceeds the limit, then... If it is infinity, otherwise: |2 2 (8) Subsequently The minimum value is obtained in the middle, which is ; (2.4.5) in The minimum value obtained is the minimum value of the loss function under the constraints; then according to The condition for obtaining the minimum value yields the inverse kinematic solution of the robotic arm path. = , 1≤i≤n; where j n ,j n-1 In order to make Take the minimum values ​​j, k; j i In order to make Take the minimum value of l, 1≤i≤n-2.

6. The method according to claim 1, characterized in that, In step (3), the process of controlling the actual motion direction of the seven-degree-of-freedom robotic arm is as follows: (3.1) The host computer reads the readings of the built-in sensors of each joint of the robotic arm in real time at a fixed communication frequency to obtain the real-time angle and force data of each joint of the robotic arm. (3.2) Based on the spatial trajectory of the robotic arm joints calculated in step (2), the expected joint angle of the robotic arm at the next sampling time is obtained; (3.3) Calculate the expected angular velocity of each joint based on the current joint angle and the expected joint angle at the next sampling time; (3.4) The host computer controls the torque of each joint motor, driving each joint to reach the desired angular velocity, thereby enabling the robotic arm to move along the path in the spatial trajectory.

Images