基于改进四元数阻尼误差模型的SINS初始对准算法

2021-11-10赵仁杰李开龙胡柏青田佳玉

系统工程与电子技术 2021年11期

赵仁杰, 李开龙, 胡柏青, 田佳玉

(海军工程大学电气工程学院, 湖北武汉 430033)

0 引言

初始对准是捷联惯性导航系统(strapdown inertial navigation system, SINS)的一项关键性技术,主要为SINS提供准确的初始姿态等信息,其中,对准的精度和速度是影响SINS后续工作的两项重要指标[1]。

经过不断研究,SINS初始对准方法逐渐丰富和完善。目前,这些方法主要可以归纳为以下两类:第一类是两阶段对准,先通过粗对准获取粗略的姿态信息,再进行精对准。其中,传统粗对准方法为解析法[2-3],即利用惯性器件敏感重力加速度和地球自转角速度,通过矢量定姿求解姿态,一般仅适用于静止或微幅扰动条件下,并对器件精度有一定要求。为克服解析法的局限性,文献[4]提出了一种基于坐标系分解的惯性系对准方法,能够有效隔离角运动,实现晃动基座粗对准;在此基础上,Wu等[5]又将对准问题转化为姿态确定问题(Wahba问题[6]),提出了优化对准(optimization-based alignment, OBA)方法,提高了信息利用率,具有较强的鲁棒性,后续得到了广泛关注和研究[7-10];精对准方法主要有罗经法、参数辨识法和卡尔曼滤波法。第二类是大失准角非线性对准[11-12],研究主要集中在非线性误差模型和非线性滤波算法两方面,其中,非线性误差模型的研究重点在于姿态[13-15];非线性滤波算法主要有拓展卡尔曼滤波、无迹卡尔曼滤波、容积卡尔曼滤波、粒子滤波等,其中,无迹卡尔曼滤波(unscented Kalman filter, UKF)凭借较高精度以及适中的计算量,在解决非线性问题中被广泛应用。

非线性对准可以简化对准过程,缩短对准时间,但实际对准效果往往却达不到最优,对此许多学者对滤波算法进行了改进,有效提高了滤波性能[16-19];文献[20]提出了一种基于坐标系一致性的矢量几何误差构建思想,在此基础上,王茂松等[21-22]指出传统速度误差只考虑了向量的大小差异,而忽视了方向差异,重新对其进行了定义,提出一种状态变换卡尔曼滤波方法,有效提高了组合导航的航向估计精度和稳定性。如果真实导航系未知,实际SINS解算应在计算坐标系下进行,上述文献将速度误差矢量统一在真实导航系下的做法仍有待讨论。Chang等[23]将速度误差统一定义在计算坐标系下,推导了改进欧拉角非线性对准模型,并通过试验验证了模型的有效性。

基于矢量误差坐标系一致性思想,本文对传统四元数非线性误差模型中的姿态误差模型和速度误差模型进行改进,将误差矢量统一投影在计算导航系下,同时引入外界阻尼信息,提出一种改进四元数阻尼误差模型对准算法,并应用于系泊状态下的SINS初始对准,通过仿真和车载试验对比了该改进算法与传统四元数阻尼误差模型对准算法以及文献[23]中欧拉角阻尼误差模型对准算法的对准效果,验证了本文所提算法的有效性。

1 四元数阻尼SINS误差模型

关于SINS的大失准角非线性误差模型已有完整推导[13-14],考虑形式比较复杂,本节引入全球定位系统(global positioning system,GPS)速度和位置阻尼信息进行模型简化,推导了四元数阻尼非线性误差模型。

为方便建模,将由GPS阻尼信息获取的参量统一用下标g表示,忽略小量误差,认为带下标g的计算参量值等效为真实值。此外,定义坐标系如下:记地心惯性坐标系为i系,地球坐标系为e系,选择“右前上”载体坐标系为b系,“东北天”地理坐标系为导航坐标系,记为n系,计算导航坐标系为n′系。

1.1 SINS状态微分方程

给出由四元数表示的SINS姿态、速度和位置微分方程,具体如下:

(1)

(2)

(3)

(4)

(5)

式中:I3×3为3维单位矩阵;RP为当地曲率矩阵,表达式为

(6)

式中:RM和RN分别表示子午圈与卯酉圈主曲率半径。

1.2 传统阻尼误差方程

引入GPS阻尼速度和位置,实际阻尼状态微分方程可表示为

(7)

(8)

(9)

(10)

(11)

(12)

(13)

式中:q*代表四元数共轭。

对式(13)左右两边同时微分可得

(14)

将式(1)和式(7)代入式(14),参照文献[14]推导得到传统的阻尼姿态误差方程,具体如下:

(15)

传统的速度误差定义为

δvn′=vn′-vn

(16)

对式(16)两边同时微分,推导可得传统的阻尼速度误差方程,具体如下:

(17)

式中:δfb表示加速度计测量误差。

直接给出传统的阻尼位置误差方程如下:

(18)

1.3 改进阻尼误差方程

基于实际SINS解算在n′系下进行这一认识,通过观察第1.2节传统阻尼非线性误差方程的推导过程,不难发现有以下两处不够严格:

(2)式(16)速度误差定义并未考虑速度矢量的方向性,只是标量意义上的减法,存在坐标系不一致问题。

针对上述问题,本节将误差统一至n′系,重新推导了改进阻尼非线性误差方程,其中,改进阻尼姿态误差方程如下所示:

(19)

考虑矢量方向性,重新定义速度误差[23]为

(20)

对式(20)左右两边同时微分可得:

(21)

将式(2)和式(8)代入式(21),推导可得改进阻尼速度误差方程,具体如下:

(22)

改进阻尼位置误差方程如下所示:

(23)

2 系泊条件下的非线性对准算法

本文研究系泊状态下的SINS初始对准,在系泊状态下,载体位置相对固定,可以通过GPS准确获取,线速度近似为零,是有效的零速约束条件。SINS误差模型通常包括姿态、速度、位置和器件误差,由于对准的目的是在短时间内快速获取准确的姿态,考虑到器件误差估计不仅受器件精度的影响,还和载体运动状态有关,且静态下器件误差的可观性不强,因此,本节仅将姿态误差和速度误差列为状态,推导系泊条件下的非线性对准模型,同时结合UKF算法给出具体的对准步骤。

2.1 非线性对准模型

(24)

式中:

(25)

(26)

以速度误差dvn′=vn′作为量测,相应量测模型如下:

y=Hx+v=[03×3,I3×3]x+v

(27)

式中:v表示速度噪声。

式(24)和式(27)共同构成了非线性对准模型。

2.2 非线性对准算法

本文选取UKF算法用于非线性对准,为避免四元数在UKF中存在的归一化约束以及方差匹配问题,采用四元数无迹估计器(unscented quaternion estimator, USQUE)框架[24]。

下文给出对准算法流程。

步骤 1初始化

定义四元数误差δq,参照下式将其转换为误差修正罗德里格斯参数δσ,设置局部状态为x=[δσT,xeT]T,其中,xe表示非姿态误差状态。

σ=ρ/(1+q0)

(28)

(29)

式中:n为状态向量维数;α为调节因子,一般取1e-4≤α≤1;κ、β均为比例因子,通常取κ=0,β≥0;

步骤 2导航解算

(30)

(31)

步骤3滤波

(1)时间更新

产生状态sigma点:

(32)

(33)

(34)

(35)

(36)

(37)

(38)

(2)量测更新

由于量测模型为线性,量测更新如下:

(39)

(40)

Pk=(In×n-KkHk)Pk/k-1

(41)

(3)姿态更新

(42)

3 试验验证

3.1 仿真试验

为验证本文所提改进四元数阻尼误差模型对准算法在系泊条件下对准的有效性,将改进算法与传统四元数阻尼误差模型对准算法以及文献[23]中改进和传统欧拉角阻尼误差模型对准算法进行对比,为简便表述,将上述4种算法分别记为Quaternion-I、Quaternion-T、Euler-I和Euler-T。

观察图1可以看出,相比其他3种算法,本文提出的Quaternion-I算法在对准速度和精度上均具有优势;此外,相同姿态表示下,改进模型对准算法的姿态估计效果整体上优于传统模型;在改进算法中,基于四元数模型的Quaternion-I算法对准效果要优于基于欧拉角模型的Euler-I算法。

图1 200次蒙特卡罗仿真姿态角估计误差

为更加直观的对比4种算法对准效果,将200次蒙特卡罗仿真试验姿态估计误差取绝对值后求平均,得到姿态平均估计误差,如图2所示。

图2 200次蒙特卡罗仿真姿态角平均估计误差

观察图2,在静基座大失准角仿真条件下,4种对准算法均能将姿态失准角收敛至小范围,由于水平姿态角的可观性强于航向角,其估计效果也明显优于航向角。在航向估计上,本文提出的Quaternion-I算法收敛速度最快,其次为Euler-I算法,而Quaternion-T算法和Euler-T算法则相对较慢,虽然也可以将航向失准角拉回至小角度,但对准结束时刻仍未稳定收敛。为便于比较,统计对准结束时刻的姿态角平均误差作为算法的对准精度,如表1所示。

表1 4种算法姿态对准精度

由表1可知,Quaternion-I算法的俯仰角估计精度为0.399′,横滚角精度为0.375′,航向角精度为0.097°,整体估计精度最优。

大失准角对准条件下,非线性滤波采用反馈校正方式,将滤波器输出结果反馈至导航结算过程,每一步都会对状态误差进行修正,进而使得误差能够快速有效收敛;同一姿态表示下,改进模型修正了由坐标系投影不一致引起的模型误差,相比传统模型具有更好的对准效果;此外,对于改进模型对准算法,由于大失准角下欧拉角表示的姿态误差矩阵会因转动次序不同而不同,模型并不严格,相比之下,四元数是作为整体被使用,不存在转动次序问题且无奇异,故四元数模型对准算法的姿态对准效果要优于欧拉角模型。仿真结果表明,本文提出的Quaternion-I算法不仅在对准速度和精度上具有优势,同时还具有较好的稳定性。

3.2 车载试验

利用惯性级激光捷联惯组静态数据进行算法验证。捷联惯组器件精度如下:陀螺随机漂移稳定性为0.01°/h,加速度计零偏稳定性为50 μg;数据采集位置为北纬34.246°。试验将捷联惯组安装在载车上,静止采集约半小时数据,实验过程存在开关车门、上下车走动等干扰活动。

本文选取600 s数据用于算法验证,在姿态基准上加入大失准角构造初始姿态,为充分验证算法有效性,设置5组不同的大失准角,具体如下:Ⅰ[-10° 10° 20°]、Ⅱ[-20° 20° 40°]、Ⅲ[-30° 30° 60°]、Ⅳ[-40° 40° 80°]、Ⅳ[-50° 50° 100°],针对不同失准角条件,执行4种算法,对比姿态对准结果,限于篇幅仅给出第Ⅲ组条件下的姿态误差对比,如图3所示。

统计5组失准角下4种算法的姿态估计误差,如表2所示。分析图3和表2可得,在不同条件下,4种算法的水平失准角均能够很快收敛,且精度较高,但在航向估计上,600 s对准时间内,随着初始失准角的增大,Euler-T算法和Quaternion-T算法的对准误差有增加趋势,而Euler-I算法和 Quaternion-I算法对准精度较好且相对稳定,反映出改进算法的收敛速度较传统算法要快,其中Quaternion-I算法对准效果最优;此外,失准角范围设置较广,Quaternion-I算法均能够快速完成对准,表明该算法具有较好的稳定性,车载试验与仿真试验结果基本一致。