1. Collision-Free Trajectory Planning for a 3-DOF Robot with a Passive Joint
Paper by Kevin M.Lynch, Naoji Shiroma, Hirohiko Arai, and Kazuo Tanie
Jinwhan Kim
Gregory Kron

2. Introduction
A lot of research so far has focused on the design and control of underactuated manipulators in free space
This research focuses on
Fast collision-free trajectory planning among obstacles for a planar 3-DOF underactuated manipulator

3. Overview : Nonholonomic system
Nonholonomic systems typically arise in the following classes of systems
1. No-slip constraint
: mobile robots, any systems with pure rolling condition
Conservation of angular momentum
: free floating multibody systems
Underactuated mechanical systems
: most types of vehicles, actuators with passive joints
 Second-order nonholonomic constraints

4. Manipulator Dynamic Equations
The third joint is a passive joint.

5. Manipulator Dynamic Equations (cont.)
Equations of motion for the third link in Cartesian coordinates
Second-order nonholonomic constraint

6. Nonholonomic Constraints
Constraint equations in “Configuration Space(Rn)”
The constraints above are said to be second-order nonholonomic if the constraints are not integrable.
Representation in “State Space(R2n)”
This looks pretty familiar!

7. State-space representation
N second-order
differential equations
2N first-order
differential equations

8. Controllability of the Robot
Proposition 1
“If there exists a free trajectory from (Θ0T,0T)T to (Θ1T,0T)T and for the fully actuated robot, then there exists a free trajectory from (Θ0T,0T)T to (Θ1T,0T)T for the robot with a passive third joint.”
This follows directly from small-time local controllability.
(Small-time) local controllability
For any state (ΘT,0T)T and any neighborhood W of (ΘT,0T)T, (ΘT,0T)T is interior to the set of states reachable by trajectories remaining in W.

9. Controllability Rank Condition
Lie bracket
Two vector fields and four Lie bracket terms span
the six-dimensional state-space.

10. (Small-time) Local Controllability
The third link of the robot is small-time locally controllable at all robot configurations Θ ∈Cfree.
Small-time local controllability of the third link directly implies small-time local controllability of the robot at all zero velocity state.
Any path in Cfree can be followed arbitrary closely, but arbitrary paths may make extensive use of Lie bracket motions, resulting in slow execution times.

11. Where we are... Small-time local controllable robot
We want a collision-free trajectory planner
Computationally efficient and quick trajectories
use of Lie brackets -> going back to zero velocity

12. Example

13. Algorithm initialize T, OPEN with link start configuration xinit
while OPEN not empty
x <- first in OPEN, remove from OPEN
if x is in the goal region
report success
if x is not near a previously occupied configuration
mark x occupied
for each of +v1, -v1, +v2, -v2
integrate forward a time dt to xnew
calculate by inverse kinematics
if path to is collision-free
make xnew a successor to x in T
compute cost of path to new config xnew
place xnew in OPEN, sorted by cost
report failure

14. Ideas of the trajectory planner
Trajectory planning in the six-dimensional state space can be decoupled into path planning in the three-dimensional configuration space followed by time-optimal time scaling of the path
If the path planner is complete, then a trajectory will be found for any two configurations in the same connected component of Cfree
The paths found by the kinematic motion planner can be executed at high speeds

15. Decoupling Motions Time scaling on a trajectory q(s(t))
Equilibrium controllability
Second-order nonholonomic constraints
Time scaling on a trajectory q(s(t))
v(q) velocity vector

16. Analysis Collision detection Parameters Complexity: O(mnc3)
at each new configuration
Parameters
integration step dt
grid resolution (c discretizations)
Complexity: O(mnc3)
Approximate solution (1/c3)

17. Time scaling
Compute the acceleration that yields minimal-time execution
deciding when to start decelerating...

18. Result

19. Thanks Microsoft for multi-format support
Result (video)
Thanks Microsoft for multi-format support

20. Conclusion
Existence of a free trajectory between two zero velo- city states in the same connected component of Cfree
Motion planning can be decoupled into path planning and time scaling
Path planner complete for dt sufficiently small
Minimizes the number of zero-speed times
Actually works!