一种基于动态窗口法的主动避障方法与流程

本发明主要应用于机器人导航技术领域，具体涉及一种机器人的主动避障方法。

背景技术：

当前，机器人导航中的路径规划主要有两部分，全局路径规划和局部路径规划。在实际使用过程中这两部分配合使用以达到更好的导航效果。全局路径规划：在已经构建完成的地图上规划处一条从起到终点的可行的无障碍轨迹。局部路径规划：根据传感器的消息和全局路径规划得到的路径，实时确定机器人的运动轨迹。局部路径规划算法主要作用是在前进过程中避开障碍物，使机器人顺利到达目标点。

现有的动态窗口法(dynamicwindowapproach)主要是避开与机器人距离稍远的静态障碍物，当机器人距离障碍物过近时会造成机器人认为自身已经撞上障碍物导致导航过程的失败。即，当前的局部路径规划算法存在的技术问题主要体现在：(1)在避障方面距离障碍物过近导致避障失败；(2)在避障的过程中无法主动避开障碍物。

技术实现要素：

本发明的发明目的在于：针对现有的局部路径规划避障过程中距离障碍物过近时无法有效避开障碍物，出现局部路径规划失败的技术问题，提供一种基于动态窗口法的主动避障方法。

步骤s1、采集机器人的当前状态参数，所述状态参数包括机器人的位姿、全局路径、距机器人当前位置距离最近的障碍物的位置、机器人能执行的最大速度、最大加速度、最大转向速度、最大转向加速度；

步骤s2、生成多条模拟路径：

基于预设的模拟路径条数n，预设的模拟运行时长t，结合机器人的最大速度、最大加速度、最大转向速度、最大转向加速度，模拟出n条可移动的路径；

其中模拟时长t小于全局路径所花的时长；

步骤s3、对每条模拟路径，计算与全局路径最近点的均值，记为pdist；

计算每条模拟路径的终点到机器人的目标位置的距离，记为gdist

计算每条模拟路径的终点到距机器人当前位置距离最近的障碍物之间的距离，记为dobs；

步骤s4、基于每条模拟路径的障碍物距离dobs，分别为每条模拟路径设置对应的安全距离dsafe；

所述安全距离dsafe表示机器人在模拟时长内按照最大速度前进不会撞到障碍物的距离阈值；

基于每条模拟路径的距离dobs与安全距离dsafe的大小关系，设置每条模拟路径的评分cobs：

若障碍物距离dobs大于安全距离dsafe，则不计分，即cobs＝0；

若障碍物距离dobs为0，则舍去该模拟路径；

若障碍物距离dobs小于或等于安全距离dsafe，则根据障碍物距离dobs的反比指数计算评分cobs；

步骤s5、对步骤s4中保留得到的各条模拟路径，通过评价函数c＝α×pdist+β×gdist+γ×cobs分别计算各条模拟路径的度量值c，并将度量值c最小的模拟路径作为机器人当前运行路径；其中α，β，γ为pdist、gdist、cobs的预设权重；

步骤s6、设置机器人在当前运行路径上的运行速度：

设置运行路径的碰撞距离docc，其中碰撞距离docc小于运行路径对应的安全距离dsafe；

判断障碍物距离dobs是否大于碰撞距离docc，若是，则设置机器人在当前运行路径上的运行速度为机器人下一时刻的运行速度；否则，设置其运行速度为负值，即向机器人发出后退指令；例如选取机器人下一时刻的运行速度负值作为机器人在当前运行路径上的运行速度；

机器人基于设置的运行速度在当前运行路径上运行，并基于当前机器人的当前位置判断机器人是否到达其目标位置，若是，则结束；否则继续执行步骤s1。

综上所述，由于采用了上述技术方案，本发明的有益效果是：机器人在距离障碍物过近时能主动倒退避开障碍物，继续向目标点前进；与现有的局部路径规划避障处理方式中只计算障碍物的距离，从而当与障碍物的距离过近时没有应对方法，导致机器人避障效果不佳的处理方式相比，本发明在距离障碍物的距离较远时和传统处理方式相近；但在距离障碍物距离较近时能够根据障碍物的距离合理控制机器人主动避开障碍物。

附图说明

图1为具体实施方式的主动避障处理流程图。

具体实施方式

为使本发明的目的、技术方案和优点更加清楚，下面结合实施方式和附图，对本发明作进一步地详细描述。

由于现有的方法在距离障碍物过近时会停下，如果障碍物不移开，机器人无法有效避开障碍物。为此，本发明提供了一种主动避障方法。

本发明基于动态窗口法对模拟路径的评分标准，在模拟路径时根据障碍物的距离确定路径的评分，寻找与机器人最近的障碍物距离，当与障碍物的距离过小时(小于预设距离阈值)，向机器人的驱动处理单元发出后退指令，从而控制机器人执行后退命令，通常可设置后退的距离为一个机器人的直径(机器人在地面的投影的外接圆的直径)；再重新规划到目标点的路径。避免机器人距离障碍物过近导致导航失败的情况，并且能够主动避开障碍物，使机器人顺利快速地到达目标点。从而提高机器人距离障碍物过近时避障的效果。

参见图1，本发明的具体实现步骤如下：

步骤s1、采集机器人的当前状态参数，所述状态参数包括机器人的位姿、全局路径(从起始位置到目标位置的全局路径)、传感器数据(传感器数据包括激光测得的距离数据，主要距机器人当前位置距离最近的障碍物的位置信息)、机器人能执行的最大速度、最大加速度、最大转向速度、最大转向加速度和机器人在地面的投影的外接圆的半径r；

步骤s2、生成多条模拟路径：

基于预设的模拟路径条数n，预设的模拟运行时长t，结合步骤s1中采集的状态参数中的速度信息(最大速度、最大加速度、最大转向速度、最大转向加速度)，模拟出n条可移动的路径；

其中模拟时长t小于全局路径所花的时长(即按照机器人当前的移动速度或最大移动速度，走完全局路径所花的时长)，本具体实施方式中，设置模拟时长(也可定义为模拟时间)为全局路径所花时长的一半。

步骤s3、对于步骤s2中模拟出来的每条路径，计算与全局路径最近点的均值，记为pdist，计算每条模拟路径的终点到机器人的目标位置(全局路径的终点)的距离，记为gdist；以及计算每条模拟路径的终点到距机器人当前位置距离最近的障碍物之间的距离，记为dobs；基于障碍物距离dobs通过步骤s4计算每条路径的评分cobs；

步骤s4、对步骤s3中得到的障碍物距离dobs，设置安全距离dsafe，即表示机器人在模拟时间内按照最大速度前进不会撞到障碍物的距离阈值。本具体实施方式中，根据机器人外切圆半径r，设置安全距离dsafe，其中，r＜dsafe≤k×r,k∈(1,3]。

基于每条模拟路径的距离dobs与安全距离dsafe的大小关系，设置每条模拟路径的评分cobs：

如果障碍物距离dobs大于安全距离dsafe，则不计分，即cobs＝0；

如果障碍物距离dobs为0，则舍去该模拟路径；

如果障碍物距离dobs小于或等于安全距离dsafe，则根据障碍物距离dobs的反比指数计算评分cobs。

本具体实施方式中，当dobs或等于dsafe时，其对应的评分cobs为：

步骤s5、对步骤s4中保留得到的各条模拟路径，通过评价函数c＝α×pdist+β×gdist+γ×cobs分别计算模拟路径的度量值c，将该度量值作为每条模拟路径的代价，其中α，β，γ为pdist、gdist、cobs的权重；优选的，其取值范围设置为(0,5]。

从当前保留的多条模拟路径中，选取代价最小的模拟路径作为机器人当前运行路径。

步骤s6、设置机器人在当前运行路径上的运行速度：

设置碰撞距离docc，其中碰撞距离小于安全距离dsafe；

对机器人当前的运行路径(对步骤s5选取出的代价最小的模拟路径)，判断其对应的障碍物距离dobs(代价最小的模拟路径的终点到距机器人当前位置距离最近的障碍物之间的距离)是否大于碰撞距离docc，若是，则设置当前运行路径所对应的速度为机器人下一时刻的运行速度；若小于或等于碰撞距离docc，则选取对应速度的负值作为机器人下一时刻的运行速度，即控制机器人后退。

机器人基于设置的运行速度在当前运行路径上运行，并基于当前机器人的当前位置判断机器人是否到达其目标位置，若是，则结束；否则继续执行步骤s1。

本发明根据障碍物的距离，设置安全距离和碰撞距离，判断机器人是否需要后退主动避开障碍物。当与障碍物的距离在安全距离内则会顺利避开障碍物；若与障碍物的距离在碰撞距离内则会选择远离障碍物的路径，退到安全距离外再重新规划路径绕开障碍物。

由于采用了上述避障手段，使得本发明在距离障碍物的距离较远时和传统算法相近；在距离障碍物距离较近时能够根据障碍物的距离合理控制机器人主动避开障碍物。

以上所述，仅为本发明的具体实施方式，本说明书中所公开的任一特征，除非特别叙述，均可被其他等效或具有类似目的的替代特征加以替换；所公开的所有特征、或所有方法或过程中的步骤，除了互相排斥的特征和/或步骤以外，均可以任何方式组合。