CN114460933B - 一种面向动态环境的移动机器人局部路径规划算法 - Google Patents

Abstract

本发明公开了一种面向动态环境的移动机器人局部路径规划算法。在已知局部障碍栅格地图和移动障碍障碍物位置的情况下，对每个移动障碍进行相对速度的计算和未来行驶轨迹的预测，然后通过对morphin局部路径规划算法进行改进，改进原算法中的路径评价规则并且将移动障碍的行驶轨迹加入算法的决策中，对与移动障碍有潜在碰撞风险的路径给予更低的分数，从而优化其结果。本发明将移动障碍的行驶轨迹进行预测，据此对传统morphin局部路径规划算法改进，提高了规划路径的安全性和平滑性。

Description

一种面向动态环境的移动机器人局部路径规划算法

技术领域

本发明属于局部路径规划技术领域，具体地说，是一种面向动态环境的移动机器人局部路径规划算法。

背景技术

提到无人驾驶技术就不得不提到路径规划，路径规划是无人车导航最基本的环节，指的是无人车在有障碍物的工作环境中，如何找到一条从起点到终点适当的运动路径，使无人车在运动过程中能安全、无碰撞地绕过所有障碍物。根据对环境信息的掌握程度不同，机器人路径规划可分为全局路径规划和局部路径规划。全局路径规划是在已知的环境中，给机器人规划一条路径，路径规划的精度取决于环境获取的准确度，全局路径规划可以找到最优解，但是需要预先知道环境的准确信息，当环境发生变化，如出现未知障碍物时，该方法就不适用。它是一种事前规划，因此对机器人系统的实时计算能力要求不高，虽然规划结果是全局的、较优的，但是对环境模型的错误及噪声鲁棒性差。而局部路径规划则环境信息完全未知或有部分可知，侧重于考虑机器人当前的局部环境信息，让机器人具有良好的避障能力，通过传感器对机器人的工作环境进行探测，以获取障碍物的位置等信息，这种规划需要搜集环境数据，并且对该环境模型的动态更新能够随时进行校正，局部规划方法将对环境的建模与搜索融为一体，要求机器人系统具有高速的信息处理能力和计算能力，对环境误差和噪声有较高的鲁棒性，能对规划结果进行实时反馈和校正，但是由于缺乏全局环境信息，所以规划结果有可能不是最优的，甚至可能找不到正确路径或完整路径。

对于无人驾驶技术来说，具有实时避障功能的局部路径规划算法是必须的，目前常用的局部路径规划算法是在栅格地图里面进行规划，对单帧栅格地图而言，进行局部路径规划已经是属于非常成熟的技术。就morphin算法而言，其核心是根据阿克曼转向结构车辆的行驶特点，预先规划若干数量的平滑弧形路径，再凭借栅格地图中障碍栅格的分布来对各条路径评分，选出分数最高的作为最优路径。但是常见的算法仅仅考虑单帧数据中障碍物对行驶的影响，而没有考虑场景中动态障碍物的影响，如移动的人或车辆，他们的运动轨迹可能会与规划的行驶路径有交叉和碰撞。

发明内容

本发明的目的在于提出一种面向动态环境的移动机器人局部路径规划算法，发现、利用移动障碍运动趋势给无人驾驶车辆提供安全、平滑的避障路径。

实现本发明目的的技术解决方案为：一种面向动态环境的移动机器人局部路径规划算法，包括以下步骤：

第一步：读取当前环境的障碍栅格地图和移动障碍信息。

第二步：对移动障碍建立队列，记录每个移动障碍在当前环境中的空间位置，并据此估计移动障碍在当前与移动机器人的相对速度，预测未来一段时间移动障碍的位置与运动轨迹。

第三步：根据第二步的结果，改进的morphin算法的路径评价方法进行局部路径规划，找出最优的行驶路径，输出最优路径的转向角度。

本发明与现有技术相比，其显著优点为：本发明考虑了场景中的静态障碍和动态障碍，不仅能实现避障，还检测场景中移动障碍，将移动障碍的运动趋势考虑进避障范围，提高了规划路径的安全性，也在一定程度上提高了路径的平滑性。

附图说明

图1是本发明方法的算法流程图。

图2是某根据某场景生成的栅格地图示意图。

图3是移动障碍在未来一段时间的预测运动轨迹示意图，原点位于栅格地图顶边中点，中心白色块就是移动障碍栅格。

图4是morphin算法预规划路径示意图。

图5是最优路径效果示意图，其中红色线是预规划路径，绿色线是最优路径。

图6是最优路径效果示意图，其中红色线是预规划路径，绿色线是最优路径，红色点是场景中移动障碍的预测运动轨迹。

具体实施方式

为了提高无人驾驶技术的安全性和避障的平滑性，本发明将考虑当前局部场景中的移动障碍物(主要是移动障碍)，来改善传统morphin局部路径规划算法。

下面结合说明书附图1-6和实施例对本发明作进一步说明。

本发明一种面向动态环境的移动机器人局部路径规划算法，包括以下步骤：

第一步：读取当前环境的障碍栅格地图和移动障碍信息，具体包括以下步骤：

1)输入当前环境的障碍栅格地图GridMap，对GridMap中的每个栅格，值为1代表其为障碍物栅格，值为0代表其为空白栅格。

2)输入当前环境中的移动障碍物信息，其数据格式为如下数据对：

ID,Position

其中ID是某个移动障碍的唯一编号，Position是其在当前场景中离原点的位置。

第二步：估计移动障碍在当前与移动机器人的相对速度，具体包括以下步骤：

1)因为在相邻两帧数据中，同一个移动障碍会被跟踪为同一个ID，对每个移动障碍ID，建立一个队列存储其在每一帧栅格地图GridMap中离原点的位置。

2)在当前帧，若出现以前未出现的某个ID，说明当前场景中出现了以前没出现过的新的移动障碍，对其建立一个新的队列并记录其位置；若出现以前存在的某个ID，说明当前移动障碍在之前出现过，将当前帧位置记录进它的队列；若以前的某个ID在当前帧并未出现，说明之前存在的某个移动障碍已经走出检测范围，将其队列删除。于是得到以下数据对：

ID,Queue_position

其中，ID是某个移动障碍的唯一编号，Queue_position是其在当前历史帧中位置队列，里面包含每一帧中编号为ID的移动障碍离原点的位置。

3)对每一个移动障碍，根据历史5帧的位置来计算其在当前帧的速度：

其中，是当前帧与之前第i帧之间计算的平均速度，其计算公式为：

其中，p_t是当前帧中移动障碍的位置，p_t-i是距当前帧i帧中移动障碍的位置。根据计算的当前帧速度，计算出该移动障碍在未来2s内在栅格地图GridMap中的轨迹Trace。于是得到如下数据：

ID,Queue_position,Trace

其中，ID是某个移动障碍的唯一编号，Queue_position是其在当前历史帧中位置队列,Trace是其在未来2s内的轨迹。

第三步：用改进的morphin算法进行路径规划，具体包括以下步骤：

1)根据阿克曼转向结构车辆的运动模型，预先规划出一定的数量的路径，这些路径的轨迹都是理想的圆弧，其半径分别对应着车辆后轮中心不同的转弯半径(选用后轮中心而不用后右轮或者后左轮是为了保证左右路径的对称性)。路径预规划长度l_planning有限且固定，路径圆弧对应的最大角度有限且固定。

2)将场景中移动障碍未来2s内的预测行走轨迹加入栅格地图GridMap中，对GridMap中的每个栅格，其值为1代表其为障碍物栅格，其值为0代表为空白栅格，其值为2代表其为移动障碍预测轨迹上的栅格。

3)得到栅格地图GridMap后，将所有预先规划的路径也投影到栅格中，由于预先规划的路径是由一些连续的离散点表示的，经投影后，每条规划路径的所有离散点将占据一定数量的栅格。对每一条路径，计算如下指标(其中，除指标①是沿用原morphin算法计算外，本发明对指标②、指标③进行了改进，并增加了指标③)：

①可通行比例W_{passible_rate}

从路径的起点开始沿着路径扫描栅格，直到遇到障碍栅格为止，则此时已经扫描过的这段长度是无障碍的，称之为无障碍路径段Seg_passible，这段长度称为l_passible。可通行比例是指Seg_passible的长度l_passible与路径预规划长度l_planning的比值。公式表示为：

在实际情况下，考虑到车辆自身有长度，若l_passible<1.5m，则令W_{passible_rate}＝0。

②安全性W_{security_rate}

对栅格地图GridMap中的每一个障碍栅格，对距其半径10个栅格内的所有栅格计算一个危险值d_i，d_i的计算公式如下：

其中，x是某个栅格距障碍栅格的距离。由于每个栅格10半径内可能有不止一个障碍物栅格，则不同障碍栅格对其造成的危险值应叠加，但最大不超过1，即：

0≤d_i≤1

在实际数据结构的表现上，一条路径是由许多离散点组成的，对于一条路径上无障碍路径段Seg_passible上的每一个点i所在的栅格危险值d_i进行积分，则整条路径的危险性为无障碍路径段Seg_passible上所有点的危险值积分除以Seg_passible的长度l_passible，则安全性W_{security_rate}公式为：

③目标趋向性W_{destination_orientation}

目标趋向性评估的是路径和目标点之间的偏离情况。已知当前车头朝向和车辆与目标点连线之间的夹角、每条路径的转弯半径、每条路径的无障碍路径段Seg_passible长度l_passible，很容易求出每条路径的无障碍路径段Seg_passible末端切线与目标之间的夹角angle，angle越大，说明路径的走向和目标之间的偏差越大。目标趋向性的计算规则如下：

④移动障碍的运动轨迹影响W_{pedestrians_impact}

根据移动障碍的速度，预测其未来2s的行走轨迹。若移动障碍轨迹与小车相比是远离趋势(即移动障碍离小车越来越远)，则无需考虑移动障碍轨迹对路径规划的影响，否则需要考虑。对与移动障碍预测运动轨迹有碰撞冲突的路径添加一个值为1的负权重，即：

4)计算每条路径的评价分数Score，公式为：

其中，c₁:c₂:c₃:c₄＝2:1:1:1。选出Score最大的路径，即为最优路径。

5)输出最优路径，输出的格式为：该最优路径对应的转向角度Angle_best

实施例

结合图1，本发明一种面向动态环境的移动机器人局部路径规划算法，实施步骤如下：

第一步：读取当前环境的障碍栅格地图和移动障碍信息，其中，某场景的障碍栅格地图如图2所示。

第二步：对移动障碍建立队列，记录每个移动障碍在当前环境中的空间位置，并据此估计移动障碍在当前的速度方向和大小，预测未来一段时间移动障碍的运动轨迹。某移动障碍在未来一段时间的预测运动轨迹如图3所示

第三步：综合整理以上结果，改进的morphin算法的路径评价方法进行局部路径规划，找出最优的行驶路径。其中，morphin算法预规划路径示意图如图4所示；改进后的最优路径效果如图5和图6所示。

Claims (3)

1.一种面向动态环境的移动机器人局部路径规划算法，其特征在于，包括以下步骤：

第一步：读取当前环境的障碍栅格地图和移动障碍信息；

第二步：对移动障碍建立队列，记录每个移动障碍在当前环境中的空间位置，并据此估计移动障碍在当前与移动机器人的相对速度，预测未来一段时间移动障碍的位置和运动轨迹；

第三步：根据第二步的结果，采用改进的morphin算法的路径评价方法进行局部路径规划，找出最优的行驶路径，输出最优路径的转向角度；

包括以下步骤：

1)根据移动机器人的阿克曼转向结构运动模型，预先规划出一定数量的路径，这些路径的轨迹都是理想的圆弧，其半径分别对应着车辆后轮中心不同的转弯半径和转向角度；路径预规划长度l_planning有限且固定，路径圆弧对应的最大角度有限且固定；

2)将场景中移动障碍未来2s内的预测运动轨迹投影到栅格地图GridMap中，对GridMap中的每个栅格，值为1代表其为障碍物栅格，值为0代表为空白栅格，值为2代表其为移动障碍预测轨迹上的栅格；

3)得到栅格地图GridMap后，将所有预先规划的路径也投影到栅格中，预先规划的每一条路径都由若干个离散点表示，经投影后，每条规划路径的所有离散点将占据一定数量的栅格；对每一条路径，计算如下四个指标：

①可通行比例W_{passible_rate}

从路径的起点开始沿着路径扫描栅格，直到遇到障碍栅格为止，则此时已经扫描过的这段长度是无障碍的，称之为无障碍路径段Seg_passible，这段长度称为l_passible；可通行比例是指Seg_passible的长度l_passible与路径预规划长度l_planning的比值；公式表示为：

在实际情况下，考虑到车辆自身有长度，若l_passible<1.5m，则令W_{passible_rate}＝0；

②安全性W_{security_rate}

对栅格地图GridMap中的每一个障碍栅格，对距其半径10个栅格内的所有栅格计算一个危险值d_i，d_i的计算公式如下：

其中，x是某个栅格距障碍栅格的距离；每个栅格10半径内可能有不止一个障碍物栅格，则不同障碍栅格对其造成的危险值应叠加，但最大不超过1，即：

0≤d_i≤1

在实际数据结构的表现上，一条路径是由许多离散点组成的，对于一条路径上无障碍路径段Seg_passible上的每一个点i所在的栅格危险值d_i进行积分，则整条路径的危险性为无障碍路径段Seg_passible上所有点的危险值积分除以Seg_passible的长度l_passible，则安全性W_{security_rate}公式为：

③目标趋向性W_{destination_orientation}

目标趋向性评估的是路径和目标点之间的偏离情况；已知当前车头朝向和车辆与目标点连线之间的夹角、每条路径的转弯半径、每条路径的无障碍路径段Seg_passible长度l_passible，求出每条路径的无障碍路径段Seg_passible末端切线与目标之间的夹角angle，angle越大，说明路径的走向和目标之间的偏差越大；目标趋向性的计算规则如下：

④移动障碍的运动轨迹影响W_{pedestrians_impact}

根据移动障碍的速度，预测其未来2s的行走轨迹；若移动障碍轨迹与小车相比是远离趋势，即移动障碍离小车越来越远，则无需考虑移动障碍轨迹对路径规划的影响，否则说明障碍物离小车越来越近，需要考虑其对小车行驶路线的影响：具体做法是对与移动障碍预测运动轨迹有碰撞冲突的路径添加一个值为1的负权重，即：

4)计算每条路径的评价分数Score，公式为：

其中，c₁:c₂:c₃:c₄＝2:1:1:1；选出Score最大的路径，即为最优路径；

5)输出最优路径，输出的格式为：该最优路径对应的转向角度Angle_best。

2.根据权利要求1所述的面向动态环境的移动机器人局部路径规划算法，其特征在于，第一步读取当前环境的障碍栅格地图和移动障碍信息的具体方法为：

1)输入当前动态局部环境的障碍栅格地图GridMap，GridMap的具体信息如下表所示：

2)输入移动障碍信息：移动机器人处于动态场景中，对周围环境栅格化之后，障碍栅格中包含静态障碍栅格和动态障碍栅格；输入的移动障碍物具体信息如下表所示：

属性 说明 移动障碍编号ID 移动障碍的唯一标识 移动障碍的位置Position 移动障碍在栅格地图中相对原点的位置

移动障碍的ID由前序跟踪算法给出，用以对不同帧中同一个移动障碍进行唯一标识；每个移动障碍在栅格地图中占据不止一个栅格，Position是指每个移动障碍的占据栅格的中心栅格位置；场景中可能有多个移动障碍。

3.根据权利要求1所述的面向动态环境的移动机器人局部路径规划算法，其特征在于，所述估计移动障碍在当前与移动机器人的相对速度包括以下步骤：

1)因为在相邻两帧数据中，同一个移动障碍会被跟踪为同一个ID，对每个移动障碍ID，建立一个队列存储其在每一帧栅格地图GridMap中离原点的位置；

2)在当前帧，若出现以前未出现的某个ID，说明当前场景中出现了以前没出现过的新的移动障碍，对其建立一个新的队列并记录其位置；若出现以前存在的某个ID，说明当前移动障碍在之前出现过，将当前帧位置记录进它的队列；若以前的某个ID在当前帧并未出现，说明之前存在的某个移动障碍已经走出检测范围，将其队列删除；于是得到以下数据对：

ID,Queue_position

其中，ID是某个移动障碍的唯一编号，Queue_position是其在当前历史帧中的位置队列，里面包含每一帧中编号为ID的移动障碍在栅格地图中离移动机器人的位置；

3)对每一个移动障碍，根据历史5帧的位置来计算其在当前帧的速度：

其中，是当前帧与之前第i帧之间计算的平均速度，其计算公式为：

其中，p_t是当前帧中移动障碍的位置，p_t-i是距当前帧i帧中移动障碍的位置；

根据计算的当前帧速度，计算出该移动障碍在未来2s内在栅格地图GridMap中的轨迹Trace，按栅格地图10Hz的更新频率，2s包含20帧栅格地图，即Trace中包含该移动障碍在未来20帧栅格地图中的位置；于是得到如下数据：

ID,Queue_position,Trace

其中，ID是某个移动障碍的唯一编号，Queue_positoon是其在当前历史帧中位置队列,Trace是其在未来2s内的轨迹。