融合GNSS观测信息的激光SLAM定位方法及系统

融合gnss观测信息的激光slam定位方法及系统
技术领域
1.本发明涉及多传感器融合定位技术领域，特别是涉及一种融合gnss观测信息的激光slam定位方法及系统。


背景技术：

2.随着北斗定位技术的成熟，我国室外的定位与导航领域达到了新的高度，户外领域可以依赖以北斗组成的多卫星定位系统联合定位，实现较高精度的定位结果，但仅基于gnss信号，在室内以及半遮挡区域容易定位失败，不能满足实时定位的需要。
3.即时定位与地图构建(simultaneous localization and mapping，slam)技术作为一种仅依赖自身传感器进行自主定位的技术，在机器人领域取得了快速发展，该方式主要依赖视觉和激光类传感器，出现了以视觉为主的视觉slam和以激光雷达为主的激光slam。相比于视觉slam，激光slam更为鲁棒，不依赖环境纹理信息。但是slam的实时定位获取的是相对位置信息，属于局部坐标系，且存在累计误差。


技术实现要素：

4.为了解决上述问题，本发明提供了一种融合gnss观测信息的激光slam定位方法及系统，将gnss/imu与激光slam相结合，能够实现室内外无缝定位并大幅提升定位精度与鲁棒性。
5.为实现上述目的，本发明提供了如下方案：
6.一种融合gnss观测信息的激光slam定位方法，包括：
7.获取目标空间内载体的激光雷达数据、惯性传感器数据和gnss数据；
8.基于所述惯性传感器数据构建惯性传感器数据约束因子；
9.基于所述激光雷达数据构建激光雷达约束因子；
10.基于所述gnss数据构建多普勒数据约束因子、测码伪距约束因子和钟差约束因子；
11.基于滑动窗口优化法对所述惯性传感器数据约束因子、所述激光雷达约束因子、所述多普勒数据约束因子、所述测码伪距约束因子以及所述钟差约束因子进行联合优化，得到所述目标空间内载体的定位信息。
12.可选地，基于所述惯性传感器数据构建惯性传感器数据约束因子，具体包括：
13.采用预积分处理的公式对所述激光雷达采集到的关键帧激光雷达数据间的惯性传感器数据进行预积分处理，得到惯性传感器数据约束因子；所述关键帧激光雷达数据为相邻两帧激光雷达数据的运动距离和旋转角度均大于相应的设定阈值的激光雷达数据；所述惯性传感器数据约束因子包括：激光雷达关键帧之间的位置、速度和旋转约束。
14.可选地，基于所述激光雷达数据构建激光雷达约束因子，具体包括：
15.利用所述惯性传感器对所述激光雷达数据进行去畸变，得到去畸变激光雷达数据；
16.利用所述去畸变激光雷达数据与关键帧激光雷达数据生成三维点云地图；
17.利用所述三维点云地图进行基于最近点迭代算法的匹配，得到激光雷达约束因子。
18.可选地，所述利用所述惯性传感器对所述激光雷达数据进行去畸变，具体包括：
19.采用时间戳和惯性传感器的运动轨迹确定在采集各帧所述激光雷达数据时激光雷达的位置和姿态，得到每帧所述激光雷达数据的坐标值；
20.将每帧所述激光雷达数据的坐标值转换至每帧所述激光雷达数据起始时刻的激光雷达坐标系下，得到去畸变激光雷达数据。
21.可选地，基于滑动窗口优化法对所述惯性传感器数据约束因子、所述激光雷达约束因子、所述多普勒数据约束因子、所述测码伪距约束因子以及所述钟差约束因子进行联合优化，具体包括：
22.基于所述惯性传感器数据约束因子、所述激光雷达约束因子、所述多普勒数据约束因子、所述测码伪距约束因子以及所述钟差约束因子，构建滑动窗口优化方程；
23.利用l-m算法对所述滑动窗口优化方程进行求解，得到所述目标空间内载体的定位信息。
24.本发明还提供了一种融合gnss观测信息的激光slam定位系统，包括：
25.数据获取模块，用于获取目标空间内载体的激光雷达数据、惯性传感器数据和gnss数据；
26.惯性传感器数据约束因子构建模块，用于基于所述惯性传感器数据构建惯性传感器数据约束因子；
27.激光雷达约束因子构建模块，用于基于所述激光雷达数据构建激光雷达约束因子；
28.约束因子、测码伪距约束因子和钟差约束因子构建模块，用于基于所述gnss数据构建多普勒数据约束因子、测码伪距约束因子和钟差约束因子；
29.优化模块，用于基于滑动窗口优化法对所述惯性传感器数据约束因子、所述激光雷达约束因子、所述多普勒数据约束因子、所述测码伪距约束因子以及所述钟差约束因子进行联合优化，得到所述目标空间内载体的定位信息。
30.可选地，所述惯性传感器数据约束因子构建模块，具体包括：
31.预积分处理单元，用于采用预积分处理的公式对所述激光雷达采集到的关键帧激光雷达数据间的惯性传感器数据进行预积分处理，得到惯性传感器数据约束因子；所述关键帧激光雷达数据为相邻两帧激光雷达数据的运动距离和旋转角度均大于相应的设定阈值的激光雷达数据；所述惯性传感器数据约束因子包括：激光雷达关键帧之间的位置、速度和旋转约束。
32.可选地，所述激光雷达约束因子构建模块，具体包括：
33.去畸变单元，用于利用所述惯性传感器对所述激光雷达数据进行去畸变，得到去畸变激光雷达数据；
34.三维点云地图生成单元，用于利用所述去畸变激光雷达数据与关键帧激光雷达数据生成三维点云地图；
35.匹配单元，用于利用所述三维点云地图进行基于最近点迭代算法的匹配，得到激
光雷达约束因子。
36.可选地，所述去畸变单元，具体包括：
37.坐标值确定子单元，用于采用时间戳和惯性传感器的运动轨迹确定在采集各帧所述激光雷达数据时激光雷达的位置和姿态，得到每帧所述激光雷达数据的坐标值；
38.坐标值转换子单元，用于将每帧所述激光雷达数据的坐标值转换至每帧所述激光雷达数据起始时刻的激光雷达坐标系下，得到去畸变激光雷达数据。
39.可选地，所述优化模块，具体包括：
40.滑动窗口优化方程构建单元，用于基于所述惯性传感器数据约束因子、所述激光雷达约束因子、所述多普勒数据约束因子、所述测码伪距约束因子以及所述钟差约束因子，构建滑动窗口优化方程；
41.求解子单元，用于利用l-m算法对所述滑动窗口优化方程进行求解，得到所述目标空间内载体的定位信息。
42.根据本发明提供的具体实施例，本发明公开了以下技术效果：
43.本发明提供的一种融合gnss观测信息的激光slam定位方法，包括：获取目标空间内载体的激光雷达数据、惯性传感器数据和gnss数据；基于所述惯性传感器数据构建惯性传感器数据约束因子；基于所述激光雷达数据构建激光雷达约束因子；基于所述gnss数据构建多普勒数据约束因子、测码伪距约束因子和钟差约束因子；基于滑动窗口优化法对上述约束因子进行联合优化，得到所述目标空间内载体的定位信息。本发明通过上述方法将gnss/imu与激光slam相结合，利用激光slam在短时间内的高精度定位优势，gnss提供全局坐标，不存在误差累计的特点。本发明融合gnss、惯性测量传感器和激光雷达三种传感器的观测数据，实时的进行室内外无缝的高精度定位。
附图说明
44.为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施例中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动性的前提下，还可以根据这些附图获得其他的附图。
45.图1为本发明提供的融合gnss观测信息的激光slam定位方法的流程图。
具体实施方式
46.下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。
47.本发明的目的是提供一种融合gnss观测信息的激光slam定位方法及系统，将gnss/imu与激光slam相结合，能够实现室内外无缝定位并大幅提升定位精度与鲁棒性。
48.为使本发明的上述目的、特征和优点能够更加明显易懂，下面结合附图和具体实施方式对本发明作进一步详细的说明。
49.实施例一
50.如图1所示，本发明提供了一种融合gnss观测信息的激光slam定位方法，包括以下步骤：
51.步骤101：获取目标空间内载体的激光雷达数据、惯性传感器数据和gnss数据。
52.步骤102：基于惯性传感器数据构建惯性传感器数据约束因子。
53.步骤103：基于激光雷达数据构建激光雷达约束因子。
54.步骤104：基于gnss数据构建多普勒数据约束因子、测码伪距约束因子和钟差约束因子。
55.步骤105：基于滑动窗口优化法对惯性传感器数据约束因子、激光雷达约束因子、多普勒数据约束因子、测码伪距约束因子以及钟差约束因子进行联合优化，得到目标空间内载体的定位信息。
56.其中，步骤101中的激光雷达数据是采用载体上自带的激光雷达(3d lidar)采集到的；惯性传感器数据是采用惯性传感器(imu)采集到的；gnss数据为gnss接收机接收到的伪距以及多普勒信号。
57.其中，步骤102具体包括：
58.采用预积分处理的公式对激光雷达采集到的关键帧激光雷达数据间的惯性传感器数据进行预积分处理，得到惯性传感器数据约束因子；关键帧激光雷达数据为相邻两帧激光雷达数据的运动距离和旋转角度均大于相应的设定阈值的激光雷达数据；惯性传感器数据约束因子包括：激光雷达关键帧之间的位置、速度和旋转约束。
59.本实施例中，为减少计算量和计算机存储空间只存储激光关键帧数据；有时候3d lidar并没有运动，或者运动距离很少，就会造成数据冗余，造成不必要的计算，基于此，下面位姿优化与局部建图过程只对激光lidar关键帧数据进行处理。
60.激光lidar关键帧的判别根据距离信息以及旋转角度大小，运动超过5m或者旋转超过10
°
会被认为关键帧。
61.激光lidar扫描频率在10-30hz之间，imu接受数据的频率在100-500之间，为了防止重复积分，节约计算量，需要对雷达关键帧数据之间的imu加速度数据和角速度数据进行积分处理，如公式(1)所示，得到激光lidar关键帧之间的位置、速度和旋转约束，即imu约束因子(惯性传感器数据约束因子)。预积分处理的公式为：
[0062][0063]
表示imu测量的加速度与角速度测量值，i、i+1表示第k帧关键帧和第k+1帧关键帧之间imu测量数据的两个时刻，δt表示两个imu数据之间的时间间隔，分别代表加速度零偏和角速度零偏。分别代表基于关键帧bk在i时刻的坐标系下的位置、速度和四元数表示的姿态，位置、速度和四元数表示的姿态，分别代表基于关键帧bk在i+1时刻的坐标
系下的位置、速度和四元数表示的姿态。两关键帧之间预积分开始时刻，是三维零向量，是单位四元数，表示将四元数形式转为旋转矩阵。由预积分项构建的测量模型，即惯性传感器数据约束因子的计算公式如公式(2)所示，其中gw表示世界坐标系下的重力向量gw＝[0,0,g]
t
。
[0064][0065]
其中，分别表示关键帧bk与关键帧b
k+1
之间的位置、速度、角度预积分增量；是单位四元数；gw表示世界坐标系下的重力向量gw＝[0,0,g]
t
，g表示重力加速度；表示世界坐标系到bk关键帧的旋转矩阵；表示关键帧bk在世界坐标系的位置；表示关键帧b
k+1
在世界坐标系的位置；δtk表示关键帧bk与关键帧b
k+1
的时间差；表示关键帧bk在世界坐标系下的速度；表示的逆矩阵，表示世界坐标系到关键帧bk的旋转的四元数；表示关键帧b
k+1
到世界坐标系的旋转的四元数；表示的逆矩阵；表示第k帧的加速度零偏；表示第k帧的角速度零偏；表示第k+1帧的加速度零偏；表示第k+1帧的角速度零偏；惯性传感器数据约束因子为预积分处理后的数据。
[0066]
其中，步骤103具体包括：
[0067]
步骤1031：利用惯性传感器对激光雷达数据进行去畸变，得到去畸变激光雷达数据。
[0068]
1)采用时间戳和惯性传感器的运动轨迹确定在采集各帧激光雷达数据时激光雷达的位置和姿态，得到每帧激光雷达数据的坐标值。
[0069]
2)根据每帧激光雷达数据的时间间隔和该过程的运动位姿，进行坐标插值，计算每个激光点云对应时间的载体位姿，从而通过外参将每帧激光雷达数据的坐标值转换至该帧激光雷达数据起始时刻的激光雷达坐标系下，得到去畸变激光雷达数据。步骤1)和2)实现的利用惯性传感器对激光雷达数据进行去畸变。
[0070]
步骤1032：利用去畸变激光雷达数据与关键帧激光雷达数据生成三维点云地图。
[0071]
步骤1033：利用三维点云地图进行基于最近点迭代算法的匹配，得到激光雷达约束因子。
[0072]
步骤1032-1033具体包括：
[0073]
利用去畸变激光雷达数据进行线特征和面特征的提取，根据相邻关键帧间的线特征与面特征进行基于距离最小化得到位姿变换(r,t)(lidar约束因子)。利用相邻关键帧的位姿匹配作为初值，然后与submap进行匹配，得到精度更好的位姿结果，即得到激光雷达约
束因子。连续几个关键帧的线特征与面特征的集合构成局部子图(submap)，用于和当前帧点云提取的特征进行scan-submap匹配。
[0074]
其中，步骤104具体包括：
[0075]
1)多普勒数据约束因子的构建
[0076]
多普勒频移是根据接收到的载波信号与设计信号之间的差异来测量的，它反映了接收机-卫星沿信号传播路径的相对运动。由于gnss信号结构的特点，多普勒测量的精度通常比码伪距高一个数量级。多普勒频移建模为：
[0077][0078]
上式和分别表示接收机和卫星在eci坐标框架下的坐标，λ表示载波信号的波长。为eci帧中从接收机到卫星的单位矢量，表示卫星钟差的漂移率，可在卫星导航信息中得到，c代表光速，表示多普勒测量噪声。
[0079]
基于建模方程，tk时刻连接系统状态和卫星sj中与多普勒测量相关的残差可表示为其方差定义为其中ns表示广播卫星空间精度指数，n
dp
是接收机测量噪声指数，θ
el
表示接收端视图上的卫星高度角。
[0080]
2)测码伪距约束因子的构建
[0081]
码伪距测量建模公式可表示为：
[0082][0083]
式中和分别为卫星s和接收机r的eci坐标；ζs为4
×
1的指标向量，对应的卫星星座实体为1，其他三个实体为0；δts为卫星钟差，可由广播导航电文得出；和分别代表对流层与电离层延迟；表示由多路径误差造成的延迟；表示测量噪声，假定服从零均值的高斯分布零均值的高斯分布建模如下：
[0084][0085]ns
表示广播卫星空间精度指数；n
pr
表示接收机报告的码伪距测量噪声指数；θ
el
表示接收端视图上的卫星高度角。
[0086]
由于卫星码伪距信号发射与到达时刻存在时间差，代表ecef坐标框架下信号到达时刻卫星的位置，代表信号发射时刻卫星所在ecef坐标框架下的位置，两者之间存在下述变换关系：
[0087]
[0088]rz
(θ)表示围绕eci的z轴旋转θ角度，ωe表示地球自转角速度，tf是gnss信号的传播时间。最后，在tk时刻连接系统状态和卫星sj上测量的码伪距残差公式可表示为：
[0089]
其中rk表示tk时刻的gnss接收机。
[0090]
3)钟差约束因子
[0091]
在tk和t
k-1
处的接收机时钟偏差有以下关系：
[0092][0093]
其中i4×1表示4
×
1元素为1的矩阵，离散情况下的残差形式表示如下：
[0094][0095]
式中，是测量时刻k与k-1之间的时间差。
[0096]
接收机时钟漂移率被建模为随机游走过程，其残差形式如下：
[0097][0098]
其中，步骤105具体包括：基于惯性传感器数据约束因子、激光雷达约束因子、多普勒数据约束因子、测码伪距约束因子以及钟差约束因子，构建滑动窗口优化方程；利用l-m算法对滑动窗口优化方程进行求解，得到目标空间内载体的定位信息。
[0099]
待估计的系统状态包括：载体相对于局部世界坐标系下的位置姿态速度加速度偏差ba、磁力计偏差bw。局部世界坐标系与东北天(enu)坐标系航向夹角ψ、接收机钟差δt、接收机钟差漂移率
[0100]
本发明采用基于滑动窗口的图优化法对约束因子进行优化求解，窗口内的状态χ可以总结为：
[0101]
χ＝[x0,x1,...xn,ψ]
[0102][0103]
δt＝[δtg,δtr,δte,δtc]
[0104]
其中，n是窗口大小。δt中的四个分量对应：接收机相对于gps、glonass、galileo、北斗的时钟偏置。
[0105]
当系统传入新一时刻的观测，就会构建出新的约束因子，产生更多的待估系统状态量。但随着时间的推移，系统状态量不断增多，持续增加的计算负担会导致整个组合算法性能下降。为了有效的降低系统的计算量，本发明采用基于滑动窗口的因子图优化，在优化过程中只保留与系统最新时刻状态最为相关的状态量存储在窗口中，对于先前的历史状态则通过边缘化构造为对窗口内状态的先验约束项参与到后续滑窗的优化，从而最大化保留原有的观测信息。在本发明中将滑动窗口的窗口大小设置为10，即窗口中只保留最新10帧的系统状态，当窗口中的状态量数目达到窗口大小后，若新一帧观测到来则需要对窗口中“最老帧”进行边缘化，并将边缘化后产生的约束项加入先验约束项中。对历史状态的边缘
化，本发明采用舒尔补方法构建边缘化项。此外利用l-m算法对约束因子构建的非线性方程组进行优化求解，得到实时高精度位置，姿态，速度，时间等信息。
[0106]
实施例二
[0107]
为了执行上述实施例一对应的方法，以实现相应的功能和技术效果，下面了提供了一种融合gnss观测信息的激光slam定位系统，包括：
[0108]
数据获取模块，用于获取目标空间内载体的激光雷达数据、惯性传感器数据和gnss数据；
[0109]
惯性传感器数据约束因子构建模块，用于基于惯性传感器数据构建惯性传感器数据约束因子；
[0110]
激光雷达约束因子构建模块，用于基于激光雷达数据构建激光雷达约束因子；
[0111]
约束因子、测码伪距约束因子和钟差约束因子构建模块，用于基于gnss数据构建多普勒数据约束因子、测码伪距约束因子和钟差约束因子；
[0112]
优化模块，用于基于滑动窗口优化法对惯性传感器数据约束因子、激光雷达约束因子、多普勒数据约束因子、测码伪距约束因子以及钟差约束因子进行联合优化，得到目标空间内载体的定位信息。
[0113]
其中，惯性传感器数据约束因子构建模块，具体包括：
[0114]
预积分处理单元，用于采用预积分处理的公式对激光雷达采集到的关键帧激光雷达数据间的惯性传感器数据进行预积分处理，得到惯性传感器数据约束因子；关键帧激光雷达数据为相邻两帧激光雷达数据的运动距离和旋转角度均大于相应的设定阈值的激光雷达数据；惯性传感器数据约束因子包括：激光雷达关键帧之间的位置、速度和旋转约束。
[0115]
其中，激光雷达约束因子构建模块，具体包括：
[0116]
去畸变单元，用于利用惯性传感器对激光雷达数据进行去畸变，得到去畸变激光雷达数据；
[0117]
三维点云地图生成单元，用于利用去畸变激光雷达数据与关键帧激光雷达数据生成三维点云地图；
[0118]
匹配单元，用于利用三维点云地图进行基于最近点迭代算法的匹配，得到激光雷达约束因子。
[0119]
其中，去畸变单元，具体包括：
[0120]
坐标值确定子单元，用于采用时间戳和惯性传感器的运动轨迹确定在采集各帧激光雷达数据时激光雷达的位置和姿态，得到每帧激光雷达数据的坐标值；
[0121]
坐标值转换子单元，用于将每帧激光雷达数据的坐标值转换至每帧激光雷达数据起始时刻的激光雷达坐标系下，得到去畸变激光雷达数据。
[0122]
其中，优化模块，具体包括：
[0123]
滑动窗口优化方程构建单元，用于基于惯性传感器数据约束因子、激光雷达约束因子、多普勒数据约束因子、测码伪距约束因子以及钟差约束因子，构建滑动窗口优化方程；
[0124]
求解子单元，用于利用l-m算法对滑动窗口优化方程进行求解，得到目标空间内载体的定位信息。
[0125]
本说明书中各个实施例采用递进的方式描述，每个实施例重点说明的都是与其他
实施例的不同之处，各个实施例之间相同相似部分互相参见即可。对于实施例公开的系统而言，由于其与实施例公开的方法相对应，所以描述的比较简单，相关之处参见方法部分说明即可。
[0126]
本文中应用了具体个例对本发明的原理及实施方式进行了阐述，以上实施例的说明只是用于帮助理解本发明的方法及其核心思想；同时，对于本领域的一般技术人员，依据本发明的思想，在具体实施方式及应用范围上均会有改变之处。综上，本说明书内容不应理解为对本发明的限制。