本系列文章是学习深蓝学院-移动机器人运动规划课程第五章最优轨迹生成 过程中所记录的笔记，本系列文章共包含四篇文章，依次介绍了微分平坦特性、无约束BVP轨迹优化、无约束BIVP轨迹优、 带约束轨迹优化等内容

   本系列文章链接如下：

   最优轨迹生成（一）—— 微分平坦

   最优轨迹生成（二）—— 无约束BVP轨迹优化

   最优轨迹生成（三）—— 无约束BIVP轨迹优化

   最优轨迹生成（四）—— 带约束轨迹优化

---

   1、全局路径规划与局部路径规划

   全局路径规划算法一般具有具备全局最优性、可以处理复杂的环境、很多时候只需要低阶信息，但全规划局算法在高维空间中效率较慢，全局规划算法在高维空间很难进行采样，很难满足动力学约束、高维空间收敛速度慢。

   局部路径规划算法一般具有局部最优性，可以处理复杂的动力学约束，局部规划算法常采用优化的方法，在高维空间中也可以快速的逼近局部最优解，收敛较快，但需要对动力学约束的显式建模、进行参数辨识、需要使用高阶信息

   基于上面对全局规划算法和局部规划算法的优缺点考虑，在进行轨迹规划时常分为前端和后端，前端往往是在低维空间搜索一个低维的可行路径或可行域，然后再通过后端做优化

---

   2、轨迹

   “轨迹是足够光滑的路径”这种认知是不准确的，轨迹是需要包含时间信息的，它是一种时间参数化的路径，轨迹也不一定是光滑的，轨迹包含时间和空间两种特性。

   如果对一个二维的位置进行时间参数化，就是一个二维的位置轨迹，我们也可以对二维的位置、速度、姿态角 进行时间参数化，这样得到的轨迹就是对位置速度姿态的轨迹。同理可拓展到三维情况

   轨迹中常说的光滑一般不仅仅指视觉上看起来是光滑，而是指该轨迹要满足动力学约束（常使用微分方程表示$\dot{x}=f(x,u)$），还要去最小化一个能量泛函$\min {\int }_0^T\mathcal{L}(x,u)\mathrm{d}t$

---

   3、为什么需要轨迹优化？

   向前面介绍的Hybrid A * 算法等满足动力学的规划算法，已经可以得到满足动力学约束的路径，为什么还需要进行轨迹优化呢？因为，我们做轨迹优化，不仅仅要考虑动力学约束，还要考虑时间最优、能量最优、硬件限制、其他任务等。

---

---

   4、微分平坦特性 （Differential Flatness）

   微分平坦是一种常微分方程，描述了无人系统的一种特性。这个常微分方程描述了状态和输入之间的关系，如果一个系统称为微分平坦系统，那么我们可以找到一个变量$z\in {\mathbb{R}}^m$（称为平坦输出），它的有限导数可以唯一地确定所有状态和输入：

   $$\begin{array}{rl}x& ={\Psi }_x(z,\dot{z},\cdots ,z^{(s-1)}),\\ u& ={\Psi }_u(z,\dot{z},\cdots ,z^{(s)}).\end{array}$$

   所以，我们定义一个映射，使得z轨迹的0阶导、1阶导、2阶导…有限阶导数来唯一的确定系统的所有状态和输入，如上所示。

   将上面的x和u代入到下式中,可以得到一个恒等式，拥有这个特性的系统称为微分平坦系统。

   $$\begin{array}{c}\dot{x}=f(x)+g(x)u,\\ f:{\mathbb{R}}^n\mapsto {\mathbb{R}}^n,g:{\mathbb{R}}^n\mapsto {\mathbb{R}}^n,x\in {\mathbb{R}}^n,u\in {\mathbb{R}}^m,\mathrm{rank}(g)=m.\end{array}$$

---

   微分平坦度消除掉微分约束，所以我们可以在规划的时候不考虑动力学约束，规划z的轨迹，当得到z的轨迹后，只要它具有零阶到s-1阶的导且连续，那么由其算出的x和u，必然满足动力学约束$\dot{x}=f(x)+g(x)u$

---

   像常见的四旋翼无人机多旋翼的无人机，空气阻力和速度成比例关系、电机朝上或者角度满足一定关系时，都是满足微分平坦特性的。

---

   我们利用微分平坦特性来避免处理无人机的微分等式约束。

---

   假设无人机的状态是由三维的位置r、速度v、角速率w、姿态矩阵R组成

   x = { r , v , R , ω } ∈ R 3 × R 3 × S O ( 3 ) × R 3 x=\{r,v,R,\omega\}\in\mathbb{R}^3\times\mathbb{R}^3\times\mathrm{SO}(3)\times\mathbb{R}^3 x={r,v,R,ω}∈R3×R3×SO(3)×R3

   控制输入选为总推力f和三轴扭矩$\tau$

   u = { f , τ } ∈ R ≥ 0 × R 3 u=\{f,\tau\}\in\mathbb{R}_{\geq0}\times\mathbb{R}^3 u={f,τ}∈R≥0​×R3

   动力学方程，常微分$\dot{x}=f(x)+g(x)u$描述了f函数与u的关系，其具体表达式如下，第一个公式表示位置的微分是速度，第二个公式是无人机的合力方程$F=m{v}^2$,第三个公式是姿态旋转矩阵的微分与角速度的关系，第四个公式是欧拉公式加一个增量

   $$\{\begin{array}{rl}\dot{r}& =v,\\ m\dot{v}& =-mg{e}_3-RD{R}^{\mathrm{T}}\sigma (\Vert v\Vert )v+Rf{e}_3,\\ \dot{R}& =R\hat{\omega },\\ M\dot{\omega }& =\tau -\omega \times M\omega -A(\omega )-B(R^{\mathrm{T}}v).\end{array}$$

   微分平坦输出选择为无人机三轴位置r和偏航角ψ

   z = { r , ψ } ∈ R 3 × S O ( 2 ) z=\{r,\psi\}\in\mathbb{R}^3\times\mathrm{SO}(2) z={r,ψ}∈R3×SO(2)

   下面来看如何用微分平坦输出z及其有限阶导数来表示x和u，即如何用r、w及它们的有限阶导数表示r、v、R、w、f 、$\tau$

   状态x中的r和v与微分平坦输出z的关系可以很容易得到，如下所示：

   $$r=rv=\dot{r}$$

   把前面的牛顿方程乘以机体坐标系下的x轴和y轴后可以得到下式

   x b = R e 1 , y b = R e 2 ( R e i ) T m v ˙ = ( R e i ) T ( − m g e 3 − R D R T σ ( ∥ v ∥ ) v + R f e 3 ) ,   ∀ i ∈ { 1 , 2 } \begin{gathered}x_b=Re_1,y_b=Re_2\\(Re_i)^\mathrm{T}m\dot{v}=(Re_i)^\mathrm{T}\left(-mge_3-RDR^\mathrm{T}\sigma(\|v\|)v+Rfe_3\right),\mathrm{~}\forall i\in\{1,2\}\end{gathered} xb​=Re1​,yb​=Re2​(Rei​)Tmv˙=(Rei​)T(−mge3​−RDRTσ(∥v∥)v+Rfe3​), ∀i∈{1,2}​

   化简后，得到下式

   ( R e i ) T ( v ˙ + d h m σ ( ∥ v ∥ ) v + g e 3 ) = 0 ,   ∀ i ∈ { 1 , 2 } . (Re_i)^{\mathrm{T}}(\dot{v}+\frac{d_h}m\sigma(\|v\|)v+ge_3)=0,\mathrm{~}\forall i\in\{1,2\}. (Rei​)T(v˙+mdh​​σ(∥v∥)v+ge3​)=0, ∀i∈{1,2}.

   这个公式的几何意义是机体坐标系下的x轴和y轴都与$\frac{d_h}{m}\sigma (\Vert v\Vert )v+g{e}_3$垂直，所以$\frac{d_h}{m}\sigma (\Vert v\Vert )v+g{e}_3$必然垂直于机体坐标系下的x轴和y轴构成的平面，即平行于机体坐标系下的z轴

   $$\begin{array}{rl}{x}_b& \perp (\dot{v}+\frac{d_h}{m}\sigma (\Vert v\Vert )v+g{e}_3)\\ y_b& \perp (\dot{v}+\frac{d_h}{m}\sigma (\Vert v\Vert )v+g{e}_3)\end{array}$$

   由于推力和机体坐标系下的z轴方向相同，所以可以将其进行归一化

   $$z_b=\mathcal{N}(\dot{v}+\frac{d_h}{m}\sigma (\Vert v\Vert )v+g{e}_3),\mathcal{N}(x):=x/\Vert x{\Vert }_2$$

   至此，我们成功通过r、r的一阶导、r的二阶导获得了机体坐标系下的z轴方向向量$z_b$

   我们可以利用机体朝上来获得推力

   $$(R{e}_3{)}^{\text{T}}m\dot{v}=(R{e}_3{)}^{\text{T}}\left(-mg{e}_3-RD{R}^{\text{T}}\sigma (\Vert v\Vert )v+Rf{e}_3\right)$$

   由上式可得

   $$f=z_b^{\mathrm{T}}(m\dot{v}+d_v\sigma (\Vert v\Vert )v+mg{e}_3)$$

   至此，我们成功用微分平坦输出z表示了推力f

---

   现在，偏航航向和机体坐标下的z轴方向都已知，偏航旋转四元数为

   $$q_{\psi }={\left(\cos (\psi /2),0,0,\sin (\psi /2)\right)}^{\mathrm{T}}$$

   倾斜旋转四元数为

   $$q_z=\frac{1}{\sqrt{2(1+z_b(3))}}(1+z_b(3),-z_b(2),z_b(1),0{)}^{\mathrm{T}}$$

   又因为，$q=q_z\otimes q_{\psi }$ 所以可得：

   $$q=\frac{1}{\sqrt{2(1+z_b(3))}}\left(\begin{array}{c}(1+z_b(3))\cos (\psi /2)\\ -z_b(2)\cos (\psi /2)+z_b(1)\sin (\psi /2)\\ z_b(1)\cos (\psi /2)+z_b(2)\sin (\psi /2)\\ (1+z_b(3))\sin (\psi /2),\end{array}\right)$$

   至此，我们获得了用微分平坦输出z表示的姿态

---

   现在旋转是已知的

   $$\dot{R}=R\hat{\omega }$$

   这意味着

   $$\omega =(R^{\mathrm{T}}\dot{R}{)}^{\vee }$$

   上式等效于：

   $$\omega =2(q_z\otimes q_{\psi }{)}^{-1}\otimes ({\dot{q}}_z\otimes q_{\psi }+q_z\otimes {\dot{q}}_{\psi })$$

   代入倾斜旋转四元数和偏航旋转四元数给出下式

   $$\omega =\left(\begin{array}{c}{\dot{z}}_b(1)\sin (\psi )-{\dot{z}}_b(2)\cos (\psi )-{\dot{z}}_b(3)(z_b(1)\sin (\psi )-z_b(2)\cos (\psi ))/(1+z_b(3))\\ {\dot{z}}_b(1)\cos (\psi )+{\dot{z}}_b(2)\sin (\psi )-{\dot{z}}_b(3)(z_b(1)\cos (\psi )+z_b(2)\sin (\psi ))/(1+z_b(3))\\ (z_b(2){\dot{z}}_b(1)-z_b(1){\dot{z}}_b(2))/(1+z_b(3))+\dot{\psi },\end{array}\right)$$

   其中：

   $$\begin{array}{c}{\dot{z}}_b=\frac{d_h}{m}\mathcal{DN}(\dot{v}+\frac{d_h}{m}\sigma (\Vert v\Vert )v+g{e}_3{)}^T(\frac{m}{d_h}\ddot{v}+\sigma (\Vert v\Vert )\dot{v}+\dot{\sigma }(\Vert v\Vert )\frac{v^{\mathrm{T}}\dot{v}}{\Vert v\Vert }v),\\ \mathcal{DN}(x):=\frac{1}{\Vert x\Vert }\left(\mathrm{I}-\frac{x{x}^{\mathrm{T}}}{x^{\mathrm{T}}x}\right).\end{array}$$

   到这里几乎所有的量（除了力矩）都被微分平台输出z及其有限阶导数表示出来了

   上面给出了w的表达式，对其求微分，利用平坦输出及其有限阶导数，可得

   $$\tau =M\dot{\omega }+\omega \times M\omega +A(\omega )+B(R^{\mathrm{T}}v)$$

---

   至此， 解析完成，在微分等式所确定的平面上进行优化算法是很困难的，通过微分平坦，我们得到一个关于微分平坦输出z的平直的空间，他没有任何的等式和曲面约束，只保留了不等式约束，在该空间中做优化是容易的，且得到解一定是满足微分等式的。

---

   我们得到微分平坦输出z的轨迹后，我们可以利用z在任何时刻的一阶导、二阶导、三阶导… 来直接调用上面推导出的对应的x和u的表达式（红色公式）获得最终的x和u的轨迹，给控制器进行跟踪，以上也是基于微分平坦的规划和控制的基本思路。

   平坦输出轨迹需要满足一定的光滑性，我们可以利用样条或者其他方法进行拼接构成，其需要进行参数化，因此，平坦输 z = { r , ψ } ∈ R 3 × S O ( 2 ) z=\{r,\psi\}\in\mathbb{R}^3\times\mathrm{SO}(2) z={r,ψ}∈R3×SO(2)在出空间中的轨迹为：

   $$z(t):[0,T]\mapsto {\mathbb{R}}^3\times \mathrm{SO}(2)$$

   通过样条参数化平坦输出轨迹的优点如下：

   •容易确定平滑准则

   •容易和封闭形式的导数计算

   •解耦三维轨迹生成

   •高数值稳定性

---

---

   参考资料：

   1、深蓝学院-移动机器人运动规划

---