无人机的微分平坦性详细推导

目标：将无人机的状态一个12维的向量用4维表示
即将无人机的位置，方向，速度和角速度用位置(X,Y,Z)和绕z轴的ψ\psiψ角表示

1.无人机非线性动力学

2.机体坐标系表示

无人机的推力始终与机架垂直因此
ZB=t∥t∥Z_B=\frac{t}{\|t\|}ZB=∥t∥t,t=[x¨,y¨,z¨+g]Tt=[\ddot x,\ddot y,\ddot z + g]^{\mathbf T}t=[x¨,y¨,z¨+g]T
然后我们通过一个中间坐标系来求YBY_BYB,坐标系如图1的，XC，YC,ZCX_C，Y_C,Z_CXC，YC,ZC。
XC=[cosψ,sinψ,0]TX_C=[cos\psi,sin\psi,0]^{\mathbf T}XC=[cosψ,sinψ,0]T
通过XCX_CXC我们得到YBY_BYB
YB=ZB×XC∥ZB×XC∥Y_B=\frac{Z_B \times X_C}{\|Z_B \times X_C\|}YB=∥ZB×XC∥ZB×XC(因为XCX_CXC和XB,ZBX_B,Z_BXB,ZB在一个平面内)
XB=YB×ZBX_B=Y_B \times Z_BXB=YB×ZB
RB=[XBYBZB]R_B=[X_B \ Y_B \ Z_B]RB=[XB YB ZB]（它表示的旋转可以由ϕ,θ,ψ\phi,\theta,\psiϕ,θ,ψ的旋转矩阵表示，因此这三个角可以被消除掉）

3.角速度替换

对下式两边求导，（对一个旋转矩阵求导，等于它的角速度乘以本身这个坐标系。旋转矩阵求导看这里: 旋转矩阵求导

ωBW=ωxXB+ωyYB+ωzZB\omega_{BW}=\omega_xX_B+\omega_yY_B+\omega_zZ_BωBW=ωxXB+ωyYB+ωzZB 表示体坐标系下的角速度
上式中的

将它带入上式得到

这里的hωh_\omegahω的所有量都是已知量
hω=ωBW×ZB=(ωxXB+ωyYB+ωzZB)×ZBh_\omega=\omega_{BW}\times Z_B=(\omega_xX_B+\omega_yY_B+\omega_zZ_B)\times Z_Bhω=ωBW×ZB=(ωxXB+ωyYB+ωzZB)×ZB
因为
XB×ZB=−YBX_B\times Z_B=-Y_BXB×ZB=−YB
YB×ZB=XBY_B\times Z_B=X_BYB×ZB=XB
ZB×ZB=0→Z_B\times Z_B=\overrightarrow{0}ZB×ZB=0
因此上式可以展开为
hω=−ωx×YB+ωy×XBh_\omega= - \omega _x \times Y_B+\omega _y\times X_Bhω=−ωx×YB+ωy×XB
然后两边点乘YBY_BYB,得到
ωx=−hω⋅YB\omega _x=-h_\omega \cdot Y_Bωx=−hω⋅YB
两边点乘XBX_BXB,得到
ωY=hω⋅XB\omega _Y=h_\omega \cdot X_BωY=hω⋅XB
因为ωBW=ωBC+ωCW\omega_{BW}=\omega _{BC}+\omega _{CW}ωBW=ωBC+ωCW,而ωBC\omega _{BC}ωBC在ZBZ_BZB上没有分量，
因此ωz=ωBW⋅ZB=ωCW⋯ZB=ψ˙ZW⋅ZB\omega _z=\omega _{BW}\cdot Z_B=\omega _{CW} \cdots Z_B=\dot \psi Z_W \cdot Z_Bωz=ωBW⋅ZB=ωCW⋯ZB=ψ˙ZW⋅ZB
至此12维变量变成了4维变量表示。
在实际的路径规划中我们只需要规划出轨迹的位置，ψ\psiψ角通常表示为速度的切向方向（群里咨询的，不知是否还有什么知识不知道）。具体的控制回路如下；
例如在mini snap规划时我们只需要将规划的轨迹加上速度的切线方向丢这个控制回路，就可以实现无人机的飞行。

mini snap我的博客园: link

https://www.cnblogs.com/gongliqiang/p/14375447.html

参考资料：深蓝学院

https://www.cnblogs.com/long5683/p/11871630.html