A Trajectory Planning Method For Six Degree-of-Freedom Robots Taking Into Account of End Effector Motion Error

Abstract

The invention discloses a trajectory planning method for six degree-of-freedom robots taking into account of end effector motion error. Specifically, the invention disclosed a method for precise planning of robot end effector continuous trajectory by combining the screw theory, the cubic spline interpolation algorithm, and particle swarm optimization algorithm. Firstly, the forward kinematics model and the inverse kinematics model of the robot is established based on the screw theory to simplify the calculation process; Cubic spline interpolation is used in joint space to ensure smooth motion; finally, the end effector tracking error is controlled within the required range with the number of key points as the variable, then take the time intervals as design variable, take the maximum angular velocity, angular acceleration and angular jerk of each joint as the constraint conditions, and use tracking error minimization as the optimization objective, to perform optimization of the trajectory, in order to obtain a planning trajectory with high planning efficiency, small tracking error and smooth motion.

Claims

1. A trajectory planning method for six degree-of-freedom robots taking into accounts of end effector motion error, wherein: key trajectory points are obtained at equal interval of an end effector continuous trajectory; interpolation programming is carried out for the angular positions of each joint obtained by inverse solution of the spinor kinematics model; at first, take the number of the key trajectory points as variable to control an end effector tracking error within a required range, then take the intervals as design variables, and take maximum angular velocity, angular acceleration and angular jerk of each joint as constraint conditions, take minimum tracking error as the optimization objective to optimize the trajectory so as to obtain the planned trajectory with high planning efficiency, small tracking error and smooth movement; comprising the steps of: step (1), a forward and an inverse kinematics model are established for the robot based on screw theory; step (2), on the end effector continuous trajectory, take N+1 key trajectory points at equal interval and get N track segments, with N representing the number of track segments; trajectory nodes of each joint are obtained by the inverse kinematics model, a cubic modified spline curve was used for interpolation programming to obtain time-related curves of angular displacement, angular velocity, angular acceleration and angular jerk; step (3), the tracking error model is established by taking points on the angular displacement curve every 20 milliseconds (ms) and calculating the end effector position through the forward kinematics model; the tracking errors of the end effector position points are calculated; a maximum tracking error max(E.sub.m) was extracted, E.sub.m is track tracking error, m is the number of joint node groups; step (4), T.sub.n represents a time interval of the track segment n, take T.sub.n=t, 1nN, plan the trajectory and calculate the maximum tracking error max(E.sub.m) following the above steps; if the accuracy requirement max(E.sub.m)<E.sub.max is not met, E.sub.max being the maximum tracking error defined according to the operation task, take N+1, and then calculate the error again, loop the calculation until the tracking error meets the condition; time interval t and motion accuracy limit E.sub.max are determined according to specific task requirements; and step (5), when N is determined, taking the time intervals as the design variable, the angular velocity, angular acceleration and angular jerk of each joints as constraints, and the minimum tracking error as the optimization objective to optimize the trajectory and obtain an optimized trajectory.

2. A trajectory planning method for six degree-of-freedom robots taking into accounts of end effector motion error according to claim 1, wherein, the forward and the inverse kinematics model in step (1) are established as follows: for the forward kinematics model: the position vector r.sub.i and rotation vector .sub.i of the No. i joint in the initial state of the robot are known as follows:
r.sub.1=[0 0 0] r.sub.2=[0 150 250] r.sub.3=[0 150 800] r.sub.4=r.sub.5=r.sub.6=[0 744 940](1)
.sub.1=[0 0 1] .sub.2=[1 0 0] .sub.3=[1 0 0] .sub.4=[0 1 0] .sub.5=[1 0 0] .sub.6=[0 0 1](2) based on screw theory, the transformation matrix between joints is expressed in exponential form: $\begin{array}{cc}\exp \left({\hat{}}_i.Math.{}_i\right)=\left[\begin{array}{cc}\exp \left({\hat{}}_i.Math.{}_i\right)& \left(I-\exp \left({\hat{}}_i.Math.{}_i\right)\right).Math.\left({}_i{v}_i\right)+{}_i.Math.{}_i^T.Math.v_i\\ 0& 1\end{array}\right]& \left(3\right)\end{array}$ wherein {circumflex over ()}.sub.i represents No. i joint spinor, .sub.i is the No. i angular displacement of joint; {circumflex over ()}.sub.i is defined as $\hat{}=\left[\begin{array}{ccc}0& -{}_3& {}_2\\ {}_3& 0& -{}_1\\ -{}_2& {}_1& 0\end{array}\right]$ by .sub.i=[.sub.1 .sub.2 .sub.3]; so that exp({circumflex over ()}.sub.i.sub.i)=I+{circumflex over ()}.sub.i sin .sub.i+{circumflex over ()}.sub.i.sup.2(1cos .sub.i); v.sub.i is the rotational linear velocity of No. i joint motion, v.sub.i=.sub.ir.sub.i; the forward kinematics model g.sub.st() of the robot is expressed as follows:
g.sub.st()=exp({circumflex over ()}.sub.1.sub.1) exp({circumflex over ()}.sub.2.sub.2) . . . exp({circumflex over ()}.sub.6.sub.6)g.sub.st(0) (4) for inverse kinematics model: the solution of joint angles is transformed into three Paden-Kahan subproblems; since the position of the robot end effector depends on joint 1, 2 and 3, and the robot's posture depends on joint 4, 5 and 6; first describe the inverse motion of the first three joints as: end position vector r.sub.e around the joint 1 to rotate .sub.1 to r.sub.e1, then around the joint 2 to rotate .sub.2 to r.sub.e2, then around the joint 3 to rotate .sub.3 to r.sub.5, so that .sub.1, .sub.2 and .sub.3 are carried out by the following three expressions, wherein, equation (5) belongs to subproblem 1, while equations (6) and (7) belong to subproblem 3
exp({circumflex over ()}.sub.1.sub.1)r.sub.e1=r.sub.e (5)
r.sub.e2exp({circumflex over ()}.sub.2.sub.2)r.sub.e1=.sub.2 (6)
r.sub.eexp({circumflex over ()}.sub.3.sub.3)r.sub.e2=.sub.3 (7) wherein r.sub.e1 is determined by end effector position vector r.sub.e=[x y z]; r.sub.e1=[0 {square root over (x.sup.2+y.sup.2)} z], .sub.2, .sub.3 is determined distance, .sub.2=r.sub.e1r.sub.2, .sub.3=r.sub.er.sub.3; next, .sub.4, .sub.5 and .sub.6 are carried out by the following three expressions, wherein, equation (8) belongs to subproblem 2 and equation (9) belongs to subproblem 1
exp({circumflex over ()}.sub.4.sub.4) exp({circumflex over ()}.sub.5.sub.5)r.sub.04=g.sub.1r.sub.04 (8)
exp({circumflex over ()}.sub.6.sub.6)r.sub.06=g.sub.2r.sub.06 (9) where r.sub.04 is on rotation axis 6, not on rotation axis 4 and 5, take r.sub.04=[0 744 0]; r.sub.06 is not on rotation axis 6, take r.sub.06=[0 150 860];
g.sub.1=[exp({circumflex over ()}.sub.1.sub.1) exp({circumflex over ()}.sub.2.sub.2) exp({circumflex over ()}.sub.3.sub.3)].sup.1g.sub.st()g.sub.st(0).sup.1;
g.sub.2=[exp({circumflex over ()}.sub.4.sub.4) exp({circumflex over ()}.sub.5.sub.5)].sup.1g.sub.1; wherein the step (2) of claim 1, obtaining and interpolating the trajectory nodes of each joint, is performed as follows: the continuous trajectory curve of the end effector is defined as follows, whose posture is maintained as =[0 0 1], and the total operation task time T is not greater than 1 minutes; $\begin{array}{cc}\{\begin{array}{c}x=-850+150.Math.\cos .Math..Math.\\ y=500+150.Math.\sin .Math..Math.\\ z=300\end{array}& \left(10\right)\end{array}$ where 0360, in order to take N+1 key trajectory points with equal interval, take =(360n/N), n=0,1, 2, . . . N; the obtained end posture is substituted into the inverse kinematics model to obtain N+1 joint trajectory nodes; a cubic modified spline curve is used to interpolate the joint trajectory nodes; for a joint, the joint trajectory is divided into N subsegments, angular displacement S.sub.n(t), angular velocity S.sub.n(t), angular acceleration S.sub.n(t) of the No. n subsegments (t [t.sub.n1, t.sub.n]) trajectory can be expressed as follows: $\begin{array}{cc}{S}_n\left(t\right)=\frac{{\left(t_n-t\right)}^3}{6.Math.T_n}.Math.{\stackrel{\mathrm{.Math.}}{}}_{n-1}+\frac{{\left(t-t_{n-1}\right)}^3}{6.Math.T_n}.Math.{\stackrel{\mathrm{.Math.}}{}}_n+\left({}_{n-1}-\frac{{\stackrel{\mathrm{.Math.}}{}}_{n-1}.Math.T_n^2}{6}\right).Math.\frac{t_n-t}{T_n}+\left({}_n-\frac{{\stackrel{\mathrm{.Math.}}{}}_n.Math.T_n^2}{6}\right).Math.\frac{t-t_{n-1}}{T_n}& \left(11\right)\\ S_n^{}\left(t\right)=\frac{1}{2.Math.T_n}\left[\left({\stackrel{\mathrm{.Math.}}{}}_n-{\stackrel{\mathrm{.Math.}}{}}_{n-1}\right).Math.t^2-2.Math.\left({\stackrel{\mathrm{.Math.}}{}}_n.Math.t_{n-1}-{\stackrel{\mathrm{.Math.}}{}}_{n-1}.Math.t_n\right).Math.t+{\stackrel{\mathrm{.Math.}}{}}_n.Math.t_{n-1}^2-{\stackrel{\mathrm{.Math.}}{}}_{n-1}.Math.t_n^2\right]+\frac{{}_n-{}_{n-1}}{T_n}-\frac{{\stackrel{\mathrm{.Math.}}{}}_n-{\stackrel{\mathrm{.Math.}}{}}_{n-1}}{6}.Math.T_n& \left(12\right)\\ .Math.S_n^{}\left(t\right)=\frac{t_n-t}{T_n}.Math.{\stackrel{\mathrm{.Math.}}{}}_{n-1}+\frac{t-t_{n-1}}{T_n}.Math.{\stackrel{\mathrm{.Math.}}{}}_n& \left(13\right)\end{array}$ wherein T.sub.n is the interval of the No. n subsegments; .sub.n, {dot over ()}.sub.n, {umlaut over ()}.sub.n is angular displacement, angular velocity and angular acceleration of the joint corresponding to time t.sub.n, {umlaut over ()}.sub.e is calculated by boundary conditions S.sub.n(t.sub.n1)=.sub.n1, S.sub.n(t.sub.n)=.sub.n and S.sub.n(t.sub.n)=S.sub.n+1(t.sub.n); wherein the step (3) of claim 1, establishing the end effector tracking error model, is performed as follows: the obtained angular displacement curves of each joint are evaluated every 20 ms to obtain M=T/0.02 sets of joint node groups, and calculating the corresponding terminal posture of each node group through the forward kinematics model as follows:
g.sub.m=exp({circumflex over ()}.sub.1.sub.1m) exp({circumflex over ()}.sub.2.sub.2m) . . . exp({circumflex over ()}.sub.6.sub.6m)g.sub.st(0) (14) wherein .sub.1m . . . .sub.6m is the No. m joint node group; the corresponding position vector P.sub.m in g.sub.m is extracted and the end effector error model E.sub.m was established as follows,
E.sub.m=P.sub.mOR (15) wherein O is the position vector of the center of the trajectory circle, R is the radius of curvature at each point on the continuous trajectory, R=150 mm; wherein the step (4) of claim 1, determining the number of end key trajectory points n, is performed as follows: take T.sub.n=2, trajectory planning for each joint is carried out according to the step 2 for the end effector, solving the end effector maximum error value max(E.sub.m); the task requires the end effector error to meet the following conditions: max(E.sub.m)E.sub.max=1 mm, if the calculation results do not meet the task requirements, increase the number of trajectory point N, continue to plan and calculate the end effector error, and continue the loop of calculation until the end effector tracking accuracy requirements are met, so as to determine N=24; wherein the step (5) of claim 1, optimizing the trajectory with minimizing end effector motion error as the objective function, is performed as follows: obtain the maximum values of the angular velocity, the angular acceleration and the angular jerk of each joint through the step (2) as follows:
S.sub.max=max(|{S.sub.n(t), 1n24}|) (19)
S.sub.max=max(|{S.sub.n(t), 1n24}|) (20)
S.sub.max=max(|{S.sub.n(t), 1n24}|) (21) so that the optimization constraint conditions is expressed as follows: $\begin{array}{cc}\{\begin{array}{c}1T_{n}3\\ S_{\max .Math..Math..Math.i}^{}{\stackrel{.}{}}_{\max .Math..Math.i}\\ S_{\max .Math..Math..Math.i}^{}{\stackrel{\mathrm{.Math.}}{}}_{\max .Math..Math.i}\\ S_{\max .Math..Math..Math.i}^{}{\stackrel{...}{}}_{\max .Math..Math.i}\end{array}& \left(22\right)\end{array}$ wherein the range of time interval T.sub.n is determined according to a specific task, {dot over ()}.sub.max i, {umlaut over ()}.sub.max i, and .sub.max i are the maximum angular velocity, angular acceleration and angular jerk allowed by No. i joint, and the objective optimization function is expressed as,
=min(E.sub.max) (23)

Description

BRIEF DESCRIPTION OF THE DRAWINGS

[0013] FIG. 1 is a diagram showing robots end effector trajectory planning flow chart.

[0014] FIG. 2 is a diagram showing parametric coordinates of a six DOF industrial robot.

[0015] FIGS. 3A, 3B, and 3C are diagrams showing the angular velocity, angular acceleration and angular jerk curves of each joints, respectively

[0016] FIG. 4 is a diagram showing the end effector tracking error curve.

DETAILED DESCRIPTION OF THE PREFERRED EMBODIMENTS

Embodiment I

[0017] Overall steps of the current disclosure are shown in FIG. 1.

[0018] Step (1), establishing a forward kinematics model and an inverse kinematics model for the robot based on screw theory.

[0019] The forward kinematics model:

[0020] As shown in FIG. 2, the position vector r.sub.i and rotation vector .sub.i of the No. i joint in the initial state of the robot are known as follows:

r.sub.1=[0 0 0] r.sub.2=[0 150 250] r.sub.3=[0 150 800] r.sub.4=r.sub.5=r.sub.6=[0 744 940](1)

.sub.1=[0 0 1] .sub.2=[1 0 0] .sub.3=[1 0 0] .sub.4=[0 1 0] .sub.5=[1 0 0] .sub.6=[0 0 1](2)

[0021] based on screw theory, the transformation matrix between joints can be expressed in exponential form:

[00001]$\begin{array}{cc}\exp \left({\hat{}}_i.Math.{}_i\right)=\left[\begin{array}{cc}\exp \left({\hat{}}_i.Math.{}_i\right)& \left(I-\exp \left({\hat{}}_i.Math.{}_i\right)\right).Math.\left({}_i{v}_i\right)+{}_i.Math.{}_i^T.Math.v_i\\ 0& 1\end{array}\right]& \left(3\right)\end{array}$

wherein {circumflex over ()}.sub.i represents No. i joint spinor, .sub.i is the No. i angular displacement of joint; [0022] {circumflex over ()}.sub.i is defined as

[00002]$\hat{}=\left[\begin{array}{ccc}0& -{}_3& {}_2\\ {}_3& 0& -{}_1\\ -{}_2& {}_1& 0\end{array}\right]$

by .sub.i=[.sub.1 .sub.2 .sub.3]; so that [0023] exp({circumflex over ()}.sub.i.sub.i)=I+{circumflex over ()}.sub.i sin .sub.i+{circumflex over ()}.sub.i.sup.2(1cos .sub.i); v.sub.i is the rotational linear velocity of No. i joint motion, v.sub.i=.sub.ir.sub.i;
the forward kinematics model g.sub.st() of the robot can be expressed as follows:

g.sub.st()=exp({circumflex over ()}.sub.1.sub.1)exp({circumflex over ()}.sub.2.sub.2) . . . exp({circumflex over ()}.sub.6.sub.6)g.sub.st(0) (4)

[0024] The inverse kinematics model:

[0025] the solution of joint angles is transformed into three Paden-Kahan subproblems; since the position of the robot end effector depends on joint 1, 2 and 3, and the robot's posture depends on joint 4, 5 and 6; first describe the inverse motion of the first three joints as: end position vector r.sub.e around the joint 1 to rotate .sub.1 to r.sub.e1, then around the joint 2 to rotate .sub.2 to r.sub.e2, then around the joint 3 to rotate .sub.3 to r.sub.5, so that .sub.1, .sub.2 and .sub.3 are carried out by the following three expressions, wherein, equation (5) belongs to subproblem 1, while equations (6) and (7) belong to subproblem 3

exp({circumflex over ()}.sub.1.sub.1)r.sub.e1=r.sub.e (5)

r.sub.e2exp({circumflex over ()}.sub.2.sub.2)r.sub.e1=.sub.2 (6)

r.sub.eexp({circumflex over ()}.sub.3.sub.3)r.sub.e2=.sub.3 (7)

wherein r.sub.e1 is determined by end effector position vector r.sub.e=[x y z]; r.sub.e1=[0 {square root over (x.sup.2+y.sup.2)} z], .sub.2, .sub.3 is determined distance, .sub.2=r.sub.e1r.sub.2, .sub.3=r.sub.er.sub.3; [0026] next, .sub.4, .sub.5 and .sub.6 are carried out by the following three expressions, wherein, equation (8) belongs to subproblem 2 and equation (9) belongs to subproblem 1

exp({circumflex over ()}.sub.4.sub.4) exp({circumflex over ()}.sub.5.sub.5)r.sub.04=g.sub.1r.sub.04 (8)

exp({circumflex over ()}.sub.6.sub.6)r.sub.06=g.sub.2r.sub.06 (9)

where r.sub.04 is on rotation axis 6, not on rotation axis 4 and 5, take r.sub.04=[0 744 0]; r.sub.06 is not on rotation axis 6, take r.sub.06=[0 150 860];

g.sub.1=[exp({circumflex over ()}.sub.1.sub.1) exp({circumflex over ()}.sub.2.sub.2) exp({circumflex over ()}.sub.3.sub.3)].sup.1g.sub.st()g.sub.st(0).sup.1;

g.sub.2=[exp({circumflex over ()}.sub.4.sub.4) exp({circumflex over ()}.sub.5.sub.5)].sup.1g.sub.1;

[0027] Step (2), obtaining and interpolating the trajectory nodes of each joint, is performed as follows:

[0028] The continuous trajectory curve of the end effector is defined as follows, whose posture is maintained as =[0 0 1], and the total operation task time T is no greater than 1 minute:

[00003]$\begin{array}{cc}\{\begin{array}{c}x=-850+150.Math.\cos .Math..Math.\\ y=500+150.Math.\sin .Math..Math.\\ z=300\end{array}& \left(10\right)\end{array}$

where 0360, in order to take N+1 key trajectory points with equal interval, take =(360n/N), n=0,1,2, . . . N;

[0029] The obtained end effector posture is substituted into the inverse kinematics model to obtain N+1 joint trajectory nodes; a cubic modified spline curve is used to interpolate the joint trajectory nodes; for a joint, the joint trajectory is divided into N subsegments, angular displacement S.sub.n(t), angular velocity S.sub.n(t), angular acceleration S.sub.n(t) of the No. n subsegments (t [t.sub.n1, t.sub.n]) trajectory can be expressed as follows:

[00004]$\begin{array}{cc}{S}_n\left(t\right)=\frac{{\left(t_n-t\right)}^3}{6.Math.T_n}.Math.{\stackrel{\mathrm{.Math.}}{}}_{n-1}+\frac{{\left(t-t_{n-1}\right)}^3}{6.Math.T_n}.Math.{\stackrel{\mathrm{.Math.}}{}}_n+\left({}_{n-1}-\frac{{\stackrel{\mathrm{.Math.}}{}}_{n-1}.Math.T_n^2}{6}\right).Math.\frac{t_n-t}{T_n}+\left({}_n-\frac{{\stackrel{\mathrm{.Math.}}{}}_n.Math.T_n^2}{6}\right).Math.\frac{t-t_{n-1}}{T_n}& \left(11\right)\\ S_n^{}\left(t\right)=\frac{1}{2.Math.T_n}\left[\left({\stackrel{\mathrm{.Math.}}{}}_n-{\stackrel{\mathrm{.Math.}}{}}_{n-1}\right).Math.t^2-2.Math.\left({\stackrel{\mathrm{.Math.}}{}}_n.Math.t_{n-1}-{\stackrel{\mathrm{.Math.}}{}}_{n-1}.Math.t_n\right).Math.t+{\stackrel{\mathrm{.Math.}}{}}_n.Math.t_{n-1}^2-{\stackrel{\mathrm{.Math.}}{}}_{n-1}.Math.t_n^2\right]+\frac{{}_n-{}_{n-1}}{T_n}-\frac{{\stackrel{\mathrm{.Math.}}{}}_n-{\stackrel{\mathrm{.Math.}}{}}_{n-1}}{6}.Math.T_n& \left(12\right)\\ .Math.S_n^{}\left(t\right)=\frac{t_n-t}{T_n}.Math.{\stackrel{\mathrm{.Math.}}{}}_{n-1}+\frac{t-t_{n-1}}{T_n}.Math.{\stackrel{\mathrm{.Math.}}{}}_n& \left(13\right)\end{array}$

wherein T.sub.n is the interval of the No. n subsegments; .sub.n, {dot over ()}.sub.n, {umlaut over ()}.sub.n is angular displacement, angular velocity and angular acceleration of the joint corresponding to time t.sub.n, {umlaut over ()}.sub.n is calculated by boundary conditions S.sub.n(t.sub.n1)=.sub.n1, S.sub.n(t.sub.n)=.sub.n and S.sub.n(t.sub.n)=S.sub.n+1(t.sub.n);

[0030] Step (3), establishing the end effector tracking error model, is performed as follows:

[0031] The obtained angular displacement curves of each joint are evaluated every 20 ms to obtain M=T/0.02 sets of joint node groups, and calculating the corresponding terminal posture of each node group through the forward kinematics model as follows:

g.sub.m=exp({circumflex over ()}.sub.1.sub.1m) exp({circumflex over ()}.sub.2.sub.2m) . . . exp({circumflex over ()}.sub.6.sub.6m)g.sub.st(0) (14)

wherein .sub.1m . . . .sub.6m is the No. m joint node group; the corresponding position vector P.sub.m in g.sub.m is extracted and the end effector error model E.sub.m was established as follows,

E.sub.m=P.sub.mOR (15)

wherein O is the position vector of the center of the trajectory circle, R is the radius of curvature at each point on the continuous trajectory, R=150 mm;

[0032] Step (4), determining the number of end key trajectory points n, is performed as follows:

[0033] take T.sub.n=2, trajectory planning for each joint is carried out according to the step 2 for the end effector, solving the end effector maximum error value max(E.sub.m); the task requires the end effector error to meet the following conditions: max(E.sub.m)E.sub.max=1 mm, if the calculation results do not meet the task requirements, increase the number of trajectory point N, continue to plan and calculate the end effector error, and continue the loop of calculation until the end effector tracking accuracy requirements are met, so as to determine N=24.

[0034] Step (5), optimizing the trajectory with minimizing end effector motion error as the objective function, is performed as follows:

[0035] obtain the maximum values of the angular velocity, the angular acceleration and the angular jerk of each joint through the step (2) as follows:

S.sub.max=max(|{S.sub.n(t), 1n24}|) (19)

S.sub.max=max(|{S.sub.n(t), 1n24}|) (20)

S.sub.max=max(|{S.sub.n(t), 1n24}|) (21)

so that the optimization constraint conditions is expressed as follows:

[00005]$\begin{array}{cc}\{\begin{array}{c}1T_{n}3\\ S_{\max .Math..Math..Math.i}^{}{\stackrel{.}{}}_{\max .Math..Math.i}\\ S_{\max .Math..Math..Math.i}^{}{\stackrel{\mathrm{.Math.}}{}}_{\max .Math..Math.i}\\ S_{\max .Math..Math..Math.i}^{}{\stackrel{...}{}}_{\max .Math..Math.i}\end{array}& \left(22\right)\end{array}$

wherein the range of time interval T.sub.n is determined according to a specific task, {dot over ()}.sub.max i, {umlaut over ()}.sub.max i, and .sub.max i are the maximum angular velocity, angular acceleration and angular jerk allowed by No. i joint, and the objective optimization function is expressed as,

=min(E.sub.max) (23)