- 背景概述
本文立足于智能车领域的轨迹规划，根据自己的整理和理解输出，权当做一篇学习笔记。这篇只是综述，每种算法的详细过程会在别的篇幅整理出来。
首先解释一下一些基本概念：
规划（planning）承接环境感知，并下启车辆控制。其规划出来的轨迹是带速度信息的路径。广义上，规划（planning）可分为路由寻径（routing）、行为决策（behavioral decision）、运动规划（motion planning）。
路由寻径（routing）：是全局路径规划，可简单的理解为传统地图导航+高精地图（包含车道信息和交通规则等）；
行为决策（behavioral decision）:决策车辆是否跟车、在遇到交通灯和行人时的等待避让、以及路口和其他车辆的交互通过；
运动规划（motion planning）：是局部路径规划，是无人车未来一段时间内的期望行驶路径，需满足汽车运动学、动力学、舒适性和无碰撞等要求。
2. 轨迹规划综述
轨迹规划的任务是计算出一个无碰撞可执行的轨迹（包含路径和速度信息），保证车辆从起点安全的驾驶到目的地，并尽可能高效。其问题的本质是一个多目标的数学优化问题。
主要的优化目标包括：
安全性：避免与场景中的障碍物发生碰撞；针对动态障碍物，由于其未来运动的不确定性，降低其未来的碰撞风险；
稳定性：由于车辆的惯性较大，灵活性差，期望轨迹需要保证车辆的物理可行性和控制器的稳定性；
舒适性：考虑到乘员的舒适性，需要在满足安全性和稳定性的同时保证车辆的驾驶舒适度，包括加减速以及转向等过程；
驾驶效率：在满足安全性和稳定性的同时，保证车辆以更快的速度驾驶，从而更短的时间到达目的地。
在实际场景中，规划过程需要考虑各种物理约束，有且不限于：
加减速度约束：受到动力系统和制动系统的性能极限，及驾驶员的安全性和舒适性的制约；
非完整性约束：车辆具有三个运动自由度，但是只有两个控制自由度，其非完整性约束决定了轨迹的物理可行性；
动力学约束：考虑到车辆的动力学特性和车身稳定性，其驾驶过程中的曲率和横摆角速度具有一定的约束；
当然，在工程中，解决特定场景的规划问题，可以进行一定程度的简化。下面针对几种应用较为广泛算法，然后针对性的介绍实际场景（APA和换道）。
2.1 基于图搜索的算法：Dijkstra、A*、D*（全局路径规划）
2.2 基于曲线拟合的算法：圆弧与直线、多项式曲线、样条曲线、贝塞尔曲线、微分平坦（局部路径规划）
2.3 基于数值优化的算法：利用目标函数和约束对规划问题进行描述和求解（局部路径规划）
2.4 基于人工势场的算法：人工势场法（全局路径规划）
2.5 基于采样的算法：RRT（全局路径规划）
2.6 基于智能法的算法：模糊逻辑、神经网络、遗传算法（略）
2.1 基于图搜索的算法
将环境进行栅格化，一条路径可以利用图搜索的算法来访问栅格图中的节点，从而得到规划
2.1.1 Dijkstra
Dijkstra算法的主要特点是以起始点为中心向外层层扩展（广度优先搜索思想），直到扩展到终点为止。迪杰斯特拉算法的成功率是最高的，因为它每次必能搜索到最优路径。但迪杰斯特拉算法的搜索速度是最慢的：随着图维度的增大，其计算效率会明显变低。
基本步骤：
指定起点s。
引进两个集合S和U。S的作用是记录已求出最短路径的顶点（以及相应的最短路径长度），而U则是记录还未求出最短路径的顶点（以及该顶点到起点s的距离）。
初始时，S中只有起点s；U中是除s之外的顶点，并且U中顶点的路径是“起点s到该顶点的路径”。然后，从U中找到路径最短的顶点，并将其加入到S中；接着，更新U中的顶点和顶点对应的路径。
重复该操作，直到遍历完所有顶点。
2.1.2 A*
Dijkstra算法是广度优先算法，是一种发散式的搜索，搜索速度是很慢。这里引入一种启发式算法的深度优先算法：A*。其基本思想：
基本步骤：
把起点加入 open list 。
重复如下过程：
a ) 遍历 open list ，查找 F 值最小的节点移到 close list ，把它作为当前要处理的节点。
b ) 判断当前方格的 8 个相邻方格的每一个方格，若为unreachalbe或者已在 close list 中则忽略。否则做如下操作。
c ) 如果它不在 open list 中，把它加入 open list ，并且把当前方格设置为它的父亲，记录该方格的 F ， G 和 H 值。
d ) 如果它已经在 open list 中，检查这条路径 ( 即经由当前方格到达它那里 ) 是否更好，用 G 值作参考。更小的 G 值表示这是更好的路径。如果是这样，把它的父亲设置为当前方格，并重新计算它的 G 和 F 值。
直到openlist为空，从终点开始，每个方格沿着父节点移动直至起点，这就是最优路径。
Dijkstra与A star对比
A star算法的现实意义
A*他是导航的基础算法（如百度地图）：
规划的第一步是路线导航。侧重于研究如何从地图上的A点前往B点：如手机导航系统。Apollo中通过路线规划模块处理该任务。
规划的第二步是：在路线规划的基础上进行轨迹规划。该轨迹由一系列点定义，每个点都有一个关联速度和一个指示何时应抵达那个点的时间戳。Apollo通过规划模块处理该任务。
路线规划的目标是，找到从地图上的A前往B的最佳路径。轨迹规划的目标是找到避免碰撞和保持舒适度的可执行轨迹。
下图形由“节点”(node)和“边缘”(edge)组成。节点代表路段，边缘代表这些路段之间的连接。例如：在交叉路口，汽车可从节点1移动到节点2、节点3或节点4，反之亦然。
我们可以对一个节点移动到另一个节点所需的成本进行建模。例如在现实生活中，拐过一个交叉路口比直行更费劲，所以从节点1到节点4的成本高于从节点1到节点3的成本.
备注：Apollo中的道路是车道线级别的，所以apollo中的node就是一条lane，而边则是车道和车道之间的连接，点对应具体的车道，而边则是一个虚拟的概念，表示车道之间的关系。下面我们可以先看下apollo中道路(road)和车道(lane)的概念。
假设我们到达了一个交叉路口，我们可以沿着公路直走、左转或右转。首先，我们将把这张地图转换为具有三个候选节点（left, straight, right）的图形。接下来，我们将对选项进行评估。在实践中，拐过交叉路口很费劲，所以我们为left节点分配了更高的g值（g值表示从起始点到候选节点的成本）。在查看公路选项之后，我们意识到必须走很长的路，才能离开公路并返回我们的目标，所以我们为straight选项分配了更高的h值（h值表示从候选节点到目的地的估计成本）。最后，我们通过将g值和h值相加来计算每个节点的f值。我们看到最低f值实际对应右边的候选节点。所以，这是我们接下来要前往的节点。
2.2 基于曲线拟合的算法
这块会在后面的APA和超车中整理出来。
基本方法：
余弦曲线：在起始位置和终点位置二次导数取最大值（加速度为最大值），舒适性差；
多项式曲线：可避免圆弧直线路径中曲率不连续的问题 ，达到曲率连续变化；
圆弧及公切线：路径曲率不连续，车辆到达曲率间断点处时需停车转向，否则因方向盘转速和车速的影响，车辆将偏离目标路径；
贝塞尔曲线：对路径进行平滑处理，达到曲率变化率连续性；
B样条曲线：对路径进行平滑处理，达到曲率变化率连续性；
微分平坦：对路径进行平滑处理，达到曲率变化率连续性。
2.3 基于数学优化的算法
这一块儿是想基于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把路径和车速平滑性，以平方项的方式进行量化，由此转化为二次规划问题。
2.4 基于人工势场的算法
APF假设车辆在一种虚拟力场下运动：车辆的初始点在一个较高的“山头”上，要到达的目标点在“山脚”下，这就形成了一种势场，车辆在这种势的引导下，避开障碍物，到达目标点。
当然，人工势场法（APF）也有缺点：可能被困在局部最优解。
2.5 基于采样的算法
RRT随机树：快速随机地扩张，一群像树一样的路径以探索（填充）空间的大部分区域，伺机找到可行的路径。
基本步骤：
起点作为一颗种子，从它开始生长枝丫；
在车辆所处的空间中，生成一个随机点 ；
在树上找到距离 最近的那个点；
朝着的方向生长，如果没有碰到障碍物就把生长后的树枝和端点添加到树上，返回 2；
但RRT缺点也很明显：
RRT 得到的路径一般质量都不是很好，例如可能包含棱角，不够光滑；
通常也远离最优路径；
难以在有狭窄通道的环境找到路径。因为狭窄通道面积小，被碰到的概率低，找到路径需要的时间要看运气。
3. 应用场景
3.1 低速场景：APA
大致分为3类：
基于轨迹规划：需额外考虑速度规划，APA为低速场景，该算法复杂一般不用。
基于路径规划：正弦曲线、多项式曲线、圆弧及公切线、贝塞尔曲线、B样条曲线。
基于经验：模糊逻辑等。
本篇着重说下以水平泊车为例的圆弧及公切线方法，样条曲线（可实现曲率连续）的解决方法与此类似，这里不再累述。
- 最小车位分析：
如下图，在泊车过程中，若以最小转弯半径行驶，则车辆右前方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.2 高速场景：超车
超车分为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;
评论（0）
您还未登录，请登录后发表或查看评论