基于机动加速度补偿的高动态无人机MEMS姿态实时解算

2021-11-08郭忠诚管练武吉彩妮

无人机 2021年4期

郭忠诚，管练武，吉彩妮

中航贵州飞机有限责任公司

针对高动态无人机机动加速和发动机高频振动导致MEMS姿态测量精度降低问题，本文研究MEMS加速度计和陀螺仪组成的低成本小型姿态测量系统，提出一种自适应Mahony互补滤波算法，以补偿机动加速度及动态整定互补滤波器PI参数，对传感器数据进行融合、补偿和修正。该方法利用加速度计三轴输出模值，以及对水平计算加速度与当地重力加速度的比较，判断机动加速度并进行补偿，然后通过自适应Mahony互补滤波算法完成姿态实时解算，最后经无人机试验验证得出，所采用的算法收敛性和平滑性较好，降低了MEMS的误差影响，可对无人机姿态进行有效跟踪和测量，为低成本小型姿态测量系统的开发和应用提供实用的方法。

姿态角是用来描述无人机空间指向的重要信息，在导航应用中对姿态信息进行分析显得至关重要。姿态测量系统（Attitude Measurement System，AMS）利用陀螺仪、加速度计等惯性传感器和微处理器等测量飞行器的姿态角。一般而言，在高动态及复杂环境下，姿态测量系统采用机械陀螺仪、光纤陀螺仪或激光陀螺仪获取姿态信息，这种姿态测量系统精度和稳定性高，但成本高、体积大、功耗高且维护复杂，不能满足现代无人机姿态测量系统在低成本、小型化、低功耗、免维护等方面的要求。近年来，随着微机电系统（Micro-Electro-Mechanical System，MEMS）技术的发展，MEMS惯性测量单元（Inertial Measurement Unit，IMU）在控制领域的应用日趋广泛，尤其是对无人机系统的姿态测量。因此，近年来基于MEMS惯性测量单元的姿态测量系统常用于实时测量和分析高动态无人机的姿态。

MEMS惯性测量单元利用加速度计和陀螺仪测量数据，加速度计主要测量机体坐标系下的线性加速度，陀螺仪主要测量机体坐标系下的角加速度。陀螺仪经过积分得到姿态角，短时间内精度高，但陀螺仪自身存在漂移，随着时间的积累误差逐渐增加，因此长时间的精度较差。与之相反，机体高频振动等因素引发的噪声，对加速度计的影响较大，短期精度较低，但其测量误差不会随时间的积累而增加。利用两者的互补关系，采用较优的算法对姿态数据进行融合，能提高姿态的测量精度和动态响应。姿态解算的主流数据融合算法包括卡尔曼滤波和互补滤波，与卡尔曼滤波相比，互补滤波的计算量较小、复杂度较低，多应用于低成本、实时的姿态测量系统。但是，传统互补滤波中，PI参数在滑行、起飞、加速、减速、转弯和降落等各种动作中均为固定值，不能根据无人机运动幅度和频率变化调整PI参数。因此，在无人机整个飞行过程中，传统互补滤波算法的动态适应性和测量精度均较差，更糟糕的是，很难将无人机速度变化引起的机动加速度与重力分量分开，尤其是在滑行和拐弯飞行过程中，无人机大幅度机动，未补偿的机动加速度会引起较大姿态测量误差。

针对无人机大幅度机动以及发动机高频振动导致无人机实时姿态测量精度降低问题，本文利用MEMS惯性测量单元构建无人机的姿态测量系统，提出一种对机动加速度进行补偿的自适应Mahony互补滤波算法，以补偿机动加速度以及动态整定互补滤波器PI参数，对传感器数据进行融合、补偿和修正，通过开展无人机地面高速滑行试验和空中飞行试验，验证所提算法的可行性和有效性。

坐标系定义

机体坐标系（b系）——ObXbYbZb

机体坐标系的三轴分别与无人机固定连接，其原点O定位在无人机的重心，ObXb轴沿无人机纵轴向前，ObYb轴沿无人机横轴向右，ObZb轴沿无人机竖直轴向上。

导航坐标系（n系）——OnXnYnZn

导航坐标系取当地地理坐标系，其原点O定位在无人机的重心，OnXn轴指向地理东向，OnYn轴指向地理北向，OnZn轴垂直于当地旋转椭球面指向天向。

b系相对于n系的方位关系用航向角Ψ、横滚角γ和俯仰角θ描述。具体而言，航向角是无人机纵轴在当地水平面上的投影线与当地地理北向的夹角，俯仰角是无人机纵轴与水平面之间的夹角，横滚角是无人机横轴与水平面之间的夹角。

坐标系转换

由于MEMS惯性测量单元固定在无人机上，各传感器基于机体坐标系测量数据，因此在姿态解算时，需要将机体坐标系的数据转换到导航坐标系，在此给出依次绕Z轴、Y轴、X轴旋转所产生的转换矩阵，用Cbn表示如下：

MEMS惯性测量单元姿态解算

MEMS惯性测量单元的基本原理框图如图1所示，MEMS惯性测量单元在初始静态条件下由加速度计感知地球重力分量计算水平姿态角，即俯仰角和横滚角信息；在动态条件下主要采用陀螺仪进行水平姿态角变化量计算。此外，无人机在动态条件下首先对机动加速度进行补偿，然后利用基于重力的自适应Mahony互补滤波算法，对陀螺的漂移进行实时跟踪校正，保障系统动态测量精度，最终输出无人机姿态角、三轴加速度和三轴角速度等数字信息。

图1 MEMS惯性测量单元基本原理框图。

MEMS惯性测量单元采用的数据融合算法是基于重力的自适应Mahony互补滤波算法，它能综合加速度计和陀螺仪各自的频率响应优势，从频率角度对两个传感器数据进行融合，以减少测量和估计误差。与此同时，为降低无人机加速起飞、爬升、拐弯、降落等大机动状态对测量误差的影响，需对无人机机动引起的加速度进行补偿修正，保证MEMS惯性测量单元的测量精度。

机动加速度补偿

其中β1和β2分别为无人机在纵轴和横轴的机动加速度补偿系数，该系数大小由所采用的惯性传感器精度和无人机机动情况共同决定。

自适应Mahony互补滤波算法

考虑到低成本姿态测量系统的计算能力和精度要求，自适应Mahony互补滤波常作为低成本姿态测量系统中的姿态解算算法。本文对MEMS惯性测量单元采集的数据进行融合，用高通滤波器处理陀螺仪测量信号的同时，采用低通滤波器平滑加速度计测量信号，并在互补滤波器中加入自适应PI调节，以形成增强型互补滤波器。滤波器设计如（3）式所示。

（1）利用经过补偿后的三轴加速度计输出模值|ƒ|的大小，确定调节比例参数kp和ki的值；

（2）将ƒ归一化；

（3）将导航坐标系下的重力向量转换到机体坐标系得到向量d；

（4）计算误差向量e；

（5）通过滤波器上一时刻的积分项项In−1和这一时刻的误差向量e计算出这一时刻的积分项；

（6）计算信息δ；

（7）得到补偿后的陀螺仪值w′；

（8）利用上一时刻的四元数qn−1和补偿后的陀螺仪值w′更新四元数；

（9）将四元数qn归一化，并将四元数转成姿态角θ和γ。重复上述步骤，即可实时解算无人机姿态角。

无人机试验验证及分析

将MEMS惯性测量单元装载于无人机航电设备舱，通过地面高速滑行试验和空中飞行试验，验证低成本MEMS惯性测量单元的实际性能。测试设备为低成本MEMS惯性测量单元构成的姿态测量系统，参考测试设备为较高精度的捷联惯性导航系统（INS），通过对比两者的实时姿态测量，分析MEMS惯性测量单元的姿态测量精度。

无人机高速滑行试验验证

在地面共进行3次高速滑行试验，验证MEMS惯性测量单元的姿态测量精度，MEMS惯性测量单元及捷联惯性导航系统在无人机高速滑行时，其姿态测量值如图2所示，姿态测量误差分析如图3所示，MEMS惯性测量单元姿态误差统计结果如表1所示，俯仰角和横滚角在高速滑行状态下误差均方根值分别为0.181°和0.116°。试验结果表明，在无人机高速滑行及发动机振动情况下，本文提出的方法可行并有效。

表1 无人机高速滑行时MEMS惯性测量单元姿态误差统计结果。

图2 无人机发动机转速变化时姿态测量值。

图3 无人机滑行速度变化时姿态测量误差。

无人机飞行试验验证

无人机飞行试验涉及高速滑行、起飞、爬升、平飞、拐弯、下降、着陆等过程，并验证MEMS惯性测量单元的姿态测量精度。整个飞行过程超过30min，其飞行轨迹如图4蓝色曲线所示。

图4 无人机飞行轨迹。

无人机在飞行过程中，MEMS惯性测量单元的俯仰角和横滚角测量值如图5和图6上半部分所示，图5和图6下半部分分别表示俯仰角和横滚角测量误差，MEMS惯性测量单元姿态误差统计结果如表2所示，其中俯仰角和横滚角误差均方根值分别为0.879° 和0.867°，最大误差主要出现在无人机拐弯时。试验结果表明，无人机飞行状态下MEMS惯性测量单元姿态测量的跟随性较好，误差较平滑，所采用的算法具有较好效果。