变结构机器人的环绕控制：一种改进自抗扰方法

Surround Control of Variable Structure Robots: An Improved Active Disturbance Rejection Method

摘要: 本文针对变结构四旋翼机器人在灾害救援与侦察任务中的飞行控制问题，提出一种基于改进自抗扰控制(ADRC)的方法。通过设计新型非线性函数lnfal替代传统fal函数，增强扩张状态观测器(ESO)对系统内外扰动的估计精度与补偿能力，从而提高控制器的抗干扰性和响应性能。在Matlab/Simulink环境中进行仿真验证，结果表明所提方法能够有效提升轨迹跟踪精度，抑制超调与振荡，加快收敛速度，表现出优于传统ADRC的鲁棒性和动态性能，适用于复杂干扰环境下的高精度控制任务。

Abstract: This paper addresses the flight control problem of a morphing quadrotor robot in disaster rescue and reconnaissance tasks by proposing an improved Active Disturbance Rejection Control (ADRC) method. A novel nonlinear function, lnfal, is designed to replace the traditional fal function, enhancing the Extended State Observer’s (ESO) capability to estimate and compensate for both internal and external disturbances, thereby improving the controller’s anti-interference performance and response speed. Simulations conducted in the Matlab/Simulink environment demonstrate that the proposed method effectively improves trajectory tracking accuracy, suppresses overshoot and oscillation, and accelerates convergence speed. The results show superior robustness and dynamic performance compared to conventional ADRC, making it suitable for high-precision control tasks in complex disturbance environments.

1. 引言

面对自然灾害、恐怖袭击与武力冲突等突发事件的侦察与监控需求，变结构机器人因其卓越的环境适应性与任务灵活性，在灾害救援与战场侦察中展现出巨大潜力[1]。该类机器人的空中模式具有非线性、欠驱动、强耦合等特性[2]，易受外界干扰，因此高性能控制器的设计对其发展至关重要。

针对变结构四旋翼机器人飞行控制问题，现有方法如PID [3]、反步控制[4]、滑模控制[5]等，易受干扰、依赖模型、易抖振或复杂度高。鉴于其精确建模困难且抗扰需求突出，本文采用自抗扰控制(ADRC)，通过实时估计与补偿内外部扰动，实现高鲁棒控制。自抗扰控制(ADRC)最初由韩京清研究员提出[6]，ADRC具备内在解耦能力，支持多通道独立控制。本文进一步提出一种基于改进fal函数的ADRC方法，用于定点环绕轨迹跟踪控制，并通过稳定性分析与仿真验证了算法的有效性。

2. 变结构四旋翼机器人模型建立

变结构四旋翼陆空机器人在飞行模式下可以视为“H”型结构的四旋翼无人机，即四个旋翼呈矩形分布。为了描述变结构四旋翼机器人的姿态和位置，需要建立与地球中心固连的地球坐标系(oe xe ye ze)和与变结构四旋翼机器人固连的机体坐标系(ob xb yb zb) [7]，如图1所示。

假设变结构四旋翼机器人为结构对称、质量分布均匀的刚体，忽略机体及螺旋桨的形变和弹性振动等复杂动态[8]。将变结构四旋翼机器人的空中运动分为质心运动和姿态运动两个部分，并考虑各种干扰项的影响，根据牛顿–欧拉方程建立变结构四旋翼机器人的数学模型[9]如下：

$\{\begin{array}{l}\ddot{x}=\left[\left(\sin \theta \cos \phi \cos \psi +\sin \phi \sin \psi \right)U_1\right]/m+dx\\ \ddot{y}=\left[\left(\sin \theta \cos \phi \sin \psi -\sin \phi \cos \psi \right)U_1\right]/m+dy\\ \ddot{z}=\left(\cos \phi \cos \theta U_1-mg\right)+dz\\ \ddot{\phi }=U_2/Ix+\left(Iy-Iz\right)\dot{\theta }\dot{\psi }/Ix+d\phi \\ \ddot{\theta }=U_3/Iy+\left(Iz-Ix\right)\dot{\phi }\dot{\psi }/Iy+d\theta \\ \ddot{\psi }=U_4/Iz+\left(Ix-Iy\right)\dot{\phi }\dot{\theta }/Iz+d\psi \end{array}$ (1)

Figure 1. Coordinate system of the variable-structure quadrotor robot

图1. 变结构四旋翼机器人坐标系

3. 改进的自抗扰控制器

3.1. 自抗扰控制器的组成结构

1) 跟踪微分器(TD)，跟踪微分器是一种在控制系统、信号处理等领域广泛应用的工具，其核心作用是实时跟踪输入信号的变化，并同步求解该信号的微分。它能够在有效抑制噪声干扰的同时，快速响应输入信号的动态特性，避免了直接对信号进行数值微分时因噪声放大导致的精度下降问题。无论是在机械系统的状态观测、运动控制中的速度/加速度估计，还是在信号处理中提取瞬时变化率等场景，跟踪微分器都能通过合理设计的非线性特性，实现对信号及其微分的平滑、准确跟踪，为系统的分析与控制提供可靠的状态信息。

2) 扩张状态观测器(ESO)，扩张状态观测器作为自抗扰控制理论的核心与精髓，其首要功能在于对系统中的“总和扰动”进行实时估计与动态补偿。它不再严格区分系统内部的未建模动态、非线性耦合、参数时变与外部的未知干扰，而是将所有这些影响系统性能的不确定性因素，统一视作一个作用于被控对象的“总和扰动”，并将其定义为一个新的扩张状态。通过建立包含该扩张状态在内的动态系统模型，扩张状态观测器能够利用系统的输入输出数据，对这一广义扰动进行实时、精准地跟踪与估计。最终，控制器可以利用这一估计值前馈补偿总和扰动，从而将复杂的非线性不确定系统近似简化为一个易于控制的线性积分器串联型系统，极大地增强了控制器的鲁棒性。

3) 非线性状态误差反馈控制率，非线性状态误差反馈控制律是一种针对非线性系统设计的控制策略。其核心思想是通过一种“非线性组合”的方式来处理误差。具体而言，该控制律将系统的状态误差输入到一个非线性函数中，这个函数能够根据误差的大小和方向，动态地调整控制输出的强度和特性。例如，当误差较大时，它可能产生强烈的控制作用以快速消除误差；而当误差接近零时，它又能平滑地减小控制作用，以避免超调和振荡。这种动态的、非线性的映射关系，使得控制器在系统偏离平衡点较远时具有快速的收敛性，在接近目标时又能保持高度的平稳性和精度，从而有效克服线性反馈控制在应对非线性系统时的局限性，显著提升系统的动态响应性能和稳态精度。

3.2. 改进型非线性函数

fal函数的设计遵循“大误差小增益，小误差大增益”的核心原则[10]。为实现平滑控制，其函数本身被设计为光滑连续、全域可导、关于原点对称，且在原点处具有有限增益。尽管该函数基本实现了这些设计目标，但在实际应用中仍存在明显的性能缺陷。具体而言，该函数在原点附近存在显著的高频抖振现象，其连续性和平滑性尚未达到理想的控制要求；同时，由于增益调节机制不够完善，在特定工况下会出现增益过大的问题，这不仅影响了系统的观测精度，还可能导致控制性能下降。这些局限性严重制约了fal函数在控制系统中的应用效果。

$\ln \text{fal}\left(e,k\right)=\{\begin{array}{l}k\ln \left(e+1\right),e\ge 0\\ -k\ln \left(1-e\right),e<0\end{array}$ (2)

如图2所示，传统fal函数在原点附近呈现出陡峭的非线性变化特征，其设计初衷在于通过提高局部增益以增强系统对小扰动的感知灵敏度。然而，这种响应特性也使其对测量噪声极为敏感，易引入高频干扰。相比之下，改进后的fal函数被构造为分段非线性奇函数，该数学形式保证了其在e = 0处连续可导，其输出波形在原点附近表现为平滑的弧线过渡，有效消除了传统fal函数在线性区与非线性段交界处的增益跳变问题。该改进设计在维持系统对小误差足够灵敏度的同时，显著降低了对噪声的敏感程度，从而在估计精度与抗干扰能力之间实现了更优的平衡。

Figure 2. Comparison of origin waveforms between fal and lnfal

图2. fal与lnfal原点波形对比

进一步从增益特性角度分析，图3展示了两种函数的增益波形对比。传统fal函数在输入接近零时呈现出典型的“尖峰”形态，虽能有效响应微小误差，但也容易放大噪声成分，不利于系统稳定运行。改进后的fal函数则通过平滑的增益过渡曲线，在保持适当非线性调节能力的同时，有效抑制了增益突变所引起的高频振荡，增强了对噪声的鲁棒性，从而能够更稳定、可靠地估计系统所受的总扰动。

Figure 3. Comparison of gain waveforms between fal and lnfal

图3. fal与lnfal增益波形对比

总体而言，改进后的lnfal函数凭借其全域平滑连续的增益特性，以及随误差增大而合理衰减的增益规律，在提升控制系统动态稳定性与抗干扰性能方面均展现出显著优势，为高精度控制场景提供了更为可靠的算法基础。

3.3. 改进扩张状态观测器的收敛性判断

基于改进fal函数设计的新型扩张状态观测器形式如下：

$\{\begin{array}{l}{e}_1\left(k+1\right)=e_2-{\beta }_{1}e\\ e_2\left(k+1\right)=e_3-{\beta }_2\ln \text{fal}\left(e_1,k\right)\\ e_3\left(k+1\right)=-{\beta }_3\ln \text{fal}\left(e_1,k\right)\end{array}$ (3)

重新计算误差方程表达式：

$e\left(k+1\right)=-A\left(e\right)e$ (4)

$A\left(e\right)=\left(\begin{array}{ccc}{\beta }_1& -1& 0\\ \frac{{\beta }_2\ln \text{fal}\left(e_1,k\right)}{e_1}& 0& 1\\ \frac{{\beta }_3\ln \text{fal}\left(e_1,k\right)}{e_1}& 0& 0\end{array}\right)$ (5)

引理1 [11]若存在矩阵：

$D=\left(\begin{array}{ccc}{d}_{11}& d_{12}& d_{13}\\ -d_{12}& d_{22}& d_{23}\\ -d_{13}& -d_{23}& d_{33}\end{array}\right)$ (6)

若存在一个主对角线元素均为正的对角矩阵D，使得DA(e)为正定对称矩阵，则系统的零解是Lyapunov渐近稳定的。

令$F=\ln \text{fal}\left(e_1,k\right)/e_1>0$ ，由函数lnfal定义知其有界，令$B={\beta }_1{\beta }_{2}F-{\beta }_{3}F$ 。当B > 0时，可得主对角线元素均为正定矩阵D，使DA(e)为对称正定矩阵。

$D=\left(\begin{array}{ccc}1& \frac{{\beta }_{2}F}{B}+{\epsilon }_2& -{\epsilon }_1\\ -\frac{{\beta }_{2}F}{B}-{\epsilon }_2& {\epsilon }_1& \frac{1}{B}+{\epsilon }_3\\ {\epsilon }_1& -\frac{1}{B}-{\epsilon }_3& {\epsilon }_1\end{array}\right)$ (7)

其中，${\epsilon }_1$ 、${\epsilon }_2$ 、${\epsilon }_3$ 都是趋近于无穷小的正数。

取误差系统(3)的Lyapunov函数为

$\begin{array}{c}V\left(t\right)\approx {\displaystyle {\int }_0^t\left[DA\left(e\right)e,\dot{e}\right]\text{d}t}+C\\ ={\displaystyle {\int }_0^t-{\left({\beta }_1{e}_1-e_2\right)}^2-{\epsilon }_1{\left({\beta }_{2}F-e_3\right)}^2-{\epsilon }_1{\left({\beta }_{3}F\right)}^2\text{d}t}+C\end{array}$(8)

式中：C为常数；t > 0。

由于${\epsilon }_1$ 是趋于无穷小的正数，则

$V\left(t\right)\cong {\displaystyle {\int }_0^t-{\left({\beta }_1{e}_1-e_2\right)}^2\text{d}t}+C$ (9)

只需C为足够大的正数，即可使V(t) > 0。对式(9)求导可得$\dot{V}\left(t\right)\le 0$ ，当且仅当系统式(4)为其零解时有V(t) = 0。故式(4)平衡点处是Lyapunov意义下渐进稳定的。在平衡点处，$F=\ln \text{fal}\left(e_1,k\right)/e_1>0$ ，则系统稳定条件B > 0，可简化为${\beta }_1{\beta }_2-{\beta }_3>0$ ，系统满足此条件，系统是渐进稳定的。

4. 变结构四旋翼机器人仿真验证

变结构四旋翼机器人可实现围绕预设固定点的稳定圆周运动，且在飞行中机头始终朝向目标点并保持45度夹角。为验证所提控制方法的有效性，基于Matlab/Simulink构建了控制系统仿真模型，对比分析了传统ADRC与本文改进ADRC的性能。仿真设定机器人以(0, 0)为中心点，初始盘旋半径10 m，半径随高度线性增加，每绕一圈高度上升0.2 m，偏航角始终指向中心。位置环干扰为2sint，姿态环干扰为[sin2t 0 sin2t]^T。ADRC参数整定过程遵循“先内环后外环、先线性后非线性”的递进原则：首先，依据期望的闭环动态响应(如调节时间要求)初步设定控制器带宽；其次，基于分离原理，将观测器带宽设置为控制器带宽的3至10倍，以保证扩张状态观测器具有足够快的扰动估计速度；最后，在固定控制器带宽与观测器带宽的基础上，对非线性函数中的调节参数α与δ进行微调，以优化暂态过程的平滑性与抗突变能力。表1与表2所列参数即按照上述流程，经仿真迭代后确定的综合最优值。

Table 1. ESO parameter table

表1. ESO参数表

名称	α1	α2	β1_x β2_x β3_x	β1_y β2_y β3_y	β1_y β2_y β3_y	β1_ϕ β2_ϕ β3_ϕ	β1_θ β2_θ β3_θ	β1_ψ β2_ψ β3_ψ	δ	δ	b	b	k	k
fal	0.5	0.25	40 700 2000	35 410 1100	500 1000 3200	150 750 125,000	150 7500 125,000	150 7500 125,000	5	0.01	30	40	^*	^*

续表

lnfal

^*

^*

40 700 2000

35 410 1100

500 1000 3200

1800 60,000 2000

2000 20,000 1800

600 30,000 1800

^*

^*

30

40

5

0.01

注：^*表示不包含此参数。

Table 2. TD/NLSEF parameter table

表2. TD/NLSEF参数表

名称	r	h	α1	α2	k1	k2	δ
TD	800	0.001	^*	^*	^*	^*	^*
NLSEF	^*	^*	0.5	0.05	2	30	5

注：^*表示不包含此参数。

由静止状态起飞沿期望轨迹运动，仿真步长为0.001 s，仿真时间为300 s，仿真结果如图4所示。

(a) ADRC控制器三维跟踪曲线

(b) 改进ADRC控制器三维跟踪曲线

Figure 4. Three-dimensional trajectory tracking curve

图4. 三维轨迹跟踪曲线

如图4所示，在初始直线运动阶段，由于机器人初始位置与目标终点存在偏差，需通过位置控制实现位姿调整与轨迹跟踪。仿真及实验表明，与传统ADRC相比，改进ADRC在跟踪中动态响应更优，收敛更快，超调与振荡显著降低，运动更平稳，实际轨迹与期望轨迹误差更小、吻合度更高，有效提升了跟踪精度与系统平稳性。

图5显示，在X方向跟踪误差对比中，传统ADRC需36.52秒进入稳态，而本文改进ADRC仅需2.702秒，调节时间缩短约92%。在轨迹突变时，传统ADRC峰值误差为0.305 m，改进方案降至0.25 m，降幅达18%。图6表明，传统方法调节过程需42.929秒，改进后压缩至12.909秒，调节时间缩短约70%。图7显示，传统ADRC峰值误差为2.173 m，改进后降至2.14 m，降幅约3%，虽提升有限，但验证了算法在不同维度的一致性与稳定性。改进ADRC在变结构机器人位置控制中展现出优异的动态性能及响应速度。在阶跃或轨迹突变时，能更快速实现快速跟踪。抗干扰实验表明，该系统在x、y、z三通道的扰动幅值和超调量均显著降低，且恢复稳定时间更短，验证了其在增强系统鲁棒性与动态平稳性方面的有效性和实用性。

Figure 5. X-direction trajectory tracking error curve

图5. X方向轨迹跟踪误差曲线

Figure 6. Y-direction trajectory tracking error curve

图6. Y方向轨迹跟踪误差曲线

Figure 7. Z-direction trajectory tracking error curve

图7. Z方向轨迹跟踪误差曲线

图8对比表明，改进ADRC仅在启动时有短暂波动，随后误差快速收敛，精度高。而传统ADRC在稳态后，滚转角误差仍在[−0.13, 0.13]区间内无规律抖动，无法实现高精度稳定跟踪。图9显示，改进ADRC在轨迹突变时能迅速恢复稳定，其余时段误差近乎为零。传统ADRC在稳态后，俯仰角误差在[−1.586, 1.584]区间内周期性抖动，无法稳定高精度跟踪。图10表明，改进ADRC在轨迹突变时仅有短暂、可接受的误差波动，并能快速恢复稳定；其余时段误差接近零。传统ADRC在稳态后，存在持续高频抖动，虽误差幅值差异不大，但反映了其抑制高频干扰与未建模动态的能力不足。基于改进ADRC控制器的变结构机器人，在姿态角跟踪误差方面较传统ADRC有明显改善，抖振大幅减弱，仅在位置突变时出现短暂较大振幅，且能快速收敛。仿真结果表明，改进ADRC在位置与姿态控制中均具有更快的响应速度和更强的抗干扰能力。

Figure 8. Rolling angle trajectory tracking error curve

图8. 滚转角轨迹跟踪误差曲线

Figure 9. Pitch angle trajectory tracking error curve

图9. 俯仰角轨迹跟踪误差曲线

Figure 10. Yaw angle trajectory tracking error curve

图10. 偏航角轨迹跟踪误差曲线

5. 结论

本文针对变结构四旋翼机器人面临的风扰、负载变化与模型不确定性等干扰，基于自抗扰控制(ADRC)机理，依据“大误差小增益、小误差大增益”原则，设计了新型非线性函数lnfal。通过在Matlab/Simulink中构建位置–姿态双环控制模型进行对比仿真，结果表明，基于lnfal函数的改进ADRC能显著提升轨迹跟踪精度，有效抑制超调与振荡，加快收敛速度，展现出更优的动态性能和鲁棒性，适用于高精度、强干扰条件下的飞行控制任务。

NOTES

^*通讯作者。