# ФАЙЛ "QUINTIC\_POLYNOMIALS\_PLANNER.PY"

Этот файл предназначен для создания пути с помощью квинтических полиномов для ровера, с учетом заданных начальных и конечных условий и ограничений на ускорение и рывок.

## ИНИЦИАЛИЗАЦИЯ БИБЛИОТЕК

```python
import math
import matplotlib.pyplot as plt
import numpy as np
```

\- math: стандартная библиотека  для математических функций.

\- matplotlib.pyplot: используется для создания графиков и визуализации данных.

\- numpy: библиотека для работы с массивами и матрицами, а также для выполнения числовых операций.

## ПАРАМЕТРЫ

```python
MAX_T = 100.0   максимальное время достижения цели [с]
MIN_T = 5.0     минимальное время достижения цели [с]
show_animation = True   флаг для отображения анимации
```

Здесь заданы параметры максимального и минимального времени для достижения цели, а также флаг для отображения анимации.<br>

## КЛАСС QuinticPolynomial

```python
class QuinticPolynomial:
    //код далее
```

Класс QuinticPolynomial определяет полином пятого порядка, используемый для вычисления траектории движения.

## КОНСТРУКТОР КЛАССА \_\_init\_\_

```python
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.

&#x20;

&#x20;Методы для вычисления траектории

\- calc\_point(t): возвращает позицию в момент времени t.

\- calc\_first\_derivative(t): возвращает скорость в момент времени t.

\- calc\_second\_derivative(t): возвращает ускорение в момент времени t.

\- calc\_third\_derivative(t): возвращает рывок (jerk) в момент времени t.

## ФУНКЦИЯ quintic\_polynomials\_planner

Функция для планирования траектории с использованием квинтических полиномов.

```python
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\. Начальные параметры и переменные:

&#x20;  \- Вычисляются начальные и конечные компоненты скорости и ускорения по осям x и y.

&#x20;

2\. Перебор возможных времен T для достижения цели:

&#x20;  \- Для каждого значения T от MIN\_T до MAX\_T с шагом MIN\_T создаются полиномы для x и y.

&#x20;

3\. Расчет траектории для каждого времени t:

&#x20;  \- Используются методы класса QuinticPolynomial для вычисления положения, скорости, ускорения и рывка.

&#x20;

4\. Проверка ограничений на ускорение и рывок:

&#x20;  \- Если максимальные значения ускорения и рывка не превышают заданные пределы, путь считается найденным.

&#x20;

5\. Анимация траектории:

&#x20;  \- При включенном флаге show\_animation отображается графическое представление траектории, скорости, ускорения и рывка.

## ФУНКЦИЯ plot\_arrow

```python
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

```python
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 определяет начальные и конечные параметры движения и вызывает функцию планирования траектории. Результаты отображаются на графиках при включенном флаге анимации.

## ТОЧКА ВХОДА

```python
if __name__ == '__main__':
    main()
```

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

1. Создайте файл с названием quintic\_polynomials\_planner.py

<br>

<figure><img src="/files/okn7TaHAjTCGqDIEbLvp" alt=""><figcaption><p>Файл quintic_polynomials_planner.py в иерархии</p></figcaption></figure>

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

<figure><img src="/files/a6cyax2WPtOHzstjjoIS" alt=""><figcaption><p>Окно программирования файла "quintic_polynomials_planner.py". Часть 1</p></figcaption></figure>

<figure><img src="/files/JrFopLz8Fs5x9FLfU0dM" alt=""><figcaption><p>Окно программирования файла "quintic_polynomials_planner.py". Часть 2</p></figcaption></figure>

<figure><img src="/files/ulfmrM3o5mwpgIGXGMpL" alt=""><figcaption><p>Окно программирования файла "quintic_polynomials_planner.py". Часть 3</p></figcaption></figure>

<figure><img src="/files/HgTv4SzHQrUyVjiPO34l" alt=""><figcaption><p>Окно программирования файла "quintic_polynomials_planner.py". Часть 4</p></figcaption></figure>

<figure><img src="/files/QRW2eEOZ9ITGS1ae9tAj" alt=""><figcaption><p>Окно программирования файла "quintic_polynomials_planner.py". Часть 5</p></figcaption></figure>

<figure><img src="/files/MSPvEh6y5PpytegVMFuX" alt=""><figcaption><p>Окно программирования файла "quintic_polynomials_planner.py". Часть 6</p></figcaption></figure>

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

   1. Откройте терминал VS Code (Terminal -> New Terminal или комбинацией Ctrl + Shift + \`)
   2. Пропишите в терминале эти команды (каждую команду прописывайте и запускайте поэтапно!): pip install matplotlib, pip install plotly, pip install numpy

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

<figure><img src="/files/0OwFHa9D75ml8b0jxKSN" alt=""><figcaption><p>Вывод при запуске кода</p></figcaption></figure>


---

# Agent Instructions: Querying This Documentation

If you need additional information that is not directly available in this page, you can query the documentation dynamically by asking a question.

Perform an HTTP GET request on the current page URL with the `ask` query parameter:

```
GET https://dh-organization.gitbook.io/droneshub-rover-kontakt/opisanie-navigacionnoi-chasti-programmy-odnomernoe-prostranstvo-i-matematicheskoe-vychislenie-puti/navigaciya-metodom-matematicheskikh-vychislenii-na-grafike/fail-quintic_polynomials_planner.py.md?ask=<question>
```

The question should be specific, self-contained, and written in natural language.
The response will contain a direct answer to the question and relevant excerpts and sources from the documentation.

Use this mechanism when the answer is not explicitly present in the current page, you need clarification or additional context, or you want to retrieve related documentation sections.