本发明属于自动驾驶车辆路径规划及其数值仿真，具体涉及一种融合sa-gao与aw-dwa算法的自动驾驶车辆路径规划方法。
背景技术：
1、智能交通体系的构建为解决未来道路交通问题带来了新的思路，而自动驾驶车辆作为其重要的组成部分，因其智能程度高、控制精度高等优点，近些年来在全世界范围内备受关注。自动驾驶车辆根据其对运行环境信息的感知程度，通常采用以下两种路径规划算法：全局路径规划算法和局部路径规划算法。全局路径规划算法需要预先获取全局环境中的所有信息，并依此来进行车辆的全局性静态路径规划；局部路径规划算法则主要依靠车载传感器或gps定位系统实时采集车辆周边的环境信息，确定本车及周围障碍物的位置及运动数据，再结合整个环境地图来实施车辆避障的局部动态路径规划。常用的全局路径规划算法包括rrt算法、prm算法、dijkstra算法、a*算法等。不过这些传统算法所得路径的曲折度较高、不确定性较强且规划效率还有待提高。巨型犰狳算法(giantarmadillooptimization,gao)是一种新近出现的智能寻优算法，该算法在搜索解过程中表现出强大的探索、开发及路径寻优能力。且相比于粒子群、遗传等优化算法，gao算法并无涉及控制参数的调试，属于无参数化设计，这使得gao算法能够更好地适用于各种复杂场景下自动驾驶车辆的全局路径规划。不过值得一提的是，此类算法依然存在陷入局部极小值问题。同时，为躲避动态障碍物，车辆需在行驶过程中具备自主局部避障的能力，因此集成于车辆的局部路径规划算法就显得至关重要了。目前，广泛使用的局部路径规划算法包括遗传算法、人工势场法和动态窗口法(dynamic windowapproach,dwa)等。相比于其他方法，dwa方法将车辆的动态特性考虑进来，凭借其在控制过程中可对车辆的最大速度、最大转角等变量的实时约束，将避障问题转变成空间中的运动约束问题，这样可以通过运动约束条件选择局部最优的路径，因此在车辆局部路径规划中得到了广泛的应用。不过值得一提的是，传统dwa方法虽可实现车辆安全避障，但其选择的都是车辆下一步的最佳路径而非全局最优路径，因此有必要对此方法进行改进。
技术实现思路
1、为此，本发明提出一种融合sa-gao与aw-dwa算法的自动驾驶车辆路径规划方法。为更好地避免巨型犰狳算法(gao)在寻优过程中陷入局部最小值问题，引入模拟退火算法(sa)对其进行改进，并利用改进后的sa-gao算法对车辆的全局路径进行规划；为使车辆能够更好地适应未来多变复杂的动态有障环境，提出一种用于车辆局部路径规划的自适应权重动态窗口法(aw-dwa)。将sa-gao与aw-dwa算法进行有机融合，在全局路径上选取适当数量的关键点作为动态窗口法的候选子目标点，以保证aw-dwa算法能够使车辆有效避开随机动态障碍物的同时又可以快速、稳定地返回至全局最优路径上，并最终为自动驾驶车辆规划出一条安全、最优的行车路径。
2、本发明解决其技术问题具体采用的技术方案是：
3、一种融合sa-gao与aw-dwa算法的自动驾驶车辆路径规划方法，利用模拟退火算法sa对巨型犰狳算法gao进行改进，以改进后的sa-gao算法对车辆的全局路径进行规划；并采用自适应权重动态窗口aw-dwa算法进行车辆局部路径规划；通过在全局路径上选取若干关键点作为动态窗口法的候选子目标点，实现车辆避开随机动态障碍物并返回至全局最优路径上。
4、进一步地，aw-dwa算法以sa-gao算法输出的e个全局路径关键点作为其子目标点，从车辆起始点qs(xs,ys)向算法当前第一个子目标点进行局部规划，并记录下规划得到的各路径点信息；再从当前子目标点向算法下一个子目标点进行局部规划，并记录下规划得到的各路径点信息；重复使用aw-dwa算法对剩下的子目标点依次进行规划，直至到达设定的最终目标点qe(xe,ye)，最后输出完整的路径信息。
5、进一步地，采用所述sa-gao算法对车辆的全局路径进行规划的具体方法包括：
6、首先对sa-gao算法进行初始化
7、设sa-gao中的gao算法种群中含有n个巨型犰狳，并对种群进行如下数学建模
8、
9、其中，分别为第个gao成员位置的横、纵坐标矢量，分别为第个gao成员在其搜索空间第d，d＝1,2,…,m维的横、纵坐标，m为搜索空间的总维度；
10、以第个gao成员在其搜索空间内的m个坐标点作为有效控制点，利用贝塞尔曲线生成原理在车辆起点qs(xs,ys)和终点qe(xe,ye)之间生成一条受搜索空间约束的拟合曲线，并提取曲线中等距横坐标及相应纵坐标组成的p个点，并将这些点视为第个gao成员所对应的车辆中间路径点
11、
12、gao算法中每个gao成员的位置代表问题的一个候选解种群中n个巨型犰狳成员的适应度函数值描述为：
13、
14、其中，f表示适应度函数矢量，表示第n个gao成员所对应的适应度函数值；
15、设gao算法的最大迭代次数为gmax且当前迭代数gnow初始化为1，并随机初始化gao算法中各种群成员在求解空间中的位置为：
16、
17、其中，分别为搜索解空间中横坐标的上、下界矢量；
18、分别为搜索解空间中纵坐标的上、下界矢量；r为在区间[0,1]中的随机数；
19、所述sa-gao算法利用sa算法中的退火温度控制来使算法求解过程向最优值的优化方向进行，对于sa算法，初始化其退火温度t0为：
20、
21、其中，fmin为gao算法种群中各成员初始适应度的最小值；
22、然后进行以下迭代计算：
23、在当前迭代代数gnow下，计算gao种群中各巨型犰狳成员的退火突跳概率，即
24、
25、将gao算法种群中各巨型犰狳成员的位置集合认为是候选白蚁丘集合
26、tm＝{tm1,tm2,…,tmn}，其中，表示第个巨型犰狳成员的位置；利用轮盘赌选择策略及退火突跳概率从候选白蚁丘集合tm中选出一个最先满足条件：
27、的白蚁丘作为gao算法的全局最优位置去参与gao种群的更新；其中，r为在区间[0,1]中选取的随机数；表示gao种群中被轮盘赌策略选中的第个成员；
28、利用所述全局最优位置，计算当前迭代代数gnow下gao种群中各成员在搜索阶段的新位置：
29、
30、其中，第个巨型犰狳在搜索阶段新位置的横、纵坐标矢量；分别表示白蚁丘的横、纵坐标矢量；
31、根据搜索阶段新位置与其原位置所对应的适应度函数值对比，对种群中各成员的位置进行实际更新：
32、
33、其中，表示第个巨型犰狳在搜索阶段新位置所对应的适应度函数值；
34、基于巨型犰狳对白蚁丘的挖掘技能，计算在当前迭代代数gnow下gao种群中各成员在挖掘阶段的新位置：
35、
36、其中，分别为第个巨型犰狳在挖掘阶段新位置的横、纵坐标矢量；
37、根据各成员在挖掘阶段新位置与其原位置所对应的适应度函数值对比，对种群中各成员的位置进行再次更新：
38、
39、其中，表示第个巨型犰狳在挖掘阶段新位置所对应的适应度函数值；
40、再进行退火操作
41、t＝lt0
42、并将退火完后得到的温度t重新赋值给t0，即t0＝t；其中，l>0为退火系数；
43、判断gao算法中的gnow是否达到最大迭代代数gmax，若未达到，则继续迭代计算进行优化，将上一轮迭代计算得到的gao种群成员挖掘阶段新位置以及退火温度t0作为sa-gao算法探索和挖掘阶段的初始解开始新的算法迭代，此时迭代代数gnow在原有基础上加1；若达到，则算法结束，输出gao成员的位置以及所插值拟合得到的一系列全局路径点。
44、进一步地，所述sa-gao算法的适应度函数为：
45、f(x1,y1,…,xp,yp)＝l×(1+k×violation)
46、其中，k是一个加权系数，用于平衡路径长度和违规程度对适应度函数的影响；
47、路径长度评价函数为：
48、
49、其中，(xi,yi)、(xi+1,yi+1)分别为车辆相邻路径点qi和qi+1的位置坐标；
50、安全性评价函数采用违规函数violation描述，即：
51、
52、其中，表示路径点qi与所有障碍物的违规程度平均值，vij表示路径点qi与障碍物j的违规程度，r0表示障碍物进行圆形膨胀处理后的半径与障碍物半径的差值；
53、设车辆起点为qs(xs,ys)，终点为qe(xe,ye)，并路径规划算法得到的中间路径信息通过路径点q1,q2,…,qp表示；其中，qi(xi,yi)，i＝1,2,…,p表示规划算法得到的第i个中间路径点，p表示中间路径点的总数；
54、设(0xjobs,0yjobs)分别表示全局路径规划时第j，j＝1,2,...,n个障碍物初始位置的横、纵坐标，n为障碍物总数；对于障碍物j而言，其到第i个路径点qi的距离dij表示为：
55、
56、其中，rjobs为第j，j＝1,2,...,n个障碍物的半径。
57、进一步地，aw-dwa算法为满足约束的可行前轮转角δf空间内搜索可选转角命令的规划方法，采用自适应权重动态窗口法aw-dwa进行车辆局部路径规划的方法具体为：
58、首先对aw-dwa算法的参数进行初始化；
59、然后在预测时间内，生成不同速度与转角组合所对应的备选运动轨迹；
60、最后通过评价函数对备选预测轨迹进行评估，并按照评价函数值越大备选轨迹越优的原则选出一条最优轨迹；通过基于车辆与障碍物距离的自适应权重调节方法，在车辆靠近障碍物时增大障碍物评价函数的权重，从而使规划路径更倾向于避开障碍物，进而生成路径。
61、进一步地，所述对aw-dwa算法的参数进行初始化具体为：对车辆的状态信息，包括：车辆的位置(x,y)、纵向速度vx、航向角横摆角速度ωr、前轮转角δf及采样间隔时间dt，向前预测时域tpre进行初始化，其中tpre为dt的整数倍；以及设定车辆的机械特性参数，包括最大横摆角速度ωrmax、最大转角δfmax以及最小转角δfmin。
62、进一步地，所述在预测时间内，生成不同速度与转角组合所对应的备选运动轨迹的方法具体为：
63、若车辆纵向速度恒定，aw-dwa轨迹规划搜索的命令集包括当前车辆前轮转角δf的命令集，且采样间隔时间dt后的前轮转角极限范围为|[[δf-ωrmaxdt,δf+ωrmaxdt]]|；综合考虑车辆动力学与机械特性的前轮转角可选命令集范围为|[[max(δfmin,δf-ωrmaxdt),min(δfmax,δf+ωrmaxdt)]]|；若设前轮转角变化的分辨率为dδf，则aw-dwa局部路径规划算法在当前窗口的采样点数量为利用这些采样点所对应的纵向速度和前轮转角并结合离散化车辆动力学模型，以预测出时域tpre内的g条备选预测轨迹。
64、进一步地，所述离散化车辆动力学模型的构建方法为：
65、基于车辆坐标系o-xy和大地坐标系o-xy；其中，车辆坐标系o-xy以车辆质心o为原点，x轴正向沿车辆正前方方向，y轴与x轴垂直且正向沿车辆左侧；大地坐标系o-xy为惯性参考坐标系，其x轴正向沿水平方向向右，y轴正向为x轴绕垂直于地面的轴线逆时针旋转90度；若设车辆的航向角为根据车辆的几何位置关系得：
66、
67、其中，分别表示车辆沿大地坐标系x轴和y轴方向上的速度，vx、vy分别表示车辆的纵向速度和横向速度；
68、基于前轮转角的小角度假设，利用牛顿力学方法建立车辆的二自由度横向动力学模型：
69、
70、其中，δf为车辆的前轮转角，为车辆的横摆角速度，m0为车辆的整车质量，iz为车辆的转动惯量，a、b分别为车辆质心到前、后轴的距离，ccf、ccr分别为车辆前、后轮的侧偏刚度；
71、使用前向欧拉法，以采样间隔时长dt,将二自由度动力学模型写成离散形式的方程：
72、
73、其中，k表示当前第k时刻，k+1表示k时刻的下一时刻。
74、进一步地，aw-dwa算法对各备选轨迹的评价函数为：
75、
76、其中，分别表示第c，c＝1,2,…,g条预测轨迹所对应的经过归一化后得到的障碍物距离评价函数及航向评价函数；β表示权重系数；
77、计算在预测时域末时刻车辆与行车环境中各障碍物的距离：
78、
79、其中，xc(kpre)、yc(kpre)分别表示预测时域末时刻车辆位置的横、纵坐标；kpre表示预测时域末时刻，xjobs(kpre)和yjobs(kpre)分别表示预测时域末时刻障碍物j的横、纵坐标；
80、基于距离权重系数β的自适应调节方法为：
81、
82、其中，dmin表示车辆与所有障碍物距离的最小值，ζ表示车辆与障碍物间的距离阈值；当dmin大于ζ时，权重系数固定为0.5；否则，权重系数根据指数函数调整权重，即车辆与障碍物的距离越近，权重越大，上限为0.9；
83、使用障碍物距离评价函数obsc(vx,δf)表示当前车辆位置与最近障碍物的距离，即：
84、obsc(vx,δf)＝dmin
85、使用航向评价函数headc(vx,δf)表示预测时域末时刻车辆位置与算法目标点位置连线同预测时域末时刻车辆航向方向夹角的补角，表达式为：
86、
87、其中，和表示预测时域末时刻算法目标点的横、纵坐标；表示预测时域末时刻车辆的航向角；
88、评价子函数的归一化处理为：
89、
90、其中，obsc(vx,δf)、headc(vx,δf)分别为第c，c＝1,2,…,g条预测轨迹的障碍物距离评价函数和航向评价函数；
91、选择评价函数值最大的纵向速度和前轮转角组合，并利用前向欧拉法离散化后的车辆动力学方程更新下一时刻车辆的状态；
92、根据自适应评价函数选择值最大的备选预测轨迹，直至车辆到达目标位置。
93、进一步地，在sa-gao所规划的全局参考路径上选取e个关键点作为aw-dwa算法的候选子目标点，以保证aw-dwa算法能够使车辆有效避开随机动态障碍物的同时能够返回至全局最优路径上；并设定当车辆距离子目标点一个车身的距离时，或其预测时域末时刻横坐标或纵坐标超过子目标点的横坐标或纵坐标时，即判定为车辆到达子目标点，并将车辆到达此子目标点时的终止状态作为其继续前往下一个子目标时的初始状态。
94、相比于现有技术，本发明及其优选方案在车辆全局路径规划方面，通过引入模拟退火算法(sa)对巨型犰狳算法(gao)进行改进，以提高改进后的sa-gao算法避免陷入局部极小值问题的能力，同时更好地保证算法的寻优精度；在车辆局部路径规划方面，提出一种用于车辆局部路径规划的自适应权重动态窗口法(aw-dwa)，以使车辆能够更好地应对未来多变复杂的动态有障行驶环境。将sa-gao与aw-dwa算法进行有机融合，在全局路径上选取适当数量的全局路径关键点作为动态窗口法的候选子目标点，以保证aw-dwa算法能够使车辆有效避开随机动态障碍物的同时又可以快速、稳定地返回至全局最优路径上，并最终为自动驾驶车辆规划出一条安全、最优的行车路径。
1.一种融合sa-gao与aw-dwa算法的自动驾驶车辆路径规划方法，其特征在于：利用模拟退火算法sa对巨型犰狳算法gao进行改进，以改进后的sa-gao算法对车辆的全局路径进行规划；并采用自适应权重动态窗口aw-dwa算法进行车辆局部路径规划；通过在全局路径上选取若干关键点作为动态窗口法的候选子目标点，实现车辆避开随机动态障碍物并返回至全局最优路径上。
2.根据权利要求1所述的基于一种融合sa-gao与aw-dwa算法的自动驾驶车辆路径规划方法，其特征在于：aw-dwa算法以sa-gao算法输出的e个全局路径关键点作为其子目标点，从车辆起始点qs(xs,ys)向算法当前第一个子目标点进行局部规划，并记录下规划得到的各路径点信息；再从当前子目标点向算法下一个子目标点进行局部规划，并记录下规划得到的各路径点信息；重复使用aw-dwa算法对剩下的子目标点依次进行规划，直至到达设定的最终目标点qe(xe,ye)，最后输出完整的路径信息。
3.根据权利要求1所述的一种融合sa-gao与aw-dwa算法的自动驾驶车辆路径规划方法，其特征在于：
4.根据权利要求3所述的一种融合sa-gao与aw-dwa算法的自动驾驶车辆路径规划方法，其特征在于：
5.根据权利要求1所述的一种融合sa-gao与aw-dwa算法的自动驾驶车辆路径规划方法，其特征在于：
6.根据权利要求5所述的一种融合sa-gao与aw-dwa算法的自动驾驶车辆路径规划方法，其特征在于：所述对aw-dwa算法的参数进行初始化具体为：对车辆的状态信息，包括：车辆的位置(x,y)、纵向速度vx、航向角横摆角速度ωr、前轮转角δf及采样间隔时间dt，向前预测时域tpre进行初始化，其中tpre为dt的整数倍；以及设定车辆的机械特性参数，包括最大横摆角速度ωrmax、最大转角δfmax以及最小转角δfmin。
7.根据权利要求5所述的一种融合sa-gao与aw-dwa算法的自动驾驶车辆路径规划方法，其特征在于：
8.根据权利要求5所述的一种融合sa-gao与aw-dwa算法的自动驾驶车辆路径规划方法，其特征在于：
9.根据权利要求5所述的一种融合sa-gao与aw-dwa算法的自动驾驶车辆路径规划方法，其特征在于：
10.根据权利要求1所述的一种融合sa-gao与aw-dwa算法的自动驾驶车辆路径规划方法，其特征在于：在sa-gao所规划的全局参考路径上选取e个关键点作为aw-dwa算法的候选子目标点，以保证aw-dwa算法能够使车辆有效避开随机动态障碍物的同时能够返回至全局最优路径上；并设定当车辆距离子目标点一个车身的距离时，或其预测时域末时刻横坐标或纵坐标超过子目标点的横坐标或纵坐标时，即判定为车辆到达子目标点，并将车辆到达此子目标点时的终止状态作为其继续前往下一个子目标时的初始状态。
