CN112268559B - 复杂环境下融合slam技术的移动测量方法 - Google Patents

复杂环境下融合slam技术的移动测量方法 Download PDF

Info

Publication number
CN112268559B
CN112268559B CN202011139815.XA CN202011139815A CN112268559B CN 112268559 B CN112268559 B CN 112268559B CN 202011139815 A CN202011139815 A CN 202011139815A CN 112268559 B CN112268559 B CN 112268559B
Authority
CN
China
Prior art keywords
factor
map
sub
point cloud
gnss
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.)
Active
Application number
CN202011139815.XA
Other languages
English (en)
Other versions
CN112268559A (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.)
Information Engineering University of PLA Strategic Support Force
Original Assignee
Information Engineering University of PLA Strategic Support Force
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 Information Engineering University of PLA Strategic Support Force filed Critical Information Engineering University of PLA Strategic Support Force
Priority to CN202011139815.XA priority Critical patent/CN112268559B/zh
Publication of CN112268559A publication Critical patent/CN112268559A/zh
Application granted granted Critical
Publication of CN112268559B publication Critical patent/CN112268559B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
    • G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
    • G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
    • G01C21/165—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation combined with non-inertial navigation instruments
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S19/00—Satellite radio beacon positioning systems; Determining position, velocity or attitude using signals transmitted by such systems
    • G01S19/38—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system
    • G01S19/39—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system the satellite radio beacon positioning system transmitting time-stamped messages, e.g. GPS [Global Positioning System], GLONASS [Global Orbiting Navigation Satellite System] or GALILEO
    • G01S19/42—Determining position
    • G01S19/45—Determining position by combining measurements of signals from the satellite radio beacon positioning system with a supplementary measurement
    • G01S19/47—Determining position by combining measurements of signals from the satellite radio beacon positioning system with a supplementary measurement the supplementary measurement being an inertial measurement, e.g. tightly coupled inertial

Abstract

本发明涉及一种复杂环境下融合SLAM技术的移动测量方法，属于点云数据处理技术领域。测量方法包括：获取激光点云数据、载体的位姿；利用载体的位姿消除激光点云数据的运动畸变；进而通过SLAM技术进行子地图的配准，构建局部因子图；局部因子图包括里程计因子、IMU预积分因子、第一配准因子、小闭环因子、以及子地图；优化局部因子图后得到载体的局部运动轨迹以及优化后的子地图；以优化后的子地图为优化单元，构建全局因子图；全局因子图包括第二配准因子、连续配准因子、大闭环因子以及GNSS因子；优化全局因子图后，得到载体的全局运动轨迹以及全局地图。本发明通过将SLAM技术与GNSS/IMU定位技术融合，有效提升了定位轨迹的平滑度，并且提高了地图构建的精度。

Description

复杂环境下融合SLAM技术的移动测量方法
技术领域
本发明涉及一种复杂环境下融合SLAM技术的移动测量方法，属于点云数据处理技术领域。
背景技术
移动测量系统(Mobile Mapping System,MMS)通常由由车载平台、定位模块和与车体固连的激光雷达组成，定位模块为全球定位系统(Global Navigation SatelliteSystem,GNSS)和惯性导航系统(Inertial Navigation Syastem,INS)组合的定位定姿系统(Position and Orientation measurement System,POS)，也即POS定位模块。在车载平台正常移动的状态下，通过POS定位模块获得车载平台基于大地坐标系下的高精度6DoF位姿，该系统具有高精度、全天时、数据采集效率高等特点，已被广泛应用于城市环境的三维重建任务中。例如：高精地图(High Definition Map,HDM)的建立，HDM是无人驾驶和智慧城市构建等高新科技领域最基础，同时也是最关键的部分。衡量HDM制备的技术指标包括地图的精度和一致性，以及制备的效率等，一般而言，在地图的精度方面要求达到绝对偏差在10cm以下；在一致性方面要求地图中没有重影；在制备效率方面要求能够高效的构建和更新HDM。
然而，传统的MMS在复杂环境下主要面临三个问题：(1)POS定位模块依赖于GNSS/INS的组合导航设备，而该设备在诸如城市峡谷、繁茂森林、隧道和地下停车场等复杂极端环境下无法获得良好的卫星观测数据，定位精度受到严重影响，甚至无法正常工作。(2)系统所构建的点云地图的一致性不稳定，在定位随机误差较大的区域产生“点云重影”。(3)生成高精度点云地图前，往往需要对点云地图进行优化和校正，而对海量规模的点云数据进行整体优化将消耗大量时间和计算资源。
针对GNSS信号缺失导致复杂环境下的定位差、地图构建不准的问题，一般有以下两种解决办法：1.对多次观测的相同区域的点云地图进行配准，从而解决GNSS信号中断时的轨迹缺失。这种方法一方面要求测量系统必须对同一区域进行多次重复测量，这无疑在很多实际应用中是难以实现的；另一方面，这类方法需要大量的人工参与去识别相同的测量区域，并不断调整匹配结果以保证足够高的点云配准精度，耗费大量的人力资源。2.在环境中布设控制点，并利用INS进行定位，通过提取和识别控制点来控制INS的漂移；显然，这种方法要进行大量的前期测绘工作，这有悖于移动测量高效获取未知环境下地理空间信息的初衷。
为此，针对GNSS信号存在间断的复杂环境，有人提出一种伪GNSS/INS组合的定位方法，以解决MMS中POS定位模块对GNSS信号的过度依赖。其核心思路是利用实时定位与地图构建(Simultaneous Localization and Mapping,SLAM)技术得到GNSS缺失环境下的位姿估计量，将位姿估计量构造为伪GNSS信号。虽然该方法最大限度的保持了典型MMS的框架，将MMS模块化，但将SLAM的估计结果重构为GNSS信号增加了计算的消耗，并且在转换中可能造成精度损失，降低高精地图的构建精度。
发明内容
本申请的目的在于提供一种复杂环境下融合SLAM技术的移动测量方法，用以解决现有融合SLAM技术的移动测量方法计算量大、精度低的问题。
为实现上述目的，本申请提出了一种复杂环境下融合SLAM技术的移动测量方法的技术方案，包括以下步骤：
1)获取激光点云数据、以及通过GNSS/IMU组合导航获取载体的位姿，所述位姿包括旋转矩阵和平移矩阵；
2)利用载体的位姿消除激光点云数据的运动畸变；
3)将消除运动畸变的激光点云数据通过SLAM技术进行子地图的配准，进而构建局部因子图；所述局部因子图包括里程计因子、IMU预积分因子、点云与子地图配准的第一配准因子、小闭环因子、以及子地图；所述小闭环因子的构建过程为：在子地图的所有激光点云数据中搜索与当前激光点云匹配的候选帧，建立除连续配准约束外的小回环约束，以第一设定距离为依据进行小闭环检测，进而确认小回环约束，并构建小闭环因子；
4)以步骤3)中各因子的残差项等于0为目标优化局部因子图，得到载体的局部运动轨迹以及优化后的子地图；
5)以优化后的子地图为优化单元，构建全局因子图；所述全局因子图包括子地图与子地图配准的第二配准因子、连续配准因子、大闭环因子以及GNSS因子；所述大闭环因子的构建过程为：在所有高精子地图中搜索与当前高精子地图匹配的候选子地图，建立除连续配准约束外的大回环约束，以第二设定距离为依据进行大闭环检测，进而确认大回环约束，并构建大闭环因子；
6)以步骤5)中各因子的残差项等于0为目标优化全局因子图，得到载体的全局运动轨迹以及全局地图。
本发明的复杂环境下融合SLAM技术的移动测量方法的技术方案的有益效果是：本发明通过将SLAM技术与GNSS/IMU定位技术融合，使定位结果不单单依赖于组合导航设备，而是一种多约束条件的融合结果，有效提升了定位轨迹的平滑度，并且提高了地图构建的精度。本发明通过局部因子图优化和全局因子图优化，采用分层优化的数据实现数据的融合，在保证数据处理效率和精度的同时，提高地图构建的一致性，以及在复杂环境下的鲁棒性。本发明在构建全局因子图时，以子地图为基本优化单元，在保证核心约束的同时剔除图中信息冗余的边，将复杂的图优化模型最大程度的简化；同时将GNSS信号建模为GNSS因子，作为一种即插即用的空间约束，使测量系统在复杂环境下可以进行无缝定位，而无需进行定位模式的切换，实现了系统在复杂环境下的定位。
进一步的，所述载体的位姿通过GNSS/INS紧耦合算法计算得到；或者由INS直接积分得到。
进一步的，为了提高配准的准确性，所述步骤2)中进行配准的算法为基于点云表面元的迭代最近邻点算法。
进一步的，所述步骤4)中优化局部因子图的触发条件为：a.局部时间跨度大于等于设定时间阈值；b.GNSS信号良好。
进一步的，为了提高局部因子图的优化计算效率，对局部因子图进行优化时采用的方法为增量式优化计算方法。
进一步的，所述步骤6)中优化全局因子图的触发条件为：GNSS失锁时间大于等于失锁时间阈值。
进一步的，为了提高全局因子图的优化计算效率，对全局因子图进行优化时采用的方法为增量式优化计算方法。
附图说明
图1是本发明复杂环境下融合SLAM技术的移动测量方法的原理框图；
图2是本发明点云数据投影的第一深度图
Figure BDA0002737877950000031
和第一法矢图/>
Figure BDA0002737877950000032
图3是本发明渲染出的第二深度图
Figure BDA0002737877950000033
和第二法矢图/>
Figure BDA0002737877950000034
图4是本发明局部因子图的示意图；
图5是本发明子地图构建的示意图；
图6是本发明全局因子图的示意图。
具体实施方式
复杂环境下融合SLAM技术的移动测量方法实施例：
本发明的主要构思在于，获取激光雷达采集到的激光点云数据
Figure BDA0002737877950000035
以及GNSS/IMU组合导航设备在空旷环境下输出6自由度载体的位姿/>
Figure BDA0002737877950000041
或者在复杂环境下的IMU观测值/>
Figure BDA0002737877950000042
(同样也为位姿)；通过载体的位姿变换对激光点云数据进行畸变消除，并且融合了SLAM技术，估计得到载体在采样时刻i的完整的运动状态/>
Figure BDA0002737877950000043
其中，θi为在采样时刻i下，三维旋转矩阵对应的李代数；ρi为在采样时刻i下，三维平移矩阵对应的平移向量；vi为在采样时刻i下，载体的速度；同时还实现激光点云数据的整体拼接，得到高精度的全局地图/>
Figure BDA0002737877950000044
上述位姿
Figure BDA0002737877950000045
中，i为激光雷达的采样时刻，R为三维旋转矩阵，t为三维平移矩阵；/>
Figure BDA0002737877950000046
为观测量；
三维旋转矩阵R定义为特殊正交群SO(3)下的量，
Figure BDA0002737877950000047
Figure BDA0002737877950000048
为3×3的矩阵；
三维平移矩阵t为欧式空间下的三维向量；
三维旋转矩阵R对应的李代数θ定义为正交群
Figure BDA0002737877950000049
Figure BDA00027378779500000410
为3×1的向量(李代数存在Log(·)映射关系)；
位姿T定义为特殊欧式群SE(3)下的量：
Figure BDA00027378779500000411
Figure BDA00027378779500000412
为4×4的矩阵，/>
Figure BDA00027378779500000413
为三维平移矩阵；
位姿T对应的李代数为
Figure BDA00027378779500000414
李代数/>
Figure BDA00027378779500000415
定义为欧式群/>
Figure BDA00027378779500000416
下的量，/>
Figure BDA00027378779500000417
具体的，复杂环境下融合SLAM技术的移动测量方法如图1所示，包括以下步骤：
1)通过激光雷达实时获取激光点云数据，并通过GNSS/IMU组合导航设备获取连续时刻载体(载体即车载平台)的位姿对激光点云数据进行运动畸变的消除；得到无畸变的点云数据。
一帧点云中，同一激光扫描中每个点的确切时刻会根据其在扫描中的顺序而有所不同，一帧点云完整的扫描一般需要数个毫秒，包括不同时刻采集的激光点云数据(激光点云数据相应的坐标系的坐标原点为载体)，由于载体是移动的，导致一帧点云中的不同时刻的激光点云数据坐标系不同，进而导致每帧点云数据产生运动畸变。为此，需要对点云数据进行运动畸变的消除，得到无畸变点云数据。
消除畸变的过程为：首先通过GNSS/IMU组合导航设备得到连续时刻的载体的位姿，接着通过GNSS/IMU滤波器得到载体在时刻i-1到时刻i，也即[i-1,i]时间内的运动状态，然后通过点云滤波器将载体的运动状态补偿到每帧点云的点云数据中，消除畸变，包括旋转补偿和平移补偿。
例如：假设载体符合匀速运动模型，第i个时刻的激光点云数据为
Figure BDA0002737877950000051
第N个子地图中第i时刻的载体的位姿为/>
Figure BDA0002737877950000052
(若GNSS观测良好，/>
Figure BDA0002737877950000053
由GNSS/INS紧耦合算法计算得到；若GNSS信号失锁，/>
Figure BDA0002737877950000054
由INS直接积分得到)；其中，/>
Figure BDA0002737877950000055
为旋转矩阵；/>
Figure BDA0002737877950000056
为位移矩阵；通过GNSS/IMU滤波器得到的运动状态为/>
Figure BDA0002737877950000057
(也即第N个子地图中第i-1时刻到i时刻的预测位姿变换)，将运动状态为/>
Figure BDA0002737877950000058
作为运动先验信息，通过内插获得载体在第i-1时刻到i时刻中任意一个时刻k相对于第i-1时刻的预测位姿变换/>
Figure BDA0002737877950000059
将激光点云数据和位姿变换输入点云滤波器，而后将所有点统一至起始采样时刻，消除运动畸变。为了方便描述，后续的无畸变点云数据也用/>
Figure BDA00027378779500000510
表述。
本实施例中，激光雷达的采样频率为10Hz，也即1s采样10帧激光点云数据，在子地图的构建中，5s构建一张子地图，也即一张子地图中包含50帧的激光点云数据。实际上，在后续的步骤中才构建子地图，但是为了清楚的表述字符的含义，这里的
Figure BDA00027378779500000511
为后续第N个子地图中第i个时刻Ni对应的载体的位姿，第N个子地图的参考时刻Nref为该子地图的初始时刻。
2)若GNSS观测良好，则通过GNSS/IMU滤波器得到里程计因子；若GNSS信号失锁，则通过IMU预积分得到IMU预积分因子。
若GNSS观测良好，通过
Figure BDA00027378779500000512
在子地图参考系下的相对位姿/>
Figure BDA00027378779500000513
及其协方差ΣOdom得到里程计因子/>
Figure BDA00027378779500000514
其中，/>
Figure BDA00027378779500000515
为第N个子地图的参考时刻相对于第i时刻的θ的观测量变化量；/>
Figure BDA00027378779500000516
为第N个子地图的参考时刻相对于第i时刻的ρ的观测量变化量。
记EOdom为里程计因子残差项，具体表现形式如下：
Figure BDA00027378779500000517
其中，[EOdom]i为第i时刻的里程计因子残差；
Figure BDA0002737877950000061
为第N个子地图第i时刻相对于参考时刻的θ的观测量变化量；/>
Figure BDA0002737877950000062
为第N个子地图的参考时刻Nref下θ的预测量；/>
Figure BDA0002737877950000063
为第N个子地图第i时刻下θ的预测量；/>
Figure BDA0002737877950000064
为第N个子地图第i时刻下ρ的预测量；/>
Figure BDA0002737877950000065
为第N个子地图的参考时刻Nref下ρ的预测量；/>
Figure BDA0002737877950000066
为第N个子地图第i时刻相对于参考时刻的ρ的观测量变化量；/>
Figure BDA0002737877950000067
为李代数的和运算；运算符||A||B表示||A||B＝ATBA。
若GNSS信号失锁，IMU观测量则被单独用来进行预积分计算，得到虚拟预积分观测量：
Figure BDA0002737877950000068
其中，bi为第i个载体坐标系(也即第i个时刻对应的载体坐标系)；由于IMU输出频率高，相邻时刻之间存在若干个IMU观测量，任意一个用k表示；
Figure BDA0002737877950000069
为第i+1个载体坐标系相对于第i个载体坐标系在旋转部分的预积分分量，同时也表示物理意义上的R的观测量变化量；/>
Figure BDA00027378779500000610
为IMU的角速度观测量；gb为角速度计零偏量；Δt为采样间隔；/>
Figure BDA00027378779500000611
为第k+1个载体坐标系相对于第k个载体坐标系的v的观测量变化量；/>
Figure BDA00027378779500000612
为IMU的加速度观测量；ab为加速度计零偏量；/>
Figure BDA00027378779500000613
为第k+1个载体坐标系相对于第k个载体坐标系的p的观测量变化量，p为欧式空间下预积分分量中的平移量。
由于预积分因子是关于(ab,gb)T的变量，以各零偏量的一阶导数展开为关于零偏变化量(δab,δgb)T的线性形式有：
Figure BDA00027378779500000614
其中，
Figure BDA00027378779500000615
为第i+1个载体坐标系相对于第i个载体坐标系的R的预测量变化量；
Figure BDA00027378779500000616
为第i+1个载体坐标系相对于第i个载体坐标系的v的观测量变化量；/>
Figure BDA00027378779500000617
为第i+1个载体坐标系相对于第i个载体坐标系的v的预测量变化量；/>
Figure BDA00027378779500000618
为第i+1个载体坐标系相对于第i个载体坐标系的p的观测量变化量；/>
Figure BDA0002737877950000071
为第i+1个载体坐标系相对于第i个载体坐标系的p的预测量变化量。/>
记预积分观测的信息矩阵为ΣPre，预积分因子
Figure BDA0002737877950000072
用于描述在IMU采样间隔中的载体的相对运动情况，表达式如下：
Figure BDA0002737877950000073
与
Figure BDA0002737877950000074
类似，/>
Figure BDA0002737877950000075
的残差项EPre为：
Figure BDA0002737877950000076
其中，[EPre]i为第i时刻的预积分因子残差；
Figure BDA0002737877950000077
为第i个载体坐标系下θ的预测量；
Figure BDA0002737877950000078
为第i+1个载体坐标系下θ的预测量；/>
Figure BDA0002737877950000079
为第i+1个载体坐标系下v的预测量；/>
Figure BDA00027378779500000710
为第i个载体坐标系下v的预测量；g为大地坐标系下的重力加速度；/>
Figure BDA00027378779500000711
为第i+1个载体坐标系与第i个载体坐标系的时间差；/>
Figure BDA00027378779500000712
为第i+1个载体坐标系下ρ的预测量；/>
Figure BDA00027378779500000713
为第i个载体坐标系下ρ的预测量。
3)利用无畸变点云数据进行点云-子地图配准，得到第一配准因子；并构建小闭环因子。
点云-子地图配准。点云配准的目的是计算
Figure BDA00027378779500000714
到第N个子地图MN的相对变换/>
Figure BDA00027378779500000715
为提升配准计算的效率和关联点的准确度，采用基于点云表面元(Surfel)的迭代最近邻点算法(Iterative Closest Point,ICP)。
具体为：a.将
Figure BDA00027378779500000716
投影为如图2所示的第一深度图/>
Figure BDA00027378779500000717
并生成第一法矢图/>
Figure BDA00027378779500000718
并且得到第N个子地图中第i时刻相对于参考时刻的位姿相对变换/>
Figure BDA00027378779500000719
b.如图3所示，从第N个子地图MN中渲染出第N个子地图中第i时刻相对于参考时刻的预测位姿相对变换
Figure BDA00027378779500000720
的第二深度图/>
Figure BDA00027378779500000721
和第二法矢图/>
Figure BDA00027378779500000722
c.将第一深度图
Figure BDA00027378779500000723
与第二深度图/>
Figure BDA00027378779500000724
匹配，即可估计相对位姿变换。配准残差EP2M可由点vS沿子地图对应法矢方向nM到匹配点vM的距离表示：
Figure BDA00027378779500000725
其中，
Figure BDA00027378779500000726
匹配点vM可以直接索引Surfel图像的对应位置得到，而无需像传统ICP从所有点云中搜索临近点，大大提升计算效率。通过最小化EP2M可以建立非线性最小二乘问题，迭代求解得到第N个子地图第i时刻相对于第i-1时刻的观测位姿变换
Figure BDA0002737877950000081
将
Figure BDA0002737877950000082
作为点云配准的空间约束，得到协方差矩阵为[ΣReg]i的第一配准因子
Figure BDA0002737877950000083
需要说明的是，构建好的第一配准因子和里程计因子在形式上完全相同，协方差矩阵根据各因子的构建过程根据Lidar和里程计的观测噪声利用协方差传播得到。记EReg为残差项，则第N个子地图中第i帧(也即第i时刻)的点云/>
Figure BDA0002737877950000084
相对第1帧的点云/>
Figure BDA0002737877950000085
的第一配准因子残差EReg可写为：
Figure BDA0002737877950000086
小闭环因子的构建过程为：在子地图MN的所有点云
Figure BDA0002737877950000087
中搜索与当前点云存在匹配关系的候选帧，建立除连续配准约束外的回环约束。小闭环检测标准以距离Dthr(第一设定距离)为依据，只有两帧对应时刻的平移量小于Dthr时认为产生了闭环，即这两帧位于现实场景中的邻近位置。根据Dthr能够筛选出多个候选的闭环帧，为减小其数量，仅把相隔时间最长的两帧作为最终确认的候选帧，并通过几帧的连续追踪，进行确认。通过确认的回环约束可构建为小闭环因子/>
Figure BDA0002737877950000088
为：
Figure BDA0002737877950000089
小闭环因子
Figure BDA00027378779500000810
在形式上与第一配准因子/>
Figure BDA00027378779500000811
完全一致，其中，/>
Figure BDA00027378779500000812
为第N个子地图的参考时刻相对于第M个子地图的参考时刻的θ的观测量变化量；/>
Figure BDA00027378779500000813
第N个子地图的参考时刻相对于第M个子地图的参考时刻的ρ的观测量变化量。
小闭环因子
Figure BDA00027378779500000814
的残差项Eloop为：
Figure BDA00027378779500000815
其中，[Eloop]i为第i时刻的小闭环因子残差；j为通过回环检测到的帧号，也即回环检测到的时刻；
Figure BDA00027378779500000816
为第N个子地图第i个时刻相对于第j时刻的θ的观测量变化量；/>
Figure BDA00027378779500000817
为第N个子地图的参考时刻相对于第j时刻的θ的预测量变化量；/>
Figure BDA00027378779500000818
为第N个子地图第i个时刻相对于参考时刻的θ的预测量变化量；/>
Figure BDA0002737877950000091
为第N个子地图第i个时刻相对于第j时刻的ρ的观测量变化量；Σloop为小闭环因子的协方差矩阵。
4)根据里程计因子、IMU预积分因子、第一配准因子、小闭环因子、以及子地图构建如图4所示的局部因子图，通过优化局部因子图得到载体的局部运动轨迹以及优化后的子地图(高精子地图)，将优化后的子地图插入子地图列表中。
局部因子图优化的频率较低，局部优化的触发条件为：a.局部时间跨度Δt1足够长，大于等于设定时间阈值WT，即Δt1≥WT；b.GNSS信号良好，可以为新构建的子地图提供高精度参考。优化计算的目标函数为：
Figure BDA0002737877950000092
为降低精度较低的约束因子在优化计算中的不良影响，计算时采用一致性检验的增量式优化计算方法，即优化时依次插入一个约束节点，当该约束节点的插入导致残差显著增加时，则丢弃该因子，从而筛选出最优的一组约束因子。
子地图的构建过程如图5所示，将
Figure BDA0002737877950000093
按照优化后的载体运动状态/>
Figure BDA0002737877950000094
拼接得到子地图MN的点云，同时，子地图MN创建时以GNSS/INS组合导航设备给出的绝对位置-第N个子地图的参考时刻的位姿/>
Figure BDA0002737877950000095
为先验信息，以及优化后计算后得到的/>
Figure BDA0002737877950000096
与子地图MN的点云一并构成子地图MN的完整数据；/>
Figure BDA0002737877950000097
为第N+1个子地图的参考时刻相对于第N个子地图的参考时刻的观测位姿变换；/>
Figure BDA0002737877950000098
为第N个子地图的最后时刻相对于参考时刻的观测位姿变换。
5)以子地图列表中的子地图为优化单元，构建表征子地图-子地图配准的第二配准因子，以及连续配准因子、GNSS因子、大闭环因子，进而构建如图6所示的全局因子图。
第二配准因子为相邻高精子地图之间的配准因子。在第N个子地图的参考时刻位姿
Figure BDA0002737877950000099
渲染得到参考深度图/>
Figure BDA00027378779500000910
和参考法矢图/>
Figure BDA00027378779500000911
采用上述基于Surfel的ICP算法配准子地图得到第N个子地图的参考时刻相对于第N-1个子地图的参考时刻的位姿变换
Figure BDA00027378779500000912
其中，/>
Figure BDA00027378779500000913
为第N-1个子地图的参考时刻的位姿；/>
Figure BDA00027378779500000914
第N个子地图的参考时刻的位姿。
然而，当由于GNSS观测质量不佳导致子地图时间跨度很长时，这种局部渲染方法将丢掉大量点云信息，尤其是远离参考位置的点云。这时，采用经典的点云ICP匹配方法，尽管计算效率下降但确保了配准精度，这也是MMS最关注的。
同样地，将MN与MN-1之间的位姿变换
Figure BDA0002737877950000101
作为第二配准因子/>
Figure BDA0002737877950000102
Figure BDA0002737877950000103
其中，
Figure BDA0002737877950000104
第N个子地图的参考时刻相对于第N-1个子地图的参考时刻的θ的观测量变化量；/>
Figure BDA0002737877950000105
第N个子地图的参考时刻相对于第N-1个子地图的参考时刻的ρ的观测量变化量。
第二配准因子
Figure BDA0002737877950000106
的残差项ERegg可列为：
Figure BDA0002737877950000107
其中，[ERegg]N为第N个子地图的第二配准因子残差；
Figure BDA0002737877950000108
为第N个子地图的参考时刻的θ的预测量；/>
Figure BDA0002737877950000109
为第N-1个子地图的参考时刻的θ的预测量；/>
Figure BDA00027378779500001010
为第N个子地图的参考时刻的ρ的预测量；/>
Figure BDA00027378779500001011
为第N-1个子地图的参考时刻的ρ的预测量；ΣRegg为第二配准因子的协方差矩阵。
结合对相互连通的子地图的特殊设计，MN与MN-1之间的相对位姿同样可以由局部优化得到，即
Figure BDA00027378779500001012
将其作为连续配准因子/>
Figure BDA00027378779500001013
有助于提升轨迹的连续性，连续配准因子/>
Figure BDA00027378779500001014
Figure BDA00027378779500001015
其中，
Figure BDA00027378779500001016
为第N个子地图的最后时刻相对于参考时刻的θ的观测量变化量；/>
Figure BDA00027378779500001017
为第N个子地图的最后时刻相对于参考时刻的ρ的观测量变化量；
连续配准因子
Figure BDA00027378779500001018
的残差项EReckon可列为：
Figure BDA00027378779500001019
其中，[EReckon]N为第N个子地图的连续配准因子残差；
Figure BDA00027378779500001020
为第N-1个子地图的最后时刻相对于参考时刻的θ的观测量变化量；/>
Figure BDA00027378779500001021
为第N个子地图的参考时刻的θ的预测量；
Figure BDA00027378779500001022
为第N-1个子地图的参考时刻的θ的预测量；/>
Figure BDA00027378779500001023
为第N个子地图的参考时刻的ρ的预测量；/>
Figure BDA00027378779500001024
为第N-1个子地图的参考时刻的ρ的预测量；/>
Figure BDA00027378779500001025
为第N-1个子地图的最后时刻相对于参考时刻的ρ的观测量变化量；ΣReckon连续配准因子的协方差矩阵。
为了建立子地图之间的空间约束关系，构建了大闭环因子，使全局地图的整体一致性更强；为了建立子地图的绝对空间约束，构建了GNSS因子，使全局地图的精度更高。
大闭环因子的建立过程与小闭环因子的建立过程基本一致，唯一区别在于闭环检测的对象是优化后的子地图而非激光点云，具体为：大闭环检测标准以距离Dthr2(第二设定距离)为依据，只有两子地图对应时刻的平移量小于Dthr2时认为产生了闭环，即这两子地图相邻。根据Dthr2能够筛选出多个候选的闭环子地图，为减小其数量，仅把相隔时间最长的两子地图作为最终确认的候选子地图，并通过几个子地图的连续追踪，进行确认。采用子地图进行闭环检测可以在保证建立正确子地图间拓扑关系的同时有效降低约束个数，从而提升优化计算的效率。大闭环因子
Figure BDA0002737877950000111
与第二配准因子基本相同，具体如下：
Figure BDA0002737877950000112
其中，M为闭环地图的索引值，
Figure BDA0002737877950000113
第N个子地图的参考时刻相对于第M个子地图的参考时刻的θ的观测量变化量；/>
Figure BDA0002737877950000114
第N个子地图的参考时刻相对于第M个子地图的参考时刻的ρ的观测量变化量。
大闭环因子
Figure BDA0002737877950000115
的残差项Eloopp为：
Figure BDA0002737877950000116
其中，[Eloopp]N为第N个子地图的大闭环因子残差；
Figure BDA0002737877950000117
为第N个子地图的参考时刻的θ的预测量；/>
Figure BDA0002737877950000118
为第M个子地图的参考时刻的θ的预测量；/>
Figure BDA0002737877950000119
为第M个子地图的参考时刻的ρ的预测量；/>
Figure BDA00027378779500001110
为第N个子地图的参考时刻的ρ的预测量；Σloopp为大闭环因子的协方差矩阵。
GNSS因子
Figure BDA00027378779500001111
由于GNSS因子只有位置观测，没有姿态，因此就是一个三维点坐标，其表现形式与残差项[EGNSS]N相同，为全局因子图中的位姿节点提供了空间位置的先验信息，且GNSS因子为即插即用的约束因子，使测量系统可以在复杂环境下稳定运行，GNSS因子的残差项EGNSS可写为：
Figure BDA00027378779500001112
其中，[EGNSS]N为第N个子地图的GNSS因子残差；
Figure BDA00027378779500001113
为第N个子地图的参考时刻的ρ的预测量；/>
Figure BDA00027378779500001114
为第N个子地图的参考时刻的ρ的观测量；ΣGNSS为GNSS因子的协方差矩阵。
6)对全局因子图进行优化，得到全局运动轨迹以及全局地图，以完成待检测物的测量。
全局优化与地图更新。采用增量式平滑算法对构建的全局因子图
Figure BDA0002737877950000121
进行优化(如图3右图)，优化仅在长时间GNSS失锁(GNSS失锁时间大于等于失锁时间阈值)或检测到闭环使触发，而后所有子地图的位姿可整体优化计算得到：
Figure BDA0002737877950000122
其中，n为子地图的总数。
由于在局部优化中各帧点云相对局部地图的位姿
Figure BDA0002737877950000123
已经确定了，因此在全局优化得到各参考位置的优化位姿/>
Figure BDA0002737877950000124
后，各帧点云的位姿/>
Figure BDA0002737877950000125
也可直接推得。
本发明将复杂的移动测量方法进行分层优化，使得全局因子图在保持紧密的同时更加简单化，本发明的移动测量方法融合了SLAM技术，提升了定位轨迹的连续性和所构建地图的一致性。

Claims (7)

1.一种复杂环境下融合SLAM技术的移动测量方法，其特征在于，包括以下步骤：
1)获取激光点云数据、以及通过GNSS/INS组合导航获取载体的位姿，所述位姿包括旋转矩阵和平移矩阵；
2)利用载体的位姿消除激光点云数据的运动畸变；
3)将消除运动畸变的激光点云数据通过SLAM技术进行子地图的配准，进而构建局部因子图；所述局部因子图包括里程计因子、IMU预积分因子、点云与子地图配准的第一配准因子、小闭环因子、以及子地图；所述小闭环因子的构建过程为：在子地图的所有激光点云数据中搜索与当前激光点云匹配的候选帧，建立除连续配准约束外的小回环约束，以第一设定距离为依据进行小闭环检测，进而确认小回环约束，并构建小闭环因子；
4)以步骤3)中各因子的残差项等于0为目标优化局部因子图，得到载体的局部运动轨迹以及优化后的子地图；
5)以优化后的子地图为优化单元，构建全局因子图；所述全局因子图包括子地图与子地图配准的第二配准因子、连续配准因子、大闭环因子以及GNSS因子；所述大闭环因子的构建过程为：在所有高精子地图中搜索与当前高精子地图匹配的候选子地图，建立除连续配准约束外的大回环约束，以第二设定距离为依据进行大闭环检测，进而确认大回环约束，并构建大闭环因子；
6)以步骤5)中各因子的残差项等于0为目标优化全局因子图，得到载体的全局运动轨迹以及全局地图。
2.根据权利要求1所述的复杂环境下融合SLAM技术的移动测量方法，其特征在于，在GNSS观测良好时，所述载体的位姿通过GNSS/INS紧耦合算法计算得到；或者在GNSS信号失锁时，由INS直接积分得到。
3.根据权利要求1所述的复杂环境下融合SLAM技术的移动测量方法，其特征在于，所述步骤2)中进行配准的算法为基于点云表面元的迭代最近邻点算法。
4.根据权利要求1所述的复杂环境下融合SLAM技术的移动测量方法，其特征在于，所述步骤4)中优化局部因子图的触发条件为：a.局部时间跨度大于等于设定时间阈值；b.GNSS信号良好。
5.根据权利要求1或4所述的复杂环境下融合SLAM技术的移动测量方法，其特征在于，对局部因子图进行优化时采用的方法为增量式优化计算方法。
6.根据权利要求1所述的复杂环境下融合SLAM技术的移动测量方法，其特征在于，所述步骤6)中优化全局因子图的触发条件为：GNSS失锁时间大于等于失锁时间阈值。
7.根据权利要求1或6所述的复杂环境下融合SLAM技术的移动测量方法，其特征在于，对全局因子图进行优化时采用的方法为增量式优化计算方法。
CN202011139815.XA 2020-10-22 2020-10-22 复杂环境下融合slam技术的移动测量方法 Active CN112268559B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202011139815.XA CN112268559B (zh) 2020-10-22 2020-10-22 复杂环境下融合slam技术的移动测量方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202011139815.XA CN112268559B (zh) 2020-10-22 2020-10-22 复杂环境下融合slam技术的移动测量方法

Publications (2)

Publication Number Publication Date
CN112268559A CN112268559A (zh) 2021-01-26
CN112268559B true CN112268559B (zh) 2023-03-28

Family

ID=74341537

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202011139815.XA Active CN112268559B (zh) 2020-10-22 2020-10-22 复杂环境下融合slam技术的移动测量方法

Country Status (1)

Country Link
CN (1) CN112268559B (zh)

Families Citing this family (10)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN112819744A (zh) * 2021-02-26 2021-05-18 中国人民解放军93114部队 Gnss和视觉slam融合的轨迹测量方法和装置
CN113252042B (zh) * 2021-05-18 2023-06-13 杭州迦智科技有限公司 一种隧道内基于激光和uwb定位方法、装置
CN113269878B (zh) * 2021-05-26 2023-04-07 上海新纪元机器人有限公司 一种基于多传感器的建图方法及系统
CN113671522B (zh) * 2021-07-07 2023-06-27 中国人民解放军战略支援部队信息工程大学 基于语义约束的动态环境激光slam方法
CN113959437A (zh) * 2021-10-14 2022-01-21 重庆数字城市科技有限公司 一种用于移动测量设备的测量方法及系统
CN114322994B (zh) * 2022-03-10 2022-07-01 之江实验室 一种基于离线全局优化的多点云地图融合方法和装置
CN115183778A (zh) * 2022-07-01 2022-10-14 北京斯年智驾科技有限公司 一种基于码头石墩的建图方法、装置、设备以及介质
CN114897942B (zh) * 2022-07-15 2022-10-28 深圳元戎启行科技有限公司 点云地图的生成方法、设备及相关存储介质
CN115265523B (zh) * 2022-09-27 2023-01-03 泉州装备制造研究所 机器人同时定位与建图方法、装置及可读介质
CN115561731B (zh) * 2022-12-05 2023-03-10 安徽蔚来智驾科技有限公司 位姿优化方法、点云地图建立方法、计算机设备及介质

Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
WO2015126499A2 (en) * 2013-12-02 2015-08-27 Andrew Irish Systems and methods for gnss snr probabilistic localization and 3-d mapping
CN110375738A (zh) * 2019-06-21 2019-10-25 西安电子科技大学 一种融合惯性测量单元的单目同步定位与建图位姿解算方法
CN110443836A (zh) * 2019-06-24 2019-11-12 中国人民解放军战略支援部队信息工程大学 一种基于平面特征的点云数据自动配准方法及装置
CN110706279A (zh) * 2019-09-27 2020-01-17 清华大学 基于全局地图与多传感器信息融合的全程位姿估计方法
CN111561923A (zh) * 2020-05-19 2020-08-21 北京数字绿土科技有限公司 基于多传感器融合的slam制图方法、系统

Family Cites Families (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CA3032812A1 (en) * 2016-08-04 2018-02-08 Reification Inc. Methods for simultaneous localization and mapping (slam) and related apparatus and systems

Patent Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
WO2015126499A2 (en) * 2013-12-02 2015-08-27 Andrew Irish Systems and methods for gnss snr probabilistic localization and 3-d mapping
CN110375738A (zh) * 2019-06-21 2019-10-25 西安电子科技大学 一种融合惯性测量单元的单目同步定位与建图位姿解算方法
CN110443836A (zh) * 2019-06-24 2019-11-12 中国人民解放军战略支援部队信息工程大学 一种基于平面特征的点云数据自动配准方法及装置
CN110706279A (zh) * 2019-09-27 2020-01-17 清华大学 基于全局地图与多传感器信息融合的全程位姿估计方法
CN111561923A (zh) * 2020-05-19 2020-08-21 北京数字绿土科技有限公司 基于多传感器融合的slam制图方法、系统

Non-Patent Citations (1)

* Cited by examiner, † Cited by third party
Title
基于滑动窗迭代最大后验估计的多源组合导航因子图融合算法;徐昊玮等;《兵工学报》;20190430(第04期);全文 *

Also Published As

Publication number Publication date
CN112268559A (zh) 2021-01-26

Similar Documents

Publication Publication Date Title
CN112268559B (zh) 复杂环境下融合slam技术的移动测量方法
US11519729B2 (en) Vision-aided inertial navigation
CN110412635B (zh) 一种环境信标支持下的gnss/sins/视觉紧组合方法
Alonso et al. Accurate global localization using visual odometry and digital maps on urban environments
Lenac et al. Fast planar surface 3D SLAM using LIDAR
Li et al. Real-time 3D motion tracking and reconstruction system using camera and IMU sensors
CN107167826B (zh) 一种自动驾驶中基于可变网格的图像特征检测的车辆纵向定位系统及方法
CN113406682B (zh) 一种定位方法、装置、电子设备及存储介质
CN112734852A (zh) 一种机器人建图方法、装置及计算设备
CN110187375A (zh) 一种基于slam定位结果提高定位精度的方法及装置
CN114526745A (zh) 一种紧耦合激光雷达和惯性里程计的建图方法及系统
CN113674412B (zh) 基于位姿融合优化的室内地图构建方法、系统及存储介质
CN114019552A (zh) 一种基于贝叶斯多传感器误差约束的定位置信度优化方法
Dawood et al. Harris, SIFT and SURF features comparison for vehicle localization based on virtual 3D model and camera
CN110412596A (zh) 一种基于图像信息和激光点云的机器人定位方法
CN112946681A (zh) 融合组合导航信息的激光雷达定位方法
CN112731503A (zh) 一种基于前端紧耦合的位姿估计方法及系统
CN116753948A (zh) 一种基于视觉惯性gnss ppp耦合的定位方法
Forno et al. Techniques for improving localization applications running on low-cost IoT devices
CN116124161A (zh) 一种基于先验地图的LiDAR/IMU融合定位方法
CN113227713A (zh) 生成用于定位的环境模型的方法和系统
Das et al. Pose-graph based crowdsourced mapping framework
Zhu et al. Camera, LiDAR, and IMU Based Multi-Sensor Fusion SLAM: A Survey
Wang et al. GIVE: A Tightly Coupled RTK-Inertial-Visual State Estimator for Robust and Precise Positioning
Mounier et al. High-Precision Positioning in GNSS-Challenged Environments: A LiDAR-Based Multi-Sensor Fusion Approach with 3D Digital Maps Registration

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