减小失锁时速度误差对组合导航精度影响的方法及系统的制作方法
【专利摘要】本发明公开了一种减小失锁时速度误差对组合导航精度影响的方法及系统，包括全球卫星定位系统接收机、微机械惯组、可编程逻辑控制器、数字信号处理器和CAN总线，全球卫星定位系统接收机的信号输出端连接可编程逻辑控制器的信号输入端，微机械惯组的信号输出端连接数字信号处理器的信号输入端，可编程逻辑控制器的信号输出端连接数字信号处理器的信号输入端，数字信号处理器的信号输出端连接CAN总线。该方法和系统能明显提高卫星失锁状态下惯导/全球卫星定位组合导航系统的导航精度。
【专利说明】减小失锁时速度误差对组合导航精度影响的方法及系统
【技术领域】
[0001]本发明涉及组合导航【技术领域】，具体地指一种通过减小失锁时速度误差提高组合导航精度的方法及系统。
【背景技术】
[0002]惯导系统是一种自主的导航系统，具有工作时不依赖任何外界导航设备的特点，且隐蔽性好，数据更新率高。但是惯导系统的导航误差会随时间积累。卫星导航GPS(GlobalPositioning System,全球卫星定位系统)能够全天候地提供高精度的三维位置、速度和时间基准信息，具有静态定位、动态导航及精密授时的功能，但是卫星导航容易受到干扰，短期稳定低，更新率较低。针对上述惯导系统和卫星导航GPS的优点和缺点，目前设计出了一种惯导/全球卫星定位组合导航系统。该组合系统将卫星和惯导系统相结合，能够发现并测量惯性导航的系统误差，弥补卫星导航的信号缺失，提高采样率，提高模糊度的搜索速度，提高周期性数据跳变的检测能力，使组合后的导航精度高于两个系统单独工作的精度，增加了观测冗余度和系统的抗干扰能力。
[0003]在惯导/全球卫星定位组合导航系统进行组合导航的过程中，当卫星出现失锁状态时，由于卫星个数的不断减少且高动态全球卫星定位系统的数据要求实时性高，使得高动态全球卫星定位系统的速度输出会产生较大误差。而对于惯导/全球卫星定位组合导航系统，惯导/全球卫星定位组合导航系统无法分辨这种误差，所以会将卫星失锁的误差直接引入惯导/全球卫星定位组合导航系统，导致惯导/全球卫星定位组合导航系统的精度明显降低。
【发明内容】
[0004]本发明的目的就是要提供一种减小失锁时速度误差对组合导航精度影响的方法及系统，该方法和系统能明显提高卫星失锁状态下惯导/全球卫星定位组合导航系统的导航精度。
[0005]为实现此目的，本发明所设计的减小失锁时速度误差对组合导航精度影响的方法，其特征在于，它包括如下步骤:
[0006]步骤1:全球卫星定位系统接收机实时监测全球卫星定位系统的卫星个数信息，同时通过全球卫星定位系统得到全球卫星定位系统接收机自身的实时位置信息和实时速度信息，并且，将全球卫星定位系统实时的卫星个数信息、全球卫星定位系统接收机自身的实时位置信息和实时速度信息通过可编程逻辑控制器传输给数字信号处理器；
[0007]步骤2:微机械惯组将自身的实时位置信息、实时速度信息和实时姿态信息也传输给数字信号处理器；
[0008]步骤3:在数字信号处理器中得到微机械惯组自身的实时位置信息和实时速度信息与全球卫星定位系统接收机自身的实时位置信息和实时速度信息之间的差值，并将该差值输入到常规的卡尔曼滤波算法，同时将微机械惯组自身的实时姿态信息也输入到常规的卡尔曼滤波算法；
[0009]此时数字信号处理器判断全球卫星定位系统实时的卫星个数信息，当得到全球卫星定位系统的卫星个数> 5颗星时，在不干预卡尔曼滤波算法量测噪声中的全球卫星定位系统速度噪声分量的情况下，将全球卫星定位系统速度噪声分量及步骤3中得到的差值和微机械惯组自身的实时姿态信息代入现有卡尔曼滤波算法内的最优估计算法，从而得到微机械惯组自身的实时位置信息误差、实时速度信息误差和实时姿态信息误差；并将上述微机械惯组自身的实时位置信息误差、实时速度信息误差和实时姿态信息误差输入微机械惯组进行导航参数校正，校正之后得到组合导航系统的导航参数；
[0010]当得到全球卫星定位系统的卫星个数由5颗星开始变为4颗星的5秒钟之内包括第5秒时，将常规卡尔曼滤波算法量测噪声中的全球卫星定位系统速度噪声分量调整为两倍的全球卫星定位系统速度噪声分量，然后将两倍的全球卫星定位系统速度噪声分量及步骤3中得到的差值和微机械惯组自身的实时姿态信息代入常规卡尔曼滤波算法内的最优估计算法，从而得到微机械惯组自身的实时位置信息误差、实时速度信息误差和实时姿态信息误差；将上述微机械惯组自身的实时位置信息误差、实时速度信息误差和实时姿态信息误差输入微机械惯组进行导航参数校正，校正之后得到组合导航系统的导航参数；
[0011]当得到全球卫星定位系统的卫星个数保持在4颗星5秒钟以上或者卫星个数恢复到5颗星及以上时，将两倍全球卫星定位系统速度噪声分量，恢复到原始全球卫星定位系统速度噪声分量，然后将原始的全球卫星定位系统速度噪声分量及步骤3中得到的差值和微机械惯组自身的实时姿态信息代入常规卡尔曼滤波算法内的最优估计算法，从而得到微机械惯组自身的实时位置信息误差、实时速度信息误差和实时姿态信息误差；将上述微机械惯组自身的实时位置信息误差、实时速度信息误差和实时姿态信息误差输入微机械惯组进行导航参数校正，校正之后得到组合导航系统的导航参数；
[0012]当得到全球卫星定位系统的卫星个数< 4颗星时，此时全球卫星定位系统无数据输出，常规卡尔曼滤波算法此时按全球卫星定位系统失锁处理。
[0013]本发明所设计能减小失锁速度误差对组合导航精度影响的系统，它包括全球卫星定位系统接收机和微机械惯组，其特征在于:它还包括可编程逻辑控制器、数字信号处理器和CAN (Controller Area Network,控制器局域网络)总线,其中，所述全球卫星定位系统接收机的信号输出端连接可编程逻辑控制器的信号输入端，微机械惯组的信号输出端连接数字信号处理器的信号输入端，所述可编程逻辑控制器的信号输出端连接数字信号处理器的信号输入端，所述数字信号处理器的信号输出端连接CAN总线。
[0014]进一步地，所述数字信号处理器中内嵌有卡尔曼滤波算法。
[0015]本发明的有益效果:
[0016]I)本发明通过判断全球卫星定位系统实时的卫星个数，并根据卫星个数来相应调整卡尔曼滤波算法量测噪声中的全球卫星定位系统速度噪声分量，从而最终实现减小全球卫星定位系统失锁速度误差对组合导航精度的影响。
[0017]2)本发明的实时性好、精度高，完全能满足动态性要求较高的场合。
[0018]3)本发明有效的解决了目前高动态全球卫星定位系统在失锁过程中引入全球卫星定位系统速度误差到卡尔曼滤波算法的问题，本发明根据卫星失锁的具体情况，在卡尔曼滤波算法中设置了相应的补偿，这样明显提高了失锁过程中组合导航系统的精度。[0019]4)本发明采用的卡尔曼滤波算法为现有成熟算法，利用该算法补偿微机械惯组误差的方法成熟可靠。
【专利附图】
【附图说明】
[0020]图1为本发明的使用状态结构示意图；
[0021]其中，I一全球卫星定位系统接收机、2—全球卫星定位系统、3—数字信号处理器、4一微机械惯组、5—可编程逻辑控制器、6- CAN总线。
【具体实施方式】
[0022]以下结合附图和具体实施例对本发明作进一步的详细说明:
[0023]如图1所示能减小失锁速度误差对组合导航精度影响的系统，它包括全球卫星定位系统接收机1、微机械惯组4、可编程逻辑控制器5、数字信号处理器3和CAN总线6，其中，所述全球卫星定位系统接收机I的信号输出端连接可编程逻辑控制器5的信号输入端，微机械惯组4的信号输出端连接数字信号处理器3的信号输入端，所述可编程逻辑控制器5的信号输出端连接数字信号处理器3的信号输入端，所述数字信号处理器3的信号输出端连接CAN总线6。所述数字信号处理器3中内嵌有卡尔曼滤波算法。
[0024]上述能减小失锁速度误差对组合导航精度影响的系统在工作时，它包括如下步骤:
[0025]步骤1:全球卫星定位系统接收机I实时监测全球卫星定位系统2的卫星个数信息，同时通过全球卫星定位系统2得到全球卫星定位系统接收机I自身的实时位置信息和实时速度信息，并且，将全球卫星定位系统2实时的卫星个数信息、全球卫星定位系统接收机I自身的实时位置信息和实时速度信息通过可编程逻辑控制器5传输给数字信号处理器3 ；
[0026]步骤2:微机械惯组4将自身的实时位置信息、实时速度信息和实时姿态信息也传输给数字信号处理器3 ；
[0027]步骤3:在数字信号处理器3中得到微机械惯组4自身的实时位置信息和实时速度信息与全球卫星定位系统接收机I自身的实时位置信息和实时速度信息之间的差值，并将该差值输入到常规的卡尔曼滤波算法，同时将微机械惯组4自身的实时姿态信息也输入到常规的卡尔曼滤波算法；此时下述卡尔曼滤波算法公式(4)中全球卫星定位系统速度噪声分量v(t)按照以下步骤进行相应设定:
[0028]此时数字信号处理器3判断全球卫星定位系统2实时的卫星个数信息，当得到全球卫星定位系统2的卫星个数> 5颗星时，在不干预卡尔曼滤波算法量测噪声中的全球卫星定位系统速度噪声分量V (t)的情况下，将全球卫星定位系统速度噪声分量V (t)及步骤3中得到的差值和微机械惯组4自身的实时姿态信息代入现有卡尔曼滤波算法内的最优估计算法，从而得到微机械惯组4自身的实时位置信息误差、实时速度信息误差和实时姿态信息误差；并将上述微机械惯组4自身的实时位置信息误差、实时速度信息误差和实时姿态信息误差输入微机械惯组4进行导航参数校正，校正之后得到组合导航系统的导航参数；
[0029]当得到全球卫星定位系统2的卫星个数由5颗星开始变为4颗星的5秒钟之内包括第5秒时(4颗星是全球卫星定位系统能否定位的临界状态，5秒钟是根据试验实际测得整体精度最好的时间长度)，将常规卡尔曼滤波算法量测噪声中的全球卫星定位系统速度噪声分量V (t)调整为两倍的全球卫星定位系统速度噪声分量V (t)，然后将两倍的全球卫星定位系统速度噪声分量V(t)及步骤3中得到的差值和微机械惯组4自身的实时姿态信息代入常规卡尔曼滤波算法内的最优估计算法，从而得到微机械惯组4自身的实时位置信息误差、实时速度信息误差和实时姿态信息误差；将上述微机械惯组4自身的实时位置信息误差、实时速度信息误差和实时姿态信息误差输入微机械惯组4进行导航参数校正，校正之后得到组合导航系统的导航参数；
[0030]当得到全球卫星定位系统2的卫星个数保持在4颗星5秒钟以上或者卫星个数恢复到5颗星及以上时(因为这种情况下，卫星定位系统已经重新获得精确速度位置信息)，将两倍全球卫星定位系统速度噪声分量V (t)，恢复到原始全球卫星定位系统速度噪声分量V (t)，然后将原始的全球卫星定位系统速度噪声分量V (t)及步骤3中得到的差值和微机械惯组4自身的实时姿态信息代入常规卡尔曼滤波算法内的最优估计算法，从而得到微机械惯组4自身的实时位置信息误差、实时速度信息误差和实时姿态信息误差；将上述微机械惯组4自身的实时位置信息误差、实时速度信息误差和实时姿态信息误差输入微机械惯组4进行导航参数校正，校正之后得到组合导航系统的导航参数；
[0031]当得到全球卫星定位系统2的卫星个数< 4颗星时(如3颗星，4颗星是卫星定位系统能否定位的临界状态)，此时全球卫星定位系统2无数据输出，常规卡尔曼滤波算法此时按全球卫星定位系统失锁处理。
[0032]通过以上步骤，可以根据卫星数情况判断全球卫星定位系统在失锁过程中的速度误差估计，从而设置正确的全球卫星定位系统速度噪声分量参数，估计出速度、位置误差，确保卡尔曼滤波在整个失锁过程都处在线性估计状态，得到最优的导航结果。若在失锁过程中，使用原始的全球卫星定位系统速度噪声分量参数，而速度的误差估计是变化的，卡尔曼滤波将按线性方程参数估计整个失锁过程的非线性状态，导航结果就会出现偏差。
·[0033]上述技术方案中，校正之后得到组合导航系统的导航参数由数字信号处理器3输出给CAN总线6。
[0034]具体实施时选取东北天坐标系为导航坐标系，将上述能减小失锁时速度误差对组合导航精度影响的装置安装于试验车上，以车体的右前上坐标系为载体坐标系建立姿态矩阵，微机械惯组4导航姿态解算采用双子样算法，同时进行速度更新和位置更新。卡尔曼滤波算法的状态向量选为:
[0035]Χ = \_φ'!! ?>ν"Τ ?μ i,\ ▽,.]( I )
[0036]其中，X为卡尔曼滤波算法状态向量，ΦηΤ为姿态误差，δνηΤ为速度误差，δ ρτ为位置误差W力陀螺常值漂移，Vf为加表偏置，T为矩阵转置符号。
[0037]相关误差模型如下:
r=-kx^,,+^<-c:kb+<)
[0038]紀=(/；；-(2< H-ω:?)χ V + V" X[?δω? + δω:)+ C；(Vbb + w:) C2；
φ = δνη[0039]其中，φη、&Β、孕分别为姿态误差、速度误差、位置误差微分，Φη为当前姿态，ω ω 为不同坐标系下地球转速，We、如:为不同坐标系下地球转速偏差，Cnb为姿态转移矩阵，<、丨分别表示微机械惯组4陀螺噪声和加表噪声，/ ,为加速度输
出，为加速度计偏差，δ vn为速度误差。
[0040]卡尔曼滤波算法量测矩阵为:
[0041]H(t) = [06X3 I6x6 06X6]T (3)
[0042]其中，H(t)为量测矩阵，06X3、06X6为全零矩阵，I6x6为对角单位矩阵。
[0043]则卡尔曼滤波算法状态空间方程为:
【权利要求】
1.一种减小失锁时速度误差对组合导航精度影响的方法，其特征在于，它包括如下步骤: 步骤1:全球卫星定位系统接收机(I)实时监测全球卫星定位系统(2)的卫星个数信息，同时通过全球卫星定位系统(2)得到全球卫星定位系统接收机(I)自身的实时位置信息和实时速度信息，并且，将全球卫星定位系统(2)实时的卫星个数信息、全球卫星定位系统接收机(I)自身的实时位置信息和实时速度信息通过可编程逻辑控制器(5)传输给数字信号处理器(3)； 步骤2:微机械惯组(4)将自身的实时位置信息、实时速度信息和实时姿态信息也传输给数字信号处理器(3); 步骤3:在数字信号处理器(3)中得到微机械惯组(4)自身的实时位置信息和实时速度信息与全球卫星定位系统接收机(I)自身的实时位置信息和实时速度信息之间的差值，并将该差值输入到常规的卡尔曼滤波算法，同时将微机械惯组(4)自身的实时姿态信息也输入到常规的卡尔曼滤波算法； 此时数字信号处理器(3)判断全球卫星定位系统(2)实时的卫星个数信息，当得到全球卫星定位系统(2)的卫星个数> 5颗星时，在不干预卡尔曼滤波算法量测噪声中的全球卫星定位系统速度噪声分量的情况下，将全球卫星定位系统速度噪声分量及步骤3中得到的差值和微机械惯组(4)自身的实时姿态信息代入现有卡尔曼滤波算法内的最优估计算法，从而得到微机械惯组(4)自身的实时位置信息误差、实时速度信息误差和实时姿态信息误差；并将上述微机械惯组(4)自身的实时位置信息误差、实时速度信息误差和实时姿态信息误差输入 微机械惯组(4)进行导航参数校正，校正之后得到组合导航系统的导航参数； 当得到全球卫星定位系统(2)的卫星个数由5颗星开始变为4颗星的5秒钟之内包括第5秒时，将常规卡尔曼滤波算法量测噪声中的全球卫星定位系统速度噪声分量调整为两倍的全球卫星定位系统速度噪声分量，然后将两倍的全球卫星定位系统速度噪声分量及步骤3中得到的差值和微机械惯组(4)自身的实时姿态信息代入常规卡尔曼滤波算法内的最优估计算法，从而得到微机械惯组(4)自身的实时位置信息误差、实时速度信息误差和实时姿态信息误差；将上述微机械惯组(4)自身的实时位置信息误差、实时速度信息误差和实时姿态信息误差输入微机械惯组(4)进行导航参数校正，校正之后得到组合导航系统的导航参数； 当得到全球卫星定位系统(2)的卫星个数保持在4颗星5秒钟以上或者卫星个数恢复到5颗星及以上时，将两倍全球卫星定位系统速度噪声分量，恢复到原始全球卫星定位系统速度噪声分量，然后将原始的全球卫星定位系统速度噪声分量及步骤3中得到的差值和微机械惯组(4)自身的实时姿态信息代入常规卡尔曼滤波算法内的最优估计算法，从而得到微机械惯组(4)自身的实时位置信息误差、实时速度信息误差和实时姿态信息误差；将上述微机械惯组(4)自身的实时位置信息误差、实时速度信息误差和实时姿态信息误差输入微机械惯组(4)进行导航参数校正，校正之后得到组合导航系统的导航参数； 当得到全球卫星定位系统(2)的卫星个数<4颗星时，此时全球卫星定位系统(2)无数据输出，常规卡尔曼滤波算法此时按全球卫星定位系统失锁处理。
2.一种为实现权利要求1所述方法而设计的能减小失锁速度误差对组合导航精度影响的系统，它包括全球卫星定位系统接收机(I)和微机械惯组(4)，其特征在于:它还包括可编程逻辑控制器(5)、数字信号处理器(3)和CAN总线(6)，其中，所述全球卫星定位系统接收机(I)的信号输出端连接可编程逻辑控制器(5)的信号输入端，微机械惯组(4)的信号输出端连接数字信号处理器(3)的信号输入端，所述可编程逻辑控制器(5)的信号输出端连接数字信号处理器(3)的信号输入端，所述数字信号处理器(3)的信号输出端连接CAN总线(6)。
3.根据权利要求2所述的能减小失锁时速度误差对组合导航精度影响的系统，其特征在于:所述数字信号处理器(3)中内嵌有卡尔曼滤波算法。
【文档编号】G01S19/47GK103592669SQ201310596586
【公开日】2014年2月19日 申请日期:2013年11月22日 优先权日:2013年11月22日
【发明者】何健伟, 吕江涛, 陆俊清 申请人:湖北航天技术研究院总体设计所