## Transcribed Text

Motion Planning for A Simple Car
In this project, you will develop a path planning algorithm for systems with differential constraints,
specifically a car-like system.
Part I. General Description
Simple Car
The state of a simple car is represented by (𝑥,𝑦,𝜃), where (𝑥,𝑦) is its position (the center of its real axis)
and 𝜃 its orientation (the direction it is pointing). See Figure 1, taken from the textbook by LaValle.
(𝜙 represents the direction the front wheels. It is negative when it is turned to the right, positive when
turned to the left. However, 𝜙 does not come into play in this homework).
The work space and obstacles
Consider the following work space and obstacles. The dimension of the space is 6 by 2. The obstacles are
defined by two half circles that center at (0, -1) and (0, 1) respectively, both having a radius of 1-dt,
where dt = 0.2 (All of these are the same as Project 3 except a different dt value is used here). Anything
within the two half circles are considered obstacle regions.
The initial configuration 𝑞𝐼 is at (-2, -0.5, 0), in red, and the goal configuration 𝑞𝐺 is at (2, -0.5, 𝜋/2),
which is in blue.
Part II. Solve the planning problem using RRT
Now solve the planning problem using RRT. Using the single-tree search outlined in sections 5.5.3 and
14.4.3. You should check periodically if the tree can be connected to 𝑞𝐺. This can be easily done by
setting 𝛼(𝑖) as 𝑞𝐺 with a certain probability. The book recommends 0.01. You could have this as an input
parameter to your program. The following figure shows how a path to goal is found (I used 0.2 for the
probability). As you can see, it is not optimal at all, but nevertheless it is a feasible path. The isolated
configurations (black triangles) in the figure are 𝛼(𝑖)’s that didn’t get connected to the graph. All edges
are in red. The final path from start to goal is in blue.
Here is another solution:
In applying RRT, you would need the following (see section 14.3):
In applying RRT, you would need the following (see section 14.3):
1- a way to sample the state space. You should sample 𝜃 in the range of [0, 2𝜋).
2- a distance metric that can determine the nearest neighbor. This can be determined by which
configuration has the shortest driving distance to 𝛼(𝑖), ignoring obstacles.
3- a collision detection that checks if there is any collision. Currently, our car robot is just a point
with a direction (like pointed triangles shown in the figure above). So, collision checking is easy.
4- a local planner method (LPM) that returns a path that connects two vertices, or an empty path if
the two vertices cannot be connected.
The last component is a BVP problem and is often challenging. The figure below shows one way we can
connect two configurations (𝑥1, 𝑦1, 𝜃1) and (𝑥2, 𝑦2, 𝜃2). We first extend lines along the directions they are
pointing. This will give us an intersection point (𝑥𝑚, 𝑦𝑚). Assuming without loss of generality that the
distance between (𝑥𝑚, 𝑦𝑚) and (𝑥2, 𝑦2) is greater. Find another point (𝑥2′, 𝑦2′) that is equidistant to
(𝑥𝑚, 𝑦𝑚) as (𝑥1,𝑦1) (see Figure below). Draw two dashed lines perpendicular to the two-line segments as
shown. The intersection of the two dashed lines are the turning center. 𝑟 is the turning radius. Finally,
the path connecting configurations (𝑥1,𝑦1,𝜃1) to (𝑥2,𝑦2,𝜃2) is the arc path from (𝑥1,𝑦1) to (𝑥2′,𝑦2′) ,
followed the line segment from (𝑥2′,𝑦2′) to (𝑥2,𝑦2). And it is a reverse motion. There are two conditions
which may invalidate this path. One is when the car is pointing at opposite directions at configurations
(𝑥1,𝑦1,𝜃1) to (𝑥2,𝑦2,𝜃2) .The other is that 𝑟 < 𝑟_min, the minimum turning radius. For your convenience, I
have included my MATLAB implementation of such a local planner (called LPM.m). It takes four
parameters, the first two of which are the two configurations to be connected. The third and fourth
parameters are minimum turning radius rMin and stepSize (the step size taken in discretizing a path)
respectively. For this project, use rMin=0.4, stepSize = 0.02. The LPM.m script returns a sequence of
configurations, a path, if it exists.
Part III. Use PRM (section 5.6) to solve the planning problem.
Please 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.

These solutions may offer step-by-step problem-solving explanations or good writing examples that include modern styles of formatting and construction
of bibliographies out of text citations and references. Students may use these solutions for personal skill-building and practice.
Unethical use is strictly forbidden.

function [path] = arcPath(q1, mid, q2, rMin, resolution)

% compute an arc that is tangent to line segment (q1, mid) at q1 and line segment (mid, q2) at q2.

% assuming q1 and q2 have the same distance to mid.

% rMin: minimum turning radius

% resolution: path resolution

% path: a discretized arc path segment between q1 and q2. It is empty if the radius of the arc is less than rMin.

r1 = q1(1:2) - mid;

r2 = q2(1:2) - mid;

pc = (q1(1:2)+q2(1:2))/2;

rc = pc - mid;

%theta = asin(norm(rc)/norm(r1));

dist2center = norm(r1)^2/norm(rc);

center = mid + rc/norm(rc)*dist2center;

radius = norm(q1(1:2)-center);

if radius < rMin

path = [];

return;

end

dp = [q1(1:2) - center; q2(1:2) - center];

theta = atan2(dp(:,2), dp(:,1));

dt = mod(theta(2) - theta(1), 2*pi);

if dt >= pi

dt = dt-2*pi;

end

deltaTheta = resolution/radius;

steps = ceil...