一种基于降维滤波的INS/GNSS杆臂误差在线估计方法与流程

一种基于降维滤波的ins/gnss杆臂误差在线估计方法
技术领域
1.本技术属于车载组合导航系统技术领域，尤其涉及一种基于降维滤波的ins/gnss杆臂误差在线估计方法。


背景技术：

2.组合导航系统是车辆辅助驾驶和自动驾驶中不可或缺的模块，负责为载体提供实时、高精度的位置、速度和姿态信息。车载组合导航系统主要包括了惯性导航模块(ins，inertial navigation system)、卫星导航模块(gnss，global navigation satellite system)，其中惯性导航模块主使用微机械式系统(mems,micro-electro-mechanical system)，其体积小，成本低，数据更新率高，且不依赖外部信息，但是由于内部的陀螺和加速度计精度较低，无法单独长时间工作，因此需要将惯导和gnss模块进行数据融合，确保高精度导航性能。
3.ins一般以内部的惯性测量单元(imu，inertial measurement unit)的几何中心作为导航定位的参考基准。gnss以接收天线的相位中心作为参考基准，当gnss工作在实时动态载波相位差分状态(rtk，real-time kinematic)时，理论上定位精度可优于3～5厘米。在实际应用中，ins与gnss天向在安装位置上存在一定的偏差，称为杆臂误差。杆臂误差在实际使用中无法避免，如果车载组合导航系统杆臂误差补偿不够准确，则会严重降低ins/gnss的组合导航性能，无法达到厘米级的定位精度。
4.现有的文献中，对ins/gnss杆臂误差一般有两种处理方案，即物理测量方案和滤波估计方案。物理测量方案顾名思义，就是通过事先测量杆臂长度，然后再组合导航中进行补偿，这类方法需要较高的测量精度，但是在车载应用环境中曲面较多，难以精确测量，而且ins与gnss的准确参考基准并不可见，无法定位，因此物理测量方案存在一定的局限性。滤波估计方案是将ins/gnss杆臂作为扩展状态变量，与速度位置误差、传感器零偏等通过一个滤波器进行在线估计。滤波估计方案存在的问题是状态变量维度较大，导致存储空间和计算量大幅增加，而且状态变量之间的耦合严重，影响了滤波估计收敛速度和估计精度。因此为了提升高精度车载组合导航系统的定位性能，需要研究一种简单易行的ins/gnss杆臂误差在线估计方法，降低组合导航算法对芯片主频和存储空间的要求，提升计算速度和滤波估计精度，确保在高精度应用场合下的车载组合导航系统定位性能。


技术实现要素：

5.本技术提出一种基于降维滤波的ins/gnss杆臂误差在线估计方法，该方法将ins/gnss杆臂误差和其他误差变量拆分，构建多个降维滤波器，降低了误差之间的耦合和存储空间，机理清晰，模型简化，且计算速度快，估计精度高，经过杆臂误差在线估计和补偿后可提升车载ins/gnss组合导航系统的定位精度，体现出了较高的工程应用价值。
6.本技术所采用的技术方案如下：
7.一种基于降维滤波的ins/gnss杆臂误差在线估计方法，该方法包括如下步骤：
8.步骤1、启动ins/gnss组合导航系统，并完成组合导航系统的位置和姿态导航参数初始化；
9.步骤2、ins惯性导航系统更新自身的导航参数，所述导航参数包括速度、位置和姿态；
10.步骤3、将误差状态变量进行拆分，构建降维滤波器；
11.步骤4、通过卡尔曼滤波在线估计各项误差状态变量；
12.步骤5、补偿杆臂误差和其余各项误差状态变量。
13.进一步的，在步骤3中，所述构建降维滤波器为构建4个状态估计滤波器，所述状态估计滤波器的模型表示为：
[0014][0015]
其中i＝1,2,3,4对应4个滤波器，xi表示状态变量，zi表示量测变量，fi表示系统矩阵，hi表示量测矩阵；wi与vi分别为系统噪声与量测噪声，二者均为白噪声。
[0016]
进一步的，在步骤3中，将误差状态变量进行拆分，包括：
[0017]
将需要估计的ins/gnss杆臂误差连同其他误差变量，拆分到4个状态估计滤波器中，对应的状态变量x1,x2,x3,x4的表达式为：
[0018]
x1＝[φ
u εz]
t
ꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(2)
[0019][0020][0021][0022]
其中φe表示东向平台偏角、φn代表示北向平台偏角、φu表示天向平台偏角、δve表示东向速度误差、δvn表示北向速度误差、δvu表示天向速度误差、δl表示纬度误差、δλ表示经度误差、δh表示高度误差、εz表示z陀螺漂移，表示x加速度计零偏，表示y加速度计零偏，表示z加速度计零偏，l
x
表示x方向杆臂，ly表示y方向杆臂，lz表示z方向杆臂。
[0023]
进一步的，在步骤3中，构建的所述降维滤波器包括：
[0024]
4个状态估计滤波器的系统矩阵表达式为：
[0025][0026]
[0027][0028][0029]
其中fu表示天向加速度，r
x
,ry分别表示地球子午圈、卯酉圈半径，l表示当地纬度，表示ins的姿态转换矩阵第i行第j列的元素；
[0030]
4个状态估计滤波器的量测变量表达式为：
[0031][0032]
z1＝ψ
ins-ψ
gnss
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(11)
[0033]
z2＝h
ins-h
gnss-δphꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(12)
[0034]
z3＝l
ins-l
gnss-δp
l
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(13)
[0035]
z4＝λ
ins-λ
gnss-δp
λ
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(14)
[0036]
其中l
x0
,l
y0
,l
z0
表示本次上电读取得到的杆臂误差值，ψ
gnss
,ψ
ins
分别表示gnss的双天线航向和ins的航向，h
gnss
,h
ins
分别表示gnss和ins的高度，l
gnss
,l
ins
分别表示gnss和ins的纬度，λ
gnss
,λ
ins
分别表示gnss和ins的经度。
[0037]
进一步的，在步骤3中，构建的所述降维滤波器还包括：4个状态估计滤波器的量测矩阵表达式为：
[0038]
h1＝[1 0]
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(15)
[0039]
h2＝[0 1 0]
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(16)
[0040][0041][0042]
其中，量测矩阵均是一维变量，滤波增益也是一维变量。
[0043]
进一步的，在步骤4中，在线进行卡尔曼滤波估计时，状态估计滤波器x3和x4共用相同的状态噪声矩阵qk和量测噪声矩阵rk，并使用相同的状态变量表达式、系统矩阵、量测变量以及量测矩阵，这4个降维滤波器的量测均是一维变量且不需要进行矩阵求逆运算，以简化和降低计算量。
[0044]
进一步的，在步骤5中，补偿其余各项误差状态变量包括速度、位置、姿态的误差补偿，计算公式如下：
[0045][0046][0047][0048]
其中，为补偿后的东向、北向、天向速度，为补偿后的纬度、经度和高度，为补偿后的姿态转换矩阵，xi(j)代表第i个滤波器的第j个状态变量。
[0049]
进一步的，在步骤5中，补偿其余各项误差状态变量包括陀螺、加速度计的误差补偿，计算公式如下：
[0050][0051]
其中为补偿后的z陀螺漂移和x、y、z加速度计零偏。
[0052]
进一步的，杆臂误差补偿的计算公式如下：
[0053][0054]
其中，表示x方向杆臂误差补偿，表示y方向杆臂误差补偿，表示z方向杆臂误差补偿；xi(j)代表第i个滤波器的第j个状态变量。
[0055]
进一步的，当杆臂误差收敛后，即可将杆臂误差写入到存储芯片中，当下次组合导航系统启动时读取出并更新4个状态估计滤波器的量测变量表达式中的l
x0
,l
y0
,l
z0
。
[0056]
与现有技术相比，本技术所具有的有益效果为：
[0057]
(1)本技术提出的基于降维滤波的ins/gnss杆臂误差在线估计方法，将杆臂误差和其他误差经过拆分构建了多个降维滤波器，机理清晰，模型简化，降低了各项误差之间的耦合，提高了杆臂误差的在线估计精度；
[0058]
(2)本技术提出的基于降维滤波的ins/gnss杆臂误差在线估计方法，设计的状态变量和量测变量的维度较低，从而显著提升了计算速度，降低组合导航算法对芯片主频和存储空间的要求，易于实施，体现出了较高的工程应用价值。
附图说明
[0059]
图1为杆臂误差在线估计方法实施的流程图；
[0060]
图2为本技术具体实施例的机体系x方向杆臂误差的仿真估计曲线；
[0061]
图3为本技术具体实施例的机体系y方向杆臂误差的仿真估计曲线；
[0062]
图4为本技术具体实施例的机体系z方向杆臂误差的仿真估计曲线；
[0063]
图5为本技术具体实施例的车载试验的运动轨迹图；
[0064]
图6为本技术具体实施例的机体系x方向杆臂误差的真实估计曲线；
[0065]
图7为本技术具体实施例的机体系y方向杆臂误差的真实估计曲线；
[0066]
图8为本技术具体实施例的机体系z方向杆臂误差的真实估计曲线；
[0067]
图9为本技术具体实施例的车载试验东向位置误差对比曲线；
[0068]
图10为本技术具体实施例的车载试验北向位置误差对比曲线；
[0069]
图11为本技术具体实施例的车载试验天向位置误差对比曲线。
具体实施方式
[0070]
为使本技术实施例的目的、技术方案和优点更加清楚，下面将结合本技术实施例中的附图，对本技术实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例是本技术一部分实施例，而不是全部的实施例。基于本技术中的实施例，本领域普通技术人员在没有作出创造性劳动前提下所获得的全部其他实施例，都属于本技术保护的范围。
[0071]
本技术的杆臂误差在线估计方法的原理如下：基于扩展状态思路对ins/gnss杆臂误差进行滤波估计时，惯导系统的标准误差模型表示为：
[0072][0073]
ins/gnss组合导航的量测变量包括7维，即：
[0074][0075]
如果直接运用上述状态方程和量测信息通过卡尔曼滤波估计ins/gnss杆臂误差和其他误差，则状态维度为18，量测维度为7，维度较高会导致状态变量之间的耦合较为严重，在进行滤波估计时不仅计算量大，还会影响误差变量的估计精度。本技术的技术方案则是将18维的状态变量和7维的量测变量进行拆分，同时简化误差方程。简化的原则如下：
[0076]
·
在车载使用环境下，组合导航系统的俯仰角和横滚角始终较小，因此航向估计回路和高度估计回路可以拆分出来；
[0077]
·
车载ins系统一般使用mems传感器，器件的零偏误差较大，因此可以忽略误差方程中的部分误差项，从而实现东向位置回路和北向位置回路解耦；
[0078]
·
mems陀螺的漂移可以在初始启动过程中直接计算均值扣除，扣除后可以认为陀螺漂移较小，无需在滤波估计中列入状态变量。
[0079]
在此基础上可以对惯导系统的标准误差模型以及7维量测变量进行简化，得到如下的状态方程组：
[0080][0081][0082][0083][0084]
上述4个状态方程组即对应于本技术提出的4个降维滤波器，此时的4个状态模型的维度分别是2维、3维、8维和8维，相比于原来的18维状态变量已明显降维。而当gnss处于rtk状态时，位置精度可达厘米级，而速度精度没有明显提升，因此在选取量测信息时可以忽略gnss的速度信息，仅保留gnss的双天线航向、纬度、经度和高度四维量测，且恰好分配到4个滤波器中，每个滤波器的量测变量仅1维，从而避免了矩阵求逆过程。经过状态变量和量测变量拆分后，误差之间的耦合显著降低，从而可以提高误差的估计速度和精度。
[0085]
此外，卡尔曼滤波的计算量与状态变量和量测变量的维度有关，根据经验公式，卡尔曼滤波的时间复杂度为o(m
2.376
n2)，其中m是量测变量维度；n是状态变量维度。由此可计算出传统方法的时间复杂度数值为425.85，本技术提出的基于降维滤波的时间复杂度数值为145.0，降低了约65％的计算量，显著提升了计算速度。
[0086]
下面结合附图对本技术作进一步介绍。
[0087]
图1给出了本技术一种基于降维滤波的ins/gnss杆臂误差在线估计方法实施的流
程图，该方法将杆臂误差和其他误差经过拆分构建了多个降维滤波器，机理清晰，模型简化，降低了各项误差之间的耦合，提高了杆臂误差的在线估计精度。此外，该方法仅包括5个步骤，并且设计的状态变量和量测变量的维度较低，从而显著提升了计算速度，降低组合导航算法对芯片主频和存储空间的要求，易于实施，体现出了较高的工程应用价值。
[0088]
为了验证本技术提出的轮速计误差在线估计方法的正确性，首先设计了仿真验证试验，通过轨迹发生器生成“8”字形运动轨迹，根据mems惯导、gnss模块和轮速计的基本性能添加各种误差，进而通过仿真程序得到ins/gnss杆臂误差的仿真估计结果。图2、图3和图4分别给出了载体坐标系x、y、z方向的杆臂误差仿真估计曲线，在仿真时杆臂误差的设定真值分别为-0.5m、0.8m和1.0m，仿真结果显示x方向杆臂的估计结果为-0.49m，y方向杆臂的估计结果为0.80m，z方向的杆臂估计结果为1.02m，ins/gnss的杆臂估计精度优于2cm，且估计过程中曲线波动较小，证明了本技术提出的ins/gnss杆臂误差在线估计方法是准确可行的。
[0089]
图5给出了本技术具体应用的车载试验的运动轨迹，试验中使用的惯导系统中mems陀螺和加速度计的零偏稳定性分别是10
°
/h和100ug，gnss模块使用um482芯片，动态rtk精度可达2cm。车载试验总时长约1000秒，行驶路线位于北京市朝阳区东四环附近，运行形式包括了直线加减速、转弯等，航向运动覆盖了0-360
°
全部范围，最大速度约30m/s。
[0090]
图6、图7和图8给出了本技术具体应用的车载试验得到的ins/gnss杆臂误差的在线估计结果，载体坐标系下杆臂的估计结果分别是-0.26m、0.20m和1.21m，误差估计曲线快速收敛，波动较小，与仿真结果基本吻合，证明了车载试验过程中ins/gnss杆臂误差的在线估计结果是可信的。
[0091]
在本技术具体应用的车载试验中，已经通过事先测量得到了ins/gnss杆臂的粗略值。为了进一步验证车载试验中ins/gnss杆臂误差在线估计结果的准确性，可以分两种情况对导航数据进行离线处理，即按本技术方法在线补偿杆臂和按事先测量值补偿杆臂，经过离线导航后得到位置输出，与车载试验过程中的基准系统进行比对，从而验证ins/gnss杆臂在线估计的准确性。图9、图10和图11分别给出了本技术具体应用的车载试验东向、北向和天向位置误差的对比曲线，可见由于杆臂事先测量精度低，导致东向和北向位置误差存在随着载体转弯而呈现出的台阶误差，而天向位置存在一定的常值误差；而经过本技术提出的ins/gnss杆臂误差在线估计和补偿后，东向、北向和天向的位置误差比较平稳，波动较小。在此基础上可以通过计算位置误差的均方根来评估位置精度，通过杆臂事先测量方法得到的东向、北向和天向位置误差的均方根分别是0.193m、0.112m和0.108m，通过本技术方法对杆臂进行在线估计和补偿后的东向、北向和天向位置误差的均方根分别是0.041m、0.047m和0.029m，位置精度提升明显，基本达到了rtk定位的精度水平，证明了本技术提出的ins/gnss杆臂误差在线估计和补偿的准确性。
[0092]
尽管上面对本技术说明性的具体实施方式进行了描述，以便于本技术领域的技术人员理解本技术，但应该清楚，本技术不限于具体实施方式的范围，对本技术领域的普通技术人员来讲，只要各种变化在所附的权利要求限定和确定的本技术的精神和范围内，这些变化是显而易见的，一切利用本技术构思的发明创造均在保护之列。