一种基于热红外摄像机和激光测距仪的地图构建方法

摘要

本发明涉及一种基于热红外摄像机和激光测距仪的地图构建方法，其特征在于通过获取目标的热红外图像代替现有技术获取目标的可见光图像，有效解决现有技术在光照条件差、有遮挡物存在的情况下无法正确地图构建的问题；利用方差加权信息熵做感兴趣区域提取，得到包含目标的矩形框，缩小了解空间、剔除了大量的噪声干扰；本发明欧氏距离和温度匹配对应点可靠性高，用RANSAC算法过滤对应点，最大程度提高了对应点的可靠性，基于ICP算法匹配计算速度快。

权利要求

1.一种基于热红外摄像机和激光测距仪的地图构建方法，其特征在于包括如下步骤：

步骤一、采用激光测距仪配合自由旋转电机构建三维激光测距平台，实现对三维场景的扫描；

步骤二、将步骤一搭建的三维激光测距平台和热红外摄像机安装于移动机器人上，构建能够同时获取目标的热红外图像和三维激光点点集的扫描装置；标定热红外摄像机，得到热红外摄像机内部参数和外部参数；调整热红外摄像机的位置，使热红外摄像机与三维激光测距平台激光测距仪的焦点一致且不阻挡彼此的扫描区域；通过计算转换得到装置的全局坐标系；

步骤三、利用步骤二构建的扫描装置扫描得到环境的原始三维激光点数据，再记录超声波传感器测得的移动机器人与前方障碍物的距离数据，构建目标环境的二维栅格地图；

步骤四、使用热红外摄像机获得环境的多幅热红外图像并对获得的多幅热红外图像进行初步的滤波降噪处理；按照水平方向和垂直方向分两步对图像进行透镜畸变校正；将移动机器人热红外成像映射方法扩展到三维激光测距平台，计算三维激光测距平台的扫描点与所对应的热红外图像像素点的数学转换关系；

步骤五、对热红外图像进行基于方差加权信息熵处理，实现对复杂背景下感兴趣区域的目标进行检测与提取处理，提取多幅热红外图像温度信息特征点和欧式距离特征点；

步骤六、确定匹配对应点之后，采用RANSAC算法求解基础矩阵，然后采用基础矩阵剔除匹配错误的对应点；

步骤七、将二维栅格地图与在不同位置和视角采集多幅热红外图像的温度信息特征点和欧式距离特征点进行多次ICP迭代匹配，实现对未知场景三维栅格地图的重构。

2.根据权利要求1所述基于热红外摄像机和激光测距仪的地图构建方法，其特征在于：步骤一中采用由水银接触的旋转电气连接器，三维激光测距平台可以自由旋转而不会导致电缆扭曲；三维激光测距平台能自由围绕激光扫描平面的0°~360°旋转得到三维激光数据。

3.根据权利要求1所述基于热红外摄像机和激光测距仪的地图构建方法，其特征在于：

步骤三中先设置三维激光测距平台以1毫秒为采样周期采集环境数据并进行模数转换，得到原始三维环境数据，再记录超声波传感器测得的移动机器人与前方障碍物的距离数据；将所获取的原始三维环境数据的三维激光点点集进行三角形网格化，以形成多个平面，并获得与多个平面中的每一个对应的法向矢量；合并接近的法向矢量以形成法向矢量列表并将法向矢量列表与所述环境特征矢量表中的各矢量进行匹配；将未能匹配的法向矢量表中的法向矢量添加到环境特征矢量表中以更新环境特征矢量表，利用相互匹配的矢量，构建局部二维栅格地图；

然后，利用里程计获得移动机器人左右轮的移动距离和移动速度，采用轨迹递推推获的方法获得当前机器人的位姿；计算机器人当前位姿在栅格地图中的投影比率；通过扩展卡尔曼滤波的定位算法将局部二维栅格地图通过鲁棒性的预测，每一个时间点环境细节的匹配融合处理后生成全局二维栅格地图。

说明书

一种基于热红外摄像机和激光测距仪的地图构建方法

技术领域

[0001] 本发明涉及移动机器人即时定位与地图构建领域，尤其涉及移动机器人在可见度很低及存在遮挡物的情况下的地图构建。它可用于移动机器人在未知区域由任意位置出发，根据激光测距仪和热红外摄像机的感知数据估计机器人位姿并构建环境地图。

背景技术

[0002] 随着机器人技术的发展，具有移动行走功能、环境感知能力以及轨迹规划能力的智能移动机器人越来越受到各国研究人员的重视。实现即时定位与地图构建(SimultaneousLocalization and Mapping,SLAM)是智能机器人最基本的功能之一，也是其完成许多任务的重要前提。

[0003] 目前在二维或三维的地图构建的应用中，主要采用基于尺度不变特征变换算法(Scale-invariant feature transform,SIFT)。SIFT算法是一种基于尺度空间的，对图像缩放、旋转甚至仿射变换保持不变性的特征匹配算法。该算法匹配能力较强，能提取稳定的特征，能够处理两幅图像之间发生平移、旋转、仿射变换、视角变换、光照变换情况下的匹配问题。但是，这种方法的缺点是对环境亮度和随机出现的路标具有很强的依赖性。而迭代最近点算法(Iterative Closest Point,ICP)，通过迭代优化矩阵，在每次迭代过程中，对目标点集上的每个点，在参考点集中寻找最近点，并利用这样的对应点，计算相应的旋转矩阵和平移向量，将其用于目标点集上，得到新的目标点集，然后进入下次迭代过程。最终得到最佳的转换矩阵，实现两点集的精确配准。

[0004] 地图构建中通常使用深度相机或其他传感器采集目标的可见光图像，获得目标信息。可见光图像是反射图像，高频成分多，在一定照度下能反映场景的细节，但在照度不佳时的可见光图像的对比度较低，质量很差，无法准确获得目标信息；热红外图像是辐射图像，灰度由目标与背景的温差决定，在光线较暗或有烟、云、雾等环境中，可见光图像质量较差，红外图像中的目标却依然清晰可辨。而热红外具备一定的穿透能力，即使在有遮挡物存在依然能够较清晰地得到目标图像，有利于在隐藏、伪装和遮挡的情况下更快、更精确地探测目标。

发明内容

[0005] 本发明要解决的技术问题是：提供一种基于热红外摄像机和激光测距仪的地图构建方法及装置，解决在可见度很低及存在遮挡物的情况下的地图构建问题。

[0006] 为解决上述技术问题，本发明采用的技术方案如下：

[0007] 一种基于热红外摄像机和激光测距仪的地图构建方法，其特征在于包括如下步骤：

[0008] 步骤一、采用激光测距仪配合自由旋转电机构建三维激光测距平台，实现对三维场景的扫描；

[0009] 步骤二、将步骤一搭建的三维激光测距平台和热红外摄像机安装于移动机器人上，构建能够同时获取目标的热红外图像和三维激光点点集的扫描装置；标定热红外摄像机，得到热红外摄像机内部参数和外部参数；调整热红外摄像机的位置，使热红外摄像机与激光测距仪的焦点一致且不阻挡彼此的扫描区域；通过计算转换得到装置的全局坐标系；

[0010] 步骤三、利用步骤二构建的扫描装置扫描得到环境的原始三维激光点数据，再记录超声波传感器测得的服务机器人与前方障碍物的距离数据，构建目标环境的二维栅格地图；

[0011] 步骤四、使用热红外摄像机获得环境的多幅热红外图像并对获得的多幅热红外图像进行初步的滤波降噪处理；按照水平方向和垂直方向分两步对图像进行透镜畸变校正；将移动机器人热红外成像映射方法扩展到移动测量系统，计算三维激光测距仪的扫描点与所对应的热红外图像像素点的数学转换关系；

[0012] 步骤五、对热红外图像进行基于方差加权信息熵处理，实现对复杂背景下感兴趣区域的目标进行检测与提取处理，提取多幅热红外图像温度信息特征点和欧式距离特征点；

[0013] 步骤六、确定匹配对应点之后，采用RANSAC算法求解基础矩阵，然后采用基础矩阵剔除匹配错误的对应点；

[0014] 步骤七、将二维栅格地图与在不同位置和视角采集多幅热红外图像的温度信息特征点和欧式距离特征点进行多次ICP迭代匹配，实现对未知场景三维栅格地图的重构。

[0015] 按上述技术方案，步骤一中采用由水银接触的旋转电气连接器，激光测距仪可以自由旋转而不会导致电缆扭曲；三维激光测距系统能自由围绕激光扫描平面的0°～360°旋转得到三维激光数据。

[0016] 按上述技术方案，步骤三中先设置三维激光测距仪以1毫秒为采样周期采集环境数据并进行模数转换，得到原始三维环境数据，再记录超声波传感器测得的服务机器人与前方障碍物的距离数据；将所获取的原始三维环境数据的三维激光点点集进行三角形网格化，以形成多个平面，并获得与多个平面中的每一个对应的法向矢量；合并接近的法向矢量以形成法向矢量列表并将法向矢量列表与所述环境特征矢量表中的各矢量进行匹配；将未能匹配的法向矢量表中的法向矢量添加到环境特征矢量表中以更新环境特征矢量表，利用相互匹配的矢量，构建局部二维栅格地图；

[0017] 然后，利用里程计获得移动机器人左右轮的移动距离和移动速度，采用轨迹递推推获的方法获得当前机器人的位姿；计算机器人当前位姿在栅格地图中的投影比率；通过扩展卡尔曼滤波的定位算法将局部二维栅格地图通过鲁棒性的预测，每一个时间点环境细节的匹配融合处理后生成全局二维栅格地图。

[0018] 本发明的重点在于：通过获取目标的热红外图像和三维激光点集，然后基于方差加权信息熵对复杂背景下感兴趣区域的目标进行检测与提取处理，并运用多次ICP迭代匹配，达到构建未知环境的三维地图的目的。

[0019] 相对于现有技术，本发明的有益效果为：

[0020] 本发明通过获取目标的热红外图像代替现有技术获取目标的可见光图像，有效解决现有技术在光照条件差、有遮挡物存在的情况下无法正确地图构建的问题；利用方差加权信息熵做感兴趣区域提取，得到包含目标的矩形框，缩小了解空间、剔除了大量的噪声干扰；本发明欧氏距离和温度匹配对应点可靠性高，用RANSAC算法过滤对应点，最大程度提高了对应点的可靠性，基于ICP算法匹配计算速度快。

[0021] 本发明具有很好的通用性和实用性，能够有效地推动红外图像的广泛应用，具有很好的经济效益。

附图说明

[0022] 图1是本发明一种基于热红外摄像机和激光测距仪的地图构建方法的流程图。

[0023] 图2是本发明中激光测距仪配合自由旋转电机构建三维激光测距平台的坐标系图。

[0024] 图3是本发明中三维激光测距平台与热红外摄像机构建的实验平台的坐标系图。

[0025] 图4是本发明中热红外摄像机的投影模型。

具体实施方式

[0026] 为使本发明的技术方案和优点更加清楚明白，参照附图对本发明作进一步的详细说明，并非对其保护范围的限制。

[0027] 图1给出了一个本发明系统的实施流程图。如图1所示，总的来说，本发明的方法包括如下步骤：

[0028] 步骤一、利用德国SICK公司LMS200激光测距仪配合自由旋转电机搭建三维激光扫描仪，通过激光测距仪实现对三维场景的扫描。三维激光测距系统是由内置电机的小型云台和LMS200激光测距仪构成的，倾斜安装方式，云台的内置电机带动激光。本发明采用由水银接触的旋转电气连接器，测距仪可以自由旋转而不会导致电缆扭曲。三维激光测距系统能自由围绕激光扫描平面的0D～360D旋转得到三维激光数据，扫描方式为垂直于水平面。将该激光扫描仪安装在上海英集斯公司MT-R自主移动机器人系统平台上。配合云台的自由旋转电机选用永久型重载磁同步电机，可保证三维激光测距系统较长的使用寿命，并具有稳定的操作功能，其旋转速度近似匀速运动。

[0029] 实际实施案例中由于在机械安装时，云台的旋转中心与激光束发射中心在空间上存在一定的偏差，其位置参数关系可由图2结构示意图来表示。假定二维测距仪LMS200坐标系统为ΣL，原点ο的坐标位于扫描平面中心，X′轴对应为二维测距仪扫描平面，Y′轴为垂直于扫描面（的面还是轴？），Z′轴为对应右手坐标系。坐标系统ΣS代表三维激光测距仪，原点坐标ο是位于一个特定的距离中心的旋转平台。X轴对应于正面的三维激光测距仪的正面，Y轴对应转动轴的表面，Z轴为满足右手坐标系。这个二维激光测距仪安装角度为与水平面夹角为β，云台扫描平面的旋转扫描速度为S

[0030] 齐次变换矩阵 TL与坐标系统ΣL、ΣS之间表达关系是：

[0031]

[0032]S S

[0033] 本发明中 RL和 tL分别是二维测距仪坐标系统ΣL相对于三维激光测距坐标系统ΣS的旋转矩阵和转换矩阵。

[0034] 测量点Lp相对于ΣL的表达为： r表示测量距离，θ代表扫描角度。

[0035] 测量点坐标Sp关于三维激光测距系统坐标系ΣS的数学关系为：

[0036] Sp=STLLp

[0037]S

[0038] 实际案例实施中为获得测量点 p的精确三维笛卡尔坐标，扫描数据的同步和扫描平面的旋转角度γ非常重要。

[0039] 步骤二、将热红外摄像机与三维激光测距仪系统一并安装在机器人MT-R上。为构建一个三维温度记录图，我们需要全幅热红外图像。由于传统红外相机的视角通常较小，为达到三维温度记录图的映射，相机也应该安装在机器人的旋转云台上。对热红外摄像机进行参数标定，得到其内部参数和外部参数。调整热红外摄像机的位置，使热红外摄像机与激光测距仪的焦点一致且不阻挡彼此的扫描区域。如图3三维激光测距平台与热红外摄像机S C构建的实验平台的坐标系图。将单个扫描点 p与对应热红外图像像素点 p匹配。热红外// //
摄像机的坐标系统为ΣC，ΣC坐标原点0 位于的相机焦点的位置，x轴X 为相机的光学// //
轴、z轴Z 为相机的垂直坐标，y轴Y 为满足右手坐标系的轴线。
C

[0040] 热红外像点 p与坐标系统ΣC的数学关系为：C C C

[0041] p=TLp（1）C W

[0042] 本发明中 TS是坐标系ΣS和ΣC之间的齐次变换，而 TS与ΣS坐标系和ΣW坐标W系(全局坐标系)之间位置关系是已知的，TC与ΣC坐标系和ΣW坐标系的关系是通过摄像机位置和相机旋转角计算。因此，

[0043]

[0044] 由此我们能得到全局坐标系ΣW转换到机器人坐标系ΣRS，当机器人静止时的一个扫描点也能将扫描点模型扩展到机器人移动时。

[0045] 步骤三、利用（步骤二）构建的扫描装置扫描得到环境的原始三维激光点数据，再记录超声波传感器测得的服务机器人与前方障碍物的距离数据，构建目标环境的二维栅格地图。

[0046] 1）设置三维激光测距仪以1毫秒为采样周期采集环境数据并进行模数转换，得到原始三维环境数据，再记录超声波传感器测得的服务机器人与前方障碍物的距离数据。将所获取的三维激光点点集进行三角形网格化，以形成多个平面，并获得与多个平面中的每一个对应的法向矢量。由于与确定平面垂直的法向矢量是唯一的，因此可以每一个法向矢量均代表一个平面，即与其垂直的那个平面。合并接近的法向矢量以形成法向矢量列表并将法向矢量列表与所述环境特征矢量表中的各矢量进行匹配。将未能匹配的法向矢量表中的法向矢量添加到环境特征矢量表中以更新环境特征矢量表，利用相互匹配的矢量，构建局部二维栅格地图。

[0047] 2）利用里程计获得移动机器人左右轮的移动距离和移动速度，采用轨迹递推推获的方法获得当前机器人的位姿。计算机器人当前位姿在栅格地图中的投影比率。通过扩展卡尔曼滤波的定位算法将局部二维栅格地图通过鲁棒性的预测，每一个时间点环境细节的匹配融合处理后生成全局二维栅格地图。

[0048] 步骤四、通过热红外摄像机获得扫描坐标点对应的热红外图像。如图4显示的摄像机投影模型。

[0049] 通过齐次方程 和方程

[0050] 将三维坐标点Q=(X,Y,Z)到齐次转换到一个像素点q=(x,y)。其中M是热红外摄像机的内部参数矩阵，fx和fy代表的焦距长度，cx和cy是投影中心的偏移量。方程(3)满足的条件是没有透镜畸变，故热红外摄像机的畸变校正是必需的。对从热红外摄像机透镜上获取的图像数据进行水平方向的校正，将进行水平方向的校正后的图像数据写入动态存储器。然后对动态存储器中的图像数据进行垂直方向的校正。按照水平方向和垂直方向分两步进行透镜畸变校正，快速地实现图像的透镜畸变校正。

[0051] 为了能快速的地图构建，我们将移动机器人热红外成像映射方法扩展到移动测量系统。该方法在三维测距系公式(3)基础上得到的。令ΣRS是机器人坐标系中激光测距仪RS捕获的数据,ΣRC是机器人坐标系中热红外相机捕获的图像。 TS是代表扫描器坐标相对于RS
机器人坐标齐次变换矩阵，Tc是表示相机坐标相对于机器人坐标齐次变换矩阵。因此齐次C
变换矩阵 TS(代表ΣS关于ΣC的)可分解为：

[0052] CTS=CTRCRCTWWTRSRSTS

[0053] 初步得到具体实施时移动机器人运动时，三维激光测距仪的扫描点所对应的热红外图像像素点的数学转换关系。

[0054] 步骤五、对热红外图像进行基于方差加权信息熵处理，对复杂背景下基于感兴趣区域的目标检测与提取。其中基于方差加权信息熵的感兴趣区域提取步骤为：

[0055] 1)将M*N大小的初始帧图像分割成m*n大小的子图像块，得到子图像块集合K IF=(u,v)，其中：m=2，n=2，1≤K≤4,1≤I≤4，0≤u≤M/m-1，0≤v≤N/n-1；

[0056] 2)对子图像块集合F=(u,v)中的每幅图像F=(i,j)，采用公式：

[0057]

[0058] 计算其方差加权信息熵H=(i,j)，得到M/(m-1)*N/(n-1)大小的熵图像，并计算信息熵图像的均值μ和方差σ；其中：0≤i≤M/m-1,0≤v≤N/n-1，s表示灰度值，ps表示每种灰度级对应的概率，表示红外图像的灰度平均值，当ps=0时，令ps*log(ps)=0；

[0059] 3)当步骤2)得到的熵图像中最大的熵值H(t,r)≥HT时，H(t,r)对应的子图像F(t,r)为种子；当H(t,r)

[0060] 4)在熵图像上，以步骤(3)得到的因子，采用基于八邻域的区域增长方法进行增长，得到包含目标的感兴趣区域ROI（centerx，centery，w，h）作为复杂背景下红外弱小目标检测的结果，其中：centerx为区域中心横坐标，centery为区域中心纵坐标，w为区域的宽度，为h区域高度，均为整数；所述基于八邻域的区域增长方法中的相似度α∈(0,1)。

[0061] 步骤六、提取多幅热红外图像温度信息特征点和欧式距离特征点。匹配点选好后，对匹配点用RANSAC算法剔除错误匹配点，提高匹配效率。RANSAC(random sample consensus)算法是一种估计数学模型参数的迭代算法，主要思路是通过采样和验证的策略，求解大部分样本特征点都能满足的数学模型的参数。本发明运用RANSAC算法的具体步骤如下：

[0062] 1)随机抽取4对热红外图像温度信息特征点和欧式距离特征点作为特征匹配点（此句有点歧义，需要澄清并修改），作为初始的内点集合，通过该内点集合计算出变换矩阵H。

[0063] 2)依次判断内点集合外的点，计算X与HX′之间的距离，如果小于设定的I值，则将“i”前的点依次加入到内点集合。

[0064] 3)重复(1)和(2)N次，选取内点个数最多的那一组作为合格的匹配点集。根据新的内点集合，运用最小二乘法更新变换矩阵H。

[0065] 假设正确匹配的点占总数的比例为p，则随机抽取的4对一匹配点不全是正确匹4
配的几率为1-p，抽取N次都抽不到4对全是正确匹配点的概率为(1-p)，在实践中，只要
4
能够保证(1-p)<0.05就可以满足实际应用的需要。

[0066] 步骤七、将二维栅格地图与在不同位置和视角采集多幅热红外图像的温度信息特征点和欧式距离特征点进行多次ICP匹配，实现对场景三维栅格地图的构建。本发明中的ICP匹配不仅使用欧氏距离而且根据温差寻找对应点。评价函数该算法的使用不仅最近迭代点的平方和而且还考虑对应点热值平方差的。在每个迭代步骤中，ICP算法选择最接近的对应点并计算其旋转矩阵R和转化矩阵t最小化ICP算法方程：

[0067]

[0068] 其中Nm和Nd分别是M处参考点数量和D处匹配点数量，是K温度的权重；hmi是点mi处温度值，hdj是点dj处温度值。当 时， 函数值最小化。ICP迭代一直进行，直至ET(R,t)收敛，此时到达迭代最佳值。最终得到环境三维栅格地图。

[0069] 本实施案例基于热红外摄像机和激光测距仪的地图构建方法与国内外现有技术相比的优势在于：首先对感兴趣区域ROI进行提取，能有效地缩小地图匹配区域，减少数据计算量；其次对匹配点用RANSAC算法剔除错误匹配点，减少数据分析数量，提高地图匹配效率，缩短匹配时间；再次通过构建环境的三维栅格地图，能够更好地体现环境信息，有助于三维地图在实际工作中的应用。

[0070] 本实施案例基于热红外摄像机和激光测距仪的地图构建方法，实现在光照强度低及目标环境存在遮挡物存在的情况下，构建目标环境三维地图，提高激光机器人在未知环境下对环境的探测能力和工作效率。本发明同样能够应用于野外探测、地震搜救等工作环境，具有较好的经济和社会效益。

[0071] 以上所述的具体实施例，对本发明的目的、技术方案和有益效果进行了进一步详细说明，应理解的是，以上所述仅为本发明的具体实施例而已，并不用于限制本发明。凡在本发明的精神和原则之内，所做的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。