一、主要內容

本文旨在基於牛頓-歐拉方程建立四旋翼飛行器的動力學和運動學模型，從而得到四旋翼飛行器的飛行控制剛體模型。包括以下內容：

1. 主要內容
2. 基本假設
3. 動力學模型與運動學模型
4. 地球坐標系與機體坐標系
5. 旋轉矩陣
6. 牛頓-歐拉方程
7. 四旋翼飛行器的動力學模型
8. 四旋翼飛行器的運動學模型
9. 四旋翼飛行器的飛行控制剛體模型

二、基本假設

為便於建立模型，現對四旋翼飛行器進行以下假設^[1]：

1. 四旋翼飛行器是均勻對稱的剛體
2. 四旋翼飛行器的質量和轉動慣量不發生改變
3. 四旋翼飛行器的幾何中心與其重心重合
4. 四旋翼飛行器隻受重力和螺旋槳拉力
5. 螺旋槳1、3為逆時針轉動，螺旋槳2、4為順時針轉動

三、動力學模型與運動學模型

建立四旋翼飛行器模型的目的在於分析四旋翼飛行器在受到外力、外力矩的情況下，其位置和姿態的變化情況。

其中，動力學模型的輸入為螺旋槳提供的拉力和力矩，輸出為四旋翼的速度和角速度；運動學模型的輸入為動力學模型的輸出，即四旋翼的速度和角速度，輸出為四旋翼的位置和姿態。

其關系如下圖所示：

圖1 四旋翼飛行器的飛行控制剛體模型

四、地球坐標系與機體坐標系

在建立模型之前，需要先對表示向量的坐標系進行定義。本文將用到兩個坐標系，即慣性坐標系（靜坐標系）——地球坐標系，以及非慣性坐標系（動坐標系）——機體坐標系。

地球坐標系取地球中心為坐標原點，其與地球固連。

機體坐標系取飛行器重心位置為坐標原點，其與四旋翼飛行器固連， O_bx_b 軸方向為飛行器前進方向。

如下圖所示：

a11f4db23738435ae102ad6596b38feb圖2 地球坐標系與機體坐標系

圖中， O_ex_ey_ez_e 為地球坐標系， O_bx_by_bz_b 為機體坐標系。

兩個坐標系都遵循右手法則，關於右手定則的具體內容見下圖^{[1]所示，這裡截取的是北航全權老師所著的《多旋翼飛行器設計與控制》中的部分內容。}

圖3 右手定則

註：在有些論文資料中，將地球坐標系中的 O_ez_e 軸和機體坐標系中的 O_bz_b 軸的方向定義為向上為正，與圖2所示方向剛好相反，如圖4^{[2]所示。這並不影響建模過程，隻影響動力學方程的正負號。}

圖4 Oz軸定義為垂直於xy平面向上

五、旋轉矩陣

旋轉矩陣的作用是將機體坐標系下表示的向量轉變到地球坐標系下表示，向量的本質並不發生任何改變，隻是改變瞭向量的表達形式。

旋轉矩陣改變向量表達形式的目的在於^[1]：

1. 在地球坐標系下表示四旋翼飛行器的位置和速度，有助於飛控手更好地確定飛行器的位置和飛行速度；
2. 在機體坐標系下表示四旋翼飛行器的拉力和力矩比較直觀，且傳感器的測量也是在機體坐標系下表示的。

旋轉矩陣的推導過程可以參見下面這兩篇文章，本文直接給出旋轉矩陣的結果。

任一向量從機體坐標系 O_bx_by_bz_b 到地球坐標系 O_ex_ey_ez_e 的旋轉矩陣表示為：

boldsymbol{R}_{b}^{e}=left[ begin{matrix} cos theta cos psi& cos psi sin theta sin phi -sin psi cos phi& cos psi sin theta cos phi +sin psi sin phi\ cos theta sin psi& sin psi sin theta sin phi +cos psi cos phi& sin psi sin theta cos phi -cos psi sin phi\ -sin theta& sin phi cos theta& cos phi cos theta\ end{matrix} right]

式中， boldsymbol{R}_{b}^{e} 右下方的 b 表示機體坐標系，右上方的 e 表示地球坐標系； phi , theta , psi 分別表示繞 x 軸旋轉的滾轉角、繞 y 軸旋轉的俯仰角以及繞 z 軸旋轉的偏航角，即歐拉角，如下圖所示：

04da4796283c843d50063f48e48b0294圖5 歐拉角的定義

六、牛頓-歐拉方程

由牛頓-歐拉方程可知，剛體運動=質心的平動+繞質心的轉動

質心的平動用牛頓第二定律描述，即：

boldsymbol{F}=mfrac{dboldsymbol{v}}{dt} (1)

式中, boldsymbol{F} 為剛體所受合外力； boldsymbol{v} 為剛體的運動速度。

繞質心的轉動由歐拉方程描述，即：

boldsymbol{M}=boldsymbol{Jdot{omega}}+boldsymbol{omega }times boldsymbol{Jomega } (2)

式中 , boldsymbol{M}為剛體所受的合外力矩； boldsymbol{J} 為3×3的慣性矩陣； boldsymbol{omega } 為剛體的角速度。

其表達的含義為：作用在剛體上的合力矩 boldsymbol{M} 使得剛體以角速度 boldsymbol{omega } 、角加速度 boldsymbol{dot{omega}} 旋轉。

註：

1. 歐拉方程的推導可以參考下面這篇文章：

2. (2)式中的“×”表示的是叉乘。若存在兩個向量 boldsymbol{a}=left[ begin{matrix} a_x& a_y& a_z\ end{matrix} right] ^T 和 boldsymbol{b}=left[ begin{matrix} b_x& b_y& b_z\ end{matrix} right] ^T ，則有：

boldsymbol{a}times boldsymbol{b}=left[ begin{matrix} 0& -a_z& a_y\ a_z& 0& -a_x\ -a_y& a_x& 0\ end{matrix} right] left[ begin{array}{c} b_x\ b_y\ b_z\ end{array} right] =left[ begin{array}{c} a_yb_z-a_zb_y\ a_zb_x-a_xb_z\ a_xb_y-a_yb_x\ end{array} right]

其中，矩陣

left[ begin{matrix} 0& -a_z& a_y\ a_z& 0& -a_x\ -a_y& a_x& 0\ end{matrix} right] 可記為 left[ boldsymbol{a} right] _{times} ，它是一個反對稱矩陣。

七、四旋翼飛行器的動力學模型

此時便可以根據牛頓-歐拉方程建立四旋翼的動力學模型瞭。

動力學模型的輸入為合外力、合外力矩，輸出為速度、角速度。在（二）中，我們假設瞭飛行器隻受重力和螺旋槳拉力，如下圖所示：

3548e838f740369641fe95ff46393c38圖6 四旋翼飛行器的受力情況

其中， T_1, T_2, T_3, T_4 分別為螺旋槳1、2、3、4所產生的拉力，由於假設瞭四旋翼飛行器是均勻對稱的剛體，所以它們的合力——總拉力 f 過 O_b 點，且始終與 O_bz_b 軸的負方向一致。

1.位置動力學模型

由牛頓第二定律(1)，有：

mboldsymbol{dot{v}}^e=boldsymbol{G}^e-boldsymbol{f}^b (3)

式中， boldsymbol{v}, boldsymbol{G} 右上角的字母 e 代表這是在地球坐標系下表示的向量； boldsymbol{f} 右上角的字母 b 代表這是在機體坐標系下表示的向量。由於拉力是由螺旋槳產生的，與四旋翼飛行器固連，故在機體坐標系下表示。

等式兩邊同時除以質量，得：

boldsymbol{dot{v}}^e=boldsymbol{g}^e-frac{boldsymbol{f}^b}{m} (4)

為瞭便於研究，將拉力轉換至地球坐標系下表示，左乘旋轉矩陣 boldsymbol{R}_{b}^{e} 即可：

boldsymbol{dot{v}}^e=boldsymbol{g}^e-boldsymbol{R}_{b}^{e}frac{boldsymbol{f}^b}{m} (5)

四旋翼飛行器的重力和拉力都是沿著O_ez_e軸的，故有：

boldsymbol{dot{v}}^e={g}boldsymbol{e}_3-frac{{f}}{m}boldsymbol{R}_{b}^{e}boldsymbol{e}_3 (6)

式中， boldsymbol{e}_3 是沿著地球坐標系中 O_ez_e 軸的單位向量，如下圖所示：

圖7 地球坐標系下的單位向量

且有：

boldsymbol{e}_1=left[ begin{array}{c} 1\ 0\ 0\ end{array} right] , boldsymbol{e}_2=left[ begin{array}{c} 0\ 1\ 0\ end{array} right] , boldsymbol{e}_3=left[ begin{array}{c} 0\ 0\ 1\ end{array} right]

此時，將(6)式展開，並將 boldsymbol{R}_{b}^{e} 、 boldsymbol{e}_3 代入可得：

left[ begin{array}{c} {dot{v}}_x\ {dot{v}}_y\ {dot{v}}_z\ end{array} right] ={g}left[ begin{array}{c} 0\ 0\ 1\ end{array} right] -frac{{f}}{m}boldsymbol{R}_{b}^{e}left[ begin{array}{c} 0\ 0\ 1\ end{array} right] \ Downarrow \ begin{cases} {dot{v}}_x=-frac{{f}}{m}left( cos psi sin theta cos phi +sin psi sin phi right)\ {dot{v}}_y=-frac{{f}}{m}left( sin psi sin theta cos phi -cos psi sin phi right)\ {dot{v}}_z={g}-frac{{f}}{m}cos phi cos theta\ end{cases}

(7)

這就得到瞭動力學模型中關於合外力和速度的方程，即位置動力學模型。接下來是關於合力矩和角速度的方程——姿態動力學模型。

2. 姿態動力學模型

由歐拉方程(2)可得：

boldsymbol{Jdot{omega}}^b+boldsymbol{omega }^btimes boldsymbol{Jomega }^b=boldsymbol{G}_{boldsymbol{a}}+boldsymbol{tau } (8)

式中， boldsymbol{omega }^b 表示在機體坐標系下的角速度； boldsymbol{G}_{boldsymbol{a}} 表示陀螺力矩； boldsymbol{tau } 表示螺旋槳在機體軸上產生的力矩，包括繞 O_bx_b 軸的滾轉力矩 {tau }_x 、繞 O_by_b 軸的俯仰力矩 {tau }_y 以及繞 O_bz_b 軸的偏航力矩 {tau }_z 。

關於角速度 boldsymbol{omega }^b ：為方便表示，分別用 p, q, r 來表示 boldsymbol{omega }^b 在機體軸上的三個分量： omega_x, omega_y, omega_z ，即：

boldsymbol{omega }^b=left[ begin{array}{c} omega _{xb}\ omega _{yb}\ omega _{zb}\ end{array} right] =left[ begin{array}{c} p\ q\ r\ end{array} right] (9)

關於陀螺力矩 boldsymbol{G}_{boldsymbol{a}} ：當電機高速旋轉的時候，相當於一個陀螺。高速旋轉的陀螺是非常穩定的個體，具有保持自身軸向不變的能力。因此如果有外力想改變陀螺轉軸的方向，那麼會產生一個陀螺力矩來抵抗這種改變。這裡直接給出 boldsymbol{G}_{boldsymbol{a}} 的計算公式：

boldsymbol{G}_{boldsymbol{a}}=left[ begin{array}{c} G_{a,phi}\ G_{a,theta}\ G_{a,psi}\ end{array} right] =left[ begin{array}{c} J_{RP}qleft( varpi _1-varpi _2+varpi _3-varpi _4 right)\ J_{RP}pleft( -varpi _1+varpi _2-varpi _3+varpi _4 right)\ 0\ end{array} right] (10)

式中， J_{RP} 表示整個電機轉子和螺旋槳繞機體轉軸的總轉動慣量； varpi _1, varpi _2, varpi _3, varpi _4 表示螺旋槳1，2，3，4的轉速。關於陀螺力矩的詳細推導可以參考下面這篇文章：

關於慣性矩陣 boldsymbol{J} ：基於假設1和假設2，慣性矩陣可表示為：

boldsymbol{J}=left[ begin{matrix} J_{xx}& & \ & J_{yy}& \ & & J_{zz}\ end{matrix} right] (11)

現在將式(9-11)代入到式(8)中，移項整理後得到：

left[ begin{array}{c} I_{xx}dot{p}\ I_{yy}dot{q}\ I_{zz}dot{r}\ end{array} right] =left[ begin{array}{c} qrleft( I_{yy}-I_{zz} right)\ prleft( I_{zz}-I_{xx} right)\ pqleft( I_{xx}-I_{yy} right)\ end{array} right] +left[ begin{array}{c} -J_{RP}qvarOmega\ J_{RP}pvarOmega\ 0\ end{array} right] +left[ begin{array}{c} tau _x\ tau _y\ tau _z\ end{array} right] \ Downarrow \ begin{cases} dot{p}=frac{1}{I_{xx}}left[ tau _x+qrleft( I_{yy}-I_{zz} right) -J_{RP}qvarOmega right]\ dot{q}=frac{1}{I_{yy}}left[ tau _y+prleft( I_{zz}-I_{xx} right) +J_{RP}pvarOmega right]\ dot{r}=frac{1}{I_{zz}}left[ tau _z+pqleft( I_{xx}-I_{yy} right) right]\ end{cases}

(12)

式中， varOmega =-varpi _1-varpi _2+varpi _3+varpi _4 。（原來的符號有錯誤，現已修改）

至此，四旋翼飛行器的動力學模型建立完成。

八、四旋翼飛行器的運動學模型

運動學模型的輸入為速度和角速度，輸出為位置和姿態。

關於位置的方程比較簡單，即：

boldsymbol{dot{p}}^e=boldsymbol{v}^e (13)

式中， boldsymbol{{p}}^e=left[ begin{matrix} x& y& z\ end{matrix} right] ^T ，用來表示飛行器在地球坐標系中的坐標位置，展開後得：

left[ begin{matrix} dot{x}& dot{y}& dot{z}\ end{matrix} right] ^T=left[ begin{matrix} v_x& v_y& v_z\ end{matrix} right] ^T (14)

接下來是關於姿態的方程。

表示姿態的方法有三種：歐拉角、旋轉矩陣、四元數。本文隻介紹歐拉角的表示方法。

姿態角的變化率與機體的旋轉角速度有如下的關系：

boldsymbol{dot{varTheta}}=boldsymbol{W}cdot boldsymbol{omega }^b (15)

式中， boldsymbol{varTheta } 為四旋翼的三個姿態角（歐拉角），且有：

boldsymbol{dot{varTheta}}=left[ begin{matrix} dot{phi}& dot{theta}& dot{psi}\ end{matrix} right] ^T ， boldsymbol{omega }^b=left[ begin{matrix} omega _{xb}& omega _{yb}& omega _{zb}\ end{matrix} right] ^T=left[ begin{matrix} p& q& r\ end{matrix} right] ^T

boldsymbol{W}=left[ begin{matrix} 1& tan theta sin phi& tan theta cos phi\ 0& cos phi& -sin phi\ 0& sin phi /cos theta& cos phi /cos theta\ end{matrix} right]

即：

left[ begin{array}{c} dot{phi}\ dot{theta}\ dot{psi}\ end{array} right] =left[ begin{matrix} 1& tan theta sin phi& tan theta cos phi\ 0& cos phi& -sin phi\ 0& sin phi /cos theta& cos phi /cos theta\ end{matrix} right] left[ begin{array}{c} p\ q\ r\ end{array} right] (16)

關於姿態角的變化率與機體的旋轉角速度為何不相等的解釋以及推導過程可以參考下面兩篇文章：

在小擾動的情況下，即各個角度的變化較小的前提下，姿態角的變化率與機體的旋轉角速度近似相等，則有：

left[ begin{array}{c} dot{phi}\ dot{theta}\ dot{psi}\ end{array} right] =left[ begin{array}{c} p\ q\ r\ end{array} right] (17)

至此，四旋翼飛行器的運動學模型建立完成。

九、四旋翼飛行器的飛行控制剛體模型

四旋翼飛行器的飛行控制剛體模型由動力學模型和運動學模型結合而成。

將式(7)與式(14)結合，得到以下方程組：

begin{cases} ddot{x}=-frac{f}{m}left( cos psi sin theta cos phi +sin psi sin phi right)\ ddot{y}=-frac{f}{m}left( sin psi sin theta cos phi -cos psi sin phi right)\ ddot{z}=g-frac{f}{m}cos phi cos theta\ end{cases} (18)

在小擾動的前提下，將式(12)與式(17)結合，得到以下方程組：

begin{cases} ddot{phi}=frac{1}{I_{xx}}left[ tau _x+qrleft( I_{yy}-I_{zz} right) -J_{RP}qvarOmega right]\ ddot{theta}=frac{1}{I_{yy}}left[ tau _y+prleft( I_{zz}-I_{xx} right) +J_{RP}pvarOmega right]\ ddot{psi}=frac{1}{I_{zz}}left[ tau _z+pqleft( I_{xx}-I_{yy} right) right]\ end{cases} (19)

將式(18)與式(19)組合在一起，並使： left[ begin{matrix} U_1& U_2& U_3& U_4\ end{matrix} right] ^T=left[ begin{matrix} f& tau _x& tau _y& tau _z\ end{matrix} right] ^T （動力學模型的輸入量，與螺旋槳轉速的平方成線性關系），即可得到四旋翼飛行器的非線性六自由度模型：

begin{cases} ddot{x}=-frac{U_1}{m}left( cos psi sin theta cos phi +sin psi sin phi right)\ ddot{y}=-frac{U_1}{m}left( sin psi sin theta cos phi -cos psi sin phi right)\ ddot{z}=g-frac{U_1}{m}cos phi cos theta\ begin{array}{l} ddot{phi}=frac{1}{I_{xx}}left[ U_2+qrleft( I_{yy}-I_{zz} right) -J_{RP}qvarOmega right]\ ddot{theta}=frac{1}{I_{yy}}left[ U_3+prleft( I_{zz}-I_{xx} right) +J_{RP}pvarOmega right]\ ddot{psi}=frac{1}{I_{zz}}left[ U_4+pqleft( I_{xx}-I_{yy} right) right]\ end{array}\ end{cases} (20)

也可以將式(6)、式(8)、式(13)、式(15)聯合起來得到以下方程組：

begin{cases} boldsymbol{dot{p}}^e=boldsymbol{v}^e\ boldsymbol{dot{v}}^e=gboldsymbol{e}_3-frac{f}{m}boldsymbol{R}_{b}^{e}boldsymbol{e}_3\ boldsymbol{dot{varTheta}}=boldsymbol{W}cdot boldsymbol{omega }^b\ boldsymbol{Jdot{omega}}^b=-boldsymbol{omega }^btimes boldsymbol{Jomega }^b+boldsymbol{G}_{boldsymbol{a}}+boldsymbol{tau }\ end{cases} (21)

十、結語

1. 下一步將給出四旋翼飛行器的控制效率模型；
2. 四旋翼飛行器的飛行控制剛體模型建立起來後，便可以根據此模型設計控制器。

——2021.02.05

參考

1. ^^{abc全權. 多旋翼飛行器設計與控制[M]. 北京: 電子工業出版社, 2018.}
2. ^MAHONY R, KUMAR V, CORKE P. Multirotor Aerial Vehicles: Modeling, Estimation, and Control of Quadrotor[J]. IEEE robotics & automation magazine, 2012,19(3): 20-32.