一种多源融合导航系统的导航方法和装置

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）式进行计算， 为对应的 的测量值。 4.根据权利要求3所述的方法，其特征在于所述步骤3还包括：当检测到载体零速时，即载体的车体坐标系m系下三个方向的速度为零，即 时，INS在测量时刻i由IMU测量值实际计算得到的载体速度 不为零，b系及e系下的速度矢量存在如下关系： （13）其中，车辆在 时刻e系下的速度由i-1时刻的速度计算得到；载体在导航坐标系n系下的姿态矢量为 ，其中 、 、 分别为 时刻载体的航向角、横滚角、俯仰角，n系姿态矢量 到e系姿态矢量 的转换关系为： （14）其中， 表示载体在i时刻由e系变换到n系的旋转矩阵，在时间间隔 内航向角的变化量 与姿态矢量各元素的关系为： （15）其中 为载体在惯性系的角速度在 内的变化量在b系下的投影，当载体静止时，在航向角上的变化量为零，由上式约束IMU输出的角速度零偏值；由公式（13）~（15）可以得到载体静止时，INS节点的状态方程为： （16）其中 为对应的 的测量值， 表示对括号内的变量进行计算的非线性函数，上标ZUPT_INS表示载体静止时，INS节点的函数；选取一颗观测质量最好的卫星，在测量时刻得到该卫星的伪距率测量方程为： （17）其中 为接收机与卫星之间的几何距离变化率，卫星的钟差变化率由星历计算得到， 为测量残差， 为包括对流层电离层在内的传播误差随时间的变化率，当用户静止时，接收机与卫星之间的几何距离变化率 是卫星的速度在卫星与接收机径向方向的大小，可得载体静止时 GNSS 节点对应的测量方程为： （18）其中符号 中， h 表示一个非线性函数，上标 ZUPT_GNSS 表示载体静止时， GNSS 节点的函数， 为对应的 的测量值；当系统检测到载体处于静止状态时，因子图中不增加新的因子节点，对应矩阵的大小维持不变，只使用新测量的信息更新系统状态。 5.根据权利要求4所述的方法，其特征在于所述步骤4还包括：测量时刻 ，因子图系统状态量 的最大后验概率值 的表达式为： （19），其中， ，表示求矩阵 的马氏距离的平方，为 对应的协方差矩阵；当检测到载体处于静止时，利用零速修正因子节点进行计算状态量的最大后验概率值 ： （20）得到系统导航解。 6.根据权利要求5所述的方法，其特征在于所述载体为车辆。 7.一种多源融合导航系统的导航装置，其特征在于包括：由全球卫星导航系统GNSS接收机、惯性导航系统INS和激光雷达LiDAR组合的导航系统，其中，所述INS包括测量惯性单元IMU;处理器和存储器，所述处理器与所述存储器通信连接，所述存储器存储有计算机指令，所述处理器运行所述计算机指令时执行权利要求1～6中任一项所述的方法中的步骤。