CN117168441B - 一种多传感器融合的slam定位和重建方法及*** - Google Patents
一种多传感器融合的slam定位和重建方法及*** Download PDFInfo
- Publication number
- CN117168441B CN117168441B CN202311445001.2A CN202311445001A CN117168441B CN 117168441 B CN117168441 B CN 117168441B CN 202311445001 A CN202311445001 A CN 202311445001A CN 117168441 B CN117168441 B CN 117168441B
- Authority
- CN
- China
- Prior art keywords
- matrix
- laser
- laser radar
- imu
- information
- 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
Links
- 238000000034 method Methods 0.000 title claims abstract description 52
- 230000004927 fusion Effects 0.000 title claims abstract description 24
- 239000011159 matrix material Substances 0.000 claims description 105
- 230000033001 locomotion Effects 0.000 claims description 43
- 238000001514 detection method Methods 0.000 claims description 22
- 238000005457 optimization Methods 0.000 claims description 17
- 238000007781 pre-processing Methods 0.000 claims description 16
- 230000004807 localization Effects 0.000 claims description 11
- 238000004040 coloring Methods 0.000 claims description 9
- 230000008569 process Effects 0.000 claims description 9
- 230000017105 transposition Effects 0.000 claims description 6
- 230000008859 change Effects 0.000 claims description 5
- 230000000452 restraining effect Effects 0.000 claims description 4
- 230000008878 coupling Effects 0.000 claims description 3
- 238000010168 coupling process Methods 0.000 claims description 3
- 238000005859 coupling reaction Methods 0.000 claims description 3
- 239000003550 marker Substances 0.000 claims description 2
- 230000000295 complement effect Effects 0.000 abstract description 2
- 238000013461 design Methods 0.000 abstract description 2
- 102000008115 Signaling Lymphocytic Activation Molecule Family Member 1 Human genes 0.000 description 36
- 108010074687 Signaling Lymphocytic Activation Molecule Family Member 1 Proteins 0.000 description 36
- 230000006870 function Effects 0.000 description 9
- 238000010586 diagram Methods 0.000 description 8
- 230000000007 visual effect Effects 0.000 description 8
- 230000001133 acceleration Effects 0.000 description 6
- 238000004590 computer program Methods 0.000 description 6
- 230000000694 effects Effects 0.000 description 6
- 238000013507 mapping Methods 0.000 description 5
- 238000004422 calculation algorithm Methods 0.000 description 4
- 238000004891 communication Methods 0.000 description 4
- 238000005516 engineering process Methods 0.000 description 4
- 238000012545 processing Methods 0.000 description 4
- 238000013519 translation Methods 0.000 description 4
- 239000013598 vector Substances 0.000 description 4
- 238000004364 calculation method Methods 0.000 description 3
- 238000006073 displacement reaction Methods 0.000 description 3
- 230000007613 environmental effect Effects 0.000 description 3
- 238000011160 research Methods 0.000 description 3
- 230000003190 augmentative effect Effects 0.000 description 2
- 238000010276 construction Methods 0.000 description 2
- 238000005286 illumination Methods 0.000 description 2
- 230000008447 perception Effects 0.000 description 2
- 230000005856 abnormality Effects 0.000 description 1
- 239000008186 active pharmaceutical agent Substances 0.000 description 1
- 230000009286 beneficial effect Effects 0.000 description 1
- 230000005540 biological transmission Effects 0.000 description 1
- 238000006243 chemical reaction Methods 0.000 description 1
- 238000012937 correction Methods 0.000 description 1
- 238000011156 evaluation Methods 0.000 description 1
- 238000000605 extraction Methods 0.000 description 1
- 239000011521 glass Substances 0.000 description 1
- 238000007689 inspection Methods 0.000 description 1
- 239000004973 liquid crystal related substance Substances 0.000 description 1
- 230000007774 longterm Effects 0.000 description 1
- 238000013178 mathematical model Methods 0.000 description 1
- 238000005259 measurement Methods 0.000 description 1
- 230000003287 optical effect Effects 0.000 description 1
- 238000002310 reflectometry Methods 0.000 description 1
- 238000009877 rendering Methods 0.000 description 1
- 239000004065 semiconductor Substances 0.000 description 1
- 230000007704 transition Effects 0.000 description 1
Classifications
-
- Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
- Y02—TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
- Y02T—CLIMATE CHANGE MITIGATION TECHNOLOGIES RELATED TO TRANSPORTATION
- Y02T10/00—Road transport of goods or passengers
- Y02T10/10—Internal combustion engine [ICE] based vehicles
- Y02T10/40—Engine management systems
Landscapes
- Navigation (AREA)
Abstract
本申请公开了一种多传感器融合的SLAM定位和重建方法，可以互补激光视觉传感器的优势，同时在缺少视觉数据的情况下***依然能够正常运行，相比于仅耦合的激光视觉设计拥有更灵活的优势；而且本申请相比于目前主流的SLAM多传感器融合方案不但拥有更高的定位精度和更快的运行速度，而且拥有更好的综合性能。
Description
技术领域
本发明涉及机器人智能感知技术领域，具体涉及一种多传感器融合的SLAM定位和重建方法及***。
背景技术
同时定位与建图(Simultaneous Localization and Mapping，SLAM)，是指搭载特定传感器的主体在没有环境先验的情况下，在运动过程中估计自己的运动同时建立环境地图。自主导航***中通过SLAM可以使机器人能够在未知的环境中移动和定位，同时还能够建立环境地图。在增强现实中SLAM能过将虚拟对象叠加在真实世界中，通过使用相机和其他传感器来跟踪用户的移动并在场景中添加虚拟对象；在救援和巡检任务中，SLAM可以为无人机提供导航和定位功能；总的来说，SLAM技术具有广泛的应用前景和重要的研究意义，可以为自主导航、增强现实、无人机和安防等领域提供有效的解决方案。
从传感器类型上来看，SLAM可以分为视觉SLAM和激光SLAM，其中，激光SLAM使用激光传感器获取环境三维点云数据，而视觉SLAM使用相机获取环境图像数据。相比之下，激光SLAM获得的数据更加精确，视觉SLAM对于环境的要求更高，需要较好的光照条件和对比度，所以激光传感器和视觉传感器相比，激光传感器在环境三维信息获取上具有绝对优势。
由于激光雷达获取到的是稠密三维空间点的坐标信息，因此，可以直接通过坐标点的投影实现SLAM场景重建功能。但是激光雷达还有如下缺陷：1)在雨天场景中，激光雷达测量到的激光反射点可能会被雨滴反射而产生虚假点云，从而影响后续的数据处理和场景重建，2)激光雷达无法感知透明或反光的物体，如镜子、玻璃等，这会对机器人的感知和导航造成一定的限制，3)激光雷达并不能捕获环境的色彩信息，使得重建出来的三维模型视觉观感不理想。
发明内容
鉴于现有技术中的上述缺陷或不足，期望提供一种多传感器融合的SLAM定位和重建方法及***。
第一方面，本申请实施例提供了一种多传感器融合的SLAM定位和重建方法，该方法包括：
S1：对可见光相机数据进行预处理，将预处理后的可见光相机数据通过相机与激光雷达外参同步到激光点云中，为激光雷达每次扫描得到的激光点赋色；
S2：计算激光雷达两次扫描间IMU的相对运动量，通过卡尔曼滤波器得到机器人姿态估计的先验信息；
S3：通过卡尔曼滤波器更新IMU先验信息得到LIO最终后验状态量，即两次激光雷达扫描帧间的相对运动信息，通过叠加相对运动信息得到机器人的运动轨迹；
S4：通过卡尔曼滤波器融合GPS与S3中得到的运动轨迹；
S5：对步骤S4进行回环检测；
S6：将后验状态叠加到激光点云上，为每一次激光扫描结合GPS信息赋予绝对的位置信息后得到三维重建图；
S7：将三维重建图发送到终端。
在其中一个实施例中，所述步骤S1包括：通过EnlightenGAN对可见光相机数据进行预处理，将预处理后的可见光相机数据通过相机与激光雷达外参同步到激光点云中，为激光雷达每次扫描得到的激光点赋色。
在其中一个实施例中，所述步骤S2包括：
计算激光雷达两次扫描间IMU的相对运动量；
通过IMU消除激光雷达的固有畸变；
通过卡尔曼滤波器耦合激光雷达和IMU信息，通过二者信息联合估计机器人姿态信息。
在其中一个实施例中，在步骤S3中，所述通过卡尔曼滤波器更新IMU先验信息得到LIO最终后验状态量，包括：
S31：构造先验协方差矩阵 其中，Fx表示激光点误差，/>是i-1时刻的后验协方差矩阵；Fw是IMU噪声系数矩阵，Qi是噪声方差矩阵；
S32：定义卡尔曼增益矩阵为gK，其中，R是观测噪声矩阵，H是观测噪声系数矩阵，J是用于约束待优化状态量并随之迭代的矩阵，右上角-1表示矩阵的逆运算，右上角T表示矩阵的转置运算，所以R-1表示观测噪声的逆矩阵，JT表示J的转置，HT符号同理；
第k次迭代可以表示为如下公式：
矩阵中的δθIk是第k次迭代IMU姿态旋转分量与先验姿态间的变化量，δθLk是IMU和激光雷达旋转外参在迭代中的变化关系，03×3是3行3列的全零矩阵，03×15是3行15列的全零矩阵，I3×3是3行3列的单位矩阵，I15×15是15行15列的单位矩阵，(δθIk)-T是δθIk的转置后求逆运算，(δθLk)-T同理；
S33：第k次迭代的后验状态如下：
其中，/>表示i时刻第k次迭代的后验状态，/>表示i时刻第k次迭代之前的后验状态，运算符/>根据状态量域的不同，含义也有所不同，在李群域表示相乘，在李代数域表示相加。运算符/>是运算符/>的反过程，gK表示卡尔曼增益矩阵，Zk表示第k次迭代的状态量残差矩阵，I表示3×3的单位矩阵，H是观测噪声系数矩阵，/>表示i时刻迭代优化前的后验状态量与i时刻状态量迭代优化前的先验状态量之间的变化量；
S34：第k次迭代的后验方程可以表示为：
其中I表示3×3的单位矩阵，gK表示卡尔曼增益矩阵，H是观测噪声系数矩阵，/>是i时刻先验协方差矩阵，雅可比矩阵J是用于约束待优化状态量并随之迭代的矩阵，右上角T表示矩阵的转置运算。当迭代达到退出条件或者指定次数后即可退出迭代，得到最终的后验状态。
在其中一个实施例中，在步骤S4之前，该方法还包括：对融合的GPS坐标进行转换。
在其中一个实施例中，所述步骤S5包括：采用ScanContext对步骤S4进行闭环检测，当检测到回环之后，对回环区域进行全局优化。
在其中一个实施例中，所述步骤S6包括：将步骤S4得到的运动轨迹投影到世界坐标系；
投影后的坐标记为WPi＝(x，y，z)。Pi表示第i个三维坐标点，左上角W表示该坐标点位于世界坐标系或全局坐标系，与之对应的还有激光雷达坐标系L、IMU坐标系I及相机坐标系C，x，y，z分别表示三个坐标轴的值。
在其中一个实施例中，所述步骤S7包括：通过互联网将三维重建图发送到终端。
第二方面，本申请实施例提供了一种多传感器融合的SLAM定位和重建***，该***包括：
预处理模块，用于对可见光相机数据进行预处理，将预处理后的可见光相机数据通过相机与激光雷达外参同步到激光点云中，为激光雷达每次扫描得到的激光点赋色；
计算模块，用于计算激光雷达两次扫描间IMU的相对运动量，通过卡尔曼滤波器得到机器人姿态估计的先验信息；
更新模块，用于通过卡尔曼滤波器更新IMU先验信息得到LIO最终后验状态量，即两次激光雷达扫描帧间的相对运动信息，通过叠加相对运动信息得到机器人的运动轨迹；
融合模块，用于通过卡尔曼滤波器融合GPS与S3中得到的运动轨迹；
检测模块，用于对步骤S4进行回环检测；
叠加模块，用于将后验状态叠加到激光点云上，为每一次激光扫描结合GPS信息赋予绝对的位置信息后得到三维重建图；
发送模块，用于将三维重建图发送到终端。
本申请的有益效果包括：
本申请提供的多传感器融合的SLAM定位和重建方法，可以互补激光视觉传感器的优势，同时在缺少视觉数据的情况下***依然能够正常运行，相比于仅耦合的激光视觉设计拥有更灵活的优势；而且本申请相比于目前主流的SLAM多传感器融合方案不但拥有更高的定位精度和更快的运行速度，而且拥有更好的综合性能。
附图说明
通过阅读参照以下附图所作的对非限制性实施例所作的详细描述，本申请的其它特征、目的和优点将会变得更明显：
图1示出了本申请实施例提供的多传感器融合的SLAM定位和重建方法的流程示意图；
图2示出了根据本申请一个实施例的多传感器融合的SLAM定位和重建***200的示例性结构框图；
图3示出了根据本申请实施例提供的又一流程示意图；
图4示出了本申请图像预处理步骤低光图像增强示意图；
图5示出了本申请移动端互联互通原理流程图；
图6示出了本申请在M2DGR数据集上的绝对轨迹误差图；
图7示出了本申请的最终重建效果示意图；
图8示出了适于用来实现本申请实施例的终端设备的计算机***的结构示意图。
具体实施方式
下面结合附图和实施例对本申请作进一步的详细说明。可以理解的是，此处所描述的具体实施例仅仅用于解释相关申请，而非对该申请的限定。另外还需要说明的是，为了便于描述，附图中仅示出了与申请相关的部分。
需要说明的是，在不冲突的情况下，本申请中的实施例及实施例中的特征可以相互组合。下面将参考附图并结合实施例来详细说明本申请。
请参考图1所示，图1示出了本申请实施例提供的一种多传感器融合的SLAM定位和重建方法，该方法包括：
步骤110：对可见光相机数据进行预处理，将预处理后的可见光相机数据通过相机与激光雷达外参同步到激光点云中，为激光雷达每次扫描得到的激光点赋色；
步骤120：计算激光雷达两次扫描间IMU的相对运动量，通过卡尔曼滤波器得到机器人姿态估计的先验信息；
步骤130：通过卡尔曼滤波器更新IMU先验信息得到LIO最终后验状态量，即两次激光雷达扫描帧间的相对运动信息，通过叠加相对运动信息得到机器人的运动轨迹；
步骤140：通过卡尔曼滤波器融合GPS与S3中得到的运动轨迹；
步骤150：对步骤S4进行回环检测；
步骤160：将后验状态叠加到激光点云上，为每一次激光扫描结合GPS信息赋予绝对的位置信息后得到三维重建图；
步骤170：将三维重建图发送到终端。
示例性的，如图3所示，本发明主要分为三大Module，即Module A为激光惯性里程计部分，利用卡尔曼滤波器融合IMU和激光雷达特征并得到激光惯性里程计的后验状态；Module B为GPS模块，将GPS的WGS84坐标通过UTM坐标转换到东北天坐标系后与激光惯性里程计进行融合，这样可以使激光惯性里程计定位精度更高、建图质量更好；Module C为视觉特征关联模块，首先将可见光相机与激光雷达进行时间戳同步后，提取与激光雷达扫描对应的相机帧，将相机帧进行预处理、特征提取等操作后与激光惯性里程计后验输出进行关联，计算激光惯性里程计后验状态投影到视觉模块的误差，并进行BA优化，最终得到多传感器融合的***后验状态。将最终的后验状态送入回环检测模块等待回环的产生并进行优化，与此同时进行三维RGB场景重建，将最终重建和定位效果实时展示在手机端。
其中方法的具体实施步骤如下：
步骤110：
对可见光相机数据进行预处理，并将预处理后的可见光相机视觉信息通过相机与激光雷达外参同步到激光点云中，为激光点云赋色。预处理采用EnlightenGAN对图像进行增强处理，改善图像亮暗部细节特征。EnlightenGAN是一种基于生成对抗网络(GAN)的图像增强技术，它可以将暗淡的图像转换为更明亮、更清晰的图像，同时保持图像的自然性和真实感。该技术由中国科学院自动化研究所的研究团队提出，是对传统图像增强技术的一种重要进步。
EnlightenGAN是利用无监督生成对抗网络实现的低光图像增强的开源算法，主要思想是通过生成对抗网络来学习输入图像和目标图像之间的映射关系，从而实现图像的增强。具体来说，它包含两个关键部分：生成器和判别器。生成器接收暗淡的图像作为输入，并生成一个更明亮、更清晰的图像，以尽可能地匹配目标图像。而判别器则评估生成器生成的图像与目标图像之间的相似度，并尝试将它们区分开来。
利用EnlightenGAN可以很好的改善相机图像的光线质量。通过EnlightenGAN的预处理，画质改善效果前后对比如图4所示。
为了实现RGB重建世界坐标点，只需要知道每个激光点的颜色即可。为了获取每个激光点的颜色，需要将激光点投影到视觉的像素平面，在像素平面查找这个激光点对应的RGB通道。记相机坐标系下的三维点为cPi＝(x，y，z)，激光系到相机系的外参数为CLR和CLt，分别表示旋转和平移，可以通过如下公式将激光系三维点投影为相机系三维点CPi：
cPi＝cLR-1(LPi-CLt)
步骤120：
IMU先验位姿估计；IMU作为惯性负载，将它和激光雷达融合起来不仅可以弥补激光雷达在姿态估计中的不确定性还可以一定程度消除激光点云的畸变，同时可以辅助激光雷达更好的应对复杂的场景。本方法利用香港大学火星实验室在FASTLIO2中提出的卡尔曼滤波器紧耦合激光雷达和IMU信息，通过二者信息联合估计机器人姿态信息，同时利用IMU消除激光雷达的固有畸变。卡尔曼滤波器是一种基于概率模型的递归滤波器，通过将测量数据和模型预测结合起来，得到对***状态的估计，适用于实时控制等要求实时性的场景，它的数学模型相对简单，实现也相对容易，适合嵌入式***等资源受限的环境。
记IMU进行状态估计时，状态量
IMU传感器的输入/>分别表示角速度与加速度，IMU传感器的噪声/>分别表示角速度噪声，加速度噪声，角速度偏置噪声，加速度偏置噪声。状态量R和t表示位姿信息，即旋转和平移，左上标I表示惯性系下的参数，左上标IL表示激光系到惯性系下的外参数。vI表示惯性传感器的速度，/>和/>分别表示惯性传感器角速度偏置和加速度偏置，/>为惯性系下重力加速度的向量形式。
由于机器人的状态量是个叠加的概念，通过IMU直接解算出来的状态量是上一时刻到当前时刻的变化量，所以要得到当前时刻的状态，还需要将这个变化量叠加上一时刻的状态：
其中，运算符根据状态量表示域的不同，含义也有所不同，在李群域表示相乘，在李代数域表示相加。令起始时刻惯性系位姿作为世界系的起点，后续状态量转换世界系坐标时均在这个起始位姿上做累加。xi表示第i时刻的状态量，i-1表示i时刻前一时刻的状态量，后文表示均如此。
通过卡尔曼滤波器进行状态量的估计时，首先假设i时刻噪声wi＝0，通过卡尔曼先验状态估计公式可以得到：
其中，表示先验状态量，/>表示后验状态量，ui表示i时刻传感器IMU传感器的输入，0表示忽略噪声的理想情况，后续将专门引入噪声进行优化。状态转移矩阵f可以表示为如下公式：
其中，Δt表示相邻两个时刻间的时间差，03×1表示三个维度全零向量，R24表示24维度矩阵，f(x，u，w)中的每一行用于约束状态量x中的每一列，公式中的其余字母含义前文均有说明，这里不在赘述。
根据激光雷达扫描原理，在运动过程中激光点云难免会出现畸变，利用IMU高频率特效可以一定程度的消除这种运动畸变，大致的过程是利用IMU在微小时间内的加速度来修正激光雷达点云位移，角速度修正方向，对于激光点Pi修正第i个激光点P′i的过程可以表示为如下公式：
其中，Ri和ti表示第i个扫描点时IMU的相对旋转和平移，其余符号含义前文已作说明，其中带有∨符号的量表示先验信息，带有符号∧的量表示后验信息。
步骤130：
激光雷达后验姿态更新，利用卡尔曼滤波器更新IMU先验信息得到LIO最终后验状态量得到两次激光雷达扫描帧间的相对运动信息。步骤S1中假设了噪声不存在，但实际情况是存在噪声的，这时就需要激光雷达对当前状态下的环境进行观测进而优化先验状态得到后验状态。
首先需要构造先验协方差矩阵如下所示：
其中，Fx表示激光点误差，主要是先验状态计算得到的当前时刻场景激光点和激光雷达实际观测的场景激光点之间的误差。Fw是IMU噪声系数矩阵，Qi是噪声方差矩阵，通常是预设值。是i-1时刻的后验协方差矩阵，右上角T表示转置运算。
为了不和前文的相机内参数矩阵K混淆，这里定义卡尔曼增益矩阵为gK。在优化先验阶段，首先需要求得一个卡尔曼增益gK，如下所示：
其中，R是观测噪声矩阵，H是观测噪声系数矩阵，J是用于约束待优化状态量并随之迭代的矩阵，右上角T表示矩阵的转置运算，右上角-1表示矩阵的逆运算，第k次迭代可以表示为如下公式：
矩阵中的δθIk是第k次迭代IMU姿态旋转分量与先验姿态间的变化量，δθLk是IMU和激光雷达旋转外参在迭代中的变化关系，03×3是3行3列的全零矩阵，03×15是3行15列的全零矩阵，I3×3是3行3列的单位矩阵，I15×15是15行15列的单位矩阵，(δθIk)-T是δθIk的转置后求逆运算，(δθLk)-T同理。
第k次迭代的后验状态如下：
其中，表示i时刻第k次迭代的后验状态，/>表示i时刻第k次迭代之前的后验状态，运算符/>根据状态量域的不同，含义也有所不同，在李群域表示相乘，在李代数域表示相加。运算符/>是运算符/>的反过程，gK表示卡尔曼增益矩阵，Zk表示第k次迭代的状态量残差矩阵，I表示3×3的单位矩阵，H是观测噪声系数矩阵，/>表示i时刻迭代优化前的后验状态量与i时刻状态量迭代优化前的先验状态量之间的变化量；第k次迭代的后验方程可以表示为：
其中，I表示3×3的单位矩阵，gK表示卡尔曼增益矩阵，H是观测噪声系数矩阵，是i时刻先验协方差矩阵，雅可比矩阵J是用于约束待优化状态量并随之迭代的矩阵，右上角T表示矩阵的转置运算。当迭代达到退出条件或者指定次数后即可退出迭代，得到最终的后验状态。
当迭代达到退出条件或者指定次数后即可退出迭代，得到最终的后验状态。
步骤140：
室外场景相比于室内场景通常GPS信号较好，本方法通过引入GPS，可以使机器人执行室外长期任务时轨迹不发生严重的累计漂移，和回环检测有异曲同工的效果，不同之处在于回环检测与优化仅在***产生回环之后有效。
不同于IMU的东北天坐标系，GPS为WGS84坐标系，即经纬海拔高度坐标系，所以在融合前还需要进行坐标转换。这里本方法使用UTM坐标系，即通用横墨卡托格网***。它将地球展开分为若干网格，每个网格都有独立的东北天坐标。通过卡尔曼滤波器融合GPS传感器数据，这样可以保障***的运行效率最大化。
引入GPS观测后，卡尔曼增益为：
其中，gK表示卡尔曼增益矩阵，H是观测噪声系数矩阵，是先验方差矩阵，R是观测噪声矩阵，右上角-1表示矩阵的逆运算，右上角T表示矩阵的转置运算。
由于GPS的海拔高度信息误差较大，实测中短时间内漂移量高达十米以上，故舍弃GPS的海拔高度通道。第k帧的残差rk使用UTM系下GPS和IMU先验估计东和北方向的距离(tx，ty)计算：
该相对距离差为相邻两时刻GPS坐标之间的位移，即第i时刻相对于i-1时刻的位移，IMU相对距离差同理。其中，表示IMU东向距离，/>表示IMU北向距离，gpstx表示GPS东向距离，gpsty表示GPS北向距离。
后验状态的通用形式为：
将误差和卡尔曼增益带入即可得到最终融合GPS的后验状态。其中，为后验协方差矩阵，I为三维单位矩阵，gK表示卡尔曼增益矩阵，H是观测噪声系数矩阵，/>为先验协方差矩阵，/>为后验状态，/>为先验状态，ri为残差。
步骤150：
回环检测的目的是判断是否到达过先前的位置。由于误差的存在，往往到达先前的位置后里程计估计的结果并未与之跟上。通过回环检测，在某一时刻到达先前所经过的位置时，会将这个信息反馈给后端，后端会把里程计当前位姿拉到先前位置，从而一定程度消除了累计误差；
产生累计误差是SLAM***长时间运行难以避免的情况，即使加入GPS约束也只能将累计误差限制在一定范围，并不能消除，闭环检测是消除累计误差比较好的方案。当***运行至之前所经过的地方，但是由于累计误差的影响，当前轨迹并没有和之间轨迹重合，闭环检测就可以很好的解决这个问题。本方法采用ScanContext进行闭环检测，当检测到回环之后，会对回环区域进行全局优化。回环模块输入的参数主要包含后验状态、激光扫描和GPS。GPS用于对回环范围进行限制并且参与到了全局优化中。
对输入的每个激光扫描进行降采样和编码，最终生成强度矩阵I。对编码生成的强度矩阵与已生成的强度矩阵进行相似度打分，如下式所示：
其中，Iq表示候选强度矩阵，Ic表示当前强度矩阵，和/>分别表示这两个矩阵对应的列向量。当分数小于一定阈值时就认为产生了回环，阈值默认设置为0.2。若Iq和Ic具有回环关系，使用ICP计算与之对应的激光雷达相对运动关系(R，t)，运动关系可以表示为如下公式：
LPc＝RLPq+t
其中，LRq为回环优化前的激光雷达坐标系下三维坐标点，LPc为回环优化后的激光雷达坐标系下三维坐标点，R是旋转矩阵，t是平移向量。
将构成回环关系的两帧对应节点和姿态约束加入因子图，使用GTSAM来求解最终姿态。
ICP计算过程可以描述为：
1)计算两组激光点的质心位置p，p′，然后计算每个点的去质心坐标：qi＝pi-p，q′i＝p′i-p′
2)构造优化问题，解算旋转矩阵R*：
3)根据2的R解算t：t*＝p-R*p′
步骤S6：
三维重建，将后验状态叠加在激光点云上，为每一次激光扫描结合GPS信息赋予绝对的位置信息；
对于激光雷达每次扫描得到的激光点LPi，利用步骤S4得到的后验状态将激光点投影到世界坐标系，投影后的点记为WPi＝(x，y，z)，投影关系如下式所示：
Pi表示第i个三维坐标点，左上角W表示该坐标点位于世界坐标系或全局坐标系，与之对应的还有激光雷达坐标系L、IMU坐标系I及相机坐标系C，x，y，z分别表示三个坐标轴的值。
步骤S7：
移动端互联互通，SLAM通过网络连接集成至Android端APP，使定位、建图结果手机端可以实时显示。
本发明SLAM与移动端互通流程如图5所示。移动端需要向SLAM端发起TCP连接请求，经过TCP协议“三次握手”完成移动端与SLAM端的连接。SLAM端会持续向移动端发送后验状态量和世界系下点云数据供移动端展示。
对于移动端定位功能，利用收到的后验状态量调用百度地图API在世界地图中进行实时位置的显示。对于建图功能，移动端会将收到的点云数据通过OpenGL图形渲染库实时渲染至屏幕。为了防止数据传输过程中异常的发生，算法引入了数据包头和校验位对接收到的数据可靠性进行双重校验，确保接收数据不会出现错误。
视觉传感器能够提供高分辨率、高精度的图像信息，但容易受到光照等因素的影响。激光传感器则能够提供高精度的三维点云信息，但分辨率较低，对于透明或反射性较强的物体容易失效。将两种传感器的信息进行融合，可以充分利用它们各自的优势。本发明将相机、激光雷达、IMU和GPS进行了融合，实现了机器人的高精定位和重建功能，具有较快的运行速度和较高的定位精度和重建质量。
在M2DGR数据集上对本方法进行全方位的试验评估，M2DGR数据集是上海交通大学研究团队创办的由地面机器人收集的大规模视觉激光数据集，包含32线3D激光雷达、GNSS、RTK/GNSS-IMU和多个多种类型的相机。该数据集总共36个序列，总时长3.73小时，场景包含户外、室内和室内外交替，环境特征涵盖了夜间和白天。与同类主流的SLAM方法比较结果如下表1所示，轨迹误差图如图6所示。
表1
由表1和图6可以看出与其他同类方法和流行的SLAM方法相比，本发明方法精度更稳定，误差控制的极低，在精度上具有较强的竞争力。表中的X表示序列继续初始化失败或成功跟踪少于60％的帧。图6中x、y轴分别表示大地坐标系东和北方向，虚线表示真实轨迹，实线表示算法预测的轨迹。可以看出本方法的运行轨迹与真实轨迹基本重合，整条轨迹无明显累计误差。
因为SLAM的工程应用对于实时的要求极高。比如辅助驾驶中，SLAM可以用于汽车对周围事物的实时感知，可以立体的绘制障碍物和汽车在当前时刻及历史时刻的空间关系，从而形象的展示给驾驶员或者做出紧急预案。如果SLAM时延较高，可能会影响到汽车对当前时刻周围事物的判断，从而失去了一部分应用价值，下表2为本方法和其它方法在私有数据集上运行在intel i7平台的时耗情况。
表2
表2中R3Live的LIO总耗时约为150毫秒，VIO耗时约为20毫秒，***总的平均耗时约为242毫秒。本方法的平均耗时为74毫秒。从表2可以看出本发明方法平均耗时只有R3LIVE和VLOAM的三分之一，相比于同类算法，本方法具有更强的综合实力。本发明方法最终重建效果示意图如图7所示。
应当注意，尽管在附图中以特定顺序描述了本申请方法的操作，但是，这并非要求或者暗示必须按照该特定顺序来执行这些操作，或是必须执行全部所示的操作才能实现期望的结果。附加地或备选地，可以省略某些步骤，将多个步骤合并为一个步骤执行，和/或将一个步骤分解为多个步骤执行。
进一步地，请参考图2，图2示出了本申请一个实施例的一种多传感器融合的SLAM定位和重建***，该***包括：
预处理模块210，用于对可见光相机数据进行预处理，将预处理后的可见光相机数据通过相机与激光雷达外参同步到激光点云中，为激光雷达每次扫描得到的激光点赋色；
计算模块220，用于计算激光雷达两次扫描间IMU的相对运动量，通过卡尔曼滤波器得到机器人姿态估计的先验信息；
更新模块230，用于通过卡尔曼滤波器更新IMU先验信息得到LIO最终后验状态量，即两次激光雷达扫描帧间的相对运动信息，通过叠加相对运动信息得到机器人的运动轨迹；
融合模块240，用于通过卡尔曼滤波器融合GPS与S3中得到的运动轨迹；
检测模块250，用于对步骤S4进行回环检测；
叠加模块260，用于将后验状态叠加到激光点云上，为每一次激光扫描结合GPS信息赋予绝对的位置信息后得到三维重建图；
发送模块270，用于将三维重建图发送到终端。
应当理解，***200中记载的诸单元或模块与参考图1描述的方法中的各个步骤相对应。由此，上文针对方法描述的操作和特征同样适用于***200及其中包含的单元，在此不再赘述。***200可以预先实现在电子设备的浏览器或其他安全应用中，也可以通过下载等方式而加载到电子设备的浏览器或其安全应用中。***200中的相应单元可以与电子设备中的单元相互配合以实现本申请实施例的方案。
下面参考图8，其示出了适于用来实现本申请实施例的终端设备或服务器的计算机***300的结构示意图。
如图8所示，计算机***300包括中央处理单元(CPU)301，其可以根据存储在只读存储器(ROM)302中的程序或者从存储部分308加载到随机访问存储器(RAM)303中的程序而执行各种适当的动作和处理。在RAM 303中，还存储有***300操作所需的各种程序和数据。CPU 301、ROM 302以及RAM 303通过总线304彼此相连。输入/输出(I/O)接口305也连接至总线304。
以下部件连接至I/O接口305：包括键盘、鼠标等的输入部分306；包括诸如阴极射线管(CRT)、液晶显示器(LCD)等以及扬声器等的输出部分307；包括硬盘等的存储部分308；以及包括诸如LAN卡、调制解调器等的网络接口卡的通信部分309。通信部分309经由诸如因特网的网络执行通信处理。驱动器310也根据需要连接至I/O接口305。可拆卸介质311，诸如磁盘、光盘、磁光盘、半导体存储器等等，根据需要安装在驱动器310上，以便于从其上读出的计算机程序根据需要被安装入存储部分308。
特别地，根据本公开的实施例，上文参考图1描述的过程可以被实现为计算机软件程序或以HTTP接口形式提供相关处理服务。例如，本公开的实施例包括一种计算机程序产品，其包括有形地包含在机器可读介质上的计算机程序，计算机程序包含用于执行图1的方法的程序代码。在这样的实施例中，该计算机程序可以通过通信部分309从网络上被下载和安装，和/或从可拆卸介质311被安装。
附图中的流程图和框图，图示了按照本申请各种实施例的***、方法和计算机程序产品的可能实现的体系架构、功能和操作。在这点上，流程图或框图中的每个方框可以代表一个模块、程序段、或代码的一部分，前述模块、程序段、或代码的一部分包含一个或多个用于实现规定的逻辑功能的可执行指令。也应当注意，在有些作为替换的实现中，方框中所标注的功能也可以以不同于附图中所标注的顺序发生。例如，两个接连地表示的方框实际上可以基本并行地执行，它们有时也可以按相反的顺序执行，这依所涉及的功能而定。也要注意的是，框图和/或流程图中的每个方框、以及框图和/或流程图中的方框的组合，可以用执行规定的功能或操作的专用的基于硬件的***来实现，或者可以用专用硬件与计算机指令的组合来实现。
描述于本申请实施例中所涉及到的单元或模块可以通过软件的方式实现，也可以通过硬件的方式来实现。所描述的单元或模块也可以设置在处理器中，例如，可以描述为：一种处理器包括第一子区域生成单元、第二子区域生成单元以及显示区域生成单元。其中，这些单元或模块的名称在某种情况下并不构成对该单元或模块本身的限定，例如，显示区域生成单元还可以被描述为“用于根据第一子区域和第二子区域生成文本的显示区域的单元”。
作为另一方面，本申请还提供了一种计算机可读存储介质，该计算机可读存储介质可以是上述实施例中前述装置中所包含的计算机可读存储介质；也可以是单独存在，未装配入设备中的计算机可读存储介质。计算机可读存储介质存储有一个或者一个以上程序，前述程序被一个或者一个以上的处理器用来执行描述于本申请的应用于透明窗口信封的文本生成方法。
以上描述仅为本申请的较佳实施例以及对所运用技术原理的说明。本领域技术人员应当理解，本申请中所涉及的申请范围，并不限于上述技术特征的特定组合而成的技术方案，同时也应涵盖在不脱离前述申请构思的情况下，由上述技术特征或其等同特征进行任意组合而形成的其它技术方案。例如上述特征与本申请中公开的(但不限于)具有类似功能的技术特征进行互相替换而形成的技术方案。
Claims (7)
1.一种多传感器融合的SLAM定位和重建方法，其特征在于，该方法包括：
S1：对可见光相机数据进行预处理，将预处理后的可见光相机数据通过相机与激光雷达外参同步到激光点云中，为激光雷达每次扫描得到的激光点赋色；
S2：计算激光雷达两次扫描间IMU的相对运动量，通过卡尔曼滤波器得到机器人姿态估计的先验信息；
S3：通过卡尔曼滤波器更新IMU先验信息得到LIO最终后验状态量，即两次激光雷达扫描帧间的相对运动信息，通过叠加相对运动信息得到机器人的运动轨迹；
S4：通过卡尔曼滤波器融合GPS与S3中得到的运动轨迹；
S5：对步骤S4进行回环检测；
S6：将后验状态叠加到激光点云上，为每一次激光扫描结合GPS信息赋予绝对的位置信息后得到三维重建图；
S7：将三维重建图发送到终端；
所述步骤S1包括：
通过EnlightenGAN对可见光相机数据进行预处理，将预处理后的可见光相机数据通过相机与激光雷达外参同步到激光点云中，为激光雷达每次扫描得到的激光点赋色；
所述步骤S2包括：
计算激光雷达两次扫描间IMU的相对运动量；
通过IMU消除激光雷达的固有畸变；
通过卡尔曼滤波器耦合激光雷达和IMU信息，通过二者信息联合估计机器人姿态信息。
2.根据权利要求1所述的多传感器融合的SLAM定位和重建方法，其特征在于，在步骤S3中，所述通过卡尔曼滤波器更新IMU先验信息得到LIO最终后验状态量，包括：
S31：构造先验协方差矩阵 其中，Fx表示激光点误差，/>是i-1时刻的后验协方差矩阵，Fw是IMU噪声系数矩阵，Qi是噪声方差矩阵，FT表示矩阵的转置运算；
S32：定义卡尔曼增益矩阵为gK，其中，R是观测噪声矩阵，H是观测噪声系数矩阵，HT表示观测噪声系数矩阵的转置运算，R-1表示观测噪声的逆矩阵，J是用于约束待优化状态量并随之迭代的矩阵，右上角-1表示对应矩阵的逆运算，右上角T表示对应矩阵的转置运算；
第k次迭代可以表示为如下公式：
矩阵中的δθIk是第k次迭代IMU姿态旋转分量与先验姿态间的变化量，δθLk是IMU和激光雷达旋转外参在迭代中的变化关系，03×3是3行3列的全零矩阵，03×15是3行15列的全零矩阵，I3×3是3行3列的单位矩阵，I15×15是15行15列的单位矩阵，(δθIk)-T是δθIk的转置后求逆运算，(δθLk)-T是δθLk的转置后求逆运算；
S33：第k次迭代的后验状态如下：
，其中，/>表示i时刻第k次迭代的后验状态，/>表示i时刻第k次迭代之前的后验状态，运算符/>根据状态量域的不同，含义也有所不同，在李群域表示相乘，在李代数域表示相加，运算符/>是运算符/>的反过程，gK表示卡尔曼增益矩阵，Zk表示第k次迭代的状态量残差矩阵，I表示3×3的单位矩阵，H是观测噪声系数矩阵，/>表示i时刻迭代优化前的后验状态量与i时刻状态量迭代优化前的先验状态量之间的变化量；
S34：第k次迭代的后验方差可以表示为：
其中I表示3×3的单位矩阵，gK表示卡尔曼增益矩阵，H是观测噪声系数矩阵，/>是i时刻先验协方差矩阵，雅可比矩阵J是用于约束待优化状态量并随之迭代的矩阵，右上角T表示矩阵的转置运算；当迭代达到退出条件或者指定次数后即可退出迭代，得到最终的后验状态。
3.根据权利要求1所述的多传感器融合的SLAM定位和重建方法，其特征在于，在步骤S4之前，该方法还包括：
对融合的GPS坐标进行转换。
4.根据权利要求1所述的多传感器融合的SLAM定位和重建方法，其特征在于，所述步骤S5包括：
采用ScanContext对步骤S4进行闭环检测，当检测到回环之后，对回环区域进行全局优化。
5.根据权利要求1所述的多传感器融合的SLAM定位和重建方法，其特征在于，所述步骤S6包括：
将步骤S4得到的运动轨迹投影到世界坐标系；
投影后的坐标记为WPi＝(x，y，z)，Pi表示第i个三维坐标点，左上角W表示该坐标点位于世界坐标系或全局坐标系，与之对应的还有激光雷达坐标系L、IMU坐标系I及相机坐标系C，x，y，z分别表示三个坐标轴的值。
6.根据权利要求1所述的多传感器融合的SLAM定位和重建方法，其特征在于，所述步骤S7包括：
通过互联网将三维重建图发送到终端。
7.一种多传感器融合的SLAM定位和重建***，其特征在于，该***包括：
预处理模块，用于对可见光相机数据进行预处理，将预处理后的可见光相机数据通过相机与激光雷达外参同步到激光点云中，为激光雷达每次扫描得到的激光点赋色；
计算模块，用于计算激光雷达两次扫描间IMU的相对运动量，通过卡尔曼滤波器得到机器人姿态估计的先验信息；
更新模块，用于通过卡尔曼滤波器更新IMU先验信息得到LIO最终后验状态量，即两次激光雷达扫描帧间的相对运动信息，通过叠加相对运动信息得到机器人的运动轨迹；
融合模块，用于通过卡尔曼滤波器融合GPS与S3中得到的运动轨迹；
检测模块，用于对步骤S4进行回环检测；
叠加模块，用于将后验状态叠加到激光点云上，为每一次激光扫描结合GPS信息赋予绝对的位置信息后得到三维重建图；
发送模块，用于将三维重建图发送到终端；
所述预处理模块包括：
通过EnlightenGAN对可见光相机数据进行预处理，将预处理后的可见光相机数据通过相机与激光雷达外参同步到激光点云中，为激光雷达每次扫描得到的激光点赋色；
所述计算模块包括：
计算激光雷达两次扫描间IMU的相对运动量；
通过IMU消除激光雷达的固有畸变；
通过卡尔曼滤波器耦合激光雷达和IMU信息，通过二者信息联合估计机器人姿态信息。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202311445001.2A CN117168441B (zh) | 2023-11-02 | 2023-11-02 | 一种多传感器融合的slam定位和重建方法及*** |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202311445001.2A CN117168441B (zh) | 2023-11-02 | 2023-11-02 | 一种多传感器融合的slam定位和重建方法及*** |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN117168441A CN117168441A (zh) | 2023-12-05 |
| CN117168441B true CN117168441B (zh) | 2024-02-20 |
Family
ID=88947201
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202311445001.2A Active CN117168441B (zh) | 2023-11-02 | 2023-11-02 | 一种多传感器融合的slam定位和重建方法及*** |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN117168441B (zh) |
Citations (7)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN110261870A (zh) * | 2019-04-15 | 2019-09-20 | 浙江工业大学 | 一种用于视觉-惯性-激光融合的同步定位与建图方法 |
| CN111045017A (zh) * | 2019-12-20 | 2020-04-21 | 成都理工大学 | 一种激光和视觉融合的巡检机器人变电站地图构建方法 |
| CN112987065A (zh) * | 2021-02-04 | 2021-06-18 | 东南大学 | 一种融合多传感器的手持式slam装置及其控制方法 |
| CN113091771A (zh) * | 2021-04-13 | 2021-07-09 | 清华大学 | 一种激光雷达-相机-惯导联合标定方法及*** |
| CN113706626A (zh) * | 2021-07-30 | 2021-11-26 | 西安交通大学 | 一种基于多传感器融合及二维码校正的定位与建图方法 |
| CN113837277A (zh) * | 2021-09-24 | 2021-12-24 | 东南大学 | 一种基于视觉点线特征优化的多源融合slam*** |
| WO2022077296A1 (zh) * | 2020-10-14 | 2022-04-21 | 深圳市大疆创新科技有限公司 | 三维重建方法、云台负载、可移动平台以及计算机可读存储介质 |
-
2023
- 2023-11-02 CN CN202311445001.2A patent/CN117168441B/zh active Active
Patent Citations (7)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN110261870A (zh) * | 2019-04-15 | 2019-09-20 | 浙江工业大学 | 一种用于视觉-惯性-激光融合的同步定位与建图方法 |
| CN111045017A (zh) * | 2019-12-20 | 2020-04-21 | 成都理工大学 | 一种激光和视觉融合的巡检机器人变电站地图构建方法 |
| WO2022077296A1 (zh) * | 2020-10-14 | 2022-04-21 | 深圳市大疆创新科技有限公司 | 三维重建方法、云台负载、可移动平台以及计算机可读存储介质 |
| CN112987065A (zh) * | 2021-02-04 | 2021-06-18 | 东南大学 | 一种融合多传感器的手持式slam装置及其控制方法 |
| CN113091771A (zh) * | 2021-04-13 | 2021-07-09 | 清华大学 | 一种激光雷达-相机-惯导联合标定方法及*** |
| CN113706626A (zh) * | 2021-07-30 | 2021-11-26 | 西安交通大学 | 一种基于多传感器融合及二维码校正的定位与建图方法 |
| CN113837277A (zh) * | 2021-09-24 | 2021-12-24 | 东南大学 | 一种基于视觉点线特征优化的多源融合slam*** |
Non-Patent Citations (3)
| Title |
|---|
| 刘向臻.基于视觉激光融合惯导的SLAM室外场景定位和重建技术研究.全文. * |
| 无人驾驶车辆视觉SLAM方法综述;李希宇;仲首任;马芳武;;汽车文摘(第07期);全文 * |
| 融合激光与视觉点云信息的定位与建图方法;张伟伟;陈超;徐军;;计算机应用与软件(第07期);全文 * |
Also Published As
| Publication number | Publication date |
|---|---|
| CN117168441A (zh) | 2023-12-05 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN109887057B (zh) | 生成高精度地图的方法和装置 | |
| US8761439B1 (en) | Method and apparatus for generating three-dimensional pose using monocular visual sensor and inertial measurement unit | |
| US11064178B2 (en) | Deep virtual stereo odometry | |
| US11227395B2 (en) | Method and apparatus for determining motion vector field, device, storage medium and vehicle | |
| Chien et al. | Visual odometry driven online calibration for monocular lidar-camera systems | |
| CN109631911B (zh) | 一种基于深度学习目标识别算法的卫星姿态转动信息确定方法 | |
| CN110749308B (zh) | 使用消费级gps和2.5d建筑物模型的面向slam的室外定位方法 | |
| CN112556719A (zh) | 一种基于cnn-ekf的视觉惯性里程计实现方法 | |
| Lacroix et al. | Digital elevation map building from low altitude stereo imagery | |
| CN114964276A (zh) | 一种融合惯导的动态视觉slam方法 | |
| Chiu et al. | Augmented reality driving using semantic geo-registration | |
| CN109341685B (zh) | 一种基于单应变换的固定翼飞机视觉辅助着陆导航方法 | |
| CN113345032B (zh) | 一种基于广角相机大畸变图的初始化建图方法及*** | |
| CN112945233B (zh) | 一种全局无漂移的自主机器人同时定位与地图构建方法 | |
| CN117710476A (zh) | 基于单目视觉的无人机位姿估计与稠密建图方法 | |
| CN117168441B (zh) | 一种多传感器融合的slam定位和重建方法及*** | |
| Kupervasser et al. | Robust positioning of drones for land use monitoring in strong terrain relief using vision-based navigation | |
| Liu et al. | 6-DOF motion estimation using optical flow based on dual cameras | |
| Conway et al. | Vision-based Velocimetry over Unknown Terrain with a Low-Noise IMU | |
| Wang et al. | Low-speed unmanned vehicle localization based on sensor fusion of low-cost stereo camera and IMU | |
| Xiong et al. | Scale-aware monocular visual-inertial pose estimation for aerial robots | |
| CN112097758A (zh) | 定位方法、装置、机器人定位方法和机器人 | |
| Shi et al. | MFVS/MIMU integrated 6-DOF autonomous navigation in known environments with extremely simple landmarks | |
| Grinstead et al. | Developing detailed a priori 3D models of large environments to aid in robotic navigation tasks | |
| CN117649619B (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 |