CN113311862B - 机器人规避障碍物的运动规划方法 - Google Patents

Abstract

本发明提供一种机器人规避障碍物的运动规划方法，基于向量场直方图算法的进行了改进，应用一种可自动调整范围的动态窗口，根据设置条件选择模式一或者模式二，并在不同的窗口模式下设置自适应阈值，使用阈值评价函数，同时考虑移动机器人硬件特性、运动特性确定阈值的选取范围，使机器人在快速适应不同环境的同时可以无碰撞且平滑到达目标点。本发明的一种机器人规避障碍物的运动规划方法，规划的路径更短，而且安全有效。

Description

机器人规避障碍物的运动规划方法

技术领域

本发明涉及机器人领域，尤其涉及一种机器人规避障碍物的运动规划方法。

背景技术

移动机器人的发展是目前科学研究的热门领域之一，随着现在科技的不断进步，机器人领域也在不断发展，自主避障能力和路径规划作为机器人的最基本的能力，也在快速发展当中，国内外学者对机器人的路径规划做了大量的研究，其中有人工势场法、bug算法、虚拟势场算法VFF等，向量场直方图法(VFH)也是其中之一。

但是目前的VFH算法还是存在一些问题，例如受阈值、栅格、窗口范围影响较大，因此不能很好的完成运动路线的规划。

发明内容

针对上述现有技术中的不足，本发明提供一种机器人规避障碍物的运动规划方法，规划的路径更短，而且安全有效。

为了实现上述目的，本发明提供一种机器人规避障碍物的运动规划方法，包括步骤：

S1：设置初始窗口大小为W*W，地图类型为栅格地图，将所述地图的每一个栅格作为一个障碍物向量，所述障碍物向量包括大小m_i,j和方向β_i,j；

S2：计算所述初始窗口内的障碍物总密度值p_n；

S3：根据所述障碍物总密度值选择窗口模式，当满足预设条件时，切换不同的所述窗口模式；

S4：根据角分辨率f，将正方形所述初始窗口分为若干个扇区，计算所述扇区个数n及障碍物所在扇区k；

S5：计算每个所述扇区的障碍物密度值，生成二维障碍物向量直方图；

S6：根据所述初始窗口的自适应阈值选择可行进的方向作为候选区域；

S7：所述障碍物密度值大于阈值的为不可行的方向，所述障碍物密度值低于阈值的可行驶的方向，将连续可通行的所述扇区作为候选山谷；

S8：构造代价函数，选择代价最小的所述候选区域作为行进方向。

优选地，所述S1步骤中，所述障碍物向量的大小m_i,j和方向β_i,j满足公式：

其中，

表示确信值，a和b表示设定的常量，(x_i,y_j)表示障碍物位置；(x₀,y₀)表示机器人中心点的位置；i,j表示大于零的自然数；d_i,j表示机器人中心点与障碍物的距离。

优选地，所述S2步骤中，所述障碍物总密度值p_n的计算公式为：

p_n＝∑_i，jm_i,j。

优选地，所述S3步骤中，所述窗口模式包括模式一和模式二；根据所述障碍物总密度值判断障碍物密集情况，所述碍物密集情况包括障碍物稀疏和障碍物密集；当障碍物稀疏时，使用模式一的第一动态窗口，所述第一动态窗口的大小为W*W，W＝14；当障碍物密集时，使用模式二的第二动态窗口，所述第二动态窗口的大小为Q*Q，Q计算公式为：

Q＝int(((p_max-p₁)*W)/p_max)；

其中p_max为最大障碍物总密度值，p₁为模式一下障碍物总密度值，W为模式一窗口大小范围；Q为模式二窗口大小范围；

所述S3步骤中最大障碍物总密度值p_max计算公式为：

其中

为机器人中心点与障碍物距离的平方；

对2种窗口模式设置不同的跳转条件，其跳转条件如下所示：

在所述模式一下：

ω为模式一临界跳转条件，当p₁＜ω时，模式保持不变，当p₁≥ω时，切换模式为模式二，其中ω为经验值；

在所述模式二下：

q为模式二下临界跳转条件，当p₂＞q时，模式保持不变，当p₂≤q时，切换模式为模式一；

其中μ为修正系数。

优选地，所述S4步骤中，所述扇区个数n及障碍物所在扇区k满足公式：

优选地，所述S5步骤中，每个所述扇区的障碍物密度值满足公式：

其中，h_k表示每个所述扇区的障碍物密度值。

优选地，所述S6步骤中，当机器人未找到目标点时，

所述自适应阈值的选择根据所述扇区内障碍物密度来进行选择；不同所述窗口模式下所述自适应阈值选择方法不同；

所述模式一下所述自适应阈值采用第一阈值T₁：

其中，τ_high表示第一高阈值，p_i表示第i个扇区的障碍物总密度值，δ₁表示第一临界条件，τ_low表示第一低阈值；

所述模式二下所述自适应阈值采用第二阈值T₂：

其中，η_high表示第二高阈值，η_low表示第二低阈值，δ₂表示第二临界条件；

所述自适应阈值的选择还考虑机器人的运动特性和硬件特性；机器人速度为v，加速度为α，则机器人速度减速到0的距离S为：

考虑到雷达的探测距离和机器人减速安全距离，雷达到机器人边缘的距离为R，其减速距离为S：

所述模式一下还满足：

τ_high＜d₁；

τ_low＞(S+R)；

其中，d₁表示雷达扫描的最大距离；

所述S6步骤中，当机器人找到目标点时，所述自适应阈值采用第三阈值T₃：

其中，T_n表示选定阈值，在模式一下T_n为T₁，在模式二下T_n为T₂；γ表示自定义值，d_rt表示机器人距离目标点的距离。

优选地，所述S7步骤中，将最靠近障碍物的所述扇区边界记为右边界k_n；则左边界k_f满足：

k_f＝k_n+s_max；

其中，s_max表示最大扇区数；

所述S7步骤中，候选方向c满足公式：

优选地，所述S8步骤中，所述代价函数g(c)为：

其中，λ₁和λ₂表示自定义常数，c为候选方向，k_l为目标方向，

是当前的所在扇区，

表示候选方向和目标方向相关的代价；

表示候选方向和机器人当前方向相关的代价。

优选地，还包括一循环跳出机制，设置触发条件，当***陷入死循环，***启动所述循环跳出机制，自动选择一个可行进的方向，跳出当前循环。

本发明由于采用了以上技术方案，使其具有以下有益效果：

本发明是基于VFH的改进算法，地图类型为栅格地图，当雷达扫描到障碍物时会给更新单元格的确信值C，因为原算法使用一个固定的窗口，在障碍物较多的情况下，会产生较大的障碍物密度值，会导致机器人无法通过或难以搜索可行进路径，本发明改进的算法中，根据当前窗口障碍物总密度值选择不同的窗口模式，当障碍物密度较小时，会选择窗口较大的模式，当障碍物密度较大时，会选择窗口较小的模式，从而使得机器人可以在密集障碍物中穿行。设置跳出机制，当机器人在选择行进方向时陷入死循环，根据其触发条件，启动跳出机制，跳出机制运行后，机器人选择任何可行进的方向作为前进方向，逃离当前位置后，再进行路径规划。

附图说明

图1为本发明实施例的机器人规避障碍物的运动规划方法的流程图；

图2为本发明实施例的第一运行轨迹图；

图3为本发明实施例的第二运行轨迹图；

图4为本发明实施例的窗口模式选择流程图。

具体实施方式

下面根据附图图1～图4，给出本发明的较佳实施例，并予以详细描述，使能更好地理解本发明的功能、特点。

请参阅图1～图4，本发明实施例的一种机器人规避障碍物的运动规划方法，包括步骤：

S1：机器人首先会根据位姿信息和里程计信息确定自己的位姿；

雷达开始扫描，扫描到障碍物时为障碍物所在的栅格会更新确信值C。

根据雷达信息构建栅格地图，再初始点和目标点开始进行路径规划，地图为栅格地图；

本实施例采用VFH算法，采用栅格法建立地图，每个栅格都有一个信任值C，以二维直角直方图网格作为世界模型；以机器人为中心，构建一个W_s×W_s的正方形窗口作为初始窗口，正方形窗口随着机器人的移动而不断移动，模型通过机载距离传感器采样的距离数据不断更新栅格中的信任值C；

设置初始窗口大小为W*W，W＝14，根据窗口内的障碍物信息，每个栅格有其确信值

通过雷达扫描更新确信值

将地图的每一个栅格作为一个障碍物向量，障碍物向量包括大小m_i,j和方向β_i,j；

S1步骤中，障碍物向量的大小m_i,j和方向β_i,j满足公式：

其中，

表示确信值，a和b表示设定的常量，(x_i,y_j)表示障碍物位置；(x₀,y₀)表示机器人中心点的位置；i,j表示大于零的自然数；d_i,j表示机器人中心点与障碍物的距离；

S2：计算初始窗口内的障碍物总密度值p_n；

p_n＝∑_i，jm_i,j。

S3：根据障碍物总密度值选择窗口模式，当满足预设条件时，切换不同的窗口模式；

S3步骤中，窗口模式包括模式一和模式二；根据障碍物总密度值判断障碍物密集情况，碍物密集情况包括障碍物稀疏和障碍物密集；当障碍物稀疏时，使用模式一的第一动态窗口，第一动态窗口的大小为14*14；当障碍物密集时，使用模式二的第二动态窗口，第二动态窗口的大小为Q*Q；可表示为公式：

Q＝int(((p_max-p₁)*W)/p_max)

其中p_max为最大障碍物总密度值，p₁为模式一下障碍物总密度值，W为模式一窗口大小范围；Q为模式二窗口大小范围；

S3步骤中最大障碍物总密度值p_max计算公式为：

其中

为机器人中心点与障碍物距离的平方；

对2种窗口模式设置不同的跳转条件，其跳转条件如下所示：

在模式一下：

ω为模式一临界跳转条件，当p₁＜ω时，模式保持不变，当p₁≥ω时，切换模式为模式二，其中ω为经验值；

在模式二下：

q为模式二下临界跳转条件，当p₂＞q时，模式保持不变，当p₂≤q时，切换模式为模式一；

其中μ为修正系数。

S4：根据角分辨率f，将正方形初始窗口分为若干个扇区，计算扇区个数n及障碍物所在扇区k；

扇区个数n及障碍物所在扇区k满足公式：

S5：计算每个扇区的障碍物密度值，生成二维障碍物向量直方图；

每个扇区的障碍物密度值满足公式：

其中，h_k表示每个扇区的障碍物密度值。

S6：根据初始窗口的自适应阈值选择可行进的方向作为候选区域；分为未找到目标点和找到目标点2种情况；

当机器人未找到目标点时，模式一和模式二下均应用自适应阈值，让机器人可以适应不同的障碍物密集环境；

自适应阈值的选择根据扇区内障碍物密度来进行选择；不同窗口模式下自适应阈值选择方法不同；

模式一下自适应阈值采用第一阈值T₁：

其中，τ_high表示第一高阈值，p_i表示第i个扇区的障碍物总密度值，δ₁表示第一临界条件，τ_low表示第一低阈值；

模式二下自适应阈值采用第二阈值T₂：

其中，η_high表示第二高阈值，η_low表示第二低阈值，δ₂表示第二临界条件；

自适应阈值的选择还考虑机器人的运动特性和硬件特性；机器人速度为v，加速度为α，则机器人速度减速到0的距离S为：

考虑到雷达的探测距离和机器人减速安全距离，雷达到机器人边缘的距离为R，其减速距离为S：

模式一下还满足：

τ_high＜d₁；

τ_low＞(S+R)；

其中，d₁表示雷达最大的扫描距离；

S6步骤中，当机器人找到目标点时，若目标点附近没有障碍物，机器人按上述阈值可顺利到达目标点，若目标点距离障碍物太近，算法会检测到较大的障碍物强度，这样会导致机器人无法接近目标点，因此当机器人找到目标点时，无论窗口在模式一还是模式二情况下，均用第三阈值T₃：

其中，T_n表示选定阈值，在模式一下T_n为T₁，在模式二下T_n为T₂；γ表示自定义值，本实施例中，γ＝0.5，d_rt表示机器人距离目标点的距离。

S7：障碍物密度值大于阈值的为不可行的方向，障碍物密度值低于阈值的可行驶的方向，将连续可通行的扇区作为候选山谷；

将最靠近障碍物的扇区边界记为右边界k_n；则左边界k_f满足：

k_f＝k_n+s_max；

其中，s_max表示最大扇区数；

连续扇区个数小于或者等于s_max，其左边界均为k_f，k_f为一个通行区域距离最远的边界，若可通行连续扇区超过s_max，则将超过的扇区作为其他候选区域。

S7步骤中，候选方向c满足公式：

S8：构造代价函数，选择代价最小的候选区域作为行进方向。

原VFH算法选择方向没有考虑到选择的方向机器人是否能达到，而且方向转变较大的情况下可能会造成机器人震荡，偏离规划好的路线，因此应用代价函数，让机器人根据代价函数选择行进的方向。

代价函数g(c)为：

其中，λ₁和λ₂表示自定义常数，c为候选方向，k_l为目标方向，

是当前的所在扇区。λ₁越大代表机器人选择这个方向的目的性就越强，λ₂越高，机器人越试图以最小的运动方向变化来执行平滑的路径，关系需满足：

λ₁＞λ₂；

表示候选方向和目标方向相关的代价；这个差异越大，候选方向越会引导机器人偏离目标方向，代价也就越大。

表示候选方向和机器人当前方向相关的代价。这种差异越大，运动方向所需要的变化就越大。

选择代价最小的一组作为行进的方向。完成一次路径规划。

下一轮路径规划首先根据窗口内障碍物总密度值选择窗口模式，再进行窗口内障碍物密度的计算，在进行上述步骤。

还包括一循环跳出机制，设置触发条件，当***陷入死循环，***启动循环跳出机制，自动选择一个可行进的方向，跳出当前循环。其中一种跳出机制是根据障碍物总密度值，如障碍物总密度值在一定时间内没有任何改变或者改变的程度较小，启动跳出机制，机器人沿着任何可行进的方向前进一段距离后，重新规划路径。

请参阅图2和图3，机器人到达目标点。

以上结合附图实施例对本发明进行了详细说明，本领域中普通技术人员可根据上述说明对本发明做出种种变化例。因而，实施例中的某些细节不应构成对本发明的限定，本发明将以所附权利要求书界定的范围作为本发明的保护范围。

Claims (8)

1.一种机器人规避障碍物的运动规划方法，包括步骤：

S1：设置初始窗口大小为W*W，地图类型为栅格地图，将所述地图的每一个栅格作为一个障碍物向量，所述障碍物向量包括大小m_i,j和方向β_i,j；

S2：计算所述初始窗口内的障碍物总密度值p_n；

S3：根据所述障碍物总密度值选择窗口模式，当满足预设条件时，切换不同的所述窗口模式；

S4：根据角分辨率f，将正方形所述初始窗口分为若干个扇区，计算所述扇区个数n及障碍物所在扇区k；

S5：计算每个所述扇区的障碍物密度值，生成二维障碍物向量直方图；

S6：根据所述初始窗口的自适应阈值选择可行进的方向作为候选区域；

S7：所述障碍物密度值大于阈值的为不可行的方向，所述障碍物密度值低于阈值的可行驶的方向，将连续可通行的所述扇区作为候选山谷；

S8：构造代价函数，选择代价最小的所述候选区域作为行进方向；

所述S3步骤中，所述窗口模式包括模式一和模式二；根据所述障碍物总密度值判断障碍物密集情况，所述碍物密集情况包括障碍物稀疏和障碍物密集；当障碍物稀疏时，使用模式一的第一动态窗口，所述第一动态窗口的大小为W*W，W＝14；碍物密集时，使用模式二的第二动态窗口，所述第二动态窗口的大小为Q*Q，Q计算公式为：

Q＝int(((p_max-p₁)*W)/p_max)

其中p_max为最大障碍物总密度值，p₁为模式一下障碍物总密度值，W为模式一窗口大小范围；Q为模式二窗口大小范围；

所述S3步骤中最大障碍物总密度值p_max计算公式为：

其中

为机器人中心点与障碍物距离的平方；

对2种窗口模式设置不同的跳转条件，其跳转条件如下所示：

在所述模式一下：

ω为模式一临界跳转条件，当p₁＜ω时，模式保持不变，当p₁≥ω时，切换模式为模式二，其中ω为经验值；

在所述模式二下：

q为模式二下临界跳转条件，当p₂＞q时，模式保持不变，当p₂≤q时，切换模式为模式一；

其中μ为修正系数；

所述S6步骤中，当机器人未找到目标点时，

所述自适应阈值的选择根据所述扇区内障碍物密度来进行选择；不同所述窗口模式下所述自适应阈值选择方法不同；

所述模式一下所述自适应阈值采用第一阈值T₁：

其中，τ_high表示第一高阈值，p_i表示第i个扇区的障碍物总密度值，δ₁表示第一临界条件，τ_low表示第一低阈值；

所述模式二下所述自适应阈值采用第二阈值T₂：

其中，η_high表示第二高阈值，η_low表示第二低阈值，δ₂表示第二临界条件；

所述自适应阈值的选择还考虑机器人的运动特性和硬件特性；机器人速度为v，加速度为α，则机器人速度减速到0的距离S为：

考虑到雷达的探测距离和机器人减速安全距离，雷达到机器人边缘的距离为R，其减速距离为S：

所述模式一下还满足：

τ_high＜d₁；

τ_low＞(S+R)；

其中，d₁表示雷达扫描最大距离；

所述S6步骤中，当机器人找到目标点时，所述自适应阈值采用第三阈值T₃：

其中，T_n表示选定阈值，在模式一下T_n为T₁，在模式二下T_n为T₂；γ表示自定义值，d_rt表示机器人距离目标点的距离。

2.根据权利要求1所述的机器人规避障碍物的运动规划方法，其特征在于，所述S1步骤中，所述障碍物向量的大小m_i,j和方向β_i,j满足公式：

其中，

表示确信值，a和b表示设定的常量，(x_i,y_j)表示障碍物位置；(x₀,y₀)表示机器人中心点的位置；i,j表示大于零的自然数；d_i,j表示机器人中心点与障碍物的距离。

3.根据权利要求2所述的机器人规避障碍物的运动规划方法，其特征在于，所述S2步骤中，所述障碍物总密度值p_n的计算公式为：

p_n＝∑_i，jm_i,j。

4.根据权利要求3所述的机器人规避障碍物的运动规划方法，其特征在于，所述S4步骤中，所述扇区个数n及障碍物所在扇区k满足公式：

5.根据权利要求4所述的机器人规避障碍物的运动规划方法，其特征在于，所述S5步骤中，每个所述扇区的障碍物密度值满足公式：

其中，h_k表示每个所述扇区的障碍物密度值。

6.根据权利要求5所述的机器人规避障碍物的运动规划方法，其特征在于，所述S7步骤中，将最靠近障碍物的所述扇区边界记为右边界k_n；则左边界k_f满足：

k_f＝k_n+s_max；

其中，s_max表示最大扇区数；

所述S7步骤中，候选方向c满足公式：

7.根据权利要求6所述的机器人规避障碍物的运动规划方法，其特征在于，所述S8步骤中，所述代价函数g(c)为：

其中，λ₁和λ₂表示自定义常数，c为候选方向，k_l为目标方向，

是当前的所在扇区，

表示候选方向和目标方向相关的代价；

表示候选方向和机器人当前方向相关的代价。

8.根据权利要求7所述的机器人规避障碍物的运动规划方法，其特征在于，还包括一种循环跳出机制，设置触发条件，当***陷入死循环，***启动所述循环跳出机制，自动选择一个可行进的方向，跳出当前循环。

Images