霍夫变换找墙体直线沿墙导航算法，在清洁机器人中，怎样提高效率

沿墙导航行为是智能移动机器人尤其是清洁机器人的重要能力之一，沿墙导航能力的应用场合很多，例如地图的构建、避障、全覆盖路径规划、未知环境的导航等。

目前对于移动机器人沿墙导航算法有较多研究，有学者提出基于有限状态机原理的沿墙算法，利用历史声纳信息和当前声纳信息对环境状态进行描述，从而对不同墙体进行分类，实现不同墙体下的沿墙运动。

有的采用波束角小、镜面反射影响较小的红外PSD传感器，提高了测量环境信息的可靠性，同时结合多模态的沿墙控制算法，使沿墙导航的功能更加平滑，但红外传感器收集的数据有限且精度较低。

有研究人员则利用超声波传感器采集障碍物的距离，实现了机器人自主沿墙导航功能，但因受到收集测试距离长度的影响，准确率较低。

某公司则采用视觉传感器收集墙体信息，通过RGB-D相机获得点云信息，然后计算出每个点的表面法线，并根据法线向量的角度阈值对每个平面进行分类。

将左右平面作为沿墙的依据，利用模糊神经网络算法，将传感器收集的数据融合来控制机器人的沿墙动作，但模糊神经网络算法训练起来比较困难。

总的来说，目前虽然使机器人能高效地完成沿墙导航任务，但实际行走路径不够平滑，且遇到曲线墙体或圆形障碍物时效果不佳等问题。

那么通过霍夫变换算法检测并提取墙体直线，选择离机器人最近的直线作为沿墙的参考直线，将机器人与参考直线的垂直距离作为沿墙距离，采集当前墙体直线状态和历史墙体直线状态进行比较来决定机器人的运动方向，对当前墙体直线进行处理后提取沿墙预设路径，按照预设路径进行导航直至完成整个沿墙任务的方法，能否解决这些难题，提升机器人的工作效率呢？

«——【·技术路线·】——»

基于霍夫变换找墙体直线的沿墙导航算法整体流程图如图1所示。

该算法进行沿墙前会确定整体的运动方向是顺时针方向还是逆时针方向，通过激光雷达对障碍物的反馈，利用霍夫变换算法检测并提取出墙体直线，选择离机器人最近的直线作为沿墙的参考直线，将机器人与参考直线的垂直距离作为沿墙距离。

采集当前状态为t时刻的墙体直线状态，历史状态为t-1时刻的墙体直线状态，根据当前墙体直线和历史墙体直线所形成的角度β来决定机器人的运动方向，对当前墙体直线进行处理后提取出沿墙预设路径。

按照沿墙预设路径进行导航直至完成整个沿墙任务。对墙体或障碍物进行霍夫变换找直线处理后，可以把曲线形墙体和圆形障碍物都转化为直线形墙体。

«——【·霍夫变换找墙体直线·】——»

室内环境中大部分的障碍物都由墙体组成，墙体可能是直线形或曲线形，也可能是一些不规则形状。

为了保证沿墙导航算法的一致性，采用霍夫变换算法把曲线形墙体和不规则形墙体分段拟合成直线墙体，使机器人沿墙导航问题整合为沿直线墙体行走问题。

霍夫变换公式如下：

霍夫变换中使用（r,θ）表示一条直线，其中r为该直线到原点的距离，θ为该直线的垂线与x轴的夹角。

假如两个点分别满足θ与r相同条件下的霍夫变换，则说明这两个点在同一条直线上；同理，对于N个点同时满足θ与r的条件，那么这N个点肯定也在同一直线上。

利用霍夫变换原理的这一特性，分别对每一组激光点云拟合得到相应直线，筛选出离机器人中心最近的直线作为沿墙导航的参考直线。

不管是直线形墙体、曲线形墙体还是不规则墙体，都可以通过霍夫变换算法找到墙体直线。

图2为采用霍夫变换算法提取出直线的效果图。

机器人运动方向的具体判断方法为：如果已知移动机器人在t时刻采集到的墙体直线状态，以及在t-1时刻采集到的墙体直线状态，根据t时刻和t-1时刻两条墙体直线所形成的角度β来判断移动机器人的运动方向。

表1为机器人运动方向的判断表。

当170°≤β≤190°时，机器人的运动方向为前进；当β角度不在这个范围内时，主要根据整体的沿墙方向来判断机器人的运动方向。

如果整体沿墙方向为顺时针方向，当0°<β<170°时，机器人的运动方向为右转；当190°<β<360°时，机器人的运动方向为左转。如果整体沿墙方向为逆时针方向，当0°<β<170°时，机器人的运动方向为左转；当190°<β<360°时，机器人的运动方向为右转。

沿墙体直线进行导航：通过历史墙体直线状态和当前墙体直线状态进行比较得到机器人的运动方向之后，对当前墙体直线进行处理。

为了保证机器人有一个安全的沿墙导航环境，需要设置一个沿墙安全距离阈值q，将当前墙体直线朝着与机器人中心的垂线方向平移q，在机器人运动方向上生成的新直线便是沿墙的预设路径。

如图3所示为提取沿墙预设路径的示意图。

机器人得到沿墙预设路径后，首先计算出预设路径的直线方程为y=k2x+b，然后根据机器人自身陀螺仪的角度来确定机器人前进方向。假设机器人前进方向为Y轴正方向，那么所得预设路径与机器人前进方向的夹角范围为[-90°，90°]。

如图4所示为机器人前进方向与预设路径夹角的示意图。

根据机器人前进方向与预设路径的位置关系，可以得到表2所示机器人沿墙行走时的转向判断表。

如果夹角在(-10°,10°)范围内时，说明机器人正沿着预设路径行走；如果夹角角度不在上述范围，则说明机器人需要调整一定角度后才能沿着预设路径行走，该调整角度是预设路径与机器人前进方向的夹角，角度大小为arctan|k2|。

沿墙体直线进行导航的流程图如图5所示。

首先判断机器人前进方向与预设路径的夹角大小是否在(-10°,10°)范围内，如果不在，则机器人旋转夹角角度后沿预设路径行走；如果在，则直接沿预设路径行走。

然后判断沿墙导航是否回到起始点，如果否，则继续执行之前步骤；如果是，则沿墙导航结束。

«——【·实验验证·】——»

为了验证该算法的可行性，在不同环境下进行沿墙导航测试，仿真环境由简单到复杂，所模拟的场景为常见的室内场景。

进行仿真实验时，沿墙的距离设置为10cm，仿真机器人基础速度设置为0.24m/s，沿墙速度设置为0.30m/s。

同时将结合历史状态的沿墙算法和该算法轨迹进行对比实验，其中机器人的移动轨迹由内置的里程计来记录，提取出沿墙距离数据用Matlab画出折线图进行对比，蓝色折线为结合历史状态的沿墙算法的沿墙距离，红色折线为该算法的沿墙距离。

不同场景下的仿真实验对比图如表3所示。

表3显示了两种算法在同一时间内100组沿墙距离数据对比的折线图。

从3组对比实验可以看出，当所沿墙体出现曲线墙体、圆柱形障碍物或连续不规则墙体时，结合历史运动状态的沿墙算法不仅会出现轨迹丢失和沿墙距离不稳定的问题，而且在面对曲线墙体和圆形障碍物时基本无法进行有效的沿墙行为，沿墙效果也会越来越不好，并且路径不连续且不平滑。

采用该算法之后，明显看出在沿各种类型墙体时路径轨迹连贯且与墙体稳定地保持沿墙距离。特别是在沿曲线墙体时，依然像走直线墙体时一样稳定，较好地解决了沿曲线墙体导航效果不佳的问题。

从沿墙距离的数据对比图也可以明显看出，该算法在沿墙导航时沿墙距离误差小，相较于原来算法有了很大提高。说明该算法不仅解决了沿墙导航时路径不平滑的问题，而且提高了沿各种墙体导航时的稳定性。

实验数据分析与讨论：根据上述实验情况对实验数据进行分析，具体结果如表4所示。

由表4可知，传统方案对激光点云拟合处理的平均耗时要比该算法长，该算法在实时性上略有提升。

在沿墙距离的误差率上，由于传统方案没有实时更新机器人与墙的位置距离计算，无法保证沿墙距离的精度，还会加大机器人在沿墙过程中碰撞墙壁的概率。而该算法在找到墙壁直线的同时，

利用点与线的关系实时更新机器人边缘与墙壁的垂直距离信息，将每一时刻机器人边缘到墙的距离与设定的沿墙距离进行比较，实时进行调整，减少沿墙距离误差的同时，相应地降低了机器人与墙的碰撞概率。

洗地机器人真实环境实验：为了进一步验证该算法的真实性，在大厅进行真实环境测试，图6是大厅的整体情况，包含了各种类型的墙体，如内拐角、外拐角以及曲线墙体。

图7是洗地机器人沿大厅一周的轨迹图。

在图7中标记A、B、C、D四个区域位置，用数字1-10标记与图8中洗地机器人实际运动图(a)-(j)相对应的位置。

A区域所处的位置是典型的内拐角墙体，实际导航的过程如图8(a)-(b)所示。

区域B所处的位置是典型的外拐角墙体，实际导航的过程如图8(c)-(d)所示。

C区域为曲线墙体，实际导航的过程如图8(e)-(g)所示。

D区域为突出墙体的柱子，实际导航的过程如图8(h)-(j)所示。

从实际沿墙导航轨迹图可以看出，沿墙导航的实际方向为A-B-C-D，洗地机器人的沿墙方向始终为右，与该算法设定的沿墙方向相同。沿墙导航运动轨迹平滑，并且始终与墙（或障碍物）保持设定的距离，对于不同类型的墙体也能很好地完成沿墙导航任务。

实验证明，通过提取出墙体直线的方式，不仅解决了沿曲线墙体效果不佳的问题，提高了沿墙精度，而且使路径更加平滑。因此，该算法可行性强、完成度高，可以完成复杂环境下的沿墙导航任务。

«——【·结语·】——»

针对移动机器人沿墙导航算法存在沿曲线墙体（圆柱形障碍物）效果不佳、路径冗余且不平滑等问题，提出的结合霍夫变换找墙体直线的沿墙导航算法。

采用霍夫变换算法提取出目标墙体直线，选取离机器人最近的直线作为参考直线，将t时刻的当前墙体直线状态和t-1时刻的历史墙体直线状态进行对比，得到移动机器人的运动方向，将当前直线进行处理后得到沿墙导航的预设路径，按照此路径进行导航直至完成整个沿墙导航任务。

实验结果表明，该算法较好地解决了沿墙导航路径不平滑和沿曲线墙体效果不佳的问题，提高了机器人沿墙导航的效率，使机器人能高效、精确地完成不同环境下的沿墙导航任务。

该算法对不同环境都有较好的适应性和实用性，不仅能较好地完成沿墙导航，还可以完成洗地机器人的定位和建图等任务。