一种基于威胁度的路径规划方法

1.本发明属于计算机应用，本发明公开一种基于威胁度的路径规划方法。


背景技术：

2.地面特种车辆在执行任务时，需要快速高效的到达指定位置，但是从出发地到目的地之间，路径存在多种可能，且路况具有不确定性，在时间有限的情况下寻找一条最优路径面临多方面的挑战。传统的路径规划存在搜索速度慢、路径规划不合理、无法为突发状况提供新的路径规划方案的问题，这些问题直接影响了任务的完成并且造成损失。


技术实现要素：

3.本发明针对地面特种车辆在执行任务时，搜索速度慢、路径规划不合理、无法为突发状况提供新的路径规划方案的问题，提出了一种基于威胁度的路径规划方法。
4.具体步骤如下：
5.(10)地面信息数据采集与预处理。
6.(11)根据现实地理环境的特征，提取特定区域地理环境中的关键信息，包括现实地理环境的经度、纬度、可通行道路、不可通行道路、路中大障碍物、植被、建筑物、道路中各类运动车辆。
7.(20)采用栅格法构建路径规划地图环境。
8.(21)构建一个50m*50m的栅格地图。
9.(22)栅格地图中不可通行的道路、路中大障碍物、植被、建筑物都视为固定障碍物，将所处栅格赋值为1；道路中各类运动车辆视为随机动态障碍物，出现时将所处栅格赋值为1；可通行道路将所处栅格赋值为0。
10.(30)使用随机动态障碍物威胁度改进a*算法的启发函数。
11.(31)a*算法的搜索效率的高低和搜索结果的好坏与评价函数有关。评价函数f(n)是由代价函数g(n)与启发函数h(n)相加得到的。g(n)是起点到当前点的路径代价函数，h(n)是当前点到终点的路径估计代价的启发函数。
12.(32)由于地面特种车辆在实际行驶过程中会遇到随机动态障碍物，随机动态障碍物会影响路径搜索的结果，因此引入随机动态障碍物的威胁度ε，ε为归一化数值，即ε∈[0，1]。当ε＝0时，表示随机动态障碍物对地面特种车辆没有威胁；当ε＝1时，表示随机动态障碍物对地面特种车辆的威胁性为无穷。
[0013]
(33)ε与随机动态障碍物的速度v、随机动态障碍物与地面特种车辆的距离d有关。速度越大并且距离越近，则威胁度越大，速度越慢且距离越远，则威胁度越小。他们之间的关系表示如下。
[0014][0015]
(34)构建威胁度的评价指标体系，指标体系包括随机动态障碍物的速度v、随机动
态障碍物与地面特种车辆的距离d、随机动态障碍物运动角度θ、随机动态障碍物类型s。
[0016]
(35)随机动态障碍物的速度v和运动角度θ可由速度传感器获得，随机动态障碍物与地面特种车辆的距离d可由激光测距仪测得。设某一时刻测得的随机动态障碍物的速度为vi，随机动态障碍物的运动角度为θ，随机动态障碍物与地面特种车辆的距离di，其中i＝1，2，
…
，n。
[0017]
(36)量化随机动态障碍物速度、运动角度、距离和类型指标，将随机动态障碍物的速度vi使用区间数表示，则将其量化，则速度威胁度为将随机动态障碍物的运动角度θi使用区间数表示，则将其量化，则运动角度威胁度为将随机动态障碍物与地面特种车辆的距离di使用区间数表示，则将其量化，则距离威胁度为其量化，则距离威胁度为随机动态障碍物的类型使用模糊评价语言量化。将量化后的指标都转换为直觉模糊数。
[0018]
(37)确定指标权重，使用熵值法来确定指标权重，根据最大熵值原理确定各指标权重。
[0019]
(38)根据随机动态障碍物、随机动态障碍物的速度和随机动态障碍物与地面特种车辆的距离构建决策矩阵，将决策矩阵规范化得到规范化决策矩阵。根据规范化决策矩阵求各指标的“理想解”和“负理想解”，计算每个方案到“理想解”和“负理想解”的距离，再计算每个方案的相对贴近度，即可得到每个随机动态障碍物威胁度ε。
[0020]
(39)当随机动态障碍物的威胁程度越大，搜索速度越慢，h(n)的权重越低；当随机动态障碍物的威胁程度越小，搜索速度越快，h(n)的权重越高。因此，改进后a*算法的评价函数为f(n)＝g(n)+(1-lnε)*h(n)。
[0021]
(40)使用改进后的a*算法寻找全局最优路径。
[0022]
(41)利用改进后的a*算法寻找全局最优路径。在进行全局路径寻优时，以当前节点为中心，建立一个九宫格，其可搜索的方向有八个，分别是上、下、左、右、左上、右上、左下、右下。
[0023]
(42)在搜索时建立一个开放列表和一个关闭列表，开放列表用来存放需要计算评价函数的未搜索过的节点，关闭列表用来存放已搜索过的节点。
[0024]
(43)当搜索到终点时即完成全局最优路径的搜索，从而得到全局最优路径。
[0025]
(50)使用融合全局路径信息的动态窗口法(dwa)寻找局部最优路径。
[0026]
(51)使用dwa算法时，需给定地面特种车辆的位置信息(x
t
，y
t
)、速度信息(v
x
，vy)、加速度信息(a
x
，ay)、角度信息θ
t
、和角速度信息ω
t
，同时还要给定采样间隔δt。
[0027]
(52)根据给定信息可计算出地面特种车辆在采样周期内的速度变化，通过计算可以得到地面特种车辆在采样周期内的行动轨迹。
[0028]
(53)在采样周期内，通过dwa算法的评价函数评价行动轨迹的好坏。为了防止地面特种车辆由于当前栅格地图中的障碍物太多，而使其搜索结果陷入局部最优解，将a*算法得到的轨迹点作为dwa算法的引导点。此时的评价函数由三部分组成，分别为特种车辆模拟轨迹末端的点到引导点的距离guid(v
x
，vy，ω
t
)、特种车辆距离障碍物的最小距离dist(v
x
，vy，ω
t
)和特种车辆轨迹对应的速度值vel(v
x
，vy，ω
t
)。评价函数由这三部分的加权和决定，
即其中且
[0029]
(54)通过dwa评价函数计算结果得到局部最优路径，得到路径规划结果。
附图说明：
[0030]
图1为本发明方法步骤图；
[0031]
图2为本发明融合算法程图；
[0032]
图3为本发明改进的a*算法搜索方向图；
[0033]
图4为本发明路径规划结果示例一；
[0034]
图5为本发明路径规划结果示例二；
[0035]
图6为本发明路径规划结果示例三；
具体实施方式
[0036]
为使本发明的目的、技术方案和优点更加清楚、明白，下面结合附图对本发明作进一步详细的说明。
[0037]
本发明是计算机应用领域的一种计算方法，提高了路径的搜索速度和平滑度，使其规避障碍物的转角更加合理，更符合地面特种车辆行驶路径的实际情况，满足现实中地面特种车辆行驶路径的需要。
[0038]
本发明的设计提出一种基于威胁度的路径规划方法(步骤见附图1)。根据算法流程图(见附图2)具体实现步骤如下：
[0039]
(10)地面信息数据采集与预处理。
[0040]
(11)根据现实地理环境的特征，提取特定区域地理环境中的关键信息，包括现实地理环境的经度、纬度、可通行道路、不可通行道路、路中大障碍物、植被、建筑物、道路中各类运动车辆。
[0041]
(20)采用栅格法构建路径规划地图环境。
[0042]
(21)构建一个50m*50m的栅格地图。
[0043]
(22)栅格地图中不可通行的道路、路中大障碍物、植被、建筑物都视为固定障碍物，将所处栅格赋值为1；道路中各类运动车辆视为随机动态障碍物，出现时将所处栅格赋值为1；可通行道路将所处栅格赋值为0。
[0044]
(30)使用随机动态障碍物威胁度改进a*算法的启发函数。
[0045]
(31)a*算法的搜索效率的高低和搜索结果的好坏与评价函数有关。评价函数f(n)是由代价函数g(n)与启发函数h(n)相加得到的。g(n)是起点到当前点的路径代价函数，h(n)是当前点到终点的路径估计代价的启发函数。
[0046]
(32)由于地面特种车辆在实际行驶过程中会遇到随机动态障碍物，随机动态障碍物会影响路径搜索的结果，因此引入随机动态障碍物的威胁度ε，ε为归一化数值，即ε∈[0，1]。当ε＝0时，表示随机动态障碍物对地面特种车辆没有威胁；当ε＝1时，表示随机动态障碍物对地面特种车辆的威胁性为无穷。
[0047]
(33)ε与随机动态障碍物的速度v、随机动态障碍物与地面特种车辆的距离d有关。速度越大并且距离越近，则威胁度越大，速度越慢且距离越远，则威胁度越小。他们之间的
关系表示如下。
[0048][0049]
(34)构建威胁度的评价指标体系，指标体系包括随机动态障碍物的速度v、随机动态障碍物与地面特种车辆的距离d、随机动态障碍物运动角度θ、随机动态障碍物类型s。
[0050]
(35)随机动态障碍物的速度v和运动角度θ可由速度传感器获得，随机动态障碍物与地面特种车辆的距离d可由激光测距仪测得。设某一时刻测得的随机动态障碍物的速度为vi，随机动态障碍物的运动角度为θ，随机动态障碍物与地面特种车辆的距离di，其中i＝1，2，
…
，n。
[0051]
(36)量化随机动态障碍物速度、运动角度、距离和类型指标，将随机动态障碍物的速度vi使用区间数表示，则将其量化，则速度威胁度为将随机动态障碍物的运动角度θi使用区间数表示，则将其量化，则运动角度威胁度为将随机动态障碍物与地面特种车辆的距离di使用区间数表示，则将其量化，则距离威胁度为将其量化，则距离威胁度为随机动态障碍物的类型使用模糊评价语言量化。将量化后的指标都转换为直觉模糊数。
[0052]
(37)确定指标权重，使用熵值法来确定指标权重，根据最大熵值原理确定各指标权重。
[0053]
(38)根据随机动态障碍物、随机动态障碍物的速度和随机动态障碍物与地面特种车辆的距离构建决策矩阵，将决策矩阵规范化得到规范化决策矩阵。根据规范化决策矩阵求各指标的“理想解”和“负理想解”，计算每个方案到“理想解”和“负理想解”的距离，再计算每个方案的相对贴近度，即可得到每个随机动态障碍物威胁度ε。
[0054]
(39)当随机动态障碍物的威胁程度越大，搜索速度越慢，h(n)的权重越低；当随机动态障碍物的威胁程度越小，搜索速度越快，h(n)的权重越高。因此，改进后a*算法的评价函数为f(n)＝g(n)+(1-lnε)*h(n)。
[0055]
(40)使用改进后的a*算法寻找全局最优路径。
[0056]
(41)利用改进后的a*算法寻找全局最优路径。在进行全局路径寻优时，以当前节点为中心，建立一个九宫格(见附图3)，其可搜索的方向有八个，分别是上、下、左、右、左上、右上、左下、右下。
[0057]
(42)在搜索时建立一个开放列表和一个关闭列表，开放列表用来存放需要计算评价函数的未搜索过的节点，关闭列表用来存放已搜索过的节点。
[0058]
(43)当搜索到终点时即完成全局最优路径的搜索，从而得到全局最优路径。
[0059]
(50)使用融合全局路径信息的动态窗口法(dwa)寻找局部最优路径。
[0060]
(51)使用dwa算法时，需给定地面特种车辆的位置信息(xt，y
t
)、速度信息(v
x
，vy)、加速度信息(a
x
，ay)、角度信息θ
t
、和角速度信息ω
t
，同时还要给定采样间隔δt。
[0061]
(52)根据给定信息可计算出地面特种车辆在采样周期内的速度变化，通过计算可以得到地面特种车辆在采样周期内的行动轨迹。
[0062]
(53)在采样周期内，通过dwa算法的评价函数评价行动轨迹的好坏。为了防止地面
特种车辆由于当前栅格地图中的障碍物太多，而使其搜索结果陷入局部最优解，将a*算法得到的轨迹点作为dwa算法的引导点。此时的评价函数由三部分组成，分别为特种车辆模拟轨迹末端的点到引导点的距离guid(v
x
，vy，ω
t
)、特种车辆距离障碍物的最小距离dist(v
x
，vy，ω
t
)和特种车辆轨迹对应的速度值vel(v
x
，vy，ω
t
)。评价函数由这三部分的加权和决定，即其中且
[0063]
(54)通过dwa评价函数计算结果得到局部最优路径，得到路径规划结果(结果见附图4、附图5和附图6)。
[0064]
本发明采用改进的a*算法与dwa算法融合进行地面特种车辆路径规划，提高了路径的搜索速度和平滑度，使其规避障碍物的转角更加合理，更符合地面特种车辆行驶路径的实际情况，满足现实中地面特种车辆行驶路径的需要。
[0065]
发明不局限于上述实施方式，任何人应得知在本发明的启示下做出的与本发明具有相同或相近的技术方案，均落入本发明的保护范围。

技术特征：
1.一种基于威胁度的径规划方法，其特征在于，所述方法的步骤如下：(10)地面信息数据采集与预处理。(20)采用栅格法构建路径规划地图环境。(30)使用随机动态障碍物威胁度改进a*算法的启发函数。(40)使用改进后的a*算法寻找全局最优路径。(50)使用融合全局路径信息的动态窗口法(dwa)寻找局部最优路径。2.根据权利要求1所述的一种基于威胁度的路径规划方法，其特征在于，所述(10)的划分步骤具体为：(11)根据现实地理环境的特征，提取特定区域地理环境中的关键信息，包括现实地理环境的经度、纬度、可通行道路、不可通行道路、路中大障碍物、植被、建筑物、道路中各类运动车辆。3.根据权利要求1所述的一种基于威胁度的路径规划方法，其特征在于，所述(20)的划分步骤具体为：(21)构建一个50m*50m的栅格地图。(22)栅格地图中不可通行的道路、路中大障碍物、植被、建筑物都视为固定障碍物，将所处栅格赋值为1；道路中各类运动车辆视为随机动态障碍物，出现时将所处栅格赋值为1；可通行道路将所处栅格赋值为0。4.根据权利要求1所述的一种基于威胁度的路径规划方法。其特征在于，所述(30)的划分步骤具体为：(31)a*算法的搜索效率的高低和搜索结果的好坏与评价函数有关。评价函数f(n)是由代价函数g(n)与启发函数h(n)相加得到的。g(n)是起点到当前点的路径代价函数，h(n)是当前点到终点的路径估计代价的启发函数。(32)由于地面特种车辆在实际行驶过程中会遇到随机动态障碍物，随机动态障碍物会影响路径搜索的结果，因此引入随机动态障碍物的威胁度ε，ε为归一化数值，即ε∈[0，1]。当ε＝0时，表示随机动态障碍物对地面特种车辆没有威胁；当ε＝1时，表示随机动态障碍物对地面特种车辆的威胁性为无穷。(33)ε与随机动态障碍物的速度v、随机动态障碍物与地面特种车辆的距离d有关。速度越大并且距离越近，则威胁度越大，速度越慢且距离越远，则威胁度越小。他们之间的关系表示如下。(34)构建威胁度的评价指标体系，指标体系包括随机动态障碍物的速度v、随机动态障碍物与地面特种车辆的距离d、随机动态障碍物运动角度θ、随机动态障碍物类型s。(35)随机动态障碍物的速度v和运动角度θ可由速度传感器获得，随机动态障碍物与地面特种车辆的距离d可由激光测距仪测得。设某一时刻测得的随机动态障碍物的速度为v
i
，随机动态障碍物的运动角度为θ，随机动态障碍物与地面特种车辆的距离d
i
，其中i＝1，2，
…
，n。(36)量化随机动态障碍物速度、运动角度、距离和类型指标，将随机动态障碍物的速度
v
i
使用区间数表示，则将其量化，则速度威胁度为将随机动态障碍物的运动角度θ
i
使用区间数表示，则将其量化，则运动角度威胁度为将随机动态障碍物与地面特种车辆的距离d
i
使用区间数表示，则将其量化，则距离威胁度为将其量化，则距离威胁度为随机动态障碍物的类型使用模糊评价语言量化。将量化后的指标都转换为直觉模糊数。(37)确定指标权重，使用熵值法来确定指标权重，根据最大熵值原理确定各指标权重。(38)根据随机动态障碍物、随机动态障碍物的速度和随机动态障碍物与地面特种车辆的距离构建决策矩阵，将决策矩阵规范化得到规范化决策矩阵。根据规范化决策矩阵求各指标的“理想解”和“负理想解”，计算每个方案到“理想解”和“负理想解”的距离，再计算每个方案的相对贴近度，即可得到每个随机动态障碍物威胁度ε。(39)当随机动态障碍物的威胁程度越大，搜索速度越慢，h(n)的权重越低；当随机动态障碍物的威胁程度越小，搜索速度越快，h(n)的权重越高。因此，改进后a*算法的评价函数为f(n)＝g(n)+(1-lnε)*h(n)。5.根据权利要求1所述的一种基于威胁度的路径规划方法。其特征在于，所述(40)的划分步骤具体为：(41)利用改进后的a*算法寻找全局最优路径。在进行全局路径寻优时，以当前节点为中心，建立一个九宫格，其可搜索的方向有八个，分别是上、下、左、右、左上、右上、左下、右下。(42)在搜索时建立一个开放列表和一个关闭列表，开放列表用来存放需要计算评价函数的未搜索过的节点，关闭列表用来存放已搜索过的节点。(43)当搜索到终点时即完成全局最优路径的搜索，从而得到全局最优路径。6.根据权利要求1所述的一种基于威胁度的路径规划方法。其特征在于，所述(50)的划分步骤具体为：(51)使用dwa算法时，需给定地面特种车辆的位置信息(x
t
，y
t
)、速度信息(v
x
，v
y
)、加速度信息(a
x
，a
y
)、角度信息θ
t
、和角速度信息ω
t
，同时还要给定采样间隔δt。(52)根据给定信息可计算出地面特种车辆在采样周期内的速度变化，通过计算可以得到地面特种车辆在采样周期内的行动轨迹。(53)在采样周期内，通过dwa算法的评价函数评价行动轨迹的好坏。为了防止地面特种车辆由于当前栅格地图中的障碍物太多，而使其搜索结果陷入局部最优解，将a*算法得到的轨迹点作为dwa算法的引导点。此时的评价函数由三部分组成，分别为特种车辆模拟轨迹末端的点到引导点的距离guid(v
x
，v
y
，ω
t
)、特种车辆距离障碍物的最小距离dist(v
x
，v
y
，ω
t
)和特种车辆轨迹对应的速度值vel(v
x
，v
y
，ω
t
)。评价函数由这三部分的加权和决定，即其中λ、μ、且(54)通过dwa评价函数计算结果得到局部最优路径，得到路径规划结果。

技术总结
本发明属于计算机应用领域，提供一种基于威胁度的路径规划方法。该方法包括：(10)地面信息数据采集与预处理。(20)采用栅格法构建路径规划地图环境。(30)使用随机动态障碍物威胁度改进A*算法的启发函数。(40)使用改进后的A*算法寻找全局最优路径。(50)使用融合全局路径信息的动态窗口法(DWA)寻找局部最优路径。其有益效果为：在现实地理环境基础上，通过使用随机动态障碍物威胁度改进的A*算法进行全局路径寻优，DWA算法对局部路径进行优化，实现动态避障。相比于传统A*算法，融合算法提高了路径的搜索速度，使其规避障碍物的转角更合理，更符合地面特种车辆行驶路径的实际情况，满足现实中地面特种车辆行驶路径的需要。现实中地面特种车辆行驶路径的需要。现实中地面特种车辆行驶路径的需要。


技术研发人员：季昌娅 陈剑 吕咏洲 时雨
受保护的技术使用者：南京邮电大学
技术研发日：2023.07.20
技术公布日：2023/9/22