APA规划和控制算法开发遇到的问题及解决方案

轨迹规划算法

轨迹规划采用五次多项式曲线、半径为最小转弯半径的圆弧、直线构成。

控制算法

纵向控制采用PID
横向控制采用纯跟踪、Stanley并融合车辆运动学原理（适用场景：低速，曲率较大）。

出现的问题

问题1：

不管是纯跟踪还是Stanley都具有预瞄距离，导致车辆不能较好的达到预期轨迹终点处，而产生误差，影响后续的控制。
轨迹的圆弧半径均是最小转弯半径，故如上图所示，第一段轨迹跟踪过程中只要车辆未能到达指定轨迹终点处，则后面车辆肯定不能较好的跟踪RS曲线，因为最小转弯半径的限制，车辆只能在RS曲线的外围环绕，而无法靠近，类似于“稳态误差”。

解决方案：
1，加大圆弧曲率半径。可以消除上述的稳态误差，使得车辆可以根据预期跟踪上轨迹。
2，延长原轨迹。在原轨迹后面加一段自己设计的轨迹，加的轨迹长度要大于预瞄距离，使车辆在行驶过程中每一刻都有目标跟踪点。
目地是保证车辆到达原轨迹终点时，与原轨迹终点的航向和横向误差最小。

问题2：

为什么在大角度倒退时，不采用纯跟踪，而用Stanley算法呢？

解决方案：
因为在倒退时，预瞄距离必须选择比较短才能较好地跟踪轨迹，因为圆弧轨迹的半径较小，一旦预瞄距离选的比较大，则车辆将不会根据预期轨迹走，而选择“超近路”，如下图所示，车辆倒退时不能根据原先规划的轨迹倒车，而是选择抄近道，不能符合预期。

在倒退时，预瞄距离必须选择比较短才能较好地跟踪轨迹，但纯跟踪不适用于预瞄距离较短的场景，会出现频繁的大角度震荡。

问题3：

泊车算法对于初始位姿依赖强，因为当车位坐标确定后，关键点B、C、D均已确定，不会因为车辆的初始位资而进行调整，故一旦初始位资不符合预期，则达到B点的效果将比较差，从而产生连锁反应，影响最终的泊车效果。
解决办法：
增加TOA调整车资的过程

问题4：

除了问题1产生的问题，对于水平车位TOD的过程，如何补偿横向跟踪时产生的航向偏差，即达到轨迹终点时，车辆不能一次性跟车位平行，若继续倒车使其平行，则车尾将超出车位线。
解决办法：
当车辆倾斜时，增加2个过程，
1，先直线往前开，一直到车头到达前车位而不超出车位为止
2，方向盘打死将车摆正