【论文笔记】Perception, Planning, Control, and Coordination for Autonomous Vehicles

单纯作为阅读笔记，文章内容可能有些混乱。

文章目录

• [1. Introduction](#1. Introduction)
• [2. Perception](#2. Perception)
• [3. Planning](#3. Planning)
• • [3.1. Autonomous Vehicle Planning Systems](#3.1. Autonomous Vehicle Planning Systems)
  • [3.2. Mission Planning](#3.2. Mission Planning)
  • [3.3. Behavioral Planning](#3.3. Behavioral Planning)
  • [3.4. Motion Planning](#3.4. Motion Planning)
  • • [3.4.1. Combinatorial Planning](#3.4.1. Combinatorial Planning)
    • [3.4.2. Sampling-Based Planning](#3.4.2. Sampling-Based Planning)
  • [3.5. Planning in Dynamic Environments](#3.5. Planning in Dynamic Environments)
  • • [3.5.1. Decision Making Structures for Obstacle Avoidance](#3.5.1. Decision Making Structures for Obstacle Avoidance)
    • [3.5.2. Planning in Space-Time](#3.5.2. Planning in Space-Time)
    • [3.5.3. Control Space Obstacle Representations](#3.5.3. Control Space Obstacle Representations)
  • [3.6. Planning Subject to Differential Constraints](#3.6. Planning Subject to Differential Constraints)
  • [3.7. Incremental Planning and Replanning](#3.7. Incremental Planning and Replanning)
• [4. Control](#4. Control)
• • [4.1. Classical Control](#4.1. Classical Control)
  • [4.2. Model Predictive Control](#4.2. Model Predictive Control)
  • [4.3. Trajectory Generation and Tracking](#4.3. Trajectory Generation and Tracking)
  • • [4.3.1. Combined Trajectory Generation and Tracking](#4.3.1. Combined Trajectory Generation and Tracking)
    • [4.3.2. Separate Trajectory Generation and Tracking](#4.3.2. Separate Trajectory Generation and Tracking)
    • • [Trajectory Generation](#Trajectory Generation)
      • [Trajectory Tracking](#Trajectory Tracking)
      • • [Geometric Path Tracking](#Geometric Path Tracking)
        • • [Pure pursuit](#Pure pursuit)
          • Stanley
        • [Trajectory Tracking with a Model](#Trajectory Tracking with a Model)

1. Introduction

作者先介绍了一些关于自动驾驶汽车在未来城市交通系统中的潜在作用，包括增加安全性、提高生产力、提高可达性、提高道路效率以及对环境的积极影响；接着介绍自动驾驶汽车的起源与发展，当然少不了介绍DARPA；作者还介绍了关于MoD（Mobility on Demands）System 相比传统以私家车为主的交通方式的优势（方便、便宜），这也是自动驾驶汽车共享化的一个方向。

......

像Google、Tesla、Uber等等公司都在自动驾驶领域有着大量的投入与研发。

......

城市场景下的自动驾驶一直是热点，DARPA Urban Challenge和V-Charge Project等等都介绍了大量这方面的工作。城市场景主要要解决动态、复杂的交通环境、交通参与者等等对自动驾驶汽车的影响。

下面是AI对V-Charge Project的解释：

V-Charge项目是一个欧洲研究项目，旨在开发先进技术，实现自动泊车和车辆充电。该项目专注于开发一种完全自动化系统，使电动汽车（EV）可以在公共停车场内自动泊车和充电，无需人为干预。

V-Charge系统包括几个组件，包括：

1. 自动泊车（AVP）系统 - 这个系统使EV可以使用先进的传感器和算法自动停放在指定的停车位上。

2. 自动充电（AC）系统 - 这个系统使EV可以自动连接和充电，无需人为干预。

3. 车队管理（FM）系统 - 这个系统允许操作员实时管理和监控EV的充电和泊车。

V-Charge项目由欧盟资助，涉及多家领先的技术公司和研究机构，包括博世、宝马和牛津大学。该项目旨在通过促进EV的使用，使充电和泊车更加方便和可访问，从而减少城市交通的环境影响。

......

下面是作者给出的架构图：

经典的以perception, planning以及control为主模块的架构图。还考虑到V2V通信对自动驾驶系统的作用。

由于研究方向的缘故，我将重点关注PNC的部分,其他部分暂且省略。

2. Perception

暂略

3. Planning

3.1. Autonomous Vehicle Planning Systems

典型的架构是将Planning分为三层：Mission Planning、Brhavioral Planning、Motion Planning。

• Mission Planning：高层次的规划，例如决定是否、何时何地、如何出行、最佳路线等。
• Brhavioral Planning：基于其他agent或交通规则作决策，例如超车、让行、并道等等。
• Motion Planning：运动规划，最典型的目标是到达目标区域，同时避免障碍物碰撞。

3.2. Mission Planning

提供路由网络定义文件(Route Network Definition File，RNDF)作为先验信息。RNDF包含可通行路段的拓扑结构，还包括停车标志(Stop Sign)位置、车道宽度和停车位位置等信息。RNDF早期是手动标注的，现在也有研究自动标注、在线标注的。

路由网络图 Road Network Graph (RNG) 存储相关信息。

经典的算法：Dijkstra，Astar，......

这里作者还提供了一篇route planning的综述

Bast, Hannah, et al. "Route Planning in Transportation Networks." arXiv: Data Structures and Algorithms,arXiv: Data Structures and Algorithms, Apr. 2015。

3.3. Behavioral Planning

Behavioral Planning需要使得车辆既能满足交规、沿着路径行驶的需求，也能与周边交通参与者进行交互。这可以通过局部目标设置、虚拟障碍物放置、可行驶区域边界调整和区域启发式成本调整的组合来实现。类似于Apollo等等rule-based的方案。

FSM在行为决策中经常被使用，但是由于是人工设计的，无法考虑到所有的复杂场景，当遇到未知场景时，就容易出问题。

作者还介绍了其他的一些工作，包括使用Linear-Temporal Logic (LTL) 完成超车场景的实验以及一些对于寻找更大规则库以适应更复杂场景的尝试等等。

3.4. Motion Planning

• Motion Planning典型的指标就是计算复杂度以及算法的完备性。
• Motion Planning的问题可以被视作一个"搬钢琴问题"，房屋和钢琴的结构图作为规划器的输入，得到一条安全无碰撞的路径。同时，该问题是一个PSPACE-hard的问题。
  • PSPACE复杂度类是指一个问题可以通过使用确定性图灵机在输入长度为 n n n时，使用的内存（空间）按照 O ( n k ) O(n^k) O(nk)的渐进趋势增长来解决，其中 k ≥ 0 k≥0 k≥0。确定性图灵机是一种假设设备，它可以在一次只能更改一个符号/值的磁带上操作，并且对于任何给定情况只能指定一种操作。一个问题A若被认为是PSPACE-hard，那么其中的每个问题/语言 B B B都可以通过多项式时间归约为 A A A，即 B ≤ p A B≤pA B≤pA，这意味着任何 B B B都可以在多项式时间内转换为 A A A的实例。
• Motion Planning 的核心就是将连续空间模型转换为离散空间模型。两类转变方式：

1. combinatorial planning:对原始问题做到完全离散化描述
2. sampling-based planning：对configuration space利用碰撞检查对离散采样点进行搜索。

3.4.1. Combinatorial Planning

combinatorial planning的方式对于低维度的问题有着不错的解决效率，但对于多障碍物、高维的复杂问题，其计算复杂度将会大大增加。

在工程应用中，直接从障碍物几何中分解空间比较困难。一种简单的方法是对configuration space或机器人actions统一的离散化，以便可以通过有限搜索来找到解决方案路径。通过在离散空间中工作，大大减少了穷举搜索的复杂性。但这种方式的完备性或者最优性取决于离散化的精度。

DARPA Grand Challenges和DUC中提到的几种方法：

• 基于对运动学可达轨迹集进行离散化的方法
• 基于道路几何形状生成轨迹搜索树
• 基于优化的速度平滑方法进一步改进上述方法。
  Wenda Xu, et al. "A Real-Time Motion Planner with Trajectory Optimization for Autonomous Vehicles." 2012 IEEE International Conference on Robotics and Automation, 2012, https://doi.org/10.1109/icra.2012.6225063。
• Astar
• Cell decomposition,这一类的方法接触的比较少

作者介绍了一些行为决策的方法。比如结合Cell Decomposition以及Linear Temporal Logic (LTL)的方法生成遵守交规的路径；基于LTL在部分情况下打破规则的方法；Cell Decomposition结合POMDP或MOMDP的方法来应对不确定的环境。Bandyopadhyay, Tirthankar, et al. "Intention-Aware Motion Planning." Springer Tracts in Advanced Robotics,Algorithmic Foundations of Robotics X, 2013, pp. 475--91, https://doi.org/10.1007/978-3-642-36279-8_29.

POMDP会假设机器人运动以及观测中的不确定性，并在解决最优策略中考虑这些不确定性。(Mixed Observability Markov Decision Processes, MOMDP) 则是对POMDP的一种拓展，在一些状态下是部分可观测的，另一些状态下是完全可观测的。Ong, Sylvie C. W., et al. "Planning under Uncertainty for Robotic Tasks with Mixed Observability." The International Journal of Robotics Research, July 2010, pp. 1053--68, https://doi.org/10.1177/0278364910369861.

POMDP难以精确求解，通常采用估计都方式去代替；同时还面临者计算复杂度随着状态量与计算视野的增长而快速增长的问题。也有些研究聚焦于连续空间中的POMDP。作者还介绍了高效的基于点的值迭代求解器SARSOP。

回头继续细看这些论文...

3.4.2. Sampling-Based Planning

Sampling-based的方法通常具有概率完备性。基于采样的算法的变体主要不同于生成搜索树的方法。经典的方法有PRM和RRT。

规划不仅需要考虑完备性和效率，也要考虑规划出的路径质量(这段主要指的是最优性，对于无人车的轨迹，还需要考虑平滑程度、时效性等等因素)。这里作者介绍了bias-RRT，以及PRM*、RRT*、Fast Marching Trees(FMT*)、Stable Sparse Trees (SST*)等渐进最优的算法。

3.5. Planning in Dynamic Environments

在城市场景中实现自动驾驶，需要考虑大量的不确定性，这些不确定性可能来自于传感器的精度误差、定位误差、环境变化、控制执行时的误差以及影响最大的周围障碍物的移动变化。

3.5.1. Decision Making Structures for Obstacle Avoidance

DARPA中有一种做法：对沿着路径可能发生碰撞的区域或交叉口合并区域进行检测，利用周围车辆轨迹来计算TTC，若即将发生碰撞，则会停止。这种方式相对保守。下面提到的defensive driving也是类似的对碰撞轨迹进行检测。这种方法的优点是计算简单，因为忽略了时间维度；但是导致其只能应用于比较简单的场景。

基于POMDP的方法可以清晰阐述自动驾驶汽车运动过程中的各类不确定性因素，包括自车控制过程中的不确定性以及周围环境、障碍物的不确定性，但POMDP难以建模，且需要对状态空间进行离散化。

3.5.2. Planning in Space-Time

为了更好地考虑障碍物的运动轨迹，需要引入时间维度。对于障碍物感知而言，获取移动障碍物的瞬时速度和位置是相对容易的，但是获取障碍物未来的行驶轨迹是比较困难的。作者在这里介绍了利用恒定速度估计障碍物的轨迹，并用快速迭代的重规划去减小误差；也介绍了一种扩大障碍物boundingbox相对保守的策略。

这张图是障碍物在 R 2 \mathbf R^2 R2space-time空间中的轨迹预测可视化图。左1和左2不考虑障碍物未来轨迹的不确定性；剩余两种方式分别考虑了速度限制和加速度限制。这样的思想同样可以用到自车速度、加速度的边界估计以及轨迹边界估计中。

3.5.3. Control Space Obstacle Representations

另一种方式是直接在控制空间里进行规划。在这里作者提到了多机器人规划的一种算法：Reciprocal Velocity Obstacles。

3.6. Planning Subject to Differential Constraints

微分约束！！！

Motion Planning的问题最终会变成一个high-level的控制问题。以往的方法中，常常由于计算复杂度、建模的复杂度忽略了控制的重要性，这会使得Motion Planning规划出的轨迹在控制部分会产生较大误差，使得轨迹效用低下，甚至会产生危险。执行轨迹和规划轨迹的误差，使得在规划部分碰撞检测的效用更低，可能会产生危险。

长度更长的轨迹可能比长度较短的轨迹执行时间要短。

对于一个系统，最重要的微分约束是时间的递进，因为时间 t t t必定会以恒定的速率 t ˙ = 1 \dot t = 1 t˙=1增长

机器人微分约束主要应用于以下几个方向：

1. 利用微分约束生成速度剖面，在这个剖面上的几何路径采用解耦的方式进行。（其实就是路径-速度解耦的思想），作者这里给出两篇论文。其中一篇应该是速度-路径规划解耦思想的最早来源。Kant, Kamal, and Steven W. Zucker. "Toward Efficient Trajectory Planning: The Path-Velocity Decomposition." The International Journal of Robotics Research, Aug. 1986, pp. 72--89, https://doi.org/10.1177/027836498600500304.
2. 应用于采样算法中，当构建树时，直接利用direct integrated的方式实时求解每一段的连接。这部分作者主要推荐了一些kinodynamic 随机采样的算法。
3. 考虑转向半径限制。比如Dubins曲线以及Reeds-Shepp曲线。还有一些启发式的算法。

微分约束解耦的方式通常求解效率低下同时可能存在找不到解的情况；综合微分约束(direct integrated differential constraint)可以弥补这些缺点，但是计算更为复杂。

通过将待采样的状态限制为一系列从起始条件开始并在obstacle-free的空间里能经由符合运动学(kinodynamic)约束的轨迹到达的状态集合，从而使得采样更加高效。

将Reachability Guidance (RG)和最近邻 (Nearest Neighbor，NN) 搜索运用到状态采样之中，可以为显着提升效率。这是利用传统的欧式空间距离进行搜索所不能做到的。

这一段内容介绍了几种将可达性指导策略（RG）应用于运动规划的分析方法。其中，通过球盒定理（Ball Box Theorem）将可达子空间表示为线性动力学逼近系统的超矩形，从而将RRT*算法的渐近最优性扩展到可达性指导变体。类似的方法在PRM和FMT的约束处理变体中也被应用，以证明其渐近最优性。此外，还介绍了Goal-Rooted Feedback Motion Tree (GR-FMT) 和用于解决双点边界值问题的分析方法，但它们都有一定的限制条件。

还有利用机器学习的方法，比如SVM去寻找可达状态。

3.7. Incremental Planning and Replanning

由于在实际工况中，自车通常不能观测到整体环境，只能观测到环境的部分，因此增长式的planning(incremental planning)就十分有必要了。同时由于动态环境的变化，原先规划出来的轨迹可能不再适用，因此就需要重规划Replanning。

Incremental planning需要获得一系列子目标或者利用启发式的算法对轨迹库进行打分。作者介绍了利用FSM生成子目标的算法，当遇到规划路径被阻塞时生成子目标，子目标是在预先定义的路径上并且间隔预先定义的距离。

作者在这一部分再次介绍了MOD（Mobility on Demands），提到了MOD中预先定义的path的重要性：可以避免局部规划陷入极小值。

考虑到每个规划周期需要有限的计算时间，并且环境可能在此期间发生变化，规划的安全机制也应该仔细设计。

Pendleton, Scott, et al. "Autonomous Golf Cars for Public Trial of Mobility-on-Demand Service." 2015 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), 2015, https://doi.org/10.1109/iros.2015.7353517.

上面这篇文献同样是速度-路径解耦的方式进行规划，根据从所需路径的加权纵向和横向偏移测量的最近障碍物的接近程度规定减速的程度（该方法被称为Dynamic Virtual Bumper）。不过在这篇文献中，动态障碍物只是被当做放大之后的静态障碍物。

此外，还有一些综合考虑速度、空间路径的一些方法，比如Inevitable Collision State Avoidance (ICS-AVOID) 、Non-Linear Velocity Obstacles (NLVO)、Time-Varying Dynamic Window。

4. Control

Control代表了系统的执行能力，也称为运动控制，是将意图转换为动作的过程。其目的通过向硬件级别提供必要的输入来执行计划的意图。Controllers、Measurements、Models等基本概念就不多做解释了。

4.1. Classical Control

控制理论基础，不详细阐述了。

反馈控制时比较常用的经典控制方法。典型的反馈控制就是PID。
u ( t ) = k d e ˙ + k p e + k i ∫ e ( t ) d t u(t)=k_d\dot{e}+k_pe+k_i\int e(t)dt u(t)=kde˙+kpe+ki∫e(t)dt

单纯的反馈控制会有以下的缺点：

1. 对误差的响应延迟，只有误差发生时才会响应。
2. 对干扰、模型误差、测量噪声采用同样的机制进行响应，这种耦合的方式不如独立响应这些误差的方式更合乎逻辑。

增加前馈控制项可以弥补这些缺陷。

前馈和反馈的比较。

现代控制理论的状态空间控制方法，这方面的控制方法非常多。

典型的线性状态空间模型：
x ( t ) ˙ = A x ( t ) + B u ( t ) y ( t ) = C x ( t ) + D u ( t ) \dot{x(t)}=Ax(t)+Bu(t)\\ y(t)=Cx(t)+Du(t) x(t)˙=Ax(t)+Bu(t)y(t)=Cx(t)+Du(t)

非线性状态空间模型可以用以下公式表示：

x t + 1 = f ( x t , u t ) + ϵ t y t = h ( x t ) + δ t x_{t+1}=f(x_t,u_t)+\epsilon_t\\ y_t=h(x_t)+\delta_t xt+1=f(xt,ut)+ϵtyt=h(xt)+δt

其中， x t x_t xt表示状态向量， u t u_t ut表示控制向量， y t y_t yt表示观测向量。 f f f是状态转移函数， h h h是观测函数， ϵ t \epsilon_t ϵt和 δ t \delta_t δt为噪声，通常假设为高斯白噪声。这是一般的非线性状态空间模型的表达方式，也存在特殊的非线性状态空间模型形式。

2自由度控制器也可以应用于非线性系统。前馈用于生成参考轨迹，而反馈用于补偿干扰和误差。

4.2. Model Predictive Control

MPC三大要素：预测模型、反馈控制、滚动优化。

出于规划和预测的目的，自动控制系统需要模型的介入。这就不得不提经典的MPC了。MPC将模型预测和优化控制相结合，通过将控制问题转化为优化问题，将未来一段时间内控制动作的优化和最优化控制策略的实时调整相结合，从而实现优化控制。

MPC的核心思想是基于预测模型，对未来系统状态进行预测，并利用这些预测结果确定控制输入，以使控制性能最优化。通过优化来选择最优的控制策略，将未来的多步控制动作序列预先计算出来，然后仅执行第一个控制动作。在下一个控制周期开始时，该过程将再次重复。这样，MPC可以处理非线性和非稳态问题，同时适用于各种不同类型的控制目标和约束。

MPC的主要优点包括：

• 能够处理非线性、时变和分布式参数系统；
• 能够处理各种不同类型的约束条件，如输入、输出和状态约束；
• 具有强鲁棒性，能够应对模型不确定性和扰动；
• 能够对多种控制目标进行优化，如跟踪、稳定性和能耗等。

MPC的框架

不过MPC比较消耗计算资源，若车载芯片算力大大提升，MPC的应用价值将会更高。

MPC 已应用于多种汽车控制应用，包括牵引控制、制动和转向、车道保持等。模型预测控制技术也被应用于各种轨迹跟踪控制问题。

MPC问题的一般描述形式：

t t t为时间序列。 v ( h ∣ t ) v(h|t) v(h∣t)代表 v v v在时间 t t t处预测时长为 h h h步。等式(7)、(8)是离散时间系统模型的描述，采样间隔为 T s T_s Ts， x ∈ R n x\in \mathbf R^n x∈Rn是系统的状态变量， u ∈ R m u\in \mathbf R^m u∈Rm为控制输入， y ∈ R p y\in \mathbf R^p y∈Rp是系统输出。 U ( t ) = ( u ( 0 ∣ t ) ， ⋅ ⋅ ⋅ ， u ( N − 1 ∣ t ) ) U(t) = (u(0|t)，···，u(N−1|t)) U(t)=(u(0∣t)，⋅⋅⋅，u(N−1∣t))是控制输入的序列，需要求解其最优值， N N N是预测时域的长度。cost function由阶段性的代价 L L L和终端代价 F F F共同构成。 N c N_c Nc和 N c u N_{cu} Ncu分别是状态约束和输出约束的时域长度。 N u N_u Nu为控制时域的长度。

MPC在每一轮时间下求解一段最优序列 U ∗ ( t ) U^*(t) U∗(t)并且输出序列的第一个值。

4.3. Trajectory Generation and Tracking

依据已知的路径进行轨迹生成主要有两种方式：一种通过优化方法在生成轨迹的同时去跟踪轨迹；另一种则是将两个部分分开进行。

4.3.1. Combined Trajectory Generation and Tracking

Combined Trajectory Generation and Tracking这种方式通常针对于最优时间轨迹方面的应用，对于复杂的环境，其实时性可能会出现问题。

作者这里给了一篇文献：Kunz, Tobias, and Mike Stilman. Time-Optimal Trajectory Generation for Path Following with Bounded Acceleration and Velocity. 2013, https://doi.org/10.7551/mitpress/9816.001.0001.

4.3.2. Separate Trajectory Generation and Tracking

Trajectory Generation

轨迹生成问题可以归结为两点边界值问题(boundary value problem)。边界条件通常是包含起始状态 x ( t 0 ) = x 0 x(t_0) = x_0 x(t0)=x0 和最终目标状态 x ( t f ) = x f x(t_f ) = x_f x(tf)=xf 的约束，系统动力学 x ˙ = f ( x , u ) \dot x = f (x, u) x˙=f(x,u) 作为附加约束。如果给定满足边界条件的状态轨迹 x ( t ) x(t) x(t)没有控制输入 u ( t ) u(t) u(t)，则轨迹被定义为不可行的。

轨迹生成主要有两类方法：

1. 基于传感器。主要应用于机器人领域，依赖环境感知生成响应轨迹，不考虑相关动力学因素。
2. 基于动力学。这方面有许多优化算法，比如应用遗传算法、梯度下降等等。这类方法也有在辅助驾驶上的应用。

Trajectory Tracking

轨迹相较于路径，还有对运动速度的描述（其实在更多其他的资料里，轨迹相比于路径多了时间 t t t这一个维度）。

主要有两种方法：

1. 基于几何的方法。
2. 基于模型的方法。基于模型的方法通常分为基于运动学和基于动力学的方法；前者适用于低速、曲率突变小的场景；后者适用于高速、曲率较大的场景。基于模型的方法通常需要路径是连续的，但可能对干扰和较大偏差鲁棒性较差。

Geometric Path Tracking

Pure pursuit

基于几何的跟踪算法使用简单的几何关系去推导转向控制律。这些方法使用前视距离去估计误差，算法的复杂程度从简单的圆弧计算到复杂的几何定理都有。纯追踪算法就是一个典型的基于几何的算法。

纯追踪算法的输入是一系列的路径点，而不是连续的曲线，这也就不需要考虑离散化所带来的影响。但是纯追踪算法难以适用于高速场景或者曲率突变的场景，同时前视距离没选好的话，会存在稳态误差(本质就是个P控制器)。

为了避免限制路径的生成方式，路径应该分段线性的方式进行表示。通过这一原理，一个光滑的有连续的一阶导和二阶导的轨迹曲线，应该有一系列密集点进行表示，从而应用于更多算法。(这一部分还需进一步的理解)

下面的文献是纯追踪算法的一个变种，基准点不是选在后轴，而是在距离后轴的一段距离 x b x_b xb的 e e e点上。

Kuwata, Yoshiaki, et al. "Motion Planning in Complex Environments Using Closed-Loop Prediction." AIAA Guidance, Navigation and Control Conference and Exhibit, 2008, https://doi.org/10.2514/6.2008-7166.

还有研究证明通过正确组合前视距离以及系统延迟可以使得纯追踪算法稳定。

纯追踪算法有一个可调的参数前视距离 L f w L_{fw} Lfw， L f w L_{fw} Lfw越小，跟踪时会离路径更近，但会产生震荡； L f w L_{fw} Lfw越大，会有跟踪误差，但跟踪效果平滑。

推导过程比较简单，就直接将原文截进来用了。

纯追踪算法的缺陷：没有考虑到目标点所在路径的曲率以及目标点的航向角。纯追踪算法仅仅计算了圆弧的几何特性，忽略了车辆的横向动力学特性，特别是当速度提升时，横向动力学的影响将会更大。

---

Stanley

Stanley算法也十分经典。

论文：Hoffmann, Gabriel M., et al. "Autonomous Automobile Trajectory Tracking for Off-Road Driving: Controller Design, Experimental Validation and Racing." 2007 American Control Conference, 2007, https://doi.org/10.1109/acc.2007.4282788.

Stanley算法通过非线性控制律来计算转向命令。该控制律主要考虑了从车辆前轴计算的跟踪误差 e f a e_{fa} efa和航向角误差 θ e \theta_e θe

θ e = θ − θ p \theta_e=\theta-\theta_p θe=θ−θp， θ \theta θ是车辆的航向角， θ p \theta_p θp是道路在 ( x p , y p ) (x_p,y_p) (xp,yp)点处的切线方向所形成的角。

如此可以得到转向角计算公式：
δ ( t ) = θ e ( t ) + t a n − 1 ( k e f a ( t ) v x ( t ) ) \delta(t)=\theta_e(t)+tan^{-1}\left(\frac{ke_{fa}(t)}{v_x(t)}\right) δ(t)=θe(t)+tan−1(vx(t)kefa(t))
k k k为增益系数， v x ( t ) v_x(t) vx(t)为车辆的纵向速度。

优点 ：能够收敛到0。相比纯追踪算法，跟踪效果更好且不会出现"cut cornor"的问题。高速状态下，适应性好。
缺点： 对干扰鲁棒性差，更容易发生震荡。需要提供拥有连续曲率的路径而不是离散点。

Trajectory Tracking with a Model

De Luca, Oriolo, and Samson等人提出了一种基于运动学自行车模型的控制算法《Feedback control of a nonholonomic car-like robot》。

如图所示，他们将路径描述成关于长度 s s s的函数， θ p \theta_p θp为 x x x轴和 ( x p , y p ) (x_p,y_p) (xp,yp)切线方向的夹角。航向角误差则为： θ e = θ − θ p ( s ) \theta_e=\theta -\theta_p(s) θe=θ−θp(s)，道路曲率被定义为： κ ( s ) = d θ p ( s ) d s \kappa(s)=\frac{d\theta_p(s)}{ds} κ(s)=dsdθp(s)，两边同乘以 s ˙ \dot s s˙，得 θ ˙ p ( s ) = κ ( s ) s ˙ \dot \theta_p(s)=\kappa(s)\dot s θ˙p(s)=κ(s)s˙。
s ˙ 和 e ˙ r a \dot s和\dot e_{ra} s˙和e˙ra如下表示：
s ˙ = v c o s ( θ e ) + θ ˙ p e r a e r a = v s i n ( θ e ) \begin{aligned}\dot{s}&=vcos(\theta_e)+\dot{\theta}pe{ra}\\e_{ra}&=vsin(\theta_e)\end{aligned} s˙era=vcos(θe)+θ˙pera=vsin(θe)

最后得到运动学模型：
[ s ˙ e ˙ r a θ ˙ e δ ˙ ] = [ c o s ( θ e ) 1 − e ˙ r a κ ( s ) s i n ( θ e ) ( t a n δ L ) − κ ( s ) c o s ( θ e ) 1 − e ˙ r a κ ( s ) 0 ] v [ 0 0 0 1 ] δ ˙ \left[\begin{array}{c}\dot{s}\\\dot{e}{ra}\\\dot{\theta}{e}\\\dot \delta\end{array}\right]=\left[\begin{array}{c}\frac{cos(\theta_{e})}{1-\dot{e}{ra}\kappa(s)}\\sin(\theta{e})\\(\frac{tan\delta}L)-\frac{\kappa(s)cos(\theta_{e})}{1-\dot{e}_{ra}\kappa(s)}\\0\end{array}\right]v\left[\begin{array}{c}0\\0\\0\\1\end{array}\right]\dot{\delta} s˙e˙raθ˙eδ˙ = 1−e˙raκ(s)cos(θe)sin(θe)(Ltanδ)−1−e˙raκ(s)κ(s)cos(θe)0 v 0001 δ˙

---

MPC在路径跟踪控制中应用不少，但是耗时较大。作者同样给了几篇关于基于动力学/运动学模型的MPC的相关文献。