战斗包子

自动驾驶学习记录

2024年09月25日 926 字大概 5 分钟

五次多项式

Jerk = \frac{da}{dt}

其加速度的导数，其绝对值越小，则越舒适

因此自动驾驶的优化目标是

find \ f(t).\ \ s.t. \ min \int_0^T(\frac{d^3f}{dt^3})^2dt

subjecto\ to\ s(0)=s_0,\dot s(0)=v_0,\ddot s(0) = a_0

s(T)=s_n,\dot s(T)=v_n,\ddot s(T) = a_n

这是一个泛函，要求得一个最佳的f(t)，使积分最小。为了匹配六个边界条件，需要五次多项式。

将f泰勒展开有

s = f(t) = f(0) + f'(1)t + \frac{f''(0)}{2!}t^2 + \frac{f'''(0)}{3!}t^3+...

= s_0+v_0t+a_0t/2+Jerk(0)\times t^3/6+...

约束可以化为

\begin{aligned}s(T)-s(0)=s_n-s_0 = \int _0^T\dot f(t)dt = C_0\\ \dot s(T)-\dot s(0)=v_n-v_0 =\int _0^T\ddot f(t)dt = C_1\\ \ddot s(T)-\ddot s(0)=a_n-a_0= \int _0^Tdddot f(t)dt = C_2\end{aligned}

min\ \int _0^T dddot f^2+ \lambda_1(\dot f-C_0/T) + \lambda_2(\ddot f-C_1/T) + \lambda_3(dddot f-C_2/T)dt

其应该满足 [EL方程 | ### 欧拉拉格朗日方程] ，应有

\frac{d^3}{dt^3}(\lambda_3+2dddot f) = 0, f^{(6)}=0

欧拉拉格朗日方程

https://zhuanlan.zhihu.com/p/698294026

\int

\frac{\partial L}{\partial g}-\frac{d}{dx}(\frac{\partial L}{\partial g'})=0

广义EL

\frac{\partial L}{\partial f} - \frac{d}{dt}\frac{\partial L}{\partial \dot f}+ \frac{d^2}{dt^2}\frac{\partial L}{\partial \ddot f} - \frac{d^3}{dt^3}\frac{\partial L}{\partial dddot f} = 0

FEMSmoother

![20241024163936.png]

![20241024163947.png]

平滑算法代价的计算

平滑代价

|P_1P_3| = |P_1P_2+P_1P_0| = |(x_2+x_0-2x_1,y_2+y_0-2y_1)|

设输入向量为

[x] =\begin{bmatrix}x_0\\y_0\\x_1\\y_1\\x_2\\y_2\end{bmatrix}

则应有

cost = \begin{bmatrix}1&0&-2&0&1&0\\ 0&1&0&-2&0&1 \end{bmatrix} \begin{bmatrix}x_0\\y_0\\x_1\\y_1\\x_2\\y_2\end{bmatrix}=Ax

使用距离平方做代价则有

|P_1P_3|^2 = x^TA^TAx

如果拓展到n个点，则有

A = \begin{bmatrix} 1&0&-2&0&1&0\\ 0&1&0&-2&0&1\\ 0&0&1&0&-2&0&1&0\\ 0&0&0&1&0&-2&0&1\\ 0&0&0&0&1&0&-2&0&1&0\\ 0&0&0&0&0&1&0&-2&0&1\\ \end{bmatrix}

变成一个上三角矩阵，其列数为2n，行数为2n-4

紧凑代价

|P_1P_2| + |P_2P_3|

几何相似代价

|P_1P_{1r}|+|P_2P_{2r}|+|P_3P_{3r}|

推导过程类似，不赘述

本文作者：战斗包子
本文链接：https://paipai121.github.io/2024/09/25/学习记录/自动驾驶学习记录/
版权声明：本文采用 CC BY-NC-SA 3.0 CN 协议进行许可

学习自动驾驶