Upload
others
View
3
Download
0
Embed Size (px)
Citation preview
ECE5463: Introduction to Robotics
Lecture Note 9: Trajectory Generation
Prof. Wei Zhang
Department of Electrical and Computer EngineeringOhio State UniversityColumbus, Ohio, USA
Spring 2018
Lecture 9 (ECE5463 Sp18) Wei Zhang(OSU) 1 / 16
Outline
• Trajectory Generation Problem
• Point-to-Point Trajectories
• Time Scaling of Straight-Line Path
• Trajectory Generation Using Via Points
Outline Lecture 9 (ECE5463 Sp18) Wei Zhang(OSU) 2 / 16
Path and Time Scaling
• Roughly, a trajectory is a specification of the robot position (configuration)as a function of time.
• It is often useful to view a trajectory as a combination of a path, a purelygeometric description of the sequence of configurations achieved by therobot, and a time scaling, which specifies the times when thoseconfigurations are reached.
• Path: a function θ : [0, 1]→ Θ that maps a scalar path parameter s to apoint in the robot configuration space Θ
- s = 0: the start of the path- s = 1: the end of the path
• Time scaling: a function s : [0, T ]→ [0, 1] that maps each time instant to avalue of the geometric path parameter.
Trajectory Generation Problem Lecture 9 (ECE5463 Sp18) Wei Zhang(OSU) 3 / 16
Trajectory Generation Problem
• Trajectory: θ(s(t)) or θ(t) for short.
• Velocity and acceleration along a trajectory θ(s(t)):
θ =dθ
dss; θ =
dθ
dss+
d2θ
ds2s2
• Trajectory Generation: Construct a trajectory (path + time scaling) so thatthe robot reaches a sequence of points in a given time
• Trajectory should be sufficiently smooth and respect limits on joint variables,velocities, accelerations, or torques
• Some of the joint limits can be “state-dependent”
• Due to time limit, we will not formally discuss path planning problems(design path to avoid obstacles).
Trajectory Generation Problem Lecture 9 (ECE5463 Sp18) Wei Zhang(OSU) 4 / 16
Straight-Line Path
• Straight-line path in joint space:
θ(s) = θstart + s(θend − θstart)
• Straight lines in joint space generally do not yield straight-line motion of theend-effector in task space
Chapter 9. Trajectory Generation 327
θ1 (deg)
θ2 (deg)
θ1
θ2 −90 90 180
θstart
θend
θstart
θend
−90
90
180
θ1 (deg)
θ2 (deg)
−90 90 180
−90
90
180
Figure 9.1: (Left) A 2R robot with joint limits 0◦ ≤ θ1 ≤ 180◦, 0◦ ≤ θ2 ≤ 150◦. (Topcenter) A straight-line path in joint space and (top right) the corresponding motionof the end-effector in task space (dashed line). The reachable endpoint configurations,subject to joint limits, are indicated in gray. (Bottom center) This curved line in jointspace and (bottom right) the corresponding straight-line path in task space (dashedline) would violate the joint limits.
Xstart and Xend are represented by a minimum set of coordinates then a straightline is defined as X(s) = Xstart +s(Xend−Xstart), s ∈ [0, 1]. Compared with thecase when joint coordinates are used, the following issues must be addressed:
• If the path passes near a kinematic singularity, the joint velocities maybecome unreasonably large for almost all time scalings of the path.
• Since the robot’s reachable task space may not be convex in X coordinates,some points on a straight line between two reachable endpoints may notbe reachable (Figure 9.1).
In addition to the issues above, if Xstart and Xend are represented as elements
May 2017 preprint of Modern Robotics, Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org
Chapter 9. Trajectory Generation 327
θ1 (deg)
θ2 (deg)
θ1
θ2 −90 90 180
θstart
θend
θstart
θend
−90
90
180
θ1 (deg)
θ2 (deg)
−90 90 180
−90
90
180
Figure 9.1: (Left) A 2R robot with joint limits 0◦ ≤ θ1 ≤ 180◦, 0◦ ≤ θ2 ≤ 150◦. (Topcenter) A straight-line path in joint space and (top right) the corresponding motionof the end-effector in task space (dashed line). The reachable endpoint configurations,subject to joint limits, are indicated in gray. (Bottom center) This curved line in jointspace and (bottom right) the corresponding straight-line path in task space (dashedline) would violate the joint limits.
Xstart and Xend are represented by a minimum set of coordinates then a straightline is defined as X(s) = Xstart +s(Xend−Xstart), s ∈ [0, 1]. Compared with thecase when joint coordinates are used, the following issues must be addressed:
• If the path passes near a kinematic singularity, the joint velocities maybecome unreasonably large for almost all time scalings of the path.
• Since the robot’s reachable task space may not be convex in X coordinates,some points on a straight line between two reachable endpoints may notbe reachable (Figure 9.1).
In addition to the issues above, if Xstart and Xend are represented as elements
May 2017 preprint of Modern Robotics, Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org
• We may prefer to have straight-line path in task space that connectsTstart ∈ SE(3) to Tend ∈ SE(3).
• There are two ways to design a “straight-line” path in task space: usingminimum representation and using screw motion
Point-to-Point Trajectories Lecture 9 (ECE5463 Sp18) Wei Zhang(OSU) 5 / 16
Straight-Line Path in Task Space: Approach 1
• Let x(s) be some minimum representation of the end-effect frame (e.g.location + Euler angles)
• The straight line can be defined as x(s) = xstart + s(xend − xstart)
• Use inverse kinematics to find the corresponding joint space path θ(s)
• Even when xstart and xend are both reachable, some points along the straightline in task space may not be reachable
Chapter 9. Trajectory Generation 327
θ1 (deg)
θ2 (deg)
θ1
θ2 −90 90 180
θstart
θend
θstart
θend
−90
90
180
θ1 (deg)
θ2 (deg)
−90 90 180
−90
90
180
Figure 9.1: (Left) A 2R robot with joint limits 0◦ ≤ θ1 ≤ 180◦, 0◦ ≤ θ2 ≤ 150◦. (Topcenter) A straight-line path in joint space and (top right) the corresponding motionof the end-effector in task space (dashed line). The reachable endpoint configurations,subject to joint limits, are indicated in gray. (Bottom center) This curved line in jointspace and (bottom right) the corresponding straight-line path in task space (dashedline) would violate the joint limits.
Xstart and Xend are represented by a minimum set of coordinates then a straightline is defined as X(s) = Xstart +s(Xend−Xstart), s ∈ [0, 1]. Compared with thecase when joint coordinates are used, the following issues must be addressed:
• If the path passes near a kinematic singularity, the joint velocities maybecome unreasonably large for almost all time scalings of the path.
• Since the robot’s reachable task space may not be convex in X coordinates,some points on a straight line between two reachable endpoints may notbe reachable (Figure 9.1).
In addition to the issues above, if Xstart and Xend are represented as elements
May 2017 preprint of Modern Robotics, Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org
Chapter 9. Trajectory Generation 327
θ1 (deg)
θ2 (deg)
θ1
θ2 −90 90 180
θstart
θend
θstart
θend
−90
90
180
θ1 (deg)
θ2 (deg)
−90 90 180
−90
90
180
Figure 9.1: (Left) A 2R robot with joint limits 0◦ ≤ θ1 ≤ 180◦, 0◦ ≤ θ2 ≤ 150◦. (Topcenter) A straight-line path in joint space and (top right) the corresponding motionof the end-effector in task space (dashed line). The reachable endpoint configurations,subject to joint limits, are indicated in gray. (Bottom center) This curved line in jointspace and (bottom right) the corresponding straight-line path in task space (dashedline) would violate the joint limits.
Xstart and Xend are represented by a minimum set of coordinates then a straightline is defined as X(s) = Xstart +s(Xend−Xstart), s ∈ [0, 1]. Compared with thecase when joint coordinates are used, the following issues must be addressed:
• If the path passes near a kinematic singularity, the joint velocities maybecome unreasonably large for almost all time scalings of the path.
• Since the robot’s reachable task space may not be convex in X coordinates,some points on a straight line between two reachable endpoints may notbe reachable (Figure 9.1).
In addition to the issues above, if Xstart and Xend are represented as elements
May 2017 preprint of Modern Robotics, Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org
Point-to-Point Trajectories Lecture 9 (ECE5463 Sp18) Wei Zhang(OSU) 6 / 16
Straight-Line Path in Task Space: Approach 2
• How do define “straight line” in SE(3)? Note that: Tstart + s(Tend − Tstart)may not lie in SE(3)
• Straight line motion in Euclidean space ↔ Screw motion in SE(3)
• Let X(s) be the screw motion path with constant twist such thatX(0) = Tstart and X(1) = Tend.
X(s) = Tstart exp(log(T−1startTend
)s)
• The translational and rotational parts of X(s) = (R(s), p(s)) are:
p(s) = pstart + s(pend − pstart), R(s) = Rstart exp(log(RTstartRend
)s)
Point-to-Point Trajectories Lecture 9 (ECE5463 Sp18) Wei Zhang(OSU) 7 / 16
Time Scaling: Cubic Polynomial
• We can design scaling function s : [0, T ]→ [0, 1] to ensure a “smooth”motion along the path θ(s) and all the velocity and acceleration constraintsare satisfied.
• We often require zero velocity at the start and the end of a path. This leadsto the following constraint:
s(0) = 0, s(T ) = 1, s(0) = s(T ) = 0
• We can use a cubic polynomial to meet these constraints:
s(t) = a0 + a1t+ a2t2 + a3t
3
⇒
Time Scaling Lecture 9 (ECE5463 Sp18) Wei Zhang(OSU) 8 / 16
Time Scaling: Cubic Polynomial
Chapter 9. Trajectory Generation 329
screw path
decoupled rotation and translation
Xstart
Xend
Figure 9.2: A path following a constant screw motion versus a decoupled path wherethe frame origin follows a straight line and the angular velocity is constant.
s s s
T t T t
T t
1 32T
6T 2
0
Figure 9.3: Plots of s(t), s(t), and s(t) for a third-order polynomial time scaling.
9.2.2.1 Polynomial Time Scaling
Third-order Polynomials A convenient form for the time scaling s(t) is acubic polynomial of time,
s(t) = a0 + a1t+ a2t2 + a3t
3. (9.9)
A point-to-point motion in time T imposes the initial constraints s(0) = s(0) = 0and the terminal constraints s(T ) = 1 and s(T ) = 0. Evaluating Equation (9.9)and its derivative
s(t) = a1 + 2a2t+ 3a3t2 (9.10)
at t = 0 and t = T and solving the four constraints for a0, . . . , a3, we find
a0 = 0, a1 = 0, a2 =3
T 2, a3 = − 2
T 3.
Plots of s(t), s(t), and s(t) are shown in Figure 9.3.
May 2017 preprint of Modern Robotics, Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org
• Assume straight-line path in joint space (other cases are similar)
θ(s) = θstart + s(θend − θstart)
• Max joint velocities are achieved at the halfway point of the motion t = T/2
• Max joint accelerations and decelerations are achieved at t = 0 and t = T :
• Joint constraints θlimit and θlimit can be checked at these critical times toensure feasibility
Time Scaling Lecture 9 (ECE5463 Sp18) Wei Zhang(OSU) 9 / 16
Time Scaling: Fifth-Order Polynomial
• We may also want to have zero accelerations at t = 0 and t = T , which leadsto the following constraints:
s(0) = 0, s(T ) = 1, s(0) = s(T ) = s(0) = s(T ) = 0
• These can be met with a fifth-order polynomial:
s(t) = a0 + a1t+ a2t2 + a3t
3 + a4t4 + a5t
5
• We can use the 6 constraints to solve uniquely for the coefficients.Chapter 9. Trajectory Generation 331
s s s
T t T t
T t
1 158T
10T 2
√3
0
Figure 9.4: Plots of s(t), s(t), and s(t) for a fifth-order polynomial time scaling.
s
1
T t t
s
v
ta T − ta T
slop
e=
a slope
=−
a
Figure 9.5: Plots of s(t) and s(t) for a trapezoidal motion profile.
The point-to-point motion consists of a constant acceleration phase s = a oftime ta, followed by a constant velocity phase s = v of time tv = T − 2ta,followed by a constant deceleration phase s = −a of time ta. The resulting sprofile is a trapezoid and the s profile is the concatenation of a parabola, linearsegment, and parabola as a function of time (Figure 9.5).
The trapezoidal time scaling is not as smooth as the cubic time scaling, but ithas the advantage that if there are known constant limits on the joint velocitiesθlimit ∈ Rn and on the joint accelerations θlimit ∈ Rn then the trapezoidalmotion using the largest v and a satisfying
|(θend − θstart)v| ≤ θlimit, (9.14)
|(θend − θstart)a| ≤ θlimit (9.15)
is the fastest straight-line motion possible. (See Exercise 9.8.)If v2/a > 1, the robot never reaches the velocity v during the motion (Ex-
ercise 9.10). The three-phase accelerate–coast–decelerate motion becomes atwo-phase accelerate–decelerate “bang-bang” motion, and the trapezoidal pro-file s(t) in Figure 9.5 becomes a triangle.
May 2017 preprint of Modern Robotics, Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org
Time Scaling Lecture 9 (ECE5463 Sp18) Wei Zhang(OSU) 10 / 16
Time Scaling: Trapezoidal
Chapter 9. Trajectory Generation 331
s s s
T t T t
T t
1 158T
10T 2
√3
0
Figure 9.4: Plots of s(t), s(t), and s(t) for a fifth-order polynomial time scaling.
s
1
T t t
s
v
ta T − ta T
slop
e=
a slope
=−
a
Figure 9.5: Plots of s(t) and s(t) for a trapezoidal motion profile.
The point-to-point motion consists of a constant acceleration phase s = a oftime ta, followed by a constant velocity phase s = v of time tv = T − 2ta,followed by a constant deceleration phase s = −a of time ta. The resulting sprofile is a trapezoid and the s profile is the concatenation of a parabola, linearsegment, and parabola as a function of time (Figure 9.5).
The trapezoidal time scaling is not as smooth as the cubic time scaling, but ithas the advantage that if there are known constant limits on the joint velocitiesθlimit ∈ Rn and on the joint accelerations θlimit ∈ Rn then the trapezoidalmotion using the largest v and a satisfying
|(θend − θstart)v| ≤ θlimit, (9.14)
|(θend − θstart)a| ≤ θlimit (9.15)
is the fastest straight-line motion possible. (See Exercise 9.8.)If v2/a > 1, the robot never reaches the velocity v during the motion (Ex-
ercise 9.10). The three-phase accelerate–coast–decelerate motion becomes atwo-phase accelerate–decelerate “bang-bang” motion, and the trapezoidal pro-file s(t) in Figure 9.5 becomes a triangle.
May 2017 preprint of Modern Robotics, Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org
• Trapezoidal time scaling:
- t ∈ [0, ta]: constant acceleration phase s = a
- t ∈ (ta, T − 2ta]: constant velocity phase s = v
- t ∈ (T − 2ta, T ]: constant deceleration phase s = −a
• Not very smooth, but allows for easy incorporation of joint velocity andacceleration limits.
• For example, a and v can be chosen to be the largest ones satisfying
|(θend − θstart)v| ≤ θlimit and |(θend − θstart)a| ≤ θlimit
Time Scaling Lecture 9 (ECE5463 Sp18) Wei Zhang(OSU) 11 / 16
Time Scaling: S-CurveChapter 9. Trajectory Generation 333
s
v
T t
1 2 3 4 5 6 7
slope
=a slope
= −a
Figure 9.6: Plot of s(t) for an S-curve motion profile consisting of seven stages: (1)constant positive jerk, (2) constant acceleration, (3) constant negative jerk, (4) con-stant velocity, (5) constant negative jerk, (6) constant deceleration, and (7) constantpositive jerk.
• Choose a and T such that aT 2 ≥ 4, ensuring that the motion is completedin time, and solve s(T ) = 1 for v:
v =1
2
(aT −√a
√aT 2 − 4
).
9.2.2.3 S-Curve Time Scalings
Just as cubic polynomial time scalings lead to infinite jerk at the beginningand end of the motion, trapezoidal motions cause discontinuous jumps in ac-celeration at t ∈ {0, ta, T − ta, T}. A solution is a smoother S-curve timescaling, a popular motion profile in motor control because it avoids vibrationsor oscillations induced by step changes in acceleration. An S-curve time scalingconsists of seven stages: (1) constant jerk d3s/dt3 = J until a desired accelera-tion s = a is achieved; (2) constant acceleration until the desired s = v is beingapproached; (3) constant negative jerk −J until s equals zero exactly at thetime s reaches v; (4) coasting at constant v; (5) constant negative jerk −J ; (6)constant deceleration −a; and (7) constant positive jerk J until s and s reachzero exactly at the time s reaches 1.
The s(t) profile for an S-curve is shown in Figure 9.6.Given some subset of v, a, J , and the total motion time T , algebraic manipu-
lation reveals the switching time between stages and conditions that ensure thatall seven stages are actually achieved, similarly to the case of the trapezoidalmotion profile.
May 2017 preprint of Modern Robotics, Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org
• Trapezoidal scaling causes discontinuities in acceleration att = 0, ta, T − 2ta, T
• An improved version is the S-curve time scaling. It consists of 7 phases.
- (1) constant jerk d3s/dt3 = J ; (2) constant acceleration s = a;
- (3) constant negative jerk −J ; (4) coasting at constant v;
- (5) constant negative jerk; (6) constant deceleration; (7) constant positive jerk
Time Scaling Lecture 9 (ECE5463 Sp18) Wei Zhang(OSU) 12 / 16
Trajectory Generation Using Via Points
• Suppose that the robot joints need to pass through a series of via points atspecified times
• Suppose that there are no strict specifications on the shape of the pathbetween consecutive points
• In this case, we can directly construct the trajectory θ(t) instead of finding apath θ(s) and then a time scaling s(t),
• Since the trajectory for each joint can be designed individually, we focus on asingle joint variable and call it β(t) to simplify notation
Polynomial Via Points Trajectory Lecture 9 (ECE5463 Sp18) Wei Zhang(OSU) 13 / 16
Trajectory Generation Using Via Points
• Suppose there are k via points (βi, βi, Ti), i = 1, . . . , k, where βi and βi arethe specified desired position and velocity, and Ti is the specified time toreach the ith via point.
• We can use a cubic polynomial to generate the ith trajectory segment.Polynomial: β(Ti + ∆t) = ai,0 + ai,1∆t+ ai,2∆t2 + ai,3∆t2
Constraints: β(Ti) = βi, β(Ti+1) = βi+1, β(Ti) = βi, β(Ti+1) = βi+1
• We can solve for all the coefficients for each segment (see page 286 oftextbook)
• This approach can be easily extended to 5th-order polynomial
Polynomial Via Points Trajectory Lecture 9 (ECE5463 Sp18) Wei Zhang(OSU) 14 / 16
Trajectory Generation Using Via Points
Example:
• Time instants: T1 = 0, T2 = 1, T3 = 2, T4 = 3;
• Via points: (0, 0), (0, 1), (1, 1), and (1, 0)
• Velocities: (0, 0), (1, 0), (0,−1), and (0, 0)
334 9.3. Polynomial Via Point Trajectories
start end start end
(a) (b)
via 2 via 3 via 2 via 3
x
y
x
y
Figure 9.7: Two paths in an (x, y) space corresponding to piecewise-cubic trajectoriesinterpolating four via points, including a start point and an end point. The velocitiesat the start and end are zero, and the velocities at vias 2 and 3 are indicated by thedashed tangent vectors. The shape of the path depends on the velocities specified atthe via points.
9.3 Polynomial Via Point Trajectories
If the goal is to have the robot joints pass through a series of via points atspecified times, without a strict specification on the shape of path betweenconsecutive points, a simple solution is to use polynomial interpolation to findjoint histories θ(t) directly without first specifying a path θ(s) and then a timescaling s(t) (Figure 9.7).
Let the trajectory be specified by k via points, with the start point occurringat T1 = 0 and the final point at Tk = T . Since each joint history is interpolatedindividually, we focus on a single joint variable and call it β to avoid a prolifera-tion of subscripts. At each via point i ∈ {1, . . . , k}, the user specifies the desiredposition β(Ti) = βi and velocity β(Ti) = βi. The trajectory has k− 1 segments,and the duration of segment j ∈ {1, . . . , k − 1} is ∆Tj = Tj+1 − Tj . The jointtrajectory during segment j is expressed as the third-order polynomial
β(Tj + ∆t) = aj0 + aj1∆t+ aj2∆t2 + aj3∆t3 (9.25)
in terms of the time ∆t elapsed in segment j, where 0 ≤ ∆t ≤ ∆Tj . Segment j
May 2017 preprint of Modern Robotics, Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org
• Change via point velocities to (0, 0), (1, 1),(1,−1), and (0, 0) will result in a differenttrajectory
334 9.3. Polynomial Via Point Trajectories
start end start end
(a) (b)
via 2 via 3 via 2 via 3
x
y
x
y
Figure 9.7: Two paths in an (x, y) space corresponding to piecewise-cubic trajectoriesinterpolating four via points, including a start point and an end point. The velocitiesat the start and end are zero, and the velocities at vias 2 and 3 are indicated by thedashed tangent vectors. The shape of the path depends on the velocities specified atthe via points.
9.3 Polynomial Via Point Trajectories
If the goal is to have the robot joints pass through a series of via points atspecified times, without a strict specification on the shape of path betweenconsecutive points, a simple solution is to use polynomial interpolation to findjoint histories θ(t) directly without first specifying a path θ(s) and then a timescaling s(t) (Figure 9.7).
Let the trajectory be specified by k via points, with the start point occurringat T1 = 0 and the final point at Tk = T . Since each joint history is interpolatedindividually, we focus on a single joint variable and call it β to avoid a prolifera-tion of subscripts. At each via point i ∈ {1, . . . , k}, the user specifies the desiredposition β(Ti) = βi and velocity β(Ti) = βi. The trajectory has k− 1 segments,and the duration of segment j ∈ {1, . . . , k − 1} is ∆Tj = Tj+1 − Tj . The jointtrajectory during segment j is expressed as the third-order polynomial
β(Tj + ∆t) = aj0 + aj1∆t+ aj2∆t2 + aj3∆t3 (9.25)
in terms of the time ∆t elapsed in segment j, where 0 ≤ ∆t ≤ ∆Tj . Segment j
May 2017 preprint of Modern Robotics, Lynch and Park, Cambridge U. Press, 2017. http://modernrobotics.org
Polynomial Via Points Trajectory Lecture 9 (ECE5463 Sp18) Wei Zhang(OSU) 15 / 16
More Discussions
•
Polynomial Via Points Trajectory Lecture 9 (ECE5463 Sp18) Wei Zhang(OSU) 16 / 16