ФАЙЛ «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
Этот код можно запустить, он выдает три графика

ПредыдущаяНавигация в одномерном пространстве (график)СледующаяОписание основной части программы

Последнее обновление 1 год назад

hashtagКласс CubicSpline1D

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

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

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

hashtagdef calc_position(self, x):

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

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

hashtagКласс CubicSpline2D

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

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

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

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

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

Класс CubicSpline1D

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

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

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

def calc_position(self, x):

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

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

Класс CubicSpline2D

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

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

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

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

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