CN113050646B - 一种用于室内移动机器人动态环境路径规划方法 - Google Patents

Abstract

本发明公开了一种动态环境下室内移动机器人局部路径规划方法，解决传统动态窗口算法(DWA)不能妥善处理动态障碍物的问题。该方法在传统局部路径规划上做了两点改进，改进策略旨在将动态障碍物的状态信息引入到DWA算法目标函数中，使得算法根据当前实际局部环境智能的调整规划路线；首先，利用障碍物的航向信息将动态障碍物分为需要避障处理的有威胁障碍物和不需要处理的无危胁障碍物，并将有威胁障碍物运动信息引入到DWA算法目标函数中，设计了速度组合评价因子和反应距离评价因子，速度组合评价因子帮助DWA选择出符合当前的线速度和角速度组合，反应距离评价因子负责挑选出最安全的避障路线，保证机器人在动态环境中的安全。

Description

一种用于室内移动机器人动态环境路径规划方法

技术领域

本发明属于室内移动机器人路径规划领域，具体涉及一种移动机器人动态环境路径规划方法。

背景技术

近年来，随着新兴技术的蓬勃发展，如人工智能，大数据，物联网，5G技术等，移动机器人被应用到了各行各业，其技术也不断被扩展，成为了21世纪衡量国家科技创新发展和高端制造业水平的一个重要指标。作为移动机器人中枢控制***的路径规划算法，直接决定着移动机器人的性能。DWA算法是目前移动机器人局部规划中使用最广泛的规划算法。传统DWA算法在遇到静态障碍物和相对行驶的动态障碍物时，往往表现良好避障路径也符合客观要求。但当动态障碍物与机器人夹角大于90°时，机器人会改变当前航向，朝向障碍物行进方向进行避障，导致避障路径不合理。出现这种情况的原因是DWA算法动态避障默认方式是将动态障碍物当作瞬时静态障碍物处理的，即在动态障碍物进入机器人视野的瞬间，算法采用静态避障的方式对动态障碍物进行路径规划。这种方式很明显的缺点是由于不考虑障碍物速度因素，对不同状态的障碍物，算法通常都会采用相同的避障方式。再者，传统DWA算法速度项的设计就是为了支持机器人的快速移动，因此在动态环境下，原始DWA每时每刻都会选择较高的避障速度。这两个问题对于机器人的动态环境避障来说，往往导致动态环境避障路径规划失败。

为了解决这个问题，国内外学者基于动态障碍物状态研究了一系列的方法，2018年Mingyang Guan等人研究了一种新的基于动态障碍物信息的DWA-CSC(DWA withCollision Suppression Cone)算法，可以使得机器人摆脱移动障碍物，不会出现与障碍物并驾齐驱的现象，但该算法并没有将动态障碍物进行速度分类，对于快速，慢速移动移动障碍物都进行固定形式的减速避障方式，没有充分利用到移动机器人的速度优势，出现机器人不必要的减速现象。

发明内容

本发明针对现有技术的不足，提供一种动态环境下室内移动机器人局部路径规划方法，解决传统动态窗口算法(DWA)不能妥善处理动态障碍物的问题。

为了达到上述目的，本发明的技术方案如下：

一种室内移动机器人动态环境路径规划方法，包括以下步骤：

步骤1，建立当前动态环境模型；

步骤2，依据移动机器人的物理限制和地图环境限制，计算出当前允许的的速度窗口V_r＝V_s∩V_a∩V_d；

步骤3，利用障碍物划分算法对机器人视野内的动态障碍物进行划分，得到需要避障的动态障碍物集合，保存运动状态信息；

步骤4：对速度空间内的速度按照时间分辨率为0.1s进行前向模拟模拟，得出候选速度轨迹；

步骤5，按照候选轨迹前向模拟终点及终点处移动机器人的位姿、状态计算航向得分，速度大小得分，障碍物距离得分；

步骤6：将步骤2中的动态障碍物信息集合与经过前向模拟后机器人的姿态速度信息，带入设计的速度组合评价因子中，挑选出符合规则的航向和速度；

步骤7：通过分析前向模拟轨迹终点与经过前向模拟时间后动态障碍物的位置关系，匹配合适的反应距离评价因子规则，得出反应距离评价得分；

步骤8：对步骤4、6、7中所有的评价得分进行归一化处理，挑选出综合得分最高的角速度与线速度作为当前环境下的运动输出。

与现有的技术相比，本发明的有益效果是：

本发明设计了速度组合评价因子，和反应距离评价因子，速度组合评价因子在机器人避障过程中实时参考障碍物速度和当前移动机器人速度，智能的在速度空间中选择最优的避障角度和避障线速度，解决了传统DWA和DWA-CSC算法避障方式固定的问题；反应距离评价因子，则根据候选速度前向模拟轨迹终点与障碍物航向的位置关系，匹配相应的反应距离评价规则，保证移动机器人在穿越移动障碍物航向时，距离移动障碍物足够远，解决动态避障的安全问题。

附图说明

图1为本发明的速度组合评价示意图；

图2为本发明的反应距离评价示意图；

图3为本发明的移动障碍物分类示意图；

图4为本发明在动态环境中的MATLAB仿真图；

图5为本发明的整体流程图。

具体实施方式

为了使本发明的目的、技术方案和优点更加清楚，下面将结合实施例对本发明作进一步地详细描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其它实施例，都属于本发明保护的范围。

目前传统DWA算法在动态环境中的弊端：1)增加路径距离；2)易陷入动态障碍物中，出现与动态障碍物并驾齐驱现象，不能保证移动机器人的安全行驶。DWA-CSC算法虽然将避障的动态障碍物信息纳入到速度组合选择的评价因子中，设计了候选速度轨迹方向抑制因子和机器人高速移动抑制因子，但存在的问题是：1)候选速度轨迹方向抑制因子通常会抑制与障碍物同航向的速度组合，导致避障航向选择方式固定；2)机器人高速移动抑制因子也会抑制高速移动的候选速度，在某种情况下会丧失机器人本身存在的速度优势。不能智能的决定动态障碍物避障方式。

本发明提供一种用于室内移动机器人动态环境路径规划方法，该方法在室内动态环境下局部路径规划DWA算法将障碍物的信息录入到评价函数中；针对传统DWA算法在动态环境避障***中出现的不能摆脱动态障碍物的问题，设计了动态障碍物划分算法及速度组合评价因子和反应距离评价因子；动态障碍物划分算法划分当前动态环境，修剪掉不需要避障的动态障碍物，减少不必要的影响，而两个评价因子携带当前动态障碍物状态信息按照预设规则对速度窗口内的候选速度进行评价，输出评分最高的线速度和角速度，完成动态环境路径规划任务。

如图5所示，本发明是一种移动机器人动态环境路径规划方法，具体描述如下：

(1)当前机器人当前室内环境模型，对移动障碍物进行位置，航向，速度检测，获取动态数据。

(2)依据移动机器人的物理限制和地图环境限制，计算出当前允许的速度窗口：速度空间分别受到最大最小速度约束、动力学约束以及安全约束。

最大最小速度约束：机器人的运动速度分为线速度v和角速度w，无论是v还是w都分别受到最大最小速度的约束。令V_S表示机器人的最大最小速度空间，即：

V_s＝{(v,w|v∈[v_min,v_max],w∈[w_min,w_max])}

式中，v_max，v_min分别是机器人的最大、最小速度，w_max，w_min分别是机器人的最大、最小角速度。

动力学约束：考虑到电机的加速性能有限，移动机器人受电机性能的影响，由于电机力矩有限,存在最大的加減速限制,因此移动机器人轨迹前向模拟的周期内,存在一个动态窗口,在该窗口内的速度是机器人能够实际达到的速度:

式中，v_a，w_a分别是机器人当前实际的线速度和角速度，

是机器人线速度的最大加速度，

是机器人角速度的最大加速度，Δt是前向模拟时间。

安全约束：为了能够使机器人在到达障碍物之前减速到0，避免与障碍物碰撞，保证机器人的安全，速度空间受到制动距离的约束。令V_a表示机器人安全的速度空间，即：

式中，dist(v,w)是速度(v,w)对应的轨迹与最近障碍物的距离。

综合上述的约束条件，令V_r表示机器人的速度空间，则V_r可表示为：

V_r＝V_s∩V_a∩V_d

(3)利用障碍物划分算法对机器人视野内的动态障碍物进行划分，得到需要避障的动态障碍物集合，保存运动状态信息。首先延长当前机器人航向向量直线和移动障碍物航向向量直线，定义两直线的交点为预测碰撞点，如果满足公式：

则此障碍物就被定义为存在威胁障碍物，相反则定义为无威胁障碍物，如图3所示，ob₁为有威胁障碍物，ob₂为无威胁障碍物。

(4)按照候选轨迹前向模拟终点及终点处移动机器人的位姿、状态计算航向得分，速度大小得分，障碍物距离得分：

G(v,w)＝σ(α·heading(v,w)+β·dist(v,w)+γ·velocity(v,w))

使得G(v,w)值最大的(v,w)即为最优速度，其中heading＝π-θ，θ表示机器人航向与目标线(机器人位置同目标点连线称为目标线)之间的夹角，heading用于衡量机器人对目标的方向性，当机器人运动方向完全指向目标点时，其值最大；dist表示预轨迹中距离障碍物的最小距离；v表示圆弧轨迹中t+1时刻的线速度，优化结果将尽量选择动态窗口V_r中线速度大的值；α，β，γ为权值参数。为避免其中一项值占比过高，在参与轨迹评优之前对其三项做归一化处理，如下式所示。

(5)速度组合评价因子：

完成对动态障碍物分类后，进行对有威胁的动态障碍物进行避障处理。接下来就需要对有威胁障碍物进行避障，设置避障策略如下，参见图1(b),若移动物体速度小，机器人须选择以较高的速度从移动物体前方绕行；参见图1(a),若移动物体速度大，机器人须选择以低速从移动物体后面绕行。保证机器人在绝对安全的情况下在较短时间内完成路径规划任务，速度组合评价因子项如下式所示。

式中cos(θ)用来表示障碍物航向与第j条前向模拟轨迹终点处航向的夹角，

表示第i个障碍物的航向向量，

表示第j条速度组合模拟轨迹终点处的航向向量，n为当前视野中障碍物的个数。η为速度航向的权重值，它的取值不应该是固定的，机器人与障碍物的距离越近，就越要偏重选取针对该障碍物的动态避障航向和速度，因此，

η_i＝d_max-d_i

d_max为机器人最大视野范围，即传感器可检测的最大范围。d_i为机器人位置到第i个动态障碍物位置的距离。若障碍物与机器人距离越近，该障碍物的威胁等级就越高，权重就越大。如图4(a)、图4(b)中2号障碍物的权重就要比1号障碍物的权重高一些，避障算法就主要以2号125度航向障碍物为主，到图4(c),图(d)中就会以1号5度航向障碍物为主。

考虑若机器人的速度大于障碍物的速度，机器人就可以从障碍物前面进行避障，因此航向的夹角θ必须为锐角，否则本次的速度组合评价因子结果是个负值，影响总体得分，如避障结果图4(c)所示，对于2号障碍物，机器人选择从障碍物前面进行避障。

若速度窗口中候选速度大小小于移动障碍物速度，则航向的夹角θ必须为钝角，才能保证速度组合评价因子为正，如避障结果图4(c)所示，对于1号障碍物，机器人选择从障碍物后面进行避障。算法避障示意图如图1所示。

(6)反应距离评价因子：

反应距离指的是机器人对移动障碍物避障时，与移动障碍物之间的相对避障距离。机器人通过控制***模拟候选速度组(v_j,ω_j)，得到模拟时间t内的路径轨迹及其终点坐标，则该终点位置与t时刻后障碍物的位置之间距离在障碍物航线上的投影就是该组速度的反应距离，前向模拟时间t一般为3到4秒。相对于移动障碍物航向直线，可以将反应距离的计算分为两类，一类是为穿越轨迹反应距离，计算点为p_through，这种轨迹通常表示此次的避障规划已经完成了；另一类是未穿越轨迹反应距离，计算点为p_shadow，表示本次规划的避障轨迹还没有完成，属于对该障碍物产生避障行为的一部分，这时应该考虑为下一阶段避障行为做准备。反应距离评价示意图如图2所示.

穿越形式的轨迹，就是本次规划的速度组，已完成了对此动态障碍物的避障任务。在求反应距离时，不能带入其终点坐标，而是代入其与障碍物航线的交点坐标，因此做如下假设，在模拟时间t，时间分辨率为dt的前向模拟轨迹中，第n步到达了穿越点p_through，则预测障碍物位置p'_obi应为以下计算：

r为膨胀化后障碍物半径。计算障碍物未来位置时，应利用其边缘坐标点，不能使用其圆心坐标，因此减去障碍物膨胀化半径。

对于未穿越形式轨迹，顾名思义就是轨迹的终点仍处于障碍物航线的下方，并没有完成对此障碍物的避障行为，因此就必须为它下一步穿越障碍物航线的规划做准备。首先根据几何运算求出未穿越轨迹终点在当前障碍物航线上的投影点p_shadow，然后求未穿越轨迹终点到达障碍物航线的距离d_s，v为前向模拟轨迹终点处机器人线速度在垂直于障碍物航向直线方向上的分速度。则p'_obi应由下式计算：

将预测障碍物位置p'_obi带入公式，求解反应距离评价因子：

κ_i与速度航向评价函数权重η相同，越靠近动态障碍物，其权重就应越大。此外，由于动态避障会导致机器人航向会短暂偏离全局规划终点，因此若还是采用原DWA的航向评价权重，会导致机器人一直朝向全局终点行驶，动态避障算法有可能会失效。因此，本发明采用在对动态障碍物避障期间，给全局航向评价因子，添加自适应权重α'。综上，本文针对动态障碍物的避障算法如下式所示：

H(v,ω,v_obi,h_obi)＝α'·heading(v,ω)+β·dist(v,ω)+γ·velocity(v,ω)+dis_hv(v_obs,h_obs,v,w)+dis_fp(v,ω)

式中β为安全制动距离评价权重，γ为线速度评价权重，ε为速度组合评价权重，ρ为反应距离评价权重。自适应航行权重因子α'计算公式如下式所示：

α_max为设置的最大航向评价权重系数。

动态环境避障路线如图4(e)，图4(f)所示，可以看出本发明算法在动态规划中不仅可以完成对静态障碍物的避障，也可以针对不同运动状态的动态障碍物智能采取不同的避障策略。

以上应用了具体个例对本发明进行阐述，只是用于帮助理解本发明，并不用以限制本发明。任何熟悉该技术的人在本发明所揭露的技术范围内的局部修改或替换，都应涵盖在本发明的包含范围之内。

Claims (4)

1.一种用于室内移动机器人动态环境路径规划方法，具体包括如下步骤：

步骤1，建立当前动态环境模型；

步骤2，依据移动机器人的物理限制和地图环境限制，计算出当前允许的速度窗；

步骤3，利用障碍物划分算法对机器人视野内的动态障碍物进行划分，得到需要避障的动态障碍物集合，保存运动状态信息；

步骤4，对速度窗口内的每一组候选速度进行前向模拟并计算航向得分，速度大小得分，障碍物距离得分；

步骤5：对速度空间内的速度按照时间分辨率为0.1s进行前向模拟，得出候选速度轨迹；

步骤6：将步骤3中的动态障碍物集合与经过前向模拟后机器人的姿态速度信息，带入设计的速度组合评价因子中，挑选出符合规则的航向和速度；

步骤7：通过分析前向模拟轨迹终点与经过前向模拟后动态障碍物的位置关系，匹配合适的反应距离评价因子规则，得出反应距离评价得分；

步骤8：对步骤4、6、7中所有的评价得分进行归一化处理，挑选出综合得分最高的角速度与线速度作为当前环境下的运动输出。

2.如权利要求1所述用于室内移动机器人动态环境路径规划方法，其特征在于，所述步骤3的动态障碍物划分算法，根据动态障碍物航向与移动机器人当前航向所形成的向量关系进行判断，有威胁障碍物向量关系描述为

其中norm()函数用来求解向量的单位向量，

为障碍物航向的方向向量，

为障碍物位置到预碰撞点的位置。

3.如权利要求1或2所述用于室内移动机器人动态环境路径规划方法，其特征在于，所述的步骤6中，速度组合评价因子公式如下：

η_i表示针对第i个移动障碍物的速度组合评价因子，由机器人与移动障碍物之间的距离自适应决定；d_max为传感器最大检测距离；v_obs,h_obs分别为移动障碍物的速度和航向，v_j,w_j分别为机器人第j个速度组合中的线速度和角速度；

η_i＝d_max-d_i

d_i为机器人位置到第i个动态障碍物位置的距离cos(θ)计算方式如下式所示：

cos(θ)用来表示障碍物航向与第j条前向模拟轨迹终点处航向的夹角，

表示第i个障碍物的航向向量，

表示第j条速度组合模拟轨迹终点处的航向向量。

4.如权利要求3所述用于室内移动机器人动态环境路径规划方法，其特征在于，所述步骤7中反应距离评价因子指的是机器人对移动障碍物避障时，在避障瞬间与移动障碍物之间的距离，按照模拟轨迹终点与动态障碍物此时的位置关系可以将反应距离的计算规则分为两类，一类是为穿越轨迹反应距离，计算点为p_through，这种轨迹表示此次的避障规划已经完成了；另一类是未穿越轨迹反应距离，计算点为p_shadow，反应距离评价如下所示：

式中κ_i与η_i计算方式一样，表示第i个动态障碍物的权重，由动态障碍物与机器人之间的距离自适应决定；

穿越形式的轨迹，p'_obi为：

式中dt为前向模拟过程中的时间分辨率，n表示第n步前向模拟中机器人到达了穿越点，r为障碍物的膨胀化半径；p_obi为第i个障碍物当前时刻的真实位置；计算障碍物未来位置时，应利用其边缘坐标点，不能使用其圆心坐标，因此减去障碍物膨胀化半径；

未穿越形式轨迹，则p'_obi为：

式中d_s为未穿越前向模拟轨迹终点到障碍物航向的直线距离，v为前向模拟轨迹终点处机器人线速度在垂直于障碍物航向直线方向上的分速度。

Images