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

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

Info

Publication number
CN115685292A
CN115685292A CN202310010536.0A CN202310010536A CN115685292A CN 115685292 A CN115685292 A CN 115685292A CN 202310010536 A CN202310010536 A CN 202310010536A CN 115685292 A CN115685292 A CN 115685292A
Authority
CN
China
Prior art keywords
measurement
time
node
carrier
speed
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Granted
Application number
CN202310010536.0A
Other languages
English (en)
Other versions
CN115685292B (zh
Inventor
王怡晨
刘小汇
文超
刘瀛翔
李宗楠
徐子晨
嵇志敏
于美婷
许可
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
National University of Defense Technology
Original Assignee
National University of Defense Technology
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by National University of Defense Technology filed Critical National University of Defense Technology
Priority to CN202310010536.0A priority Critical patent/CN115685292B/zh
Publication of CN115685292A publication Critical patent/CN115685292A/zh
Application granted granted Critical
Publication of CN115685292B publication Critical patent/CN115685292B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Abstract

本发明公开了一种多源融合导航系统的导航方法和装置，包括：构造零速检测统计量；构造因子图；构造零速修正因子节点；因子图的计算。本发明利用输出的位姿变化量作为检测量进行广义似然比零速检测，检测到载体处于零速时，不增加新的因子节点，有效减轻了因子图的存储量和计算量；多源融合导航系统的测量值仍然对系统状态进行约束和更新，保证了零速时系统状态值估计的正确性。

Description

一种多源融合导航系统的导航方法和装置
技术领域
本发明涉及卫星导航技术领域，特别是涉及GNSS（Global Navigation SatelliteSystem，全球卫星导航系统）与INS（Inertial Navigation System，惯性导航系统）、激光雷达（LiDAR）多源融合导航系统组合模式下，利用测量信号进行导航的方法和装置，特别适用于城市中行驶车辆的自动驾驶辅助导航设备中。
背景技术
车载导航系统的定位技术是实现导航和自动驾驶关键技术之一。多源导航系统通过融合多种传感器信息可有效提高系统的整体性能和环境适应能力，能实现卫星导航信号受干扰、遮挡等各类复杂环境下的稳定可靠定位。在多源融合算法方面，因子图能够解决多源融合系统中测量值的异步传输、即插即用、多速率数据的融合问题，同时也能解决状态方程或观测方程非线性的问题，具有良好的扩展性和灵活性，是近年研究的热点。但是，随着观测量的不断增加，因子图中产生的变量节点也会逐渐增多，这给实时系统带来巨大的计算和存储负担，对处理器的处理能力和存储能力都是重大挑战。
发明内容
为解决现有技术中存在的问题，本发明在扩展的增量平滑与建图方法（Incremental Smoothing and Mapping 2 , iSAM2）iSAM2的基础上，结合GNSS、INS和LiDAR构成的多源融合导航系统，将因子图算法进行优化。其中，因子图的概念于2001年由Kschischang等人提出，它是概率图模型的一种表示方式，用来表示全局函数和局部函数关系的模型图，被广泛应用于人工智能、信号处理、数字通信等领域，能对各种复杂概率问题进行求解。本发明采用因子图建模，其主要目的是将复杂的概率问题进行因式化分解，即将复杂的全局函数分解为多个局部函数相乘的形式，当已知局部函数之间的相关性时，只需要分析每个局部函数就可以得到全局复杂概率问题的解。因子图中含有因子节点与变量节点，两种节点通过局部函数相连构成双向图。因子图通过各传感器的观测量建立当前系统状态和前一状态的联系，并构造代价函数，通过求解系统状态量后验概率的全局或者局部最大值，获得当前状态最优估计值。因子图能够解决多源融合系统中测量值的异步传输、即插即用、多速率数据的融合问题，同时也能解决状态方程或观测方程非线性的问题，具有良好的扩展性和灵活性。
一方面，本发明提出一种多源融合导航系统的导航方法，包括：步骤1：构造零速检测统计量，利用零速检测统计量判断载体是否处于静止状态；步骤2：构造因子图，因子图用于将多源融合导航系统的各传感器的观测量建立当前系统状态和前一状态的联系并用于优化计算；步骤3：基于步骤1的判断结果，构造零速修正因子节点，零速修正因子节点用于计算载体的状态量，以降低因子图的存储和简化计算；步骤4：基于构造的零速修正因子节点，计算因子图，得到系统导航解，导航解提供载体的三维位置、三维速度、三维姿态角信息、载体本地时钟误差、时钟误差漂移率信息。
进一步的，多源融合导航系统为全球卫星导航系统GNSS接收机、惯性导航系统INS和激光雷达LiDAR组合的导航系统，其中，INS包括测量惯性单元IMU。
进一步的，步骤1还包括：
利用LiDAR进行点云匹配后输出的相对位姿参数作为检测统计量，构造广义似然比公式，得到如下零速检测公式：
Figure 583122DEST_PATH_IMAGE001
（1）
其中，
Figure 876700DEST_PATH_IMAGE002
为第n次检测的检测统计量集合，所述检测统计量是LiDAR在各个测量时刻的测量值，具体包括由点云匹配后得到的相对位置
Figure 855020DEST_PATH_IMAGE003
和相对位姿变化量
Figure 575851DEST_PATH_IMAGE004
，N为检测数据个数，
Figure 272412DEST_PATH_IMAGE005
和
Figure 369681DEST_PATH_IMAGE006
分别表示第m时刻相对位置
Figure 936928DEST_PATH_IMAGE003
和相对位姿变化量
Figure 94240DEST_PATH_IMAGE004
的取值，
Figure 746939DEST_PATH_IMAGE007
为实单位四元数
Figure 647898DEST_PATH_IMAGE008
，上标符号“T”表示矩阵或矢量的转置运算，
Figure 538494DEST_PATH_IMAGE009
为检测窗口长度，
Figure 663445DEST_PATH_IMAGE010
表示
Figure 537860DEST_PATH_IMAGE011
的二范数运算，
Figure 242511DEST_PATH_IMAGE012
表示
Figure 784351DEST_PATH_IMAGE013
的二范数运算，
Figure 17886DEST_PATH_IMAGE014
表示相对位置
Figure 910755DEST_PATH_IMAGE015
的测量方差，
Figure 887939DEST_PATH_IMAGE016
表示相对位姿变化量
Figure 549864DEST_PATH_IMAGE004
的测量方差。
当检测统计量
Figure 219880DEST_PATH_IMAGE017
小于设定的门限
Figure 68887DEST_PATH_IMAGE018
时认为载体处于静止状态。
进一步的，步骤2还包括：
步骤2.1、计算过程模型因子节点，过程模型因子节点为INS节点，具体包括：步骤2.1、计算过程模型因子节点，过程模型因子节点为INS节点。
在测量时刻
Figure 380920DEST_PATH_IMAGE019
，IMU在载体坐标系b系的输出测量值为加速度
Figure 631773DEST_PATH_IMAGE020
和角度
Figure 738269DEST_PATH_IMAGE021
，令IMU的测量值
Figure 808993DEST_PATH_IMAGE022
。
Figure 393558DEST_PATH_IMAGE023
为IMU在b系三个坐标轴上的加速度计测量偏差，
Figure 764497DEST_PATH_IMAGE024
为IMU在b系三个坐标轴上的陀螺仪测量偏差，IMU输出测量值的时间间隔为
Figure 776315DEST_PATH_IMAGE025
；INS因子节点的更新周期为时间间隔
Figure 865494DEST_PATH_IMAGE026
，且
Figure 253750DEST_PATH_IMAGE027
，其中，
Figure 744774DEST_PATH_IMAGE028
，且时间间隔
Figure 661914DEST_PATH_IMAGE026
的大小与GNSS接收机输出测量值的时间间隔一致，在时间间隔
Figure 238389DEST_PATH_IMAGE026
内对IMU测量值在b系下的速度、姿态角变化量分别进行预积分，得到速度、姿态角在时间间隔T内的变化量
Figure 899178DEST_PATH_IMAGE029
和
Figure 244708DEST_PATH_IMAGE030
。
进一步得到测量时刻
Figure 433550DEST_PATH_IMAGE031
时，多源融合导航系统的速度估计值
Figure 762900DEST_PATH_IMAGE032
、位置估计值
Figure 492959DEST_PATH_IMAGE033
、姿态角估计值
Figure 692996DEST_PATH_IMAGE034
、加速度计测量偏差估计值
Figure 951939DEST_PATH_IMAGE035
和陀螺仪测量偏差估计值
Figure 503006DEST_PATH_IMAGE036
分别按照如下公式计算：
Figure 771176DEST_PATH_IMAGE037
（2）
Figure 825720DEST_PATH_IMAGE038
（3）
Figure 786723DEST_PATH_IMAGE039
（4）
Figure 293927DEST_PATH_IMAGE040
（5）
Figure 162526DEST_PATH_IMAGE041
（6）
其中,
Figure 274839DEST_PATH_IMAGE042
、
Figure 406743DEST_PATH_IMAGE043
、
Figure 401244DEST_PATH_IMAGE044
分别为多源融合导航系统在地球地固坐标系e系下测量时刻
Figure 276796DEST_PATH_IMAGE019
时的位置、速度、姿态角;
Figure 305932DEST_PATH_IMAGE045
表示从测量时刻
Figure 343158DEST_PATH_IMAGE019
到测量时刻
Figure 90534DEST_PATH_IMAGE046
，由b系变换到e系的旋转矩阵，
Figure 238618DEST_PATH_IMAGE047
表示重力矢量，在时间间隔
Figure 387840DEST_PATH_IMAGE048
内为一个常量，测量时刻
Figure 595967DEST_PATH_IMAGE019
到测量时刻
Figure 830640DEST_PATH_IMAGE046
的时间间隔是
Figure 251257DEST_PATH_IMAGE048
。
过程模型因子节点的状态量为
Figure 786143DEST_PATH_IMAGE049
，
Figure 634014DEST_PATH_IMAGE050
，
Figure 887140DEST_PATH_IMAGE051
为包括15维状态变量的空间，由公式（2）~（6）计算得到测量时刻
Figure 845869DEST_PATH_IMAGE046
的INS节点的状态量的估计值
Figure 500841DEST_PATH_IMAGE052
，将上述公式（2）~（6）合成一个非线性函数，使用符号
Figure 254034DEST_PATH_IMAGE053
来表示，h表示一个非线性函数，上标INS表示INS节点的函数，得到INS节点的状态方程为：
Figure 197719DEST_PATH_IMAGE054
（7）。
步骤2.2、构造测量模型因子节点，测量模型因子节点为GNSS节点，具体包括：
测量时刻i，GNSS接收机的测量值
Figure 553614DEST_PATH_IMAGE055
，其中
Figure 469617DEST_PATH_IMAGE056
，为接收机在测量时刻
Figure 264484DEST_PATH_IMAGE019
对第j颗可见卫星的伪距测量值，
Figure 633149DEST_PATH_IMAGE057
，
Figure 527155DEST_PATH_IMAGE058
为
Figure 359982DEST_PATH_IMAGE059
对应的伪距率，J为可见卫星的最大数量。
由导航电文得到在测量时刻
Figure 720556DEST_PATH_IMAGE019
第j颗卫星在e系下的位置
Figure 169992DEST_PATH_IMAGE060
及对应的速度
Figure 805373DEST_PATH_IMAGE061
，则测量时刻
Figure 492706DEST_PATH_IMAGE019
所有可见卫星的位置矢量和速度矢量分别为
Figure 86499DEST_PATH_IMAGE062
和
Figure 960914DEST_PATH_IMAGE063
。
GNSS接收机在测量时刻
Figure 196723DEST_PATH_IMAGE019
的状态量为
Figure 738563DEST_PATH_IMAGE064
，
Figure 440940DEST_PATH_IMAGE065
，其中，
Figure 864968DEST_PATH_IMAGE066
为包括8维状态变量的空间，
Figure 310992DEST_PATH_IMAGE067
为GNSS接收机的本地时间和GNSS系统时间之间的差，
Figure 504076DEST_PATH_IMAGE068
为
Figure 377355DEST_PATH_IMAGE067
的变化率。
GNSS接收机的测量方程，也就是GNSS节点的测量方程为：
Figure 288679DEST_PATH_IMAGE069
（8）
其中，符号
Figure 335132DEST_PATH_IMAGE070
中，h表示一个非线性函数，上标GNSS表示GNSS节点的函数，c表示光速，
Figure 789247DEST_PATH_IMAGE071
表示接收机与卫星在视线方向上的位置大小，
Figure 426902DEST_PATH_IMAGE072
表示接收机与卫星在视线方向上的速度大小，计算的结果即为估计的伪距与伪距率。
步骤2.3、构造LiDAR因子节点，具体包括：
对于测量时刻接收到的LiDAR测量数据，首先提取其线特征点和面特征点，得到特征集合
Figure 763205DEST_PATH_IMAGE073
和
Figure 347770DEST_PATH_IMAGE074
，上标e和p分别表示线特征和面特征，利用估计的包含旋转和平移的坐标变换矩阵
Figure 515447DEST_PATH_IMAGE075
对其进行坐标变换，将特征集合
Figure 730527DEST_PATH_IMAGE073
和
Figure 616444DEST_PATH_IMAGE074
由b系转换到世界坐标系w系，则LiDAR在测量时刻i的测量值为
Figure 942383DEST_PATH_IMAGE076
，其中
Figure 167828DEST_PATH_IMAGE077
为特征点
Figure 412864DEST_PATH_IMAGE078
的集合，
Figure 723760DEST_PATH_IMAGE079
为特征线
Figure 384548DEST_PATH_IMAGE080
的集合。
对于测量时刻i的LiDAR测量数据，w系下的第个线、面特征点
Figure 526817DEST_PATH_IMAGE081
、
Figure 83700DEST_PATH_IMAGE082
到上一时刻线、面特征点
Figure 881892DEST_PATH_IMAGE083
、
Figure 408688DEST_PATH_IMAGE084
的距离
Figure 343146DEST_PATH_IMAGE085
、
Figure 867668DEST_PATH_IMAGE086
分别表示如下：
Figure 418735DEST_PATH_IMAGE087
（9）
Figure 421326DEST_PATH_IMAGE088
（10）
其中
Figure 272608DEST_PATH_IMAGE089
为对矢量
Figure 233611DEST_PATH_IMAGE090
的取模运算，u、v、w为点
Figure 209657DEST_PATH_IMAGE091
附近的线、面特征点的编号，也就是对于i时刻的第u个线、面特征点，分别为
Figure 812676DEST_PATH_IMAGE092
、
Figure 190568DEST_PATH_IMAGE093
，对于i-1时刻的第u个线、面特征点分别为
Figure 853631DEST_PATH_IMAGE094
、
Figure 316973DEST_PATH_IMAGE095
，对于i、i-1时刻第v、w个线、面特征点的表示与第u个线、面特征点类似，线特征点
Figure 723684DEST_PATH_IMAGE096
属于集合
Figure 752819DEST_PATH_IMAGE097
，面特征点
Figure 258887DEST_PATH_IMAGE098
属于集合
Figure 537422DEST_PATH_IMAGE099
。
通过高斯-牛顿方法得到：
Figure 685506DEST_PATH_IMAGE100
（11）
即最小化
Figure 834728DEST_PATH_IMAGE019
时刻所有特征点到上一时刻特征点的距离，由此将得到相对位姿变换矩阵
Figure 308435DEST_PATH_IMAGE101
，定义测量时刻i的LiDAR的状态量
Figure 277528DEST_PATH_IMAGE102
，且
Figure 229303DEST_PATH_IMAGE103
，其中，
Figure 233031DEST_PATH_IMAGE104
为包含6维状态变量的空间。
因此LiDAR的测量方程为：
Figure 877639DEST_PATH_IMAGE105
（12）
其中符号
Figure 537291DEST_PATH_IMAGE106
中，h表示一个非线性函数，上标Lidar表示Lidar节点的函数，
Figure 89495DEST_PATH_IMAGE107
为一个非线性函数，自变量是
Figure 885412DEST_PATH_IMAGE108
和
Figure 321878DEST_PATH_IMAGE109
，表示利用（9）~（11）式进行计算，
Figure 468826DEST_PATH_IMAGE109
为对应的
Figure 824721DEST_PATH_IMAGE110
的测量值。
进一步的，步骤3还包括：
当检测到载体零速时，即载体的车体坐标系m系下三个方向的速度为零，即
Figure 740724DEST_PATH_IMAGE111
时，INS在测量时刻由IMU测量值实际计算得到的载体速度
Figure 461555DEST_PATH_IMAGE112
不为零，b系及e系下的速度矢量存在如下关系：
Figure 689274DEST_PATH_IMAGE113
（13）
其中，车辆在i时刻e系下的速度由i-1时刻的速度计算得到。
载体在导航坐标系n系下的姿态矢量为
Figure 989806DEST_PATH_IMAGE114
，其中
Figure 619370DEST_PATH_IMAGE115
、
Figure 42261DEST_PATH_IMAGE116
、
Figure 163801DEST_PATH_IMAGE117
分别为i时刻载体的航向角、横滚角、俯仰角，n系姿态矢量
Figure 861499DEST_PATH_IMAGE118
到e系姿态矢量
Figure 283253DEST_PATH_IMAGE119
的转换关系为：
Figure 345887DEST_PATH_IMAGE120
（14）
其中，
Figure 485881DEST_PATH_IMAGE121
表示载体在i时刻由e系变换到n系的旋转矩阵，在时间间隔
Figure 987269DEST_PATH_IMAGE122
内航向角的变化量
Figure 529109DEST_PATH_IMAGE123
与姿态矢量各元素的关系为：
Figure 762644DEST_PATH_IMAGE124
（15）
其中
Figure 389935DEST_PATH_IMAGE125
为载体在惯性系的角速度在
Figure 429435DEST_PATH_IMAGE126
内的变化量在b系下的投影，当载体静止时，在航向角上的变化量为零，由上式约束IMU输出的角速度零偏值。
由公式（13）~（15）可以得到载体静止时，INS节点的状态方程为：
Figure 560202DEST_PATH_IMAGE127
（16）
其中
Figure 495797DEST_PATH_IMAGE128
为对应的
Figure 610384DEST_PATH_IMAGE129
的测量值，
Figure 391258DEST_PATH_IMAGE130
表示对括号内的变量进行计算的非线性函数，上标ZUPT_INS表示载体静止时，INS节点的函数。
选取一颗观测质量最好的卫星，在测量时刻得到该卫星的伪距率测量方程为：
Figure 907690DEST_PATH_IMAGE131
（17）
其中
Figure 748607DEST_PATH_IMAGE132
为接收机与卫星之间的几何距离变化率，卫星的钟差变化率由星历计算得到，
Figure 350489DEST_PATH_IMAGE133
为测量残差，
Figure 669475DEST_PATH_IMAGE134
为包括对流层电离层在内的传播误差随时间的变化率，当用户静止时，接收机与卫星之间的几何距离变化率
Figure 40414DEST_PATH_IMAGE132
是卫星的速度在卫星与接收机径向方向的大小，可得载体静止时GNSS节点对应的测量方程为：
Figure 317811DEST_PATH_IMAGE135
（18）
其中符号
Figure 875832DEST_PATH_IMAGE136
中，h表示一个非线性函数，上标ZUPT_GNSS表示载体静止时，GNSS节点的函数，
Figure 529667DEST_PATH_IMAGE137
为对应的
Figure 489533DEST_PATH_IMAGE138
的测量值。
当系统检测到载体处于静止状态时，因子图中不增加新的因子节点，对应矩阵的大小维持不变，只使用新测量的信息更新系统状态。
进一步的，步骤4还包括：
测量时刻i+1，因子图系统状态量
Figure 937832DEST_PATH_IMAGE139
的最大后验概率值
Figure 248727DEST_PATH_IMAGE140
的表达式为：
Figure 440674DEST_PATH_IMAGE142
Figure 255046DEST_PATH_IMAGE144
（19），
其中，
Figure 139826DEST_PATH_IMAGE145
，表示求矩阵X的马氏距离的平方，
Figure 938017DEST_PATH_IMAGE146
为X对应的协方差矩阵。
当检测到载体处于静止时，利用零速修正因子节点进行计算状态量的最大后验概率值
Figure 668076DEST_PATH_IMAGE147
：
Figure 602534DEST_PATH_IMAGE148
Figure 127056DEST_PATH_IMAGE149
（20）
得到系统导航解。
进一步的，在上述技术方案的基础上，所述载体为车辆。
另一方面，本发明还提出一种多源融合导航系统的导航装置，包括：由全球卫星导航系统GNSS接收机、惯性导航系统INS和激光雷达LiDAR组合的导航系统，其中，INS包括测量惯性单元IMU;处理器和存储器，处理器与存储器通信连接，存储器存储有计算机指令，处理器运行计算机指令时执行上述技术方案的方法中的步骤。
本发明由于通过在扩展的增量平滑与建图方法（Incremental Smoothing andMapping 2 , iSAM2）iSAM2的基础上，结合GNSS/INS/ LiDAR多源融合导航系统，利用LiDAR输出的位姿变化量作为检测量进行广义似然比零速检测，检测到载体处于零速时，不增加新的因子节点，有效减轻了因子图的存储量和计算量；IMU/GNSS/LiDAR的测量值仍然对系统状态进行约束和更新，保证了零速时系统状态值估计的正确性。并且，在因子图计算过程中，将IMU测量值进行预积分，预积分的时刻及时间间隔和GNSS接收机输出的测量值时刻及间隔一致，保证了两种传感器输出测量数据的同步。本发明带来的明显技术效果主要如下。
（1）提出了利用LiDAR输出的位姿变化量作为检测量进行广义似然比零速检测，相比传统的利用IMU输出的三维加速度、姿态测量值作为广义似然比零速检测量方法，由于LiDAR位姿测量值的精度高，因此该方法检测性能更好。本发明在检测到载体处于零速时，不增加新的因子节点，有效减轻了因子图的存储量和计算量。
（2）在载体零速时，将IMU/GNSS/LiDAR的测量值对系统状态进行约束和更新，保证了系统状态值估计的正确性。相比传统因子图方法，本发明在多源融合系统导航定位性能不变的情况下，能有效降低因子图的存储量和计算量，特别适用于车载多源融合系统在城市行驶中频繁启停的场景。
附图说明
为了更清楚地说明本发明具体实施方式或现有技术中的技术方案下面将对具体实施方式或现有技术描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图是本发明的一些实施方式，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。
图1是本发明的流程示意图；
图2是本发明的因子图结构与经典因子图的对比，其中a是经典因子图算法，b是本发明所提供方法；
图3是本发明的实验车辆和传感器搭建图；
图4是本发明的车辆行驶轨迹图；
图5是本发明的零速检测结果；
图6是本发明与经典因子图计算节点数对比；
图7是本发明与经典因子图三维定位精度对比。
具体实施方式
为了便于理解，本具体实施方式是本发明公开的优选实施例，以详细说明本发明的结构和发明点，但并不作为本发明权利要求的限定保护范围。
图1是本发明提出的多源导航系统的导航方法的一个优选实施例的流程示意图，下面结合图1进行详细说明。
参见图1所示的包括全球卫星导航系统GNSS、惯性导航系统INS和激光雷达LiDAR组合的多源导航系统的因子图方法的流程图，包括：第一步，构造零速检测统计量。
作为一个更优的实施例，该步骤利用LiDAR由点云匹配后输出的相对位姿参数作为检测统计量，构造广义似然比公式，得到以下的零速检测公式：
Figure 678123DEST_PATH_IMAGE150
（1）
其中，
Figure 149556DEST_PATH_IMAGE151
为第n次检测的检测统计量集合，检测统计量是LiDAR在各个测量时刻的测量值，具体包括由点云匹配后得到的相对位置
Figure 837DEST_PATH_IMAGE152
和相对位姿变化量
Figure 165102DEST_PATH_IMAGE153
，为检测数据个数，
Figure 469045DEST_PATH_IMAGE154
和
Figure 540906DEST_PATH_IMAGE155
分别表示第m时刻相对位置
Figure 715535DEST_PATH_IMAGE152
和相对位姿变化量
Figure 785123DEST_PATH_IMAGE156
的取值，
Figure 45203DEST_PATH_IMAGE157
为实单位四元数
Figure 451913DEST_PATH_IMAGE158
，上标符号“T”表示矩阵或矢量的转置运算，
Figure 215470DEST_PATH_IMAGE159
为检测窗口长度，
Figure 518275DEST_PATH_IMAGE160
表示
Figure 265651DEST_PATH_IMAGE161
的二范数运算，
Figure 413736DEST_PATH_IMAGE162
表示
Figure 94116DEST_PATH_IMAGE163
的二范数运算，
Figure 567823DEST_PATH_IMAGE164
表示相对位置
Figure 271336DEST_PATH_IMAGE165
的测量方差，
Figure 223112DEST_PATH_IMAGE166
表示相对位姿变化量
Figure 35296DEST_PATH_IMAGE167
的测量方差。
当检测统计量
Figure 414325DEST_PATH_IMAGE168
小于设定的门限
Figure 605135DEST_PATH_IMAGE169
时认为处于静止状态。
第二步，构造因子图。
在因子图中，包括两种节点；一种是过程模型因子节点，即INS节点；一种是测量模型因子节点，即GNSS和LiDAR节点。在任意一个测量时刻
Figure 360601DEST_PATH_IMAGE170
，因子图系统状态量为
Figure 15574DEST_PATH_IMAGE171
，
Figure 565504DEST_PATH_IMAGE172
，
Figure 509189DEST_PATH_IMAGE173
表示第一个测量时刻，多源融合导航系统状态量，
Figure 599505DEST_PATH_IMAGE174
表示第二个测量时刻，多源融合导航系统状态量，依次论推，则可知
Figure 312246DEST_PATH_IMAGE175
包括了截至到时刻
Figure 33077DEST_PATH_IMAGE170
多源融合导航系统状态量的所有值。
作为一个更优的实施方式，该步骤中的具体包括：
步骤2.1、计算过程模型因子节点，过程模型因子节点为INS节点。
假设在测量时刻i，已知IMU在b系（body frame，载体坐标系）的输出测量值为加速度
Figure 464058DEST_PATH_IMAGE176
和角度
Figure 358065DEST_PATH_IMAGE177
，令
Figure 925313DEST_PATH_IMAGE178
。此处的载体为车辆，或者其他可以测控的移动载体。
Figure 82625DEST_PATH_IMAGE179
为IMU在b系三个坐标轴上的加速度计测量偏差，
Figure 204164DEST_PATH_IMAGE180
为IMU在b系三个坐标轴上的陀螺仪测量偏差；IMU输出测量值的时间间隔为
Figure 636283DEST_PATH_IMAGE181
。为了降低计算负担，采用预积分的方法。选定INS因子节点的更新周期为时间间隔T，且
Figure 526878DEST_PATH_IMAGE182
。时间间隔T的大小与GNSS接收机输出测量值的时间间隔一致。在时间间隔T内对IMU测量值在b系下的速度、姿态角变化量分别进行预积分，得到下述结果：
Figure 855091DEST_PATH_IMAGE183
Figure 995086DEST_PATH_IMAGE184
其中，
Figure 699737DEST_PATH_IMAGE185
、
Figure 38314DEST_PATH_IMAGE186
分别为b系下在测量时刻i，经过时间间隔T积分后得到的多源融合导航系统的速度、姿态角的变化量。由以上预积分求出的变化量，进一步可以求得测量时刻
Figure 740691DEST_PATH_IMAGE187
时，多源融合导航系统的速度、位置、姿态角的估计值：
Figure 164719DEST_PATH_IMAGE188
（2）
Figure 345165DEST_PATH_IMAGE189
（3）
Figure 803828DEST_PATH_IMAGE190
（4）
Figure 677106DEST_PATH_IMAGE191
（5）
Figure 791692DEST_PATH_IMAGE192
（6）
其中,
Figure 572567DEST_PATH_IMAGE193
、
Figure 88998DEST_PATH_IMAGE194
、
Figure 195495DEST_PATH_IMAGE195
分别为多源融合导航系统在e系（Earth-Centered Earth-Fixed, 地球地固坐标系）下测量时刻i时的位置、速度、姿态角，由INS递推的原理可知这些为已知量;
Figure 531798DEST_PATH_IMAGE196
表示从测量时刻i到测量时刻i+1，由b系变换到e系的旋转矩阵；
Figure 381943DEST_PATH_IMAGE197
表示重力矢量，在时间间隔T内为一个常量。值得说明的是，测量时刻到测量时刻
Figure 487302DEST_PATH_IMAGE198
的时间间隔是T。
设过程模型因子节点(即INS节点)的状态量
Figure 499120DEST_PATH_IMAGE199
，
Figure 119457DEST_PATH_IMAGE200
，
Figure 242134DEST_PATH_IMAGE201
为包括15维状态变量的空间，上标“T”表示矩阵的转置运算，则由前面的公式（2）~（6）计算得到测量时刻i+1的INS节点的状态量的估计值
Figure 467579DEST_PATH_IMAGE202
。
将上述公式（2）~（6）合成一个非线性函数，使用符号
Figure 447036DEST_PATH_IMAGE203
来表示，h表示一个非线性函数，上标INS表示INS节点的函数，得到INS节点的状态方程为：
Figure 23511DEST_PATH_IMAGE204
（7）。
步骤2.2、构造测量模型因子节点，即GNSS节点。
已知测量时刻i，GNSS接收机的测量值
Figure 684300DEST_PATH_IMAGE205
，其中
Figure 29830DEST_PATH_IMAGE206
，
Figure 383451DEST_PATH_IMAGE207
为接收机在测量时刻i对第j颗可见卫星的伪距测量值，
Figure 447222DEST_PATH_IMAGE208
，
Figure 911702DEST_PATH_IMAGE209
为
Figure 111739DEST_PATH_IMAGE210
对应的伪距率，J为可见卫星的最大数量。
由导航电文可以得到测量时刻i第j颗卫星在e系下的位置
Figure 901840DEST_PATH_IMAGE211
及对应的速度
Figure 656170DEST_PATH_IMAGE212
。则测量时刻所有可见卫星的位置矢量和速度矢量为
Figure 455498DEST_PATH_IMAGE213
和
Figure 244463DEST_PATH_IMAGE214
。
设GNSS接收机在测量时刻的状态量为
Figure 205466DEST_PATH_IMAGE215
，
Figure 712670DEST_PATH_IMAGE216
，其中，
Figure 784532DEST_PATH_IMAGE217
为包括8维状态变量的空间，
Figure 490319DEST_PATH_IMAGE218
为GNSS接收机的本地时间和GNSS系统时间的差，
Figure 622223DEST_PATH_IMAGE219
为
Figure 351145DEST_PATH_IMAGE218
的变化率。
则GNSS接收机的测量方程可以写为：
Figure 492276DEST_PATH_IMAGE220
（8）
其中，符号
Figure 255833DEST_PATH_IMAGE221
中，h表示一个非线性函数，上标GNSS表示GNSS节点的函数，c表示光速，
Figure 558638DEST_PATH_IMAGE222
表示接收机与卫星在视线方向上的位置大小，
Figure 306015DEST_PATH_IMAGE223
表示接收机与卫星在视线方向上的速度大小，计算的结果即为估计的伪距与伪距率。
步骤2.3、构造LiDAR因子节点。
对于测量时刻i接收到的LiDAR测量数据，首先提取其线特征点和面特征点，得到特征集合
Figure 465818DEST_PATH_IMAGE224
和
Figure 615040DEST_PATH_IMAGE225
，上标
Figure 557588DEST_PATH_IMAGE226
和
Figure 526681DEST_PATH_IMAGE227
分别表示线特征和面特征，利用估计的坐标变换矩阵
Figure 744035DEST_PATH_IMAGE228
（包含旋转和平移）对其进行坐标变换，将特征集合
Figure 747764DEST_PATH_IMAGE224
和
Figure 126792DEST_PATH_IMAGE225
由B系转换到World坐标系（w系），则LiDAR在测量时刻i的测量值为
Figure 583181DEST_PATH_IMAGE229
，其中
Figure 135386DEST_PATH_IMAGE230
为特征点
Figure 196882DEST_PATH_IMAGE224
的集合，
Figure 809129DEST_PATH_IMAGE231
为特征线
Figure 690498DEST_PATH_IMAGE225
的集合。对于测量时刻i的LiDAR测量数据，w系下的第k个线、面特征点
Figure 249655DEST_PATH_IMAGE232
、
Figure 962396DEST_PATH_IMAGE233
的距离
Figure 948807DEST_PATH_IMAGE234
、
Figure 645367DEST_PATH_IMAGE235
分别表示如下：
Figure 477057DEST_PATH_IMAGE236
（9）
Figure 841042DEST_PATH_IMAGE237
（10）
其中
Figure 998354DEST_PATH_IMAGE238
为对矢量
Figure 854315DEST_PATH_IMAGE239
的取模运算，u、v、w为点
Figure 552012DEST_PATH_IMAGE240
附近的线、面特征点的编号，也就是对于i时刻的第u个线、面特征点，分别为
Figure 442608DEST_PATH_IMAGE241
、
Figure 567559DEST_PATH_IMAGE242
，对于i-1时刻的第u个线、面特征点分别为
Figure 707553DEST_PATH_IMAGE243
、
Figure 146625DEST_PATH_IMAGE244
，对于i、i-1时刻第v、w个线、面特征点的表示与第u个线、面特征点类似，线特征点
Figure 688464DEST_PATH_IMAGE245
属于集合
Figure 187579DEST_PATH_IMAGE246
，面特征点
Figure 814869DEST_PATH_IMAGE247
属于集合
Figure 260894DEST_PATH_IMAGE248
。
通过高斯-牛顿方法对如下最优问题进行求解：
Figure 719557DEST_PATH_IMAGE249
（11）
即最小化i时刻所有特征点到上一时刻特征点的距离，由此将得到相对位姿变换矩阵
Figure 389573DEST_PATH_IMAGE250
，定义i时刻的LiDAR的状态量
Figure 973001DEST_PATH_IMAGE251
（
Figure 816192DEST_PATH_IMAGE252
为包括6维状态变量的空间）。
因此LiDAR的测量方程为：
Figure 535886DEST_PATH_IMAGE253
（12）
其中符号
Figure 376804DEST_PATH_IMAGE254
中，h表示一个非线性函数，上标Lidar表示Lidar节点的函数，
Figure 713107DEST_PATH_IMAGE255
为一个非线性函数，自变量是
Figure 297672DEST_PATH_IMAGE256
和
Figure 668611DEST_PATH_IMAGE257
，表示利用（9）~（11）式进行计算，
Figure 680429DEST_PATH_IMAGE258
为对应的
Figure 566345DEST_PATH_IMAGE259
的测量值。
由此可得LiDAR因子节点的定义如下：
Figure 689022DEST_PATH_IMAGE260
其中代价函数的定义为:
Figure 914467DEST_PATH_IMAGE261
；
Figure 97187DEST_PATH_IMAGE262
表示马氏距离的平方，
Figure 408082DEST_PATH_IMAGE263
为
Figure 600029DEST_PATH_IMAGE264
对应的协方差矩阵。
第三步，构造零速修正因子节点：
假设多源融合导航系统的载体为城市中行驶的车辆，当车辆处于静止状态时，利用车辆速度为零的约束可以限制INS误差的积累和GNSS接收机的时钟漂移，此时不需要进行车辆位置和速度解算，极大的降低了因子图中计算和存储的复杂度。当检测到车辆零速时，即车体坐标系（m系）下三个方向的速度为零，即
Figure 945560DEST_PATH_IMAGE265
。忽略INS安装角的偏差，认为INS的车辆坐标系（b系）与车体坐标系重合。但由于IMU测量误差，使得INS在测量时刻由IMU测量值实际计算得到的车辆速度
Figure 33602DEST_PATH_IMAGE266
不为零，b系及e系下的速度矢量存在如下关系：
Figure 97373DEST_PATH_IMAGE267
（13）
其中车辆在i时刻e系下的速度可以由i-1时刻的速度计算得到，如式所示。假设车辆在导航坐标系（n系）下的姿态矢量为
Figure 561852DEST_PATH_IMAGE268
，其中
Figure 27468DEST_PATH_IMAGE269
、
Figure 286411DEST_PATH_IMAGE270
、
Figure 571899DEST_PATH_IMAGE271
分别为i时刻车辆的航向角、横滚角、俯仰角。n系姿态矢量
Figure 105649DEST_PATH_IMAGE272
到e系姿态矢量
Figure 488089DEST_PATH_IMAGE273
的转换关系为：
Figure 183512DEST_PATH_IMAGE274
（14）
上式中，
Figure 690717DEST_PATH_IMAGE275
表示车辆在i时刻由e系变换到n系的旋转矩阵。在时间间隔
Figure 762578DEST_PATH_IMAGE276
内航向角的变化量
Figure 468366DEST_PATH_IMAGE277
与姿态矢量各元素的关系为：
Figure 600270DEST_PATH_IMAGE278
（15）
其中
Figure 606489DEST_PATH_IMAGE279
为车辆在惯性系的角速度在
Figure 482042DEST_PATH_IMAGE280
内的变化量在b系下的投影。当车辆静止时，在航向角上的变化量为零，由上式可以约束IMU输出的角速度零偏值。
由公式（13）~（15）可以得到车辆静止时，INS节点的状态方程为：
Figure 245598DEST_PATH_IMAGE281
（16）
其中
Figure 548404DEST_PATH_IMAGE282
为对应的
Figure 233463DEST_PATH_IMAGE283
的测量值，
Figure 443864DEST_PATH_IMAGE284
表示对括号内的变量进行计算的非线性函数，上标ZUPT_INS表示车辆静止时，INS节点的函数。
同时GNSS接收机与卫星系统时的钟差和钟差变化率在车辆静止时，也可以用速度为零进行约束控制。选取一颗观测质量最好的卫星，在测量时刻得到该卫星的伪距率测量方程为：
Figure 61927DEST_PATH_IMAGE285
（17）
其中
Figure 801213DEST_PATH_IMAGE286
为接收机与卫星之间的几何距离变化率，卫星的钟差变化率可由星历计算得到，在测量方程中省略，其值归结到伪距率测量值中，
Figure 770306DEST_PATH_IMAGE287
为测量残差。
Figure 987661DEST_PATH_IMAGE288
为包括对流层电离层等在内的传播误差随时间的变化率，在短时间内可以忽略。当用户静止时，接收机与卫星之间的几何距离变化率
Figure 725810DEST_PATH_IMAGE286
其实就是卫星的速度在卫星与接收机径向方向的大小。
可得车辆静止时GNSS节点对应的测量方程为：
Figure 370418DEST_PATH_IMAGE289
（18）
其中符号
Figure 30069DEST_PATH_IMAGE290
中，h表示一个非线性函数，上标ZUPT_GNSS表示车辆静止时，GNSS节点的函数，
Figure 785536DEST_PATH_IMAGE291
为对应的
Figure 440508DEST_PATH_IMAGE292
的测量值。
当系统检测到车辆处于静止状态时，因子图中不增加新的因子节点，对应矩阵的大小维持不变，只对系统状态量进行更新。图2所示为经典因子图方法与本发明在结构上的比较，其中（a）图为传统的经典因子图算法结构，（b）图为本发明提出的方法的结构，当车辆静止的时候，就出现了零速修正节点，这个节点对应的零速修正节点因子就取代了在正常行驶过程中传统的因子节点用于计算车辆的状态量，测量值则不再更新，从而使得因子图的存储和计算得以简化。
第四步，因子图的计算：
由上述推导得，测量时刻
Figure 193700DEST_PATH_IMAGE293
，因子图系统状态量
Figure 934123DEST_PATH_IMAGE294
的最大后验概率值
Figure 696543DEST_PATH_IMAGE295
的表达式为：
Figure 206022DEST_PATH_IMAGE296
（19）
当检测到车辆处于静止时，利用零速修正因子节点进行计算状态量的最大后验概率值
Figure 395695DEST_PATH_IMAGE297
：
Figure 826676DEST_PATH_IMAGE298
Figure 658366DEST_PATH_IMAGE299
（20）
利用基于平滑和建图的标准C++的库GTSAM（Georgia Tech Smoothing andMapping），对上述公式（19）或（20）进行求解计算，可以得到系统导航解。
图3为本发明提出的GNSS/INS/LIDAR多源融合导航系统的实验验证平台。如图3所示，激光雷达采用镭神智能的C16-151B，为16线机械式雷达，30度垂直视场角，最大测量量程可达150m。 GNSS/INS组合导航设备使用华测的CGI-410的组合导航系统，INS中的IMU数据输出速率100Hz，GNSS接收机数据输出速率为1Hz，实验所用的各传感器具体指标如表1所示。
Figure 756772DEST_PATH_IMAGE300
实验场景为城市道路，由于存在等待红绿灯与礼让行人，实验期间存在多个零速时间段，车辆轨迹如图4中“+”字曲线所示。
采用广义似然比作为检测器对目标车辆进行零速检测，其结果如图5所示，其中检测结果曲线在图中只有“0”和“1”两种取值，取值为“0”时代表检测到零速，为“1”时表示目标处于运动状态。由于存在环境高频噪声以及静止时汽车震动噪声，因此在检测器前加入低通滤波器以提高检测率，实验结果表明检测率达到93%时，误检率为0.7%。
实验条件同上，车辆在城市中行驶过程中，有频繁启停的情况，利用本发明所提方法对GNSS接收机、IMU和LIDAR的测量值利用因子图模型进行计算，实时得到车辆的位置、速度和姿态等信息。图6为本发明所提方法与经典因子图方法在整个车辆行驶过程中的计算时间的对比图。行驶时间为943秒，其中车辆停止时间为285秒，从图中可以看出，在刚开始的时间段内，由于启停的次数有限，本发明的计算时间与经典因子图的基本一致，随着时间推移，启停次数增多后，本发明方法计算时间增速较缓，相比于经典因子图在计算代价方面优势越来越明显，实时性提升显著，而经典因子图的图增大的速度与时间呈线性关系。对因子图中节点数量进行统计，在本次943秒的实验时间内，主节点数量从经典方法的9432个降至6577个，降低30.27%。当然，随着车辆停止的次数和时间增多，其节点数下降将更明显。
另外，图7为定位精度的对比图，计算本发明与经典因子图定位的三维误差，由图可见，本发明与经典因子图方法定位精度基本相同，最大误差为0.08m。 本发明在车辆零速区间不更新因子图的状态节点，只是利用IMU和GNSS接收机输出的测量值来约束系统状态，总体性能水平与经典因子图相当。
以上是本发明的一些具体实施方式，但本发明并不仅局限于上述方式，所有对本发明技术特征的简单变换，凡依本发明专利申请范围所述的构造、特征及原理所做的等效变化或修饰，都将落入本发明的保护范围之内。

Claims (7)

1.一种多源融合导航系统的导航方法，其特征在于包括：
步骤1：构造零速检测统计量，利用所述零速检测统计量判断载体是否处于静止状态；
步骤2：构造因子图，所述因子图用于将多源融合导航系统的各传感器的观测量建立当前系统状态和前一状态的联系并用于优化计算；
步骤3：基于步骤1的判断结果，构造零速修正因子节点，所述零速修正因子节点用于计算载体的状态量，以降低因子图的存储和简化计算；
步骤4：基于构造的所述零速修正因子节点，计算因子图，得到系统导航解，所述导航解提供载体的三维位置、三维速度、三维姿态角信息、载体本地时钟误差、时钟误差漂移率信息；
其中，所述步骤1还包括：
利用LiDAR进行点云匹配后输出的相对位姿参数作为检测统计量，构造广义似然比公式，得到如下零速检测公式：
Figure 275067DEST_PATH_IMAGE001
其中，
Figure 54191DEST_PATH_IMAGE002
为第n次检测的检测统计量集合，所述检测统计量是LiDAR在各个测量时刻的测量值，具体包括由点云匹配后得到的相对位置
Figure 896245DEST_PATH_IMAGE003
和相对位姿变化量
Figure 353771DEST_PATH_IMAGE004
，为检测数据个数，
Figure 230461DEST_PATH_IMAGE005
和
Figure 849661DEST_PATH_IMAGE006
分别表示第m时刻相对位置
Figure 179011DEST_PATH_IMAGE003
和相对位姿变化量
Figure 440228DEST_PATH_IMAGE004
的取值，
Figure 171424DEST_PATH_IMAGE007
为实单位四元数
Figure 20912DEST_PATH_IMAGE008
，上标符号“T”表示矩阵或矢量的转置运算，
Figure 837559DEST_PATH_IMAGE009
为检测窗口长度，
Figure 371308DEST_PATH_IMAGE010
表示
Figure 957010DEST_PATH_IMAGE011
的二范数运算，
Figure 449171DEST_PATH_IMAGE012
表示
Figure 753114DEST_PATH_IMAGE013
的二范数运算，
Figure 90554DEST_PATH_IMAGE014
表示相对位置
Figure 596009DEST_PATH_IMAGE015
的测量方差，
Figure 993493DEST_PATH_IMAGE016
表示相对位姿变化量
Figure 784731DEST_PATH_IMAGE004
的测量方差；
当所述检测统计量
Figure 925863DEST_PATH_IMAGE017
小于设定的门限
Figure 220578DEST_PATH_IMAGE018
时认为载体处于静止状态。
2.根据权利要求1所述的方法，其特征在于所述多源融合导航系统为全球卫星导航系统GNSS接收机、惯性导航系统INS和激光雷达LiDAR组合的导航系统，其中，所述INS包括测量惯性单元IMU。
3.根据权利要求2所述的方法，其特征在于所述步骤2还包括：
步骤2.1、计算过程模型因子节点，所述过程模型因子节点为INS节点，具体包括：
在测量时刻
Figure 54541DEST_PATH_IMAGE019
，所述IMU在载体坐标系b系的输出测量值为加速度
Figure 67497DEST_PATH_IMAGE020
和角度
Figure 829565DEST_PATH_IMAGE021
，令IMU的测量值
Figure 244365DEST_PATH_IMAGE022
；
Figure 983651DEST_PATH_IMAGE023
为IMU在b系三个坐标轴上的加速度计测量偏差，
Figure 483903DEST_PATH_IMAGE024
为IMU在b系三个坐标轴上的陀螺仪测量偏差，IMU输出测量值的时间间隔为
Figure 966837DEST_PATH_IMAGE025
；INS因子节点的更新周期为时间间隔
Figure 501723DEST_PATH_IMAGE026
，且
Figure 474227DEST_PATH_IMAGE027
，其中，
Figure 461775DEST_PATH_IMAGE028
，且时间间隔
Figure 485750DEST_PATH_IMAGE026
的大小与GNSS接收机输出测量值的时间间隔一致，在时间间隔
Figure 609564DEST_PATH_IMAGE026
内对IMU测量值在b系下的速度、姿态角变化量分别进行预积分，得到速度、姿态角在时间间隔T内的变化量
Figure 956232DEST_PATH_IMAGE029
和
Figure 431075DEST_PATH_IMAGE030
：
进一步得到测量时刻
Figure 318129DEST_PATH_IMAGE031
时，多源融合导航系统的速度估计值
Figure 562028DEST_PATH_IMAGE032
、位置估计值
Figure 814018DEST_PATH_IMAGE033
、姿态角估计值
Figure 507649DEST_PATH_IMAGE034
、加速度计测量偏差估计值
Figure 136076DEST_PATH_IMAGE035
和陀螺仪测量偏差估计值
Figure 500062DEST_PATH_IMAGE036
分别按照如下公式计算：
Figure 922953DEST_PATH_IMAGE037
（2）
Figure 372389DEST_PATH_IMAGE038
（3）
Figure 601245DEST_PATH_IMAGE039
（4）
Figure 554157DEST_PATH_IMAGE040
（5）
Figure 413529DEST_PATH_IMAGE041
（6）
其中,
Figure 87611DEST_PATH_IMAGE042
、
Figure 323421DEST_PATH_IMAGE043
、
Figure 396419DEST_PATH_IMAGE044
分别为多源融合导航系统在地球地固坐标系e系下测量时刻i时的位置、速度、姿态角;
Figure 426692DEST_PATH_IMAGE045
表示从测量时刻i到测量时刻
Figure 585141DEST_PATH_IMAGE046
，由b系变换到e系的旋转矩阵，
Figure 93482DEST_PATH_IMAGE047
表示重力矢量，在时间间隔T内为一个常量，测量时刻i到测量时刻
Figure 20987DEST_PATH_IMAGE046
的时间间隔是T；
过程模型因子节点的状态量为
Figure 222161DEST_PATH_IMAGE048
，
Figure 661714DEST_PATH_IMAGE049
，所述
Figure 177009DEST_PATH_IMAGE050
为包括15维状态变量的空间，由公式（2）~（6）计算得到测量时刻的INS节点的状态量的估计值
Figure 21337DEST_PATH_IMAGE051
，将上述公式（2）~（6）合成一个非线性函数，使用符号
Figure 393413DEST_PATH_IMAGE052
来表示，h表示一个非线性函数，上标INS表示INS节点的函数，得到INS节点的状态方程为：
Figure 526454DEST_PATH_IMAGE053
（7）；
步骤2.2、构造测量模型因子节点，所述测量模型因子节点为GNSS节点，具体包括：
测量时刻i，GNSS接收机的测量值
Figure 642177DEST_PATH_IMAGE054
，其中
Figure 544274DEST_PATH_IMAGE055
，
Figure 824602DEST_PATH_IMAGE056
为接收机在测量时刻i对第j颗可见卫星的伪距测量值，
Figure 179359DEST_PATH_IMAGE057
，
Figure 895512DEST_PATH_IMAGE058
为
Figure 652115DEST_PATH_IMAGE056
对应的伪距率，J为可见卫星的最大数量；
由导航电文得到在测量时刻i第j颗卫星在e系下的位置
Figure 365993DEST_PATH_IMAGE059
及对应的速度
Figure 208047DEST_PATH_IMAGE060
，则测量时刻所有可见卫星的位置矢量和速度矢量分别为
Figure 931153DEST_PATH_IMAGE061
和
Figure 276683DEST_PATH_IMAGE062
；
GNSS接收机在测量时刻的状态量为
Figure 424112DEST_PATH_IMAGE063
，
Figure 284621DEST_PATH_IMAGE064
，其中，所
Figure 342576DEST_PATH_IMAGE065
为包括8维状态变量的空间，
Figure 339350DEST_PATH_IMAGE066
为GNSS接收机的本地时间和GNSS系统时间之间的差，
Figure 863873DEST_PATH_IMAGE067
为
Figure 211677DEST_PATH_IMAGE066
的变化率；
GNSS接收机的测量方程，也就是GNSS节点的测量方程为：
Figure 545094DEST_PATH_IMAGE068
（8）
其中，符号
Figure 927534DEST_PATH_IMAGE069
中，h表示一个非线性函数，上标GNSS表示GNSS节点的函数，c表示光速，
Figure 950854DEST_PATH_IMAGE070
表示接收机与卫星在视线方向上的位置大小，
Figure 458058DEST_PATH_IMAGE071
表示接收机与卫星在视线方向上的速度大小，计算的结果即为估计的伪距与伪距率；
步骤2.3、构造LiDAR因子节点，具体包括：
对于测量时刻接收到的LiDAR测量数据，首先提取其线特征点和面特征点，得到特征集合
Figure 326657DEST_PATH_IMAGE072
和
Figure 829183DEST_PATH_IMAGE073
，上标e和p分别表示线特征和面特征，利用估计的包含旋转和平移的坐标变换矩阵
Figure 90832DEST_PATH_IMAGE074
对其进行坐标变换，将特征集合
Figure 85333DEST_PATH_IMAGE072
和
Figure 757623DEST_PATH_IMAGE073
由b系转换到世界坐标系w系，则LiDAR在测量时刻i的测量值为
Figure 114655DEST_PATH_IMAGE075
，其中
Figure 214198DEST_PATH_IMAGE076
为特征点
Figure 695995DEST_PATH_IMAGE077
的集合，
Figure 171976DEST_PATH_IMAGE078
为特征线
Figure 120865DEST_PATH_IMAGE079
的集合；
对于测量时刻i的LiDAR测量数据，w系下的第k个线、面特征点
Figure 922467DEST_PATH_IMAGE080
、
Figure 625981DEST_PATH_IMAGE081
到上一时刻线、面特征点
Figure 905653DEST_PATH_IMAGE082
、
Figure 971698DEST_PATH_IMAGE083
的距离
Figure 413043DEST_PATH_IMAGE084
、
Figure 600924DEST_PATH_IMAGE085
分别表示如下：
Figure 887548DEST_PATH_IMAGE086
（9）
Figure 808100DEST_PATH_IMAGE087
（10）
其中
Figure 623609DEST_PATH_IMAGE088
为对矢量
Figure 629611DEST_PATH_IMAGE089
的取模运算，u、v、w为点
Figure 454348DEST_PATH_IMAGE090
附近的线、面特征点的编号，也就是对于i时刻的第u个线、面特征点，分别为
Figure 494985DEST_PATH_IMAGE091
、
Figure 215816DEST_PATH_IMAGE092
，对于i-1时刻的第u个线、面特征点分别为
Figure 977624DEST_PATH_IMAGE093
、
Figure 340472DEST_PATH_IMAGE094
，对于i、i-1时刻第v、w个线、面特征点的表示与第u个线、面特征点类似，线特征点
Figure 704457DEST_PATH_IMAGE095
属于集合
Figure 596190DEST_PATH_IMAGE096
，面特征点
Figure 45626DEST_PATH_IMAGE097
属于集合
Figure 540061DEST_PATH_IMAGE098
；通过高斯-牛顿方法得到：
Figure 227394DEST_PATH_IMAGE099
（11）
即最小化i时刻所有特征点到上一时刻特征点的距离，由此将得到相对位姿变换矩阵
Figure 818257DEST_PATH_IMAGE100
，定义测量时刻i的LiDAR的状态量
Figure 286147DEST_PATH_IMAGE101
，且
Figure 53115DEST_PATH_IMAGE102
，其中，
Figure 594955DEST_PATH_IMAGE103
为包含6维状态变量的空间；
因此LiDAR的测量方程为：
Figure 625228DEST_PATH_IMAGE104
（12）
其中符号
Figure 986939DEST_PATH_IMAGE105
中，h表示一个非线性函数，上标Lidar表示Lidar节点的函数，
Figure 26439DEST_PATH_IMAGE106
为一个非线性函数，自变量是
Figure 485102DEST_PATH_IMAGE107
和
Figure 220364DEST_PATH_IMAGE108
，表示利用（9）~（11）式进行计算，
Figure 600530DEST_PATH_IMAGE108
为对应的
Figure 178142DEST_PATH_IMAGE109
的测量值。
4.根据权利要求3所述的方法，其特征在于所述步骤3还包括：
当检测到载体零速时，即载体的车体坐标系m系下三个方向的速度为零，即
Figure 756891DEST_PATH_IMAGE110
时，INS在测量时刻i由IMU测量值实际计算得到的载体速度
Figure 597808DEST_PATH_IMAGE111
不为零，b系及e系下的速度矢量存在如下关系：
Figure 996428DEST_PATH_IMAGE112
（13）
其中，车辆在
Figure 580993DEST_PATH_IMAGE113
时刻e系下的速度由i-1时刻的速度计算得到；
载体在导航坐标系n系下的姿态矢量为
Figure 276898DEST_PATH_IMAGE114
，其中
Figure 554296DEST_PATH_IMAGE115
、
Figure 174633DEST_PATH_IMAGE116
、
Figure 94048DEST_PATH_IMAGE117
分别为
Figure 585072DEST_PATH_IMAGE113
时刻载体的航向角、横滚角、俯仰角，n系姿态矢量
Figure 830108DEST_PATH_IMAGE118
到e系姿态矢量
Figure 937742DEST_PATH_IMAGE119
的转换关系为：
Figure 864109DEST_PATH_IMAGE120
（14）
其中，
Figure 9307DEST_PATH_IMAGE121
表示载体在i时刻由e系变换到n系的旋转矩阵，在时间间隔
Figure 159666DEST_PATH_IMAGE122
内航向角的变化量
Figure 285754DEST_PATH_IMAGE123
与姿态矢量各元素的关系为：
Figure 15812DEST_PATH_IMAGE124
（15）
其中
Figure 215850DEST_PATH_IMAGE125
为载体在惯性系的角速度在
Figure 333847DEST_PATH_IMAGE122
内的变化量在b系下的投影，当载体静止时，在航向角上的变化量为零，由上式约束IMU输出的角速度零偏值；
由公式（13）~（15）可以得到载体静止时，INS节点的状态方程为：
Figure 619335DEST_PATH_IMAGE126
（16）
其中
Figure 970330DEST_PATH_IMAGE127
为对应的
Figure 556032DEST_PATH_IMAGE128
的测量值，
Figure 517035DEST_PATH_IMAGE129
表示对括号内的变量进行计算的非线性函数，上标ZUPT_INS表示载体静止时，INS节点的函数；
选取一颗观测质量最好的卫星，在测量时刻得到该卫星的伪距率测量方程为：
Figure 86556DEST_PATH_IMAGE130
（17）
其中
Figure 423997DEST_PATH_IMAGE131
为接收机与卫星之间的几何距离变化率，卫星的钟差变化率由星历计算得到，
Figure 864206DEST_PATH_IMAGE132
为测量残差，
Figure 324006DEST_PATH_IMAGE133
为包括对流层电离层在内的传播误差随时间的变化率，当用户静止时，接收机与卫星之间的几何距离变化率
Figure 52927DEST_PATH_IMAGE134
是卫星的速度在卫星与接收机径向方向的大小，可得载体静止时GNSS节点对应的测量方程为：
Figure 259305DEST_PATH_IMAGE135
（18）
其中符号
Figure 819600DEST_PATH_IMAGE136
中，h表示一个非线性函数，上标ZUPT_GNSS表示载体静止时，GNSS节点的函数，
Figure 919143DEST_PATH_IMAGE137
为对应的
Figure 197677DEST_PATH_IMAGE138
的测量值；
当系统检测到载体处于静止状态时，因子图中不增加新的因子节点，对应矩阵的大小维持不变，只使用新测量的信息更新系统状态。
5.根据权利要求4所述的方法，其特征在于所述步骤4还包括：
测量时刻
Figure 673658DEST_PATH_IMAGE139
，因子图系统状态量
Figure 557300DEST_PATH_IMAGE140
的最大后验概率值
Figure 824815DEST_PATH_IMAGE141
的表达式为：
Figure 59487DEST_PATH_IMAGE142
Figure 73580DEST_PATH_IMAGE143
（19），
其中，
Figure 342887DEST_PATH_IMAGE144
，表示求矩阵
Figure 253074DEST_PATH_IMAGE145
的马氏距离的平方，为
Figure 709463DEST_PATH_IMAGE145
对应的协方差矩阵；
当检测到载体处于静止时，利用零速修正因子节点进行计算状态量的最大后验概率值
Figure 527246DEST_PATH_IMAGE146
：
Figure 182219DEST_PATH_IMAGE147
Figure 62975DEST_PATH_IMAGE148
（20）
得到系统导航解。
6.根据权利要求5所述的方法，其特征在于所述载体为车辆。
7.一种多源融合导航系统的导航装置，其特征在于包括：
由全球卫星导航系统GNSS接收机、惯性导航系统INS和激光雷达LiDAR组合的导航系统，其中，所述INS包括测量惯性单元IMU;
处理器和存储器，所述处理器与所述存储器通信连接，
所述存储器存储有计算机指令，所述处理器运行所述计算机指令时执行权利要求1～6中任一项所述的方法中的步骤。
CN202310010536.0A 2023-01-05 2023-01-05 一种多源融合导航系统的导航方法和装置 Active CN115685292B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202310010536.0A CN115685292B (zh) 2023-01-05 2023-01-05 一种多源融合导航系统的导航方法和装置

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202310010536.0A CN115685292B (zh) 2023-01-05 2023-01-05 一种多源融合导航系统的导航方法和装置

Publications (2)

Publication Number Publication Date
CN115685292A true CN115685292A (zh) 2023-02-03
CN115685292B CN115685292B (zh) 2023-03-21

Family

ID=85057334

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202310010536.0A Active CN115685292B (zh) 2023-01-05 2023-01-05 一种多源融合导航系统的导航方法和装置

Country Status (1)

Country Link
CN (1) CN115685292B (zh)

Cited By (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN116299576A (zh) * 2023-05-12 2023-06-23 中国人民解放军国防科技大学 一种组合导航系统的欺骗干扰检测方法与装置
CN116817928A (zh) * 2023-08-28 2023-09-29 北京交通大学 基于因子图优化的卫导/惯导列车多源融合定位的方法

Citations (10)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN110207692A (zh) * 2019-05-13 2019-09-06 南京航空航天大学 一种地图辅助的惯性预积分行人导航方法
CN111221018A (zh) * 2020-03-12 2020-06-02 南京航空航天大学 一种用于抑制海上多路径的gnss多源信息融合导航方法
DE102019214008A1 (de) * 2019-09-13 2021-03-18 Robert Bosch Gmbh Verfahren und Vorrichtung zur Lokalisierung eines mobilen Agenten in einer Umgebung mit dynamischen Objekten
CN112946711A (zh) * 2021-01-29 2021-06-11 中国人民解放军国防科技大学 一种gnss/ins组合导航系统的导航方法
WO2021232470A1 (zh) * 2020-05-19 2021-11-25 北京数字绿土科技有限公司 基于多传感器融合的slam制图方法、系统
CN114046790A (zh) * 2021-10-22 2022-02-15 南京航空航天大学 一种因子图双重回环的检测方法
CN114088104A (zh) * 2021-07-23 2022-02-25 武汉理工大学 一种自动驾驶场景下的地图生成方法
CN114545472A (zh) * 2022-01-26 2022-05-27 中国人民解放军国防科技大学 一种gnss/ins组合系统的导航方法和装置
CN115235454A (zh) * 2022-09-15 2022-10-25 中国人民解放军国防科技大学 行人运动约束的视觉惯性融合定位与建图方法和装置
CN115542349A (zh) * 2022-10-12 2022-12-30 中电科卫星导航运营服务有限公司 复杂环境多源融合定位自主完好性评估方法

Patent Citations (10)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN110207692A (zh) * 2019-05-13 2019-09-06 南京航空航天大学 一种地图辅助的惯性预积分行人导航方法
DE102019214008A1 (de) * 2019-09-13 2021-03-18 Robert Bosch Gmbh Verfahren und Vorrichtung zur Lokalisierung eines mobilen Agenten in einer Umgebung mit dynamischen Objekten
CN111221018A (zh) * 2020-03-12 2020-06-02 南京航空航天大学 一种用于抑制海上多路径的gnss多源信息融合导航方法
WO2021232470A1 (zh) * 2020-05-19 2021-11-25 北京数字绿土科技有限公司 基于多传感器融合的slam制图方法、系统
CN112946711A (zh) * 2021-01-29 2021-06-11 中国人民解放军国防科技大学 一种gnss/ins组合导航系统的导航方法
CN114088104A (zh) * 2021-07-23 2022-02-25 武汉理工大学 一种自动驾驶场景下的地图生成方法
CN114046790A (zh) * 2021-10-22 2022-02-15 南京航空航天大学 一种因子图双重回环的检测方法
CN114545472A (zh) * 2022-01-26 2022-05-27 中国人民解放军国防科技大学 一种gnss/ins组合系统的导航方法和装置
CN115235454A (zh) * 2022-09-15 2022-10-25 中国人民解放军国防科技大学 行人运动约束的视觉惯性融合定位与建图方法和装置
CN115542349A (zh) * 2022-10-12 2022-12-30 中电科卫星导航运营服务有限公司 复杂环境多源融合定位自主完好性评估方法

Non-Patent Citations (2)

* Cited by examiner, † Cited by third party
Title
TAO DU 等: "An Integrated INS/Lidar Odometry/Polarized Camera Pose Estimation via Factor Graph Optimization for Sparse Environment" *
吴学佳 等: "基于因子图的行人INS/GNSS组合导航算法仿真研究" *

Cited By (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN116299576A (zh) * 2023-05-12 2023-06-23 中国人民解放军国防科技大学 一种组合导航系统的欺骗干扰检测方法与装置
CN116299576B (zh) * 2023-05-12 2023-12-12 中国人民解放军国防科技大学 一种组合导航系统的欺骗干扰检测方法与装置
CN116817928A (zh) * 2023-08-28 2023-09-29 北京交通大学 基于因子图优化的卫导/惯导列车多源融合定位的方法
CN116817928B (zh) * 2023-08-28 2023-12-01 北京交通大学 基于因子图优化的卫导/惯导列车多源融合定位的方法

Also Published As

Publication number Publication date
CN115685292B (zh) 2023-03-21

Similar Documents

Publication Publication Date Title
CN115685292B (zh) 一种多源融合导航系统的导航方法和装置
CN113945206B (zh) 一种基于多传感器融合的定位方法及装置
CN108731670B (zh) 基于量测模型优化的惯性/视觉里程计组合导航定位方法
CN105509739A (zh) 采用固定区间crts平滑的ins/uwb紧组合导航系统及方法
CN112697138B (zh) 一种基于因子图优化的仿生偏振同步定位与构图的方法
CN114019552A (zh) 一种基于贝叶斯多传感器误差约束的定位置信度优化方法
CN114526745B (zh) 一种紧耦合激光雷达和惯性里程计的建图方法及系统
CN110187375A (zh) 一种基于slam定位结果提高定位精度的方法及装置
CN114545472B (zh) 一种gnss/ins组合系统的导航方法和装置
CN110412596A (zh) 一种基于图像信息和激光点云的机器人定位方法
CN115060257B (zh) 一种基于民用级惯性测量单元的车辆变道检测方法
CN115046540A (zh) 一种点云地图构建方法、系统、设备和存储介质
CN114690229A (zh) 一种融合gps的移动机器人视觉惯性导航方法
CN114608568A (zh) 一种基于多传感器信息即时融合定位方法
CN113052855B (zh) 一种基于视觉-imu-轮速计融合的语义slam方法
CN112729283A (zh) 一种基于深度相机/mems惯导/里程计组合的导航方法
CN113029173A (zh) 车辆导航方法及装置
CN116576849A (zh) 一种基于gmm辅助的车辆融合定位方法及系统
CN115930977A (zh) 特征退化场景的定位方法、系统、电子设备和可读存介质
CN116222541A (zh) 利用因子图的智能多源组合导航方法及装置
CN114690230A (zh) 一种基于视觉惯性slam的自动驾驶车辆导航方法
CN110849349A (zh) 一种基于磁传感器与轮式里程计融合定位方法
CN113503872B (zh) 一种基于相机与消费级imu融合的低速无人车定位方法
CN116380057B (zh) 一种gnss拒止环境下无人机自主着陆定位方法
CN116222556B (zh) 一种基于多源传感器融合的室内定位方法及系统

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
SE01 Entry into force of request for substantive examination
SE01 Entry into force of request for substantive examination
GR01 Patent grant
GR01 Patent grant