本文解析论文： A Practical Trajectory Planning Framework for Autonomous Ground Vehicles Driving in Urban Environments 来自国防科大团队
文章依旧沿用了经典的横向的空间规划，和纵向的速度规划。在横向上，横向位置是关于纵向距离S的三阶多项式。纵向上速度是关于时间T的四阶多项式。对速度的规划采用了典型的机器人速度规划方法，梯形速度规划，然后在做后处理进行速度平滑。 另外，在对轨迹的前处理中，对车道中心线为代表的原水曲线使用了考虑waypoints 指向变换和累计曲率因素的轨迹平滑算法。
下面简单总结这三个技术点： 轨迹平滑： 算法考虑了轨迹点方向变换和轨迹点曲率这两项因素，轨迹平滑方向倾向于选择轨迹为直线，曲率最小的曲线作为优化方向。效果如下图所示：
横向位置规划： 位置规划的曲线是横向位置关于纵向路线S的三阶多项式。其中可变的边界条件是初始点和终点的横向位置，起点的车辆朝向theta, 在这里，为了方便起见，限定终点的theta恒为0。根据这四个边界条件确定close form solution. 对于cost function的评价，选择了四个指标：考虑倾向于选择较长的，曲率小的，偏离参考线近的，以及相邻两次规划变化小的备选线。 我验证了一下，基本没有问题。 但是这里的弊端和使用五阶多形式是一样的，就是初始位置的超调现象。我随便设定了几个参数来看超调现象是否可以接受。
clc clear all s0 = 0; sf = 25; L0 = 1; % Lf = 0; theta0 = 15/180*pi; figure for s = 0:0.5:sf for Lf = -1:0.2:1 A = [1 s0 s0^2 s0^3; 1 sf sf^2 sf^3; 0 1 2*s0 3*s0^2; 0 1 2*sf 3*sf^2]; B = [L0;Lf;tan(theta0);0]; C = A\B; y = C(1) + C(2)*s + C(3)*s^2 + C(4)* s^3; plot(s,y,'-bo') hold on end end L0 = 0; % Lf = 0; theta0 = -3/180*pi; for s = 0:0.5:sf for Lf = -1:0.2:1 A = [1 s0 s0^2 s0^3; 1 sf sf^2 sf^3; 0 1 2*s0 3*s0^2; 0 1 2*sf 3*sf^2]; B = [L0;Lf;tan(theta0);0]; C = A\B; y = C(1) + C(2)*s + C(3)*s^2 + C(4)* s^3; plot(s,y,'-r*') hold on end end由于限制了终点的theta=0.因此这个方案对于五阶多项式来说超调现象减少了很多，但因为是polynomial,无法完全避免这个问题。
速度规划： 首先我们要设定一些对速度的限制： 主要提到了三个限制：纵向速度，横向速度以及纵向加速度 这里面的Dsafe类似ACC中的安全距离公式： 我们设定速度关于时间T的函数为四阶多项式： 通过给定四个边界条件获得close form solution: 初始位置的加速度就是当前加速度。tf是我们自己设定的，vf根据情况确定，比如跟车时就是前车速度，停障是就是0等等。af是终点处加速度，sf是设定的路径长度。对于梯形速度profile，分别对梯形的两边单独计算这个profile： 但是这篇文章对方案中的behavior planning 部分是没有提及的，因此我们难以了解到车辆变道和超车的时机以及条件等细节。
