清洁机器人的路径规划方法、装置、存储介质和设备与流程

1.本发明属于清洁机器人技术领域，具体地讲，涉及一种清洁机器人的路径规划方法、路径规划装置、计算机可读存储介质和计算机设备。


背景技术：

2.动态窗口法是清洁机器人运动路径规划中常用的局部路径算法。动态窗口法(dynamic window approach，简称dwa)的原理为在一个受限制的速度窗口中采样多组速度，并模拟这些速度在一定时间内的运动轨迹，然后通过一个评价函数对这些运动轨迹打分，根据评价函数的打分结果选出最优的速度以发送给清洁机器人的执行机构。此算法缺点为当清洁机器人的窗口与目标点朝向相反时，会由于陷入局部最优而使得清洁机器人实际运行轨迹的路径偏移率较高。


技术实现要素：

3.本发明解决的技术问题是：如何在路径规划过程中及时将清洁机器人从陷入局部最优状态中脱离出来，以提高路径的平滑度和减短路径长度。
4.本技术公开了一种清洁机器人的路径规划方法，所述路径规划方法包括：
5.获取速度限制窗口，确定所述速度限制窗口内的若干速度组合，所述速度组合包括线速度和角速度；
6.利用预定的速度评价函数对各个所述速度组合进行评价，以筛选出候选速度组合；
7.根据所述候选速度组合中的角速度以及清洁机器人与目标点的位置关系判断所述清洁机器人是否陷入局部最优；
8.若是则将所述候选速度组合中的线速度调整为零并保持所述候选速度组合中的角速度不变，作为最优速度组合；若否则直接将所述候选速度组合作为最优速度组合。
9.优选地，所述获取速度限制窗口的方法为：
10.根据清洁机器人的移动最大速度限制、移动最大加速度限制和安全速度限制确定所述速度限制窗口。
11.优选地，所述利用预定的速度评价函数对各个所述速度组合进行评价的方法包括：
12.根据所述速度组合预测得到运动轨迹；
13.根据所述运动轨迹确定方向角评价值和障碍物距离评价值，根据所述速度组合中的角速度大小确定角速度评价值；
14.根据所述方向角评价值、所述障碍物距离评价值和所述角速度评价值确定所述速度组合对应的速度评价函数值。
15.优选地，所述候选速度组合为所述若干速度组合中速度评价函数值最大的速度组合。
16.优选地，根据所述候选速度组合中的角速度以及清洁机器人与目标点的位置关系判断所述清洁机器人是否陷入局部最优的方法包括：
17.判断所述候选速度组合中的角速度大小的绝对值是否大于阈值以及判断所述目标点是否处于所述清洁机器人运动方向的相反方向；
18.若所述角速度大小的绝对值大于阈值且所述目标点处于所述清洁机器人运动方向的相反方向，判断所述清洁机器人陷入局部最优，否则判断所述清洁机器人未陷入局部最优。
19.优选地，所述判断所述目标点是否处于所述清洁机器人运动方向的相反方向的方法为：
20.根据所述运动轨迹确定预测点的运动方向和目标方向之间的夹角；
21.根据所述夹角的大小确定所述目标点是否处于所述清洁机器人运动方向的相反方向。
22.优选地，所述角速度评价值的计算公式为：
[0023][0024]
其中，angular_velocity(ω)表示角速度评价值，|ω|表示角速度大小的绝对值。
[0025]
本技术还公开了一种清洁机器人的路径规划装置，所述路径规划装置包括：
[0026]
速度窗口获取模块，用于获取速度限制窗口，确定所述速度限制窗口内的若干速度组合，所述速度组合包括线速度和角速度；
[0027]
速度评价函数模块，用于利用预定的速度评价函数对各个所述速度组合进行评价，以筛选出候选速度组合；
[0028]
局部最优判断模块，用于根据所述候选速度组合中的角速度以及清洁机器人与目标点的位置关系判断所述清洁机器人是否陷入局部最优；
[0029]
最优速度确定模块，用于当所述清洁机器人陷入局部最优时将所述候选速度组合中的线速度调整为零并保持所述候选速度组合中的角速度不变，作为最优速度组合；以及用于当所述清洁机器人未陷入局部最优时直接将所述候选速度组合作为最优速度组合。
[0030]
本技术还公开了一种计算机可读存储介质，所述计算机可读存储介质存储有清洁机器人的路径规划程序，所述清洁机器人的路径规划程序被处理器执行时实现上述的清洁机器人的路径规划方法。
[0031]
本技术还公开了一种计算机设备，所述计算机设备包括计算机可读存储介质、处理器和存储在所述计算机可读存储介质中的清洁机器人的路径规划程序，所述清洁机器人的路径规划程序被处理器执行时实现上述的清洁机器人的路径规划方法。
[0032]
本发明公开的一种清洁机器人的路径规划方法和路径规划装置，具有如下技术效果：
[0033]
在利用动态窗口法计算得到候选速度组合之后，增加了清洁机器人是否陷入局部最优状态的判断步骤，并在陷入局部最优状态时，则将候选速度组合中的线速度置零并保持角速度不变，此时清洁机器人会进行自转运动，以尽快摆脱局部最优状态，降低路径偏移率。
附图说明
[0034]
图1为本发明的实施例一的清洁机器人的路径规划方法的流程图；
[0035]
图2为本发明的实施例一中预测运动路径的示意图；
[0036]
图3为本发明的实施例一进行方向角计算的示意图；
[0037]
图4为本发明的实施例一的清洁机器人与目标点的位置关系示意图；
[0038]
图5为本发明的实施例一的测试环境图；
[0039]
图6为清洁机器人使用传统动态窗口法进行路径规划的测试路径图；
[0040]
图7为在本发明的实施例一的清洁机器人的路径规划方法的测试路径图；
[0041]
图8为本发明的实施例二的清洁机器人的路径规划装置的原理框图；
[0042]
图9为本发明的实施例四的计算机设备示意图。
具体实施方式
[0043]
为了使本发明的目的、技术方案及优点更加清楚明白，以下结合附图及实施例，对本发明进一步详细说明。应当理解，此处所描述的具体实施例仅仅用以解释本发明，并不用于限定本发明。
[0044]
在详细描述本技术的各个实施例之前，首先简单描述本技术的技术构思：现有技术中在采用动态窗口法(dynamic window approach，dwa)进行清洁机器人的路径规划时，往往未考虑预测得到的运动方向与目标方向存在较大差别甚至与目标点相反的情况，即清洁机器人会陷入局部最优，直接按照动态窗口法确定的速度去执行时，导致出现实际运行轨迹的路径偏移率较高的问题。为此，本技术提供清洁机器人的路径规划方法，在利用动态窗口法计算得到候选速度组合之后，判断此时的清洁机器人是否陷入了局部最优状态，若未陷入则直接将候选速度组合作为最优速度组合，按照该最优速度组合执行；若陷入了局部最优状态，则将候选速度组合中的线速度置零并保持角速度不变，此时清洁机器人会进行自转运动。由于在局部最优状态下，清洁机器人的运动方向为背离目标点的方向，此时如果继续按照该运动方向运动会使得清洁机器人更加地偏离目标点，通过将线速度置零进行自转运动，即实现掉头，以尽快摆脱局部最优状态。
[0045]
具体来说，如图1所示，本实施例一的清洁机器人的路径规划方法包括如下步骤：
[0046]
步骤s10：获取速度限制窗口，确定所述速度限制窗口内的若干速度组合，速度组合包括线速度和角速度；
[0047]
步骤s20：利用预定的速度评价函数对各个速度组合进行评价，以筛选出候选速度组合；
[0048]
步骤s30：根据候选速度组合中的角速度以及清洁机器人与目标点的位置关系判断清洁机器人是否陷入局部最优；
[0049]
步骤s41：若是则将候选速度组合中的线速度调整为零并保持候选速度组合中的角速度不变，作为最优速度组合；
[0050]
步骤s42：若否则直接将候选速度组合作为最优速度组合。
[0051]
具体地，步骤s10中的速度限制窗口vr是指考虑清洁机器人的实际运动限制后所得到的一个速度范围：
[0052]vr
＝{(νr,ωr)|ν1≤νr≤ν2,ω1≤ωr≤ω2}
[0053]
这个速度范围由多种因素决定，如清洁机器人的速度限制、清洁机器人驱动电机的加速度限制、清洁机器人的最小转动角度等。
[0054]
进一步地，步骤s10中获取速度限制窗口的具体方法如下：
[0055]
为了更加贴近清洁机器人的实际应用场景，主要考虑以下三种运动限制：清洁机器人移动的最大速度限制、清洁机器人驱动电机的最大加速度限制以及清洁机器人在遇到障碍物前能否及时停止移动。
[0056]
首先，关于清洁机器人的移动最大速度限制vs＝(νs,ωs)，由于清洁机器人移动的速度是不可能无限增加的，所以应该考虑其移动最大速度的限制范围，采用如下公式来表示：
[0057]vs
＝{(νs,ωs)|νs∈[ν
min
,ν
max
],ωs∈[ω
min
,ω
min
]}
[0058]
其中，一般最小线速度ν
min
取零，最小角速度ω
min
和最大角速度ω
min
互为相反数。但考虑到当线速度νs为零时，评价函数就无法发挥作用，也可以把最小线速度ν
min
设置为非零的正最小值。
[0059]
其次，由于清洁机器人的驱动电机的转矩有限，存在一个最大的加速度限制，因此可以假设清洁机器人当前速度为(νc,ωc)，在一个有限的时间周期δt内，清洁机器人的移动最大加速度的限制范围vd为：
[0060][0061]
其中，νa、νb、ωa、ωb分别表示为线速度最大减速度、线速度最大加速度、角速度最大减速度以及角速度最大加速度。
[0062]
最后，当清洁机器人检测到障碍物时，应留有一段刹车距离用于减速。当清洁机器人与最近障碍物的距离小于刹车距离时，机器人就会与障碍物发生碰撞，为此应设计一个安全停止速度，也就是使清洁机器人在遇到障碍物前能及时停止的速度。
[0063]
显然，这个安全停止速度应与清洁机器人与最近障碍物的距离成正比。当距离越小时，这个安全停止速度也应相应减小。安全速度限制范围va应该满足如下不等式：
[0064][0065]
其中，dist表示清洁机器人与最近障碍物的距离，va表示安全停止速度的线速度部分，ωa表示安全停止速度的角速度部分。
[0066]
在计算得到以上三种速度限制后，得到了三个速度限制范围，它们的交集就是在当前状态下可以取到的速度限制窗口vr：
[0067]vr
＝vs∩vd∩va。
[0068]
进一步地，步骤s10中确定所述速度限制窗口内的若干速度组合的具体方法如下：
[0069]
根据预先设置的速度增量，得到线速度序列和角速度序列，然后对线速度序列和角速度序列进行组合。其中，速度增量根据清洁机器人的运动精度要求确定。
[0070]
示例性地，假设计算得到的速度限制窗口为：νr∈[0,3],ωr∈[-20,20]
[0071]
预先设置的速度增量为δv＝0.01m/s,δω＝1
°
/s，则可以得到以下的线速度序列和角速度序列：
[0072]
v:[0,0.01,...,3.00],n＝101,ω:[-20
°
,-19
°
,...,20
°
],n＝41
[0073]
对以上线速度序列和角速度序列进行组合，可以得到101
×
41组速度组合。
[0074]
由此，在确定速度组合之后，便可以对各个速度组合预测得到运动轨迹，从而可以进行步骤s20。
[0075]
进一步地，步骤s20中利用预定的速度评价函数对各个速度组合进行评价的方法包括：
[0076]
根据所述速度组合预测得到运动轨迹；根据所述运动轨迹确定方向角评价值和障碍物距离评价值，根据所述速度组合中的角速度大小确定角速度评价值；根据所述方向角评价值、所述障碍物距离评价值和所述角速度评价值确定所述速度组合对应的速度评价函数值。
[0077]
具体地，如图2所示，步骤s20中根据速度组合预测得到运动轨迹的具体方法可以按以下方式进行：
[0078]
假设速度组合为(νi,ωi)，清洁机器人当前位置为(x0,y0,θ0,)，预测时间周期为t。
[0079]
首先设置一个时间增量δt，以得到时间序列[0,δt,2δt,3δt,...,t]。
[0080]
然后根据下述轨迹预测公式就可以得到预测的轨迹点(x,y,θ,)：
[0081]
x＝x0 v
i cosθ
·
δt,y＝y0 v
i sinθ
·
δt,θ＝θ0 ωi·
δt
[0082]
需要说明的是，此处的轨迹预测公式是根据近似的运动模型得到的。即当运动距离很小时，用直线去近似弧线所得到的运动模型。并且，在整个预测的时间周期t内，默认清洁机器人的速度不变。
[0083]
通过对时间序列[0,δt,2δt,3δt,...,t]内的每个值进行计算，就可以得到一系列的运动轨迹点了。
[0084]
最后对得到的每个运动轨迹点进行判断。假设某个运动轨迹点与障碍物碰撞，则应舍弃这个运动轨迹点以及后面的所有运动轨迹点，选择上一个运动轨迹点作为最终预测的运动轨迹点。由此，可以根据速度组合预测得到运动轨迹。
[0085]
具体地，步骤s20中根据运动轨迹确定方向角评价值的方法如下：
[0086]
如图3所示，方向角评价值包括用于评价机器人在给定角速度下运动的角度与目标角度之间的差值，计算公式如下。
[0087]
heading(vi,ωi)＝180
°‑
|target-curθ|
[0088]
其中，target表示目标角度，curθ表示预测的运动角度。
[0089]
根据计算公式可知，该差值越小，方向角评价值越高。
[0090]
具体地，步骤s20中根据运动轨迹确定障碍物距离评价值的方法如下：
[0091]
dist(vi,ωi)用于表示机器人当前位置与最近的障碍物之间的距离。如果轨迹上无障碍物，则设定一个常数作为障碍物距离评价值。当机器人与障碍物的距离越大，则该障碍物距离评价值应该越高。因此，可以直接用距离大小来确定障碍物距离评价值。另外值得注意的是，在计算距离时还应该考虑机器人的半径。
[0092]
当角速度不为零时，应该计算的距离是弧线长度，而不是长度。这个弧长的计算也比较简单，根据运动模型可知，当角速度和线速度不变时，机器人的运动轨迹为一个标准的圆形。该圆的半径为(角速度单位为弧度每秒)：r＝|v/ω|(ω≠0)
[0093]
设机器人当前的位姿为(x1,y1,θ1,)(这里并不是预测点的位姿)，则该圆的圆心坐标为：xc＝x-ωvsinθ，yc＝y ωvcosθ。然后通过余弦公式可以计算出角度最后应用弧长公式就可以得到弧长
[0094]
具体地，步骤s20中根据所述速度组合中的角速度大小确定角速度评价值的方法如下：
[0095]
角速度评价值的计算公式为：
[0096][0097]
其中，angular_velocity(ω)表示角速度评价值，|ω|表示角速度大小的绝对值。对于路径规划而言，显然角速度大小的绝对值越小越好。
[0098]
具体地，步骤s20中根据所述方向角评价值、所述障碍物距离评价值和所述角速度评价值确定所述速度组合对应的速度评价函数值的方法包括：
[0099]
首先，在计算得到方向角评价值、所述障碍物距离评价值和所述角速度评价值后，对其分别进行归一化处理。
[0100]
然后，按以下速度评价函数公式计算得到所述速度组合对应的速度评价函数值：
[0101]
g(vi,ωi)＝σ(α
·
heading(vi,ωi) β
·
dist(vi,ωi) γ
·
angular_velocity(ωi))
[0102]
上述公式中g(vi,ωi)表示速度评价函数值，α、β、γ表示归一化系数，将上述计算得到的方向角评价值、障碍物距离评价值和角速度评价值代入速度评价函数公式，就可以对当前的速度组合做出评价。遍历该速度限制窗口内的各组速度组合，将速度评价函数值最大的速度组合作为候选速度组合。
[0103]
进一步地，步骤s20中筛选出候选速度组合的方法为将所述若干速度组合中速度评价函数值最大的速度组合作为所述候选速度组合为。
[0104]
在步骤s30中，根据所述候选速度组合中的角速度以及清洁机器人与目标点的位置关系判断清洁机器人是否陷入局部最优的方法包括：判断所述候选速度组合中的角速度的绝对值是否大于阈值以及判断目标点是否处于清洁机器人运动方向的相反方向；若角速度大小的绝对值大于阈值且目标点处于清洁机器人运动方向的相反方向，确定清洁机器人陷入局部最优，否则清洁机器人未陷入局部最优。
[0105]
示例性地，阈值设置为45度，当角速度大小的绝对值大于45度时，则可认为清洁机器人大概率陷入了局部最优。
[0106]
示例性地，判断目标点是否处于清洁机器人运动方向的相反方向的方法为：根据运动轨迹确定预测点的运动方向和目标方向之间的夹角；根据夹角的大小确定目标点是否处于清洁机器人运动方向的相反方向。如图4的a所示，当预测点的运动方向和目标方向之间的夹角为锐角时，代表目标点处于清洁机器人运动方向的相同方向上；如图4的b所示，当预测点的运动方向和目标方向之间的夹角为钝角时，代表目标点处于清洁机器人运动方向的相反方向上。
[0107]
为了验证本实施例一的路径规划方法的有益效果，进行了对比测试，测试环境如图5所示，图6为清洁机器人使用传统动态窗口法进行路径规划的测试路径图，图7为本实施例一规划方法的测试路径图，两种规划方法下所走距离和转折弧度如下表所示。
[0108] 所走距离(m)转折弧度(rad)传统dwa规划4.02803289.904602
本实施例一规划方法3.64182472.624977
[0109]
根据上表可知，通过改进算法的评价函数，引入角速度评价函数，使得转折弧度减少了23.79％，使得路径长度减少10.6％。由此可见本实施例一规划方法不仅提高了算法的效率，而且降低了路径长度，提高了平滑度。
[0110]
如图8所示，本实施例二公开了清洁机器人的路径规划装置，该路径规划装置包括速度窗口获取模块100、速度评价函数模块200、局部最优判断模块300和最优速度确定模块400。其中，速度窗口获取模块100用于获取速度限制窗口，确定速度限制窗口内的若干速度组合，速度组合包括线速度和角速度。速度评价函数模块200用于利用预定的速度评价函数对各个速度组合进行评价，以筛选出候选速度组合。局部最优判断模块300用于根据候选速度组合中的角速度以及清洁机器人与目标点的位置关系判断清洁机器人是否陷入局部最优。最优速度确定模块400用于当清洁机器人陷入局部最优时将候选速度组合中的线速度调整为零并保持候选速度组合中的角速度不变，作为最优速度组合；以及用于当清洁机器人未陷入局部最优时直接将候选速度组合作为最优速度组合。
[0111]
具体地，速度窗口获取模块100还用于根据清洁机器人的移动最大速度限制、移动最大加速度限制和安全速度限制确定速度限制窗口。
[0112]
进一步地，速度评价函数模块200还用于：根据速度组合预测得到运动轨迹；根据运动轨迹确定方向角评价值和障碍物距离评价值，根据速度组合中的角速度大小确定角速度评价值，以及根据方向角评价值、障碍物距离评价值和角速度评价值确定速度组合对应的速度评价函数值。
[0113]
进一步地，局部最优判断模块300还用于：判断候选速度组合中的角速度大小的绝对值是否大于阈值以及判断目标点是否处于清洁机器人运动方向的相反方向；若角速度大小的绝对值大于阈值且目标点处于清洁机器人运动方向的相反方向，判断清洁机器人陷入局部最优，否则判断清洁机器人未陷入局部最优。
[0114]
示例性地，局部最优判断模块300还用于：根据所述运动轨迹确定预测点的运动方向和目标方向之间的夹角；根据所述夹角的大小确定所述目标点是否处于所述清洁机器人运动方向的相反方向。
[0115]
其中，速度窗口获取模块100、速度评价函数模块200、局部最优判断模块300和最优速度确定模块400的更加详细的工作过程可参考实施例一的相关步骤的描述，在此不进行赘述。
[0116]
本实施例三还公开了一种计算机可读存储介质，计算机可读存储介质存储有清洁机器人的路径规划程序，清洁机器人的路径规划程序被处理器执行时实现上述的清洁机器人的路径规划方法。
[0117]
本实施例四还公开了一种计算机设备，在硬件层面，如图9所示，该计算机设备包括处理器12、内部总线13、网络接口14、计算机可读存储介质11。处理器12从计算机可读存储介质中读取对应的计算机程序然后运行，在逻辑层面上形成请求处理装置。当然，除了软件实现方式之外，本说明书一个或多个实施例并不排除其他实现方式，比如逻辑器件抑或软硬件结合的方式等等，也就是说以下处理流程的执行主体并不限定于各个逻辑单元，也可以是硬件或逻辑器件。计算机可读存储介质11上存储有清洁机器人的路径规划程序，所述清洁机器人的路径规划程序被处理器执行时实现上述的清洁机器人的路径规划方法。
[0118]
计算机可读存储介质包括永久性和非永久性、可移动和非可移动媒体可以由任何方法或技术来实现信息存储。信息可以是计算机可读指令、数据结构、程序的模块或其他数据。计算机可读存储介质的例子包括，但不限于相变内存(pram)、静态随机存取存储器(sram)、动态随机存取存储器(dram)、其他类型的随机存取存储器(ram)、只读存储器(rom)、电可擦除可编程只读存储器(eeprom)、快闪记忆体或其他内存技术、只读光盘只读存储器(cd-rom)、数字多功能光盘(dvd)或其他光学存储、磁盒式磁带、磁盘存储、量子存储器、基于石墨烯的存储介质或其他磁性存储设备或任何其他非传输介质，可用于存储可以被计算设备访问的信息。
[0119]
上面对本发明的具体实施方式进行了详细描述，虽然已表示和描述了一些实施例，但本领域技术人员应该理解，在不脱离由权利要求及其等同物限定其范围的本发明的原理和精神的情况下，可以对这些实施例进行修改和完善，这些修改和完善也应在本发明的保护范围内。