一种用于低速场景的航向快速初始化方法和导航系统与流程

1.本发明涉及卫星导航技术领域，特别是涉及一种用于低速场景的航向快速初始化方法和导航系统。


背景技术：

2.捷联惯性导航系统在进入导航解算状态前，需先进行初始对准，获得准确的初始信息，作为后续解算的基础。高精度惯导可以根据陀螺输出，计算航向角；而mems-imu由于陀螺噪声较大，地球自转角速度淹没在传感器噪声中，一般需要外部信息辅助确定初始航向角。
3.现有针对低精度imu的方案一般采用单一策略，例如仅使用载体速度或仅使用载体位置变化信息，计算初始航向角。由于仅采用单一策略，难以满足特定使用场景下的快速对准需求。当导航系统处在低速运动状态时，速度结果一般精度较低，会对航向角计算带来较大误差；依据位置信息计算航向角则对系统运动轨迹和时长有一定要求。部分技术考虑依靠卡尔曼滤波对初始航向角误差进行收敛，但在没有足够机动的情况下，收敛时间可能过长，影响组合解算精度。


技术实现要素：

4.本发明所要解决的技术问题是提供一种用于低速场景的航向快速初始化方法和导航系统，能够缩短全球导航卫星系统(gnss)/捷联惯性导航系统(sins)组合导航系统初始化时间，同时提高航向对准精度。
5.本发明解决其技术问题所采用的技术方案是：提供一种用于低速场景的航向快速初始化方法，包括以下步骤：
6.获取捷联惯性导航系统参数，并在所述捷联惯性导航系统参数满足预设条件时，进入惯性导航解算模式；
7.在gnss定位结果可靠时，判断导航系统当前所处的运动状态；
8.当所述导航系统当前所处的运动状态为静止状态时，根据加速度计输出计算姿态水平角；当所述导航系统当前所处的运动状态为转弯状态时，根据已得到的航向角信息对航向角进行收敛；当所述导航系统当前所处的运动状态为直行状态时，进入航向对准阶段；
9.所述航向对准阶段中，判断gnss的速度信息是否达到预设标准，当达到预设标准时，使用gnss的速度信息计算航向角及不确定度；否则，基于若干历元位移计算航向角及不确定度。
10.当gnss定位结果不可靠时，基于所述捷联惯性导航系统参数进行航向初始化。
11.所述在gnss定位结果可靠时，判断导航系统当前所处的运动状态具体为：
12.通过窗口时间内若干组加速度计的输出计算标准差，并根据所述标准差判断所述导航系统是否处于静止状态；
13.当所述导航系统不处于静止状态时，通过窗口时间内陀螺输出计算角度变化率，
并根据所述角度变化率判断所述导航系统是否处于转弯状态。
14.所述根据加速度计输出计算姿态水平角的计算公式为：其中，θ为俯仰角，γ为横滚角，和分别为加速度计x轴、y轴和z轴输出；g为重力加速度矢量在导航坐标系n系的z轴上的分量。
15.所述使用gnss的速度信息计算航向角及不确定度的计算公式为：其中，为航向角，为gnss的东向速度结果，为gnss的北向速度结果，为gnss的东向速度结果的标准差，为gnss的北向速度结果的标准差。
16.所述基于若干历元位移计算航向角及不确定度的计算公式为：其中，为航向角，为n个历元gnss的位移东向分量之和，为n个历元gnss的位移北向分量之和，为第i个历元gnss的东向位置结果标准差，为第i个历元gnss的北向位置结果标准差。
17.所述的用于低速场景的航向快速初始化方法还包括航向角检测模式，具体为：判断航向角是否达到稳定状态，当航向角未达到稳定状态是，对航向角进行持续检测，持续检测时采用三种独立的策略，分别为：gnss速度计算航向角、位移矢量计算航向角和大航向失准角策略计算航向角，所述三种独立的策略的权值随载体速度和运动状态变化进行动态调整，当三种独立的策略综合判断的航向角达到稳定状态时，调整航向信息，结束对航向角的检测。
18.当捷联惯性导航系统参数满足保存条件时，将所述捷联惯性导航系统参数进行保存，以便所述导航系统在下次上电时读取信息。
19.本发明解决其技术问题所采用的技术方案是：提供一种导航系统，包括处理器、存储器以及计算机程序，所述计算机程序被存储在所述存储器中，并且被配置成由所述处理器执行，所述计算机程序包括用于执行上述的用于低速场景的航向快速初始化方法。
20.有益效果
21.由于采用了上述的技术方案，本发明与现有技术相比，具有以下的优点和积极效果：本发明采用不同策略计算并监测航向角，缩短了初始对准时间，且在初始化精度较低时，进行组合解算并通过后台持续监测航向角，根据系统所处运动状态，动态调整三种独立策略的权重，综合评估航向角精度，从而更快完成误差收敛，提高前期结果的准确性。
附图说明
22.图1是本发明实施方式所依赖的导航系统软件框架示意图；
23.图2是本发明实施方式的流程图；
24.图3是自行车在低速骑行场景下的gnss结果轨迹图；
25.图4是gnss结果轨迹中前30秒内自行车的运动速度示意图。
具体实施方式
26.下面结合具体实施例，进一步阐述本发明。应理解，这些实施例仅用于说明本发明而不用于限制本发明的范围。此外应理解，在阅读了本发明讲授的内容之后，本领域技术人员可以对本发明作各种改动或修改，这些等价形式同样落于本技术所附权利要求书所限定的范围。
27.一般来说，相关技术方案中mems级惯导完成初始对准需要提供gnss位置、速度结果以及一定的机动需求，特别是要求载体速度达到一定阈值。它在gnss难以提供可靠结果，或载体不能满足机动条件时，无法完成初始对准。对于自行车、滑板车、无人割草机等载体，它们难以达到较高的运动速度，且使用场景中出现林荫、高楼遮挡的概率很高，因此现有导航系统很难满足快速初始化，从而影响系统进入组合解算阶段。
28.以下记载中“第一初始化”、“第二初始化”代表不同的初始化方式，并不限定顺序、准确度等其他属性。
29.惯性导航解算模式是以惯性解算得到的载体位置、速度和姿态为基础，根据不同的载体机动，结合不同的虚拟观测信息进行辅助，例如在载体静止时使用零速修正(zero velocity update,zupt)，在地面上运动时使用非完整性约束(non-holonomic constraint)。基于卡尔曼滤波器(kalman filter,kf)，建立状态方程和相应的量测方程，对状态量进行估计。
30.gnss/sins松组合解算模式是采用gnss和sins分别解算得到的位置和速度结果，基于卡尔曼滤波进行组合，在解算得到导航参数的同时，对惯导误差进行校正。gnss在稳定可用的状态下，为系统提供位置速度结果和相应的不确定度；在不可用时，gnss/sins松组合解算模式可退化为惯性导航解算模式。
31.本实施方式涉及一种用于低速场景的航向快速初始化方法，其依赖图1所示的导航系统实现，包括采集惯导数据的mems-imu，接收gnss信号并搭载算法单元的gnss定位芯片。其中，传感器主要提供gnss和imu二进制数据，驱动层按照固定频率读取传感器缓存空间的数据，并按照传感器寄存器的定义，将数据转换成特定形式。数据在算法层经过预处理后，用于初始对准及后续组合解算。当解算结果可靠时，实时录入存储模块。系统上电进行初始化时，若由于信号遮挡等原因，传感器层无gnss数据传入，可从存储模块读取导航参
数，快速进行初始化。如图2所示，其具体包括：
32.步骤1，当所述导航系统上电时，读取存储模块中的sins参数；
33.步骤2，当所述sins参数满足预设条件时，根据sins参数完成第一初始化，所述导航系统进入惯性导航解算模式；
34.其中，所述sins参数包括但不限于sins位置参数和姿态参数。
35.所述sins参数满足预设条件是指，所述sins参数中至少包括位置参数和姿态参数，且参数值在预设范围内。例如，位置参数的经纬度预设范围与地球经纬度范围一致，如果所读取的姿态参数不在此范围内，则不满足预设条件；姿态参数的预设范围为0-360
°
，如果所读取的姿态参数不在此范围内，则不满足预设条件。
36.进入惯性导航解算模式后，所述导航系统的初始化内容还包括：
37.步骤3，判断导航系统是否可以获得可靠的gnss定位结果，当gnss定位结果可靠时，根据gnss信息进行第二初始化。
38.所述gnss定位结果可靠为结果满足预设标准，例如结果的位置精度高于1m，速度精度高于0.3m/s。
39.步骤4，判断导航系统所处运动状态，其中，运动状态包括静止、直行和转弯。
40.判断导航系统所处运动状态的原理如下：
41.imu的加速度计输出在系统静止时趋于平稳，在运动时波动较大。所以依据加速度计原始输出的标准差(standard deviation,std)判断系统是否运动。若在窗口时间内有n组数据，则这n组数据的某1轴标准差可表示为
[0042][0043]
式中ai为第i个历元的加计单轴输出，μ为对应轴的n组数据的平均值。
[0044]
当标准差满足预设条件时认为系统静止，例如标准差小于0.05m/s2。
[0045]
系统的旋转运动可以反映在imu的陀螺输出中。采用窗口时间内陀螺输出计算的角度变化，即角度变化率，判断系统是否转弯。判断载体转弯需满足的条件可表示为
[0046][0047]
式中δ为角度变化率，n为窗口时间内的数据总量，g(i)为陀螺单轴输出，dt为采样间隔，t为窗口时长，例如1秒。
[0048]
当角度变化率满足预设条件时认为系统转弯，例如角度变化率大于2
°
/s。
[0049]
步骤5，当系统判断处在静止状态时，根据加速度计输出计算水平姿态角；当系统判断处在转弯状态时，若系统已得到航向角信息，可对航向角进行收敛；当系统判断处在直行状态时，进入航向对准阶段。
[0050]
在步骤5中，根据加速度计输出计算水平姿态角的原理如下：
[0051]
加速度计在静止状态下仅受到地球重力的作用，由于加速度计输出的是比力，所以静止时的输出在导航坐标系n系中的投影为-gn，这与imu所在坐标系b系中的投影可以形成如下关系式：
[0052][0053]
式中，fb为加速度计输出矢量，g为当地重力加速度矢量，为导航坐标系到惯导坐标系的旋转矩阵。
[0054]
导航坐标系n系取当地水平坐标系，原点位于载体质心，坐标系的x轴沿子午线指向地理北方向，y轴沿当地纬线指东，z轴沿地理垂线指向地向，并与x轴、y轴构成右手直角坐标系。
[0055]
惯导所在坐标系b系坐标原点位于惯性器件质心，x轴指向载体前进方向，y轴指向载体右侧，z轴指向载体地向，三轴遵循右手准则。
[0056]
所以静止时惯导输出与重力的关系可表示为：
[0057][0058]
其中，和分别为加速度计x轴、y轴和z轴输出；g为重力加速度矢量在导航坐标系n系的z轴上的分量。
[0059]
旋转矩阵与欧拉角存在转换关系，可表示为：
[0060][0061]
其中，θ、γ分别为航向角、俯仰角、横滚角。
[0062]
所以静止时惯导输出与重力的关系可表示为欧拉角形式：
[0063][0064]
解得惯导的俯仰角和横滚角：
[0065][0066][0067]
进入航向对准阶段后，所述导航系统的初始化内容还包括：
[0068]
步骤6，判断gnss速度是否达到预设标准，当达到预设标准时，使用gnss提供的速度计算航向角及不确定度；
[0069]
当未达到预设标准时，基于若干历元位移计算航向角及不确定度。
[0070]
达到预设标准是指gnss速度结果反映的载体速度达到一定阈值，例如速度超过1m/s。
[0071]
gnss速度计算航向角及不确定度的原理如下：
δvd]
t
，由北东地三个方向的速度误差组成；
[0089]
位置误差方程表示为
[0090][0091]
式中，δrc为计算系c系下的位置误差，计算系c系为在sins解算位置处的当地水平坐标系；δvc定义为速度误差；为计算系相对于地固系的角速度在计算系的投影；vc为c系的速度。而(vc×
)为vc的反对称矩阵。(vc×
)
e
由下式计算：
[0092][0093]
式中，和为速度的前两项分量。
[0094]
速度误差方程表示为：
[0095][0096]
式中，为惯导所在坐标系到平台系p系的方向余弦矩阵，平台系p系为sins直接计算获得的导航系；gc和δgc分别表示c系下的重力加速度和其误差，而重力误差表示如下：
[0097][0098]
式中，rn,rm分别表示卯酉圈和子午圈曲率半径。
[0099]
δfb和分别为b系下加速度计和陀螺仪的输出误差；是地球自转角速度在c系的投影，和分别为北方向和东方向的分量；为c系速度在地方向的分量。和分别为c系相对于e系的角速度在c的投影矢量，在东和北方向的分量。fc(ψ
′
,vc)定义如下:
[0100][0101]
姿态误差方程表示为：
[0102][0103]
式中，为计算系c相对惯性系的角速度在c系的投影，和为其前两项的分
量。
[0104]
步骤8，在导航系统组合解算过程中，当sins参数满足保存条件时，实时将sins参数保存至存储模块内，以便下次系统上电时读取初始信息；
[0105]
满足保存条件指认为sins参数可靠，例如滤波结果的位置标准差小于1m，速度标准差小于0.3m/s，姿态标准差小于1
°
。
[0106]
本发明的实验效果部分示例，如图3和图4所示。图3是一段自行车在低速骑行场景下的gnss结果轨迹，“s”标注点为轨迹起点，“e”标注点为轨迹终点，每个轨迹点之间的采样间隔为1秒。在起点静止几个历元后，自行车开始运动。图4显示了轨迹前30秒内自行车的运动速度，完全处在较低的速度状态下。一般根据速度计算航向角的方法难以满足阈值要求，若强行降低阈值，航向角误差可能很大，本实验的误差达到32
°
。若采用累积位移计算航向角的方法，误差降至4
°
，且对准时间仍然较短。说明根据不同策略计算航向角的必要性。
[0107]
本发明还涉及一种导航系统，包括处理器、存储器以及计算机程序，所述计算机程序被存储在所述存储器中，并且被配置成由所述处理器执行，所述计算机程序包括用于执行上述的用于低速场景的航向快速初始化方法。
[0108]
不难发现，本发明采用不同策略计算并监测航向角，缩短了初始对准时间，且在初始化精度较低时，进行组合解算并通过后台持续监测航向角，根据系统所处运动状态，动态调整三种独立策略的权重，综合评估航向角精度，从而更快完成误差收敛，提高前期结果的准确性。