CS计算机代考程序代写 algorithm Lecture 3

Lecture 3

Lecture 3. Safe Control and
Motion Planning

Changliu Liu
Assistant Professor
Robotics Institute

Carnegie Mellon University

What Have We Learned So Far?

• Dynamic models of vehicles

• Tracking control

Today

• Safe Control

• Motion Planning

• Search-based planning: A*

• Optimization-based planning: CFS

Safe Control

• Fundamental question: how to avoid obstacles?

• Two problems

• Avoiding static obstacles

• Avoiding dynamic obstacles

Safe Set
• The collection of all states that are collision-free.

• Need to consider the collision volume of the vehicle.

Cartesian space State space

XS
AAAB9HicbVDLSgMxFL1TX7W+qi7dBIvgqsyIoMuiG5cV7QPaodxJ0zY0kxmTTKEM/Q43LhRx68e482/MtLPQ1gOBwzn3ck9OEAuujet+O4W19Y3NreJ2aWd3b/+gfHjU1FGiKGvQSESqHaBmgkvWMNwI1o4VwzAQrBWMbzO/NWFK80g+mmnM/BCHkg84RWMlvxuiGVEUaXvWe+iVK27VnYOsEi8nFchR75W/uv2IJiGThgrUuuO5sfFTVIZTwWalbqJZjHSMQ9axVGLItJ/OQ8/ImVX6ZBAp+6Qhc/X3Roqh1tMwsJNZSL3sZeJ/Xicxg2s/5TJODJN0cWiQCGIikjVA+lwxasTUEqSK26yEjlAhNbanki3BW/7yKmleVD236t1fVmo3eR1FOIFTOAcPrqAGd1CHBlB4gmd4hTdn4rw4787HYrTg5DvH8AfO5w/0dZI0AAAB9HicbVDLSgMxFL1TX7W+qi7dBIvgqsyIoMuiG5cV7QPaodxJ0zY0kxmTTKEM/Q43LhRx68e482/MtLPQ1gOBwzn3ck9OEAuujet+O4W19Y3NreJ2aWd3b/+gfHjU1FGiKGvQSESqHaBmgkvWMNwI1o4VwzAQrBWMbzO/NWFK80g+mmnM/BCHkg84RWMlvxuiGVEUaXvWe+iVK27VnYOsEi8nFchR75W/uv2IJiGThgrUuuO5sfFTVIZTwWalbqJZjHSMQ9axVGLItJ/OQ8/ImVX6ZBAp+6Qhc/X3Roqh1tMwsJNZSL3sZeJ/Xicxg2s/5TJODJN0cWiQCGIikjVA+lwxasTUEqSK26yEjlAhNbanki3BW/7yKmleVD236t1fVmo3eR1FOIFTOAcPrqAGd1CHBlB4gmd4hTdn4rw4787HYrTg5DvH8AfO5w/0dZI0AAAB9HicbVDLSgMxFL1TX7W+qi7dBIvgqsyIoMuiG5cV7QPaodxJ0zY0kxmTTKEM/Q43LhRx68e482/MtLPQ1gOBwzn3ck9OEAuujet+O4W19Y3NreJ2aWd3b/+gfHjU1FGiKGvQSESqHaBmgkvWMNwI1o4VwzAQrBWMbzO/NWFK80g+mmnM/BCHkg84RWMlvxuiGVEUaXvWe+iVK27VnYOsEi8nFchR75W/uv2IJiGThgrUuuO5sfFTVIZTwWalbqJZjHSMQ9axVGLItJ/OQ8/ImVX6ZBAp+6Qhc/X3Roqh1tMwsJNZSL3sZeJ/Xicxg2s/5TJODJN0cWiQCGIikjVA+lwxasTUEqSK26yEjlAhNbanki3BW/7yKmleVD236t1fVmo3eR1FOIFTOAcPrqAGd1CHBlB4gmd4hTdn4rw4787HYrTg5DvH8AfO5w/0dZI0AAAB9HicbVDLSgMxFL1TX7W+qi7dBIvgqsyIoMuiG5cV7QPaodxJ0zY0kxmTTKEM/Q43LhRx68e482/MtLPQ1gOBwzn3ck9OEAuujet+O4W19Y3NreJ2aWd3b/+gfHjU1FGiKGvQSESqHaBmgkvWMNwI1o4VwzAQrBWMbzO/NWFK80g+mmnM/BCHkg84RWMlvxuiGVEUaXvWe+iVK27VnYOsEi8nFchR75W/uv2IJiGThgrUuuO5sfFTVIZTwWalbqJZjHSMQ9axVGLItJ/OQ8/ImVX6ZBAp+6Qhc/X3Roqh1tMwsJNZSL3sZeJ/Xicxg2s/5TJODJN0cWiQCGIikjVA+lwxasTUEqSK26yEjlAhNbanki3BW/7yKmleVD236t1fVmo3eR1FOIFTOAcPrqAGd1CHBlB4gmd4hTdn4rw4787HYrTg5DvH8AfO5w/0dZI0

Problem Statement
• Safe strategy

• Do nothing when the current state is
safe

• Push the state to go back to the interior
of the safe set when the current state is
on the boundary of the safe set

• Push the state toward the safe set
when the current state is outside the
safe set

Problem Statement
• How can we implement the safety

strategy automatically?

• We can also the energy function
approach!

• This time, we will assign low energy to
safe states (in ) and high energy to
unsafe states.

• Then we derive control law to maintain
the system in safe states.

𝒳S

Safe Control

• Design the energy function (called as safety index)

• Where is the safety margin and
computes the distance between the vehicle and the
obstacle.

• We need to make sure

ϕ(x) = dmin − d(x, O)

dmin d(x, O)

ϕ ≤ 0

Energy Functions

• For goal reaching, the energy function is

• For safety, the energy function is

• What are the differences?

V(x) =
1
2

∥x − G∥2

ϕ(x) = dmin − d(x, O)

Safe Control

• Step 1: compute the (nominal) control law to ensure .

• Step 2: at every time step, modify the nominal control signal to ensure
safety by solving the following optimization

V(xk+1) − V(xk) < 0 urk min uk ∥uk − u r k∥ s.t. ϕ(xk+1) ≤ 0 10 What would be the solution of this optimization? Single Integrator • Dynamics • Nominal control • Safety constraint • Two cases: • If , then we will set the final control as • If , then we will set the final control as , xk+1 = xk + Δt uk urk = kp(G − xk) ϕ(xk+1) = dmin − ∥xk + Δtuk − O∥ ≤ 0 ∥xk + Δtu r k − O∥ ≥ dmin uk = u r k ∥xk + Δtu r k − O∥ < dmin uk = u r k + Δuk Δuk = c(xk + Δtu r k − O) c = dmin − ∥xk + Δtu r k − O∥ Δt∥xk + Δturk − O∥ 11 min uk ∥uk − u r k∥ s.t. ϕ(xk+1) ≤ 0 Single Integrator 12 min uk ∥uk − u r k∥ s.t. ϕ(xk+1) ≤ 0 uk = kp(G − xk) + max {0, dmin − ∥xk + Δtkp(G − xk) − O∥ Δt∥xk + Δtkp(G − xk) − O∥ }(xk + Δtkp(G − xk) − O) Move toward goal Move away from obstacle (active only when close to the obstacle) Simulation 13 -10 -8 -6 -4 -2 0 2 4 6 8 10 -10 -8 -6 -4 -2 0 2 4 6 8 10 Single Integrator % Set up controller kp = 1; ur = @(x) kp*(goal-x); % Nominal control dr = @(x,ur) x+dt*ur-obstacle; c = @(x,ur) max(0,dmin-norm(dr(x,ur)))/norm(dr(x,ur))/dt; u = @(x) ur(x) + c(x,ur(x))*dr(x,ur(x)); % Simulation for k = 1:kmax x = single_integrator(x, u(x), dt); if norm(x-goal)AAAB9HicbVDLSgMxFL1TX7W+qi7dBIvgqsyIoMuiG5cV7QPaodxJ0zY0kxmTTKEM/Q43LhRx68e482/MtLPQ1gOBwzn3ck9OEAuujet+O4W19Y3NreJ2aWd3b/+gfHjU1FGiKGvQSESqHaBmgkvWMNwI1o4VwzAQrBWMbzO/NWFK80g+mmnM/BCHkg84RWMlvxuiGVEUaXvWe+iVK27VnYOsEi8nFchR75W/uv2IJiGThgrUuuO5sfFTVIZTwWalbqJZjHSMQ9axVGLItJ/OQ8/ImVX6ZBAp+6Qhc/X3Roqh1tMwsJNZSL3sZeJ/Xicxg2s/5TJODJN0cWiQCGIikjVA+lwxasTUEqSK26yEjlAhNbanki3BW/7yKmleVD236t1fVmo3eR1FOIFTOAcPrqAGd1CHBlB4gmd4hTdn4rw4787HYrTg5DvH8AfO5w/0dZI0AAAB9HicbVDLSgMxFL1TX7W+qi7dBIvgqsyIoMuiG5cV7QPaodxJ0zY0kxmTTKEM/Q43LhRx68e482/MtLPQ1gOBwzn3ck9OEAuujet+O4W19Y3NreJ2aWd3b/+gfHjU1FGiKGvQSESqHaBmgkvWMNwI1o4VwzAQrBWMbzO/NWFK80g+mmnM/BCHkg84RWMlvxuiGVEUaXvWe+iVK27VnYOsEi8nFchR75W/uv2IJiGThgrUuuO5sfFTVIZTwWalbqJZjHSMQ9axVGLItJ/OQ8/ImVX6ZBAp+6Qhc/X3Roqh1tMwsJNZSL3sZeJ/Xicxg2s/5TJODJN0cWiQCGIikjVA+lwxasTUEqSK26yEjlAhNbanki3BW/7yKmleVD236t1fVmo3eR1FOIFTOAcPrqAGd1CHBlB4gmd4hTdn4rw4787HYrTg5DvH8AfO5w/0dZI0AAAB9HicbVDLSgMxFL1TX7W+qi7dBIvgqsyIoMuiG5cV7QPaodxJ0zY0kxmTTKEM/Q43LhRx68e482/MtLPQ1gOBwzn3ck9OEAuujet+O4W19Y3NreJ2aWd3b/+gfHjU1FGiKGvQSESqHaBmgkvWMNwI1o4VwzAQrBWMbzO/NWFK80g+mmnM/BCHkg84RWMlvxuiGVEUaXvWe+iVK27VnYOsEi8nFchR75W/uv2IJiGThgrUuuO5sfFTVIZTwWalbqJZjHSMQ9axVGLItJ/OQ8/ImVX6ZBAp+6Qhc/X3Roqh1tMwsJNZSL3sZeJ/Xicxg2s/5TJODJN0cWiQCGIikjVA+lwxasTUEqSK26yEjlAhNbanki3BW/7yKmleVD236t1fVmo3eR1FOIFTOAcPrqAGd1CHBlB4gmd4hTdn4rw4787HYrTg5DvH8AfO5w/0dZI0AAAB9HicbVDLSgMxFL1TX7W+qi7dBIvgqsyIoMuiG5cV7QPaodxJ0zY0kxmTTKEM/Q43LhRx68e482/MtLPQ1gOBwzn3ck9OEAuujet+O4W19Y3NreJ2aWd3b/+gfHjU1FGiKGvQSESqHaBmgkvWMNwI1o4VwzAQrBWMbzO/NWFK80g+mmnM/BCHkg84RWMlvxuiGVEUaXvWe+iVK27VnYOsEi8nFchR75W/uv2IJiGThgrUuuO5sfFTVIZTwWalbqJZjHSMQ9axVGLItJ/OQ8/ImVX6ZBAp+6Qhc/X3Roqh1tMwsJNZSL3sZeJ/Xicxg2s/5TJODJN0cWiQCGIikjVA+lwxasTUEqSK26yEjlAhNbanki3BW/7yKmleVD236t1fVmo3eR1FOIFTOAcPrqAGd1CHBlB4gmd4hTdn4rw4787HYrTg5DvH8AfO5w/0dZI0

Too fast to stop

Double Integrator
• Since is very small, we can ignore that term in the dynamics: ,

• Then the safety constraint can be written as

• Two cases

• If , then we will set the final control as

Δt2 pk+1 ≈ pk + Δtvk
vk+1 = vk + Δtuk

ϕ(xk+1)(:= φ(xk, uk)) = dmin − ∥pk + Δtvk − O∥ − kϕ(pk + Δtvk − O)
T(vk + Δtuk)

φ(xk, u
r
k) ≤ 0 uk = u

r
k

φ(xk, u
r
k) > 0 uk = u

r
k + Δuk

Δuk = c(pk + Δtvk − O) c =
φ(xk, u

r
k)

kϕΔt∥pk + Δtvk − O∥

Double Integrator
16

min
uk

∥uk − u
r
k∥ s.t. ϕ(xk+1) ≤ 0

uk = u
r
k + max {0,

φ(xk, u
r
k)

Δtkϕ∥pk + Δtvk − O∥ }(pk + Δtvk − O)

Move toward goal
Move away from obstacle (active only when close to the obstacle)

urk = kp(G − pk) + kvvk

Simulation
17

-10 -8 -6 -4 -2 0 2 4 6 8 10
-10

-8

-6

-4

-2

10
Double Integrator with collision avoidance

% Set up controller
kp = 1;
kd = 2;
ur = @(x) kp*(goal-x(1:2)) – kd*(x(3:4)); % Nominal control
dr = @(x,ur) x(1:2)+dt*x(3:4)-obstacle;
ddotr = @(x,ur) (x(1)+dt*x(3)-obstacle(1))*(x(3)+dt*ur(1))+
(x(2)+dt*x(4)-obstacle(2))*(x(4)+dt*ur(2));
kphi = 0.5;
varphi = @(x,ur) dmin – norm(dr(x,ur)) – kphi*ddotr(x,ur);
c = @(x,ur) max(0,varphi(x,ur))/norm(dr(x,ur))/dt/kphi;
u = @(x) ur(x) + c(x,ur(x))*dr(x,ur(x));

% Simulation
for k = 1:kmax
x = double_integrator(x, u(x), dt);
if norm(x(1:2)-goal)AAAB9HicbVDLSgMxFL1TX7W+qi7dBIvgqsyIoMuiG5cV7QPaodxJ0zY0kxmTTKEM/Q43LhRx68e482/MtLPQ1gOBwzn3ck9OEAuujet+O4W19Y3NreJ2aWd3b/+gfHjU1FGiKGvQSESqHaBmgkvWMNwI1o4VwzAQrBWMbzO/NWFK80g+mmnM/BCHkg84RWMlvxuiGVEUaXvWe+iVK27VnYOsEi8nFchR75W/uv2IJiGThgrUuuO5sfFTVIZTwWalbqJZjHSMQ9axVGLItJ/OQ8/ImVX6ZBAp+6Qhc/X3Roqh1tMwsJNZSL3sZeJ/Xicxg2s/5TJODJN0cWiQCGIikjVA+lwxasTUEqSK26yEjlAhNbanki3BW/7yKmleVD236t1fVmo3eR1FOIFTOAcPrqAGd1CHBlB4gmd4hTdn4rw4787HYrTg5DvH8AfO5w/0dZI0AAAB9HicbVDLSgMxFL1TX7W+qi7dBIvgqsyIoMuiG5cV7QPaodxJ0zY0kxmTTKEM/Q43LhRx68e482/MtLPQ1gOBwzn3ck9OEAuujet+O4W19Y3NreJ2aWd3b/+gfHjU1FGiKGvQSESqHaBmgkvWMNwI1o4VwzAQrBWMbzO/NWFK80g+mmnM/BCHkg84RWMlvxuiGVEUaXvWe+iVK27VnYOsEi8nFchR75W/uv2IJiGThgrUuuO5sfFTVIZTwWalbqJZjHSMQ9axVGLItJ/OQ8/ImVX6ZBAp+6Qhc/X3Roqh1tMwsJNZSL3sZeJ/Xicxg2s/5TJODJN0cWiQCGIikjVA+lwxasTUEqSK26yEjlAhNbanki3BW/7yKmleVD236t1fVmo3eR1FOIFTOAcPrqAGd1CHBlB4gmd4hTdn4rw4787HYrTg5DvH8AfO5w/0dZI0AAAB9HicbVDLSgMxFL1TX7W+qi7dBIvgqsyIoMuiG5cV7QPaodxJ0zY0kxmTTKEM/Q43LhRx68e482/MtLPQ1gOBwzn3ck9OEAuujet+O4W19Y3NreJ2aWd3b/+gfHjU1FGiKGvQSESqHaBmgkvWMNwI1o4VwzAQrBWMbzO/NWFK80g+mmnM/BCHkg84RWMlvxuiGVEUaXvWe+iVK27VnYOsEi8nFchR75W/uv2IJiGThgrUuuO5sfFTVIZTwWalbqJZjHSMQ9axVGLItJ/OQ8/ImVX6ZBAp+6Qhc/X3Roqh1tMwsJNZSL3sZeJ/Xicxg2s/5TJODJN0cWiQCGIikjVA+lwxasTUEqSK26yEjlAhNbanki3BW/7yKmleVD236t1fVmo3eR1FOIFTOAcPrqAGd1CHBlB4gmd4hTdn4rw4787HYrTg5DvH8AfO5w/0dZI0AAAB9HicbVDLSgMxFL1TX7W+qi7dBIvgqsyIoMuiG5cV7QPaodxJ0zY0kxmTTKEM/Q43LhRx68e482/MtLPQ1gOBwzn3ck9OEAuujet+O4W19Y3NreJ2aWd3b/+gfHjU1FGiKGvQSESqHaBmgkvWMNwI1o4VwzAQrBWMbzO/NWFK80g+mmnM/BCHkg84RWMlvxuiGVEUaXvWe+iVK27VnYOsEi8nFchR75W/uv2IJiGThgrUuuO5sfFTVIZTwWalbqJZjHSMQ9axVGLItJ/OQ8/ImVX6ZBAp+6Qhc/X3Roqh1tMwsJNZSL3sZeJ/Xicxg2s/5TJODJN0cWiQCGIikjVA+lwxasTUEqSK26yEjlAhNbanki3BW/7yKmleVD236t1fVmo3eR1FOIFTOAcPrqAGd1CHBlB4gmd4hTdn4rw4787HYrTg5DvH8AfO5w/0dZI0

ϕ(xk+1) = φ(xk, uk) ≤ 0

Poses a constraint control

Let be the set of all control that
satisfies the constraint.

We we are doing is just to project the
nominal control to the set

US uk

Multiple obstacles
19

U1S

U2S

U3S

US Reference

Modified

U1S

U2S

U3S US
Reference

Motion Planning
20

A* search Convex Feasible Set

What Is the Difference Between
Planning and Control?

Planning Problem

• Planning objects

• Boundary conditions

• Constraints

• Objectives

Planning Objects

• Path planning

• Speed profile planning

• Trajectory planning

Time

Path

Trajectory

Boundary Conditions

• Boundary constraints

• : initial condition

• : target condition

• Fixed horizon planning

• Fixed target planning

A
AAAB8nicbVDLSsNAFL2pr1pfVZdugkVwVRIRdFl147KCfUAbymQ6aYdOZsLMjVBCP8ONC0Xc+jXu/BsnbRbaemDgcM69zLknTAQ36HnfTmltfWNzq7xd2dnd2z+oHh61jUo1ZS2qhNLdkBgmuGQt5ChYN9GMxKFgnXByl/udJ6YNV/IRpwkLYjKSPOKUoJV6/ZjgmBKR3cwG1ZpX9+ZwV4lfkBoUaA6qX/2homnMJFJBjOn5XoJBRjRyKtis0k8NSwidkBHrWSpJzEyQzSPP3DOrDN1IafskunP190ZGYmOmcWgn84hm2cvF/7xeitF1kHGZpMgkXXwUpcJF5eb3u0OuGUUxtYRQzW1Wl46JJhRtSxVbgr988ippX9R9r+4/XNYat0UdZTiBUzgHH66gAffQhBZQUPAMr/DmoPPivDsfi9GSU+wcwx84nz9wTpFXAAAB8nicbVDLSsNAFL2pr1pfVZdugkVwVRIRdFl147KCfUAbymQ6aYdOZsLMjVBCP8ONC0Xc+jXu/BsnbRbaemDgcM69zLknTAQ36HnfTmltfWNzq7xd2dnd2z+oHh61jUo1ZS2qhNLdkBgmuGQt5ChYN9GMxKFgnXByl/udJ6YNV/IRpwkLYjKSPOKUoJV6/ZjgmBKR3cwG1ZpX9+ZwV4lfkBoUaA6qX/2homnMJFJBjOn5XoJBRjRyKtis0k8NSwidkBHrWSpJzEyQzSPP3DOrDN1IafskunP190ZGYmOmcWgn84hm2cvF/7xeitF1kHGZpMgkXXwUpcJF5eb3u0OuGUUxtYRQzW1Wl46JJhRtSxVbgr988ippX9R9r+4/XNYat0UdZTiBUzgHH66gAffQhBZQUPAMr/DmoPPivDsfi9GSU+wcwx84nz9wTpFXAAAB8nicbVDLSsNAFL2pr1pfVZdugkVwVRIRdFl147KCfUAbymQ6aYdOZsLMjVBCP8ONC0Xc+jXu/BsnbRbaemDgcM69zLknTAQ36HnfTmltfWNzq7xd2dnd2z+oHh61jUo1ZS2qhNLdkBgmuGQt5ChYN9GMxKFgnXByl/udJ6YNV/IRpwkLYjKSPOKUoJV6/ZjgmBKR3cwG1ZpX9+ZwV4lfkBoUaA6qX/2homnMJFJBjOn5XoJBRjRyKtis0k8NSwidkBHrWSpJzEyQzSPP3DOrDN1IafskunP190ZGYmOmcWgn84hm2cvF/7xeitF1kHGZpMgkXXwUpcJF5eb3u0OuGUUxtYRQzW1Wl46JJhRtSxVbgr988ippX9R9r+4/XNYat0UdZTiBUzgHH66gAffQhBZQUPAMr/DmoPPivDsfi9GSU+wcwx84nz9wTpFXAAAB8nicbVDLSsNAFL2pr1pfVZdugkVwVRIRdFl147KCfUAbymQ6aYdOZsLMjVBCP8ONC0Xc+jXu/BsnbRbaemDgcM69zLknTAQ36HnfTmltfWNzq7xd2dnd2z+oHh61jUo1ZS2qhNLdkBgmuGQt5ChYN9GMxKFgnXByl/udJ6YNV/IRpwkLYjKSPOKUoJV6/ZjgmBKR3cwG1ZpX9+ZwV4lfkBoUaA6qX/2homnMJFJBjOn5XoJBRjRyKtis0k8NSwidkBHrWSpJzEyQzSPP3DOrDN1IafskunP190ZGYmOmcWgn84hm2cvF/7xeitF1kHGZpMgkXXwUpcJF5eb3u0OuGUUxtYRQzW1Wl46JJhRtSxVbgr988ippX9R9r+4/XNYat0UdZTiBUzgHH66gAffQhBZQUPAMr/DmoPPivDsfi9GSU+wcwx84nz9wTpFX

t0, p0
AAAB73icbVBNS8NAEJ3Ur1q/qh69LBbBg5REBD0WvXisYGuhDWGz3bRLN5u4OxFK6J/w4kERr/4db/4bt20O2vpg4PHeDDPzwlQKg6777ZRWVtfWN8qbla3tnd296v5B2ySZZrzFEpnoTkgNl0LxFgqUvJNqTuNQ8odwdDP1H564NiJR9zhOuR/TgRKRYBSt1MHAPSNp4AbVmlt3ZyDLxCtIDQo0g+pXr5+wLOYKmaTGdD03RT+nGgWTfFLpZYanlI3ogHctVTTmxs9n907IiVX6JEq0LYVkpv6eyGlszDgObWdMcWgWvan4n9fNMLryc6HSDLli80VRJgkmZPo86QvNGcqxJZRpYW8lbEg1ZWgjqtgQvMWXl0n7vO65de/uota4LuIowxEcwyl4cAkNuIUmtICBhGd4hTfn0Xlx3p2PeWvJKWYO4Q+czx+52I8YAAAB73icbVBNS8NAEJ3Ur1q/qh69LBbBg5REBD0WvXisYGuhDWGz3bRLN5u4OxFK6J/w4kERr/4db/4bt20O2vpg4PHeDDPzwlQKg6777ZRWVtfWN8qbla3tnd296v5B2ySZZrzFEpnoTkgNl0LxFgqUvJNqTuNQ8odwdDP1H564NiJR9zhOuR/TgRKRYBSt1MHAPSNp4AbVmlt3ZyDLxCtIDQo0g+pXr5+wLOYKmaTGdD03RT+nGgWTfFLpZYanlI3ogHctVTTmxs9n907IiVX6JEq0LYVkpv6eyGlszDgObWdMcWgWvan4n9fNMLryc6HSDLli80VRJgkmZPo86QvNGcqxJZRpYW8lbEg1ZWgjqtgQvMWXl0n7vO65de/uota4LuIowxEcwyl4cAkNuIUmtICBhGd4hTfn0Xlx3p2PeWvJKWYO4Q+czx+52I8YAAAB73icbVBNS8NAEJ3Ur1q/qh69LBbBg5REBD0WvXisYGuhDWGz3bRLN5u4OxFK6J/w4kERr/4db/4bt20O2vpg4PHeDDPzwlQKg6777ZRWVtfWN8qbla3tnd296v5B2ySZZrzFEpnoTkgNl0LxFgqUvJNqTuNQ8odwdDP1H564NiJR9zhOuR/TgRKRYBSt1MHAPSNp4AbVmlt3ZyDLxCtIDQo0g+pXr5+wLOYKmaTGdD03RT+nGgWTfFLpZYanlI3ogHctVTTmxs9n907IiVX6JEq0LYVkpv6eyGlszDgObWdMcWgWvan4n9fNMLryc6HSDLli80VRJgkmZPo86QvNGcqxJZRpYW8lbEg1ZWgjqtgQvMWXl0n7vO65de/uota4LuIowxEcwyl4cAkNuIUmtICBhGd4hTfn0Xlx3p2PeWvJKWYO4Q+czx+52I8YAAAB73icbVBNS8NAEJ3Ur1q/qh69LBbBg5REBD0WvXisYGuhDWGz3bRLN5u4OxFK6J/w4kERr/4db/4bt20O2vpg4PHeDDPzwlQKg6777ZRWVtfWN8qbla3tnd296v5B2ySZZrzFEpnoTkgNl0LxFgqUvJNqTuNQ8odwdDP1H564NiJR9zhOuR/TgRKRYBSt1MHAPSNp4AbVmlt3ZyDLxCtIDQo0g+pXr5+wLOYKmaTGdD03RT+nGgWTfFLpZYanlI3ogHctVTTmxs9n907IiVX6JEq0LYVkpv6eyGlszDgObWdMcWgWvan4n9fNMLryc6HSDLli80VRJgkmZPo86QvNGcqxJZRpYW8lbEg1ZWgjqtgQvMWXl0n7vO65de/uota4LuIowxEcwyl4cAkNuIUmtICBhGd4hTfn0Xlx3p2PeWvJKWYO4Q+czx+52I8Y

Z
AAAB8nicbVDLSsNAFL2pr1pfVZdugkVwVRIRdFl047KCfWAbymQ6aYdOZsLMjVBCP8ONC0Xc+jXu/BsnbRbaemDgcM69zLknTAQ36HnfTmltfWNzq7xd2dnd2z+oHh61jUo1ZS2qhNLdkBgmuGQt5ChYN9GMxKFgnXBym/udJ6YNV/IBpwkLYjKSPOKUoJV6/ZjgmBKRPc4G1ZpX9+ZwV4lfkBoUaA6qX/2homnMJFJBjOn5XoJBRjRyKtis0k8NSwidkBHrWSpJzEyQzSPP3DOrDN1IafskunP190ZGYmOmcWgn84hm2cvF/7xeitF1kHGZpMgkXXwUpcJF5eb3u0OuGUUxtYRQzW1Wl46JJhRtSxVbgr988ippX9R9r+7fX9YaN0UdZTiBUzgHH66gAXfQhBZQUPAMr/DmoPPivDsfi9GSU+wcwx84nz+WS5FwAAAB8nicbVDLSsNAFL2pr1pfVZdugkVwVRIRdFl047KCfWAbymQ6aYdOZsLMjVBCP8ONC0Xc+jXu/BsnbRbaemDgcM69zLknTAQ36HnfTmltfWNzq7xd2dnd2z+oHh61jUo1ZS2qhNLdkBgmuGQt5ChYN9GMxKFgnXBym/udJ6YNV/IBpwkLYjKSPOKUoJV6/ZjgmBKRPc4G1ZpX9+ZwV4lfkBoUaA6qX/2homnMJFJBjOn5XoJBRjRyKtis0k8NSwidkBHrWSpJzEyQzSPP3DOrDN1IafskunP190ZGYmOmcWgn84hm2cvF/7xeitF1kHGZpMgkXXwUpcJF5eb3u0OuGUUxtYRQzW1Wl46JJhRtSxVbgr988ippX9R9r+7fX9YaN0UdZTiBUzgHH66gAXfQhBZQUPAMr/DmoPPivDsfi9GSU+wcwx84nz+WS5FwAAAB8nicbVDLSsNAFL2pr1pfVZdugkVwVRIRdFl047KCfWAbymQ6aYdOZsLMjVBCP8ONC0Xc+jXu/BsnbRbaemDgcM69zLknTAQ36HnfTmltfWNzq7xd2dnd2z+oHh61jUo1ZS2qhNLdkBgmuGQt5ChYN9GMxKFgnXBym/udJ6YNV/IBpwkLYjKSPOKUoJV6/ZjgmBKRPc4G1ZpX9+ZwV4lfkBoUaA6qX/2homnMJFJBjOn5XoJBRjRyKtis0k8NSwidkBHrWSpJzEyQzSPP3DOrDN1IafskunP190ZGYmOmcWgn84hm2cvF/7xeitF1kHGZpMgkXXwUpcJF5eb3u0OuGUUxtYRQzW1Wl46JJhRtSxVbgr988ippX9R9r+7fX9YaN0UdZTiBUzgHH66gAXfQhBZQUPAMr/DmoPPivDsfi9GSU+wcwx84nz+WS5FwAAAB8nicbVDLSsNAFL2pr1pfVZdugkVwVRIRdFl047KCfWAbymQ6aYdOZsLMjVBCP8ONC0Xc+jXu/BsnbRbaemDgcM69zLknTAQ36HnfTmltfWNzq7xd2dnd2z+oHh61jUo1ZS2qhNLdkBgmuGQt5ChYN9GMxKFgnXBym/udJ6YNV/IBpwkLYjKSPOKUoJV6/ZjgmBKRPc4G1ZpX9+ZwV4lfkBoUaA6qX/2homnMJFJBjOn5XoJBRjRyKtis0k8NSwidkBHrWSpJzEyQzSPP3DOrDN1IafskunP190ZGYmOmcWgn84hm2cvF/7xeitF1kHGZpMgkXXwUpcJF5eb3u0OuGUUxtYRQzW1Wl46JJhRtSxVbgr988ippX9R9r+7fX9YaN0UdZTiBUzgHH66gAXfQhBZQUPAMr/DmoPPivDsfi9GSU+wcwx84nz+WS5Fw

te, pe
AAAB73icbVBNS8NAEJ3Ur1q/qh69LBbBg5REBD0WvXisYGuhDWGznbRLN5u4uxFK6J/w4kERr/4db/4bt20O2vpg4PHeDDPzwlRwbVz32ymtrK6tb5Q3K1vbO7t71f2Dtk4yxbDFEpGoTkg1Ci6xZbgR2EkV0jgU+BCObqb+wxMqzRN5b8Yp+jEdSB5xRo2VOibAM5IGGFRrbt2dgSwTryA1KNAMql+9fsKyGKVhgmrd9dzU+DlVhjOBk0ov05hSNqID7FoqaYzaz2f3TsiJVfokSpQtachM/T2R01jrcRzazpiaoV70puJ/Xjcz0ZWfc5lmBiWbL4oyQUxCps+TPlfIjBhbQpni9lbChlRRZmxEFRuCt/jyMmmf1z237t1d1BrXRRxlOIJjOAUPLqEBt9CEFjAQ8Ayv8OY8Oi/Ou/Mxby05xcwh/IHz+QNbmI+CAAAB73icbVBNS8NAEJ3Ur1q/qh69LBbBg5REBD0WvXisYGuhDWGznbRLN5u4uxFK6J/w4kERr/4db/4bt20O2vpg4PHeDDPzwlRwbVz32ymtrK6tb5Q3K1vbO7t71f2Dtk4yxbDFEpGoTkg1Ci6xZbgR2EkV0jgU+BCObqb+wxMqzRN5b8Yp+jEdSB5xRo2VOibAM5IGGFRrbt2dgSwTryA1KNAMql+9fsKyGKVhgmrd9dzU+DlVhjOBk0ov05hSNqID7FoqaYzaz2f3TsiJVfokSpQtachM/T2R01jrcRzazpiaoV70puJ/Xjcz0ZWfc5lmBiWbL4oyQUxCps+TPlfIjBhbQpni9lbChlRRZmxEFRuCt/jyMmmf1z237t1d1BrXRRxlOIJjOAUPLqEBt9CEFjAQ8Ayv8OY8Oi/Ou/Mxby05xcwh/IHz+QNbmI+CAAAB73icbVBNS8NAEJ3Ur1q/qh69LBbBg5REBD0WvXisYGuhDWGznbRLN5u4uxFK6J/w4kERr/4db/4bt20O2vpg4PHeDDPzwlRwbVz32ymtrK6tb5Q3K1vbO7t71f2Dtk4yxbDFEpGoTkg1Ci6xZbgR2EkV0jgU+BCObqb+wxMqzRN5b8Yp+jEdSB5xRo2VOibAM5IGGFRrbt2dgSwTryA1KNAMql+9fsKyGKVhgmrd9dzU+DlVhjOBk0ov05hSNqID7FoqaYzaz2f3TsiJVfokSpQtachM/T2R01jrcRzazpiaoV70puJ/Xjcz0ZWfc5lmBiWbL4oyQUxCps+TPlfIjBhbQpni9lbChlRRZmxEFRuCt/jyMmmf1z237t1d1BrXRRxlOIJjOAUPLqEBt9CEFjAQ8Ayv8OY8Oi/Ou/Mxby05xcwh/IHz+QNbmI+CAAAB73icbVBNS8NAEJ3Ur1q/qh69LBbBg5REBD0WvXisYGuhDWGznbRLN5u4uxFK6J/w4kERr/4db/4bt20O2vpg4PHeDDPzwlRwbVz32ymtrK6tb5Q3K1vbO7t71f2Dtk4yxbDFEpGoTkg1Ci6xZbgR2EkV0jgU+BCObqb+wxMqzRN5b8Yp+jEdSB5xRo2VOibAM5IGGFRrbt2dgSwTryA1KNAMql+9fsKyGKVhgmrd9dzU+DlVhjOBk0ov05hSNqID7FoqaYzaz2f3TsiJVfokSpQtachM/T2R01jrcRzazpiaoV70puJ/Xjcz0ZWfc5lmBiWbL4oyQUxCps+TPlfIjBhbQpni9lbChlRRZmxEFRuCt/jyMmmf1z237t1d1BrXRRxlOIJjOAUPLqEBt9CEFjAQ8Ayv8OY8Oi/Ou/Mxby05xcwh/IHz+QNbmI+C

te
AAAB6nicbVBNS8NAEJ3Ur1q/qh69BIvgqSQi6LHoxWNF+wFtKJvtpF262YTdiVBKf4IXD4p49Rd589+4bXPQ1gcDj/dmmJkXplIY8rxvp7C2vrG5Vdwu7ezu7R+UD4+aJsk0xwZPZKLbITMohcIGCZLYTjWyOJTYCke3M7/1hNqIRD3SOMUgZgMlIsEZWemBetgrV7yqN4e7SvycVCBHvVf+6vYTnsWoiEtmTMf3UgomTJPgEqelbmYwZXzEBtixVLEYTTCZnzp1z6zSd6NE21LkztXfExMWGzOOQ9sZMxqaZW8m/ud1Moqug4lQaUao+GJRlEmXEnf2t9sXGjnJsSWMa2FvdfmQacbJplOyIfjLL6+S5kXV96r+/WWldpPHUYQTOIVz8OEKanAHdWgAhwE8wyu8OdJ5cd6dj0VrwclnjuEPnM8fVGiN0A==AAAB6nicbVBNS8NAEJ3Ur1q/qh69BIvgqSQi6LHoxWNF+wFtKJvtpF262YTdiVBKf4IXD4p49Rd589+4bXPQ1gcDj/dmmJkXplIY8rxvp7C2vrG5Vdwu7ezu7R+UD4+aJsk0xwZPZKLbITMohcIGCZLYTjWyOJTYCke3M7/1hNqIRD3SOMUgZgMlIsEZWemBetgrV7yqN4e7SvycVCBHvVf+6vYTnsWoiEtmTMf3UgomTJPgEqelbmYwZXzEBtixVLEYTTCZnzp1z6zSd6NE21LkztXfExMWGzOOQ9sZMxqaZW8m/ud1Moqug4lQaUao+GJRlEmXEnf2t9sXGjnJsSWMa2FvdfmQacbJplOyIfjLL6+S5kXV96r+/WWldpPHUYQTOIVz8OEKanAHdWgAhwE8wyu8OdJ5cd6dj0VrwclnjuEPnM8fVGiN0A==AAAB6nicbVBNS8NAEJ3Ur1q/qh69BIvgqSQi6LHoxWNF+wFtKJvtpF262YTdiVBKf4IXD4p49Rd589+4bXPQ1gcDj/dmmJkXplIY8rxvp7C2vrG5Vdwu7ezu7R+UD4+aJsk0xwZPZKLbITMohcIGCZLYTjWyOJTYCke3M7/1hNqIRD3SOMUgZgMlIsEZWemBetgrV7yqN4e7SvycVCBHvVf+6vYTnsWoiEtmTMf3UgomTJPgEqelbmYwZXzEBtixVLEYTTCZnzp1z6zSd6NE21LkztXfExMWGzOOQ9sZMxqaZW8m/ud1Moqug4lQaUao+GJRlEmXEnf2t9sXGjnJsSWMa2FvdfmQacbJplOyIfjLL6+S5kXV96r+/WWldpPHUYQTOIVz8OEKanAHdWgAhwE8wyu8OdJ5cd6dj0VrwclnjuEPnM8fVGiN0A==AAAB6nicbVBNS8NAEJ3Ur1q/qh69BIvgqSQi6LHoxWNF+wFtKJvtpF262YTdiVBKf4IXD4p49Rd589+4bXPQ1gcDj/dmmJkXplIY8rxvp7C2vrG5Vdwu7ezu7R+UD4+aJsk0xwZPZKLbITMohcIGCZLYTjWyOJTYCke3M7/1hNqIRD3SOMUgZgMlIsEZWemBetgrV7yqN4e7SvycVCBHvVf+6vYTnsWoiEtmTMf3UgomTJPgEqelbmYwZXzEBtixVLEYTTCZnzp1z6zSd6NE21LkztXfExMWGzOOQ9sZMxqaZW8m/ud1Moqug4lQaUao+GJRlEmXEnf2t9sXGjnJsSWMa2FvdfmQacbJplOyIfjLL6+S5kXV96r+/WWldpPHUYQTOIVz8OEKanAHdWgAhwE8wyu8OdJ5cd6dj0VrwclnjuEPnM8fVGiN0A==

pe
AAAB6nicbVBNS8NAEJ3Ur1q/oh69LBbBU0lE0GPRi8eK9gPaUDbbSbt0swm7G6GE/gQvHhTx6i/y5r9x2+agrQ8GHu/NMDMvTAXXxvO+ndLa+sbmVnm7srO7t3/gHh61dJIphk2WiER1QqpRcIlNw43ATqqQxqHAdji+nfntJ1SaJ/LRTFIMYjqUPOKMGis9pH3su1Wv5s1BVolfkCoUaPTdr94gYVmM0jBBte76XmqCnCrDmcBppZdpTCkb0yF2LZU0Rh3k81On5MwqAxIlypY0ZK7+nshprPUkDm1nTM1IL3sz8T+vm5noOsi5TDODki0WRZkgJiGzv8mAK2RGTCyhTHF7K2EjqigzNp2KDcFffnmVtC5qvlfz7y+r9ZsijjKcwCmcgw9XUIc7aEATGAzhGV7hzRHOi/PufCxaS04xcwx/4Hz+AE5Qjcw=AAAB6nicbVBNS8NAEJ3Ur1q/oh69LBbBU0lE0GPRi8eK9gPaUDbbSbt0swm7G6GE/gQvHhTx6i/y5r9x2+agrQ8GHu/NMDMvTAXXxvO+ndLa+sbmVnm7srO7t3/gHh61dJIphk2WiER1QqpRcIlNw43ATqqQxqHAdji+nfntJ1SaJ/LRTFIMYjqUPOKMGis9pH3su1Wv5s1BVolfkCoUaPTdr94gYVmM0jBBte76XmqCnCrDmcBppZdpTCkb0yF2LZU0Rh3k81On5MwqAxIlypY0ZK7+nshprPUkDm1nTM1IL3sz8T+vm5noOsi5TDODki0WRZkgJiGzv8mAK2RGTCyhTHF7K2EjqigzNp2KDcFffnmVtC5qvlfz7y+r9ZsijjKcwCmcgw9XUIc7aEATGAzhGV7hzRHOi/PufCxaS04xcwx/4Hz+AE5Qjcw=AAAB6nicbVBNS8NAEJ3Ur1q/oh69LBbBU0lE0GPRi8eK9gPaUDbbSbt0swm7G6GE/gQvHhTx6i/y5r9x2+agrQ8GHu/NMDMvTAXXxvO+ndLa+sbmVnm7srO7t3/gHh61dJIphk2WiER1QqpRcIlNw43ATqqQxqHAdji+nfntJ1SaJ/LRTFIMYjqUPOKMGis9pH3su1Wv5s1BVolfkCoUaPTdr94gYVmM0jBBte76XmqCnCrDmcBppZdpTCkb0yF2LZU0Rh3k81On5MwqAxIlypY0ZK7+nshprPUkDm1nTM1IL3sz8T+vm5noOsi5TDODki0WRZkgJiGzv8mAK2RGTCyhTHF7K2EjqigzNp2KDcFffnmVtC5qvlfz7y+r9ZsijjKcwCmcgw9XUIc7aEATGAzhGV7hzRHOi/PufCxaS04xcwx/4Hz+AE5Qjcw=AAAB6nicbVBNS8NAEJ3Ur1q/oh69LBbBU0lE0GPRi8eK9gPaUDbbSbt0swm7G6GE/gQvHhTx6i/y5r9x2+agrQ8GHu/NMDMvTAXXxvO+ndLa+sbmVnm7srO7t3/gHh61dJIphk2WiER1QqpRcIlNw43ATqqQxqHAdji+nfntJ1SaJ/LRTFIMYjqUPOKMGis9pH3su1Wv5s1BVolfkCoUaPTdr94gYVmM0jBBte76XmqCnCrDmcBppZdpTCkb0yF2LZU0Rh3k81On5MwqAxIlypY0ZK7+nshprPUkDm1nTM1IL3sz8T+vm5noOsi5TDODki0WRZkgJiGzv8mAK2RGTCyhTHF7K2EjqigzNp2KDcFffnmVtC5qvlfz7y+r9ZsijjKcwCmcgw9XUIc7aEATGAzhGV7hzRHOi/PufCxaS04xcwx/4Hz+AE5Qjcw=

Time

Obstacle

Start

Fixed
Horizon

Fixed
Target

Constraints

• Dynamics

• Safety

Time

Obstacle

Start

Target

Objectives

• Smoothness of the trajectory

• Shortest path

• Distance to unsafe region

• * Magnitude of control effort

Planning Methods
Planning by construction Planning by modification

Time Time

Reference
Trajectory

Search-based planning (A*, D*)
Sampling-based planning (RRT)

Optimization-based planning
Elastic band

Today: Path Planning

• Today we will focus on path planning (no temporal information) with
static obstacles.

• The methods can be easily extended to trajectory planning with dynamic
obstacles with known trajectory.

• In the following lectures, we will talk about how to deal with unknown
dynamic obstacles.

Search-Based Planning
• Problem: given the boundary conditions and ,

how to find a shortest connecting path that
satisfies the dynamic constraint
and avoids obstacles.

• A path is

• Idea: enumerate possible paths forward until we
reach the goal.

x0 G

xk+1 = f(xk, uk)

x0 → x1 → x2 → … → xN = G

GThese transitions
need to satisfy

dynamic constraints

These states should
be collision-free

* The index does not
necessarily represent time

Search-Based Planning

• Let us consider single integrator
dynamics (simplest, basically no
constraint on the path)

• For every current state , there are
uncountably many successors

• We need to pick finitely many
successors to keep the search tractable

xk+1

xk
xk+1

Example
31

Breath-First Search (BFS)Depth-First Search (DFS) Heuristic Search (A*)

Example: Depth First Search
32

Example: Depth First Search
33

Example: Depth First Search
34

2 6

3 4 5 6

2 3 4 5 6

3 4 5

Example: Depth First Search
35

2 6

3 4 5 6

2 3 4 5 6

3 4 5

Dead end!

Example: Depth First Search
36

3 4 5 6

2 3 4 5 6

3 4 5

What might the problem with DFS?

Example: Breath First Search
37

Example: Breath First Search
38

Example: Breath First Search
39

Example: Breath First Search
40

Example: Breath First Search
41

Example: Breath First Search
42

Example: Breath First Search
43

What might the problem with BFS?

Example: A* Search
44

Heuristic: distance to goal (without obstacle)

Example: A* Search
45

Heuristic: distance to goal

26
2

Example: A* Search
46

2 6

3 4 5 6

2 3 4 5 6

3 4 5
24

22 21 20 19 18

22 21 20 19

24 23 22 21 20

Example: A* Search
47

2 6

3 4 5 6

2 3 4 5 6

3 4 5
24

22 21 20 19 18

22 21 20 19

24 23 22 21 20

8
17

Example
48

Advanced Example
49

Summary

• During search, we maintain three kinds of node:

• Current node

• Open set: nodes to be expanded

• Closed set: nodes already visited

• Initially, the open set only contains the goal; the
closed set is empty.

Summary
• At every iteration, we pick a node from the open set

for expansion

• DFS: pick the successor node of the current node
that has the smallest cost (distance traveled)

• BFS: pick the node in the open set with smallest
cost

• A*: pick the node in the open set with smallest
cost + heuristics

• If that node is the goal node, we are done.

Summary
• During the expansion, we add all the successors

of the current node to the open set.

• If the newly added node already exists in the
open set, then we check if it has a smaller cost this
time. If so, we update the cost and change its
ancestor. Otherwise, we skip the step.

• This only happens in DFS and A*

• After expansion, we put the current node into
closed set and go to next iteration.

Code: A*
53

function plan(obj)
X = obj.S;
g = 0;
obj.t(X) = obj.CLOSED;
while (X ~= obj.G)
exp_array = obj.expand_array(X, g);
for i = 1: size(exp_array,2)
% if the successor is already on the openlist
if obj.t( exp_array(1,i) ) == obj.OPEN
% find the position of this successor in the openlist
j = find(obj.openlist(1,:) == exp_array(1,i) );
obj.openlist(5,j) = min(obj.openlist(5,j), exp_array(5,i));
if obj.openlist(5,j) == exp_array(5,i)
obj.openlist(2,j) = X; % Parant node
obj.openlist(3,j) = exp_array(3,i); % path cost
obj.openlist(4,j) = exp_array(4,i); % heuristic
end
% if the successor is NEW, insert to the openlist
else
obj.insert(exp_array(:,i), X);
end
end
[X, parent, g] = obj.MIN_STATE(); % find next node
obj.DELETE(X, parent, g); % add to the closed set
end
%Find out the node with the smallest fn
disp(‘done planning’)
end

Advanced Search: Lattice A*
54

For unicycle model (3 state)

Advanced Search: Heuristics
55

0 20 40 60 80 100
0

100

(a) The occupancy grid map. (b) The extracted geometric objects. (c) The computed boundary layer B✓ .

Fig. 4: Computing the boundary layer in mixed environment.

Define a minimum tangent circle as a circle of radius
R := 1/max that passes (x, y) with tangent direction
v(✓) := [cos ✓, sin ✓] as shown in Fig.3a. Considering the
direction v(✓), the left circle centered at O1 := (x �
R sin ✓, y + R cos ✓) is denoted as C1 and the right circle
centered at O2 := (x+R sin ✓, y�R cos ✓) is denoted as C2.
In the following discussion, the boundary layer is computed
assuming the goal point is far from the identified geometric
objects.

1) Boundary Layer for Line Segments: Suppose the two
end points of a line segment l is p1 = (x1, y1) and p2 =
(x2, y2), with outward normal direction ~n. p1 is on the left
of p2 regarding the normal direction ~n. If ~n · v(✓) < 0, the boundary layer is nonempty, e.g. B✓(l) 6= ; where ; denotes an empty set, and is enclosed by the line segment l and the following three curves: (i) the contour Z1 of (x, y) such that C1 passes p2; (ii) the contour Z2 of (x, y) such that C2 passes p1; (iii) the contour Z3 of (x, y) such that C1 or C2 is tangent to l, whichever is closer to l. The three curves are illustrated in Fig.3b. Note that it is possible that Z1 and Z2 intersects. As C2 or C1 is tangent to l, the distance from Z3 to l is d = R�R|~v(✓ � ⇡ 2 ) · ~n|. Hence the area enclosed by l and Z3 satisfies 0  (x� x1, y � y1) · ~n  d. (8) For Z1, kO1p2k = R. The area enclosed by Z1 satisfies that ��!p2p1 · ��! p2O1 � 0 or k ��! p2O1k  R. (9) For Z2, kO2p2k = R. The area enclosed by Z2 satisfies that ��!p1p2 · ��! p1O2 � 0 or k ��! p1O2k  R. (10) Then B✓(l) is defined by (8), (9) and (10). 2) Boundary Layer for Concave Corners: Consider a corner point is p0 = (x0, y0), the line on the left is denoted as l1 with outward normal vector ~n1, and the line on the right is denoted as l2 with outward normal vector ~n2. As illustrated in Fig.3c, the boundary layer for the corner, e.g. B✓(c), is enclosed by the lines l1 and l2 and the following two curves: (i) the contour Z1 of (x, y) such that C1 is tangent to l2; (ii) the contour Z2 of (x, y) such that C2 is tangent to l1. As C2 is tangent to l1, the distance from Z1 to l1 is d1 = R�R~v(✓ � ⇡ 2 ) · ~n1. (11) As C1 is tangent to l2, the distance from Z2 to l2 is d2 = R�R~v(✓ + ⇡ 2 ) · ~n2. (12) Then B✓(c) = {(x, y)|(x� x0, y � y0) · ~n1 2 [0, d1], (x� x0, y � y0) · ~n2 2 [0, d2]}. 3) Boundary Layer for Mixed Environments: In a mixed environment, suppose the information of the obstacles is stored in an occupancy grid map. The first step is to find all line segments from the occupancy grid map together with their outward normal vectors. The second step is to find all concave corners from the set of line segments. Then we compute the boundary layer for each line segment and each concave corner. The process is illustrated in Fig.4. This method gives a good approximation of the true boundary layer defined in (6) if the goal point B is not in the computed B(O), which is assumed in this paper. Moreover, the computation is purely geometric. C. The Boundary Layer Heuristic Given the boundary layer B(O), the boundary layer heuris- tic is defined as hBL(N ) = 8 < : cg (x, y, ✓) 2 B(O), d = 1 cg (x, y, ✓ + ⇡) 2 B(O), d = �1 0 else . (13) In the following discussion, we show the admissibility and consistency of hBL and hBL+hD. Since consistency implies admissibility, we will be focused on consistency. 1) Consistency of hBL: For any two nodes Ni and Nj , we need to show hBL(Ni) � hBL(Nj)  J⇤(Ni,Nj). By definition (13), hBL(Ni) � hBL(Nj) can only be ±cg or 0. Since J⇤(Ni,Nj) � 0, we only need to consider the case hBL(Ni) = cg and hBL(Nj) = 0, i.e. Ni is in the boundary layer but Nj is not. By definition of B(O) in (6), the vehicle needs to shift gear to get out of boundary layer. Then J⇤(Ni,Nj) � cg = hBL(Ni) � hBL(Nj). Hence the heuristic hBL is consistent. C. Liu, Y. Wang, and M. Tomizuka, "Boundary layer heuristic for search-based nonholonomic path planning in maze-like environments", in IEEE Intelligent Vehicle Symposium. IEEE, 2017, pp. 831-836. http://www.me.berkeley.edu/~cliu/files/iv17-1.pdf http://www.me.berkeley.edu/~cliu/files/iv17-1.pdf Optimization Based Planning • Optimization based planning is usually used to “smooth” the paths generated by search • The key challenge is the non convexity of the problem • Idea: turn the non convex optimization to a sequence of convex optimization 56 Convex Optimization 57 Convex Function Convex Set Motion Planning Problems Are Non Convex 58 Interpolating these two trajectories will not always result in a feasible trajectory Why Convexity Is Important? • Freedom to do linear interpolation or linear perturbation (gradient descent) • Mature algorithms to solve convex problems • Quadratic programming 59 Time Reference Trajectory Making the Problem Convex 60 Convex corridor

Related Posts