CN112904855A - 基于改进动态窗口的跟随机器人局部路径规划方法 - Google Patents

Abstract

本发明公开一种基于改进动态窗口的跟随机器人局部路径规划方法，应用于机器人领域，针对相对复杂的环境中，现有的跟随机器人局部路径规划低效的问题，本发明采用改进DWA算法作为路径规划的主要算法，一方面与传统的机器人路径规划相比，DWA算法更简洁高效，反应速度更快；另一方面，本发明的方法通过对DWA算法评分规则的改进，使其更适应机器人执行跟随任务的特点。

Description

基于改进动态窗口的跟随机器人局部路径规划方法

技术领域

本发明属于机器人领域，特别涉及一种跟随机器人路径规划技术。

背景技术

动态窗口法，即DWA(dynamic window approach)算法，是一种通过对机器人线速度与角速度多次采样，并优选出合适的速度进行执行的方法。DWA算法主要是对采样的速度进行模拟，求出速度对应的模拟轨迹，并依据一定的规则对模拟出的每一条轨迹进行打分，根据模拟轨迹分数进行优选。DWA算法有诸多优点，首先DWA算法的主要输入是机器人的速度，即该算法主要与机器人的运动动力学相关。其次，DWA考虑了机器人的制动性能，在对机器人速度要求较高的场景，这一考虑十分重要。最后是该算法本身的复杂度不高，计算速度和反应速度较快。目前，DWA算法主要用于与A*算法等全局路径规划算法结合，实现机器人在已知地图中的定位导航。

对于跟随机器人，其实时性与安全性是十分重要的性能指标，一个良好的路径规划方法可以极大地提升这两种指标。如果使用传统的机器人定位导航方法，即全局路径规划+ 局部路径规划的方式，由于跟随机器人的跟随目标一般都不处于静止状态，因此，这种方式会随着目标的移动不断的更新全局路径，性能很差。同时，也因为目标并不处于静止状态，所以机器人在遇到对称性问题时的决策也会影响机器人的效率。

如图1所示，机器人无论是按照路径A移动或按照路径B移动，都可安全的到达目标点，但是，当目标并未处于静止状态时，机器人沿这两条路径开始移动就会导致不同的结果。

发明内容

为解决上述技术问题，本发明提出一种基于改进动态窗口的跟随机器人局部路径规划方法，使跟随机器人更适应任务环境，提升执行跟随任务的效率。

本发明采用的技术方案为：基于改进动态窗口的跟随机器人局部路径规划方法，包括：

S1、根据机器人的运动模型以及当前机器人的运动状态确定用于下一步的速度空间；

S2、基于该速度空间，在设定分辨率下进行采样，得到若干机器人的估计轨迹；

S3、基于机器人的朝向、障碍物、速度对步骤S2得到的若干估计轨迹进行评分，选取评分最高的估计轨迹提供给机器人的运动机构执行。

步骤S1具体为：

S11、确定机器人的速度模型；

S12、获取机器人当前的运动状态；

S13、根据机器人当前的运动状态和步骤S11确定的机器人速度模型，计算出将来一段时间内的速度窗口。

步骤S2具体包括以下分步骤：

S21、根据确定的速度采样率，计算路径的估计速度；

S22、根据路径的估计速度与机器人当前的位姿，得到机器人在采样时间内的估计位姿变化；

S23、根据估计位姿更新运送状态，得到估计轨迹。

本发明的有益效果：本发明的方法仅使用改进DWA算法作为路径规划的主要算法，一方面与传统的机器人路径规划相比，DWA算法更简洁高效，反应速度更快；另一方面，本发明的方法通过对DWA算法评分规则的改进，使其更适应机器人执行跟随任务的特点。总之，本发明的方法可以作为跟随机器人的路径规划实施方案。

附图说明

图1为对称性问题说明示意图；

图2为本发明的系统框图；

图3为本发明实施例提供的机器人运动角度示意图；

图4为本发明实施例提供的同边同向与同边异向示意图；

图5为本发明实施例提供的原始DWA方法路径规划结果；

图6为本发明实施例提供的改进DWA路径规划结果。

图7为使用本发明方法中的改进DWA算法作为路径规划方法执行行人跟随任务的实验结果。

具体实施方式

为便于本领域技术人员理解本发明的技术内容，下面结合附图对本发明内容进一步阐释。

如图2所示，本发明的方法主要包含三个部分，分别为速度空间确定部分、速度采样和路径估计部分，以及路径评分和优选部分。速度空间确定部分的主要功能是根据机器人的运动模型以及当前机器人的运动状态确定用于下一步的速度空间，即确定窗口。速度采样和路径估计部分的主要功能是根据上一步获取的速度窗口，根据一定的分辨率进行采样，并根据每组速度采样估算机器人如果按照该速度移动会出现的路径。路径评分和优选部分的主要功能是根据上一步获取的若干路径，结合本发明方法的评分规则，逐一进行评分计算，在获取全部路径评分后，找出评分最优的路径，机器人将按照该路径对应的速度组合进行运动。

1、速度空间的确定

机器人的速度模型是在机器人设计的过程中就已经确认的，假设机器人的设计速度模型为：

K＝[v_max ω_max a β]

其中，K表示该机器人的速度模型，包括机器人运行的最大速度v_max、最大角速度ω_max、以及机器人加速度a、角加速度β。

机器人当前实时的运动状态可能由机器人的IMU(Inertial Measurement Unit，惯性测量单元)或其他传感器获取，主要包括以下信息：

T＝[x y θ v ω]

T表示机器人的运动状态，其中主要包含机器人当前所在位置和朝向角[x，y，θ]，以及机器人的速度状态[v，ω](即运动速度)。

在获得了上述信息后，首先我们可以得到机器人在实时运动状态下，受加速度影响，在未来一段时间内(本实施例指采样时间d_t内)，机器人可能的速度范围值：

V_d＝[v-a*d_tv+a*d_tω-β*dtω+β*d_t]

其中，机器人实时线速度与角速度v和ω由机器人运动状态T获取，线加速度与角加速度a和β由机器人运动模型K获取。当然，速度范围值是根据机器人运动模型计算的理想状态下可能的速度，但是这个速度不能超过机器人运动模型设置的极限值，由机器人运动模型K可以得出极限值VS。

V_s＝[0v_max-ω_maxω_max]

设置机器人无法进行后退动作，因此机器人的最小线速度就为0，所以机器人的速度窗口应为Vr：

Vr＝[max((v-a*d_t)，0)min((v+a*d_t)，v_max)max((ω-β*dt)， -ω_max)min((ω+β*d_t)，ω_max)]

＝[v_rmin v_rmax ω_rmin ω_rmax]

2、速度采样和路径估计

由于机器人的速度空间包括两种速度，分别为线速度和角速度，因此，在速度采样的时候需要分别采样，并逐一计算。假设采样的线速度分辨率为d_v、角速度分辨率为d_ω，那么用于路径估计的速度可由下式表示：

分辨率大小的设置是处理器计算性能和算法效果权衡的结果，本实施例中d_v、d_ω分别取值为0.01m/s、57.2958rad/s；

在获得了估计速度[v_e，ω_e]后，结合当前位姿[x，y，θ]，即可获得机器人在采样时间内的估计位姿变化：

其中，H是经过采样时间d_t后的轨迹变化。

计算估计轨迹的过程如图3所示，设定时间标记t＝0，根据计算得到的位姿变化H更新机器人当前的运动状态T＝T+[H 0 0]，将当前位置存入估计估计数组，并更新时间标记t＝t+d_t，若t小于d_esti，则返回计算位姿变化H，否则得到轨迹估计数组。

其中，d_t是轨迹估计采样时间，d_esti是轨迹估计的总时长，d_t和d_esti的取值都和处理器计算性能相关，本实施例中d_t和d_esti分别取值为0.1s、0.5s。得到的轨迹估计数组将用于路径评分和优选模块进行筛选。

3、路径评分和优选

本部分的作用是对上述估计轨迹进行评分，评分主要有三部分组成：

α＝j*α_h+k*αs+i*α_v

其中，α为总评分，α_h为机器人的朝向评分，α_s为机器人的障碍物评分，α_v为机器人的速度评分，j，k，i分别为三个评分的权重，本实施例中分别取值为0.05、0.2、0.1。

机器人的朝向评分用来描述机器人的朝向和目标的差距，对于静止目标来说，我们期望机器人最好完全朝向目标移动，然而，对于动态目标来说，这种方法很有可能使机器人向一个远离目标的方向运动。作为一种改进方式，使用如下方法计算朝向评分：

首先，我们要对目标的运动方向进行估计，即获取目标运动方向是向机器人朝向的右侧还是左侧。如图4所示，具体实现方法是计算当前时刻和上一时刻目标位置与机器人当前时刻位置的夹角θ_t和θ_t-1，并进行比较，如果θ_t＞θ_t-1，则目标向右运动，反之，目标则向左运动。

其次，计算朝向评分：

α_h＝(180-abs(θ-θ_t))*K_h

其中，θ为机器人朝向的角度值，K_h为目标运动方向权重，该权重的值与目标位置和目标运动方向有关，分为同边同向和同边异向，如图5所示，K_h的取值已在表1中给出。

障碍物权重值由激光雷达数据获取，即，根据当前时刻激光雷达获取的周围障碍物信息计算生成的预估机器人轨迹距离障碍物的最小距离，具体步骤如下：

首先，根据激光雷达的障碍物数组s_i(i＝1，2，...，720)，计算全局坐标下每个点的直角坐标(x_i，y_i)(i＝1，2，...，720)，结合根据窗口采样速度预估的机器人位姿(x_p，y_p)，获得障碍物权重：

α_s＝min(norm((x_p，y_p)，(x_i，y_i))-d_safe(i＝1，2，...，720)

其中，norm取两点的平方和的平方根，即获取两点距离，d_safe为人为设置的安全距离参数，其具体取值与机器人外壳和期望的避障距离相关。

速度评分的主要依据为该路径评估的速度绝对值大小，即方法期望选出速度尽量快、效率尽量高的路径，具体方法如下：

a_v＝abs(v)

结合上述评分计算方式，在所有窗口采样速度计算完成后，选取评分最高的一条估计轨迹，提供给机器人的运动机构执行。

实验验证

方法使用matlab仿真平台进行验证，实验所用的机器人运动模型等参数如下表所示，跟随目标按正弦曲线路径变速移动。

表1实验仿真参数

作为对照试验，使用传统DWA局部路径规划算法作为跟随机器人的路径规划算法进行实验，实验结果如图6所示。由实验结果可知，使用传统方法进行路径规划时，机器人会做很多不必要的运动，表现的相对来说比较激进，虽然可以保证任务可以顺利完成，但是在相对复杂的环境中，这种方法显得较为低效。

使用本发明方法中的改进DWA算法作为路径规划方法执行行人跟随任务的实验结果如图7所示。由实验结果可知，使用本发明方法作为路径规划算法使机器人在执行跟随任务时轨迹与目标轨迹更为贴近，运行稳定，且更为高效。

本领域的普通技术人员将会意识到，这里所述的实施例是为了帮助读者理解本发明的原理，应被理解为本发明的保护范围并不局限于这样的特别陈述和实施例。对于本领域的技术人员来说，本发明可以有各种更改和变化。凡在本发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的权利要求范围之内。

Claims (7)

1.基于改进动态窗口的跟随机器人局部路径规划方法，其特征在于，包括：

S1、根据机器人的运动模型以及当前机器人的运动状态确定用于下一步的速度空间；

S2、基于该速度空间，在设定分辨率下进行采样，得到若干机器人的估计轨迹；

S3、基于机器人的朝向、障碍物、速度对步骤S2得到的若干估计轨迹进行评分，选取评分最高的估计轨迹提供给机器人的运动机构执行。

2.根据权利要求1所述的基于改进动态窗口的跟随机器人局部路径规划方法，其特征在于，步骤S1具体为：

S11、确定机器人的速度模型：

S12、获取机器人当前的运动状态和位姿：

S13、根据机器人当前的运动状态和步骤S11确定的机器人速度模型，计算出将来一段时间内的速度窗口。

3.根据权利要求1所述的基于改进动态窗口的跟随机器人局部路径规划方法，其特征在于，步骤S2具体包括以下分步骤：

S21、根据确定的速度采样率，计算路径的估计速度；

S22、根据路径的估计速度与机器人当前的位姿，得到机器人在采样时间内的估计位姿变化；

S23、根据估计位姿更新运送状态，得到估计轨迹。

4.根据权利要求3所述的基于改进动态窗口的跟随机器人局部路径规划方法，其特征在于，步骤S23具体为：判断当前标记实际是否小于设定的轨迹估计的总时长，若是则返回步骤S22；否则根据估计位姿更新运动状态，得到估计轨迹。

5.根据权利要求4所述的基于改进动态窗口的跟随机器人局部路径规划方法，其特征在于，更新后的运动状态根据上一运动状态与位姿估计叠加得到。

6.根据权利要求5所述的基于改进动态窗口的跟随机器人局部路径规划方法，其特征在于，机器人的朝向评分表达式为：

α_h＝(180-abs(θ-θ_t))*K_h

其中，α_h表示机器人的朝向评分结果，θ为机器人朝向的角度值，K_h为目标运动方向权重，θ_t表示当前时刻目标位置与当前时刻机器人位置的夹角；abs( )表示求绝对值。

7.根据权利要求6所述的基于改进动态窗口的跟随机器人局部路径规划方法，其特征在于，机器人的障碍物评分表达式为：

α_s＝min(norm((x_p,y_p),(x_i,y_i))-d_safe

其中，α_s表示机器人的障碍物评分结果，norm取两点的平方和的平方根，d_safe为设置的安全距离参数，(x_p,y_p)表示全局坐标第p个点的直角坐标，(x_i,y_i)表示全局坐标第i个点的直角坐标。

Images