摘要
提出了一种实用的多旋翼无人机绳载负载摆角估计方法，该方法对保证多旋翼无人机安全工作至关重要。为了不依赖于额外的(不在系统上的，例如动作捕捉)传感器来监测摆角，本文提出的方法提供了一个自主的（在无人机-负载系统上的）摆角估计，只使用一个惯性测量单元(IMU)和一个附加在负载上的测压单元。利用扰动观测器(disturbance observer。DOB)导出的外力估计来估计摆角。所提出的基于DOB的扰动估计技术的独特结构只利用了IMU数据。
系统动力学
A 带负载和不带负载的无人机动力学
多旋翼无人机动力学模型如下：
$$\left\{\begin{array}{c} m \ddot{\mathbf{x}}=R(\mathbf{q}) T_{f}+m g \mathbf{z}_{\mathbf{e}} \\ J \dot{\mathbf{\Omega}}=T_{m}-\boldsymbol{\Omega} \times J \boldsymbol{\Omega} \end{array}\right.\tag{1}$$其中 $m$ 为无人机质量， $\mathbf{x}=\left[\begin{array}{lll}x & y& z\end{array}\right]^{T}$ 为其在地球坐标系中的位置。
$R(\mathbf{q})$ 是机体坐标系到地球坐标系的旋转矩阵，其中 $\mathrm{q}=\left[\begin{array}{lll}\phi & \theta & \psi\end{array}\right]^{T}$为地球坐标系中的滚转角、俯仰角、偏航角。$T_{f}$为推力扭矩矢量，在机体坐标系内的 $T_{f}=$ $\left[0-0-\Sigma F_{i}\right]^{T}$， $g$ 为重力加速度，$J$为转动惯量, $\boldsymbol{\Omega}=\left[\begin{array}{llr}p & q & r\end{array}\right]^{T}$为在机体坐标系中角速度，而 $T_{m}=\left[\tau_{r} \tau_{p} \tau_{y}\right]^{T}$为在机体坐标系中的推力扭矩矢量。 为了稳定，无人机的姿态运动在滚转和俯仰角度都被限制在±0.3弧度。这使得机体的陀螺效应很小，可以忽略(1)式中的 $\Omega \times J \Omega$。也使得下式角速度坐标变换方程中的变换矩阵 $W(\mathbf{q}) \approx I_{3 \times 3}$。 $\dot{\mathbf{q}}=W(\mathbf{q}) \Omega \approx \Omega \tag{2}$最后，±0.3弧度约束使得小角度假设是有效的，可以令$\sin (*) \approx *$ and $\cos (*) \approx 1$ 。公式(1)可以改写为：平动动力学：
$$\left[\begin{array}{c} \ddot{x} \\ \ddot{y} \\ \ddot{z} \end{array}\right]=\frac{1}{m}\left[\begin{array}{ccc} \Sigma F_{i} s \psi & \Sigma F_{i} c \psi & 0 \\ -\Sigma F_{i} c \psi & \Sigma F_{i} s \psi & 0 \\ 0 & 0 & \frac{-1}{m} c \phi c \theta \end{array}\right]\left[\begin{array}{c} \phi \\ \theta \\ \Sigma F_{i} \end{array}\right]+\left[\begin{array}{c} 0 \\ 0 \\ g \end{array}\right]_{(3)} \tag{3}$$与转动动力学
$$\left[\begin{array}{c} \ddot{\phi} \\ \ddot{\theta} \\ \ddot{\psi} \end{array}\right]=\left[\begin{array}{ccc} \frac{1}{J_{x x}} & 0 & 0 \\ 0 & \frac{1}{J_{y y}} & 0 \\ 0 & 0 & \frac{1}{J_{z z}} \end{array}\right]\left[\begin{array}{c} \tau_{r} \\ \tau_{p} \\ \tau_{r} \end{array}\right] \tag{4}$$为简洁起见，将式(3)(4)表示为
$$\left\{\begin{array}{l} \ddot{\mathbf{x}}=G\left(\mathbf{q}, \Sigma F_{i}\right) \mathbf{\Gamma}+g \mathbf{z}_{e} \\ \ddot{\mathbf{q}}=J^{-1} T_{m} \end{array}\right. \tag{5}$$在进一步的描述中， 其中 $\boldsymbol{\Gamma}=\left[\phi, \theta, \Sigma F_{i}\right]^{T}$ 为无人机控制的状态集 $G\left(\mathbf{q}, \Sigma F_{i}\right)$ 是$\ddot{\mathbf{X}}$ 与 $\boldsymbol{\Gamma}$之间的关系矩阵。
同时，当负载附加时，系绳张力起扰力作用。在这一方面，在方程(5)中增加一个额外的力和一个力矩项，就完成了负载附加后的方程。将式(5)改写后，无人机的动力学方程为
$$\left\{\begin{array}{l} \ddot{\mathbf{x}}=G\left(\mathbf{q}, \Sigma F_{i}\right) \boldsymbol{\Gamma}+g \mathbf{z}_{e}+\frac{1}{m} F_{s} \\ \ddot{\mathbf{q}}=J^{-1}\left(T_{m}+T_{s}\right) \end{array}\right. \tag{6}$$其中$F_{s}=\left[\begin{array}{lll}F_{s, x} & F_{s, y} & F_{s, z}\end{array}\right]^{T}$和$T_{s}=\left[T_{s, x} T_{s, y} T_{s, z}\right]^{T}$分别是由复杂期产生的扰动力和力矩。方程(6)的概念通过推导扰动力与摆角的关系给出了摆角估计的关键思想。
同时，用于仿真的无人机动力学既要考虑全动力学，又要考虑吊挂载荷动力学。因此，我们应该将仿真的动力学写为
$$\left\{\begin{array}{c} m \ddot{\mathbf{x}}=R(\mathbf{q}) T_{f}+F_{s}+m g \mathbf{z}_{c} \\ J \dot{\Omega}=T_{m}+T_{s}-\Omega \times J \Omega \end{array}\right.\tag{7}$$但(7)式不能直接用于仿真，因为计算$F_{s}$和$T_{s}$随时间变化的解还未知。$F_{s}$和$T_{s}$是由系统的机械约束产生的力和力矩。在我们的案例中，机械约束是无人机和负载之间的恒定长度。因此，对于无人机和负载的仿真，我们需要在长度约束的基础上，计算出每个时间步长中的$F_{s}$值。
B 负载-无人机动力学仿真
多年来，对悬载直升机的约束动力学系统建模进行了研究。最有前途的方法之一是Udwadia-Kalaba (UK)方程。U-K方程是基于高斯最小约束原理的动力学方程，其中系统运动由满足约束条件下最接近无约束加速度的系统的加速度矢量控制。在方程中，可以写成
$$\ddot{\zeta}=\arg \min \Delta\left(\ddot{\zeta}_{\delta}\right) \tag{8}$$其中 $\Delta\left(\ddot{\zeta}_{\delta}\right)=\left(\ddot{\zeta}_{\delta}-\ddot{\zeta}_{u}\right)^{T} M\left(\ddot{\zeta}_{\delta}-\ddot{\zeta}_{u}\right)=\Upsilon^{T} \Upsilon$ 需要最小化的成本函数， $\zeta_{\delta}$ 表示所有可能的加速度矢量, $\ddot{\zeta}_{u}$自由状态下不受任何物理约束的加速度而 $M$为质量矩阵。$\Upsilon$ 满足式(8)和约束方程 $A \ddot{\zeta}_{\delta}=b$，则有
$$\Upsilon=\left(A M^{-\frac{1}{2}}\right)^{+}\left(b-A \ddot{\zeta}_{u}\right)=M^{\frac{1}{2}}\left(\ddot{\zeta}_{\delta}-\ddot{\zeta}_{u}\right) \tag{9}$$因此，作用在物体上的总力为
$$F_{\text {total }}=M \ddot{\zeta}=F_{u}+F_{c}=M\left(\ddot{\zeta}_{u}+M^{-\frac{1}{2}}\left(A M^{\frac{-1}{2}}\right)+\left(b-A \ddot{\zeta}_{u}\right)\right) \tag{10}$$其中 $F_{u}=M \zeta_{u}$ 是无约束力 而 $F_{c}=$ $M^{\frac{1}{2}}\left(A M^{\frac{-1}{2}}\right)^{+}\left(b-A \ddot{\zeta}_{u}\right)$是约束力。
对于绳载负载无人机的仿真，设置位置矢量为 $\zeta=\left[\begin{array}{llll}\mathbf{x}_{h} & \mathbf{q}_{h} & \mathbf{x}_{l} & \mathbf{q}_{l}\end{array}\right]^{T}$，其中$\mathbf{x}_{h}$ 和 $\mathbf{q}_{h}, \mathbf{x}_{l}$和$\mathbf{q}_{l}$ 分别给出了无人机和吊挂物的位置和姿态。 质量矩阵 $M=\operatorname{diag}\left(m_{u} I_{3 \times 3}, J_{u}, m_{l} I_{3 \times 3}, J_{l}\right)$ 由 $m_{u}$ 及 $J_{u}, m_{l}$ 及 $J_{l}$组成，分别为无人机和负载的质量和力矩. 将无约束力设为$F_{u}=\left[\begin{array}{ll}{\left[T_{f}+m_{u} g \mathbf{z}_{e}\right]^{T}} & T_{m}^{T} m_{l} g \mathbf{z}_{e}^{T} & 0_{1 \times 3}\end{array}\right]^{T} .$。
摆动角估计与轴向力估计
我们基于负载产生轴向扰动力提出一种摆动角估计技术。因此，我们需要分别估计$x_e、y_e$和$z_e$轴上的扰动力。
首先，我们将定义负载的摆动角度。然后，我们将提出一种利用加速度- DOB[10]估计扰动力的方法。最后，我们将介绍利用估计的干扰力估计摆角的方法。
A 摆动角度的定义
如下图所示。α是$z_e$轴与缆绳在$y_e-z_e$平面上的夹角。β是轴$z^\prime_e$和缆绳之间在$x^\prime_e - z^\prime_e$平面上的夹角，其中$x^\prime_ey^\prime_ez^\prime_e$坐标系为$x_ey_ez_e$绕$x_e$轴旋转得到的，旋转后的轴$z^\prime_e$方向与$y_e-z_e$平面上中缆绳的方向相同。
B DOB导出的扰动力估计
DOB算法最初是为受干扰系统的鲁棒控制而开发的。DOB的主要思想是在下一步的控制中估计干扰量并对其进行补偿。但我们不是用估计的扰动来补偿，而是用它来监测由吊挂载荷产生的扰动力。在Robust acceleration control of a hexarotor UAV with a disturbance observer | IEEE Conference Publication | IEEE Xplore中，引入了一种旨在补偿横向力扰动的DOB算法，与传统的DOB应用于无人机相比，这是一种独特的方法，目标实现姿态控制的鲁棒性。
在此基础上，提出了一种横向力控制方法，实现了无人机各轴的横向加速度控制。由于DOB算法需要输入具有相同物理意义的命令来补偿扰动，因此获得无人机的加速度控制对DOB算法的实现至关重要。
下图为Robust acceleration control of a hexarotor UAV with a disturbance observer | IEEE Conference Publication | IEEE Xplore中引入的DOB作用力控制算法的结构。$\ddot{x}_d$是在原加速度命令的基础上添加扰动补偿信号而生成的每个轴上的整体加速度命令。$\ddot{x}_d$随后如下式被转化为$\Gamma_d$：
为了计算出 公式(11)中的$\Gamma_{\mathrm{d}}$， 需要其总拉力 $\Sigma F_{i}$计算出 $G^{-1}$。虽然许多研究将推力值作为一种可测量的输出状态，但获取精确的推力值非常困难。这种在$\Sigma F_{i}$中的不确定性是由于转子动力学的不完善和电池电压的持续下降，使得电子速度控制器(ESC)难以保持输入指令和输出电压/电流之间的恒定关系。因此，需要引入了一种利用式(1)代替输入输出推力指令关系的数学模型估计$\Sigma F_{i}$的替代方法。得到的$T_f$估计为
$$T_{f}=m R^{-1}(\mathbf{q})\left(\ddot{\mathbf{x}}-g \mathbf{z}_{c}\right) . \tag{12}$$上式中的变量都是已知的，通过估计总推力，我们现在可以在式(11)中计算$Γ_d$，这是一个$Λ_p$的输入命令，$Λ_p$图2为中的灰框，它由带有姿态和推力控制器的六旋翼装置组成($y =Λ_pΓ_d$)。
为了估计干扰力， $\tilde{\ddot{x}}_{\mathrm{d}}+d$的估计量$\hat{\tilde{\ddot{x}}}_{d}$应该被计算。符号 $\hat{*}$ 代表全文中的估计量. 首先, $\Gamma$的估计量$\hat{\Gamma}$导致在 $\ddot{\mathbf{x}}$的基础上计算最近的加速度。 在这个时候，由于轴向力的干扰会影响无人机的正常性能，$\hat{\boldsymbol{\Gamma}}$ 会不同于 $\Gamma$ 。 随后通过使用正常传递函数的倒数$\Lambda_{n}^{-1}$计算出期望姿态指令$\hat{\boldsymbol{\Gamma}}_{\mathrm{d}}$。 $\Lambda_{n}$ 代表 $\Lambda_{p}$ 处于正常状态。然后通过, 信号通过$G$ 模块将 $\hat{\Gamma}_{d}$ 转化为了 $\hat{\tilde{\ddot{x}}}_{d}$ （期望的轴向加速度指令），这也可能与实际轴向加速度指令有很大的不同，因为它是基于$\hat{\boldsymbol{\Gamma}}_{\mathrm{d}}$计算出来的，最后，估计的扰动加速度$\hat{d}$由下式计算出。
$$\hat{d}=\hat{\tilde{\ddot{\mathbf{x}}}}_{d}-\tilde{\ddot{\mathbf{x}}}_{d}=G\left(\mathbf{q}, \Sigma F_{i}\right) \Lambda_{n}{ }^{-1} G^{-1}\left(\mathbf{q}, \Sigma F_{i}\right) \ddot{\mathbf{x}}-\tilde{{\ddot{x}}}_{d} \tag{13}$$ $\hat{d}$为受到外界影响的扰动加速度指令估计值，因此可以通过将质量乘以各轴上估计的扰动来估计各轴上的扰动力，即 $$\hat{F}_{s}=m \hat{d} \tag{14}$$干扰力的估计是本文实现摆动角估计的一个关键概念。该方法的特殊意义在于，$\hat{d}$不仅是一个用于增强系统稳定性的集中值，而且是一个具有物理意义的值，特别是一个通过简单修改作用于系统的外力。此外，它是非常实用的，因为没有其他传感器，但只需要IMU的工作。
我们认为，这种方法是本文中介绍的一个独特的概念。但由于现代无人机硬件的不完善，这种方法在实现上存在一定的局限性。在下一小节中，我们将讨论局限性，并找出解决问题的方法。
C 克服物理极限
所提出的扰动估计的一个问题是几乎不可能精确的$z_e$方向扰动估计。这是因为与$x_e$和$y_e$方向模型相比，推进器的传递函数不断变化，这与上一小节引入的$T_f$估计不准确的原因是一样的。因此，需要另一种方法来补充这种不准确性。
本文提出的实用解决方案是使用一个测绳拉力的测力元件，它给出了绳索产生的整体扰动力的范数$|| F_s||$。根据摆角的定义，地球坐标系中中的轴向扰动力为
$$\left\{\begin{array}{l} F_{s, x}=\left\|F_{s}\right\| \sin \beta \\ F_{s, y}=\left\|F_{s}\right\| \cos \beta \sin \alpha \\ F_{s, z}=\left\|F_{s}\right\| \cos \beta \cos \alpha \end{array}\right. \tag{15}$$其中 $F_{s, x}, F_{s, y}$ 和 $\left\|F_{s}\right\|$ 为已知量，然而 $F_{s, z}$为未知量。然而，尽管有$F_{s, z}$的不确定性，我们现在能够计算摆动角度，因为我们可以改变方程(15)中的前两个方程为
$$\left\{\begin{array}{l} \frac{F_{s, x}}{\left\|F_{s}\right\|}=\sin \beta \\ \frac{F_{s, y}}{\left\|F_{s}\right\|}=\cos \beta \sin \alpha \end{array}\right.\tag{16}$$方程左边的所有变量已知，即可以得到两个变量 ( $\alpha$ 和 $\beta$ )。
D 摆角估计
在我们进行摆动角估计之前，需要对引入的基于DOB的$F_{s,x}, F_{s,y}$估计方法进行修改，以考虑负载。由于动力学由式(5)改变为式(6)，因此式(12)中引入的总体推力估计方法也需要改变。基于式(7)的修正推力估计方程为
$$T_{f}=m R^{-1}(\mathbf{q})\left(\ddot{\mathbf{x}}-g \mathbf{z}_{e}-\frac{1}{m} F_{s}\right) \tag{17}$$其中将$F_s$加到原方程中。因此，我们需要一个精确的$F_s$估计来得到好的$T_f$估计结构.
然而，$F_{s}$ 中的$F_{s, x}$和$F_{s, y}$ 需要精确的估计量 $T_{f}$ 总推力因为公式(13)中的 $G\left(\mathbf{q}, \Sigma F_{i}\right)$ 使用 $\Sigma F_{i}$ 进行计算。同时，总推力估算还需要 $\hat{F}_{s, z}$ (由结合$\hat{\alpha}$与$\hat{\beta}$计算得到)。下图是显示估计值之间关系的图表。这四个估计量都相互依赖。但式(17)的推力估计误差是有限的，因为飞行过程中$F_s$不会超过某一水平。因此，其余的估计量也有一个有限的输出误差。
在此基础上，摆角估计如下:
$$\left\{\begin{aligned} \hat{\beta} &=\arcsin \left(\frac{\hat{F}_{s, x}}{\left\|F_{s}\right\|}\right) \\ \hat{\alpha} &=\arcsin \left(\frac{\hat{F}_{s, y}}{\left\|F_{s}\right\| \cos \hat{\beta}}\right) \end{aligned}\right.$$然后用更新后的估计摆动角度计算更好的$F_{s, z}=\left\|F_{s}\right\| \cos \beta \cos \alpha\\$在方程(15)。.从而补充了 $\hat{F}_{s}$的所有分量而实现了更精确的 $T_{f}$ 估计。估计量之间的关系图如下图所示，这四个估计量都相互依赖。
