一、主要思路
路径规划和跟踪控制算法是保障智能驾驶汽车在环境部分已知状态下实现安全行驶的核心因素,两者之间有着紧密的联系,规划路径的合理性与安全性及路径跟踪的精度是保证智能车辆安全行驶的关键所在。传统的路径规划和跟踪控制算法输出性能在实际驾驶场景中存在不同缺陷。同时,由于目前应用于路径规划及跟踪控制领域的多数算法最初的应用主体为移动机器人,所以在直接将其应用于车辆时也表现出了许多明显的不足,如对结构参数的考虑不足、运动学或动力学模型不匹配等。
路径规划和跟踪控制算法在智能驾驶研究中有重要意义,因此本文以校园环境中运行的智能车为研究对象,主要从智能驾驶汽车全局路径规划算法、局部路径规划算法和跟踪控制算法三个方面展开研究,具体的研究内容如下:
首先基于校园场景下结构化道路特征对环境进行建模,提出一种基于关键节点信息的改进 A*算法,并将其应用于全局路径规划环节中;通过地图预览、应用基于安全距离的碰撞场改进传统 A*算法代价函数、应用准均匀三次 B 样条曲线结合地图预览信息对路径进行有效平滑等方式,实现了 A*算法的计算效率、规划路径的安全性和可行性的综合提升。
其次,本文通过对经典局部路径规划算法动态窗口算法和向量场直方图算法进行对比,分析不同算法应用于局部道路环境下的综合表现,并最终选择了 DWA 算法用于智能驾驶车辆局部路径规划,对 DWA 算法进行了改进研究,改进 DWA 算法实时性和规划路径安全性都能够较好地满足车辆实际行驶需求。
再次,在路径跟踪控制方面,本文主要通过采用模型预测控制算法实现低速状态下对期望路径的准确跟踪。仿真结果表明,模型预测路径跟踪控制器能够在保持车辆稳定性的基础上,实现对期望路径的精确跟踪。其次,通过构建 Stanley 算法实现对期望速度的稳定跟踪。
二、算法设计与代码
%%%%%%%%%%%%%%%%%离散粒子群算法解决0-1背包问题%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%初始化%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
clear all; %清除所有变量
close all; %清图
clc; %清屏
N=100; %群体粒子个数
D=10; %粒子维数
T=200; %最大迭代次数
c1=1.5; %学习因子1
c2=1.5; %学习因子2
Wmax=0.8; %惯性权重最大值
Wmin=0.4; %惯性权重最小值
Vmax=10; %速度最大值
Vmin=-10; %速度最小值
V = 300; %背包容量
C = [95,75,23,73,50,22,6,57,89,98]; %物品体积
W = [89,59,19,43,100,72,44,16,7,64]; %物品价值
afa = 2; %惩罚函数系数
%%%%%%%%%%%%%%%%初始化种群个体(限定位置和速度)%%%%%%%%%%%%%%%%
x=randint(N,D); %随机获得二进制编码的初始种群
v=rand(N,D) * (Vmax-Vmin)+Vmin;
%%%%%%%%%%%%%%%%%%初始化个体最优位置和最优值%%%%%%%%%%%%%%%%%%%
p=x;
pbest=ones(N,1);
for i=1:N
pbest(i)= func4(x(i,:),C,W,V,afa);
end
%%%%%%%%%%%%%%%%%%%初始化全局最优位置和最优值%%%%%%%%%%%%%%%%%%
g=ones(1,D);
gbest=eps;
for i=1:N
if(pbest(i)>gbest)
g=p(i,:);
gbest=pbest(i);
end
end
gb=ones(1,T);
%%%%%%%%%%%按照公式依次迭代直到满足精度或者迭代次数%%%%%%%%%%%%%
for i=1:T
for j=1:N
%%%%%%%%%%%%%%更新个体最优位置和最优值%%%%%%%%%%%%%%%%%
if (func4(x(j,:),C,W,V,afa)>pbest(j))
p(j,:)=x(j,:);
pbest(j)=func4(x(j,:),C,W,V,afa);
end
%%%%%%%%%%%%%%%%更新全局最优位置和最优值%%%%%%%%%%%%%%%
if(pbest(j)>gbest)
g=p(j,:);
gbest=pbest(j);
end
%%%%%%%%%%%%%%%%计算动态惯性权重值%%%%%%%%%%%%%%%%%%%%
w=Wmax-(Wmax-Wmin)*i/T;
%%%%%%%%%%%%%%%%%跟新位置和速度值%%%%%%%%%%%%%%%%%%%%%
v(j,:)=w*v(j,:)+c1*rand*(p(j,:)-x(j,:))...
+c2*rand*(g-x(j,:));
%%%%%%%%%%%%%%%%%%%%边界条件处理%%%%%%%%%%%%%%%%%%%%%%
for ii=1:D
if (v(j,ii)>Vmax) | (v(j,ii)< Vmin)
v(j,ii)=rand * (Vmax-Vmin)+Vmin;
end
end
vx(j,:)=1./(1+exp(-v(j,:)));
for jj=1:D
if vx(j,jj)>rand
x(j,jj)=1;
else
x(j,jj)=0;
end
end
end
%%%%%%%%%%%%%%%%%%%%记录历代全局最优值%%%%%%%%%%%%%%%%%%%%%
gb(i)=gbest;
end
g; %最优个体
figure
plot(gb)
xlabel('迭代次数');
ylabel('适应度值');
title('适应度进化曲线')
算法流程
基于地图预览模块获取的道路转向关键节点和在行驶过程中产生的避障转向节点进行路径平滑,采用准均匀三次 B 样条曲线对相应的轨迹进行优化。如图 2-10 为不同形式路径的平滑示意图。如图 2-10 分别为基于道路转向关键节点和避障转向节点进行的路径平滑过程示意图。其中,基于道路转向关键节点进行路径平滑时,共选用 7个控制节点,其位置分别为基于选定的关键转向节点向两侧各扩展相等间距为 1m 的三个节点;基于避障转向节点进行平滑时,以两个变向节点为主要控制点,向前后两侧扩展相等间距为 1m 的三个节点得到另外六个补充控制点。
仿真
仿真2
结果比较
路径规划算法利用地图数据和车辆的当前位置,来确定车辆应该遵循的最佳路径。这些算法考虑了多种因素,如道路条件、交通流量、车辆限制以及用户的偏好。通过综合考虑这些因素,车辆可以选择最快、最短或最经济的路径。例如,在高峰时段,算法可以避开拥堵的道路,选择更畅通的路线。而在用户偏好方面,算法可以优先选择经过景点或商业区的路线,以提供更好的出行体验。
跟踪控制算法负责实时控制车辆的运动。它们根据车辆的当前状态和目标位置,计算出车辆应该采取的行动,如加速、转向和刹车。这些算法需要高精度的传感器数据和准确的车辆模型,以确保车辆能够按照规划的路径行驶。例如,当车辆接近转弯时,算法会适时减速,并根据转弯半径和车辆的动力学特性来调整转向角度,以保证安全和平稳的转弯。
车辆路径规划和跟踪控制算法在自动驾驶和智能交通系统中发挥着重要的作用。它们不仅可以提高车辆的行驶效率和安全性,还可以提供更舒适的乘坐体验。在自动驾驶系统中,这些算法能够实现全自动的路径规划和控制,让车辆能够自主地行驶,并根据交通状况做出相应的调整。而在智能交通系统中,这些算法可以与其他系统进行协同,实现交通流量的优化和拥堵的缓解。
随着技术的不断发展,车辆路径规划和跟踪控制算法也在不断演进和改进。新的传感器技术和高精度地图数据的使用,使得算法能够更准确地感知环境和预测交通状况。同时,机器学习和人工智能的应用,也为算法带来了更高的智能化和自适应性。这些进步将进一步提升车辆的导航能力和行驶效果,为人们提供更安全、快捷和舒适的出行体验。