四旋翼飞行器的建模
引言
本文是我在学习四旋翼飞行器建模时做的笔记，主要依据这个项目的文档。鉴于理论力学早就全部还给老师，文中对基础力学知识进行了必要的阐述，虽不够详尽，但应足以辅助形象理解。
坐标轴和参考系
在四旋翼无人机的建模中，定义了两种主要的坐标系，惯性坐标系和机体坐标系。惯性坐标系\(\mathcal{A}\)由\(\mathbf{a}_1\)、\(\mathbf{a}_2\)和\(\mathbf{a}_3\)定义，其中\(\mathbf{a}_3\)指向上方。惯性坐标系相当于在地面上观察无人机时所使用的固定坐标系。机体坐标系\(\mathcal{B}\)附着在无人机的重心\(C\)上，随着无人机的运动而运动。\(\mathbf{b}_1\)轴通常指向无人机的正前方，\(\mathbf{b}_3\)轴垂直于旋翼平面，无人机悬停时\(\mathbf{b}_3\)指向正上方。机体坐标系用于描述无人机自身各个部件的相对位置和运动。
惯性特征
质量是物体抵抗「平动」的能力，而转动惯量则是物体抵抗「旋转」的能力。例如，一根长棍子，绕中心旋转容易（转动惯量小），绕端点旋转难（转动惯量大）。
惯性积描述绕不同轴的旋转之间的耦合效应。由于\(\mathbf{b}_i\)是主轴，四旋翼绕这些轴旋转时，不会产生交叉的惯性积（类似「不同方向的力不互相干扰」），因此相对于质心的惯性矩阵\(I\)在\(\mathbf{b}_i\)坐标系下是一个对角矩阵： \[ I = \begin{bmatrix} 绕 \mathbf{b}_1 的转动惯量 & 0 & 0 \\ 0 & 绕 \mathbf{b}_2 的转动惯量 & 0 \\ 0 & 0 & 绕 \mathbf{b}_3 的转动惯量 \end{bmatrix} \]
转动惯量矩阵是对角矩阵时，旋转动力学方程解耦，运动更加简单。从文档中给的数值看，绕\(\mathbf{b}_3\)的转动惯量最大，所以四旋翼绕垂直轴\(\mathbf{b}_3\)旋转更难，因为四个电机分布在平面内，远离旋转轴。
电机模型
回顾力矩的概念，力矩描述力对物体产生旋转效果的能力，是力与力臂的叉乘：\(\tau = r \times F\)。其中，\(r\)是从转动轴到力作用点的位置矢量，\(F\)是施加的力，向量叉乘\(\times\)的方向由右手定则确定。用扳手拧螺丝时，力越大（\(F\)越大），或扳手越长（\(r\)越大），拧螺丝越容易（力矩\(\tau\)越大）。如果垂直于扳手施力，力矩最大；如果斜着推，力矩会减小（因为只有垂直分量的力有效）。
记旋翼角速度为\(\omega_i\)，它的方向由右手定则判定。图1中，逆时针转时方向朝上（\(+\mathbf{b}_3\)方向），顺时针转时方向朝下（\(-\mathbf{b}_3\)方向）。旋翼旋转产生垂直推力\(F_i = k_F \omega_i^2\)，推力方向始终朝上（\(+\mathbf{b}_3\)方向），与旋翼旋转方向无关（仅由桨叶攻角决定）。同时，旋翼旋转时，空气反作用产生的反扭矩\(M_i = k_M \omega_i^2\)，反扭矩方向与角速度矢量\(\omega_i\)方向相反。电机的响应特性可以用一阶微分方程描述：
\[ \dot{\omega}_i = k_m (\omega_i^{\text{des}} - \omega_i) \]
其中\(k_F\)、\(k_M\)、\(k_m\)均为参数。建模时可以假设电机控制器是理想的，即实际电机速度\({\omega}_i\)等于指令电机速度\({\omega}_i^{\text{des}}\)。
刚体动力学
运动学
四旋翼飞行器中，将机体坐标系\(\mathcal{B}\)中的向量转换到惯性坐标系\(\mathcal{A}\)的旋转矩阵为
\[ \mathbin{^\mathcal{A}[R]_\mathcal{B}} = \begin{bmatrix} c\psi c\theta - s\phi s\psi s\theta & -c\phi s\psi & c\psi s\theta + c\theta s\phi s\psi \\ c\theta s\psi + c\psi s\phi s\theta & c\phi c\psi & s\psi s\theta - c\psi c\theta s\phi \\ -c\phi s\theta & s\phi & c\phi c\theta \end{bmatrix} \]
上标\(\mathcal{A}\)表示目标坐标系，即我们希望将向量转换到惯性坐标系\(\mathcal{A}\)中。下标\(\mathcal{B}\)表示源坐标系，即原始向量是在机体坐标系\(\mathcal{B}\)中表示的。\(c = \cos\)，\(s = \sin\)。\(\phi\)称为滚转角（roll），是绕\(\mathbf{b}_1\)转的角度；\(\theta\)称为俯仰角（pitch），是绕\(\mathbf{b}_2\)转的角度；\(\psi\)称为偏航角（yaw），是绕\(\mathbf{b}_3\)转的角度。这三个角度称为欧拉角，旋转矩阵\(R\)基于\(Z-X-Y\)欧拉角构建。
角速度矢量
\[ \mathbin{^\mathcal{A}\omega_\mathcal{B}} = p\mathbf{b}_1 + q\mathbf{b}_2 + r\mathbf{b}_3 \]
符号\(\mathbin{^\mathcal{A}\omega_\mathcal{B}}\)中的上标\(\mathcal{A}\)表明角速度的参考系是\(\mathcal{A}\)，即我们是从参考系\(\mathcal{A}\)的视角来观察坐标系\(\mathcal{B}\)的旋转情况。下标\(\mathcal{B}\)表示这个角速度矢量的分量是在坐标系\(\mathcal{B}\)中表示的，即我们使用坐标系\(\mathcal{B}\)的基向量来描述角速度的大小和方向。角速度矢量分解到\(\mathcal{B}\)的三个正交基向量\(\mathbf{b}_1, \mathbf{b}_2, \mathbf{b}_3\)上，分别对应绕三个主轴的角速度分量：\(p\)是绕\(\mathbf{b}_1\)轴的滚转角速度（roll），\(q\)是绕\(\mathbf{b}_2\)轴的俯仰角速度（pitch），\(r\)是绕\(\mathbf{b}_3\)轴的偏航角速度（yaw）。
在惯性坐标系\(\mathcal{A}\)中分析角速度时需通过旋转矩阵进行分量转换，直接使用机体坐标系\(\mathcal{B}\)的分量更方便。首先，机载陀螺仪直接测量\(p,q,r\)，反映机体瞬时旋转状态；其次，姿态控制器设计基于机体坐标系，避免频繁坐标转换；最后，转动惯量矩阵\(I\)在机体坐标系中为对角阵，动力学方程更简洁。因此，本文分析角速度时采用机体坐标系\(\mathcal{B}\)的分量，符合工程实际。
下面的公式描述了四旋翼飞行器的机体角速度\(p, q, r\)与欧拉角的导数\(\dot{\phi}, \dot{\theta}, \dot{\psi}\)之间的转换关系：
\[ \begin{bmatrix} p \\ q \\ r \end{bmatrix} = \begin{bmatrix} \cos\theta & 0 & -\cos\phi \sin\theta \\ 0 & 1 & \sin\phi \\ \sin\theta & 0 & \cos\phi \cos\theta \end{bmatrix} \begin{bmatrix} \dot{\phi} \\ \dot{\theta} \\ \dot{\psi} \end{bmatrix} \]
平动动力学
四旋翼飞行器的平动动力学方程基于牛顿第二定律（Newton's laws of motion）建立，描述了质心在惯性坐标系\(\mathcal{A}\)中的加速度与外力的关系。其形式为：
\[ m \ddot{\mathbf{r}} = \begin{bmatrix} 0 \\ 0 \\ -mg \end{bmatrix} + R \begin{bmatrix} 0 \\ 0 \\ F_1+F_2+F_3+F_4 \end{bmatrix} \]
在四旋翼中，合外力由重力和旋翼推力组成。方程中的列向量表示出了这两个力在惯性坐标系\(\mathcal{A}\)的三个轴\(\mathbf{a}_1, \mathbf{a}_2, \mathbf{a}_3\)上的分量。重力方向竖直向下，大小为\(mg\)。旋翼推力是四个旋翼产生的总推力\(u_1=\sum_{k=1}^4 F_i\)，方向垂直于旋翼平面（沿机体坐标系的\(\mathbf{b}_3\)轴），\(u_1\)可看作第一个输入信号。通过旋转矩阵\(R\)将推力从机体坐标系\(\mathcal{B}\)转换到惯性坐标系\(\mathcal{A}\)。
\(\mathbf{u}_1\)主要用于控制飞行器的垂直运动，并通过姿态调整间接影响其水平移动。当四旋翼水平悬停时（\(\phi = \theta = 0\)），旋转矩阵\(R\)为单位矩阵，推力完全沿惯性系\(Z\)轴（\(\mathbf{a}_3\)）方向，与重力平衡（\(u_1 = mg\)）。当四旋翼倾斜（\(\phi \neq 0\)或\(\theta \neq 0\)）时，推力方向在惯性系中产生水平分量，驱动飞行器平动。推力与重力的合力决定飞行器的升降（如\(u_1 > mg\)时加速上升）。推力的水平分量（由姿态角\(\phi\)和\(\theta\)决定）驱动飞行器前后或左右移动。
转动动力学
四旋翼飞行器的转动动力学方程基于欧拉方程（Euler's rotation equations）建立，描述了机体角加速度与外力矩（由旋翼推力差产生）及陀螺效应（角速度与角动量的耦合）的关系：
\[ I \begin{bmatrix} \dot{p} \\ \dot{q} \\ \dot{r} \end{bmatrix} = \begin{bmatrix} L(F_2 - F_4) \\ L(F_3 - F_1) \\ M_1 - M_2 + M_3 - M_4 \end{bmatrix} - \begin{bmatrix} p \\ q \\ r \end{bmatrix} \times I \begin{bmatrix} p \\ q \\ r \end{bmatrix} \]
它是根据刚体转动的欧拉方程
\[ I \dot{\boldsymbol{\omega}} + \boldsymbol{\omega} \times I \boldsymbol{\omega} = \boldsymbol{\tau}_{\text{ext}} \]
得到的。其中\(I \dot{\boldsymbol{\omega}}\)为角加速度引起的惯性力矩，\(\boldsymbol{\omega} \times I \boldsymbol{\omega}\)为陀螺效应（科里奥利力项），\(\boldsymbol{\tau}_{\text{ext}}\)为外部力矩。
如图2所示，旋翼旋转产生的推力\(F_i\)方向都朝上（\(+\mathbf{b}_3\)方向）。旋翼2和4的推力差产生滚转力矩，绕\(\mathbf{b}_1\)轴：
\[ \tau_{\text{roll}} = L(F_2 - F_4) \]
旋翼3和1的推力差产生俯仰力矩，绕\(\mathbf{b}_2\)轴：
\[ \tau_{\text{pitch}} = L(F_3 - F_1) \]
假设旋翼1和3旋转方向为\(-\mathbf{b}_3\)方向（顺时针），旋翼2和4旋转方向为\(+\mathbf{b}_3\)方向（逆时针）。反扭矩方向与旋翼旋转方向相反，所以\(M_1\)和\(M_3\)是\(+\mathbf{b}_3\)方向，\(M_2\)和\(M_4\)是\(-\mathbf{b}_3\)方向，如图2所示。旋翼反扭矩差产生偏航力矩，绕\(\mathbf{b}_3\)轴：
\[ \tau_{\text{yaw}} = M_1 - M_2 + M_3 - M_4 \]
因此，第二个输入信号\(\mathbf{u}_2\)由旋翼推力差和反扭矩产生：
\[ \mathbf{u}_2 = \begin{bmatrix} 0 & L & 0 & -L \\ -L & 0 & L & 0 \\ \gamma & -\gamma & \gamma & -\gamma \end{bmatrix} \begin{bmatrix} F_1 \\ F_2 \\ F_3 \\ F_4 \end{bmatrix}, \quad \gamma = \frac{k_M}{k_F} \]
\(\mathbf{u}_2\)通过调整旋翼推力差产生控制力矩，实现对滚转、俯仰和偏航角的调节。增加旋翼2的推力（\(F_2 \uparrow\)）或减少旋翼4的推力（\(F_4 \downarrow\)），产生沿\(\mathbf{b}_1\)方向的逆时针滚转力矩，进行滚转控制。增加旋翼3的推力（\(F_3 \uparrow\)）或减少旋翼1的推力（\(F_1 \downarrow\)），产生沿\(\mathbf{b}_2\)方向的前倾俯仰力矩，进行俯仰控制。通过反扭矩差（\(M_1 + M_3 > M_2 + M_4\)），产生沿\(\mathbf{b}_3\)方向的逆时针偏航，进行偏航控制。
当四旋翼绕某一轴旋转时，角动量与其他轴的旋转耦合，产生额外的惯性力矩。由于四旋翼通常满足\(I_{xx} = I_{yy}\)（对称设计），因此有
\[ \boldsymbol{\omega} \times I \boldsymbol{\omega} = \begin{bmatrix} (I_{zz} - I_{yy}) q r \\ (I_{xx} - I_{zz}) p r \\ (I_{yy} - I_{xx}) p q \end{bmatrix} = \begin{bmatrix} (I_{zz} - I_{xx}) q r \\ (I_{xx} - I_{zz}) p r \\ 0 \end{bmatrix} \]
当四旋翼快速滚转（\(p\)大）时，陀螺效应会导致俯仰轴产生额外的力矩（如\(-(I_{xx} - I_{zz}) p r\)），需控制器补偿。悬停时有\(p \approx q \approx r \approx 0\)，陀螺效应可忽略，方程则简化为\(I \dot{\boldsymbol{\omega}} = \boldsymbol{\tau}_{\text{ext}}\)。
总结
平动动力学方程与转动动力学方程共同构建了四旋翼飞行器的完整动力学模型体系。平动动力学方程在惯性坐标系\(\mathcal{A}\)中构建，完整表征了质心运动规律：
\[ m\ddot{\mathbf{r}} = -mg\mathbf{a}_3 + ^\mathcal{A}[R]_\mathcal{B}u_1\mathbf{b}_3 \]
该方程通过旋转矩阵\(^\mathcal{A}[R]_\mathcal{B}\)将机体坐标系下的总推力\(u_1\mathbf{b}_3\)映射至惯性系，从而实现三维位置控制。转动动力学方程则在机体坐标系\(\mathcal{B}\)中构建，揭示刚体旋转特性：
\[ I\dot{\boldsymbol{\omega}} = \mathbf{u}_2 - \boldsymbol{\omega} \times I\boldsymbol{\omega} \]
力矩输入\(\mathbf{u}_2\)实现三轴独立控制，非线性项\(\boldsymbol{\omega} \times I\boldsymbol{\omega}\)物理表征陀螺耦合效应。