mirror of
https://github.com/AtsushiSakai/PythonRobotics.git
synced 2026-02-02 04:15:06 -05:00
237 lines
6.4 KiB
Python
237 lines
6.4 KiB
Python
"""
|
|
|
|
Quintic Polynomials Planner
|
|
|
|
author: Atsushi Sakai (@Atsushi_twi)
|
|
|
|
Ref:
|
|
|
|
- [Local Path planning And Motion Control For Agv In Positioning](http://ieeexplore.ieee.org/document/637936/)
|
|
|
|
"""
|
|
|
|
import math
|
|
|
|
import matplotlib.pyplot as plt
|
|
import numpy as np
|
|
|
|
# parameter
|
|
MAX_T = 100.0 # maximum time to the goal [s]
|
|
MIN_T = 5.0 # minimum time to the goal[s]
|
|
|
|
show_animation = True
|
|
|
|
|
|
class QuinticPolynomial:
|
|
|
|
def __init__(self, xs, vxs, axs, xe, vxe, axe, T):
|
|
|
|
# calc coefficient of quinic polynomial
|
|
self.xs = xs
|
|
self.vxs = vxs
|
|
self.axs = axs
|
|
self.xe = xe
|
|
self.vxe = vxe
|
|
self.axe = axe
|
|
|
|
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
|
|
|
|
|
|
def quintic_polynomials_planner(sx, sy, syaw, sv, sa, gx, gy, gyaw, gv, ga, max_accel, max_jerk, dt):
|
|
"""
|
|
quintic polynomial planner
|
|
|
|
input
|
|
sx: start x position [m]
|
|
sy: start y position [m]
|
|
syaw: start yaw angle [rad]
|
|
sa: start accel [m/ss]
|
|
gx: goal x position [m]
|
|
gy: goal y position [m]
|
|
gyaw: goal yaw angle [rad]
|
|
ga: goal accel [m/ss]
|
|
max_accel: maximum accel [m/ss]
|
|
max_jerk: maximum jerk [m/sss]
|
|
dt: time tick [s]
|
|
|
|
return
|
|
time: time result
|
|
rx: x position result list
|
|
ry: y position result list
|
|
ryaw: yaw angle result list
|
|
rv: velocity result list
|
|
ra: accel result list
|
|
|
|
"""
|
|
|
|
vxs = sv * math.cos(syaw)
|
|
vys = sv * math.sin(syaw)
|
|
vxg = gv * math.cos(gyaw)
|
|
vyg = gv * math.sin(gyaw)
|
|
|
|
axs = sa * math.cos(syaw)
|
|
ays = sa * math.sin(syaw)
|
|
axg = ga * math.cos(gyaw)
|
|
ayg = ga * math.sin(gyaw)
|
|
|
|
time, rx, ry, ryaw, rv, ra, rj = [], [], [], [], [], [], []
|
|
|
|
for T in np.arange(MIN_T, MAX_T, MIN_T):
|
|
xqp = QuinticPolynomial(sx, vxs, axs, gx, vxg, axg, T)
|
|
yqp = QuinticPolynomial(sy, vys, ays, gy, vyg, ayg, T)
|
|
|
|
time, rx, ry, ryaw, rv, ra, rj = [], [], [], [], [], [], []
|
|
|
|
for t in np.arange(0.0, T + dt, dt):
|
|
time.append(t)
|
|
rx.append(xqp.calc_point(t))
|
|
ry.append(yqp.calc_point(t))
|
|
|
|
vx = xqp.calc_first_derivative(t)
|
|
vy = yqp.calc_first_derivative(t)
|
|
v = np.hypot(vx, vy)
|
|
yaw = math.atan2(vy, vx)
|
|
rv.append(v)
|
|
ryaw.append(yaw)
|
|
|
|
ax = xqp.calc_second_derivative(t)
|
|
ay = yqp.calc_second_derivative(t)
|
|
a = np.hypot(ax, ay)
|
|
if len(rv) >= 2 and rv[-1] - rv[-2] < 0.0:
|
|
a *= -1
|
|
ra.append(a)
|
|
|
|
jx = xqp.calc_third_derivative(t)
|
|
jy = yqp.calc_third_derivative(t)
|
|
j = np.hypot(jx, jy)
|
|
if len(ra) >= 2 and ra[-1] - ra[-2] < 0.0:
|
|
j *= -1
|
|
rj.append(j)
|
|
|
|
if max([abs(i) for i in ra]) <= max_accel and max([abs(i) for i in rj]) <= max_jerk:
|
|
print("find path!!")
|
|
break
|
|
|
|
if show_animation: # pragma: no cover
|
|
for i, _ in enumerate(time):
|
|
plt.cla()
|
|
plt.gcf().canvas.mpl_connect('key_release_event',
|
|
lambda event: [exit(0) if event.key == 'escape' else None])
|
|
plt.grid(True)
|
|
plt.axis("equal")
|
|
plot_arrow(sx, sy, syaw)
|
|
plot_arrow(gx, gy, gyaw)
|
|
plot_arrow(rx[i], ry[i], ryaw[i])
|
|
plt.title("Time[s]:" + str(time[i])[0:4] +
|
|
" v[m/s]:" + str(rv[i])[0:4] +
|
|
" a[m/ss]:" + str(ra[i])[0:4] +
|
|
" jerk[m/sss]:" + str(rj[i])[0:4],
|
|
)
|
|
plt.pause(0.001)
|
|
|
|
return time, rx, ry, ryaw, rv, ra, rj
|
|
|
|
|
|
def plot_arrow(x, y, yaw, length=1.0, width=0.5, fc="r", ec="k"): # pragma: no cover
|
|
"""
|
|
Plot arrow
|
|
"""
|
|
|
|
if not isinstance(x, float):
|
|
for (ix, iy, iyaw) in zip(x, y, yaw):
|
|
plot_arrow(ix, iy, iyaw)
|
|
else:
|
|
plt.arrow(x, y, length * math.cos(yaw), length * math.sin(yaw),
|
|
fc=fc, ec=ec, head_width=width, head_length=width)
|
|
plt.plot(x, y)
|
|
|
|
|
|
def main():
|
|
print(__file__ + " start!!")
|
|
|
|
sx = 10.0 # start x position [m]
|
|
sy = 10.0 # start y position [m]
|
|
syaw = np.deg2rad(10.0) # start yaw angle [rad]
|
|
sv = 1.0 # start speed [m/s]
|
|
sa = 0.1 # start accel [m/ss]
|
|
gx = 30.0 # goal x position [m]
|
|
gy = -10.0 # goal y position [m]
|
|
gyaw = np.deg2rad(20.0) # goal yaw angle [rad]
|
|
gv = 1.0 # goal speed [m/s]
|
|
ga = 0.1 # goal accel [m/ss]
|
|
max_accel = 1.0 # max accel [m/ss]
|
|
max_jerk = 0.5 # max jerk [m/sss]
|
|
dt = 0.1 # time tick [s]
|
|
|
|
time, x, y, yaw, v, a, j = quintic_polynomials_planner(
|
|
sx, sy, syaw, sv, sa, gx, gy, gyaw, gv, ga, max_accel, max_jerk, dt)
|
|
|
|
if show_animation: # pragma: no cover
|
|
plt.plot(x, y, "-r")
|
|
|
|
plt.subplots()
|
|
plt.plot(time, [np.rad2deg(i) for i in yaw], "-r")
|
|
plt.xlabel("Time[s]")
|
|
plt.ylabel("Yaw[deg]")
|
|
plt.grid(True)
|
|
|
|
plt.subplots()
|
|
plt.plot(time, v, "-r")
|
|
plt.xlabel("Time[s]")
|
|
plt.ylabel("Speed[m/s]")
|
|
plt.grid(True)
|
|
|
|
plt.subplots()
|
|
plt.plot(time, a, "-r")
|
|
plt.xlabel("Time[s]")
|
|
plt.ylabel("accel[m/ss]")
|
|
plt.grid(True)
|
|
|
|
plt.subplots()
|
|
plt.plot(time, j, "-r")
|
|
plt.xlabel("Time[s]")
|
|
plt.ylabel("jerk[m/sss]")
|
|
plt.grid(True)
|
|
|
|
plt.show()
|
|
|
|
|
|
if __name__ == '__main__':
|
|
main()
|