traj.py - master - Logrus/CopterControl

traj.py
109 строк4 KB
Nikolay Nosorev
Initial commit
04 окт 2025, 16:17
04 окт 2025, 16:1799ef28c
import numpy as np
import matplotlib.pyplot as plt
from scipy.interpolate import CubicSpline
from scipy.optimize import minimize

# ------------------------------
# 1. Контрольные точки
waypoints = np.array([
    [0, 0, 0],
    [5, 2, 1],
    [10, 0, 0],
    [5, -3, -1],
    [0, 0, 0]
])
n_points = len(waypoints)
t_points = np.linspace(0, 1, n_points)

# ------------------------------
# 2. Кубический сплайн
cs_x = CubicSpline(t_points, waypoints[:, 0], bc_type='clamped')
cs_y = CubicSpline(t_points, waypoints[:, 1], bc_type='clamped')
cs_z = CubicSpline(t_points, waypoints[:, 2], bc_type='clamped')

# ------------------------------
# 3. Snap-оптимизированная траектория (7-й степени)
# Простейшая аппроксимация через минимум интеграла четвертой производной

def poly7(t, coeffs):
    # coeffs: [a0..a7]
    return sum([coeffs[i] * t**i for i in range(8)])

def poly7_deriv(t, coeffs, order=1):
    result = 0
    for i in range(order, 8):
        term = coeffs[i]
        for k in range(order):
            term *= (i - k)
        result += term * t**(i - order)
    return result

snap_traj_coeffs = []

for dim in range(3):
    coeffs_list = []
    for i in range(n_points - 1):
        t0, t1 = 0, 1  # нормализуем сегмент до [0,1]
        p0, p1 = waypoints[i, dim], waypoints[i+1, dim]

        # минимизируем интеграл четвертой производной по t
        def objective(c):
            # интеграл snap^2 ? sum(snap(t_i)^2)
            ts = np.linspace(t0, t1, 20)
            return np.sum([poly7_deriv(t, c, 4)**2 for t in ts])

        # условия: p(t0)=p0, p(t1)=p1, скорость и ускорение ноль на концах сегмента
        def constraints(c):
            return [
                poly7(c[0], c) - p0,  # позиция на начале сегмента
                poly7(c[1], c) - p1,  # позиция в конце сегмента
            ]

        # грубое приближение: линейные коэффициенты
        c0 = np.zeros(8)
        c0[0] = p0
        c0[1] = p1 - p0
        res = minimize(objective, c0)
        coeffs_list.append(res.x)
    snap_traj_coeffs.append(coeffs_list)

# ------------------------------
# 4. Визуализация
fig = plt.figure(figsize=(12,6))
ax = fig.add_subplot(121, projection='3d')
t_plot = np.linspace(0, 1, 200)

# Кубический сплайн
ax.plot(cs_x(t_plot), cs_y(t_plot), cs_z(t_plot), label='Cubic Spline', color='blue')

# Snap-оптимизированная траектория
pos_snap = []
for i, t in enumerate(t_plot*(n_points-1)):
    seg = int(np.floor(t))
    if seg >= n_points-1: seg = n_points-2
    t_local = t - seg
    x = poly7(t_local, snap_traj_coeffs[0][seg])
    y = poly7(t_local, snap_traj_coeffs[1][seg])
    z = poly7(t_local, snap_traj_coeffs[2][seg])
    pos_snap.append([x,y,z])
pos_snap = np.array(pos_snap)
ax.plot(pos_snap[:,0], pos_snap[:,1], pos_snap[:,2], label='Snap-optimized', color='red')

ax.scatter(waypoints[:,0], waypoints[:,1], waypoints[:,2], color='black', label='Waypoints')
ax.set_title("3D Trajectory")
ax.legend()

# ------------------------------
# Позиция по времени
ax2 = fig.add_subplot(122)
ax2.plot(t_plot, cs_x(t_plot), label='Spline X', color='blue')
ax2.plot(t_plot, cs_y(t_plot), label='Spline Y', color='green')
ax2.plot(t_plot, cs_z(t_plot), label='Spline Z', color='cyan')

ax2.plot(t_plot, pos_snap[:,0], '--', label='Snap X', color='red')
ax2.plot(t_plot, pos_snap[:,1], '--', label='Snap Y', color='orange')
ax2.plot(t_plot, pos_snap[:,2], '--', label='Snap Z', color='magenta')
ax2.set_title("Position vs Time")
ax2.legend()

plt.show()