Robotic arm stepping path trajectory optimization method based on reinforcement learning

Abstract

The present invention discloses a method for optimizing a robot arm's step-by-step path trajectory based on reinforcement learning. The robot arm moves according to a policy function, and the output of the policy function is optimized by a policy optimization algorithm. After each movement of the robot arm, the rotation angle of the joint rotating around the z-axis is calculated. When the joint rotating around the z-axis is basically facing the target point, the output angle value of the corresponding joint is locked. After locking the corresponding output angle value, training continues. When the distance between the end posture and the target posture is less than a certain threshold, the training is terminated in advance, otherwise until the training is completed. A reward function is used during the training process. The step-by-step training of the present invention can effectively reduce the learning time and quickly locate the target point with the optimal posture. By locking the output value of the joint action that has been trained first, the model can achieve better results in a shorter time by using fewer joints. The design of the reward function optimizes the model planning path and achieves the optimal path with the least action.

Description

Technical Field

[0001] The present invention relates to path planning technology, and in particular to a method for optimizing a robotic arm's step-by-step path trajectory based on reinforcement learning. Background Art

[0002] Robotic arms are widely used in manufacturing, warehousing, medical and other fields to perform various complex tasks. Path planning is a key technology to ensure that robotic arms can perform tasks safely and efficiently within the workspace.

[0003] Current path planning methods include planning in Cartesian space and joint space, as well as inserting intermediate points between the start and target positions through interpolation algorithms to generate a smooth path. However, these methods have difficulty dealing with constraint problems in the workspace and lack global considerations. Later, motion planning algorithms such as A* and RRT emerged that can consider the global environment and adapt to different dynamic constraints, but they have high computational complexity.

[0004] Modern methods mostly use machine learning techniques, such as reinforcement learning, which can learn and optimize path selection strategies to adapt to different environments and tasks. However, most learning stability and convergence are poor. In addition, analytical methods are usually difficult to model for inverse kinematics problems. Different robotic arms require different DH parameter tables. Numerical methods have errors and lack stability.

[0005] Therefore, in order to solve the above problems, the present invention proposes a step-by-step training and learning method, which can quickly converge the results based on the optimization path. Summary of the invention

[0006] In order to solve the deficiencies in the prior art, the object of the present invention is to provide a method for optimizing the step-by-step path trajectory of a robotic arm based on reinforcement learning.

[0007] To achieve the purpose of the present invention, the technical solution adopted by the present invention is:

[0008] A method for optimizing a robot's step-by-step path trajectory based on reinforcement learning, comprising the following steps:

[0009] (1) Perform robot arm movement according to the strategy function;

[0010] (2) After each movement of the robot arm, calculate the rotation angle of the joint rotating around the z-axis;

[0011] (3) When the joint rotating around the z-axis is basically facing the target point, the output angle value of the corresponding joint is locked;

[0012] (4) After locking the corresponding output angle value, continue training. When the distance between the terminal posture and the target posture is less than a certain threshold, terminate the training in advance, otherwise continue until the end of the training.

[0013] Furthermore, in step (1), the strategy function is used to observe the distance between the end posture and the target posture in the X, Y, and Z axis directions, and take the corresponding output according to a certain mapping, and the output includes joint bending and rotation angles.

[0014] Furthermore, step (1) also includes a policy optimization algorithm, which optimizes the output of the policy function by iteratively updating the parameters of the policy function.

[0015] Furthermore, in step (2), the rotation angle of the joint rotating around the z-axis is calculated, specifically,

[0016] Calculate the rotation angle of the joint closest to the base that rotates around the z-axis, and find the angle between the line connecting the projection of the target posture in the XY plane and the coordinate origin and the rotated joint after the rotation; secondly, calculate the rotation angle of the next joint that rotates around the z-axis, and based on the rotation angle of the previous joint, calculate the angle between the line connecting the projection of the target posture in the XY plane and the coordinate origin and the joint after this rotation; repeat the above steps until the last joint that rotates around the z-axis.

[0017] Furthermore, in step (3), the reward for determining the angle between the angle of the joint rotating around the z-axis and the target posture direction is range Is it greater than -0.05, when reward range When >-0.05, all joints rotating around the z-axis basically face the target point, and all joints are locked.

[0018] Furthermore, in step (4), a reward function is used in the training process, and the reward function is designed as:

[0019] Value=reward distance +reward angle +reward pose +reward range

[0020] Among them, reward distance Reward representing the distance between the robot end pose and the target pose, reward angle Represents the rotation angle and reward, pose Represents the reward for the distance between the last joint rotated around the z-axis and the target pose in the z-axis direction, reward range Represents the reward for all joints rotating around the z-axis.

[0021] Furthermore, reward distance The function is specifically:

[0022] When the distance is less than 0.1, In other cases,

[0023] Among them, r represents the distance between the end pose of the robot arm and the target pose, and δ represents a hyperparameter of the Huber loss function, which is taken as 0.1 here.

[0024] Furthermore, reward angle The function is used to judge the strategy optimization algorithm. The goal is to make the value of the function close to 0. Specifically,

[0025]

[0026] Among them, SUM represents the sum of the absolute values ​​of the outputs of all joints, reward angle The larger the value, the smaller the bending angle of each joint.

[0027] Furthermore, reward pose The function is used to judge the strategy optimization algorithm. The goal is to make the value of the function close to 0. Specifically,

[0028] When the current height of the last joint rotating around the z-axis is less than the target pose height, and the target pose height is lower than the initial pose height of the last joint rotating around the z-axis, reward pose The value of In other cases, reward pose =0;

[0029] Among them, Z target Represents the target pose height, Z cur Represents the current height of the joint that was last rotated about the z-axis.

[0030] Furthermore, reward range The function is specifically:

[0031] When angles_1 difference <0.1 and And angles_3 difference <0.1 and At this time, reward range =1,

[0032] In other cases, add the absolute values ​​of all angle differences and negate them.

[0033]

[0034] Among them, angles_1 difference Represents the angle difference between the line connecting Link1 and the target projected onto the XY plane and the origin and the positive direction of the X axis, angles_2difference Represents the sum of the rotation angles of Link2 and Link1 and the angle difference between the line connecting the target projected onto the XY plane and the origin and the positive direction of the X axis, angles_3 difference Represents the sum of the rotation angles of Link1, Link2, and Link3 and the angle difference between the line connecting the target projected to the XY plane and the origin and the positive direction of the X axis, angles_n difference Represents the angle difference between the sum of the rotation angles of Link1, Link2, ... and Linkn and the line connecting the target projected onto the XY plane and the origin and the positive direction of the X-axis.

[0035] The beneficial effect of the present invention is that, compared with the prior art, the step-by-step learning method proposed in the present invention can effectively reduce the learning time and quickly locate the target point with the optimal posture. Step-by-step training, by locking the output value of the previously trained joint action, enables the model to achieve better results in a shorter time by using fewer joints. The design of the reward function optimizes the model planning path to achieve the optimal path with the least action. BRIEF DESCRIPTION OF THE DRAWINGS

[0036] Figure 1 This is a flow chart of the method for optimizing the step-by-step path trajectory of a robotic arm based on reinforcement learning according to the present invention;

[0037] Figure 2 This is a model structure diagram in this embodiment;

[0038] Figure 3 This is the average reward graph of the training process in this embodiment;

[0039] Figure 4 This is the model training diagram in this embodiment;

[0040] Figure 5 This is the end diagram of the model training in this embodiment. DETAILED DESCRIPTION

[0041] The technical solution of the present invention is further described below in conjunction with the accompanying drawings and embodiments. The following embodiments are only used to more clearly illustrate the technical solution of the present invention, and cannot be used to limit the protection scope of this application.

[0042] like Figure 1 As shown, the robot arm step-by-step path trajectory optimization method based on reinforcement learning of the present invention comprises the steps of:

[0043] Step 1: Build a reinforcement learning environment, set up the strategy function and strategy optimization algorithm;

[0044] The reinforcement learning environment is a custom environment built on the basis of OpenAI's Gym. The observation space of the reinforcement learning environment includes the joint angle rotation range, the end pose, the distance between the end pose and the target pose, and the rotation angle of all joints rotating around the z-axis.

[0045] The strategy function is used to observe the distance between the end pose and the target pose in the X, Y, and Z axis directions, and take the corresponding output according to a certain mapping, including the joint bending and rotation angles. The strategy function uses the MultiInputPolicy multi-input strategy network.

[0046] The policy optimization algorithm is used to optimize the output of the policy function to obtain the optimal policy. The reinforcement learning policy optimization algorithm uses the DDPG (Deep Determinidtic Policy Gradient) deep deterministic policy gradient algorithm in the Stable_Baselines3 library.

[0047] Step 2: Robotic arm step training;

[0048] (2.1) The initial rotation angles of all joints of the robot arm are 0. First, intensive training is performed without any conditional judgment. The robot arm moves according to the strategy function.

[0049] The strategy function is used to observe the distance between the end pose and the target pose in the X, Y, and Z axis directions, and take the corresponding output according to a certain mapping. The output includes joint bending and rotation angles.

[0050] The policy function uses the MultiInputPolicy multi-input policy network. In the MultiInputPolicy multi-input policy network, the policy function may consider multiple different inputs at the same time, such as the current state, the historical state, the state of other agents, etc., such as the observation space mentioned above. These inputs are combined and processed through the network to produce one or more actions as output. Each rotation of the robot arm is the output of the policy function, and the specific output action is selected and adjusted according to the policy optimization algorithm.

[0051] The policy optimization algorithm is used to optimize the output of the policy function to obtain the optimal policy. The reinforcement learning policy optimization algorithm uses the DDPG (Deep Determinidtic Policy Gradient) deep deterministic policy gradient algorithm in the Stable_Baselines3 library. The goal of the policy optimization algorithm is to iteratively update the parameters of the policy function so that the agent can obtain a higher expected return when following the policy, that is, to maximize the reward function value. Therefore, it accompanies the entire training process, and the subsequent step training process uses this algorithm for optimization.

[0052] (2.2) After each movement of the robot arm, calculate the rotation angle of the joint rotating around the z-axis;

[0053] First, calculate the angle between the projection of the target posture on the XY plane and the line connecting the coordinate origin and the positive direction of the X-axis of the world coordinate system;

[0054] Secondly, calculate the rotation angle of the joint closest to the base that rotates around the z-axis, and find the angle between the projection of the target posture on the XY plane and the line connecting the coordinate origin and the rotated joint after the rotation;

[0055] Secondly, the rotation angle of the next joint rotating around the z-axis is calculated. Based on the rotation angle of the previous joint, the angle between the projection of the target posture on the XY plane and the line connecting the coordinate origin and the joint after this rotation is calculated.

[0056] Repeat the above steps until the last joint rotates around the z-axis.

[0057] (2.3) When the joint rotating around the z-axis is basically facing the target point, the output angle value of the corresponding joint is locked;

[0058] Determine whether the reward for the angle between the joint rotation around the z-axis and the target posture direction is greater than -0.05. range When >-0.05, it means that all joints rotating around the z-axis are basically facing the target point. At this time, all joints are locked. Specifically, the output action of the network model is ignored in each action, and the last joint angle before locking is used.

[0059] (2.4) After locking the corresponding output angle value, continue training. When the distance between the terminal posture and the target posture is less than a certain threshold, terminate the training in advance, otherwise continue until the end of the training.

[0060] After locking the rotation joint around the z-axis, the next condition is judged. When the distance between the end pose and the target pose is less than 0.01, the training is terminated early, otherwise the training continues.

[0061] The reward function is used in the training process to make the end position of the robot arm reach the target position. When the end position of the robot arm is close to the target position, a positive reward is given, and when the end position is far away from the target position, a negative reward is given.

[0062] The reward function is designed as:

[0063] Value=reward distance +reward angle +reward pose +reward range

[0064] Among them, reward distance As the main reward, it represents the reward for the distance between the robot end position and the target position. The rest of the rewards are auxiliary rewards. angle Represents the rotation angle and reward, pose Represents the reward for the distance between the last joint rotated around the z-axis and the target pose in the z-axis direction, reward range Represents the reward for all joints rotating around the z-axis.

[0065] reward distance The reward represents the distance between the robot end pose and the target point pose. The specific changes are made based on the Huber loss function. When the distance is less than 0.1, In other cases,

[0066] Among them, r represents the distance between the end pose of the robot arm and the target pose, and δ represents a hyperparameter of the Huber loss function, which is taken as 0.1 here.

[0067] reward angle Represents the impact of the output angle value of each joint on the reward. It calculates the sum of the absolute values ​​of the output of all joints and normalizes it with the arc tangent function, i.e. reward angle The value of

[0068] Among them, SUM represents the sum of the absolute values ​​of the outputs of all joints, reward angle The larger the value, the smaller the bending angle of each joint, which optimizes the range of motion.

[0069] reward angle The function is used to determine the strategy optimization algorithm, and the goal is to make the value of the function close to 0.

[0070] reward pose Represents the reward for the distance between the last joint rotating around the z-axis and the target pose in the z-axis direction, in order to ensure that the overall posture of the robot is approaching from above the target pose, that is, optimizing the trajectory. When the current height of the last joint rotating around the z-axis is less than the target pose height, and the target pose height is lower than the initial pose height of the last joint rotating around the z-axis, reward pose The value of In other cases, reward pose =0.

[0071] Among them, Z target Represents the target pose height, Z curRepresents the current height of the joint that was last rotated about the z-axis.

[0072] reward pose The function is used to determine the strategy optimization algorithm, and the goal is to make the value of the function close to 0.

[0073] reward range Represents the effect of the angle of all joints rotating around the z-axis and the angle of the target pose direction on the reward, in order to ensure that all joints rotating around the z-axis of the robot arm are facing the target.

[0074] All joints rotating around the z-axis must be judged based on the previous joint rotating around the z-axis. First, determine the angle deviation between Link1 and the target point angles_1 difference , this angle deviation is defined as the difference between the angle between the target point projected onto the line connecting the XY plane and the origin and the positive direction of the X axis and the angle of rotation of Link1. Then, the angle deviation between Link2 and the target point is calculated. 2difference This angle deviation is calculated based on Link1, and the sum of the rotation angles of Link1 and Link2 is calculated, and the difference between the angle between the target point projected onto the XY plane and the origin and the positive direction of the X axis is calculated. Link3 accumulates the rotation angles of Link1 and Link2, and the calculation method is the same as above, and finally angles_3 is obtained. difference . And so on, we get angles_n difference .

[0075] After calculating all the angle differences, make a judgment. When angles_1 difference <0.1 and And angles_3 difference <0.1 and At this time, reward range =1, otherwise, add the absolute values ​​of all angle differences and take the inverse.

[0076] Among them, angles_1 difference Represents the angle difference between the line connecting Link1 and the target projected onto the XY plane and the origin and the positive direction of the X axis, angles_2 difference Represents the sum of the rotation angles of Link2 and Link1 and the angle difference between the line connecting the target projected onto the XY plane and the origin and the positive direction of the X axis, angles_3 difference Represents the sum of the rotation angles of Link1, Link2, and Link3 and the angle difference between the line connecting the target projected to the XY plane and the origin and the positive direction of the X axis, angles_n differenceRepresents the angle difference between the sum of the rotation angles of Link1, Link2, ... and Linkn and the line connecting the target projected onto the XY plane and the origin and the positive direction of the X-axis.

[0077] The reward function is designed as follows:

[0078]

[0079] Where α and β are constant coefficients. The coefficients here represent weights, which are used to weigh the proportions of these reward values ​​in the training process. pose The value of is -100, and other values ​​are relatively large, such as -10. In this case, the value of the β parameter can be set to 0.1 to reduce the reward. pose The impact brought about by this is that the policy function does not focus too much on the reward pose But the specific value depends on the specific situation.

[0080] like Figure 2 As shown, in this embodiment, the initial joint angles of the model are all 0, and intensive training is performed first, without any conditional judgment; for this example model, when the reward range When >-0.05, it means that all joints rotating around the z-axis are basically facing the direction of the target point. At this time, the Joint1, Joint3, and Joint5 joints in this example are locked. In this example, 1, 3, and 5 are joints rotating around the Z axis, that is, vertical rotation.

[0081] After locking the rotation joint around the z-axis, the next condition is judged. When the distance between the end pose and the target pose is less than 0.01, for this example, the training is terminated early, otherwise the training continues.

[0082] Figure 4 This is the model training diagram in this embodiment; Figure 5 This is the end diagram of the model training in this embodiment.

[0083] The reward function is designed as:

[0084] Value=reward distance +reward angle +reward pose +reward range ;

[0085] Among them, reward distance The reward represents the distance between the model's end pose and the target point. The specific changes are made based on the Huber loss function. When the distance is less than 0.1, In other cases,

[0086] Among them, r represents the Euclidean distance between the model end pose and the target pose, and δ represents a hyperparameter in Huber Loss, which takes a value of 0.1 here.

[0087] reward angle It represents the impact of the output angle value of each joint of the model on the reward. It calculates the sum of the absolute values ​​of the output of all joint axes and normalizes them using the inverse tangent function, i.e. reward angle The value of

[0088] Among them, SUM represents the sum of the absolute values ​​of the outputs of all joint axes, reward angle The larger the value, the smaller the bending angle of each joint of the model, which optimizes the range of motion.

[0089] reward pose Represents the reward for the distance between Joint6 and the target pose in the z-axis direction. In order to ensure that the overall pose of the robot is approaching from above the target point, that is, optimizing the trajectory. When the height of Joint6 is less than the height of the target pose, and the height of the target pose is lower than the height of the initial pose of the robot Joint6, reward pose The value of In other cases, reward pose =0.

[0090] Among them, Z target Represents the height of the target pose, Z cur Represents the current height of Joint6.

[0091] reward range Represents the effect of the angle between all joints rotating around the z-axis and the target pose direction on the reward, in order to ensure that all joints rotating around the z-axis of the robot arm are facing the target (such as Joint1, Joint3, and Joint5 in this example).

[0092] All joints rotating around the z-axis must be judged based on the previous joint rotating around the z-axis. For this example, first determine the angle deviation between Link1 and the target point angles_1 difference , this angle deviation is defined as the difference between the angle between the target point projected onto the line connecting the XY plane and the origin and the positive direction of the X axis and the angle of rotation of Link1. Next, the angle deviation between Link3 and the target point is calculated. This angle deviation is calculated based on Link1, and the sum of the rotation angles of Link1 and Link3 is calculated, and the difference between the angle between the target point projected onto the XY plane and the origin and the positive direction of the X axis is calculated. Link5 accumulates the rotation angles of Link1 and Link3, and the calculation method is the same as above, and finally angles_3 is obtained. difference .

[0093] After calculating all the angle differences, make a judgment. When angles_1 difference <0.1 and And angles_3 difference <0.1, then reward range =1, otherwise, add up the absolute values ​​of all angle differences, take the inverse, and use it as the reward range Therefore, reward range The function is as follows:

[0094]

[0095] Among them, angles_1 difference Represents the angle difference between the line connecting Link1 and the target projected onto the XY plane and the origin and the positive direction of the X axis, angles_2 difference It represents the sum of the rotation angles of Link3 and Link1 and the angle difference between the line connecting the target projected onto the XY plane and the origin and the positive direction of the X axis. difference It represents the angle difference between the sum of the rotation angles of Link1, Link3, and Link5 and the line connecting the target projected onto the XY plane and the origin and the positive direction of the X axis.

[0096] Therefore, for this example, the reward function is designed as follows:

[0097]

[0098] Among them, α and β are constant coefficients.

[0099] Figure 3 This is the average reward graph of the training process in this embodiment. In this example, the best value of each part of the reward function is 0, that is, the closer the reward function value is to 0, the better the training effect. This graph represents the change curve of the reward value over time during the model training process.

[0100] The beneficial effect of the present invention is that, compared with the prior art, the step-by-step learning method proposed in the present invention can effectively reduce the learning time and quickly locate the target point with the optimal posture. Step-by-step training, by locking the output value of the previously trained joint action, enables the model to achieve better results in a shorter time by using fewer joints. The design of the reward function optimizes the model planning path to achieve the optimal path with the least actions.

[0101] The applicant of the present invention has made a detailed explanation and description of the implementation examples of the present invention in conjunction with the drawings in the specification. However, those skilled in the art should understand that the above implementation examples are only preferred implementation schemes of the present invention, and the detailed description is only to help readers better understand the spirit of the present invention, but not to limit the scope of protection of the present invention. On the contrary, any improvements or modifications based on the inventive spirit of the present invention should fall within the scope of protection of the present invention.

Claims

1. A method for optimizing the stepping path trajectory of a robotic arm based on reinforcement learning, characterized in that: Includes steps: (1) Perform robot arm movement according to the strategy function; (2) After each movement of the robot arm, calculate the rotation angle of the joint rotating around the z-axis; (3) When the joint rotating around the z-axis is basically facing the target point, the output angle value of the corresponding joint is locked; (4) After locking the corresponding output angle value, continue training. When the distance between the terminal posture and the target posture is less than a certain threshold, terminate the training in advance, otherwise continue until the end of the training.

2. The method for optimizing the step-by-step path trajectory of a robotic arm based on reinforcement learning according to claim 1, characterized in that: In step (1), the strategy function is used to observe the distance between the end pose and the target pose in the X, Y, and Z axis directions, and take the corresponding output according to a certain mapping, and the output includes joint bending and rotation angles.

3. The method for optimizing the step-by-step path trajectory of a robotic arm based on reinforcement learning according to claim 2, characterized in that: Step (1) also includes a policy optimization algorithm, which optimizes the output of the policy function by iteratively updating the parameters of the policy function.

4. The method for optimizing the step-by-step path trajectory of a robotic arm based on reinforcement learning according to claim 1, characterized in that: In step (2), the rotation angle of the joint rotating around the z-axis is calculated, specifically, Calculate the rotation angle of the joint closest to the base that rotates around the z-axis, and find the angle between the line connecting the projection of the target posture in the XY plane and the coordinate origin and the rotated joint after the rotation; secondly, calculate the rotation angle of the next joint that rotates around the z-axis, and based on the rotation angle of the previous joint, calculate the angle between the line connecting the projection of the target posture in the XY plane and the coordinate origin and the joint after this rotation; repeat the above steps until the last joint that rotates around the z-axis.

5. The method for optimizing the step-by-step path trajectory of a robotic arm based on reinforcement learning according to claim 4, characterized in that: In step (3), the reward for determining the angle between the angle of the joint rotating around the z-axis and the target posture direction is range Is it greater than -0.05, when reward range When >-0.05, all joints rotating around the z-axis basically face the target point, and all joints are locked.

6. The method for optimizing the step-by-step path trajectory of a robotic arm based on reinforcement learning according to claim 1, characterized in that: In step (4), a reward function is used in the training process, and the reward function is designed as: Value=reward distance +reward angle +reward pose +reward range Among them, reward distance Reward representing the distance between the robot end pose and the target pose, reward angle Represents the rotation angle and reward, pose Represents the reward for the distance between the last joint rotated around the z-axis and the target pose in the z-axis direction, reward range Represents the reward for all joints rotating around the z-axis.

7. The method for optimizing the step-by-step path trajectory of a robotic arm based on reinforcement learning according to claim 6, characterized in that: reward distance The function is specifically: When the distance is less than 0.1, In other cases, Among them, r represents the Euclidean distance between the end pose of the robot arm and the target pose, and δ represents a hyperparameter of the Huber loss function, which is taken as 0.1 here.

8. The method for optimizing the step-by-step path trajectory of a robotic arm based on reinforcement learning according to claim 6, characterized in that: reward angle The function is used to judge the strategy optimization algorithm. The goal is to make the value of the function close to 0. Specifically, Among them, SUM represents the sum of the absolute values ​​of the outputs of all joints, reward angle The larger the value, the smaller the bending angle of each joint.

9. The method for optimizing the step-by-step path trajectory of a robotic arm based on reinforcement learning according to claim 6, characterized in that: reward pose The function is used to judge the strategy optimization algorithm. The goal is to make the value of the function close to 0. Specifically, When the current height of the last joint rotating around the z-axis is less than the target pose height, and the target pose height is lower than the initial pose height of the last joint rotating around the z-axis, reward pose The value of In other cases, reward pose =0; Among them, Z target Represents the target pose height, Z cur Represents the current height of the joint that was last rotated about the z-axis.

10. The method for optimizing the step-by-step path trajectory of a robotic arm based on reinforcement learning according to claim 6, characterized in that: reward range The function is specifically: When angles_1 difference <0.1 and And angles_3 difference <0.1 and… and At this time, reward range =1, In other cases, add the absolute values ​​of all angle differences and negate them. Among them, angles_1 difference Represents the angle difference between Link1 and the line connecting the target projected onto the XY plane and the origin and the positive direction of the X axis, angles_2 difference Represents the sum of the rotation angles of Link2 and Link1 and the angle difference between the line connecting the target projected onto the XY plane and the origin and the positive direction of the X axis, angles_3 difference Represents the sum of the rotation angles of Link1, Link2, and Link3 and the angle difference between the line connecting the target projected to the XY plane and the origin and the positive direction of the X axis, angles_n difference Represents the angle difference between the sum of the rotation angles of Link1, Link2, ... and Linkn and the line connecting the target projected onto the XY plane and the origin and the positive direction of the X-axis.

Images