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

Abstract

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

Description

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

技术领域

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

背景技术

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

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

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

发明内容

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

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

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

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

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

globalresolution+mean_drift·ΔT<ΔT·V_obs (1)

其中，globalresolution表示所述栅格地图的地图分辨率，mean_drift表示自车的漂移误差，V_obs表示所述障碍物的最小移动速度；

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

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

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

其中，V_m表示所述无人驾驶车的速度集合，v表示线速度，w表示角速度，v_min表示最小线速度，v_max表示最大线速度，w_min表示最小角速度，w_max表示最大角速度；

其中，V_d表示限制加减速的所述无人驾驶车的速度集合，v_c表示当前线速度，w_c表示当前角速度，

表示最大线减速度，

表示最大角减速度，

表示最大线加速度，

表示最大角加速度，Δt表示所述动态窗口的周期长度；

其中，V_a表示所述无人驾驶车不与所述障碍物碰撞的速度集合，dist(v,w)为表示所述无人驾驶车至所述障碍物的距离；

其中，x，y表示所述无人驾驶车的位置，θ_t表示所述无人驾驶车的偏航角，Δt表示所述动态窗口的周期长度，(x_r，y_r)表示所述障碍物的位置，k表示根据实际场景预设的常数，N_r表示预设的所述障碍物的等效权重参数，至少基于所述障碍物的质量和易受伤程度确定，θ表示所述障碍物的运动方向相对于所述无人驾驶车的速度方向的夹角，v_r为所述障碍物的速度；

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

步骤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_d为所述动态障碍物对于所述无人驾驶车的风险值，其值通过下式(8)确定：

其中，k表示根据实际场景预设的常数，N_r表示预设的所述障碍物的等效权重参数，至少基于所述障碍物的质量和易受伤程度确定，θ表示所述障碍物的运动方向相对于所述无人驾驶车的速度方向的夹角，v_r为所述障碍物的速度，|r_j|表示所述障碍物与所述无人驾驶车之间的距离。

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

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

附图说明

图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_drift·ΔT<ΔT·V_obs (1)

其中，globalresolution表示已知的栅格地图的地图分辨率，mean_drift表示自车的漂移误差，V_obs表示所述障碍物的最小移动速度。

本文中，考虑传感器的误差，比如：里程计的漂移和激光雷达的测量误差，根据测量误差以及不确定性膨胀系数至分辨率中(传感器和漂移的数据是可以通过其他模块求得的)，通过Δ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_obs的取值为经验值，根据要探测的障碍物的最小移动速度预先确定，且满足上述式(1)的条件。

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

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

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

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

式中，V_m表示所述无人驾驶车的速度集合，v表示线速度，w表示角速度，v_min表示最小线速度，v_max表示最大线速度，w_min表示最小角速度，w_max表示最大角速度。

对于给定的无人驾驶车，其速度会预先设置v_min、v_max、w_min以及w_max的值，且v_min、v_max、w_min以及w_max的值随着应用场景的不同可以相应调整。

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

其中，V_d表示限制加减速的无人驾驶车能够达到的速度集合，v_c表示当前线速度，w_c表示当前角速度，

表示最大线减速度，

表示最大角减速度，

表示最大线加速度，

表示最大角加速度。

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

以及

的值，且

以及

的值随着应用场景的不同可以相应调整。

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

设刹车时的加速度为

和

V_a为无人驾驶车不与障碍物碰撞的速度集合，通过下式(4)确定V_a：

其中，V_a表示无人驾驶车不与障碍物碰撞的速度集合，

和

表示刹车时的加速度，dist(v,w)表示无人驾驶车至障碍物的距离。

4.考虑障碍物与无人驾驶车之间的距离|r_j|,根据下式(5)限制无人驾驶车的车速V_r：

其中，x，y表示无人驾驶车的位置，θ_t表示无人驾驶车的偏航角，Δt表示动态窗口取样周期，(x_r，y_r)表示障碍物的位置；

其值需要大于

其中，通过下述方式确定|r_j|应满足的限制，

设障碍物(例如行人)的位置为(x_r,y_r),速度为v_r，动态窗口的时长，即周期长度为Δt。障碍物下一时刻的运动方向未知，现有速度方向的风险最大，与现有方向差值越大的速度方向风险越小。风险大的地方则代表行人去到的可能性越大，与车发生碰撞的概率越大。

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

其中，R_d为障碍物对无人驾驶车产生的风险，k表示根据实际场景进行标定的常数，N_r表示障碍物的等效权重参数，其基于障碍物的质量M_r和易受伤程度V_r等因素确定，关系为N_r＝αM_r+βV_r其中α、β为待定权重，其和等于1。k与N_r为可提前测定的常数，与道路情况和障碍物质量等参数相关。θ表示障碍物的运动方向相对于无人驾驶车的速度方向的夹角，v_r为障碍物的速度。

假设风险阈值为R，当R_d>R时巡检车与行人有撞车风险，则可求出|r_j|的可取值范围表示为式(9)：

5.通过上述式(2)-(5)得到无人驾驶车的速度空间为：V＝V_m∩V_a∩V_d∩V_r,该集合以外的速度都不能在该时间间隔内达到。

步骤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_d为动态障碍物对于无人驾驶车的风险值，其值可通过式(8)确定，风险值越大则评分越低，β‘表示权重系数，为预设值。

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

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

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

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

Claims (4)

1.一种基于动态窗口的障碍物局部避障方法，应用于无人驾驶车，其特征在于，以预设时间长度作为所述动态窗口的周期长度，在所述动态窗口内规划所述局部路径，包括：

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

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

globalresolution+mean_drift·ΔT<ΔT·V_obs (1)

其中，globalresolution表示所述栅格地图的地图分辨率，mean_drift表示自车的漂移误差，V_obs表示预设的要探测障碍物的最小移动速度；

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

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

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

其中，V_m表示所述无人驾驶车的速度集合，v表示线速度，w表示角速度，v_min表示最小线速度，v_max表示最大线速度，w_min表示最小角速度，w_max表示最大角速度；

其中，V_d表示限制加减速的所述无人驾驶车的速度集合，v_c表示当前线速度，w_c表示当前角速度，

表示最大线减速度，

表示最大角减速度，

表示最大线加速度，

表示最大角加速度，Δt表示所述动态窗口的周期长度；

其中，V_a表示所述无人驾驶车不与所述障碍物碰撞的速度集合，dist(v,w)为表示所述无人驾驶车至所述障碍物的距离；

其中，x，y表示所述无人驾驶车的位置，θ_t表示所述无人驾驶车的偏航角，Δt表示所述动态窗口的周期长度，(x_r，y_r)表示所述障碍物的位置，k表示根据实际场景预设的常数，N_r表示预设的所述障碍物的等效权重参数，至少基于所述障碍物的质量和易受伤程度确定，θ表示所述障碍物的运动方向相对于所述无人驾驶车的速度方向的夹角，v_r为所述障碍物的速度；

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

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

2.根据权利要求1所述的基于动态窗口的障碍物局部避障方法，其特征在于，包括：根据是否检测到所述障碍物自适应调整所述动态窗口的周期长度，若在所述动态窗口内没有检测到所述障碍物，则根据预设策略自动增加所述动态窗口的周期长度，若在所述动态窗口内检测到的所述障碍物的数量超过上限，则根据预设策略自动减小所述动态窗口的时长。

3.根据权利要求1所述的基于动态窗口的障碍物局部避障方法，其特征在于，步骤2包括：

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

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

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

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

4.根据权利要求1所述的基于动态窗口的障碍物局部避障方法，其特征在于，如果所述障碍物为静态障碍物，则评价函数为下式(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_d为所述动态障碍物对于所述无人驾驶车的风险值，其值通过下式(8)确定：

其中，k表示根据实际场景预设的常数，N_r表示预设的所述障碍物的等效权重参数，至少基于所述障碍物的质量和易受伤程度确定，θ表示所述障碍物的运动方向相对于所述无人驾驶车的速度方向的夹角，v_r为所述障碍物的速度，|r_j|表示所述障碍物与所述无人驾驶车之间的距离。

Images