基于三向卡尔曼滤波平滑器的GNSS/INS组合定位定向算法制造技术

本发明专利技术公开了基于三向卡尔曼滤波平滑器的GNSS/INS组合定位定向算法，GNSS使用载波相位差分观测值求得位置量，采用捷联惯性导航的独立推算，结合惯导元器件的精度水平给出系统噪声协方差矩阵、状态向量的协方差矩阵进行正向滤波，使用第一次正向滤波结束时刻的估计结果作为反向滤波的初始信息，以反向滤波的结束时刻的估计结果作为第二次正向滤波的初始信息，反向滤波和第二次正向滤波的估计结果进行平滑得到最终的位置、速度和姿态角信息。本发提高GNSS/INS组合导航后处理过程中的精度水平，尤其是在GNSS卫星故障期间和观测数据的开始阶段的精度。

【技术实现步骤摘要】
基于三向卡尔曼滤波平滑器的GNSS/INS组合定位定向算法
本专利技术涉及高精度的GNSS/INS组合定位测姿后处理
，具体是基于三向卡尔曼滤波平滑器的GNSS/INS组合定位定向算法。
技术介绍
在GNSS/INS组合定位定向数据处理过程中，人们关心的侧重点会随着应用领域的不同而不同，如无人机的导航，关心的重点在于组合的实时性和可靠性，而在测绘领域，数据可以进行后处理，相应的对于精度的要求会大幅度提高。在城市峡谷中，常见高楼、隧道、立交桥等建筑物会对GNSS卫星的信号产生遮挡，造成卫星失锁或者比较严重的多路劲效应，降低数据的质量。导致GNSS卫星无法提供无时间积累误差的位置信息，原先的组合导航定定向模式会转变成惯性导航系统单独工作，此时系统的精度主要取决于惯性元器件的精度、卫星失锁的时间长短以及再失锁前整个系统的精度水平。惯性导航的原理也就决定了系统的精度会受到时间积累的影响，对于高精度的后处理技术来说这是必须要解决的问题点。另一方面在GNSS/INS组合导航中，多使用的是卡尔曼滤波器进行组合处理，此时滤波器中系统噪声的初试协方差矩阵、状态向量的协方差矩阵一般是根据经验值预设的，这和实际的真值之间差值一般挺大，组合导航滤波器中特有的姿态角对准精度也是不高。这些量都必须要在滤波器中进行估计后才能慢慢达到收敛，收敛之前解算的数据精度通常不高。以上两个问题点都会使得系统的精度水平波动很大，在实际的应用中，人们最希望的是整个观测时间段内都能保持高精度的定位定向精度。因此有人采用双向滤波结果加权平均的方法，有人采用顺向滤波，反向平滑的方法。除此之外，少有文献和专利来解决这个问题。
技术实现思路
本专利技术的目的在于提供基于三向卡尔曼滤波平滑器的GNSS/INS组合定位定向算法，有效解决了因GNSS数据丢弃期间IMU独立导航时参数快速漂移的问题，提高GNSS/INS组合导航后处理过程中的精度水平，尤其是在GNSS卫星故障期间和观测数据的开始阶段的精度。为实现上述目的，本专利技术提供如下技术方案：基于三向卡尔曼滤波平滑器的GNSS/INS组合定位定向算法，包括下列步骤：(1)GNSS使用载波相位差分观测值求得厘米级的位置量，INS采用开始时一定长度的观测数据做粗对准或者精对准得到初始的姿态角，利用所算的初始姿态角进行捷联惯性导航的独立推算，结合惯导元器件的精度水平给出系统噪声协方差矩阵Q，状态向量的协方差矩阵P0，使用厘米级的位置量、系统噪声协方差矩阵Q和状态向量的协方差矩阵P0进行正向滤波，储存过程中各个时刻的估计结果其中，是状态向量，k是刻度因子，P是状态量的方差；(2)在第一次正向滤波后，使用第一次正向滤波结束时刻的估计结果作为反向滤波的初始信息，即第二次滤波，保存第二次过程中各个时刻的估计结果(3)以反向滤波的结束时刻的估计结果作为第二次正向滤波的初始信息，即第三次滤波，同样保存第三次过程中各个时刻的估计结果(4)以第二次滤波和第三次滤波的估计结果进行平滑得到最终的位置、速度和姿态角信息。作为本专利技术进一步的方案：反向滤波的初始历元是正向滤波的结束历元，初始滤波参数是正向滤波结束时的参数。作为本专利技术进一步的方案：第二次正向滤波的初始历元是反向滤波的结束历元，初始滤波参数是反向滤波结束时的参数。作为本专利技术进一步的方案：最终输出的结果是以反向滤波和第二次正向滤波做平滑后的结果。本专利技术的原理：在GNSS/INS的组合定位测姿中，滤波的状态方程是非线性的，在经过线性化后各状态量也就变成各状态的误差量，因而得采用扩展卡尔曼滤波的形式进行组合，其状态、量测方程如下式：Z＝HX(t)+V(2)式中：X(t)是状态向量，其协方差矩阵为P阵，W(t)是系统噪声，其相应的协方差矩阵为Q，G(t)是噪声驱动矩阵，F(t)是状态转移矩阵；Z是量测值，V的协方差矩阵是R阵。滤波开始时需要根据惯导元器件的精度水平给出系统噪声协方差矩阵Q，状态向量的协方差矩阵P0，而此时给出的P0一般会比实际的值偏大，滤波器处于发散状态，因而采用一次滤波就会导致系统开始阶段的组合精度不高。当以地固系作为导航坐标系时，惯导系统的导航推算方程是：即可见惯导的惯导推算是对先利用陀螺仪求出旋转矩阵(利用陀螺输出的角速度算得)，按照公式把加速度计输出的fb转化成fe，经公式一次积分得到速度ve，公式二次积分得到位置xe。而受惯导器材硬件属性的影响，其输出的原始观测量中存在误差，需按照公式和进行误差补偿后，才能得到真值。fRealb＝kd×(fRawb-d)(7)通常陀螺和加表的零偏、刻度因子的初值按照表1给出，这和其真值存在较大偏差，需在组合导航中实时校正。基于以上两个原因，在本
技术实现思路
的步骤1中，第一次正向滤波的开始阶段系统精度较低。当GNSS卫星有效的时候，可按照公式对陀螺和加表的零偏量b、d，刻度因子kb、kd进行实时的校准，各状态量的方差pk/k降低；当GNSS卫星无效时，此时的导航参数只能由惯导单独算得，即按照公式(3)-(5)计算，此时陀螺和加表的零偏量b、d，刻度因子kb、kd只能使用失锁前的值，和系统当前的状态量不匹配，且从公式(3)-(5)可知，利用惯导计算导航参数时，是基于积分的原理，所以误差会随着时间累积，状态量的方差pk/k只能依据公式推估得到，因而系统的精度随着时间累积而降低。在进行正向滤波时，系统的精度在卫星无效期间的前半部分增长缓慢，反向滤波时，卫星无效期间的后半部分误差增长缓慢(如图2所示)。即反应在本专利技术的步骤2-3中，在卫星失锁期间，系统的精度随着时间累积而降低。随着卫星的恢复和观测时间的推进，滤波器会逐渐趋于平稳，因采用第一次正向反向结束时的结果作为反向滤波的初始信息，同理反向滤波的结束信息作为第二次正向滤波的初始信息，因而本专利技术的步骤2-3中提到在观测数据开始阶段反向滤波和第二次正向滤波均能保持高精度的系统状态。在步骤4中，使用反向滤波和第二次正向滤波的结果做平滑，作为本专利技术的最终结果。使用的平滑公式是公式(8)-(9)。式中上标S表示最终的平滑结果，上标b表示反向滤波结果，k＝0,1,2,...,n-1,n。与现有技术相比，本专利技术的有益效果是：本专利技术提供了基于三向卡尔曼滤波平滑器的GNSS/INS组合定位定向算法，以第一次正向滤波的结束历元结果做为反向滤波的起始历元和初始滤波参数，以反向滤波的结束历元结果作为第二次正向滤波的起始历元和初始滤波参数，提高单次正向滤波初始收敛前参数精度，有效解决了因GNSS数据丢弃期间IMU独立导航时参数快速漂移的问题，提高了参数精度，尤其是位置参数。本专利技术提高GNSS/INS组合导航后处理过程中的精度水平，尤其是在GNSS卫星故障期间和观测数据的开始阶段的精度。附图说明图1是基于三向卡尔曼滤波平滑器的GNSS/INS组合定位定向算法流程图；图2是基于三向卡尔曼滤波平滑器的GNSS/INS组合定位定向算法效果示意图；图3是车载运动轨迹图；图4是人为删除GNSS结果轨迹图；图5是经GNSS/INS单次正向滤波组合后的陀螺零偏图；图6是经GNSS/INS单次正向滤波组合后的陀螺刻度因子图；图7是经GNSS/INS单次正向滤波组合后的加表零偏图；图8是经GNSS/INS单次正向滤波组合后的加表本文档来自技高网...

【技术保护点】
基于三向卡尔曼滤波平滑器的GNSS/INS组合定位定向算法，其特征在于，包括下列步骤：(1)GNSS使用载波相位差分观测值求得厘米级的位置量，INS采用开始时一定长度的观测数据做粗对准或者精对准得到初始的姿态角，利用所算的初始姿态角进行捷联惯性导航的独立推算，结合惯导元器件的精度水平给出系统噪声协方差矩阵Q，状态向量的协方差矩阵P0，使用上述数据进行正向滤波，储存过程中各个时刻的估计结果Pk/k，其中，是状态向量，k是刻度因子，P是状态量的方差；(2)在第一次正向滤波后，使用第一次正向滤波结束时刻的估计结果Pend1作为反向滤波的初始信息，即第二次滤波，保存第二次过程中各个时刻的估计结果Pk/k2；(3)以反向滤波的结束时刻的估计结果Pend2作为第二次正向滤波的初始信息，即第三次滤波，同样保存第三次过程中各个时刻的估计结果Pk/k3；(4)以第二次滤波和第三次滤波的估计结果进行平滑得到最终的位置、速度和姿态角信息。

【技术特征摘要】
1.基于三向卡尔曼滤波平滑器的GNSS/INS组合定位定向算法，其特征在于，包括下列步骤：(1)GNSS使用载波相位差分观测值求得厘米级的位置量，INS采用开始时一定长度的观测数据做粗对准或者精对准得到初始的姿态角，利用所算的初始姿态角进行捷联惯性导航的独立推算，结合惯导元器件的精度水平给出系统噪声协方差矩阵Q，状态向量的协方差矩阵P0，使用厘米级的位置量、系统噪声协方差矩阵Q和状态向量的协方差矩阵P0进行正向滤波，储存过程中各个时刻的估计结果Pk/k，其中，是状态向量，k是刻度因子，P是状态量的方差；(2)在第一次正向滤波后，使用第一次正向滤波结束时刻的估计结果Pend1作为反向滤波的初始信息，即第二次滤波，保存第二次过程中各个时刻的估计结果Pk/k2；(3)以反向滤波的结束时刻的估...

【专利技术属性】
技术研发人员：孙红星，丁学文，王晖，
申请(专利权)人：孙红星，
类型：发明
国别省市：湖北;42