多旋翼無人機模組化-牛頓歐拉法

多旋翼無人機模組化

模組化之前我們先分析一下多旋翼無人機有哪些狀态量：用于表示線運動的三軸位置、速度和加速度；用于表示角運動的三軸姿态角和姿态角速度；這一共是15個狀态量。

首先來看線運動方程的建立過程：我們取地理坐标系為北東地(NED)，機體系為前右下：

由牛頓第二定律有：

F = m a F=m a F=ma

即

F = m [ a x a y a z ] F=m \left[ \begin{array}{l}{a_{x}} \\ {a_{y}} \\ {a_{z}}\end{array}\right] F=m⎣⎡​ax​ay​az​​⎦⎤​

注意此處的 F F F和加速度 a a a均是在地理系下的表示( F F F表示四個旋翼産生的總升力)，進一步将上式展開可得

[ 0 0 m g ] − R b e [ 0 0 f 1 + f 2 + f 3 + f 4 ] = m [ a x a y a z ] \left[ \begin{array}{c}{0} \\ {0} \\ {m g}\end{array}\right] - \boldsymbol{R}_{b}^{e} \left[ \begin{array}{c}{0} \\ {0} \\ {f_{1}+f_{2}+f_{3}+f_{4}}\end{array}\right]=m \left[ \begin{array}{l}{a_{x}} \\ {a_{y}} \\ {a_{z}}\end{array}\right] ⎣⎡​00mg​⎦⎤​−Rbe​⎣⎡​00f1​+f2​+f3​+f4​​⎦⎤​=m⎣⎡​ax​ay​az​​⎦⎤​

其中 R b e \boldsymbol{R}_{b}^{e} Rbe​表示由機體坐标系到地理坐标系的轉換矩陣，由于 f i , i = 1 … 4 f_i,i=1…4 fi​,i=1…4分别表示四個旋翼沿機體 Z Z Z軸負方向産生的升力，故需要将其轉換到地理系中；

R b e = [ c θ c ψ s ϕ s θ c ψ − c ϕ s ψ c ϕ s θ c ψ + s ϕ s ψ c θ s ψ s ϕ s θ s ψ + c ϕ c ψ c ϕ s θ s ψ − s ϕ c ψ − s θ s ϕ c θ c ϕ c θ ] \boldsymbol{R}_{b}^{e}=\left[ \begin{array}{ccc}{c \theta c \psi} & {s \phi s \theta c \psi-c \phi s \psi} & {c \phi s \theta c \psi+s \phi s \psi} \\ {c \theta s \psi} & {s \phi s \theta s \psi+c \phi c \psi} & {c \phi s \theta s \psi-s \phi c \psi} \\ {-s \theta} & {s \phi c \theta} & {c \phi c \theta}\end{array}\right] Rbe​=⎣⎡​cθcψcθsψ−sθ​sϕsθcψ−cϕsψsϕsθsψ+cϕcψsϕcθ​cϕsθcψ+sϕsψcϕsθsψ−sϕcψcϕcθ​⎦⎤​

聯立上式進一步化簡即可得到線運動的模型表示：

{ x ¨ = ( c ϕ s θ c ψ + s ϕ s ψ ) − F m y ¨ = ( c ϕ s θ s ψ − s ϕ c ψ ) − F m z ¨ = g − ( c ϕ c θ ) F m \left\{\begin{array}{c}{\ddot{x}=(\mathrm{c} \phi \mathrm{s} \theta \mathrm{c} \psi+\mathrm{s} \phi \mathrm{s} \psi) \frac{-F}{m}} \\ {\ddot{y}=(\mathrm{c} \phi \mathrm{s} \theta \mathrm{s} \psi-\mathrm{s} \phi \mathrm{c} \psi) \frac{-F}{m}} \\ {\ddot{z}=g-(\mathrm{c} \phi \mathrm{c} \theta) \frac{F}{m}}\end{array}\right. ⎩⎨⎧​x¨=(cϕsθcψ+sϕsψ)m−F​y¨​=(cϕsθsψ−sϕcψ)m−F​z¨=g−(cϕcθ)mF​​

線運動的模組化過程很簡單，但是多旋翼不光能移動還能旋轉，這個旋轉過程就需要剛體的旋轉相關知識。

物體如何旋轉跟旋翼産生的力矩有關，力矩與力F和距離l有如下公式： M = F ∗ l M=F * l M=F∗l

而這裡的力矩又與多旋翼動力布局有關

如果是十字型的布局：

F = ∑ i = 1 4 T i = c T ( w 1 2 + w 2 2 + w 3 2 + w 4 2 ) F=\sum_{i=1}^{4} T_{i}=c_{\mathrm{T}}\left(w_{1}^{2}+w_{2}^{2}+w_{3}^{2}+w_{4}^{2}\right) F=i=1∑4​Ti​=cT​(w12​+w22​+w32​+w42​)

{ τ x = d c T ( − w 2 2 + w 4 2 ) τ y = d c T ( w 1 2 − w 3 2 ) τ z = c M ( w 1 2 − w 2 2 + w 3 2 − w 4 2 ) \left\{\begin{array}{l}{\tau_{x}=d c_{\mathrm{T}}\left(-w_{2}^{2}+w_{4}^{2}\right)} \\ {\tau_{y}=d c_{\mathrm{T}}\left(w_{1}^{2}-w_{3}^{2}\right)} \\ {\tau_{z}=c_{\mathrm{M}}\left(w_{1}^{2}-w_{2}^{2}+w_{3}^{2}-w_{4}^{2}\right)}\end{array}\right. ⎩⎨⎧​τx​=dcT​(−w22​+w42​)τy​=dcT​(w12​−w32​)τz​=cM​(w12​−w22​+w32​−w42​)​

寫成矩陣形式如下：

[ F τ x τ y τ z ] = [ c T c T c T c T 0 − d c T 0 d c T d c T 0 − d c T 0 c M − c M c M − c M ] [ w 1 2 w 2 2 w 3 2 w 4 2 ] \left[ \begin{array}{l}{F} \\ {\tau_{x}} \\ {\tau_{y}} \\ {\tau_{z}}\end{array}\right]=\left[ \begin{array}{cccc}{c_{\mathrm{T}}} & {c_{\mathrm{T}}} & {c_{\mathrm{T}}} & {c_{\mathrm{T}}} \\ {0} & {-d c_{\mathrm{T}}} & {0} & {d c_{\mathrm{T}}} \\ {d c_{\mathrm{T}}} & {0} & {-d c_{\mathrm{T}}} & {0} \\ {c_{\mathrm{M}}} & {-c_{\mathrm{M}}} & {c_{\mathrm{M}}} & {-c_{\mathrm{M}}}\end{array}\right] \left[ \begin{array}{c}{w_{1}^{2}} \\ {w_{2}^{2}} \\ {w_{3}^{2}} \\ {w_{4}^{2}}\end{array}\right] ⎣⎢⎢⎡​Fτx​τy​τz​​⎦⎥⎥⎤​=⎣⎢⎢⎡​cT​0dcT​cM​​cT​−dcT​0−cM​​cT​0−dcT​cM​​cT​dcT​0−cM​​⎦⎥⎥⎤​⎣⎢⎢⎡​w12​w22​w32​w42​​⎦⎥⎥⎤​

對X型動力布局：

F = ∑ i = 1 4 T i = c T ( w 1 2 + ϖ 2 2 + ϖ 3 2 + ϖ 4 2 ) F=\sum_{i=1}^{4} T_{i}=c_{\mathrm{T}}\left(w_{1}^{2}+\varpi_{2}^{2}+\varpi_{3}^{2}+\varpi_{4}^{2}\right) F=i=1∑4​Ti​=cT​(w12​+ϖ22​+ϖ32​+ϖ42​)

{ τ x = d c T ( 2 ω 1 2 / 2 − 2 ω 2 2 / 2 − 2 ω 3 2 / 2 + 2 ω 4 2 / 2 ) τ y = d c T ( 2 ω 1 2 / 2 + 2 ω 2 2 / 2 − 2 ω 3 2 / 2 − 2 ω 4 2 / 2 ) τ z = c M ( ω 1 2 − ω 2 2 + ω 3 2 − ω 4 2 ) \left\{\begin{array}{c}{\tau_{x}=d c_{\mathrm{T}}\left(\sqrt{2} \omega_{1}^{2} / 2-\sqrt{2} \omega_{2}^{2} / 2-\sqrt{2} \omega_{3}^{2} / 2+\sqrt{2} \omega_{4}^{2} / 2\right)} \\ {\tau _{y}=d c_{\mathrm{T}}\left(\sqrt{2} \omega_{1}^{2} / 2+\sqrt{2} \omega_{2}^{2} / 2-\sqrt{2} \omega_{3}^{2} / 2-\sqrt{2} \omega_{4}^{2} / 2\right)} \\ {\tau _{z}=c_{\mathrm{M}}\left(\omega_{1}^{2}-\omega_{2}^{2}+\omega_{3}^{2}-\omega_{4}^{2}\right)}\end{array}\right. ⎩⎨⎧​τx​=dcT​(2 ​ω12​/2−2 ​ω22​/2−2 ​ω32​/2+2 ​ω42​/2)τy​=dcT​(2 ​ω12​/2+2 ​ω22​/2−2 ​ω32​/2−2 ​ω42​/2)τz​=cM​(ω12​−ω22​+ω32​−ω42​)​

上式中 F F F為四個旋翼産生的總升力， τ x , τ y , τ z \tau _x ,\tau _y ,\tau _z τx​,τy​,τz​分别表示旋翼産生的作用在三個姿态通道的控制力矩。

根據歐拉方程 M = J ε + ω × J ω M=J \varepsilon+\omega \times J \omega M=Jε+ω×Jω

其中 ε \varepsilon ε表示角加速度， ω \omega ω表示角速度， M M M表示姿态通道控制力矩；

歐拉角速度

Θ ˙ = [ ϕ ˙ θ ˙ ψ ˙ ] \dot{\boldsymbol{\Theta}}=\left[ \begin{array}{c}{\dot{\phi}} \\ {\dot{\theta}} \\ {\dot{\psi}}\end{array}\right] Θ˙=⎣⎡​ϕ˙​θ˙ψ˙​​⎦⎤​

與機體角速度

Ω = [ w x w y w z ] {\boldsymbol{\Omega}}=\left[ \begin{array}{c}{w_x} \\ {w_y} \\ {w_z}\end{array}\right] Ω=⎣⎡​wx​wy​wz​​⎦⎤​

之間的關系為

Θ ˙ = W Ω \dot{\boldsymbol{\Theta}}=\boldsymbol{W} \boldsymbol{\Omega} Θ˙=WΩ

W = [ 1 tan ⁡ θ sin ⁡ ϕ tan ⁡ θ cos ⁡ ϕ 0 cos ⁡ ϕ − sin ⁡ ϕ 0 sin ⁡ ϕ / cos ⁡ θ cos ⁡ ϕ / cos ⁡ θ ] W=\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] W=⎣⎡​100​tanθsinϕcosϕsinϕ/cosθ​tanθcosϕ−sinϕcosϕ/cosθ​⎦⎤​假設多旋翼無人機姿态角在小角度内變化，則可認為 W = I W=I W=I，即 Θ ˙ = Ω \dot{\boldsymbol{\Theta}}= \boldsymbol{\Omega} Θ˙=Ω

角加速度為 [ ϕ ¨ θ ¨ ψ ¨ ] \left[ \begin{array}{c}{\ddot{\phi}} \\ {\ddot{\theta}} \\ {\ddot{\psi}}\end{array}\right] ⎣⎡​ϕ¨​θ¨ψ¨​​⎦⎤​

轉動慣量矩陣為

J = [ J x x − J x y − J x z − J y x J y y − J y z − J z x − J z y J z z ] \mathbf{J}=\left[ \begin{array}{ccc}{J_{x x}} & {-J_{x y}} & {-J_{x z}} \\ {-J_{y x}} & {J_{y y}} & {-J_{y z}} \\ {-J_{z x}} & {-J_{z y}} & {J_{z z}}\end{array}\right] J=⎣⎡​Jxx​−Jyx​−Jzx​​−Jxy​Jyy​−Jzy​​−Jxz​−Jyz​Jzz​​⎦⎤​

其中 J x y = J y x ， J x z = J z x ， J y z = J z y J_{xy}=J_{yx}，J_{xz}=J_{zx}，J_{y z}=J_{z y} Jxy​=Jyx​，Jxz​=Jzx​，Jyz​=Jzy​,對于标準多旋翼這樣的中心對成物體，有 J x y = J x z = J y z = 0 J_{xy}=J_{xz}=J_{y z}=0 Jxy​=Jxz​=Jyz​=0,則有

[ J x x 0 0 0 J y y 0 0 0 J z z ] \left[ \begin{array}{lll}{J_{x x}} & {0} & {0} \\ {0} & {J_{y y}} & {0} \\ {0} & {0} & {J_{z z}}\end{array}\right] ⎣⎡​Jxx​00​0Jyy​0​00Jzz​​⎦⎤​

整理可得

{ ϕ ¨ = θ ˙ ψ ˙ J y − J z J x + τ x J x θ ¨ = ϕ ˙ ψ ˙ J z − J x J y + τ y J y ψ ¨ = ϕ ˙ θ ˙ J x − J y J z + τ z J z \left\{\begin{array}{l}{\ddot{\phi}=\dot{\theta} \dot{\psi} \frac{J_{y}-J_{z}}{J_{x}}+\frac{\tau_{x}}{J_{x}}} \\ {\ddot{\theta}=\dot{\phi} \dot{\psi} \frac{J_{z}-J_{x}}{J_{y}}+\frac{\tau_{y}}{J_{y}}} \\ {\ddot{\psi}=\dot{\phi} \dot{\theta} \frac{J_{x}-J_{y}}{J_{z}}+\frac{\tau_{z}}{J_{z}}}\end{array}\right. ⎩⎪⎨⎪⎧​ϕ¨​=θ˙ψ˙​Jx​Jy​−Jz​​+Jx​τx​​θ¨=ϕ˙​ψ˙​Jy​Jz​−Jx​​+Jy​τy​​ψ¨​=ϕ˙​θ˙Jz​Jx​−Jy​​+Jz​τz​​​

至此，多旋翼的模型建立完畢。可以看到模組化的其實就是為了得到輸入的力與加速度的關系，通過牛頓方程可以得到線運動的加速度，通過歐拉方程可以得到角加速度，是以這種模組化方法也叫牛頓-歐拉方程。