本发明属于惯性导航与卫星导航技术领域，具体涉及一种低速GNSS/MEMS组合导航系统的初始化方法。
背景技术：
北斗卫星导航系统是与美国GPS、俄罗斯GLONASS以及欧洲GALILEO并驾齐驱的覆盖全球的星基导航系统(GNSS)。随着北斗三期布网的逐步完善，北斗系统正逐步普及应用于海陆空各类军民载体。卫星导航系统的显著特点是直接提供绝对定位坐标，且误差不随时间积累，但存在无线电信号易受遮挡和干扰的不足。惯性导航系统(Inertial Navigation System,INS)能够自主隐蔽地进行连续的三维空间导航与测姿，不存在信号的电磁干扰，能跟踪和反映运动载体的机动且输出平稳。但惯性导航的不足在于其导航误差会随时间积累，当外界不给予其修正时，误差增长无界。
微机电系统MEMS(Micro Electronic Mechanical System)是随着半导体集成电路微细加工技术和超精密机械加工技术的发展而发展起来的集微型传感器、执行器、信号处理与控制电路、接口电路、通信和电源一体的微型机电系统。MEMS惯性传感器主要指硅微加速度计和硅微陀螺仪这两种惯性传感器，用于测量运动载体的线加速度和旋转角速度。MEMS惯性传感器继承了传统惯性传感器的完全自主性、保密性强、不存在信号电磁干扰等特点，同时又具有尺寸小、重量轻、成本低、功耗小、可靠性高等传统惯性传感器无法比拟的优点。但受制造工艺所限，其用作导航定位时，误差随时间快速积累。
GNSS与惯性传感器相结合的组合导航系统充分发挥了二者的优势，形成有机互补。优势在于：惯导在短时间内能够保持较高的精度，且由于其不受外界工作环境影响，可以补偿GNSS定位过程中产生的随机误差；同时，GNSS提供的绝对定位测速信息可以补偿惯导随时间累积的误差，而保证长距离运行中的测量精度；短时间内惯导的动态信息可以有助于改善GNSS高动态及有干扰环境中的信号失锁和跳变问题。
惯性导航系统通过加速度计测量载体的三维的线性运动，陀螺测量载体的三维角运动，进而在导航坐标系上进行积分来获取速度、位置和姿态。惯导在导航定位中的使用需要完成初始化，然后在初始信息的基础上进行积分推算来获取当前时刻的导航结果。初始化信息包括三维位置、速度以及姿态。初始信息的误差成为惯导系统的主要误差源之一，该误差随时间传递，且经积分后误差将无界积累。
高精度高成本的惯导系统可以通过高精度陀螺来感受地球自转，并结合重力场在车辆载体坐标系上的投影自主完成初始化。但是MEMS陀螺仪精度低，15°/小时的地球自转角速率完全湮没于陀螺的噪声中，惯导系统无法完成自主初始化，因此需要借助于GNSS速度信息的辅助。在低速运动的环境下(比如＜3m/s)，GNSS接收机所观测到的卫星与载体之间的多普勒频移相对运动不明显，因此根据多普勒所计算的GNSS速度误差较大，MEMS惯导的初始化难度增加。
技术实现要素：
本发明的目的是提供一种低速GNSS/MEMS组合导航系统的初始化方法，该方法不要求车辆的行驶速度门限，从启动即可开始滤波估计，可以大大缩短系统的初始化准备时间，使低速状态下的组合导航系统初始化成为可能，且不受载体运动的限制。
本发明所采用的技术方案是，低速GNSS/MEMS组合导航系统的初始化方法，具体按照以下步骤实施：利用GNSS的位置确定组合系统的初始位置，利用重力场投影及GNSS推导得到的总比力的模来确定水平初始姿态，即俯仰角和横滚角；将GNSS所观测到的低速运动划分为若干区间，建立与区间数量相同的单维滤波器族，并执行时间预测回路以及量测更新回路的计算，每个滤波器以初始航向误差为滤波器估计状态，以速度为滤波器观测量；通过比较滤波结果确定估计误差方差最小的航向作为该时刻的初始值，当连续若干时刻航向由同一区间的滤波器确定时，则初始化航向结束，该滤波器所确定的初始航向即为组合导肮系统的初始航向；初始水平姿态与初始航向所构建的方向余弦矩阵即为组合导航系统的三维初始姿态信息。
本发明的特点还在于，
MEMS惯性传感器为三个相互正交的MEMS加速度计和三个相互正交的MEMS陀螺仪，分别用于测量载体的线加速度和角速度；所述的GNSS包括北斗、GPS、格洛纳斯、伽利略。
GNSS的位置由GNSS定位结果直接获取，若为静态状态，则做平均平滑处理。
利用重力场投影及GNSS推导得到的总比力的模来确定水平初始姿态，即俯仰角和横滚角，其确定方法如下：
在静态或匀速情况下，水平初始姿态可由如下算法计算：
其中，r，p分别为水平安装角度的横滚角和俯仰角在第k时刻的计算分量；分别为对应的第k时刻的滤波估计量；Nr,Np分别为对应的滤波增益，选取为0到1之间的常值；fK，i分别为第k时刻的第i，i＝0,1,2，轴的加速度计输出；i＝0,1,2，分别为第i轴的加速度计零偏估计值，该值混合着重力场沿着各个轴向的投影；
在动态情况下，所述由GNSS推导得到的总比力的模的算法如下：
其中aGNSS，k，i为第k时刻GNSS沿导航坐标系第i轴的加速度；VGNSS，k，i和VGNSS，k，i分别为第k+1及第k时刻GNSS沿导航坐标系第i轴的速度；Tk+1-Tk为k到k+1时刻的时间间隔。
当载体处于匀速运动时，水平初始姿态可由如下算法计算：
时间预测回路以及量测更新回路的计算，具体如下：
(1)时间预测回路：
Pk+1，k，i＝Pk，i+Qk，i
Kk+1，i＝Pk+1，k，i/(Pk+1，k，i+Rk，i)
(2)量测更新回路：
δxk+1，i＝δxk，i+Kk+1，i·Zk，i
Pk+1，i＝(1-Kk+1，i)2Pk+1，k，i
其中，Pk+1，i和Pk，i分别为第k+1和第k时刻第i区间的滤波器状态方差；Pk+1，k，i为根据Pk，i对第k+1时刻第i区间的滤波器状态方差的估计；Qk，i为第k时刻第i区间的系统噪声；Rk+1，i为第k时刻第i区间的量测噪声；Kk+1，i为第k+1时刻的滤波增益；为第k+1时刻初始航向角的误差；zk，i为滤波信息；
(3)由GNSS根据速度所推算得到的航向角，计算如下：
zGNSS，k，i＝tan-1(VGNSS，k，i，e/VGNSS，k，i，n)
zGNSS，k，i为第k时刻第i区间的GNSS航向角；VGNSS，k，i，e与VGNSS，k，i，e分别表示第k时刻第i区间GNSS的东向和北向速度。
比较滤波结果确定估计误差方差最小的航向作为该时刻的初始值的具体准则如下：
Jm＝min{Pk+1，1，Pk+1，2，…，Pk+1，n}n＝1，2，…6
其中，Pk+1，i分别为第k+1时刻第i区间的滤波器状态方差；Jm为准则函数，m为最小方差对应的区间编号，当连续若干次最小方差出现于同一区间时，则初始航向估计结束。
方向余弦矩阵的矩阵元素以刚体欧拉角方式的三维旋转构成，旋转顺序为沿横滚角-俯仰角-航向角。
本发明的有益效果是：该方法通过在线估计算法，借助于重力场投影来确定水平初始姿态，并将其与安装误差角度解耦；针对于地面车辆的低速运动环境，将GNSS所观测到的低速运动划分为若干区间，建立与区间数量相同的单维滤波器族，从而通过滤波结果确定估计误差方差最小的航向作为初始值。结合GNSS的位置、速度以及所估计的姿态来完成组合导航系统的初始化。在该方法中并不要求车辆的行驶速度门限，从启动即可开始滤波估计，因此可以大大缩短系统的初始化准备时间，使低速状态下的组合导航系统初始化成为可能，且不受载体运动的限制。该方法计算量小，实时性强，有助于实现GNSS/MEMS组合系统在各种地面载体上的批量化应用。
附图说明
图1是本发明的系统流程图。
具体实施方式
下面结合附图和具体实施方式对本发明进行详细说明。
本发明提供一种低速GNSS/MEMS组合导航系统的初始化方法，系统流程图如图1所示，载体涉及但不限于自行车、电动车、低速农业车辆、环卫车辆、施工工程车辆、无人送货车、自提柜等。该技术在线估计组合导航系统的初始化信息，具体按照以下步骤实施：利用GNSS的位置确定组合系统的初始位置，利用重力场投影及GNSS推导得到的总比力的模来确定水平初始姿态，即俯仰角和横滚角；将GNSS所观测到的低速运动划分为若干区间，建立与区间数量相同的单维滤波器族，并执行时间预测回路以及量测更新回路的计算，每个滤波器以初始航向误差为滤波器估计状态，以速度为滤波器观测量；通过比较滤波结果确定估计误差方差最小的航向作为该时刻的初始值，当连续若干时刻航向由同一区间的滤波器确定时，则初始化航向结束，该滤波器所确定的初始航向即为组合导肮系统的初始航向；初始水平姿态与初始航向所构建的方向余弦矩阵即为组合导航系统的三维初始姿态信息。
MEMS是指微机电系统，MEMS惯性传感器是构成MEMS惯性导航系统的核心部件，包括MEMS加速度计和MEMS陀螺仪。
MEMS IMU指由MEMS惯性传感器构成的测量单元，MEMS惯性传感器为三个相互正交的MEMS加速度计和三个相互正交的MEMS陀螺仪，分别用于测量载体的线加速度和角速度；所述的GNSS包括北斗、GPS、格洛纳斯、伽利略。
GNSS的位置由GNSS定位结果直接获取，若为静态状态，则做平均平滑处理。
第一步，利用重力场投影及GNSS推导得到的总比力的模来确定水平初始姿态，即俯仰角和横滚角，其确定方法如下：
在静态或匀速情况下，水平初始姿态可由如下算法计算：
其中，r，p分别为水平安装角度的横滚角和俯仰角在第k时刻的计算分量；分别为对应的第k时刻的滤波估计量；Nr,Np分别为对应的滤波增益，选取为0到1之间的常值；fk，i分别为第k时刻的第i，i＝0,1,2，轴的加速度计输出；i＝0,1,2，分别为第i轴的加速度计零偏估计值，该值混合着重力场沿着各个轴向的投影；
在动态情况下，所述由GNSS推导得到的总比力的模的算法如下：
其中aGNSS，k，i为第k时刻GNSS沿导航坐标系第i轴的加速度；VGNSS，k+1，i和VGNSS，k，i分别为第k+1及第k时刻GNSS沿导航坐标系第i轴的速度；Tk+1-Tk为k到k+1时刻的时间间隔；
当载体处于匀速运动时，水平初始姿态可由如下算法计算：
第二步，将GNSS所观测到的低速运动划分为若干区间，具体实施为：
表1：依据GNSS速度及其不确定性划分运动状态区间
需要指出的是速度及其不确定性门限的选择可以根据数据做调整，上述所列数值可以选取它值。
第三步：建立与区间对应的滤波器，并执行时间预测回路以及量测更新回路的计算，与区间对应的滤波器算法分为时间预测回路以及量测更新回路，具体如下：
(1)时间预测回路：
Pk+1，k，i二Pk，i+Qk，i
Kk+1，i二Pk+1，k，i/(Pk+1，k，i+Rk，i)
(2)量测更新回路：
δxk+1，i＝δxk，i+Kk+1，i·zk，i
Pk+1，i＝(1-Kk+1，i)2Pk+1，k，i
其中，Pk+1，i和Pk，i分别为第k+1和第k时刻第i区间的滤波器状态方差；Pk+1，k，i为根据Pk，i对第k+1时刻第i区间的滤波器状态方差的估计；Qk，i为第k时刻第i区间的系统噪声；Rk+1，i为第k时刻第i区间的量测噪声；Kk+1，i为第k+1时刻的滤波增益；为第k+1时刻初始航向角的误差；zk，i为滤波信息；
时间预测回路每一时刻保持运行，但量测更新回路只在GNSS所推算的速度落入相应区间时进行。
(3)由GNSS根据速度所推算得到的航向角，计算如下：
zGNSS，k，i＝tan-1(VGNSS，k，i，e/VGNSS，k，i，n)
zGNSS，k，i为第k时刻第i区间的GNSS航向角；VGNSS，k，i，e与VGNSS，k，i，n分别表示第k时刻第i区间GNSS的东向(e)和北向(n)速度。
第四步，比较滤波结果确定估计误差方差最小的航向作为该时刻的初始值的具体准则如下：
Jm＝nnn{Pk+1，1，Pk+1，2，…，Pk+1，n}n＝1，2，…6
其中，Pk+1，i分别为第k+1时刻第i区间的滤波器状态方差；Jm为准则函数，m为最小方差对应的区间编号，当连续若干次最小方差出现于同一区间时，则初始航向估计结束。
第五步：建立载体的机体坐标系与系统的导航坐标系的方向余弦矩阵，三维初始姿态信息构成的方向余弦矩阵建立了机体坐标系到导航坐标系的转换关系，其矩阵元素以刚体欧拉角方式的三维旋转构成，旋转顺序为沿横滚角—俯仰角—航向角。
本发明的优点是：该方法通过在线估计算法，借助于重力场投影来确定水平初始姿态，并将其与安装误差角度解耦；针对于地面车辆的低速运动环境，将GNSS所观测到的低速运动划分为若干区间，建立与区间数量相同的单维滤波器族，从而通过滤波结果确定估计误差方差最小的航向作为初始值。结合GNSS的位置、速度以及所估计的姿态来完成组合导航系统的初始化。在该方法中并不要求车辆的行驶速度门限，从启动即可开始滤波估计，因此可以大大缩短系统的初始化准备时间，使低速状态下的组合导航系统初始化成为可能，且不受载体运动的限制。该方法计算量小，实时性强，有助于实现GNSS/MEMS组合系统在各种地面载体上的批量化应用。