> For the complete documentation index, see [llms.txt](https://dh-organization.gitbook.io/droneshub-rover-kontakt/llms.txt). Markdown versions of documentation pages are available by appending `.md` to page URLs; this page is available as [Markdown](https://dh-organization.gitbook.io/droneshub-rover-kontakt/opisanie-navigacionnoi-chasti-programmy-odnomernoe-prostranstvo-i-matematicheskoe-vychislenie-puti/navigaciya-v-odnomernom-prostranstve-grafik/fail-cubic_spline_planner.py.md).

# ФАЙЛ «CUBIC\_SPLINE\_PLANNER.PY»

Файл «cubic\_spline\_planner.py» реализует класс кубической сплайн-интерполяции в одномерном пространстве. Этот инструмент позволяет эффективно интерполировать и аппроксимировать наборы данных, что может быть полезно для различных задач в робототехнике и компьютерной графике, таких как планирование траекторий или сглаживание данных.

## Класс CubicSpline1D

Класс CubicSpline1D позволяет выполнять интерполяцию кубическим сплайном для набора данных, определяемого их координатами x и y. Он вычисляет коэффициенты для сегментов сплайна и предоставляет методы для оценки сплайна и его производных в любой заданной точке в пределах диапазона предоставленных данных.

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

```python
import math
import numpy as np
import bisect
```

* **`math`**: Эта библиотека помогает выполнять простые математические вычисления, такие как нахождение квадратного корня или вычисление синуса. 
* **`numpy`**: Эта библиотека нужна для работы с большими массивами чисел. Если вам нужно делать что-то сложное с множеством чисел, например, находить их среднее или умножать, `numpy` делает это быстро и удобно.
* **`bisect`**: Эта библиотека помогает управлять отсортированными списками. Если вам нужно добавить новое число в список, который уже отсортирован, `bisect` поможет сделать это так, чтобы список остался в порядке.

## ИНИЦИАЛИЗАЦИЯ КЛАССА

```python
def __init__(self, x, y):
h = np.diff(x)
        if np.any(h < 0):
        raise ValueError("x coordinates must be sorted in ascending order")
 
    self.a, self.b, self.c, self.d = [], [], [], []
    self.x = x
    self.y = y
    self.nx = len(x)  # dimension of x
 
    # calc coefficient a
    self.a = [iy for iy in y]
 
    # calc coefficient c
    A = self.__calc_A(h)
    B = self.__calc_B(h, self.a)
    self.c = np.linalg.solve(A, B)
 
    # calc spline coefficient b and d
    for i in range(self.nx - 1):
        d = (self.c[i + 1] - self.c[i]) / (3.0 * h[i])
        b = 1.0 / h[i] * (self.a[i + 1] - self.a[i]) \
            - h[i] / 3.0 * (2.0 * self.c[i] + self.c[i + 1])
        self.d.append(d)
        self.b.append(b)

```

Конструктор класса принимает два параметра: x и y, которые представляют собой координаты заданных точек данных. Координаты x должны быть отсортированы по возрастанию. При инициализации рассчитываются коэффициенты кубического сплайна (a, b, c, d) на основе переданных данных.

## МЕТОД РАССЧЕТА ПОЗИЦИИ

## def calc\_position(self, x):

```python
def calc_position(self, x):
if x < self.x[0]:
return None
elif x > self.x[-1]:
    return None
 
i = self.__search_index(x)
dx = x - self.x[i]
position = self.a[i] + self.b[i] * dx + \
    self.c[i] * dx ** 2.0 + self.d[i] * dx ** 3.0
 
return position

```

Этот метод вычисляет значение y для заданного x с использованием ранее рассчитанных коэффициентов кубического сплайна. Если x выходит за пределы диапазона входных значений, метод возвращает None.

## МЕТОДЫ РАССЧЕТА ПРОИЗВОДНЫХ

```python
def calc_first_derivative(self, x):
        if x < self.x[0]:
        return None
elif x > self.x[-1]:
    return None
 
i = self.__search_index(x)
dx = x - self.x[i]
dy = self.b[i] + 2.0 * self.c[i] * dx + 3.0 * self.d[i] * dx ** 2.0
return dy
```

Метод вычисляет первую производную сплайна в заданной точке x. Аналогично расчету позиции, если x вне диапазона данных, возвращается None.

<pre class="language-python"><code class="lang-python">def calc_second_derivative(self, x):
<strong>    if x &#x3C; self.x[0]:
</strong>        return None
    elif x > self.x[-1]:
        return None
 
    i = self.__search_index(x)
    dx = x - self.x[i]
    ddy = 2.0 * self.c[i] + 6.0 * self.d[i] * dx
    return ddy
</code></pre>

Этот метод вычисляет вторую производную сплайна в указанной точке. Также он возвращает None, если x находится вне диапазона данных.

## ВСПОМОГАТЕЛЬНЫЕ МЕТОДЫ

<pre class="language-python"><code class="lang-python">def __search_index(self, x):
<strong>    return bisect.bisect(self.x, x) - 1
</strong></code></pre>

Метод находит индекс соответствующего сегмента для значения x с использованием двоичного поиска. Это оптимизирует процесс поиска и делает его более эффективным.

```python
def __calc_A(self, h):
            	A = np.zeros((self.nx, self.nx))
A[0, 0] = 1.0
for i in range(self.nx - 1):
    if i != (self.nx - 2):
        A[i + 1, i + 1] = 2.0 * (h[i] + h[i + 1])
    A[i + 1, i] = h[i]
    A[i, i + 1] = h[i]
 
A[0, 1] = 0.0
A[self.nx - 1, self.nx - 2] = 0.0
A[self.nx - 1, self.nx - 1] = 1.0
return A
```

Этот метод создает матрицу для системы уравнений, которая используется для нахождения коэффициента c сплайна.

```python
def __calc_B(self, h, a):
B = np.zeros(self.nx)
for i in range(self.nx - 2):
    B[i + 1] = 3.0 * (a[i + 2] - a[i + 1]) / h[i + 1]\
        - 3.0 * (a[i + 1] - a[i]) / h[i]
return B
```

Метод формирует вектор B, который также используется при расчетах для получения коэффициентов сплайна.

## Класс CubicSpline2D

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

## ИНИЦИАЛИЗАЦИЯ КЛАССА

```python
def __init__(self, x, y):
	self.s = self.__calc_s(x, y)
	self.sx = CubicSpline1D(self.s, x)
	self.sy = CubicSpline1D(self.s, y)
```

При создании объекта класса CubicSpline2D принимаются два массива координат x и y. Метод \_\_calc\_s вычисляет длину пути на основе этих координат, а затем создаются одномерные кубические сплайны для x и y, чтобы обеспечить возможность дальнейшей интерполяции.

## МЕТОД ВЫЧИСЛЕНИЯ ПОЛОЖЕНИЯ

```python
def calc_position(self, s):
x = self.sx.calc_position(s)
y = self.sy.calc_position(s)
 
return x, y

```

Метод calc\_position принимает расстояние s от начальной точки и возвращает соответствующие координаты (x, y). Если s выходит за пределы диапазона данных, возвращается None.

&#x20;Алгоритм работы:

1\. Вычисляется позиция по сплайнам sx и sy для данного s.

2\. Возвращаются значения x и y.

## МЕТОД ВЫЧИСЛЕНИЯ КРИВИЗНЫ

```python
def calc_curvature(self, s):
dx = self.sx.calc_first_derivative(s)
ddx = self.sx.calc_second_derivative(s)
dy = self.sy.calc_first_derivative(s)
ddy = self.sy.calc_second_derivative(s)
k = (ddy * dx - ddx * dy) / ((dx ** 2 + dy ** 2)**(3 / 2))
return k
```

Метод calc\_curvature рассчитывает кривизну пути для заданного расстояния s. Он использует производные сплайнов, чтобы определить, насколько сильно изменяется направление.

&#x20;Алгоритм работы:

1\. Вычисляются первые и вторые производные по x и y.

2\. Рассчитывается кривизна с использованием формулы, основанной на производных.

## МЕТОД ВЫЧИСЛЕНИЯ УГЛА НАКЛОНА

```python
def calc_yaw(self, s):
dx = self.sx.calc_first_derivative(s)
dy = self.sy.calc_first_derivative(s)
yaw = math.atan2(dy, dx)
return yaw
```

Метод calc\_yaw возвращает угол наклона (yaw) для данной позиции s, который представляет собой тангенс угла.

&#x20;Алгоритм работы:

1\. Вычисляются первые производные по x и y.

2\. Рассчитывается угол наклона с использованием функции atan2.

## ФУНКЦИЯ ДЛЯ РАССЧЕТА СПЛАЙНА ПУТИ

```python
def calc_spline_course(x, y, ds=0.1):
sp = CubicSpline2D(x, y)
s = list(np.arange(0, sp.s[-1], ds))
 
rx, ry, ryaw, rk = [], [], [], []
for i_s in s:
    ix, iy = sp.calc_position(i_s)
    rx.append(ix)
    ry.append(iy)
    ryaw.append(sp.calc_yaw(i_s))
    rk.append(sp.calc_curvature(i_s))
 
return rx, ry, ryaw, rk, s
```

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

Алгоритм работы:

1\. Создается объект CubicSpline2D с заданными координатами.

2\. Для каждой позиции s вычисляются x, y, yaw и кривизна, которые сохраняются в списках.

\
Процесс подготовки кода:

1. Создайте новый файл и назовите его cubic\_spline\_planner.py

<figure><img src="/files/XxKh5czamqMfb1HdAT2x" alt=""><figcaption><p>Файл "cubic_spline_planner.py" в окне иерархии</p></figcaption></figure>

2. Поэтапно копируйте код в окно программирования

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

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

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

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

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

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

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

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

3. Установите необходимые библиотеки: из всех библиотек установить надо только numpy, если она не установлена, то вот как это сделать:

   1. Откройте терминал (Terminal -> New Terminal или комбинацией Ctrl + Shift + \`)
   2. Пропишите в терминал команду: pip install numpy и нажмите Enter

   Этот код можно запустить, он выдает три графика

<figure><img src="/files/y8zrDkDQ5la1nuDQGRFP" alt=""><figcaption><p>Результат вывода кода</p></figcaption></figure>