基于滞后补偿ESO的多旋翼SO(3)全姿态控制

朱玉颖 张果正 任哲昆

(西南科技大学信息工程学院 四川绵阳 621000)

在无人机上安装的姿态传感器以矢量方向测量的形式测量其姿态的部分信息，通过姿态估计器处理矢量信息得到机体的方向。传感器测量和处理这些矢量时可能导致几十毫秒(甚至更大)的延迟，这种延迟会对观测器或滤波器的稳定性和鲁棒性产生负面影响，如果它们没有得到合适的补偿，则会降低整个系统的性能。传统的解决传感器延迟的方法是采取一种具有所需性能的估计器进行无延迟测量，虽然这类改进在工程应用中比较普遍，但是它们需要复杂的稳定性分析和比较精细的增益调谐，这就导致了估计器的瞬态响应较差。目前应用比较广泛的单步滞后无序量测滤波算法(OOSM-GMPHD)[1-2]虽然易于处理延迟的数据且比较灵活，但会占用较大的内存，对于大多数嵌入式观测器设计难以实现。本文针对传感器测量的延迟问题，提出了一个改进的扩展状态观测器，补偿矢量测量中的延迟影响。本设计基于SO(3)[3-5]上精确的连续时间非线性姿态运动学，而不借助于参数化、线性化或离散化，有效地利用姿态运动学和矢量测量模型的对称性设计一个简单的通用扩张状态观测器，只需要非常小的计算能力，就能灵活地在嵌入式系统中实现，而且在此基础上将姿态的R3×3旋转矩阵的表示方法改进为SO(3)上的姿态分离来实现多旋翼无人机全姿态控制[6-9]。

1 扩张状态观测器滞后补偿改进的实现

四旋翼飞行器在实际飞行过程中容易受到外界的干扰，为了提高系统的稳定性，人们通过在系统中设计观测器来减小扰动的影响，但却忽略了传感器采集信号时的延迟时间，导致了观测器输出不准确[10-11]。本文将改进后的状态观测器应用在多旋翼姿态控制的角速度环中，改善控制效果。

1.1 状态观测器估计滞后原理

首先对传感器采集信息的延迟问题建立了如图1所示的简单模型。该模型由预采样延迟和后采样延迟以及一个零阶保持器组成。yi(t)为传感器待采集的信号量，∂i为预采样延迟的时间，图中的ZOH把第tki时刻的采样信号值一直保持到第tki+1时刻的前一瞬时,把第tki+1时刻的采样值一直保持到第tki+2时刻，以此类推，从而把一个脉冲信号序列变成一个连续的阶梯信号。图中右侧的后采样延迟，表示传感器采集到的信号量转化为用户可以直接使用的数字量的延迟时间σi。通过以上的分析我们可以得到模型最后的输出为：

图1 传感器采样延迟模型Fig.1 Sampling delay model of the sensor

t∈[tki+σi,tki+1+σi]

(1)

由此可以看出，得到的输出结果相对于原始信号有一定的延迟，因此状态观测器估计出的值也会有一定的延迟[12-13]。本文提出一种改良的状态观测器来解决延迟问题。

1.2 扩张状态观测器滞后补偿改进

状态观测器滞后补偿模型如图2所示。本文设计了一种改良的状态观测器对传感器预采样和后采样的延迟时间进行补偿，并对系统中的状态变量进行观测，为方便说明，将改良后的状态观测器拆分成一个预测器和一个修正器。

图2 状态观测器滞后补偿模型Fig.2 Lag compensation model of state observer

(2)

其中：e为扩张状态观测器中间计算变量；Z1_delay为滞后的角速度估计状态量；Z1为角速度状态估计量；Z2为角加速度状态估计量；Z3为系统扰动；∂1,∂2,∂3为可调参数，b为可调参数补偿因子，l为可调参数步长。因为内环本身就是一个包含力矩的动力学方程，所以系统存在扰动影响，因此设计出完整的姿态内环控制流程如图3所示。

图3 完整的姿态内环控制框图Fig.3 Block diagram of complete attitude inner ring control

2 SO(3)上的全姿态控制

2.1 扩张状态观测器滞后补偿改进

(3)

2.2 表示出飞行器SO(3)上的实际姿态

(4)

2.3 表示出飞行器SO(3)上的姿态误差

图4 SO(3)姿态矩阵 R3×3Fig.4 Attitude matrix R3×3 of SO(3)

(5)

(6)

对上述两个姿态误差向量进行相加即可得到姿态控制器3个通道的全部误差公式：

(7)

通过以上姿态误差运算即可解耦分离出俯仰、横滚和航向上的姿态控制，然后分别对这3个姿态控制通道进行控制律设计即可。由于多旋翼飞行器姿态外环本身就是一个简单的一阶运动学模型，而且没有任何内部、外部扰动，所以姿态外环的控制律设计使用P比例控制器就可以使得系统稳定，姿态外环的控制率可以设计为：

(8)

至此，状态观测器滞后问题的解决方案以及飞行器在SO(3)上的姿态外环控制器设计方法已经全部列出。通过以上内容的研究，将改良的状态观测器融入SO(3)的全姿态控制中，既可解决由于传感器本身采样带来的状态观测器估计延迟问题，又可显著提高无人机的控制效果。

3 实验结果分析

本设计的实验平台是自制的350 mm轴距多旋翼无人机飞行器，其飞行器的实物图片如图5所示，飞行器结构参数如表1所示。

图5 飞行器实物图Fig.5 Aircraft object

表1 飞行器结构参数Table 1 Structural parameters of the aircraft

通过飞行器实际飞行测试验证，本文提出的状态观测器滞后补偿改进的方法有效解决了传感器测量带来的纯滞后影响。传统的观测器当传感器具有一定的采样延迟时，观测器的估计值具有一定的延迟，如图6所示，在100 ms后估计值明显滞后于测量值。而从滞后补偿观测器的角速度估计图(图7)可以看出，改进后的状态观测器估计出的角速度明显超前于传感器测量角速度，且在估计角速度数据稳定跟踪传感器测量角速度数据的前提下更加平滑。此外，在自研的飞行器实验平台上测量了一些必要的无人机结构参数辅助我们整定控制器参数。从悬停飞行的角度控制误差图(图8)可以看出，无人机在悬停飞行的50 s中，姿态角控制误差始终小于0.5度，由此证明本文提出的姿态控制器设计不仅从理论上可以实现，而且在飞行器实际飞行中也可以达到稳定的效果。

图6 普通观测器的角速度估计图Fig.6 Angular velocity estimation of ordinary observer

4 结论

本文提出了一种适用于在延迟矢量测量下的姿态估计的状态观测器滞后补偿改进方法。利用姿态运动学和系统输出映射的对称性，改进的滞后补偿观测器能够重建矢量测量的连续时间无延迟估计。在SO(3)的全姿态控制中加入改良的状态观测器，通过理论分析与实验证明该观测器在采样延迟较大的情况也能达到很好的滞后补偿效果。通过该观测器所得到的估计值相位超前于实际值，而且数据更加平滑，系统的控制效果明显提升，在工程实际中具有较大的应用价值。