github/PythonRobotics
1
1
Fork 0
mirror of https://github.com/AtsushiSakai/PythonRobotics.git synced 2026-01-14 00:27:55 -05:00
Code Issues Packages Projects Releases Wiki Activity

Merge remote-tracking branch 'origin/master'

Browse Source
This commit is contained in:
Atsushi Sakai
2020-01-27 17:39:34 +09:00
parent d4cb0f981d 33f4b94412
commit d1161378a4
5 changed files with 115 additions and 84 deletions
Download Patch File Download Diff File Expand all files Collapse all files

1
.gitignore vendored
View File

@@ -70,3 +70,4 @@ target/
.ipynb_checkpoints
matplotrecorder/*
.vscode/settings.json

15
PathPlanning/Dijkstra/dijkstra.py
View File

@@ -33,7 +33,7 @@ class Dijkstra:
self.x = x # index of grid
self.y = y # index of grid
self.cost = cost
self.pind = pind
self.pind = pind # index of previous Node
def __str__(self):
return str(self.x) + "," + str(self.y) + "," + str(self.cost) + "," + str(self.pind)
@@ -88,10 +88,10 @@ class Dijkstra:
closedset[c_id] = current
# expand search grid based on motion model
for i, _ in enumerate(self.motion):
node = self.Node(current.x + self.motion[i][0],
current.y + self.motion[i][1],
current.cost + self.motion[i][2], c_id)
for move_x, move_y, move_cost in self.motion:
node = self.Node(current.x + move_x,
current.y + move_y,
current.cost + move_cost, c_id)
n_id = self.calc_index(node)
if n_id in closedset:
@@ -124,11 +124,6 @@ class Dijkstra:
return rx, ry
def calc_heuristic(self, n1, n2):
w = 1.0 # weight of heuristic
d = w * math.hypot(n1.x - n2.x, n1.y - n2.y)
return d
def calc_position(self, index, minp):
pos = index*self.reso+minp
return pos

6
PathPlanning/DynamicWindowApproach/dynamic_window_approach.py
View File

@@ -22,7 +22,7 @@ def dwa_control(x, config, goal, ob):
dw = calc_dynamic_window(x, config)
u, trajectory = calc_final_input(x, dw, config, goal, ob)
u, trajectory = calc_control_and_trajectory(x, dw, config, goal, ob)
return u, trajectory
@@ -101,7 +101,7 @@ def calc_dynamic_window(x, config):
x[4] - config.max_dyawrate * config.dt,
x[4] + config.max_dyawrate * config.dt]
# [vmin,vmax, yaw_rate min, yaw_rate max]
# [vmin, vmax, yaw_rate min, yaw_rate max]
dw = [max(Vs[0], Vd[0]), min(Vs[1], Vd[1]),
max(Vs[2], Vd[2]), min(Vs[3], Vd[3])]
@@ -124,7 +124,7 @@ def predict_trajectory(x_init, v, y, config):
return traj
def calc_final_input(x, dw, config, goal, ob):
def calc_control_and_trajectory(x, dw, config, goal, ob):
"""
calculation final input with dynamic window
"""

58
PathPlanning/FrenetOptimalTrajectory/frenet_optimal_trajectory.py
View File

@@ -17,6 +17,17 @@ import matplotlib.pyplot as plt
import copy
import math
import cubic_spline_planner
import sys
import os
sys.path.append(os.path.dirname(os.path.abspath(__file__)) +
"/../QuinticPolynomialsPlanner/")
try:
from quintic_polynomials_planner import QuinticPolynomial
except ImportError:
raise
SIM_LOOP = 500
@@ -44,50 +55,6 @@ KLON = 1.0
show_animation = True
class quintic_polynomial:
def __init__(self, xs, vxs, axs, xe, vxe, axe, T):
# calc coefficient of quintic polynomial
self.a0 = xs
self.a1 = vxs
self.a2 = axs / 2.0
A = np.array([[T**3, T**4, T**5],
[3 * T ** 2, 4 * T ** 3, 5 * T ** 4],
[6 * T, 12 * T ** 2, 20 * T ** 3]])
b = np.array([xe - self.a0 - self.a1 * T - self.a2 * T**2,
vxe - self.a1 - 2 * self.a2 * T,
axe - 2 * self.a2])
x = np.linalg.solve(A, b)
self.a3 = x[0]
self.a4 = x[1]
self.a5 = x[2]
def calc_point(self, t):
xt = self.a0 + self.a1 * t + self.a2 * t**2 + \
self.a3 * t**3 + self.a4 * t**4 + self.a5 * t**5
return xt
def calc_first_derivative(self, t):
xt = self.a1 + 2 * self.a2 * t + \
3 * self.a3 * t**2 + 4 * self.a4 * t**3 + 5 * self.a5 * t**4
return xt
def calc_second_derivative(self, t):
xt = 2 * self.a2 + 6 * self.a3 * t + 12 * self.a4 * t**2 + 20 * self.a5 * t**3
return xt
def calc_third_derivative(self, t):
xt = 6 * self.a3 + 24 * self.a4 * t + 60 * self.a5 * t**2
return xt
class quartic_polynomial:
def __init__(self, xs, vxs, axs, vxe, axe, T):
@@ -164,7 +131,8 @@ def calc_frenet_paths(c_speed, c_d, c_d_d, c_d_dd, s0):
for Ti in np.arange(MINT, MAXT, DT):
fp = Frenet_path()
lat_qp = quintic_polynomial(c_d, c_d_d, c_d_dd, di, 0.0, 0.0, Ti)
# lat_qp = quintic_polynomial(c_d, c_d_d, c_d_dd, di, 0.0, 0.0, Ti)
lat_qp = QuinticPolynomial(c_d, c_d_d, c_d_dd, di, 0.0, 0.0, Ti)
fp.t = [t for t in np.arange(0.0, Ti, DT)]
fp.d = [lat_qp.calc_point(t) for t in fp.t]

119
PathPlanning/QuinticPolynomialsPlanner/quintic_polynomials_planner.ipynb
View File

@@ -2,45 +2,112 @@
"cells": [
{
"cell_type": "markdown",
"metadata": {},
"source": [
"# Quintic polynomials planner"
],
"metadata": {
"collapsed": false
}
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"## Quintic polynomials for one dimensional robot motion\n",
"\n",
"We assume a dimensional robot motion $x(t)$ at time $t$ is formulated as a quintic polynomials based on time as follows:\n",
"We assume a one-dimensional robot motion $x(t)$ at time $t$ is formulated as a quintic polynomials based on time as follows:\n",
"\n",
"$x(t) = a_0+a_1t+a_2t^2+a_3t^3+a_4t^4$\n",
"$x(t) = a_0+a_1t+a_2t^2+a_3t^3+a_4t^4+a_5t^5$ --(1)\n",
"\n",
"a_0, a_1. a_2, a_3 are parameters of the quintic polynomial.\n",
"$a_0, a_1. a_2, a_3, a_4, a_5$ are parameters of the quintic polynomial.\n",
"\n",
"It is assumed that terminal states (start and end) are known as boundary conditions.\n",
"\n",
"Start position, velocity, and acceleration are $x_s, v_s, a_s$ respectively.\n",
"\n",
"End position, velocity, and acceleration are $x_e, v_e, a_e$ respectively.\n",
"\n",
"So, when time is 0.\n",
"\n",
"$x(0) = a_0 = x_s$ -- (2)\n",
"\n",
"Then, differentiating the equation (1) with t, \n",
"\n",
"$x'(t) = a_1+2a_2t+3a_3t^2+4a_4t^3+5a_5t^4$ -- (3)\n",
"\n",
"So, when time is 0,\n",
"\n",
"$x(0) = a_0 = x_s$\n",
"$x'(0) = a_1 = v_s$ -- (4)\n",
"\n",
"$x_s$ is a start x position.\n",
"Then, differentiating the equation (3) with t again, \n",
"\n",
"Then, differentiating this equation with t, \n",
"\n",
"$x'(t) = a_1+2a_2t+3a_3t^2+4a_4t^3$\n",
"$x''(t) = 2a_2+6a_3t+12a_4t^2$ -- (5)\n",
"\n",
"So, when time is 0,\n",
"\n",
"$x'(0) = a_1 = v_s$\n",
"$x''(0) = 2a_2 = a_s$ -- (6)\n",
"\n",
"$v_s$ is a initial speed for x axis.\n",
"so, we can calculate $a_0$, $a_1$, $a_2$ with eq. (2), (4), (6) and boundary conditions.\n",
"\n",
"=== TBD ==== \n"
],
"metadata": {
"collapsed": false
}
"$a_3, a_4, a_5$ are still unknown in eq(1).\n"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"We assume that the end time for a maneuver is $T$, we can get these equations from eq (1), (3), (5):\n",
"\n",
"$x(T)=a_0+a_1T+a_2T^2+a_3T^3+a_4T^4+a_5T^5=x_e$ -- (7)\n",
"\n",
"$x'(T)=a_1+2a_2T+3a_3T^2+4a_4T^3+5a_5T^4=v_e$ -- (8)\n",
"\n",
"$x''(T)=2a_2+6a_3T+12a_4T^2+20a_5T^3=a_e$ -- (9)\n"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"From eq (7), (8), (9), we can calculate $a_3, a_4, a_5$ to solve the linear equations.\n",
"\n",
"$Ax=b$\n",
"\n",
"$\\begin{bmatrix} T^3 & T^4 & T^5 \\\\ 3T^2 & 4T^3 & 5T^4 \\\\ 6T & 12T^2 & 20T^3 \\end{bmatrix}\n",
"\\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}$\n",
"\n",
"We can get all unknown parameters now"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"## Quintic polynomials for two dimensional robot motion (x-y)\n",
"\n",
"If you use two quintic polynomials along x axis and y axis, you can plan for two dimensional robot motion in x-y plane.\n",
"\n",
"$x(t) = a_0+a_1t+a_2t^2+a_3t^3+a_4t^4+a_5t^5$ --(10)\n",
"\n",
"$y(t) = b_0+b_1t+b_2t^2+b_3t^3+b_4t^4+b_5t^5$ --(11)\n",
"\n",
"It is assumed that terminal states (start and end) are known as boundary conditions.\n",
"\n",
"Start position, orientation, velocity, and acceleration are $x_s, y_s, \\theta_s, v_s, a_s$ respectively.\n",
"\n",
"End position, orientation, velocity, and acceleration are $x_e, y_e. \\theta_e, v_e, a_e$ respectively.\n",
"\n",
"Each velocity and acceleration boundary condition can be calculated with each orientation.\n",
"\n",
"$v_{xs}=v_scos(\\theta_s), v_{ys}=v_ssin(\\theta_s)$\n",
"\n",
"$v_{xe}=v_ecos(\\theta_e), v_{ye}=v_esin(\\theta_e)$\n"
]
},
{
"cell_type": "code",
"execution_count": null,
"metadata": {},
"outputs": [],
"source": []
}
],
"metadata": {
@@ -52,25 +119,25 @@
"language_info": {
"codemirror_mode": {
"name": "ipython",
"version": 2
"version": 3
},
"file_extension": ".py",
"mimetype": "text/x-python",
"name": "python",
"nbconvert_exporter": "python",
"pygments_lexer": "ipython2",
"version": "2.7.6"
"pygments_lexer": "ipython3",
"version": "3.7.5"
},
"pycharm": {
"stem_cell": {
"cell_type": "raw",
"source": [],
"metadata": {
"collapsed": false
}
},
"source": []
}
}
},
"nbformat": 4,
"nbformat_minor": 0
}
"nbformat_minor": 1
}