CN115077519A - 基于模板匹配与激光惯导松耦合的定位建图方法和装置 - Google Patents
基于模板匹配与激光惯导松耦合的定位建图方法和装置 Download PDFInfo
- Publication number
- CN115077519A CN115077519A CN202210735304.7A CN202210735304A CN115077519A CN 115077519 A CN115077519 A CN 115077519A CN 202210735304 A CN202210735304 A CN 202210735304A CN 115077519 A CN115077519 A CN 115077519A
- Authority
- CN
- China
- Prior art keywords
- frame
- time
- robot
- laser
- odometer
- 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 46
- 238000013507 mapping Methods 0.000 title claims abstract description 15
- 230000008878 coupling Effects 0.000 title claims abstract description 11
- 238000010168 coupling process Methods 0.000 title claims abstract description 11
- 238000005859 coupling reaction Methods 0.000 title claims abstract description 11
- 238000005259 measurement Methods 0.000 claims abstract description 46
- 230000000007 visual effect Effects 0.000 claims abstract description 22
- 230000010354 integration Effects 0.000 claims abstract description 12
- 238000006073 displacement reaction Methods 0.000 claims description 45
- 230000033001 locomotion Effects 0.000 claims description 30
- 239000011159 matrix material Substances 0.000 claims description 14
- 238000004422 calculation algorithm Methods 0.000 claims description 10
- 230000001133 acceleration Effects 0.000 claims description 9
- 238000013519 translation Methods 0.000 claims description 7
- 238000012937 correction Methods 0.000 claims description 6
- 238000001514 detection method Methods 0.000 claims description 6
- 238000009616 inductively coupled plasma Methods 0.000 claims description 6
- 238000012545 processing Methods 0.000 claims description 6
- 238000004364 calculation method Methods 0.000 claims description 5
- 230000008859 change Effects 0.000 claims description 5
- 230000006870 function Effects 0.000 claims description 5
- 230000008569 process Effects 0.000 claims description 5
- 230000009466 transformation Effects 0.000 claims description 5
- 230000004927 fusion Effects 0.000 claims description 4
- 206010034719 Personality change Diseases 0.000 claims description 3
- 230000005484 gravity Effects 0.000 claims description 3
- 230000017105 transposition Effects 0.000 claims description 2
- 238000000605 extraction Methods 0.000 claims 2
- 238000005286 illumination Methods 0.000 abstract description 5
- 238000010586 diagram Methods 0.000 description 5
- 238000004590 computer program Methods 0.000 description 4
- 238000013459 approach Methods 0.000 description 3
- 230000015556 catabolic process Effects 0.000 description 2
- 238000006731 degradation reaction Methods 0.000 description 2
- 238000012986 modification Methods 0.000 description 2
- 230000004048 modification Effects 0.000 description 2
- 230000003287 optical effect Effects 0.000 description 2
- 238000005457 optimization Methods 0.000 description 2
- 230000003252 repetitive effect Effects 0.000 description 2
- 230000009286 beneficial effect Effects 0.000 description 1
- 238000006243 chemical reaction Methods 0.000 description 1
- 238000007689 inspection Methods 0.000 description 1
- 238000009434 installation Methods 0.000 description 1
- 230000004807 localization Effects 0.000 description 1
- 238000012544 monitoring process Methods 0.000 description 1
- 239000013589 supplement Substances 0.000 description 1
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
- 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
- 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/20—Instruments for performing navigational calculations
-
- 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/38—Electronic maps specially adapted for navigation; Updating thereof
- G01C21/3804—Creation or updating of map data
- G01C21/3833—Creation or updating of map data characterised by the source of data
- G01C21/3841—Data obtained from two or more sources, e.g. probe vehicles
Landscapes
- Engineering & Computer Science (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Automation & Control Theory (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
- Image Analysis (AREA)
Abstract
本发明提供了基于模板匹配与激光惯导松耦合的定位建图方法和装置，所述方法包括以下步骤：步骤1，计算惯性测量单元IMU预积分因子；步骤2，计算激光里程计因子；步骤3，计算回环因子；步骤4，计算模板匹配视觉里程计；步骤5，通过激光里程计和视觉里程计联合判断保存关键帧。本发明能够保证机器人在无需GPS的空旷环境或者室内长走廊环境的连续有效定位，设置朝下的摄像头较传统的朝前摄像头受光照和动态物体的影响小，模板匹配vo独立运行，vo失效并不影响激光里程计保存关键帧，并且，取vo的相对位姿而不是绝对位姿的方法降低了累计误差。
Description
技术领域
本发明属于机器人定位技术领域，尤其涉及基于模板匹配与激光惯导松耦合的定位建图方法和装置。
背景技术
鲁棒定位是对自动驾驶陆地车辆的基本要求。在走廊、隧道等相似场景或操场、田野等空旷场景下，基于点云信息的传统激光SLAM(Simultaneous Localization andMapping，即时定位与建图)在一些特定特征稀疏场景下将会退化，这是由于机器人所处环境的几何信息缺乏某些方向的约束，导致激光SLAM系统在相应方向上的状态估计准确性降低。对于激光雷达而言，结构特征稀疏场景通常为连续重复场景，在此场景下几何约束信息不足，因此无法通过点云配准正确恢复运动信息，也就无法分辨几何上连续重复的场景，如在长直走廊、长隧道、开阔的公路、单侧墙、桥梁等典型的单方向结构特征稀疏场景中，激光SLAM在前后方向上定位与建图都将产生较大偏差，这是由传感器本身的限制导致的。在这类场景下，激光SLAM将发生退化，导致机器人定位结果较大程度偏离真实值，所得地图亦会出现交叉重叠的现象，进而导致后续的机器人导航任务的失败。
目前，处理该问题的一种措施是在SLAM系统中进行多传感器融合，实现信息互补和优化组合，提高整体精度。例如，在激光SLAM系统中增加光学相机，使用相机来识别在场景中事先放置的二维码路标，以此生成闭环信号。然而该方法对场景的光照条件有较高的要求，且需要人为布置路标，在实际应用中局限性较大；有学者使用UWB(Ultra Wide Band，无线超宽带脉冲技术)与激光雷达融合的方法解决隧道等几何环境单一环境的定位问题，但UWB应用场景有限，信号无法覆盖室外大范围场景；PL2VI-SAM算法包含了一个紧耦合的激光雷达单目相机惯导SLAM系统，该系统由激光雷达惯导子系统(LIS)以及点线特征融合的视觉惯导子系统(PLVIS)组成，两个子系统相辅相成，实现了一个完整的激光雷达视觉惯性里程计(LVIO)。但该系统也易受光照变化的影响，且在场景中存在动态物体的情况下会导致相机跟踪失败；LVI-SAM算法针对退化场景的问题对系统进行了改进，将激光雷达、相机以及IMU紧耦合实现激光雷达视觉惯性里程计，其包含视觉惯性系统(VINS-MONO)以及雷达惯性系统(LIO-SAM)，两个子系统相互辅助完成系统初始化、状态估计以及回环检测，当遇到低纹理或退化场景的时候进行故障检测，并对系统进行重新初始化，可以有效阻止系统跟踪失败，但会对定位精度产生一定影响。
融合了视觉里程计的方法能在一定程度上纠正偏差，但是引入的视觉里程计是通常是基于特征的方法，基于特征的方法通过在连续图像帧之间提取图像特征，并在所提取的特征之间对这些特征进行匹配或跟踪。该方法适用于纹理丰富的场景，易受动态物体和光照变化的影响，例如具有大量特征点的城市环境。这种方法无法处理单一模式的无纹理或低纹理环境，因为在这些环境中无法检测到足够的显著特征。另一种通过视觉估计机器人位置的方法是基于外观的方法，即监视获取的图像的外观变化以及其中的像素信息强度。这种方法中常用的方法是模板匹配。此方法从当前图像帧中选择一个模板，并尝试在下一帧中匹配此模板。通过匹配两个连续图像帧之间的模板，来检索机器人位移和旋转角度。基于特征的方法相比之下，模板匹配方法非常适合在低纹理场景中使用。
激光SLAM的退化问题若能得到良好解决，机器人感知能力的鲁棒性与可靠性都将获得显著提高。在面临物流、巡检、搜救等任务中常见的结构特征稀疏场景时，机器人可表现出更好的自身定位能力、场景还原能力，为导航功能提供更稳定可靠的保障。
发明内容
发明目的：本发明所要解决的技术问题是针对现有技术的不足，提供基于模板匹配与激光惯导松耦合的定位建图方法，在LIO-SAM算法的因子图基础上，加入朝下的摄像头和IMU(Inertial Measurement Unit，惯性测量单元)通过模板匹配算法进行航迹推算，构建模板匹配VO(Visual Odometry，视觉里程计)因子。在激光里程计失效从而不保存关键帧的情况下使用模板匹配VO判断运动状态并决定是否保存关键帧，如果保持关键帧，则将通过模板匹配得到的里程数加入两帧关键帧之间代替激光里程计，通过因子图进行优化。同时，提取IMU预积分得到的速度，在固定运动阈值保存关键帧算法中加入速度阈值判断，提高了定位建图精度。
所述方法包括以下步骤：
步骤1，计算惯性测量单元IMU(Inertial Measurement Unit惯性测量单元)预积分因子；
步骤2，计算激光里程计因子；
步骤3，计算回环因子；
步骤4，计算模板匹配视觉里程计；
步骤5，通过激光里程计和视觉里程计联合判断保存关键帧。
步骤1包括：针对在不同频率下测量得到的惯性测量单元IMU和激光雷达LiDAR数据，设定i时刻和j时刻分别为选取一个关键帧的起始时刻和终止时刻，将i时刻至j时刻的传感器测量数据转换成为两个关键帧之间的单个运动约束，利用关键帧的单个运动约束简化i时刻至j时刻内惯性测量单元IMU数据的处理过程，同时以激光雷达点云帧为准选取关键帧，使激光雷达传感器和IMU传感器在数据融合时实现时间戳的对齐。
步骤1中，通过如下公式计算惯性测量单元IMU在t时刻的原始角速度测量值
和原始加速度测量值
其中，bt表示偏置，nt表示白噪声，
是从世界帧W到机器人帧B的旋转矩阵，g是W中的恒定重力矢量；ωt为真实角速度值，
为角速度偏置，at为真实加速度值，
为加速度偏置，
为加速度白噪声，
为角速度白噪声；
通过如下公式计算在t+Δt时刻机器人的速度vt+Δt、位置pt+Δt和旋转角Rt+Δt：
其中，
Rt表示从世界帧到机器人帧的旋转矩阵，exp是以自然常数e为底的指数函数；
应用惯性测量单元IMU预积分方法来获得两个时间戳之间的机器人相对运动，通过如下公式计算在i时刻和j时刻之间的机器人相对运动：
其中Δvij为在i时刻和j时刻之间的机器人相对速度变化，
为机器人在i时刻的旋转矩阵的转置，Rj为机器人在j时刻的旋转矩阵，ΔRij为机器人在i和j时刻的相对旋转矩阵。vj和vi分别为机器人j时刻的速度和i时刻的速度，Δtij为机器人在i和j时刻的时间间隔，Δpij为机器人在i和j时刻之间的位置变化，pj和pi分别为机器人在j时刻的位置和i时刻的位置。
步骤2包括：利用当前激光帧起止时刻间的惯性测量单元IMU数据计算旋转增量，使用惯性测量单元IMU里程计数据计算平移增量，对当前激光帧激光点云每一时刻的激光点进行运动畸变校正，同时用惯性测量单元IMU数据的姿态角、惯性测量单元IMU里程计数据的位姿，对当前激光帧激光位姿进行粗略初始化；
对经过运动畸变校正后的当前帧激光点云，计算每个点的曲率，提取角点、平面点；获取前后两帧的角点和平面点特征点云后，进行帧间点云匹配，获取相邻两帧平移旋转变换，由初始位姿进行累计，从而获得激光里程计信息，并发布出去，激光里程计信息包括(rx，ry，rz，tx，ty，tz)，rx，ry，rz，tx，ty，tz分别表示横滚角，俯仰角，航向角，x坐标，y坐标，z坐标；当机器人姿态变化超过了自定义的阈值时，选择一个激光雷达帧Fi+1作为关键帧；将保存的关键帧Fi+1与因子图中的一个新的机器人状态节点xi+1相关联(因子图是一个在贝叶斯推理中得到广泛应用的模型，是由各种因子构建出来的)，将上一个关键帧和当前时刻保存的关键帧之间的激光点云特征全丢弃。
步骤3包括：当在因子图中添加一个新的状态节点时，首先搜索地图(地图是点云地图，是机器人运动过程中不断保存下来的，是现有技术)，在历史关键帧中以机器人当前位姿为搜索点(除了当前关键帧其余关键帧都是历史关键帧)，查找半径为X1(一般取值为7m)范围内的若干个位姿，利用时间作为约束，查找历史位姿对应时间与当前位姿对应时间的时间差大于X2(一般取值为30s)的位姿的帧设为匹配帧，在匹配帧周围提取局部关键帧map，执行点云配准scan-to-map(scan是当前帧，map是当前帧时空维度上临近的帧组成的集合匹配，点云配准)，使用PCL函数库的ICP算法求解出当前点云和目标点云之间的相对位姿：
T1＝Ticp*Tk，
其中Tk是当前位姿，Ticp是ICP算法计算出来的当前帧与回环检测帧之间的相对位姿；T1为点云配准后的目标位姿，包含roll,pitch,yaw,x,y,z，其中roll,pitch,yaw分别表示横滚角、俯仰角、航向角。
步骤4包括：将单目相机垂直地面安装在移动机器人前端，拍摄一系列包含地面纹理特征的图像序列，从图像序列中按顺序依次取两帧图像，记为第i帧图像和第i+1帧图像，在第i帧图像中心区域取一块高为Th、宽为Tw的矩形区域作为模板，并在第i+1帧图像中对所述模板进行匹配，即在第i+1帧图像中找到与所述模板相似程度最高的矩形区域，利用归一化互相关系数作为模板相似度准则。
步骤4中，将匹配所得矩形区域的左上角坐标(u1,v1)与模板的左上角坐标(u0,v0)作差运算，分别得到模板在图像坐标系中x轴方向的像素位移增量Δu和y轴方向的像素位移增量Δv：
将Δu和Δv转换为相机在世界坐标系中的真实位移增量，具体公式如下所示：
其中Δxc和Δyc分别为相机坐标系中相机在x方向的位移增量和y方向的位移增量，fx和fy分别为相机在x方向的归一化焦距和y方向的归一化焦距，h为相机距离地面的高度；
通过如下公式将位移增量从相机坐标系转换到机器人坐标系：
其中Δxv、Δyv分别为机器人坐标系的x方向的位移增量和y方向的位移增量；
利用惯性测量单元IMU提供的偏航角代替基于阿克曼转向模型计算的旋转角，作为相机从第i时刻到第i+1时刻的旋转角θi+1；
根据第i时刻相机的位置坐标
从第i时刻到第i+1时刻的位移增量
以及第i+1时刻惯性测量单元IMU解算出的相机旋转角度θi+1，利用如下公式计算出第i+1时刻机器人的位置坐标(xv,i+1，yv,i+1)：
其中，
为第i+1时刻的旋转矩阵。
步骤5包括：引入模板匹配VO判断激光里程计与真实运动状态，如果在相似环境或空旷环境中激光里程计失效，不保存关键帧，从而显示移动机器人没有移动；基于模板匹配VO，通过运动阈值保存关键帧并将两帧关键帧之间的位移插入两帧关键帧之间，恢复机器人位姿。
步骤5具体包括如下步骤：
步骤5-1，在机器人操作系统ROS(Robot Operating System)中订阅模板匹配VO的话题，话题的消息为机器人移动的x方向的位移和y方向的位移；将接收到的话题信息msg依次放入队列VoQueue中，最后一个放入队列中的消息总是被最先拿出来，这个特性通常称为后进先出队列；
步骤5-2，每当新接收一个msg，都与最早进入的msg话题的x方向位移和y方向位移分别作差：
dt_vo_y＝VoQueue.back().gyVoQueue.font().y
dt_vo_x＝VoQueue.back().x-VoQueue.font().x；
其中dt_vo_y为从开始时刻到当前时刻在y方向的位移，dt_vo_x为从开始时刻到当前时刻在x方向的位移，VoQueue.back()为提取队列VoQueue的队尾元素，VoQueue.font()为提取队列VoQueue的队首元素；
步骤5-3，得到机器人当前时刻的总位移ds：
步骤5-4，定义一个bool型变量vo_save，当ds＞1时，清空VoQueue，并将vo_save置为0；ds＜＝1时，vo_save置为1，同时计算激光里程计LO；
步骤5-5，定义一个bool型变量save_frame，如果激光里程计超过设定的阈值(一般设置为1m和10°)，save_frame置为1，保存当前关键帧并将LO加入前一帧关键帧和当前关键帧两帧之间；
步骤5-6，如果激光里程计没有超过阈值，save_frame置为0，而模板匹配VO有里程，vo_save为1，则保存关键帧，并把vo加入两帧之间；
步骤5-7，如果激光里程计没有设定的阈值则save_frame置为0；
步骤5-8，如果save_frame和vo_save都为0，则判定当前机器人没有移动，不保存关键帧。
本发明还提供了基于模板匹配与激光惯导松耦合的定位建图装置，包括：
预积分因子计算模块，用于，计算惯性测量单元IMU预积分因子；
激光里程计因子计算模块，用于，计算激光里程计因子；
回环因子计算模块，用于，计算回环因子；
模板匹配视觉里程计计算模块，用于，计算模板匹配视觉里程计；
判定模块，用于，通过激光里程计和视觉里程计联合判断保存关键帧。
本发明具有如下有益效果：本发明针对在走廊、隧道等相似场景或操场、田野等空旷场景中，激光雷达观测到的数据会在某一维度呈现高度的相似性，导致点云配准结果收敛至错误位姿，严重地影响了SLAM整体的性能的问题，提出加入朝下的摄像头利用模板匹配算法航迹推算，将模板匹配vo因子加入到因子图中补偿由于激光雷达观测值劣化产生的定位失效，保证机器人在无需GPS的空旷环境或者室内长走廊环境的连续有效定位。朝下的摄像头较传统的朝前摄像头受光照和动态物体的影响小，模板匹配vo独立运行，vo失效并不影响激光里程计保存关键帧，并且，取vo的相对位姿而不是绝对位姿的方法降低了累计误差。
附图说明
下面结合附图和具体实施方式对本发明做更进一步的具体说明，本发明的上述和/或其他方面的优点将会变得更加清楚。
图1是模板匹配示意图。
图2是因子图。
图3是本发明方法流程图。
图4是保存关键帧流程图。
图5是相机坐标系与图像坐标系示意图。
具体实施方式
如图1、图2、图3、图4所示，本发明提供了基于模板匹配与激光惯导松耦合的定位建图方法，包括如下步骤：
步骤1，IMU预积分因子
针对在不同频率下测量得到的惯性测量单元IMU和激光雷达LiDAR数据，设定i时刻和j时刻分别为选取一个关键帧的起始时刻和终止时刻，将i时刻至j时刻的传感器测量数据转换成为两个关键帧之间的单个运动约束，利用关键帧的单个约束简化i时刻至j时刻内惯性测量单元IMU数据的处理过程，同时以激光雷达点云帧为准选取关键帧，能够使两种传感器在数据融合时实现时间戳的对齐。
通过如下公式计算惯性测量单元IMU在t时刻的原始角速度测量值
和原始加速度测量值
可以使用惯性测量单元IMU的原始测量结果来推算机器人的运动，通过如下公式计算在t+Δt时刻机器人的速度vt+Δt、位置pt+Δt和旋转角Rt+Δt：
其中，
应用惯性测量单元IMU预积分方法来获得两个时间戳之间的机器人相对运动，通过如下公式计算在i时刻和j时刻之间的机器人相对运动：
步骤2，激光里程计因子
利用当前激光帧起止时刻间的惯性测量单元IMU数据计算旋转增量，使用惯性测量单元IMU里程计数据计算平移增量，进而对当前激光帧的激光点云每一时刻的激光点进行运动畸变校正，同时用惯性测量单元IMU数据的姿态角(RPY，roll、pitch、yaw)、惯性测量单元IMU里程计数据的位姿，对当前激光帧激光位姿进行粗略初始化。对经过运动畸变校正后的当前帧激光点云，计算每个点的曲率，进而提取角点、平面点(用曲率的大小进行判定)；当机器人姿态变化超过了自定义的阈值时，选择了一个激光雷达帧Fi+1作为关键帧。然后，将保存的关键帧Fi+1与因子图中的一个新的机器人状态节点xi+1相关联。两个关键帧之间的特征全丢弃。以这种方式保持一个相对稀疏的因子图(因子图是一个在贝叶斯推理中得到广泛应用的模型，是由各种因子构建出来的，如图2所示)，在lio-sam论文中，位置和角度的阈值设定为1m和10°，即如果移动1m或者旋转10°就把当前的激光帧作为关键帧保存。
步骤3，回环因子
回环检测作为自主导航系统一个重要的部分，能够使无人车系统感知到之前到达过的位置，在实现了闭环检测的过程后，能够在因子图上添加闭环节点，进而对整个闭环上的变量节点进行全局优化，得到系统全局一致性的位姿。
当在因子图中添加一个新的状态节点时，首先搜索地图，在历史关键帧中找距离相近，时间相隔较远的帧设为匹配帧，在匹配帧周围提取局部关键帧map，同样执行scan-to-map匹配，得到位姿变换，构建闭环因子数据，加入因子图优化。
步骤4，计算模板匹配视觉里程计
将单目相机垂直地面安装在移动机器人前端，拍摄一系列包含地面纹理特征的图像序列。从图像序列中按顺序依次取两帧图像，记为第i帧图像和第i+1帧图像，在第i帧图像中心区域取一块高为Th、宽为Tw的矩形区域作为模板，并在第i+1帧图像中对所述模板进行匹配，即在第i+1帧图像中找到与所述模板相似程度最高的矩形区域，利用归一化互相关系数作为模板相似度准则。
如图1所示是模板匹配示意图，将匹配所得矩形区域的左上角坐标(u1,v1)与模板的左上角坐标(u0,v0)作差运算，分别得到模板在图像坐标系中x轴方向的像素位移增量Δu和y轴方向的像素位移增量Δv：
基于针孔相机模型，根据标定好的相机内参以及相机安装高度进行坐标转换，将模板在图像坐标系中的像素位移增量Δu和Δv转换为相机在世界坐标系中的真实位移增量，具体公式如下所示：
其中Δxc和Δyc分别为相机坐标系中相机在x方向的位移增量和y方向的位移增量，fx和fy分别为相机在x方向的归一化焦距和y方向的归一化焦距，可以通过对相机的标定得到，h为相机距离地面的高度。如图5所示，由于相机坐标系和机器人坐标系的x方向相差180度，通过如下公式将位移增量从相机坐标系转换到机器人坐标系：
利用惯性测量单元IMU提供的偏航角代替基于阿克曼转向模型计算的旋转角，作为相机从第i时刻到第i+1时刻的旋转角θi+1。
根据第i时刻相机的位置坐标
从第i时刻到第i+1时刻的位移增量
以及第i+1时刻惯性测量单元IMU解算出的相机旋转角度θi+1，利用如下公式计算出第i+1时刻机器人的位置坐标：
其中，
为第i+1时刻的旋转矩阵。
步骤5，通过激光里程计和视觉里程计联合判断保存关键帧
在相似环境中，由于帧间扫描匹配失效，激光里程计判定机器人的运动没有超过运动阈值，因此不保存关键帧，继而不更新机器人的状态节点，导致定位和建图与真实值误差较大。为了解决这个问题，本发明另外安装惯性测量单元IMU和朝下的摄像头，引入模板匹配VO判断激光里程计与真实运动状态，如果在相似环境(或空旷环境)中激光里程计失效，不保存关键帧，从而显示移动机器人没有移动。通过模板匹配VO通过运动阈值保存关键帧并将两帧关键帧之间的位移插入两帧关键帧之间，在一定程度上恢复机器人位姿，修改后的因子图、算法整体框图、流程图如图2、图3、图4所示。
步骤5具体包括：
步骤5-1，在ROS中订阅模板匹配VO的话题，将接收到的msg依次放入容器VoQueue中，最后一个放入队列中的消息总是被最先拿出来；
步骤5-2，每当新接收一个msg，都与最早进入的msg的x和y分别作差：
dt_vo-y＝VoQueue.back()·y-VoQueue.font().y
dt_vo_x＝VoQueue.back()x-VoQueue.font().x；
步骤5-3，得到机器人当前时刻的总位移ds：
步骤5-4，当ds＞1(m)(自己设定的阈值)时，清空VoQueue，并将vo_save置为0；ds<＝1时，vo_save置为1，同时计算激光里程计LO；
步骤5-5，如果激光里程计超过设定的阈值(save_frame置1)，保存当前关键帧并将LO加入两帧之间；
步骤5-6，如果激光里程计没有超过阈值(save_frame为0)，而模板匹配VO有里程(vo_save为1)，则保存关键帧，并把vo加入两帧之间。
步骤5-7，如果激光里程计没有设定的阈值则save_frame置0；
步骤5-8，如果save_frame和vo_save都为0，则判定当前机器人没有移动，不保存关键帧；
综上，加入朝下的摄像头通过模板匹配算法航迹推算，利用激光里程计和视觉里程计联合判断保存关键帧的方法，从而实现机器人在长走廊等相似场景或者空旷环境中移动时，在一定程度上克服由于激光雷达观测到的数据不变而产生的定位失效问题，恢复机器人位姿。
具体实现中，本申请提供计算机存储介质以及对应的数据处理单元，其中，该计算机存储介质能够存储计算机程序，所述计算机程序通过数据处理单元执行时可运行本发明提供的基于模板匹配与激光惯导松耦合的定位建图方法的发明内容以及各实施例中的部分或全部步骤。所述的存储介质可为磁碟、光盘、只读存储记忆体(read-only memory，ROM)或随机存储记忆体(random access memory，RAM)等。
本领域的技术人员可以清楚地了解到本发明实施例中的技术方案可借助计算机程序以及其对应的通用硬件平台的方式来实现。基于这样的理解，本发明实施例中的技术方案本质上或者说对现有技术做出贡献的部分可以以计算机程序即软件产品的形式体现出来，该计算机程序软件产品可以存储在存储介质中，包括若干指令用以使得一台包含数据处理单元的设备(可以是个人计算机，服务器，单片机。MUU或者网络设备等)执行本发明各个实施例或者实施例的某些部分所述的方法。
本发明提供了基于模板匹配与激光惯导松耦合的定位建图方法和装置，具体实现该技术方案的方法和途径很多，以上所述仅是本发明的优选实施方式，应当指出，对于本技术领域的普通技术人员来说，在不脱离本发明原理的前提下，还可以做出若干改进和润饰，这些改进和润饰也应视为本发明的保护范围。本实施例中未明确的各组成部分均可用现有技术加以实现。
Claims (10)
1.基于模板匹配与激光惯导松耦合的定位建图方法，其特征在于，包括以下步骤：
步骤1，计算惯性测量单元IMU预积分因子；
步骤2，计算激光里程计因子；
步骤3，计算回环因子；
步骤4，计算模板匹配视觉里程计；
步骤5，通过激光里程计和视觉里程计联合判断保存关键帧。
2.根据权利要求1所述的方法，其特征在于，步骤1包括：针对在不同频率下测量得到的惯性测量单元IMU和激光雷达LiDAR数据，设定i时刻和j时刻分别为选取一个关键帧的起始时刻和终止时刻，将i时刻至j时刻的传感器测量数据转换成为两个关键帧之间的单个运动约束，利用关键帧的单个运动约束简化i时刻至j时刻内惯性测量单元IMU数据的处理过程，同时以激光雷达点云帧为准选取关键帧，使激光雷达传感器和IMU传感器在数据融合时实现时间戳的对齐。
3.根据权利要求2所述的方法，其特征在于，步骤1中，通过如下公式计算惯性测量单元IMU在t时刻的原始角速度测量值
和原始加速度测量值
其中，bt表示偏置，nt表示白噪声，
是从世界帧W到机器人帧B的旋转矩阵，g是W中的恒定重力矢量；ωt为真实角速度值，
为角速度偏置，at为真实加速度值，
为加速度偏置，
为加速度白噪声，
为角速度白噪声；
通过如下公式计算在t+Δt时刻机器人的速度vt+Δt、位置pt+Δt和旋转角Rt+Δt：
其中，
Rt表示从世界帧到机器人帧的旋转矩阵，exp是以自然常数e为底的指数函数；
应用惯性测量单元IMU预积分方法来获得两个时间戳之间的机器人相对运动，通过如下公式计算在i时刻和j时刻之间的机器人相对运动：
其中Δvij为在i时刻和j时刻之间的机器人相对速度变化，
为机器人在i时刻的旋转矩阵的转置，Rj为机器人在j时刻的旋转矩阵，ΔRij为机器人在i和j时刻的相对旋转矩阵。vj和vi分别为机器人j时刻的速度和i时刻的速度，Δtij为机器人在i和j时刻的时间间隔，Δpij为机器人在i和j时刻之间的位置变化，pj和pi分别为机器人在j时刻的位置和i时刻的位置。
4.根据权利要求3所述的方法，其特征在于，步骤2包括：利用当前激光帧起止时刻间的惯性测量单元IMU数据计算旋转增量，使用惯性测量单元IMU里程计数据计算平移增量，对当前激光帧激光点云每一时刻的激光点进行运动畸变校正，同时用惯性测量单元IMU数据的姿态角、惯性测量单元IMU里程计数据的位姿，对当前激光帧激光位姿进行粗略初始化；
对经过运动畸变校正后的当前帧激光点云，计算每个点的曲率，提取角点、平面点；获取前后两帧的角点和平面点特征点云后，进行帧间点云匹配，获取相邻两帧平移旋转变换，由初始位姿进行累计，从而获得激光里程计信息，并发布出去，激光里程计信息包括(rx，ry，rz，tx，ty，tz)，rx，ry，rz，tx，ty，tz分别表示横滚角，俯仰角，航向角，x坐标，y坐标，z坐标；当机器人姿态变化超过了自定义的阈值时，选择一个激光雷达帧Fi+1作为关键帧；将保存的关键帧Fi+1与因子图中的一个新的机器人状态节点xi+1相关联，将上一个关键帧和当前时刻保存的关键帧之间的激光点云特征全丢弃。
5.根据权利要求4所述的方法，其特征在于，步骤3包括：当在因子图中添加一个新的状态节点时，首先搜索地图，在历史关键帧中以机器人当前位姿为搜索点，查找半径为X1范围内的位姿，利用时间作为约束，查找历史位姿对应时间与当前位姿对应时间的时间差大于X2的位姿的帧设为匹配帧，在匹配帧周围提取局部关键帧map，执行点云配准scan-to-map，使用PCL函数库的ICP算法求解出当前点云和目标点云之间的相对位姿：
T1＝Ticp*Tk，
其中Tk是当前位姿，Ticp是ICP算法计算出来的当前帧与回环检测帧之间的相对位姿；T1为点云配准后的目标位姿，包含roll,pitch,yaw,x,y,z，其中roll,pitch,yaw分别表示横滚角、俯仰角、航向角。
6.根据权利要求5所述的方法，其特征在于，步骤4包括：将单目相机垂直地面安装在移动机器人前端，拍摄一系列包含地面纹理特征的图像序列，从图像序列中按顺序依次取两帧图像，记为第i帧图像和第i+1帧图像，在第i帧图像中心区域取一块高为Th、宽为Tw的矩形区域作为模板，并在第i+1帧图像中对所述模板进行匹配，即在第i+1帧图像中找到与所述模板相似程度最高的矩形区域，利用归一化互相关系数作为模板相似度准则。
7.根据权利要求6所述的方法，其特征在于，步骤4中，将匹配所得矩形区域的左上角坐标(u1,v1)与模板的左上角坐标(u0,v0)作差运算，分别得到模板在图像坐标系中x轴方向的像素位移增量Δu和y轴方向的像素位移增量Δv：
将Δu和Δv转换为相机在世界坐标系中的真实位移增量，具体公式如下所示：
其中Δxc和Δyc分别为相机坐标系中相机在x方向的位移增量和y方向的位移增量，fx和fy分别为相机在x方向的归一化焦距和y方向的归一化焦距，h为相机距离地面的高度；
通过如下公式将位移增量从相机坐标系转换到机器人坐标系：
其中Δxv、Δyv分别为机器人坐标系的x方向的位移增量和y方向的位移增量；
利用惯性测量单元IMU提供的偏航角代替基于阿克曼转向模型计算的旋转角，作为相机从第i时刻到第i+1时刻的旋转角θi+1；
根据第i时刻相机的位置坐标
从第i时刻到第i+1时刻的位移增量
以及第i+1时刻惯性测量单元IMU解算出的相机旋转角度θi+1，利用如下公式计算出第i+1时刻机器人的位置坐标(xv，i+1，yv，i+1)：
其中，
为第i+1时刻的旋转矩阵。
8.根据权利要求7所述的方法，其特征在于，步骤5包括：引入模板匹配VO判断激光里程计与真实运动状态，如果在相似环境或空旷环境中激光里程计失效，不保存关键帧，从而显示移动机器人没有移动；基于模板匹配VO，通过运动阈值保存关键帧并将两帧关键帧之间的位移插入两帧关键帧之间，恢复机器人位姿。
9.根据权利要求8所述的方法，其特征在于，步骤5具体包括如下步骤：
步骤5-1，在机器人操作系统ROS中订阅模板匹配VO的话题，话题的消息为机器人移动的x方向的位移和y方向的位移；将接收到的话题信息msg依次放入队列VoQueue中，最后一个放入队列中的消息总是被最先拿出来；
步骤5-2，每当新接收一个msg，都与最早进入的msg话题的x方向位移和y方向位移分别作差：
dt_vo_y＝VoQueue.back().y-VoQueue.font().y
dt_uo_x＝VoQueue.back().x-VoQueue.font().x；
其中dt_vo_y为从开始时刻到当前时刻在y方向的位移，dt_vo_x为从开始时刻到当前时刻在x方向的位移，VoQueue.back()为提取队列VoQueue的队尾元素，VoQueue.font()为提取队列VoQueue的队首元素；
步骤5-3，得到机器人当前时刻的总位移ds：
步骤5-4，定义一个bool型变量vo_save，当ds＞1时，清空VoQueue，并将vo_save置为0；ds＜＝1时，vo_save置为1，同时计算激光里程计LO；
步骤5-5，定义一个bool型变量save_frame，如果激光里程计超过设定的阈值，save_frame置为1，保存当前关键帧并将LO加入前一帧关键帧和当前关键帧两帧之间；
步骤5-6，如果激光里程计没有超过阈值，save_frame置为0，而模板匹配VO有里程，vo_save为1，则保存关键帧，并把vo加入两帧之间；
步骤5-7，如果激光里程计没有设定的阈值则save_frame置为0；
步骤5-8，如果save_frame和vo_save都为0，则判定当前机器人没有移动，不保存关键帧。
10.基于模板匹配与激光惯导松耦合的定位建图装置，其特征在于，包括：
预积分因子计算模块，用于，计算惯性测量单元IMU预积分因子；
激光里程计因子计算模块，用于，计算激光里程计因子；
回环因子计算模块，用于，计算回环因子；
模板匹配视觉里程计计算模块，用于，计算模板匹配视觉里程计；
判定模块，用于，通过激光里程计和视觉里程计联合判断保存关键帧。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202210735304.7A CN115077519A (zh) | 2022-06-27 | 2022-06-27 | 基于模板匹配与激光惯导松耦合的定位建图方法和装置 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202210735304.7A CN115077519A (zh) | 2022-06-27 | 2022-06-27 | 基于模板匹配与激光惯导松耦合的定位建图方法和装置 |
Publications (1)
| Publication Number | Publication Date |
|---|---|
| CN115077519A true CN115077519A (zh) | 2022-09-20 |
Family
ID=83254966
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202210735304.7A Pending CN115077519A (zh) | 2022-06-27 | 2022-06-27 | 基于模板匹配与激光惯导松耦合的定位建图方法和装置 |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN115077519A (zh) |
Cited By (4)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN115265523A (zh) * | 2022-09-27 | 2022-11-01 | 泉州装备制造研究所 | 机器人同时定位与建图方法、装置及可读介质 |
| CN115326068A (zh) * | 2022-10-17 | 2022-11-11 | 北京理工大学 | 激光雷达-惯性测量单元融合里程计设计方法及系统 |
| CN118135006A (zh) * | 2024-05-07 | 2024-06-04 | 武汉理工大学 | 基于图像共视特征点的水下机器人姿态估计方法及装置 |
| WO2024197815A1 (zh) * | 2023-03-24 | 2024-10-03 | 华侨大学 | 一种工程机械的建图、方法、设备、及可读存储介质 |
-
2022
- 2022-06-27 CN CN202210735304.7A patent/CN115077519A/zh active Pending
Cited By (5)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN115265523A (zh) * | 2022-09-27 | 2022-11-01 | 泉州装备制造研究所 | 机器人同时定位与建图方法、装置及可读介质 |
| CN115265523B (zh) * | 2022-09-27 | 2023-01-03 | 泉州装备制造研究所 | 机器人同时定位与建图方法、装置及可读介质 |
| CN115326068A (zh) * | 2022-10-17 | 2022-11-11 | 北京理工大学 | 激光雷达-惯性测量单元融合里程计设计方法及系统 |
| WO2024197815A1 (zh) * | 2023-03-24 | 2024-10-03 | 华侨大学 | 一种工程机械的建图、方法、设备、及可读存储介质 |
| CN118135006A (zh) * | 2024-05-07 | 2024-06-04 | 武汉理工大学 | 基于图像共视特征点的水下机器人姿态估计方法及装置 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN112734852B (zh) | 一种机器人建图方法、装置及计算设备 | |
| CN112985416B (zh) | 激光与视觉信息融合的鲁棒定位和建图方法及系统 | |
| CN115077519A (zh) | 基于模板匹配与激光惯导松耦合的定位建图方法和装置 | |
| CN113870343B (zh) | 相对位姿标定方法、装置、计算机设备和存储介质 | |
| CN110044354A (zh) | 一种双目视觉室内定位与建图方法及装置 | |
| CN107167826B (zh) | 一种自动驾驶中基于可变网格的图像特征检测的车辆纵向定位系统及方法 | |
| CN112734841B (zh) | 一种用轮式里程计-imu和单目相机实现定位的方法 | |
| CN114526745A (zh) | 一种紧耦合激光雷达和惯性里程计的建图方法及系统 | |
| JP2023021098A (ja) | マップ構築方法、装置及び記憶媒体 | |
| CN117593650B (zh) | 基于4d毫米波雷达与sam图像分割的动点滤除视觉slam方法 | |
| CN114325634A (zh) | 一种基于激光雷达的高鲁棒性野外环境下可通行区域提取方法 | |
| CN115031718B (zh) | 一种多传感器融合的无人船同步定位与建图方法(slam)及系统 | |
| Song et al. | End-to-end learning for inter-vehicle distance and relative velocity estimation in ADAS with a monocular camera | |
| CN116310679A (zh) | 多传感器融合目标检测方法、系统、介质、设备及终端 | |
| CN116295412A (zh) | 一种基于深度相机的室内移动机器人稠密建图与自主导航一体化方法 | |
| CN116127405A (zh) | 一种融合点云地图、运动模型和局部特征的位置识别方法 | |
| CN113066129A (zh) | 基于动态环境下的目标检测的视觉定位与建图系统 | |
| Wang et al. | 3D-LIDAR based branch estimation and intersection location for autonomous vehicles | |
| CN112731503A (zh) | 一种基于前端紧耦合的位姿估计方法及系统 | |
| JP2020076714A (ja) | 位置姿勢推定装置 | |
| CN114565669A (zh) | 一种场端多相机融合定位方法 | |
| Dong et al. | PVE-LIOM: Pseudo-Visual Enhanced LiDAR-Inertial Odometry and Mapping | |
| CN115371668B (zh) | 一种基于图像识别和惯性导航的隧道无人机定位系统 | |
| CN117629241B (zh) | 一种基于连续时间模型的多相机视觉惯性里程计优化方法 | |
| CN117392347B (zh) | 一种地图构建方法、装置、计算机设备及可读存储介质 |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| PB01 | Publication | ||
| PB01 | Publication |