Skip to content

Commit eb1fec7

Browse files
authored
Clean up Path planning docs (AtsushiSakai#579)
* update docs * update docs
1 parent 9d759d1 commit eb1fec7

File tree

6 files changed

+120
-263
lines changed

6 files changed

+120
-263
lines changed

PathPlanning/QuinticPolynomialsPlanner/quintic_polynomials_planner.ipynb

Lines changed: 0 additions & 143 deletions
This file was deleted.

PathPlanning/RRTStar/Figure_1.png

-117 KB
Binary file not shown.

PathPlanning/RRTStar/rrt_star.ipynb

Lines changed: 0 additions & 74 deletions
This file was deleted.

docs/modules/path_planning/path_planning_main.rst

Lines changed: 4 additions & 31 deletions
Original file line numberDiff line numberDiff line change
@@ -91,6 +91,9 @@ Ref:
9191
- `Optimal rough terrain trajectory generation for wheeled mobile
9292
robots <http://journals.sagepub.com/doi/pdf/10.1177/0278364906075328>`__
9393

94+
95+
96+
9497
State Lattice Planning
9598
----------------------
9699

@@ -179,27 +182,8 @@ This is a simple path planning code with Rapidly-Exploring Random Trees
179182
Black circles are obstacles, green line is a searched tree, red crosses
180183
are start and goal positions.
181184

182-
.. _rrt*:
183-
184-
RRT\*
185-
~~~~~
186-
187-
|10|
188-
189-
This is a path planning code with RRT\*
190-
191-
Black circles are obstacles, green line is a searched tree, red crosses
192-
are start and goal positions.
193-
194185
.. include:: rrt_star.rst
195186

196-
Ref:
197-
198-
- `Incremental Sampling-based Algorithms for Optimal Motion
199-
Planning <https://arxiv.org/abs/1005.0416>`__
200-
201-
- `Sampling-based Algorithms for Optimal Motion
202-
Planning <http://citeseerx.ist.psu.edu/viewdoc/download?doi=10.1.1.419.5503&rep=rep1&type=pdf>`__
203187

204188
RRT with dubins path
205189
~~~~~~~~~~~~~~~~~~~~
@@ -367,20 +351,9 @@ Ref:
367351
Autonomous
368352
Vehicles <http://citeseerx.ist.psu.edu/viewdoc/download?doi=10.1.1.294.6438&rep=rep1&type=pdf>`__
369353

370-
Quintic polynomials planning
371-
----------------------------
372354

373-
Motion planning with quintic polynomials.
374-
375-
|2|
376-
377-
It can calculate 2D path, velocity, and acceleration profile based on
378-
quintic polynomials.
379-
380-
Ref:
355+
.. include:: quintic_polynomials_planner.rst
381356

382-
- `Local Path Planning And Motion Control For Agv In
383-
Positioning <http://ieeexplore.ieee.org/document/637936/>`__
384357

385358
Dubins path planning
386359
--------------------
Lines changed: 106 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,106 @@
1+
2+
Quintic polynomials planning
3+
----------------------------
4+
5+
Motion planning with quintic polynomials.
6+
7+
|2|
8+
9+
It can calculate 2D path, velocity, and acceleration profile based on
10+
quintic polynomials.
11+
12+
13+
14+
Quintic polynomials for one dimensional robot motion
15+
~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
16+
17+
We assume a one-dimensional robot motion :math:`x(t)` at time :math:`t` is
18+
formulated as a quintic polynomials based on time as follows:
19+
20+
.. math:: x(t) = a_0+a_1t+a_2t^2+a_3t^3+a_4t^4+a_5t^5
21+
:label: eq1
22+
23+
:math:`a_0, a_1. a_2, a_3, a_4, a_5` are parameters of the quintic polynomial.
24+
25+
It is assumed that terminal states (start and end) are known as boundary conditions.
26+
27+
Start position, velocity, and acceleration are :math:`x_s, v_s, a_s` respectively.
28+
29+
End position, velocity, and acceleration are :math:`x_e, v_e, a_e` respectively.
30+
31+
So, when time is 0.
32+
33+
.. math:: x(0) = a_0 = x_s
34+
:label: eq2
35+
36+
Then, differentiating the equation :eq:`eq1` with t,
37+
38+
.. math:: x'(t) = a_1+2a_2t+3a_3t^2+4a_4t^3+5a_5t^4
39+
:label: eq3
40+
41+
So, when time is 0,
42+
43+
.. math:: x'(0) = a_1 = v_s
44+
:label: eq4
45+
46+
Then, differentiating the equation :eq:`eq3` with t again,
47+
48+
.. math:: x''(t) = 2a_2+6a_3t+12a_4t^2
49+
:label: eq5
50+
51+
So, when time is 0,
52+
53+
.. math:: x''(0) = 2a_2 = a_s
54+
:label: eq6
55+
56+
so, we can calculate :math:`a_0, a_1, a_2` with eq. :eq:`eq2`, :eq:`eq4`, :eq:`eq6` and boundary conditions.
57+
58+
:math:`a_3, a_4, a_5` are still unknown in eq :eq:`eq1`.
59+
60+
We assume that the end time for a maneuver is :math:`T`, we can get these equations from eq :eq:`eq1`, :eq:`eq3`, :eq:`eq5`:
61+
62+
.. math:: x(T)=a_0+a_1T+a_2T^2+a_3T^3+a_4T^4+a_5T^5=x_e
63+
:label: eq7
64+
65+
.. math:: x'(T)=a_1+2a_2T+3a_3T^2+4a_4T^3+5a_5T^4=v_e
66+
:label: eq8
67+
68+
.. math:: x''(T)=2a_2+6a_3T+12a_4T^2+20a_5T^3=a_e
69+
:label: eq9
70+
71+
From eq :eq:`eq7`, :eq:`eq8`, :eq:`eq9`, we can calculate :math:`a_3, a_4, a_5` to solve the linear equations: :math:`Ax=b`
72+
73+
.. math:: \begin{bmatrix} T^3 & T^4 & T^5 \\ 3T^2 & 4T^3 & 5T^4 \\ 6T & 12T^2 & 20T^3 \end{bmatrix}\begin{bmatrix} a_3\\ a_4\\ a_5\end{bmatrix}=\begin{bmatrix} x_e-x_s-v_sT-0.5a_sT^2\\ v_e-v_s-a_sT\\ a_e-a_s\end{bmatrix}
74+
75+
We can get all unknown parameters now.
76+
77+
Quintic polynomials for two dimensional robot motion (x-y)
78+
~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
79+
80+
If you use two quintic polynomials along x axis and y axis, you can plan for two dimensional robot motion in x-y plane.
81+
82+
.. math:: x(t) = a_0+a_1t+a_2t^2+a_3t^3+a_4t^4+a_5t^5
83+
:label: eq10
84+
85+
.. math:: y(t) = b_0+b_1t+b_2t^2+b_3t^3+b_4t^4+b_5t^5
86+
:label: eq11
87+
88+
It is assumed that terminal states (start and end) are known as boundary conditions.
89+
90+
Start position, orientation, velocity, and acceleration are :math:`x_s, y_s, \theta_s, v_s, a_s` respectively.
91+
92+
End position, orientation, velocity, and acceleration are :math:`x_e, y_e. \theta_e, v_e, a_e` respectively.
93+
94+
Each velocity and acceleration boundary condition can be calculated with each orientation.
95+
96+
:math:`v_{xs}=v_scos(\theta_s), v_{ys}=v_ssin(\theta_s)`
97+
98+
:math:`v_{xe}=v_ecos(\theta_e), v_{ye}=v_esin(\theta_e)`
99+
100+
References:
101+
~~~~~~~~~~~
102+
103+
- `Local Path Planning And Motion Control For Agv In
104+
Positioning <http://ieeexplore.ieee.org/document/637936/>`__
105+
106+
Lines changed: 10 additions & 15 deletions
Original file line numberDiff line numberDiff line change
@@ -1,27 +1,22 @@
1+
RRT\*
2+
~~~~~
13

2-
Simulation
3-
^^^^^^^^^^
4-
5-
.. code-block:: ipython3
6-
7-
from IPython.display import Image
8-
Image(filename="Figure_1.png",width=600)
4+
.. figure:: https://github.com/AtsushiSakai/PythonRoboticsGifs/raw/master/PathPlanning/RRTstar/animation.gif
5+
:alt: gif
96

7+
This is a path planning code with RRT\*
108

9+
Black circles are obstacles, green line is a searched tree, red crosses are start and goal positions.
1110

11+
Simulation
12+
^^^^^^^^^^
1213

1314
.. image:: rrt_star_files/rrt_star_1_0.png
1415
:width: 600px
1516

1617

17-
18-
.. figure:: https://github.com/AtsushiSakai/PythonRoboticsGifs/raw/master/PathPlanning/RRTstar/animation.gif
19-
:alt: gif
20-
21-
gif
22-
2318
Ref
2419
^^^
20+
- `Sampling-based Algorithms for Optimal Motion Planning <https://arxiv.org/pdf/1105.1186.pdf>`__
21+
- `Incremental Sampling-based Algorithms for Optimal Motion Planning <https://arxiv.org/abs/1005.0416>`__
2522

26-
- `Sampling-based Algorithms for Optimal Motion
27-
Planning <https://arxiv.org/pdf/1105.1186.pdf>`__

0 commit comments

Comments
 (0)