基于SE(3)-EKF 的旋翼无人机GNSS/SINS 空中快速对准方法

李 昕，孟硕林，黄观文，张 勤，梁峻唯

（长安大学 地质工程与测绘学院，西安 710054）

近年来，旋翼无人机（Unmanned Aerial Vehicle,UAV）技术得到了迅速发展。与固定翼飞行器相比，旋翼无人机具备更高的飞行灵活性和更强的适应性，因此在各个领域都得到了广泛应用。然而，要实现旋翼无人机的精准姿态控制和位置控制，关键在于导航模块必须提供稳定可靠的导航数据。通常情况下，这些导航数据是通过基于卡尔曼滤波的GNSS/SINS（Global Navigation Satellite System/Strapdown Inertial Navigation System）组合导航算法计算获得[1]。目前，采用的惯性基滤波算法通常是基于小失准角[2-4]，要求在初始对准阶段获得高精度的姿态信息，以确保后续滤波算法的精度和稳定性，否则可能会引入较大的模型误差，导致滤波的收敛速度减慢，甚至无法收敛。另一方面，由于旋翼无人机通常会受到强烈的高频振动影响，导致惯性测量单元（Inertial Measurement Unit,IMU）的测量噪声水平显著升高[5]，这可能会加剧初始姿态不准确所导致的导航误差。因此，为了解决这一问题，需要考虑采用更精细的姿态控制和导航方法，以提高UAV 在各种工作环境下的导航性能。

UAV 可以使用磁力计获得较为准确的初始方位，然而搭载的电子罗盘易受到电力线等强电磁干扰，仅依靠磁力计难以保证准确的姿态。受限于载重能力和成本，UAV 的组合导航模块多采用低成本微机电系统（Micro Electro Mechanical System,MEMS）IMU，其陀螺仪零偏噪声一般较大，静基座无法精确地敏感到地球自转信息[6]，因此常采用动态对准方案。对于旋翼机，任意时刻的飞行方向具有不确定性，车载和固定翼飞行器常用的速度矢量法等简单可靠的动态粗对准方案不再适用。目前有三种常用解决方案应用于UAV 初始对准：1）基于比力方程的动态粗对准算法，通过GNSS 计算载体在导航系下的加速度，构造载体系与导航系的转换关系，进而求解姿态角。文献[6]采用载体短时直线加速运动实现粗对准，对载体的运动有一定的限制；文献[7]利用惯性导航系统的比力方程建立姿态的精确关系表达式，并采用数值求解算法实现空中对准。2）基于大失准角条件下的非线性误差模型与滤波的对准算法，结合非线性滤波器处理非线性误差模型。文献[8]提出了一种结合大失准角SINS 误差模型、回溯法和非线性卡尔曼滤波的对准算法，实现了在大失准角条件下高精度的初始对准；3）基于优化的初始对准方案，将惯导对准转化为寻找最小特征向量的优化问题。文献[9]根据推导的速度/位置积分公式，提出了一种基于优化的粗对准方法，利用GNSS位置/速度信息作为输入，在短时间内获得高精度的初始航向角，但对初始噪声方差信息较为敏感。上述这些传统对准方法在对准精度、收敛速度以及载体机动需求等方面还存在一定的提升空间。

GNSS/SINS 组合导航及对准中传统扩展卡尔曼滤波（Extended Kalman Filter,EKF）在定义位置和速度等状态误差时仅顾及大小差异，而忽视方向上的差异，这会导致滤波初值特别是姿态失准角误差较大时，状态误差坐标系定义不一致的问题[10,11]。近些年基于李群和流形理论发展了一种新的滤波方法，即SEk(3)-EKF。这种滤波将传统欧氏空间的姿态、速度、位置等状态及误差定义在了群空间，通过利用李群的乘法封闭特性以及与李代数之间的仿射特性，构造出相应的非线性误差量，进而推导出独立于状态的线性状态空间模型。由于状态空间模型独立于状态估值，能够保持“不变”，因此称之为不变EKF[12]。由于新滤波重新定义的速度、位置误差中包含了姿态项，能够更加准确地表征真正的误差[13]。文献[14]对SEk(3)-EKF 的特性和相应滤波框架进行了深入研究，并给出惯性基的左不变和右不变EKF 在常见坐标框架下的具体数学表达式；文献[15]提出了一种基于李群的等变滤波方法，通过实验证明在大失准角条件下，左不变等变滤波方法表现出更好的性能，优于传统的EKF 方法；文献[16]对速度误差进行变换，实现了基于不变EKF 的准静基座大失准角的初始对准，该方法在载体晃动下表现出较好的性能。

本文研究了旋翼无人机SINS 的动态初始对准问题，并基于MEMS 惯导机械编排理论推导出一种不依赖磁力计的基于GNSS 速度辅助的左不变SE(3)-EKF方法。考虑到GNSS 定位结果可能在固定解、精度较低的浮点解和单点解之间频繁切换，而GNSS 多普勒测速精度相对稳定[17]，且速度测量可以更快地修正方位，因此本方法将姿态和速度用于构建更简洁的矩阵李群，并在此基础上进行GNSS 速度量测更新。不同于SE2(3)-EKF，本文仅采用速度之差作为量测向量，称之为SE(3)-EKF，对比分析了该方法相对于传统EKF 和非线性无迹卡尔曼滤波（Unscented Kalman Filter,UKF）的优势，通过旋翼无人机GNSS/SINS 组合导航飞行实验，验证了不同初始失准角误差情况下本文初始对准算法的性能。

1 基于SE(3)-EKF 的动态对准模型

SE(3)-EKF 和传统EKF 一般都使用误差作为待估状态，主要区别是SE(3)-EKF 不是简单地直接采用估计状态与真实状态的差值作为误差，而是更为细致地考虑了状态定义的坐标框架一致性问题，并通过在李群空间重新定义误差，提供了更为严密和紧凑的数学形式。本文采用GNSS 速度辅助SINS 对准，为了描述状态误差，设计了包含姿态、速度的李群状态误差模型，并将陀螺零偏误差和加速度计零偏误差仍定义在传统向量空间，在此基础上，推导了状态误差的微分方程以及GNSS 速度测量的更新方程。

1.1 李群框架下惯导误差

本文选择地心地固坐标系（Earth-Centered Earth-Fixed,ECEF）作为组合导航系统的投影坐标系，记为e系。使用ECEF 坐标系下的惯导机械编排算法进行惯导更新[18]，定义投影到e系下的姿态、速度状态为一个群（矩阵）χ。

根据李群的性质有：

其中，χ-1表示χ的逆，同样满足χ-1∈SE(3)。

若χ表示真实状态，表示标称状态，定义姿态和速度状态误差为η∈SE(3)，区别于传统状态矢量(X-)做差的数学形式。

李群空间中误差定义分为左不变η=和右不变η=两种类型。本文选择左不变误差进行处理，根据式(1)(2)，左不变误差η以及零偏误差ζ的具体表达式为：

通过式(3)定义的误差形式可发现，在李群框架下定义的速度误差中包含了姿态项，同时考虑了真值和估值在数值大小和方向两个方面的差异，因此误差定义上相对于传统向量空间做差的方法更为合理、紧凑，解决了速度误差状态定义的基准不一致问题。

1.2 李群框架下惯导左不变误差方程

其中，θ=为轴角。

若φ为较小角度，在流形上微分可得与之对应的切空间，李群的幺元处切空间则为对应李代数[13]，由李群与李代数的特性，误差η满足：

其中，姿态、速度误差表示为ξ=[φ]T，^符号表示将向量空间映射到李代数的线性同构。

依据MEMS 惯导误差模型，可推导出李群状态误差的微分方程，忽略高阶的误差项，李群空间下的姿态和速度误差方程具体形式为[14]：

零偏误差可建模为随机游走过程：

其中，wbg、wba为陀螺和加速度计零偏白噪声。

所有的状态误差项为δ x，系统噪声项为w，则SE(3)-EKF 状态方程为：

其中，F为状态转移矩阵；G为系统噪声的分配矩阵。

式(8)-(11)参数化的具体形式为：

IMU 采样间隔为Δt，离散化后的状态转移矩阵记为Φ，过程噪声矩阵满足Q=E[wwT]，滤波预测的状态误差δ xk,k-1及对应的协方差矩阵Pk,k-1递推公式为：

其中，q是白噪声w对应的方差强度矩阵，其量纲单位与功率谱密度单位一致。

1.3 SE(3)-EKF 速度量测方程

GNSS 可提供位置和速度信息作为卡尔曼滤波的量测信息，用于抑制IMU 推算结果的发散。实际情况下，惯导系统的初始对准经常面临复杂环境，GNSS输出结果可能会在固定解、浮动解和单点解之间频繁切换，导致GNSS 输出的位置信息精度不一致；GNSS多普勒测速并不依赖于基准站，相对较为稳定。另外，对于MEMS 惯导，可以忽略由位置误差引起的重力加速度误差以及由地球自转引起的导航坐标系旋转误差，位置误差的微分方程仅与速度误差相关，本文关注的姿态对准不会受益于位置误差的微分方程。因此，本文仅采用GNSS 测量得到的速度作为量测，并给出SE(3)-EKF 的速度量测方程的推导。

假设IMU与GNSS天线相位中心之间的杆臂量为lb，满足：

其中，vGNSS为GNSS 速度。(×)可表示为：

将式(19)代入式(18)，并利用反对称矩阵相似变换性质[18]：

考虑速度和姿态存在误差，并顾及IMU 陀螺零偏，由IMU 推算的GNSS 天线相位中心的速度为：

对式(22)进行扰动，得到：

其中，δ ve为e系中的速度误差。

忽略二阶小量对式(23)整理得到：

GNSS 实际测量的速度为：

其中，nv为GNSS 测速白噪声，满足=Rv。

式(24)和式(25)做差，得到滤波的量测方程：

因此，量测矩阵H参数化形式为：

SE(3)-EKF 与常规EKF 的增益矩阵形式一致，状态更新与协方差矩阵计算方法也一致。

其中，δ zk代表k时刻的观测误差量。

根据式(31)更新状态误差δ xk，进行误差闭环修正。若bk为陀螺和加速度计的零偏向量，则得到更新后的状态为：

2 实验与分析

本次无人机飞行实验于2022 年11 月在西安市雁塔区进行，使用了大疆经纬MATRICE 600 PRO 六旋翼无人机，该无人机搭载了POMS-G6615D6 组合导航设备，其中MEMS 陀螺仪和加速度计的零偏稳定性分别为10 °/h 及0.1 mg，随机游走参数分别为及。IMU 的采样率为100 Hz，GNSS 板卡采用的是诺瓦泰718D。通过使用IE 软件进行紧组合，前后向平滑处理实验数据得到姿态的准确值。本次实验设备和无人机飞行的三维轨迹如图1 所示。

图1 设备安装图和无人机飞行轨迹图Fig.1 Equipment installation and trajectory of UAV

由于旋翼无人机在飞行时通常不会出现太大的俯仰角和横滚角变化，但航向角的变化较大，因此，在无人机飞行的初始阶段模拟了三种不同的初始失准角场景，旨在比较传统的EKF 对准方法、UKF 对准方法以及本文提出的SE(3)-EKF 对准方法，并对这三种方法的性能进行评估。本文选择飞行过程约200 s 的数据进行计算和分析。第一种场景存在较小的横滚和俯仰误差，但航向误差较大，失准角α=[3 ° 4 ° 45 °]；第二种场景存在较小的横滚和俯仰误差，但航向误差更大，失准角α=[3 ° 4 ° 95 °]；第三种场景是航向、俯仰和横滚角均严重失准，失准角α=[20 °20 ° 175 °]。

2.1 实验一

在实验一中，三个方向上的失准角模拟为α=[3 ° 4 ° 45 °]。图2 显示了三种方法估计的姿态角误差，可以看出在该场景下，三种方法都能够将姿态角收敛到相对稳定的范围。从局部放大图可发现，三种方法的水平姿态角收敛时间差异不大，航向在100 s 左右均趋于收敛。表1 给出姿态角收敛后的均方根误差（Root Mean Squared Error,RMSE）。从表1 可以看出，三种方法对准后的姿态角精度相差不大，水平姿态约为0.1 °，航向达到0.5 °左右。总的来说，在实验一的失准角情况下，三种方法的对准精度和效率差距不大，SE(3)-EKF 表现最优。这是因为模拟的初始失准角相对较小，传统EKF 的非线性程度不算严重，传统速度误差定义在向量空间造成的基准不一致性相对较小。

表1 姿态角误差的RMSE（单位：°）Tab.1 RMSE of attitude angle error (Unit: °)

图2 实验1 横滚角、俯仰角和航向角误差Fig.2 The roll,pitch,and yaw error of experiment 1

2.2 实验二

在实验二中，航向角误差进一步增大，此时失准角为α=[3 ° 4 ° 95 °]。图3 展示了三种方法估计的姿态角误差。当初始航向角误差进一步增大，传统EKF线性模型的非线性程度也相应增加，导致的模型误差进一步增大，需要更多的GNSS 量测校正，因此EKF在姿态的收敛速度上比UKF方法和SE(3)-EKF方法要慢。UKF 方法对模型的非线性程度没有限制，并且误差模型没有小失准角假设限制，SE(3)-EKF 方法在李群框架下重新定义了更为严谨的速度误差模型，此二者对大失准角情况下收敛速度更快。在大失准角的实验场景中，虽然UKF 方法能在一定程度上弥补EKF的缺陷，但仍然比SE(3)-EKF 方法慢，并且收敛精度也更低。相比于实验一，UKF 对准精度也有所下降。因此，在初始失准角较大的情况下，将速度误差定义为估计值与真值的直差形式已经不再合理，此时采用大失准角误差模型结合UKF 滤波对准方案的效果会受到一定影响，而SE(3)-EKF 方法在速度群误差定义方面更加严谨，因此在这种情况下表现较为稳定。表2 统计的姿态角误差RMSE 表明：SE(3)-EKF 方法在短时间内（200 s）实现了水平姿态对准精度约为0.1 °，其精度相对于传统EKF 方法提高了50%以上，相对于UKF 方法提高了20%以上；航向失准角精度约为0.4 °，相对于EKF 方法提高了45%以上，相对于UKF方法提高了43%以上。

图3 实验2 横滚角、俯仰角和航向角误差Fig.3 The roll,pitch and yaw error of experiment 2

图4 实验3 横滚角、俯仰角和航向角误差Fig.4 The roll,pitch and yaw error of experiment 3

2.3 实验三

实验三模拟了极端失准的情况，失准角为α=[20 °20 ° 175 °]。在这种情况下，模型的非线性程度非常严重，导致传统的EKF 和UKF 方法对应的航向误差在200 s 内发散，而SE(3)-EKF 方法仍然能够迅速地实现姿态角收敛，表明了SE(3)-EKF 中基于李代数的线性状态误差能够很好地表征李群上的非线性误差，具有更高的滤波精度。表3 统计的姿态角误差RMSE 表明：在极端失准情况下，SE(3)-EKF 计算的姿态角误差仍然保持着与实验一和实验二相当的水平，水平姿态精度约0.1 °，航向约0.5 °，突显了SE(3)-EKF 对准方法的稳健性。

表3 姿态角误差的RMSE（单位：°）Tab.3 RMSE of attitude angle error (Unit: °)

进一步对比图2-4 可发现，SE(3)-EKF 的航向角误差都会迅速降至较小值，然后继续收敛到更高、更稳定的精度。在三个实验场景中，SE(3)-EKF 的航向角误差曲线如图5 所示，可看出在约10 s 内，航向角误差已从初始误差降至8 °以内，然后持续向更小的误差值收敛。尽管此时航向误差仍然较大，但已经能够满足一些对初始姿态要求不高的组合导航应用。此外，还可以发现，SE(3)-EKF 方法的航向角收敛性能基本上与初始姿态误差无关。

图5 SE(3)-EKF 航向角误差Fig.5 The yaw error of SE(3)-EKF

3 结论

本文提出了一种 GNSS 速度辅助的基于SE(3)-EKF 框架的旋翼无人机空中对准方法。该方法重新定义了速度误差形式，设计了GNSS 速度量测更新方法。在飞行实验中模拟了三种不同大小的初始姿态角误差，对比分析了EKF、UKF 和SE(3)-EKF 的性能。实验结果显示，在较小初始失准角的场景下，EKF和UKF 方法能够实现姿态收敛；而基于SE(3)-EKF的对准方法在不同失准角的场景中，水平姿态误差均可收敛到约0.1 °，方位误差可收敛到约0.5 °，并且比EKF 和UKF 具有更快的收敛速度。由于SE(3)-EKF在滤波计算过程中参与计算的矩阵维度和EKF 相同，所以两种方法的计算效率相当。因此，基于SE(3)-EKF的对准方法具备较好的工程应用价值。