Newton’s Method for Constrained Variational Problems with Applications to Robot Path Planning Yueshi Shen Dept. of Information Engineering RSISE, Australian National University

Talk Outline Introduction Problem description Optimal path planning under motion constraints Example: WAM Future work and outlook

Talk Outline Introduction Problem description Optimal path planning under motion constraints Example: WAM Future work and outlook

Robot Path Planning Three sub-problems in automatic task executions for multi-body robotic systems P1 Plan an end-effector path p(t) in the task space P2 Find the corresponding joint trajectory q(t) P3 Design a feedback control law

Literature Review Collision-free path planning for a single rigid object [Latombe 1991] Optimal end-effector path tracking [Martin et al 1989], [Yoshikawa 1990], [Agrawal & Xu 1994] Direct joint trajectory planning with respect to dynamical optimality [Singh & Leu 1991], [Wang & Hamam 1992], [Wang et al 2001] Formulate P1,2,3 as an optimal control problem [Cahill et al 1998], [Lo Bianco & Piazzi 2002]

Our Philosophy Try to solve sub-problems P1, P2 in one attempt Eliminate the necessity of computing the robot’s feasible configuration space Aim at some synthetically (joint and end-effector) geometrical optimality Robot kinematic model incorporated in the cost function

Talk Outline Introduction Problem description Optimal path planning under motion constraints Example: WAM Future work and outlook

Problem Description Find a sufficiently smooth q-dim joint trajectory which minimizes the cost function J End-effector’s position/orientation, linear/angular velocity can be expressed in L1

Compliant Motion Tasks

Problem Description (cont.) Furthermore, the manipulator is subject to l end-effector constraints Also, there are (m-l) inequality constraints, e.g., mechanical stops, obstacle avoidance

Problem Description (cont.) Applying the Lagrange multipliers, the previous constrained optimization problem is equivalent to Using the calculus of variations, the corresponding Euler-Lagrange equation becomes

Talk Outline Introduction Problem description Optimal path planning under motion constraints Example: WAM Future work and outlook

Trajectory Planning Scheme Step 1 (numerical trajectory optimization): Compute the optimal trajectory q(t)’s discrete intermediate points qk Step 2 (interpolation): Interpolating qk to get a sufficiently smooth q(t) such that the end-effector constraints are still fulfilled

Newton’s Method for Variational Problems (overview)

Newton’s Method for Variational Problems (overview cont.)

Discretization Scheme Discretize [t0, tn] by regular partition We define

Newton’s Method for Variational Problems (overview cont.)

Integration Scheme Apply the Trapezoidal Rule on integrating L2 brackets

Integration Scheme (cont.) Apply the Mid-point Rule on integrating L2 brackets

Newton’s Method for Variational Problems (overview cont.)

Approximation Scheme can all be approximated by , and the boundary values of need special treatment

Newton’s Method for Variational Problems (overview cont.)

Algorithm 1 (Numerical Trajectory Optimization) Step 1: pick a reasonable guess of Q (Q:={qk,µk}) Step 2: Update Q by the following law: Keep applying step 2 until is small enough Absolute value

Algorithm 1, Modified (Numerical Trajectory Optimization) Mod. 1: Gradually increase the number of time partitions n to make initial guesses more efficient Mod. 2: Introduce the step size δ to improve the numerical stability: δ should satisfy the Armijo condition and can be calculated by the backtracking line search

Algorithm 2 (Interpolation) Step 1: Interpolate qk by a cubic spline qorg(t) Step 2: Interpolate pk by a smooth curve p(t) on the working surface [Hüper & Silva Leite 2002] Step 3: Repeatedly adjust qorg(t) to fit p(t) by

Talk Outline Introduction Problem description Optimal path planning under motion constraints Example: WAM Future work and outlook

Example: WAM WAM is a 4-degree-of-freedom robot manipulator with 4 revolute joints WAM has human- like kinematics

Example: WAM (cont.)

Talk Outline Introduction Problem description Optimal path planning under motion constraints Example: WAM Future work and outlook

Future Work and Outlook Unify and under one discretization scheme Extensions to non-holonomic constraints Interpolation for rotation group SO3 A joint-space control algorithm for manipulator’s compliant motion control (to be presented at IEEE-ICMA 2005)

Thanks and Questions