CN115526914A - 基于多传感器的机器人实时定位和彩色地图融合映射方法 - Google Patents

基于多传感器的机器人实时定位和彩色地图融合映射方法 Download PDF

Info

Publication number
CN115526914A
CN115526914A CN202211074370.0A CN202211074370A CN115526914A CN 115526914 A CN115526914 A CN 115526914A CN 202211074370 A CN202211074370 A CN 202211074370A CN 115526914 A CN115526914 A CN 115526914A
Authority
CN
China
Prior art keywords
robot
coordinate system
point cloud
laser
point
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
Application number
CN202211074370.0A
Other languages
English (en)
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.)
Yanshan University
Original Assignee
Yanshan University
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 Yanshan University filed Critical Yanshan University
Priority to CN202211074370.0A priority Critical patent/CN115526914A/zh
Publication of CN115526914A publication Critical patent/CN115526914A/zh
Pending legal-status Critical Current

Links

Images

Classifications

    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T7/00—Image analysis
    • G06T7/20—Analysis of motion
    • G06T7/269—Analysis of motion using gradient-based methods
    • 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
    • G01C21/1652—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 with ranging devices, e.g. LIDAR or RADAR
    • 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
    • G01S17/00—Systems using the reflection or reradiation of electromagnetic waves other than radio waves, e.g. lidar systems
    • G01S17/86—Combinations of lidar systems with systems other than lidar, radar or sonar, e.g. with direction finders
    • 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
    • G01S17/00—Systems using the reflection or reradiation of electromagnetic waves other than radio waves, e.g. lidar systems
    • G01S17/88—Lidar systems specially adapted for specific applications
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06F—ELECTRIC DIGITAL DATA PROCESSING
    • G06F16/00—Information retrieval; Database structures therefor; File system structures therefor
    • G06F16/20—Information retrieval; Database structures therefor; File system structures therefor of structured data, e.g. relational data
    • G06F16/29—Geographical information databases
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06N—COMPUTING ARRANGEMENTS BASED ON SPECIFIC COMPUTATIONAL MODELS
    • G06N3/00—Computing arrangements based on biological models
    • G06N3/02—Neural networks
    • G06N3/08—Learning methods
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T7/00—Image analysis
    • G06T7/70—Determining position or orientation of objects or cameras
    • G06T7/73—Determining position or orientation of objects or cameras using feature-based methods
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T2207/00—Indexing scheme for image analysis or image enhancement
    • G06T2207/10—Image acquisition modality
    • G06T2207/10016—Video; Image sequence
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T2207/00—Indexing scheme for image analysis or image enhancement
    • G06T2207/10—Image acquisition modality
    • G06T2207/10024—Color image
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T2207/00—Indexing scheme for image analysis or image enhancement
    • G06T2207/10—Image acquisition modality
    • G06T2207/10028—Range image; Depth image; 3D point clouds
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T2207/00—Indexing scheme for image analysis or image enhancement
    • G06T2207/20—Special algorithmic details
    • G06T2207/20081—Training; Learning

Landscapes

  • Engineering & Computer Science (AREA)
  • Physics & Mathematics (AREA)
  • Remote Sensing (AREA)
  • Radar, Positioning & Navigation (AREA)
  • General Physics & Mathematics (AREA)
  • Theoretical Computer Science (AREA)
  • Computer Networks & Wireless Communication (AREA)
  • Electromagnetism (AREA)
  • Computer Vision & Pattern Recognition (AREA)
  • Databases & Information Systems (AREA)
  • General Engineering & Computer Science (AREA)
  • Data Mining & Analysis (AREA)
  • Artificial Intelligence (AREA)
  • Molecular Biology (AREA)
  • Biophysics (AREA)
  • Computational Linguistics (AREA)
  • Life Sciences & Earth Sciences (AREA)
  • Evolutionary Computation (AREA)
  • General Health & Medical Sciences (AREA)
  • Biomedical Technology (AREA)
  • Computing Systems (AREA)
  • Health & Medical Sciences (AREA)
  • Mathematical Physics (AREA)
  • Software Systems (AREA)
  • Automation & Control Theory (AREA)
  • Multimedia (AREA)
  • Image Processing (AREA)

Abstract

本发明提出一种基于多传感器的机器人实时定位和彩色地图融合映射方法，快速采集周边环境信息，并实时构建彩色全局地图。视觉、激光、惯性传感器采集模块通过读取传感器的信息，对激光点云数据进行累加，形成点云扫描帧，然后进行预处理，得到提取后的特征点云，同时对不同传感器采集到的数据进行时间同步。利用采集到的激光和惯性数据实时对机器人进行自定位，同时构建单帧点云地图。利用视觉传感器采集到的RGB信息，对构建的全局点云地图进行纹理和颜色渲染，构建单帧彩色地图。

Description

基于多传感器的机器人实时定位和彩色地图融合映射方法
技术领域
本发明涉及机器人技术领域，尤其涉及一种基于多传感器的机器人实时自定位和彩色稠密地图融合映射方法。
背景技术
随着近年来科技的发展，移动机器人在工业领域和民用领域的应用不断深入，而定位与导航能力是移动机器人的关键和基础。
目前主流的同时定位与建图技术主要有基于激光雷达的方法、基于相机的方法和多传感器融合的方法，其中多传感器融合方法多使用激光里程计和视觉里程计的结果直接耦合优化，从而得到一个较为准确的位姿，然后输出一个经过位姿矫正后未经渲染的点云地图或者语义地图，这样做的优点在于可以适应更为多样性的环境，避免传感器退化，缺陷在于计算成本较高，并且只利用了视觉信息中的特征点部分，没有实现完全的利用。
发明内容
本发明提出了基于多传感器的机器人实时定位和彩色地图融合映射方法，能够生成基于机器人运动的自我定位信息和实时彩色全局地图，可以通过网络上传到服务器端，可以远程调用查看，摆脱操作人员的视野局限，全方位的掌握整个环境的变化信息。
为解决上述技术问题，本发明所采用的技术方案是：
基于多传感器的机器人实时定位和彩色地图融合映射方法，包括以下步骤：
步骤1：读取视觉传感器、激光雷达传感器、惯性传感器的数据，对不同源的图像、激光点云和imu位姿数据进行时间同步，获得统一的时间系统下的待融合数据，并利用惯性传感器采集到的前20帧数据进行初始化，建立初始位姿估计。
步骤2：由于激光雷达的逐点扫描特性，点云数据中的每个点都有自己独立的采集时间，一帧激光点云往往包含有数万个点，也就有数万个连续的不同时间戳，同一帧中采集到的第一个点和最后一个点之间存在着不可忽视的时间误差，对于运动中的机器人，这种时间误差必然导致不可避免的空间误差，为了消除这种误差，我们使用了采集频率远远超过激光雷达传感器的imu传感器，对imu传感器采集到的惯性数据进行前向传播，得到各个时刻机器人位姿的先验估计；得到各个时刻间对机器人的位姿的先验估计后，通过对时刻进行插值，计算出每个激光点采集时刻和激光点云帧采集结束时刻间机器人的相对运动，将激光点从采集时刻的机器人坐标系转移到点云帧采集结束时刻的坐标系中，对当前帧中每一个激光点进行上述操作后，得到消除帧间运动畸变的点云帧采集结束时刻点云；
步骤3：由于点云帧采集结束时刻点云是对经过下采样的点云数据直接利用imu信息进行运动畸变消除得到的，因此必然会受到激光雷达传感器和惯性imu传感器的采集数据误差的影响，误差随着时间不断累积，最终导致跟踪丢失，定位和建图的精度都完全得不到保障。为了消除传感器误差的影响，得到更精确的位姿估计，我们使用误差状态迭代卡尔曼滤波器对不同帧间采集到的信息进行比较，在机器人运动过程中，一个地标物往往会在连续很多帧上同时存在，并且和机器人的相对位置不断变化，对新采集到的点云帧中的每一点，在地图中提取和当前位置最近的五个点拟合出一个小平面，通过迭代优化，不断缩小激光点和它对应的拟合平面间的距离，直到小于阈值，从而得到点云帧采集结束时刻对机器人运动的最优估计；利用计算出的最优估计，将点云帧采集结束时刻视觉传感器采集到的图像转移归一化坐标系并最终转移到雷达坐标系中，对当前帧中的每一个点，利用预标定过的激光雷达传感器和视觉传感器的旋转平移关系，从三维空间坐标转移到像素坐标系，给视觉传感器视场范围内的每个激光点赋予RGB信息，得到经过RGB渲染的彩色点云；为了解决激光雷达和相机数据采集频率不同的问题，我们使用了基于时间的插帧处理，利用激光雷达传感器采集时刻的前后两帧图片，使用稠密光流法估计出激光雷达传感器采集时刻的视觉数据，实现激光雷达传感器和视觉传感器的时间同步；
步骤4：对经过RGB渲染的彩色点云通过点云帧采集结束时刻对机器人运动的最优估计，将点云从点云帧采集结束时刻的机器人运动坐标系转移到独立的世界坐标系，从而构建出当前帧的彩色点云地图；为了便于对地图中的点进行最近邻搜索，将地图中的点以K维树的形式组织起来，使得新采集到的每一个点都可以用最短的时间找到它在地图中的最邻近点；通过时间累积，将机器人采集到的多帧信息累加，得到连续各帧间的机器人运动最优估计和各帧的彩色点云地图，实现机器人的自我定位并建立全局彩色地图。
本发明技术方案的进一步改进在于：所述步骤2具体包括以下步骤：
步骤2.1：建立imu运动学模型，对于机器人运动的描述中最重要的两个物理量是当前时刻的位姿和相对于坐标系原点的位移量，但是位姿和位移并不能通过传感器直接测量得到，我们选择使用惯性imu传感器测量得到的角速度和加速度数据对时间维度积分得到所求的位姿和位移状态，因此建立在世界坐标系下的状态表达方程：
Figure BDA0003829049500000031
在建立世界坐标系的状态表达方程中一共有6个状态量，分别是机器人在世界坐标系下的位姿RI，即机器人相对于初始状态的旋转量，使用一个3*3的旋转矩阵来表示；机器人在世界坐标系下相对于原点的位移量pI，使用一个3*1的平移向量来表示；机器人在x、y、z三个轴上的瞬时速度vI,使用用一个3*1的速度向量来表示；机器人惯性imu传感器采集数据的在角速度和加速度上的偏差bω和ba；机器人世界坐标系相对于真实世界坐标系下的重力矢量g；
步骤2.2：对于建立好的状态方程，对于imu数据进行预积分，建立imu运动学连续模型；其中，机器人在世界坐标系下的位姿的一阶微分量等于机器人相对于初始状态的旋转矩阵右乘消除噪音后的角速度向量的反对称矩阵；机器人在世界坐标系下的位移的一阶微分量等于机器人在x、y、z三个轴上的瞬时速度；机器人的速度的一阶微分量等于机器人相对于初始状态的旋转矩阵乘以消除噪音后的加速度向量再加上机器人世界坐标系下的重力矢量；机器人惯性imu传感器采集数据的在角速度和加速度上的偏差bω和ba的模型为带高斯噪声nbω和nba的随机游动过程；机器人世界坐标系下的重力矢量g的一阶微分量为0，由此可得机器人的imu运动学方程如下：
Figure BDA0003829049500000041
Figure BDA0003829049500000042
Figure BDA0003829049500000043
Figure BDA0003829049500000044
Figure BDA0003829049500000045
Figure BDA0003829049500000046
对上述方程组离散化可得
Figure BDA0003829049500000047
步骤2.3：步骤1中进行初始化，得到了机器人运动状态的初始估计，当机器人收到imu传感器的输入时，即可根据步骤2.2中建立的离散运动学模型进行前向传播，通过初始估计和imu传感器每一帧采集到的加速度和角速度数据，迭代出每一帧时刻的机器人位姿的先验估计；具体的迭代方程如下
xi+1＝xi+(Δtf(xi，ui，wi))
Figure BDA0003829049500000048
其中控制量ui为imu传感器采集到的的加速度数据am和角速度数据ωm，误差量w为imu传感器的累积误差nω、na和imu传感器采集到的数据的误差nbω、nba；
由于imu传感器的工作频率远高于激光雷达传感器，因此机器人运动状态会在激光雷达传感器的两帧时间中不断前向传播，直到激光雷达传感器采集完完整的一帧。
步骤2.4：激光雷达传感器采集完完整一帧点云后，根据imu传感器数据前向传播得到的各个时刻的机器人运动状态进行插值计算，得到每个激光点采集时刻对应的机器人运动状态，并计算出每个激光点采集时刻到点云帧采集结
束时刻的机器人相对运动状态
Figure BDA0003829049500000053
其中j为每个雷达点的采集时刻，k为点云帧的采集时刻；由于前向传播得到的机器人运动状态是基于imu传感器坐标系得到的，我们还应将j时刻采集到的激光点先从j时刻的激光雷达坐标系转移到imu传感器坐标系，再利用j时刻到k时刻的机器人的相对运动将激光点从j时刻的imu传感器坐标系转移到k时刻的imu传感器坐标系，最后再将激光点先从k时刻的imu传感器坐标系转移到激光雷达坐标系，激光点的坐标系转换过程如下公式所示：
Figure BDA0003829049500000051
其中ITL是激光雷达坐标系和imu传感器坐标系间的旋转平移关系，由于激光雷达传感器和imu传感器二者之间有固定的物理连接关系，所以ITL是一个固定量，并通过预标定得到。
本发明技术方案的进一步改进在于：所述步骤3具体包括以下步骤：
步骤3.1：步骤2.4中得到的点云帧采集结束时刻k的点云
Figure BDA0003829049500000052
是激光雷达采集到的点云经过预处理和下采样后直接利用imu传感器信息积分出的机器人运动状态计算出来的，因此同时收到激光雷达和imu传感器的双重误差影响，并且由于前向传播机制，机器人运动状态的误差会不断累积。因此我们构建一个误差状态迭代卡尔曼滤波器来优化机器人的运动状态，减小传感器导致的误差；将点云帧采集结束时刻k的点云
Figure BDA0003829049500000061
转换到imu传感器坐标，再利用前向传播得到的k时刻机器人运动的先验估计，可以得到激光点j的在激光雷达点云帧采集结束时刻k的估计全局坐标系坐标点
Figure BDA0003829049500000062
Figure BDA0003829049500000063
对于每个雷达点而言，最近的平面或边缘应该由其地图上附近的点来定义。由此，残差
Figure BDA0003829049500000064
定义为激光点j的在激光雷达点云帧采集结束时刻k的估计全局坐标系坐标点
Figure BDA0003829049500000065
与地图上最近的平面或边缘的距离：
Figure BDA0003829049500000066
其中Gj等于地图上与激光点j最近的5个点所拟合出的小平面的法向量
Figure BDA0003829049500000067
Gqj为估计全局坐标系坐标点
Figure BDA0003829049500000068
在拟合平面上的投影点。
步骤3.2：当传感器误差为0时，imu数据前向传播得到的机器人状态先验估计即为机器人的实际运动状态，此时估计全局坐标系坐标点
Figure BDA0003829049500000069
即为实际地图上的点，因此
Figure BDA00038290495000000610
与Gqj处于同一拟合平面，残差
Figure BDA00038290495000000611
等于0，然而实际计算中我们可以发现由于误差影响任何时刻下
Figure BDA00038290495000000612
都不等于0，使用一个误差迭代卡尔曼滤波器来迭代优化
Figure BDA00038290495000000613
从而得到当残差小于阈值时的机器人运动状态的估计作为最佳估计；根据步骤2.3构建的机器人运动状态迭代方程，取运动状态的真实值为xi,建立误差状态的动态模型如下
Figure BDA00038290495000000614
将白噪声w的协方差记为Q，则前向传播的协方差
Figure BDA00038290495000000615
可按下式迭代计算得到
Figure BDA00038290495000000616
假设激光点测量值
Figure BDA0003829049500000071
对应的真实值为
Figure BDA0003829049500000072
真实值与观测值之间的误差原始测量噪声为
Figure BDA0003829049500000073
则有
Figure BDA0003829049500000074
将
Figure BDA0003829049500000075
代入残差计算方程可得
Figure BDA0003829049500000076
在
Figure BDA0003829049500000077
处进行一阶泰勒展开，可得残差方程的一阶近似
Figure BDA0003829049500000078
将激光点对应真实值的残差方程表达为观测值残差方程与真实值残差方程的雅克比矩阵与vj之和的形式，其中vj代表原始测量噪声
Figure BDA0003829049500000079
所产生的观测误差，是要通过迭代优化消除掉的干扰量；
将得到的观测误差方程与建立的状态误差方程联立，得到需要优化的观测误差的最大后验估计值
Figure BDA00038290495000000710
对观测误差的最大后验估计值用卡尔曼滤波器进行最小二乘优化，得到收敛并小于阈值的
Figure BDA00038290495000000711
此时对应的运动状态估计值
Figure BDA00038290495000000712
即为最优状态估计
Figure BDA00038290495000000713
步骤3.3：将步骤3.2中得到的最优状态估计
Figure BDA00038290495000000714
假设为这一点云帧采集结束时刻k的机器人运动状态真实值，可得k时刻的机器人相对于世界坐标系状态转换矩阵
Figure BDA00038290495000000715
包括旋转矩阵
Figure BDA00038290495000000716
和平移向量
Figure BDA00038290495000000717
利用摄像头采集到的像素点
Figure BDA00038290495000000718
的时间戳信息计算出摄像头图片采集时刻q与激光雷达点云帧采集结束时刻k之间机器人的相对运动状态，并得到对应摄像头图片采集时刻q与激光雷达点云帧采集结束时刻k之间的状态转换矩阵
Figure BDA00038290495000000719
将k时刻激光雷达坐标系中的激光点
Figure BDA00038290495000000720
转移到q时刻的激光雷达坐标系中，公式如下
Figure BDA00038290495000000721
得到q时刻的激光点云
Figure BDA0003829049500000081
后，利用预标定过的激光雷达传感器和视觉摄像头传感器间的坐标转换关系CTI和相机内参I将激光点从激光雷达传感器的xyz坐标系转移到相机的归一化坐标系，最后再转移到相机的像素坐标系，公式如下：
Figure BDA0003829049500000082
Figure BDA0003829049500000083
Figure BDA0003829049500000084
由于相机和激光雷达采集频率不同，激光雷达扫描结束时刻并没有精确对应的相机帧，因此无法对激光点云进行精确的RGB渲染，为了得到激光雷达扫描结束时刻的相机数据，我们使用稠密光流法得到激光雷达扫描结束时刻的相机数据，由于相邻两帧间采集间隔时间较短，可以假设两帧中同一物体的灰度不变，通过前后两帧中各个光流场的位移，估计出激光雷达时刻的相机数据，公式如下
Figure BDA0003829049500000085
根据激光点对应的像素点坐标进行筛选，像素坐标超出图片尺寸的激光点即为落在摄像头视野盲区外的点，将这部分激光点筛除掉，并将视觉摄像头采集到的像素点用双线性插值法计算出剩余的激光点的像素坐标处对应的RGB值，公式如下
Figure BDA0003829049500000086
将插值计算出的RGB值作为激光点云
Figure BDA0003829049500000087
的属性，保存为彩色激光点云
Figure BDA0003829049500000088
本发明技术方案的进一步改进在于：所述步骤4具体包括以下步骤：
步骤4.1：将得到RGB属性的激光点云
Figure BDA0003829049500000091
再依次转换回k时刻的雷达坐标系中，得到点云
Figure BDA0003829049500000092
随着状态
Figure BDA0003829049500000093
的更新，可得k时刻的机器人惯性坐标系相对于全局框架世界坐标系状态转换矩阵
Figure BDA0003829049500000094
同时已知预标定过的ITL是激光雷达坐标系和imu传感器坐标系间的旋转平移关系，将点云从k时刻雷达坐标系转移到全局框架下的世界坐标系，即可得到用于建图的彩色点云
Figure BDA0003829049500000095
步骤4.2：将地图以k维树的形式组织起来，k维树是二叉树的变种，用于储存维度为k的空间中的点，我们的激光雷达点依照xyz三个维度进行分割，构造k＝3的k维树；
在第一帧彩色点云
Figure BDA0003829049500000096
存放入地图中后，对这一帧中每一个点的x坐标值进行遍历并排序，将中值点作为根节点，所有x坐标值值小于根节点的点放入左侧的子树，所有x坐标值大于根节点的点放入右侧的子树；完成第一层的分割对左右两棵子树中点的y坐标值进行分别的遍历和排序，分别将中值点作为第二层的根节点，所有y坐标值值小于对应根节点的点放入对应根节点左侧的子树，所有y坐标值大于对应根节点的点放入对应根节点右侧的子树；完成第二层的分割后再分别对四棵子树的z坐标值进行遍历和排序，依次循环，直到最后一层的每个子树都包含一个点，即完成了k维树的初始化构建；
从第二帧开始，每当有新的点云存放入地图，点云中的每个点递归地从根节点向下搜索，并将新点的除法轴上的坐标与存储在树节点上的点进行比较，直到发现一个叶节点然后追加一个新的树节点；当一帧中的所有点都完成更新后，从叶节点向上判断子树是否已经不平衡，即一侧子树中的节点数是否远多于另一侧，如果检测到不平衡，则将对应子树重建；
步骤4.3：随着机器人的运动，将多帧间的机器人运动状态最优估计
Figure BDA0003829049500000101
可视化显示出来，即可得到机器人运动中的实时定位效果，将多帧用于建图的彩色点云
Figure BDA0003829049500000102
累加在同一张地图中，即可得到对应地图，当机器人完成对当前环境的遍历后，即可建立全局彩色地图。
由于采用了上述技术方案，本发明取得的技术进步是：
本发明提出了基于多传感器的机器人实时定位和彩色地图融合映射方法，通过读取机器人传感器组的数据，实时进行机器人的自定位，同时构建全局彩色地图。
本发明相对比其它方法，提出了一种快速的激光视觉数据融合方法，可以以极小的时间成本，为激光雷达所采集到的点云帧进行纹理和颜色渲染，利用激光信息和视觉信息的耦合，避免单一传感器造成的误差，构建全局彩色地图。同时，由于工业现场环境情况较为复杂，激光传感器和视觉传感器很难提取到相同的特征点集，本算法直接使用降采样后的完整点云帧进行地图构建，利用预标定的传感器外参进行数据的匹配。
附图说明
图1为算法整体流程图；
图2为激光传感器数据图；
图3为视觉传感器数据图；
图4为特征匹配算法图；
图5为数据融合算法图；
图6为全局地图效果图。
具体实施方式
以下将参考附图详细说明本发明的具体实施方式。
本发明的整体系统流程图如图1所示，步骤1：读取传感器数据，所使用传感器有OAK-D Lite相机、livox Horizon固态激光雷达、BMI088惯性传感器。
步骤2：对激光雷达点云数据进行预处理，将接受到的点云数据按照扫描时间累积，形成点云帧，并且进行体素化的下采样，降低计算量。
步骤3：利用读取到的惯性传感器数据进行初始化，构建惯性传感器数据的运动模型，对点云数据进行前向传播，得到时间对齐的点云帧。
步骤4：对步骤3得到的时间对齐的点云帧中的每一个点，利用惯性传感器数据进行反向传播，得到坐标系对齐的点云帧。
步骤5：对坐标系对齐的点云帧构建残差及误差迭代卡尔曼滤波器，不断迭代优化残差，得到帧间位姿的最优估计和位姿优化后的点云帧。
步骤6：对相机数据进行预处理，将接受到的相机数据先根据各个传感器内部时间差进行时间坐标系的同步，然后使用稠密光流法进行插帧处理，进行精密时间同步。
步骤7：将精密时间同步后的相机数据和位姿优化后的点云帧进行坐标系同步，构建融合的相机和激光雷达数据。
步骤8：将多帧融合后的彩色点云数据转入独立的全局坐标系，构建全局彩色地图。
所述步骤1具体包括以下步骤：
步骤1.1：读取BMI088惯性传感器的数据，包括加速度、角速度，和各个采样时刻的时间戳。由于激光雷达和惯性传感器是集成在一起的，因此传感器内部时间在同一个时间系中。
步骤1.2：读取livox Horizon激光雷达的数据，包括激光点的XYZ信息和反射强度，和各个激光点采集时刻的内部时间戳，激光雷达采集到的数据可视化后如图2所示。
步骤1.3：读取OAK-D Lite相机的数据，包括相机视场中当前帧各个像素点的RGB值，和当前帧采集时刻的内部时间戳。相机采集到的数据可视化后如图3所示。
所述步骤2包括具体以下步骤：
步骤2..1：对激光雷达采集到的激光点进行时间累积，形成激光点云帧。
步骤2.2：对激光点云帧进行初步筛选，剔除掉视场边缘的点、反射强度过低的点和视野盲区的点。
步骤2.3：对激光点云进行按0.1cm划分成单元格，每个单元格中的点加权求和，构造稀疏点云。
所述步骤3具体包括以下步骤：
步骤3.1：将前20帧传感器数据用于初始化，消除零点漂移。
步骤3.2：将对IMU数据进行预积分，得到IMU的离散运动学模型。
步骤3.3：对每帧获得的IMU数据进行积分，得到各个时刻的位置和速度的初始估计。
步骤3.4：积分一直持续到点云扫描完一帧，将此时的估计位姿作为激光点云帧扫描结束时刻的初始位姿估计。
步骤3.5：将当前帧的各个激光点，直接转换到到点云帧扫描结束时刻的坐标系中。
所述步骤4具体包括以下步骤：
步骤4.1：对于当前帧每一个激光点，根据步骤3.3得到的初始估计插值得到采集时刻对应的IMU状态。
步骤4.2：进行反向传播，将点云帧结束时刻和各个激光点采集时刻的运动状态中的位移信息进行相减，可以得到每个激光点的相对位移。
步骤4.3：利用相对位移，对各个激光点进行运动修正，得到优化后的帧结束时刻各个激光点所在的位置。
所述步骤5包括以下步骤：
步骤5.1：对于每个激光点，从地图中筛选最近的5个点，拟合出一个小平面。
步骤5.2：计算激光点到拟合平面的距离，作为点的残差。
步骤5.3：基于IMU运动模型和激光点的残差，构建误差迭代卡尔曼滤波器。
步骤5.4：迭代优化IMU的运动状态，得到残差小于阈值时的运动状态。
步骤5.5：将步骤5.4得到的运动状态，作为当前帧的最优位姿估计。
步骤5.6：利用最优位姿估计，对点云帧进行运动补偿，得到最优运动状态下的点云帧。
所述步骤6包括以下步骤：
步骤6.1：在接收到第一帧激光数据和相机数据时，计算两个传感器内部时钟的时间差。
步骤6.2：对相机时钟进行时间补偿，转换到激光雷达的时间系中。
步骤6.3：对于采集到的点云帧，获取到激光帧结束时刻前后两帧视觉帧。
步骤6.4：对于前后两帧视觉帧，计算图像上所有的点的偏移量，形成稠密光流场。
步骤6.5：通过稠密光流场，对图像的运动进行像素级别的估计。
步骤6.6：利用图像的运动估计，计算出激光帧结束时刻的估计视觉帧。
所述步骤7包括以下步骤：
步骤7.1：对激光雷达和相机数据进行预标定。
步骤7.2：提取激光雷达帧和视觉帧中的线特征信息。
步骤7.3：对两个传感器坐标系间的转换矩阵进行初始估计。
步骤7.4：基于初始估计，进行不同传感器间线特征的匹配。
步骤7.5：将不同传感器线特征之间的偏差设为残差。
步骤7.6：迭代优化转换矩阵，得到残差小于阈值的转换矩阵最优估计，最优估计的可视化结果如图4所示。
步骤7.7：将激光点云的XYZ坐标乘以转换矩阵，转移到相机坐标系中。
步骤7.8：将激光点云的xy坐标乘以相机内参，转化成像素坐标。
步骤7.9：剔除掉相机视场外的激光点。
步骤7.10：对于每个相机视场内的激光点，获取离它最近的四个点的RGB值。
步骤7.11：对这个四个点进行加权求和，计算激光点对应像素坐标的RGB值。
步骤7.12：将计算出的RGB值作为激光点的属性保存。
步骤7.13：将获得RGB属性后的激光点，转移到激光雷达坐标系中，形成单帧的彩色点云地图，可视化结果如图5所示。
所述步骤8包括以下步骤：
步骤8.1：将步骤7.12中得到单帧彩色点云转入到独立的全局坐标系中，即发布到全局地图中。
步骤8.2：迭代步骤3.3－8.1，直至采集流程结束，结果如图6所示。

Claims (4)

1.基于多传感器的机器人实时定位和彩色地图融合映射方法，其特征在于包括以下步骤：
步骤1：读取视觉传感器、激光雷达传感器、惯性传感器的数据，对不同源的图像、激光点云和imu位姿数据进行时间同步，获得统一的时间系统下的待融合数据，并利用惯性传感器采集到的前20帧数据进行初始化，建立初始位姿估计；
步骤2：基于imu传感器的高工作频率，惯性传感器选用imu传感器，对imu传感器采集到的惯性数据进行前向传播，得到各个时刻机器人位姿的先验估计；得到各个时刻间对机器人的位姿的先验估计后，通过对时刻进行插值，计算出每个激光点采集时刻和激光点云帧采集结束时刻间机器人的相对运动，将激光点从采集时刻的机器人坐标系转移到点云帧采集结束时刻的坐标系中，对当前帧中每一个激光点进行上述操作后，得到消除帧间运动畸变的点云帧采集结束时刻点云；
步骤3：使用误差状态迭代卡尔曼滤波器对不同帧间采集到的信息进行比较，在机器人运动过程中，一个地标物往往会在连续很多帧上同时存在，并且和机器人的相对位置不断变化，对新采集到的点云帧中的每一点，在地图中提取和当前位置最近的五个点拟合出一个小平面，通过迭代优化，不断缩小激光点和它对应的拟合平面间的距离，直到小于阈值，从而得到点云帧采集结束时刻对机器人运动的最优估计；利用计算出的最优估计，将点云帧采集结束时刻视觉传感器采集到的图像转移归一化坐标系并最终转移到雷达坐标系中，对当前帧中的每一个点，利用预标定过的激光雷达传感器和视觉传感器的旋转平移关系，从三维空间坐标转移到像素坐标系，给视觉传感器视场范围内的每个激光点赋予RGB信息，得到经过RGB渲染的彩色点云；使用基于时间的插帧处理，利用激光雷达传感器采集时刻的前后两帧图片，使用稠密光流法估计出激光雷达传感器采集时刻的视觉数据，实现激光雷达传感器和视觉传感器的时间同步；
步骤4：对经过RGB渲染的彩色点云通过点云帧采集结束时刻对机器人运动的最优估计，将点云从点云帧采集结束时刻的机器人运动坐标系转移到独立的世界坐标系，从而构建出当前帧的彩色点云地图；将地图中的点以K维树的形式组织起来，使得新采集到的每一个点都可以用最短的时间找到它在地图中的最邻近点；通过时间累积，将机器人采集到的多帧信息累加，得到连续各帧间的机器人运动最优估计和各帧的彩色点云地图，实现机器人的自我定位并建立全局彩色地图。
2.根据权利要求1所述的基于多传感器的机器人实时定位和彩色地图融合映射方法，其特征在于所述步骤2具体包括以下步骤：
步骤2.1：建立imu运动学模型，对于机器人运动的描述中最重要的两个物理量是当前时刻的位姿和相对于坐标系原点的位移量，使用imu传感器测量得到的角速度和加速度数据对时间维度积分得到所求的位姿和位移状态，因此建立在世界坐标系下的状态表达方程：
Figure FDA0003829049490000021
在建立世界坐标系的状态表达方程中一共有6个状态量，分别是机器人在世界坐标系下的位姿RI，即机器人相对于初始状态的旋转量，使用一个3*3的旋转矩阵来表示；机器人在世界坐标系下相对于原点的位移量pI，使用一个3*1的平移向量来表示；机器人在x、y、z三个轴上的瞬时速度vI,使用用一个3*1的速度向量来表示；机器人惯性imu传感器采集数据的在角速度和加速度上的偏差bω和ba；机器人世界坐标系相对于真实世界坐标系下的重力矢量g；
步骤2.2：对于建立好的状态方程，对于imu数据进行预积分，建立imu运动学连续模型；其中，机器人在世界坐标系下的位姿的一阶微分量等于机器人相对于初始状态的旋转矩阵右乘消除噪音后的角速度向量的反对称矩阵；机器人在世界坐标系下的位移的一阶微分量等于机器人在x、y、z三个轴上的瞬时速度；机器人的速度的一阶微分量等于机器人相对于初始状态的旋转矩阵乘以消除噪音后的加速度向量再加上机器人世界坐标系下的重力矢量；机器人惯性imu传感器采集数据的在角速度和加速度上的偏差bω和ba的模型为带高斯噪声nbω和nba的随机游动过程；机器人世界坐标系下的重力矢量g的一阶微分量为0，由此可得机器人的imu运动学方程如下：
Figure FDA0003829049490000031
Figure FDA0003829049490000034
Figure FDA0003829049490000035
Figure FDA0003829049490000036
Figure FDA0003829049490000037
Figure FDA0003829049490000038
对上述方程组离散化可得
Figure FDA0003829049490000032
步骤2.3：步骤1中进行初始化，得到了机器人运动状态的初始估计，当机器人收到imu传感器的输入时，即可根据步骤2.2中建立的离散运动学模型进行前向传播，通过初始估计和imu传感器每一帧采集到的加速度和角速度数据，迭代出每一帧时刻的机器人位姿的先验估计；具体的迭代方程如下
xi+1＝xi+(Dtf(xi,ui,wi))
Figure FDA0003829049490000033
其中控制量ui为imu传感器采集到的的加速度数据am和角速度数据ωm,误差量w为imu传感器的累积误差nω、na和imu传感器采集到的数据的误差nbω、nba；
步骤2.4：激光雷达传感器采集完完整一帧点云后，根据imu传感器数据前向传播得到的各个时刻的机器人运动状态进行插值计算，得到每个激光点采集时刻对应的机器人运动状态，并计算出每个激光点采集时刻到点云帧采集结束时刻的机器人相对运动状态
Figure FDA0003829049490000041
其中j为每个雷达点的采集时刻，k为点云帧的采集时刻；将j时刻采集到的激光点先从j时刻的激光雷达坐标系转移到imu传感器坐标系，再利用j时刻到k时刻的机器人的相对运动将激光点从j时刻的imu传感器坐标系转移到k时刻的imu传感器坐标系，最后再将激光点先从k时刻的imu传感器坐标系转移到激光雷达坐标系，激光点的坐标系转换过程如下公式所示：
Figure FDA0003829049490000042
其中ITL是激光雷达坐标系和imu传感器坐标系间的旋转平移关系，由于激光雷达传感器和imu传感器二者之间有固定的物理连接关系，所以ITL是一个固定量，并通过预标定得到。
3.根据权利要求2所述的基于多传感器的机器人实时定位和彩色地图融合映射方法，其特征在于所述步骤3具体包括以下步骤：
步骤3.1：构建一个误差状态迭代卡尔曼滤波器来优化机器人的运动状态，减小传感器导致的误差；将点云帧采集结束时刻k的点云
Figure FDA0003829049490000043
转换到imu传感器坐标，再利用前向传播得到的k时刻机器人运动的先验估计，可以得到激光点j的在激光雷达点云帧采集结束时刻k的估计全局坐标系坐标点
Figure FDA0003829049490000044
Figure FDA0003829049490000045
最近的平面或边缘应该由其地图上附近的点来定义，残差
Figure FDA0003829049490000046
定义为激光点j的在激光雷达点云帧采集结束时刻k的估计全局坐标系坐标点
Figure FDA0003829049490000047
与地图上最近的平面或边缘的距离：
Figure FDA0003829049490000048
其中Gj等于地图上与激光点j最近的5个点所拟合出的小平面的法向量
Figure FDA0003829049490000051
Gqj为估计全局坐标系坐标点
Figure FDA0003829049490000052
在拟合平面上的投影点；
步骤3.2：当传感器误差为0时，imu数据前向传播得到的机器人状态先验估计即为机器人的实际运动状态，此时估计全局坐标系坐标点
Figure FDA0003829049490000053
即为实际地图上的点，因此
Figure FDA0003829049490000054
与Gqj处于同一拟合平面，残差
Figure FDA0003829049490000055
等于0，由于误差影响任何时刻下
Figure FDA0003829049490000056
都不等于0，使用一个误差迭代卡尔曼滤波器来迭代优化
Figure FDA0003829049490000057
从而得到当残差小于阈值时的机器人运动状态的估计作为最佳估计；根据步骤2.3构建的机器人运动状态迭代方程，取运动状态的真实值为xi,建立误差状态的动态模型如下
Figure FDA0003829049490000058
将白噪声w的协方差记为Q，则前向传播的协方差
Figure FDA0003829049490000059
可按下式迭代计算得到
Figure FDA00038290494900000510
假设激光点测量值
Figure FDA00038290494900000518
对应的真实值为
Figure FDA00038290494900000511
真实值与观测值之间的误差原始测量噪声为
Figure FDA00038290494900000512
则有
Figure FDA00038290494900000513
将
Figure FDA00038290494900000514
代入残差计算方程可得
Figure FDA00038290494900000515
在
Figure FDA00038290494900000519
处进行一阶泰勒展开，可得残差方程的一阶近似
Figure FDA00038290494900000516
将激光点对应真实值的残差方程表达为观测值残差方程与真实值残差方程的雅克比矩阵与vj之和的形式，其中vj代表原始测量噪声
Figure FDA00038290494900000517
所产生的观测误差，是要通过迭代优化消除掉的干扰量；
将得到的观测误差方程与建立的状态误差方程联立，得到需要优化的观测误差的最大后验估计值
Figure FDA0003829049490000061
对观测误差的最大后验估计值用卡尔曼滤波器进行最小二乘优化，得到收敛并小于阈值的
Figure FDA0003829049490000062
此时对应的运动状态估计值
Figure FDA0003829049490000063
即为最优状态估计
Figure FDA0003829049490000064
步骤3.3：将步骤3.2中得到的最优状态估计
Figure FDA0003829049490000065
假设为这一点云帧采集结束时刻k的机器人运动状态真实值，可得k时刻的机器人相对于世界坐标系状态转换矩阵
Figure FDA0003829049490000066
包括旋转矩阵
Figure FDA0003829049490000067
和平移向量
Figure FDA0003829049490000068
利用摄像头采集到的像素点
Figure FDA0003829049490000069
的时间戳信息计算出摄像头图片采集时刻q与激光雷达点云帧采集结束时刻k之间机器人的相对运动状态，并得到对应摄像头图片采集时刻q与激光雷达点云帧采集结束时刻k之间的状态转换矩阵
Figure FDA00038290494900000610
将k时刻激光雷达坐标系中的激光点
Figure FDA00038290494900000611
转移到q时刻的激光雷达坐标系中，公式如下
Figure FDA00038290494900000612
得到q时刻的激光点云
Figure FDA00038290494900000613
后，利用预标定过的激光雷达传感器和视觉摄像头传感器间的坐标转换关系CTI和相机内参I将激光点从激光雷达传感器的xyz坐标系转移到相机的归一化坐标系，最后再转移到相机的像素坐标系，公式如下：
Figure FDA00038290494900000614
Figure FDA00038290494900000615
Figure FDA00038290494900000616
使用稠密光流法得到激光雷达扫描结束时刻的相机数据，由于相邻两帧间采集间隔时间较短，可以假设两帧中同一物体的灰度不变，通过前后两帧中各个光流场的位移，估计出激光雷达时刻的相机数据，公式如下
Figure FDA0003829049490000071
根据激光点对应的像素点坐标进行筛选，像素坐标超出图片尺寸的激光点即为落在摄像头视野盲区外的点，将这部分激光点筛除掉，并将视觉摄像头采集到的像素点用双线性插值法计算出剩余的激光点的像素坐标处对应的RGB值，公式如下
Figure FDA0003829049490000072
将插值计算出的RGB值作为激光点云
Figure FDA0003829049490000073
的属性，保存为彩色激光点云
Figure FDA0003829049490000074
4.根据权利要求3所述的基于多传感器的机器人实时定位和彩色地图融合映射方法，其特征在于所述步骤4具体包括以下步骤：
步骤4.1：将得到RGB属性的激光点云
Figure FDA0003829049490000075
再依次转换回k时刻的雷达坐标系中，得到点云
Figure FDA0003829049490000076
随着状态
Figure FDA0003829049490000077
的更新，可得k时刻的机器人惯性坐标系相对于全局框架世界坐标系状态转换矩阵
Figure FDA0003829049490000078
同时已知预标定过的ITL是激光雷达坐标系和imu传感器坐标系间的旋转平移关系，将点云从k时刻雷达坐标系转移到全局框架下的世界坐标系，即可得到用于建图的彩色点云
Figure FDA0003829049490000079
步骤4.2：将地图以k维树的形式组织起来，k维树是二叉树的变种，用于储存维度为k的空间中的点，我们的激光雷达点依照xyz三个维度进行分割，构造k＝3的k维树；
在第一帧彩色点云
Figure FDA0003829049490000081
存放入地图中后，对这一帧中每一个点的x坐标值进行遍历并排序，将中值点作为根节点，所有x坐标值值小于根节点的点放入左侧的子树，所有x坐标值大于根节点的点放入右侧的子树；完成第一层的分割对左右两棵子树中点的y坐标值进行分别的遍历和排序，分别将中值点作为第二层的根节点，所有y坐标值值小于对应根节点的点放入对应根节点左侧的子树，所有y坐标值大于对应根节点的点放入对应根节点右侧的子树；完成第二层的分割后再分别对四棵子树的z坐标值进行遍历和排序，依次循环，直到最后一层的每个子树都包含一个点，即完成了k维树的初始化构建；
从第二帧开始，每当有新的点云存放入地图，点云中的每个点递归地从根节点向下搜索，并将新点的除法轴上的坐标与存储在树节点上的点进行比较，直到发现一个叶节点然后追加一个新的树节点；当一帧中的所有点都完成更新后，从叶节点向上判断子树是否已经不平衡，即一侧子树中的节点数是否远多于另一侧，如果检测到不平衡，则将对应子树重建；
步骤4.3：随着机器人的运动，将多帧间的机器人运动状态最优估计
Figure FDA0003829049490000082
可视化显示出来，即可得到机器人运动中的实时定位效果，将多帧用于建图的彩色点云
Figure FDA0003829049490000083
累加在同一张地图中，即可得到对应地图，当机器人完成对当前环境的遍历后，即可建立全局彩色地图。
CN202211074370.0A 2022-09-02 2022-09-02 基于多传感器的机器人实时定位和彩色地图融合映射方法 Pending CN115526914A (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202211074370.0A CN115526914A (zh) 2022-09-02 2022-09-02 基于多传感器的机器人实时定位和彩色地图融合映射方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202211074370.0A CN115526914A (zh) 2022-09-02 2022-09-02 基于多传感器的机器人实时定位和彩色地图融合映射方法

Publications (1)

Publication Number Publication Date
CN115526914A true CN115526914A (zh) 2022-12-27

Family

ID=84696902

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202211074370.0A Pending CN115526914A (zh) 2022-09-02 2022-09-02 基于多传感器的机器人实时定位和彩色地图融合映射方法

Country Status (1)

Country Link
CN (1) CN115526914A (zh)

Cited By (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN116380148A (zh) * 2023-04-06 2023-07-04 中国人民解放军93209部队 多传感器目标跟踪系统的两级时空误差标校方法及装置
CN116678423A (zh) * 2023-05-26 2023-09-01 小米汽车科技有限公司 多源融合定位方法、装置及车辆

Cited By (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN116380148A (zh) * 2023-04-06 2023-07-04 中国人民解放军93209部队 多传感器目标跟踪系统的两级时空误差标校方法及装置
CN116380148B (zh) * 2023-04-06 2023-11-10 中国人民解放军93209部队 多传感器目标跟踪系统的两级时空误差标校方法及装置
CN116678423A (zh) * 2023-05-26 2023-09-01 小米汽车科技有限公司 多源融合定位方法、装置及车辆
CN116678423B (zh) * 2023-05-26 2024-04-16 小米汽车科技有限公司 多源融合定位方法、装置及车辆

Similar Documents

Publication Publication Date Title
CN107255476B (zh) 一种基于惯性数据和视觉特征的室内定位方法和装置
CN111929699B (zh) 一种顾及动态障碍物的激光雷达惯导里程计与建图方法及系统
CN115526914A (zh) 基于多传感器的机器人实时定位和彩色地图融合映射方法
CN110189399B (zh) 一种室内三维布局重建的方法及系统
JP6456141B2 (ja) 地図データの生成
CN110298914B (zh) 一种建立果园中果树冠层特征地图的方法
WO2023131123A1 (zh) 组合导航设备与激光雷达的外参标定方法及装置
CN113269837B (zh) 一种适用于复杂三维环境的定位导航方法
CN107917710B (zh) 一种基于单线激光的室内实时定位与三维地图构建方法
CN111780781B (zh) 基于滑动窗口优化的模板匹配视觉和惯性组合里程计
CN111524194B (zh) 一种激光雷达和双目视觉相互融合的定位方法及终端
CN114526745A (zh) 一种紧耦合激光雷达和惯性里程计的建图方法及系统
CN111623773B (zh) 一种基于鱼眼视觉和惯性测量的目标定位方法及装置
CN112465732A (zh) 一种车载激光点云与序列全景影像的配准方法
CN115479598A (zh) 基于多传感器融合的定位与建图方法及紧耦合系统
CN115272596A (zh) 一种面向单调无纹理大场景的多传感器融合slam方法
CN116449384A (zh) 基于固态激光雷达的雷达惯性紧耦合定位建图方法
CN114397642A (zh) 一种基于图优化的三维激光雷达与imu外参标定方法
CN116758234A (zh) 一种基于多点云数据融合的山地地形建模方法
CN112580683A (zh) 一种基于互相关的多传感器数据时间对齐系统及其方法
CN115218906A (zh) 面向室内slam的视觉惯性融合定位方法及系统
CN115355904A (zh) 一种针对地面移动机器人的Lidar-IMU融合的slam方法
CN117388830A (zh) 激光雷达与惯性导航的外参标定方法、装置、设备及介质
CN110021041B (zh) 基于双目相机的无人驾驶场景增量式网格化结构重建方法
CN113495281B (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