一种智能汽车路径规划及避障跟踪方法

1.本公开涉及智能汽车决策规划领域，具体涉及一种智能汽车路径规划及避障跟踪算法。


背景技术：

2.智能车辆通过车上装备的传感装置感知周围环境，利用人工智能技术模拟人类的驾驶习惯和处理紧急事故的应对方式，避免了人类在极端条件下心理压力对行为能力影响的缺陷，这使得汽车具有自主行驶的能力，让汽车的行驶变得安全可靠。
3.路径跟踪控制是指智能车辆能够跟随路径规划层所得到的路径并使车辆安全稳定地在路面上行驶，路径跟踪控制的主要目的是能够根据智能车辆的运动学和动力学约束，从而输出相应的控制变量，如车轮制动力、前轮转角等。
4.目前的智能汽车规划控制的步骤为先进行全局路径规划，然后进行局部路径规划，最后进行跟踪控制。常用的全局路径规划主要方法有a*算法、遗传算法和蚁群算法。其中，a*算法有易于陷入局部最优、搜索量大的缺点，遗传算法存在计算量庞大、规划时间长，无法满足实时性要求的问题，而蚁群算法搜索能力强但容易陷入局部最优。


技术实现要素：

5.有鉴于此，本发明主要目的在于提供一种智能汽车路径规划及避障跟踪方法。所述方法包括下述步骤：
6.s100、获取路段经纬度、高度坐标，利用高斯投影得到路段平面坐标，进而获取路段位置连接关系；
7.s200、根据路段位置连接关系，设置路段的相交点为节点，建立邻接矩阵，邻接矩阵上的元素表示在交通规则上允许由一个节点驶向另一个节点的路段，元素的值表示两个节点间的距离；
8.s300、利用改进的蚁群算法获取全局路径并输出；
9.s400、对输出的全局路径进行障碍物位置判断，以实现路径局部优化；
10.s500、基于局部优化后的路径，利用车辆动力学方程建立预测模型，获取智能车辆的状态量和控制量，从而实现智能车辆的路径规划及轨迹跟踪。
11.优选地，在所述方法中，述改进的蚁群算法包括下述步骤：
12.s301、将当前循环次数设置为0，当前蚂蚁个数设置为0，设置信息素浓度初始值，循环总数，蚂蚁总数；
13.s302、判断循环次数是否不大于循环总数；若是，则将当前循环次数增加1，执行s303；否则，输出当前最优路径；
14.s303、判断当前蚂蚁编号是不大于蚂蚁总数；若是，执行步骤s304；否则，执行步骤s309；
15.s304、利用改进的启发函数计算候选节点概率；
16.所述改进的启发函数通过下式计算：
[0017][0018]
式中：d
ij
表示路段(i，j)之间的欧式距离，d
je
表示下一个可行节点j和目标节点e之间的欧氏距离；c1和c2是服从0和1之间均匀分布的随机数，c1表示节点j对路径选择的影响的权重，c2表示目标节点e对路径选择影响的权重；s305、利用信息素浓度和启发函数计算候选节点概率，基于所述概率利用轮盘赌原则，确定下一个节点，并将其加入路径中；
[0019]
s306、判断当前蚂蚁是否完成了搜索；若当前蚂蚁完成了搜索，执行s308；若当前蚂蚁未完成搜索，执行s307；
[0020]
s307、判断当前蚂蚁是否还存在可选择的节点，若当前蚂蚁还有可选择的节点，则返回s304；若当前蚂蚁没有可选择的节点，执行s308；
[0021]
s308、将当前蚂蚁编号加1，返回s303；
[0022]
s309、从此次迭代中所有蚂蚁的路径中，获取此次迭代的最优路径；
[0023]
s310、判断此次迭代的最优路径是否为所有迭代中的最优路径；若是将在此次路径上的信息浓度通过下式更新：
[0024]
τ
ij
(t)＝(1-ρ)
×
τ
ij
(t-1) δτ
ij
[0025]
否则，将在此次路径上的信息浓度通过下式更新：
[0026]
τ
ij
(t)＝(1-2ρ)
×
τ
ij
(t-1)
[0027]
上述两个式中：ρ∈(0,0.5)，用于防止信息素无限积累的信息素挥发程度)；；τ
ij
(t)为更新后信息素浓度；δτ
ij
为此轮蚂蚁遗留信息素，计算公式如下：
[0028][0029]
式中：m为每轮迭代蚂蚁总数量，q为信息素增加强度系数，l为当只蚂蚁从起始节点到达目标节点的路径长度；返回s302。
[0030]
优选的，在所述方法中，所述s300之后还包括下述步骤：
[0031]
利用三次b样条曲线拟合，对全局优化路径进行拟合优化后输出，提高道路行驶舒适性。
[0032]
优选的，在所述方法中，所述障碍物局部优化通过下述步骤实现：
[0033]
利用势场法思想建立如下障碍物威胁代价函数：
[0034][0035][0036]
式中：wi表示第i个障碍物在所有障碍物威胁代价中的权重；(x,y)表示当前智能车辆所在位置；(x
obsi
,y
obsi
)表示当前障碍物所在位置；δ为一个设定值，用于防止j
obsi
计算结果为无穷大。
[0037]
优选的，在所述方法中，所述局部优化还包括使跟踪代价与控制代价的总和最小。
[0038]
与现有技术相比：
[0039]
本发明在路径规划中基于改进的蚁群算法，能够快速地生成可行路径，实现路径规划。进一步地，通过对全局路径进行拟合优化，以提高驾驶舒适性。并对障碍物进行了考虑，进行了路径局部优化，躲避障碍物提高驾驶安全性。
附图说明
[0040]
为了更清楚地说明本技术实施例中的技术方案，下面将对实施例描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本技术的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动性的前提下，还可以根据这些附图获得其他的附图。
[0041]
图1、为本发明方法流程示意图；
[0042]
图2、为一个实施例中的经纬度地图示意图；
[0043]
图3、为将图1的经纬度地图转化为平面地图示意图；
[0044]
图4、为本发明中路口的道路节点示意图；
[0045]
图5、为一个实施例中进行路径规划结果示意图；
[0046]
图6、为本发明中涉及的改进的蚁群算法流程示意图。
具体实施方式
[0047]
下面将结合本技术实施例中的附图，对本技术实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本技术一部分实施例，而不是全部的实施例。基于本技术中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本技术保护的范围。
[0048]
在一个实施例中，采用如图1所示的方法实现智能汽车路径规划及避障跟踪方法。所述方法包括下述步骤：
[0049]
s100、获取路段经纬度、高度坐标，利用高斯投影得到路段平面坐标，获取路段位置连接关系；
[0050]
s200、根据路段位置连接关系，设置路段的相交点为节点，建立邻接矩阵，邻接矩阵上的元素表示在交通规则上允许由一个节点驶向另一个节点的路段，元素的值表示两个节点间的距离；
[0051]
s300、利用改进的蚁群算法获取全局路径并输出；
[0052]
s400、对输出的全局路径进行障碍物位置判断，以实现路径局部优化；
[0053]
s500、基于局部优化后的路径，利用车辆动力学方程建立预测模型，获取智能车辆的状态量和控制量，从而实现智能车辆的路径规划及轨迹跟踪。
[0054]
具体实施时，通过惯导设备获取西安80坐标系得到的原始数据，原始数据图如图2所示。该区域的长半轴a＝6378140
±
5(m)，短半轴b＝6356755.2882(m)，扁率α＝1/298.257，第一偏心率平方e2＝0.00669438499959，第二偏心率平方e
′2＝0.00673950181947。将经纬度坐标(l，b)经高斯投影得到路段平面坐标(x，y)。高斯投影正算公式如下：
[0055][0056][0057]
其中：角度都为弧度。
[0058]
b为点的纬度，l”＝l-l0，l为点的经度,l0为中央子午线经度。
[0059]
采用6度带投影，该地区经度为119.5
°
，处于第20带，中央子午线经度l0＝117
°
。
[0060]
n为子午圈曲率半径，t＝tanb，η2＝e
′2cos2b，
[0061]
x为子午线弧长：
[0062][0063]
其中：a0,a2,a4,a6,a8为基本常量，按如下公式计算：
[0064][0065]
m0,m2,m4,m6,m8为基本常量，按如下公式计算：
[0066][0067]
利用上述公式可以求得平面坐标(x，y)，进而获取路段位置连接关系。
[0068]
为了方便分析，可以进一步将高斯投影结果进行平移转换，使得新坐标值便于计算，得到的平面地图如图3所示。
[0069]
所设计地图为车道级，在双向双车道中，以丁字路口为例，一个路口将存在四个道路节点，如图4所示。
[0070]
根据路段位置连接关系，设置路段的相交点为节点。根据交通规则及节点连接关系，建立邻接矩阵，如下所示：
[0071][0072]
在这个实施例中，选定起点和终点后，采用改进的蚁群算法进行全局路径规划，获得的最优路径规划图如图5所示。所述改进的蚁群算法的流程示意图如图6所示，包括下述步骤：
[0073]
s301、将当前循环次数设置为0，当前蚂蚁个数设置为0，设置信息素浓度初始值，循环总数，蚂蚁总数；
[0074]
s302、判断循环次数是否不大于循环总数；若是，则将当前循环次数增加1，执行s303；否则，输出当前最优路径；
[0075]
s303、判断当前蚂蚁编号是不大于蚂蚁总数；若是，执行步骤s304；否则，执行步骤s309；
[0076]
s304、利用改进的启发函数计算候选节点概率；
[0077]
所述改进的启发函数通过下式计算：
[0078][0079]
式中：d
ij
表示路段(i，j)之间的欧式距离，d
je
表示下一个可行节点j和目标节点e之间的欧氏距离；c1和c2是服从0和1之间均匀分布的随机数，c1表示节点j对路径选择的影响的权重，c2表示目标节点e对路径选择影响的权重；
[0080]
s305、利用信息素浓度和改进的启发函数计算候选节点概率，基于所述概率利用轮盘赌原则，确定下一个节点，并将其加入路径中；
[0081]
s306、判断当前蚂蚁是否完成了搜索；若当前蚂蚁完成了搜索，执行s308；若当前蚂蚁未完成搜索，执行s307；
[0082]
s307、判断当前蚂蚁是否还存在可选择的节点，若当前蚂蚁还有可选择的节点，则返回s304；若当前蚂蚁没有可选择的节点，执行s308；
[0083]
s308、将当前蚂蚁编号加1，返回s303；
[0084]
s309、从此次迭代中所有蚂蚁的路径中，获取此次迭代的最优路径；
[0085]
s310、判断此次迭代的最优路径是否为所有迭代中的最优路径；若是将在此次路径上的信息浓度通过下式更新：
[0086]
τ
ij
(t)＝(1-ρ)
×
τ
ij
(t-1) δτ
ij
[0087]
否则，将在此次路径上的信息浓度通过下式更新，可以防止信息素过度挥发导致路径上无信息素：
[0088]
τ
ij
(t)＝(1-2ρ)
×
τ
ij
(t-1)
[0089]
上述两个式中：ρ∈(0,0.5)，用于防止信息素无限积累的信息素挥发程度，ρ优选为0.4；τ
ij
(t)为更新后信息素浓度；δτ
ij
为此轮蚂蚁遗留信息素，计算公式如下：
[0090][0091]
式中：m为每轮迭代蚂蚁总数量，q为信息素增加强度系数，l为当只蚂蚁从起始节点到达目标节点的路径长度；返回s302。
[0092]
改进的蚁群算法采用改进的启发函数可以增强路径节点之间的距离差异性，增强启发信息值，从而增强搜索过程的导向性，充分缩小搜索空间，减少在路径不完整时丢失的蚂蚁数量，进而提高搜索效率，避免陷入局部最优的问题。
[0093]
在从候选节点确定下一个节点时，由于概率大的路径不一定是最优的，因此在改进的蚁群算法中，通过轮盘赌原则，使得有选择小概率路径的可能，增加遍历性。
[0094]
在信息素的更新上，为了避免后续蚂蚁选择无效路径的概率增加，增加计算时间，本发明采用的方式为：如果此次迭代结果小于等于已有迭代最优结果，则在次路径上增加遗留信息素，增加以后选择此路的机会。如果大于已有迭代最优结果，则将信息素挥发浓度增加为双倍，在全部路径上挥发，不增加此次迭代最优路径上信息素浓度。通过这种方式，可以促进蚂蚁在全局路径的遍历性，防止陷入局部最优。
[0095]
在一个实施例中，采用蚁群算法获取的最短路径长度为609米，时间是0.513878秒，最短路径用节点序号表示为：
[0096]
30-29-32-37-38-40-56-57-58-153-151-148-149-150。
[0097]
用改进的蚁群算法对相同拓扑地图搜索最短路径，获取上述相同最短路径，时间为0.281918秒，算法运算时间大大缩短。
[0098]
为了提高舒适性，本发明方法在s300之后，利用三次b样条曲线拟合，对全局路径进行拟合优化后输出，提高道路行驶舒适性。
[0099]
为了提高安全性，本发明对输出的全局路径进行障碍物位置判断，进行障碍物规避设计，实现局部路径优化，具体为利用势场法思想建立如下障碍物威胁代价函数：
[0100][0101][0102]
式中：wi表示第i个障碍物在所有障碍物威胁代价中的权重；(x,y)表示当前智能车辆所在位置；(x
obsi
,y
obsi
)表示当前障碍物所在位置；δ为一个设定值，用于防止j
obsi
计算结果为无穷大。
[0103]
在进行障碍物威胁代价函数中，将障碍物大小、距离等因素考虑在内，保证行驶安全性与舒适性。
[0104]
障碍物的位置信息可以通过智能汽车的环境感知系统检测周围障碍物并对障碍物信息进行输出，而智能汽车的定位系统，可以获得智能车辆的位置信息。
[0105]
基于障碍物威胁代价函数可以生成智能汽车的前轮转角增量及速度增量。
[0106]
更优地，在考虑障碍物威胁代价函数的同时，考虑跟踪代价与控制代价函数，将使
得两者同时取得最小值时的路径作为实现避障跟踪的最优规划路径。取得最小值的约束包括车辆状态约束、控制约束、舒适性约束。其中：车辆状态约束包括侧向位置约束及横摆角速度约束，控制约束包括转角增量与速度增量的约束，舒适性约束为加速度、加加速度的约束。通过上述约束，提高车辆乘坐的安全性和舒适性。
[0107]
基于局部优化后的路径，利用车辆动力学方程建立预测模型，获取智能车辆的状态量和控制量，从而实现智能车辆的路径规划及轨迹跟踪。所述状态量包括但不限于车辆位置，横摆角速度，速度，前轮转角等。所述控制量包括前轮转角增量，速度增量等。
[0108]
综上，本发明的路径规划和避障方法，减少了获得最优路径的搜索时间，提高了智能车辆的舒适性和安全性，具有很大的应用价值。
[0109]
将上述优选的实施方式同时实施，可以获得一个同时考虑时间、舒适和安全的路径规划及轨迹跟踪方法。
[0110]
通过以上的实施方式的描述，所属领域的技术人员可以清楚地了解到本公开方法可借助软件加必需的通用硬件的方式来实现，当然也可以通过专用硬件包括专用集成电路、专用cpu、专用存储器、专用元器件等来实现。一般情况下，凡由计算机程序完成的功能都可以很容易地用相应的硬件来实现，而且，用来实现同一功能的具体硬件结构也可以是多种多样的，例如模拟电路、数字电路或专用电路等。但是，对本公开而言更多情况下，软件程序实现是更佳的实施方式。
[0111]
尽管以上结合附图对本发明的实施方案进行了描述，但本发明并不局限于上述的具体实施方案和应用领域，上述的具体实施方案仅仅是示意性的、指导性的，而不是限制性的。本领域的普通技术人员在本说明书的启示下和在不脱离本发明权利要求所保护的范围的情况下，还可以做出很多种的形式，这些均属于本发明保护之列。