轨迹规划理论(一)微分平坦

本系列笔记主要为笔者对轨迹规划的基础理论归纳整理，方便学习，如有问题，望各位大佬指出。

绪论

轨迹规划时常分为前端和后端，前端往往是在低维空间搜索一个低微的可行路径(图搜索法、随机采样法)或者可行域；然后再后端优化。
轨迹的定义：一种时间参数化的路径。轨迹的解析中要包含时间信息，轨迹不一定光滑(如下图)，但其需要包含时间和空间两种特性。

二维轨迹：(位置、速度、偏航yaw)
$$\begin{gathered} [x(t),y(t)]:[0,T]\to {\mathbb{R}}^2 \\ [x(t),y(t),v_x(t),v_y(t),\varphi (t)]:[0,T]\to {\mathbb{R}}^4\times SO(2) \end{gathered}$$
三维轨迹：(位置、速度、旋转、角速度)
$$\begin{gathered} [x(t),y(t),z(t),\varphi (t)]:[0,T]\to {\mathbb{R}}^3\times SO(2) \\ [r(t),v(t),R(t),w(t)]:[0,T]\to {\mathbb{R}}^6\times SO(3)\times {\mathbb{R}}^3 \end{gathered}$$

微分平坦性(differential flatness)

文献[1]中最早在非线性系统中提出了微分平坦的概念，[3-4]将该概念应用到了移动机器人的轨迹规划当中，该性质使得能从平坦输出的有限导数中恢复平坦系统的完整状态和输入。
微分平坦性是一种常微分方程，描述了无人系统的一种特性。这个常微分方程描述了状态和输入之间的关系，如果一个系统称为微分平坦系统，那么可以找到一个变量$Z\in {\mathbb{R}}^m$（称之为平坦输出，flat input），它的有限导数可以唯一确定所有状态和输入：
$$\begin{gathered} x={\Phi }_x(z,\dot{z},...,z^{(s-1)}) \\ u={\Phi }_u(z,\dot{z},...,z^{(s)}) \end{gathered}$$
将上式代入动力学系统，就得到了微分平坦系统。
$$\dot{x}=f(x)+g(x)u$$
如此一来，微分平坦系统消掉了微分约束，此后的规划中不用考虑动力学约束，规划出轨迹Z，只要它具有零阶到s-1阶可导且连续，那么由其算出的x和u，必然满足动力学约束。

无人机的微分平坦性

可以参考文献[5]，给出了四旋翼无人机的微分平坦性的证明。
定义无人机在三维空间中状态量是位置r、速度v、角速度w、姿态矩阵R
x = { r , v , R , w } ∈ R 3 × R 3 × S O ( 3 ) × R 3 x=\{r,v,R,w\}\in\R^3\times\R^3\times SO(3)\times\R^3 x={r,v,R,w}∈R3×R3×SO(3)×R3
控制输入选为总推力$f$和三轴扭矩$\tau$
u = { f , τ } ∈ R ≥ 0 × R 3 u=\{f,\tau\}\in\R_{\ge0}\times\R^3 u={f,τ}∈R≥0​×R3
动力学微分方程，依次为位置微分为速度；无人机的合力方程；姿态旋转矩阵的微分与角速度的关系；欧拉公式。

微分平坦输出为无人机的位置和偏航角
z = { r , ϕ } ∈ R 3 × S O ( 2 ) z=\{r,\phi\}\in\R^3\times SO(2) z={r,ϕ}∈R3×SO(2)
微分平坦输出证明：

易得：
$$r=r,v=\dot{r}$$
将合力方程投影到机体坐标上，得：

化简得如下式，则可推得机体的x轴和y轴都与$\frac{d_h}{m}\sigma (||v||)v+g{e}_3$垂直，则机体z轴与之平行

而推力和机体坐标系下的z轴方向相同，可以通过对上式归一化获得$z_b$
$$z_b=\mathrm{N}(\dot{v}+\frac{d_h}{m}\sigma (||v||)v+g{e}_3),\mathrm{N}(x):=x/||x|{|}_2$$
将合力方程投影到机体z轴上，得到推力的微分平坦表达：
$$f=z_b^T(m\dot{v}+d_v\sigma (||v||)v+mg{e}_3)$$
偏航角获得偏航四元数：

由Z轴的向量方向可推得倾转旋转四元数：

旋转四元数$q=q_z\otimes q_{\varphi }$，则获得了姿态的平坦微分输出表示

现在旋转已知，则$\dot{R}=R\hat{w}$，则$w=(R^T\dot{R}{)}^{\vee }$，推得：

最后获得扭矩$\tau$：

参考博客

参考文献

[1] M. Fliess, J. L´evine, P. Martin, and P. Rouchon, “Flatness and defect of non-linear systems: Introductory theory and examples,” International Journal of Control, vol. 61, no. 6, pp. 1327–1361, 1995.
[2] M. J. Van Nieuwstadt and R. M. Murray, “Real-time trajectory generation for differentially flat systems,” International Journal of Robust and Nonlinear Control, vol. 8, no. 11, pp. 995–1020, 1998.
[3] P. Martin, R. M. Murray, and P. Rouchon, “Flat systems, equivalence and trajectory generation,” California Institute of Technology, Pasadena, Calif, USA, Tech. Rep. CDS 2003-008, 2003.
[4] J.-C. Ryu and S. K. Agrawal, “Differential flatness-based robust control of mobile robots in the presence of slip,” The International Journal of Robotics Research, vol. 30, no. 4, pp. 463–475, 2011.
[5] Mellinger D , Kumar V .Minimum snap trajectory generation and control for quadrotors[J].IEEE, 2011.DOI:10.1109/ICRA.2011.5980409.