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

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

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

Manipulator Dynamic Equations The third joint is a passive joint.

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

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!

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

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.

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

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

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

Example

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

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

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

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)

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

Result

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

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!