Planning manipulator trajectories under dynamics constraints using - - PowerPoint PPT Presentation

Planning manipulator trajectories under dynamics constraints using minimum-time shortcuts

Quang-Cuong Pham

Department of Mechano-Informatics University of Tokyo

November 8th, 2012 IFToMM ASIAN Conference on Mechanism and Machine Science Tokyo, Japan

Outline

Motivations Time-optimal path parameterization algorithm Trajectory smoothing using time-optimal shortcuts

Motivations Time-optimal path parameterization algorithm Trajectory smoothing using time-optimal shortcuts

Outline

Motivations Time-optimal path parameterization algorithm Trajectory smoothing using time-optimal shortcuts

0 / 16

Motivations Time-optimal path parameterization algorithm Trajectory smoothing using time-optimal shortcuts

Time-optimal motion planning

◮ In the literature : minimum energy, minimum torque, maximum

• smoothness. . . planning algorithms

◮ But what is the most important in industry is time

1 / 16

Motivations Time-optimal path parameterization algorithm Trajectory smoothing using time-optimal shortcuts

Dynamics constraints

◮ In the literature : time-optimal motion planning under velocity and

acceleration limits (e.g. Hauser and Ng-Thow-Hing 2010)

◮ These are kinematics constraints ◮ But what physically constraints the performance of the robot is the

torque limits (= dynamics constraints)

◮ This case is much harder because strongly nonlinear !

2 / 16

Motivations Time-optimal path parameterization algorithm Trajectory smoothing using time-optimal shortcuts

Time-optimal path parameterization algorithm under torque limits

◮ If the path is fixed, very nice algorithm developed in the 80’s and 90’s

by Bobrow, Dubowsky, Gibson, Shin, McKay, Pfeiffer, Johanni, Slotine,

• Shiller. . . and many others

◮ But extensions to the non-fixed path case are less convincing

3 / 16

Motivations Time-optimal path parameterization algorithm Trajectory smoothing using time-optimal shortcuts

Our approach

◮ Time-optimal path planning in high-dimension, cluttered, environments

by combining three ideas

• 1. Randomized motion planning (e.g. RRT)
• 2. Trajectory smoothing by shortcuts
• 3. Time-optimal path parameterization algorithm

4 / 16

Motivations Time-optimal path parameterization algorithm Trajectory smoothing using time-optimal shortcuts

Outline

Motivations Time-optimal path parameterization algorithm Trajectory smoothing using time-optimal shortcuts

4 / 16

Motivations Time-optimal path parameterization algorithm Trajectory smoothing using time-optimal shortcuts

Path parameterization algorithm

◮ Inputs :

◮ Manipulator equation

M(q)¨ q + ˙ q⊤C(q)˙ q + g(q) = τ,

◮ Torque limits for each joint i

τ min

i

≤ τi(t) ≤ τ max

i

◮ A given path q(s)s∈[0,L] (set of points in the joint

space)

◮ Output : the time parameterization

s : [0, T] − → [0, L] t − → s(t) that minimizes the traversal time T

5 / 16

Motivations Time-optimal path parameterization algorithm Trajectory smoothing using time-optimal shortcuts

Outline of the algorithm

◮ Time minimal ⇔ highest possible ˙

s (without violating the torque limits)

◮ Express the manipulator equations in terms of s, ˙

s,¨ s

◮ The torque limits become

α(s, ˙ s) ≤ ¨ s ≤ β(s, ˙ s), where

◮ α(s, ˙

s) is the minimum acceleration at (s, ˙ s)

◮ β(s, ˙

s) is the maximum acceleration at (s, ˙ s)

◮ If α(s, ˙

s) > β(s, ˙ s) : no possible acceleration ¨ s

◮ Maximum velocity curve defined by α(s, ˙

s) = β(s, ˙ s)

6 / 16

Motivations Time-optimal path parameterization algorithm Trajectory smoothing using time-optimal shortcuts

Phase plane (s, ˙ s) integration

0.0 0.2 0.4 0.6 0.8 1.0

s

0.0 0.5 1.0 1.5 2.0 2.5 3.0

˙ s

maximum acceleration minimum acceleration maximum velocity curve

◮ “Bang-bang” behavior, switch points can be found very efficiently ◮ Computation time O(n2N)

◮ n : number of dofs ◮ N : number of time-discretization steps

◮ Example: n = 4, N = 500 takes ∼ 2s in Python

7 / 16

Motivations Time-optimal path parameterization algorithm Trajectory smoothing using time-optimal shortcuts

Global time-optimal algorithm

◮ Find the time-optimal trajectory between given initial and final

configurations

◮ Generate paths by grid search and apply the path parameterization

algorithm on each path

Shiller and Dubowsky, 1991

8 / 16

Motivations Time-optimal path parameterization algorithm Trajectory smoothing using time-optimal shortcuts

Outline

Motivations Time-optimal path parameterization algorithm Trajectory smoothing using time-optimal shortcuts

8 / 16

Motivations Time-optimal path parameterization algorithm Trajectory smoothing using time-optimal shortcuts

Trajectory smoothing using time-optimal shortcuts

◮ Grid search does not work in higher dimensions (dof > 3) ◮ RRT works well in high-dof, cluttered spaces, but produces non optimal

trajectories

Karaman and Frazzoli, 2011

◮ Post-process with shortcuts, e.g. Hauser and Ng-Thow-Hing 2010

(acceleration and velocity limits)

◮ Here we propose to use time-optimal shortcuts with torque limits

9 / 16

Motivations Time-optimal path parameterization algorithm Trajectory smoothing using time-optimal shortcuts

Time-optimal shortcuts

10 / 16

Motivations Time-optimal path parameterization algorithm Trajectory smoothing using time-optimal shortcuts

Simulation results

RRT trajectory before shortcutting

0.0 0.5 1.0 1.5 2.0 Time (s)

0.0 0.5 1.0 1.5 2.0 2.5 3.0 Joint angles (rad)

11 / 16

Motivations Time-optimal path parameterization algorithm Trajectory smoothing using time-optimal shortcuts

Simulation results

Joint angles profiles after ... 0 shortcut

0.0 0.5 1.0 1.5 2.0 Time (s)

0.0 0.5 1.0 1.5 2.0 2.5 3.0 Joint angles (rad)

12 / 16

Motivations Time-optimal path parameterization algorithm Trajectory smoothing using time-optimal shortcuts

Simulation results

Joint angles profiles after ... 1 shortcut

0.0 0.5 1.0 1.5 2.0 Time (s)

0.0 0.5 1.0 1.5 2.0 2.5 3.0 Joint angles (rad)

12 / 16

Motivations Time-optimal path parameterization algorithm Trajectory smoothing using time-optimal shortcuts

Simulation results

Joint angles profiles after ... 2 shortcuts

0.0 0.5 1.0 1.5 2.0 Time (s)

0.0 0.5 1.0 1.5 2.0 2.5 3.0 Joint angles (rad)

12 / 16

Motivations Time-optimal path parameterization algorithm Trajectory smoothing using time-optimal shortcuts

Simulation results

Joint angles profiles after ... 3 shortcuts

0.0 0.5 1.0 1.5 2.0 Time (s)

0.0 0.5 1.0 1.5 2.0 2.5 3.0 Joint angles (rad)

12 / 16

Motivations Time-optimal path parameterization algorithm Trajectory smoothing using time-optimal shortcuts

Simulation results

Joint angles profiles after ... 4 shortcuts

0.0 0.5 1.0 1.5 2.0 Time (s)

0.0 0.5 1.0 1.5 2.0 2.5 3.0 Joint angles (rad)

12 / 16

Motivations Time-optimal path parameterization algorithm Trajectory smoothing using time-optimal shortcuts

Simulation results

Joint angles profiles after ... 5 shortcuts

0.0 0.5 1.0 1.5 2.0 Time (s)

0.0 0.5 1.0 1.5 2.0 2.5 3.0 Joint angles (rad)

12 / 16

Motivations Time-optimal path parameterization algorithm Trajectory smoothing using time-optimal shortcuts

Simulation results

Joint angles profiles after ... 6 shortcuts

0.0 0.5 1.0 1.5 2.0 Time (s)

0.0 0.5 1.0 1.5 2.0 2.5 3.0 Joint angles (rad)

12 / 16

Motivations Time-optimal path parameterization algorithm Trajectory smoothing using time-optimal shortcuts

Simulation results

Torques profiles of the final trajectory

0.0 0.2 0.4 0.6 0.8 1.0

Time (s)

5 10 15

Torque (Nm)

13 / 16

Motivations Time-optimal path parameterization algorithm Trajectory smoothing using time-optimal shortcuts

Computation time

◮ Time limit for one rep : 15s

◮ ∼100 shortcuts attempts ◮ ∼6 effective shortcuts

◮ No significant improvement after ∼7 effective shortcuts ◮ Choosing the best out of 10 reps (computation time: 2min30s)

approaches the best out of 100 reps (computation time: 25min) by a margin of 9%

14 / 16

Motivations Time-optimal path parameterization algorithm Trajectory smoothing using time-optimal shortcuts

Discussion

◮ Limitations

◮ No guarantee of global optimality, but works well in practice ◮ Torque jumps ⇒ third-order optimization (motor model)

◮ Directions of research

◮ Heuristic for choosing the endpoints of the random shortcuts? ◮ Heuristic to choose the shortcut path between two given endpoints? ◮ Robust integration of velocity limits 15 / 16

Motivations Time-optimal path parameterization algorithm Trajectory smoothing using time-optimal shortcuts

Conclusion

◮ We have presented an efficient algorithm for planning time-optimal

trajectories in high-dimension, cluttered environments

◮ We did so by combining three ideas

• 1. Randomized motion planning (e.g. RRT)
• 2. Trajectory smoothing by shortcuts
• 3. Time-optimal path parameterization algorithm

◮ Thank you very much for your attention, questions and comments !

16 / 16