[发明专利]一种基于激光雷达的自适应沿墙导航方法

权利要求书

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

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

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

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

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

2.如权利要求1所述的一种基于激光雷达的自适应沿墙导航方法，其特征在于，所述步骤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。

3.如权利要求2所述的一种基于激光雷达的自适应沿墙导航方法，其特征在于，所述步骤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。

4.如权利要求3所述的一种基于激光雷达的自适应沿墙导航方法，其特征在于，所述步骤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时，机器人摆脱内角；否则，机器人继续原地旋转。

5.如权利要求3所述的一种基于激光雷达的自适应沿墙导航方法，其特征在于，所述步骤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算得的前向线速度和角速度移动。