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

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

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

4. 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

5. 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]

6. 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

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

8. 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

9. Compliant Motion Tasks

10. 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

11. 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

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

13. 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

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

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

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

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

18. Integration Scheme Apply the Trapezoidal Rule on integrating L2
brackets

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

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

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

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

23. 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

24. 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

25. 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

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

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

28. Example: WAM (cont.)

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

34. 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)

35. Thanks and Questions