[发明专利]一种多源融合导航系统的导航方法和装置

权利要求书

1.一种多源融合导航系统的导航方法，其特征在于包括：

步骤1：构造零速检测统计量，利用所述零速检测统计量判断载体是否处于静止状态；

步骤2：构造因子图，所述因子图用于将多源融合导航系统的各传感器的观测量建立当前系统状态和前一状态的联系并用于优化计算；

步骤3：基于步骤1的判断结果，构造零速修正因子节点，所述零速修正因子节点用于计算载体的状态量，以降低因子图的存储和简化计算；

步骤4：基于构造的所述零速修正因子节点，计算因子图，得到系统导航解，所述导航解提供载体的三维位置、三维速度、三维姿态角信息、载体本地时钟误差、时钟误差漂移率信息；

其中，所述步骤1还包括：

利用LiDAR进行点云匹配后输出的相对位姿参数作为检测统计量，构造广义似然比公式，得到如下零速检测公式：

其中，为第n次检测的检测统计量集合，所述检测统计量是LiDAR在各个测量时刻的测量值，具体包括由点云匹配后得到的相对位置和相对位姿变化量，为检测数据个数， 和分别表示第m时刻相对位置和相对位姿变化量的取值，为实单位四元数，上标符号“T”表示矩阵或矢量的转置运算， 为检测窗口长度，表示的二范数运算，表示的二范数运算， 表示相对位置的测量方差， 表示相对位姿变化量的测量方差；

当所述检测统计量小于设定的门限时认为载体处于静止状态。

2.根据权利要求1所述的方法，其特征在于所述多源融合导航系统为全球卫星导航系统GNSS接收机、惯性导航系统INS和激光雷达LiDAR组合的导航系统，其中，所述INS包括测量惯性单元IMU。

3.根据权利要求2所述的方法，其特征在于所述步骤2还包括：

步骤2.1、计算过程模型因子节点，所述过程模型因子节点为INS节点，具体包括：

在测量时刻，所述IMU在载体坐标系b系的输出测量值为加速度和角度，令IMU的测量值；

为IMU在b系三个坐标轴上的加速度计测量偏差， 为IMU在b系三个坐标轴上的陀螺仪测量偏差，IMU输出测量值的时间间隔为；INS因子节点的更新周期为时间间隔，且，其中，，且时间间隔的大小与GNSS接收机输出测量值的时间间隔一致，在时间间隔内对IMU测量值在b系下的速度、姿态角变化量分别进行预积分，得到速度、姿态角在时间间隔T内的变化量和：

进一步得到测量时刻时，多源融合导航系统的速度估计值、位置估计值、姿态角估计值、加速度计测量偏差估计值和陀螺仪测量偏差估计值分别按照如下公式计算：

（2）

（3）

（4）

（5）

（6）

其中, 、、分别为多源融合导航系统在地球地固坐标系e系下测量时刻i时的位置、速度、姿态角; 表示从测量时刻i到测量时刻，由b系变换到e系的旋转矩阵， 表示重力矢量，在时间间隔T内为一个常量，测量时刻i到测量时刻的时间间隔是T；

过程模型因子节点的状态量为，，所述为包括15维状态变量的空间，由公式（2）~（6）计算得到测量时刻的INS节点的状态量的估计值，将上述公式（2）~（6）合成一个非线性函数，使用符号来表示，h表示一个非线性函数，上标INS表示INS节点的函数，得到INS节点的状态方程为：

（7）；

步骤2.2、构造测量模型因子节点，所述测量模型因子节点为GNSS节点，具体包括：

测量时刻i，GNSS接收机的测量值，其中，为接收机在测量时刻i对第j颗可见卫星的伪距测量值，，为对应的伪距率，J为可见卫星的最大数量；

由导航电文得到在测量时刻i第j颗卫星在e系下的位置及对应的速度，则测量时刻所有可见卫星的位置矢量和速度矢量分别为和；

GNSS接收机在测量时刻的状态量为，

，其中，所为包括8维状态变量的空间，为GNSS接收机的本地时间和GNSS系统时间之间的差，为的变化率；

GNSS接收机的测量方程，也就是GNSS节点的测量方程为：

（8）

其中，符号中，h表示一个非线性函数，上标GNSS表示GNSS节点的函数，c表示光速，表示接收机与卫星在视线方向上的位置大小，表示接收机与卫星在视线方向上的速度大小，计算的结果即为估计的伪距与伪距率；

步骤2.3、构造LiDAR因子节点，具体包括：

对于测量时刻接收到的LiDAR测量数据，首先提取其线特征点和面特征点，得到特征集合和，上标e和p分别表示线特征和面特征，利用估计的包含旋转和平移的坐标变换矩阵对其进行坐标变换，将特征集合和由b系转换到世界坐标系w系，则LiDAR在测量时刻i的测量值为，其中为特征点的集合，为特征线的集合；

对于测量时刻i的LiDAR测量数据，w系下的第k个线、面特征点、到上一时刻线、面特征点、的距离、分别表示如下：

（9）

（10）

其中为对矢量的取模运算，u、v、w为点附近的线、面特征点的编号，也就是对于i时刻的第u个线、面特征点，分别为、，对于i-1时刻的第u个线、面特征点分别为、，对于i、i-1时刻第v、w个线、面特征点的表示与第u个线、面特征点类似，线特征点属于集合，面特征点属于集合；通过高斯-牛顿方法得到：

（11）

即最小化i时刻所有特征点到上一时刻特征点的距离，由此将得到相对位姿变换矩阵，定义测量时刻i的LiDAR的状态量

，且，其中， 为包含6维状态变量的空间；

因此LiDAR的测量方程为：

（12）

其中符号中，h表示一个非线性函数，上标Lidar表示Lidar节点的函数，为一个非线性函数，自变量是和，表示利用（9）~（11）式进行计算，为对应的的测量值。