带障碍物记忆功能的多激光雷达数据融合方法与流程

本发明涉及激光雷达技术领域，具体涉及一种带障碍物记忆功能的多激光雷达数据融合方法。

背景技术：

激光作为当前机器人导航重要器件，相当于机器人的眼睛，在机器人避障时，激光数据的实时性和准确性决定了机器人能否安全避障。由于结构遮挡以及机器人安装高度等问题，现有激光雷达设备性能不佳、单一激光覆盖范围有限，因此，需要多激光雷达协调作业。

多激光雷达协调作业最重要的问题是多激光雷达数据的融合。激光雷达高速扫描，以10hz、2000点数据激光为例，激光每0.1秒需要处理2000个数据点，每个数据点都有特定的角度、时间，且点和点的时间差仅为1/20000s，在如此短的时间内，不同的设备很难做到时间同步，且不同设备性能不一，这就会导致当机器人或者障碍物运动时，不同激光探测到的障碍物信息不同，如果直接对不同激光数据进行融合，会产生大量噪点，且障碍物相对雷达运动速度越快，误差越大。

现有技术中存在两种数据融合方法：一是自适应消息同步方法：这种方法相当于时间同步器，从多个激光接收不同的消息后，当且仅当多个激光雷达具有相同时间戳的消息时才输出各自的点云数据。因此会由于强行等待时间同步而导致消息发布频率下降。二是直接融合方法：这种方法直接对多个激光雷达数据进行融合，缺点明显，不仅会因为时间差导致同步错误，出现大量噪点，而且也没有考虑障碍记忆功能。

技术实现要素：

本发明针对以上问题提出带障碍物记忆功能的多激光雷达数据融合方法，首创性提出了障碍物生命周期的概念来记忆发现的障碍物，不仅能有效解决运动障碍噪点问题，对于避障后期低矮障碍不可见的现象也能有效解决。

本发明提供一种带障碍物记忆功能的多激光雷达数据融合方法，具体包括如下步骤：

步骤s1：设定主激光雷达和辅激光雷达；

步骤s2：若障碍物对于主激光雷达不可见而辅激光雷达可见时，将辅激光雷达测量的障碍物的点云数据坐标(x′，y′)转化为全局地图坐标(x，y)；

步骤s3：判断全局数据链表中是否存在该点云数据坐标(x′，y′)对应的全局地图坐标(x，y)？若否，则保存至全局数据链表，并为该点创造生命周期t；若是，则将该点云数据坐标(x′，y′)对应的全局地图坐标(x，y)权重加一；

步骤s4：辅激光雷达每循环扫描一周，该点云数据坐标(x′，y′)对应的全局地图坐标(x，y)权重减少一定量μ，其生命周期t对应减少一定量v；

步骤s5：检测全局数据链表中存储的点云数据坐标(x′，y′)对应的全局地图坐标(x，y)的权重、生命周期t以及该点距机器人的距离，当权重＜2，仅保存该点数据并不转化为融合激光数据；当生命周期t＜0，删除该点；当该点到机器人的距离大于阈值时，删除该点；其他情况，转化为融合激光数据。

在本发明提供的带障碍物记忆功能的多激光雷达数据融合方法的一种较佳实施例中，步骤s2所述点云数据坐标(x′，y′)转化为全局地图坐标(x，y)的公式为：

其中，α为辅激光雷达点云数据坐标系相对全局地图坐标系的夹角，(x0，y0)辅激光雷达点云数据坐标系原点相对全局地图坐标系的初始位置坐标。

在本发明提供的带障碍物记忆功能的多激光雷达数据融合方法的一种较佳实施例中，步骤s3中，创造的生命周期t具体为：

t＝a*w*时间常数+b*l/v；

其中l为当前障碍物距离机器人的距离，v为机器人避障速度，a、b为权重系数，a+b＝1，w为该点当前权重。

在本发明提供的带障碍物记忆功能的多激光雷达数据融合方法的一种较佳实施例中，所述数据融合方法还适用于多个超声波、多个红外测距传感器、多个毫米波雷达他们中的一种或多种的任意组合。

相较于现有技术，本发明提供的多激光雷达数据融合方法带有障碍物记忆功能，能实时有效的融合多个激光雷达数据，同时能有效过滤噪点。解决了自适应消息同步方法强行等待时间同步而导致消息发布频率下降的问题，同时解决了直接数据融合方法出现大量噪点的问题。另外，该方法同样适用于其它类似原理的距离传感器，如超声、红外测距传感器、毫米波雷达等或者其任意组合。

附图说明

为了更清楚地说明本发明实施例中的技术方案，下面将对实施例描述中所使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其它的附图，其中：

图1是本发明提供的带障碍物记忆功能的多激光雷达数据融合方法的工作流程图；

图2是具体实施例1不同水平面多激光雷达信号区域示意图；

图3是同水平面多激光雷达信号区域示意图；

图4是笛卡尔坐标变换图；

图5(a)是普通数据融合方法避障效果图；

图5(b)是本发明提供的带障碍物记忆功能的多激光雷达数据融合方法避障效果图。

具体实施方式

下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅是本发明的一部分实施例，而不是全部的实施例。

请参阅图1，是本发明提供的带障碍物记忆功能的多激光雷达数据融合方法工作流程示意图。所述带障碍物记忆功能的多激光雷达数据融合方法，具体包括如下步骤：

步骤s1：设定主激光雷达和辅激光雷达。设定主、辅激光雷达便于机器人导航参照，通常情况下机器人参照主激光雷达的导航数据进行运行导航避障。

步骤s2：若障碍物对于主激光雷达不可见而辅激光雷达可见时，将辅激光雷达测量的障碍物的点云数据坐标(x′，y′)转化为全局地图坐标(x，y)。全局地图为机器人导航避障的稳定不变的参考系，将点云数据坐标转化为全局地图坐标有利于直观体现障碍物在实体环境中的位置。

步骤s3：判断全局数据链表中是否存在该点云数据坐标(x′，y′)对应的全局地图坐标(x，y)？若否，则保存至全局数据链表，并为该点创造生命周期t；若是，则将该点云数据坐标(x′，y′)对应的全局地图坐标(x，y)权重w加一。点云数据坐标(x′，y′)对应的全局地图坐标(x，y)权重w初始值为0。生命周期t＝a*w*时间常数+b*l/v；其中l为障碍物距离机器人的距离，v为机器人避障速度，a、b为权重系数，a+b＝1，w为该点当前权重值。所述时间常数根据机器人避障系数确定。

步骤s4：辅激光雷达每循环扫描一周，该点云数据坐标(x′，y′)对应的全局地图坐标(x，y)权重w减少一定量μ，其生命周期t对应减少一定量v。具体实施方式中，μ根据实际情况进行确定，一般情况下取0.1左右，具体根据障碍物及路况的复杂程度确定，路况越复杂，减少量越小，以便后续判定过程中尽可能的不误删障碍物，但是过小的情况容易出现较多噪点。即：

t(n+1)＝t(n)-μ；w(n+1)＝w(n)-v。其中，n为辅激光雷达旋转周次。

步骤s5：检测全局数据链表中存储的点云数据坐标(x′，y′)对应的全局地图坐标(x，y)的权重、生命周期t以及该点距机器人的距离，当权重＜2，仅保存该点数据并不转化为融合激光数据；当生命周期t＜0，删除该点；当该点到机器人的距离大于阈值时，删除该点。具体实施过程中，阈值一般取稍大于机器人最大横截面半径为宜。

具体的，步骤s2所述点云数据坐标(x′，y′)转化为全局地图坐标(x，y)的转化公式为：

其中，α为辅激光雷达点云数据坐标系相对全局地图坐标系的夹角，(x0，y0)辅激光雷达点云数据坐标系原点相对全局地图坐标系的初始位置坐标。

具体实施例中，所述数据融合方法还适用于多个超声波、多个红外测距传感器、多个毫米波雷达他们中的一种或多种的任意组合。

具体实施例中，在机器人设置有多个激光雷达的情况下，主、辅激光雷达的选择是非常有必要的，以便机器人巡航参考，原则上对主、辅激光雷达的选择是没有要求的，但是一般选择视野开阔的激光雷达作为主激光雷达的避障效果更加，例如若激光雷达设置在不同水平面，则选择位置相对较高的视野相对开阔的激光雷达作为主激光雷达，其余作为辅激光雷达。

具体实施例1

如图2所示，激光雷达a、b位于不同水平面上，对于高度大于激光雷达a的障碍物，激光雷达a、b同时可见；对于低于激光雷达a且大于激光雷达b的物体，只有激光雷达b可见，激光雷达a不可见；对于低于激光雷达b高度的障碍物，激光雷达b距离障碍物足够远时可见，但当机器人和障碍物之间的距离不断缩短至超过距离阈值时障碍物不可见。

机器人周围环境可分为三个区域：单个激光单独可见区域、激光公共可见区域和所有激光都不可见区域，其投影在水平面的信号分布如图3所示。

对于单个激光单独可见区域，不需要考虑数据融合问题，直接以单激光数据为准，此区域点云数据记为pk(alone)；点云数据集为

对于所有激光雷达都不可见区域，此区域点云数据集为p(invisible)＝0；

对于激光雷达公共可见区域，此区域点云数据集为p(common)：

理想情况下，公共区域因为障碍物高度，距离不同，如图3所示，可以分为3种区域，

(1)激光雷达a可见，激光雷达b不可见，如障碍物较远或为非地面突出障碍的情况。

(2)激光雷达b可见，激光雷达a不可见，如障碍物低于激光雷达a高度且高于激光雷达b的高度的情况；

(3)激光雷达a，b都可见；如障碍物高于激光雷达a的情况；

对于(1)(3)两种情况，此区域点云数据直接以激光雷达a的点云数据集p(a)为准：

对于情况(2)，此区域点云数据直接以雷达b的点云数据集p(b)为准：

由此，p(common)＝p(a)+p(b)

最终，理想情况下融合后激光雷达的点云数据集为：

p＝p(alone)+p(invisible)+p(common)。

现实情况下，如果没有障碍物记忆功能，则将存在不安全因素，例如假设机器人处于静止状态，障碍物处于运动状态，当障碍物在d点时，激光雷达b可见，激光雷达a不可见，由d运动到c后，障碍物对于激光雷达a和b都不可见，而此时障碍物并没有消失。因此，为能保证所有情况下避障的安全性，本发明数据融合引入障碍物记忆功能。

具体方法如下：

步骤一、设定主激光雷达和辅激光雷达。激光雷达a视野开阔，所以作为主激光雷达，激光雷达b作为辅激光雷达。

步骤二、若障碍物对于主激光雷达a不可见，辅激光雷达b可见，则将辅激光雷达b对于障碍的点云数据(x’，y’)转化为全局地图坐标(x，y)，具体转化公式为：

其中，α为辅激光雷达b点云数据坐标系相对全局地图坐标系的夹角，(x0，y0)辅激光雷达b点云数据坐标系原点相对全局地图坐标系的初始位置坐标。如图4所示。x轴、y轴组成的坐标系为全局地图坐标系，x’轴、y’轴组成的坐标系为辅激光雷达b点云数据坐标系。

通过以上公式可将辅激光雷达b测得障碍物的点云数据坐标(x’，y’)转化为全局地图坐标(x，y)；同理可解出x′，y′便得逆变换公式如下：

步骤三、判断全局数据链表中是否存在该点云数据坐标(x′，y′)对应的全局地图坐标(x，y)？若否，则保存至全局数据链表，并为该点创造生命周期t；若是，则将该点云数据坐标(x′，y′)对应的全局地图坐标(x，y)权重w加一；

具体的生命周期t＝a*w*time+b*l/v；

其中l为障碍物距离，v为机器人避障速度，a、b为权重系数，a+b＝1，w为该点当前权重值；

步骤四、辅激光雷达b每循环一次，该点云数据坐标(x′，y′)对应的全局地图坐标(x，y)生命周期t会减少μ，权重w对应减少v。

t(n+1)＝t(n)-μ；

w(n+1)＝w(n)-v；

步骤五、检测全局数据链表中存储的所有点云数据坐标(x′，y′)对应的全局地图坐标(x，y)的权重w、生命周期t以及该点距机器人的距离，当权重＜2，仅保存该点数据并不转化为融合激光数据；当生命周期t＜0，删除该点；当该点到机器人的距离大于阈值时，删除该点。

本专利增加了障碍物点云数据权重w及生命周期t的概念，且步骤五避免了引入障碍物记忆的可能导致的另一个问题，即由于机器人及障碍物都可能处于运动状态，如图3中的障碍物从p点运动到q点的情况，障碍物本来在激光公共可见区域，但此时由于激光扫描时间不同步，就变成了激光单独可见区域(p点主激光雷达a可见，辅激光雷达b不可见；q点主激光雷达a不可见，辅激光雷达b可见)，此时q点为噪点，如果将q点记忆并加入全局数据链表中的话，后期再转化为激光数据就会在运动轨迹上出现一路的噪点，且噪点会一直存在，当噪点完全挡住机器人时，就会导致机器人避障失败。

对于图3中，q点的情况，通过步骤五解决，可以通过设置合理的该点到机器人的距离阈值来解决，一般以机器人最大横切面半径为准，当q点的判断范围内主激光雷达a都没有探测到任何障碍，则将其加入全局数据链表；另外，此判断步骤可使噪点出现次数少，权重低，生命周期也短，即使误判断了也会很快消失，不会影响机器人避障。

对于图2中，障碍物由d运动到c的情况，机器人接近过程中，该测得点云数据出现的次数非常多，权重w大，生命周期t也长，该生命周期t完全满足避障时间需求。如图5(b)为采用本专利方法，噪点生命周期短，短暂出现后会立即消失。而如图5(a)为普通方法融合，若记忆障碍物，行人走过时会产生噪点，且永远不会消失。

相较于现有技术，本发明提供的所述带障碍物记忆功能的多激光雷达数据融合方法带有障碍物记忆功能，能实时有效的融合多激光雷达数据，同时能有效过滤噪点。且该方法同样适用于其它类似原理的距离传感器，如超声波，红外测距传感器，毫米波雷达等。