一种基于超声波雷达与激光雷达的建图方法、装置及系统

本发明涉及扫地机器人自主导航建图及避障技术领域，尤其是涉及一种基于超声波雷达与激光雷达的建图方法、装置及系统。

背景技术：

随着现代化建设的加速发展，超级市场，大型码头，国际机场，各种会展中心，大型物流仓库，高级酒店，大型休闲公园，校园、医院等场所的数量和规模不断增加，各种大型生活小区和活动场所日益增多，人类活动和物流运输等变得更为复杂。需要大量的人力物力来确保清洁卫生，工业扫地机器人应运而生。

目前市场上工业扫地机器人主要还是由人来驾驶，依然需要投入人力，而无人驾驶扫地车很好的地解决了这一点。无人驾驶扫地车是将环境感知、动态决策、路径规划以及行为控制集中于一体的多功能复合系统，他利用车载传感器来感知车辆周围环境，并根据感知所获得的环境地图规划巡逻的路径，在清扫过程中通过感知存在的障碍物来控制车辆的转向和速度，从而使车辆能够安全可靠的在规划路径上行驶。无人驾驶扫地车集自动控制、体系结构、人工智能、视觉技术等众多技术于一身，是计算机科学模式识别和智能控制技术高度发展的产物，在工厂、仓库、写字楼等公共区域有着广阔的前景。

无人驾驶扫地车的建图避障主要依靠激光雷达，少数基于视觉特征点，而其工作环境则主要分布在工厂、机场、写字楼等大量存在玻璃落地窗或者塑料防护罩这样透光物品的场所。无论是哪种方法，在面对玻璃、塑料等透光物品或是镜面物体时，建图效果得不到保障，为下一步的导航避障埋下隐患。

技术实现要素：

有鉴于此，有必要提供一种基于超声波雷达与激光雷达的建图方法、装置及系统，用以解决现有的建图避障方法在面对玻璃、塑料等透明物体或镜面物体时建图效果不佳的技术问题。

为了实现上述目的，第一方面，本发明提供了一种基于超声波雷达与激光雷达的建图方法，包括：

通过激光雷达获取移动小车周围的障碍物点云信息；

通过超声波雷达获取移动小车周围的障碍物距离信息；

根据所述障碍物点云信息及所述障碍物距离信息获取移动小车周围的障碍物融合点云信息；

通过深度相机获取移动小车的定位信息；

根据所述障碍物融合点云信息及所述定位信息对移动小车周围的障碍物进行建图。

优选地，根据所述障碍物点云信息及所述障碍物距离信息获取移动小车周围的障碍物融合点云信息，具体为：分别将超声波雷达获取的各个方向的障碍物距离信息进行位置变换，得到各个方向的障碍物与激光雷达之间的转换距离值；分别判断超声波雷达获取的各个方向的障碍物是否为透明障碍物，若是，则对该方向预设角度范围内的角度方向赋予所述转换距离值，以得到透明障碍物点云信息；将所述透明障碍物点云信息与所述障碍物点云信息进行融合，得到移动小车周围的障碍物融合点云信息。

优选地，分别将超声波雷达获取的各个方向的障碍物距离信息进行位置变换，得到各个方向的障碍物与激光雷达之间的转换距离值，具体为：分别将超声波雷达获取的各个方向的障碍物距离值与对应的超声波雷达与激光雷达之间的距离值相加，得到各个方向的障碍物与激光雷达之间的转换距离值。

优选地，分别判断超声波雷达获取的各个方向的障碍物是否为透明障碍物，具体方法为：将超声波雷达获取的各个方向的障碍物的转换距离值与激光雷达获取的对应方向的障碍物距离值进行对比，若二者的差值大于预设阈值且保持多次，则超声波雷达获取的该方向的障碍物为透明障碍物。

优选地，将所述透明障碍物点云信息与所述障碍物点云信息进行融合，得到移动小车周围的障碍物融合点云信息，具体为：将所述透明障碍物点云信息与所述障碍物点云信息分别发布相同数据类型的激光雷达话题，以得到移动小车周围的障碍物融合点云信息。

优选地，通过深度相机获取移动小车的定位信息，具体为：通过移动小车上安装的深度相机获取移动小车周围的图像；通过获取到的图像得到移动小车的定位信息。

优选地，通过获取到的图像得到移动小车的定位信息，具体包括：捕捉连续两帧图像i^t及i^t+1；对所述两帧图像i^t及i^t+1进行畸变校准；计算畸变校准后时刻t和时刻t+1的视差图，以获取深度图像；通过特征点检测算法检测畸变校准后的图像i^t中的特征点，并与畸变校准后的图像i^t+1中的特征匹配；根据所述深度图像获取所述特征点的3d坐标值；根据特征点的3d坐标值得到移动小车的定位信息。

优选地，根据所述障碍物融合点云信息及所述定位信息对移动小车周围的障碍物进行建图，具体为：采用gmapping实时建图算法，根据障碍物融合点云信息及所述定位信息创建移动小车周围障碍物的二维占用栅格图。

第二方面，本发明还提供了一种基于超声波雷达与激光雷达的建图装置，包括中央信息处理器及存储器，所述存储器上存储有计算机程序，所述计算机程序被所述中央信息处理器执行时，实现所述基于超声波雷达与激光雷达的建图方法。

第三方面，本发明还提供了一种基于超声波雷达与激光雷达的建图系统，包括所述基于超声波雷达与激光雷达的建图装置，还包括移动小车、激光雷达、若干个超声波雷达及深度相机；所述激光雷达、各个所述超声波雷达及所述深度相机均安装于所述移动小车上，所述激光雷达、各个所述超声波雷达及所述深度相机均与所述基于超声波雷达与激光雷达的建图装置电连接；所述激光雷达用于采集移动小车周围的障碍物点云信息，并将所述障碍物点云信息发送至基于超声波雷达与激光雷达的建图装置；所述超声波雷达用于采集移动小车周围的障碍物距离信息，并将所述障碍物距离信息发送至基于超声波雷达与激光雷达的建图装置；所述深度相机用于采集移动小车的定位信息，并将所述定位信息发送至基于超声波雷达与激光雷达的建图装置。

与现有技术相比，本发明提出的技术方案的有益效果是：结合激光雷达和超声波雷达的特点生成完整的障碍物点云地图，通过对激光雷达和超声波雷达数据信息进行融合,解决了现有的建图避障方法在面对玻璃、塑料等透明物体或镜面物体时建图效果不佳的技术问题。

附图说明

图1是本发明提供的基于超声波雷达与激光雷达的建图方法的一实施例的流程示意图；

图2是图1中步骤s3的流程示意图；

图3是图1中步骤s4的流程示意图；

图4是图3中步骤s42的流程示意图；

图5是本发明提供的基于超声波雷达与激光雷达的建图系统的一实施例的结构示意图；

图6是图5中的基于超声波雷达与激光雷达的建图系统的安装方式示意图；

图中：1-中央信息处理器、2-存储器、3-移动小车、4-激光雷达、5-超声波雷达、6-深度相机、7-底层控制模块、8-移动驱动模块、9-手机app。

具体实施方式

下面结合附图来具体描述本发明的优选实施例，其中，附图构成本申请一部分，并与本发明的实施例一起用于阐释本发明的原理，并非用于限定本发明的范围。

请参照图1，图1为本发明提供的基于超声波雷达与激光雷达的建图方法的一实施例的流程示意图，所述基于超声波雷达与激光雷达的建图方法包括：

s1、通过激光雷达获取移动小车周围的障碍物点云信息；激光雷达每扫射一周便可发布几百个位置信息，所以它是建图避障时最主要用到的仪器，但其面对透明障碍物时会出现直接透过或者信息丢失的问题。

s2、通过超声波雷达获取移动小车周围的障碍物距离信息；超声波雷达与激光雷达不同，其可以克服激光雷达面对透明障碍物时出现的直接透过或者信息丢失的缺陷，但普通的超声波雷达一次只能接收一个位置信息，所以直接使用多组超声波雷达替代激光雷达不太现实，因此，需要将激光雷达信息与超声波雷达进行融合，本实施例中，通过四个超声波雷达分别获取移动小车前方、后方、左侧方及右侧方的障碍物距离信息。

s3、根据所述障碍物点云信息及所述障碍物距离信息获取移动小车周围的障碍物融合点云信息；

s4、通过深度相机获取移动小车的定位信息。

s5、根据所述障碍物融合点云信息及所述定位信息对移动小车周围的障碍物进行建图，具体为：采用gmapping实时建图算法，根据障碍物融合点云信息及所述定位信息创建移动小车周围障碍物的二维占用栅格图。

本发明结合激光雷达和超声波雷达的特点生成完整的障碍物点云地图，通过对激光雷达和超声波雷达数据信息进行融合,解决了现有的建图避障方法在面对玻璃、塑料等透明物体或镜面物体时建图效果不佳的技术问题。

为了具体实现激光雷达信息与超声波雷达信息的融合，请参照图2，在一优选的实施例中，所述步骤s3包括：

s31、分别将超声波雷达获取的各个方向的障碍物距离信息进行位置变换，得到各个方向的障碍物与激光雷达之间的转换距离值；具体为：分别将超声波雷达获取的各个方向的障碍物距离值与对应的超声波雷达与激光雷达之间的距离值相加，得到各个方向的障碍物与激光雷达之间的转换距离值。

s32、分别判断超声波雷达获取的各个方向的障碍物是否为透明障碍物，具体方法是：

将超声波雷达获取的各个方向的障碍物的转换距离值与激光雷达获取的对应方向的障碍物距离值进行对比，若二者的差值大于预设阈值且保持多次，或在该方向激光雷达无信号且保持多次，则超声波雷达获取的该方向的障碍物为透明障碍物。其中，若二者的差值大于预设阈值且保持多次，则表明激光雷达探测到的障碍物与超声波雷达探测到的障碍物不同，说明激光雷达直接穿透了超声波雷达所探测到的障碍物，因此超声波雷达探测到的障碍物为透明障碍物；若在该方向激光雷达无信号且保持多次，则表明激光雷达在该方向上的最大探测范围内未探测到障碍物，表明超声波雷达探测到的障碍物为透明障碍物。

具体来说，由于激光雷达数据点均分在一周360度中，假设发布360个激光点，顺时针均分，那么每一度对应一个数据点，以x轴为正方向，左侧(y轴)为90度，则设返回的数据点记为数据点90，正前方为数据点180，以此类推。左侧方的超声波雷达产生的数据和激光雷达数据点90(y轴方向)是描述同一个方向的数据，也就是说左方的超声波信号是对比的第90位数据，如果对比结果超出阈值或无激光雷达数据，则该方向第90位数据采用超声波数据，若连续一定次数该方向都使用超声波信号，则判断其为透明障碍物。

若判断超声波雷达获取的某个方向的障碍物是透明障碍物，则对该方向预设角度范围内的角度方向赋予所述转换距离值，以得到透明障碍物点云信息；

具体来说，由于超声波雷达只有来自前后两侧的四个距离信息，只能对四个角度方向赋值，形成四个单独的点，但是这么少的位置信息融合在真正的激光雷达的信息里，可能不会被算法判断为障碍(或者障碍太小)，所以此处将每个点发散成五个或者更多，分别赋给第88位到92位(对应左侧方超声波探测器)、第178位到182位(对应后方超声波探测器)、第268位到272位(对应右侧方超声波探测器)和第0位到4位(对应前侧方超声波探测器)这20个点，刚好环绕在小车的前后和左右两侧，在遇到障碍物时，最多能形成四团小点云，这四团点云会在建图和避障的过程中被视为障碍。

s33、将所述透明障碍物点云信息与所述障碍物点云信息进行融合，得到移动小车周围的障碍物融合点云信息，具体为：将所述透明障碍物点云信息与所述障碍物点云信息分别发布相同数据类型的激光雷达话题，以得到移动小车周围的障碍物融合点云信息。

请参照图3，深度相机作为视觉里程计最主要就是从几个相邻帧图像中，估计相机的运动，从而确定移动小车的定位信息，具体来说，所述步骤s4包括：

s41、通过移动小车上安装的深度相机获取移动小车周围的图像；

s42、通过获取到的图像得到移动小车的定位信息。

请参照图4，步骤s42具体包括：

s421、捕捉连续两帧图像i^t及i^t+1；

s422、对所述两帧图像i^t及i^t+1进行畸变校准，在计算深度图像之前，为了补偿镜头带来的畸变，通过相机标定以及校准等过程，可以得到去畸变的图像对，图像对通过校准后，同一目标在左右图像中的水平位置是对齐的；

s423、计算畸变校准后时刻t和时刻t+1的视差图，以获取深度图像；

s424、通过特征点检测算法(fast算法)检测畸变校准后的图像i^t中的特征点，并与畸变校准后的图像i^t+1中的特征匹配；

s425、根据所述深度图像获取所述特征点的3d坐标值，此时，获得两组点云数据；

s426、根据特征点的3d坐标值得到移动小车的定位信息，选取点云数据中的一部分子集，使得所有的匹配都是互相兼容的，两组点的3d坐标均已知，从而可以估计相机的运动，从而确定移动小车的定位信息。

请参照图5，本发明还提供了一种基于超声波雷达与激光雷达的建图装置，包括中央信息处理器1及存储器2，所述存储器2上存储有计算机程序，所述计算机程序被所述中央信息处理器1执行时，实现所述基于超声波雷达与激光雷达的建图方法。

请参照图5和图6，本发明还提供了一种基于超声波雷达与激光雷达的建图系统，包括所述基于超声波雷达与激光雷达的建图装置，还包括移动小车3、激光雷达4、若干个超声波雷达5及深度相机6；所述激光雷达4、各个所述超声波雷达5及所述深度相机6均安装于所述移动小车3上，所述激光雷达4、各个所述超声波雷达5及所述深度相机6均与所述基于超声波雷达与激光雷达的建图装置电连接；所述激光雷达4用于采集移动小车周围的障碍物点云信息，并将所述障碍物点云信息发送至基于超声波雷达与激光雷达的建图装置；所述超声波雷达5用于采集移动小车3周围的障碍物距离信息，并将所述障碍物距离信息发送至基于超声波雷达与激光雷达的建图装置；所述深度相机6用于采集移动小车3的定位信息，并将所述定位信息发送至基于超声波雷达与激光雷达的建图装置。

请参照图5，在一优选的实施例中，所述基于超声波雷达与激光雷达的建图系统还包括底层控制模块7及移动驱动模块8，底层控制模块7控制移动驱动模块8从而驱动移动小车3移动，本实施例中，通过手机app9与底层控制模块7进行蓝牙通信，从而实现通过手机app9控制移动小车3的移动，以使移动小车3的位置发生改变从而便于采用gmapping算法实时建图。

本发明的有益效果如下：

(1)通过对激光雷达和超声波雷达数据信息进行融合,解决了现有的建图避障方法在面对玻璃、塑料等透明物体或镜面物体时建图效果不佳的技术问题；

(2)通过将超声波雷达获取的障碍物距离值与超声波雷达与激光雷达之间的距离值相加，得到障碍物与激光雷达之间的转换距离值，从而可使超声波雷达获取的信息与激光雷达获取的信息统一，以便于对激光雷达和超声波雷达数据信息进行融合；

(3)通过将超声波雷达获取的信息与激光雷达获取的信息进行对比，判断超声波雷达探测到的障碍物是否为透明障碍物，若是，则采用激光雷达获取的信息，否则，则采用超声波雷达获取的信息，通过这种处理，可降低数据处理量，提高了计算效率；

(4)通过将超声波雷达获取的信息进行放大，克服了因为普通的超声波雷达一次只能接收一个位置信息而造成的超声波雷达信息与激光雷达信息融合时，超声波雷达信息易被算法忽略的技术问题；

(5)通过gmapping实时建图算法，以障碍物融合点云信息及深度相机获取的移动小车的定位信息作为输入项，可对移动小车周围的障碍物进行实时建图。

以上所述仅为本发明较佳的具体实施方式，但本发明的保护范围并不局限于此，任何熟悉本技术领域的技术人员在本发明揭露的技术范围内，可轻易想到的变化或替换，都应涵盖在本发明的保护范围之内。