ФАЙЛ "QUINTIC_POLYNOMIALS_PLANNER.PY"
Этот файл предназначен для создания пути с помощью квинтических полиномов для ровера, с учетом заданных начальных и конечных условий и ограничений на ускорение и рывок.
ИНИЦИАЛИЗАЦИЯ БИБЛИОТЕК
import math
import matplotlib.pyplot as plt
import numpy as np- math: стандартная библиотека для математических функций.
- matplotlib.pyplot: используется для создания графиков и визуализации данных.
- numpy: библиотека для работы с массивами и матрицами, а также для выполнения числовых операций.
ПАРАМЕТРЫ
MAX_T = 100.0 максимальное время достижения цели [с]
MIN_T = 5.0 минимальное время достижения цели [с]
show_animation = True флаг для отображения анимацииЗдесь заданы параметры максимального и минимального времени для достижения цели, а также флаг для отображения анимации.
КЛАСС QuinticPolynomial
class QuinticPolynomial:
//код далееКласс QuinticPolynomial определяет полином пятого порядка, используемый для вычисления траектории движения.
КОНСТРУКТОР КЛАССА __init__
def __init__(self, xs, vxs, axs, xe, vxe, axe, time):
вычисление коэффициентов квинтического полинома
self.a0 = xs
self.a1 = vxs
self.a2 = axs / 2.0
A = np.array([[time 3, time 4, time 5],
[3 time 2, 4 time 3, 5 time 4],
[6 time, 12 time 2, 20 time 3]])
b = np.array([xe - self.a0 - self.a1 time - self.a2 time 2,
vxe - self.a1 - 2 self.a2 time,
axe - 2 self.a2])
x = np.linalg.solve(A, b)
self.a3 = x[0]
self.a4 = x[1]
self.a5 = x[2]
Конструктор вычисляет коэффициенты квинтического полинома, решая систему уравнений.
- self.a0, self.a1, self.a2: начальные положения, скорость и ускорение.
- A и b: матрицы для решения системы уравнений с помощью np.linalg.solve.
Методы для вычисления траектории
- calc_point(t): возвращает позицию в момент времени t.
- calc_first_derivative(t): возвращает скорость в момент времени t.
- calc_second_derivative(t): возвращает ускорение в момент времени t.
- calc_third_derivative(t): возвращает рывок (jerk) в момент времени t.
ФУНКЦИЯ quintic_polynomials_planner
Функция для планирования траектории с использованием квинтических полиномов.
def quintic_polynomials_planner(sx, sy, syaw, sv, sa, gx, gy, gyaw, gv, ga, max_accel, max_jerk, dt):
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
Процесс вычисления
1. Начальные параметры и переменные:
- Вычисляются начальные и конечные компоненты скорости и ускорения по осям x и y.
2. Перебор возможных времен T для достижения цели:
- Для каждого значения T от MIN_T до MAX_T с шагом MIN_T создаются полиномы для x и y.
3. Расчет траектории для каждого времени t:
- Используются методы класса QuinticPolynomial для вычисления положения, скорости, ускорения и рывка.
4. Проверка ограничений на ускорение и рывок:
- Если максимальные значения ускорения и рывка не превышают заданные пределы, путь считается найденным.
5. Анимация траектории:
- При включенном флаге show_animation отображается графическое представление траектории, скорости, ускорения и рывка.
ФУНКЦИЯ plot_arrow
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)Функция для отрисовки стрелки на графике, обозначающей направление движения. Если входные параметры не являются числами, функция вызывает себя рекурсивно для всех элементов.
ОСНОВНАЯ ФУНКЦИЯ main
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 # goal speed [m/s]
ga = 0.1 # goal accel [m/ss]
max_accel = 1.0 # max accel [m/ss]
max_jerk = 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()Основная функция main определяет начальные и конечные параметры движения и вызывает функцию планирования траектории. Результаты отображаются на графиках при включенном флаге анимации.
ТОЧКА ВХОДА
if __name__ == '__main__':
main()Этот блок позволяет запускать скрипт как самостоятельное приложение. Процесс подготовки кода:
Создайте файл с названием quintic_polynomials_planner.py
Поэтапно добавьте код в окно программирования:
Установите необходимые библиотеки: в данном случае у нас не предустановлены библиотеки matplotlib, plotly и numpy. Если они у вас не установлены, установите библиотеки таким образом:
Откройте терминал VS Code (Terminal -> New Terminal или комбинацией Ctrl + Shift + `)
Пропишите в терминале эти команды (каждую команду прописывайте и запускайте поэтапно!): pip install matplotlib, pip install plotly, pip install numpy
После установки библиотек вы можете попробовать запустить этот код, у вас откроется приложение Python с графиком и стрелками, посмотрите как он работает
Last updated