车载惯性定位标定的理论推导及误差分析
2023-03-09余胜义
余胜义, 彭 惠
(北京航天发射技术研究所,北京 100076)
0 引言
相较多传感器信息融合滤波的定位方法,航位推算定位方法的计算相对简单,对导航计算机的要求较低,系统模型简单,不存在长时间滤波易导致系统发散的问题[1-2],广泛应用于中精度车载惯性定位系统中。航位推算定位的系统误差主要为方位安装偏差角误差(以下简称“安偏角”)及里程计刻度系数误差[3-5](以下简称“里程系数”)。对惯性定位系统误差的标定是惯性定位系统的前提,常见的标定惯性定位系统误差的方法有以下两种:
一种是将安偏角和里程系数作为状态量,通过卡尔曼滤波器估计可获得安偏角和里程系数[6-7],这种方法对导航计算机的要求较高,且受标定路线所限,若安偏角和里程系数的可观测性差[8],则难以完成标定[9-10];
另一种是将载车运动投影到平面中,采用起始点和终止点的标准位置以及惯性定位系统的输出进行标定计算,标定计算结果与行驶路径无关[11],标定计算量小,易于实现。为了解决常用的起点纬度标定计算法在安装偏差角90°附近时误差较大已不适用的问题,本文对这种标定方法的公式进行了推导,并对两种计算方法的误差进行了分析。
1 定位系统误差可标性证明
如图1所示,设标准起点X1、Y1,标准终点X2、Y2,惯性定位解算终点X3、Y3,假设行驶全程中惯性导航系统给出方位的安偏角为ε(航向漂移很小),里程系数为k。kΔSi是第i个解算周期中测量的行驶里程;φi+ε是第i个解算周期中测量的方位角。
图1 航位推算原理示意图
1.1 航位推算原理
航位推算是把每个解算周期内测量的行驶里程分解到北、东向后累加。如图1所示的I放大解算周期的分解三角形中,可得
ΔXi=kΔSisin(φi+ε)
=kcosεΔSisinφi+ksinεΔSicosφi
(1)
ΔYi=kΔSicos(φi+ε)
=kcosεΔSicosφi-ksinεΔSisinφi
(2)
相应地进行经度γi、纬度λi更新,累加实现航位推算
γi=γi-1+(ΔXi)/(Ni-1cosλi-1)
(3)
λi=λi-1+(ΔYi)/RMi-1
(4)
从标准起点出发,惯性解算终点累加公式如下
(5)
(6)
若不存在系统误差时,即k=1,ε=0,得到标准终点-起点之间累加公式如下
(7)
(8)
1.2 可标性证明
把式(7)、式(8)代入式(5)、式(6)得
X3-X1=kcosε(X2-X1)+ksinε(Y2-Y1)
(9)
Y3-Y1=kcosε(Y2-Y1)-ksinε(X2-X1)
(10)
式中,tanφ=(X2-X1)/(Y2-Y1),φ正好是图1中航迹AC与北向夹角。
将式(9)、式(10)两边平方后求和,可得
(11)
(12)
从式(9)、式(10)可以看出,惯性定位的终点与行驶路径无关,只与起终点坐标及系统存在的里程系数k及安偏角ε相关。使用式(11)、式(12)即可通过起终点坐标计算出系统误差k、ε。从图1可以得到k、ε的几何意义,k为直线段长度之比AD/AC,ε为夹角∠CAD。
综上所述,标定求解是将载车运动投影到平面中,起终点直线连线进行计算。而使用墨卡托投影[12-14]的平面直角坐标系中,等角航线为直线,可以借鉴等角航线的反解计算[15-17]进行标定求解,比较两等角航线的航程和航向角得到系统误差,k为航程之比,ε为航向角之差。
2 惯性定位系统误差标定计算
标定计算过程:已知起点A经纬度γ1、λ1,标准终点C经纬度γ2、λ2及惯性定位终点D经纬度γ3、λ3,计算出AC、AD两等角航线航程和航向角,比较即可得到安偏角及里程系数。
2.1 等角航线的航向角及航程计算
2.1.1 航向角及航程精确计算
(1)航向角
如图2所示,等角航线是椭球面上一条与经线保持同一角度的曲线,航线与经线夹角即航向角,设为φ,起点A(经度γ1、纬度λ1),终点C(经度γ2、纬度λ2)。
图2 等角航线示意图
图2的微分三角形中
dx=dssinφ=Ncosλdγ
(13)
dy=dscosφ=RMdλ
(14)
与航位推算的经度、纬度更新式(3)、式(4)解算周期无限小时一致。
对式(13)、式(14)进行积分变换,可得
(15)
(2)航程
等角航线的航程S在φ≠±90°(即λ2≠λ1)时用南北向航程M(子午线弧长差)除以航向角余弦而得,在φ=±90°(即λ2=λ1)时使用纬度圈半径计算,得
(16)
(17)
其中
可得
(18)
(19)
使用式(18)、式(19)可以对航向角和航程进行计算。式中rλ0、RC0对应着不同的纬度值,计算过程较复杂。文献[21]提出了一种简化计算方式,使用等量纬度差精确计算航向角,使用平均纬度计算子午线弧长差,在150n mile以内航程有1m精度。本文更进一步简化,将式(18)、式(19)中rλ0、RC0近似为航程平均纬度点的纬度圈、子午圈曲率半径,可在一定航程内获得满足精度要求的航向角和航程。
2.1.2 使用平均纬度计算航向角及航程
设平均纬度为λ0,式(18)、式(19)中rλ0、RC0近似为rλ0≈N(λ0)cos(λ0),RC0≈RM(λ0),式(18)、式(19)变为使用平均纬度计算公式
(20)
(21)
式中,Δγ=γ2-γ1、Δλ=λ2-λ1,φ0、S0为近似值,设相应的误差为Eφ、ES。
(1)航向角计算及误差
使用式(20)计算航向角即是对等量纬度差g进行了简化计算。
(22)
式中,g0为近似值,设相应的误差为Eg。使用泰勒公式将等量纬度q(λ2)、q(λ1)以λ0为基础展开到2次项并求差得
ζ(Δλ/2))+q(3)(λ0-
υ(Δλ/2)))(Δλ/2)3
(23)
式中,0<ζ,υ<1,λ2-λ0=λ0-λ1=Δλ/2。
航程较短(318km以内,Δλ≤0.05)可忽略Eg中高次项,计算误差时Δλ≈S0cosφ0/a,忽略小量,可得误差为拉格朗日余项差值
q(3)(λ0-υ(Δλ/2)))
(24)
忽略小量,化简可得误差比例为
(25)
带来的角度误差为
(26)
(2)航程计算及误差
使用式(21)计算航程即是对等量纬度差g、子午线弧长差M均进行了简化计算。
L(λ2)-L(λ1)≈M0=RM(λ0)Δλ
(27)
式中,M0为近似值,设相应的误差为EM。
使用泰勒公式对子午线弧长L(λ2)、L(λ1)以λ0为基础展开到2次项并求差得
L(λ2)-L(λ1)=RM(λ0)(Δλ)+
q(3)(λ0-υ(Δλ/2)))(Δλ/2)3
(28)
式中,0<ζ,υ<1,λ2-λ0=λ0-λ1=Δλ/2。
航程较短可忽略EM中高次项,则误差为
L(3)(λ0-υ(Δλ/2)))
(29)
忽略小量,化简可得误差比例为
(30)
对二元函数S(φ,M)=M/cosφ在M0、φ0处进行泰勒展开,航程较短时,可忽略误差中高次项,求得航程误差比例为
(31)
将式(26)、式(30)代入式(31)可得
(32)
误差中主要值为第二项,再化简
(33)
根据式(33)估算的误差,55°纬度,277.8km(即150n mile)计算的误差绝对值最大为28m(比例为1×10-4),比文献[21]中给出的值大,主要是因为使用式(21)计算航程对等量纬度差g亦进行了简化计算,增加了航程计算误差。从式(32)可以看出,对等量纬度差g进行简化计算带来的误差占主要部分。
根据式(26)、式(33),在55°纬度,90km行驶距离,带来的航程相对误差最大为1×10-5,误差最大为0.95m,角度误差绝对值不超过4.3″。
2.2 使用平均纬度标定计算及误差
2.2.1 计算公式
按照式(20)、式(21)分别计算AD、AC等角航线的航向角及航程,航向角求差得标定计算的安偏角Ap,航程相除得标定计算的里程系数Xs。设λ0=(λ2+λ1)/2,λD=(λ3+λ1)/2,计算公式为
(34)
Xs=
(35)
2.2.2 误差分析
使用式(20)、式(21)可以得到两条航线AD、AC航向角及航程,设φD、SD、φ0、S0为计算值,EφD、ESD、Eφ0、ES0为计算的误差。
标定计算的安偏角
Ap=(φD+EφD)-(φ0-Eφ0)
=(φD-φ0)+(EφD-Eφ0)
(36)
将式(26)代入,估算误差时,φD-φ0≈ε,SD/S0≈k,纬度取λ1,标定计算得到的Ap与设定的ε比较,得安偏角标定误差
Ap-ε≈EφD-Eφ0
(k2cos2(φ0+ε)sin(2φ0+2ε)-
cos2φ0sin(2φ0))
(37)
标定计算的里程系数Xs
(38)
将式(33)代入,估算误差时,φD-φ0≈ε、SD/S0≈k,忽略二阶小量,纬度取λ1,标定计算得到的Xs与设定的k比较,得里程系数标定误差
(k2sin2(2φ0+2ε)-sin2(2φ0))
(39)
2.3 使用起点纬度进行标定计算及误差
2.3.1 计算公式
中精度车载惯性定位设备标定计算时常采用起点纬度值进行式(34)、式(35)的计算,即λ0=λD=λ1,公式变为
(40)
Xs=
(41)
2.3.2 误差分析
类比2.1.2节的分析,可得
Eφ≈-0.25(S0/a)tanλ1sin(2φ0)cosφ0
(42)
ES/S0≈-0.25(S0/a)tanλ1sin(2φ0)sinφ0
(43)
可得安偏角标定计算误差
Ap-ε≈0.25(S0/a)tanλ1·
(ksin(2φ0+2ε)cos(φ0+ε)-
sin(2φ0)cosφ0)
(44)
可得里程系数标定计算误差
(ksin(2φ0+2ε)sin(φ0+ε)-
sin(2φ0)sinφ0)
(45)
3 仿真分析
3.1 惯性定位航位推算流程
惯性定位的航位推算流程如图3所示。
图3 惯性定位航位推算流程图
3.2 使用平均纬度标定计算误差仿真
设定航向角,航程按照图3 流程分别计算得到标准点及含系统误差的惯性定位输出点经纬度,按照前面的标定计算公式计算出惯性定位输出包含的系统误差。
设定k=1.2,ε=92.5°,计算不同航向角下的安偏角和里程系数的标定误差,并与式(28)、式(29)计算比较,绘制如图4所示,可见公式能准确估算误差:安偏角不大于0.1″,里程系数不大于5×10-7。
图4 使用平均纬度求解标定误差仿真
采用计算机仿真计算,起点A纬度55°、经度110°。设定不同的里程系数k及安偏角ε组合,计算不同航向角下的标定值误差,结果如图5所示。安偏角标定误差不超过7″,里程系数标定误差不超过1.5×10-5。
图5 使用平均纬度进行标定计算误差
3.3 使用起点纬度标定计算误差仿真
设定k=1.2,ε=92.5°(航程变为10km),计算不同航向角下的安偏角和里程系数的标定误差,与式(44)、式(45)计算比较,绘制如图6所示,可见公式能准确估算误差:安偏角不大于1″,里程系数不大于1×10-5。
图6 理论公式与模拟计算标定误差对比
不同设置值模拟计算绘制系列如图7所示。角度误差最大到180″,里程系数误差最大达9×10-4,在ε≈90°时求解误差大。
图7 使用起点纬度求解的标定误差
在ε≈0°附近模拟计算系列如图8所示。安偏角求解误差不大于50″,里程系数求解误差不大于2.5×10-4。
图8 安偏角小时使用起点纬度的标定误差
在进行标定试验时,若使用起点纬度进行标定计算,可根据惯性定位设备的安装情况,估计并设定安偏角初值,使进行航位推算时的实际安偏角在0°附近,或者进行标定迭代试验,减少该方法的标定误差。
4 结论
本文通过借鉴等角航线的理论得出使用平均纬度进行标定计算的公式,并推导出误差公式,可得以下结论:
1)仿真结果表明,该方法标定在南北纬不大于55°,90km航程以内安偏角标定误差不超过6″,里程系数标定误差不超过1.5×10-5。若在更长的行程下可使用本文的公式进行误差估计。
2)使用起点纬度法进行标定求解的误差较大,只能在更短的航程以及安偏角较小时使用,安偏角较大时则需在估计初值的前提下使用。
3)基于本文的理论,航位推算的纬度经度更新使用起点纬度计算的曲率半径,在高纬度、较大步长时会损失定位精度,后续将进一步开展研究。