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 q ⊤ C ( q )˙ M ( q )¨ q + ˙ q + g ( q ) = τ, ◮ Torque limits for each joint i τ min ≤ τ i ( t ) ≤ τ max i 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 ] − → s ( t ) 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 3.0 maximum velocity curve 2.5 2.0 1.5 s ˙ 1.0 maximum acceleration 0.5 minimum acceleration 0.0 0.0 0.2 0.4 0.6 0.8 1.0 s ◮ “Bang-bang” behavior, switch points can be found very efficiently ◮ Computation time O ( n 2 N ) ◮ n : number of dofs ◮ N : number of time-discretization steps ◮ Example: n = 4 , N = 500 takes ∼ 2 s 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 3.0 2.5 2.0 1.5 Joint angles (rad) 1.0 0.5 0.0 � 0.5 � 1.0 0.0 0.5 1.0 1.5 2.0 Time (s) 11 / 16

Motivations Time-optimal path parameterization algorithm Trajectory smoothing using time-optimal shortcuts Simulation results Joint angles profiles after ... 0 shortcut 3.0 2.5 2.0 1.5 Joint angles (rad) 1.0 0.5 0.0 � 0.5 � 1.0 0.0 0.5 1.0 1.5 2.0 Time (s) 12 / 16

Motivations Time-optimal path parameterization algorithm Trajectory smoothing using time-optimal shortcuts Simulation results Joint angles profiles after ... 1 shortcut 3.0 2.5 2.0 1.5 Joint angles (rad) 1.0 0.5 0.0 � 0.5 � 1.0 0.0 0.5 1.0 1.5 2.0 Time (s) 12 / 16

Motivations Time-optimal path parameterization algorithm Trajectory smoothing using time-optimal shortcuts Simulation results Joint angles profiles after ... 2 shortcuts 3.0 2.5 2.0 1.5 Joint angles (rad) 1.0 0.5 0.0 � 0.5 � 1.0 0.0 0.5 1.0 1.5 2.0 Time (s) 12 / 16

Motivations Time-optimal path parameterization algorithm Trajectory smoothing using time-optimal shortcuts Simulation results Joint angles profiles after ... 3 shortcuts 3.0 2.5 2.0 1.5 Joint angles (rad) 1.0 0.5 0.0 � 0.5 � 1.0 0.0 0.5 1.0 1.5 2.0 Time (s) 12 / 16

Motivations Time-optimal path parameterization algorithm Trajectory smoothing using time-optimal shortcuts Simulation results Joint angles profiles after ... 4 shortcuts 3.0 2.5 2.0 1.5 Joint angles (rad) 1.0 0.5 0.0 � 0.5 � 1.0 0.0 0.5 1.0 1.5 2.0 Time (s) 12 / 16

Motivations Time-optimal path parameterization algorithm Trajectory smoothing using time-optimal shortcuts Simulation results Joint angles profiles after ... 5 shortcuts 3.0 2.5 2.0 1.5 Joint angles (rad) 1.0 0.5 0.0 � 0.5 � 1.0 0.0 0.5 1.0 1.5 2.0 Time (s) 12 / 16

Motivations Time-optimal path parameterization algorithm Trajectory smoothing using time-optimal shortcuts Simulation results Joint angles profiles after ... 6 shortcuts 3.0 2.5 2.0 1.5 Joint angles (rad) 1.0 0.5 0.0 � 0.5 � 1.0 0.0 0.5 1.0 1.5 2.0 Time (s) 12 / 16

Motivations Time-optimal path parameterization algorithm Trajectory smoothing using time-optimal shortcuts Simulation results Torques profiles of the final trajectory 15 10 Torque (Nm) 5 0 � 5 � 10 � 15 0.0 0.2 0.4 0.6 0.8 1.0 Time (s) 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