An Error Modeling Method For End-Effector Space-Curve Trajectory Of Six Degree-of-Freedom Robots

Abstract

The invention disclosed an error modeling method for six degree-of-freedom robot end effector space-curve trajectory. More specifically, the invention is focused on end effector continuous space-curve trajectory tasks, and provides an error model taking into account of the influence of interpolation algorithm and joint linkage parameter error. This method selects key trajectory points on the ideal trajectory and by inverse solution converts them to the joint space, and performs interpolation; meanwhile the linkage parameter error taken into account to obtain the actual end effector position. The distance from the planned trajectory point to the ideal trajectory curve is used as the comprehensive error to reflect the deviation from the planned trajectory to the ideal trajectory. A simple and practical error model is obtained, which provides a theoretical basis for controlling the end effector-tracking accuracy.

Claims

1. An error modeling method for six degree-of-freedom robot end effector space-curve trajectory, comprising the following steps: (1) selecting N trajectory points on the space-curve, wherein N is determined by specific operational task, and obtaining displacement or angular displacement of each joint based on an inverse solution model; (2) selecting an interpolation algorithm and performing interpolation to obtain functional relationships between joint variables and time, wherein M joint variables are selected by taking a point every 20 milliseconds (ms), and wherein a total motion time obtained by the interpolation algorithm being T(s), M is T/0.02; (3) taking into account of the structural errors of each joint of the robot, obtaining positive solution to obtain M corresponding robot end effector trajectory points Q; (4) selecting point P on an ideal trajectory so that the points Q is on a normal line that passes the point P, therefore defining an trajectory error E as the distance between the point P and the point Q, so that the problem is transformed into solving the error E based on a known ideal space trajectory curve equation and coordinates of the point Q; wherein when E approaches infinitesimal, planned trajectory coincide with the ideal trajectory; and (5) based on the curve equation, obtaining a tangent equation that passes the point P, calculating the coordinate of the point P, with the condition of PQPP.sub.1 (P.sub.1 Is any point of the tangent line), so as to obtain the error E.

2. A method according to claim 1, wherein: the step (1) of obtaining displacement or angular displacement of each joint is performed by: setting robot end effector operational task space-curve equation as following: $\{\begin{array}{c}F\left(x,y,z\right)=0\\ G\left(x,y,z\right)=0\end{array}$ selecting the N trajectory points evenly on the curve, and obtaining the angular displacement of the each joint of the robot by inverse solution; the step (2) of performing interpolation for each joint variables is performed by: using an interpolation algorithm to interpolate the joint variables, and obtaining the functional relationship between the i joint variable and the motion time as follows:
.sub.i=f.sub.i(t) taking a function value every 20 ms on a function curve obtained according to the above formula, so as to obtain M displacement values .sub.i of each joint, and calculate M corresponding trajectory points Q through the forward kinematics model; the step (3) of calculating corresponding robot end effector trajectory points is performed as follows: the robot end effector position being related to the displacement .sub.i of each joint, also being related to robot D-H linkage parameters, e.g., linkage length a.sub.1, linkage twist angle .sub.i, joint distance d.sub.i and joint angle .sub.i, therefore, the forward kinematics model of the robot is expressed as follows:
Pos=g.sub.st(.sub.i,a.sub.i,.sub.i,d.sub.i,.sub.i) furthermore, there being the robot linkage parameter errors from the process of manufacturing and assembly, and such errors being able to affect robot end effector positioning accuracy, known linkage parameters thus can be expressed as a.sub.i+a.sub.i,.sub.i+.sub.i,d.sub.i+d.sub.i,.sub.i,+.sub.i, respectively; when the structural errors of each joint of the robot are taken into account, the position of the robot end effector is expressed as:
Pos(actual)=g.sub.st(.sub.i,a.sub.i+a.sub.i,.sub.i+.sub.i,d.sub.i+d.sub.i,.sub.i+.sub.i) wherein the joint angle .sub.i is obtained by interpolation, so that the actual position of the robot end effector is also affected by the interpolation algorithm; wherein by substituting the M joint angles .sub.i into the above equation, the M corresponding robot end effector trajectory points are obtained; the step (4) of calculating error E is performed as follows: setting the point P as a point on an ideal trajectory, and Q is on the normal line passing P, the point P.sub.1 is on tangent line passing the point P, thus PQPP.sub.1, setting the spatial coordinates of each point as P (x.sub.0, y.sub.0, z.sub.0) and P.sub.1 (x.sub.1, y.sub.1, z.sub.1), in order to faithfully reflect the deviation between the actual end effector trajectory and the ideal trajectory, the trajectory error E is defined as the distance between point P and point Q. As E approaches infinitesimal, the planned trajectory coincides with the ideal trajectory; wherein the equation of the tangent line passing the point P is obtained from a space-curve function as follows: $\{\begin{array}{c}\frac{F}{x}.Math.{}_{\left(x_0,y_0,z_0\right)}.Math.\left(x-x_0\right)+\frac{F}{y}.Math.{}_{\left(x_0,y_0,z_0\right)}.Math.\left(y-y_0\right)+\frac{F}{z}.Math.{}_{\left(x_0,y_0,z_0\right)}.Math.\left(z-z_0\right)=0\\ \frac{G}{x}.Math.{}_{\left(x_0,y_0,z_0\right)}.Math.\left(x-x_0\right)+\frac{G}{y}.Math.{}_{\left(x_0,y_0,z_0\right)}.Math.\left(y-y_0\right)+\frac{G}{z}.Math.{}_{\left(x_0,y_0,z_0\right)}.Math.\left(z-z_0\right)=0\end{array}$ take xx.sub.0=x, yy.sub.0 and zz.sub.0 is obtained from the above formula, satisfying the following conditions: $\{\begin{array}{c}F\left(x,y,z\right)=0.Math.\\ G\left(x,y,z\right)=0.Math.\\ \left(x-x_0\right).Math.\left(X-x_0\right)+\left(y-y_0\right).Math.\left(Y-y_0\right)+\left(z-z_0\right).Math.\left(Z-z_0\right)=0\end{array}$ solving the position of point P (x.sub.0,y.sub.0,z.sub.0) from the above equations, obtaining the error E as follows:
E=|{right arrow over (PQ)}|={square root over ((Xx.sub.0).sup.2+(Yy.sub.0).sup.2+(Zz.sub.0).sup.2)}.

Description

BRIEF DESCRIPTION OF THE DRAWINGS

[0013] FIG. 1 shows the error diagram of the planned space-curve trajectory.

DETAILED DESCRIPTION OF THE PREFERRED EMBODIMENTS

Embodiment I

[0014] The step (1) of obtaining displacement or angular displacement of each joint is performed by:

[0015] Setting robot end effector operational task space-curve equation as following:

[00001]$\{\begin{array}{c}F\left(x,y,z\right)=0\\ G\left(x,y,z\right)=0\end{array}$

[0016] Selecting the N trajectory points evenly on the curve, and obtaining the angular displacement of each joint of the robot by inverse solution;

[0017] The step (2) of performing interpolation for each joint variables is performed by: using an interpolation algorithm to interpolate the joint variables, and obtaining the functional relationship between the i joint variable and the motion time as follows:

.sub.i=f.sub.i(t)

[0018] Taking a function value every 20 ms on a function curve obtained according to the above formula, so as to obtain M displacement values .sub.i of each joint, and calculate M corresponding trajectory points Q through the forward kinematics model;

[0019] The step (3) of calculating corresponding robot end effector trajectory points is performed as follows:

[0020] The robot end effector position being related to the displacement .sub.i of each joint, also being related to robot D-H linkage parameters, e.g., linkage length a.sub.i, linkage twist angle .sub.i, joint distance d.sub.i and joint angle .sub.i, therefore, the forward kinematics model of the robot is expressed as follows:

Pos=g.sub.st(.sub.i,a.sub.i,.sub.i,d.sub.i,.sub.i)

[0021] Furthermore, there being the robot linkage parameter errors from the process of manufacturing and assembly, and such errors being able to affect robot end effector positioning accuracy, known linkage parameters thus can be expressed as:

a.sub.i+a.sub.i,.sub.i+.sub.i,d.sub.i+d.sub.i,.sub.i+.sub.i,

respectively; when the structural errors of each joint of the robot are taken into account, the position of the robot end effector is expressed as:

Pos(actual)=g.sub.st(.sub.i,a.sub.i+a.sub.i,.sub.i+.sub.i,d.sub.i+d.sub.i,.sub.i+.sub.i)

[0022] wherein the joint angle .sub.i is obtained by interpolation, so that the actual position of the robot end effector is also affected by the interpolation algorithm; wherein by substituting the M joint angles .sub.i into the above equation, the M corresponding robot end effector trajectory points are obtained;

[0023] The step (4) of calculating error E is performed as follows:

[0024] Setting the point P as a point on an ideal trajectory, and Q is on the normal line passing P, the point P.sub.1 is on tangent line passing the point P, thus PQPP.sub.1, setting the spatial coordinates of each point as P (x.sub.0, y.sub.0, z.sub.0) and P.sub.1 (x.sub.1, y.sub.1, z.sub.1), in order to faithfully reflect the deviation between the actual end effector trajectory and the ideal trajectory, the trajectory error E is defined as the distance between point P and point Q. As E approaches infinitesimal, the planned trajectory coincides with the ideal trajectory;

[0025] wherein the equation of the tangent line passing the point P is obtained from a space-curve function as follows:

[00002]$\{\begin{array}{c}\frac{F}{x}.Math.{}_{\left(x_0,y_0,z_0\right)}.Math.\left(x-x_0\right)+\frac{F}{y}.Math.{}_{\left(x_0,y_0,z_0\right)}.Math.\left(y-y_0\right)+\frac{F}{z}.Math.{}_{\left(x_0,y_0,z_0\right)}.Math.\left(z-z_0\right)=0\\ \frac{G}{x}.Math.{}_{\left(x_0,y_0,z_0\right)}.Math.\left(x-x_0\right)+\frac{G}{y}.Math.{}_{\left(x_0,y_0,z_0\right)}.Math.\left(y-y_0\right)+\frac{G}{z}.Math.{}_{\left(x_0,y_0,z_0\right)}.Math.\left(z-z_0\right)=0\end{array}$

[0026] Take xx.sub.0=x, yy.sub.0 and zz.sub.0 is obtained from the above formula, satisfying the following conditions:

[00003]$\{\begin{array}{c}F\left(x,y,z\right)=0.Math.\\ G\left(x,y,z\right)=0.Math.\\ \left(x-x_0\right).Math.\left(X-x_0\right)+\left(y-y_0\right).Math.\left(Y-y_0\right)+\left(z-z_0\right).Math.\left(Z-z_0\right)=0\end{array}$

[0027] Finally, solving the position of point P (x.sub.0,y.sub.0,z.sub.0) from the above equations, obtaining the error E as follows:

E=|{right arrow over (PQ)}|={square root over ((Xx.sub.0).sup.2+(Yy.sub.0).sup.2+(Zz.sub.0).sup.2)}.