四旋翼无人机运动控制（学习笔记）（一）

CHW_dream

于 2024-07-30 11:23:06 发布

阅读量174

点赞数 9

文章标签：无人机学习笔记

本文链接：https://blog.csdn.net/CHW_dream/article/details/140775006

版权

动力学建模

四旋翼无人机本身为欠驱动系统，无法独立控制位置和姿态。通过控制无人机四个驱动电机的转速来控制产生的升力和力矩来改变无人机的姿态角，从而改变无人机的位置。无人机的升力和力矩与四个旋翼转速的关系式为：

$\begin{bmatrix} f\\ \tau_x\\ \tau_y\\ \tau_z \end{bmatrix}=\begin{bmatrix} c_T & c_T & c_T &c_T \\ \frac{\sqrt{2}}{2}dc_T&-\frac{\sqrt{2}}{2}dc_T & -\frac{\sqrt{2}}{2}dc_T &\frac{\sqrt{2}}{2}dc_T \\ \frac{\sqrt{2}}{2}dc_T& \frac{\sqrt{2}}{2}dc_T & -\frac{\sqrt{2}}{2}dc_T & -\frac{\sqrt{2}}{2}dc_T\\ c_M&-c_M &c_M &-c_M \end{bmatrix}\begin{bmatrix} \omega_{1}^{2}\\ \omega_{2}^{2}\\ \omega_{3}^{2}\\ \omega_{4}^{2} \end{bmatrix} (1)$

其中， $f$ 为无人机的升力， $\tau_x,\tau_y,\tau_z$ 分别为无人机各个轴的力矩， $c_T$ 为升力系数， $c_M$ 为反扭距系数， $d$ 为无人机的轴距， $\omega_i$ 为各个旋翼的转速。

常见的四旋翼无人机动力学建模方法有牛顿-欧拉法和拉格朗日法。当无人机的姿态角变化变化不大即不会导致奇异性问题时，选用牛顿-欧拉法建立无人机动力学模型清晰易懂。本文将采用牛顿-欧拉法建立四旋翼无人机的动力学模型。

定义机体坐标系 $B$ 相对于惯性坐标系 $I$ 的位置矢量为 $\boldsymbol p=\begin{bmatrix} x & y & z \end{bmatrix}^{\textrm{T}}$ ，欧拉姿态角为 $\boldsymbol{\mathit{\Theta}} =\begin{bmatrix} \phi & \theta & \psi \end{bmatrix}^{\textrm{T}}$ ，分别表示滚转角、俯仰角和偏航角。无人机姿态角采用X-Y-Z的形式表示，则机体坐标系 $B$ 到惯性坐标系 $I$ 的旋转矩阵可表示为：

$\bold{R_{b}^{I}}=\boldsymbol{\mathrm{R_z\left ( \psi \right )}}\boldsymbol{\mathrm{R_y\left ( \theta \right )}}\boldsymbol{\mathrm{R_x\left ( \phi \right )}}\\= \begin{bmatrix} c\psi &-s\psi &0 \\ s\psi& c\psi & 0\\ 0& 0 & 1 \end{bmatrix}\begin{bmatrix} c\theta & 0 &s\theta \\ 0 & 1 & 0\\ -s\theta& 0 &c\theta \end{bmatrix}\begin{bmatrix} 1 & 0 & 0\\ 0 &c\phi & -s\phi\\ 0& s\phi &c\phi \end{bmatrix}\\= \begin{bmatrix} c\psi c\theta & c\psi s\theta s\phi - s\psi c\phi & s\phi s\psi + c\psi s\theta c\phi\\ s\psi c\theta & s\psi s\phi s\theta + c\psi c\phi &-s\phi c\psi + s\psi s\theta c\phi \\ -s\theta& s\phi c\theta & c\phi c\theta \end{bmatrix} (2)$

为了简洁表示公式，本文将 $\mathrm{sin}$ 记为s，将 $\mathrm{cos}$ 记为c。

无人机所受的力只有重力和自身产生的升力，由牛顿第二运动方程可得：

$m\boldsymbol{\ddot{p}}=\boldsymbol G-f \boldsymbol e_3 (3)$

其中， $m$ 为无人机的质量， $\boldsymbol G$ 为无人机的重力， $\boldsymbol{e_3}$ 是世界惯性坐标系下的单位向量 $\begin{bmatrix} 0 & 0 &1 \end{bmatrix}^{\textrm{T}}$ 。

将式（3）化简并将升力转换到世界坐标系下表示可得到：

$\boldsymbol{\ddot{p}}=g\boldsymbol{e_3}-\frac{f}{m}\bold{R_b^I}\boldsymbol{e_3}(4)$

将式（4）展开并将 $\bold{R_b^I},\boldsymbol{e_3}$ 代入可得到：

无人机所受的力矩只有自身产生的力矩，由欧拉第二运动方程可得：

$\bold J \dot{\boldsymbol{\omega_b}}+\boldsymbol{\omega_b}\times \bold J \boldsymbol{\omega_b}=\boldsymbol {G_a} + \boldsymbol \tau (6)$

其中， $\boldsymbol {\omega_b}$ 为无人机在机体坐标系下的角速度， $\bold J$ 为无人机的对角转动惯量矩阵， $\boldsymbol {G_a}$ 为陀螺力矩。

$\bold J$ 可表示为：

$\bold J=\begin{bmatrix}J_{xx} &0 &0 \\0 & J_{yy}&0 \\0 & 0&J{zz} \end{bmatrix}(7)$

$\boldsymbol {G_a}$ 可表示为：

$\boldsymbol{G_a}=\begin{bmatrix} G_{a,\phi}\\ G_{a,\theta}\\ G_{a,\psi} \end{bmatrix}=\begin{bmatrix} J_{RP}\omega_{yb}(\omega_{1}-\omega_{2}+\omega_{3}-\omega_{4})\\ J_{RP}\omega_{xb}(-\omega_{1}+\omega_{2}-\omega_{3}+\omega_{4})\\ 0 \end{bmatrix}(8)$

其中， $J_{RP}$ 表示整个电机转子和螺旋桨绕机体转轴的总转动惯量。

将式（7-8）代入（6）中，化简后可得到：

$\left\{\begin{array}{l} \dot{\omega}_{xb}=\frac{1}{J_{xx}}[\tau_{x}+\omega_{yb}\omega_{zb}(J_{yy}-J_{zz})-J_{RP}\omega_{yb}\Omega]\\\dot{\omega}_{yb}=\frac{1}{J_{yy}}[\tau_{y}+\omega_{xb}\omega_{zb}(J_{zz}-J_{xx})+ J_{RP}\omega_{xb}\Omega] \\ \dot{\omega}_{zb}=\frac{1}{J_{zz}}[\tau_{z}+\omega_{xb}\omega_{yb}(J_{xx}-J_{yy})]\end{array}\right.(9)$

其中， $\Omega=-\omega_1 +\omega_2 -\omega_3 +\omega_4$ 。

角速度与欧拉角的变换率之间的关系为：

$\begin{bmatrix} \omega_x\\ \omega_y\\ \omega_z \end{bmatrix}=\begin{bmatrix} 1 & 0 &-s\theta \\ 0&c\phi &c\theta s\phi \\ 0&-s\phi &c\theta c\phi \end{bmatrix}\begin{bmatrix} \dot{\phi}\\ \dot{\theta} \\ \dot{\psi} \end{bmatrix}(10)$

在小扰动的情况下，即欧拉角的各个角度变化较小时，欧拉角的变化率与无人机在机体坐标系下的角速度近似相等，则可得到：

$\begin{bmatrix} \dot{\phi}\\\dot{\theta} \\ \dot{\psi} \end{bmatrix}=\begin{bmatrix} \omega_{xb}\\\omega_{yb} \\ \omega_{zb} \end{bmatrix}(11)$

将式（11）代入式（9）并结合式（5），则最终四旋翼无人机的动力学模型可表示为：

$\left\{\begin{array}{l} \ddot{x}=-\frac{f}{m}\left ( c\psi s\theta c\phi + s\psi s\phi \right )\\ \ddot{y}=-\frac{f}{m}(s\psi s\theta c\phi - c\psi s\phi)\\ \ddot{z}=g-\frac{f}{m}c\phi c\theta \\ \ddot{\phi}=\frac{1}{J_{xx}}[\tau_{x}+\dot{\theta}\dot{\psi}(J_{yy}-J_{zz})-J_{RP}\dot{\theta}\Omega]\\ \ddot{\theta}=\frac{1}{J_{yy}}[\tau_{y}+\dot{\phi}\dot{\psi}(J_{zz}-J_{xx})+ J_{RP}\dot{\phi}\Omega] \\ \ddot{\psi}=\frac{1}{J_{zz}}[\tau_{z}+\dot{\phi}\dot{\theta}(J_{xx}-J_{yy})] \end{array}\right.(12)$

CHW_dream

关注

9
点赞
踩
1

收藏

觉得还不错? 一键收藏
0
评论
四旋翼无人机运动控制（学习笔记）（一）

常见的四旋翼无人机动力学建模方法有牛顿-欧拉法和拉格朗日法。当无人机的姿态角变化变化不大即不会导致奇异性问题时，选用牛顿-欧拉法建立无人机动力学模型清晰易懂。本文采用牛顿-欧拉法建立四旋翼无人机的动力学模型。
复制链接

扫一扫