CN112965082B - 一种基于激光雷达的自适应沿墙导航方法 - Google Patents

Abstract

本发明涉及机器人智能控制领域，尤其涉及一种基于激光雷达的自适应沿墙导航方法，首先将激光雷达扫描的区域划分为7个区域，再初始化机器人与激光雷达；然后获取各区域的激光线距离墙面最短距离和其对应索引、机器人前进方向与墙面的夹角角度、距离墙面最短距离与沿墙理想距离的差值；接着根据获得的各区域的激光线距离墙面最短距离和沿墙理想距离，判断出机器人相对墙面的位置；最后机器人计算出前向线速度和角速度，执行速度指令进行移动。本发明解决了移动机器人难以自适应沿墙导航的问题，使机器人能够适应复杂环境，自适应地调节移动速度，包括线速度和角速度，使之按照固定距离沿墙移动，同时能够根据周围障碍物的变化快速地改变沿墙策略。

Description

一种基于激光雷达的自适应沿墙导航方法

技术领域

本发明涉及机器人智能控制领域，尤其涉及一种基于激光雷达的自适应沿墙导航方法。

背景技术

沿墙导航是指机器人能够沿着环境中墙体的基本轮廓，并与墙体保持一定距离进行移动。当沿墙导航与其他智能行为结合时，移动机器人就可以完成一些复杂的任务。以清洁机器人为例，清洁机器人需要沿着墙边打扫灰尘污渍，此时就需要沿墙导航功能。

而目前机器人往往是利用单点测距传感器进行沿墙导航，该种方法无法感知除了单点测距传感器正对方向以外的环境信息，因而无法有效地获取复杂的环境信息。此外，目前机器人普遍的沿墙导航控制方法对于环境缺乏适应性，不能实时地、动态地、具有自适应能力地调整移动速度。

发明内容

为了解决现有技术中存在的上述移动机器人难以自适应沿墙导航的问题，本发明提出了一种基于激光雷达的自适应沿墙导航方法，使机器人能够主动适应复杂环境，移动时保持距离墙面距离不变，使得机器人以固定距离沿墙移动，其具体技术方案如下：

一种基于激光雷达的自适应沿墙导航方法，以20hz的频率重复执行如下步骤：

步骤1，将激光雷达扫描的区域划分为7个区域，再初始化机器人与激光雷达；

步骤2，获取各区域的激光线距离墙面最短距离和其对应索引、机器人前进方向与墙面的夹角角度、距离墙面最短距离与沿墙理想距离的差值；

步骤3，根据步骤2获得的各区域的激光线距离墙面最短距离和沿墙理想距离，判断出机器人相对墙面的位置；

步骤4，机器人计算出前向线速度和角速度，执行速度指令进行移动。

进一步的，所述步骤1具体包括：

步骤1.1，将机器人上的激光雷达扫描的区域按照360条激光线，对应360°，划分为7个区域：右后、右、右前、前、左前、左、左后，分别对应第1-52、53-104、105-155、156-205、206-256、257-308、309-360条激光线；

步骤1.2，用户根据实际使用要求调节设置机器人沿墙方向变量direction，确认机器人沿左墙导航时direction设置为1，右墙时direction设置为-1；设置比例系数P、微分系数D、角度比例系数P_angle、沿墙理想距离dist_wall、最大线速度max_speed。

进一步的，所述步骤2具体包括：

步骤2.1，分别遍历第1-52、53-104、105-155、156-205、206-256、257-308、309-360条激光线测出的距离值并记录索引，取各自范围内的最短距离，记为dist_1、dist_2、…、dist_7，取其中距离墙面最短的距离dist_min，并记录取得该最短距离时对应的索引index_min；

步骤2.2，计算机器人前进方向与墙面的夹角角度，记为angle_min，单位是弧度制，表达式为：

angle_min = (index_min – 360/2) * angle_increment

其中angle_increment为激光雷达角度分辨率，即每条激光线相较于上一条激光线增加的角度，为激光雷达产品固定的参数，单位是弧度制；

步骤2.3，计算dist_min与沿墙理想距离dist_wall的差值，记为误差e；计算此刻误差与上一刻误差的差值，记为diff_e。

进一步的，所述步骤3具体包括：

步骤3.1，判断机器人当前是否位于外角：当右后区域的最短距离dist_1< dist_wall或者左后区域的最短距离dist_7< dist_wall，且其他区域的最短距离大于阈值时，机器人位于外角；否则，不位于外角；

当机器人位于外角时，机器人原地旋转，原地旋转的同时，判断机器人是否摆脱外角：当右区域最短距离dist_2< dist_wall或者左区域最短距离dist_6< dist_wall时，机器人摆脱外角，然后判断当前是否位于内角；否则，机器人继续原地旋转；

步骤3.2，判断机器人当前是否位于内角：当右前、前、左前区域的最短距离均小于dist_wall时，机器人位于内角；否则，不位于内角；

当机器人位于内角时，机器人原地旋转，原地旋转的同时，判断机器人是否摆脱内角：当右后区域最短距离dist_1< dist_wall且左后区域最短距离dist_7< dist_wall时，机器人摆脱内角；否则，机器人继续原地旋转。

进一步的，所述步骤4具体包括：

步骤4.1，进入自适应沿墙模式，计算前向线速度、角速度，

前向线速度v计算方法为：

若dist_4 < wall_dist ， v = 0,

若dist_4< wall_dist * 2， v = max_speed / 2,

若 angle_min > 1.75， v = max_speed / 3,

其他， v = max_speed,

角速度w计算方法为：

w = direction * (P*e + D*diff_e) + P_angle * (angle_min –π /2 * direction)

其中π是圆周率；

步骤4.2，机器人按照步骤4.1算得的前向线速度和角速度移动。

本发明的有益效果是：

（1）本发明的机器人获取周围360度的环境信息并加以利用，能够有效适应复杂环境，尤其是有内角、外角的环境；

（2）本发明能够自适应地调节机器人的移动速度，包括线速度和角速度，使之按照固定距离沿墙移动；

（3）本发明能够使机器人动态避障，当机器人周围突然出现障碍物时，机器人可以迅速调整移动速度，使沿墙导航的任务继续顺利执行。

附图说明

图1 是本发明的流程图；

图2 是本发明的激光雷达安装及初始化示意图；

图3是本发明中机器人位于外角的示意图；

图4是本发明中机器人位于内角的示意图。

具体实施方式

为了使本发明的目的、技术方案和技术效果更加清楚明白，下面结合说明书附图和具体实施例，对本发明作进一步详细说明。

实施例：

如图1-2所示，本发明提供一种基于激光雷达的自适应沿墙导航方法，将机器人上的激光雷达扫描的区域按照360条激光线，对应360°，平均划分为7个区域：右后、右、右前、前、左前、左、左后，分别对应第1-52、53-104、105-155、156-205、206-256、257-308、309-360条激光线，然后依照下列步骤完成自适应沿墙导航功能：

步骤1：初始化机器人与激光雷达。用户根据实际使用要求调节设置机器人沿墙方向变量direction，确认机器人沿左墙导航时direction设置为1，右墙时direction设置为-1；设置比例系数P、微分系数D、角度比例系数P_angle、沿墙理想距离dist_wall、最大线速度max_speed。

根据机器人运行状况调整比例系数P、微分系数D、角度比例系数P_angle，调整规则为：如果机器人左右摇摆幅度过大，则减小比例系数P和角度比例系数P_angle；如果机器人调整速度的响应过慢，则增大比例系数P和角度比例系数P_angle；如果机器人一直左右摆动，则增加微分系数D，否则不变。

步骤2：计算激光测距时7个区域：右后、右、右前、前、左前、左、左后，各自扫描到的最短距离，分别记为dist_1、dist_2、…、dist_7。计算方法具体为：分别遍历第1-52、53-104、105-155、156-205、206-256、257-308、309-360条激光线测出的距离值，分别取各自范围内的最短距离，记为dist_1、dist_2、…、dist_7。

遍历360条激光线并记录索引，即第n条对应着索引n，取360条中距离墙面最短的距离dist_min，并记录取得该最短距离时对应的索引index_min。

计算机器人前进方向与墙面的夹角角度，记为angle_min，单位是弧度制，表达式为：

angle_min = (index_min – 360/2) * angle_increment

其中angle_increment为激光雷达角度分辨率，即每条激光线相较于上一条激光线增加的角度，为激光雷达产品固定的参数，单位是弧度制。

计算dist_min与沿墙理想距离dist_wall的差值，记为误差e；计算此刻误差与上一刻误差的差值，记为diff_e。

步骤3：判断机器人当前是否位于外角：当右后区域的最短距离dist_1< dist_wall或者左后区域的最短距离dist_7< dist_wall，且其他区域的最短距离大于阈值时，机器人位于外角；否则，不位于外角；

如图3所示，当机器人位于外角时，机器人原地旋转，原地旋转的同时，判断机器人是否摆脱外角：当右区域最短距离dist_2< dist_wall或者左区域最短距离dist_6< dist_wall时，机器人摆脱外角，然后判断当前是否位于内角；否则，机器人继续原地旋转。

判断机器人当前是否位于内角：当右前、前、左前区域的最短距离均小于dist_wall时，机器人位于内角；否则，不位于内角，执行步骤4。

如图4所示，当机器人位于内角时，机器人原地旋转，原地旋转的同时，判断机器人是否摆脱内角：当右后区域最短距离dist_1< dist_wall且左后区域最短距离dist_7<dist_wall时，机器人摆脱内角，执行步骤4；否则，机器人继续原地旋转。

步骤4：进入自适应沿墙模式，计算前向线速度、角速度，

前向线速度v计算方法为：

若dist_4 < wall_dist ， v = 0,

若dist_4< wall_dist * 2， v = max_speed / 2,

若 angle_min > 1.75， v = max_speed / 3,

其他， v = max_speed,

角速度w计算方法为：

w = direction * (P*e + D*diff_e) + P_angle * (angle_min –π /2 * direction)

其中π是圆周率。

最后机器人执行速度指令，即机器人按照计算得到的前向线速度和角速度移动。

在本发明以20hz的频率重复执行上述步骤1-4。

本发明并不限于上述实施例，在本领域普通技术人员所具备的知识范围内，还可以在不脱离本发明宗旨的前提下作出各种变化，比如将本算法应用于无人驾驶汽车的沿护栏导航任务中等等。

Claims (2)

1.一种基于激光雷达的自适应沿墙导航方法，其特征在于，以20hz的频率重复执行如下步骤：

步骤1，将激光雷达扫描的区域划分为7个区域，再初始化机器人与激光雷达；

步骤2，获取各区域的激光线距离墙面最短距离和其对应索引、机器人前进方向与墙面的夹角角度、距离墙面最短距离与沿墙理想距离的差值；

步骤3，根据步骤2获得的各区域的激光线距离墙面最短距离和沿墙理想距离，判断出机器人相对墙面的位置；

步骤4，机器人计算出前向线速度和角速度，执行速度指令进行移动；

所述步骤1具体包括：

步骤1.1，将机器人上的激光雷达扫描的区域按照360条激光线，对应360°，划分为7个区域：右后、右、右前、前、左前、左、左后，分别对应第1-52、53-104、105-155、156-205、206-256、257-308、309-360条激光线；

步骤1.2，用户根据实际使用要求调节设置机器人沿墙方向变量direction，确认机器人沿左墙导航时direction设置为1，右墙时direction设置为-1；设置比例系数P、微分系数D、角度比例系数P_angle、沿墙理想距离dist_wall、最大线速度max_speed；

所述步骤2具体包括：

步骤2.1，分别遍历第1-52、53-104、105-155、156-205、206-256、257-308、309-360条激光线测出的距离值并记录索引，取各自范围内的最短距离，记为dist_1、dist_2、…、dist_7，取其中距离墙面最短的距离dist_min，并记录取得该最短距离时对应的索引index_min；

步骤2.2，计算机器人前进方向与墙面的夹角角度，记为angle_min，单位是弧度制，表达式为：

angle_min = (index_min – 360/2) * angle_increment

其中angle_increment为激光雷达角度分辨率，即每条激光线相较于上一条激光线增加的角度，为激光雷达产品固定的参数，单位是弧度制；

步骤2.3，计算dist_min与沿墙理想距离dist_wall的差值，记为误差e；计算此刻误差与上一刻误差的差值，记为diff_e；

所述步骤3具体包括：

步骤3.1，判断机器人当前是否位于外角：当右后区域的最短距离dist_1< dist_wall或者左后区域的最短距离dist_7< dist_wall，且其他区域的最短距离大于阈值时，机器人位于外角；否则，不位于外角；

当机器人位于外角时，机器人原地旋转，原地旋转的同时，判断机器人是否摆脱外角：当右区域最短距离dist_2< dist_wall或者左区域最短距离dist_6< dist_wall时，机器人摆脱外角，然后判断当前是否位于内角；否则，机器人继续原地旋转；

步骤3.2，判断机器人当前是否位于内角：当右前、前、左前区域的最短距离均小于dist_wall时，机器人位于内角；否则，不位于内角；

当机器人位于内角时，机器人原地旋转，原地旋转的同时，判断机器人是否摆脱内角：当右后区域最短距离dist_1< dist_wall且左后区域最短距离dist_7< dist_wall时，机器人摆脱内角；否则，机器人继续原地旋转。

2.如权利要求1所述的一种基于激光雷达的自适应沿墙导航方法，其特征在于，所述步骤4具体包括：

步骤4.1，进入自适应沿墙模式，计算前向线速度、角速度，

前向线速度v计算方法为：

若dist_4 < wall_dist ， v = 0,

若dist_4< wall_dist * 2， v = max_speed / 2,

若 angle_min > 1.75， v = max_speed / 3,

其他， v = max_speed,

角速度w计算方法为：

w = direction * (P*e + D*diff_e) + P_angle * (angle_min – π /2 * direction)

其中π是圆周率；

步骤4.2，机器人按照步骤4.1算得的前向线速度和角速度移动。