# Robot continuous path planning

2020-12-07 19:20:51

picture source ：here

# 1 summary

The continuous path planning of robot mainly involves the posture of the base 、 The planning of the end position or posture of a manipulator , In the process , Position can be uniquely represented by a three-dimensional vector , Therefore, the planning of the end position of the manipulator is mainly for the planning of the three-dimensional vector coordinates , And for posture planning , Because the method of gesture representation is not unique , Therefore, there will be a variety of attitude planning methods . But whether it's planning or interpolation for position and posture , The corresponding planning algorithm is universal .

Because the rotation of a rigid body can be expressed in many ways , Euler angle is generally used 、 Axis angle and quaternion, etc , Euler angles are due to the fact that the divisor is a sine and cosine , Therefore, in practical use, there will be strange posture , However, the quaternion based method can effectively avoid the problem of pose singularity .

Combining the advantages and disadvantages of various representations , In this paper, the axis angle is generally used in the attitude planning, that is, the axis of rotation and the corresponding rotation angle are calculated according to the expected attitude , When participating in the actual attitude calculation of the manipulator, the quaternion is selected as the attitude representation , And because Euler angle is relatively intuitive , The Euler angle is shown in terms of the actual posture .

The continuous path planning of robot can be divided into point-to-point continuous path and multi-point continuous path . For space robot Cartesian continuous path , We need to plan its position or posture trajectory reasonably , Cartesian trajectories are generally parameterized functions related to time . Generally, in Cartesian continuous path, such as polynomial 、 Trigonometric functions and exponential functions as basis functions , This shows that a segment of position is continuous , Track where the speed and acceleration meet the requirements . From a mathematical point of view , In time t_0-t_1 The trajectories of the inner dimension can be expressed as follows ：

In style q: Scalar value of position or attitude in a certain direction ;t_0,t_1 Start and end times ;

# 2 Point to point continuous path planning

If the trajectory is polynomial based , And the velocity and acceleration at the beginning and the end have initial values , The trajectory is described as follows

In style q_0,q_f The position or attitude scalar of a certain direction after planning ,v_0,v_f The velocity values at the beginning and the end of time ;a_0,a_f Acceleration values at the start and end of the time .

Under the above conditions , Definition T=t_f-t_0,h=q_f-q_0, Then the trajectory based on quintic polynomial can be arbitrarily expressed as follows

among

In style c_i Constants of polynomials ,T Total time ,h The difference between the end value and the initial value .

# 3 Continuous path planning between multiple points

The actual manipulator is in motion , It may pass through multiple intermediate nodes , Such a path can avoid obstacles , And can more smoothly complete the set tracking task . Theoretically , For the trajectory between multiple points, a polynomial can be used to connect the intermediate points , But there is a numerical stability problem in the process of solving , therefore , For multi-point trajectory planning , It is usually implemented by interpolation .

In this paper, spline curve is used to plan the trajectory between multiple points , To ensure the continuity of the trajectory , The initial and terminal velocities of the trajectory are generally specified . therefore , Given n+1 A little bit , Such as (t_k,q_k). The boundary velocity is scalar v_0 and v_n,. The desired trajectory is planned according to the spline curve described by the following equation

among ：

In style ：

q_k,\dot q_k,\ddot q_k, After the first k The scalar value of points 、 Speed value 、 Acceleration value ;

s(t) Spline function

c_{ki} Spline coefficients

v_k After the first k A point velocity value

t_k After the first k The time value of points

In the process , If the velocity in the middle is known , Then you can get

In style u_k The time interval between every two points u_k=t_{k+1}-t{k}

Besides , If the velocity of the intermediate point is unknown , Then the trajectory between the points can be calculated by the acceleration continuity of the interpolation points . The acceleration can be expressed as follows

For the above , Through substitution c_{k,2},c_{k,3},c_{k+1,2} The expression of , It can be deduced that

The above expression relationship can be written as a matrix expression as follows

v^{'}=[v_0,v_1,......v_n]^T ,d^{'}=[d_0,d_1,......d_{n-2}]^T ,d^{'} Is the matrix related to time interval and position .

Because in the above formula , The velocity equations of the initial and final moments of the trajectory are known , therefore , It is necessary to eliminate the terms related to the initial velocity and the terminal velocity , So that we can get

among