一种基于动态窗口的障碍物局部避障方法

摘要

本发明公开了一种基于动态窗口的障碍物局部避障方法，应用于无人驾驶车，每一动态窗口对应一局部路径，包括：以预设时间长度作为所述动态窗口的周期长度，在所述动态窗口内规划所述局部路径，包括：步骤1，在每一个所述动态窗口周期内，从周期开始时刻取间隔ΔT相等的三个以上时刻的传感器信息，生成局部的栅格地图；步骤2，将所述栅格地图上的数据换算到世界坐标系，与车载地图比较，确定障碍物；步骤3，通过各限制条件确定无人驾驶车的速度集合；步骤4，根据预设的评价函数对速度集合内的速度进行评分，根据评分最高的速度调整无人驾驶车的速度。

说明书

技术领域

本发明涉及自动驾驶技术领域，特别是关于一种基于动态窗口的障碍物局部避障方法。

背景技术

路径规划是自动驾驶导航最基本的环节，指的是无人驾驶车在有障碍物的工作环境中找到一条从起点到终点合适的运动路径，使车能在运动过程中安全地绕过障碍物到达终点。

一般情况下，路径规划主要涉及三个问题：一是明确起点和终点的位置，二是规避障碍物，三是尽可能做到路径上的优化。根据对环境信息的掌握程度不同，巡检车的路径规划可以分为全局路径规划和局部路径规划。全局路径规划主要是建立在已知环境信息如地图的情况下，对车从起点到终点做一个路径规划。局部路径规划主要针对当过程中环境发生变化，出现未知障碍物等情况时，侧重考虑局部环境的信息，使巡检车能够有效躲避障碍。

然而，现有相关技术无法有效对未知障碍物进行检测和避障。

发明内容

本发明实施例的目的在于提供一种基于动态窗口的障碍物局部避障方法来克服或至少减轻现有技术的上述缺陷中的至少一个。

为实现上述目的，本发明实施例提供一种基于动态窗口的障碍物局部避障方法，应用于无人驾驶车，每一动态窗口对应一局部路径，包括：

以预设时间长度作为所述动态窗口的周期长度，在所述动态窗口内规划所述局部路径，包括：

步骤1，在每一个所述动态窗口周期内，从周期开始时刻取间隔ΔT相等的三个以上时刻的传感器信息，生成局部的栅格地图，所述间隔ΔT为预设值；

所述间隔ΔT的取值满足下式(1)：

globalresolution+mean

其中，globalresolution表示所述栅格地图的地图分辨率，mean

步骤2，将所述栅格地图上的数据换算到世界坐标系，与车载地图比较，确定障碍物；

步骤3，通过下式(2)-(5)的交集确定所述无人驾驶车的速度集合：

V

其中，V

其中，V

其中，V

其中，x，y表示所述无人驾驶车的位置，θ

通过所述式(2)-(5)得到所述无人驾驶车的速度集合为：V＝V

步骤4，根据预设的评价函数对所述速度集合内的速度进行评分，根据评分最高的速度调整所述无人驾驶车的速度。

可选的，步骤1包括：判断所述间隔ΔT的值是否满足式(1)，若不满足，则调整所述间隔ΔT的值。

可选的，根据是否有障碍物自适应调整所述动态窗口的时长，若在所述动态窗口内没有检测到障碍物，则根据预设策略自动增加所述动态窗口的时长，若在所述动态窗口内检测到的障碍物数量超过上限，则自动减小所述动态窗口的时长。

可选的，步骤2包括：

通过比较去除所述已知地图上的障碍物，确定所述栅格地图中增加的障碍物；

在第一个时刻采集的第一帧地图中，提取所述障碍物的特征；

在第二和第三个时刻采集的第二和第三帧地图中，根据所述障碍物的特征进行匹配，并提取所述障碍物的姿态信息，确定所述障碍物的位移；

如果所述障碍物的位移超过阈值，则判定为动态障碍物；否则判定为静态障碍物。

可选的，如果所述障碍物为静态障碍物，则评价函数为下式(6)：

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

其中，G(v,w)表示评价函数，heading(v,w)为所述无人驾驶车按照所述速度集合中的速度达到模拟轨迹末端时的朝向和所述障碍物之间的角度差距，dist(v,w)表示所述无人驾驶车在当前轨迹上与所述障碍物之间的距离，velocity(v,w)为对速度进行采样得到的值，

α、β和γ分别为三个因素的权重系数，σ表示对三个因素的评分进行平滑处理的系数，α、β、γ和σ均为已知常量；

如果所述障碍物为动态障碍物，则评价函数为下式(7)：

其中，β‘表示权重系数，为预设值，R

其中，k表示根据实际场景预设的常数，N

本发明由于采取以上技术方案，其具有以下优点：

采用本发明所涉及的避障方法，在不影响全局路径规划的前提下，将动态障碍物的识别和运动预测加至局部轨迹生成的评价函数进行优化，形成一个可以避障动态障碍物的自动驾驶系统，提供了一种有效对动态障碍物进行检测和避障的方案。

附图说明

图1为本发明实施例提供的基于动态窗口的障碍物局部避障方法的流程示意图。

图2为本发明实施例提供的规划每个动态窗口周期内的局部路径的方法的流程示意图。

图3a至图3c为本发明实施例提供的在每一个动态窗口周期中取间隔ΔT相等的三帧地图的示意图。

图4为本发明实施例提供的无人驾驶车采用各速度产生不同的运动轨迹与行人相对位置的示意图。

具体实施方式

下面结合附图和实施例对本发明进行详细的描述。

本发明主要针对较低速运行下的无人驾驶车，主要传感器为单线激光雷达和轮速计。在低速运行的应用场景下，主要出现的未知障碍物为一般的静态障碍物以及低速运动的障碍物，主要为行人。

如图1所示，本发明实施例提供的基于动态窗口的障碍物局部避障方法包括：

步骤100，从全局规划模块得到一条全局规划路径，利用多个动态窗口划分该全局路径。

其中，全局规划路径由无人驾驶车的全局规划器从起点到终点规划得到。例如通过常见的定位系统规划路径。

其中，动态窗口的含义是：以周期性的时间当作窗口，该时间周期内的速度、位移等相当于是窗口内的值，由于时间周期的大小随时间或者路况发生变化，所以是动态的，即动态变化的时间段。

本申请中，动态窗口的周期长度，即动态窗口的时长为经验值，可以预设在无人驾驶车内，该值也可以在预设范围内调整。

可选的，还可以根据是否检测到障碍物自适应调整动态窗口的周期长度，若在动态窗口内没有检测到所述障碍物，则根据预设策略自动增加动态窗口的周期长度，若在动态窗口内检测到的障碍物的数量超过上限，则根据预设策略自动减小动态窗口的时长。例如，当周围环境没有障碍物时，周期T的取值会较大，减小程序开销；当周围环境障碍物较多时，周期T取值减小，增加避障灵敏性。

步骤200，规划每个动态窗口周期内的局部路径。

如图2所示，步骤200具体包括：

步骤210，识别障碍物。

局部地图取样：在每一个动态窗口周期中，取间隔ΔT相等的三个以上时刻的传感器信息，生成局部的栅格地图。优选的，取三个时刻的传感器信息，生成局部的栅格地图，例如图3a-图3c所示的三帧地图。间隔ΔT的初始值为预设值，例如1-3秒。

激光雷达数据按符合上述条件的时间间隔ΔT取样后，将激光雷达的数据换算到世界坐标系，与已知地图(例如无人驾驶车内预先安装加载的地图)进行比较，确定障碍物，并根据障碍物的位移变化判断为静态障碍物或动态障碍物。

具体的，去除已知地图上的静态障碍物(如墙壁等)。剩下的障碍物中，则可以通过栅格地图判断静态障碍物和动态障碍物。

在第一个时刻采集的第一帧地图中，提取各个障碍物的特征(例如连续占格的宽度和长度)。在第二和第三个时刻采集的第二和第三帧地图中，根据障碍物特征进行匹配，并提取障碍物的姿态信息。动态障碍物会发生明显的运动变化。本实例中，设定一个位移阈值，如果障碍物的位移超过该阈值，则判定为动态障碍物。该阈值为大于地图分辨率的预设值。

其中，间隔ΔT的取值需要满足下式(1)：

globalresolution+mean

其中，globalresolution表示已知的栅格地图的地图分辨率，mean

本文中，考虑传感器的误差，比如：里程计的漂移和激光雷达的测量误差，根据测量误差以及不确定性膨胀系数至分辨率中(传感器和漂移的数据是可以通过其他模块求得的)，通过ΔT的取值的限定保证能够观测到动态障碍物的位移变化。

在一个示例中，假设动态障碍物为行人，假设行人运动速度为0.5m/s，宽度为0.3m，已知地图分辨率为0.05m，取样间隔为ΔT，自车的漂移误差为0.05m/s(雷达误差较小0.05mm忽略不计)。在无误差的世界坐标系下，可以观测到的行人在ΔT内的移动距离为ΔT·0.5m/s。由于存在漂移误差,地图实际分辨率为resolution＝0.05+0.05·ΔT，因此，要想观测到行人的运动，则需要满足地图实际分辨率小于行人移动的距离,从而计算出取样间隔ΔT表示为式(10)：

0.05+0.05·ΔT<ΔT·0.5 (10)

由式(10)可以计算得到条件：ΔT>0.12s。

本步骤中，V

步骤220，通过各限制条件的交集确定无人驾驶车的速度集合。

若步骤210中没有识别出障碍物，则只需要按照全局路径前进即可；若识别出障碍物，则在本动态窗口内规划局部路径，通过包括但不限于如下1-5的限制条件来确定本动态窗口内的速度集合：

1.限定无人驾驶车的最大速度和最小速度：

V

式中，V

对于给定的无人驾驶车，其速度会预先设置v

2.限制巡检车的最大加减速.

其中，V

对于给定的无人驾驶车，可以预先设置加速度

3.限制无人驾驶车的安全速度，安全速度是指无人驾驶车能够在撞到障碍物之前停下。

设刹车时的加速度为

其中，V

4.考虑障碍物与无人驾驶车之间的距离|r

其中，x，y表示无人驾驶车的位置，θ

其中，通过下述方式确定|r

设障碍物(例如行人)的位置为(x

针对该场景，提供一种风险模型，该风险模型为经验模型，地图中r点的障碍物对j点的无人驾驶车产生的风险为：

其中，R

假设风险阈值为R，当R

5.通过上述式(2)-(5)得到无人驾驶车的速度空间为：V＝V

步骤230，根据评价函数对速度集合内的速度进行评分，根据评分最高的速度调整无人驾驶车的速度。

对速度集合内的所有速度进行采样，并对各速度的行驶轨迹(如图3)进行评分。

评分标准为评价函数：

根据动态窗口法(Dynamic window approach)的流程，生成评价函数。如果障碍物为静态障碍物，则评价函数依照动态窗口法的评价函数(6)：

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

式中，heading(v,w)为当前设定的采样速度(即步骤220中确定的速度集合中的速度)达到模拟轨迹末端时的朝向和目标之间的角度差距。dist(v,w)代表无人驾驶车在当前轨迹上与障碍物之间的距离，如果没有障碍物，则设定为一个常数。velocity(v,w)为对速度进行采样得到的值。G(v,w)为评价函数，分数越高代表该轨迹越好，α，β，γ分别为三个因素的权重系数，σ系数则是对三个因素的评分做一个平滑处理。四个参数均为已知常量，其值可以根据经验调整。

如果障碍物为动态障碍物，则评价函数为：

公式中,R

优选的，对上述评价函数的系数进行归一化处理。

通过上述评价函数，将得分最高的采样值确定为无人驾驶车需要的速度值，相应调整无人驾驶车的速度。

图4为本发明实施例提供的无人驾驶车(巡检车)采用各速度产生不同的运动轨迹与行人相对位置的示意图。巡检车采用速度集合内的各速度将产生不同的运动轨迹，对行人可能的运动产生不同的风险，通过本发明的方法确定最优速度，巡检车采用最优路径通过。

最后需要指出的是：以上实施例仅用以说明本发明的技术方案，而非对其限制。本领域的普通技术人员应当理解：可以对前述各实施例所记载的技术方案进行修改，或者对其中部分技术特征进行等同替换；这些修改或者替换，并不使相应技术方案的本质脱离本发明各实施例技术方案的精神和范围。