CN115685292A - 一种多源融合导航***的导航方法和装置 - Google Patents
一种多源融合导航***的导航方法和装置 Download PDFInfo
- 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
Links
- 238000000034 method Methods 0.000 title claims abstract description 57
- 230000004927 fusion Effects 0.000 title claims abstract description 32
- 238000001514 detection method Methods 0.000 claims abstract description 37
- 238000004364 calculation method Methods 0.000 claims abstract description 15
- 238000012937 correction Methods 0.000 claims abstract description 13
- 238000005259 measurement Methods 0.000 claims description 117
- 230000006870 function Effects 0.000 claims description 33
- 230000008859 change Effects 0.000 claims description 30
- 239000013598 vector Substances 0.000 claims description 30
- 239000011159 matrix material Substances 0.000 claims description 22
- 230000003068 static effect Effects 0.000 claims description 18
- 230000008569 process Effects 0.000 claims description 16
- 238000012886 linear function Methods 0.000 claims description 12
- 230000009466 transformation Effects 0.000 claims description 11
- 238000006243 chemical reaction Methods 0.000 claims description 6
- 230000014509 gene expression Effects 0.000 claims description 5
- 230000005484 gravity Effects 0.000 claims description 3
- 239000005433 ionosphere Substances 0.000 claims description 3
- 238000013519 translation Methods 0.000 claims description 3
- 230000010354 integration Effects 0.000 description 5
- 238000004422 calculation algorithm Methods 0.000 description 4
- 238000009499 grossing Methods 0.000 description 4
- 238000010586 diagram Methods 0.000 description 3
- 238000013507 mapping Methods 0.000 description 3
- 241000135164 Timea Species 0.000 description 2
- 230000005540 biological transmission Effects 0.000 description 2
- 238000004891 communication Methods 0.000 description 2
- 238000005516 engineering process Methods 0.000 description 2
- 238000012986 modification Methods 0.000 description 2
- 230000004048 modification Effects 0.000 description 2
- 238000012545 processing Methods 0.000 description 2
- 230000001133 acceleration Effects 0.000 description 1
- 238000009825 accumulation Methods 0.000 description 1
- 238000013473 artificial intelligence Methods 0.000 description 1
- 150000001875 compounds Chemical class 0.000 description 1
- 238000000354 decomposition reaction Methods 0.000 description 1
- 230000007423 decrease Effects 0.000 description 1
- 238000009795 derivation Methods 0.000 description 1
- 230000000694 effects Effects 0.000 description 1
- 238000002474 experimental method Methods 0.000 description 1
- 229910052705 radium Inorganic materials 0.000 description 1
- HCWPIIXVSYCSAN-UHFFFAOYSA-N radium atom Chemical compound [Ra] HCWPIIXVSYCSAN-UHFFFAOYSA-N 0.000 description 1
- 238000011160 research Methods 0.000 description 1
- 230000002194 synthesizing effect Effects 0.000 description 1
- 238000012360 testing method Methods 0.000 description 1
- 230000036962 time dependent Effects 0.000 description 1
- 238000012795 verification Methods 0.000 description 1
Images
Landscapes
- Navigation (AREA)
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进行点云匹配后输出的相对位姿参数作为检测统计量,构造广义似然比公式,得到如下零速检测公式:
其中,为第n次检测的检测统计量集合,所述检测统计量是LiDAR在各个测量时刻的测量值,具体包括由点云匹配后得到的相对位置和相对位姿变化量,N为检测数据个数, 和分别表示第m时刻相对位置和相对位姿变化量的取值,为实单位四元数,上标符号“T”表示矩阵或矢量的转置运算,为检测窗口长度, 表示的二范数运算,表示的二范数运算,表示相对位置的测量方差,表示相对位姿变化量的测量方差。
进一步的,步骤2还包括:
步骤2.1、计算过程模型因子节点,过程模型因子节点为INS节点,具体包括:步骤2.1、计算过程模型因子节点,过程模型因子节点为INS节点。
为IMU在b系三个坐标轴上的加速度计测量偏差,为IMU在b系三个坐标轴上的陀螺仪测量偏差,IMU输出测量值的时间间隔为;INS因子节点的更新周期为时间间隔,且,其中,,且时间间隔的大小与GNSS接收机输出测量值的时间间隔一致,在时间间隔内对IMU测量值在b系下的速度、姿态角变化量分别进行预积分,得到速度、姿态角在时间间隔T内的变化量和。
其中, 、、分别为多源融合导航***在地球地固坐标系e系下测量时刻时的位置、速度、姿态角; 表示从测量时刻到测量时刻,由b系变换到e系的旋转矩阵,表示重力矢量,在时间间隔内为一个常量,测量时刻到测量时刻的时间间隔是。
过程模型因子节点的状态量为,,为包括15维状态变量的空间,由公式(2)~(6)计算得到测量时刻的INS节点的状态量的估计值,将上述公式(2)~(6)合成一个非线性函数,使用符号来表示,h表示一个非线性函数,上标INS表示INS节点的函数,得到INS节点的状态方程为:
步骤2.2、构造测量模型因子节点,测量模型因子节点为GNSS节点,具体包括:
GNSS接收机的测量方程,也就是GNSS节点的测量方程为:
步骤2.3、构造LiDAR因子节点,具体包括:
对于测量时刻接收到的LiDAR测量数据,首先提取其线特征点和面特征点,得到特征集合和,上标e和p分别表示线特征和面特征,利用估计的包含旋转和平移的坐标变换矩阵对其进行坐标变换,将特征集合和由b系转换到世界坐标系w系,则LiDAR在测量时刻i的测量值为,其中为特征点的集合,为特征线的集合。
其中为对矢量的取模运算,u、v、w为点附近的线、面特征点的编号,也就是对于i时刻的第u个线、面特征点,分别为、,对于i-1时刻的第u个线、面特征点分别为、,对于i、i-1时刻第v、w个线、面特征点的表示与第u个线、面特征点类似,线特征点属于集合,面特征点属于集合。
通过高斯-牛顿方法得到:
因此LiDAR的测量方程为:
进一步的,步骤3还包括:
其中,车辆在i时刻e系下的速度由i-1时刻的速度计算得到。
由公式(13)~(15)可以得到载体静止时,INS节点的状态方程为:
选取一颗观测质量最好的卫星,在测量时刻得到该卫星的伪距率测量方程为:
其中为接收机与卫星之间的几何距离变化率,卫星的钟差变化率由星历计算得到,为测量残差,为包括对流层电离层在内的传播误差随时间的变化率,当用户静止时,接收机与卫星之间的几何距离变化率是卫星的速度在卫星与接收机径向方向的大小,可得载体静止时GNSS节点对应的测量方程为:
当***检测到载体处于静止状态时,因子图中不增加新的因子节点,对应矩阵的大小维持不变,只使用新测量的信息更新***状态。
进一步的,步骤4还包括:
得到***导航解。
进一步的,在上述技术方案的基础上,所述载体为车辆。
另一方面,本发明还提出一种多源融合导航***的导航装置,包括:由全球卫星导航***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由点云匹配后输出的相对位姿参数作为检测统计量,构造广义似然比公式,得到以下的零速检测公式:
其中,为第n次检测的检测统计量集合,检测统计量是LiDAR在各个测量时刻的测量值,具体包括由点云匹配后得到的相对位置和相对位姿变化量,为检测数据个数,和分别表示第m时刻相对位置和相对位姿变化量的取值,为实单位四元数,上标符号“T”表示矩阵或矢量的转置运算,为检测窗口长度,表示的二范数运算,表示的二范数运算,表示相对位置的测量方差,表示相对位姿变化量的测量方差。
第二步,构造因子图。
在因子图中,包括两种节点;一种是过程模型因子节点,即INS节点;一种是测量模型因子节点,即GNSS和LiDAR节点。在任意一个测量时刻,因子图***状态量为,,表示第一个测量时刻,多源融合导航***状态量,表示第二个测量时刻,多源融合导航***状态量,依次论推,则可知包括了截至到时刻多源融合导航***状态量的所有值。
作为一个更优的实施方式,该步骤中的具体包括:
步骤2.1、计算过程模型因子节点,过程模型因子节点为INS节点。
假设在测量时刻i,已知IMU在b系(body frame,载体坐标系)的输出测量值为加速度和角度,令。此处的载体为车辆,或者其他可以测控的移动载体。为IMU在b系三个坐标轴上的加速度计测量偏差,为IMU在b系三个坐标轴上的陀螺仪测量偏差;IMU输出测量值的时间间隔为。为了降低计算负担,采用预积分的方法。选定INS因子节点的更新周期为时间间隔T,且。时间间隔T的大小与GNSS接收机输出测量值的时间间隔一致。在时间间隔T内对IMU测量值在b系下的速度、姿态角变化量分别进行预积分,得到下述结果:
其中,、分别为b系下在测量时刻i,经过时间间隔T积分后得到的多源融合导航***的速度、姿态角的变化量。由以上预积分求出的变化量,进一步可以求得测量时刻时,多源融合导航***的速度、位置、姿态角的估计值:
其中, 、、分别为多源融合导航***在e系(Earth-Centered Earth-Fixed, 地球地固坐标系)下测量时刻i时的位置、速度、姿态角,由INS递推的原理可知这些为已知量; 表示从测量时刻i到测量时刻i+1,由b系变换到e系的旋转矩阵;表示重力矢量,在时间间隔T内为一个常量。值得说明的是,测量时刻到测量时刻的时间间隔是T。
步骤2.2、构造测量模型因子节点,即GNSS节点。
则GNSS接收机的测量方程可以写为:
步骤2.3、构造LiDAR因子节点。
对于测量时刻i接收到的LiDAR测量数据,首先提取其线特征点和面特征点,得到特征集合和,上标和分别表示线特征和面特征,利用估计的坐标变换矩阵(包含旋转和平移)对其进行坐标变换,将特征集合和由B系转换到World坐标系(w系),则LiDAR在测量时刻i的测量值为,其中为特征点的集合,为特征线的集合。对于测量时刻i的LiDAR测量数据,w系下的第k个线、面特征点、的距离、分别表示如下:
其中为对矢量的取模运算,u、v、w为点附近的线、面特征点的编号,也就是对于i时刻的第u个线、面特征点,分别为、,对于i-1时刻的第u个线、面特征点分别为、,对于i、i-1时刻第v、w个线、面特征点的表示与第u个线、面特征点类似,线特征点属于集合,面特征点属于集合。
通过高斯-牛顿方法对如下最优问题进行求解:
因此LiDAR的测量方程为:
由此可得LiDAR因子节点的定义如下:
第三步,构造零速修正因子节点:
假设多源融合导航***的载体为城市中行驶的车辆,当车辆处于静止状态时,利用车辆速度为零的约束可以限制INS误差的积累和GNSS接收机的时钟漂移,此时不需要进行车辆位置和速度解算,极大的降低了因子图中计算和存储的复杂度。当检测到车辆零速时,即车体坐标系(m系)下三个方向的速度为零,即。忽略INS安装角的偏差,认为INS的车辆坐标系(b系)与车体坐标系重合。但由于IMU测量误差,使得INS在测量时刻由IMU测量值实际计算得到的车辆速度不为零,b系及e系下的速度矢量存在如下关系:
其中车辆在i时刻e系下的速度可以由i-1时刻的速度计算得到,如式所示。假设车辆在导航坐标系(n系)下的姿态矢量为,其中、、分别为i时刻车辆的航向角、横滚角、俯仰角。n系姿态矢量到e系姿态矢量的转换关系为:
由公式(13)~(15)可以得到车辆静止时,INS节点的状态方程为:
同时GNSS接收机与卫星***时的钟差和钟差变化率在车辆静止时,也可以用速度为零进行约束控制。选取一颗观测质量最好的卫星,在测量时刻得到该卫星的伪距率测量方程为:
其中为接收机与卫星之间的几何距离变化率,卫星的钟差变化率可由星历计算得到,在测量方程中省略,其值归结到伪距率测量值中,为测量残差。为包括对流层电离层等在内的传播误差随时间的变化率,在短时间内可以忽略。当用户静止时,接收机与卫星之间的几何距离变化率其实就是卫星的速度在卫星与接收机径向方向的大小。
可得车辆静止时GNSS节点对应的测量方程为:
当***检测到车辆处于静止状态时,因子图中不增加新的因子节点,对应矩阵的大小维持不变,只对***状态量进行更新。图2所示为经典因子图方法与本发明在结构上的比较,其中(a)图为传统的经典因子图算法结构,(b)图为本发明提出的方法的结构,当车辆静止的时候,就出现了零速修正节点,这个节点对应的零速修正节点因子就取代了在正常行驶过程中传统的因子节点用于计算车辆的状态量,测量值则不再更新,从而使得因子图的存储和计算得以简化。
第四步,因子图的计算:
利用基于平滑和建图的标准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所示。
实验场景为城市道路,由于存在等待红绿灯与礼让行人,实验期间存在多个零速时间段,车辆轨迹如图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进行点云匹配后输出的相对位姿参数作为检测统计量,构造广义似然比公式,得到如下零速检测公式:
其中,为第n次检测的检测统计量集合,所述检测统计量是LiDAR在各个测量时刻的测量值,具体包括由点云匹配后得到的相对位置和相对位姿变化量,为检测数据个数, 和分别表示第m时刻相对位置和相对位姿变化量的取值,为实单位四元数,上标符号“T”表示矩阵或矢量的转置运算, 为检测窗口长度,表示的二范数运算,表示的二范数运算, 表示相对位置的测量方差, 表示相对位姿变化量的测量方差;
2.根据权利要求1所述的方法,其特征在于所述多源融合导航***为全球卫星导航***GNSS接收机、惯性导航***INS和激光雷达LiDAR组合的导航***,其中,所述INS包括测量惯性单元IMU。
3.根据权利要求2所述的方法,其特征在于所述步骤2还包括:
步骤2.1、计算过程模型因子节点,所述过程模型因子节点为INS节点,具体包括:
为IMU在b系三个坐标轴上的加速度计测量偏差, 为IMU在b系三个坐标轴上的陀螺仪测量偏差,IMU输出测量值的时间间隔为;INS因子节点的更新周期为时间间隔,且,其中,,且时间间隔的大小与GNSS接收机输出测量值的时间间隔一致,在时间间隔内对IMU测量值在b系下的速度、姿态角变化量分别进行预积分,得到速度、姿态角在时间间隔T内的变化量和:
其中, 、、分别为多源融合导航***在地球地固坐标系e系下测量时刻i时的位置、速度、姿态角; 表示从测量时刻i到测量时刻,由b系变换到e系的旋转矩阵, 表示重力矢量,在时间间隔T内为一个常量,测量时刻i到测量时刻的时间间隔是T;
过程模型因子节点的状态量为,,所述为包括15维状态变量的空间,由公式(2)~(6)计算得到测量时刻的INS节点的状态量的估计值,将上述公式(2)~(6)合成一个非线性函数,使用符号来表示,h表示一个非线性函数,上标INS表示INS节点的函数,得到INS节点的状态方程为:
步骤2.2、构造测量模型因子节点,所述测量模型因子节点为GNSS节点,具体包括:
GNSS接收机的测量方程,也就是GNSS节点的测量方程为:
步骤2.3、构造LiDAR因子节点,具体包括:
对于测量时刻接收到的LiDAR测量数据,首先提取其线特征点和面特征点,得到特征集合和,上标e和p分别表示线特征和面特征,利用估计的包含旋转和平移的坐标变换矩阵对其进行坐标变换,将特征集合和由b系转换到世界坐标系w系,则LiDAR在测量时刻i的测量值为,其中为特征点的集合,为特征线的集合;
其中为对矢量的取模运算,u、v、w为点附近的线、面特征点的编号,也就是对于i时刻的第u个线、面特征点,分别为、,对于i-1时刻的第u个线、面特征点分别为、,对于i、i-1时刻第v、w个线、面特征点的表示与第u个线、面特征点类似,线特征点属于集合,面特征点属于集合;通过高斯-牛顿方法得到:
因此LiDAR的测量方程为:
4.根据权利要求3所述的方法,其特征在于所述步骤3还包括:
由公式(13)~(15)可以得到载体静止时,INS节点的状态方程为:
选取一颗观测质量最好的卫星,在测量时刻得到该卫星的伪距率测量方程为:
其中为接收机与卫星之间的几何距离变化率,卫星的钟差变化率由星历计算得到,为测量残差,为包括对流层电离层在内的传播误差随时间的变化率,当用户静止时,接收机与卫星之间的几何距离变化率是卫星的速度在卫星与接收机径向方向的大小,可得载体静止时GNSS节点对应的测量方程为:
当***检测到载体处于静止状态时,因子图中不增加新的因子节点,对应矩阵的大小维持不变,只使用新测量的信息更新***状态。
6.根据权利要求5所述的方法,其特征在于所述载体为车辆。
7.一种多源融合导航***的导航装置,其特征在于包括:
由全球卫星导航***GNSS接收机、惯性导航***INS和激光雷达LiDAR组合的导航***,其中,所述INS包括测量惯性单元IMU;
处理器和存储器,所述处理器与所述存储器通信连接,
所述存储器存储有计算机指令,所述处理器运行所述计算机指令时执行权利要求1~6中任一项所述的方法中的步骤。
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)
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)
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 | 中电科卫星导航运营服务有限公司 | 复杂环境多源融合定位自主完好性评估方法 |
-
2023
- 2023-01-05 CN CN202310010536.0A patent/CN115685292B/zh active Active
Patent Citations (10)
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)
Title |
---|
TAO DU 等: "An Integrated INS/Lidar Odometry/Polarized Camera Pose Estimation via Factor Graph Optimization for Sparse Environment" * |
吴学佳 等: "基于因子图的行人INS/GNSS组合导航算法仿真研究" * |
Cited By (4)
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) | 一种多源融合导航***的导航方法和装置 | |
JP2022019642A (ja) | マルチセンサ融合に基づく測位方法及び装置 | |
CN114526745B (zh) | 一种紧耦合激光雷达和惯性里程计的建图方法及*** | |
CN105509739A (zh) | 采用固定区间crts平滑的ins/uwb紧组合导航***及方法 | |
CN107289932B (zh) | 基于mems传感器和vlc定位融合的单卡尔曼滤波导航装置和方法 | |
CN112697138B (zh) | 一种基于因子图优化的仿生偏振同步定位与构图的方法 | |
CN114019552A (zh) | 一种基于贝叶斯多传感器误差约束的定位置信度优化方法 | |
CN110187375A (zh) | 一种基于slam定位结果提高定位精度的方法及装置 | |
CN112729283A (zh) | 一种基于深度相机/mems惯导/里程计组合的导航方法 | |
CN113052855B (zh) | 一种基于视觉-imu-轮速计融合的语义slam方法 | |
CN114545472B (zh) | 一种gnss/ins组合***的导航方法和装置 | |
CN115046540A (zh) | 一种点云地图构建方法、***、设备和存储介质 | |
CN115060257B (zh) | 一种基于民用级惯性测量单元的车辆变道检测方法 | |
CN112797985A (zh) | 基于加权扩展卡尔曼滤波的室内定位方法及室内定位*** | |
CN115930977A (zh) | 特征退化场景的定位方法、***、电子设备和可读存介质 | |
CN113503872B (zh) | 一种基于相机与消费级imu融合的低速无人车定位方法 | |
CN114690229A (zh) | 一种融合gps的移动机器人视觉惯性导航方法 | |
CN114608568A (zh) | 一种基于多传感器信息即时融合定位方法 | |
US12019170B1 (en) | GNSS and INS integrated navigation positioning method and system thereof | |
CN113029173A (zh) | 车辆导航方法及装置 | |
CN117388830A (zh) | 激光雷达与惯性导航的外参标定方法、装置、设备及介质 | |
CN116576849A (zh) | 一种基于gmm辅助的车辆融合定位方法及*** | |
CN116222541A (zh) | 利用因子图的智能多源组合导航方法及装置 | |
CN114690230A (zh) | 一种基于视觉惯性slam的自动驾驶车辆导航方法 | |
CN114527481A (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 |