基于Minimum Snap的轨迹生成/优化

---

前言

可以看看我的这几篇Blog1，Blog2，Blog3。

把路径从相对低维的空间去转换到全维度的状态空间，就是轨迹生成/优化，生成一条动力学可行，光滑的路径是路径规划的终极目标。

为什么要生成光滑的路径原因：

微分平坦

平坦性：可以类比为线性系统当中的能控性，平坦性的定义，核心就是，能取平坦输出及其各阶导数$y_f,{\dot{y}}_f,{\ddot{y}}_f,\dots ,y_f^{(k)}$为微分参数，作为基向量，来代数地表示原有的状态变量x以及输入变量u，要满足这个条件，意味着平坦输出的各阶导数线性无关，也就是说不存在这样一个线性微分方程。
具体可以参考善道大佬的这篇解释

举例：无人机系统

可以把机器人系统的全维度状态空间的Planning问题转化为Flat outputs（平坦的输出空间）里的问题，来进行轨迹规划。

一些要求：

---

---

无人机的动力学描述：
牛顿方程反应的是力的关系，欧拉方程反应的是转动的关系。

---

---

Tips:无人机永远只能产生垂直于自己机体桨平面的力，且没有机头这个概念。

---

---

需求：根据无人机的基本公式，去推把无人机的12维状态空间，给压缩到(x,y,z,Yaw)的平坦输出的微分空间里。无人机做Planning，要不要对姿态做Planning？其姿态肯定是与加速度紧紧绑在一起的，就是因为无人机和的推力永远只能产生垂直于自己机体桨平面。所以对于无人机的路径规划，姿态和加速度两者只能去其一，所以选择了加速度Planning，这样就把欧拉角干掉了（因为欧拉角还有旋转矩阵的因素）（除了Yaw角，Pitch和Roll都被干掉了）（ 一个姿态产生一个推力，现在沿着Yaw角进行旋转，但产生的力还是垂直桨平面的力，是独立的，所以还是要单独规划Yaw角），所以就在加速度的空间上进行Planning。

---

---

因为位置的组合，以及其导数的组合已经在平坦空间当中了，所以无需证明。所以x,y,z及其导数就搞定了。

---

---

t向量：无人机体坐标系的Z轴，其方向是t这个向量的方向，就表示世界坐标系下三个轴的加速度，表示和加速方向，与姿态紧紧绑定的。
现在body系下的Z轴方向（${\mathbf{z}}_B=\frac{\mathbf{t}}{\Vert \mathbf{t}\Vert },\mathbf{t}={\left[{\ddot{\mathbf{\sigma }}}_1,{\ddot{\mathbf{\sigma }}}_2,{\ddot{\mathbf{\sigma }}}_3+g\right]}^T$）就已经表示成了Flatness Outputs（$\mathbf{\sigma }=[x,y,z,\psi {]}^T$）的代数组合的形式。
无人机的姿态实际上就是这个ody系下三个轴的方向，所以说搞定了${\mathbf{x}}_B,{\mathbf{y}}_B,{\mathbf{z}}_B$其实就已经搞定了无人机自己旋转的三个角了（Pitch，Roll，Yaw），这个是一一对应的关系，组合起来就是一个旋转矩阵。为了方便看${\mathbf{x}}_B,{\mathbf{y}}_B$，引入了一个中间的坐标系C，C系与W系（World系）只差了一个偏航角，所以说${\mathbf{x}}_C$肯定可以只用Yaw角来表示的，${\mathbf{x}}_C={\left[\cos {\mathbf{\sigma }}_4,\sin {\mathbf{\sigma }}_4,0\right]}^T$。利用C系${\mathbf{x}}_C$，以及B系（Body系）的${\mathbf{z}}_B$，运用叉乘的关系，就可以算出${\mathbf{y}}_B$，以及${\mathbf{x}}_B$，${\mathbf{R}}_B$。
$${\mathbf{y}}_B=\frac{{\mathbf{z}}_B\times {\mathbf{x}}_C}{\Vert {\mathbf{z}}_B\times {\mathbf{x}}_C\Vert },{\mathbf{x}}_B={\mathbf{y}}_B\times {\mathbf{z}}_B{\mathbf{R}}_B=\left[\begin{array}{lll}{\mathbf{x}}_B& {\mathbf{y}}_B& {\mathbf{z}}_B\end{array}\right]$$

---

---

现在，如何把角速度表示成(x,y,z,Yaw)的线性组合？

---

---

$\ddot{p}$的就是加速度$a$，之后对方程两边求导，就得出了上式$\dot{a}$。
${\mathbf{\omega }}_{BW}$就是在世界系下看机体的角速度。
$${\mathbf{\omega }}_{BW}={\omega }_x{\mathbf{x}}_B+{\omega }_y{\mathbf{y}}_B+{\omega }_z{\mathbf{z}}_B$$
就是在三个轴上三个角速度的分量。
${\dot{u}}_1$就是在垂直于无人机桨平面的推力合力在Body系方向上的分量后的求导项。
$${\mathbf{h}}_{\omega }={\mathbf{\omega }}_{BW}\times {\mathbf{z}}_B=\frac{m}{u_1}\left(\dot{\mathbf{a}}-\left({\mathbf{z}}_B\cdot \dot{\mathbf{a}}\right){\mathbf{z}}_B\right)$$
${\mathbf{\omega }}_{BW}\times {\mathbf{z}}_B$这个叉乘就用上面的${\mathbf{\omega }}_{BW}$表达式代入，再一个个用右手叉乘展开就行，例如$x_B\times {\mathbf{z}}_B$，右手叉乘，得出的就是一个反向的$y_B$，其余的以此类推，最后${\mathbf{h}}_{\omega }$是所有量都满足我们的要求的，就可以得出：
$${\omega }_x=-{\mathbf{h}}_{\omega }\cdot {\mathbf{y}}_B,{\omega }_y={\mathbf{h}}_{\omega }\cdot {\mathbf{x}}_B$$
Z轴上的分量要特殊处理，如下所示

---

---

所以，综上，所有的12个变量，位置速度角速度欧拉角都搞定了，成功的使用Flatness Outputs中的(x,y,z,Yaw)进行了表示。

---

---

综上，对无人机的运动规划只需要在Flatness Outputs中的(x,y,z,Yaw)进行。

---

Planning-Control Loop

---

---

位置控制器解算出$u_1$，姿态控制器解算出$u_2，u_3，u_4$三个方向的力矩。规划就只是规划了红框的这一部分。

---

轨迹表示形式

---

Minimum-Snap轨迹生成

一段的情况，一维的生成光滑的轨迹，最简单情况只有一段的轨迹，就是一个典型的BVP问题。

---

---

---

具体可看上一篇OBVP的blog。

---

多段的情况，

---

---

对每一段假设都具备一个多项式函数，不能只假设每个位置的速度以及加速度为0了，还得首尾给个两点的速度值。那么速度与加速度是超参数，怎么指定最好？
现在用无人机系统举例，如何求解最佳的$v$和$a$。

---

---

反复强调其加速度和姿态是绑定在一起的，角速度和推力是绑定在一起的。

---

---

所以说最小化jerk（加速度的导数）就能最小化角速度，就能最小化推力，就能最小化视差率。
最小化snap，就能使得推力的变化变的更加平坦。

---

---

现在目标已经明确了，就是要计算每一段轨迹的多项式的系数，使得总体的轨迹满足首末约束，经过中间点，在中间点，首末段的高阶导数都是一样的。使得整体轨迹可以光滑且最小化其jerk和snap。

---

---

每段轨迹都是多项式。
若每段轨迹的参数化系数都一样的，比如都初始化最高幂次一样，则会使问题简化。

---

---

---

想要轨迹几阶连续与几阶光滑是不一样的，k阶光滑意味着k+1阶连续。
保证光滑与连续性与想最小化轨迹的几阶导数也是彼此独立的，没有一一对应的联系。

---

---

---

---

---

每段轨迹的多项式系数就可以用OBVP来求解。

---

---

---

总结

经过以上对Minimum Snap的推导就转化成了凸优化问题，凸优化问题先不肝了哈哈哈，今天就到这吧23333.