基于因子图的冗余惯组斜置轴在线标定方法研究

胡任祎, 王秀春, 史丽楠, 贺彦峰, 陈 平

(北京航天自动控制研究所, 北京 100854)

1 引言

惯性测量单元(Inertial Measurement Unit ,IMU)具有全自主、适应性强的特点,在导航领域中得到普遍应用[1]。由于受到生产工艺水平、维护成本等因素限制,仅靠单一惯性元件提升导航系统的可靠性面临投入大、周期长、见效慢等问题。而冗余型惯性测量单元(Redundant Inertial Measurement Unit, RIMU)较之于普通IMU 元件,通过冗余安装多个惯性仪表,具有更高的可靠性和精度,可有效提升单一惯性元件的系统可靠性[2]。国内外现役运载火箭大多采用冗余技术提高惯性系统的可靠性,通过设置冗余惯性元件,采用冗余管理算法对测量信息进行重构,提升运载火箭惯性导航系统的可靠性[3]。

RIMU 系统采用3 个以上的非正交配置,与正交型IMU 不同的是,非正交配置的安装误差无法使用正交惯组地面标定方法[4-5](12 位置标定法)进行标定,且地面条件下无法完全模拟真实环境。同时,惯组的器件误差主要包含标度因数误差、安装误差、零偏与白噪声[6],而标度因数误差与安装误差存在耦合。

传统针对线性系统的Kalman 滤波[7]无法完成此类耦合情况下的标定工作,EKF 滤波[8]多运用于状态量相互耦合的场景,但耦合的状态量相互影响而导致估计精度下降。自标定法得到的安装误差精度受自身惯组精确度影响,且在线条件下无法通过高精度的加速度与角速度信息进行标定,依赖三正交惯组进行标定时需要考虑非正交误差。因子图[9]作为一种参数优化方法,相比于滤波法模型可调节,收敛速度取决于优化算法以及计算机的算力[10]。Cheng 等[11]构建了线性系统误差模型,采用自标定形式,利用Kalman 滤波方法进行标定。自标定精度受到自身惯组精确度影响,导致安装误差角标定结果较差;Cheng等[12]也提出离线条件下的斜置轴标定,提升了标定精度,但离线条件下只能得到高精度加速度与角速度数据,而陀螺仪安装误差表现为姿态角误差,将角速度积分会扩大误差,影响标定结果;Lu等[13]与Cho 等[14]以三正交惯组为基础,对斜置轴角度进行估计,但未考虑三正交惯组是否存在非正交误差的情况;梁晴[15]将安装误差用高斯马尔可夫过程表示,并给出不同主轴下的斜置轴安装误差模型;Lu 等[16]利用星敏感器作为外部导航信息,通过12 位置试验方式,采用Kalman 滤波对IMU 的标度因数误差、安装误差、零偏以及星敏感器安装误差进行标定。

本文提出一种在线条件下冗余惯组斜置轴的标度因数误差与安装误差的因子图标定方法。新的安装误差模型不依赖三正交惯组,能够标定冗余惯组斜置轴的标度因数误差及安装误差。采用GPS 作为外部导航信息,构建冗余惯组斜置轴的标度因数误差与安装误差的耦合模型,使用因子图方法,选择窗口与阈值,有效解耦并测算出标度因数误差与安装误差角,并验证算法的有效性。

2 数学模型

2.1 安装误差模型

2.1.1 坐标系定义

1)载体坐标系(b系):坐标系原点o与载体重心重合,xb为载体俯仰轴,指向载体右侧为正;yb为载体滚转轴,指向载体前侧为正;zb为载体偏航轴,指向载体上侧为正。

2)传感器真实坐标系(s系):坐标原点o位于传感器中心,zs为传感器敏感轴,此轴表示传感器测量值的方向,xs表示传感器旋转轴,其与ozb以及ozts共面,ys与xs,zs两轴互相正交,其满足右手定则。

3)传感器理论坐标系(ts系):坐标原点o位于传感器中心,与s系区别在于,ts系不存在安装误差,是理论模型。

三坐标系的示意图如图1 所示。

图1 三坐标系示意图Fig.1 Diagram of three coordinate systems

2.1.2 冗余惯组安装误差模型

斜置型惯组如图2 所示,斜置型惯组在体坐标系中的位置可用α,β表示,其中α为传感器敏感轴到xboyb平面与xb轴的夹角,β为传感器敏感轴到zb轴的夹角。

图2 斜置型惯组示意图Fig.2 Diagram of redundant configuration

因此,以斜置陀螺仪为例,RIMU 到体坐标系的转换关系可用式(1)表示:

其中,ωxb,ωyb,ωzb表示体坐标系三轴测得的角速度,ωsi,i=1,2,…n表示第i个斜置陀螺仪测量的角速度。两者的转换关系可用安装矩阵H表示,如式(2)所示:

其中,ωs为冗余惯组角速度测量值,ωb为体坐标系三轴角速度。在保证基本导航功能的条件下,H为列满秩矩阵,因此其广义逆矩阵存在,如式(4)所示:

安装误差是由安装时的不完美造成惯性器件敏感轴与理论位置不一致所导致。由于安装误差的存在,传感器的真实坐标系与理论坐标系不重合,因此可通过坐标系旋转的方法对准两坐标系,旋转的角度可作为衡量安装误差的具体参数。

根据图1 所示,先以ys轴为旋转轴,转动θy角度,令平面ysozs与ytsozts重合,再以xs为旋转轴,转动θx角度,令两坐标系重合。因此可通过计算θx与θy数值,进而标定安装误差。由于θx与θy为小量,因此可近似得到式(5):

在传感器坐标系中,陀螺仪的测量值可表示为式(6):

结合式(6),建立安装误差模型如式(7)所示:

其中,ωzs为传感器理论坐标系z轴测量值,ωxs为传感器理论坐标系x轴测量值,ωys为传感器理论坐标系y轴测量值。

已知载体坐标系与传感器坐标系的转换关系如式(8)所示:

式中,ω-s为斜置惯组在传感器真实坐标系下的测量值,将其投影到载体坐标系下,如式(10)所示:

式中,C1,C2,C3分别为xs轴、ys轴和zs轴到b系三轴的投影矩阵。

2.2 数学模型

2.2.1 器件误差模型

考虑标度因数误差与安装误差,结合式(10)安装误差的建模形式,存在式(11):

式中,fb为加速度计测量的比力,vn为导航坐标系下的载体速度,本文导航坐标系采用东北天坐标系表示法。

分别对捷联惯导位置(纬度、经度、高度)微分方程式求偏差,可得位置误差方程,如式(16)所示:

由于标度因数误差与安装误差存在耦合关系,因此需对状态方程求Jacobian 矩阵。由式(14)～(16)设计状态方程,如式(17)所示:

F具体形式见式(18)～(28),其中×为向量的反对称矩阵。

2.2.3 因子图模型

因子图方法主要应用于数据融合、参数优化,并将其转化为状态估计问题,根据观测到的信息对当前状态进行推断。由于测量具有不确定性,虽然无法准确地反应所求变量的真实状态,但利用测量可推断出真实状态的概率。因此,状态估计可用条件概率密度表示p(X|Z) ,其中,X为待估计的状态,Z为传感器的测量值。当条件概率值达到最大时,X-即为X的估计值。对状态X的估计,可采用最大后验估计的方法,如式(29)所示:

对式(29)应用贝叶斯公式,可将后验概率密度转化为状态变量的先验概率密度p(X) 和该状态下测量值的概率密度p(Z|X) 的乘积,并通过观测值的概率密度归一化,得式(30):

当给定测量值Z时,p(Z) 为已知量,式(30)可进一步转化为式(31):

l(X;Z) ∝p(Z|X) 为关于状态X的似然函数。因此式(31)可展开为式(32):

式(31)将联合概率密度表示为一系列因式乘积的形式,乘积中的每一项都可以表示成一个因子,将这些因子表示成图的形式即为因子图。

因子图是一种基于马尔科夫链的图模型,在形式上可记为G= (F,X) , 变量节点记为xj∈X,表示待估计的状态。因子节点记为fi∈F,表示变量的后验概率密度。

因子节点只与其测量模型中出现的状态变量节点连接。当一个新的传感器测量加入因子图中,只需建立该传感器测量值的因子模型并与相关的变量节点建立连接。

如图3 所示,变量节点为带估计的器件误差,由高频率的IMU 因子相连接,每个变量节点均有外部数据因子进行约束。

图3 器件误差标定的因子图Fig.3 Factor graph system of calibration

因子图G的数学模型f(X) 是构成因子图的因子的乘积,如式(33)所示:

实际中与所求状态相关的代价函数通常为非线性,因此常通过非线性优化方法求解。

2.2.4 误差因子

根据外部导航信息提供的速度、位置数据与惯组自身解算所得数据构建残差,以建立外部数据因子模型,如式(37)所示:

其中zi为卫星导航数据,hGPS(Xi,ui) 为带有器件误差的惯导在当前时刻的解算数据。

根据建立的状态方程,状态量自身对下一时刻存在一步估计,将其作为惯性因子节点,如式(38)所示:

因此在线标定问题可转化为优化问题,如式(40)所示:

因子图的在线估计流程如图4 所示,首先构造因子图模型,变量节点设置为包含有姿态、速度、位置误差及器件误差的状态量,根据外部数据因子与惯性因子构建代价函数,利用L-M 算法对待估计变量进行优化补偿。

图4 因子图在线估计流程Fig.4 Procedure of calibration w ith factor graph method

与滤波法不用,因子图法无法做到每帧更新,原因为单帧的残差信息不足以优化带有3 个器件误差参数的非线性方程,因此需要构建滑动窗口。窗口长度包含的数据量能够实现对器件误差参数的单次优化,由于待标定的参数共有6 个,因此窗口长度所包含的卫星导航数据不得少于6 组。同时设置反馈回路,当前窗口完成一次优化后,将单次器件误差优化结果补偿至原始惯导解算数据中,补偿标度因数误差与安装误差,下一次优化将针对被补偿后的惯导解算数据,做到逐次优化,直至结果收敛。算法具体流程图如图5 所示。

图5 因子图方法反馈流程示意图Fig.5 Procedure of feedback w ith factor graph method

3 仿真验证

本文采用MATLAB 2019b 软件仿真,利用因子图方法,针对冗余惯组斜置轴的器件误差进行标定。

3.1 仿真条件

设定载体的运动方式如图6 所示,ωx,ωy,ωz为三轴角速度,ax,ay,az为三轴加速度。

图6 角速度与加速度示意图Fig.6 Diagram of angular velocity and acceleration

考虑标度因数误差、安装误差与白噪声,设置惯组斜置角分别为α= 45°,β= 30° ;标度因数误差S=5×10-4;安装误差设置为30″;陀螺白噪声大小设置为0.001°/h,加速度计白噪声大小设置为10× 10-6g/h,其中g为重力加速度,h为随机游走系数(由白噪声产生的随机时间累积的陀螺输出误差系数),单位为h;IMU 频率为100 Hz,GPS 频率为10 Hz。

3.2 仿真分析

为测试验证因子图方法在不同标度因数与安装误差条件下的标定性能,设置多组不同的误差系数,表1 为待标定的误差表。

表1 在线标定情况下误差系数表Table 1 Error index under on-line calibration condition

测量出GPS 与IMU 的速度、位置差值,分别采用EKF 和因子图算法进行仿真分析并比对。

表2 为2 种算法的标定精度对比。仿真结果表明,因子图算法能够在给定的仿真条件下标定出标度因数误差与安装误差,误差控制在10%以内,具有良好的标定效果。

表2 标定结果对比表Table 2 Comparison of calibration results %

根据建立的模型与条件进行仿真,陀螺仪与加速度计的标定误差如表3 所示,图7 为仿真结果示意图。仿真结果表明,针对不同的误差系数,因子图方法均能良好标定,误差在12%以内。说明其具有稳定的标定性能。

表3 陀螺仪在线标定结果与误差统计表Table 3 Calibration results and errors of gyro under on-line condition %

图7 标定结果示意图Fig.7 Diagram of calibration results

4 结论

本文建立不依赖正交系统的斜置型惯组安装误差模型,通过设定滑动窗口与反馈的方式,利用外部导航信息提供的速度、位置数据,建立基于因子图的标定方法。

1) 由于惯组标定存在天地不一致问题,即惯组在地面的标定结果与飞行过程中的标定结果存在偏差,且地面静态实验无法准确激励出器件误差参数,提出在线标定的方法,在飞行过程中利用GPS 作为外部导航信息进行标定以解决天地不一致的问题。

2) 针对EKF 滤波本身无法完全解耦的问题,本文提出了基于因子图的标定方法,设置滑动窗口与反馈,选择合适的窗口与反馈,对原始数据进行补偿,排除了零偏对安装误差标定的影响,并降低了由于标度因数误差与安装误差的耦合关系而造成的影响,对标度因数误差与安装误差进行逐次逼近估计,提高两者的标定精度。仿真结果表明,在不同误差系数下的标定误差均控制在12%以内。