Some old and new algorithms for robot motion planning Quang-Cuong Pham Department of Mechano-Informatics University of Tokyo October 16th, 2012 Robotics Laboratory Seoul National University
Outline Time-optimal control under dynamics constraints Time-optimal path parameterization algorithm Trajectory smoothing using time-optimal shortcuts Critically dynamic motion planning for humanoid robots Affine trajectory deformation Motivation Proposed algorithm Examples of application
Outline Time-optimal control under dynamics constraints Time-optimal path parameterization algorithm Trajectory smoothing using time-optimal shortcuts Critically dynamic motion planning for humanoid robots Affine trajectory deformation Motivation Proposed algorithm Examples of application
Time-optimal path parameterization algorithm Time-optimal control under dynamics constraints Trajectory smoothing using time-optimal shortcuts Affine trajectory deformation Critically dynamic motion planning for humanoid robots Path parameterization algorithm ◮ Time-optimal path parameterization under torque limits algorithm (Bobrow et al 1985, and many others) ◮ 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 ] ◮ Output : the time parameterization s : [0 , T ] − → [0 , L ] t �− → s ( t ) that minimizes the traversal time T 3 / 35
Time-optimal path parameterization algorithm Time-optimal control under dynamics constraints Trajectory smoothing using time-optimal shortcuts Affine trajectory deformation Critically dynamic motion planning for humanoid robots Transforming the manipulator equations ◮ Let’s express the manipulator equations in terms of s , ˙ s , ¨ s ◮ Differentiate q with respect to s q = q s ˙ ˙ s s 2 q = q s ¨ ¨ s + q ss ˙ ◮ Substitute in the manipulator equation to obtain � � M ( q ( s )) q ss ( s ) + q s ( s ) ⊤ C ( q ( s )) q s ( s ) M ( q ( s )) q s ( s )¨ s + s + g ( q ( s )) ˙ = τ ( s ) which can be rewritten as s 2 + c ( s ) = τ ( s ) a ( s )¨ s + b ( s )˙ 4 / 35
Time-optimal path parameterization algorithm Time-optimal control under dynamics constraints Trajectory smoothing using time-optimal shortcuts Affine trajectory deformation Critically dynamic motion planning for humanoid robots Transforming the torque constraints ◮ The torque constraints become s 2 + c i ( s ) ≤ τ max τ min ≤ a i ( s )¨ s + b i ( s )˙ i i ◮ Set of 2 n inequalities s 2 − c i ( s ) s 2 − c i ( s ) τ min s ≤ τ max − b i ( s )˙ − b i ( s )˙ i i ≤ ¨ a i ( s ) a i ( s ) ◮ Minimal time = high ˙ s ◮ Let’s go to the phase plane ( s , ˙ s ) . . . 5 / 35
Time-optimal path parameterization algorithm Time-optimal control under dynamics constraints Trajectory smoothing using time-optimal shortcuts Affine trajectory deformation Critically dynamic motion planning for humanoid robots 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 (Pfeiffer and Johanni 1987, Slotine and Yang 1989, Shiller and Lu 1992) 6 / 35
Time-optimal path parameterization algorithm Time-optimal control under dynamics constraints Trajectory smoothing using time-optimal shortcuts Affine trajectory deformation Critically dynamic motion planning for humanoid robots Global time-optimal algorithm ◮ Generate paths by grid search and apply the path parameterization algorithm Shiller and Dubowsky, 1991 7 / 35
Time-optimal path parameterization algorithm Time-optimal control under dynamics constraints Trajectory smoothing using time-optimal shortcuts Affine trajectory deformation Critically dynamic motion planning for humanoid robots 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 and velocity limits 8 / 35
Time-optimal path parameterization algorithm Time-optimal control under dynamics constraints Trajectory smoothing using time-optimal shortcuts Affine trajectory deformation Critically dynamic motion planning for humanoid robots Time-optimal shortcuts Pham, Asian MMS , 2012 9 / 35
Time-optimal path parameterization algorithm Time-optimal control under dynamics constraints Trajectory smoothing using time-optimal shortcuts Affine trajectory deformation Critically dynamic motion planning for humanoid robots Simulation results Velocity profiles Torque profiles Pham, Asian MMS , 2012 10 / 35
Time-optimal path parameterization algorithm Time-optimal control under dynamics constraints Trajectory smoothing using time-optimal shortcuts Affine trajectory deformation Critically dynamic motion planning for humanoid robots Critically dynamic motion planning for humanoid robots ◮ “ZMP is defined as that point on the ground at which the net moment of the inertial forces and the gravity forces has no component along the horizontal axes” (Vukobratovic, 1969) ◮ Condition for dynamic balance: ZMP contained in the support area P R Vukobratovic and Borovac, IJHR , 2004 11 / 35
Time-optimal path parameterization algorithm Time-optimal control under dynamics constraints Trajectory smoothing using time-optimal shortcuts Affine trajectory deformation Critically dynamic motion planning for humanoid robots Optimal time parameterization ◮ ZMP equation � z i + g ) x i − � x i z i − � i m i (¨ i m i ¨ i ( M i ) y x ZMP = , � i m i (¨ z i + g ) ◮ Express as a function of joint angles x i = r ( q ) ◮ Differentiating yields q ⊤ r qq ˙ x i = r q ˙ q x i = r q ¨ q + ˙ q ◮ Parameterize q by a path parameter s as q = q ( s ) ◮ This gives s 2 ˙ ¨ q = q s ˙ s q = q s ¨ s + q ss ˙ 12 / 35
Time-optimal path parameterization algorithm Time-optimal control under dynamics constraints Trajectory smoothing using time-optimal shortcuts Affine trajectory deformation Critically dynamic motion planning for humanoid robots Optimal time parameterization (continued) ◮ Replacing yields s + ( r q q ss + q ⊤ s 2 x i = ( r q q s )¨ s r qq q s )˙ ◮ Thus ¨ x i can be expressed as s 2 , ¨ x i = a x i ( s )¨ s + b x i ( s )˙ ◮ Finally one can express s 2 + c ( s ) x ZMP = a ( s )¨ s + b ( s )˙ s 2 + mg d ( s )¨ s + e ( s )˙ ◮ This last expression can be treated by a Bobrow-like algorithm ◮ One can run the Bobrow algorithm to find the optimal parameterization s and which verifies x min ≤ x ZMP ≤ x max ≤ ≤ y min y ZMP y max 13 / 35
Time-optimal path parameterization algorithm Time-optimal control under dynamics constraints Trajectory smoothing using time-optimal shortcuts Affine trajectory deformation Critically dynamic motion planning for humanoid robots Simulation results ZMP in space ZMP as function of time Phase-space ( s , ˙ s ) 0.15 0.10 Y-axis (medio-lateral) (m) 0.05 0.00 � 0.05 � 0.10 � 0.15 � 0.20 � 0.2 � 0.1 0.0 0.1 0.2 X-axis (antero-posterior) (m) Pham and Nakamura, Humanoids , 2012 14 / 35
Outline Time-optimal control under dynamics constraints Time-optimal path parameterization algorithm Trajectory smoothing using time-optimal shortcuts Critically dynamic motion planning for humanoid robots Affine trajectory deformation Motivation Proposed algorithm Examples of application
Motivation Time-optimal control under dynamics constraints Proposed algorithm Affine trajectory deformation Examples of application Why deform trajectories ? ◮ To deal with perturbations ◮ To retarget motion-captured motions ◮ Advantages ◮ save time ◮ natural-looking motions 15 / 35
Motivation Time-optimal control under dynamics constraints Proposed algorithm Affine trajectory deformation Examples of application Existing approaches ◮ Spline-based (e.g. Lee and Shin, SIGGRAPH, 1999) � q → q + a i s i ◮ Dynamic-system-based (e.g. Ijspeert et al, ICRA, 2002) � q = f ( q , ˙ a i ψ i ) , q solution of a i → a ′ i 16 / 35
Motivation Time-optimal control under dynamics constraints Proposed algorithm Affine trajectory deformation Examples of application Existing approaches (continued) Drawback: extrinsic basis functions ( s i , ψ i ) ⇒ artefacts ◮ loss of smoothness (splines that undulate too much,. . . ) ◮ undesirable frequencies, phase-shift ◮ loss of invariance ◮ ⇒ computational effort to reduce artefacts, preserve invariance ◮ need of reintegration 17 / 35
Motivation Time-optimal control under dynamics constraints Proposed algorithm Affine trajectory deformation Examples of application Affine geometry and invariance ◮ Affine transformation : R n → R n x �→ u + M ( x ) ◮ Affine transformations : group of dimension n + n 2 ◮ Affine invariance : properties preserved by this group ◮ non-affine-invariant : euclidean distance, angle, circles, euclidean velocity,. . . ◮ affine-invariant : straight lines, parallelism, midpoints, ellipses, affine distance, affine velocity,. . . 18 / 35
Recommend
More recommend
