自动驾驶学习记录

五次多项式

$$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}|$$

推导过程类似，不赘述