CN104375505B - 一种基于激光测距的机器人自主寻路方法 - Google Patents

Abstract

一种基于激光测距的机器人自主寻路方法，涉及机器人寻路技术领域。本方法所采用的步骤为1)建立以机器人几何中心为极点的极坐标系；2)根据机器人半径R和安全距离D，将障碍物进行膨胀；3)自适应的阈值设定、扇区划分和速度调整。本发明建立以机器人中心为极点的极坐标系，便于直接使用激光测距得到的数据，避免坐标转换造成大量运算和数据无形丢失。根据周围环境调整机器人行驶速度，保证行驶安全。本发明通过激光测距感知周围环境，在前方通过狭窄时或前方障碍物较近时，改变自适应阈值，调整机器人行驶速度以确保安全。

Description

一种基于激光测距的机器人自主寻路方法

技术领域：

本发明涉及一种机器人的寻路技术，尤其是一种基于激光测距引导机器人在障碍物空间中寻找可行驶路径的方法。

背景技术：

近些年来，智能车等机器人成为研究热点。激光测距被广泛应用于机器人路径规划系统。激光测距具有很多其他传感器不可比拟优点，精度高，分辨率高，探测距离远，视野宽广和稳定性好，工作性能不受光照条件影响，具有良好的环境识别能力。

专利“一种基于激光测距仪的移动机器人避障方法(CN102541057A)”提供一种避障方法。该方法建立机器人坐标系，以机器人当前位置为圆心，在坐标原点模拟激光发射N条射线作为机器人运动的候选方向；激光信息分组，在每组中选择障碍点将障碍点映射到机器人坐标系中；将机器人膨化成半径为R的圆，通过一个障碍点做圆的两条切线，通过切线与X轴的夹角求得机器人对于该障碍点可通过的候选方向范围，得到机器人可通过的可行方向；定义代价函数评价各个可行方向，以选择机器人下一周期运动的最佳方向；采用速度控制策略给出机器人线速度和角速度。

专利“一种基于最近距离向量场直方图的避障路径规划方法(CN103455034A)”，提供一种基于最近距离向量场直方图的避障路径规划方法，其首先将机器人当前扫描范围均分为n个扇区，若第k个扇区内的障碍物距离机器人中心点的最近距离向量为NDV_k，获取|NDV_k-1-NDV_k|与机器人直径比较，根据比较结果得到机器人当前位置的局部环境中障碍物个数。接着设定最小避障阈值n_s，若NDV_k≥n_s，则扇区k的角度范围为避障区间，否则为自由行走区间。最后确定瞬时目标点的搜索范围，并在所述搜索范围中获取瞬时目标点，机器人运动到达所述瞬时目标点后，确定新的瞬时目标点并向新的瞬时目标点运动，直至到达终点。

上述的两个专利中，“一种基于激光测距仪的移动机器人避障方法(CN102541057A)”容易忽略一些可行驶方向，在障碍物密集型环境中甚至可能找不到可行驶方向，发生这种现象的原因是存在障碍物的方向即为不可行方向，未考虑距离与速度因素，通过障碍物点候选方向的交集确定可行驶方向，导致一些可行驶方向被忽略；而专利“一种基于最近距离向量场直方图的避障路径规划方法(CN103455034A)”确定目标点需依赖其它传感器，而且可能会产生不理想的瞬时目标点，导致无法到达目标点，其不可能仅依靠单传感器完成避障路径规划，激光传感器无法检测障碍物的完整轮廓，对目标点识别能力不强。

发明内容

为了克服已有方法在障碍物密集环境找不到可行驶方向和可能会产生不理想瞬时目标点的问题，本发明提供一种基于激光测距的机器人寻路方法，该方法不仅能通过自适应策略准确找到可行驶方向，而且根据机器人工作环境可以调整机器人行驶速度，避免在狭窄通道和障碍物距离较近时发生碰撞。

本发明采用如下技术方案：

1)建立以机器人几何中心为极点的极坐标系；2)根据机器人半径R和安全距离D，将障碍物进行膨胀；3)自适应的阈值设定、扇区划分和速度调整。

具体技术方案如下：

1)建立以机器人几何中心为极点的极坐标系，以垂直机器人航向角向右方向为0度，角度按逆时针顺序变大至180度；机器人瞬时运动空间为从0度到180度范围内，半径为激光测距的最大探测距离。

2)将机器人瞬时运动空间按一定角度分辨率a划分为N个扇区，除边缘两个扇区外，其他每个扇区大小相同，角度分辨率a与扇区数量N的关系如式(1)所示；为了保证机器人瞬时运动空间中有一个扇区的中心方向为机器人航向，每种扇区划分策略中，第一个扇区和最后一个扇区所覆盖角度为其他扇区覆盖角度的一半，即为a/2。

N＝180/a+1 (1)

3)角度分辨率a在机器人运动过程中进行自适应调整。机器人移动速度v(t)对应刹停距离S，T₁为驾驶员反应时间，μ为设湿滑路面附着系数，g为重力加速度，保证2·arctan(a/2)·S＞R，当机器人的速度增大时，刹停距离S增大，角度分辨率a要相应地减小。

4)根据机器人半径R和安全距离D将障碍物进行膨胀。将障碍点膨胀为以障碍点为圆心，半径为R+D的圆形区域，障碍点影响范围为从极点出发的膨胀圆的两条切线之间区域。对障碍物膨胀是为了弥补将机器人视为一个节点处理带来的问题。将机器人视为一个节点处理，模型简单，计算方便。机器人若非规则的，其半径R按最大半径处理。

5)计算每个扇区的阻值。通过比较一个扇区内所有障碍物点到机器人几何中心的距离，选择距离最近的一个障碍物点。根据距离最近的一个障碍物点，计算该障碍点的阻值作为所在扇区的阻值，该扇区内其他障碍物的阻值可忽略，如式(2)所示。m_i为扇区阻值，为扇区最近障碍点距离的平方，b为一个常量，其大小由激光测距探测范围L决定，b＝L²。

6)根据机器人当前移动速度v(t)对应的刹停距离式中，S为刹车距离，v(t)为当前移动速度，T₁为驾驶员反应时间，μ为设湿滑路面附着系数，g为重力加速度，自适应地设定阈值T，使以判断可行驶扇区；自适应阈值可以避免找不到可行驶区域或与障碍物碰撞的问题，确保机器人安全行驶。

7)根据所有扇区的情况，在多个可行驶扇区集中，选择最大的可行驶区域，以其中心方向作为下一时刻方向θ(t+1)，t为当前时刻。选择最大的可行驶区域是为了保证机器人行驶安全和避免行驶到不通的道路。

8)根据阈值T的大小和障碍物分布情况，调整机器人行驶速度。在转大角度、可行区域比较狭窄和阈值较小时，降低机器人速度，如式(3)所示。c为常量，T为当前阈值，α为最大可行驶区域覆盖角度，v(t+1)下一时刻机器人行驶速度，θ(t+n)为t+n时刻的预处理角度方向盘转角，t为当前时刻，t+n为n个控制周期之后时刻(n可取1、2、3、4...)，n越大，速度控制越精确，耗时越大；m_i为当前方向盘转角对应扇区的阻值；初始速度v(0)＝0。

9)扇区数量N的取值可以为361、181、121、91、61，对应的a的角度为0.5、1、1.5、2、3。角度分辨率越小，计算越精确，计算量也相应增加；角度分辨率越大，计算越粗略，计算量也相应减小。

技术方案比较：

1)本发明是采用的以机器人中心为极点的极坐标；而现有技术实施方案采用笛卡尔坐标，将激光测距信息转换到笛卡尔坐标系下。本发明可以避免造成数据丢失和大量数据转换计算。

2)本发明是采用膨胀障碍物的方法，根据机器人半径R和安全距离D将障碍物进行膨胀；现有技术实施方案是将机器人膨胀成一个半径为R的圆。本发明通过安全距离D可以在更大程度上避免碰撞障碍物，将机器人当做一个点来处理会更简单。

3)本发明的方法根据障碍物分布情况自适应地调整阈值和扇区划分策略，根据阈值和方向盘转角自适应地调整机器人行驶速度；现有技术实施方案没有涉及自适应的阈值设定和扇区划分，仅提供几种可能的策略；在速度控制方面，现有技术方案仅考虑方向盘转角，未考虑狭窄通道等情况。本发明的方法自适应能力更强，安全性更高，速度控制更合理。

与现有技术相比较，本发明具有以下有益效果及优点：

1)本发明提出的基于激光测距的机器人寻路方法，能够在位置环境下准确地寻找到可行驶路径，原理简单，算法复杂度低和实时性高。本发明建立以机器人中心为极点的极坐标系，便于直接使用激光测距得到的数据，避免坐标转换造成大量运算和数据无形丢失。

2)本发明提出的基于激光测距的机器人寻路方法，根据周围环境调整机器人行驶速度，保证行驶安全。本发明通过激光测距感知周围环境，在前方通过狭窄时或前方障碍物较近时，改变自适应阈值，调整机器人行驶速度以确保安全。

附图说明：

图1是本发明基于激光测距的机器人寻路方法的流程图。

图2是本发明方法中一种将机器人运动空间划分成N＝61个扇区策略

具体实施方式：

结合附图以具体实例对技术方案进行说明，本发明是一种基于激光测距的机器人寻路方法，如附图1所示，本发明解决其技术问题所采用的技术方案是：

10)建立以机器人几何中心为极点的极坐标系，以垂直机器人航向角向右方向为0度，角度按逆时针顺序变大至180度；机器人瞬时运动空间为从0度到180度范围内，半径为激光测距的最大探测距离。

11)将机器人瞬时运动空间按一定角度分辨率a＝3°划分为N＝61个扇区，除边缘两个扇区外，其他每个扇区大小相同，角度分辨率a与扇区数量N的关系如式(1)所示；为了保证机器人瞬时运动空间中有一个扇区的中心方向为机器人航向，每种扇区划分策略中，第一个扇区和最后一个扇区所覆盖角度为其他扇区覆盖角度的一半，即为a/2。

N＝180/a+1 (1)

12)角度分辨率a在机器人运动过程中进行自适应调整。机器人移动速度v(t)对应刹停距离S，T₁为驾驶员反应时间，μ为设湿滑路面附着系数，g为重力加速度，保证2·arctan(a/2)·S＞R，当机器人的速度增大时，刹停距离S增大，角度分辨率a要相应地减小。

13)根据机器人半径R和安全距离D将障碍物进行膨胀。将障碍点膨胀为以障碍点为圆心，半径为R+D的圆形区域，障碍点影响范围为从极点出发的膨胀圆的两条切线之间区域。对障碍物膨胀是为了弥补将机器人视为一个节点处理带来的问题。将机器人视为一个节点处理，模型简单，计算方便。机器人若非规则的，其半径R按最大半径处理。

14)计算每个扇区的阻值。通过比较一个扇区内所有障碍物点到机器人几何中心的距离，选择距离最近的一个障碍物点。根据距离最近的一个障碍物点，计算该障碍点的阻值作为所在扇区的阻值，该扇区内其他障碍物的阻值可忽略，如式(2)所示。m_i为扇区阻值，为扇区最近障碍点距离的平方，b为一个常量，其大小由激光测距探测范围L决定，b＝L²。

15)根据机器人当前移动速度v(t)对应的刹停距离自适应地设定阈值T，使以判断可行驶扇区。自适应阈值可以避免找不到可行驶区域或与障碍物碰撞的问题，确保机器人安全行驶。

16)根据所有扇区的情况，在多个可行驶扇区集中，选择最大的可行驶区域，以其中心方向作为下一时刻方向θ(t+1)，t为当前时刻。选择最大的可行驶区域是为了保证机器人行驶安全和避免行驶到不通的道路。

17)根据阈值T的大小和障碍物分布情况，调整机器人行驶速度。在转大角度、可行区域比较狭窄和阈值较小时，降低机器人速度，如式(3)所示。c为常量，T为当前阈值，α为最大可行驶区域覆盖角度，v(t+1)下一时刻机器人行驶速度，θ(t+n)为t+n时刻的预处理角度方向盘转角，t为当前时刻，t+n为n个控制周期之后时刻(n可取1、2、3、4...)，n越大，速度控制越精确，耗时越大；m_i为当前方向盘转角对应扇区的阻值；初始速度v(0)＝0。

18)扇区数量N的取值可以为361、181、121、91、61，对应的a的角度为0.5、1、1.5、2、3。角度分辨率越小，计算越精确，计算量也相应增加；角度分辨率越大，计算越粗略，计算量也相应减小。

Claims (1)

1.一种基于激光测距的机器人自主寻路方法，其特征在于：

其采用如下技术方案：

建立以机器人几何中心为极点的极坐标系；2)根据机器人半径R和安全距离D，将障碍物进行膨胀；3)自适应的阈值设定、扇区划分和速度调整；

具体技术方案如下：

1)建立以机器人几何中心为极点的极坐标系，以垂直机器人航向角向右方向为0度，角度按逆时针顺序变大至180度；机器人瞬时运动空间为从0度到180度范围内，半径为激光测距的最大探测距离；

2)将机器人瞬时运动空间按一定角度分辨率a划分为N个扇区，除边缘两个扇区外，其他每个扇区大小相同，角度分辨率a与扇区数量N的关系如式(1)所示；为了保证机器人瞬时运动空间中有一个扇区的中心方向为机器人航向，每种扇区划分策略中，第一个扇区和最后一个扇区所覆盖角度为其他扇区覆盖角度的一半，即为a/2；

N＝180/a+1 (1)

3)角度分辨率a在机器人运动过程中进行自适应调整；机器人移动速度v(t)对应刹停距离S，T₁为驾驶员反应时间，μ为设湿滑路面附着系数，g为重力加速度，保证2·arctan(a/2)·S＞R，当机器人的速度增大时，刹停距离S增大，角度分辨率a要相应地减小；

4)根据机器人半径R和安全距离D将障碍物进行膨胀；将障碍点膨胀为以障碍点为圆心，半径为R+D的圆形区域，障碍点影响范围为从极点出发的膨胀圆的两条切线之间区域；对障碍物膨胀是为了弥补将机器人视为一个节点处理带来的问题；将机器人视为一个节点处理，模型简单，计算方便；机器人若非规则的，其半径R按最大半径处理；

5)计算每个扇区的阻值；通过比较一个扇区内所有障碍物点到机器人几何中心的距离，选择距离最近的一个障碍物点；根据距离最近的一个障碍物点，计算该障碍点的阻值作为所在扇区的阻值，该扇区内其他障碍物的阻值可忽略，如式(2)所示；m_i为扇区阻值，为扇区最近障碍点距离的平方，b为一个常量，其大小由激光测距探测范围L决定，b＝L²；

$m_i=b/d_i^2---\left(2\right)$

6)根据机器人当前移动速度v(t)对应的刹停距离式中，S为刹车距离，v(t)为当前移动速度，T₁为驾驶员反应时间，μ为设湿滑路面附着系数，g为重力加速度，自适应地设定阈值T，使以判断可行驶扇区；自适应阈值可以避免找不到可行驶区域或与障碍物碰撞的问题，确保机器人安全行驶；

7)根据所有扇区的情况，在多个可行驶扇区集中，选择最大的可行驶区域，以其中心方向作为下一时刻方向θ(t+1)，t为当前时刻；选择最大的可行驶区域是为了保证机器人行驶安全和避免行驶到不通的道路；

8)根据阈值T的大小和障碍物分布情况，调整机器人行驶速度；在转大角度、可行区域比较狭窄和阈值较小时，降低机器人速度，如式(3)所示；c为常量，T为当前阈值，α为最大可行驶区域覆盖角度，v(t+1)下一时刻机器人行驶速度，θ(t+n)为t+n时刻的预处理角度方向盘转角，t为当前时刻，t+n为n个控制周期之后时刻(n可取1、2、3、4...)，n越大，速度控制越精确，耗时越大；m_i为当前方向盘转角对应扇区的阻值；初始速度v(0)＝0；

$v(t+1)=\frac{c\·T\·\mathrm{\α}}{\mathrm{\θ}(t+n)\·m_i}---\left(3\right)$

9)扇区数量N的取值可以为361、181、121、91、61，对应的a的角度为0.5、1、1.5、2、3；角度分辨率越小，计算越精确，计算量也相应增加；角度分辨率越大，计算越粗略，计算量也相应减小。