本文立足于智能车领域的轨迹规划,根据自己的整理和理解输出,权当做一篇学习笔记。这篇只是综述,每种算法的详细过程会在别的篇幅整理出来。
首先解释一下一些基本概念:
规划(planning)承接环境感知,并下启车辆控制。其规划出来的轨迹是带速度信息的路径。广义上,规划(planning)可分为路由寻径(routing)、行为决策(behavioral decision)、运动规划(motion planning)。
路由寻径(routing):是全局路径规划,可简单的理解为传统地图导航+高精地图(包含车道信息和交通规则等);
行为决策(behavioral decision):决策车辆是否跟车、在遇到交通灯和行人时的等待避让、以及路口和其他车辆的交互通过;
运动规划(motion planning):是局部路径规划,是无人车未来一段时间内的期望行驶路径,需满足汽车运动学、动力学、舒适性和无碰撞等要求。
轨迹规划的任务是计算出一个无碰撞可执行的轨迹(包含路径和速度信息),保证车辆从起点安全的驾驶到目的地,并尽可能高效。其问题的本质是一个多目标的数学优化问题。
主要的优化目标包括:
安全性:避免与场景中的障碍物发生碰撞;针对动态障碍物,由于其未来运动的不确定性,降低其未来的碰撞风险;
稳定性:由于车辆的惯性较大,灵活性差,期望轨迹需要保证车辆的物理可行性和控制器的稳定性;
舒适性:考虑到乘员的舒适性,需要在满足安全性和稳定性的同时保证车辆的驾驶舒适度,包括加减速以及转向等过程;
驾驶效率:在满足安全性和稳定性的同时,保证车辆以更快的速度驾驶,从而更短的时间到达目的地。
在实际场景中,规划过程需要考虑各种物理约束,有且不限于:
加减速度约束:受到动力系统和制动系统的性能极限,及驾驶员的安全性和舒适性的制约;
非完整性约束:车辆具有三个运动自由度,但是只有两个控制自由度,其非完整性约束决定了轨迹的物理可行性;
动力学约束:考虑到车辆的动力学特性和车身稳定性,其驾驶过程中的曲率和横摆角速度具有一定的约束;
当然,在工程中,解决特定场景的规划问题,可以进行一定程度的简化。下面针对几种应用较为广泛算法,然后针对性的介绍实际场景(APA和换道)。
2.1 基于图搜索的算法:Dijkstra、A*、D*(全局路径规划)
2.2 基于曲线拟合的算法:圆弧与直线、多项式曲线、样条曲线、贝塞尔曲线、微分平坦(局部路径规划)
2.3 基于数值优化的算法:利用目标函数和约束对规划问题进行描述和求解(局部路径规划)
2.4 基于人工势场的算法:人工势场法(全局路径规划)
2.5 基于采样的算法:RRT(全局路径规划)
2.6 基于智能法的算法:模糊逻辑、神经网络、遗传算法(略)
将环境进行栅格化,一条路径可以利用图搜索的算法来访问栅格图中的节点,从而得到规划
Dijkstra算法的主要特点是以起始点为中心向外层层扩展(广度优先搜索思想),直到扩展到终点为止。迪杰斯特拉算法的成功率是最高的,因为它每次必能搜索到最优路径。但迪杰斯特拉算法的搜索速度是最慢的:随着图维度的增大,其计算效率会明显变低。
基本步骤:
Dijkstra算法是广度优先算法,是一种发散式的搜索,搜索速度是很慢。这里引入一种启发式算法的深度优先算法:A*。其基本思想:
基本步骤:
Dijkstra与A star对比
A*他是导航的基础算法(如百度地图):
备注:Apollo中的道路是车道线级别的,所以apollo中的node就是一条lane,而边则是车道和车道之间的连接,点对应具体的车道,而边则是一个虚拟的概念,表示车道之间的关系。下面我们可以先看下apollo中道路(road)和车道(lane)的概念。
假设我们到达了一个交叉路口,我们可以沿着公路直走、左转或右转。首先,我们将把这张地图转换为具有三个候选节点(left, straight, right)的图形。接下来,我们将对选项进行评估。在实践中,拐过交叉路口很费劲,所以我们为left节点分配了更高的g值(g值表示从起始点到候选节点的成本)。在查看公路选项之后,我们意识到必须走很长的路,才能离开公路并返回我们的目标,所以我们为straight选项分配了更高的h值(h值表示从候选节点到目的地的估计成本)。最后,我们通过将g值和h值相加来计算每个节点的f值。我们看到最低f值实际对应右边的候选节点。所以,这是我们接下来要前往的节点。
这块会在后面的APA和超车中整理出来。
基本方法:
这一块儿是想基于Apollo整理,但是内容比较大,这里只概述一下,后续会用专门的一篇整理Apollo的rounting和motion planning(自动驾驶之轨迹规划2——Apollo规划与控制公开课)和自动驾驶之轨迹规划6——Apollo EM Motion Planner。Apollo的决策规划模块是“轻决策重规划”,且有好几种planner,RTK planner是基于录制的轨迹规划行车路线,EM planner是路径和车速分层规划,lattice planner是直接高维轨迹规划(路径和车速一起规划)。这里先只讲下EM planner。
在规划过程中解决决策问题。一般先是由rounting模块进行全局规划,得出reference line,然后motion planning在此基础上进行局部轨迹规划。motion planning将路径和车速分层规划,并在SL和ST坐标系下,使用动态规划进行路径和车速的决策和粗规划,然后使用二次规划进行平滑处理。
动态规划:使用动态规划的原因是Apollo把道路进行切片撒点,把轨迹问题变成分段最优问题,即动态规划中的最优子结构。
二次规划:使用二次规划的原因是Apollo把路径和车速平滑性,以平方项的方式进行量化,由此转化为二次规划问题。
APF假设车辆在一种虚拟力场下运动:车辆的初始点在一个较高的“山头”上,要到达的目标点在“山脚”下,这就形成了一种势场,车辆在这种势的引导下,避开障碍物,到达目标点。
当然,人工势场法(APF)也有缺点:可能被困在局部最优解。
RRT随机树:快速随机地扩张,一群像树一样的路径以探索(填充)空间的大部分区域,伺机找到可行的路径。
基本步骤:
但RRT缺点也很明显:
大致分为3类:
基于轨迹规划:需额外考虑速度规划,APA为低速场景,该算法复杂一般不用。
基于路径规划:正弦曲线、多项式曲线、圆弧及公切线、贝塞尔曲线、B样条曲线。
基于经验:模糊逻辑等。
本篇着重说下以水平泊车为例的圆弧及公切线方法,样条曲线(可实现曲率连续)的解决方法与此类似,这里不再累述。
1. 最小车位分析:
如下图,在泊车过程中,若以最小转弯半径行驶,则车辆右前方B点不得与车库a点碰撞(决定了车库的最小长度),车辆右后方C点不得与车库内侧bc边碰撞(决定了车库的最小宽度)。由此得到两个不等式(如下图)
用matlab分析后发现:车辆泊车所需车位尺寸与车辆转弯半径相关,所需车位长度随转弯半径增大而增大,宽度随转弯半径增大而减小。
代码如下:
clc;
clear;
Lr=0.95;
Lk=1.645;
Lf=0.8;
L=2.405;
Lad=zeros(60,1);
Lab=zeros(60,1);
Rv=zeros(60,1);
for i=1:60
R=4+i/10;
ladmin=Lr+((R+Lk/2)^2+(L+Lf)^2-(R-Lk/2)^2)^0.5;
labmin=Lk/2+((R+Lk/2)^2+Lr^2)^0.5-R;
Rv(i,1)=R;
Lad(i,1)=ladmin;
Lab(i,1)=labmin;
end
% h=figure();
% plot(Rv,Lad,'linewidth',2);
% title('车位长度');
% xlabel('实际转弯半径 / m');
% ylabel('车位长度 / m');
% grid on;
h=figure();
plot(Rv,Lab,'linewidth',2);
title('车位宽度');
xlabel('实际转弯半径 / m');
ylabel('车位宽度 / m');
grid on;
frame = getframe(h); % 获取frame
img = frame2im(frame); % 将frame变换成imwrite函数可以识别的格式
imwrite(img,'车位宽度.jpg'); % 保存到工作目录下,名字为"a.png"
2. 初始泊车位置可行性分析
另外车辆在泊车过程中并不是在任何位置都可以泊进车库,因此需要进行初始泊车位置的可行性分析。这里就需要考虑路线的可行性,即需要满足碰撞要求和最小转弯半径要求:
优化自变量:我们可以通过5个参数,来对这样一条路径(上图红线:一段直线+一段圆弧+一段直线+一段圆弧)进行几何刻画,两个直线的长度,两个圆弧的半径,圆弧的角度(两个圆弧的角度应相等,否则车子会停偏)。
目标函数:我们需要求解上述的这样一条组合线,使得该组合线段的起点(也就是C0尽量接近于实际车子的后轴中心点。换句话说如果存在这样一条圆弧直线的泊车组合线,且起点就在本车后轴中心点,则车子就可以按照这个路径进行泊车。如果组合线起点C0距离本车后轴中心点较远,则需要把车子挪到该起点后再沿着组合线进行泊车)
碰撞约束:
车辆沿圆弧行驶时,为避免车辆与道路边界物体或对面行驶来的车辆发生碰撞,车身轮廓左前点不越过道路边界线,此为不等式约束。
车辆沿直线行驶时,车辆右侧轮廓线 在车位顶点 a 左上方,此为不等式约束。
车辆参数约束:
路径曲线圆弧半径不小于车辆最小转弯半径,此为不等式约束。
上面这个问题是个非线性规划问题,我在Matlab求解数学优化问题中有描述解决办法,这里直接给出结果,经过matlab分析:可行泊车起点如下:
matlab代码如下:
fminconXY:
% 水平泊车起始区域分析
% [x fval exitflag] = fmincon(fun,x0,A,b,Aeq,beq,lb,ub,nonlcon)
% 输入参数:
% fun:目标函数
% x0:因变量的迭代起始点
% A,b:因变量的线性不等式约束
% Aeq,beq:因变量的线性等式约束
% lb,ub:因变量的上界和下界
% nonlcon:非线性约束
%
% 输出参数:
% x:求解得出的最优参数值
% fval:在x为最优参数值时的目标函数(fun)值
% exitflag:输出fmincon额外条件值
%% solver
clc;
clear;
h=figure();
for m=5.6:0.4:12 % SX 给定的泊车起点X坐标
for n=1:0.4:3 % SY 给定的泊车起点Y坐标
R=4.2; % 最小转弯半径
pi=3.141592653;
LimitMIN=0.00001;
LimitMAX=5000000;
lb = [LimitMIN,LimitMIN,LimitMIN,R,R]; % lb,ub:因变量的上界和下界
ub=[LimitMAX,LimitMAX,pi,LimitMAX,LimitMAX];
A=[]; % A,b:因变量的线性不等式约束
b=[];
Aeq=[]; % Aeq,beq:因变量的线性等式约束
beq=[];
x0 = [0,0,0,8,2]; % x0:因变量的迭代起始点
% 全局变量
global Lr; Lr=0.95; % 汽车后悬长度
global SX; SX=m; % 泊车起点X坐标
global SY; SY=n; % 泊车起点Y坐标
global xa; xa=5.83; % 车库长度
global ya; ya=0;
global lk; lk=1.8; % 车库宽度
global lw; lw=4; % 通道宽度
nonlcon = @NonConstr; % nonlcon:非线性约束
fun=@ObjFunc; % fun:目标函数
[objfun,fval,exitflag,output] = fmincon(fun,x0,A,b,Aeq,beq,lb,ub,nonlcon)
% 给定起始点坐标和规划出来的坐标之间的误差不宜过大,过大则舍弃
if (exitflag~=1)||(fval>0.05)
break;
end
%% ploter
f=objfun;
l1=f(1);
l2=f(2);
theta=f(3);
R1=f(4);
R2=f(5);
xc4=Lr; yc4=-lk/2;
xc3=xc4+R2*sin(theta); yc3=yc4+R2*(1-cos(theta));
xc2=xc3+l2*cos(theta); yc2=yc3+l2*sin(theta);
xc1=xc2+R1*sin(theta); yc1=yc2+R1*(1-cos(theta));
xc0=xc1+l1; yc0=yc1;
xo2=xc4; yo2=yc4+R2;
xo1=xc1; yo1=yc1-R1;
% 曲线连接点坐标
plot(xc0,yc0,'+','linewidth',2); % 实际计算出的泊车起点坐标
hold on;
% 车库
parksite=[-1,0;
0,0;
0,-lk;
xa,-lk;
xa,0;
13,0];
roadsite=[-1,lw;13,lw];
plot(parksite(:,1),parksite(:,2),'-','linewidth',2);
hold on;
plot(roadsite(:,1),roadsite(:,2),'-','linewidth',2);
hold on;
end
end
% title('给定水平泊车初始位置,规划泊车路线');
title('水平泊车起始区域分析');
xlabel('X轴 / m');
ylabel('Y轴 / m');
grid on;
axis([-2 14 -2 5]);
% 输出为图片
frame = getframe(h); % 获取frame
img = frame2im(frame); % 将frame变换成imwrite函数可以识别的格式
imwrite(img,'水平泊车起始区域分析.jpg'); % 保存到工作目录下,名字为"a.png"
ObjFunc:
function [objfun,fval,exitflag,output] = ObjFunc(t)
l1=t(1);
l2=t(2);
theta=t(3);
R1=t(4);
R2=t(5);
global Lr;
global lk;
global SX;
global SY;
global xa;
global ya;
xo=SX; yo=SY;
xc4=Lr; yc4=-lk/2;
xc3=xc4+R2*sin(theta); yc3=yc4+R2*(1-cos(theta));
xc2=xc3+l2*cos(theta); yc2=yc3+l2*sin(theta);
xc1=xc2+R1*sin(theta); yc1=yc2+R1*(1-cos(theta));
xc0=xc1+l1; yc0=yc1;
objfun=(xc0-xo)^2+(yc0-yo)^2;
end
NonConstr:
function [c,ceq] = NonConstr(t)
l1=t(1);
l2=t(2);
theta=t(3);
R1=t(4);
R2=t(5);
global Lr;
global lk;
global SX;
global SY;
global xa;
global ya;
global lw;
xc4=Lr; yc4=-lk/2;
xc3=xc4+R2*sin(theta); yc3=yc4+R2*(1-cos(theta));
xc2=xc3+l2*cos(theta); yc2=yc3+l2*sin(theta);
xc1=xc2+R1*sin(theta); yc1=yc2+R1*(1-cos(theta));
xc0=xc1+l1; yc0=yc1;
xo2=xc4; yo2=yc4+R2;
xo1=xc1; yo1=yc1-R1;
RA1=((R1+0.8225)^2+10.272025)^0.5;
% 不等式约束:默认为小于等于0
c(1) = yo1+RA1-lw;
c(2)=-(abs((tan(theta)*xa-ya-xc2*tan(theta)+yc2)*cos(theta))-lk/2);
c(3)=-(tan(theta)*(xa-xc2-lk/2*sin(theta))+yc2+lk/2*cos(theta));
% 等式约束
ceq = [];
end
3. 给定泊车起始点,规划最优路径
同样,也根据matlab的非线性求解器进行最优路径规划,这里举了一个例子,在车辆初始位置为(10,2)的位置得到一条泊车路径:
具体matlab代码与初始泊车位置可行性分析类似,这里不再累述。
4. 简化路径规划算法
当然,非线性规划再实际工程实现中可能存在实时性的问题,我也有见到一些文献进行基于人类驾驶经验的简化,如下:
库内姿态调整
当车库较小时,可能无法一次泊车入库,需要在车库内进行姿态i调整,这种场景与前面整理的方法类似,因此这里只给出库内姿态调整大致一些思路:库内姿态调整可看作在上述泊车路径的基础上增加一段圆弧C4C5。(车辆初始车身不正也可看作在起点C0前增加一段圆弧。)
库内姿态调整的最小车位分析有点不太一样,需要使用多目标优化进行求解(求车库长度和车库宽度的最小值),我做了一点简化(在目标函数上把多目标问题转化为单目标问题进行优化)。
// ObjFunc.m
function [objfun,fval,exitflag,output] = ObjFunc(t)
R2=t(1);
R3=t(2);
theta3=t(3);
global Coe;
global Lf;
global Lr;
global Lk;
global L;
global v2max;
global wo;
lc1=((R2+Lk/2)^2+(L+Lf)^2-((R2+R3)*cos(theta3)-R3-Lk/2)^2)^0.5...
-R2*sin(theta3)+Lr*cos(theta3)+Lk/2*sin(theta3); % 沿O2圆行驶,车辆右前点碰撞要求
lc2=(Lk/2+R3)*sin(theta3)+Lr*cos(theta3)+L+Lf; % 沿O3行驶,车辆与前后碰撞要求
lk1=Lk/2+R3+(Lk/2-R3)*cos(theta3)+Lr*sin(theta3); % 不含车轮回正距离的车库宽度
return_c1=v2max*atan(L/R2)/wo*cos(theta3); % 车轮回正的车库长度体现
return_c2=v2max*atan(L/R3)/wo*cos(theta3);
return_k=v2max*atan(L/R2)/wo*sin(theta3); % 车轮回正的车库宽度体现
lc_min=max(lc1+return_c1,lc2+return_c2); % 含车轮回正距离的车库长度
lk_min=lk1+return_k; % 含车轮回正距离的车库宽度
objfun=Coe*lc_min+(1-Coe)*lk_min;
end
暴力求解如下:
库内姿态调整的起始可行区域与上面的可行区域分析类似
库内姿态调整的泊车路线同样类似
这种基于圆弧直线的方法也存在很多问题点,如圆弧与直线连接处存在曲率突变,车速不匀速等问题。目前均有工程化解决方案,但还在梳理所以留在后面整理。
另外垂直泊车和45°/60°泊车,与水平泊车方法类似,这里不再累述。
超车分为3个阶段:变道,超越和并道。从本质上看,可认为是驾驶员的两次换道和一次超越行为的综合结果。目前基于多项式的方法用的多一些(对标:LKA也是基于3次多项式)。
matlab参考代码如下:
clc;
clear;
t=0; % 换道初始时刻
t_1=9; % 换道结束时刻
v=15; % 车速
T=[t^5 t^4 t^3 t^2 t 1;
5*t^4 4*t^3 3*t^2 2*t 1 0;
20*t^3 12*t^2 6*t 2 0 0
t_1^5 t_1^4 t_1^3 t_1^2 t_1 1;
5*t_1^4 4*t_1^3 3*t_1^2 2*t_1 1 0;
20*t_1^3 12*t_1^2 6*t_1 2 0 0];
% 边界条件:初始时刻和终点时刻的纵向位移、纵向速度、纵向加速度
st_X=[0 v 0 v*t_1 v 0]';
% 边界条件:初始时刻和终点时刻的横向位移、横向速度、横向加速度
st_Y=[0 0 0 3 0 0]';
Q=T\st_X; % 求得纵向相关系数
P=T\st_Y; % 求得横向相关系数
%% plot_X
T_1=zeros(t_1*10,1);
X=zeros(t_1*10,1);
for i=1:1:t_1*10
t_1=i/10;
T_1(i,1)=t_1;
T=[t^5 t^4 t^3 t^2 t 1;
5*t^4 4*t^3 3*t^2 2*t 1 0;
20*t^3 12*t^2 6*t 2 0 0
t_1^5 t_1^4 t_1^3 t_1^2 t_1 1;
5*t_1^4 4*t_1^3 3*t_1^2 2*t_1 1 0;
20*t_1^3 12*t_1^2 6*t_1 2 0 0];
x=T(4,:)*Q;
x_dot=T(5,:)*Q;
x_ddot=T(6,:)*Q;
X(i,1)=x;
X_dot(i,1)=x_dot;
X_ddot(i,1)=x_ddot;
end
figure();
plot(T_1,X)
title('纵向距离');
grid on;
figure();
plot(T_1,X_dot)
title('纵向速度');
grid on;
figure();
plot(T_1,X_ddot)
title('纵向加速度');
grid on;
%% plot_Y
T_1=zeros(t_1*10,1);
Y=zeros(t_1*10,1);
for i=1:1:t_1*10
t_1=i/10;
T_1(i,1)=t_1;
T=[t^5 t^4 t^3 t^2 t 1;
5*t^4 4*t^3 3*t^2 2*t 1 0;
20*t^3 12*t^2 6*t 2 0 0
t_1^5 t_1^4 t_1^3 t_1^2 t_1 1;
5*t_1^4 4*t_1^3 3*t_1^2 2*t_1 1 0;
20*t_1^3 12*t_1^2 6*t_1 2 0 0];
y=T(4,:)*P;
y_dot=T(5,:)*P;
y_ddot=T(6,:)*P;
Y(i,1)=y;
Y_dot(i,1)=y_dot;
Y_ddot(i,1)=y_ddot;
end
figure();
plot(T_1,Y)
title('横向距离');
grid on;
figure();
plot(T_1,Y_dot)
title('横向速度');
grid on;
figure();
plot(T_1,Y_ddot)
title('横向加速度');
grid on;
未经允许禁止转载