GNSS/SINS定位稳健SE2(3)-EKF方法

李 昕,孟硕林,黄观文,张 勤,李晗旭

长安大学地质工程与测绘学院,陕西 西安 710054

GNSS/SINS组合导航中传统EKF框架中位置和速度等状态误差定义只考虑了大小差异,忽略了方向上的差异,当滤波初值尤其是姿态失准角误差较大时,引起的状态误差坐标系定义不一致的问题较为突出[1-3]。针对此问题,文献[4]提出了一种速度变换旨在消去状态变换矩阵中的比力项,对应的φ角误差模型保证了速度误差状态定义的坐标系一致性。文献[5]基于状态误差矢量的几何变换提出了几何扩展卡尔曼滤波(GEKF)算法,在模拟和实测数据中均取得了优于传统EKF的良好性能。由于EKF是对非线性状态和量测方程在状态初值上进行一阶近似线性化和离散化处理,一旦状态初值即线性化点误差过大,直接导致依赖状态估计值的状态雅可比(F矩阵)和观测雅可比矩阵(H矩阵)出现较大偏差,利用F矩阵和H矩阵继续误差传递和量测更新,又进一步放大误差,整个误差传递系统形成正反馈,最终导致滤波器发散。另外EKF在组合导航状态估计中还存在所谓的方差估计不一致问题,即对本来不可观的状态也进行了滤波估计,直接引起误差传递出现为较大的偏差,以及过于乐观的协方差估计,这个现象在地图同步定位与建图(SLAM)领域较常见,在载体震动等不利运动状态下也会导致IMU相关状态矩阵误差迅速累积。文献[6]指出导致GNSS/SINS组合导航系统方差估计不一致问题的主要原因在于系统矩阵中的比力项影响,并在文献[4]的研究基础上进一步提出了ST-EKF,解决了GNSS/SINS组合导航中因高频振动等引起的方差估计不一致问题[6-7]。

近些年,基于李群和流形理论又发展了一种滤波,即SEk(3)-EKF,其核心思想是将姿态、速度及位置(AVP)等状态及误差定义在群空间而非传统欧氏空间,根据李群的乘法封闭特性以及李群和李代数之间的仿射特性,重新设计顾及姿态误差的更紧凑的位置、坐标状态误差动力学方程,旨在满足误差传递F矩阵或GNSS等量测H矩阵保持不变或缓变,实现F或H矩阵与状态估值独立,因此能够做到“不变”,也称之为不变EKF[8-10]。不变EKF对应的误差定义具备较好的自洽性,有效克服了传统EKF过于依赖初值的缺陷,具有较好的滤波收敛性和一致性,理论上更为严密,在机器人姿态估计、SLAM及视觉里程计(VO)等惯性基导航领域均得到了较好的应用[11-14]。文献[15]研究了不变EKF的理论及在SLAM和SINS初始对准等领域的应用,文献[16]研究了SEk(3)的特性及对应的滤波框架,推导了几种常见坐标框架下的SINS左不变和右不变EKF具体数学表达,文献[17]研究了一种顾及陀螺零偏的SE2(3)-EKF的姿态估计方法,文献[18]对速度误差进行变换实现了基于不变EKF的准静基座大失准角线性初始对准。在基于不变EKF的GNSS/SINS组合导航方面相对研究较少,尤其是针对实测数据和分析验证方面。文献[19]较早设计了一种针对低成本GPS/磁力计辅助的SINS误差动力学模型的右不变EKF,并进行仿真试验验证,文献[16]给出了松组合GNSS坐标量测更新的左不变EKF滤波形式。实际上,对于不同的组合导航应用,SEk(3)-EKF的状态误差量的构建形式并不统一,较为灵活,除了姿态、速度及位置状态还需考虑陀螺零偏等其他待估参数,选择具体的投影坐标系及左不变还是右不变状态定义均需要根据实际导航器件特性、组合模式、不同载体和观测环境等来确定。

另外,针对复杂观测环境或者载体机动较多的导航场景,GNSS/SINS目前普遍采用的是抗差或自适应滤波,这方面已有大量的研究基础[20-22],如抗差M估计、Sage-husa滤波及抗差自适应滤波等。这类自适应滤波一般是在EKF的框架基础上,考虑历史信息贡献或者平衡载体动力学模型和观测模型,一般情况下可一定程度提高组合定位的可用性、精度和可靠性[23-24],但当惯导预测值误差较大或载体动力学模型误差和GNSS观测异常同时存在等情况下,常规EKF由于上述客观局限性导致其滤波性能面临着较大的挑战,基于新的SEk(3)-EKF框架下的抗差或自适应滤波为缓解这一问题提供了可能。考虑到GNSS/SINS组合定位中姿态和位置误差相互耦合,对于MEMS惯导,通常先利用GNSS或磁力计等辅助求解载体姿态初值,实际应用中还会发生IMU断电或者类似于旋翼无人机大的机动,造成较大的姿态角误差,该阶段传统方法比较耗时,且常需要载体有加速、转弯等机动,此时如果GNSS信号异常,又进一步影响到姿态的求解,常规抗差滤波及组合定位性能急剧下降,甚至直接发散导致定位失效。基于此,本文针对复杂环境下GNSS/SINS松组合模式中GNSS观测异常频繁发生的问题,以MEMS惯导机械编排理论和经典抗差方法为基础,研究一种基于GNSS位置量测的15状态的左不变稳健SE2(3)-EKF方法,并以城市环境下的车载GNSS/SINS组合导航数据为算例,验证了大失准角误差和GNSS异常情况下新的滤波方法性能。

1 方法介绍

SEk(3)-EKF相对传统EKF最大的区别就是坐标框架一致性问题,重新定义了待估状态的数学形式,针对GNSS/SINS松组合经典的15状态模型,即姿态(A)、速度(V)、位置(P)、陀螺零偏(bg)、加速度计零偏(ba)及形成李群(AVP)-欧氏空间(bg/ba)混合状态误差模型,进而在误差EKF的基础上推导状态误差微分方程和GNSS量测更新方程。标称状态采用MEMS惯导机械编排进行时间更新,与传统EKF一致[25-26]。本文重点就稳健SE2(3)-EKF中的状态误差定义、误差传递和GNSS量测更新展开介绍。

1.1 误差定义

选择ECEF系(e系)作为组合导航投影坐标系,定义投影到e系下的AVP状态为一个群(矩阵)χ,bg/ba状态记为矢量θ

(1)

(2)

表1 AVP群误差定义形式

为了方便计算机编程实现,本文选择误差定义到标称状态对应的“估计”系。关于选择左不变还是右不变定义,比较灵活,通常右不变可保证状态矩阵F不变或缓变,载体震动或机动较多有一定优势,针对GNSS观测,有研究表明左不变能够实现量测矩阵H不变或缓变[16]。限于篇幅,本文主要针对左不变误差形式展开讨论,根据式(1)和式(2),左不变η及零偏误差ζ的具体表达式为

(3)

式(3)中,η∈SE2(3),根据李群仿射变换理论,η转化为李代数ξ的具体形式为

(4)

(5)

(6)

式中,θ=|φ|,为旋转轴角。

记t时刻的基于左不变定义的李群(χt)-欧氏空间(θt)混合状态误差为et

(7)

记zt为观测误差,则基于混合状态误差模型的系统方程为

(8)

(9)

1.2 误差传递

李群空间的速度和位置误差δv(φ)、δp(φ)和传统欧氏空间对应的误差满足式(10)

(10)

(11)

(12)

(13)

(14)

式(3)零偏误差项(ζ)通常可建立为随机游走过程

(15)

(16)

式中,wbg、wba为过程噪声,同样可通过Allan方差分析确定。

式(7)混合状态误差变换后的向量形式记δx,系统噪声项w,则SE2(3)-EKF状态方程为

(17)

式中,F为状态矩阵;G为系统噪声分配矩阵,由式(12)—式(16)可得

(18)

(19)

IMU更新时间间隔为τ,当τ较小时,离散化后的状态矩阵为Φ,过程噪声矩阵为Q,状态误差δxt,t-1及方差-协方差Pt,t-1递推公式为

δxt,t-1=Φt,t-1δxt-1

(20)

(21)

Φt,t-1=I+τ·Ft-1

(22)

(23)

式中,δxt,t-1≡0,因为误差每一次闭环修正后都重置为0;q为是白噪声w对应的方差强度矩阵,量纲单位与功率谱密度的单位一致。

1.3 量测更新

不考虑GNSS/SINS二者之间的空间杆臂,速度观测方程为

(24)

速度观测误差方程可以变形为

(25)

构造新的误差量ev,使之包含SE2(3)-EKF中采用的误差ξ

(26)

如果考虑杆臂,记GNSS/SINS之间的杆臂为lb,满足以下观测方程

(27)

(28)

本文以位置观测为例,考虑零偏项,t时刻误差方程为

(29)

(30)

(31)

δet,t-1=et-Htδxt,t-1=Htδxt+Nt-Htδxt,t-1=

(32)

由于短时间内IMU具有较高的递推精度,可根据新息残差δe可检测出GNSS异常,考虑到松组合冗余观测较少,本文采用不归零的等价权函数,噪声调节系数为λ

(33)

(34)

由于ξ为群误差,ζ为矢量误差,前者采用“指数”更新,后者采用“加法”更新

(35)

(36)

(37)

式(37)和常规EKF误差更新形式一致,说明增益矩阵Kt也与常规EKF形式完全相同,根据最小均方根误差准则,结合式(34)可直接推导出增益矩阵Kt及后验方差-协方差矩阵Pt

(38)

(39)

根据式(36)和式(37)更新所有状态误差后,进行状态闭环修正,得到更新后的状态为

(40)

(41)

1.4 滤波对比

基于左不变的LIEKF以EKF算法为基础,两者的区别主要在于状态向量的误差定义不同,传统误差定义中,姿态误差定义在特殊正交群而其他状态误差均定义于欧氏空间中。LIEKF将姿态、速度及位置引入一个群中,构成特殊欧氏群SE2(3)。图1为基于群空间状态误差传递示意图,由于采用了指数映射,李代数形式的线性状态误差可以很好地表征李群上的非线性误差,误差传递后仍满足高斯分布特性,能够兼容常规EKF线性高斯分布的假设,解决了传统包含姿态误差的状态误差坐标框架定义不一致性问题。

图1 群空间状态误差传递Fig.1 The error propagation in group space

由于误差定义在李群中,滤波中的状态转移矩阵F、量测矩阵H的推导以及反馈校正方式显然与EKF有所区别,稳健REKF与RLIEKF又是在EKF与LIEKF的基础上增加了量测矩阵R的抗差因子调节。本文试验部分将对比分析EKF、REKF、LIEKF及RLIEKF 4种滤波性能,本文总结了几种滤波方法的区别,见表2,其中表中“常规”可参考文献[22]。

表2 4种滤波算法对比

2 试验与分析

2.1 姿态角收敛性能分析

由第1节理论部分可知,左不变EKF(LIEKF)顾及了姿态误差的影响,为了验证大失准角下的LIEKF性能,考虑到姿态角的确定主要难点是航向角,限于篇幅,本文以航向角为例,在车载试验初始阶段模拟了150°、120°、90°、60°及30°的航向失准角误差,GNSS差分定位结果作为量测信息,噪声设置为2 m。图3给出了两种失准角对应的LIEKF和EKF航向角误差序列。可以看出,初始航向失准角为150°,传统EKF姿态无法收敛,因为大的姿态误差导致IMU递推位置误差过大,二者相互耦合,无法通过GNSS量测更新来实现航向收敛,而LIEKF则表现出极快的收敛速度。

表3统计了5种航向失准角误差对应的两种滤波收敛(误差小于2°)时间,可以看出即使相对较小的失准角误差,EKF仍需要5 min左右收敛,而LIEKF收敛时间都在半分钟左右,基本与失准角误差大小无关,这是由于LIEKF在误差传递和量测更新阶段顾及了姿态误差,尤其是量测更新H矩阵保持不变的特性,独立于状态估计值,第1节中的线性模型即可精确反推出李群上的非线性误差,即对于初始状态估计远远偏离真实值的情形,利用LIEKF线性模型同样可以做到滤波很快收敛。本文从试验结果进一步验证了LIEKF在姿态角收敛方面的绝对优势。

表3 不同失准角两种滤波收敛时间

2.2 定位性能分析

组合导航在市区环境通常都会面临GNSS观测异常,初始阶段或者中途IMU断电等异常还会存在大的姿态失准误差。由于LIEKF在大失准角误差时很快能够收敛,意味着实际应用中GNSS/SINS定位可以迅速地进入组合定位模式,从而更好地应对GNSS信号短暂缺失和多径干扰等异常不利影响,增加了对环境的抗干扰性。

为了验证本文提出的稳健左不变EKF(RLIEKF)定位性能,在车载试验实测数据基础上进行了航向失准和GNSS异常进行了模拟。整个路段按照每隔约20 min分3个时段(T1、T2、T3),分别对应了3种典型车载导航场景,其中T1时段需要进行姿态初始对准,初始航向失准角误差模拟为90°,并考虑到建筑物等多径干扰,模拟了500 s GNSS随机粗差;T2时段姿态已经收敛,但还是会存在GNSS粗差,模拟了500 s GNSS随机粗差;T3时段开始时刻由于IMU异常或者大的机动,航向失准角误差模拟为90°,途经隧道遮挡以及刚出隧道时树木林立,模拟了10 s GNSS信号中断以及随后的1 min的GNSS观测异常。

2.2.1 场景一

图4给出了EKF、LIEKF以及具备抗差功能的REKF和RLIEKF 4种滤波算法在场景1中的定位误差对比情况,可以看出LIEKF和RLIEKF很快就达到了较高的定位精度,前期EKF定位误差较大,因为姿态还未收敛到一定精度,此时GNSS异常出现,REKF则表现出更差的性能,因为基于MEMS惯导推算的新息误差仍较大,利用式(33)—式(34)反而放大了GNSS异常影响,因此不具备抗差功能。RLIEKF在整个阶段则均较为稳定,具备“既快又强”的抗GNSS粗差能力。

图4 T1时段不同滤波方法的性能对比Fig.4 Comparison of four filtering methods in T1 period

2.2.2 场景二

场景二由于EKF和LIEKF姿态均已很好的收敛,意味着式(10)—式(11)中φ角影响已经较小,由姿态误差带来的状态误差坐标系定义不一致性也较小,若φ=0,此时LIEKF理论上便退化为常规EKF。图5给出了此时段对应的4种滤波方法的X方向定位误差序列。可以看出EKF与LIEKF误差曲线基本一致,针对出现的GNSS异常,REKF和RLIEKF也表现出一致的抗差能力,从试验上验证了常规场景下LIEKF和RLIEKF同样适用于常规场景,计算效率上,EKF、REKF基本相当,经统计,二者单历元平均耗时为0.027 1及0.027 0 s,都具备较好的实时性。

图5 T2时段4种滤波方法X方向定位误差Fig.5 X direction positioning error of four filtering methods in T2 period

2.2.3 场景三

场景三在T2开始阶段出现了GNSS信号中断以及多径等干扰,这在市区环境中频繁出现,是一种较为典型的复杂观测环境。图6给出了T3时段4种滤波方法的定位性能对比。可以看出EKF和REKF对应的X方向定位结果在2500历元附近出现了大的误差,通过轨迹图(相对基站的E/N坐标)可以进一步发现,由于GNSS信号中断了10 s,此时还存在姿态误差,REKF对应的平面轨迹迅速漂移,出隧道后又面临树木遮挡,存在较大多径干扰,REKF由于隧道内惯导位置递推精度较差,同样造成新息误差较大,无法较好地识别GNSS异常观测,定位性能进一步变差。由于后续观测条件较好,二者均姿态收敛,定位误差和轨迹也趋于相同。针对此场景,RLIEKF则基本不受GNSS信号短暂缺失和多径等干扰,保持了良好的定位精度和稳健性。

图6 T3时段不同滤波方法的性能对比Fig.6 Comparison of four filtering methods in T3 period

3 结 论

针对GNSS/SINS组合导航经常面临大的姿态失准和GNSS异常,传统EKF及抗差算法由于在误差传递和量测更新中未考虑由姿态误差引起的位置、速度误差定义坐标框架不一致性和线性化误差较大问题,导致其定位性能受到影响。本文提出了SE2(3)-EKF框架下的稳健GNSS/SINS组合定位方法,通过对状态及误差形式重新定义,并在此基础上研究了相应的误差传递和GNSS量测更新的稳健定位方法,通过车载试验和几种常见的场景模拟,对比分析了EKF、REKF、LIEKF及RLIEKF的性能,得出了以下结论。

(1) LIEKF采用了群-向量混合误差模型,基于李代数的线性状态误差可以很好地表征李群上的非线性误差,兼容了EKF滤波线性假设,由于顾及了姿态误差,具有更高精度的动力学模型和量测更新精度,H矩阵独立于状态预测值保持不变,因此对于大失准角有着极快的收敛速度,对于150°、120°、90°、60°及30° 5种航向失准角,都能够在半分钟左右收敛。

(2) 当存在大失准角误差和GNSS异常时,传统的REKF由于H矩阵存在较大误差,新息误差不准确导致其性能反而不如EKF,不具备抗粗差能力。而RLIEKF由于采用了新的新息误差定义,不受姿态误差的影响,可以很快地收敛并进入组合导航模式,具备应对GNSS信号短暂缺失和较好抗GNSS粗差能力;当姿态收敛到一定精度,LIEKF便趋于退化为常规EKF,性能也趋于相同;在计算效率方面,LIEKF上和EKF均是计算维度相同的线性矩阵,效率相当。因此RLIEKF是EKF较好的拓展,具备更强的抗GNSS异常能力,在城市等复杂环境下具备更好的稳健性和实用性。