CN112082567A - 基于改进Astar和灰狼算法结合的地图路径规划方法 - Google Patents

Abstract

本发明公开了一种基于改进Astar和灰狼算法结合的地图路径规划方法，具体包括：基于数据采集设备构建高精度地图；在Astar算法中引入车辆运动学模型，并将其导入OpenDRIVE的xml文件中；使用Astar算法在arcgis导出的地图数据中查找出最优路径点，并使用RS曲线与Astar算法所查找出的最优路径点相结合生成车辆可跟踪的运动轨迹；将arcgis的点线面数据与OpenDRIVE的xml文件相匹配；对匹配后的结果使用GWO算法进行安全性与舒适性的优化，选出最优路径；在OpenDRIVE文件中选取出最优路径后，可改变相应最优路径上的road等的颜色，使用LGSVL模拟器等进行可视化。

Description

基于改进Astar和灰狼算法结合的地图路径规划方法

技术领域

本发明涉及地图路径规划技术领域，具体涉及基于改进Astar和灰狼算法结合的地图路径规划方法。

背景技术

自动驾驶车主要提供地图、定位、感知、导航和控制功能。地图场景表示的中心元素是车道的路线，地图场景建模的主要目标是在保持一定精度的情况下有效地表示道路几何形状，同时保持一定的精确度。

目前，有多种方法可以获得准确的道路几何数据。例如，在传统地理信息系统中，从飞机上获得高分辨率航空相机图像，通过图像处理装置提取道路几何图形。许多研究使用基于车辆的检测方法获得更精确的道路几何形状。在这种方法中，装有各种传感器的探测车探索道路并收集传感器数据以获得道路几何信息。合理的道路几何表示是保证存储效率和可用性以及绘图精度的一个重要方面。在2006年1月31日，OpenDRIVE地图格式首次公开路面，OpenDRIVE是一种开放文件格式，用于道路网络的逻辑描述。它是由一组模拟专业人员开发并维护的，并得到了模拟行业的大力支持。OpenDRIVE是独立于供应商的，可以免费使用且无其他义务；其包含真实道路网络的所有关键功能，是一种具有广泛国际用户基础的既定格式，具有透明的流程持续不断的开发。OpenDRIVE自2006年以来一直在市场上，并且由于实际用例而合并了众多用户意见。

全局路径规划完成类似于人类驾驶员在驾驶过程中对路径规划的工作，是自动驾驶汽车核心的任务之一。路径规划模块需要收集来自定位、感知、数据库等一系列基础模块的数据，并对这些数据进行综合评估,给出在限定条件下的最优路径规划。路径规划是汽车完成驾驶决策及进一步运动的基础，其在整个自动驾驶系统的框架中是必不可少且至关重要的部分。全局路径规划的目标是根据已知电子地图和起点、终点信息，采用路径搜索算法生成一条最优化的(时间最短、路径长度最短等)全局期望路径。

目前，自动驾驶汽车在求解全局路径规划问题上已经有许多成熟的算法。常见的算法包括Dijkstra算法、Floyd算法以及启发式搜索算法，如Astar算法等。Dijkstra算法是求解最优路径问题的最经典的算法，但它是一种盲目的搜索算法，在多年的研究中，各国学者都对其进行不断的改进，该算法的求解速度也大大提高。Astar算法是启发式的搜索算法的代表，它在搜索过程中减小了网络图的搜索空间，从而节省了搜索的时间，因而计算效率较高。但Astar算法搜索出的路径不具备连通性，不适合无人驾驶汽车使用，所以应用到无人车领域时需要作进一步的改进。

发明内容

本发明的目的在于提供基于改进Astar和灰狼算法结合的地图路径规划方法，通过全景视觉相机采集图像数据，GPS采集车辆定位信息，并将图像信息与定位信息进行数据融合，将融合后的数据使用arcgis进行数据整理与标注，参考OpenDRIVE地图格式构建高精度地图，从而解决上述背景技术中提出的问题。

为实现上述目的，本发明提供如下技术方案：基于改进Astar和灰狼算法结合的地图路径规划方法，具体包括移动以下步骤：

步骤一、基于数据采集设备构建高精度地图，数据采集设备包括全景相机及数据采集车，具体构建方法为：

(1)使用采集车每前进10米停车采集一张图片以及与图片相对应的车辆此刻GPS数据；

(2)将图片进行透视变化并使用PS等拼接为一整张地图；

(3)因为车辆绝对坐标已知，各标志物相对车辆的位置已知，则标志物的绝对坐标已知，使用arcgis对整张地图进行标注；

(4)保存地图数据并使用arcgis输出地图的点线面数据；

(5)将地图数据转换为OpenDRIVE的xml文件，并将车辆定位信息导入其中；

步骤二、在Astar算法中引入车辆运动学模型，并根据车辆运动学模型计算出的车辆定位信息，并按照步骤一的方式将其导入OpenDRIVE的xml文件中，所述引入车辆运动学模型为以后轴为原点的车辆运动学模型，其可以简化为二维平面上运动的刚体结构，所述车辆运动学模型的构建过程为：

(1)假设任意时刻车辆的状态为q＝(x,y,θ)，车辆坐标的原点位于后轴的中心位置，坐标轴与车身平行；

(2)构建车辆运动模型：

式中：s表示车辆的速度，φ表示steering angle(左为正，右为负)，L表示前轮和后轮的距离，如果φ保持不变，车辆就会在原地转圈，半径为ρ；

(3)对车辆运动模型进行推导变形；在一个非常短的时间Δt内，可以近似认为车辆按照车身的方向运动，用d_x、d_y表示在d_t时间内车辆在x轴、y轴移动的距离，则有：

tanθ＝sinθ/cosθ

用ω表示Δt时间内车身运动的距离，则有：

d_ω＝ρdθ

ρ＝L/tanφ

由上述公式变形合并，可得：

对等式两侧除以d_t，并且基于

的条件可以最终得到等式：

整理可得车辆运动学模型为：

基于上述车辆运动学模型，在给定了某个时刻的控制输入(α，φ)以后，可以估算车辆在下一时刻的状态信息；

步骤三、使用Astar算法在arcgis导出的地图数据中查找出最优路径点，并使用RS曲线与Astar算法所查找出的最优路径点相结合生成车辆可跟踪的运动轨迹；

步骤四、用arcgis的点线面数据的路径点与步骤二所构造的OpenDRIVE的xml文件相匹配；

步骤五、对匹配后的结果使用GWO算法进行安全性与舒适性的优化，选出最优路径；

步骤六、在OpenDRIVE文件中选取出最优路径后，可改变相应最优路径上的road等的颜色，使用LGSVL模拟器等进行可视化。

优选地，所述步骤五中GWO算法的优化方法为：

(1)引入评价指标，给OpenDRIVE文件中的road、junction等引入可通行行指标α，安全性指标β，舒适性指标δ，将(α，β，δ)作为GWO的优化过程中的解进行指导优化；

(2)使用灰狼算法建立如下数学模型，并进行种群迭代计算：

D＝C·X_P(t)-X(t)

X(t+1)＝X_P(t)-A·D

A＝2a·r₁-a

C＝2r₂

其中，t为当前迭代次数，表示hadamard乘积操作；A和C是协同系数向量；X_p表示猎物的位置向量；X(t)表示当前灰狼的位置向量；在整个迭代过程中a由2线性降到0；r₁和r₂是[0，1]中的随机向量；

(3)在每次迭代过程中，保留当前种群中的最好三只灰狼(α，β，δ),然后根据们的位置信息来更新其它搜索代理(包括ω)的位置，将该行为的数学模型进行如下表示：

D_α＝C₁·X_α-X

D_β＝C₂·X_β-X

D_δ＝C₃·X_δ-X

X₁＝X_α-A₁·D_α

X₂＝X_β-A₂·D_β

X₃＝X_δ-A₃·D_δ

式中：X_α、X_β、X_δ分别表示当前种群中α，β，δ的位置向量；X表示灰狼的位置向量；D_α、D_β、D_δ分别表示当前候选灰狼与最优三条狼之间的距离；当|A|>1时，灰狼之间尽量分散在各区域并搜寻猎物；当|A|<1时，灰狼将集中捜索某个或某些区域的猎物。

优选地，所述步骤三中Astar算法在arcgis导出的地图数据中查找出最优路径点的方法为：选取arcgis输出的地图点线面数据中的点，再依次将其放入到Astar算法中的open list列表里进行计算。

优选地，所述步骤三中使用RS曲线与Astar算法所查找出的最优路径点相结合生成车辆可跟踪的运动轨迹的方法为：使用RS曲线基于Astar算法搜索出的最优路径点来向外输出路径，具体的。

优选地，所述步骤一种的数据采集设备为带有Ladybag3.5全景相机及GPS组合惯导的数据采集车。

本发明的技术效果和优点：本发明通过全景视觉相机采集图像数据，GPS采集车辆定位信息，并将图像信息与定位信息进行数据融合，将融合后的数据使用arcgis进行数据整理与标注，参考OpenDRIVE地图格式构建高精度地图，从而实现高精度地图完成路径规划。

附图说明

图1为本发明的流程图；

图2为本发明Astar算法流程图；

图3为本发明RS曲线输出路径流程图。

具体实施方式

为了使本发明的实现技术手段、创作特征、达成目的与功效易于明白了解，下面结合具体图示，进一步阐述本发明，在本发明的描述中，需要说明的是，除非另有明确的规定和限定，术语“安装”、“连”、“连接”应做广义理解，例如，可以是固定连接，也可以是可拆卸连接，或一体地连接或是机械连接，也可以是电连接；可以是直接相连，也可以通过中间媒介间接相连，可以两个元件内部的连通。

实施例

如图1所示基于改进Astar和灰狼算法结合的地图路径规划方法，具体包括移动以下步骤：

步骤一、基于数据采集设备构建高精度地图，所述数据采集设备包括有Ladybag3.5全景相机及GPS组合惯导的数据采集车，具体构建方法为：

(1)使用采集车每前进10米停车采集一张图片以及与图片相对应的车辆此刻GPS数据；

(2)将图片进行透视变化并使用PS等拼接为一整张地图；

(3)因为车辆绝对坐标已知，各标志物相对车辆的位置已知，则标志物的绝对坐标已知，使用arcgis对整张地图进行标注；

(4)保存地图数据并使用arcgis输出地图的点线面数据；

(5)将地图数据转换为OpenDRIVE的xml文件，并将车辆定位信息导入其中；

步骤二、在Astar算法中引入车辆运动学模型，并根据车辆运动学模型计算出的车辆定位信息，并按照步骤一的方式将其导入OpenDRIVE的xml文件中，所述引入车辆运动学模型为以后轴为原点的车辆运动学模型，其可以简化为二维平面上运动的刚体结构，所述车辆运动学模型的构建过程为：

(1)假设任意时刻车辆的状态为q＝(x,y,θ)，车辆坐标的原点位于后轴的中心位置，坐标轴与车身平行；

(2)构建车辆运动模型：

式中：s表示车辆的速度，φ表示steering angle(左为正，右为负)，L表示前轮和后轮的距离，如果φ保持不变，车辆就会在原地转圈，半径为ρ；

(3)对车辆运动模型进行推导变形；在一个非常短的时间Δt内，可以近似认为车辆按照车身的方向运动，用d_x、d_y表示在d_t时间内车辆在x轴、y轴移动的距离，则有：

tanθ＝sinθ/cosθ

用ω表示Δt时间内车身运动的距离，则有：

d_ω＝ρdθ

ρ＝L/tanφ

由上述公式变形合并，可得：

对等式两侧除以d_t，并且基于

的条件可以最终得到等式：

整理可得车辆运动学模型为：

基于上述车辆运动学模型，在给定了某个时刻的控制输入(α，φ)以后，可以估算车辆在下一时刻的状态信息；

步骤三、使用Astar算法在arcgis导出的地图数据中查找出最优路径点，并使用RS曲线与Astar算法所查找出的最优路径点相结合生成车辆可跟踪的运动轨迹，具体方法为：选取arcgis输出的地图点线面数据中的点，再依次将其放入到图2所示的Astar算法流程中的openlist列表里进行计算；再使用RS曲线基于Astar算法搜索出的最优路径点来向外输出路径，具体RS曲线输出路径过程如图3所示；

步骤四、用arcgis的点线面数据的路径点与步骤二所构造的OpenDRIVE的xml文件相匹配；OpenDRIVE格式为使用可扩展标记语言(XML)语法描述基于轨迹的道路网络提供了通用基础。存储在OpenDRIVE文件中的数据以分析的方式描述了道路的几何结构以及影响逻辑的道路沿线特征(如车道、标志、信号)。它允许使用真实道路中的相同元素(直线、曲线、回旋线、超高剖面、车道、信号等)精确描述模拟道路。它与左右手驾驶规则兼容，并且可以根据信号功能进行本地化。

步骤五、对匹配后的结果使用GWO算法进行安全性与舒适性的优化，选出最优路径，所述GWO算法的优化方法为：

(1)引入评价指标，给OpenDRIVE文件中的road、junction等引入可通行行指标α，安全性指标β，舒适性指标δ，将(α，β，δ)作为GWO的优化过程中的解进行指导优化；

(2)使用灰狼算法建立如下数学模型，并进行种群迭代计算：

D＝C·X_P(t)-X(t)

X(t+1)＝X_P(t)-A·D

A＝2a·r₁-a

C＝2r₂

其中，t为当前迭代次数，表示hadamard乘积操作；A和C是协同系数向量；X_p表示猎物的位置向量；X(t)表示当前灰狼的位置向量；在整个迭代过程中a由2线性降到0；r₁和r₂是[0，1]中的随机向量；

(3)在每次迭代过程中，保留当前种群中的最好三只灰狼(α，β，δ),然后根据们的位置信息来更新其它搜索代理(包括ω)的位置，将该行为的数学模型进行如下表示：

D_α＝C₁·X_α-X

D_β＝C₂·X_β-X

D_δ＝C₃·X_δ-X

X₁＝X_α-A₁·D_α

X₂＝X_β-A₂·D_β

X₃＝X_δ-A₃·D_δ

式中：X_α、X_β、X_δ分别表示当前种群中α，β，δ的位置向量；X表示灰狼的位置向量；D_α、D_β、D_δ分别表示当前候选灰狼与最优三条狼之间的距离；当|A|>1时，灰狼之间尽量分散在各区域并搜寻猎物；当|A|<1时，灰狼将集中捜索某个或某些区域的猎物；在构建灰狼算法的攻击猎物模型的过程中，a值的减少会引起A的值也随之波动。换句话说，A是一个在区间[-a，a]上的随机向量，其中a在迭代过程中呈线性下降。当A在[-1，1]区间上时，则捜索代理(Search Agent)的下一时刻位置可以在当前灰狼与猎物之间的任何位置上。

灰狼主要依赖α，β，δ的信息来寻找猎物。它们开始分散地去搜索猎物位置信息，然后集中起来攻击猎物。对于分散模型的建立，通过|A|＞1使其捜索代理远离猎物，这种搜索方式使GWO能进行全局搜索。GWO算法中的另一个搜索系数是C。C向量是在区间范围[0，2]上的随机值构成的向量，此系数为猎物提供了随机权重，以便増加(|C|＞1)或减少(|C|＜1)。这有助于GWO在优化过程中展示出随机搜索行为，以避免算法陷入局部最优。值得注意的是，C并不是线性下降的，C在迭代过程中是随机值，该系数有利于算法跳出局部，特别是算法在迭代的后期显得尤为重要。

步骤六、在OpenDRIVE文件中选取出最优路径后，可改变相应最优路径上的road等的颜色，使用LGSVL模拟器等进行可视化。

最后应说明的是：以上所述仅为本发明的优选实施例而已，并不用于限制本发明，尽管参照前述实施例对本发明进行了详细的说明，对于本领域的技术人员来说，其依然可以对前述各实施例所记载的技术方案进行修改，或者对其中部分技术特征进行等同替换,凡在本发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。

Claims (4)

1.基于改进Astar和灰狼算法结合的地图路径规划方法，其特征在于：具体包括移动以下步骤，

步骤一、基于数据采集设备构建高精度地图，数据采集设备包括全景相机及数据采集车，具体构建方法为：

(1)使用采集车每前进10米停车采集一张图片以及与图片相对应的车辆此刻GPS数据；

(2)将图片进行透视变化并使用PS等拼接为一整张地图；

(3)因为车辆绝对坐标已知，各标志物相对车辆的位置已知，则标志物的绝对坐标已知，使用arcgis对整张地图进行标注；

(4)保存地图数据并使用arcgis输出地图的点线面数据；

(5)将地图数据转换为OpenDRIVE的xml文件，并将车辆定位信息导入其中；

步骤二、在Astar算法中引入车辆运动学模型，并根据车辆运动学模型计算出的车辆定位信息，并按照步骤一的方式将其导入OpenDRIVE的xml文件中，所述引入车辆运动学模型为以后轴为原点的车辆运动学模型，其可以简化为二维平面上运动的刚体结构，所述车辆运动学模型的构建过程为：

(1)假设任意时刻车辆的状态为q＝(x,y,θ)，车辆坐标的原点位于后轴的中心位置，坐标轴与车身平行；

(2)构建车辆运动模型：

式中：s表示车辆的速度，φ表示steering angle(左为正，右为负)，L表示前轮和后轮的距离，如果φ保持不变，车辆就会在原地转圈，半径为ρ；

(3)对车辆运动模型进行推导变形；在一个非常短的时间Δt内，可以近似认为车辆按照车身的方向运动，用d_x、d_y表示在d_t时间内车辆在x轴、y轴移动的距离，则有：

tanθ＝sinθ/cosθ

用ω表示Δt时间内车身运动的距离，则有：

d_ω＝ρdθ

ρ＝L/tanφ

由上述公式变形合并，可得：

对等式两侧除以d_t，并且基于

的条件可以最终得到等式：

整理可得车辆运动学模型为：

基于上述车辆运动学模型，在给定了某个时刻的控制输入(α，φ)以后，可以估算车辆在下一时刻的状态信息；

步骤三、使用Astar算法在arcgis导出的地图数据中查找出最优路径点，并使用RS曲线与Astar算法所查找出的最优路径点相结合生成车辆可跟踪的运动轨迹；

步骤四、用arcgis的点线面数据的路径点与步骤二所构造的OpenDRIVE的xml文件相匹配；

步骤五、对匹配后的结果使用GWO算法进行安全性与舒适性的优化，选出最优路径；

步骤六、在OpenDRIVE文件中选取出最优路径后，可改变相应最优路径上的road等的颜色，使用LGSVL模拟器等进行可视化。

优选地，所述步骤五中GWO算法的优化方法为：

(1)引入评价指标，给OpenDRIVE文件中的road、junction等引入可通行行指标α，安全性指标β，舒适性指标δ，将(α，β，δ)作为GWO的优化过程中的解进行指导优化；

(2)使用灰狼算法建立如下数学模型，并进行种群迭代计算：

D＝C·X_P(t)-X(t)

X(t+1)＝X_P(t)-A·D

A＝2a·r₁-a

C＝2r₂

其中，t为当前迭代次数，表示hadamard乘积操作；A和C是协同系数向量；X_p表示猎物的位置向量；X(t)表示当前灰狼的位置向量；在整个迭代过程中a由2线性降到0；r₁和r₂是[0，1]中的随机向量；

(3)在每次迭代过程中，保留当前种群中的最好三只灰狼(α，β，δ),然后根据们的位置信息来更新其它搜索代理(包括ω)的位置，将该行为的数学模型进行如下表示：

D_α＝C₁·X_α-X

D_β＝C₂·X_β-X

D_δ＝C₃·X_δ-X

X₁＝X_α-A₁·D_α

X₂＝X_β-A₂·D_β

X₃＝X_δ-A₃·D_δ

式中：X_α、X_β、X_δ分别表示当前种群中α，β，δ的位置向量；X表示灰狼的位置向量；D_α、D_β、D_δ分别表示当前候选灰狼与最优三条狼之间的距离；当|A|>1时，灰狼之间尽量分散在各区域并搜寻猎物；当|A|<1时，灰狼将集中捜索某个或某些区域的猎物。

2.根据权利要求1所述的基于改进Astar和灰狼算法结合的地图路径规划方法，其特征在于：所述步骤三中Astar算法在arcgis导出的地图数据中查找出最优路径点的方法为：选取arcgis输出的地图点线面数据中的点，再依次将其放入到Astar算法中的open list列表里进行计算。

3.根据权利要求1所述的基于改进Astar和灰狼算法结合的地图路径规划方法，其特征在于：所述步骤三中使用RS曲线与Astar算法所查找出的最优路径点相结合生成车辆可跟踪的运动轨迹的方法为：使用RS曲线基于Astar算法搜索出的最优路径点来向外输出路径，具体的。

4.根据权利要求1所述的基于改进Astar和灰狼算法结合的地图路径规划方法，其特征在于：所述步骤一种的数据采集设备为带有Ladybag3.5全景相机及GPS组合惯导的数据采集车。

Images