CN113084821A - Spraying robot time optimal trajectory planning method based on dynamics - Google Patents

Abstract

A dynamic-based time optimal trajectory planning method for a spraying robot relates to a robot trajectory planning method. Given a Cartesian space path; calculate the parameters related to the interpolation points: the number of interpolation points, Cartesian space parameters, joint space parameters and dynamic equation coefficients, Cartesian space parameters include position, attitude, linear velocity, Euler angular velocity, linear acceleration and Euler angular acceleration, joint space parameters include joint angle, joint velocity and joint acceleration; calculate velocity range; calculate velocity optimal value; smooth trajectory; output joint parameters. Considering both kinematic constraints and dynamic constraints by introducing dynamic models, it has obvious advantages in time compared with traditional trajectory planning methods. During operation, at least one joint motor reaches the limit operating state, and the driving performance is fully utilized, and The parameters of each joint will not exceed the limit value, improve work efficiency, and have universality.

Description

A Dynamics-based Time Optimal Trajectory Planning Method for Spraying Robot

technical field

The invention relates to a robot trajectory planning method, in particular to a dynamic-based spraying robot time optimal trajectory planning method, which belongs to the technical field of robot control.

Background technique

The application and development of industrial robots have promoted the progress of society, but with the continuous development of social economy, higher requirements have been placed on industrial automation technology. When spraying with robots, in order to improve the production efficiency of spraying robots and create greater economic value, it is necessary to make the robots move faster, more accurately and more safely. Therefore, in-depth research on trajectory planning of spraying robots is required.

However, the traditional trajectory planning method only considers the constraints of kinematics, does not give full play to the driving performance of the joint motor, and also has the problem that the planned speed exceeds the limit area, and the obtained trajectory is not the optimal trajectory in time, as shown in Figure 2. In order to overcome these shortcomings and achieve the time-optimized effect, it is necessary to consider the dynamic constraints for trajectory planning.

In the prior art, there are methods for time optimal planning, such as numerical integration method and convex optimization method. However, the numerical integration method is not universal and is not suitable for all dynamic models, while the convex optimization method has a large amount of calculation and is not efficient. Therefore, there is an urgent need for a time-optimized trajectory planning method for spraying robots.

SUMMARY OF THE INVENTION

The purpose of the present invention is to provide a time-optimized trajectory planning method for a spraying robot based on dynamics, which not only considers kinematic constraints, but also introduces a dynamic model to consider dynamic constraints, and has obvious advantages in time.

In order to achieve the above object, the present invention adopts the following technical scheme: a dynamic-based spraying robot time optimal trajectory planning method, comprising the following steps:

Step 1. Given a Cartesian space path: use the teaching point of the spraying robot to give a teaching point, and determine the path through the teaching point;

Step 2. Calculate the relevant parameters of the interpolation point: introduce the path parameter s, s represents the path length from the starting point to the interpolation point, set the path length to l, s∈[0,l], calculate the number of interpolation points, Cartesian space parameters, joint space parameters and dynamic equation coefficients, the Cartesian space parameters include: position, attitude, linear velocity, Euler angular velocity, linear acceleration and Euler angular acceleration, the joint space parameters include: joint angle, joint velocity and joint acceleration;

2.1 Number of interpolation points: the value formula is n=m×l, where m is the empirical coefficient;

2.2 If the path is an arc trajectory, the center, radius and center angle of the circle need to be calculated. If the path is a straight line or a spline curve, skip this step;

2.3 Cartesian space parameters: According to the specific analysis of different paths, the position and attitude are expressed by expressions related to the path parameter s, the attitude is expressed by quaternion, the intermediate attitude is calculated by spherical linear interpolation, the linear velocity and Euler angular velocity is the differential about the position and attitude, and the linear acceleration and the Euler angular acceleration are the second-order differential about the position and attitude;

2.4 Joint space parameters: The known position and attitude are used to obtain the joint angle through the inverse solution, and then use the relationship between the linear velocity ps and the joint velocity qs:

qs=J ^-1 ·ps

Find the joint velocity, where J represents the Jacobian matrix,

Then, derivation of both sides of the above formula is carried out to obtain the joint acceleration;

2.5 Calculate the coefficients of the dynamic equation: the derivative of the joint angle q to the time t of the path completion and the path parameter s have the following relationship:

It is known that the nonlinear dynamic model of the robot derived by the Lagrange equation method is:

表示科氏力与离心力矩阵，g(q)表示重力引起的力矩向量，将上面两个公式代入动力学模型中，整理后得：Among them, M(q) represents the inertia matrix,

Represents the Coriolis force and the centrifugal force matrix, and g(q) represents the moment vector caused by gravity. Substitute the above two formulas into the dynamic model, and get:

For the coefficients of the dynamic equation, the formula method is used to directly solve it;

Step 3. Calculate the velocity range: analyze each interpolation point, the constraints include Cartesian space constraints and joint space constraints, where Cartesian space constraints include linear velocity constraints, Euler angular velocity constraints, linear acceleration constraints and Euler angular acceleration constraints , the joint space constraints include joint torque constraints, joint velocity constraints and joint acceleration constraints, so that the corresponding parameter value is less than the constraint value. When considering the joint torque constraint, the joint acceleration constraint is not considered, and the constraint is converted into a parameter containing the square of the parameter speed and the parameter acceleration. The formula, comprehensively considering the Cartesian space constraints and the joint space constraints, calculates the maximum value of the square of the parameter speed of each interpolation point point by point, the minimum value is 0, and the enclosed area is the speed range that the robot can achieve;

Step 4. Calculate the optimal value of speed: each trajectory is divided into three parts: acceleration section, middle section and deceleration section. To obtain the optimal trajectory of time, the acceleration section needs to accelerate at the maximum acceleration, and the middle section needs to run close to the upper speed limit and decelerate. The segment needs to decelerate at the minimum acceleration, considering the dynamic and kinematic constraints comprehensively, given the limit acceleration that each interpolation point can reach, and use the kinematic formula to calculate the optimal solution;

Step 5. Smooth the trajectory: smooth the trajectory obtained in step 4;

Step 6. Output joint parameters: For the interpolation points in step 5, rearrange them according to the interpolation period to obtain new interpolation points, and then calculate the joint parameters related to time.

Compared with the prior art, the beneficial effects of the present invention are: the present invention not only considers the kinematic constraints, but also introduces the dynamic model to consider the dynamic constraints. It has obvious advantages, because at least one joint motor reaches the limit running state during operation, so that the joint motor driving performance of the robot can be fully utilized, and the joint parameters of the robot will not exceed the limit value, which improves work efficiency and is universal.

Description of drawings

Fig. 1 is the flow chart of the present invention;

Fig. 2 is the contrast schematic diagram of the traditional trajectory and the time optimal trajectory of the present invention;

3 is a schematic diagram of a time optimal trajectory of an embodiment;

4 is a schematic diagram of a joint angle ratio value of an embodiment;

Fig. 5 is the joint velocity ratio value schematic diagram of the embodiment;

FIG. 6 is a schematic diagram of joint moment ratio values of an embodiment.

Detailed ways

The technical solutions in the present invention will be clearly and completely described below with reference to the accompanying drawings in the embodiments of the present invention. Obviously, the described embodiments are only a part of the embodiments of the present invention, rather than all the embodiments, based on the present invention The embodiments in the present invention, all other embodiments obtained by those of ordinary skill in the art without creative work, fall within the protection scope of the present invention.

As shown in Figure 1, a dynamics-based method for planning the optimal trajectory of spraying robot time, including the following steps:

Step 1. Given a Cartesian space path: use the teaching point of the spraying robot to give a teaching point, and determine the path through the teaching point;

Step 2. Calculate the parameters related to the interpolation point: introduce the path parameter s, s represents the path length from the starting point to the interpolation point, set the path length to l, s∈[0,l], change s to change the speed and acceleration and then Complete the planning, and calculate the number of interpolation points, Cartesian space parameters, joint space parameters and dynamic equation coefficients. The Cartesian space parameters include: position, attitude, linear velocity, Euler angular velocity, linear acceleration and Euler angular acceleration , the joint space parameters include: joint angle, joint velocity and joint acceleration;

2.1 The number of interpolation points: proportional to the length of the path, the greater the length of the path, the more the number of interpolation points, the value formula is n=m×l, where m is the empirical coefficient;

2.2 If the path is an arc trajectory, the center, radius and center angle of the circle need to be calculated. If the path is a straight line or a spline curve, skip this step;

2.3 Cartesian space parameters: According to the specific analysis of different paths, the position and attitude are expressed by expressions related to the path parameter s, the attitude is expressed by quaternion, the intermediate attitude is calculated by spherical linear interpolation, the linear velocity and Euler angular velocity is the differential about the position and attitude, and the linear acceleration and the Euler angular acceleration are the second-order differential about the position and attitude;

2.4 Joint space parameters: The known position and attitude are used to obtain the joint angle through the inverse solution, and then use the relationship between the linear velocity ps and the joint velocity qs:

qs=J ^-1 ·ps

Find the joint velocity, where J represents the Jacobian matrix,

Then, derivation of both sides of the above formula is carried out to obtain the joint acceleration;

2.5 Calculate the coefficients of the dynamic equation: the derivative of the joint angle q to the time t of the path completion and the path parameter s have the following relationship:

It is known that the nonlinear dynamic model of the robot derived by the Lagrange equation method is:

表示科氏力与离心力矩阵，g(q)表示重力引起的力矩向量，将上面两个公式代入动力学模型中，整理后得：Among them, M(q) represents the inertia matrix,

Represents the Coriolis force and the centrifugal force matrix, and g(q) represents the moment vector caused by gravity. Substitute the above two formulas into the dynamic model, and get:

For the coefficients of the dynamic equation, the formula method is used to directly solve it;

Step 3. Calculate the speed range: Since the constraints corresponding to each interpolation point are different, each interpolation point needs to be analyzed. The constraints include Cartesian space constraints and joint space constraints, where Cartesian space constraints include linear velocity constraints, Euler constraints Angular velocity constraints, linear acceleration constraints and Euler angular acceleration constraints, joint space constraints include joint torque constraints, joint velocity constraints and joint acceleration constraints, so that the corresponding parameter value is less than the constraint value,

In addition, joint acceleration constraints are not considered when considering joint moment constraints,

和参数加速度

的公式，综合考虑笛卡尔空间约束和关节空间约束，逐点求出各插值点参数速度平方

的最大值，最小值为0，围成的区域即为机器人能达到的速度范围；Convert Constraint to Parametric Velocity Squared

and parameter acceleration

The formula of , comprehensively considering the Cartesian space constraints and joint space constraints, the parameter velocity square of each interpolation point is calculated point by point

The maximum value of , the minimum value is 0, and the enclosed area is the speed range that the robot can achieve;

Step 4. Calculate the optimal value of speed: each trajectory is divided into three parts: acceleration section, middle section and deceleration section. To obtain the optimal trajectory of time, the acceleration section needs to accelerate at the maximum acceleration, and the middle section needs to run close to the upper speed limit and decelerate. The segment needs to decelerate at the minimum acceleration, considering the dynamic and kinematic constraints comprehensively, given the limit acceleration that each interpolation point can reach, and use the kinematic formula to calculate the optimal solution;

Step 5. Smooth the trajectory: smooth the trajectory obtained in step 4;

Step 6. Output joint parameters: For the interpolation points in step 5, rearrange them according to the interpolation period to obtain new interpolation points, and then calculate the joint parameters related to time.

Example: a spraying robot completes a circular arc trajectory in Cartesian space

The specific time optimal trajectory planning method is as follows:

Step 1. Given a Cartesian space path: the teach pendant is given three teaching points, the positions and attitudes are P1(x ₁ , y ₁ , z ₁ , α ₁ , β ₁ , γ ₁ ), P2 (x ₂ ) , y ₂ , z ₂ , α ₂ , β ₂ , γ ₂ ), P3(x ₃ , y ₃ , z ₃ , α ₃ , β ₃ , γ ₃ ). Among them, the first three coordinates in the coordinates represent the spatial position, and the last three coordinates use Euler angles to represent the attitude;

Step 2. Calculate the parameters related to the interpolation point:

2.1 The number of interpolation points: take the value according to the length l of the arc path, the value formula is n=m×l, where m is the empirical coefficient;

2.2 Determine the center and radius, convert the space arc into a plane arc, and use the method of intersecting three planes to obtain the center of the circle to determine a circumscribed circle. The equation of the plane A1 where the circle is located is:

a ₁ x+b ₁ y+c ₁ z+d ₁ =0

The equation of the vertical bisector A2 of P1P2 is as follows:

a ₂ x+b ₂ y+c ₂ z+d ₂ =0

The equation of the vertical bisector A3 of P2P3 is as follows:

a ₃ x+b ₃ y+c ₃ z+d ₃ =0

Combining the above three equations, we can get:

The center P0 (x ₀ , y ₀ , z ₀ ) can be obtained through the above formula, and then the arc radius can be obtained according to the center of the circle:

After finding the circle center and radius, a new coordinate system is established based on the circle center. The X-axis of the new coordinate system is the vector direction of P0P1, and the cosine of the X-axis direction is as follows:

The Z axis of the new coordinate system is the normal phasor direction of the plane A1, and the cosine of the direction is as follows:

After determining the directions of the X-axis and Z-axis of the new coordinate system, the direction of the Y-axis of the new coordinate system can be obtained according to the right-hand rule, and then the homogeneous transformation matrix R can be determined. Transform the space arc of , to a plane arc in the new coordinate system. Finally, determine whether the arc is clockwise or counterclockwise, and find the central angle;

2.3 Calculate the Cartesian space parameters:

The positions of the interpolation points are:

where C represents the coordinates of the center of the circle, R represents the radius of the center of the circle,

The line speed is:

The linear acceleration is:

The expression of the robot attitude adopts the quaternion method. Given three points P1, P2, and P3, the attitude matrices R1, R2, and R3 can be determined, and then the quaternions q _a , q _b , and q _c can be obtained. For the multi-pose problem, the unit quaternion spherical linear interpolation is used, and then the two-level interpolation is used, and the expression is:

Then, the quaternion is converted into Euler angles α, β, γ, that is, the required attitude is obtained.

The shaft angle and shaft speed are obtained by quaternion, and the Euler angular velocity is obtained by using the relationship between the shaft speed and the Euler angular velocity:

Among them, the matrix represented by J is:

By processing the above formula, the Euler angular acceleration can be obtained:

Among them, the matrix represented by D is:

2.4 Calculate joint space parameters:

Using the position p obtained in 2.3, the joint angle q corresponding to each interpolation point is obtained through the inverse solution, and then, using the relationship between the linear velocity and the joint velocity, the joint velocity and joint acceleration are obtained as:

2.5 Calculate the coefficients of the kinetic equation:

The derivative of the joint angle q with respect to the path completion time t has the following relationship with the path parameter s:

It is known that the nonlinear dynamic model of the robot derived by the Lagrange equation method is:

表示科氏力与离心力矩阵，g(q)表示重力引起的力矩向量，将上面两个公式代入动力学模型中，整理后得：Among them, M(q) represents the inertia matrix,

Represents the Coriolis force and the centrifugal force matrix, and g(q) represents the moment vector caused by gravity. Substitute the above two formulas into the dynamic model, and get:

For the coefficients of the dynamic equation, the formula method is used to directly solve it;

Step 3. Calculate the speed range:

Analysis of the constraints, due to the limitation of the linear velocity v _max , can be obtained:

In the same way, due to the limitation of the angular velocity w _max , we can get:

Due to the limitation of linear acceleration a _max , we can get:

In the same way, due to the limitation of angular acceleration α _max , we can get:

Limited by the maximum torque τ _max of the motor, we can get:

Limited by the joint velocity qt _max , expressed as:

代表参数速度，

代表参数加速度。in,

represents the parameter velocity,

represents the parameter acceleration.

的最大值，最小值为0；Combining all the above formulas, the parameter velocity square of each interpolation point is calculated point by point

The maximum value of , and the minimum value is 0;

Step 4. Calculate the optimal speed value:

Considering the dynamic and kinematic constraints, the acceleration is:

代表后一个插值点对应的最大速度的平方。where u takes a smaller value,

Represents the square of the maximum velocity corresponding to the latter interpolation point.

From the kinematic formula of physics, the square of the velocity of the next interpolation point is obtained as:

In this way, the optimal speed value of each interpolation point is obtained in turn;

Step 5. Smooth the trajectory:

The conventional method can be adopted, or the following improved moving average filtering method can be used to divide each section of the path into two sections for processing. First find the deceleration point, and then use different methods to deal with the two sections before and after.

The front part of the path:

Back of the path:

Step 6. Output joint parameters:

The resulting velocity and acceleration in step 4 are path parameters, not time. Set the interpolation cycle, and adjust the obtained sequence value according to the interpolation cycle to obtain a new sequence value. The linear velocity of the robot end is calculated by the difference method, and the velocity-time image is drawn as shown in Figure 3. As can be seen from Figure 3, the image is smooth, the acceleration section accelerates at the maximum acceleration, the middle section runs close to the limit, and the end section is relatively gentle, so the overall running time has a greater advantage than the traditional trajectory planning algorithm. Finally, the joint angle is obtained through the inverse solution, then the joint velocity and joint acceleration are obtained through the difference method, and the shutdown torque is obtained by substituting into the dynamic equation, and all the obtained joint parameters are divided by the limit value of the corresponding parameter to obtain the joint parameter ratio image, as shown in the figure 4 to Figure 6. The ratio graph does not change the image trend, and the ratio remains between ±1, which represents safe operation, so Figures 4-6 are basically kept within the safe range, and the objects in Figures 3 and 4 are relatively flat, so they are ideal.

It will be apparent to those skilled in the art that the present invention is not limited to the details of the above-described exemplary embodiments, but that the present invention may be implemented in other packaging forms without departing from the spirit or essential characteristics of the present invention. Therefore, the embodiments are to be regarded in all respects as illustrative and not restrictive, and the scope of the invention is to be defined by the appended claims rather than the foregoing description, which are therefore intended to fall within the scope of the claims. All changes within the meaning and range of the equivalents of , are embraced within the invention. Any reference signs in the claims shall not be construed as limiting the involved claim.

In addition, it should be understood that although this specification is described in terms of embodiments, not each embodiment only includes an independent technical solution, and this description in the specification is only for the sake of clarity, and those skilled in the art should take the specification as a whole , the technical solutions in each embodiment can also be appropriately combined to form other implementations that can be understood by those skilled in the art.

Claims (4)

1. A spraying robot time optimal trajectory planning method based on dynamics is characterized in that: the method comprises the following steps:

step 1, giving a Cartesian space path: giving a teaching point by using a teaching device of the spraying robot, and determining a path through the teaching point;

step 2, calculating relevant parameters of the interpolation points: introducing a path parameter s, s representing the path length from a starting point to an interpolation point, setting the path length as l, s belonging to [0, l ], calculating the number of the interpolation points, a Cartesian space parameter, a joint space parameter and a kinetic equation coefficient, wherein the Cartesian space parameter comprises: position, gesture, linear velocity, euler angular velocity, linear acceleration and euler angular acceleration, joint space parameter includes: joint angle, joint velocity, and joint acceleration;

2.1 number of interpolation points: the value formula is n-mxl, wherein m is an empirical coefficient;

2.2 if the path is a circular arc track, the circle center, the radius and the circle center angle need to be calculated, and if the path is a straight line or a spline curve, the step is skipped;

2.3 Cartesian spatial parameters: specifically analyzing different paths, expressing positions and postures by adopting an expression related to a path parameter s, expressing the postures by adopting quaternion, calculating the middle posture by adopting spherical linear interpolation, wherein linear velocity and Euler angular velocity are differential values related to the positions and postures, and linear acceleration and Euler angular acceleration are second-order differential values related to the positions and postures;

2.4 joint space parameters: the known position and posture obtains a joint angle through an inverse solution, and then the relation between the linear velocity ps and the joint velocity qs is utilized:

qs＝J^-1·ps

determining the joint velocity, wherein J represents the Jacobian matrix,

then, performing two-side derivation on the formula to obtain the joint acceleration;

2.5 calculation of kinetic equation coefficients: the derivative of the joint angle q with respect to the time t of path completion has the following relationship with the path parameter s:

the nonlinear dynamical model of the robot derived by the lagrange equation method is known as follows:

wherein M (q) represents an inertia matrix,

representing a matrix of Coriolis force and centrifugal force, g (q) representing a moment vector caused by gravity, substituting the two formulas into a kinetic model, and finishing to obtain:

directly solving the coefficient of the dynamic equation by a formula method;

step 3, calculating a speed range: analyzing each interpolation point, wherein the constraints comprise Cartesian space constraints and joint space constraints, the Cartesian space constraints comprise linear velocity constraints, Euler angular velocity constraints, linear acceleration constraints and Euler angular acceleration constraints, the joint space constraints comprise joint torque constraints, joint speed constraints and joint acceleration constraints, corresponding parameter values are smaller than constraint values, the joint acceleration constraints are not considered when the joint torque constraints are considered, the constraints are converted into a formula containing parameter velocity squares and parameter accelerations, the Cartesian space constraints and the joint space constraints are comprehensively considered, the maximum value of the parameter velocity squares of each interpolation point is calculated point by point, the minimum value is 0, and the surrounded area is the velocity range which can be reached by the robot;

step 4, calculating an optimal speed value: comprehensively considering dynamics and kinematic constraints, giving the ultimate acceleration which can be reached by each interpolation point, and calculating the optimal solution by using a kinematic formula;

step 5, track smoothing: carrying out smoothing treatment on the track obtained in the step 4;

step 6, outputting joint parameters: and (5) rearranging the interpolation points in the step 5 according to the interpolation period to obtain new interpolation points, and then calculating joint parameters related to time.

2. The dynamics-based time-optimal trajectory planning method for a painting robot according to claim 1, characterized in that: and 4, when the optimal speed value is calculated, each section of track is regarded as an acceleration section, a middle section and a deceleration section, and in order to obtain the track with optimal time, the acceleration section needs to be accelerated at the maximum acceleration, the middle section needs to run close to the upper limit of the speed, and the deceleration section needs to be decelerated at the minimum acceleration.

3. The dynamics-based time-optimal trajectory planning method for a painting robot according to claim 1, characterized in that: and 5, when the track is smoothed, firstly finding a deceleration point for each section of the path and dividing the deceleration point into a front section and a rear section for processing.

4. The dynamics-based time-optimal trajectory planning method for a painting robot according to claim 1, characterized in that: and 6, setting an interpolation period, adjusting the obtained sequence value according to the interpolation period to obtain a new interpolation point, obtaining a joint angle through inverse solution, and obtaining a joint speed and a joint acceleration through a difference method.

Images