ФАЙЛ «FRENET_FRAME.PY»

Файл «Frenet_frame.py» представляет собой основной скрипт для запуска ровера и управления его движением. Он содержит функции для планирования траектории, управления ровером и взаимодействия с внешними системами.

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

import asyncio
import contextlib
import json
import socket
import threading
import tracemalloc
from matplotlib import markers
from matplotlib.transforms import Affine2D
import numpy as np
import copy
import math
import sys
import pathlib
from video_processing import Realsense
sys.path.append(str(pathlib.Path(__file__).parent.parent))
import plotly.graph_objs as go
from plotly.utils import PlotlyJSONEncoder
import tornado.ioloop
import tornado.web
import tornado.websocket
import json

from quintic_polynomials_planner import \
    QuinticPolynomial
import cubic_spline_planner

asyncio: библиотека для работы с асинхронными операциями в Python.
json: библиотека для работы с форматом JSON.
socket: библиотека для работы с сетевыми сокетами.
threading: библиотека для работы с многопоточностью.
tracemalloc: библиотека для отслеживания использования памяти.
math: библиотека для математических операций.
sys: библиотека для работы с системными функциями.
pathlib: библиотека для работы с файловыми путями.
numpy: библиотека для работы с числами и массивами.
matplotlib: библиотека для создания графиков и визуализации данных.
plotly: библиотека для создания интерактивных графиков и визуализации данных.
video_processing: отдельный код для работы с видеопотоком (см. раздел "Нейронная сеть и машинное зрение" -> Описание программы машинного зрения -> ФАЙЛ "VIDEO_PROCESSING")
Realsense: класс, хранящийся в файле video_processing.
tornado: библиотека для создания веб-серверов и работы с веб-сокетами.
quintic_polynomials_planner: отдельный код для работы с методом навигации (см. раздел "Описание навигационной части программы (одномерное пространство и математическое вычисление пути)" -> Навигация методом математических вычислений на графике -> ФАЙЛ "QUINTIC_POLYNOMIALS_PLANNER.PY")
cubic_spline_planner: отдельный код для работы с методом навигации (см. раздел "Описание навигационной части программы (одномерное пространство и математическое вычисление пути)" -> Навигация в одномерном пространстве (график) -> ФАЙЛ «CUBIC_SPLINE_PLANNER.PY»)

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

class QuarticPolynomial:
	def __init__(self, xs, vxs, axs, vxe, axe, time):
    	   self.a0 = xs
           self.a1 = vxs
           self.a2 = axs / 2.0
 
           A = np.array([[3 * time ** 2, 4 * time ** 3],
           [6 * time, 12 * time ** 2]])
           b = np.array([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]

Атрибуты:

a0 (float): Свободный член полинома, начальное положение.

a1 (float): Коэффициент при первой степени t, начальная скорость.

a2 (float): Коэффициент при второй степени t, половина начального ускорения.

a3 (float): Коэффициент при третьей степени t.

a4 (float): Коэффициент при четвертой степени t.

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

МЕТОД ПОСТРОЕНИЯ ТРАЕКТОРИИ

def calc_frenet_paths(c_speed, c_accel, c_d, c_d_d, c_d_dd, s0):
    frenet_paths = []
 
# generate path to each offset goal
for di in np.arange(-MAX_ROAD_WIDTH, MAX_ROAD_WIDTH, D_ROAD_W):
 
    # Lateral motion planning
    for Ti in np.arange(MIN_T, MAX_T, DT):
        fp = FrenetPath()
 
        # lat_qp = quintic_polynomial(c_d, c_d_d, c_d_dd, di, 0.0, 0.0, Ti)
        lat_qp = QuinticPolynomial(c_d, c_d_d, c_d_dd, di, 0.0, 0.0, Ti)
 
        fp.t = [t for t in np.arange(0.0, Ti, DT)]
        fp.d = [lat_qp.calc_point(t) for t in fp.t]
        fp.d_d = [lat_qp.calc_first_derivative(t) for t in fp.t]
        fp.d_dd = [lat_qp.calc_second_derivative(t) for t in fp.t]
        fp.d_ddd = [lat_qp.calc_third_derivative(t) for t in fp.t]
 
        # Longitudinal motion planning (Velocity keeping)
        for tv in np.arange(TARGET_SPEED - D_T_S * N_S_SAMPLE,
                            TARGET_SPEED + D_T_S * N_S_SAMPLE, D_T_S):
            tfp = copy.deepcopy(fp)
            lon_qp = QuarticPolynomial(s0, c_speed, c_accel, tv, 0.0, Ti)
 
            tfp.s = [lon_qp.calc_point(t) for t in fp.t]
            tfp.s_d = [lon_qp.calc_first_derivative(t) for t in fp.t]
            tfp.s_dd = [lon_qp.calc_second_derivative(t) for t in fp.t]
            tfp.s_ddd = [lon_qp.calc_third_derivative(t) for t in fp.t]
 
            Jp = sum(np.power(tfp.d_ddd, 2))  # square of jerk
            Js = sum(np.power(tfp.s_ddd, 2))  # square of jerk
 
            # square of diff from target speed
            ds = (TARGET_SPEED - tfp.s_d[-1]) ** 2
 
            tfp.cd = K_J * Jp + K_T * Ti + K_D * tfp.d[-1] ** 2
            tfp.cv = K_J * Js + K_T * Ti + K_D * ds
            tfp.cf = K_LAT * tfp.cd + K_LON * tfp.cv
 
            frenet_paths.append(tfp)
 
return frenet_paths

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

МЕТОД ПРЕОБРАЗОВАНИЯ ТРАЕКТОРИИ

def calc_global_paths(fplist, csp):
	for fp in fplist:
 
        # calc global positions
        for i in range(len(fp.s)):
            ix, iy = csp.calc_position(fp.s[i])
            if ix is None:
                break
            i_yaw = csp.calc_yaw(fp.s[i])
            di = fp.d[i]
            fx = ix + di * math.cos(i_yaw + math.pi / 2.0)
            fy = iy + di * math.sin(i_yaw + math.pi / 2.0)
            fp.x.append(fx)
            fp.y.append(fy)
 
        # calc yaw and ds
        for i in range(len(fp.x) - 1):
            dx = fp.x[i + 1] - fp.x[i]
            dy = fp.y[i + 1] - fp.y[i]
            fp.yaw.append(math.atan2(dy, dx))
            fp.ds.append(math.hypot(dx, dy))
 
        fp.yaw.append(fp.yaw[-1])
        fp.ds.append(fp.ds[-1])
 
        # calc curvature
        for i in range(len(fp.yaw) - 1):
            fp.c.append((fp.yaw[i + 1] - fp.yaw[i]) / fp.ds[i])
 
    return fplist

Параметры:

- fplist (list of FrenetPath objects): Список объектов путей в системе координат Френе.

- csp (CubicSpline2D object): Объект кубического сплайна для пути.

Этот метод преобразует пути из координат Френе в глобальные координаты. Он используется для преобразования траектории ровера в глобальные координаты.

МЕТОД ПРОВЕРКИ СТОЛКНОВЕНИЯ

def check_collision(fp, ob):
	for i in range(len(ob[:, 0])):
        d = [((ix - ob[i, 0]) ** 2 + (iy - ob[i, 1]) ** 2)
             for (ix, iy) in zip(fp.x, fp.y)]
 
        collision = any([di <= ROBOT_RADIUS ** 2 for di in d])
 
        if collision:
            return False
 
    return True

Параметры:

- fp (FrenetPath object): Объект пути в системе координат Френе.

- ob (numpy.ndarray): Массив препятствий, где каждое препятствие представлено координатами (x, y).

Этот метод проверяет столкновение траектории с препятствиями. Он используется для проверки безопасности траектории ровера.

МЕТОД ПРОВЕРКИ ПУТЕЙ И ОТСУТСТВИЕ ПРЕПЯТСТВИЙ

def check_paths(fplist, ob):
   
    ok_ind = []
    for i, _ in enumerate(fplist):
        if any([v > MAX_SPEED for v in fplist[i].s_d]):  # Max speed check
            continue
        elif any([abs(c) > MAX_CURVATURE for c in
                  fplist[i].c]):  # Max curvature check
            continue
        elif not check_collision(fplist[i], ob):
            continue
 
        ok_ind.append(i)
 
    return [fplist[i] for i in ok_ind]

Параметры:

- fplist (list of FrenetPath objects): Список объектов путей в системе координат Френе.

- ob (list of tuples): Список препятствий в формате (x, y).

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

МЕТОД ПЛАНИРОВАНИЯ ТРАЕКТОРИИ

def frenet_optimal_planning(csp, s0, c_speed, c_accel, c_d, c_d_d, c_d_dd, ob):
	fplist = calc_frenet_paths(c_speed, c_accel, c_d, c_d_d, c_d_dd, s0)
    fplist = calc_global_paths(fplist, csp)
    fplist = check_paths(fplist, ob)
 
    # find minimum cost path
    min_cost = float("inf")
    best_path = None
    for fp in fplist:
        if min_cost >= fp.cf:
            min_cost = fp.cf
            best_path = fp
 
    return best_path

Параметры:

- csp (CubicSpline2D object): Объект кубического сплайна, представляющий путь.

- s0 (float): Начальное значение длины дуги на сплайне.

- c_speed (float): Текущая скорость.

- c_accel (float): Текущее ускорение.

- c_d (float): Текущее боковое смещение.

- c_d_d (float): Текущая производная бокового смещения.

- c_d_dd (float): Текущая вторая производная бокового смещения.

- ob (list of tuples): Список препятствий в формате (x, y).

Этот метод планирует оптимальную траекторию в системе координат Френе. Он используется для планирования траектории ровера.

МЕТОД ГЕНЕРАЦИИ ЦЕЛЕВОГО КУРСА

def generate_target_course(x, y):
  csp = cubic_spline_planner.CubicSpline2D(x, y)
  s = np.arange(0, csp.s[-1], 0.1)
 
  rx, ry, ryaw, rk = [], [], [], []
  for i_s in s:
      ix, iy = csp.calc_position(i_s)
      rx.append(ix)
      ry.append(iy)
      ryaw.append(csp.calc_yaw(i_s))
      rk.append(csp.calc_curvature(i_s))
 
  return rx, ry, ryaw, rk, csp

Параметры:

- x (list): Список X-координат.

- y (list): Список Y-координат.

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

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

def calculate_relative_angle(x, center_x=360, max_angle=50):
	# Преобразование диапазона x (0-640) в угол (-60 до +60 градусов)
    relative_angle = (x - center_x)/(center_x/max_angle)
    # Ограничение угла пределами -60 до +60 градусов
    if relative_angle < -max_angle:
        relative_angle = -max_angle
    elif relative_angle > max_angle:
        relative_angle = max_angle
    return -relative_angle

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

def mass(xs, ys, tar):
	#...

МЕТОД ОБРАБОТКИ ДАННЫХ О МАССЕ

Параметры:

- xs (float): X-координата начальной точки.

- ys (float): Y-координата начальной точки.

- tar (float): Целевой угол в градусах.

Функция пытается получить данные с помощью метода `give_mass`. В случае ошибки

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

у которых расстояние до некоторой цели приблизительно равно 0.5 (с допуском 0.2).

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

def degres_to_mark(x, y, dxs, dys):
  dx = dxs - x
  dy = dys - y
  target_angle = math.degrees(math.atan2(dy, dx))
  return target_angle

Параметры:

- x (float): X-координата начальной точки.

- y (float): Y-координата начальной точки.

- dxs (float): X-координата конечной точки.

- dys (float): Y-координата конечной точки.

МЕТОД ЗАПУСКА РОВЕРА

def main(x, y, dx, dy, con=None):
	#...

Параметры:

- x (float): Начальная координата X объекта.

- y (float): Начальная координата Y объекта.

- dx (float): Конечная координата X объекта.

- dy (float): Конечная координата Y объекта.

- con (object): Объект управления, который может быть использован для отправки команд управления.

Функция `main` инициализирует начальное состояние объекта и планирует траекторию от начальной до конечной точки.

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

В процессе планирования функция может адаптировать траекторию, если обнаруживаются новые препятствия.

Результаты планирования визуализируются с помощью библиотеки Plotly, если установлен флаг `show_animation`.

Функция также поддерживает отправку команд управления через объект `con`, если он предоставлен.

КЛАСС ЗАПУСКА С ВНЕШНЕГО ФАЙЛА

class Run_Robot():
    def __init__(self,x,y,dx,dy,con):
        self.x=x
        self.y=y
        self.dx=dx
        self.dy=dy
        self.con=con
       
    def going_to_coor_aruco(self):
        main(self.x,self.y,self.dx,self.dy,self.con)

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

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

Поэтапно копируйте код в файл:

Установите необходимые библиотеки: в данном случае в Python не предустановлены следующие библиотеки: matplotlib, numpy, plotly, tornado. Если эти библиотеки у вас не установлены, следуйте следующим шагам:
1. Откройте терминал в Visual Studio Code (Terminal -> New Terminal или комбинацией Ctrl + Shift + `)
2. Пропишите в терминал данные команды (записывать и запускать поочередно!): pip install numpy; pip install matplotlib; pip install tornado
Запускать код нет смысла, так как он является алгоритмизацией движения ровера.

ПредыдущаяФАЙЛ «CONTACT_STATE.PY»СледующаяОписание навигационной части программы (одномерное пространство и математическое вычисление пути)

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