ФАЙЛ «CUBIC_SPLINE_PLANNER.PY»

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

Класс CubicSpline1D

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

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

import math
import numpy as np
import bisect

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

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

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):

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.

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

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.

def calc_second_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]
    ddy = 2.0 * self.c[i] + 6.0 * self.d[i] * dx
    return ddy

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

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

def __search_index(self, x):
    return bisect.bisect(self.x, x) - 1

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

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 сплайна.

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

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

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

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, чтобы обеспечить возможность дальнейшей интерполяции.

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

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.

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

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

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

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

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. Он использует производные сплайнов, чтобы определить, насколько сильно изменяется направление.

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

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

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

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

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, который представляет собой тангенс угла.

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

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

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

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

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 и кривизна, которые сохраняются в списках.

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

Создайте новый файл и назовите его cubic_spline_planner.py

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

Установите необходимые библиотеки: из всех библиотек установить надо только numpy, если она не установлена, то вот как это сделать:
1. Откройте терминал (Terminal -> New Terminal или комбинацией Ctrl + Shift + `)
2. Пропишите в терминал команду: pip install numpy и нажмите Enter
Этот код можно запустить, он выдает три графика

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

Last updated 10 months ago