Intelligent Robotics

05. Motion Planning (Part 1)

Daeun Song
Department of Artificial Intelligence
Ewha Womans University
songd@[Link]
1
Review: Forward Kinematics

▪ Forward Kinematics (FK): given a robot’s configuration q and its kinematic

structure, compute the world-frame coordinate of every link
• Input: joint angles/translations q = (q1, q2, ⋯, qn)
0
• Output: transformation matrices 1T(q), 02T(q), ⋯, 0nT(q) for every link

2
Review: Inverse Kinematics
▪ Inverse Kinematics (IK): given a robot’s end effector pose x, compute the
robot’s joint configuration q
• Input: desired end-effector pose x*
• Output: joint angles/translations q = (q1, q2, ⋯, qn), such that x* = f(q)

3
What is Motion Planning?
▪ Motion Planning: The use of computational methods to generate a robot’s
motion to achieve a specified task.
• Input: robot’s state, environmental conditions, dynamic predictive models, and
task specifications
• Output: low-level motion representation (sent to controller)

4
Review: Configuration Space

▪ Configuration Space (C-Space): A configuration space C is a set of all

possible configurations of the robot
C ⊆ ℝn, n = DoF, q = (q1, q2, …, qn)

• Every point q ∈ C corresponds to one robot pose

• The structure of C is determined by the robot’s degrees of freedom (DoF)

[Link] 5
Review: Configuration Space
▪ Why C-Space?
• In C-space, the robot is treated as a point moving through a free space F
• All geometric complexity of the robot is “absorbed” into the shape of Cobs
• Motion planning becomes:
find a continuous path y : [0,1] → F such that y(0) = qI and y(1) = qG
Cobs

6
Kinematic vs. Kinodynamic Planning
▪ Kinematic (Path) Planning
• Find a path y : [0,1] → F
• Constraints: collision avoidance, joint limits
▪ Kinodynamic Planning
• Find a trajectory q(t) satisfying dynamics x· = f(x, u)
• Required for: agile drones, cars, under actuated robots

Kinematic (Path) Planning Kinodynamic Planning

Output Path y : [0,1] → F Trajectory q(t)
Considers Geometry, joint limits, collisions + velocity, acceleration, forces
Ignores Time, dynamics Nothing
Analogy Route on a map Driving schedule with speed limits
Difficulty Tractable Much harder
7
Motion Planning Problem Formulation
▪ Given:
• Configuration space C ⊆ ℝn
• Start configuration qI ∈ F ●
• Goal configuration qG ∈ F ●
• Free space F = C − Cobs

▪ Find:
• A continuous path y such that
y : [0,1] → F
y(0) = qI, y(1) = qG, y(s) ∈ F ∀ s ∈ [0,1]

8
Path Representation
▪ A path is a continuous function, but we need a finite representation
▪ Waypoint Sequence:
• Store a finite sequence of configurations q0, q1, ⋯, qk
• Interpolate between consecutive waypoints
• q0 = qI, qk = qG, qi ∈ F ∀i
• Simple and Intuitive, but, potentially jerky at waypoints (no smoothness
guaranteed

9
Path Representation
▪ A path is a continuous function, but we need a finite representation
▪ Polynomial / Spline:
• Polynomial coefficients derived from control points
• Enforces C 1 or C 2 continuity at waypoints
• Used when smooth motion is required (e.g. robot arms)

10
Classical Planners
▪ Discretization-based: represent space with a regular grid (Grid Search)
▪ Geometry-based: use obstacle geometry to build a search structure
• Visibility Graph: graph of collision-free straight-line connections
• Cell Decomposition: graph of connected free-space regions

11
Bug Algorithm
▪ Planning with local information only
▪ Key Idea:
• No map required, only local contact sensing + small memory
• Simple reactive rules

12
Bug Algorithm
▪ Bug1:

X Algorithm Bug 1
1. Move in a straight line path toward G.
I
2. If the robot reaches G, we are done.
G
3. If the robot hits an obstacle, then follow the
right-hand rule and traverse its boundary
4. This continues until the wall no longer is
pointing against the straight line from X
toward G. Once this occurs, continue from
Step 1.

13
Bug Algorithm
▪ Bug2:
Algorithm Bug 2
1. Move in a straight line path toward G.

2. If the robot reaches G, we are done.

I P 3. If the robot hits an obstacle, remember the
G point P at which it is hit. Follow the right
hand-rule.
4. If the robot crosses the line PG as it walks
along the wall, determine whether ||X - G|| <
||P - G|| and the wall is not pointing against
P the straight line from X toward G. If both
conditions are satis ed, continue from step
1. Otherwise, keep walking.
5. If P is reached again, return “No path”.

14
fi
Bug Algorithm
▪ Bug1 and Bug2 comparison
Bug1 Bug2
Boundary traversal Partial boundary Full boundary
Path length Generally shorter Generally longer
Worst case Can be worse than Bug2 More robust
Optimality Not optimal Not optimal

Bug1 Bug2 15
Grid Search
▪ Key Idea:
• Discretize C into a uniform grid with resolution h
• Build graph G(V, E): V (vertices) = grid points, E (edges) = collision-free connections
• Run graph search (Dijkstra/A*) on G

Searching with 4-connected grid points 16

Grid Search
▪ Key Idea:
• Discretize C into a uniform grid with resolution h
• Build graph G(V, E): V (vertices) = grid points, E (edges) = collision-free connections
• Run graph search (Dijkstra/A*) on G

Searching with 8-connected grid points 17

Grid Search
▪ Grid Search Algorithms

Dijkstra A*
Strategy Expand lowest cost so far Expand lowest f(n) = g(n) + h(n)
Heuristic None h(n) = Euclidean distance to the goal
Nodes expanded Many (explores all directions) Fewer (biased toward goal)
Optimality Optimal Optimal (if h is admissible)
Speed Slower Faster
18
Grid Search
▪ Free Space Sampling
▪ Only grid points in F becomes vertices
▪ Edges crossing Cobs are removed
▪ Resulting graph approximates the connectivity of F
▪ Finer grid (smaller h) → better approximation, but more vertices:

(h)
n
1
|V| = O

19
Grid Search
▪ Limitations
• Grid points may miss the goal or narrow passages entirely if resolution h is too coarse
• Grid search finds the shortest path on the grid, not in continuous F
• True shortest path is curved - grid path is always axis-aligned → suboptimal
• Curse of Dimensionality: Total gird points grow exponentially with DoF
k points per axis, n dimensions → total grid points: k n

20
Visibility Graph
▪ Key Idea:
▪ Shortest path in polygonal environment passes through obstacle corners (vertices)
▪ Build graph G connecting all mutually visible points
▪ Run graph search (Dijkstra/A*) on G

Vertex set V all-collision-free edges E Shortest path via Dijkstra

21
Visibility Graph
▪ Algorithm:
1. V ← {q_I, q_G} ∪ {obstacle vertices}
2. E ← {}
3. For all pairs u, v ∈ V:
4. if uv is obstacle edge or collision-free:
5. add (u, v) to E
6. Search G = (V, E) with cost = Euclidean distance
7. Return path

22
Cell Decomposition
▪ Key Idea:
▪ Decomposes F into simple convex cells
▪ Build connectivity graph between adjacent cells
▪ Path = sequence of cells from qI’s cell to qG’s cell

Trapezoidal Decomposition 23
Cell Decomposition
▪ Trapezoidal Decomposition
▪ Sweep a vertical line left → right through all obstacle vertices
▪ 4 possible events at each vertex: continue / split / birth / merge
▪ Output: trapezoids (convex) + triangles (degenerate trapezoids)

Trapezoidal Decomposition 24
Cell Decomposition
▪ Query Phase
▪ Locate cells containing qI and qG
▪ Search adjacency graph for connected cell sequence
▪ Connect midpoints of shared cell boundaries → collision-free path

Query phase 25
Demo
▪ 3D A* Motion Planning for 3-Link Robot Arm

[Link] 26