无人机物理学

# 无人机物理学 来源：https://iahmed.me/post/drone-physics/ 一架友好的无人机，能够完成令人惊叹的空气动力学壮举。一架友好的无人机，能够完成令人惊叹的空气动力学壮举。 本文介绍无人机物理学。读者应具备初等线性代数、初等微积分和初等经典力学的基础知识。本文改编自我在[自适应控制](https://iahmed.me/about/)方面的研究，以及发表在[AIAA DASC 2023](https://ieeexplore.ieee.org/abstract/document/9925862)上关于`multirotor`（[https://multirotor.readthedocs.io/](https://multirotor.readthedocs.io/)）的论文，这是一个用于无人机的Python仿真框架。本文的符号大量借鉴了Charles Tytler（[https://github.com/charlestytler/QuadcopterSim](https://github.com/charlestytler/QuadcopterSim)）的出色工作。 在[X](https://x.com/hazrmard/status/2067012993003438469)、[Reddit](https://www.reddit.com/r/ControlTheory/comments/1u8e4q1/drone_physics_i_wrote_equations_of_motion_and/)、[HackerNews](https://news.ycombinator.com/item?id=48562521)上加入讨论。 > 更多交互式讲解，请参见[Poor Man’s Autograd](https://iahmed.me/post/poor_mans_autograd/)和[Surprise! A Derivation of Entropy](https://iahmed.me/post/surprise-derivation-entropy/)。 以下主题按顺序介绍： 1. 用于描述飞行器的坐标系。 2. 用于描述飞行器的状态变量。 3. 作用于机体的力和力矩。 4. 运动方程（线性和角）。 5. 电机和螺旋桨如何产生力和力矩。 6. 控制系统如何确定螺旋桨转速。 ## 描述飞行器 多旋翼无人机被建模为六自由度：三个线性轴用于线性运动：$x, y, z$，以及三个角轴用于旋转运动：$\phi, \theta, \psi$。为了使用坐标，需要约定一个坐标系。通常使用[北-东-地（NED）系统](https://en.wikipedia.org/wiki/Aircraft_principal_axes)。正$x$轴方向视为“前”/北方向。正$y$轴为“右”/东，正$z$轴为“下”。这是一个右手坐标系；拇指方向轴的正旋转方向为卷曲手指的方向。例如，正z旋转从+x到+y。 使用两个参考系来表示机体的状态： 1. 名义惯性参考系$n$是静态参考系，其轴与任意的全局方向对齐。它们表示为列向量$\hat{n} = [\hat{x}^n, \hat{y}^n, \hat{z}^n]^T$。 2. 机体固定非惯性参考系$b$的轴相对于运动刚体的重心对齐。它们表示为列向量$\hat{b} = [\hat{x}^b, \hat{y}^b, \hat{z}^b]^T$。机体坐标系随飞行器移动和旋转。设机体坐标系原点连接在无人机的质心上。 ### 线性表示 机体固定参考系$b$固定在飞行器的质心上。即，飞行器相对于机体坐标系的位移始终为0。飞行器的位置由机体坐标系相对于惯性坐标系的位移$\hat{r}^n$表示。 飞行器的速度是机体坐标系相对于惯性坐标系的速度。这里，我们选择用机体坐标系坐标来表示速度。 ### 角表示 机体在惯性参考系中的取向遵循Tait-Bryan角约定。即，取向可由三个顺序旋转描述：偏航（$\psi$）、俯仰（$\theta$）和滚转（$\phi$）——*按此顺序*。旋转顺序很重要。从惯性系$\hat{n}$开始，偏航$\psi$是机体坐标系绕惯性$z$轴的旋转$R(\psi)$。从此偏航后的坐标系$\hat{n}_\psi$开始，俯仰$\theta$是绕新$y$轴的旋转$R(\theta)$。从此偏航和俯仰后的坐标系$\hat{n}_{\psi,\theta}$开始，滚转$\phi$是绕新$x$轴的最终旋转$R(\phi)$。最终产物$\hat{n}_{\psi,\theta,\phi}$即机体参考系。 飞行器的角速度$\hat{w}^T=[\omega_x,\omega_y,\omega_z]$是机体坐标系绕自身旋转的瞬时角速率。而每个取向角都定义在其自身的参考系（$\hat{n}, \hat{n}_{\psi}, \hat{n}_{\psi,\theta}=\hat{b}$）中，这是从惯性轴到机体坐标系的有序旋转序列（先偏航，再俯仰，最后滚转）。 滚转旋转最后发生，其结果是机体坐标系。因此，绕x轴的速率$\omega_x$等于滚转速率$\dot{\phi}$。俯仰发生在初始偏航之后但在滚转之前。因此，绕y轴的速率$\omega_y$是坐标系$\hat{n}_{\psi}$中的俯仰速率，再经过滚转$\phi$旋转后得到机体坐标系。最后，偏航旋转首先在惯性系中发生。因此，绕机体坐标系z轴的速率$\omega_z$是惯性系中的偏航速率，再经过俯仰$\theta$和滚转$\phi$旋转后得到。这可以用矩阵方程表示： $$ \begin{bmatrix} \omega_x \\ \omega_y \\ \omega_z \end{bmatrix} = R(\phi)\cdot R(\theta) \begin{bmatrix} 0 \\ 0 \\ \dot{\psi} \end{bmatrix} + R(\phi) \begin{bmatrix} 0 \\ \dot{\theta} \\ 0 \end{bmatrix} + \begin{bmatrix} \dot{\phi} \\ 0 \\ 0 \end{bmatrix} $$ $$ \begin{bmatrix} \omega_x \\ \omega_y \\ \omega_z \end{bmatrix} = \begin{bmatrix} \dot{\phi} + \dot{\psi} \sin{\theta}\\ - \dot{\psi} \sin{\phi} \cos{\theta} + \dot{\theta}\\ \dot{\psi} \cos{\phi} \cos{\theta} \end{bmatrix} $$ ### 惯性系与机体系的协调 机体坐标系可能因（1）位移和（2）旋转而不同于惯性系。机体坐标系的原点固定在无人机的原点。因此，机体坐标系相对于惯性系的位移就是无人机的惯性位置：$\hat{r}^n=[x,y,z]^T$。 一个相对于机体坐标系原点的向量，如果移动到惯性坐标系原点，将会看起来被旋转了。给定惯性系中的向量$\hat{\mathcal{V}}^n=[x^n,y^n,z^n]^T$和同一向量在机体坐标系原点的形式$\hat{\mathcal{V}}^b=[x^b,y^b,z^b]^T$，从机体到惯性参考系的旋转矩阵$R_b^n$定义为，其中每个旋转矩阵为： $$ \begin{align} R(\phi) = \begin{bmatrix} 1 & 0 & 0 \\ 0 & c\phi & -s\phi \\ 0 & s\phi & c\phi \end{bmatrix} \end{align} $$ $$ \begin{align} R(\theta) = \begin{bmatrix} c\theta & 0 & s\theta \\ 0 & 1 & 0 \\ -s\theta & 0 & c\theta \end{bmatrix} \end{align} $$ $$ \begin{align} R(\psi) = \begin{bmatrix} c\psi & -s\psi & 0 \\ s\psi & c\psi & 0 \\ 0 & 0 & 1 \end{bmatrix} \end{align} $$ $$ \begin{align} \hat{\mathcal{V}}^b &= R(\phi)\cdot R(\theta) \cdot R(\psi) \cdot \hat{\mathcal{V}}^n \\ \hat{\mathcal{V}}^b &= R_n^b \hat{\mathcal{V}}^n \\ \begin{bmatrix} x^b \\ y^b \\ z^b \end{bmatrix} &= \begin{bmatrix} c\psi c\theta & - s\psi c\theta & s\theta\\ s\phi s\theta c\psi + s\psi c\phi & - s\phi s\psi s\theta + c\phi c\psi & - s\phi c\theta\\ s\phi s\psi - s\theta c\phi c\psi & s\phi c\psi + s\psi s\theta c\phi & c\phi c\theta \end{bmatrix} \begin{bmatrix} x^n \\ y^n \\ z^n \end{bmatrix} \end{align} $$ 这里$\phi | \theta | \psi$的$c | s$分别指余弦和正弦。当从惯性视角观察机体坐标系中的力时，此旋转很有用。上述变换可在旋转参考系之间瞬时转换向量。 ### 旋转系中的导数协调 然而，如果参考系旋转时向量正在变化，那么变化率就没那么简单了。向量的变化率来源于（1）向量本身在机体系中的变化，以及（2）坐标系坐标的变化。 即，在以瞬时角速度$\hat{\omega}=[\omega_x,\omega_y,\omega_z]$绕其轴旋转的机体系中，向量在机体系中的时间导数$\hat{\mathcal{V}}^b = \hat{b} \cdot \hat{\mathcal{V}}$，在*同一位置的惯性系*中测量，由[传输定理](https://en.wikipedia.org/wiki/Transport_theorem?oldformat=true)给出： $$ \begin{align} \frac{d \hat{\mathcal{V}}^b}{d t} &= \hat{b} \cdot \frac{d \hat{\mathcal{V}}}{d t} + \frac{d \hat{b}}{d t} \cdot \hat{\mathcal{V}} \\ &= \hat{b} \cdot \frac{d \hat{\mathcal{V}}}{d t} + \hat{\omega} \times \hat{\mathcal{V}}^b \\ &= \hat{b} \cdot \frac{d \hat{\mathcal{V}}}{d t} + \begin{bmatrix} 0 & -\omega_z & \omega_y \\ \omega_z & 0 & -\omega_x \\ -\omega_y & \omega_x & 0 \end{bmatrix} \hat{\mathcal{V}} \end{align} $$ 传输定理与叉积假设机体系具有瞬时角速度$\hat{\omega}$绕每个轴。每个基向量将在时间间隔$dt$内经历旋转。对于小的$dt$，角位移很小，$\hat{\omega} dt$，单位向量尖端所画出的弧线可近似为一条直线，其大小为$1 \cdot \hat{\omega} dt$（弧长等于半径乘以以弧度为单位的角度）： $$ \hat{x}^b \rightarrow \hat{x}^b + 0\hat{x}^b + \omega_z dt \hat{y}^b - \omega_y dt \hat{z}^b \\ \hat{y}^b \rightarrow \hat{y}^b - \omega_z dt \hat{x}^b + 0 \hat{y}^b + \omega_x dt \hat{z}^b \\ \hat{z}^b \rightarrow \hat{z}^b + \omega_y dt \hat{x}^b - \omega_x dt \hat{y}^b + 0 \hat{z}^b $$ 在矩阵形式中，每个列向量表示基向量的变化：$$ \hat{b}_{t+dt} = \hat{b}_{t} + \begin{bmatrix} 0 & -\omega_z & \omega_y \\ \omega_z & 0 & -\omega_x \\ -\omega_y & \omega_x & 0 \end{bmatrix} dt $$ 参考系基向量的变化率为： $$ \frac{d \hat{b}}{dt} = \begin{bmatrix} 0 & -\omega_z & \omega_y \\ \omega_z & 0 & -\omega_x \\ -\omega_y & \omega_x & 0 \end{bmatrix} $$ 矩阵乘法等价于与向量$[\omega_x, \omega_y, \omega_z]^T$的叉积。 加性项是一个虚拟力，用于解释非惯性系。一旦考虑这一点，动力学就可以像惯性系一样求解。这可用于积分机体坐标系中的加速度，以求得机体坐标系速度。 ### 飞行器的状态 飞行器动力学完全由12个状态变量表示：每个维度的线位移和角位移及其时间导数： 1. $\hat{r}^n=[x,y,z]^T$是惯性系中的导航坐标。这是机体坐标系相对于惯性系的线位移。 2. $\hat{v^b}=[v^b_x, v^b_y, v^b_z]^T$是飞行器沿机体坐标系轴的速度。 3. $\hat{\Phi}=[\phi, \theta, \psi]^T$是机体参考系$b$相对于惯性参考系的欧拉角（滚转、俯仰、偏航）取向。 4. $\hat{\omega}=[\omega_x, \omega_y, \omega_z]^T$是沿每个机体坐标系轴的瞬时角速度。 ## 多旋翼无人机的动力学 上一节列举了描述飞行器的变量。本节描述变量如何随时间变化。由于状态分为线性变量和角变量，动力学方程也将分为线性和旋转方程。这里，我们选择用机体坐标系坐标来表示力和力矩： 1. $\hat{F}^b=[F_x^b,F_y^b,F_z^b]^T$是沿三个机体轴的总力，其中$\hat{F}^b=R_n^b \hat{F}^n$。 2. $\hat{\tau}^b=[\tau_x^b,\tau_y^b,\tau_z^b]^T$是沿三个机体轴的力矩，其中$\hat{\tau}^b=R_n^b \hat{\tau}^n$。 作用于飞行器的力是推力和重力。当力施加在距重心一定距离处时，会产生力矩。（我们可以计入阻力，但暂时假设其不重要。）重力$\hat{F_g}^n=[0,0,mg]$在惯性参考系中。在机体系中变为$\hat{F_g}^b=R^b_n \hat{F_g}^n$。$p$个螺旋桨在机体系中的总力（推力）为$\hat{T}=[0,0,\sum_i^p T_i]^T$，其中$T_i$是第$i$个螺旋桨的推力。因此，作用在质心上的总力为$\hat{F}^b=\hat{T} + \hat{F_g}^b$。 多旋翼被建模为刚体。刚体相对于其重心具有恒定的质量分布。 ### 线性运动方程 动力学的基础是牛顿第二运动定律：力是动量的变化率。这里用机体坐标系变量表示。 $$ \hat{F}^b = \frac{d}{dt}(m\hat{v}^b) $$ 上述关系可以利用传输定理提供的抽象重新表述。这将给出根据惯性系作用在机体上的力。在两个坐标系中质量都视为常数。于是得到： $$ \begin{align} \hat{F}^b &= m(\hat{\dot{v}}^b + \hat{\omega} \times \hat{v}^b) \end{align} $$ 线性状态变量（$\hat{r}^n$, $\hat{v}^b$）主要由作用在飞行器上的力决定。因此，求解加速度$\hat{\dot{v}}^b$，并将其与作用在机体坐标系上的加速度$\hat{F}^b / m$相等，即可得到机体坐标系中速度$\hat{v}^b$的变化率。 $$ \begin{align} \hat{T} + R_n^b \hat{F_g}^n &= m(\hat{\dot{v}}^b + \hat{\omega} \times \hat{v}^b)\\ \hat{\dot{v}}^b &= \frac{\hat{T} + R_n^b \hat{F_g}^n}{m} - \hat{\omega} \times \hat{v}^b\\ \begin{bmatrix} \dot{v}_x \\ \dot{v}_y \\ \dot{v}_z \end{bmatrix} &= \begin{bmatrix} - \omega_{y} v_{z} + \omega_{z} v_{y} + g \sin{\theta}\\ \omega_{x} v_{z} - \omega_{z} v_{x} - g \sin{\phi} \cos{\theta}\\ -\frac{T}{m} - \omega_{x} v_{y} + \omega_{y} v_{x} + g\cos{\phi} \cos{\theta} \end{bmatrix} \end{align} $$ 使用$R_b^n$和当前机体坐标系速度$\hat{v}^b$，可得到惯性系中位置$r^n$的变化率。 $$ \begin{bmatrix} \dot{r}^n_x \\ \dot{r}^n_y \\ \dot{r}^n_z \end{bmatrix} = R_b^n \begin{bmatrix} v_x \\ v_y \\ v_z \end{bmatrix} $$ ### 角运动方程 牛顿第二定律也适用于角运动。力矩是角动量的变化率。 $$ \hat{\tau} = \frac{d}{dt}(I\hat{\omega}) $$ 其中$I$是转动惯量矩阵，也称为角质量。 从线性运动到角运动假设力作用在距原点某一距离$\hat{r}$处的点质量$m$上，引起绕旋转轴的转动。只有垂直于位置向量的力分量才会引起旋转。这可以通过叉积得到。注意，平行于位置向量的分量会引起线性加速度。 微小位移