Trajectory optimization method of underwater manipulator based on dynamics

By introducing dynamic models and time scaling factor adjustment in the trajectory planning of underwater robotic arm, trajectory planning is optimized, and the problem of joint motor torque exceeding the limit is solved, improving the operating efficiency and accuracy of the robotic arm.

CN115351790BActive Publication Date: 2024-11-22NANJING UNIV OF AERONAUTICS & ASTRONAUTICS
View PDF 0 Cites 0 Cited by

Patent Information

Application Number
CN202211144194.3
Authority / Receiving Office
CN · China
Patent Type
Patents(China)
Current Assignee / Owner
Filing Date
2022-09-20
Publication Date
2024-11-22
Estimated Expiration
2042-09-20

AI Technical Summary

Technical Problem

The existing underwater robotic arm trajectory planning algorithm ignores dynamic constraints, resulting in joint motor torque exceeding the limit, resulting in motor oversaturation and shutdown problems.

Method used

Design a dynamics-based online trajectory optimization algorithm to calculate joint motor torque through dynamics model, introduce time scaling factor to adjust interpolation path points, ensure that the torque is within the dynamic limit, and use straight line, arc and s-type velocity planning algorithms to optimize trajectory .

Benefits of technology

It effectively avoids unnecessary speed reduction and only adjusts in the torque exceeding the limit to ensure the operation efficiency of the robotic arm and avoids motor oversaturation and shutdown.

✦ Generated by Eureka AI based on patent content.

Smart Images

  • Figure CN115351790B_ABST
    Figure CN115351790B_ABST
Patent Text Reader

Abstract

The present invention discloses a method for optimizing the trajectory of an underwater manipulator considering dynamic characteristics, and belongs to the field of mechanical equipment control. The existing technology only considers the kinematic constraints of the manipulator such as the speed and acceleration of the manipulator for the trajectory planning algorithm, while ignoring the dynamic constraints of the manipulator, which may lead to the situation that the joint motor torque exceeds the limit when the manipulator is running, which will make the manipulator unable to accurately execute the planned trajectory or even shut down. The present invention adds the dynamic constraints of the manipulator into the trajectory optimization algorithm, divides them into time scaling factors and look-ahead time, and when it is detected that the joint torque exceeds the limit, the time scaling factor is timely introduced to locally adjust the joint torque, so that the joint torque meets the dynamic limit. Compared with the global speed regulation of the prior art, the trajectory optimization of the present invention avoids unnecessary speed reduction, and only adjusts the part where the joint torque exceeds the limit, ensuring the efficiency of the underwater manipulator during operation while meeting the dynamic limit.
Need to check novelty before this filing date? Find Prior Art

Description

Technical Field

[0001] The invention discloses a dynamics-based underwater manipulator trajectory optimization method, in particular to an underwater manipulator trajectory optimization method taking dynamic characteristics into consideration, and belongs to the field of motion control of underwater manipulators. Background Art

[0002] When there are dynamic coupling or nonlinear effects in the automated mechanism, the torque required of the drive system during trajectory execution may exceed its physical limit. This is very likely to happen when the motor load is not a constant value but a nonlinear function. As a typical mechanism with dynamic coupling and nonlinear effects, the underwater manipulator has highly coupled joints, and the joint motor load exhibits nonlinear characteristics as the manipulator posture changes.

[0003] Usually, the trajectory planning algorithm for the robot only considers the kinematic constraints of the robot such as the speed and acceleration, while ignoring the dynamic constraints of the robot. This may cause the joint motor torque to exceed the limit when the robot is running a predetermined trajectory, which will cause the joint motor to be in an oversaturated state, making it impossible for the robot to accurately execute the planned trajectory or even causing the motor to alarm and shut down. Summary of the invention

[0004] The purpose of the present invention is to provide a dynamics-based underwater manipulator trajectory optimization method, which adds the dynamic constraints of the manipulator into the trajectory optimization algorithm. After analyzing the influence of the introduction of the time scaling factor on the kinematic and dynamic parameters such as the manipulator joint speed, acceleration, torque, etc., an online trajectory optimization algorithm is designed based on this as a theoretical basis to solve the problem that when the existing underwater manipulator trajectory planning algorithm runs a predetermined trajectory, the joint motor torque exceeds the limit, resulting in the joint motor being in an oversaturated state, which makes the manipulator unable to accurately execute the planned trajectory, and even causes the motor to alarm and shut down.

[0005] The dynamic speed control algorithm for underwater manipulator trajectory optimization based on dynamics designed by the present invention comprises the following steps:

[0006] Step 1: Linear path planning, circular path planning and S-type speed planning are used for the Cartesian space trajectory planning of the underwater manipulator; S-type speed planning is used for the joint space trajectory planning of the underwater manipulator to obtain interpolation path points corresponding to different interpolation times, and the obtained interpolation path points are the planned trajectory;

[0007] Step 2: Determine the initial look-ahead time, calculate the joint motor torque corresponding to the interpolation path point obtained in step 1 within the initial look-ahead time through the dynamic model, compare the calculated joint motor torque with the rated torque of the joint motor, and determine whether the joint motor torque exceeds the limit within this look-ahead time range. If not, the initial time scaling factor is 1, and jump to step 4; if the joint motor torque exceeds the limit, then according to the obtained joint motor torque τ i The upper limit of the rated torque of the motor And the corresponding lower limit of the rated torque of the joint motor To calculate the time scaling factor, it is as follows:

[0008] When τ i >0, if Then the maximum value of the time scaling factor λ max for Minimum time scaling factor λ min for like Then the maximum value of the time scaling factor λ max for Minimum time scaling factor λ min is 0;

[0009] When τ i =0, the motor is in a stopped state;

[0010] When τ i <0, if Then the maximum value of the time scaling factor λ max for Minimum time scaling factor λ min is 0; if Then the maximum value of the time scaling factor λ max for Minimum time scaling factor λ min for

[0011] The obtained time scaling factor is stored in the target time scaling factor sequence maintained by the monotonic queue. Each target time scaling factor in this sequence is an element in this sequence. Each element in this sequence will start a time scaling adjustment task.

[0012] Step 3: Start a time scaling adjustment task, use the head element in the target time scaling factor sequence as the target factor of the adjustment task, adjust the actual time scaling factor; and adjust the speed based on the actual time scaling factor. According to the actual time scaling factor, calculate the interpolation path point, which is the interpolation path point after speed adjustment. The robot executes the interpolation path point after speed adjustment and jumps to step 5.

[0013] Step 4: If there is no element in the target time scaling factor sequence, the robot arm will perform interpolation path points according to the original planned trajectory;

[0014] Step 5: When the robot arm has executed the last interpolation path point, jump to step 11; if the robot arm has not executed the last interpolation path point, jump to step 6.

[0015] Step 6: Calculate the joint motor torque corresponding to the remaining interpolation path points within the look-ahead time through the dynamic model, compare the calculated joint motor torque with the rated torque of the joint motor, and determine whether the joint motor torque exceeds the limit within the look-ahead time range. If not, jump to step 8; if not, jump to step 7.

[0016] Step 7: According to the obtained joint motor torque τ i The upper limit of the motor torque of this joint corresponding to the manual And the corresponding lower limit of the motor torque of this joint To calculate the time scaling factor, it is as follows:

[0017] When τ i >0, if Then the maximum value of the time scaling factor λ max for Minimum time scaling factor λ min for like Then the maximum value of the time scaling factor λ max for Minimum time scaling factor λ min is 0;

[0018] When τ i =0, the motor is in a stopped state;

[0019] When τ i <0, if Then the maximum value of the time scaling factor λ max for Minimum time scaling factor λ min is 0; if Then the maximum value of the time scaling factor λ max for Minimum time scaling factor λ min for The obtained time scaling factor is stored in the target time scaling factor sequence maintained by the monotonic queue.

[0020] Step 8: If there is a time scaling adjustment task being executed, jump to step 9; if there is no time scaling adjustment task being executed, jump to step 10.

[0021] Step 9: Determine whether the time scaling adjustment task corresponding to the head element of the target time factor sequence has been completed. If it has been completed, pop up the head element to perform the next time scaling adjustment task; if it has not been completed, continue to execute the current time scaling adjustment task, calculate the interpolation path point according to the actual time scaling factor, execute this interpolation path point, and jump to step 5.

[0022] Step 10: If the target time factor sequence is empty, set the time scaling factor to 1, and there is no need to perform the time scaling adjustment task. Execute the planned interpolation path points and jump to step 5. If there are still elements in the target time factor sequence, start a new time scaling adjustment task based on the head element, calculate the interpolation path points based on the actual time scaling factor, obtain the new interpolation path points, execute this new interpolation path points, and jump to step 5.

[0023] Step 11: End the algorithm and change the original interpolation time by modifying the value of the time scaling factor to achieve the purpose of time scaling and obtain the optimized trajectory interpolation path points. These optimized interpolation path points are the new planned trajectory.

[0024] Furthermore, the straight line path planning described in step 1 is:

[0025] Input parameters: coordinates of the first and last points (x0, y0, z0, a0, b0, c0), (x1, y1, z1, a1, b1, c1);

[0026] Output the current end interpolation point (x t ,y t , z t , a t , b t , c t );

[0027] Under normal circumstances, the distance between the two points is calculated according to the coordinates of the starting and ending points, and the distance between the two points is planned by S-shaped speed, and the distance obtained by S-shaped speed planning is st Interpolation is performed based on the total distance, that is,

[0028] x t =x0+k·(x1-x0)

[0029] y t =y0+k·(y1-y0)

[0030] z t =z0+k·(z1-z0)

[0031] a t =a0+k·(a1-a0)

[0032] b t = b0 + k·(b1 - b0)

[0033] c t = c0 + k·(c1 - c0)

[0034] k = distance st / distance

[0035] The arc path planning is as follows:

[0036] Input parameters: the coordinates of the start and end points of the arc, and the coordinates of an intermediate point;

[0037] Output the interpolation point (x t , y t , z t , a t , b t , c t ) at the current moment;

[0038] Establish a local plane coordinate system according to the three points given by the user, and calculate the center coordinates and radius of the arc in the local plane coordinate; then establish a new local plane coordinate system with the center as the coordinate origin, and at the same time determine the mapping relationship T between the local plane coordinate system and the base coordinate system, where T is a 4*4 homogeneous transformation matrix; calculate the coordinates of the three points given by the user in the local coordinate system, and at the same time calculate the arc angle θ, perform an S-shaped speed planning on the arc angle θ to obtain θ st , and based on this, perform interpolation in the local plane coordinate system. After obtaining the interpolation result in the local plane coordinate system, obtain the interpolation result (x t , y t , z t , a t , b t , c t ) in the base coordinate system through the mapping relationship T between the local plane coordinate system and the base coordinate system, that is

[0039] x′ t = r*cosθ st

[0040] y′ t = r*sinθ st

[0041]

[0042] The existing S-shaped speed planning is as follows:

[0043] The S-shaped acceleration and deceleration planning is divided into three major segments and seven small segments; the first major segment is the acceleration segment, that is, the running time t is between 0 and Ta (0 <= t < Ta), which is divided into three small segments in total. The first segment has a constant jerk jmax Accelerate the acceleration from 0 to a preset maximum acceleration a max , the second stage accelerates at a constant maximum acceleration, and the third stage decelerates the acceleration to zero at a constant deceleration; the second major stage is the constant maximum speed operation stage, that is, the operation time is between Ta and Ta + Tv (Ta <= t < Ta + Tv), and it runs at a constant maximum speed Vmax; the third major stage is the deceleration stage, that is, the operation time is between Ta + Tv and Ta + Tv + Td (Ta + Tv <= t <= Ta + Tv + Td), which is divided into three small segments. The first segment decelerates the acceleration from 0 to a at a constant deceleration min , the second segment decelerates at a constant deceleration, and the third segment increases the deceleration from a min to 0;

[0044] The input variables of the joint S-shaped acceleration and deceleration planning are: initial joint position, target joint position, initial joint angular velocity, target joint angular velocity; the outputs are: joint position, joint angular velocity, joint angular acceleration; the input variables of the Cartesian S-shaped acceleration and deceleration planning are: initial end Cartesian position, target end Cartesian position, initial Cartesian space velocity, target Cartesian space velocity; the outputs are end position, end velocity, end acceleration;

[0045] The joint S-shaped acceleration and deceleration planning plans the joint velocity of the joint, and after interpolation, the joint angle interpolation points at each moment are obtained, so as to smooth the joint angular velocity and make the joint angular acceleration continuous, reducing the impact phenomenon in the joint space during the operation of the robotic arm; the end S-shaped planning plans the Cartesian space velocity of the end of the robotic arm, and after interpolation, the end position of the robotic arm at each moment is obtained, and the joint positions are obtained through inverse kinematics;

[0046] After planning using the S-shaped acceleration and deceleration planning algorithm, the jerk, acceleration, velocity and other motion parameters during the trajectory operation, as well as the durations of the seven segments of motion of jerk, uniform acceleration, deceleration, constant speed, acceleration and deceleration, uniform deceleration, and deceleration are determined; according to the above parameters, the function expression of the joint position is completely determined, which is a function of the interpolation time; substituting different interpolation time values into this function, the interpolation trajectory points corresponding to different interpolation times are obtained, and this series of interpolation trajectory points is the planned trajectory.

[0047] Further, the rated torque of the joint motor in step 2 is recorded in the joint motor instruction manual.

[0048] Further, the dynamic model in step 2 is obtained by Lagrangian dynamics modeling using the energy method to obtain the dynamic equation:

[0049] The Lagrangian function is a scalar function representing the difference between the kinetic energy and potential energy of the robotic arm, and can be expressed as:

[0050] L=KP (Formula 1-9)

[0051] Among them, K is the total kinetic energy of the robot arm, and P is the total potential energy of the robot arm;

[0052] Furthermore, the second Lagrangian equation is introduced based on the Lagrangian function:

[0053]

[0054] Among them, q is the n-dimensional joint position vector, is the n-dimensional joint velocity vector, τ is the n-dimensional joint driving torque vector;

[0055] The dynamic model of the robot arm can be obtained based on the second kind of Lagrangian equations;

[0056] The Lagrangian function needs to calculate the kinetic energy and potential energy of the robot; for a point on the robot link i, its coordinate r in the link i coordinate system i For [x i ,y i ,z i ,1] T , then its position coordinates in the generalized coordinate system 0 are The speed is:

[0057]

[0058] The kinetic energy of the point is calculated from the velocity of the point:

[0059]

[0060] Integrating over rod i, the kinetic energy of rod i is

[0061]

[0062] The change to J i is the mass distribution matrix of connecting rod i, and its specific form is:

[0063]

[0064] The change to J ix ,J iy ,J iz is the moment of inertia of the connecting rod coordinate system i about the three axes, J ixy ,J ixz ,J iyz is the inertia product in the connecting rod coordinate system i, x ic ,y ic ,z ic is the position of the connecting rod center of mass in the connecting rod coordinate system, m iis the mass of the connecting rod;

[0065] From formula (1-13), the total kinetic energy of the robot arm is:

[0066]

[0067] The potential energy of connecting rod i is

[0068]

[0069] Among them, g i is the gravity acceleration vector in the connecting rod coordinate system i, g 0 The gravitational acceleration vector in the generalized coordinate system, is the coordinate position of the connecting rod center of mass in the connecting rod coordinate system i;

[0070] From the potential energy of a single link, the total potential energy of the robotic arm can be obtained as

[0071]

[0072] According to the expressions of the total kinetic energy and total potential energy of the robot, the Lagrangian function and its partial derivatives can be calculated:

[0073]

[0074]

[0075] Substituting equations 1-17 and 1-18 into the second kind of Lagrange equations, we get:

[0076]

[0077] It can be written in matrix form and further simplified as

[0078]

[0079] This equation is the state space equation of the robot arm dynamics, where H(q) is the mass matrix of the robot arm, For Proportional to the inertial force, The jth dimension of are the centrifugal force and Coriolis force of the robot arm, The jth dimension of G(q) is the gravity term of the robot arm, and the jth dimension of G(q) is

[0080] This dynamic equation is not the final dynamic equation. Since the underwater manipulator is equipped with a buoyancy module to create a weightless environment, the buoyancy it receives cancels out the gravity, thus eliminating the gravity term G(q) in the dynamic equation. At the same time, since the underwater manipulator is affected by fluid resistance during movement, the Morison equation is used to model the fluid resistance on the manipulator to obtain the fluid resistance term τ. H ; Therefore, the final kinetic equation is:

[0081]

[0082] The above final dynamic equation is n-dimensional, where n is the number of joints of the robot. Analyze the i-th row in the final dynamic equation, where i corresponds to the i-th joint of the robot, and obtain the dynamic equation of joint i. Modify the form of the above dynamic equation to obtain the modified dynamic equation of joint i:

[0083]

[0084] Among them, m i is the i-th column of the mass matrix H(q); yes Another form of expression is ” is the fluid resistance term;

[0085] For the trajectory q(t) in the dynamics equation, an intermediate parameter u is introduced to generate a new trajectory The intermediate parameter u is derived from a strictly increasing scalar function t = α(u), and the trajectory is described by a parametric curve as:

[0086]

[0087] The dynamic equation for executing this new trajectory can be expressed as:

[0088]

[0089] At the same time, the velocity and acceleration relationship between the new trajectory and the old trajectory is:

[0090]

[0091]

[0092] Substituting equations 1-24 and 1-25 into equation 1-23, the dynamic equation of the new trajectory can be described as:

[0093]

[0094] Furthermore, the initial look-ahead time of the robot described in step 2 is to calculate the look-ahead time of each joint, and then take the maximum look-ahead time among the look-ahead times of each joint as the initial look-ahead time of the robot, so the calculation formula of the look-ahead time is:

[0095]

[0096]

[0097] Among them, T i p is the look-ahead time of joint i, is the real-time speed of joint i during the operation of the robot, is the maximum acceleration of joint i, i.e., the acceleration constraint, T p It is the final look-ahead time of the dynamic speed control algorithm of the entire robot arm.

[0098] Furthermore, the method for determining the time scaling factor in step 2 is as follows:

[0099] Take the intersection of the ranges of the time scaling factors λ for each joint:

[0100]

[0101] in, is the lower limit of the time scaling factor for joint i, is the upper limit of the time scaling factor of joint i, and the overall time scaling factor adjustment range [λ min ,λ max ], the upper limit λ of the overall time scaling factor adjustment range should always be selected max As the value of the time scaling factor λ.

[0102] Furthermore, the method for judging whether the time scaling adjustment task corresponding to the head element of the target time factor sequence has been executed as described in step 9 is to judge whether the current actual time scaling factor value has reached the head element value of the target time factor sequence. If so, the task is completed; if not, the task is not yet completed.

[0103] The calculation method of the interpolation time described in step 11 is:

[0104] t n =t n-1 +λ·Δ t

[0105] Among them, t n is the interpolation time of the nth control cycle, t n-1 is the interpolation time of the n-1th control cycle, λ is the time scaling factor, Δ tis the control cycle of the robot arm; after the interpolation time value is modified, t n Substituting the function expression of the joint position, we can get the optimized trajectory interpolation points, which are the new planned trajectory.

[0106] The present invention has the following advantages and beneficial effects: after analyzing the influence of introducing a time scaling factor on the kinematic and dynamic parameters such as the mechanical arm joint speed, acceleration, torque, etc., the present invention uses this as a theoretical basis to design an online trajectory optimization algorithm. On the basis of the mechanical arm running according to the established trajectory planned by the trajectory planner in the prior art, an online trajectory optimization algorithm is designed. While the mechanical arm runs according to the established trajectory, the torque of each joint of the mechanical arm is calculated in real time. When it is detected that the joint torque exceeds the torque upper limit, the time scaling factor is introduced in time to perform local trajectory optimization to adjust the joint torque, so that the joint torque meets the dynamic limit. Compared with the global speed regulation of the prior art, the trajectory optimization of the present invention avoids unnecessary speed reduction, and only adjusts the part where the joint torque exceeds the limit, ensuring the efficiency of the underwater mechanical arm during operation while meeting the dynamic limit. BRIEF DESCRIPTION OF THE DRAWINGS

[0107] Figure 1 :Flowchart of underwater manipulator trajectory optimization method based on dynamics;

[0108] Figure 2 : Schematic diagram of S-shaped acceleration and deceleration planning of a single joint of an underwater manipulator;

[0109] Figure 3 : Comparison table of methods for determining time scaling factors. DETAILED DESCRIPTION

[0110] A specific embodiment of the present invention is as follows:

[0111] like Figure 1 As shown, the underwater manipulator trajectory optimization method based on dynamics in this embodiment has the following specific steps:

[0112] Step 1: Linear and circular path planning and S-type speed planning are used for the Cartesian space trajectory planning of the underwater manipulator; S-type speed planning is used for the joint space trajectory planning of the underwater manipulator, and the straight line path planning is:

[0113] Input parameters: coordinates of the first and last points (x0, y0, z0, a0, b0, c0), (x1, y1, z1, a1, b1, c1);

[0114] Output the current end interpolation point (x t ,y t , z t , a t , b t, c t ).

[0115] Under normal circumstances, the distance between the two points is calculated according to the coordinates of the starting and ending points, and the distance between the two points is planned by S-shaped speed, and the distance obtained by S-shaped speed planning is st Interpolation is performed based on the total distance, that is,

[0116] x t =x0+k·(x1-x0)

[0117] y t =y0+k·(y1-y0)

[0118] z t =z0+k·(z1-z0)

[0119] a t =a0+k·(a1-a0)

[0120] b t =b0+k·(b1-b0)

[0121] c t =c0+k·(c1-c0)

[0122] k=distance st / distance

[0123] The existing arc path planning is:

[0124] Input parameters: coordinates of the first and last points of the arc, and the coordinates of the middle point.

[0125] Output the current end interpolation point (x t ,y t , z t , a t , b t , c t ).

[0126] A local plane coordinate system is established based on the three points given by the user, and the coordinates of the center of the arc and the radius are calculated in the local plane coordinates. Then a new local plane coordinate system is established with the center of the circle as the coordinate origin, and the mapping relationship T between the local plane coordinate system and the base coordinate system is determined. T is a 4*4 homogeneous transformation matrix. The coordinates of the three points given by the user in the local coordinate system are calculated, and the arc angle θ is calculated. The arc angle θ is planned by S-type speed to obtain θ st , and interpolate in the local plane coordinate system based on this. After obtaining the interpolation result in the local plane coordinate system, the interpolation result (x t ,yt , z t , a t , b t , c t ), that is

[0127] x' t = r * cosθ st

[0128] y' t = r * sinθ st

[0129]

[0130] The S-shaped velocity profile is as follows:

[0131] The S-shaped acceleration and deceleration profile is divided into three major segments and seven sub-segments. The first major segment is the acceleration segment, that is, the running time t is between 0 and Ta (0 <= t < Ta), which is divided into three sub-segments. In the first sub-segment, the acceleration is increased from 0 to the pre-set maximum acceleration a at a constant jerk jmax max , in the second sub-segment, it runs at a constant maximum acceleration, and in the third sub-segment, the acceleration is reduced to zero at a constant deceleration. The second major segment is the constant maximum velocity running segment, that is, the running time is between Ta and Ta + Tv (Ta <= t < Ta + Tv), and it runs at a constant maximum velocity Vmax; the third major segment is the deceleration segment, that is, the running time is between Ta + Tv and Ta + Tv + Td (Ta + Tv <= t <= Ta + Tv + Td), which is divided into three sub-segments. In the first sub-segment, the acceleration is reduced from 0 to a at a constant deceleration min , in the second sub-segment, it decelerates at a constant deceleration, and in the third sub-segment, the deceleration is increased from a min to 0 at a constant acceleration, such as Figure 2 .

[0132] The input variables of the joint S-shaped acceleration and deceleration profile are: initial joint position, target joint position, initial joint angular velocity, target joint angular velocity. The outputs are: joint position, joint angular velocity, joint angular acceleration. The input variables of the Cartesian S-shaped acceleration and deceleration profile are: initial end Cartesian position, target end Cartesian position, initial Cartesian space velocity, target Cartesian space velocity. The outputs are end position, end velocity, end acceleration.

[0133] The joint S-shaped acceleration and deceleration profile plans the joint velocity of the joint, and after interpolation, the joint angle interpolation points at each moment are obtained, so that the joint angular velocity is smooth and the joint angular acceleration is continuous, effectively reducing the impact phenomenon in the joint space during the operation of the manipulator. The end S-shaped profile plans the Cartesian space velocity of the end of the manipulator, and after interpolation, the positions of the end of the manipulator at each moment are obtained, and the joint positions are obtained through the inverse kinematics.

[0134] After using the S-type acceleration and deceleration planning algorithm for planning, the motion parameters such as jerk, acceleration, and velocity during trajectory operation, as well as the duration of the seven motion segments of acceleration, uniform acceleration, deceleration, uniform speed, acceleration and deceleration, uniform deceleration, and deceleration are determined; based on the above parameters, the function expression of the joint position is completely determined, which is a function of the interpolation time.

[0135] Substituting different interpolation time values ​​into the function, we can obtain the interpolation trajectory points corresponding to different interpolation times. This series of interpolation trajectory points is the planned trajectory.

[0136] Step 2: Determine the initial look-ahead time. The initial look-ahead time of the robot is to calculate the look-ahead time of each joint, and then take the maximum look-ahead time among the look-ahead times of each joint as the initial look-ahead time of the robot. Therefore, the calculation formula for the look-ahead time is:

[0137]

[0138]

[0139] Among them, T i p is the look-ahead time of joint i, is the real-time speed of joint i during the operation of the robot, is the maximum acceleration of joint i, i.e., the acceleration constraint, T p is the final look-ahead time of the dynamic speed control algorithm of the entire robot arm. Then, the joint motor torque corresponding to all interpolation path points in step 1 within the initial look-ahead time is calculated through the dynamic model. The dynamic model is the Lagrangian dynamic model obtained by the energy method to obtain the dynamic equation:

[0140] The Lagrangian function is a scalar function that represents the difference between the kinetic energy and potential energy of the robot arm, which can be expressed as:

[0141] L=KP (Formula 1-9)

[0142] Among them, K is the total kinetic energy of the robot arm, and P is the total potential energy of the robot arm;

[0143] Furthermore, the second Lagrangian equation is introduced based on the Lagrangian function:

[0144]

[0145] Among them, q is the n-dimensional joint position vector, is the n-dimensional joint velocity vector, τ is the n-dimensional joint driving torque vector;

[0146] The dynamic model of the robot arm can be obtained based on the second kind of Lagrangian equations;

[0147] The Lagrangian function needs to calculate the kinetic energy and potential energy of the robot; for a point on the robot link i, its coordinate r in the link i coordinate system i For [x i ,y i ,z i ,1] T , then its position coordinates in the generalized coordinate system 0 are The speed is:

[0148]

[0149] The kinetic energy of the point is calculated from the velocity of the point:

[0150]

[0151] Integrating over rod i, the kinetic energy of rod i is

[0152]

[0153] The change to J i is the mass distribution matrix of connecting rod i, and its specific form is:

[0154]

[0155] The change to J ix ,J iy ,J iz is the moment of inertia of the connecting rod coordinate system i about the three axes, J ixy ,J ixz ,J iyz is the inertia product in the connecting rod coordinate system i, x ic ,y ic ,z ic is the position of the connecting rod center of mass in the connecting rod coordinate system, m i is the mass of the connecting rod;

[0156] From formula (1-13), the total kinetic energy of the robot arm is:

[0157]

[0158] The potential energy of connecting rod i is

[0159]

[0160] Among them, g i is the gravity acceleration vector in the connecting rod coordinate system i, g 0 The gravitational acceleration vector in the generalized coordinate system, is the coordinate position of the connecting rod center of mass in the connecting rod coordinate system i;

[0161] From the potential energy of a single link, the total potential energy of the robotic arm can be obtained as

[0162]

[0163] According to the expressions of the total kinetic energy and total potential energy of the robot, the Lagrangian function and its partial derivatives can be calculated:

[0164]

[0165]

[0166] Substituting equations 1-17 and 1-18 into the second kind of Lagrange equations, we get:

[0167]

[0168] It can be written in matrix form and further simplified as

[0169]

[0170] This equation is the state space equation of the robot arm dynamics, where H(q) is the mass matrix of the robot arm, For Proportional to the inertial force, The jth dimension of are the centrifugal force and Coriolis force of the robot arm, The jth dimension of G(q) is the gravity term of the robot arm, and the jth dimension of G(q) is

[0171] This dynamic equation is not the final dynamic equation. Since the underwater manipulator is equipped with a buoyancy module to create a weightless environment, the buoyancy it receives cancels out the gravity, thus eliminating the gravity term G(q) in the dynamic equation. At the same time, since the underwater manipulator is affected by fluid resistance during movement, the Morison equation is used to model the fluid resistance on the manipulator to obtain the fluid resistance term τ. H ; Therefore, the final kinetic equation is:

[0172]

[0173] The above final dynamic equation is n-dimensional, where n is the number of joints of the robot. Analyze the i-th row in the final dynamic equation, where i corresponds to the i-th joint of the robot, and the dynamic equation of joint i is obtained. Slightly modify the form of the above dynamic equation to obtain the modified dynamic equation of joint i:

[0174]

[0175] Among them, m i is the i-th column of the mass matrix H(q); yes Another form of expression is ” is the fluid resistance term;

[0176] For the trajectory q(t) in the dynamics equation, an intermediate parameter u is introduced to generate a new trajectory The intermediate parameter u is derived from a strictly increasing scalar function t = α(u), and the trajectory is described by a parametric curve as:

[0177]

[0178] The dynamic equation for executing this new trajectory can be expressed as:

[0179]

[0180] At the same time, the velocity and acceleration relationship between the new trajectory and the old trajectory is:

[0181]

[0182]

[0183] Substituting equations 1-24 and 1-25 into equation 1-23, the dynamic equation of the new trajectory can be described as:

[0184] Compare the calculated joint motor torque with the torque in the joint motor manual parameters to determine whether the joint motor torque exceeds the limit within the forward-looking time range. If not, the initial time scaling factor is 1 and jump to step 4; if the joint motor torque exceeds the limit, then according to the obtained joint motor torque τ i The upper limit of the motor torque of this joint corresponding to the manual And the corresponding lower limit of the motor torque of this joint To calculate the time scaling factor λ, as Figure 3 As shown, the intersection of the ranges of the time scaling factors λ of each joint is then taken:

[0185]

[0186] in, is the lower limit of the time scaling factor for joint i, is the upper limit of the time scaling factor of joint i; after fully determining the overall time scaling factor adjustment range [λ min ,λ max ], the upper limit λ of the overall time scaling factor adjustment range should always be selected maxAs the value of the time scaling factor λ, at this time, the torque of each joint motor is always between the upper and lower limits of the joint motor torque while avoiding unnecessary low speed affecting the operating efficiency of the robot arm.

[0187] The obtained time scaling factor is stored in the target time scaling factor sequence maintained by the monotonic queue. Each target time scaling factor in this sequence is an element in this sequence, and each element in this sequence will start a time scaling adjustment task.

[0188] Step 3: Start a time scaling adjustment task, use the head element in the target time scaling factor sequence as the target factor of the adjustment task, and adjust the actual time scaling factor; and adjust the speed based on the actual time scaling factor. According to the actual time scaling factor, the interpolation path point is calculated and obtained. This interpolation path point is the interpolation path point after speed adjustment. The robot executes the interpolation path point after speed adjustment and jumps to step 5.

[0189] The time scaling adjustment task needs to be used in conjunction with the monotonic queue data structure. After calculating a new target time scaling factor, it enters the adjustment task with the time scaling factor as the target. The task is unique and will only terminate when specific conditions are met. The monotonic queue is a linear table with a first-in-first-out (FIFO) characteristic, and its elements meet the characteristics of monotonically increasing or monotonically decreasing. When using the monotonic queue to maintain the target time scaling factor sequence, this sequence is always maintained as a monotonically decreasing sequence. The head element is the adjustment target of the current task and the smallest element in this monotonic queue. Every time a new target time scaling factor is calculated, this target scaling factor is compared with the tail element of the monotonic queue. If the tail element is larger, the tail element is popped out, otherwise the new target scaling factor is directly pushed into the tail. When an adjustment task is completed, the queue pops out the head element, which is the target time scaling factor corresponding to the task. In this way, the smallest element in the target time scaling factor sequence can always be guaranteed to be the target of the adjustment task, ensuring that the time scaling factor has enough time to be adjusted in place.

[0190] Step 4: If there is no element in the target time scaling factor sequence, the robot arm will perform interpolation path points according to the original planned trajectory.

[0191] Step 5: When the robot arm has executed the last interpolation path point, jump to step 11; if the robot arm has not executed the last interpolation path point, jump to step 6.

[0192] Step 6: Calculate the joint motor torque corresponding to the remaining interpolation path points within the look-ahead time through the dynamic model, compare the calculated joint motor torque with the torque in the joint motor manual parameter, and determine whether the joint motor torque exceeds the limit within the look-ahead time range. If not, jump to step 8; if not, jump to step 7.

[0193] Step 7: According to the obtained joint motor torque τ i The upper limit of the motor torque of this joint corresponding to the manual And the corresponding lower limit of the motor torque of this joint The time scaling factor is calculated by using Table 1, and the obtained time scaling factor is stored in the target time scaling factor sequence maintained by the monotonic queue.

[0194] Step 8: If there is a time scaling adjustment task being executed, jump to step 9; if there is no time scaling adjustment task being executed, jump to step 10.

[0195] Step 9: Determine whether the time scaling adjustment task corresponding to the head element of the target time factor sequence has been completed, that is, determine whether the current actual time scaling factor value has reached the head element value of the target time factor sequence; if it has been completed, pop up the head element to perform the next time scaling adjustment task; if it has not been completed, continue to execute the current time scaling adjustment task, calculate the interpolation path point according to the actual time scaling factor, execute this interpolation path point, and jump to step 5.

[0196] Step 10: If the target time factor sequence is empty, set the time scaling factor to 1, and there is no need to perform the time scaling adjustment task. Execute the planned interpolation path points and jump to step 5. If there are still elements in the target time factor sequence, start a new time scaling adjustment task based on the head element, calculate the interpolation path points based on the actual time scaling factor, obtain the new interpolation path points, execute this new interpolation path points, and jump to step 5.

[0197] Step 11: End the algorithm and change the original interpolation time by modifying the value of the time scaling factor to achieve the purpose of time scaling. The calculation method of the interpolation time is:

[0198] t n =t n-1 +λ·Δ t

[0199] Among them, t n is the interpolation time of the nth control cycle, t n-1 is the interpolation time of the n-1th control cycle, λ is the time scaling factor, Δ t is the control cycle of the robot arm; after the interpolation time value is modified, tn Substituting the function expression of the joint position, we can get the optimized trajectory interpolation points, which are the new planned trajectory.

[0200] The underwater manipulator runs on a new planned trajectory, thus avoiding unnecessary speed reduction and excessive torque of the joint motor, achieving the purpose of the present invention and achieving the beneficial effects.

Claims

1. A dynamics-based underwater manipulator trajectory optimization method, characterized in that: The following steps are involved: Step 1: Linear path planning, circular path planning and S-type speed planning are used for the Cartesian space trajectory planning of the underwater manipulator; S-type speed planning is used for the joint space trajectory planning of the underwater manipulator to obtain interpolation path points corresponding to different interpolation times, and the obtained interpolation path points are the planned trajectory; Step 2: Determine the initial look-ahead time, calculate the joint motor torque corresponding to the interpolation path point obtained in step 1 within the initial look-ahead time through the dynamic model, compare the calculated joint motor torque with the rated torque of the joint motor, and determine whether the joint motor torque exceeds the limit within the look-ahead time range. If not, the initial time scaling factor is 1, and jump to step 4; If the joint motor torque exceeds the limit, then according to the obtained joint motor torque τ i The upper limit of the rated torque of the motor And the corresponding lower limit of the rated torque of the joint motor To calculate the time scaling factor, it is as follows: When τ i >0, if Then the maximum value of the time scaling factor λ max for Minimum time scaling factor λ min for like Then the maximum value of the time scaling factor λ max for Minimum time scaling factor λ min is 0; When τ i =0, the motor is in a stopped state; When τ i <0, if Then the maximum value of the time scaling factor λ max for Minimum time scaling factor λ min is 0; if Then the maximum value of the time scaling factor λ max for Minimum time scaling factor λ min for The obtained time scaling factor is stored in the target time scaling factor sequence maintained by the monotonic queue. Each target time scaling factor in this sequence is an element in this sequence. Each element in this sequence will start a time scaling adjustment task. Step 3: Start a time scaling adjustment task, use the head element in the target time scaling factor sequence as the target factor of the adjustment task, and adjust the actual time scaling factor; The speed is adjusted based on the actual time scaling factor, and an interpolation path point is calculated according to the actual time scaling factor. This interpolation path point is the interpolation path point after the speed adjustment. The robot arm executes the interpolation path point after the speed adjustment and jumps to step 5. Step 4: If there is no element in the target time scaling factor sequence, the robot arm will perform interpolation path points according to the original planned trajectory; Step 5: When the robot arm has executed the last interpolation path point, jump to step 11; if the robot arm has not executed the last interpolation path point, jump to step 6; Step 6: Calculate the joint motor torque corresponding to the remaining interpolation path points within the look-ahead time through the dynamic model, compare the calculated joint motor torque with the rated torque of the joint motor, and determine whether the joint motor torque exceeds the limit within the look-ahead time range. If not, jump to step 8. If the joint motor torque exceeds the limit, jump to step 7; Step 7: According to the obtained joint motor torque τ i The upper limit of the motor torque of this joint corresponding to the manual And the corresponding lower limit of the motor torque of this joint To calculate the time scaling factor, it is as follows: When τ i >0, if Then the maximum value of the time scaling factor λ max for Minimum time scaling factor λ min for like Then the maximum value of the time scaling factor λ max for Minimum time scaling factor λ min is 0; When τ i =0, the motor is in a stopped state; When τ i <0, if Then the maximum value of the time scaling factor λ max for Minimum time scaling factor λ min is 0; if Then the maximum value of the time scaling factor λ max for Minimum time scaling factor λ min for The obtained time scaling factor is stored in the target time scaling factor sequence maintained by the monotonic queue; Step 8: If there is a time scaling adjustment task being executed, jump to step 9; if there is no time scaling adjustment task being executed, jump to step 10; Step 9: Determine whether the time scaling adjustment task corresponding to the head element of the target time factor sequence has been completed. If completed, pop up the head element to perform the next time scaling adjustment task; If the execution is not completed, continue to execute the current time scaling adjustment task, calculate the interpolation path point according to the actual time scaling factor, execute the interpolation path point, and jump to step 5; Step 10: If the target time factor sequence is empty, set the time scaling factor to 1, no time scaling adjustment task is required, execute the planned interpolation path points, and jump to step 5; If there are still elements in the target time factor sequence, start a new time scaling adjustment task according to the head element, calculate the interpolation path point according to the actual time scaling factor, obtain a new interpolation path point, execute this new interpolation path point, and jump to step 5; Step 11: End the algorithm and change the original interpolation time by modifying the value of the time scaling factor to achieve the purpose of time scaling and obtain the optimized trajectory interpolation path points. These optimized interpolation path points are the new planned trajectory.

2. The underwater manipulator trajectory optimization method based on dynamics according to claim 1 is characterized in that: The straight line path planning described in step 1 is: Input parameters: coordinates of the first and last points (x0, y0, z0, a0, b0, c0), (x1, y1, z1, a1, b1, c1); Output the current end interpolation point (x t ,y t , z t , a t , b t , c t ); Under normal circumstances, the distance between the two points is calculated according to the coordinates of the starting and ending points, and the distance between the two points is planned by S-shaped speed, and the distance obtained by S-shaped speed planning is st Interpolation is performed based on the total distance, that is, x t =x0+k·(x1-x0) y t =y0+k·(y1-y0) With t =z0+k (z1-z0) A t =a0+k·(a1-a0) b t =b0+k·(b1-b0) c t =c0+k·(c1-c0) k=distance st / distance The arc path planning is: Input parameters: coordinates of the first and last points of the arc, and the coordinates of the middle point; Output the current end interpolation point (x t ,y t , z t , a t , b t , c t ); A local plane coordinate system is established based on the three points given by the user, and the coordinates of the center of the arc and the radius are calculated in the local plane coordinates. Then a new local plane coordinate system is established with the center of the circle as the coordinate origin, and the mapping relationship T between the local plane coordinate system and the base coordinate system is determined. T is a 4*4 homogeneous transformation matrix. The coordinates of the three points given by the user in the local coordinate system are calculated, and the arc angle θ is calculated. The arc angle θ is planned by S-type speed to obtain θ st , and interpolate in the local plane coordinate system based on this. After obtaining the interpolation result in the local plane coordinate system, the interpolation result (x t ,y t , z t , a t , b t , c t ),Right now x′ t =r*cosθ st y′ t =r*sinθ st The existing S-shaped speed plan is: The S-shaped acceleration and deceleration profile is divided into three major segments and seven minor segments. The first major segment is the acceleration segment, i.e., the running time t is between 0 and Ta (0 <= t < Ta), which is divided into three minor segments. In the first segment, with a constant jerk j max the acceleration is increased from 0 to the pre-set maximum acceleration a max . In the second segment, it accelerates at a constant maximum acceleration. In the third segment, the acceleration is decreased to zero with a constant deceleration jerk. The second major segment is the constant maximum speed segment, i.e., the running time is between Ta and Ta + Tv (Ta <= t < Ta + Tv), and it runs at a constant maximum speed Vmax. The third major segment is the deceleration segment, i.e., the running time is between Ta + Tv and Ta + Tv + Td (Ta + Tv <= t <= Ta + Tv + Td), which is divided into three minor segments. In the first segment, the acceleration is decreased from 0 to a with a constant deceleration jerk min . In the second segment, it decelerates at a constant deceleration. In the third segment, the deceleration is increased from a min to 0 with a constant acceleration jerk; The input variables of joint S-type acceleration and deceleration planning are: initial joint position, target joint position, initial joint angular velocity, target joint angular velocity; the output is: joint position, joint angular velocity, joint angular acceleration; the input variables of Cartesian S-type acceleration and deceleration planning are: initial terminal Cartesian space position, target terminal Cartesian position, initial Cartesian space velocity, target Cartesian space velocity; The output is end position, end velocity, and end acceleration; The joint S-shaped acceleration and deceleration planning plans the joint velocity of the joint, and obtains the joint angle interpolation points at each moment after interpolation, so as to make the joint angular velocity smooth and the joint angular acceleration continuous, and reduce the impact of the joint space when the robot is running; the terminal S-shaped planning plans the Cartesian space velocity of the end of the robot, and obtains the end position of the robot at each moment after interpolation, and obtains the position of each joint through the inverse kinematic solution; After using the S-type acceleration and deceleration planning algorithm for planning, the motion parameters such as jerk, acceleration, speed, and the duration of the seven motion segments of acceleration, uniform acceleration, deceleration, uniform speed, acceleration and deceleration, uniform deceleration, and deceleration are determined; based on the above parameters, the function expression of the joint position is completely determined, which is a function of the interpolation time; substituting different interpolation time values ​​into the function, the interpolation trajectory points corresponding to different interpolation times are obtained, and this series of interpolation trajectory points is the planned trajectory.

3. The underwater manipulator trajectory optimization method based on dynamics according to claim 1 is characterized in that: The kinetic model described in step 2 is obtained by Lagrangian kinetic modeling using the energy method: The Lagrangian function is a scalar function that represents the difference between the kinetic energy and potential energy of the robot arm, which can be expressed as: L=KP (Formula 1-9) Among them, K is the total kinetic energy of the robot arm, and P is the total potential energy of the robot arm; Furthermore, the second Lagrangian equation is introduced based on the Lagrangian function: Among them, q is the n-dimensional joint position vector, is the n-dimensional joint velocity vector, τ is the n-dimensional joint driving torque vector; The dynamic model of the robot arm can be obtained based on the second kind of Lagrangian equations; The Lagrangian function needs to calculate the kinetic energy and potential energy of the robot; for a point on the robot link i, its coordinate r in the link i coordinate system i For [x i ,y i ,z i ,1] T , then its position coordinates in the generalized coordinate system 0 are The speed is: The kinetic energy of the point is calculated from the velocity of the point: Integrating over rod i, the kinetic energy of rod i is The change to J i is the mass distribution matrix of connecting rod i, and its specific form is: The change to J ix ,J iy ,J iz is the moment of inertia of the connecting rod coordinate system i about the three axes, J ixy ,J ixz ,J iyz is the inertia product in the connecting rod coordinate system i, x ic ,y ic ,z ic is the position of the connecting rod center of mass in the connecting rod coordinate system, m i is the mass of the connecting rod; From formula (1-13), the total kinetic energy of the robot arm is: The potential energy of connecting rod i is Among them, g i is the gravity acceleration vector in the connecting rod coordinate system i, g 0 The gravitational acceleration vector in the generalized coordinate system, is the coordinate position of the connecting rod center of mass in the connecting rod coordinate system i; From the potential energy of a single link, the total potential energy of the robotic arm can be obtained as According to the expressions of the total kinetic energy and total potential energy of the robot, the Lagrangian function and its partial derivatives can be calculated: Substituting equations 1-17 and 1-18 into the second kind of Lagrange equations, we get: It can be written in matrix form and further simplified as This equation is the state space equation of the robot arm dynamics, where H(q) is the mass matrix of the robot arm, For Proportional to the inertial force, The jth dimension of are the centrifugal force and Coriolis force of the robot arm, The jth dimension of G(q) is the gravity term of the robot arm, and the jth dimension of G(q) is This dynamic equation is not the final dynamic equation. Since the underwater manipulator is equipped with a buoyancy module to create a weightless environment, the buoyancy it receives cancels out the gravity, thus eliminating the gravity term G(q) in the dynamic equation. At the same time, since the underwater manipulator is affected by fluid resistance during movement, the Morison equation is used to model the fluid resistance on the manipulator to obtain the fluid resistance term τ. H ; Therefore, the final kinetic equation is: The above final dynamic equation is n-dimensional, where n is the number of joints of the robot. Analyze the i-th row in the final dynamic equation, where i corresponds to the i-th joint of the robot, and obtain the dynamic equation of joint i. Modify the form of the above dynamic equation to obtain the modified dynamic equation of joint i: Among them, m i is the i-th column of the mass matrix H(q); yes Another form of expression is is the fluid resistance term; For the trajectory q(t) in the dynamics equation, an intermediate parameter u is introduced to generate a new trajectory The intermediate parameter u is derived from a strictly increasing scalar function t = α(u), and the trajectory is described by a parametric curve as: The dynamic equation for executing this new trajectory can be expressed as: At the same time, the velocity and acceleration relationship between the new trajectory and the old trajectory is: Substituting equations 1-24 and 1-25 into equation 1-23, the dynamic equation of the new trajectory can be described as:

4. The underwater manipulator trajectory optimization method based on dynamics according to claim 1 is characterized in that: The initial look-ahead time of the robot described in step 2 is to calculate the look-ahead time of each joint, and then take the maximum look-ahead time among the look-ahead times of each joint as the initial look-ahead time of the robot. Therefore, the calculation formula of the look-ahead time is: in, is the look-ahead time of joint i, is the real-time speed of joint i during the operation of the robot, is the maximum acceleration of joint i, i.e., the acceleration constraint, T p It is the final look-ahead time of the dynamic speed control algorithm of the entire robot arm.

5. The underwater manipulator trajectory optimization method based on dynamics according to claim 1, characterized in that: The time scaling factor described in step 2 is determined as follows: Take the intersection of the ranges of the time scaling factors λ for each joint: in, is the lower limit of the time scaling factor for joint i, is the upper limit of the time scaling factor of joint i, and the overall time scaling factor adjustment range [λ min ,λ max ], the upper limit λ of the overall time scaling factor adjustment range should always be selected max As the value of the time scaling factor λ.

6. The underwater manipulator trajectory optimization method based on dynamics according to claim 1, characterized in that: The method for judging whether the time scaling adjustment task corresponding to the head element of the target time factor sequence has been executed as described in step 9 is to judge whether the current actual time scaling factor value has reached the head element value of the target time factor sequence. If it has reached it, the task is completed; if not, the task is not yet completed.

7. The underwater manipulator trajectory optimization method based on dynamics according to claim 1, characterized in that: The calculation method of the interpolation time described in step 11 is: t n =t n-1 +λ·D t Among them, t n is the interpolation time of the nth control cycle, t n-1 is the interpolation time of the n-1th control cycle, λ is the time scaling factor, Δ t is the control cycle of the robot arm; after the interpolation time value is modified, t n Substituting the function expression of the joint position, we can get the optimized trajectory interpolation points, which are the new planned trajectory.