A method and system for multi-joint motion planning of a robot arm

Abstract

The application discloses a kind of robot arm multi-joint motion planning method and system, belong to cognitive development and robot motion control cross field.The application establishes the positive model for mechanical arm and the multiple reverse models corresponding to each joint, the positive model is used to establish the mapping of the angle of each joint of mechanical arm to the end pose of mechanical arm, each reverse model is used to establish the mapping of the end pose of mechanical arm to the angle of corresponding joint, by referring to the mechanism of human arm motion planning, the joint close to shoulder joint is mainly used when mechanical arm approaches in long distance, and the elbow joint is mainly used when approaching in short distance, and the model is optimized by greedy strategy and auxiliary loss function optimization.The application can select appropriate joint instruction according to the displacement size of the end of mechanical arm in Cartesian space to execute, to realize more effective motion planning.

Description

Technical Field

[0001] This invention belongs to the interdisciplinary field of cognitive development and robot motion control, and specifically relates to a motion planning method and system for a multi-joint robot arm. Background Technology

[0002] As robots are increasingly used in social life, people's demands for their intelligence are also rising. There is a growing desire to find methods to build robotic systems with greater autonomy and adaptability. Scholars have explored various approaches to address this issue, one effective method being to draw inspiration from the movement mechanisms of humans or other organisms. Because humans and other organisms possess strong inherent adaptability, flexibility, and evolutionary capabilities, in this sense, borrowing from human movement mechanisms is a valuable source of inspiration for solving sensory-motor coordination problems in intelligent robots.

[0003] Among the many capabilities robots may possess, arm motion control is one of the fundamental abilities they should have. Since the advent of the first programmable robotic arm in the 1960s, robotic arm motion has received widespread attention and extensive research, playing a significant role in industrial manufacturing. Therefore, for robots to coexist with humans in society and provide convenience for human life, arm motion skills are one of their essential capabilities. Among the various motion skills of a robotic arm, reaching is one of the most important. It is the first step in various more complex arm movements, including grasping and placing, and provides the necessary foundation for the development and refinement of future motion, perception, and cognitive behaviors.

[0004] When a person moves their arm, different joints play different roles in the hand's movement; the movement of one joint triggers the movement of another. The same principle applies to humanoid robots. The rotation of different joints has different effects on the movement of the arm's end effector. For example, rotating either the shoulder or elbow joint can move the robotic arm upwards, but for the same amplitude of movement, the shoulder joint requires a smaller rotation angle than the elbow joint; and for the same angle of rotation, rotating the shoulder joint results in a much larger amplitude of movement at the end effector than rotating the elbow joint. Figure 1 As shown, rotating the shoulder or elbow joint moves the end effector of the robotic arm upwards. For the same amplitude of end effector movement l1 = l2, the angle required for the shoulder joint to rotate is smaller than that for the elbow joint, α1 < α2; however, for the same angle α1 = α2, rotating the shoulder joint results in a much larger amplitude of end effector movement than rotating the elbow joint, l1 > l2. Generally speaking, the movement of joints closer to the base has a greater impact on the end effector.

[0005] Because the connection methods between joints differ, the energy consumed by each joint under the same motion angle and speed conditions also varies. Joints closer to the base consume significantly more energy than those closer to the end effector. During robot arm motion planning, multiple solutions may exist, meaning multiple sets of joint angles can bring the end effector to the desired position. Among these solutions, the robot should select the set of joint angles with the lowest energy consumption. Generally, for movements requiring large displacements at the end effector, joints closer to the base, such as the shoulder joint, should be used as much as possible, while for fine movements requiring small displacements, joints closer to the end effector, such as the wrist joint, should be used. For different end effector movements, the rotation of multiple joints should be selected to achieve more efficient motion planning. However, how to select joint rotations to achieve effective motion planning remains a challenge. Summary of the Invention

[0006] The purpose of this invention is to propose a multi-joint motion planning method and system for a robotic arm, which can select appropriate joint commands to execute based on the displacement of the end effector of the robotic arm in Cartesian space, so as to achieve more efficient motion planning.

[0007] To achieve the above objectives, the present invention adopts the following technical solution:

[0008] A motion planning method for a multi-joint robotic arm includes the following steps:

[0009] For the robotic arm, a forward model and multiple reverse models corresponding to each joint are established. The forward model is used to establish the mapping from the angle of each joint of the robotic arm to the pose of the robotic arm end effector. Each reverse model is used to establish the mapping from the pose of the robotic arm end effector to the angle of the corresponding joint.

[0010] Input the current angles of each joint and the relative position of the robotic arm end to the target into the reverse model to generate a set of motion commands containing the rotation angles of each joint.

[0011] The positive model is used to generate a prediction of the execution of this set of motion commands, which is the pose of the robotic arm's end effector.

[0012] For each joint, a greedy strategy is used to select the optimal motion command that makes the end effector of the robotic arm closest to the target based on the prediction results of the positive model.

[0013] Without executing the selected optimal motion command, the next optimal motion command is selected based on the prediction results of the positive model, and the entire motion trajectory is planned iteratively in this way.

[0014] The forward and backward models are pre-trained in a simulation environment, and then optimized and adjusted based on the collected physical robot data, and the optimized auxiliary loss function is calculated.

[0015] Furthermore, an inverse model for each joint is established based on the conditional generative adversarial network (CGAN). CGAN includes a generator and a discriminator. The generator generates a set of motion commands based on the current joint angle of the robotic arm and the relative position of the robotic arm end to the target. The discriminator uses the current joint angle of the robotic arm and the relative position of the robotic arm end to the target as discrimination conditions to determine the probability of whether it is a real motion command or a generated motion command.

[0016] Furthermore, CGAN uses adversarial training between the generator and the discriminator to make the motion commands generated by the generator gradually approach real motion commands. The generator adjusts its parameters to generate motion commands that are closer to reality, and the discriminator adjusts its parameters to improve its ability to distinguish between real motion commands and generated motion commands.

[0017] Furthermore, the formula for calculating the relative position of the robotic arm's end effector to the target is:

[0018]

[0019] Among them, P target P represents the target location. end_effector This indicates the current position of the robotic arm's end effector.

[0020] Furthermore, the expression for the greedy strategy is:

[0021]

[0022] in, For the optimal motion command of the joint, P target Indicates the target location, FM is the forward model, and q cur Let Δq be the current joint angle of the robotic arm. j This represents the rotation angle of the joint.

[0023] Furthermore, when selecting the optimal motion command, the amplitude λ of the robotic arm's end effector toward the target is calculated. i The calculation formula is:

[0024]

[0025] in, For the optimal motion command of the joint, P target Indicates the target location, FM is the forward model, and q cur This represents the current joint angle of the robotic arm.

[0026] Furthermore, the probability of selecting the optimal motion command for the joint is calculated based on the range of motion:

[0027]

[0028] Where, p i Let λ be the probability. i Let λ1 + λ2 + ... + λ be the range of motion of the i-th joint. n Let L be the total range of motion of all joints. Further, the optimized auxiliary loss function L is as follows:

[0029] L = L1 + L2

[0030]

[0031] Where L1 is the mean square error between the target relative position Δp and the arm end displacement Δp′ predicted by the positive model, L2 is the auxiliary loss function, and Δq i w represents the change in the i-th joint. i This represents a function relating the distance d between the end of the arm and the target, where α is the coefficient.

[0032] Furthermore, the function w i include w3 = w4 = d, w5 = d 2 and w6=d 3 .

[0033] A motion planning system for a multi-joint robotic arm includes:

[0034] A forward model module is used to establish the mapping from the angles of each joint of the robotic arm to the pose of the robotic arm's end effector; the forward model generates prediction results based on motion commands, and the prediction results are the pose of the robotic arm's end effector.

[0035] Multiple inverse model modules corresponding to each joint are used to establish a mapping from the end-effector pose to the angle of the corresponding joint. The inverse model generates a set of motion commands containing the rotation angle of each joint based on the input current angle of each joint and the relative position of the end-effector to the target.

[0036] The motion command selection module is used to select the optimal motion command for each joint based on the prediction results of the forward model using a greedy strategy. If the selected optimal motion command is not executed, the module continues to select the next optimal motion command based on the prediction results of the forward model, and so on iteratively to plan the entire motion trajectory.

[0037] The training module is used to pre-train the forward and backward models in a simulation environment, then optimize and adjust the forward and backward models based on the collected physical robot data, and calculate the optimized auxiliary loss function.

[0038] The technical effects achieved by this invention are as follows:

[0039] 1. This invention draws on the motion planning mechanism of the human arm and combines the different functions of each joint of the robotic arm. When approaching from a distance, the robotic arm mainly uses the joints near the shoulder joint, and when approaching from a short distance, it mainly uses the elbow joint. By rationally allocating the motion tasks of the joints, the efficiency and accuracy of the robotic arm when approaching are improved.

[0040] 2. This invention performs pre-training in a simulation environment, reducing the wear and tear on the physical robot, and optimizes the model using a small amount of physical robot data to ensure the model's effectiveness in practical applications.

[0041] 3. This invention uses a greedy strategy to select the optimal motion command and uses a positive model to predict and verify the selection results at each step, ensuring that the planned motion trajectory is efficient and feasible.

[0042] 4. This invention uses motion commands generated by the inverse model and prediction results from the forward model to iteratively perform motion planning, thereby achieving adaptive path planning and improving overall control performance.

[0043] 5. This invention designs an optimized auxiliary loss function to optimize the model, thereby improving the motion coordination of each joint and further enhancing the accuracy of motion planning.

[0044] 6. This invention calculates the selection probability based on the range of motion of each joint, flexibly adjusts the motion commands of each joint, and ensures the coordination and effectiveness of the overall motion. Attached Figure Description

[0045] Figure 1 This is a schematic diagram showing how rotating the shoulder or elbow joint moves the end of the robotic arm upwards.

[0046] Figure 2 This is a schematic diagram of the implementation framework of a multi-joint motion planning method for a robot arm according to the present invention.

[0047] Figure 3 It is a reverse model structural diagram of each joint of the robotic arm.

[0048] Figure 4 This is a structural diagram of a single joint inverse model based on CGAN. Detailed Implementation

[0049] To make the various technical features, advantages, or effects of the present invention more apparent and understandable, a detailed description is provided below in conjunction with the accompanying drawings.

[0050] This invention proposes a motion planning method for multiple joints of a robotic arm, the overall implementation framework of which is as follows: Figure 2 As shown. First, an internal model of the robot is established, including a forward model and a backward model. The forward model is used to establish the mapping from the joint angles of each joint of the robotic arm to the pose of the robotic arm's end effector in Cartesian space, and a neural network is used to model the forward model. The backward model is used to establish the mapping from the end effector pose to the joint angles. To reflect the different effects of the movement of each joint on the displacement of the robotic arm's end effector, a backward model is established for each joint of the robotic arm, that is, the rotation angle of each joint is calculated based on the current joint position of the robotic arm and the desired end effector displacement direction. In the process of establishing the internal model, since collecting a large amount of data on the physical robot would cause wear and tear on the robot, the forward and backward models are pre-trained in a simulation environment, and then the models are optimized and adjusted based on a small amount of data collected from the physical robot. Then, the multi-joint motion selection of the robotic arm is adopted by referencing the motion mechanism of the human arm. That is, when approaching from a distance, the joints of the robotic arm near the shoulder joint are prioritized, and when approaching from a short distance, the elbow joint of the robotic arm is prioritized. The joint motion is optimized by using an auxiliary loss function. Finally, after selecting a motion command for a joint at each step, it is not executed immediately. Instead, a forward model is used to predict the result after the command is executed, and the next motion command is selected based on this prediction. After planning an entire sequence of motion commands in this iterative manner, these motion commands are then executed.

[0051] This embodiment proposes a motion planning method for a multi-joint robotic arm, and its specific processing steps are as follows:

[0052] 1) Establish reverse models for each joint: Establish a reverse model (IM) for each joint of the robotic arm. i , where i refers to the i-th joint of the robotic arm, i = 1, 2, ..., n, and n is the number of joints in the robotic arm.

[0053] like Figure 3 As shown, the input to the inverse model is the current state of the robotic arm, i.e., the current joint angle qcu of the robotic arm. r The desired end-effector displacement direction, i.e., the relative position Δp between the end of the robotic arm and the target, is given, and the output is the corresponding joint rotation angle Δq. i .

[0054] q cur = (q1, q2, ..., q n )

[0055]

[0056] Δq i =IM i (q cur ,Δp)

[0057] Among them, P target Indicates the target position, i.e., the position that the robotic arm's end effector needs to reach; P end_effector This indicates the current position of the robotic arm's end effector.

[0058] This embodiment uses Conditional Generative Adversarial Networks (CGANs) to build an inverse model for each joint, the structure of which is as follows: Figure 4 As shown, it includes a generator and a discriminator. The generator is based on the input random noise z and the current joint condition information (including direction, amplitude value, and current joint angle [Δp, q]). cur The discriminator receives the actual motion command Δq and the generated motion command Δq, as well as the direction, amplitude value, and current joint angle [Δp, q]. cur As a discrimination criterion, the probability of whether a motion command is real or generated is calculated and output. CGAN uses adversarial training between the generator and the discriminator to make the motion commands generated by the generator gradually approach real motion commands. The generator continuously adjusts its parameters to generate increasingly realistic motion commands, while the discriminator continuously adjusts its parameters to better distinguish between real and generated motion commands. Based on the inverse model of each joint of CGAN, motion commands are generated by the generator and discriminated by the discriminator. The generator and discriminator compete and improve with each other, and ultimately the generator can generate motion commands that are close to real, making the motion planning of each joint of the robotic arm more accurate and efficient. Each joint has an independent CGAN model, and these models work together to achieve overall motion planning for multiple joints of the robotic arm.

[0059] 2) Establish a forward model of the robotic arm: Collect data from the physical robot and construct a forward model FM of the robotic arm, which is used to map from the joint angle q to the end pose p of the robotic arm.

[0060] p = FM(q)

[0061] 3) Motion Selection: Based on the current state of the robotic arm and the relative position of the end effector to the target, the backward model for each joint generates a set (k) of motion commands, all of which can cause the end effector to move towards the target. These commands are then predicted using the forward model. For the n joints of the robotic arm, a greedy strategy is employed to select the optimal motion command that drives the end effector to move closest to the target. And calculate the corresponding motion amplitude λ towards the target. i .

[0062]

[0063] Next, select the corresponding motion command based on the amplitude of the motion, that is, with probability p. i Select the corresponding motion command for the i-th joint.

[0064]

[0065] 4) Motion planning: In the previous step, a probability selection model was built based on the magnitude of each joint approaching the target in each step. After selecting the motion command for the current step, it is not executed immediately. Instead, the next action is selected based on the result of the positive model prediction. An entire motion trajectory is planned iteratively. After verifying the feasibility of the planned trajectory, these motion commands are executed.

[0066] 5) Model Optimization: Since collecting a large amount of data on the physical robot would cause wear and tear on the robot, we chose to pre-train the forward and backward models in a simulation environment, and then optimize and adjust the model based on a small amount of data collected from the physical robot. The auxiliary loss function L optimized in this method consists of two parts, as shown below:

[0067] L = L1 + L2

[0068]

[0069] The first part of the loss, L1, is the mean square error (MSE) between the target relative position Δp and the predicted end-effector displacement Δp′ output by the positive model. The second part of the loss, L2, is the auxiliary loss function, Δq. i This represents the change in the i-th joint, where α is a coefficient and w i This can be expressed as a function related to the distance d between the robotic arm's end effector and the target. The larger d is, the closer the joint is to the robot's body (base coordinates), the greater the w value. i The smaller the value, the farther the joint is from the robot's body (base coordinates) and the more likely it is to correspond to w. i The larger it is. Based on this principle, this section discusses w. i One design approach is shown below:

[0070]

[0071] w3 = w4 = d

[0072] w5=d 2

[0073] w6=d 3

[0074] Although the present invention has been disclosed above with reference to embodiments, it is not intended to limit the present invention. Appropriate modifications or equivalent substitutions made by those skilled in the art to the technical solutions of the present invention should be covered within the protection scope of the present invention, which is defined by the claims.

Claims

1. A motion planning method for a multi-joint robotic arm, characterized in that, Includes the following steps: For the robotic arm, a forward model and multiple reverse models corresponding to each joint are established. The forward model is used to establish the mapping from the angle of each joint of the robotic arm to the pose of the robotic arm end effector. Each reverse model is used to establish the mapping from the pose of the robotic arm end effector to the angle of the corresponding joint. Input the current angles of each joint and the relative position of the robotic arm end to the target into the reverse model to generate a set of motion commands containing the rotation angles of each joint. The positive model is used to generate a prediction of the execution of this set of motion commands, which is the pose of the robotic arm's end effector. For each joint, a greedy strategy is used to select the optimal motion command that makes the end effector of the robotic arm closest to the target based on the prediction results of the positive model. Without executing the selected optimal motion command, the next optimal motion command is selected based on the prediction results of the positive model, and the entire motion trajectory is planned iteratively in this way. The forward and backward models are pre-trained in a simulation environment, and then optimized and adjusted based on the collected physical robot data, and the optimized auxiliary loss function is calculated. Wherein, the optimized auxiliary loss function for ,in relative position of the target Compared with the arm end displacement predicted by the positive model Mean square error between ; As an auxiliary loss function, , Indicates the first The change in the number of joints Indicates the distance from the end of the arm and the target. Related functions, The coefficients; the function Configured as: when The larger the size, the closer the joint is to the robot's body. The smaller the joint, the farther it is from the robot's body. The larger the joint, the more likely it is to be the joint closest to the shoulder joint when approaching from a distance, and the more likely it is to be the joint closest to the elbow joint when approaching from a close distance.

2. The method as described in claim 1, characterized in that, The conditional generative adversarial network (CGAN) is used to build an inverse model for each joint. CGAN includes a generator and a discriminator. The generator generates a set of motion commands based on the current joint angle of the robotic arm and the relative position of the robotic arm end to the target. The discriminator uses the current joint angle of the robotic arm and the relative position of the robotic arm end to the target as discrimination conditions to determine the probability of whether it is a real motion command or a generated motion command.

3. The method as described in claim 2, characterized in that, CGAN uses adversarial training between the generator and the discriminator to make the motion commands generated by the generator gradually approach real motion commands. The generator adjusts its parameters to generate motion commands that are closer to reality, and the discriminator adjusts its parameters to improve its ability to distinguish between real motion commands and generated motion commands.

4. The method as described in claim 1, characterized in that, The formula for calculating the relative position of the robotic arm's end effector to the target is: in, Indicates the target location. This indicates the current position of the robotic arm's end effector.

5. The method as described in claim 1, characterized in that, The expression for the greedy strategy is: in, This represents the optimal movement command for the joint. Indicates the target location. Forward model, For the current robotic arm joint angle, This represents the rotation angle of the joint.

6. The method as described in claim 5, characterized in that, When selecting the optimal motion command, calculate the amplitude of motion of the robotic arm's end effector toward the target. The calculation formula is: in, This represents the optimal movement command for the joint. Indicates the target location. Forward model, This represents the current joint angle of the robotic arm.

7. The method as described in claim 6, characterized in that, Calculate the probability of selecting the optimal motion command for the joint based on the range of motion: in, For probability, For the first The range of motion of each joint This represents the total range of motion of all joints.

8. The method as described in claim 1, characterized in that, function include , , , and .

9. A motion planning system for a multi-joint robotic arm, implementing the method described in any one of claims 1-8, characterized in that, include: A forward model module is used to establish the mapping from the angles of each joint of the robotic arm to the pose of the robotic arm's end effector; The positive model generates a prediction result based on motion commands, which is the pose of the robotic arm's end effector. Multiple inverse model modules corresponding to each joint are used to establish the mapping from the end-effector pose to the angle of the corresponding joint; The reverse model generates a set of motion commands containing the rotation angles of each joint based on the current angles of each joint and the relative position of the robotic arm end to the target. The motion command selection module is used to select the optimal motion command for each joint based on the prediction results of the positive model using a greedy strategy. Without executing the selected optimal motion command, the next optimal motion command is selected based on the prediction results of the positive model, and the entire motion trajectory is planned iteratively in this way. The training module is used to pre-train the forward and backward models in a simulation environment, then optimize and adjust the forward and backward models based on the collected physical robot data, and calculate the optimized auxiliary loss function.

Images