本文讨论下无人驾驶领域的路线规划和控制相关的技术。
1.路线规划
1.1 路线规划采用A*算法
(具体过程和流程后续补上)
1.2 路线规划转化为实际行驶路线
如上图所示，s表示路线规划起点，e表示路线规划终点，斜线填充的网格表示障碍物位置。
蓝色的线为路线规划算法规划出的路线，但在实际行驶中，车辆不可能直接原地90度的转弯，不仅可操作性不强，对于乘客也是非常危险的行为。在实际的行驶中，我们更希望车辆的路线如红色线路所示，非常平缓的从起点驶向终点（绿色路线虽然距离更近，但起点45度的转角也会让乘客不适）。
那么我们的问题来了，如何由路线规划算法生成的路线转换为实际行驶的路线，即如何由蓝色路线生成红色路线呢,如何由蓝色路线生成平滑的红色线路呢？
假设路线规划的结果为点序列:[x1,x2,…xn]
平滑后路线规划的点序列:[y1,y2,…yn]
求解最优解的方法采用梯度下降法(gradient descent)，即通过多次迭代，调整yi使得目标函数取得最小值。
循环执行迭代过程直到达到迭代次数上限或者目标函数值下降至指定阈值。
1.3 算法实现
上图代码一个5x5的网格地图，红色圆圈代表一条从(0,0)到(4,4)的规划路线，下面代码演示了如何由这条路线生成一条平滑线路。
2、车辆控制
平滑处理的路线还需要转换为实际的车辆行驶操作，汽车的控制算法是一个复杂的课题，本文只讨论PID算法简单的实现，不讨论背后复杂的理论体系，Google无人驾驶采用的控制算法的原理与此类似。
2.1 P Controller
如图所示、车辆A以恒定的速度向前运动，车辆的两个前轮可以随意控制其方向，后轮不可控制其方向；黑色带箭头的实线表示路线规划输出车辆目标运行轨迹，如何使得车辆按照目标轨迹运行呢？
当然，首先要控制车辆靠近目标轨迹并且沿着目标轨迹运行，实际操作中我们运用横向轨迹误差法(Cross-track Error,目标轨迹与车辆当前位置的距离)不断的计算调整车辆的状态，使其不断的靠近目标轨迹。Cross-track Error的值越大，车头偏离原行驶方向的角度就越大，随着Cross-track Erro的减小，车辆偏离原行驶方向的角度要不断减小，直至轨迹重合。这种类型的车辆控制称为P controller，P为proportional的简写。之所以称为proportional controller。
假设alpha表示车辆驾驶的角度，则有：
2.2 车辆抖动
单纯的Pcontroller会导致上图所示的问题，车辆在目标轨迹附近波动前行，这种波动可能非常小，以至于在某些场景下我们可以忽略。
2.3 PD Control
如何消除车辆在目标路线附近来回波动？PD Control可用来解决这一问题。PD Control不仅考虑Cross-Track Error还考虑到Cross-Track Error对时间的微分。
这样一来，车辆的运动就不仅仅由Cross-Track Error相关，还与相邻Step之间的Cross-Track Error差值相关。
2.4 PD Control实现
通过结果可以看到，车辆更加平滑的移动到目标路线，随着时间推移，车辆停留在目标路线上。
还要注意一个问题是，车辆的系统偏差(systematic bias)问题。比如汽车的初始状态，比如车辆启动的时候，如果车辆的车轮方向存在倾斜，这对于人类司机不会造成困扰，但是对于自动驾驶的车辆，会导致行驶位置的偏离。
2.5 解决系统偏差(systematic bias)问题
假设你开着一辆车，按照经验习惯，调整到目标线路时，方向盘应该打到某个位置，但是有一天你突然发现相同的调整角度并未使你靠近目标位置，这时候你会将方向盘调整到更大角度，从而达到靠近目标位置的目的。
我们通过加权随时间不断累积的偏差(bias)值来消除系统偏差问题，公式如下:
这就是PID Controller,它包含了proportional、differential、integral三个部分。
2.6 参数优化问题
PID控制算法中的权重参数如何选择？我们运用了叫做”Twiddle”的算法，一次调整一个参数，最终生成最优参数集。
流程如下：
1) 初始化参数集合p=[0,0,0],表示我们目前为止对参数集最好的估计；初始化增量数组dp=[1,1,1],表示我们将要用来调整p的数值。
2) 选择参数集中的一个参数(假设为p[0]),更新p[0]的值p[0]=p[0]+dp[0],然后将参数集p代入PID算法，计算error，如果计算出的Error优于之前发现的所有Error，则保留p[0]的值，并更新dp[0]=dp[0]*1.1;反之，还原p[0]的值至更新前的值，然后更新p[0]=p[0]-dp[0]，并将参数集p代入PID算法计算Error，如果计算出的Error优于之前发现的所有Error，则保留p[0]的值，并更新dp[0]=dp[0]*1.1，否则还原p[0]至原始值，并且更新dp[0]=dp[0]*0.9,继续重复上述过程，直至找到合适的值或则dp数组元素的和超出设定阈值。
閱讀更多 半杯茶的小酒杯 的文章
