本文立足于智能车领域的轨迹规划,根据自己的整理和理解输出,权当做一篇学习笔记。这篇只是综述,每种算法的详细过程会在别的篇幅整理出来。
首先解释一下一些基本概念:
路由寻径(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)的概念。
这块会在后面的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坐标系下,使用动态规划进行路径和车速的决策和粗规划,然后使用二次规划进行平滑处理。
APF假设车辆在一种虚拟力场下运动:车辆的初始点在一个较高的“山头”上,要到达的目标点在“山脚”下,这就形成了一种势场,车辆在这种势的引导下,避开障碍物,到达目标点。
RRT随机树:快速随机地扩张,一群像树一样的路径以探索(填充)空间的大部分区域,伺机找到可行的路径。
基本步骤:
但RRT缺点也很明显:
大致分为3类:
基于轨迹规划:需额外考虑速度规划,APA为低速场景,该算法复杂一般不用。
基于路径规划:正弦曲线、多项式曲线、圆弧及公切线、贝塞尔曲线、B样条曲线。
基于经验:模糊逻辑等。
本篇着重说下以水平泊车为例的圆弧及公切线方法,样条曲线(可实现曲率连续)的解决方法与此类似,这里不再累述。
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. 初始泊车位置可行性分析
另外车辆在泊车过程中并不是在任何位置都可以泊进车库,因此需要进行初始泊车位置的可行性分析。这里就需要考虑路线的可行性,即需要满足碰撞要求和最小转弯半径要求:
% 水平泊车起始区域分析 % [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)的位置得到一条泊车路径:
4. 简化路径规划算法
当然,非线性规划再实际工程实现中可能存在实时性的问题,我也有见到一些文献进行基于人类驾驶经验的简化,如下:
// 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次多项式)。
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;
联系客服