Research Article
An improved geodesic algorithm for trajectory planning of multi-joint robots
International Journal of Advanced Robotic Systems September-October 2016: 1–11 ª The Author(s) 2016 DOI: 10.1177/1729881416657742 arx.sagepub.com
Youdong Chen1, Ling Li1, and Wei Tang2
Abstract The present work proposes an improved geodesic algorithm for the trajectory planning of multi-joint robots. First, all of the joint variables are chosen to set up a generalized local coordinate system for the product of the positional space and the orientational one. Second, by defining a new Riemannian metric that contains both the positional and rational parameters, the traditional geodesic algorithm is improved so that it becomes capable of planning robot trajectories that include both the position and the orientation. To demonstrate the effectiveness of the improvement, trajectories are planned for two typical joint robots: one being a planar 3R and the other a spatial RRPP. It is verified that the improved algorithm can generate not only smooth motions for the joints but also smooth and accurate motions for the end-effector. The improved algorithm applies to multi-joint robots with no more than six degrees of freedom. Keywords trajectory planning, multi-joint robot, geodesic, Riemannian metric, smooth motion Date received: 18 May 2015; accepted: 21 May 2016 Topic: Robot Manipulation and Control Topic Editor: Andrey V Savkin Associate Editor: Gerasimos Rigatos; Oleg Yakimenko; Mahdi Tavakoli
Introduction Trajectory planning is a fundamental problem in the design of multi-joint robots. 1 The polynomial-function-based methods are widely used to plan robot trajectories. Although the linear and parabolic polynomials can meet the requirement of trajectory in some simple cases,2 they often lead to discontinuous interpolation, which sometimes gives rise to unsatisfactory planning results. To overcome this shortcoming, higher-order (such as cubic and quartic) polynomials are instead employed to conduct trajectory planning.1,3,4 In this respect, the most frequently used interpolation functions are splines which are piecewise polynomials, including the cubic splines5,6 and the B-splines.7,8 These polynomials-based methods are generally employed to plan the robot trajectories at the joint level, which inevitably leads to inaccurate movements of the end-effector in the Cartesian space. To enhance the motion precision of the endeffector, one method is to convert its Cartesian path into the joint trajectories at some intermediate points (called knots). The trajectory segments between any neighboring knots can
then be calculated by interpolation. Although this method guarantees the accuracy of the knot positions in the Cartesian space, the trajectory of the end-effector is unforeseeable, because it involves no forward kinematic calculation. Besides polynomials, geodesic functions can also be used to plan trajectories for joint robots.9–13 The fundamental idea of the geodesic-based trajectory planning method is to replace line or arc segments in a Euclidean space by geodesics in a Riemannian manifold. Because a geodesic curve generally represents (locally) the shortest path between two points in a Riemannian manifold,14 the geodesic-based method can always yield an optimal trajectory. 1
The school of Mechanical Engineering and Automation, Beihang University, Beijing, China 2 Hefei University of Technology, Hefei, Anhui, China Corresponding author: Youdong Chen, The school of Mechanical Engineering and Automation, Beihang University, Beijing, 100191, China. Email:
[email protected]
Creative Commons CC-BY: This article is distributed under the terms of the Creative Commons Attribution 3.0 License (http://www.creativecommons.org/licenses/by/3.0/) which permits any use, reproduction and distribution of the work without further permission provided the original work is attributed as specified on the SAGE and Open Access pages (https://us.sagepub.com/en-us/nam/ open-access-at-sage).
2
International Journal of Advanced Robotic Systems
Some literature addressed the geodesic trajectory planning in the joint space.15,16 In order to generate joint trajectory by the geodesic method, inverse kinematic analysis has to be performed.15 However, it is usually a difficult work to get a constant solution to the inverse kinematic equations. To avoid the troublesome inverse kinematic analysis, Selig and Ovseevitch employed the group theory to describe the geodesic paths in the joint space.16 In addition to the group theory, the theory of Riemannian geometry can also be directly used to make the geodesic trajectory planning easy to operate.10 This involves two main steps. First, the Riemannian metric needs to be constructed according to the planning task; second, the trajectories are planned by solving the geodesic equations. Either the distance or the kinetic energy can be chosen as the Riemannian metric to achieve the optimal trajectory with the minimum distance or minimum kinematic energy.17 Because only the positional parameters are considered, the distance-based Riemannian metric cannot be used to tackle the orientation problems that involve the rotational parameters. Due to this reason, when the distance metric is chosen as the Riemannian metric, the geodesic method is unsuitable to be applied in planning trajectories for both planar 3R robots and multijoint robots with more than three degrees of freedom (DOFs). In this case, it has been demonstrated that not all of the joint trajectories of the Kuka youBot arm that has five joints can be planned by the geodesic method.10 By constructing a Riemannian metric that contains both the positional and rotational parameters, the present paper proposes an improved geodesic algorithm for trajectory planning. Because the rotational parameters are included into the Riemannian metric, the improved algorithm can be used to tackle the orientational problems, and thus it can be applied in trajectory planning for both planar 3R robots and multi-joint robots with no more than six DOFs. It is demonstrated by two examples that the improved algorithm can yield not only smooth motions for the joints but also smooth and accurate trajectories for the end-effector.
The improved geodesic trajectory planning algorithm Geodesics are natural generalization of lines. A geodesic is (locally) the shortest path between points in a Riemannian manifold. Constructing different manifolds and Riemannian metrics, one can obtain different geodesics such as lines, arcs and paths of minimum distance or minimum kinetic energy. Assume that M is a Riemannian manifold and g is a Riemannian metric of M. In general, the Riemannian metric can be represented as g ¼ ð d1
d 2
dn Þ Gð d 1
d2
dn Þ T (1)
where G ¼ ðgij Þnn is the coefficient matrix of the Riemannian metric; i ði ¼ 1; . . . ; nÞ are joint positions which can be linear or angular; and ‘d‘ denotes the differential operator.
Boundary conditions A, B
Construct a Riemannian manifold(M, g) Calculate Christoffel symbols
Inverse solutions ,
Geodesic equations 2 2
+
= 0,
, =1
( 0) =
,
( 1) =
Joint trajectories ( ), a geodesic on the product space
Pull back the geodesic to the position space
An accurate motion of the end-effector, smooth joint trajectories
Figure 1. Flowchart of the geodesic algorithm.
It is desirable to deduce equations describing the geodesic paths in the joint space. Let g : I ! M be a smooth curve, where I is an interval. In a local coordinate system fi g (the definition of local coordinate system will be given later), a curve can be written as i ¼ i ðsÞ;
ðs 2 I; i ¼ 1; 2; . . . ; nÞ
(2)
We say that g is a geodesic if and only if it satisfies the geodesic equations n d 2 i X dk dj ¼0 þ i ds 2 k;j¼1 kj ds ds
ði ¼ 1; 2; . . . ; nÞ
(3)
The Christoffel symbols ikj are given in terms of the Riemannian metric by ! n @gjm @gkj 1X i mi @gkm kj ¼ g þ (4) 2 m¼1 @j @k @m where gmi is the element of G1 .18 The solution of (3) with boundary conditions gives the joint trajectories. The flow graph of the geodesic algorithm is shown in Figure 1. The key step is to construct a Riemannian manifold M and its Riemannian metric g such that a geodesic of the Riemannian manifold is the planned path. We take all of the possible positions and orientations of a robot as a Riemannian manifold M which is the product manifold of the position space and the orientation space. To be specific, M consists of all possible positions and orientations, p^ ¼ ðn; o; a; pÞ, where p is the end-effector position and the rotation matrix R ¼ ðnoaÞ represents its orientation.1 The Riemannian metric is defined by g ¼ g1 þ g2
(5)
Chen et al.
3
ϕ
V1
M1
φ
M ϕ –1 γ
ϕ*γ
x
I
U
Figure 3. A diagram of the pull-back. x
V2 Figure 2. Mappings of local coordinates.
where g1 ¼ ðdpÞ 2 (p is the end-effector position vector) is the distance metric of M1 , and g2 is a perturbation component to ensure that the metric g is a positive-definite quadratic form. Because a quadratic form can be written as the sum of squares by the applying the non-singular linear transformations,20 it follows that the perturbation component g2 can also be written as a form of sum of squares. In order to optimize the orientation, we define g2 as g2 ¼ ðdnÞ 2 þ ðdoÞ 2 þ ðdaÞ 2
(6)
Then, the Riemannian metric becomes g ¼ d p^2 ¼ ðdpÞ 2 þ ðdnÞ 2 þ ðdoÞ 2 þ ðdaÞ 2
(7)
Obviously, the Riemannian metric contains not only the positional parameters but also the rotational ones. For robots, the kinematical equations give a description from the joint space to the Cartesian space. Because the kinematical equations are nonlinear, the inverse solution is not always unique. There is always no one-to-one mapping between the Cartesian space and the joint space. For this reason, we cannot directly take all the joint variables as the coordinate system of the Cartesian space. In order to solve this problem, local coordinates are introduced. Assuming U is a neighbourhood of a manifold M. If : U ! ðU Þ Rn (ðU Þ is open) is the homeomorph: ism and denote i ðQÞ¼ððQÞÞi , ðU ; i Þ is a local coordinate system of M and i ði ¼ i; . . . ; nÞ are local coordinates.19 When the joint variables can be regarded as local coordinates of M, for an arbitrary point Q0 2 M and a neighbourhood of Q0 , U , the inverse solutions of Q0 may be not unique. Without lost of generality, assume there are two ð1Þ ð2Þ inverse solutions i and i ði ¼ i; . . . ; nÞ. Let V1 and V2 denote neighbourhoods of ð1Þ and ð2Þ , respectively, which satisfy xðV 1 Þ ¼ U , xðV 2 Þ ¼ U , where x ¼ xðÞ is the forward kinematics equation. Define : U ! V1 Rn as follows ðQ 0 Þ ¼ ð1Þ (8) ðQÞ ¼ x1 ðQÞjV 1 ; Q 2 U : Here is a homeomorphism mapping and i ðQÞ¼ ððQÞÞi are local coordinates. The mappings of the local coordinates are shown in Figure 2.
After the Riemannian metric is constructed, the geodesics on the product space can be obtained by (3). It deserves nothing that the geodesics represented by joint variables are on the product space, but it is desirable to describe the planned trajectory on the positional space with a specific shape such as a line. Therefore, we need to pull the geodesics back to the positional space to obtain an accurate Cartesian path. For a given embedding ’ : M1 ! M and a geodesic g : I ! M, the pull-back of g is defined as follows ’ gðsÞ ¼ ’1 gðsÞ; ðs 2 IÞ;
(9)
where ’ g is the pullback of g. The diagram of the pullback is shown in Figure 3. The immediate output of the geodesic algorithm is joint trajectories. The motion performed by the robot end-effector is exactly a geodesic path. When g2 ¼ 0, the improved geodesic algorithm degenerates to the traditional one, i.e. g ¼ g1 ¼ ðdpÞ 2 . Because the Riemannian metric of the traditional geodesic algorithm only contains the positional parameters, the coefficient matrix G will become singular, if there are more than three DOFs. Due to this reason, the traditional geodesic algorithm can only be used to plan trajectories for planar 2R and spatial 3R robots.10 As shown in (7), the improved Riemannian metric contains not only the positional parameters but also the rotational ones so that the coefficient matrix G will not become singular when the DOFs are more than three. Therefore, the improved geodesic algorithm can be used to plan trajectories for multi-joint robots with more than three DOFs. In addition, when the metric is defined as (7), the manifold we focus on is position and orientation space which is at most six dimensional. The amount of joints which can be selected as local coordinates i ði ¼ 1; . . . ; nÞ is at most six, i.e. n 6.
The properties of the improved algorithm Effect of the perturbation component Let us do a thorough study on the effect of the perturbation component g2 . Lemma 1. The trajectory planned by the geodesic algorithm is a geodesic. Proof. For low-DOF robots, if we take the Riemannian metric as g ¼ g1 ¼ ðdpÞ 2 , the trajectory planned by the geodesic algorithm is a geodesic. Because the calculation
4 of a geodesic is on the joint space and the positional space, no pullback is needed. For multi-DOF robots, if (7) is taken as the Riemannian metric, we need to show that the pullback of a geodesic is a geodesic. Given Riemannian manifolds ðM1 ; g 1 Þ and ðM 2 ; g2 Þ, M 1 M2 is the Riemannian product of M 1 and M 2 . Assume that P and Q are the projection mappings from T ðM1 M 2 Þ to TM 1 and TM2 , respectively, where TM denotes the tangent space of M. The Riemannian metric of M 1 M 2 is : defined by gðX ; Y Þ¼g1 ðPX ; PY Þ þ g2 ðQX ; QY Þ. Then, both M 1 and M2 are all geodesic submanifolds of M 1 M 2 .22 If M1 is the positional space and M ¼ M 1 M 2 is the position and orientation space, M 1 is a geodesic submanifold of M. That is, assuming that ’ : M1 ! M is an embedding and g is a geodesic of M, the pullback of g (i.e. ’ g) is a geodesic of M 1 . c From Lemma 1, the perturbation component g2 will not make a perturbation when no simplification is made. It is not easy to find and describe the embedding and the pullback. In Section 4, we will make simplifications and take the projection of a geodesic from M to M 1 as the trajectories of the positional space and the joint space. The perturbation may take place when we replace the geodesic pull-back by the projection. The longer the distance is, the more evident the perturbation will be.
Output controllability Lemma 2 The velocity of the trajectory planned by the geodesic algorithm is controllable. Proof. From Section 3.1, the trajectory planned by the improved algorithm is either a geodesic or the projection of a geodesic (a simplified case). First, we declare that the velocity of a geodesic is controllable. If a geodesic gðs; vÞ (v is the initial velocity of g) is defined on s 2 ð"; "Þ, then gðs; vÞ ; ð > 0Þ is also a geodesic that is defined on s 2 " ; " ; moreover, gðs; vÞ ¼ gðs; vÞ.21 This means that the initial velocity can be controlled by adjusting the range of parameter s. Because a geodesic generally has constant speed (the norm of speed is constant),18 the velocity of a geodesic is controllable. Second, the velocity of the geodesic projection is still controllable. Because the velocity of a geodesic on the position and orientation space is controllable, the velocity of the geodesic projection from the position and orientation space to the position space is also controllable. c Because of Lemma 2, we can adjust the velocity to make the robot move the entire distance in a given time. Thus, in the following simulations, we simply take a unified convention that the parameter s always ranges from 0 to 1. One can change the interval of the parameter s to meet other demands. If the velocity of a geodesic is set to be 1, the parameter s stands for the arc length. If parameter s is time, the motion time can be predefined. In this way, time
International Journal of Advanced Robotic Systems optimal trajectory planning can be realized, as the time can be optimized to meet the kinematical constraints.
Trajectory scalings Problem 1. Given a space M and two points x 0 , x 1 2 M, with no other constraints, how should we choose our interpolant x : ½0; T ! M, T > 0 so that xð0Þ ¼ x 0 ; xðT Þ ¼ x 1 ? Here, we can choose a geodesic to connect the two points. If the given points were specified as x 0 ; x 1 ; . . . ; xn , (n > 1), we might choose x to be a piecewise-geodesic, which is usually not differentiable at the junctions. There is frequently a jump discontinuity of velocity curves at the junctions. With Lemma 2, we can perform trajectory scalings to reduce the gaps. Assume that x 1 is a junction, g1 ðsÞ ðs 2 ½0; T1 Þ is a geodesic connecting x 0 and x 1 , and g2 ðtÞ ðt 2 ½T1 ; T 2 Þ is another geodesic connecting x 1 and x 2 . 0 0 Denote g 1 ð0Þ ¼ v 0 ; g 2 ðT 1 Þ ¼ v 1 ðjv 0 j 6¼ jv 1 jÞ. Because a 0 0 geodesic has constant speed, jg 1 j ¼ jv 0 j and jg 2 j ¼ jv 1 j. We scale g2 to reduce the gap between g1 and g2 . As mentioned in Section 3.2, gðs; vÞ ¼ gðs; vÞ ; > 0. By means of this property, g2 ðt; v 2 Þ ¼ g2 ð1 t; v 2 Þ. jv 0 j ¼ jv 2 j 0j requires ¼ jv jv 1 j. Set
g3 ðu; v 2 Þ ¼ g2
# ! " jv 0 j jv 1 j u; v 1 ; u 2 T 1 ; T1 þ ðT 2 T 1 Þ ; jv 1 j jv 0 j (10)
jv 0 j jv 1 j v 1 .
0
0
0
Then g 3 ðT1 Þ ¼ v 2 , and jg 3 j ¼ jg 1 j ¼ where v 2 ¼ 1j jv 0 j. So g3 ðu; v 2 Þ ðu 2 ½T1 ; T 1 þ jv jv 0 j ðT 2 T 1 Þ Þ is a scaling of g 2 which reduces the gap between the velocity curves of g1 and g2 . Here g2 should be replaced by g3 . Because of the definition of g3 , the paths are the same geodesic when g 2 and g3 are mapped into the Cartesian space. When the projection of a geodesic is taken as the trajectory and perturbations arise, we can select several points on the shortest path that connects the initial point and the end point. Applying the improved algorithm to each neighbouring pair, we obtain a piecewise-geodesic that is still a line when it is mapped into the positional space. With the trajectory scaling as mentioned above, the piecewise-geodesic can be second-order continuous at the junctions. One can smooth the piecewise-geodesic at the junctions when higher-order continuity is needed.
Simulations The improved geodesic algorithm for a planar 3R robot A planar 3R robot is shown in Figure 4, where li ; i ði ¼ 1; 2; 3Þ denote linkage lengths and rotatory angles, respectively. The Riemannian metric is defined as follows
Chen et al.
5 Table 1. Boundary conditions of the planar 3R robot.
l3
θ3 p n o
θ2 l2
l1
A
B
(2.5 m, 1.5 m, 0 m) ð0:866; 0:5; 0Þ ð0:5; 0:866; 0Þ
(1.5 m, 2.5 m, 0 m) ð0:714; 0:7; 0Þ ð0:7; 0:714; 0Þ
θ1 Table 2. A set of inverse kinematic solutions of the planar 3R robot. Figure 4. A planar 3R robot.
g
¼ ¼
where 8 p¼ > > < n¼ o ¼ > > : a¼
ðdpÞ 2 þ ðdnÞ 2 þ ðdoÞ 2 þ ðdaÞ 2 ð d1 d 2 d3 ÞGð d1 d 2
d 3 Þ T
(11)
s 1 (rad) 2 (rad) 3 (rad)
A
B
0 0.258 0.583 0.312
1 0.970 0.380 0.574
In many practical systems, the calculation time is an important factor that should be taken into consideration. Unlike traditional polynomial-based methods, the proposed method does not require a constant solution to the inverse kinematics, except at the endpoints. The computational effi(12) ciency here mainly depends on the solving process of the G ¼ ðgij Þ 33 is the coefficient matrix of the Riemannian geodesic equations, which are second-order nonlinear ordimetric, and its elements are nary differential equations with the form as (3). For the planar 8 3R robot, Equation (3) is a system of second-order differential 2 2 g11 ¼ 2 þ ðs 23 l 1 þ s 3 l2 Þ þ ðc 23 l 1 þ c 3 l 2 þ l 3 Þ ; > > equations with three unknowns. When the boundary condi> > > g12 ¼ g21 ¼ 2 þ l 22 þ 2c 3 l 2 l 3 þ l32 þ l1 ðc 2 l2 þ c 23 l 3 Þ; tions are given, the function bvp4c in MATLAB can be used > < g13 ¼ g31 ¼ 2 þ l 3 ðc 23 l 1 þ c 3 l2 þ l 3 Þ; to solve these equations with high efficiency. > g ¼ 2 þ l 22 þ 2c 3 l 2 l 3 þ l32 ; 22 > To simplify, assume that l 1 ¼ l 2 ¼ l 3 ¼ 1 m, and the > > > g23 ¼ g32 ¼ 2 þ c 3 l2 l3 þ l 32 ; > robot moves linearly from the initial point A to the end : g33 ¼ 2 þ l 32 point B. The values of A and B are shown in Table 1. A (13) set of inverse kinematic solutions are shown in Table 2. As The corresponding geodesic is determined by (3) and mentioned in Section 3.2, the parameter s is set to be dimensionless and ranges from 0 to 1. One can change the (4). By calculation, we get values and add a unit to the parameter. 8 c l þ l þ c l l þ c l 1 1 1 2 1 2 3 3 2 3 3 1 > The cubic polynomial interpolation is chosen to make a > 11 ¼ ; 12 ¼ 21 ¼ 22 ¼ ; > > s2 l1 s2 l1 > > comparison to demonstrate the smoothness of the trajectory > > > c l > 3 3 1 1 1 1 1 planned by the improved algorithm. To enhance the accu> 13 ¼ 31 ¼ 23 ¼ 32 ¼ 33 ¼ ; > > > s2 l1 racy of the contour in the Cartesian space, we choose 11 > > > > > 1 via-points which uniformly distribute on the linear path. In > ðl 12 þ l2 ðl2 þ c 3 l 3 Þ þ l1 ð2c 2 l2 þ c 23 l3 ÞÞ ; 211 ¼ > > > l l s order to obtain a smooth end-effector motion, the end2 1 2 > > > > > effector velocity is set to be constant at these via-points. l ðl þ c 3 l3 Þ þ l1 ðc 2 l2 þ c 23 l 3 Þ > > ; 212 ¼ 221 ¼ 222 ¼ 2 2 > > The joint positions at the knots are obtained by the inverse < s2 l1 l2 kinematics. Cartesian velocities at the via-points are conðc l þ c 3 l2 Þl3 > > ; 213 ¼ 231 ¼ 223 ¼ 232 ¼ 233 ¼ 23 1 > verted into joint velocities at the knots by using the inverse > > s2 l1 l2 > > Jacobian. The trajectory segments between knots is calcu> > > l 1 þ c 2 l 2 þ c 23 l 3 3 > lated by interpolating cubic polynomials for each joint. > ; 11 ¼ > > s2 l2 > > The trajectories and their velocities, accelerations and > > > c l c > 23 3 2 3 3 3 jerks of the end-effector obtained by the improved algo> ; 12 ¼ 21 ¼ 22 ¼ > > > s2 s2 l2 rithm and the polynomial interpolation are compared in > > > > > c l Figure 5. It is shown that the Cartesian path is accurate (see > > 3 ¼ 331 ¼ 323 ¼ 332 ¼ 333 ¼ 23 3 > : 13 s2 l2 Figure 5(a)). The end-effector velocity, acceleration and jerk determined by the improved geodesic algorithm almost (14) keep constant, but those obtained by the cubic polynomial The geodesic is identified by given initial conditions or interpolation oscillate obviously (see Figure 5.(b)). The maximum absolute value of position jerks for the end-effector of boundary conditions. ðc 1 l 1 þ c 12 l 2 þ c 123 l 3 ; s 1 l 1 þ s 12 l2 þ s 123 l3 ; 0Þ ðc 123 ; s 123 ; 0Þ ðs 123 ; c123 ; 0Þ ð0; 0; 1Þ
6
International Journal of Advanced Robotic Systems
1.418
2.8 2.6
vel.
geodesic method polynomial method
1.416 1.414 1.412 0
2.4
0.1
0.2
0.3
0.4
0.5
0.6
0.7
0.8
0.9
1
0.1
0.2
0.3
0.4
0.5
0.6
0.7
0.8
0.9
1
s
2.2
acc.
py (m)
1
0.5
2 0
1.8
jerk
1.6 1.4
0
s
60
1
1.5
2
2.5
20 0
3
geodesic method polynomial method
40
0
0.1
0.2
0.3
0.4
0.5
px (m)
s
(a)
(b)
0.6
0.7
0.8
0.5
1
0.4 0.35
o,geodesic o,polynomial
0.8
0
0.1
0.2
0.3
0.4
0.5
0.6
0.7
0.8
0.9
1
0
0.1
0.2
0.3
0.4
0.5
0.6
0.7
0.8
0.9
1
0.1
0.2
0.3
0.4
0.5
0.6
0.7
0.8
0.9
1
2
acc.
n y , oy
vel.
n,polynomial
0.9
1
geodesic method polynomial method
0.45
n,geodesic
0.9
0.7
s
1.5 1 0.5 0
0.6
150
0.4 −1
jerk
0.5
−0.5
0
0.5
n x , ox
100 50 0
1
s
0
(c)
s
(d)
Figure 5. The simulation results of the planar 3R robot in the Cartesian space: (a) the position; (b) the position velocity, acceleration and jerk; (c) the orientation; (d) the orientation velocity, acceleration and jerk. Table 3. Comparisons of maximum absolute jerk values. Joint Maximum absolute jerk The geodesic algorithm The polynomial interpolation
End-effector
1
2
3
Position
Orientation
106.901 105.428
214.241 126.839
107.347 126.789
0.042 41.901
0.022 139.229
the geodesic trajectory is 0.042, while that of the polynomial trajectory is 41.901, as shown in Table 3. The end-effector orientation velocity, acceleration and jerk determined by the geodesic are almost constant, but those obtained by the cubic polynomial interpolation oscillate obviously (see Figure 5.(d)). The maximum absolute value of orientation jerks for the end-effector of the geodesic trajectory is 0.022, while that of the polynomial interpolation is 139.229 (see Table 3).
The trajectories, velocities, accelerations and jerks of the three joints are shown in Figure 6. The joint acceleration determined by the improved geodesic algorithm is smooth, but that obtained by the polynomial interpolation fluctuates. The fluctuation of the jerks planned by the polynomial interpolation is even more remarkable. The maximum absolute value of each joint jerk determined by both methods are given in Table 3. The jerk trajectory of each joint determined
Chen et al.
7
1
2
0.8
1.5
10
150
100 5
0.6
1 50
0.4
0.5
0
Joint 1,geodesic Joint 1,polynomial Joint 2,geodesic Joint 2,polynomial Joint 3,geodesic Joint 3,polynomial
−0. 2
−0. 4
0
−0.5
Joint jerks
0
Joint accelerations
0.2
Joint velocities
Joint positions (rad)
0
−5
−50
−100 −1
−10
−150 −1.5 −15
−0. 6
−0. 8
−200
−2
0
0.5
s
1
−2.5
0
0.5
s
1
−20
0
0.5
s
1
−250
0
0.5
s
1
Figure 6. The trajectory planning of the planar 3R robot in the joint space.
by the geodesic algorithm is continuous, but that obtained by the polynomial interpolation is discontinuous. In both the Cartesian space and the joint space, the geodesic trajectory planning is smoother than the polynomial one. In the Cartesian space, the geodesic orientation is also smoother than that generated by the polynomial interpolation. In addition, if we take d ¼ ðdpÞ 2 as the Riemannian metric, i.e.
In this case, it is easy to verify that jDj is zero, i.e. the coefficient matrix D is singular. Therefore, the Riemannian metric d ¼ ðdpÞ 2 cannot be applied to plan a linear motion for a planar 3R robot.
d ¼ ðdpÞ2 ¼ ð d 1
Consider the spatial RRPP robot shown in Figure 7. It is a simplified model of the XHK 5140 automatic toolchanging CNC vertical boring and milling machine tool with four DOFs. The first two joints are revolute joints, while the other two are prismatic. Its link parameters are shown in Table 4. The T matrix of the end-effector is 0 1 nx ox ax px B ny oy ay py C C T ¼ B (17) @ nz oz az pz A 0 0 0 1
d 2
d 3 ÞDð d1
d 2
d 3 Þ T (15)
Then, the elements of the coefficient matrix D ¼ ðdij Þ 33 of the Riemannian metric will become 8 d11 ¼ ðs 23 l 1 þ s3 l2 Þ2 þ ðc 23 l 1 þ c 3 l 2 þ l 3 Þ 2 ; > > > > > d ¼ d 21 ¼ l 22 þ 2c 3 l 2 l 3 þ l 32 þ l1 ðc2 l2 þ c23 l3 Þ; > < 12 d13 ¼ l 3 ðc 23 l1 þ c3 l2 þ l3 Þ; (16) 2 2 > d > 22 ¼ l 2 þ 2c 3 l 2 l 3 þ l 3 ; > > > ¼ d 32 ¼ l 3 ðc 3 l 2 þ l 3 Þ; d > : 23 d33 ¼ l 32
The improved geodesic algorithm for a spatial RRPP robot
8
International Journal of Advanced Robotic Systems Table 5. Boundary conditions of the spatial RRPP robot.
p n o a
+d
d4
a1 5
a3
A
B
ð 0:5 m; 0:9 m; 0:7 mÞ (0:1; 0:810; p1ffiffi3) (0:102; 0:569; 0:817) (0:990; 0:141; 0:026)
ð 0:6 m; 0:8 m; 0:6 mÞ (0:105; 0:810; p1ffiffi3) (0:017; 0:580; 0:816) (0:994; 0:095; 0:047)
l
d3
Table 6. A set of inverse kinematic solutions of the spatial RRPP robot.
Figure 7. A spatial RRPP robot.
Table 4. Link parameters of the spatial RRPP robot. I 1 2
ai1
ai1
di
i
joint variable
0
0 arccos p1ffiffi3 0
0 0
1 2
1 2
d3 d4 d5
0
d3 d4
pl ffiffi 2
3 4 5
0 a3 0
where 8 > > nx ¼ > > > > > > > > > > ox ¼ > > > > > > > > > > oz ¼ > > > > > > > > > > > ay ¼ > > > > > > < px ¼ > > > > > > > > > > > > > > > > > > py ¼ > > > > > > > > > > > > > > > > > > > > > : pz ¼
2
0
2
0
pffiffiffi 2 pffiffiffi s 1 ; 3
pffiffiffi 2 1 ny ¼ pffiffiffi c 1 ; nz ¼ pffiffiffi ; 3 3 1 1 c 1 c2 þ pffiffiffi s1 s2 ; oy ¼ s 1 c 2 pffiffiffi c 1 s 2 ; 3 3 pffiffiffi 2 1 pffiffiffi s2 ; ax ¼ c 1 s 2 þ pffiffiffi s 1 c 2 ; 3 3 pffiffiffi 2 1 s 1 s 2 þ pffiffiffi c1 c2 ; az ¼ pffiffiffi c2 ; 3 3 pffiffiffi s1 ðs 2 a 3 2d 3 c 2 d4 Þ pffiffiffi þ c 1 ða1 þ c 2 a3 þ s2 d4 Þ 3 c2 s1 ffiffiffi þ c1 s 2 Þd5 ; þð p 3 pffiffiffi c1 ðs 2 a3 2d 3 þ s 2 d4 Þ pffiffiffi þ c 1 ða1 þ c 2 a3 þ s 2 d4 Þ 3 c2 c1 ffiffiffi þ s 1 s 2 Þd5 ; þð p 3 pffiffiffi pffiffiffi 2s 2 a 3 þ d3 2c 2 ðd4 þ d 5 Þ pffiffiffi 3
(18) The Riemannian metric is given by g
2 ¼ ðdpÞ 2 þ ðdnÞ 2 þ ðdoÞ 2 þ ðdaÞ 0 1 d1 B d2 C C ¼ ð d 1 d2 dd3 dd4 ÞGB @ dd3 A dd4
(19)
s 1 2 d3 d4
(rad) (rad) (m) (m)
A
B
0 0.123 1.539 0.276 0.640
1 0.129 1.629 0.239 0.643
where G ¼ ðgij Þ 44 is the coefficient matrix of the metric, and its elements are 8 pffiffiffi 1 > > ð6 þ ðc g ¼ a þ a 2s 2 d3 Þ2 11 2 1 3 > > 3 > > > > pffiffiffi > > > þðs 2 a1 þ 2c 2 d3 þ d4 þ d 5 Þ 2 > > > > > > þ2ða 1 þ c2 a3 þ s 2 ðd4 þ d5 ÞÞ 2 Þ ; > > > pffiffiffi > 1 > > g12 ¼ g21 ¼ pffiffiffi ð2 þ a 3 ðc2 a 1 þ a3 2s 2 d 3 Þ > > > 3 > > > < pffiffiffi þðd4 þ d5 Þðs2 a1 þ 2c2 d3 þ d 4 þ d5 ÞÞ ; sffiffiffi > > > 2 > > ða1 þ c2 a 3 þ s 2 ðd4 þ d 5 ÞÞ ; ¼ g ¼ g 13 31 > > 3 > > > > > pffiffiffi > > > c 2 a 1 þ a 3 2s 2 d 3 > p ffiffiffi ; > g14 ¼ g41 ¼ > > 3 > > > > > > 2 > > g22 ¼ 2 þ a 23 þ ðd4 þ d 5 Þ ; g23 ¼ g32 ¼ g34 ¼ g 43 ¼ 0 ; > > : g24 ¼ g42 ¼ a3 ; g 33 ¼ g 44 ¼ 1 (20) For simplicity, assume that a1 ¼ a 3 ¼ d5 ¼ 1 m, and the robot moves linearly from the initial point A to the end point B. The values of A and B are given in Table 5. A set of inverse kinematic solutions are shown in Table 6. Because the traditional geodesic algorithm is unsuitable for the trajectory planning of the RRPP robot, only the results planned by the improved geodesic algorithm are provided here. The trajectory and velocity, acceleration and the jerk of the end-effector are shown in Figure 8. The trajectories, velocities, accelerations and jerks of the joints are shown in Figure 9. The orientations of the end-effector are shown in Figure 10. It is shown that the geodesic trajectory is smooth in both the Cartesian space and the joint space. Therefore, the improved geodesic algorithm is effective in treating the spatial RRPP robot, which has four DOFs: two revolute and two prismatic.
Chen et al.
9
0.2
vel.
−0.6 −0.62
0.15 0.1
−0.64
0
0.1
0.2
0.3
0.4
0.5
0.6
0.7
0.8
0.9
1
0.1
0.2
0.3
0.4
0.5
0.6
0.7
0.8
0.9
1
0.1
0.2
0.3
0.4
0.5
0.6
0.7
0.8
0.9
1
s
0.16
acc.
−0.66 −0.68
0.08 0
−0.7 −1 −0.5
−0.9 −0.55
−0.6
−0.8
−0.65
s
0.03
jerk
−0.72 −0.45
0
py (m)
0.02 0.01 0
px (m)
(a)
s
(b)
Figure 8. The trajectory planning of the spatial RRPP robot in the Cartesian space.
1
0.02
0.02
0.5
0
0
0
−0.02
−0.02
0.03
0.025
−1
−0.04
−0.06
Joint 1 Joint 2 Joint 3 Joint 4
0.015 Joint jerks
−0. 5
Joint accelerations
Joint velocities
Joint positions (rad/mm)
0.02
−0.04
0.01
−0.06 0.005
−1. 5
−2
−0.08
0
0.5
s
1
−0.1
−0.08
0
0.5
1
−0.1
0
0
s
Figure 9. The trajectory planning of the spatial RRPP robot in the joint space.
0.5
s
1
−0.005
0
0.5
s
1
10
International Journal of Advanced Robotic Systems
0.8165
0.816
oz
2
1.5
0.8155 0.05
0.5
0 0
−0.988 −0.989
0.815
az
nz
1
0 −0.99
0.106 −0.02 0.105
−0. 5 0.104
0.8105
0.103
−0.1 −0.06 0.565
0.101
ny
−0.08
0.102
0.81
0.57
nx
0.8095 0.1
−0.991
−0.05 −0.04
−0.1
ox
0.575
−0.12 0.58
oy
−0.992
−0.12 −0.993 −0.14
ay
−0.16
−0.994
ax
−0.18 −0.995
Figure 10. The planned orientation of the spatial RRPP robot in the Cartesian space.
Conclusion and future work An improved geodesic algorithm is proposed for the trajectory planning of multi-joint robots. The geodesic trajectory planning has many advantages, but there still leave many fundamental problems unsolved such as orientation trajectory planning. The method can be used to tackle the orientation problem and can be applied to multi-joint robots. Smooth trajectories of robot end-effectors and joints are obtained. The joint variables are chosen to constitute a local coordinate system of the product of the positional space and the orientational space. By defining a new Riemannian metric that contains both the positional and rotational parameters, the traditional geodesic algorithm is improved so that it becomes capable of planning trajectories that include not only the position but also the orientation. Simulations are performed on the trajectory planning of both a planar 3R robot and another spatial RRPP robot, and it is demonstrated that the improved geodesic algorithm is effective in generating smooth motions for both the end-effector and the joints. Finally, there is a limitation which is the present improved algorithm applies to robots with no more than six DOFs. In addition, the simulations provided in this paper only focus on linear motions. These are two limitations of
the present work. Therefore, the future work will be directed to construct appropriate metrics to plan trajectories of robots with more than six or redundant DOFs, to achieve the geodesic planning of curvilinear motions, and to develop the control part. Declaration of conflicting interests The author(s) declared no potential conflicts of interest with respect to the research, authorship, and/or publication of this article.
Funding The author(s) disclosed receipt of the following financial support for the research, authorship, and/or publication of this article: This work was supported by the National Key Technology R&D Program of China under Grant No. 2014BAF04B01.
References 1. Craig JJ. Introduction to Robots: Mechanics and Control, 3rd edn. Englewood Cliffs: Pearson/Prentice Hall, 2005. 2. Taylor RH. Planning and execution of straight line manipulator trajectories. IBM J Res Development 1979; 23: 424–436. 3. Lin CS, Chang PR and Luh JYS. Formulation and optimization of cubic polynomial joint trajectories for industrial robots. IEEE Trans Automatic Control 1983; 28: 1066–1074.
Chen et al. 4. Boryga M and Grabos´ A. Planning of manipulator motion trajectory with higher-degree polynomials use. Mech Machine Theory 2009; 44: 1400–1419. 5. Piazzi A and Visioli A. Global minimum-jerk trajectory planning of robot manipulators. IEEE Trans Indust Electron 2000; 47: 140–149. 6. Chettibi T, Lehtihet HE, Haddad M and Hanchi A. Minimum cost trajectory planning for industrial robots. Eur J Mech A Solids 2004; 23: 703–715. 7. Wang CH and Horng JG. Constrained minimum-time path planning for robot manipulators via virtual knots of the cubic B-spline functions. IEEE Trans Automatic Control 1990; 35: 573–577. 8. Gasparetto A and Zanotto V. A new method for smooth trajectory planning of robot manipulators. Mech Machine Theory 2007; 42: 455–471. 9. Zˇefran M, Kumar V and Croke CB. On the generation of smooth three-dimensional rigid body motions. IEEE Trans Robotics Automation 1998; 14: 576–589. 10. Zhang L and Zhou C. Kuka youBot arm shortest path planning based on geodesics. In: Proceedings IEEE International Conference on Robotics and Biomimetics, 2013, pp. 2317–2321. 11. Park FC and Ravani B. Be`zier curves on Riemannian manifolds and lie groups with kinematic applications. AMSE J Mech Design 1995; 117: 36–40.
11 12. Kuffner JJ. Effective sampling and distance metrics for 3D rigid body path planning. In: Proceedings IEEE International Conference on Robotics and Automation, 2004, pp. 3993–3998. 13. Noakes L and Popiel T. Geometry for robot path planning. Robotica 2007; 25: 691–701. 14. Gallot S, Hulin D and Lafontaine J. Riemannian Geometry. Berlin: Springer-Verlag, 2004. 15. Tchon´ K and Dule¸ba I. On inverting singular kinematics and geodesic trajectory generation for robot manipulators. J Intell Robotic Syst 1993; 8: 325–359. 16. Selig JM and Ovseevitch AI. Manipulating robots along helical trajectories. Robotica 1996; 14: 261–267. 17. Zhang L and Zhou C. Robot optimal trajectory planning based on geodesics. In: Proceedings IEEE International Conference on Control and Automation, 2007, pp. 2433–2436. 18. Spivak M. A Comprehensive Introduction to Differential Geometry, Vol. 1. Berkeley, CA: Publish or Perish Press, 1979. 19. Warner FW. Foundations of Differentiable Manifolds and Lie Groups. New York: Springer, 1983. 20. O’Meara OT. Introduction to Quadratic Forms. Berlin: Springer-Verlag, 1973. 21. do Carmo M. Differential Geometry of Curves and Surfaces. Englewood Cliffs, NJ: Prentice-Hall, 1976. 22. Senlin X and Yilong N. Submanifolds of product Riemannian manifold. Acta Math Sci 2000 20(B): 213–218.
