一种基于改进混合A*算法的车头泊入路径重规划方法技术

本发明专利技术公开了一种基于改进混合A*算法的车头泊入路径重规划方法：利用超声波雷达获取车位周围障碍物信息；利用摄像头视觉识别算法获取目标停车位位置信息；利用组合惯导设备获取车辆坐标位置信息；利用相切圆规划方法生成车辆泊出车位的参考路径；利用评价函数选取参考路径中与车辆当前姿态接近的目标点；利用混合A*算法根据起始点和目标点、根据周围障碍物信息搜索得到路径；利用曲率平滑方法得到完整平滑的泊车路径。本发明专利技术解决了现有混合A*算法搜索时间长，搜索路径曲率不连续的问题，可以实现实时高效规划泊车路径，提高了混合A*算法搜索路径的适应性，能够在多种重规划环境下保证规划得到平滑的泊车路径。证规划得到平滑的泊车路径。证规划得到平滑的泊车路径。

【技术实现步骤摘要】
一种基于改进混合A*算法的车头泊入路径重规划方法


[0001]本专利技术涉及自动泊车路径规划领域领域，尤其涉及一种基于改进混合A*算法的车头泊入路径重规划方法。

技术介绍

[0002]汽车保有量持续增多，随之而来的是交通环境拥挤，城市停车位资源紧张，停车位空间小等问题。
[0003]自动泊车技术的出现和发展为解决泊车问题提供了新思路，让泊车变得更智能、更安全、更便捷，将有效解决驾驶员找车位难、停车难、找车难等痛点。
[0004]自主泊车系统的路径规划功能主要是根据车辆周围环境信息和目标停车位位置规划出车辆在不与周围物体发生碰撞的情况下，从当前位置驶入目标停车位的路径，规划得到的路径尽可能短而且路径曲率比较平滑。
[0005]现有的路径规划方法大多考虑车辆起始位置在停车位附近，而自动泊车过程中车辆周围可能出现障碍物导致泊车过程中断，发生这种情况后车辆将无法沿着原有的泊车路径行驶，需要重新规划泊车路径，即路径规划模块的重规划功能。
[0006]混合A*算法是路径规划常用的规划算法，混合A*算法引用了启发信息搜索路径效率较高，根据车辆动力学模型搜索得到的泊车路径可以满足车辆的动力学约束，通过设计合理的启发函数可以提高混合A*算法的搜索效率。
[0007]由于车辆在泊车过程中触发重规划的位置是随机的，而且停车位周围的障碍物位置也会发生变化，当泊车场景发生变化后，传统的混合A*算法无法保证搜索效率，容易在启发值接近的区域反复搜索，搜索得到的路径存在曲率不连续等问题。
[0008]因此，如何提高混合A*算法的搜索效率，避免在启发值接近的区域反复搜索为改进混合A*算法提供了新的思路。

技术实现思路

[0009]本专利技术的目的在于克服传统混合A*算法的缺点和不足，提供一种基于改进混合A*算法的车头泊入路径重规划方法。本专利技术解决了现有混合A*算法搜索时间长，搜索路径曲率不连续的问题，可以实现实时高效规划泊车路径，提高了混合A*算法搜索路径的适应性，能够在多种重规划环境下保证规划得到平滑的泊车路径。
[0010]本专利技术设计相切圆规划方法得到车辆从停车位驶出的参考路径，相切圆规划方法能适应泊车场景变化。
[0011]本专利技术设计评价函数选取相切圆路径点作为混合A*算法搜索的目标点，提高混合A*算法搜索效率。
[0012]本专利技术设计曲率平滑方法将改进混合A*搜索路径输出为平滑路径，解决了混合A*算法搜索路径曲率不连续问题。
[0013]本专利技术通过下述技术方案实现：
[0014]一种基于改进混合A*算法的车头泊入路径重规划方法，包括以下步骤：
[0015]S1、输入车辆起点位置姿态，初始化探索节点列表OpenList和封闭节点列表CloseList，将车辆起点作为初始节点放入OpenList中；
[0016]S2、根据车位坐标位置获取目标停车位置，利用相切圆规划方法得到车辆从车位内驶出的参考路径，相切圆规划方法的具体步骤如下：
[0017]S2.1、计算车辆与车位框线相切泊出的圆弧半径R，根据圆弧公式计算车辆倒车驶出停车位的位置，作为下一步圆弧规划的起点；
[0018]S2.2、重复进行如下步骤：
[0019]S2.2.1、切换车辆档位D，利用圆弧公式计算车辆以最小转弯半径r从当前位置行驶至车身姿态与车位平行的位置的圆弧路径；
[0020]S2.2.2、遍历圆弧路径点判断是否与障碍物发生碰撞，并记录发生碰撞的路径点；
[0021]S2.2.3、保留圆弧起点到不碰撞的圆弧路径点之间的圆弧路径作为相切圆路径，并将不发生碰撞的圆弧路径点更新为下一步圆弧规划起点；
[0022]S2.3、直到车辆在圆弧路径终点的车身姿态与车位平行，返回相切圆路径；
[0023]S3、根据评价函数J(i)得到参考路径中与车辆起点位置接近的路径点，作为混合A*算法搜索的目标点Target；
[0024]S4、利用混合A*算法，并根据周围障碍物位置信息，搜索得到一条无碰撞泊车路径。具体包括如下步骤：
[0025]S4.1、重复进行如下步骤：
[0026]S4.1.1、在OpenList中选取代价值F(n)最小的节点作为父节点tnode，将tnode放入CloseList中；
[0027]S4.1.2、判断tnode是否为搜索的目标点Target，如果是目标点则输出搜索路径，如果不是则进入下一步；
[0028]S4.1.3、判断是否可以利用Reeds
‑
Shepp路径规划得到从当前节点tnode到达目标点Target的路径，如果可以则输出混合A*算法搜索路径和RS路径，如果不可以则进入下一步；
[0029]S4.1.4、利用车辆动力学公式，给车辆固定速度，不同的轮胎转角计算得到tnode能到达的下一节点NextNode，并将不发生碰撞的NextNode放入OpenList中；
[0030]S4.2、直指搜索得到目标节点Target，退出循环；
[0031]S5、拼接混合A*算法搜索路径和相切圆路径，如果搜索路径中包含RS路径，则拼接混合A*算法搜索路径、RS路径和相切圆路径。
[0032]S6、将拼接路径利用曲率平滑方法得到从车辆起点位置姿态行驶至目标停车位置姿态的一条完整平滑的路径。
[0033]上述步骤S1中，起点位置姿态包括车辆位置的x0，y0坐标，车辆航向角θ0，车轮转角δ0和车辆档位D0，采用的坐标系为大地坐标系。
[0034]上述步骤S2中，车辆与车位框线相切泊出的圆弧半径R的计算公式如下：
[0035]ΔS＝Carparking.W/2
‑
threshold；
[0036]R＝(Vehicle.LF2+Vehicle.W2/4
‑
ΔS2)/(2*(ΔS
‑
Vehicle.W/2))；
[0037]其中Carparking.W为停车位宽度，threshold为安全距离阈值，Vehicle.LF为车辆
前悬加上车辆轴距的长度，Vehicle.W为车身宽度。
[0038]上述步骤S2中，相切圆规划方法中采用的圆弧计算公式如下：
[0039]x
i
＝x0+R(cosα0‑
cosα
i
)；
[0040]y
i
＝y0+R(sinα
i
‑
sinα0)；
[0041][0042][0043]其中(x0，y0，θ0)为圆弧规划起点，(x
i
，y
i
，θ
i
)为圆弧规划终点。α0，α1为圆弧起点和终点的圆心角。圆弧计算公式采用的坐标系为大地坐标系。
[0044]上述步骤S3中，评价函数为J(i)＝k1(x
i
‑
x0)+k2(y
i
‑本文档来自技高网...

【技术保护点】

【技术特征摘要】
1.一种基于改进混合A*算法的车头泊入路径重规划方法，其特征在于，包括以下步骤：S1、输入车辆起点位置姿态，初始化探索节点列表OpenList和封闭节点列表CloseList，将车辆起点作为初始节点放入OpenList中；S2、根据车位坐标位置获取目标停车位置，利用相切圆规划方法得到车辆从车位内驶出的参考路径，相切圆规划方法的具体步骤如下：S2.1、计算车辆与车位框线相切泊出的圆弧半径R，根据圆弧公式计算车辆倒车驶出停车位的位置，作为下一步圆弧规划的起点；S2.2、重复进行如下步骤：S2.2.1、切换车辆档位D，利用圆弧公式计算车辆以最小转弯半径r从当前位置行驶至车身姿态与车位平行的位置的圆弧路径；S2.2.2、遍历圆弧路径点判断是否与障碍物发生碰撞，并记录发生碰撞的路径点；S2.2.3、保留圆弧起点到不碰撞的圆弧路径点之间的圆弧路径作为相切圆路径，并将不发生碰撞的圆弧路径点更新为下一步圆弧规划起点；S2.3、直到车辆在圆弧路径终点的车身姿态与车位平行，返回相切圆路径；S3、根据评价函数J(i)得到参考路径中与车辆起点位置接近的路径点，作为混合A*算法搜索的目标点Target；S4、利用混合A*算法，并根据周围障碍物位置信息，搜索得到一条无碰撞泊车路径；具体包括如下步骤：S4.1、重复进行如下步骤：S4.1.1、在OpenList中选取代价值F(n)最小的节点作为父节点tnode，将tnode放入CloseList中；S4.1.2、判断tnode是否为搜索的目标点Target，若是目标点则输出搜索路径，若不是则进入下一步；S4.1.3、判断是否可利用Reeds
‑
Shepp路径规划得到从当前节点tnode到达目标点Target的路径，若是，则输出混合A*算法搜索路径和RS路径，若不可以则进入下一步；S4.1.4、利用车辆动力学公式，给车辆固定速度，不同的轮胎转角计算得到tnode能到达的下一节点NextNode，并将不发生碰撞的NextNode放入OpenList中；S4.2、直指搜索得到目标节点Target，退出循环；S5、拼接混合A*算法搜索路径和相切圆路径，若搜索路径中包含RS路径，则拼接混合A*算法搜索路径、RS路径和相切圆路径；S6、将拼接路径利用曲率平滑方法得到从车辆起点位置姿态行驶至目标停车位置姿态的一条完整平滑的路径。2.根据权利要求1所述基于改进混合A*算法的车头泊入路径重规划方法，其特征在于：步骤S1中，起点位置姿态包括车辆位置的x0，y0坐标，车辆航向角θ0，车轮转角δ0和车辆档位D0；采用的坐标系为大地坐标系。3.根据权利要求1所述基于改进混合A*算法的车头泊入路径重规划方法，其特征在于，步骤S2中，车辆与车位框线相切泊出的圆弧半径R的计算公式如下：ΔS＝Carparking.W/2
‑
threshold；R＝(Vehicle.LF2+Vehicle.W2/4
‑
Δs2)/(2*(ΔS
‑
Vehicle.W/2))；
其中Carparking.W为停车位宽度，threshold为安全距离阈值，Vehicle.LF为车辆前悬加上车辆轴距的长度，Vehicle.W为车身宽度；步骤S2中，相切圆规划方法中采用的圆弧计算公式如下：x
i
＝x0+R(cosα0‑
cosα
i
)；y
...

【专利技术属性】
技术研发人员：赵克刚，钟浩龙，梁志豪，石翠铎，
申请(专利权)人：华南理工大学，
类型：发明
国别省市：