CN115930937A - 一种多传感器的同时定位和建图方法、终端及存储介质 - Google Patents
一种多传感器的同时定位和建图方法、终端及存储介质 Download PDFInfo
- Publication number
- CN115930937A CN115930937A CN202211438953.7A CN202211438953A CN115930937A CN 115930937 A CN115930937 A CN 115930937A CN 202211438953 A CN202211438953 A CN 202211438953A CN 115930937 A CN115930937 A CN 115930937A
- Authority
- CN
- China
- Prior art keywords
- sensor
- pose
- mapping
- camera
- sensors
- 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.)
- Pending
Links
- 238000000034 method Methods 0.000 title claims abstract description 63
- 238000013507 mapping Methods 0.000 title claims description 39
- 238000005259 measurement Methods 0.000 claims abstract description 57
- 238000005457 optimization Methods 0.000 claims abstract description 35
- 238000004364 calculation method Methods 0.000 claims abstract description 14
- 230000008878 coupling Effects 0.000 claims abstract description 14
- 238000010168 coupling process Methods 0.000 claims abstract description 14
- 238000005859 coupling reaction Methods 0.000 claims abstract description 14
- 238000001914 filtration Methods 0.000 claims abstract description 12
- 230000006870 function Effects 0.000 claims description 32
- 239000011159 matrix material Substances 0.000 claims description 30
- 230000004807 localization Effects 0.000 claims description 23
- 230000004927 fusion Effects 0.000 claims description 18
- 238000012545 processing Methods 0.000 claims description 10
- 230000008859 change Effects 0.000 claims description 8
- 230000001133 acceleration Effects 0.000 claims description 6
- 230000010354 integration Effects 0.000 claims description 4
- 238000003384 imaging method Methods 0.000 claims description 3
- 238000010586 diagram Methods 0.000 abstract description 6
- 230000000007 visual effect Effects 0.000 description 10
- 102000008115 Signaling Lymphocytic Activation Molecule Family Member 1 Human genes 0.000 description 7
- 108010074687 Signaling Lymphocytic Activation Molecule Family Member 1 Proteins 0.000 description 7
- 238000004590 computer program Methods 0.000 description 5
- 230000000694 effects Effects 0.000 description 5
- 230000008569 process Effects 0.000 description 5
- 238000011161 development Methods 0.000 description 3
- 238000005286 illumination Methods 0.000 description 3
- 238000004891 communication Methods 0.000 description 2
- 238000006073 displacement reaction Methods 0.000 description 2
- 238000012986 modification Methods 0.000 description 2
- 230000004048 modification Effects 0.000 description 2
- 238000007476 Maximum Likelihood Methods 0.000 description 1
- 239000003086 colorant Substances 0.000 description 1
- 230000007547 defect Effects 0.000 description 1
- 238000005516 engineering process Methods 0.000 description 1
- 238000012886 linear function Methods 0.000 description 1
- 230000003287 optical effect Effects 0.000 description 1
- 239000004065 semiconductor Substances 0.000 description 1
- 230000003068 static effect Effects 0.000 description 1
- 238000013519 translation Methods 0.000 description 1
Images
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
- Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
Abstract
本发明公开了一种多传感器的同时定位和建图方法、终端及存储介质，包括：对各传感器进行初始化处理，获取各个传感器的测量误差，并根据各个传感器的测量误差计算得到协方差，确定多传感器耦合的优化函数；将所述协方差和传感器的测量值输入到SLAM***的滑动窗口滤波器中，通过所述滑动窗口滤波器及协方差调整各传感器测量值的权重；在滑动窗口滤波中根据所述各传感器测量值的权重进行优化计算，根据优化后的权重对位姿和地图点进行修正。本发明将不同传感器的测量误差输入到多传感器SLAM***，以传感器本身的测量误差决定各自权重，并考虑历史位姿先验信息对误差的影响，动态调整各传感器权重，使SLAM***在不同的场景下达到最优。
Description
技术领域
本发明涉及移动机器人领域，尤其涉及的是一种多传感器的同时定位和建图方法、终端及存储介质。
背景技术
同时定位与建图(SLAM)指的是机器人在不确定自身位姿的条件下，在完全未知的环境中创建地图，同时利用地图进行自主定位。SLAM因为其重要的理论和应用价值而引起移动机器人研究人员的极大兴趣，它被认为是实现真正全自主移动机器人的关键。
目前，SLAM按照使用的传感器可以分为两类：一类是基于激光雷达的激光SLAM(Lidar SLAM)；另一类是基于视觉的SLAM(Visual SLAM)。
在SLAM的发展之初，一般使用激光和超声测距完成对自身位姿的估计。因为这些传感器的精度较差，所以常常结合传感器的不确定性模型参与计算。随着半导体技术的发展，计算能力更强的芯片和更便宜的基于CMOS感光元件的相机得到了普及，鲁棒性更强，更精准但运算能力需求更高的视觉SLAM也从而得到了发展。相机的角度分辨率很高，这使得视觉SLAM对旋转更为敏感，但是由于缺少直接的位置测量，估计距离的不确定性也会随着实际距离的增加而增加。
在视觉SLAM中，由于仅使用单目相机无法完成深度的测量，一般使用双目相机完成对物体距离的间接估计，如ORB-SLAM2，但是由于双目相机的基线长度是固定的，所以估计距离的误差也会随着物体距离的增加而增加。另外，由于唯一的传感器只有相机的缘故，在光照不好的条件下相机会产生大量的噪点，影响估计的精度。因此视觉SLAM也不适合用于夜晚或者暗光场景。
为了解决只使用相机带来的缺陷，基于传感器融合的SLAM也逐渐成为主流。在基于相机和IMU融合的SLAM中，如ORB-SLAM3，IMU不止为相机提供直接的尺度信息，也会提供额外的角度测量信息。但是IMU的制造厂商繁多，IMU的规格也各不相同，因此经常需要为不同的场景调整不同的参数才能得到较好的精度。另外，IMU所提供的尺度信息并不是直接测量得到的，而是需要二次积分，这也会带来较大的累积误差，这也影响了IMU初始化的可靠性。
对于所有的基于传感器融合的SLAM来说，如何决定不同传感器的权重是尤为重要的，但是现有的基于传感器融合的SLAM都使用了通过调参得到的固定的权重，这使得它们无法在不同的场景下最优。
因此，现有技术还有待改进。
发明内容
本发明要解决的技术问题在于，针对现有技术缺陷，本发明提供一种多传感器的同时定位和建图方法、终端及存储介质，以解决目前多传感器SLAM***无法动态调整权重导致不能在不同场景下达到最优效果的技术问题。
本发明解决技术问题所采用的技术方案如下：
第一方面，本发明提供一种多传感器的同时定位和建图方法，包括：
对各传感器进行初始化处理，获取各个传感器的测量误差，并根据各个传感器的测量误差计算得到协方差，确定多传感器耦合的优化函数；各传感器均为用于传感器融合的机器人SLAM***的传感器；
将所述协方差和传感器的测量值输入到SLAM***的滑动窗口滤波器中，通过所述滑动窗口滤波器及协方差调整各传感器测量值的权重；
在滑动窗口滤波中根据所述各传感器测量值的权重进行优化计算，根据优化后的权重对机器人的位姿和机器人所在的地图点进行修正。
在本发明的一种实现方式中，所述传感器包括：相机、激光雷达以及IMU；所述相机、所述激光雷达以及所述IMU为SLAM***中的传感器，用于传感器融合的SLAM***。
在本发明的一种实现方式中，所述测量误差包括机器人的位姿和地图以及机器人的相对位姿变化；
所述对各传感器进行初始化处理，获取各个传感器的测量误差，包括：
通过特征点法和光束平差法对相机和/或激光雷达采集的数据进行处理，解算出机器人的位姿和地图；
通过积分的形式对IMU采集的数据进行处理，得到机器人在一段时间内的相对位姿变化。
在本发明的一种实现方式中，所述通过特征点法和光束平差法对相机和/或激光雷达采集的数据进行处理，根据所述测量误差解算出机器人的位姿和地图，包括：
通过所述光束平差法调整特征点的位置和所述相机的位姿，最小化重投影误差；其中，所述相机观测的最小化重投影误差的函数为：
其中z为所述特征点在图像上的实际成像位置，p为所述特征点的位置，ξ为所述相机的位姿，K为所述相机的外参，h为将所述特征点的位置投影到所述图像上的非线性函数，Ac为观测对应的信息矩阵；
将所述激光雷达的点云反投影到图像上，使所述相机的二维观测的部分特征点包含深度数据；
通过追踪三维特征点，通过所述光束平差法最小化三维的重投影误差，所述激光雷达观测的最小化误差函数为：
g为将所述特征点在世界坐标系中的位置转化为相机坐标系中位置的函数，Λl为观测对应的信息矩阵；
基于滑动窗口进行深度补全，增加当前帧点云的密度。
在本发明的一种实现方式中，所述通过积分的形式对IMU采集的数据进行处理，根据所述测量误差得到机器人在一段时间内的相对位姿变化，包括：
对角速度和加速度的测量值进行积分，得到一段时间内物体的相对运动数据；
确定最小化误差函数；所述IMU观测的最小化误差函数为：
其中ξ为所述机器人的位姿，z为所述IMU的观测数据，f为预积分函数，Λi为所述IMU观测对应的信息矩阵。
在本发明的一种实现方式中，所述确定多传感器耦合的优化函数，包括：
将相机、激光雷达以及IMU进行耦合，其中，耦合的优化函数为：
在本发明的一种实现方式中，所述在滑动窗口滤波中根据所述各传感器测量值的权重进行优化计算，包括：
将历史的观测转化为当前待优化位姿之间的约束，得到先验信息；
获取预设待优化位姿的阈值，当历史位姿数量达到所述预设待优化位姿的阈值，且有新的位姿待加入优化时，将最旧的历史位姿信息边缘化并转化为剩下待优化的先验信息；
得到最小二乘优化后所述待优化位姿所对应的信息矩阵。
在本发明的一种实现方式中，所述所述得到最小二乘优化后所述待优化位姿所对应的信息矩阵，之后包括：
在滑动窗口滤波器中维护待优化位姿对应的信息矩阵；
更新所述信息矩阵时同时对所述信息矩阵求逆，得到所述待优化位姿所对应的协方差。
第二方面，本发明还提供一种终端，包括：处理器以及存储器，所述存储器存储有多传感器的同时定位和建图的执行程序，所述多传感器的同时定位和建图的执行程序被所述处理器执行时用于实现如第一方面所述的多传感器的同时定位和建图方法的步骤。
第三方面，本发明还提供一种存储介质，所述存储介质为计算机可读存储介质，所述存储介质存储有多传感器的同时定位和建图的执行程序，所述多传感器的同时定位和建图的执行程序被处理器执行时用于实现如第一方面所述的多传感器的同时定位和建图方法的步骤。
本发明采用上述技术方案具有以下效果：
本发明通过对各传感器进行初始化处理，获取各个传感器的测量误差，并根据各个传感器的测量误差计算得到协方差，确定多传感器耦合的优化函数；将所述协方差和传感器的测量值输入到SLAM***的滑动窗口滤波器中，通过所述滑动窗口滤波器及协方差调整各传感器测量值的权重；在滑动窗口滤波中根据所述各传感器测量值的权重进行优化计算，根据优化后的权重对位姿和地图点进行修正。本发明将不同传感器的测量误差输入到多传感器SLAM***，以传感器本身的测量误差决定各自权重，并考虑历史位姿先验信息对误差的影响，动态调整各传感器权重，使SLAM***在不同的场景下达到最优。
附图说明
为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施例或现有技术描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图示出的结构获得其他的附图。
图1是本发明的一种实现方式中多传感器的同时定位和建图的流程图。
图2是本发明的重投影误差的示意图。
图3是本发明相机图像特征点和激光雷达点云的匹配图。
图4是本发明的待优化的位姿和地图点的示意图。
图5是本发明的滑动窗口滤波器中信息矩阵变化的示意图。
图6是本发明的一种实现方式中终端的功能原理图。
本发明目的的实现、功能特点及优点将结合实施例，参照附图做进一步说明。
具体实施方式
为了便于理解本申请，下面将参照相关附图对本申请进行更全面的描述。附图中给出了本申请的较佳实施方式。但是，本申请可以以许多不同的形式来实现，并不限于本文所描述的实施方式。相反地，提供这些实施方式的目的是使对本申请的公开内容理解的更加透彻全面。
除非另有定义，本文所使用的所有的技术和科学术语与属于本申请的技术领域的技术人员通常理解的含义相同。本文中在本申请的说明书中所使用的术语只是为了描述具体的实施方式的目的，不是旨在于限制本申请。
示例性方法
同时定位与建图(SLAM)指的是机器人在不确定自身位姿的条件下，在完全未知的环境中创建地图，同时利用地图进行自主定位。SLAM因为其重要的理论和应用价值而引起移动机器人研究人员的极大兴趣，它被认为是实现真正全自主移动机器人的关键。目前，SLAM按照使用的传感器可以分为两类：一类是基于激光雷达的激光SLAM(Lidar SLAM)；另一类是基于视觉的SLAM(Visual SLAM)。
在SLAM的发展之初，一般使用激光和超声测距完成对自身位姿的估计。因为这些传感器的精度较差，所以常常结合传感器的不确定性模型参与计算。而在视觉SLAM中，由于仅使用单目相机无法完成深度的测量，一般使用双目相机完成对物体距离的间接估计，如ORB-SLAM2，但是由于双目相机的基线长度是固定的，所以估计距离的误差也会随着物体距离的增加而增加。另外，由于唯一的传感器只有相机的缘故，在光照不好的条件下相机会产生大量的噪点，影响估计的精度。在基于相机和IMU融合的SLAM中，如ORB-SLAM3，IMU不止为相机提供直接的尺度信息，也会提供额外的角度测量信息。但是IMU的制造厂商繁多，IMU的规格也各不相同，因此经常需要为不同的场景调整不同的参数才能得到较好的精度。另外，IMU所提供的尺度信息并不是直接测量得到的，而是需要二次积分，这也会带来较大的累积误差，这也影响了IMU初始化的可靠性。
对于所有的基于传感器融合的SLAM来说，如何决定不同传感器的权重是尤为重要的，但是现有的基于传感器融合的SLAM没有从传感器本身的测量误差去决定他们之间的权重(比如手动调参)，都使用了通过调参得到的固定的权重，也并没有考虑到历史位姿的先验信息对误差的影响，这使得它们无法在不同的场景下最优。
针对上述技术问题，本发明实施例提供一种多传感器的同时定位和建图方法，使用协方差动态调整各个传感器的权重，同时考虑到历史位姿的先验信息对误差的影响并进行优化，结合了三种不同传感器的优点，可以在不同的场景下达到最优。
如图1所示，本发明实施例提供一种多传感器的同时定位和建图方法，包括以下步骤：
步骤S100，对各传感器进行初始化处理，获取各个传感器的测量误差，并根据各个传感器的测量误差计算得到协方差，确定多传感器耦合的优化函数，各传感器均为用于传感器融合的机器人SLAM***的传感器。
在传感器静止状态下，统计传感器对同一个物体的某个特征的多次测量的读数，通过测量这个独立同分布的读数集合的标准差，即可得知传感器的测量误差。
在本实施例中，为了使SLAM***可以在不同的场景下达到最优效果，需要确定传感器融合的SLAM***中的传感器种类；考虑到激光SLAM和视觉SLAM各自的优点和缺陷，结合不同传感器的优点选取传感器类型。
具体地，本实施例的一种实现方式中，步骤S100包括以下步骤：
步骤S101，所述传感器包括：相机、激光雷达以及IMU；所述相机、所述激光雷达以及所述IMU为SLAM***中的传感器，用于传感器融合的SLAM***。
仅使用单目相机无法完成深度的测量，一般使用双目相机完成对物体距离的间接估计，但是由于双目相机的基线长度是固定的，所以估计距离的误差也会随着物体距离的增加而增加。另外，传感器只有相机的情况下，在光照不好的条件下相机会产生大量的噪点，影响估计的精度，不适合用于夜晚或者暗光场景；IMU不止为相机提供直接的尺度信息，也会提供额外的角度测量信息；激光雷达通过测量飞行时间差提供直接的距离测量，而且因为是主动式测量，激光雷达在光照不好的条件下也可以正常工作。因此在本实施例中，SLAM***的输入选取相机，激光雷达和IMU三种不同的传感器组成。
步骤S102，通过特征点法和光束平差法对相机和/或激光雷达采集的数据进行处理，根据所述测量误差解算出机器人的位姿和地图；
位姿即位置和姿态，包括机器人在三维空间中的位置和旋转，包含三个自由度的平移和三个自由度的旋转。
地图点即是路标点。SLAM***在工作时，会测量环境中特征比较明显、辨识度较高的物体或区域，并把它们当作路标。而路标点则是由SLAM***估计出来的这些具有特征的物体在三维空间中的相对位置。
具体地，本实施例的一种实现方式中，步骤S102具体包括以下步骤：
步骤S102a，通过所述光束平差法调整特征点的位置和所述相机的位姿，最小化重投影误差。
特征点法是指通过追踪图像中的特征点来估计机器人的运动。而在计算机视觉中，图像的特征点即是指图像中能够反映图像特征和识别图像中目标物体的点。每一特征点都有其唯一的特征值，而同一特征点在不同图像中的变化不大，利用这一点就可以追踪图像中的特征点。在找到不同图像中特征点的匹配关系后，就可以用光束平差法来求解出特征点的空间位置和机器人的位姿。
光束平差法是指通过调整特征点的位置和相机的位姿，最小化特征点投影到图像上的位置和特征点在图像上实际位置的误差，即重投影误差，使得经过特征点反射出的光线逐渐收束到相机光心的过程。
如图2所示，xkj和xij是特征点Xj在i，j两帧图像上的对应观测，πk(Xj)和πk(Xi)则是特征点Xj根据自身位置和图像位姿投影到在i，j两帧图像上对应的位置，xkj和πk(Xj)，以及xij和πk(Xi)在各自图像上的距离则为重投影误差，重投影误差越小，则特征点和对应图像的相对位姿就越准确。
光束平差法一般使用高斯牛顿法或者列文伯格-马夸尔特法(Levenberg-Marquardt)等非线性优化方法去最小化重投影误差。设特征点在图像上的实际成像位置为z，特征点的位置为p，相机的位姿为ξ，相机的外参为K，h是把特征点的位置投影到图像上的非线性函数，Λc是观测对应的信息矩阵，所述相机观测的最小化重投影误差的函数为：
步骤S102b，将所述激光雷达的点云反投影到图像上，使所述相机的二维观测的部分特征点包含深度数据。
由于相机只是二维观测，无法提供尺度信息，因此需要激光雷达的距离测量以提供另外一个维度的信息。因为激光雷达返回的点云较为稀疏，无法直接提取特征，而将激光雷达的点云反投影到图像上，就可以使部分特征点包含深度数据。
本发明在结合相机的特征点和激光雷达的点云时，使用的是硬匹配，即相机的特征点和点云反投影回图像上的位置完全一致时，才将赋予对应的特征点深度信息，但这样只有很少一部分特征点才能和点云匹配，同时也只有少数点云能与特征点匹配，如图3所示，途中根据不同的色彩将没有匹配的图像的特征点、没有匹配的激光雷达的反投影到图像上的点云、硬匹配的特征点和点云、软匹配的特征点和点云区分出来。为了增加匹配的数量，可以使用软匹配，即设定一个阈值，若特征点与点云之间距离小于阈值，则将其匹配到一起，同时也为这些匹配赋上协方差，距离越近，则协方差越小，并将这个协方差用于求解SLAM的最小二乘法中。
步骤S102c，通过追踪三维特征点，通过所述光束平差法最小化三维的重投影误差。
通过追踪这些三维特征点，并用光束平差法最小化三维的重投影误差，就可以为***提供准确的尺度信息，所述激光雷达观测的最小化误差函数为：
g是把特征点在世界坐标系中的位置转化到相机坐标系中位置的函数，Λl是观测对应的信息矩阵。
步骤S102d，基于滑动窗口进行深度补全，增加当前帧点云的密度。
尽管将激光雷达的点云反投影到图像上可以给予部分特征点深度数据，但是由于点云和特征点都较为稀疏，只有小部分的特征点与点云匹配从而有深度信息。为了尽可能让更多的特征点都有深度数据，使用了基于滑动窗口的深度补全，简单地说，就是将之前十帧的点云数据投影在当前帧上，增加当前帧点云的密度，就能让更多的特征点都有深度数据。
步骤S103，通过积分的形式对IMU采集的数据进行处理，根据所述测量误差得到机器人在一段时间内的相对位姿变化。
具体地，本实施例的一种实现方式中，步骤S103具体包括以下步骤：
步骤S103a，对角速度和加速度的测量值进行积分，得到一段时间内物体的相对运动数据。
IMU可以提供短时间内物体运动的估计，但是IMU的直接测量值只有角速度和加速度。为了得到一段时间内物体的相对运动，例如位移，旋转和速度的估计，则需要对角速度和加速度的测量值进行积分(加速度的积分是速度，速度的积分是位移)。假设在t0处有位置、速度和旋转p0、v0和R0，则经过时间t后，位置、速度和旋转pt、vt和Rt为：
vt＝v0+gt+∫[R(am-ba-na)]dt
Rt＝R0Exp(∫(ωm-bω-nω)dt)
步骤S103b，确定最小化误差函数。
所述IMU观测的最小化误差函数为：
其中ξ是机器人的位姿，z是IMU的观测数据，f是预积分函数，Λi是IMU观测对应的信息矩阵。
因此，将相机、激光雷达以及IMU进行耦合，其中，耦合的优化函数为：
如图1所示，在本发明实施例的一种实现方式中，多传感器的同时定位和建图方法还包括以下步骤：
步骤S200，将所述协方差和传感器的测量值输入到SLAM***的滑动窗口滤波器中，通过所述滑动窗口滤波器及协方差调整各传感器测量值的权重。
在本发明实施例中，使用相机，激光雷达和IMU的传感器融合SLAM***，它结合了三种不同传感器的优点。为了可以在不同的场景下达到最优，本发明使用协方差动态调整各个传感器的权重。在***运行之初，会初始化所有的传感器，并测量每个传感器在当前环境下的测量误差，以协方差的形式来表示，并以此来确定各个传感器的权重，并输入到多传感器融合SLAM***中，用以自动决定不同传感器的权重，从而相信更准确的传感器的测量。
把初始化得到的协方差和传感器的测量值输入SLAM***的滑动窗口滤波中，滑动窗口滤波会根据协方差决定不同测量值的权重，而得到的权重会在滑动窗口滤波进行优化计算的时候起作用。
如图1所示，在本发明实施例的一种实现方式中，多传感器的同时定位和建图方法还包括以下步骤：
步骤S300，在滑动窗口滤波中根据所述各传感器测量值的权重进行优化计算，根据优化后的权重对机器人的位姿和机器人所在的地图点进行修正。
具体地，本实施例的一种实现方式中，步骤S300具体包括以下步骤：
步骤S301，将历史的观测转化为当前待优化位姿之间的约束，得到先验信息。
在SLAM***中，若想得到最精确的结果，理应将历史中的所有的观测都纳入最小二乘优化中，如图4所示。但SLAM问题，本质上是一个最大后验概率问题：
x*＝argminxp(x|z)
其中，x是待估计的量，z是观测。若将所有的历史观测都纳入计算中，则可以忽略掉先验信息，问题即可转化为最大似然问题，然后以最小二乘的形式求解：
但是随着时间的增长，待求解位姿和地图点的数量急剧增加，优化问题所需的时间在有限的算力下无法满足SLAM实时性的需求，因此需要限制待优化的位姿的数量从而限制优化问题的尺度以保证SLAM的实时性，若直接丢掉历史的位姿和旧的地图点，就会破坏先前这些位姿对剩下位姿的约束，因此在丢掉历史的观测之前，应当把这些观测转化为当前待优化位姿之间的约束，即先验信息。
步骤S302，获取预设待优化位姿的阈值，当历史位姿数量达到所述预设待优化位姿的阈值，且有新的位姿待加入优化时，将最旧的历史位姿信息边缘化并转化为剩下待优化的先验信息；
滑动窗口滤波器就是指，将待优化的位姿的上限设为N，当历史位姿数量为N且有新的位姿待加入优化时，将最旧的历史位姿信息边缘化并转化为剩下待优化的先验信息，周而复始。
步骤S303，得到最小二乘优化后所述待优化位姿所对应的信息矩阵。
设待优化的变量为xc，待边缘化的变量为xm，那么此时求解SLAM问题的最小二乘形式为：
H是前一次最小二乘优化后待优化位姿所对应的信息矩阵，Hmm是待边缘化位姿所对应的区块，而J(x*)是h对应的雅可比矩阵。由此也可看出，滑动窗口滤波器需要维护一个和待优化位姿对应的信息矩阵以储存先验信息，如图5所示，当新位姿6加入时，需要在H中扩充对应的区块，然后将最旧的位姿1对应的区块边缘化掉，则位姿对应的信息矩阵就是Λc。
步骤S304，更新所述信息矩阵时同时对所述信息矩阵求逆，得到所述待优化位姿所对应的协方差。
由于在滑动窗口滤波器中维护了待优化位姿对应的信息矩阵H，则只需每次更新信息矩阵H的时候对H求逆，则可得到位姿所对应的协方差。估计位姿的协方差，在没有真值的情况下可以帮助判断SLAM质量的好坏。
本实施例通过上述技术方案达到以下技术效果：
本发明公开了一种多传感器的同时定位和建图方法、终端及存储介质，包括：对各传感器进行初始化处理，获取各个传感器的测量误差，并根据各个传感器的测量误差计算得到协方差，确定多传感器耦合的优化函数；将所述协方差和传感器的测量值输入到SLAM***的滑动窗口滤波器中，通过所述滑动窗口滤波器及协方差调整各传感器测量值的权重；在滑动窗口滤波中根据所述各传感器测量值的权重进行优化计算，根据优化后的权重对位姿和地图点进行修正。本发明将不同传感器的测量误差输入到多传感器SLAM***，以传感器本身的测量误差决定各自权重，并考虑历史位姿先验信息对误差的影响，动态调整各传感器权重，使SLAM***在不同的场景下达到最优。
示例性设备
基于上述实施例，本发明还提供一种终端，包括：通过***总线连接的处理器、存储器、接口、显示屏以及通讯模块；其中，所述处理器用于提供计算和控制能力；所述存储器包括存储介质以及内存储器；所述存储介质存储有操作***和计算机程序；所述内存储器为所述存储介质中的操作***和计算机程序的运行提供环境；所述接口用于连接外部设备，例如，移动终端以及计算机等设备；所述显示屏用于显示相应的信息；所述通讯模块用于与云端服务器或移动终端进行通讯。
所述计算机程序被所述处理器执行时用以实现一种多传感器的同时定位和建图方法的步骤。
本领域技术人员可以理解的是，图6中示出的原理框图，仅仅是与本发明方案相关的部分结构的框图，并不构成对本发明方案所应用于其上的终端的限定，具体的终端可以包括比图中所示更多或更少的部件，或者组合某些部件，或者具有不同的部件布置。
在一个实施例中，提供了一种终端，其中，包括：处理器和存储器，所述存储器存储有多传感器的同时定位和建图的执行程序，所述多传感器的同时定位和建图的执行程序被所述处理器执行时用于实现如上所述的多传感器的同时定位和建图方法的步骤。
在一个实施例中，提供了一种存储介质，其中，所述存储介质存储有多传感器的同时定位和建图的执行程序，所述多传感器的同时定位和建图的执行程序被所述处理器执行时用于实现如上所述的多传感器的同时定位和建图方法的步骤。
本领域普通技术人员可以理解实现上述实施例方法中的全部或部分流程，是可以通过计算机程序来指令相关的硬件来完成，计算机程序可存储于一非易失性存储介质中，该计算机程序在执行时，可包括如上述各方法的实施例的流程。其中，本发明所提供的各实施例中所使用的对存储器、存储、数据库或其它介质的任何引用，均可包括非易失性和/或易失性存储器。
综上，本发明公开了一种多传感器的同时定位和建图方法、终端及存储介质，包括：初始化传感器，确定传感器融合的SLAM***中的传感器种类，确定多传感器耦合的优化函数，获取各个传感器的测量误差，用协方差的形式表示；将所述协方差和传感器的测量值输入到所述SLAM***的滑动窗口滤波中，滑动窗口滤波根据所述协方差动态调整不同传感器测量值的权重；在滑动窗口滤波中根据所述不同传感器测量值的权重进行优化计算。本发明通过将不同传感器的测量误差输入到多传感器融合SLAM***中，从传感器本身的测量误差去决定各自权重，并考虑到历史位姿的先验信息对误差的影响，通过动态调整各传感器权重，使得SLAM***能在不同的场景下达到最优。
应当理解的是，本发明的应用不限于上述的举例，对本领域普通技术人员来说，可以根据上述说明加以改进或变换，所有这些改进和变换都应属于本发明所附权利要求的保护范围。
Claims (10)
1.一种多传感器的同时定位和建图方法，其特征在于，包括：
对各传感器进行初始化处理，获取各个传感器的测量误差，并根据各个传感器的测量误差计算得到协方差，确定多传感器耦合的优化函数；各传感器均为用于传感器融合的机器人SLAM***的传感器；
将所述协方差和传感器的测量值输入到SLAM***的滑动窗口滤波器中，通过所述滑动窗口滤波器及协方差调整各传感器测量值的权重；
在滑动窗口滤波中根据所述各传感器测量值的权重进行优化计算，根据优化后的权重对机器人的位姿和机器人所在的地图点进行修正。
2.根据权利要求1所述多传感器的同时定位和建图方法，其特征在于，所述传感器包括：相机、激光雷达以及IMU；所述相机、所述激光雷达以及所述IMU为SLAM***中的传感器，用于传感器融合的SLAM***。
3.根据权利要求2所述多传感器的同时定位和建图方法，其特征在于，所述测量误差包括机器人的位姿和地图以及机器人的相对位姿变化；
所述对各传感器进行初始化处理，获取各个传感器的测量误差，包括：
通过特征点法和光束平差法对相机和/或激光雷达采集的数据进行处理，根据所述测量误差解算出机器人的位姿和地图；
通过积分的形式对IMU采集的数据进行处理，根据所述测量误差得到机器人在一段时间内的相对位姿变化。
4.根据权利要求3所述多传感器的同时定位和建图方法，其特征在于，所述通过特征点法和光束平差法对相机和/或激光雷达采集的数据进行处理，根据所述测量误差解算出机器人的位姿和地图，包括：
通过所述光束平差法调整特征点的位置和所述相机的位姿，最小化重投影误差；其中，所述相机观测的最小化重投影误差的函数为：
其中z为所述特征点在图像上的实际成像位置，p为所述特征点的位置，ξ为所述相机的位姿，K为所述相机的外参，h为将所述特征点的位置投影到所述图像上的非线性函数，Λc为观测对应的信息矩阵；
将所述激光雷达的点云反投影到图像上，使所述相机的二维观测的部分特征点包含深度数据；
通过追踪三维特征点，通过所述光束平差法最小化三维的重投影误差，所述激光雷达观测的最小化误差函数为：
g为将所述特征点在世界坐标系中的位置转化为相机坐标系中位置的函数，Λl为观测对应的信息矩阵；
基于滑动窗口进行深度补全，增加当前帧点云的密度。
5.根据权利要求3所述多传感器的同时定位和建图方法，其特征在于，所述通过积分的形式对IMU采集的数据进行处理，根据所述测量误差得到机器人在一段时间内的相对位姿变化，包括：
对角速度和加速度的测量值进行积分，得到一段时间内物体的相对运动数据；
确定最小化误差函数；所述IMU观测的最小化误差函数为：
其中ξ为所述机器人的位姿，z为所述IMU的观测数据，f为预积分函数，Λi为所述IMU观测对应的信息矩阵。
6.根据权利要求1所述多传感器的同时定位和建图方法，其特征在于，所述确定多传感器耦合的优化函数，包括：
将相机、激光雷达以及IMU进行耦合，其中，耦合的优化函数为：
7.根据权利要求1所述多传感器的同时定位和建图方法，其特征在于，所述在滑动窗口滤波中根据所述各传感器测量值的权重进行优化计算，包括：
将历史的观测转化为当前待优化位姿之间的约束，得到先验信息；
获取预设待优化位姿的阈值，当历史位姿数量达到所述预设待优化位姿的阈值，且有新的位姿待加入优化时，将最旧的历史位姿信息边缘化并转化为剩下待优化的先验信息；
得到最小二乘优化后所述待优化位姿所对应的信息矩阵。
8.根据权利要求7所述多传感器的同时定位和建图方法，其特征在于，所述得到最小二乘优化后所述待优化位姿所对应的信息矩阵，之后包括：
在滑动窗口滤波器中维护待优化位姿对应的信息矩阵；
更新所述信息矩阵时同时对所述信息矩阵求逆，得到所述待优化位姿所对应的协方差。
9.一种终端，其特征在于，包括：处理器以及存储器，所述存储器存储有多传感器的同时定位和建图的执行程序，所述多传感器的同时定位和建图的执行程序被所述处理器执行时用于实现如权利要求1-8中任意一项所述的多传感器的同时定位和建图方法的步骤。
10.一种计算机可读存储介质，其特征在于，所述计算机可读存储介质存储有多传感器的同时定位和建图的执行程序，所述多传感器的同时定位和建图的执行程序被处理器执行时用于实现如权利要求1-8中任意一项所述的多传感器的同时定位和建图方法的步骤。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202211438953.7A CN115930937A (zh) | 2022-11-17 | 2022-11-17 | 一种多传感器的同时定位和建图方法、终端及存储介质 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202211438953.7A CN115930937A (zh) | 2022-11-17 | 2022-11-17 | 一种多传感器的同时定位和建图方法、终端及存储介质 |
Publications (1)
| Publication Number | Publication Date |
|---|---|
| CN115930937A true CN115930937A (zh) | 2023-04-07 |
Family
ID=86651567
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202211438953.7A Pending CN115930937A (zh) | 2022-11-17 | 2022-11-17 | 一种多传感器的同时定位和建图方法、终端及存储介质 |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN115930937A (zh) |
Cited By (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN116659510A (zh) * | 2023-06-02 | 2023-08-29 | 海南大学 | 水下机器人定位与避障方法、装置及存储介质 |
-
2022
- 2022-11-17 CN CN202211438953.7A patent/CN115930937A/zh active Pending
Cited By (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN116659510A (zh) * | 2023-06-02 | 2023-08-29 | 海南大学 | 水下机器人定位与避障方法、装置及存储介质 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN111156998B (zh) | 一种基于rgb-d相机与imu信息融合的移动机器人定位方法 | |
| CN109887057B (zh) | 生成高精度地图的方法和装置 | |
| US9071829B2 (en) | Method and system for fusing data arising from image sensors and from motion or position sensors | |
| JP6552729B2 (ja) | 異なる分解能を有するセンサーの出力を融合するシステム及び方法 | |
| CN111750853B (zh) | 一种地图建立方法、装置及存储介质 | |
| JP7131994B2 (ja) | 自己位置推定装置、自己位置推定方法、自己位置推定プログラム、学習装置、学習方法及び学習プログラム | |
| CN109708649B (zh) | 一种遥感卫星的姿态确定方法及*** | |
| CN111288989B (zh) | 一种小型无人机视觉定位方法 | |
| WO2024027350A1 (zh) | 车辆定位方法、装置、计算机设备、存储介质 | |
| CN111665512B (zh) | 基于3d激光雷达和惯性测量单元的融合的测距和绘图 | |
| WO2022077296A1 (zh) | 三维重建方法、云台负载、可移动平台以及计算机可读存储介质 | |
| CN112444246B (zh) | 高精度的数字孪生场景中的激光融合定位方法 | |
| CN114111776B (zh) | 定位方法及相关装置 | |
| CN113436261B (zh) | 一种面向封闭园区自动驾驶的单目视觉惯性定位方法 | |
| WO2021081774A1 (zh) | 一种参数优化方法、装置及控制设备、飞行器 | |
| CN114964276A (zh) | 一种融合惯导的动态视觉slam方法 | |
| CN114758011B (zh) | 融合离线标定结果的变焦相机在线标定方法 | |
| CN116619358A (zh) | 一种矿山自主探测机器人自适应定位优化与建图方法 | |
| CN114047766B (zh) | 面向室内外场景长期应用的移动机器人数据采集***及方法 | |
| CN114690229A (zh) | 一种融合gps的移动机器人视觉惯性导航方法 | |
| CN110989619A (zh) | 用于定位对象的方法、装置、设备和存储介质 | |
| CN114529585A (zh) | 基于深度视觉和惯性测量的移动设备自主定位方法 | |
| CN114723811A (zh) | 一种非结构化环境四足机器人立体视觉定位建图方法 | |
| CN112762929B (zh) | 一种智能导航方法、装置和设备 | |
| CN114485648B (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 |