多旋翼模組化之歐拉角微分

本文來自于zing的博文硬核推導-歐拉角微分方程

首先，為了後續推導友善；我們将pitch簡寫為p ,roll簡寫為r,yaw簡寫為y，旋轉矩陣可寫為

C b n = [ cos ⁡ p cos ⁡ y − cos ⁡ r sin ⁡ y + sin ⁡ r sin ⁡ p cos ⁡ y sin ⁡ r sin ⁡ y + cos ⁡ r sin ⁡ p cos ⁡ y cos ⁡ p sin ⁡ y cos ⁡ r cos ⁡ y + sin ⁡ r sin ⁡ p sin ⁡ y − sin ⁡ r cos ⁡ y + cos ⁡ r sin ⁡ p sin ⁡ y − sin ⁡ p sin ⁡ r cos ⁡ p cos ⁡ r cos ⁡ p ] \mathbf{C}_{b}^{n}=\left[ \begin{array}{ccc}{\cos p \cos y} & {-\cos r \sin y+\sin r \sin p \cos y} & {\sin r \sin y+\cos r \sin p \cos y} \\ {\cos p \sin y} & {\cos r \cos y+\sin r \sin p \sin y} & {-\sin r \cos y+\cos r \sin p \sin y} \\ {-\sin p} & {\sin r \cos p} & {\cos r \cos p}\end{array}\right] Cbn​=⎣⎡​cospcosycospsiny−sinp​−cosrsiny+sinrsinpcosycosrcosy+sinrsinpsinysinrcosp​sinrsiny+cosrsinpcosy−sinrcosy+cosrsinpsinycosrcosp​⎦⎤​

d ( C b n ) d t = C b n = [ − p ˙ c ( y ) s ( p ) − y ˙ c ( p ) s ( y ) r ˙ s ( r ) s ( y ) − y ˙ c ( r ) c ( y ) + p ˙ c ( p ) c ( y ) s ( r ) + r ˙ c ( r ) c ( y ) s ( p ) − y ˙ s ( p ) s ( r ) s ( y ) r ˙ c ( r ) s ( y ) + y ˙ c ( y ) s ( r ) + p ˙ c ( p ) c ( r ) c ( y ) − r ˙ c ( y ) s ( p ) s ( r ) − y ˙ c ( r ) s ( p ) s ( y ) y ˙ c ( p ) c ( y ) − p ˙ s ( p ) s ( y ) p ˙ c ( p ) s ( r ) s ( y ) − y ˙ c ( r ) s ( y ) − r ˙ c ( y ) s ( r ) + r ˙ c ( r ) s ( p ) s ( y ) + y ˙ c ( y ) s ( p ) s ( r ) y ˙ s ( r ) s ( y ) − r ˙ c ( r ) c ( y ) + p ˙ c ( p ) c ( r ) s ( y ) + y ˙ c ( r ) c ( y ) s ( p ) − r ˙ s ( p ) s ( r ) s ( y ) − p ˙ c ( p ) r ˙ c ( p ) c ( r ) − p ˙ s ( p ) s ( r ) − p ˙ c ( r ) s ( p ) − r ˙ c ( p ) s ( r ) ] \frac{d\left(\mathbf{C}_{b}^{n}\right)}{d t}=\mathbf{C}_{b}^{n}=\left[ \begin{array}{ccc}{-\dot{p} c(y) s(p)-\dot{y} c(p) s(y)} & {\dot{r} s(r) s(y)-\dot{y} c(r) c(y)+\dot{p} c(p) c(y) s(r)+\dot{r} c(r) c(y) s(p)-\dot{y} s(p) s(r) s(y)} & {\dot{r} c(r) s(y)+\dot{y} c(y) s(r)+\dot{p} c(p) c(r) c(y)-\dot{r} c(y) s(p) s(r)-\dot{y} c(r) s(p) s(y)} \\ {\dot{y} c(p) c(y)-\dot{p} s(p) s(y)} & {\dot{p} c(p) s(r) s(y)-\dot{y} c(r) s(y)-\dot{r} c(y) s(r)+\dot{r} c(r) s(p) s(y)+\dot{y} c(y) s(p) s(r)} & {\dot{y} s(r) s(y)-\dot{r} c(r) c(y)+\dot{p} c(p) c(r) s(y)+\dot{y} c(r) c(y) s(p)-\dot{r} s(p) s(r) s(y)} \\ {-\dot{p} c(p)} & {\dot{r} c(p) c(r)-\dot{p} s(p) s(r)} & {-\dot{p} c(r) s(p)-\dot{r} c(p) s(r)}\end{array}\right] dtd(Cbn​)​=Cbn​=⎣⎡​−p˙​c(y)s(p)−y˙​c(p)s(y)y˙​c(p)c(y)−p˙​s(p)s(y)−p˙​c(p)​r˙s(r)s(y)−y˙​c(r)c(y)+p˙​c(p)c(y)s(r)+r˙c(r)c(y)s(p)−y˙​s(p)s(r)s(y)p˙​c(p)s(r)s(y)−y˙​c(r)s(y)−r˙c(y)s(r)+r˙c(r)s(p)s(y)+y˙​c(y)s(p)s(r)r˙c(p)c(r)−p˙​s(p)s(r)​r˙c(r)s(y)+y˙​c(y)s(r)+p˙​c(p)c(r)c(y)−r˙c(y)s(p)s(r)−y˙​c(r)s(p)s(y)y˙​s(r)s(y)−r˙c(r)c(y)+p˙​c(p)c(r)s(y)+y˙​c(r)c(y)s(p)−r˙s(p)s(r)s(y)−p˙​c(r)s(p)−r˙c(p)s(r)​⎦⎤​

上式可寫為：

C ˙ b n = C b n [ b ω ] × \dot{\mathbf{C}}_{b}^{n}=\mathbf{C}_{b}^{n}\left[^{b} \boldsymbol{\omega}\right]_{ \times} C˙bn​=Cbn​[bω]×​

其中 [ b ω ] × = [ 0 p ˙ s ( r ) − y ˙ c ( p ) c ( r ) p ˙ c ( r ) + y ˙ c ( p ) s ( r ) y ˙ c ( p ) c ( r ) − p ˙ s ( r ) 0 y ˙ s ( p ) − d r − p ˙ c ( r ) − y ˙ c ( p ) s ( r ) d r − y ˙ s ( p ) 0 ] \left[^{b} \omega\right]_{ \times}=\left[ \begin{array}{ccc}{0} & {\dot{p} s(r)-\dot{y} c(p) c(r)} & {\dot{p} c(r)+\dot{y} c(p) s(r)} \\ {\dot{y} c(p) c(r)-\dot{p} s(r)} & {0} & {\dot{y} s(p)-d r} \\ {-\dot{p} c(r)-\dot{y} c(p) s(r)} & {d r-\dot{y} s(p)} & {0}\end{array}\right] [bω]×​=⎣⎡​0y˙​c(p)c(r)−p˙​s(r)−p˙​c(r)−y˙​c(p)s(r)​p˙​s(r)−y˙​c(p)c(r)0dr−y˙​s(p)​p˙​c(r)+y˙​c(p)s(r)y˙​s(p)−dr0​⎦⎤​

即可寫成

b ω = [ r ˙ − y ˙ s ( p ) p ˙ c ( r ) + y ˙ c ( p ) s ( r ) − p ˙ s ( r ) + y ˙ c ( p ) c ( r ) ] ^{b} \omega=\left[ \begin{array}{c}{\dot{r}-\dot{y} s(p)} \\ {\dot{p} c(r)+\dot{y} c(p) s(r)} \\ {-\dot{p} s(r)+\dot{y} c(p) c(r)}\end{array}\right] bω=⎣⎡​r˙−y˙​s(p)p˙​c(r)+y˙​c(p)s(r)−p˙​s(r)+y˙​c(p)c(r)​⎦⎤​

是以

[ ω x ω y ω z ] = [ r ˙ − y ˙ s ( p ) p ˙ c ( r ) + y ˙ c ( p ) s ( r ) − p ˙ s ( r ) + y ˙ c ( p ) c ( r ) ] \left[ \begin{array}{c}{\omega_{x}} \\ {\omega_{y}} \\ {\omega_{z}}\end{array}\right]=\left[ \begin{array}{c}{\dot{r}-\dot{y} s(p)} \\ {\dot{p} c(r)+\dot{y} c(p) s(r)} \\ {-\dot{p} s(r)+\dot{y} c(p) c(r)}\end{array}\right] ⎣⎡​ωx​ωy​ωz​​⎦⎤​=⎣⎡​r˙−y˙​s(p)p˙​c(r)+y˙​c(p)s(r)−p˙​s(r)+y˙​c(p)c(r)​⎦⎤​

上式可寫為：

[ ω x ω y ω z ] = [ 1 0 − s ( r ) 0 c ( r ) s ( r ) c ( p ) 0 − s ( r ) c ( r ) c ( p ) ] [ r ˙ p ˙ y ˙ ] \left[ \begin{array}{c}{\omega_{x}} \\ {\omega_{y}} \\ {\omega_{z}}\end{array}\right]=\left[ \begin{array}{ccc}{1} & {0} & {-s(r)} \\ {0} & {c(r)} & {s(r) c(p)} \\ {0} & {-s(r)} & {c(r) c(p)}\end{array}\right] \left[ \begin{array}{c}{\dot{r}} \\ {\dot{p}} \\ {\dot{y}}\end{array}\right] ⎣⎡​ωx​ωy​ωz​​⎦⎤​=⎣⎡​100​0c(r)−s(r)​−s(r)s(r)c(p)c(r)c(p)​⎦⎤​⎣⎡​r˙p˙​y˙​​⎦⎤​

進一步得到歐拉角的微分形式：

[ r ˙ p ˙ y ˙ ] = [ 1 tan ⁡ θ sin ⁡ ϕ tan ⁡ θ cos ⁡ ϕ 0 cos ⁡ ϕ − sin ⁡ ϕ 0 sin ⁡ ϕ / cos ⁡ θ cos ⁡ ϕ / cos ⁡ θ ] [ ω x ω y ω z ] \left[ \begin{array}{c}{\dot{r}} \\ {\dot{p}} \\ {\dot{y}}\end{array}\right]=\left[ \begin{array}{ccc}{1} & {\tan \theta \sin \phi} & {\tan \theta \cos \phi} \\ {0} & {\cos \phi} & {-\sin \phi} \\ {0} & {\sin \phi / \cos \theta} & {\cos \phi / \cos \theta}\end{array}\right] \left[ \begin{array}{c}{\omega_{x}} \\ {\omega_{y}} \\ {\omega_{z}}\end{array}\right] ⎣⎡​r˙p˙​y˙​​⎦⎤​=⎣⎡​100​tanθsinϕcosϕsinϕ/cosθ​tanθcosϕ−sinϕcosϕ/cosθ​⎦⎤​⎣⎡​ωx​ωy​ωz​​⎦⎤​