CN105773620A - Track planning and control method of free curve of industrial robot based on double quaternions - Google Patents

Abstract

The invention discloses a trajectory planning control method for free curves of industrial robots based on double quaternions, which uses control point data in Cartesian space to describe the contours of free curves in space, and uses Adams differential equations to perform NURBS interpolation Densify the calculation, and adjust the interpolation speed adaptively with the maximum contour error and maximum acceleration as constraints, and then use the double quaternion to transform the position and attitude of the robot in Cartesian space to the four-dimensional space by using the short line segment obtained by interpolation. Spherical linear interpolation is performed on the trajectory of the robot with hyperspherical rotation, and finally the NURBS free curve trajectory planning of industrial robots is realized.

Description

Trajectory planning control method of free curve of industrial robot based on double quaternion

technical field

The invention relates to a trajectory planning control method of an industrial robot free curve based on a double quaternion, and belongs to the technical field of robot trajectory planning.

Background technique

Modern manufacturing has higher and higher requirements for robot performance, and the trajectory planning algorithm of the robot in the task space occupies an important position in the robot control system, which directly affects the performance and efficiency of the robot's end motion. Moreover, in robot motion control, the basic straight line and arc trajectory curves can no longer meet the application requirements of industrial processing, and the commonly used B-spline curves, Bézier curves, and Clothoid curves cannot be described with an accurate and unified representation method. curves and free curves.

Usually, the expected trajectory of the robot is given a series of Cartesian or joint space points in advance, and the speed passing through the point or the time between two points is given, and the maximum speed allowed by the robot movement is also limited, and then respectively Realize the position trajectory planning and attitude trajectory planning of the robot end effector, while the attitude trajectory planning of industrial robots usually uses the Euler angle method and the equivalent axis method to interpolate the attitude, but the Euler angle has the defect of universal deadlock, etc. The effective axis method has the problem that the rotation axis cannot be determined when the rotation amount is 0. Although using the quaternion method can solve the above-mentioned problems of robot attitude trajectory interpolation, other interpolation algorithms need to be used for trajectory position interpolation, which has a large amount of calculation and affects the real-time requirements of the control system for trajectory planning.

Contents of the invention

The technical problem to be solved by the present invention is to overcome the defects of the prior art, to provide a control method for NURBS curve trajectory planning of industrial robots based on double quaternion, which can provide an efficient and high-precision control method for industrial robots to realize free curve trajectory planning. Control Method.

In order to solve the problems of the technologies described above, the technical scheme adopted in the present invention is as follows:

The trajectory planning control method of the industrial robot free curve based on the double quaternion comprises the following steps:

1) Establish the mathematical model of the space pose of the robot end effector based on the double quaternion and the free curve mathematical model described by the robot end effector in the task space NURBS;

2) Given the control point sequence D of the free curve described by the end effector of the robot in the task space NURBS, and the corresponding attitude R of the control points;

The control point sequence D is expressed as: D={d ₀ , d ₁ ,...,d _n }, n is the number of control points;

The attitude corresponding to the control point is represented by an attitude rotation matrix R _3×3 ;

3) According to the Hartley-Judd method, the node vector U corresponding to the control point in the control point sequence of the step 2) is solved, and the specific process is as follows:

For a given control point d _i , i=0,1,…,n, predefine a non-uniform rational B-spline curve of degree k, and determine its node vector U=[u ₀ ,u ₁ ,…,u The specific node value in _n+k+1 ], the solution of the node value is as follows:

Take the repetition degree of nodes at both ends as k+1, and take the definition domain of the curve as the normative parameter domain, that is, u∈[u _k ,u _n+1 ]=[0,1], so u ₀ ＝u ₁ ＝ ...＝u _k ＝0, u _n+1 ＝u _n+2 ＝...＝u _n+k+1 ＝1, other node values need to be calculated and solved, as follows:

Calculated as follows:

In the formula, l _j is the length of each side of the control polygon, l _j = |d _j -d _j-1 |,

From formula (4) can get:

Then all node values are available;

Among them, u is the densified node value between two adjacent nodes of the node vector U, u _i , i=0,1,...,n+k+1, represents a specific node value in the node vector U;

4) Densify the node vector U according to the theoretical algorithm of Adams differential equation, the specific process is as follows:

The implicit form of the three-step fourth-order Adams differential equation is expressed as:

Among them, T is the interpolation period, are the first derivatives of u _i-2 , u _i-1 , u _i , u _i+1 respectively;

Will Substituting into the above formula, we can get:

ΔL _i represents the feed step size of the control point d _i ;

Simplify by combining forward and backward differences instead of differentiation:

backward differencing, forward difference,

forward difference, forward difference

Substituting the above formula into formula (7), we get:

Then the simplified iterative formula of Adams differential equation interpolation algorithm is obtained:

Then the densified node vector can be obtained, where, Indicates the estimated value of u _i+1 ;

5) According to the mathematical model of the space pose of the robot end effector in step 1), and using the adaptive speed control algorithm to correct the densified node vector, and finally obtain the optimal densified node vector, the correction process as follows:

will parameter As the estimated value of parameter interpolation, it is substituted into the NURBS equation to obtain the corresponding estimated interpolation point:

Indicates estimated value The estimated interpolation point of ,

Thus, the corresponding estimated feed step size is obtained as:

Estimated Feed Step The deviation between and the feed step ΔL _i is expressed by the relative error δ _i :

When the relative error δ _i is within the allowable range, then is the desired p(u _i+1 ), otherwise it is corrected according to the formula until it reaches the allowable range of δ _i :

Finally, the optimal densified node vector is obtained;

6) Utilize the optimal densified node vector in step 5), and according to the free curve mathematical model described by the robot end effector in the task space NURBS in step 1), finally obtain the position of the interpolation point on the curve;

7) According to the interpolation point position and attitude data of adjacent space curves, perform double quaternion conversion, the specific steps are as follows:

7-1) For each robot terminal pose homogeneous transformation matrix ^B T _E , as follows:

Firstly, the attitude rotation matrix R _3×3 is transformed through the transformation relationship between the rotation matrix and the quaternion to obtain the rotation quaternion Q corresponding to the attitude rotation matrix, and at the same time obtain the translation vector P=[p _x (u), p _y (u ), p _z (u)] ^T ;

7-2) Convert the translation vector P of the three-dimensional space into a quaternion of the four-dimensional space, and the conversion formula is as follows:

D _p =cos(ψ/2)+sin(ψ/2)v(16)

In the formula, R _l is the radius of the large sphere in the four-dimensional space, ψ=|P|/R _l , v is the unit vector on the translation vector, v=P/|P|; when |P|=0, v is zero vector;

7-3) Through the following formula, calculate the double quaternion space pose of the end pose of the robot converted to the four-dimensional space Parts G and H of the

G = D _p Q,

In the formula, is the conjugate of the quaternion D _p ;

7-4) Discretize the double rotation trajectory of the double quaternion to obtain a series of interpolation double quaternion points, which need to be converted into a rotation quaternion and a translation vector. The conversion algorithm is as follows:

Q=(G+H)/(2cosψ)(17)

In the formula,

8) Perform inverse kinematics processing on the pose of the robot end effector obtained by interpolation to obtain the joint angle and drive the joint motion.

In the aforementioned step 1),

The mathematical model of the space pose of the robot end effector based on the double quaternion is:

in, Indicates the multiplied quaternion space pose of the robot end effector, ξ and η satisfy ξ ² = ξ, η ² = η, ξ+η = 1, ξη = 0, G and H are unit quaternions;

The free curve mathematical model described by the end effector of the robot in the task space NURBS is: any NURBS curve of degree k can be expressed as a piecewise rational polynomial vector function:

Among them, p(u) represents the position vector of the free curve described by the end effector of the robot in the task space NURBS, ω _i is called the weight factor; d _i is the control point of the free curve; n is the number of control points; N _{i, k} ( u) is a B-spline basis function determined by the node vector U=[u ₀ ,u ₁ ,…,u _n+k+1 ], which is expressed by the Deboer-Cox recursive definition formula:

In the formula, stipulate u is the densified node value between two adjacent nodes of the node vector U, u _i , i=0,1,...,n+k+1, represents a specific node value in the node vector U.

The beneficial effect that the present invention reaches:

The invention can realize the trajectory planning of the NURBS free curve of the industrial robot in the Cartesian space, and provides a control method for effectively improving the working efficiency and the working quality of the industrial robot, reducing the speed fluctuation and improving the working environment of the robot.

Description of drawings

Fig. 1 is the schematic flow chart of the trajectory planning control method of the industrial robot free curve based on the double quaternion of the present invention;

Fig. 2 is the schematic diagram of the position of the straight line segment in the space expressed by the double quaternion of the trajectory planning control method of the industrial robot free curve based on the double quaternion of the present invention;

Fig. 3 is the schematic diagram of the trajectory planning control method of the industrial robot free curve based on the quaternion of the present invention. The quaternion expresses the schematic diagram of the attitude of the straight line segment in the space. The direction of the arrow in the figure is the rotation vector axis represented by the quaternion.

detailed description

The present invention will be further described below in conjunction with the accompanying drawings. The following examples are only used to illustrate the technical solution of the present invention more clearly, but not to limit the protection scope of the present invention.

The concept of free curve is proposed to describe more complex geometric shapes in order to improve the processing efficiency and precision of industrial robots. The Non-Uniform Rational B-Spline (NURBS, Non-Uniform Rational B-Spline) curve can accurately see the distribution characteristics of the control points of the curve shape, and can effectively solve the shortcoming that the value points cannot be evenly distributed.

Quaternion is a new mathematical modeling tool based on Clifford algebra, which is developed on the basis of quaternion. The position and attitude in Cartesian space can be converted to four-dimensional space by using double quaternion, which can not only express the position and attitude in space separately, but also express the position and attitude in space in a unified way , so that the hyperspherical rotation is used to perform spherical linear interpolation on the trajectory of the robot for the pose of the start and end points in the four-dimensional space.

As shown in Figure 1, the trajectory planning control method of the industrial robot free curve based on the double quaternion of the present invention comprises the following steps:

Step 1. Establish the mathematical model of the space pose of the robot end effector based on the double quaternion and the free curve mathematical model described by the robot end effector in the task space NURBS;

The mathematical model of the space pose of the robot end effector based on the double quaternion is:

In the formula, Indicates the multiplied quaternion space pose of the robot end effector, ξ and η satisfy ξ ² =ξ, η ² =η, ξ+η=1, ξη=0, G and H are unit quaternions.

The free curve mathematical model described by the end effector of the robot in the task space NURBS is: any NURBS curve of degree k can be expressed as a piecewise rational polynomial vector function:

In the formula, p(u) represents the position vector of the free curve described by the robot end effector in the task space NURBS, that is, p(u)=[p _x (u), p _y (u), p _z (u)] ^T ;d _i is the control point of the free curve, that is, d _i =[ _xi ,y _i , _zi ], then the control point sequence D={d ₀ ,d ₁ ,…,d _n }(i=0,1,… ,n), n is the number of control points; ω _i is called the weight factor, and ω _i is respectively associated with control points d _i (i=0,1,…,n), when ω _i =1 (i=0, 1,...,n), a NURBS curve of degree k degenerates into a B-spline curve of degree k;

N _i,k (u) is a B-spline basis function determined by the node vector U=[u ₀ ,u ₁ ,…,u _n+k+1 ], which is determined by the widely used DeBoer-Cox (DeBoor- Cox) recursive definition formula expresses:

In the formula, stipulate u is the densified node value between two adjacent nodes of the node vector U, u _i , i=0,1,...,n+k+1, represents a specific node value in the node vector U.

Then the three-dimensional coordinate form of the NURBS free curve is:

Step 2. Given the control point sequence D of the free curve described by the end effector of the robot in the task space NURBS, and the corresponding posture R of the control point in the free curve space;

The control point sequence D of the given robot task space free curve is: D={d ₀ , d ₁ ,...,d _n }(i=0,1,...,n), n is the number of control points;

The corresponding posture adopts the representation of the posture rotation matrix R _3×3 ;

Step 3. Solve the node vector U corresponding to the free curve control point described by the task space NURBS according to the Hartley-Judd method;

In this step, for the control point d _i of the free curve described by the given NURBS, i=0,1,...,n, a k-degree non-uniform rational B-spline (NURBS) curve is predefined, and its node vector must be determined at the same time A specific node value in U=[u ₀ ,u ₁ ,...,u _n+k+1 ].

In order to better control the behavior of the curve at the end points of the curve, the repetition degree of the nodes at both ends of the present invention is taken as k+1, and the definition domain of the curve is usually taken as the normative parameter domain, that is, u∈[u _k ,u _n+1 ]=[0,1], so u ₀ ＝u ₁ ＝…＝u _k ＝0, u _n+1 ＝u _n+2 ＝…＝u _n+k+1 ＝1, and the remaining internal nodes need Calculate and solve, the method is as follows:

The Hartley-Judd method used in this step has nothing to do with the parity of the order of the curve. A unified calculation formula is adopted, and the calculation method is more reasonable. The calculation formula is as follows:

In the formula, l _j is the length of each side of the control polygon, that is, l _j = |d _j -d _j-1 |. From formula (4) can get:

Then all node values in the node vector corresponding to the control point can be obtained.

Step 4. Densify the node vector U according to the Adams differential equation theory algorithm;

Parameter densification refers to the mapping from the three-dimensional trajectory space to the one-dimensional parameter space. In the parametric interpolation mode, the densification of the data is the densification process of the parameters, which is mapped by the feed step ΔL of the trajectory space. Go to the parameter space to obtain the parameter increment Δu and the next parameter coordinate: u _i+1 =u _i +Δu _i .

The Adams differential equation adopted in this step densifies the parameters, and the calculation formula is as follows:

The implicit form using the three-step fourth-order Adams differential equation is expressed as:

In the formula, T is the interpolation cycle, are the first derivatives of u _i-2 , u _i-1 , u _i , u _i+1 respectively;

Will Substituting into the above formula, we can get:

ΔL _i represents the feed step size of the control point d _i .

In order to meet the requirements of direct interpolation of high-speed NURBS curves and the calculation speed of real-time interpolation, the algorithm is simplified by combining forward and backward differential (as shown in the following formula) instead of differential:

(backward difference), (forward difference),

(forward difference), (forward difference)

Substituting the above formula into formula (7), we get:

Then the simplified Adams differential equation interpolation algorithm iterative formula can be obtained:

Then the densified node vector can be obtained, where, Indicates the estimated value of u _i+1 .

Step 5. According to the free curve mathematical model described by the robot end effector in the task space NURBS in step 1, the adaptive speed control algorithm is used to correct the densified node vector, and finally obtain the optimal densified node vector;

will parameter As the estimated value of parameter interpolation, it is substituted into the NURBS equation to obtain the corresponding estimated interpolation point:

is the estimated interpolation point,

So as to get the corresponding estimated feed step size for:

Estimated Feed Step Size Using Estimated Method The deviation between ΔL and the feed step size can be evaluated by relative error:

δ _i represents the relative error at the control point d _i .

When the relative error δ _i is within the allowable range, it can be considered is the desired p(u _i+1 ), otherwise it is corrected according to the formula until it reaches the allowable range of δ _i :

Finally, the optimized densified node vector is obtained.

Step 6. Using the optimized densified node vector and according to the free curve mathematical model described by the end effector of the robot in the task space NURBS in step 1, finally obtain the position of the interpolation point on the curve.

Step 7, according to the position of the interpolation point of the adjacent space curve and the attitude data, perform the double quaternion conversion;

To convert the pose homogeneous transformation matrix into a double quaternion, when the calculation accuracy δ _m of the pose matrix in the given three-dimensional space and the maximum boundary L of the robot workspace can be obtained by the formula:

Obtain the radius R _l of the big sphere in the four-dimensional space, that is, the super-large sphere.

To convert the pose homogeneous transformation matrix into a double quaternion, the conversion algorithm is as follows:

7-1) For each robot end pose homogeneous transformation matrix ^B T _E , that is, the homogeneous transformation matrix of the robot end flange center coordinate system E relative to the base coordinate system B, as follows:

Firstly, the attitude rotation matrix R _3×3 is transformed through the transformation relationship between the rotation matrix and the quaternion to obtain the rotation quaternion Q corresponding to the attitude rotation matrix, and at the same time obtain the translation vector P=[p _x (u), p _y (u ), p _z (u)] ^T ;

7-2) Approximately convert the translation vector P in the three-dimensional space into a quaternion in the four-dimensional space, and the conversion formula is as follows:

D _p =cos(ψ/2)+sin(ψ/2)v(16)

In the formula, ψ=|P|/R _l , v is the unit vector on the translation vector, v=P/|P|; when |P|=0, v is the zero vector;

7-3) Through the following formula, calculate the double quaternion space pose of the end pose of the robot converted to the four-dimensional space Parts G and H of the

G = D _p Q,

In the formula, is the conjugate of the quaternion D _p ;

7-4) Discretize the double rotation trajectory of the double quaternion to obtain a series of interpolation double quaternion points, which need to be converted into a rotation quaternion and a translation vector. The conversion algorithm is as follows:

Q=(G+H)/(2cosψ)(17)

In the formula,

Step 8: Perform inverse kinematics processing on the pose of the robot end effector obtained by interpolation to obtain joint angles and drive the joints to move.

The specific method is: consider the adjacent interpolation points in the robot task space as the start and end points, then the attitude rotation matrix R _s of the start and end points and the corresponding translation vector P _s , the attitude rotation matrix R _e and the corresponding translation vector P _e data , which are converted into multiple quaternion space poses respectively:

and

in, is the double quaternion space pose corresponding to the attitude rotation matrix R _s , G _s and H _s are The corresponding unit quaternion, is the double quaternion space pose corresponding to the attitude rotation matrix _{Re ,} and _Ge and He are The corresponding unit quaternion.

Spherical linear interpolation is performed on the G and H parts of the starting and ending points times the quaternion space pose respectively, and it can be obtained:

In the formula, G(t) represents the spherical linear interpolation of the G part of the starting and ending point times the quaternion space pose, H(t) represents the spherical linear interpolation of the H part of the starting and ending point times the quaternion space pose, α=arccos (G _s ·G _e ), β=arccos(H _s ·H _e )(G _s ·G _e , H _s ·He _e are the dot product of the two quaternions of G _s and Ge _, H _s and He _e respectively ), l(t)∈[0,1] can be obtained by normalizing the interpolation period.

According to the above formula, the spherical linear interpolation of the double quaternion space pose can be expressed as:

This formula is a representation method of double quaternion spherical linear interpolation. In the actual interpolation calculation process, the spherical linear interpolation of the two parts of the G part and the H part of the double quaternion is included, respectively through the G part and the H part. The spherical linear interpolation of the unit quaternion completes the spherical linear interpolation of the double quaternion, and obtains the double quaternion of the interpolation intermediate point, and the position and attitude of the double quaternion spherical linear interpolation space line segment are shown in Figures 2 and 3 respectively As shown, the double quaternion is transformed into a pose homogeneous transformation matrix T of the interpolated intermediate point through the inverse process of step 7. Finally, through the inverse kinematics processing of the robot model, the real-time joint angles are obtained and the joints are driven to move.

The above is only a preferred embodiment of the present invention, it should be pointed out that for those of ordinary skill in the art, without departing from the technical principle of the present invention, some improvements and modifications can also be made. It should also be regarded as the protection scope of the present invention.

Claims (2)

1. The trajectory planning control method of the industrial robot free curve based on the times quaternion, is characterized in that, comprises the following steps: 1) Establish the mathematical model of the space pose of the robot end effector based on the double quaternion and the free curve mathematical model described by the robot end effector in the task space NURBS; 2) Given the control point sequence D of the free curve described by the end effector of the robot in the task space NURBS, and the corresponding attitude R of the control points; The control point sequence D is expressed as: D={d ₀ , d ₁ ,...,d _n }, n is the number of control points; The attitude corresponding to the control point is represented by an attitude rotation matrix R _3×3 ; 3) According to the Hartley-Judd method, the node vector U corresponding to the control point in the control point sequence of the step 2) is solved, and the specific process is as follows: For a given control point d _i , i=0,1,…,n, predefine a non-uniform rational B-spline curve of degree k, and determine its node vector U=[u ₀ ,u ₁ ,…,u The specific node value in _n+k+1 ], the solution of the node value is as follows: The repetition degree of the nodes at both ends is taken as k+1, and the definition domain of the curve is taken as the normative parameter domain, namely u∈[u _k ,u _n+1 ]=[0,1], then u ₀ =u ₁ =...=u _k =0, u _n+1 =u _n+2 =...=u _n+k+1 =1, other node values need to be calculated and solved, as follows: Calculated as follows: ${\mathrm{<span\; class="notranslate">\; u</span>}}_{\mathrm{<span\; class="notranslate">\; i</span>}}<span\; class="notranslate">\; -</span>{\mathrm{<span\; class="notranslate">\; u</span>}}_{\mathrm{<span\; class="notranslate">\; i</span>}<span\; class="notranslate">\; -</span>\mathrm{<span\; class="notranslate">\; 1</span>}}<span\; class="notranslate">\; =</span>\frac{\underset{\mathrm{<span\; class="notranslate">\; j</span>}<span\; class="notranslate">\; =</span>\mathrm{<span\; class="notranslate">\; i</span>}<span\; class="notranslate">\; -</span>\mathrm{<span\; class="notranslate">\; k</span>}}{\overset{\mathrm{<span\; class="notranslate">\; i</span>}<span\; class="notranslate">\; -</span>\mathrm{<span\; class="notranslate">\; 1</span>}}{<span\; class="notranslate">\; \&Sigma;</span>}}{\mathrm{<span\; class="notranslate">\; l</span>}}_{\mathrm{<span\; class="notranslate">\; j</span>}}}{\underset{\mathrm{<span\; class="notranslate">\; the\; s</span>}<span\; class="notranslate">\; =</span>\mathrm{<span\; class="notranslate">\; k</span>}<span\; class="notranslate">\; +</span>\mathrm{<span\; class="notranslate">\; 1</span>}}{\overset{\mathrm{<span\; class="notranslate">\; no</span>}<span\; class="notranslate">\; +</span>\mathrm{<span\; class="notranslate">\; 1</span>}}{<span\; class="notranslate">\; \&Sigma;</span>}}\underset{\mathrm{<span\; class="notranslate">\; j</span>}<span\; class="notranslate">\; =</span>\mathrm{<span\; class="notranslate">\; the\; s</span>}<span\; class="notranslate">\; -</span>\mathrm{<span\; class="notranslate">\; k</span>}}{\overset{\mathrm{<span\; class="notranslate">\; the\; s</span>}<span\; class="notranslate">\; -</span>\mathrm{<span\; class="notranslate">\; 1</span>}}{<span\; class="notranslate">\; \&Sigma;</span>}}{\mathrm{<span\; class="notranslate">\; l</span>}}_{\mathrm{<span\; class="notranslate">\; j</span>}}}<span\; class="notranslate">\; ,</span>\mathrm{<span\; class="notranslate">\; i</span>}<span\; class="notranslate">\; =</span>\mathrm{<span\; class="notranslate">\; k</span>}<span\; class="notranslate">\; +</span>\mathrm{<span\; class="notranslate">\; 1</span>}<span\; class="notranslate">\; ,</span><span\; class="notranslate">\; ...</span><span\; class="notranslate">\; ,</span>\mathrm{<span\; class="notranslate">\; no</span>}<span\; class="notranslate">\; +</span>\mathrm{<span\; class="notranslate">\; 1</span>}<span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; 4</span>}<span\; class="notranslate">\; )</span>$ In the formula, l _j is the length of each side of the control polygon, l _j = |d _j -d _j-1 |, From formula (4) can get: ${\mathrm{<span\; class="notranslate">\; u</span>}}_{\mathrm{<span\; class="notranslate">\; i</span>}}<span\; class="notranslate">\; =</span>\underset{\mathrm{<span\; class="notranslate">\; the\; s</span>}<span\; class="notranslate">\; =</span>\mathrm{<span\; class="notranslate">\; k</span>}<span\; class="notranslate">\; +</span>\mathrm{<span\; class="notranslate">\; 1</span>}}{\overset{\mathrm{<span\; class="notranslate">\; i</span>}}{<span\; class="notranslate">\; \&Sigma;</span>}}<span\; class="notranslate">\; (</span>{\mathrm{<span\; class="notranslate">\; u</span>}}_{\mathrm{<span\; class="notranslate">\; the\; s</span>}}<span\; class="notranslate">\; -</span>{\mathrm{<span\; class="notranslate">\; u</span>}}_{\mathrm{<span\; class="notranslate">\; the\; s</span>}<span\; class="notranslate">\; -</span>\mathrm{<span\; class="notranslate">\; 1</span>}}<span\; class="notranslate">\; )</span><span\; class="notranslate">\; =</span>\frac{\underset{\mathrm{<span\; class="notranslate">\; the\; s</span>}<span\; class="notranslate">\; =</span>\mathrm{<span\; class="notranslate">\; k</span>}<span\; class="notranslate">\; +</span>\mathrm{<span\; class="notranslate">\; 1</span>}}{\overset{\mathrm{<span\; class="notranslate">\; i</span>}}{<span\; class="notranslate">\; \&Sigma;</span>}}\underset{\mathrm{<span\; class="notranslate">\; j</span>}<span\; class="notranslate">\; =</span>\mathrm{<span\; class="notranslate">\; the\; s</span>}<span\; class="notranslate">\; -</span>\mathrm{<span\; class="notranslate">\; k</span>}}{\overset{\mathrm{<span\; class="notranslate">\; the\; s</span>}<span\; class="notranslate">\; -</span>\mathrm{<span\; class="notranslate">\; 1</span>}}{<span\; class="notranslate">\; \&Sigma;</span>}}{\mathrm{<span\; class="notranslate">\; l</span>}}_{\mathrm{<span\; class="notranslate">\; j</span>}}}{\underset{\mathrm{<span\; class="notranslate">\; the\; s</span>}<span\; class="notranslate">\; =</span>\mathrm{<span\; class="notranslate">\; k</span>}<span\; class="notranslate">\; +</span>\mathrm{<span\; class="notranslate">\; 1</span>}}{\overset{\mathrm{<span\; class="notranslate">\; no</span>}<span\; class="notranslate">\; +</span>\mathrm{<span\; class="notranslate">\; 1</span>}}{<span\; class="notranslate">\; \&Sigma;</span>}}\underset{\mathrm{<span\; class="notranslate">\; j</span>}<span\; class="notranslate">\; =</span>\mathrm{<span\; class="notranslate">\; the\; s</span>}<span\; class="notranslate">\; -</span>\mathrm{<span\; class="notranslate">\; k</span>}}{\overset{\mathrm{<span\; class="notranslate">\; the\; s</span>}<span\; class="notranslate">\; -</span>\mathrm{<span\; class="notranslate">\; 1</span>}}{<span\; class="notranslate">\; \&Sigma;</span>}}{\mathrm{<span\; class="notranslate">\; l</span>}}_{\mathrm{<span\; class="notranslate">\; j</span>}}}<span\; class="notranslate">\; ,</span>\mathrm{<span\; class="notranslate">\; i</span>}<span\; class="notranslate">\; =</span>\mathrm{<span\; class="notranslate">\; k</span>}<span\; class="notranslate">\; +</span>\mathrm{<span\; class="notranslate">\; 1</span>}<span\; class="notranslate">\; ,</span>\mathrm{<span\; class="notranslate">\; ...</span>}<span\; class="notranslate">\; ,</span>\mathrm{<span\; class="notranslate">\; no</span>}<span\; class="notranslate">\; +</span>\mathrm{<span\; class="notranslate">\; 1</span>}<span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; 5</span>}<span\; class="notranslate">\; )</span>$ Then all node values are available; Among them, u is the densified node value between two adjacent nodes of the node vector U, u _i , i=0,1,...,n+k+1, represents a specific node value in the node vector U; 4) Densify the node vector U according to the theoretical algorithm of Adams differential equation, the specific process is as follows: The implicit form of the three-step fourth-order Adams differential equation is expressed as: ${\mathrm{<span\; class="notranslate">\; u</span>}}_{\mathrm{<span\; class="notranslate">\; i</span>}<span\; class="notranslate">\; +</span>\mathrm{<span\; class="notranslate">\; 1</span>}}<span\; class="notranslate">\; =</span>{\mathrm{<span\; class="notranslate">\; u</span>}}_{\mathrm{<span\; class="notranslate">\; i</span>}}<span\; class="notranslate">\; +</span>\frac{\mathrm{<span\; class="notranslate">\; T</span>}}{\mathrm{<span\; class="notranslate">\; twenty\; four</span>}}<span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; 9</span>}{\stackrel{<span\; class="notranslate">\; \&CenterDot;</span>}{\mathrm{<span\; class="notranslate">\; u</span>}}}_{\mathrm{<span\; class="notranslate">\; i</span>}<span\; class="notranslate">\; +</span>\mathrm{<span\; class="notranslate">\; 1</span>}}<span\; class="notranslate">\; +</span>\mathrm{<span\; class="notranslate">\; 19</span>}{\stackrel{<span\; class="notranslate">\; \&Center\; Dot;</span>}{\mathrm{<span\; class="notranslate">\; u</span>}}}_{\mathrm{<span\; class="notranslate">\; i</span>}}<span\; class="notranslate">\; -</span>\mathrm{<span\; class="notranslate">\; 5</span>}{\stackrel{<span\; class="notranslate">\; \&CenterDot;</span>}{\mathrm{<span\; class="notranslate">\; u</span>}}}_{\mathrm{<span\; class="notranslate">\; i</span>}<span\; class="notranslate">\; -</span>\mathrm{<span\; class="notranslate">\; 1</span>}}<span\; class="notranslate">\; +</span>{\stackrel{<span\; class="notranslate">\; \&Center\; Dot;</span>}{\mathrm{<span\; class="notranslate">\; u</span>}}}_{\mathrm{<span\; class="notranslate">\; i</span>}<span\; class="notranslate">\; -</span>\mathrm{<span\; class="notranslate">\; 2</span>}}<span\; class="notranslate">\; )</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; 6</span>}<span\; class="notranslate">\; )</span>$ Among them, T is the interpolation period, are the first derivatives of u _i-2 , u _i-1 , u _i , u _i+1 respectively; Will Substituting into the above formula, we can get: ${\mathrm{<span\; class="notranslate">\; u</span>}}_{\mathrm{<span\; class="notranslate">\; i</span>}<span\; class="notranslate">\; +</span>\mathrm{<span\; class="notranslate">\; 1</span>}}<span\; class="notranslate">\; =</span>{\mathrm{<span\; class="notranslate">\; u</span>}}_{\mathrm{<span\; class="notranslate">\; i</span>}}<span\; class="notranslate">\; +</span>\frac{\mathrm{<span\; class="notranslate">\; 1</span>}}{\mathrm{<span\; class="notranslate">\; twenty\; four</span>}}<span\; class="notranslate">\; (</span>\frac{\mathrm{<span\; class="notranslate">\; 9</span>}{\mathrm{<span\; class="notranslate">\; \&Delta;\; L</span>}}_{\mathrm{<span\; class="notranslate">\; i</span>}<span\; class="notranslate">\; +</span>\mathrm{<span\; class="notranslate">\; 1</span>}}}{\sqrt{{\stackrel{<span\; class="notranslate">\; \&Center\; Dot;</span>}{\mathrm{<span\; class="notranslate">\; x</span>}}}_{\mathrm{<span\; class="notranslate">\; i</span>}<span\; class="notranslate">\; +</span>\mathrm{<span\; class="notranslate">\; 1</span>}}^{\mathrm{<span\; class="notranslate">\; 2</span>}}<span\; class="notranslate">\; +</span>{\stackrel{<span\; class="notranslate">\; \&Center\; Dot;</span>}{\mathrm{<span\; class="notranslate">\; the\; y</span>}}}_{\mathrm{<span\; class="notranslate">\; i</span>}<span\; class="notranslate">\; +</span>\mathrm{<span\; class="notranslate">\; 1</span>}}^{\mathrm{<span\; class="notranslate">\; 2</span>}}<span\; class="notranslate">\; +</span>{\stackrel{<span\; class="notranslate">\; \&Center\; Dot;</span>}{\mathrm{<span\; class="notranslate">\; z</span>}}}_{\mathrm{<span\; class="notranslate">\; i</span>}<span\; class="notranslate">\; +</span>\mathrm{<span\; class="notranslate">\; 1</span>}}^{\mathrm{<span\; class="notranslate">\; 2</span>}}}}<span\; class="notranslate">\; +</span>\frac{\mathrm{<span\; class="notranslate">\; 19</span>}{\mathrm{<span\; class="notranslate">\; \&Delta;L</span>}}_{\mathrm{<span\; class="notranslate">\; i</span>}}}{\sqrt{{\stackrel{<span\; class="notranslate">\; \&CenterDot;</span>}{\mathrm{<span\; class="notranslate">\; x</span>}}}_{\mathrm{<span\; class="notranslate">\; i</span>}}^{\mathrm{<span\; class="notranslate">\; 2</span>}}<span\; class="notranslate">\; +</span>{\stackrel{<span\; class="notranslate">\; \&Center\; Dot;</span>}{\mathrm{<span\; class="notranslate">\; the\; y</span>}}}_{\mathrm{<span\; class="notranslate">\; i</span>}}^{\mathrm{<span\; class="notranslate">\; 2</span>}}<span\; class="notranslate">\; +</span>{\stackrel{<span\; class="notranslate">\; \&Center\; Dot;</span>}{\mathrm{<span\; class="notranslate">\; z</span>}}}_{\mathrm{<span\; class="notranslate">\; i</span>}}^{\mathrm{<span\; class="notranslate">\; 2</span>}}}}<span\; class="notranslate">\; -</span>\frac{\mathrm{<span\; class="notranslate">\; 5</span>}{\mathrm{<span\; class="notranslate">\; \&Delta;L</span>}}_{\mathrm{<span\; class="notranslate">\; i</span>}<span\; class="notranslate">\; -</span>\mathrm{<span\; class="notranslate">\; 1</span>}}}{\sqrt{{\stackrel{<span\; class="notranslate">\; \&CenterDot;</span>}{\mathrm{<span\; class="notranslate">\; x</span>}}}_{\mathrm{<span\; class="notranslate">\; i</span>}<span\; class="notranslate">\; -</span>\mathrm{<span\; class="notranslate">\; 1</span>}}^{\mathrm{<span\; class="notranslate">\; 2</span>}}<span\; class="notranslate">\; +</span>{\stackrel{<span\; class="notranslate">\; \&CenterDot;</span>}{\mathrm{<span\; class="notranslate">\; the\; y</span>}}}_{\mathrm{<span\; class="notranslate">\; i</span>}<span\; class="notranslate">\; -</span>\mathrm{<span\; class="notranslate">\; 1</span>}}^{\mathrm{<span\; class="notranslate">\; 2</span>}}<span\; class="notranslate">\; +</span>{\stackrel{<span\; class="notranslate">\; \&Center\; Dot;</span>}{\mathrm{<span\; class="notranslate">\; z</span>}}}_{\mathrm{<span\; class="notranslate">\; i</span>}<span\; class="notranslate">\; -</span>\mathrm{<span\; class="notranslate">\; 1</span>}}^{\mathrm{<span\; class="notranslate">\; 2</span>}}}}<span\; class="notranslate">\; +</span>\frac{{\mathrm{<span\; class="notranslate">\; \&Delta;L</span>}}_{\mathrm{<span\; class="notranslate">\; i</span>}<span\; class="notranslate">\; -</span>\mathrm{<span\; class="notranslate">\; 2</span>}}}{\sqrt{{\stackrel{<span\; class="notranslate">\; \&Center\; Dot;</span>}{\mathrm{<span\; class="notranslate">\; x</span>}}}_{\mathrm{<span\; class="notranslate">\; i</span>}<span\; class="notranslate">\; -</span>\mathrm{<span\; class="notranslate">\; 2</span>}}^{\mathrm{<span\; class="notranslate">\; 2</span>}}<span\; class="notranslate">\; +</span>{\stackrel{<span\; class="notranslate">\; \&Center\; Dot;</span>}{\mathrm{<span\; class="notranslate">\; the\; y</span>}}}_{\mathrm{<span\; class="notranslate">\; i</span>}<span\; class="notranslate">\; -</span>\mathrm{<span\; class="notranslate">\; 2</span>}}^{\mathrm{<span\; class="notranslate">\; 2</span>}}<span\; class="notranslate">\; +</span>{\stackrel{<span\; class="notranslate">\; \&Center\; Dot;</span>}{\mathrm{<span\; class="notranslate">\; z</span>}}}_{\mathrm{<span\; class="notranslate">\; i</span>}<span\; class="notranslate">\; -</span>\mathrm{<span\; class="notranslate">\; 2</span>}}^{\mathrm{<span\; class="notranslate">\; 2</span>}}}}<span\; class="notranslate">\; )</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; 7</span>}<span\; class="notranslate">\; )</span>$ ΔL _i represents the feed step size of the control point d _i ; Simplify by combining forward and backward differences instead of differentiation: backward differencing, forward difference, forward difference, forward difference Substituting the above formula into formula (7), we get: ${\mathrm{<span\; class="notranslate">\; u</span>}}_{\mathrm{<span\; class="notranslate">\; i</span>}<span\; class="notranslate">\; +</span>\mathrm{<span\; class="notranslate">\; 1</span>}}<span\; class="notranslate">\; =</span>{\mathrm{<span\; class="notranslate">\; u</span>}}_{\mathrm{<span\; class="notranslate">\; i</span>}}<span\; class="notranslate">\; +</span>\frac{\mathrm{<span\; class="notranslate">\; T</span>}}{\mathrm{<span\; class="notranslate">\; twenty\; four</span>}}<span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; 9</span>}{\stackrel{<span\; class="notranslate">\; \&Center\; Dot;</span>}{\mathrm{<span\; class="notranslate">\; u</span>}}}_{\mathrm{<span\; class="notranslate">\; i</span>}<span\; class="notranslate">\; +</span>\mathrm{<span\; class="notranslate">\; 1</span>}}<span\; class="notranslate">\; +</span>\mathrm{<span\; class="notranslate">\; 19</span>}{\stackrel{<span\; class="notranslate">\; \&Center\; Dot;</span>}{\mathrm{<span\; class="notranslate">\; u</span>}}}_{\mathrm{<span\; class="notranslate">\; i</span>}}<span\; class="notranslate">\; -</span>\mathrm{<span\; class="notranslate">\; 5</span>}{\stackrel{<span\; class="notranslate">\; \&Center\; Dot;</span>}{\mathrm{<span\; class="notranslate">\; u</span>}}}_{\mathrm{<span\; class="notranslate">\; i</span>}<span\; class="notranslate">\; -</span>\mathrm{<span\; class="notranslate">\; 1</span>}}<span\; class="notranslate">\; +</span>{\stackrel{<span\; class="notranslate">\; \&Center\; Dot;</span>}{\mathrm{<span\; class="notranslate">\; u</span>}}}_{\mathrm{<span\; class="notranslate">\; i</span>}<span\; class="notranslate">\; -</span>\mathrm{<span\; class="notranslate">\; 2</span>}}<span\; class="notranslate">\; )</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; 8</span>}<span\; class="notranslate">\; )</span>$ Then the simplified iterative formula of Adams differential equation interpolation algorithm is obtained: ${\stackrel{<span\; class="notranslate">\; ~</span>}{\mathrm{<span\; class="notranslate">\; u</span>}}}_{\mathrm{<span\; class="notranslate">\; i</span>}<span\; class="notranslate">\; +</span>\mathrm{<span\; class="notranslate">\; 1</span>}}<span\; class="notranslate">\; =</span>\frac{\mathrm{<span\; class="notranslate">\; 1</span>}}{\mathrm{<span\; class="notranslate">\; 4</span>}}<span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; 9</span>}{\mathrm{<span\; class="notranslate">\; u</span>}}_{\mathrm{<span\; class="notranslate">\; i</span>}}<span\; class="notranslate">\; -</span>\mathrm{<span\; class="notranslate">\; 6</span>}{\mathrm{<span\; class="notranslate">\; u</span>}}_{\mathrm{<span\; class="notranslate">\; i</span>}<span\; class="notranslate">\; -</span>\mathrm{<span\; class="notranslate">\; 1</span>}}<span\; class="notranslate">\; +</span>{\mathrm{<span\; class="notranslate">\; u</span>}}_{\mathrm{<span\; class="notranslate">\; i</span>}<span\; class="notranslate">\; -</span>\mathrm{<span\; class="notranslate">\; 2</span>}}<span\; class="notranslate">\; )</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; 9</span>}<span\; class="notranslate">\; )</span>$ Then the densified node vector can be obtained, where, Indicates the estimated value of u _i+1 ; 5) According to the mathematical model of the space pose of the robot end effector in step 1), and using the adaptive speed control algorithm to correct the densified node vector, and finally obtain the optimal densified node vector, the correction process as follows: will parameter As the estimated value of parameter interpolation, it is substituted into the NURBS equation to obtain the corresponding estimated interpolation point: $\stackrel{<span\; class="notranslate">\; ~</span>}{\mathrm{<span\; class="notranslate">\; p</span>}}<span\; class="notranslate">\; (</span>{\mathrm{<span\; class="notranslate">\; u</span>}}_{\mathrm{<span\; class="notranslate">\; i</span>}<span\; class="notranslate">\; +</span>\mathrm{<span\; class="notranslate">\; 1</span>}}<span\; class="notranslate">\; )</span><span\; class="notranslate">\; =</span>\mathrm{<span\; class="notranslate">\; p</span>}<span\; class="notranslate">\; (</span>{\stackrel{<span\; class="notranslate">\; ~</span>}{\mathrm{<span\; class="notranslate">\; u</span>}}}_{\mathrm{<span\; class="notranslate">\; i</span>}<span\; class="notranslate">\; +</span>\mathrm{<span\; class="notranslate">\; 1</span>}}<span\; class="notranslate">\; )</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; 10</span>}<span\; class="notranslate">\; )</span>$ Indicates estimated value The estimated interpolation point of , Thus, the corresponding estimated feed step size is obtained as: $\mathrm{<span\; class="notranslate">\; \&Delta;</span>}{\stackrel{<span\; class="notranslate">\; ~</span>}{\mathrm{<span\; class="notranslate">\; L</span>}}}_{\mathrm{<span\; class="notranslate">\; i</span>}}<span\; class="notranslate">\; =</span><span\; class="notranslate">\; |</span>\stackrel{<span\; class="notranslate">\; ~</span>}{\mathrm{<span\; class="notranslate">\; p</span>}}<span\; class="notranslate">\; (</span>{\mathrm{<span\; class="notranslate">\; u</span>}}_{\mathrm{<span\; class="notranslate">\; i</span>}<span\; class="notranslate">\; +</span>\mathrm{<span\; class="notranslate">\; 1</span>}}<span\; class="notranslate">\; )</span><span\; class="notranslate">\; -</span>\mathrm{<span\; class="notranslate">\; p</span>}<span\; class="notranslate">\; (</span>{\mathrm{<span\; class="notranslate">\; u</span>}}_{\mathrm{<span\; class="notranslate">\; i</span>}}<span\; class="notranslate">\; )</span><span\; class="notranslate">\; |</span><span\; class="notranslate">\; =</span>\sqrt{{<span\; class="notranslate">\; (</span>{\stackrel{<span\; class="notranslate">\; ~</span>}{\mathrm{<span\; class="notranslate">\; x</span>}}}_{\mathrm{<span\; class="notranslate">\; i</span>}<span\; class="notranslate">\; +</span>\mathrm{<span\; class="notranslate">\; 1</span>}}<span\; class="notranslate">\; -</span>{\mathrm{<span\; class="notranslate">\; x</span>}}_{\mathrm{<span\; class="notranslate">\; i</span>}}<span\; class="notranslate">\; )</span>}^{\mathrm{<span\; class="notranslate">\; 2</span>}}<span\; class="notranslate">\; +</span>{<span\; class="notranslate">\; (</span>{\stackrel{<span\; class="notranslate">\; ~</span>}{\mathrm{<span\; class="notranslate">\; the\; y</span>}}}_{\mathrm{<span\; class="notranslate">\; i</span>}<span\; class="notranslate">\; +</span>\mathrm{<span\; class="notranslate">\; 1</span>}}<span\; class="notranslate">\; -</span>{\mathrm{<span\; class="notranslate">\; x</span>}}_{\mathrm{<span\; class="notranslate">\; i</span>}}<span\; class="notranslate">\; )</span>}^{\mathrm{<span\; class="notranslate">\; 2</span>}}<span\; class="notranslate">\; +</span>{<span\; class="notranslate">\; (</span>{\stackrel{<span\; class="notranslate">\; ~</span>}{\mathrm{<span\; class="notranslate">\; z</span>}}}_{\mathrm{<span\; class="notranslate">\; i</span>}<span\; class="notranslate">\; +</span>\mathrm{<span\; class="notranslate">\; 1</span>}}<span\; class="notranslate">\; -</span>{\mathrm{<span\; class="notranslate">\; x</span>}}_{\mathrm{<span\; class="notranslate">\; i</span>}}<span\; class="notranslate">\; )</span>}^{\mathrm{<span\; class="notranslate">\; 2</span>}}}<span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; 11</span>}<span\; class="notranslate">\; )</span>$ Estimated Feed Step The deviation between and the feed step ΔL _i is expressed by the relative error δ _i : ${\mathrm{<span\; class="notranslate">\; \&delta;</span>}}_{\mathrm{<span\; class="notranslate">\; i</span>}}<span\; class="notranslate">\; =</span>\frac{<span\; class="notranslate">\; |</span>{\mathrm{<span\; class="notranslate">\; \&Delta;L</span>}}_{\mathrm{<span\; class="notranslate">\; i</span>}}<span\; class="notranslate">\; -</span>\mathrm{<span\; class="notranslate">\; \&Delta;</span>}{\stackrel{<span\; class="notranslate">\; ~</span>}{\mathrm{<span\; class="notranslate">\; L</span>}}}_{\mathrm{<span\; class="notranslate">\; i</span>}}<span\; class="notranslate">\; |</span>}{{\mathrm{<span\; class="notranslate">\; \&Delta;\; L</span>}}_{\mathrm{<span\; class="notranslate">\; i</span>}}}<span\; class="notranslate">\; \&times;</span>\mathrm{<span\; class="notranslate">\; 100</span>}\mathrm{<span\; class="notranslate">\; \%</span>}<span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; 12</span>}<span\; class="notranslate">\; )</span>$ When the relative error δ _i is within the allowable range, then is the desired p(u _i+1 ), otherwise it is corrected according to the formula until it reaches the allowable range of δ _i : ${\mathrm{<span\; class="notranslate">\; u</span>}}_{\mathrm{<span\; class="notranslate">\; i</span>}<span\; class="notranslate">\; +</span>\mathrm{<span\; class="notranslate">\; 1</span>}}<span\; class="notranslate">\; =</span>{\mathrm{<span\; class="notranslate">\; u</span>}}_{\mathrm{<span\; class="notranslate">\; i</span>}}<span\; class="notranslate">\; +</span>\frac{{\mathrm{<span\; class="notranslate">\; \&Delta;\; L</span>}}_{\mathrm{<span\; class="notranslate">\; i</span>}}}{\mathrm{<span\; class="notranslate">\; \&Delta;</span>}{\stackrel{<span\; class="notranslate">\; ~</span>}{\mathrm{<span\; class="notranslate">\; L</span>}}}_{\mathrm{<span\; class="notranslate">\; i</span>}}}<span\; class="notranslate">\; (</span>{\stackrel{<span\; class="notranslate">\; ~</span>}{\mathrm{<span\; class="notranslate">\; u</span>}}}_{\mathrm{<span\; class="notranslate">\; i</span>}<span\; class="notranslate">\; +</span>\mathrm{<span\; class="notranslate">\; 1</span>}}<span\; class="notranslate">\; -</span>{\mathrm{<span\; class="notranslate">\; u</span>}}_{\mathrm{<span\; class="notranslate">\; i</span>}}<span\; class="notranslate">\; )</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; 13</span>}<span\; class="notranslate">\; )</span>$ Finally, the optimal densified node vector is obtained; 6) Utilize the optimal densified node vector in step 5), and according to the free curve mathematical model described by the end effector of the robot in the task space NURBS in step 1), finally obtain the position of the interpolation point on the curve; 7) According to the interpolation point position and attitude data of the adjacent space curve, the double quaternion conversion is performed, and the specific steps are as follows: 7-1) For each robot terminal pose homogeneous transformation matrix ^B T _E , as follows: ${\mathrm{<span\; class="notranslate">\; T</span>}_{\mathrm{<span\; class="notranslate">\; B</span>}}}_{\mathrm{<span\; class="notranslate">\; E.</span>}}<span\; class="notranslate">\; =</span>\left[\begin{array}{cc}\mathrm{<span\; class="notranslate">\; R</span>}& \mathrm{<span\; class="notranslate">\; P</span>}\\ \mathrm{<span\; class="notranslate">\; 0</span>}& \mathrm{<span\; class="notranslate">\; 1</span>}\end{array}\right]<span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; 15</span>}<span\; class="notranslate">\; )</span>$ Firstly, the attitude rotation matrix R _3×3 is transformed through the transformation relationship between the rotation matrix and the quaternion to obtain the rotation quaternion Q corresponding to the attitude rotation matrix, and at the same time obtain the translation vector P=[p _x (u), p _y (u ), p _z (u)] ^T ; 7-2) Convert the translation vector P of the three-dimensional space into a quaternion of the four-dimensional space, and the conversion formula is as follows: D _p =cos(ψ/2)+sin(ψ/2)v(16) In the formula, R _l is the radius of the large sphere in the four-dimensional space, ψ=|P|/R _l , v is the unit vector on the translation vector, v=P/|P|; when |P|=0, v is zero vector; 7-3) Through the following formula, calculate the double quaternion space pose of the end pose of the robot converted to the four-dimensional space Parts G and H of the $\mathrm{<span\; class="notranslate">\; G</span>}<span\; class="notranslate">\; =</span>{\mathrm{<span\; class="notranslate">\; D.</span>}}_{\mathrm{<span\; class="notranslate">\; p</span>}}\mathrm{<span\; class="notranslate">\; Q</span>}<span\; class="notranslate">\; ,</span>\mathrm{<span\; class="notranslate">\; h</span>}<span\; class="notranslate">\; =</span>{\mathrm{<span\; class="notranslate">\; D.</span>}}_{\mathrm{<span\; class="notranslate">\; p</span>}}^{<span\; class="notranslate">\; *</span>}\mathrm{<span\; class="notranslate">\; Q</span>}$ In the formula, is the conjugate of the quaternion D _p ; 7-4) Discretize the double rotation trajectory of the double quaternion to obtain a series of interpolation double quaternion points, which need to be converted into a rotation quaternion and a translation vector. The conversion algorithm is as follows: Q=(G+H)/(2cosψ)(17) $\mathrm{<span\; class="notranslate">\; P</span>}<span\; class="notranslate">\; =</span>\left\{\begin{array}{cc}\frac{{\mathrm{<span\; class="notranslate">\; R</span>}}_{\mathrm{<span\; class="notranslate">\; l</span>}}\mathrm{<span\; class="notranslate">\; \&psi;</span>}}{\mathrm{<span\; class="notranslate">\; the\; s</span>}\mathrm{<span\; class="notranslate">\; i</span>}\mathrm{<span\; class="notranslate">\; no</span>}\mathrm{<span\; class="notranslate">\; \&psi;</span>}}<span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; G</span>}<span\; class="notranslate">\; -</span>\mathrm{<span\; class="notranslate">\; h</span>}<span\; class="notranslate">\; )</span>{\mathrm{<span\; class="notranslate">\; Q</span>}}^{<span\; class="notranslate">\; *</span>}& <span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; \&psi;</span>}<span\; class="notranslate">\; \&NotEqual;</span>\mathrm{<span\; class="notranslate">\; 0</span>}<span\; class="notranslate">\; )</span>\\ \mathrm{<span\; class="notranslate">\; 0</span>}& <span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; \&psi;</span>}<span\; class="notranslate">\; =</span>\mathrm{<span\; class="notranslate">\; 0</span>}<span\; class="notranslate">\; )</span>\end{array}\right.<span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; 18</span>}<span\; class="notranslate">\; )</span>$ In the formula, 8) Inverse kinematics processing is performed on the pose of the robot end effector obtained by interpolation to obtain the joint angle and drive the joint motion. 2. the trajectory planning control method of the industrial robot free curve based on the multiple quaternion according to claim 1, is characterized in that, in described step 1), The mathematical model of the space pose of the robot end effector based on the double quaternion is: $\stackrel{<span\; class="notranslate">\; ~</span>}{\mathrm{<span\; class="notranslate">\; G</span>}}<span\; class="notranslate">\; =</span>\mathrm{<span\; class="notranslate">\; \&xi;</span>}\mathrm{<span\; class="notranslate">\; G</span>}<span\; class="notranslate">\; +</span>\mathrm{<span\; class="notranslate">\; \&eta;</span>}\mathrm{<span\; class="notranslate">\; h</span>}<span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; 1</span>}<span\; class="notranslate">\; )</span>$ in, Indicates the position and orientation of the multiplied quaternion space of the robot end effector, ξ and η satisfy ξ ² = ξ, η ² = η, ξ+η = 1, ξη = 0, G and H are both unit quaternions; The free curve mathematical model described by the end effector of the robot in the task space NURBS is: any NURBS curve of degree k can be expressed as a piecewise rational polynomial vector function: $\mathrm{<span\; class="notranslate">\; p</span>}<span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; u</span>}<span\; class="notranslate">\; )</span><span\; class="notranslate">\; =</span>\frac{\underset{\mathrm{<span\; class="notranslate">\; i</span>}<span\; class="notranslate">\; =</span>\mathrm{<span\; class="notranslate">\; 0</span>}}{\overset{\mathrm{<span\; class="notranslate">\; no</span>}}{<span\; class="notranslate">\; \&Sigma;</span>}}{\mathrm{<span\; class="notranslate">\; \&omega;</span>}}_{\mathrm{<span\; class="notranslate">\; i</span>}}{\mathrm{<span\; class="notranslate">\; d</span>}}_{\mathrm{<span\; class="notranslate">\; i</span>}}{\mathrm{<span\; class="notranslate">\; N</span>}}_{\mathrm{<span\; class="notranslate">\; i</span>}<span\; class="notranslate">\; ,</span>\mathrm{<span\; class="notranslate">\; k</span>}}<span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; u</span>}<span\; class="notranslate">\; )</span>}{\underset{\mathrm{<span\; class="notranslate">\; i</span>}<span\; class="notranslate">\; =</span>\mathrm{<span\; class="notranslate">\; 0</span>}}{\overset{\mathrm{<span\; class="notranslate">\; no</span>}}{<span\; class="notranslate">\; \&Sigma;</span>}}{\mathrm{<span\; class="notranslate">\; \&omega;</span>}}_{\mathrm{<span\; class="notranslate">\; i</span>}}{\mathrm{<span\; class="notranslate">\; N</span>}}_{\mathrm{<span\; class="notranslate">\; i</span>}<span\; class="notranslate">\; ,</span>\mathrm{<span\; class="notranslate">\; k</span>}}<span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; u</span>}<span\; class="notranslate">\; )</span>}<span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; 2</span>}<span\; class="notranslate">\; )</span>$ Among them, p(u) represents the position vector of the free curve described by the end effector of the robot in the task space NURBS, ω _i is called the weight factor; d _i is the control point of the free curve; n is the number of control points; N _{i, k} ( u) is a B-spline basis function determined by the node vector U=[u ₀ ,u ₁ ,…,u _n+k+1 ], which is expressed by the Deboer-Cox recursive definition formula: $\left\{\begin{array}{c}{\mathrm{<span\; class="notranslate">\; N</span>}}_{\mathrm{<span\; class="notranslate">\; i</span>}<span\; class="notranslate">\; ,</span>\mathrm{<span\; class="notranslate">\; 0</span>}}<span\; class="notranslate">\; =</span>\left\{\begin{array}{cc}\mathrm{<span\; class="notranslate">\; 1</span>}& \mathrm{<span\; class="notranslate">\; i</span>}\mathrm{<span\; class="notranslate">\; f</span>}<span\; class="notranslate">\; (</span>{\mathrm{<span\; class="notranslate">\; u</span>}}_{\mathrm{<span\; class="notranslate">\; i</span>}}<span\; class="notranslate">\; \&le;</span>\mathrm{<span\; class="notranslate">\; u</span>}<span\; class="notranslate">\; \&le;</span>{\mathrm{<span\; class="notranslate">\; u</span>}}_{\mathrm{<span\; class="notranslate">\; i</span>}<span\; class="notranslate">\; +</span>\mathrm{<span\; class="notranslate">\; 1</span>}}<span\; class="notranslate">\; )</span>\\ \mathrm{<span\; class="notranslate">\; 0</span>}& \mathrm{<span\; class="notranslate">\; o</span>}\mathrm{<span\; class="notranslate">\; t</span>}\mathrm{<span\; class="notranslate">\; h</span>}\mathrm{<span\; class="notranslate">\; e</span>}\mathrm{<span\; class="notranslate">\; r</span>}\mathrm{<span\; class="notranslate">\; w</span>}\mathrm{<span\; class="notranslate">\; i</span>}\mathrm{<span\; class="notranslate">\; the\; s</span>}\mathrm{<span\; class="notranslate">\; e</span>}\end{array}\right.\\ {\mathrm{<span\; class="notranslate">\; N</span>}}_{\mathrm{<span\; class="notranslate">\; i</span>}<span\; class="notranslate">\; ,</span>\mathrm{<span\; class="notranslate">\; k</span>}}<span\; class="notranslate">\; =</span>\frac{\mathrm{<span\; class="notranslate">\; u</span>}<span\; class="notranslate">\; -</span>{\mathrm{<span\; class="notranslate">\; u</span>}}_{\mathrm{<span\; class="notranslate">\; i</span>}}}{{\mathrm{<span\; class="notranslate">\; u</span>}}_{\mathrm{<span\; class="notranslate">\; i</span>}<span\; class="notranslate">\; +</span>\mathrm{<span\; class="notranslate">\; k</span>}}<span\; class="notranslate">\; -</span>{\mathrm{<span\; class="notranslate">\; u</span>}}_{\mathrm{<span\; class="notranslate">\; i</span>}}}{\mathrm{<span\; class="notranslate">\; N</span>}}_{\mathrm{<span\; class="notranslate">\; i</span>}<span\; class="notranslate">\; ,</span>\mathrm{<span\; class="notranslate">\; k</span>}<span\; class="notranslate">\; -</span>\mathrm{<span\; class="notranslate">\; 1</span>}}<span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; u</span>}<span\; class="notranslate">\; )</span><span\; class="notranslate">\; +</span>\frac{{\mathrm{<span\; class="notranslate">\; u</span>}}_{\mathrm{<span\; class="notranslate">\; i</span>}<span\; class="notranslate">\; +</span>\mathrm{<span\; class="notranslate">\; k</span>}<span\; class="notranslate">\; +</span>\mathrm{<span\; class="notranslate">\; 1</span>}}<span\; class="notranslate">\; -</span>\mathrm{<span\; class="notranslate">\; u</span>}}{{\mathrm{<span\; class="notranslate">\; u</span>}}_{\mathrm{<span\; class="notranslate">\; i</span>}<span\; class="notranslate">\; +</span>\mathrm{<span\; class="notranslate">\; k</span>}<span\; class="notranslate">\; +</span>\mathrm{<span\; class="notranslate">\; 1</span>}}<span\; class="notranslate">\; -</span>{\mathrm{<span\; class="notranslate">\; u</span>}}_{\mathrm{<span\; class="notranslate">\; i</span>}<span\; class="notranslate">\; +</span>\mathrm{<span\; class="notranslate">\; 1</span>}}}{\mathrm{<span\; class="notranslate">\; N</span>}}_{\mathrm{<span\; class="notranslate">\; i</span>}<span\; class="notranslate">\; ,</span>\mathrm{<span\; class="notranslate">\; k</span>}<span\; class="notranslate">\; -</span>\mathrm{<span\; class="notranslate">\; 1</span>}}<span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; u</span>}<span\; class="notranslate">\; )</span>\end{array}\right.<span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; 3</span>}<span\; class="notranslate">\; )</span>$ In the formula, stipulate u is the densified node value between two adjacent nodes of the node vector U, u _i , i=0,1,...,n+k+1, represents a specific node value in the node vector U.