PS control 论文（1）

对应文章地址：MUDE-based control of quadrotor for accurate attitude tracking - ScienceDirect

这篇文章是阿木实验室创始人戚煜华的最新文章，文章的实验非常充分，具有可重复性。

摘要

针对一种基于高精度执行器动力学模型的四旋翼无人机，本文提出了一种基于改进的不确定性和干扰估计器(modified uncertainty and disturbance estimator, MUDE)的姿态控制器，。执行器动力学近似于一阶加时滞动态模型，而执行器的静态和动态模型都是使用电机测试平台建立出来的。执行器模型被进一步用作虚拟传感器，以提供执行器为控制器生成的扭矩估算值。通过这些估算，可以构建出一个MUDE，其不仅可以补偿姿态动力学中的不确定性和干扰，还可以补偿时间延迟和执行器动力学中的干扰。对执行器扰动抑制的系统性考虑明显改善了机动过程中的整体姿态跟踪性能，这种方法明显优于其他假设在执行器理想状态下的现有解决方案。此外，基于本文所提出的闭环系统的稳定性和性能分析表明可以通过调整单个参数来减少估计误差和闭环跟踪误差。最后，通过对比本文提出的控制方法、经典不确定和干扰估计器以及串级PID控制器应用于实际四旋翼无人机平台后的效果，表明了本文提出的控制方法在信号追踪以及干扰抑制的有效性和优势。

1 引言

据我们所知，本文首次提出了在控制设计中利用高精度执行器动态模型来同时提高姿态跟踪和抗干扰性能的思想。本工作的主要目的是开发一种简单有效的四旋翼姿态控制控制解决方案，它不仅能够在快速的机动过程中提供高精度的跟踪性能，而且能够补偿姿态运动和执行器动力学方面的模型的严重的不确定性和外部扰动。为此，首先建立了四旋翼姿态和执行器动力学模型，并利用电机测试平台对执行器静态和动态模型的参数进行了识别。实验证明了执行器的时延是不可忽略的，因此这是一个一阶加时滞模型，而不是一个简单的一阶模型，采用了执行器动力学描述，并进一步用于控制设计和扰动估计。同时，静态模型作为推力和油门信号之间的数学映射，构成了四旋翼飞行控制器的推力混合模块。提出了一种改进的不确定性和扰动估计器(MUDE)，用于估计和补偿姿态和执行器动力学上的集总扰动，以及执行器的时滞。在一个真实的四旋翼平台上实现了本文控制方法的验证。实验结果表明，与大多数商用无人机中使用的级联PID控制器以及传统基于UDE的控制器相比，本文中的控制方法为步长输入提供了更小的超冲量，为时变输入提供了更好的跟踪精度，以及在干扰下更快的性能恢复能力。本文的主要贡献总结如下：

（1） 利用高精度执行器动力学模型提高反馈控制性能，且明显优于现有的假设理想的执行器或者使用简化的或静态的执行器模型的大部分控制方式。进一步将执行器模型作为虚拟传感器，为反馈控制器提供执行器产生的扭矩估计，有助于扩大系统稳定裕度，提高姿态跟踪的收敛速度，从而提高机动过程中的姿态跟踪精度。

（2） 在设计MUDE中系统考虑扰动执行器动力学，以提高整体扰动抑制能力，具体地说，虚拟传感器还使用在MUDE来获得更好的干扰估计，而现有的方法避免在抗干扰机制中使用不可估量的执行器状态。

（3） 提出了一种在两个不同坐标下分别设计状态反馈控制器和MUDE的新思想，以解决由于考虑执行器动力学而引起的不匹配扰动问题。引入了一种非奇异坐标变换，建立了一个具有等效匹配干扰的辅助系统，并在新的坐标下设计了反馈控制器。

2 四旋翼系统建模

在本节中，首先，利用反馈线性化技术建立了姿态动态模型，并利用反馈线性化技术建模进行了线性化。其次，介绍了执行器模型，并给出了辨识建模结果。

2.1 姿态动力模型

如图1所示，本工作采用X机身结构的四旋翼平台，它由安装在机臂末端的四个电机控制。欧拉角Θ=[𝜙，𝜃，𝜓]用来表示四旋翼的方向，其中𝜙是滚转角，𝜃是俯仰角，𝜓分别是偏航角。通过应用所谓的小角近似，惯性系中四旋翼的姿态动力学可描述为

$$\begin{aligned} \dot{\phi} &=\frac{1}{J_{x x}}\left[\left(J_{y y}-J_{z z}\right) \dot{\theta}_{\bar{\psi}}+J_{r} \bar{\Omega} \dot{\theta}+d_{\mathrm{rx}}+\tau_{x}\right] \\ \theta &=\frac{1}{J_{y y}}\left[\left(J_{z z}-J_{x x}\right) \phi \dot{\psi}-J_{r} \bar{\Omega} \phi+d_{r y}+\tau_{y}\right] \\ \ddot{\varphi} &=\frac{1}{J_{z z}}\left[\left(J_{x x}-J_{y y}\right) \phi \dot{\theta}+d_{r z}+\tau_{z}\right] \end{aligned} \tag{1}$$

其中𝑱=diag（[𝐽𝑥𝑥，𝐽𝑦𝑦，𝐽𝑧𝑧]）是四旋翼的转动惯量，𝐽𝑟表示螺旋桨和转子在𝑧轴中的惯性。 定义𝛺=𝜛1+ 𝜛2− 𝜛3− 𝜛4，其中𝜛𝑖是电机的转速。 𝒅𝜏=[𝑑𝜏𝑥 𝑑𝜏𝑦 𝑑𝜏𝑧]𝑇是扰动转矩，如果四旋翼在实践中不是严格对称的，即惯性积𝐽𝑥𝑦、𝐽𝑦𝑧和𝐽𝑧𝑥不是零，耦合动力学也被集中在这个术语中。 𝝉=[𝜏𝑥 𝜏𝑦 𝜏𝑧]𝑇是四个电机产生的转矩，可以计算为

其中 $J=\operatorname{diag}\left(\left[J_{x x}, J_{y y}, J_{z z}\right]\right)$是四旋翼的转动惯量， $J_{r}$ 表示螺旋桨和转子在𝑧轴中的惯性。定义𝛺=𝜛1+ 𝜛2− 𝜛3− 𝜛4，其中𝜛𝑖是电机的转速。$d_{t}=\left[\begin{array}{lll}d_{z x} & d_{x y} & d_{z z}\end{array}\right]^{T}$ 是扰动转矩， 如果四旋翼在实践中不是严格对称的，即惯性积 $J_{x y}, J_{y z}$ 和 $J_{z x}$ 不为零， 耦合动力学也集中在这一项中。 $\tau=\left[\begin{array}{lll}\tau_{x} & \tau_{y} & \tau_{z}\end{array}\right]^{T}$ 是四个电机产生的转矩，可以计算为

$$\begin{aligned} &\tau_{x}=\frac{\sqrt{2}}{2} l\left(-T_{1}+T_{2}+T_{3}-T_{4}\right), \\ &\tau_{y}=\frac{\sqrt{2}}{2} l\left(T_{1}-T_{2}+T_{3}-T_{4}\right), \\ &\tau_{z}=\frac{c_{{M}}}{c_{T}}\left(T_{1}+T_{2}-T_{3}-T_{4}\right) . \end{aligned} \tag{2}$$

其中𝑙是四旋翼的臂长度，𝑐𝑀和𝑐𝑇是下一节将确定的执行器子参数，而𝑇𝑖是来自第i个执行器的执行器产生的推力。

为了解耦和简化动力学（1），采用反馈线性化技术。 将虚拟输入定义为

$$u_{x}=\frac{\tau_{x}}{J_{x x}}, \quad u_{y}=\frac{\tau_{y}}{J_{y y}}, \quad u_{z}=\frac{\tau_{z}}{J_{z z}}, \tag{3}$$

并将集总干扰定义为

$$\begin{aligned} &f_{\phi}=\frac{1}{J_{x x}}\left[\left(J_{y y}-J_{z z}\right) \dot{\theta} \dot{\psi}+d_{e x}+J_{y} \bar{\Omega} \dot{\theta}\right] \\ &f_{\theta}=\frac{1}{J_{y y}}\left[\left(J_{z z}-J_{x x}\right) \phi \dot{\psi} \dot{\psi}+d_{x y}-J_{y} \bar{\Omega} \phi\right] \\ &f_{\psi}=\frac{1}{J_{z z}}\left[\left(J_{x x}-J_{y y}\right) \dot{\phi} \dot{\phi}+d_{z z}\right] . \end{aligned}\tag{4}$$

则动力学（1）可以改写成如下简化的形式

$$\dot{\theta}=u+f \text {. } \tag{5}$$

其中$u=\left[\begin{array}{lll} u_{x} & u_{y} & u_{z} \end{array}\right]^{T}, f=\left[\begin{array}{lll} f_{\phi} & f_{\theta} & f_{\psi} \end{array}\right]^{T} .$

备注1. 为了减少模型不确定性对控制性能的影响，在四旋翼动力学中耦合项$\left(J_{y y}-J_{z z}\right) \dot{\theta} \psi,\left(J_{z z}-J_{x x}\right) \phi \dot{\psi}$, 和$\left(J_{x x}-J_{y y}\right) \dot{\theta} \dot{\phi}$ 被包含在（4）中的集总干扰项中，并将通过所提出的基于MUDE的控制对它们进行动态估计和补偿。如果这些耦合项可以精确已知或近似在合理的范围内，他们可以合并入虚拟输入（3)并被基于反馈线性化技术产生的相应的补偿项所抵消，而剩余建模误差将包含在集总干扰(4)中。

如图2所示，一个四旋翼的每个执行器由一个ESC、一个电机以及一个螺旋桨组成。电机由基于每个电机的输入油门$𝜎_𝑖$和电池电压$𝑈_𝑏$ 输入的ESC所驱动，而ESC将油门信号转换为多个失相电压输出。使用以下方程式描述的旋转电机驱动螺旋桨产生的力$𝑇_𝑖$和无功力矩$𝜏_𝑖$：

$$𝑇_𝑖=c_T𝜛^2_i，𝜏_𝑖=c_M𝜛^2_i \tag{6}$$

其中$𝑐_𝑇$和$𝑐_M$分别是推力常数和转矩常数。

为了确定执行器模型的内部参数，采用来自RCbenchmark的第1580系列推力支架，其应用方式如图3所示。可以同时测量和记录油门指令、推力、扭矩、电压以及电机转速。

2.2.1 静态模型辨识

在相同的实验条件下（正常的大气温度和工作范围内的电池电压），给出了[0,1]范围内的不同油门指令，并记录了电机达到稳态时的结果。将实验数据导入矩阵曲线拟合工具箱，得到静态模型识别结果，如图4所示。

观察到推力/扭矩与转速之间的关系满足二次方程（6）。 然后从拟合曲线可以得到电机参数：$𝑐_𝑇=8.308×10^{−8}N∕RPM^2，𝑐_𝑀=1.456×10^{−9}N m/RPM^2$。

要将推力指令$𝑇_𝑖$（从控制器获得的控制输入）映射到油门信号$𝜎_𝑖$（自动驾驶仪中的低级别信号），应采用以下四阶多项式近似：

$$𝜎_𝑖=p_1T_i^4+p_2T_i^3+p_3T_i^2+p_4T_i+p_5 \tag{7}$$

其中$p_1 = −0.00069, p_2 = 0.01271, p_3 = −0.07948, p_4 = 0.30521, p_5 = 0.00878$。

2.2.2 动态模型辨识

静态模型只给出了一个基于静态信号的电机动力学的一个简单的近似值。然而，在电机的动态响应中，需要系统地考虑时间延迟和响应时间。本文将该模型简化为以下一阶加时延形式，而不是为各个执行机构建立一个全面的模型（考虑电气，机械和空气性能）。

$$T=\frac{e^{-\tau_{0} s}}{\alpha s+1} T_{s s} \tag{8}$$

其中， $\tau_{0}$是时间延迟， $\alpha$ 是时间常数， $T_{s s}$是力的稳态。

在相同的实验条件下，向系统输入阶跃信号，记录每次试验的实验数据。从5图中所示的曲线拟合结果表明，执行器响应的时滞是显著的，响应过程接近FOPDT模型。

为了提高识别结果的准确性，本实验在不同的初始以及最终输入油门信号的环境下重复了8次，覆盖了电机的大部分工作范围。

从表一中实验结果可以看出，执行器的时滞总是存在的。由于在大多数无人机中，姿态控制的控制频率只有250Hz（每个控制周期0.004s），因此在实际操作中不能忽视执行器的时间延迟。平均实验的测试结果得到：

$$\alpha=0.035, \quad \tau_{0}=0.04 \mathrm{~s} .\tag{9}$$

将（8)代入(2），给出执行器在时域上的动态模型为

$$\dot{\boldsymbol{\tau}}(t)=-\frac{1}{\alpha} \boldsymbol{\tau}(t)+\frac{1}{\alpha} \boldsymbol{u}_{\tau}\left(t-\tau_{0}\right)+\boldsymbol{d}_{\tau},\tag{10}$$

其中 $u_{\tau}=\left[\begin{array}{lll}u_{\phi} & u_{\theta} & u_{\psi}\end{array}\right]^{T}$ 是扭矩命令， 而 $d_{\tau}$表示在执行器上的集中干扰，如电池电压降和接地效应。

在实际应用中，公式化执行器模型，即公式（10），令 $d_{\tau}=0$，则将其作为虚拟传感器来估计电机产生的转矩。在转矩估计良好的情况下，可以通过系统地考虑非理想执行器来设计内环控制器，以实现姿态的高精度控制。

备注2.建模结果见表1和公式（9），且只适用于第5节中所述特定执行机构系统(包括电机、ESC和电池)。如果执行器的任何部件被更改，则需要重新进行模型建立。建模实验在相同的实验条件下进行：常温（25.0◦C）和工作范围蓄电池电压（11.1∼12.6V）。在实验条件合理的变化范围内，鉴定结果是有意义的。为了验证建模结果，将从实际飞行中记录的相同扭矩命令 $u_{\tau}$ 应用于推力支架和建立的标准执行器模型，然后比较了该模型对试验台的推力测量结果和推力估计结果。请注意，推力估计是由𝑇=𝜏/𝐿计算的，其中𝜏是估计的扭矩，而𝐿=0.165m是四旋翼的臂长。比较结果如图6所示，基于此可以看出估计误差约为±0.1N。本文的执行器模型具有较高的精度，即验证结果只表现出估计误差，其幅度为实际执行器产生的推力的4%。

3 MUDE控制器设计

结合(1)和(10)公式，可以得到全内环动力学(包括姿态和执行器动力学）的线性时变(LTI)模型如下式所示.

$$\left[\begin{array}{c} \dot{\Theta} \\ \ddot{\Theta} \\ \dot{\tau} \end{array}\right]=\left[\begin{array}{ccc} 0 & \boldsymbol{I}_{3} & 0 \\ 0 & 0 & \boldsymbol{J}^{-1} \\ \mathbf{0} & 0 & -\frac{1}{\alpha} \boldsymbol{I}_{3} \end{array}\right]\left[\begin{array}{c} \Theta \\ \dot{\Theta} \\ \tau \end{array}\right]+\left[\begin{array}{c} 0 \\ 0 \\ \frac{1}{\alpha} \boldsymbol{I}_{3} \end{array}\right] u_{\tau}+\left[\begin{array}{c} 0 \\ \boldsymbol{J}^{-1} \boldsymbol{f}_{1} \\ \boldsymbol{f}_{2} \end{array}\right] \tag{11}$$

其中 $\boldsymbol{I}_{3}$ 为 $3 \times 3$ 单位矩阵, $\boldsymbol{f}_{1}=\boldsymbol{f}$是总扰动扭矩,而 $f_{2}=\frac{1}{\alpha}\left[u_{\tau}\left(t-\tau_{0}\right)-u_{\tau}(t)\right]+d_{\tau}$ 是执行器上的集中扰动。对集中扰动采用以下假设。

假设1. 扰动$f_{1}, f_{2}$和导数$\dot{f}_{1}$对于所有$t \geq 0$的情况都是有界的。

下面，将分别设计每个通道的控制器。请注意，这三个通道的动力学形式与（11）给出的形式相同。因此，在不丧失通用性的情况下，设计了俯仰角通道的控制器，然后可以应用具有不同控制增益（根据模型参数𝑱）的相同控制器结构。得到俯仰角运动的动力学模型如下：

$$\left[\begin{array}{c} \dot{\theta} \\ \ddot{\theta} \\ \dot{\tau}_{y} \end{array}\right]=\left[\begin{array}{ccc} 0 & 1 & 0 \\ 0 & 0 & \frac{1}{J_{y y}} \\ 0 & 0 & -\frac{1}{\alpha} \end{array}\right]\left[\begin{array}{c} \theta \\ \dot{\theta} \\ \tau_{y} \end{array}\right]+\left[\begin{array}{c} 0 \\ 0 \\ \frac{1}{\alpha} \end{array}\right] u_{\theta}+\left[\begin{array}{c} 0 \\ \frac{f_{1}}{J_{y y}} \\ f_{2} \end{array}\right] \tag{12}$$

其中，$f_{1}$和$f_{2}$分别表示俯仰执行器中的集中扰动转矩和执行器上的集中扰动。请注意，扰动转矩$f_{1}$与转矩命令$𝑢_𝜃$不出现在同一通道中，这被称为不匹配扰动。抑制不匹配扰动的关键难点不在于扰动估计，而在于难以针对$𝑢_𝜃$设计适当的补偿信号来抵消不同通道的影响。为了解决这个问题，引入了一个坐标变换来开发一个具有匹配干扰并等价于（12）的辅助系统。定义一个辅助变量

$$\eta=\tau_{y}+f_{1},\tag{13}$$

它表示四旋翼上的总扭矩，然后求导得到

$$\begin{aligned} \dot{\eta} &=-\frac{1}{\alpha} \tau_{y}+\frac{1}{\alpha} u_{\theta}+f_{2}+\dot{f}_{1} \\ &=-\frac{1}{\alpha} \eta+\frac{1}{\alpha} u_{\theta}+f_{2}+\frac{1}{\alpha} f_{1}+\dot{f}_{1} . \end{aligned} \tag{14}$$

动力学公式（12）可以转化为
$$\left[\begin{array}{c}\dot{\theta} \\ \ddot{\theta} \\ \dot{\eta}\end{array}\right]=\left[\begin{array}{ccc}0 & 1 & 0 \\ 0 & 0 & \frac{1}{J_{y y}} \\ 0 & 0 & -\frac{1}{\alpha}\end{array}\right]\left[\begin{array}{c}\theta \\ \dot{\theta} \\ \eta\end{array}\right]+\left[\begin{array}{l}0 \\ 0 \\ \frac1{\alpha}\end{array}\right]\left(u_{\theta}+\bar{f}\right) \tag{15}$$,
其中$\bar{f}=\alpha f_{2}+f_{1}+\alpha \dot{f_{1}}$ 是等效匹配扰动，该辅助系统称为变换姿态控制模型，可以更直观地设计了抗干扰机制。

为了以（15）的形式控制一类扰动二阶系统，可以在经典PD控制器中引入UDE来动态补偿集中扰动的影响。最初在中UDE的基本思想是，在频域中，集中的扰动通过具有适当带宽的滤波器来近似。使用相同的想法，当给出所需的状态 $\ddot{\theta}_{d}, \dot{\theta}_{d}$和$\theta_{d}$时，设计了以下形式的基于MUDE的控制器

$$u_{\theta}=\ddot{\theta}_{d}+k_{p} \tilde{\theta}+k_{d} \tilde{\dot\theta}+k_{\eta} \tilde{\eta}-\hat{f},\tag{16}$$

其中

$$\tilde{\theta}=\theta_{d}-\theta, \quad \tilde{\dot\theta}=\dot{\theta}_{d}-\dot{\theta} \quad \tilde{\eta}=\eta_{d}-\eta \text {, }\tag{17}$$

而 $\hat{\bar{f}}$ 是估计总扰动 $\bar{f}$的MUDE。 但是由于以下两个事实，控制器（16）不能直接使用。首先，状态反馈 $\eta$ 是不可用的，因为 $\eta$ 包含一个未知的干扰 $f_{1}$ 而且因此 $\eta$ 是不可测的。其次，由于$\eta$是不可用的，所以 $\hat{f}$不能从变换的姿态控制模型（15）中导出。 $\eta$.

为了解决这些问题，下面使用了姿态控制模型(12)。虽然 $\bar{f}$ 不能直接估计，但它的估计仍然可以通过分别通过估计三个独立的对象来组成。这些估计数分别用 $\hat{f}_{1}, \hat{\dot{f}}_{1}$ 以及 $\hat{f}_{2}$表示，并将在之后进行设计。对于 $\hat{f}_{1}$ ，

$$\tilde{\eta}=\tau_{d}-\eta=\tilde{\tau}-\hat{f}_{1},\tag{18}$$

然后将控制器(16)修改为

$$u_{\theta}=\underbrace{\ddot{\theta}_{d}+k_{p} \tilde{\theta}+k_{d} \tilde{\dot\theta}+k_{\eta} \tilde{\tau}}_{\text {nominal control }}-\underbrace{\left[\left(k_{\eta}+1\right) \hat{f}_{1}+\alpha \hat{\dot{f}}_{1}+\alpha \hat{f}_{2}\right]}_{\text {MUDE }}], \tag{19}$$

其中 $\tilde{\tau}=\tau_{d}-\tau_{y}$ 可以通过使用不受干扰的模型（10）作为具有输入$u_{\theta}$的虚拟传感器来获得, 而 $\tau_{d}$ 是期望的扭矩 ，计算公式为

$$\tau_{d}=J_{y y} \ddot{\theta}_{d} \text {. }\tag{20}$$

下一步是通过分别为 $f_{1}, \dot{f}_{1}$ 和 $f_{2}$设计估计器在(19)中构造MUDE。 首先，类似于经典的UDE， $f_{1}$可以设计为

$$\hat{F}_{1}(s)=G_{f 1}(s) F_{1}(s)=G_{f 1}(s)\left[J_{y y} \ddot{\theta}(s)-\tau_{y}(s)\right], \tag{21}$$

而且选择

$$G_{f_{1}}(s)=\frac{1}{T^{f_{1} }s+1}, T^{f_{1}}>0, \tag{22}$$

生成

$$\hat{F}_{1}(s)=\frac{s}{T^{f_{1} }s+1} \dot{\theta}(s)-\frac{1}{T^{f_{1} }s+1} \tau_{y}(s) .\tag{23}$$

其次， $\dot{f}_{1}$的估计器被设计来满足

$$\hat{\dot{F}}_{1}(s)=G_{\dot{f}_{1}}(s)\left[\ddot{\theta}(s)-\dot{\tau}_{y}(s)\right] .\tag{24}$$

其中 $\ddot{\theta}(s), \ddot{\theta}(s)$ 和 $\dot{\tau}_{y}(s)$, 表示角加速度、角加加速度和扭矩的导数，不能从机载传感器测量，因此滤波器$$G_{f_{1}}(s)$$的相对阶数必须不能低于2以确保（24）的物理可实现性。因此，应考虑以下二阶滤波器

$$G_{\dot{f}_{1}}(s)=\frac{1}{T_{1}^{\dot{f}_{1}} s^{2}+T_{2}^{\dot{f}_{1}} s+1}, T_{1}^{f_{1}}>0, T_{2}^{f_{1}}>0,\tag{25}$$

可以推出：

$$\hat{F}_{1}(s)=\frac{s^{2}}{T_{1}^{\dot{f}_{1}} s^{2}+T_{2}^{\dot{f}_{1}} s+1} \dot{\theta}(s)-\frac{s}{T_{1}^{\dot{f}_{1} }s^{2}+T_{2}^{\dot{f}_{1}} s+1}{\tau_{y}}(s) \text {. } \tag{26}$$

第三， $f_{2}$可以由下式被估计

$$\hat{F}_{2}(s)=G_{f_{2}}(s)\left[\dot{\tau}_{y}(s)+\frac{\tau_{y}(s)}{\alpha}-\frac{u_{\theta}(s)}{\alpha}\right] . \tag{27}$$

其中

$$G_{f_{2}}(s)=\frac{1}{T^{f_{2}}s+1}, T^{f_{2}}>0,\tag{28}$$

被选择以避免使用不可用的信号 $\tau_{y}(s)$。结合公式（16），（27）以及（28）可以生成

$$\begin{aligned} \hat{F}_{2}(s)=& \frac{1}{\alpha T^{f_{2}} s}\left[(\alpha s+1) \tau_{y}(s)-\ddot{\theta}_{d}(s)-k_{p} \tilde{\dot\theta}(s)-k_{d} \bar{\theta}(s)\right.\\ &\left.-k_{\eta} \tilde{\tau}_{y}(s)+\left(k_{\eta}+1\right) \hat{F}_{1}(s)+\alpha \hat{\dot{F}}_{1}(s)\right] . \end{aligned} \tag{29}$$

然后，从以设计公式（23），（26）以及（19）构造了MUDE。

备注3 针对所考虑的问题，经典的基于UDE的控制器满足

$$u_{\theta}^{\mathrm{UDE}}=\underbrace{\ddot{\theta}_{d}+k_{p} \tilde{\theta}+k_{d} \tilde{\dot\theta}}_{\text {nominal control }}-\underbrace{\left[-\frac{1}{T_{f}}\left(k_{d} \tilde{\theta}+k_{p} \int_{0}^{t} \tilde{\theta} d t+\tilde{\theta}\right)\right]}_{\mathrm{UDE} \hat{f}_{1}} .\tag{30}$$

MUDE是经典UDE的一个变体，因为它们具有相同的设计理念：基于频域的设计和使用适当的滤波器以避免使用状态导数测量。然而，基于MUDE的控制在几个方面都区别于经典的基于UDE的控制。首先，基于MUDE的控制器（19）还包含了一个转矩反馈项$k_{\eta} \tilde{\tau}$ 扩大系统的稳定裕度，提高系统的暂态性能。这一修改对于在四旋翼积极机动时实现高精度的姿态跟踪至关重要。其次，基于MUDE的控制是在两种不同坐标（12)和(15）的基础上来设计的，以解决考虑现实执行器带来的不匹配扰动问题，而经典的基于UDE的控制是基于简单的扰动双积分器模型（5）设计的。MUDE对扰动𝑓1的估计精度高于经典的UDE，因为经典的基于UDE的控制假设理想的执行器，基于$u_{\theta}=\tau_{y}$ 而进行$\hat{f}_{1}$的推导，这导致因实际上$𝑢_𝜃$和$𝜏_𝑦$之间的存在差别而产生的估计误差。最后，MUDE不仅能够估计对姿态运动的集中扰动$𝑓_1$，而且还能够估计时间延迟和扰动（由$𝑓_2$表示）对执行器的影响。

4.稳定性和性能分析

跟踪误差的动力学模型为

$$\left[\begin{array}{c} \tilde{\dot\theta} \\ \tilde{\ddot{\theta}} \\ \tilde{\dot\eta} \end{array}\right]=A \underbrace{\left[\begin{array}{c} \tilde{\theta} \\ \tilde{\dot\theta} \\ \bar{\eta} \end{array}\right]}_{\tilde{x}}-B\left[\left(k_{\eta}+1\right) \bar{f}_{1}+\alpha\left(\bar{\dot{f}}_{1}+\bar{f}_{2}\right)\right] .\tag{31}$$

其中

$$\begin{aligned}&\boldsymbol{A}=\left[\begin{array}{ccc} 0 & 1 & 0 \\ 0 & 0 & \frac{1}{J_{y y}} \\ -\frac{k_{p}}{\alpha} & -\frac{k_{d}}{\alpha} & -\frac{\left(k_{\eta}+1\right)}{\alpha} \end{array}\right], \quad \boldsymbol{B}=\left[\begin{array}{c} 0 \\ 0 \\ \frac{1}{\alpha} \end{array}\right], \\ &\tilde{f}_{1}=f_{1}-\hat{f}_{1}, \quad \tilde{f}_{1}=f_{1}-\hat{f}_{1}, \quad \tilde{f}_{2}=f_{2}-\hat{f}_{2} . \end{aligned}$$ $\boldsymbol{A}$ 的特征多项式 $$\left|s \boldsymbol{I}_{3}-\boldsymbol{A}\right|=s^{3}+\frac{k_{\eta}+1}{\alpha} s^{2}+\frac{k_{d}}{J_{y y} \alpha} s+\frac{k_{p}}{J_{y y} \alpha} .\tag{34}$$

利用Routh判据，给出了稳定性条件

$$k_{p}>0, k_{d}>0, k_{\eta}>-1,\left(k_{\eta}+1\right) k_{d}-\alpha k_{p}>0 .\tag{35}$$

在稳定性条件下，跟踪精度依赖于估计残差

$$\tilde{f}_{\text {res }}=\left(k_{\eta}+1\right) \tilde{f}_{1}+\alpha\left(\tilde{f}_{1}+\tilde{f}_{2}\right) .\tag{36}$$

可以看出 $\alpha\left(\tilde{f}_{1}+\tilde{f}_{2}\right)$ 对跟踪精度影响不大，由于电机的时间常数$\alpha \approx 0.04$ 很小. 因此估计器的带宽$\dot{f}_{1}$ 和 $f_{2}$(这也决定了噪声灵敏度) 不一定要太大。估计误差满足

$$\begin{aligned} &\tilde{F}_{1}(s)=\left[1-G_{f_{1}}(s)\right] F_{1}(s)=\frac{T^{f_{1}} s}{T^{f_{1} s+1}} F_{1}(s), \\ &\tilde{F}_{1}(s)=\left[1-G_{f_{1}}(s)\right] \dot{F}_{1}(s)=\frac{T_{1}^{f_{1}} s^{2}+T_{2}^{\dot{f}_{1}} s}{T_{1}^{f_{1}} s^{2}+T_{2}^{f_{1}} s+1} \dot{F}_{1}(s), \\ &\tilde{F}_{2}(s)=\left[1-G_{f_{2}}(s)\right] F_{2}(s)=\frac{T^{f_{2}} s}{T^{f_{2} s+1}} F_{2}(s) . \end{aligned}\tag{37-39}$$

为了说明估计误差是如何与滤波器参数相关的，应用了以下参数映射

$$c_{0}=\varepsilon \alpha_{1}, c_{1}=\varepsilon \alpha_{2}, \ldots, c_{k}=\varepsilon \alpha_{k},\tag{40}$$

稳定低通滤波器的一般形式为：

$$G(s)=\frac{1}{c_{0} s^{k+1}+\cdots+c_{k} s+1},\tag{41}$$

其中 $\alpha_{1}, \ldots \alpha_{k+2}$ 和 $\varepsilon$是正参数值。 然后可以得到以下结果。

引理1. 对于所有式(41)中参数为(40)的稳定滤波器，使$\varepsilon \rightarrow 0$ 导致 $\bar{F}(s)=[1-G(s)] F(s)$足够小 以保证 $F(s)$ 是有界的。
证明： 由 (40) 和 (41)，容易得到

$$\tilde{F}(s)=\varepsilon \cdot \frac{\alpha_{1} s^{k+1}+\cdots+\alpha_{k+1} s}{\varepsilon \alpha_{1} s^{k+1}+\cdots+\varepsilon \alpha_{k+1} s+1} F(s) .\tag{42}$$

注意映射(40)不会改变滤波器$G(s)$的极点 。因为它的分母与与$G(s)$相同， $$ \frac{\alpha_{1} s^{k+1}+\cdots+\alpha_{k+1} s}{\varepsilon \alpha_{1} s^{k+1}+\cdots+\varepsilon \alpha_{k+1} s+1}$$有界输入-有界输出稳定的。然后由于𝐹(𝑠)的有界性，很明显，𝜀→0使𝐹(𝑠)任意小，即完成证明。

引理1得出了可以通过调整滤波器参数来减少所提出的UDEs的估计误差 $\hat{F}_{1}, \hat{\dot{F}}_{1}$ 和 $\hat{F}_{2}$， 该结论将用于得到估计残余误差（36）和跟踪误差的最终边界。

定理 1. 在假设1和稳定性条件(35)下，当下列参数映射
$$T^{f_{1}}=\varepsilon \alpha_{1}, T_{1}^{\dot{f}_{1}}=\varepsilon \alpha_{2}, T_{2}^{\dot{f}_{1}}=\varepsilon \alpha_{3}, T^{f_{2}}=\varepsilon \alpha_{4}, \quad \alpha_{1}, \alpha_{2}, \alpha_{3}, \alpha_{4}, \varepsilon>0 \tag{43}$$,
应用于滤波器 $G_{f_{1}}(s), G_{\dot{f}_{1}}(s)$ 和$G_{f_{2}}(s)$，估计残差(36) 被$f_{b}(\varepsilon)$限制， 而当t趋于∞时，跟踪误差 $\|\tilde{x}(t)\|_{2}$ 最终被 $\bar{f}_{b}(\varepsilon)$ 所约束为有界。 其中满足当$\varepsilon \rightarrow 0$时，该边界满足$f_{b}(\varepsilon) \rightarrow 0$ 和 $\bar{f}_{b}(\varepsilon) \rightarrow 0$。

证明： 假设1保证了 $F{1}(s), \dot{F}{1}(s),F_{2}(s)$的有界性。将引理1的结果直接应用于估计误差动力学(37)- (39) 直接产生了 $\tilde{F}_{r e s}(s)=\varepsilon H(s)$，其中

$$H(s)=\frac{\alpha_{1} s}{\varepsilon \alpha_{1} s+1} F_{1}(s)+\frac{\alpha_{2} s^{2}+\alpha_{3} s}{\varepsilon \alpha_{2} s^{2}+\varepsilon \alpha_{3} s+1} \dot{F}_{1}(s)+\frac{\alpha_{4} s}{\varepsilon \alpha_{4} s+1} F_{2}(s) \tag{44}$$

是有界信号。因此在时域中可以得出

$$\tilde{f}_{\text {res }}(t) \leq \varepsilon\|h(t)\|_{\infty} \stackrel{\Delta}{=} f_{b}(\varepsilon),\tag{45}$$

其中 当 $\varepsilon \rightarrow 0$时，$f_{b}(\varepsilon) \rightarrow 0$ 。在稳定条件(35)下，很明显(31)的零输入响应收敛于原点，例如 当$t \rightarrow \infty$，$\left\|\tilde{x}_{z i r}(t)\right\|_{2}=\left\|e^{A t} \tilde{x}(0)\right\|_{2} \rightarrow 0$ 。零状态响应的2-范数满足

$$\begin{aligned} \left\|\tilde{x}_{z s r}(t)\right\|_{2} &=\left\|\int_{0}^{t} e^{A\left(t-t_{s}\right)} \boldsymbol{B} \tilde{f}_{r e s}\left(t_{s}\right) d t_{s}\right\|_{2} \\ & \leq\left\|\int_{0}^{t} e^{A\left(t-t_{s}\right)} d t_{s}\right\|_{2}\|\boldsymbol{B}\|_{2}\left\|\tilde{f}_{r e s}(t)\right\|_{\infty} \\ & \leq\left\|\left(e^{A t}-\boldsymbol{I}_{3}\right)\right\|_{2}\left\|\boldsymbol{A}^{-1}\right\|_{2}\|\boldsymbol{B}\|_{2}\left\|\tilde{f}_{r e s}(t)\right\|_{\infty} \end{aligned} \tag{46}$$

由公式(45)和(46)， 当 $t \rightarrow \infty$导致

$$\|\tilde{x}(t)\|_{2} \leq\left\|\tilde{x}_{z i r}(t)\right\|_{2}+\left\|\tilde{x}_{z s r}(t)\right\|_{2} \leq\left\|A^{-1}\right\|_{2}\|B\|_{2} f_{b}(\varepsilon) \stackrel{\Delta}{=} \bar{f}_{b}(\varepsilon) \tag{47}$$

证毕。

定理1 证明了减少 $\varepsilon$ (相应地减少了滤波器参数 $T^{f_{1}}, T_{1}^{\dot{f}_{1}}, T_{2}^{f_{1}},T^{f_{2}}$ ) 减小稳态估计误差和闭环跟踪误差。这一结果提供了滤波器参数与跟踪精度之间的明确关系，使参数调优更容易。这个结果提供了一个清晰的滤波器参数和跟踪精度之间的关系，使参数调整更容易。

备注4 但在实际应用中，抑制干扰与降低mudde对测量噪声的灵敏度之间存在矛盾. $\theta$ 和 $\dot{\theta}$中测量噪声是由于低成本的板载传感器，而$\tau_{y}$由执行器建模所引起的估计误差和辨识误差，可能一起限制最小可行的$\varepsilon$。因此，需要在提高跟踪性能和降低测量噪声和执行器模型不确定性的敏感性之间进行权衡。

5.实验结果

5.1 实验装置

实验采用了一个由F330D JI框架、PropDrive28261200Kv电机、LitteBee20AESCS、8045螺旋桨和3S2200米Ah25C LIPO电池组成的四旋翼原型。

如图7所示，实验在旋转试验平台上进行。平台的底座安装在桌子上，四旋翼安装在平台的可旋转端。四旋翼车身框架的𝑦轴与平台的旋转轴对齐，从而只能激活四旋翼的俯仰运动。在实验中，带有有效载荷罩的竹棒连接在四旋翼上，有效载荷可以挂在钩上，以模拟俯仰运动上可能时变的外部扰动转矩。

5.2 控制器测试

在实验中，测试了三个控制器，包括级联PID控制器（CPID），由（30）给出的经典的基于UDE的控制器和基于MUDE的控制器（19）。串级PID控制器嵌入在PX4固件中，广泛应用于大多数商用无人机中。它的设计形式如下：

$$u_{\theta}^{\mathrm{CPID}}=k_{p} \tilde{\dot{\theta}}+k_{i} \int_{0}^{t} \tilde{\dot{\theta}} d t+k_{d} \tilde{\ddot{\theta}} \tag{48}$$

其中姿态角速率${\dot{\theta}_d}$的数值由比例控制器${\dot{\theta}_d}=𝑘\tilde{𝜃}$生成。

备注5 在基于MUDE的控制器中，在频域上设计了不确定性和扰动估计器（23)(26)(29）。同时为了实现C语言编程，并在运行PX4固件1.8.2版本的皮克鹰飞行控制器上实现整个解决方案，需要将它们转换为离散时间形式。所提出的解决方案的计算复杂度被认为很低，因为皮克斯鹰只配备了一个运行在168兆赫的STM32F427处理器。这些代码发表在Github网站上，可以在网站https://github.com/potato77/Firmware_MUDE上找到。

前述实验中所研究的反馈控制器，俯仰角由PX4固件中嵌入的EKF模块(扩展卡尔曼滤波)获得，俯仰角速率由陀螺仪测量并通过低通滤波器处理。默认控制器反馈增益如表2所示。需要注意的是，经典UDE和MUDE使用相同的控制反馈增益$𝑘_𝑝$和$𝑘_𝑑$，以保证比较结果的公平性。CPID中选择了经过飞行验证的反馈增益。

5.3 实验结果

实验中考虑的案例研究总结见表3。考虑了三个典型的期望，其中正弦期望满足

$$\theta_{d}=0.5236 \cdot \sin (4 t) \mathrm{rad} \tag{49}$$

其中，0.5236rad=30度几乎是真实飞行期间四旋翼的最大倾斜角度。通过在钩子上悬挂有效载荷(150g)，将扰动添加到四转子俯仰运动中，在稳态时，当𝜃=0时，等效扰动转矩约为0.6N m。当俯仰角度变化时，负载充当对系统的时变扰动（当忽略有效载荷的摆动时，误差为0.6cos𝜃 N m）。在案例1-4中，对CPID控制器（48）、基于UDE的控制（30）和所提出的基于MUDE的控制（19）进行了测试和比较。在案例5-6中，$T^{𝑓_{1}}$和$T^{𝑓_{2}}$的参数分别被应用于MUDE，证明了所提出的调参准则的有效性。在案例7中，手动控制四旋翼在室内环境中飞行，并给出了真实的飞行测试结果。

本文实验视频可以在https://www.bilibili.com/av60962814/（适用于案例1-6）和https://www.bilibili.com/BV1v54y1q7X7（针对案例7）找到。

5.3.1 案例1

试验结果如图8所示，可以看出三个控制器的跟踪误差很接近，但当四旋翼跟踪一个恒定的参考信号时，基于MUDE的控制产生的跟踪误差最小，如图8(a)所示。由于四旋翼处于稳态状态，转矩命令$𝑢_𝜃$收敛到零的一个小邻域，因此电机以几乎恒定的速度旋转，如图8(b)所示。 在这种特殊情况下，执行器的动力学可以被忽略。

如图8（c）-（e）所示的正弦轨迹跟踪结果。 然而，基于MUDE的控制器在跟踪精度上有了显著的提高，而其竞争控制器的结果显示出一些幅值约为0.05∼0.08rad的残差。这是因为当四旋翼进行运动时，由于执行器的动力学和时间延迟，执行器产生的扭矩与扭矩信号不一致。 与CPID和经典的基于UDE的控制相比，MUDE中$\hat{f}_2$项有效地补偿了这种转矩差，所提出的控制器（19）中的附加转矩误差反馈项$k_{\eta} \tilde{\tau}$ 有助于扩大系统稳定裕度，提高姿态跟踪的收敛速度，从而同时提高了整体稳态和瞬态跟踪性能。这一点由这些控制器生成的转矩命令进行验证，如图8(e)所示。CPID缺乏前馈控制，因此转矩命令具有明显的相位延迟，而经典UDE控制中的前馈项在一定程度上有助于纠正这种相位延迟。该MUDE还补偿了执行器的动态延迟和时间延迟，从而进一步纠正了转矩命令中的相位延迟。在以上分析的基础上，对于准确的姿态跟踪问题，特别是当四旋翼快速运动时，考虑执行器动力学的控制设计具有现实意义，其中所考虑的执行器模型是一阶系统。将在案例6中显示，本文考虑的执行器的时间延迟对跟踪性能有显著影响，因此在解决执行器延迟问题上，所提出基于MUDE的控制方法是一个不错的解决方案。

5.3.2 案例2

本文研究了在实际飞行任务中极其常见的阶跃轨迹跟踪。试验结果如图9所示。需要注意的是，由于阶跃型期望相对于时间是不可微的，因此所提出的控制器和经典的基于UDE的控制器的角速率参数由与CPID的相同的比例控制器${\dot{\theta}_d}=𝑘\tilde{𝜃}$计算，而基于MUDE的控制的前馈控制和转矩期望设置为零。结果表明，基于MUDE的控制跟踪误差无超调，收敛速度快，振荡小，稳态误差很小。相比之下，基于CPID和基于UDE的控制会导致不同级别的超调和恢复速度比基于MUDE的控制要慢。在振荡方面，CPID在所有被测试的控制器中是最好的，而经典的UDE在过渡过程中存在明显的振荡。

5.3.3 案例3

在此情况下，考虑了不同控制器的恒定抗干扰抑制性能。四旋翼在𝑡=0∼3s而有效载荷被挂在𝑡的=3s的钩子上。试验结果如图10所示，可以从图中的屏幕截图中看,从图10(a)和图10(b)可以看出基于MUDE的控制受干扰影响最小，并且恢复跟踪性能比其他两个控制器更快。此外，图10(c)显示了由MUDE生成的三个估计项。当四转子受几乎恒定的外部扭矩0.6N*m时，$\hat{𝑓}_1$迅速收敛到0.6。而$\hat{\dot{𝑓}}_1$和$\hat{𝑓}_2$则收敛到0。这些结果是合理的，因为 $\hat{\dot{𝑓}}_1$和$\hat{𝑓}_2$在稳态条件下接近零。

5.3.4 案例4

在案例4中，研究了三个控制器在时变干扰下的跟踪性能。实验以四旋翼跟踪正弦轨迹开始，然后在𝑡=10s左右将有效载荷悬挂到钩子上。如图11(a)-(b)所示，在稳态(𝑡=0∼10s)中，提出的基于MUDE的控制提供了对正弦参考的几乎完美的跟踪。在抗干扰方面，基于MUDE的控制器能够提供快速的跟踪性能恢复，并在稳态跟踪精度方面在三个控制器中表现最好。

从案例1-4的实验结果可以看出，基于MUDE的控制不仅可以抵抗系统上的各种干扰，而且在运动过程中还能提供较高的跟踪精度。

5.3.5 案例5

本案例研究说明了调整MUDE的$𝑇^{𝑓_1}$以提供更好的外部干扰转矩抑制性能的有效性。实验设置与案例3相同。分别选择四个不同的参数输入$𝑇^{𝑓_1}=\left\{ {1,0.4,0.1,0.05} \right\}$，相应的结果如图12(a)所示。可分析得到，通过减少$𝑇^{𝑓_1}$以提高了抗干扰性能。更具体地说，当添加扰动时，一个较小的$𝑇^{𝑓_1}$提供了一个较小的偏离稳定状态，并且可以在较短的时间内估计和补偿扰动。然而，由于测量噪声$𝑇^{𝑓_1}$在实践中不能任意小。从（23）中，一个小的$𝑇^{𝑓_1}$可以显著放大角速率测量结果 中的高频噪声，这正好解释了图12(b)中当选择$𝑇^{𝑓_1}$小于0.1时所示 中的震荡。虽然震荡可以被四旋翼的阻尼和执行器的动力学大大过滤掉，但它仍然可能导致执行器器的磨损，甚至是系统的不稳定。因此，在软件程序中应该小心确定$𝑇^{𝑓_1}$的大小。

5.3.6 案例6

调整参数$𝑇^{𝑓_2}$的结果如图13(a)-(c)所示。四旋翼在𝑡=0∼10s徘徊，并𝑡=10s后跟踪正弦轨迹。$𝑇^{𝑓_2}$是为估计执行器的时间延迟和扰动项 $f_{2}=\frac{1}{\alpha}\left[u_{\tau}\left(t-\tau_{0}\right)-u_{\tau}(t)\right]+d_{\tau}$而设计的估计器（29）的参数。可以观察到，在悬停过程中，调整$𝑇^{𝑓_2}$对跟踪精度的影响不大。这是合理的，因为在稳态条件下，电机几乎以一个恒定的速度旋转（这是稳定转矩指令的直接结果）。因此，集中扰动$𝑓_2$很大程度上是由执行器上的输入扰动$𝒅_𝜏$决定的，在大多数情况下，这是可以忽略不计的。然而，当跟踪时变参考(𝑡=10∼20s)时，由于转矩命令$𝒖_𝜏$不再恒定，由于模型不确定项 ，$𝒇_2$的影响变得显著。因此，当$𝑇^{𝑓_2}$从2减少到0.2时，跟踪性能得到了显著的改善。 这一结果表明，执行器的时间延迟是不能忽略的，调整$𝑇^{𝑓_2}$参数是非常重要的，特别是对于快速运动。

5.3.7 案例7

在此情况下，评估了所提出的基于MUDE的控制在实际飞行中的抗干扰和跟踪性能。四旋翼设置为手动飞行模式，随机和快速的滚动/俯仰命令（即期望欧拉角）是通过遥控器由飞行员给出的。一个150克的悬浮有效载荷附着在一个起落架上(见图14(f))，结合四旋翼的耦合动力学，可视为添加到四旋翼中的时变外部扰动转矩。飞行测试结果如图14(a)-(e)所示。一般来说，基于MUDE的控制能够抑制干扰，并提供良好的姿态跟踪性能。具体地说，由于参考值是随机的，在这种情况下可能会突然变化，因此跟踪误差表现出与案例2相似的峰值，如图14(b)所示。尽管有这些峰值，基于MUDE的控制还是可以快速消除跟踪错误。如附图14(c)-(d)所示，演示了pitch角和roll角的扰动估计结果，从中可以看出，由悬挂的有效载荷产生的外部扰动力矩由MUDE动态估计。利用这些估计信号，控制器可以主动补偿干扰，不仅保证了巨大干扰下的飞行安全，而且实现了对参考进行快速、准确的跟踪。

6 结论

提出了一种基于MUDE的鲁棒控制器，用于四旋翼精确的姿态跟踪和外部干扰抑制。结果表明，在标准控制器和UDE设计中系统考虑执行器动力学，可以提高经典基于UDE算法的方案的跟踪性能，特别是在快速运动时的跟踪性能。执行器的不可估量的状态，即执行器产生的扭矩，是用识别的FOPTD模型估计的。通过调整MUDE的单个参数，可以显著降低姿态跟踪和扰动估计误差，并在实际的四旋翼平台上进行了实验验证。