基于INS辅助GNSS单天线测姿的组合导航方法及系统技术领域
本发明涉及导航领域，特别涉及一种基于INS辅助GNSS单天线测姿的组
合导航方法及系统。
背景技术
常规INS/GNSS组合导航，状态变量包括两部分，一部分是惯导系统(INS)
的误差状态，包括姿态，速度，位置，传感器零偏共15维，其状态方程为：
X I · ( t ) = F I ( t ) X I ( t ) + Γ I ( t ) W I ( t ) ]]>
式中， X I = [ φ E , φ N , φ U , δv E , δv N , δv U , δL , δλ , δh , ϵ x , ϵ y , ϵ z , ▿ x , ▿ y , ▿ z ] T : ]]>φE、φN、φU分别为INS
东、北、天的方向失准角，δVE、δVN、δVU分别为INS东、北、天的速度误差，
δL、δλ、δH分别为INS经度、纬度和高度的位置误差；εx、εy、εz分别为INS
载体系X、Y、Z三轴的陀螺仪漂移，分别为INS载体系X、Y、Z
三轴的加速度计漂移。
误差状态的另一部分是GNSS的误差状态，包括钟差与钟漂。其观测方程
为
X · G ( t ) = F G ( t ) X G ( t ) + Γ G ( t ) W G ( t ) ]]>
式中，XG＝[δtuδttu]T：δtu为时钟误差等效的距离误差，δttu为与时钟频率误
差等效的距离误差。
将INS误差状态方程与GNSS误差状态方程合并，则得到组合系统的状态
方程如下：
X I · ( t ) X G · ( t ) = F I ( t ) 0 0 F G ( t ) X I ( t ) X G ( t ) + Γ I ( t ) 0 0 Γ G ( t ) W I ( t ) W G ( t ) ]]>
观测变量为其中Zρ＝[δρ1，δρ2，δρ3，δρ4]为伪距测量值之差；
Z ρ · = [ δ ρ · 1 , δ ρ · 2 , δ ρ · 3 , δ ρ · 4 ] ]]>为多普勒测量值之差。
伪距观测方程为
Zρ(t)＝Hρ(t)X(t)+Vρ(t)
多普勒观测方程为
Z ρ · ( t ) = H ρ · ( t ) X ( t ) + V ρ · ( t ) ]]>
将伪距观测方程与多普勒观测方程合并，可得组合系统的观测方程如下：
Z ( t ) = h ρ ( t ) H ρ · ( t ) X ( t ) + V ρ ( t ) V ρ · ( t ) ]]>
状态方程与观测方程均为线性，一般采用卡尔曼滤波进行数据融合。以上
INS/GNSS组合滤波器的观测量中无姿态相关量，姿态误差与陀螺误差可观测性
较差，估计效果不够理想，姿态难以保持较高精度，因此需要增加姿态测量值。
姿态测量值可通过常用的GNSS单天线测姿方法获得。
常用GNSS单天线测姿方法主要有GNSS多天线测姿与GNSS单天线测姿。
GNSS多天线测姿在固定基线长度的情况下，利用GNSS载波相位观测值求解基
线的增量，进而求解姿态；GNSS单天线测姿首先由GNSS在地固系中的速度与
速度微分而来的加速度计算伪姿态，然后转换为姿态。GNSS多天线测姿精度较
高，但与GNSS单天线测姿相比有天线阵列复杂、安装工艺难度大，成本高等
缺点。GNSS单天线测姿具有成本低、精度高、体积小、重量轻、结构简单等优
点，在性价比方面有其他姿态测量所无法企及的优势，但在低速情况下姿态误
差较大。而且，以上两种方法在GNSS受遮挡等环境下失锁时均不可用，需要
使用其它传感器辅助GNSS单天线测姿。
发明内容
有鉴于此，为克服上述至少一个缺点，并提供下述至少一种优点。本发明
公开了一种组合导航方法及系统，采用本发明解决了常规的INS/GNSS组合导
航系统中姿态可观测性差、估计效果不理想与GNSS单天线测姿在低速度情况
下精度较差的问题，提高了姿态可观测性与估计精度。
为解决上述技术问题，本发明采用以下技术方案：
一方面，提供了一种组合导航方法，包括以下步骤：
在第一速度区间范围内，利用第一组合滤波获取姿态测量值，所述第一组
合滤波为观测方程中包含INS与GNSS姿态角测量值差值的INS/GNSS组合滤
波；
在第二速度区间范围内，利用第二组合滤波和附加姿态滤波获取姿态测量
值，所述第二组合滤波为不包含INS与GNSS姿态角测量值差值的INS/GNSS
组合滤波，所述附加姿态滤波用于对所述第二组合滤波的滤波结果进行姿态滤
波；
在第三速度区间范围内，利用所述第二组合滤波获取姿态测量值。
进一步的，在滤波的稳定状态下，对每次获取的所述姿态测量值进行输出
校正，并在完成预定次数的输出校正后进行一次反馈校正。
进一步的，对在第二速度区间范围内获取的所述姿态测量值的步骤为：
对所述第二组合滤波估计获得的位置误差、速度误差、姿态误差进行输出
校正，获取最终位置、速度结果与中间姿态结果；
在反馈校正时刻，利用所述第二组合滤波估计获得的位置、速度误差与角
速度零偏，通过反馈校正，修改INS的位置矩阵、速度与加速度；
将所述中间姿态结果和所述加速度进行附加姿态滤波，获取所述姿态测量
值。
进一步的，所述第一速度区间中的速度大于所述第二速度区间范围大于所
述第三速度区间范围。
进一步的，所述附加姿态滤波的状态方程为：
X · A = F A X A + Γ A W A ]]>
式中，XA＝[φE，φN，φU，εx，εY，εZ]T，φE、φN、φU分别为INS东、北、天的方向失
准角，εx、εy、εz分别为INS载体系X、Y、Z三轴的陀螺仪漂移；
状态系数矩阵为 F A = ( F AN ) 3 × 3 C b n 0 ( F AM ) 3 × 3 6 × 6 ; ]]>
其中FAN的非零元素为
F AN ( 1,2 ) = ω ie sin L + V e R n + h tan L , F AN ( 1,3 ) = - ( ω ie cos L + V e R n + h ) ]]>
F AN ( 2,1 ) = - ( ω ie sin L + V e R n + h tan L ) , F AN ( 2,3 ) = - V n R n + h ; ]]>
F AN ( 3,1 ) = ω ie cos L + V e R n + h , F AN ( 3,2 ) = V n R m + h ]]>
FAN为 F AM = Diag - 1 T rx - 1 T ry - 1 T rz . ]]>
另一方面提供了一种组合导航系统，包括：
速度判断模块，用于对速度进行判断，确定当前速度处于第一速度区间范
围或第二速度区间范围或第三速度区间范围；
第一组合滤波模块，用于在第一速度区间范围内，利用第一组合滤波获取
姿态测量值，所述第一组合滤波为观测方程中包含INS与GNSS姿态角测量值
差值的INS/GNSS组合滤波；
第二组合滤波模块，用于在第二速度区间范围内，利用第二组合滤波获取
中间测量值和/或在第三速度区间范围内，利用第二组合滤波获取姿态测量值，
所述第二组合滤波为不包含INS与GNSS姿态角测量值差值的INS/GNSS组合
滤波；
和，附加姿态滤波模块，用于在第二速度区间范围内，利用所述中间测量
值获取姿态测量值。
进一步的，还包括输出校正模块和反馈校正模块，所述输出校正模块用于
对每次获取的所述姿态测量值进行输出校正；所述反馈校正模块用于在所述输
出校正模块完成预定次数的校正后进行一次反馈校正。
进一步的，在第二速度区间范围内，所述输出校正模块对所述第二组合滤
波模块获取的位置误差、速度误差、姿态误差进行输出校正，获得最终位置、
速度结果与中间姿态结果；
所述反馈校正模块利用所述第二组合模块获得的位置、速度误差与角速度
零偏，修改INS的位置矩阵、速度与加速度；
所述附加姿态滤波模块对所述中间姿态结果和所述加速度进行附加姿态滤
波，获取所述姿态测量值。
进一步的，所述第一速度区间中的速度大于所述第二速度区间范围大于所
述第三速度区间范围。
进一步的，所述附加姿态滤波模块用于进行附加姿态滤波的状态方程为：
X · A = F A X A + Γ A W A ]]>
式中，XA＝[φE，φN，φU，εx，εy，εz]T，φE、φN、φU分别为INS东、北、天的方向失
准角，εx、εy、εz分别为INS载体系X、Y、Z三轴的陀螺仪漂移；
状态系数矩阵为 F A = ( F AN ) 3 × 3 C b n 0 ( F AM ) 3 × 3 6 × 6 ; ]]>
其中FAN的非零元素为
F AN ( 1,2 ) = ω ie sin L + V e R n + h tan L , F AN ( 1,3 ) = - ( ω ie cos L + V e R n + h ) ]]>
F AN ( 2,1 ) = - ( ω ie sin L + V e R n + h tan L ) , F AN ( 2,3 ) = - V n R n + h ; ]]>
F AN ( 3,1 ) = ω ie cos L + V e R n + h , F AN ( 3,2 ) = V n R m + h ]]>
FAN为 F AM = Diag - 1 T rx - 1 T ry - 1 T rz . ]]>
通过采用上述技术方案，本发明的所达到的有益效果为：
本发明针对常规INS/GNSS组合姿态可观测性差、估计效果不理想与GNSS
单天线测姿低速时精度较差的问题，按照速度对组合滤波的不同场景进行划分，
从而提高了高速、中速情况下的姿态估计精度，并避免了低速情况下姿态测量
值较大误差的引入。
在上述高速模式下，本发明在常规INS/GNSS组合滤波器的观测方程中增加
了INS与GNSS姿态测量值之差为观测量，提高了组合导航系统姿态误差与陀
螺误差的可观测性与估计精度。
同时在中速模式下，考虑到GNSS单天线测姿精度稍差，在INS/GNSS组合
滤波器之后附加姿态滤波器，在避免其对组合滤波定位测速的影响的同时提高
了姿态误差与陀螺误差的估计精度。
本发明综合考虑滤波器收敛特性与模式特性，设计了初始输出校正、分频混
合校正与分频混合校正配合使用的组合校正方式，可在各种滤波阶段与滤波模
式下避免误差积累，保持滤波精度。
附图说明
为了更清楚地说明本发明实施例中的技术方案，下面将对本发明实施例描
述中所需要使用的附图作简单的介绍，显而易见地，下面描述中的附图仅仅是
本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的
前提下，还可以根据本发明实施例的内容和这些附图获得其他的附图。
图1为本发明基于INS辅助GNSS单天线测姿的组合导航系统第一实施例
示意图；
图2为本发明基于INS辅助GNSS单天线测姿的组合导航系统第二实施例
示意图；
图3为本发明基于INS辅助GNSS单天线测姿的组合导航方法示意图。
具体实施方式
为使本发明解决的技术问题、采用的技术方案和达到的技术效果更加清楚，
下面将结合附图对本发明实施例的技术方案作进一步的详细描述，显然，所描
述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中
的实施例，本领域技术人员在没有作出创造性劳动前提下所获得的所有其他实
施例，都属于本发明保护的范围。
下面结合附图并通过具体实施方式来进一步说明本发明的技术方案。
图1为本发明基于INS辅助GNSS单天线测姿的组合导航系统第一实施例
示意图；
图2为本发明基于INS辅助GNSS单天线测姿的组合导航系统第二实施例
示意图。
本发明基于INS辅助GNSS单天线测姿的组合导航系统，该系统可装配于
汽车、移动终端等设备中。在这些设备中，GNSS系统可利用接收到的导航卫星
信号进行导航以及实现姿态的测量，惯性导航系统INS可根据当前载体设备的
运动实现惯性导航并可对GNSS系统所测量的姿态值进行补偿。
参考图1，在本发明实施例中，基于INS辅助GNSS单天线测姿的组合导
航系统，包括，速度判断模块、第一组合滤波模块、第二组合滤波模块、附加
姿态滤波模块。
速度判断模块用于获取载体设备当前的速度，并根据获取的速度大小进行
判断，进而确定载体设备当前的运动状态，即处于第一速度区间范围或第二速
度区间范围或第三速度区间范围。在本实施例中，第一速度区间范围、第二速
度区间范围和第三速度区间范围代表不同的速度区间，例如可按照顺序对应为
高速区间、中速区间和低速区间。进一步，高速区间范围可界定为速率＞60km/h，
中速区间范围可界定为速率为20～60km/h，低速区间范围可界定为速率＜20km/h。
对于不同的速度区间范围，本发明实施例采用不同的滤波模式，当速度判
断模块确定载体设备处于第一速度区间范围内即高速区间范围内时，由于GNSS
单天线测姿精度较高，可用于观测方程，增强姿态误差与陀螺误差可观测性，
因此，利用第一组合滤波模块，在第一组合滤波模块在INS/GNSS组合滤波器
的观测方程中增加了INS与GNSS姿态角测量值之差，其中增加部分如下：
Za＝HaXI+Va
其中，Za为INS与GNSS姿态角测量值之差(GNSS姿态角测量值使用GNSS
速度测量值与INS加速度值计算)；XI为INS与IMU误差，定义同上；
H a = I 3 * 3 0 3 * 6 C b n 0 3 * 3 . ]]>
当速度判断模块确定载体设备处于第二速度区间范围内即中速区间范围内
时，GNSS单天线测姿精度稍差，为避免其对原组合滤波定位测速的影响，INS
与GNSS姿态角测量值之差并不用于原观测方程，因此，采用第二组合滤波模
块和附件姿态滤波模块。
其中，附加姿态滤波模块所采用的滤波方程为：
X · A = F A X A + Γ A W A ]]>
式中，XA＝[φE，φN，φU，εx，εy，εz]T，各元素定义与以上相同；
状态系数矩阵为 F A = ( F AN ) 3 × 3 C b n 0 ( F AM ) 3 × 3 6 × 6 ; ]]>
其中FAN的非零元素为
F AN ( 1,2 ) = ω ie sin L + V e R n + h tan L , F AN ( 1,3 ) = - ( ω ie cos L + V e R n + h ) ]]>
F AN ( 2,1 ) = - ( ω ie sin L + V e R n + h tan L ) , F AN ( 2,3 ) = - V n R n + h ; ]]>
F AN ( 3,1 ) = ω ie cos L + V e R n + h , F AN ( 3,2 ) = V n R m + h ]]>
FAN为 F AM = Diag - 1 T rx - 1 T ry - 1 T rz . ]]>
其观测方程为
ZA＝HAXA+VA
式中，ZA为INS与GNSS姿态角测量值之差(GNSS姿态角测量值使用精度
较高的原INS/GNSS组合速度与INS加速度计算)； H A = I 3 * 3 C b n . ]]>
当速度判断模块确定载体设备处于第三速度区间范围内即低速区间范围内
时，GNSS姿态角测量精度较差不宜使用，只使用组合姿态结果，因此，仍然采
用第二组合滤波模块。
进一步参考图2，针对上述各滤波模块的收敛特性，本实施例中，在上述基
于INS辅助GNSS单天线测姿的组合导航系统基础上，进一步包含了输出校正
模块和反馈校正模块。
对手输出校正模块和反馈校正模块针对上述各滤波模块工作的不同阶段，
进而采用不同的校正模式。
在初始阶段，各滤波模块中的滤波器从滤波开始到稳定需要一段时间，而
在滤波初期，由于状态初值和噪声方差初值的选取与实际不符，估计误差较大，
因此仅由输出校正模块进行输出校正。
当各滤波模块中的滤波器逐渐进入稳态后，输出校正的频率与滤波频率相
同，即每一次滤波运算后都进行一次输出校正。而为了避免仅有输出校正时误
差积累与过频繁的反馈校正降低估计精度的问题，在每进行10次输出校正后进
行一次反馈校正。
对于上述输出校正和反馈校正相结合的模式，由于在不同的速度区间范围
内，对应不同的滤波模块，即在第一速度区间范围内只使用第一组合滤波模块，
在第三速度区间范围内只使用第二组合滤波模块，因此输出校正和反馈校正均
可一步完成。
对于第二速度区间范围，由于使用第二组合滤波模块和附加姿态滤波模块
因此，采用以下过程分布完成输出校正和反馈校正。
输出校正模块对所述第二组合滤波模块获取的位置误差、速度误差、姿态
误差进行输出校正，获得最终位置、速度结果与中间姿态结果；
反馈校正模块利用第二组合模块获得的位置、速度误差与角速度零偏，修
改INS的位置矩阵、速度与加速度；
附加姿态滤波模块对中间姿态结果和加速度进行附加姿态滤波，获取姿态
测量值。
同时，在反馈校正时刻，反馈校正模块利用第二组合滤波模块和附加姿态
滤波模块估计获得的姿态误差和脱落飘移修改INS捷联解算的姿态矩阵与角速
度。
图3为本发明基于INS辅助GNSS单天线测姿的组合导航方法示意图。
参考图3，在本发明实施例的基于INS辅助GNSS单天线测姿的组合导航
方法中，经过初始对准之后，启动INS捷联解算，在滤波时刻，对当前载体设
备的速度进行判断，以确定当前速度所处的速度区间。
若处于第一速度区间，即载体设备为高速模式时，则计算GNSS单天线姿
态测量值，并利用包含INS与GNSS姿态角测量值差值的INS/GNSS组合滤波
进行滤波。该第一速度区间的滤波模式对应于INS与GNSS相组合的第一组合
滤波。
若处于第二速度区间，即载体设备为中速模式时，采用第二组合滤波，该
第二组合滤波为不包含INS与GNSS姿态角测量值差值的INS/GNSS组合滤波，
并且在此基础上采用附加姿态滤波用于获取最终的姿态测量值。
若处于第三速度区间，即载体设备为低速模式时，仅采用第二组合滤波以
获取姿态测量值。
对于上述获取的姿态测量值，本实施例进一步通过校正进行修正。对于校
正，在不同时刻并且针对不同的滤波模式，采用不同的校正形式。
进一步参考图3，在滤波初始阶段，仅采用输出校正对姿态测量值进行校正。
当滤波进入稳定阶段后，则采用分配混合校正和分模式混合校正相结合的方式
进行。对于分频混合校正是指在没进行多次输出校正后，进行一次反馈校正，
本实施例中，输出校正和反馈校正的频次比为10次∶1次。对于分模式混合校
正则是针对第一速度区间、第三速度区间的滤波模式和第二速度区间的滤波模
式分别进行。对于第一速度区间和第三速度区间由于仅采用单一滤波，则一次
完成输出校正和反馈校正。对于第二速度区间，由于采用第二组合滤波和附加
姿态滤波相结合的滤波模块，则采用下述校正过程：
对第二组合滤波估计获得的位置误差、速度误差、姿态误差进行输出校正，
获取最终位置、速度结果与中间姿态结果；
在反馈校正时刻，利用第二组合滤波估计获得的位置、速度误差与角速度
零偏，通过反馈校正，修改INS的位置矩阵、速度与加速度；
将中间姿态结果和所述加速度进行附加姿态滤波，获取姿态测量值。
以上实施例提供的技术方案中的全部或部分内容可以通过软件编程实现，
其软件程序存储在可读取的存储介质中，存储介质例如：计算机中的硬盘、光
盘或软盘。
注意，上述仅为本发明的较佳实施例及所运用技术原理。本领域技术人员
会理解，本发明不限于这里所述的特定实施例，对本领域技术人员来说能够进
行各种明显的变化、重新调整和替代而不会脱离本发明的保护范围。因此，虽
然通过以上实施例对本发明进行了较为详细的说明，但是本发明不仅仅限于以
上实施例，在不脱离本发明构思的情况下，还可以包括更多其他等效实施例，
而本发明的范围由所附的权利要求范围决定。