Joint torque limited planar serial manipulator trajectory planning method and application thereof

Abstract

This invention discloses a trajectory planning method for a planar serial manipulator with joint torque constraints and its application. The method involves: acquiring the manipulator's initial state parameters, desired state parameters, total motion time, and joint drive torque constraints; determining the time allocation and zero-velocity transition point for the deceleration and acceleration processes; determining the forward time trajectory of the deceleration process; determining the reverse time trajectory of the acceleration process, and performing a time inversion operation about the midpoint of the acceleration process to obtain the forward time trajectory of the acceleration process; splicing the forward time trajectories of the deceleration and acceleration processes; determining whether the spliced ​​trajectory is smooth; if so, outputting the spliced ​​trajectory; otherwise, replanning; and inputting the spliced ​​trajectory into a trajectory tracking controller, which controls the manipulator to complete the desired motion. This method has low data processing volume and high speed, meeting the needs of near real-time control and showing good application prospects.

Description

Technical Field

[0001] This invention relates to the field of robot motion planning technology, and specifically to a method for trajectory planning of a planar serial manipulator with limited joint torque and its application. Background Technology

[0002] A robotic arm is an automated device that mimics certain movements of a human hand and arm to grasp, move objects, or operate tools according to a fixed program. It was the earliest industrial robot and the first modern robot. It can replace heavy human labor to achieve mechanization and automation of production, and can operate in hazardous environments to protect human safety. Therefore, it is widely used in machinery manufacturing, metallurgy, electronics, light industry, and nuclear energy sectors. Generally, a robotic arm includes a base, multiple connecting links, and an actuator. The connecting links are connected sequentially to form a strip-shaped moving part. The actuator is installed at the end of the strip-shaped moving part away from the base. Joints are provided between the base and the connecting links, and between adjacent connecting links. Adjustments to the actuator are achieved by driving these joints.

[0003] To ensure that the end effector of a robotic arm can complete the tasks set by the operator as accurately as possible, a feasible time series needs to be solved between the current and desired states (position, velocity, acceleration) of the robotic arm, taking into account constraints such as the robotic arm's configuration space, actuator performance, and obstacles in the workspace. Motion planning is the foundation of robotic arm trajectory control, and its performance is crucial to the efficiency, stability, and energy consumption of the robotic arm.

[0004] Trajectory planning is divided into Cartesian space planning and joint space planning. Cartesian space planning is performed within the robot's workspace, focusing on the trajectory curve of the robot's end effector. Joint space planning determines the angles of the robot's joints. Currently, planning robot motion trajectories typically uses optimization theory. Inequalities are constructed through constraints, and the optimization problem is solved to obtain the desired trajectory. Usually, such trajectories not only achieve the desired motion effect but also optimize efficiency, energy consumption, and other conditions.

[0005] Conventional industrial robots typically have a heavy, powerful base joint actuator, while the joints outside the base, which need to move with the robot, can only be fitted with lighter, less powerful actuators. Therefore, the driving limitations of the joint actuators must be considered when designing motion planning algorithms. Using conventional optimization methods would consume significant computational resources to detect whether the torque exceeds the limit, typically requiring offline planning instead of real-time control. Conversely, relying solely on setpoint control without planning would fail to meet the planning requirement of the end effector reaching the desired speed at the desired position.

[0006] Therefore, developing a robotic arm trajectory planning method that is applicable to general trajectory planning tasks, generates trajectories that are easy to track, requires less data processing, and has a faster data processing speed is of great practical significance. Summary of the Invention

[0007] Due to the aforementioned deficiencies in existing technologies, this invention provides a robotic arm trajectory planning method that is applicable to general trajectory planning tasks, generates trajectories that are easy to track, has a small data processing volume, and a fast data processing speed, thus overcoming the shortcomings of existing robotic arm trajectory planning methods, such as large data processing volume and slow processing speed.

[0008] To achieve the above objectives, the present invention provides the following technical solution:

[0009] A method for trajectory planning of a planar serial manipulator with joint torque constraints includes the following steps:

[0010] (1) Obtain the initial state parameters, desired state parameters, total motion time, and joint drive torque limit of the robot arm;

[0011] (2) Divide the trajectory from the initial state to the desired state into two segments: one is the acceleration process and the other is the deceleration process. Determine the time allocation and zero-speed transition point for the deceleration and acceleration processes.

[0012] (3) Determine the positive time trajectory of the deceleration process based on the differential equation composed of the setpoint controller and the dynamic equation of the manipulator;

[0013] (4) Determine the reverse time trajectory of the acceleration process based on the differential equation composed of the setpoint controller and the dynamic equation of the manipulator, and perform time inversion operation on it about the midpoint of the acceleration process to obtain the forward time trajectory of the acceleration process.

[0014] (5) Combine the positive time trajectory of the deceleration process obtained in step (3) with the positive time trajectory of the acceleration process obtained in step (4).

[0015] (6) Determine whether the trajectory obtained by splicing in step (5) is smooth. If so, output the trajectory obtained by splicing in step (5); otherwise, return to step (2).

[0016] (7) Input the output of step (6) into the trajectory tracking controller, and the trajectory tracking controller controls the robot.

[0017] The present invention provides a method for trajectory planning of a joint torque-constrained planar serial manipulator. The trajectory planning task is divided into a deceleration process and an acceleration process. The desired manipulator trajectory is obtained by splicing the forward time trajectory of the deceleration process with the forward time trajectory of the acceleration process obtained using time-reversal symmetry at the zero-velocity transition point. During deceleration, the manipulator is an energy dissipation system, so a trajectory satisfying the torque constraint can be quickly obtained through a setpoint controller. However, during acceleration, the manipulator is an energy input system and is unstable, making it impossible to obtain a trajectory reaching the desired state through a setpoint controller. Time-reversal symmetry applies to all systems satisfying the Lagrange equations. Therefore, the forward time trajectory of the acceleration process can be calculated based on this property, using the reverse time trajectory of the acceleration process. The trajectory obtained using this method is easy to track and has high accuracy. The forward time trajectory of the deceleration process and the reverse time trajectory of the acceleration process can be obtained by solving differential equations. Compared with existing methods, this method requires less data processing and has a faster data processing speed, meeting the needs of near real-time control and showing promising application prospects.

[0018] As a preferred technical solution:

[0019] As described above, the trajectory planning method for a planar serial manipulator with limited joint torque includes initial state parameters such as initial state joint angle, initial state angular velocity, and initial state angular acceleration.

[0020] The desired state parameters include the desired state joint angle, the desired state angular velocity, and the desired state angular acceleration.

[0021] As described above, in the planar serial manipulator trajectory planning method with joint torque limitation, step (2) determines the time allocation of the deceleration process and the acceleration process. Specifically, the time of the deceleration process and the time of the acceleration process can be arbitrarily allocated, but the sum of the two must be equal to the total motion time. One possible way is that both are half of the total motion time.

[0022] The trajectory planning method for a planar serial manipulator with joint torque constraints, as described above, includes the following process for determining the setpoint controller:

[0023] i) Compensate each link sequentially from the joint furthest from the base to the base joint for coupling torque caused by the series structure. The coupling torque includes centrifugal force and inertial force terms, thereby simplifying the multi-link model into a pendulum model.

[0024] ii) Design PD controllers sequentially from the joint furthest from the base of the robot arm to the joint of the base to obtain the desired input torque, and stack all the desired input torques to obtain the desired input torque vector;

[0025] iii) Based on the torque limit, design a saturation function, then embed the desired input torque vector into the saturation function to determine the final control input.

[0026] This invention treats each link of the robotic arm as an equivalent pendulum model, designs a PD controller to calculate the control torque, and finally embeds a saturation function to meet the requirements of the maximum torque limit.

[0027] As described above, in the planar serial manipulator trajectory planning method with limited joint torque, the forward time trajectory of the acceleration process in step (4) is specifically obtained based on the reverse time trajectory of the acceleration process according to the principle of time inversion symmetry.

[0028] As described above, in a planar serial manipulator trajectory planning method with limited joint torque, the joint angle trajectory and angular acceleration trajectory in the reverse time of the acceleration process are evenly symmetric with the joint angle trajectory and angular acceleration trajectory in the forward time of the acceleration process about the midpoint of time.

[0029] The angular velocity trajectory of the reverse time of the acceleration process is oddly symmetrical with respect to the angular velocity trajectory of the forward time of the acceleration process about the midpoint of time.

[0030] The present invention also provides a computer device, the computer device comprising:

[0031] At least one processor; and,

[0032] A memory communicatively connected to the at least one processor; wherein,

[0033] The memory stores computer-readable instructions, and when the processor executes the computer-readable instructions, it implements the joint torque-constrained planar serial manipulator trajectory planning method as described above.

[0034] In addition, the present invention provides a computer-readable storage medium storing computer-readable instructions, which, when executed by a processor, implement the joint torque-constrained planar serial manipulator trajectory planning method as described above.

[0035] The above technical solution is only one feasible technical solution of the present invention. The scope of protection of the present invention is not limited thereto. Those skilled in the art can reasonably adjust the specific design according to actual needs.

[0036] The above invention has the following advantages or beneficial effects:

[0037] (1) The joint torque-limited planar serial manipulator trajectory planning method of the present invention divides the trajectory planning task into a deceleration process and an acceleration process. The forward time trajectory of the deceleration process and the forward time trajectory of the acceleration process obtained by using time inversion symmetry are spliced ​​at the zero speed transition point to obtain the desired manipulator trajectory. The manipulator is controlled based on the desired manipulator trajectory. The method step sequence design is reasonable.

[0038] (2) The planar serial manipulator trajectory planning method with joint torque limited in this invention adds a stage for judging whether the spliced ​​trajectory is smooth, which significantly improves the overall accuracy of the method;

[0039] (3) The joint torque-constrained planar serial manipulator trajectory planning method of the present invention transforms the forward time trajectory planning of the acceleration process into the reverse time trajectory planning based on the time inversion symmetry, which simplifies the solution of the problem and obtains a trajectory that satisfies the torque constraint, making it easy to track and with high accuracy.

[0040] (4) The trajectory planning method of the planar serial manipulator with limited joint torque of the present invention can obtain the forward time trajectory of the deceleration process and the reverse time trajectory of the acceleration process by solving differential equations. Compared with the existing methods, the amount of data processing is small and the data processing speed is fast, which can meet the needs of real-time control and has good application prospects. Attached Figure Description

[0041] The invention, its features, shape, and advantages will become more apparent from the following detailed description of non-limiting embodiments with reference to the accompanying drawings. Like reference numerals denote like parts throughout the drawings. The drawings are not drawn to scale; their focus is on illustrating the gist of the invention.

[0042] Figure 1 This is a flowchart illustrating the step sequence of the planar serial manipulator trajectory planning method with limited joint torque according to the present invention.

[0043] Figure 2 This is a schematic diagram of a planar multi-link serial manipulator model to which the method of the present invention is applicable;

[0044] Figure 3 , 4 Figures 5 and 6 represent the results obtained after applying the joint torque-constrained planar series manipulator trajectory planning method of the present invention to a planar three-bar manipulator model for trajectory planning (joint angle trajectory, angular velocity trajectory, and angular acceleration trajectory).

[0045] Figure 6 A tracking error graph for using computational torque tracking control to calculate the trajectory planning results;

[0046] Figure 7 The output torque graph of the trajectory planning results is used for computational torque tracking control. Detailed Implementation

[0047] The structure of the present invention will be further described below with reference to the accompanying drawings and specific embodiments, but this is not intended to limit the present invention.

[0048] Example 1

[0049] A trajectory planning method for a planar serial manipulator with joint torque constraints is proposed. The planar multi-link serial robot model applied in this method is as follows: Figure 2 As shown, it satisfies the closed-loop dynamic equations of the manipulator, as follows:

[0050]

[0051] where θ∈R n Let M(θ) ∈ R be the joint coordinates of the robotic arm. n×n The inertia matrix, Let g(θ) ∈ R be a Coriolis matrix. n It is the gravity vector, u∈R n The output torque of the robotic arm joint actuator satisfies |u i |≤τ i,max , τ i,max This represents the maximum driving torque of each joint actuator.

[0052] The method specifically includes the following steps (the order of the steps is as follows): Figure 1 As shown):

[0053] Step 1: Read the user-inputted desired robot state and the current state of the robotic arm. The state includes the robot's joint angles, angular velocities, and angular accelerations, denoted as follows: Read the total planning time t input by the user f and the maximum joint torque vector τ max ;

[0054] Step 2: Divide the trajectory from the initial state to the desired state into two segments: one for acceleration and one for deceleration. Determine the time t at the transition point to zero velocity. m and state (θ) m ,0,0). Transition point time t m It needs to be less than the planned total time, and can generally be selected as half of the total time. The transition point connects the deceleration process and the acceleration process of the motion. At this moment, the robot needs to reach a stationary state. The configuration of the transition point can be selected as the average of the initial configuration and the desired configuration.

[0055] Step 3: Calculate the forward time trajectory of the deceleration process. During deceleration, each joint of the robot arm starts moving from its initial state and stabilizes in the transition configuration θ. mSpecifically, the trajectory is obtained by constructing a differential equation using the setpoint controller and the manipulator's dynamic equations, and then solving the numerical solution. The target configuration of the setpoint controller is θ. m The robotic arm's initial state is The setpoint controller can be selected arbitrarily, as long as the torque is within the limit range.

[0056] The setpoint controller is constructed as follows:

[0057] Define the absolute coordinates of each joint of the robotic arm.

[0058]

[0059] Where L is a unit lower triangular matrix, and the absolute coordinates of the current joint represent the sum of the base joint angles to the current joint angle.

[0060] Based on theoretical mechanics, the connecting rod after the i-th joint can be modeled as a simple pendulum. The following equation is the simplified dynamic equation of the simple pendulum model:

[0061]

[0062] Where, τ i Let the driving torque be the torque of the i-th joint. The mass is the equivalent simple pendulum model. The moment of inertia of the equivalent simple pendulum model, For the centroid of the equivalent simple pendulum model, To compensate for the coupling torque, and Compensate for the inertial force and centrifugal force on the i-th joint from the first link to the (i-1)-th link, respectively. and Compensate for the inertial force and centrifugal force on the i-th joint from the (i+1)-th link to the last link.

[0063] The target configuration is defined as θ d Its absolute coordinates are Define the position error as For each joint, a PD controller is designed, resulting in the following expression for the desired torque of each joint.

[0064]

[0065] Calculate the desired torque starting from the last joint and stack all torques to obtain the vector τ. Considering the maximum driving torque condition of the manipulator joints, the control input is defined as...

[0066] u = sat(τ, τ max (5)

[0067] Where sat(·) is the standard saturation function, which can be defined as any function that satisfies τ≤τ max A conditional function. One possible definition of sat(·) is as follows:

[0068] sat(τ,τ max )=[sat(τ1,τ 1,max ),…,sat(τ n ,τ n,max )] T

[0069]

[0070] Step 4, the forward time trajectory of the acceleration process During acceleration, the joints of the robotic arm transition from configuration θ. m The movement begins and reaches the desired state. Specifically, the reverse time trajectory of the acceleration process is first solved. Similar to step 3, by simultaneously solving equations (1) and (5), the initial state of the robot is set as follows: The target configuration is θ m Solving the differential equation yields the reverse time trajectory of the acceleration process. Secondly, according to the principle of time-reversal symmetry, a system satisfying the Lagrange equations under time-reversal operation possesses symmetry. This can be described as follows: the angle and angular acceleration curves of the two motions of the manipulator are even-symmetric about the midpoint of time, and the angular velocity curve is odd-symmetric about the midpoint of time. The control torque required to control the manipulator to complete these two motions is even-symmetric about the midpoint of time.

[0071] The forward time trajectory of the acceleration process is obtained through the following mapping.

[0072]

[0073] Step 5: Segment the deceleration trajectory and the acceleration trajectory, determine the feasibility of the trajectory, and connect the two trajectory segments at the transition time t. m At the splicing point, the continuity of the trajectory is checked according to the following formula;

[0074]

[0075] in, and ε represents the instants before and after the transition time, respectively. p ,ε v ,ε a These represent the maximum jumps in the allowable angle, angular velocity, and angular acceleration curves, respectively. If the end of the deceleration trajectory and the initial acceleration trajectory satisfy the condition of equation (7), i.e., the trajectory meets the continuity requirement, then the planning is successful and the desired trajectory is generated. Otherwise, the time allocation is readjusted, transition points are selected, and the controller parameters are adjusted to replan the trajectory.

[0076] Step 6: Output the trajectory to the robot trajectory tracking controller and execute the control program.

[0077] The accompanying drawings and embodiments illustrate the results of trajectory planning using the present invention on a three-bar planar serial manipulator;

[0078] Table 1 shows the parameters of the planar manipulator, including mass, moment of inertia, length, center of mass position, and maximum driving torque values ​​for each joint.

[0079] Table 1 Parameters of the Planar Three-Link Manipulator

[0080]

[0081] The planning objective is to guide the robot from state [0,0,0] to the target state [π,0,0], with the entire process taking 4.6 seconds. The transition time is chosen to be half the total time, i.e., 2.3 seconds. The transition configuration is the midpoint between the initial configuration and the desired configuration.

[0082] Figure 3 , 4 Figures 5 and 6 represent trajectories planned using the method proposed in this patent, including angle, angular velocity, and angular acceleration curves. The curve for the first half of the deceleration process is consistent with a typical setpoint control curve, stabilizing from the initial configuration to the transition configuration. In the second half of the acceleration process, the angle and angular acceleration curves are evenly symmetric about the midpoint of the acceleration phase time (3.45s). The angular velocity curve is oddly symmetric about the midpoint of the acceleration phase time. This is consistent with the time-reversal symmetry theory.

[0083] Figure 6 It is a trajectory tracking error. Figure 7 The trajectory generated using the method proposed in this patent is used to apply the control torque required for trajectory tracking control.

[0084] In the latter half of the acceleration phase, the joint actuator output torque approached saturation but did not exceed the maximum torque limit. The trajectory tracking error fluctuated within the acceptable range. These two results demonstrate that the trajectory generated by this invention can complete the trajectory planning task, meet the conditions for controlling the robot to reach the target state, and that the torque required for trajectory tracking is within limits.

[0085] Example 2

[0086] A computer device includes: at least one processor and a memory communicatively connected to the at least one processor;

[0087] The memory stores computer-readable instructions, and when the processor executes the computer-readable instructions, it implements the trajectory planning method for a planar serial manipulator with limited joint torque as described in Example 1.

[0088] Example 3

[0089] A computer-readable storage medium stores computer-readable instructions, which, when executed by a processor, implement the trajectory planning method for a planar serial manipulator with joint torque constraints as described in Embodiment 1.

[0090] If the aforementioned functions are implemented as software functional units and sold or used as independent products, they can be stored in a computer-readable storage medium. Based on this understanding, the technical solution of this invention, or the part that contributes to the prior art, or a part of the technical solution, can be embodied in the form of a software product. This computer software product is stored in a storage medium and includes several instructions to cause a computer device (which may be a personal computer, server, or network device, etc.) to execute all or part of the steps of the methods described in the various embodiments of this invention. The aforementioned storage medium includes various media capable of storing program code, such as USB flash drives, portable hard drives, read-only memory (ROM), random access memory (RAM), magnetic disks, or optical disks.

[0091] Those skilled in the art should understand that variations can be implemented by combining existing technology with the above embodiments, which will not be elaborated here. Such variations do not affect the essence of the present invention, and will not be elaborated here either.

[0092] The preferred embodiments of the present invention have been described above. It should be understood that the present invention is not limited to the specific embodiments described above, and the devices and structures not described in detail should be understood as being implemented in a conventional manner in the art. Any person skilled in the art can make many possible variations and modifications to the technical solutions of the present invention using the methods and techniques disclosed above, or modify them into equivalent embodiments with equivalent changes, without departing from the scope of the present invention. This does not affect the essential content of the present invention. Therefore, any simple modifications, equivalent changes, and modifications made to the above embodiments based on the technical essence of the present invention without departing from the content of the present invention's technical solutions still fall within the protection scope of the present invention.

Claims

1. A trajectory planning method for a planar serial manipulator with limited joint torque, characterized in that: Includes the following steps: (1) Obtain the initial state parameters, desired state parameters, total motion time, and joint drive torque limit of the robot arm; (2) Divide the trajectory from the initial state to the desired state into two segments, one for acceleration and one for deceleration. Determine the time allocation and zero-speed transition point for the deceleration and acceleration processes. Specifically, the time allocation for the deceleration and acceleration processes is determined as follows: allocate the time for the deceleration process and the time for the acceleration process according to the requirements and ensure that the sum of the time for the deceleration process and the time for the acceleration process is equal to the total motion time. (3) Determine the positive time trajectory of the deceleration process based on the differential equation composed of the setpoint controller and the dynamic equation of the manipulator; (4) Determine the reverse time trajectory of the acceleration process based on the differential equation composed of the setpoint controller and the dynamic equation of the manipulator, and perform time inversion operation on it about the midpoint of the acceleration process to obtain the forward time trajectory of the acceleration process. (5) Combine the forward time trajectory of the deceleration process obtained in step (3) with the forward time trajectory of the acceleration process obtained in step (4); (6) Determine whether the trajectory obtained by splicing in step (5) is smooth. If so, output the trajectory obtained by splicing in step (5); otherwise, return to step (2). (7) Input the output of step (6) into the trajectory tracking controller, and the trajectory tracking controller controls the robot arm; The process for determining the setpoint controller is as follows: i) Compensate for the coupling torque caused by the series structure in each link sequentially from the joint of the robot arm furthest from the base to the joint of the base; ii) Design PD controllers sequentially from the joint furthest from the base of the robot to the joint of the base to obtain the desired input torque, and stack all the desired input torques to obtain the desired input torque vector; iii) Based on the torque constraint, design a saturation function, embed the desired input torque vector into the saturation function, and determine the final control input; In step (4), the forward time trajectory of the acceleration process is specifically obtained based on the reverse time trajectory of the acceleration process according to the principle of time reversal symmetry. The joint angle trajectory and angular acceleration trajectory of the reverse time of the acceleration process are evenly symmetric with the joint angle trajectory and angular acceleration trajectory of the forward time of the acceleration process about the midpoint of time. The angular velocity trajectory of the reverse time of the acceleration process and the angular velocity trajectory of the forward time of the acceleration process are oddly symmetrical about the midpoint of time. Step (6) specifically involves: By splicing the deceleration and acceleration trajectories and determining the feasibility of the trajectory, the two trajectories are considered at the transition moment. At the splicing point, the continuity of the trajectory is checked according to the following formula; in, This represents the positive time trajectory of the deceleration process. To accelerate the positive time trajectory of the process, These represent the robot's joint angle, angular velocity, and angular acceleration at time t. and These represent the instants before and after the transition moment, respectively. These represent the maximum jumps in the allowable angle, angular velocity, and angular acceleration curves, respectively. If the end of the deceleration trajectory and the beginning of the acceleration trajectory satisfy the conditions of the above formula, i.e., the trajectory meets the continuity requirement, then the planning is successful and the desired trajectory is generated. Otherwise, the time allocation is readjusted, transition points are selected, and the controller parameters are adjusted to replan the trajectory.

2. The trajectory planning method for a planar serial manipulator with limited joint torque according to claim 1, characterized in that, The initial state parameters include the initial state joint angle, the initial state angular velocity, and the initial state angular acceleration; The desired state parameters include the desired state joint angle, the desired state angular velocity, and the desired state angular acceleration.

3. A computer device, characterized in that: The computer device includes: At least one processor; and, A memory communicatively connected to the at least one processor; wherein, The memory stores computer-readable instructions, and when the processor executes the computer-readable instructions, it implements the trajectory planning method for a planar serial manipulator with joint torque constraints as described in any one of claims 1 to 2.

4. A computer-readable storage medium, characterized in that: The computer-readable storage medium stores computer-readable instructions, which, when executed by a processor, implement the joint torque-constrained planar serial manipulator trajectory planning method as described in any one of claims 1 to 2.

Images