ФАЙЛ "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()

Этот блок позволяет запускать скрипт как самостоятельное приложение. Процесс подготовки кода:

  1. Создайте файл с названием quintic_polynomials_planner.py

  1. Поэтапно добавьте код в окно программирования:

  1. Установите необходимые библиотеки: в данном случае у нас не предустановлены библиотеки matplotlib, plotly и numpy. Если они у вас не установлены, установите библиотеки таким образом:

    1. Откройте терминал VS Code (Terminal -> New Terminal или комбинацией Ctrl + Shift + `)

    2. Пропишите в терминале эти команды (каждую команду прописывайте и запускайте поэтапно!): pip install matplotlib, pip install plotly, pip install numpy

    После установки библиотек вы можете попробовать запустить этот код, у вас откроется приложение Python с графиком и стрелками, посмотрите как он работает

PreviousНавигация методом математических вычислений на графикеNextНавигация в одномерном пространстве (график)

Last updated