Description
In this assignment, you will develop a path planning algorithm for systems with
differential constraints, specifically a car-like system.
Dubins car: Recall that the configuration of a simple car is given by q = (x, y, θ),
where (x, y) is the position of the center of its rear axle and θ is its orientation. Let
s and φ denote the speed and the steering angle (negative when the front wheel is
turned to the right and positive when it is turned to the left). See Figure 1, taken from
the textbook.
The motion of the car can be described by the following configuration
transition equation
x˙ = s cos θ
y˙ = s sin θ
˙θ =
s
L
tan φ
Figure 1: The simple car with three degrees of freedom.
Consider the Dubins version of the simple car. We assume that the car moves at
constant speed s = 1 and φ ∈ [−φmax, φmax], where φmax ∈ (0, π/2). The minimum
turning radius is ρmin = L/ tan φmax = 0.5.
It can be shown that the shortest path
between any two configurations can always be characterized by a Dubins curve, which
is one of the following types: LRL, RLR, LSL, LSR, RSL, RSR. Here, L and R
corresponds to turning as sharply as possible to the left and right, respectively, while S
corresponds to driving the car straight.
The world and obstacle region: The car is operating in a 2D world W = [−3, 3] ×
[−1, 1] with obstacle region O shown in Figure 2. The obstacle region is defined by two
half circles centered at (0, −1) and (0, 1), respectively, both having radius 1 − dt, where
dt = 0.2. Anything within the two half circles are considered obstacle regions. (This
is similar to Homework 4 except that Homework 4 defines the C-space rather than the
world and dt is larger in this assignment.)
The initial configuration is qI = (−2, −0.5, 0) and the goal configuration is qG =
(2, −0.5, π/2).
Figure 2: The world and obstacles. The red half-circles are the obstacles. The blue and
red arrow represent the initial and goal configurations, respectively.
1. (20 points for COM S 4760, 10 points for COM S 5760) Path planning with RRT:
Use the single-tree search outlined in Section 14.4.3. You should check periodically
if the tree can be connected to qG. This can be easily done by setting α(i) as qG
with a certain probability p. For example, the book recommends p = 0.01. You
should have this as a parameter in your code. Once qG is successfully added to the
tree, quit the loop and compute the path from qI to qG. Plot both the resulting
tree and path. The result should be similar to Figure 3, which uses p = 0.1.
Figure 3: The result of RRT planning with p = 0.1. The black curves are the edges in
the tree. The black arrows are the configurations of the vertices. The blue curve is the
path from qI to qG extracted by RRT.
Hint:
(a) Sampling: As opposed to Homework 4, a configuration in this assignment
includes 3 values x, y and θ. Make sure you provide an appropriate range for
all of them.
(b) Dubins curves: There are existing libraries for computing the shortest
paths between any configurations for a Dubins car. Feel free to use any of
them. For example, if your code is in python, you may consider the dubins
library (https://pypi.org/project/dubins/). OMPL also includes the
ompl::base::DubinsStateSpace class (https://ompl.kavrakilab.org/classompl_
1_1base_1_1DubinsStateSpace.html), which allows you to compute the
shortest Dubins path between SE(2) configurations.
(c) Collision checking: Besides checking for collisions with the 2 half-circle
obstacles, for this assignment, you also need to check that each Dubins curve
remains within the world W = [−3, 3] × [−1, 1]. In Homework 4, this check
is not necessary because an edge between any 2 configurations are simply
a straight line and the configuration space is convex, so as long as all the
samples are within the configuration space, any line segment between them
is also within the configuration space.
(d) Approximate solutions for finding nearest points: For each edge, discretize the corresponding Dubins curve such that consecutive points are not
further than ∆q from each other. When finding the nearest point in the
swath S of G to α(i), you can simply go through these discretized states and
select one that is closest to α(i).
2. (COM S 5760 only, 10 points) Path planning with PRM:
Use the PRM algorithm described in Figure 5.25 in the textbook to solve the
same planning problem given above. As for the number of nodes N, try a different
value until you can solve the problem. When connecting to the nodes in the
neighborhood, use Nearest K and set K = 15.
Hint: As opposed to Homework 4, in this assignment, an edge from configuration
q2 to configuration q1 may not simply be the reverse of an edge from q1 to q2. This
is because a Dubins car can only move forward so it cannot simply traverse an
edge from q1 to q2 backward to obtain an edge from q2 to q1. The CONNECT function
for PRM, therefore, needs to perform collision checking for both the forward and
reverse edges.
Submission: Please submit a single zip file on Canvas containing the followings
• your code (with comments, explaining clearly what each function/class is doing),
• the plots from each task, similar to Figure 3, and
• a text file explaining clearly how to compile and run your code.