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

Abstract

The invention discloses a track planning and control method of a free curve of an industrial robot based on double quaternions. Control point data of the Cartesian space are adopted for describing the contour of the space free curve, and meanwhile an Adams differential equation is adopted for NURBS interpolation compacting calculation, and interpolation speed is adjusted in a self-adaption manner with the maximum contour error and maximum acceleration as constraint conditions; and then the position and posture of the robot in the Cartesian space are converted to a four-dimensional space through a short linear segment obtained through interpolation by adopting the double quaternions, the motion trail of the robot is subjected to spherical linear interpolation through hyperspherical rotation, and finally the track of the NURBS free curve of the industrial robot is planned.

Description

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

Technical Field

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

Background

The requirement of modern manufacturing industry on the performance of the robot is higher and higher, and the trajectory planning algorithm of the robot in the task space plays an important role in a robot control system, and the motion performance and efficiency of the tail end of the robot are directly influenced. In addition, in the motion control of the robot, basic straight-line and circular-arc track curves cannot meet the application requirements of industrial processing, and the common B-spline curve, Bezier curve and Clothoid curve cannot describe a standard analytical curve and a free curve by using an accurate and uniform representation method.

Generally, a series of cartesian or joint space points are given in advance for a desired trajectory of a robot, a speed passing through the points or a time between the two points is given, in addition, a maximum speed allowed by the motion of the robot is limited, and then a position trajectory planning and a posture trajectory planning of a robot end effector are respectively realized, while an industrial robot posture trajectory planning usually adopts an euler angle method and an equivalent axis method to interpolate a posture, but the euler angle has the defect of universal deadlock, and the equivalent axis method has the problem that a rotating shaft cannot be determined when the rotating amount is 0. Although the quaternion method can solve the problem of robot attitude track interpolation, other interpolation algorithms are needed for track position interpolation, the computation amount is large, and the real-time requirement of a control system on track planning is influenced.

Disclosure of Invention

The invention aims to overcome the defects of the prior art, provides the NURBS curve path planning control method of the industrial robot based on the multiple quaternion, and can provide an efficient and high-precision control method for realizing the free curve path planning of the industrial robot.

In order to solve the technical problems, the technical scheme adopted by the invention is as follows:

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

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

2) giving a control point sequence D of a free curve described by the robot end effector in a task space NURBS and a corresponding posture R of the control point;

the control point sequence D is represented as: d ═ D₀，d₁，…，d_nN is the number of control points;

the corresponding postures of the control points adopt a posture rotation matrix R_3×3Represents;

3) solving the node vector U corresponding to the control point in the control point sequence in the step 2) according to a Hartley-Giardia method, wherein the concrete process is as follows:

for a given control point d_iI-0, 1, …, n, predefining a k-degree non-uniform rational B-spline curve and determining its node vector U-U₀,u₁,…,u_n+k+1]The specific node value in (1) is solved as follows:

taking the repetition degree of the nodes at two ends as k +1, and taking the definition domain of the curve as a standard parameter domain, namely u ∈ [ u_k,u_n+1]＝[0,1]Is then u₀＝u₁＝…＝u_k＝0，u_n+1＝u_n+2＝…＝u_n+k+1When the node value is 1, the other node values need to be calculated and solved as follows:

the calculation formula is as follows:

in the formula I_jTo control the length of each side of a polygon,/_j＝|d_j-d_j-1|，

From formula (4):

then all node values can be obtained;

wherein U is a densified node value between two adjacent nodes of the node vector U, and U is_iI is 0,1, … …, n + k +1, and represents a specific node value in the node vector U;

4) carrying out densification processing on the node vector U according to an Adams differential equation theoretical algorithm, wherein the specific process is as follows:

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

wherein T is an interpolation period,are each u_i-2、u_i-1、u_i、u_i+1The first derivative of (a);

will be provided withSubstituting the above formula, one can obtain:

ΔL_irepresents a control point d_iThe feed step size of (a);

the method of combining forward and backward difference instead of differentiation is adopted for simplification:

the difference in the backward direction is obtained by the difference,the difference is applied in the forward direction,

the difference is applied in the forward direction,forward differential

Substituting the above formula for formula (7) to obtain:

and further obtaining a simplified Adams differential equation interpolation algorithm iterative formula:

the post-densification node vector may then be obtained, where,represents u_i+1A predicted value of (2);

5) according to the mathematical model of the space pose of the robot end effector in the step 1), correcting the densified node vector by adopting a self-adaptive speed control algorithm to finally obtain an optimal densified node vector, wherein the correcting process comprises the following steps:

will be parameterSubstituting the predicted value as the parameter interpolation into the NURBS equation to obtain a corresponding predicted interpolation point:

representing an estimateToThe point of the interpolation is estimated and,

so as to obtain the corresponding estimated feeding step length as follows:

feed step length is estimatedAnd a feed step Δ L_iWith a deviation therebetween, by a relative error_iTo show that:

when relative error_iWhen within the allowable range, thenIs the desired p (u)_i+1) Otherwise, the correction is carried out according to the following formula until the result is reached_iWithin the allowable range:

finally, obtaining an optimal densification node vector;

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

7) carrying out multiple quaternion conversion according to the interpolation point position and the attitude data of the adjacent space curves, and specifically comprising the following steps:

7-1) homogeneous transformation matrix for terminal pose of each robot^BT_EAs follows:

firstly, rotating the attitude by a matrix R_3×3Obtaining a rotation quaternion Q corresponding to the attitude rotation matrix through the conversion relation between the rotation matrix and the quaternion, and simultaneously obtaining a translation vector P ═ P_x(u),p_y(u),p_z(u)]^T；

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

D_p＝cos(ψ/2)+sin(ψ/2)v(16)

in the formula, R_lIs the large spherical radius of the four-dimensional space, psi ═ P |/R_lV is a unit vector on the translation vector, v ═ P/| P |; when | P | ═ 0, v is a zero vector;

7-3) calculating to obtain a multiple quaternion space pose converted from the tail end pose of the robot to a four-dimensional space through the following formulaPart G and part H;

G＝D_pQ，

in the formula,is a quaternion D_pConjugation of (1);

7-4) discretizing the double-rotation track of the multiple quaternion to obtain a series of interpolation multiple quaternion points, wherein the interpolation multiple quaternion points need to be converted into rotation quaternion and translation vector, and the conversion algorithm is as follows:

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

in the formula,

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

In the aforementioned step 1) of the method,

the mathematical model of the robot end effector space pose based on the multiple quaternion is as follows:

wherein,representing the multiple quaternion spatial pose of the robot end effector, ξ and η satisfy ξ²＝ξ，η²η + η is 1, ξη is 0, and G and H are unit quaternions;

the mathematical model of the free curve described by the robot end effector in the task space NURBS is as follows: any k-th NURBS curve is expressed as a piecewise rational polynomial vector function:

where p (u) represents a position vector, ω, of a free-form curve described by the robot end-effector in task space NURBS_iReferred to as weight factors; d_iIs a free curve control point; n is the number of control points; n is a radical of_i,k(U) is formed by the node vector U ═ U₀,u₁,…,u_n+k+1]Decided B-splineBasis functions, expressed by the de boolean-cox recurrence defining formula:

in the formula, provision is made forU is the value of the node between two adjacent nodes of the node vector U, U_iAnd i is 0,1, … …, n + k +1, and indicates a specific node value in the node vector U.

The invention achieves the following beneficial effects:

the invention can realize the trajectory planning of the NURBS free curve of the industrial robot in the Cartesian space, and provides the control method which can effectively improve the working efficiency and the working quality of the industrial robot, reduce the speed fluctuation and improve the working environment of the robot.

Drawings

FIG. 1 is a schematic flow chart of a trajectory planning control method of an industrial robot free curve based on multiple quaternions;

FIG. 2 is a schematic diagram of a multiple quaternion representation space straight line segment position of the trajectory planning control method of the industrial robot free curve based on the multiple quaternion;

fig. 3 is a schematic diagram of expressing the attitude of a space straight line segment by a multiple quaternion in the trajectory planning control method of the free curve of the industrial robot based on the multiple quaternion, wherein the arrow direction in the diagram is a rotating vector axis of the attitude expressed by the quaternion.

Detailed Description

The invention is further described below with reference to the accompanying drawings. The following examples are only for illustrating the technical solutions of the present invention more clearly, and the protection scope of the present invention is not limited thereby.

The free curve concept is proposed to describe relatively complex geometries to improve the machining efficiency and accuracy of industrial robots. And a Non-uniform rational B-Spline (NURBS) curve can accurately show the distribution characteristics of control points of a curve model through perspective and can effectively solve the defect that model value points cannot be uniformly distributed.

The multiple quaternion is a new mathematical modeling tool based on Clifford algebra and is developed on the basis of quaternions. The positions and postures in the Cartesian space can be converted into the four-dimensional space by adopting the multiple quaternion, the positions and the postures in the space can be respectively represented, and the positions and the postures in the space can also be represented in a unified mode, so that the poses of the start point and the stop point in the four-dimensional space are subjected to spherical linear interpolation on the motion trail of the robot by using the hypersphere rotation.

As shown in fig. 1, the trajectory planning control method of the industrial robot free curve based on multiple quaternions of the invention comprises the following steps:

establishing a mathematical model of a robot end effector space pose based on a multiple quaternion and a free curve mathematical model described by the robot end effector in a task space NURBS;

the mathematical model of the robot end effector space pose based on the multiple quaternion is as follows:

in the formula,representing the multiple quaternion spatial pose of the robot end effector, ξ and η satisfy ξ²＝ξ，η²η + η is 1, ξη is 0, and G and H are unit quaternions.

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

where p (u) represents a position vector of a free curve described by the robot end effector in the task space NURBS, i.e., [ p (u) ═ p ]_x(u),p_y(u),p_z(u)]^T；d_iAs a free curve control point, i.e. d_i＝[x_i,y_i,z_i]Then the control point sequence D ═ D₀，d₁，…，d_n(i is 0,1, …, n), wherein n is the number of control points; omega_iCalled the weight factor, ω_iRespectively connected with the control point d_i(i-0, 1, …, n) when ω is_iWhen 1(i is 0,1, …, n), a k-th NURBS curve degenerates to a k-th B-spline curve;

N_i,k(U) is formed by the node vector U ═ U₀,u₁,…,u_n+k+1]The B-spline basis function of the decision is represented by the widely used de-boolean-Cox (debor-Cox) recursion definition formula:

in the formula, provision is made forU is the value of the node between two adjacent nodes of the node vector U, U_iAnd i is 0,1, … …, n + k +1, and indicates a specific node value in the node vector U.

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

giving a control point sequence D of a free curve described by the robot end effector in a task space NURBS and a 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 as follows: d ═ D₀，d₁，…，d_n(i is 0,1, …, n), wherein n is the number of control points;

adopting an attitude rotation matrix R corresponding to the attitude_3×3The expression of (1);

solving a node vector U corresponding to a free curve control point described by the task space NURBS according to a Hartley-Giardian method;

control point d of the free curve described in this step for a given NURBS_iI-0, 1, …, n, predefining a k-degree non-uniform rational B-spline (NURBS) curve, and determining its node vector U-U₀,u₁,…,u_n+k+1]The specific node value in (1).

In order to facilitate better control of the behavior of the curve at the curve end points, the repetition degree of the nodes at two ends of the invention is taken as k +1, and the definition domain of the curve is generally taken as a standard parameter domain, namely u ∈ [ u_k,u_n+1]＝[0,1]Is then u₀＝u₁＝…＝u_k＝0，u_n+1＝u_n+2＝…＝u_n+k+1And (3) calculating and solving the rest inner nodes as 1 by the following method:

the Hartley-Giardia method adopted in the step is independent of the parity of the curve times, a unified calculation formula is adopted, the calculation method is more reasonable, and the calculation formula is as follows:

in the formula I_jFor controlling the length of each side of the polygon, i.e. /)_j＝|d_j-d_j-1L. From formula (4):

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

Step four, carrying out densification processing on the node vector U according to an Adams differential equation theoretical algorithm;

the parameter densification refers to mapping from a three-dimensional track space to a one-dimensional parameter space, and under a parametric interpolation mode, data densification is represented as a parameter densification process, namely, a feeding step length delta L of the track space is mapped to the parameter space to obtain a parameter increment delta u and a next parameter coordinate: u. of_i+1＝u_i+Δu_i。

The Adams (Adams) differential equation adopted in the step is used for compacting the parameter, and the calculation formula is as follows:

the implicit format of the Altemus differential equation, which takes three and four orders, is expressed as:

in the formula, T is an interpolation period,are each u_i-2、u_i-1、u_i、u_i+1The first derivative of (a);

will be provided withSubstituting the above formula, one can obtain:

ΔL_irepresents a control point d_iThe feed step size of (2).

In order to ensure the requirement of direct interpolation of a high-speed NURBS curve and the calculation speed of real-time interpolation, the algorithm is simplified by adopting a method of combining forward and backward differences (as shown in the following formula) instead of differentiation:

(backward difference) of the signals,(forward differential),

(forward differential),(Forward difference)

Substituting the above formula for formula (7) to obtain:

and further obtaining a simplified Altemus differential equation interpolation algorithm iterative formula:

the post-densification node vector may then be obtained, where,represents u_i+1An estimate of (2).

Fifthly, according to a free curve mathematical model described by the robot end effector in the task space NURBS in the first step, correcting the encrypted node vector by adopting a self-adaptive speed control algorithm, and finally obtaining an optimal encrypted node vector;

will be parameterSubstituting the predicted value as the parameter interpolation into the NURBS equation to obtain a corresponding predicted interpolation point:

it is to estimate the interpolation point of the interpolation,

thereby obtaining the corresponding estimated feeding step lengthComprises the following steps:

estimated feeding step length obtained by using estimation methodAnd the feed step Δ L, can be assessed by the relative error:

_irepresents a control point d_iRelative error of (c).

When relative error_iWhen the temperature is within the allowable range, the temperature can be considered asIs the desired p (u)_i+1) Otherwise, the correction is carried out according to the following formula until the result is reached_iWithin the allowable range:

and finally obtaining the optimized densified node vector.

And step six, utilizing the optimized densification node vector and finally obtaining the interpolation point position on the curve according to the free curve mathematical model described by the robot end effector in the task space NURBS in the step one.

Seventhly, performing multiple quaternion conversion according to interpolation point positions and attitude data of adjacent space curves;

converting the pose homogeneous transformation matrix into multiple quaternions, and calculating the accuracy of the pose matrix in a given three-dimensional space_mAnd the maximum boundary L of the robot workspace, by the formula:

obtaining the radius R of a large sphere, namely an ultra-large sphere, in a four-dimensional space_l。

The pose homogeneous transformation matrix is converted into a multiple quaternion, and the conversion algorithm is as follows:

7-1) homogeneous transformation matrix for terminal pose of each robot^BT_EI.e. the centre coordinates of the flange at the end of the robotThe homogeneous transformation matrix of system E with respect to base coordinate system B is as follows:

firstly, rotating the attitude by a matrix R_3×3Obtaining a rotation quaternion Q corresponding to the attitude rotation matrix through the conversion relation between the rotation matrix and the quaternion, and simultaneously obtaining a translation vector P ═ P_x(u),p_y(u),p_z(u)]^T；

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

D_p＝cos(ψ/2)+sin(ψ/2)v(16)

wherein psi ═ P |/R_lV is a unit vector on the translation vector, v ═ P/| P |; when | P | ═ 0, v is a zero vector;

7-3) calculating to obtain a multiple quaternion space pose converted from the tail end pose of the robot to a four-dimensional space through the following formulaPart G and part H;

G＝D_pQ，

in the formula,is a quaternion D_pConjugation of (1);

7-4) discretizing the double-rotation track of the multiple quaternion to obtain a series of interpolation multiple quaternion points, wherein the interpolation multiple quaternion points need to be converted into rotation quaternion and translation vector, and the conversion algorithm is as follows:

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

in the formula,

and step eight, performing inverse kinematics processing on the pose of the robot end effector obtained by interpolation to obtain a joint angle and driving the joint to move.

The specific method comprises the following steps: taking the adjacent interpolation points in the robot task space as the starting and stopping points, the attitude rotation matrix R of the starting and stopping points can be used_sWith corresponding translation vector P_sAttitude rotation matrix R_eWith corresponding translation vector P_eData are respectively converted into multiple quaternion space poses:

and

wherein,rotating matrix R for attitude_sCorresponding multiple quaternion spatial pose, G_sAnd H_sIs composed ofThe corresponding unit quaternion is then calculated,rotating matrix R for attitude_eCorresponding multiple quaternion spatial pose, G_eAnd H_eIs composed ofThe corresponding unit quaternion.

Respectively carrying out spherical linear interpolation on the G part and the H part of the start-stop point multiple quaternion space pose to obtain:

in the formula, G (t) represents a spherical linear interpolation of the G part of the quaternion spatial pose multiplied by the start point, H (t) represents a spherical linear interpolation of the H part of the quaternion spatial pose multiplied by the start point, and α is arccos (G)_s·G_e)，β＝arccos(H_s·H_e)(G_s·G_e、H_s·H_eAre each G_sAnd G_e、H_sAnd H_eDot product of two quaternions), l (t) ∈ [0,1]May be obtained by normalizing the interpolation period.

From the above equation, the spherical linear interpolation of the multiple quaternion spatial pose can be expressed as:

the formula is a representation method of multiple quaternion spherical linear interpolation, spherical linear interpolation of G part and H part of multiple quaternion is included in the actual interpolation calculation process, the spherical linear interpolation of the multiple quaternion is completed through the spherical linear interpolation of unit quaternion of the G part and the H part respectively, the multiple quaternion of an interpolation intermediate point is obtained, the position and the posture of a linear line section of a multiple quaternion spherical linear interpolation space are respectively shown in figures 2 and 3, and then the multiple quaternion is converted into a pose homogeneous transformation matrix T of the interpolation intermediate point through the inverse process of step seven. And finally, obtaining real-time joint angles through inverse kinematics processing of the robot model, and driving each joint to move.

The above description is only a preferred embodiment of the present invention, and it should be noted that, for those skilled in the art, several modifications and variations can be made without departing from the technical principle of the present invention, and these modifications and variations 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 multiple quaternion is characterized by comprising the following steps of:

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

2) giving a control point sequence D of a free curve described by the robot end effector in a task space NURBS and a corresponding posture R of the control point;

the control point sequence D is represented as: d＝{d₀，d₁，···，d_nN is the number of control points;

the corresponding postures of the control points adopt a posture rotation matrix R_3×3Represents;

3) solving the node vector U corresponding to the control point in the control point sequence in the step 2) according to a Hartley-Giardia method, wherein the concrete process is as follows:

for a given control point d_iI-0, 1, …, n, predefining a k-degree non-uniform rational B-spline curve and determining its node vector U-U₀,u₁,…,u_n+k+1]The specific node value in (1) is solved as follows:

the repetition degree of the nodes at two ends is taken as k +1, and the definition domain of the curve is taken as a standard parameter domain, namely

u∈[u_k,u_n+1]＝[0,1]Is then u₀＝u₁＝…＝u_k＝0，u_n+1＝u_n+2＝…＝u_n+k+1When the node value is 1, the other node values need to be calculated and solved as follows:

the calculation formula is as follows:

$u_i-u_{i-1}=\frac{\underset{j=i-k}{\overset{i-1}{\Σ}}{l}_j}{\underset{s=k+1}{\overset{n+1}{\Σ}}\underset{j=s-k}{\overset{s-1}{\Σ}}{l}_j},i=k+1,...,n+1---\left(4\right)$

in the formula I_jTo control the length of each side of a polygon,/_j＝|d_j-d_j-1|，

From formula (4):

$u_i=\underset{s=k+1}{\overset{i}{\Σ}}(u_s-u_{s-1})=\frac{\underset{s=k+1}{\overset{i}{\Σ}}\underset{j=s-k}{\overset{s-1}{\Σ}}{l}_j}{\underset{s=k+1}{\overset{n+1}{\Σ}}\underset{j=s-k}{\overset{s-1}{\Σ}}{l}_j},i=k+1,\mathrm{...},n+1---\left(5\right)$

then all node values can be obtained;

wherein U is a densified node value between two adjacent nodes of the node vector U, and U is_iI is 0,1, … …, n + k +1, and represents a specific node value in the node vector U;

4) carrying out densification processing on the node vector U according to an Adams differential equation theoretical algorithm, wherein the specific process is as follows:

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

$u_{i+1}=u_i+\frac{T}{24}(9{\stackrel{\·}{u}}_{i+1}+19{\stackrel{\·}{u}}_i-5{\stackrel{\·}{u}}_{i-1}+{\stackrel{\·}{u}}_{i-2})---\left(6\right)$

wherein T is an interpolation period,are each u_i-2、u_i-1、u_i、u_i+1The first derivative of (a);

will be provided withSubstituting the above formula, one can obtain:

$u_{i+1}=u_i+\frac{1}{24}(\frac{9{\mathrm{\ΔL}}_{i+1}}{\sqrt{{\stackrel{\·}{x}}_{i+1}^2+{\stackrel{\·}{y}}_{i+1}^2+{\stackrel{\·}{z}}_{i+1}^2}}+\frac{19{\mathrm{\ΔL}}_i}{\sqrt{{\stackrel{\·}{x}}_i^2+{\stackrel{\·}{y}}_i^2+{\stackrel{\·}{z}}_i^2}}-\frac{5{\mathrm{\ΔL}}_{i-1}}{\sqrt{{\stackrel{\·}{x}}_{i-1}^2+{\stackrel{\·}{y}}_{i-1}^2+{\stackrel{\·}{z}}_{i-1}^2}}+\frac{{\mathrm{\ΔL}}_{i-2}}{\sqrt{{\stackrel{\·}{x}}_{i-2}^2+{\stackrel{\·}{y}}_{i-2}^2+{\stackrel{\·}{z}}_{i-2}^2}})---\left(7\right)$

ΔL_irepresents a control point d_iThe feed step size of (a);

the method of combining forward and backward difference instead of differentiation is adopted for simplification:

the difference in the backward direction is obtained by the difference,the difference is applied in the forward direction,

the difference is applied in the forward direction,forward differential

Substituting the above formula for formula (7) to obtain:

$u_{i+1}=u_i+\frac{T}{24}(9{\stackrel{\·}{u}}_{i+1}+19{\stackrel{\·}{u}}_i-5{\stackrel{\·}{u}}_{i-1}+{\stackrel{\·}{u}}_{i-2})---\left(8\right)$

and further obtaining a simplified Adams differential equation interpolation algorithm iterative formula:

${\tilde{u}}_{i+1}=\frac{1}{4}(9u_i-6u_{i-1}+u_{i-2})---\left(9\right)$

the post-densification node vector may then be obtained, where,represents u_i+1A predicted value of (2);

5) according to the mathematical model of the space pose of the robot end effector in the step 1), correcting the densified node vector by adopting a self-adaptive speed control algorithm to finally obtain an optimal densified node vector, wherein the correcting process comprises the following steps:

will be parameterSubstituting the predicted value as the parameter interpolation into the NURBS equation to obtain a corresponding predicted interpolation point:

$\tilde{p}\left(u_{i+1}\right)=p\left({\tilde{u}}_{i+1}\right)---\left(10\right)$

representing an estimateThe estimated interpolation point of the interpolation point is estimated,

so as to obtain the corresponding estimated feeding step length as follows:

$\mathrm{\Δ}{\tilde{L}}_i=|\tilde{p}\left(u_{i+1}\right)-p\left(u_i\right)|=\sqrt{{({\tilde{x}}_{i+1}-x_i)}^2+{({\tilde{y}}_{i+1}-x_i)}^2+{({\tilde{z}}_{i+1}-x_i)}^2}---\left(11\right)$

feed step length is estimatedAnd a feed step Δ L_iWith a deviation therebetween, by a relative error_iTo show that:

${\mathrm{\δ}}_i=\frac{|{\mathrm{\ΔL}}_i-\mathrm{\Δ}{\tilde{L}}_i|}{{\mathrm{\ΔL}}_i}\×100\%---\left(12\right)$

when relative error_iWhen within the allowable range, thenIs the desired p (u)_i+1) Otherwise, the correction is carried out according to the following formula until the result is reached_iWithin the allowable range:

$u_{i+1}=u_i+\frac{{\mathrm{\ΔL}}_i}{\mathrm{\Δ}{\tilde{L}}_i}({\tilde{u}}_{i+1}-u_i)---\left(13\right)$

finally, obtaining an optimal densification node vector;

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

7) carrying out multiple quaternion conversion according to the interpolation point position and the attitude data of the adjacent space curves, and specifically comprising the following steps:

7-1) homogeneous transformation matrix for terminal pose of each robot^BT_EAs follows:

${T_B}_E=\left[\begin{array}{cc}R& P\\ 0& 1\end{array}\right]---\left(15\right)$

firstly, rotating the attitude by a matrix R_3×3Obtaining a rotation quaternion Q corresponding to the attitude rotation matrix through the conversion relation between the rotation matrix and the quaternion, and simultaneously obtaining a translation vector P ═ P_x(u),p_y(u),p_z(u)]^T；

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

D_p＝cos(ψ/2)+sin(ψ/2)v(16)

in the formula, R_lIs the large spherical radius of the four-dimensional space, psi ═ P |/R_lV is a unit vector on the translation vector, v ═ P/| P |; when | P | ═ 0, v is a zero vector;

7-3) calculating to obtain a multiple quaternion space pose converted from the tail end pose of the robot to a four-dimensional space through the following formulaPart G and part H;

$G=D_{p}Q,H=D_p^{*}Q$

in the formula,is a quaternion D_pConjugation of (1);

7-4) discretizing the double-rotation track of the multiple quaternion to obtain a series of interpolation multiple quaternion points, wherein the interpolation multiple quaternion points need to be converted into rotation quaternion and translation vector, and the conversion algorithm is as follows:

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

$P=\left\{\begin{array}{cc}\frac{R_l\mathrm{\ψ}}{sin\mathrm{\ψ}}(G-H)Q^{*}& (\mathrm{\ψ}\≠0)\\ 0& (\mathrm{\ψ}=0)\end{array}\right.---\left(18\right)$

in the formula,

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

2. The multiple quaternion-based trajectory planning control method for an industrial robot freeform curve according to claim 1, characterized in that in the step 1),

the mathematical model of the robot end effector space pose based on the multiple quaternion is as follows:

$\tilde{G}=\mathrm{\ξ}G+\mathrm{\η}H---\left(1\right)$

wherein,representing the multiple quaternion spatial pose of the robot end effector, ξ and η satisfy ξ²＝ξ，η²η + η is 1, ξη is 0, and G and H are unit quaternions;

the mathematical model of the free curve described by the robot end effector in the task space NURBS is as follows: any k-th NURBS curve is expressed as a piecewise rational polynomial vector function:

$p\left(u\right)=\frac{\underset{i=0}{\overset{n}{\Σ}}{\mathrm{\ω}}_i{d}_i{N}_{i,k}\left(u\right)}{\underset{i=0}{\overset{n}{\Σ}}{\mathrm{\ω}}_i{N}_{i,k}\left(u\right)}---\left(2\right)$

where p (u) represents a position vector, ω, of a free-form curve described by the robot end-effector in task space NURBS_iReferred to as weight factors; d_iIs a free curve control point; n is the number of control points; n is a radical of_i,k(U) is formed by the node vector U ═ U₀,u₁,…,u_n+k+1]The determined B-spline basis function is represented by a DeBoolean-Corx recursion definition formula:

$\left\{\begin{array}{c}{N}_{i,0}=\left\{\begin{array}{cc}1& if(u_i\≤u\≤u_{i+1})\\ 0& otherwise\end{array}\right.\\ N_{i,k}=\frac{u-u_i}{u_{i+k}-u_i}{N}_{i,k-1}\left(u\right)+\frac{u_{i+k+1}-u}{u_{i+k+1}-u_{i+1}}{N}_{i,k-1}\left(u\right)\end{array}\right.---\left(3\right)$

in the formula, provision is made forU is the value of the node between two adjacent nodes of the node vector U, U_iAnd i is 0,1, … …, n + k +1, and indicates a specific node value in the node vector U.