无人驾驶路径规划(三)局部路径规划 |
您所在的位置:网站首页 › 简述轨迹规划的任务和方法是什么 › 无人驾驶路径规划(三)局部路径规划 |
前言:对于无人驾驶路径规划系列的第二篇RRT算法的改进部分,由于有些内容属于个人想到的创新点,有想法投一篇小论文所以暂时没有公开,等后续完成后我会再公开介绍。今天第三篇内容开启一个新的算法介绍:Frenet坐标系下的动态规划。我花了将近半个月的时间来了解、研究算法原理,理解网上python开源的代码,最后根据个人理解在matlab上进行了复现。如果还没有看过我前面文章的读者,可以点击下方的传送门: 无人驾驶路径规划(一)全局路径规划 - RRT算法原理及实现 同样,如果文中有错误或侵权的地方还请各位读者指出,我会及时作出修改,笔者在这先行谢过。 一、轨迹规划方法简介在第一篇文章中我对无人驾驶路径规划技术做了个简单的介绍,即可以分为全局路径规划和局部路径规划两部分。今天介绍的Frenet坐标系下动态轨迹规划就属于局部路径规划的内容。在这我先简要介绍一下轨迹规划的方法。参考论文:基于激光雷达的环境识别及局部路径规划技术研究。 无人车的局部路径规划不仅需要考虑到道路交通中的静态障碍物的避障问题,还需要对环境中行人、车辆等动态障碍物未来轨迹做出的预判,对未来时间段内的碰撞可能性进行分析,保证车辆安全、舒适的行驶,因而在求解过程中需要考虑时间维度 t。当路径点增加了时间这一维度,实质上转变为了轨迹点 那么Frenet坐标系下动态轨迹规划实际上就属于第三种,将规划问题分解为纵向和横向来分别求解。 二、Frenet坐标系转换 1、Frenet坐标系概念在轨迹规划过程中,使用笛卡尔直角坐标系并不能很好的描述车辆当前位置与当前所在车道的关系,因此需要引入Frenet坐标系的概念。在Frenet坐标系中,使用道路中心线作为参考线,将车辆的轨迹点投影到参考线上得到参考点,令沿参考线方向为纵轴s,垂直于参考线方向为横轴d。Frenet坐标系可以很容易确定车辆偏离车道中心线的距离及沿着车道线行驶的距离,可以忽略道路曲率的影响,相比于笛卡尔坐标系下的描述更为简洁和直观。 Frenet坐标系和笛卡尔坐标系关系如下: 假设车辆行驶至t时刻实际轨迹和参考轨迹如下: 全局坐标系下,t时刻车辆的状态可以描述为:
Frenet坐标系下,车辆的状态量可以描述为:
笛卡尔坐标系与Frenet坐标系状态量转化可以表示为如下关系: 由于具体推导过程比较繁琐和冗长,我在这不进行展开介绍,有兴趣的读者可以参考这篇论文的内容:基于Frenet坐标系的自动驾驶轨迹规划与优化算法。 三、动态轨迹规划过程 1、基于jerk实时轨迹规划轨迹规划过程实际上是一个优化问题。通常来说,乘坐舒适性和安全性是轨迹规划的目标,过大的加速度变化会使得乘客感到不适。因此轨迹规划过程中可以将加速度变化率,也就是“加加速度”-jerk作为优化的目标,来保证乘坐的舒适性。 根据文献中的介绍,经过Frenet分解后可以构建一个一维积分系统: 其中 Takahashi已证明对于上述系统由 其中 横向规划主要承担的是避障、换道等任务,已知在 则有: 为简化运算,令 代入上式,可以解得 由此可以求得 通过设置采样间隔 对于纵向轨迹规划,需要考虑到车辆的实际行驶场景需求,例如跟车、定速巡航、停车、汇流等等。不同的行驶情景有不同的期望配置。在这我仅针对最简单的定速巡航情景进行介绍。 由于是定速巡航,那么此时沿着参考线方向的位置配置则无需考虑,仅需要配置纵向速度 则有: 同样,令 代入上式,可以解得 通过设置采样间隔 对于横向和纵向轨迹规划,都生成了一系列的备选轨迹,通过设定评价函数来确定每一时刻的最优路径。 根据参考论文,横向轨迹评价函数如下: 其中: 纵向轨迹评价函数如下: 与横向评价函数唯一不同的是 最后将二者进行合并,加入权重系数,即可得到最优路径评价函数: 通常轨迹评价函数越小,代表该轨迹的代价越小,优先度越高。 5、碰撞检测Atsushi Sakai在Python代码中设置的碰撞检测方法是: 假设车辆在备选轨迹一个采样时刻内沿参考线方向行驶纵向距离s,若结束后车辆与障碍物的距离小于设定的阈值,则该轨迹不满足碰撞检测需求,进行剔除。对所有备选轨迹进行该项操作。 我在matlab移植代码的时候,没有对这部分进行修改。 四、Python-Matlab代码移植我在网上找到的算法的Python代码是日本无人驾驶工程师Atsushi Sakai编写的,并已经在github上开源。我在文末也给出了源码的链接,有需要的读者可以自行获取。 为了加深对算法的理解,同时保持之前所有算法验证平台的一致性,我将Python源码移植到了matlab2019。在这我不对Python源码进行解释,仅对我移植后的matlab代码进行一个介绍。 1、初始参数定义(宏定义)在程序最开始先给出需要用到的参数的定义及数值,例如最大采样横向距离、横向距离采样间隔、最大采样时间、采样时间间隔、最大加速度/速度等等,具体如下: SIM_LOOP = 500; MAX_SPEED = 50.0 / 3.6; % 最大速度 [m/s] MAX_ACCEL = 2.0; % 最大加速度 [m/ss] MAX_CURVATURE = 1.0; % 最大曲率 [1/m] MAX_ROAD_WIDTH = 7.0; % 最大采样横向距离 [m] D_ROAD_W = 1.0; % 横向距离采样间隔 [m] DT = 0.2; % 采样时间间隔 [s] MAX_T = 5.0; % 最大采样时间 [s] MIN_T = 4.0; % 最小采样时间 [s] TARGET_SPEED = 30.0 / 3.6; % 期望速度 [m/s] D_T_S = 5.0 / 3.6; % 纵向速度采样间隔 [m/s] N_S_SAMPLE = 1; ROBOT_RADIUS = 2; % 碰撞检测阈值 [m] robot_width = 1; % 机器人宽 [m] robot_length = 2; % 机器人长 [m] % 评价函数权重 K_J = 0.1; K_T = 0.1; K_D = 1.0; K_LAT = 1.0; K_LON = 1.0; %% 参数范围确定 c_speed = 10.0 / 3.6; % 当前速度 c_d = 2.0; % 当前横向位移 c_d_d = 0.0; % 当前横向速度 c_d_dd = 0.0; % 当前横向加速度 s0 = 0.0; % 当前沿车道线位移 area = 20.0; 2、障碍物点及参考路径生成原文中是给定了几个参考路径点,采用三次样条的方法生成参考路径。 %% 参考路径点 nodes = [0, 0; 10, -6; 20.5 5; 35, 6.5; 70.5, 0; 100, 5]; %% 设置障碍物坐标点 ob = [20, 10 30, 9; 30, 6; 35, 9; 50, 3; 75, 0]; %% 生成期望路径 csp = Cubic_spline(nodes);Cubic_spline是我自定义的三次样条生成函数。 3、定义FrenetPath结构体定义FrenetPath结构体,存储每一个备选路径的横向、纵向规划结果,及笛卡尔坐标系下的状态量: %% Frenet轨迹结构体 FrenetPath.t = []; FrenetPath.d = []; FrenetPath.d_d = []; FrenetPath.d_dd = []; FrenetPath.d_ddd = []; FrenetPath.s = []; FrenetPath.s_d = []; FrenetPath.s_dd = []; FrenetPath.s_ddd = []; FrenetPath.cd = 0.0; FrenetPath.cv = 0.0; FrenetPath.cf = 0.0; FrenetPath.x = []; FrenetPath.y = []; FrenetPath.yaw = []; FrenetPath.ds = []; FrenetPath.c = []; 4、Frenet动态轨迹规划算法这一部分是该算法的核心函数,即完成了纵向、横向轨迹的规划过程,备选轨迹的评价过程及碰撞检测过程。最后将当前时刻最优轨迹输出给path,path属于FrenetPath结构体类型。 path = calc_frenet_paths_fixed_velocity(csp, c_speed, c_d, c_d_d, c_d_dd, ... s0,MAX_ROAD_WIDTH,D_ROAD_W, MIN_T, MAX_T, DT, ... FrenetPath, TARGET_SPEED,D_T_S, N_S_SAMPLE, ... K_D, K_J, K_LAT, K_LON, K_T, ob, MAX_ACCEL, ... MAX_SPEED, MAX_CURVATURE,ROBOT_RADIUS);calc_frenet_path_fixed_velocity函数具体内容如下: function fplist = calc_frenet_paths_fixed_velocity(csp, c_speed, c_d, c_d_d, c_d_dd, ... s0,MAX_ROAD_WIDTH,D_ROAD_W, MIN_T, MAX_T, DT, ... FrenetPath, TARGET_SPEED,D_T_S, N_S_SAMPLE, ... K_D, K_J, K_LAT, K_LON, K_T, ob, MAX_ACCEL, ... MAX_SPEED, MAX_CURVATURE,ROBOT_RADIUS); min_cost = [inf,inf]; cost = []; flag = 1; for di=-MAX_ROAD_WIDTH:D_ROAD_W:MAX_ROAD_WIDTH for Ti = MIN_T:DT:MAX_T fp = FrenetPath; lat_qp = QuinticPolynomial(c_d, c_d_d, c_d_dd, di, 0, 0, Ti); for t=0:DT:Ti-DT fp.t(1,end+1) = t; end for t=fp.t(1,1):DT:fp.t(1,end) fp.d(1,end+1) = lat_qp.a0 + lat_qp.a1*t + lat_qp.a2*t^2 + lat_qp.a3*t^3 + lat_qp.a4*t^4 + lat_qp.a5*t^5; fp.d_d(1,end+1) = lat_qp.a1 + 2*lat_qp.a2*t + 3*lat_qp.a3*t^2 + 4*lat_qp.a4*t^3 + 5*lat_qp.a5*t^4; fp.d_dd(1,end+1) = 2*lat_qp.a2 + 6*lat_qp.a3*t + 12*lat_qp.a4*t^2 + 20*lat_qp.a5*t^3; fp.d_ddd(1,end+1) = 6*lat_qp.a3 + 24*lat_qp.a4*t + 60*lat_qp.a5*t^2; end for tv = TARGET_SPEED - D_T_S * N_S_SAMPLE : D_T_S: TARGET_SPEED + D_T_S * N_S_SAMPLE tfp = fp; lon_qp = QuarticPolynomial(s0, c_speed, 0.0, tv, 0.0, Ti); for t=fp.t(1,1):DT:fp.t(1,end) tfp.s(1,end+1) = lon_qp.b0 + lon_qp.b1*t + lon_qp.b2*t^2 + lon_qp.b3*t^3 + lon_qp.b4*t^4; tfp.s_d(1,end+1) = lon_qp.b1 + 2*lon_qp.b2*t + 3*lon_qp.b3*t^2 + 4*lon_qp.b4*t^3; tfp.s_dd(1,end+1) = 2*lon_qp.b2 + 6*lon_qp.b3*t + 12*lon_qp.b4*t^2 ; tfp.s_ddd(1,end+1) = 6*lon_qp.b3 + 24*lon_qp.b4*t; end Jp = sum(tfp.d_ddd .^2); Js = sum(tfp.s_ddd .^2); ds = (TARGET_SPEED - tfp.s_d(1,end))^ 2; tfp.cd = K_J * Jp + K_T * Ti + K_D * tfp.d(1,end)^2; tfp.cv = K_J * Js + K_T * Ti + K_D * ds; tfp.cf = K_LAT * tfp.cd + K_LON * tfp.cv; tfp = calc_global_paths(tfp, csp); flag = check_paths(tfp, ob, MAX_ACCEL, MAX_SPEED, MAX_CURVATURE,ROBOT_RADIUS); if (flag == 1) cost(end+1,:) = [tfp.cf, di]; if min_cost(end,1) >= tfp.cf min_cost(end+1,:) = [tfp.cf, di]; fplist = tfp; end else flag = 1; end end end end end其中QuinticPolynomial和QuarticPolynomial函数就是轨迹规划的具体求解公式: function lat_qp = QuinticPolynomial(xs, vxs, axs, xe, vxe, axe, time) lat_qp.a0 = xs; lat_qp.a1 = vxs; lat_qp.a2 = axs/2; A = [time^3, time^4, time^5; 3*time^2, 4*time^3, 5*time^4; 6*time, 12*time^2, 20*time^3]; B = [xe - lat_qp.a0 - lat_qp.a1*time - lat_qp.a2*time^2; vxe - lat_qp.a1 - 2*lat_qp.a2*time; axe - 2*lat_qp.a2]; temp = A^(-1)*B; lat_qp.a3 = temp(1,1); lat_qp.a4 = temp(2,1); lat_qp.a5 = temp(3,1); end function lon_qp = QuarticPolynomial(xs, vxs, axs, vxe, axe, time) lon_qp.b0 = xs; lon_qp.b1 = vxs; lon_qp.b2 = axs / 2.0; A = [3*time^2, 4*time^3 6*time, 12*time^2]; B = [vxe - lon_qp.b1 - 2*lon_qp.b2*time; axe - 2*lon_qp.b2]; temp = A^(-1)*B; lon_qp.b3 = temp(1,1); lon_qp.b4 = temp(2,1); endcalc_global_paths用于求解笛卡尔坐标系下的状态量: function fplist = calc_global_paths(fplist, csp) for i = 1:1:size(fplist.s,2) [idx] = findPoint(fplist.s(1,i), csp); i_x = csp(idx,1); i_y = csp(idx,2); i_yaw = (csp(idx+1,2) - csp(idx,2))/(csp(idx+1,1) - csp(idx,1)); di = fplist.d(1,i); fplist.x(1,end+1) = i_x - di * sin(i_yaw); fplist.y(1,end+1) = i_y + di * cos(i_yaw); end for i = 1:1:size(fplist.x,2)-1 dx = fplist.x(1,i+1) - fplist.x(1,i); dy = fplist.y(1,i+1) - fplist.y(1,i); fplist.yaw(1,end+1) = atan(dy/dx); fplist.ds(1,end+1) = sqrt(dx^1 + dy^2); end fplist.yaw(1,end+1) = fplist.yaw(1,end); fplist.ds(1,end+1) = fplist.ds(1,end); for i=1:1:size(fplist.yaw,2)-1 fplist.c(1,end+1) = (fplist.yaw(1,i+1) - fplist.yaw(1,i+1))/fplist.ds(1,i); end fplist.c(1,end+1) = fplist.c(1,end); endcheck_paths用于碰撞检测,在这里还加入了最大纵向加速度、最大纵向速度和最大曲率约束。我设置了一个标志位flag,只有当通过碰撞检测,即标志位为1的时候,才会进行后续的判断,否则该轨迹就被舍弃: function flag = check_paths(fplist, ob, MAX_ACCEL, MAX_SPEED, MAX_CURVATURE,ROBOT_RADIUS) flag = 1; for i=1:1:size(fplist.s_d ,2) if (fplist.s_d(1,i) > MAX_SPEED) flag = 0; break end if (fplist.s_dd(1,i) > MAX_ACCEL) flag = 0; break end if (fplist.c(1,i) > MAX_CURVATURE) flag = 0; break end end if (flag == 1) for i=1:1:size(fplist.x,2) for j=1:1:size(ob) d = sqrt((ob(j,1)-fplist.x(1,i))^2 + (ob(j,2)-fplist.y(1,i))^2); if(d = tfp.cf min_cost(end+1,:) = [tfp.cf, di]; fplist = tfp; end else flag = 1; end 5、主循环部分当循环次数小于最大循环次数时,重复进行轨迹动态规划的过程,如果车辆到达了参考轨迹的最后一个点,则提前跳出循环过程。每次循环都对车辆当前的位置和最优轨迹进行显示: %% 循环过程 for i=1:1:SIM_LOOP path = calc_frenet_paths_fixed_velocity(csp, c_speed, c_d, c_d_d, c_d_dd, s0,MAX_ROAD_WIDTH, ... D_ROAD_W, MIN_T, MAX_T, DT, FrenetPath, TARGET_SPEED,... D_T_S, N_S_SAMPLE,K_D, K_J, K_LAT, K_LON, K_T, ob,... MAX_ACCEL, MAX_SPEED, MAX_CURVATURE,ROBOT_RADIUS); s0 = path.s(1,2); c_d = path.d(1,2); c_d_d = path.d_d(1,2); c_d_dd = path.d_dd(1,2); c_speed = path.s_d(1,2); plot(csp(:,1), csp(:,2),'-.b'); hold on plot(csp(:,1), csp(:,2)+8,'-k'); plot(csp(:,1), csp(:,2)-8,'-k'); plot(ob(:,1), ob(:,2),'*g'); plot_robot(robot_length, robot_width, path.yaw(1,1) , path.x(1,1), path.y(1,1)); plot(path.x(1,1), path.y(1,1),'vc','MarkerFaceColor','c','MarkerSize',6); plot(path.x(1,:), path.y(1,:),'-r','LineWidth',2); plot(path.x(1,:), path.y(1,:),'ro','MarkerFaceColor','r','MarkerSize',4); set(gca,'XLim',[path.x(1,1) - area, path.x(1,1) + area]); set(gca,'YLim',[path.y(1,1) - area, path.y(1,1) + area]); grid on title('Frenet'); xlabel('横坐标 x'); ylabel('纵坐标 y'); pause(0.01); hold off if sqrt((path.x(1,1) - nodes(end,1))^2+ (path.y(1,1) - nodes(end,2))^2) |
今日新闻 |
推荐新闻 |
CopyRight 2018-2019 办公设备维修网 版权所有 豫ICP备15022753号-3 |