【Paper Notes】Perception, Planning, Control, and Coordination for Autonomous Vehicles

Purely as reading notes, the content of the article may be a bit confusing.

1. Introduction

The author first introduces some of the potential roles of autonomous vehicles in future urban transportation systems, including increased safety, increased productivity, improved accessibility, improved road efficiency, and positive impacts on the environment; then he introduces the origins and impact of autonomous vehicles Development, of course, requires an introduction to DARPA; the author also introduces the advantages (convenience and cheapness) of the MoD (Mobility on Demands) System compared to traditional transportation methods dominated by private cars, which is also a direction for the sharing of self-driving cars.
…
Companies such as Google, Tesla, Uber, etc. all have a lot of investment and research and development in the field of autonomous driving.
...
Autonomous driving in urban scenarios has always been a hot topic, and DARPA Urban Challenge and V-Charge Project have introduced a lot of work in this area. Urban scenes mainly address the impact of dynamic and complex traffic environments, traffic participants, etc. on autonomous vehicles.

Below is AI’s explanation of the V-Charge Project :

Project V-Charge is a European research project aiming to develop advanced technologies for autonomous parking and vehicle charging. The project focuses on developing a fully automated system that allows electric vehicles (EVs) to park and charge autonomously in public parking lots without human intervention.

The V-Charge system consists of several components, including:

1. Automated Parking (AVP) system - This system allows EVs to automatically park in designated parking spaces using advanced sensors and algorithms.

2. Automatic Charging (AC) System - This system allows EVs to connect and charge automatically without human intervention.

3. Fleet Management (FM) System – This system allows operators to manage and monitor EV charging and parking in real time.

The V-Charge project is funded by the European Union and involves a number of leading technology companies and research institutions, including Bosch, BMW and the University of Oxford. The project aims to reduce the environmental impact of urban transport by promoting the use of EVs and making charging and parking more convenient and accessible.

...
The following is the architecture diagram given by the author:

a classic architecture diagram with perception, planning and control as the main modules. The role of V2V communication in autonomous driving systems is also considered.

Due to the research direction, I will focus on the PNC part and omit other parts for the time being.

2. Perception

For now

3. Planning

3.1. Autonomous Vehicle Planning Systems

The typical architecture divides Planning into three layers: Mission Planning, Brahavioral Planning, and Motion Planning.

• Mission Planning: High-level planning, such as deciding whether, when, where, how to travel, the best route, etc.
• Bhavioral Planning: Making decisions based on other agents or traffic rules, such as overtaking, yielding, merging, etc.
• Motion Planning: Motion planning, the most typical goal is to reach the target area while avoiding collision with obstacles.

3.2. Mission Planning

Provide a Route Network Definition File ( RNDF ) as a priori information. RNDF contains the topology of traversable road sections, as well as information such as stop sign locations, lane widths, and parking space locations. RNDF was manually annotated in the early days, but now there are also studies on automatic annotation and online annotation.

The Road Network Graph (RNG) stores relevant information.
Classic algorithms: Dijkstra, Astar,…

Here the author also provides a review of 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 needs to enable vehicles to not only meet traffic regulations and travel along paths, but also interact with surrounding traffic participants. This can be achieved through a combination of local target setting, virtual obstacle placement, drivable area boundary adjustments, and area heuristic cost adjustments. Similar to Apollo and other rule-based solutions.

FSM is often used in behavioral decision-making, but because it is artificially designed, it cannot take into account all complex scenarios. When encountering unknown scenarios, it is easy to cause problems.

The author also introduced some other work, including experiments using Linear-Temporal Logic (LTL) to complete overtaking scenarios and some attempts to find a larger rule base to adapt to more complex scenarios, etc.

3.4. Motion Planning

• Typical indicators of Motion Planning are computational complexity and algorithm completeness .
• The problem of Motion Planning can be regarded as a "piano moving problem". The structural diagram of the house and piano is used as the input of the planner to obtain a safe and collision-free path. At the same time, this problem is a PSPACE-hard problem.
  • The PSPACE complexity class refers to a problem that can be solved by using a deterministic Turing machine on an input of length $When\; n$ , the memory (space) used is$O\left(n^k\right)$, where$k\ge 0$ . A deterministic Turing machine is a hypothetical device that operates on a tape that can only change one symbol/value at a time, and can only specify one operation for any given situation. If a question A is considered PSPACE-hard, then every question/language$B$ can be reduced to$A$ , immediately$B\le pA$ , which means any$B$ can be converted to AAin polynomial timeAn instance of$A.$
• The core of Motion Planning is to convert a continuous space model into a discrete space model. Two types of transformation methods:

1. combinatorial planning: A completely discretized description of the original problem
2. Sampling-based planning: Use collision checking to search discrete sampling points in the configuration space.

3.4.1. Combinatorial Planning

The combinatorial planning method has good efficiency in solving low-dimensional problems, but for complex problems with multiple obstacles and high dimensions, the computational complexity will be greatly increased.

In engineering applications, it is difficult to decompose space directly from obstacle geometry. A simple approach is a uniform discretization of the configuration space or robot actions so that solution paths can be found by finite search. By working in a discrete space, the complexity of exhaustive searches is greatly reduced. But the completeness or optimality of this approach depends on the accuracy of the discretization.

Several methods mentioned in DARPA Grand Challenges and DUC:

• Method based on discretizing kinematically reachable trajectory sets
• Generate trajectory search tree based on road geometry
• An optimization-based velocity smoothing method further improves the above method.
  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.
• Speed
• Cell decomposition , this type of method is rarely used

The author introduces some methods of behavioral decision-making. For example, methods combining Cell Decomposition and Linear Temporal Logic (LTL) are used to generate paths that comply with traffic regulations; methods based on LTL are used to break the rules in some cases; Cell Decomposition is combined with POMDP or MOMDP methods to deal with uncertain environments. 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 assumes uncertainties in robot motion and observations and takes these uncertainties into account in solving the optimal strategy. (Mixed Observability Markov Decision Processes, MOMDP) ​​is an extension of POMDP. It is partially observable in some states and fully observable in other states. Ong, Sylvie CW, 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 is difficult to solve accurately, and estimation methods are usually used instead; at the same time, it also faces the problem that the computational complexity increases rapidly with the growth of state quantities and computational horizons. There are also some studies focusing on POMDP in continuous space. The authors also introduce the efficient point-based value iteration solver SARSOP.

Let’s go back and take a closer look at these papers…

3.4.2. Sampling-Based Planning

Sampling-based methods usually have probabilistic completeness. Variants of sampling-based algorithms differ primarily in the method of generating search trees. Classic methods include PRM and RRT.

Planning not only needs to consider completeness and efficiency, but also the quality of the planned path (this paragraph mainly refers to optimality. For the trajectory of unmanned vehicles, factors such as smoothness, timeliness, etc. also need to be considered) . Here the author introduces bias-RRT, as well as progressively optimal algorithms such as PRM*, RRT*, Fast Marching Trees (FMT*), and Stable Sparse Trees (SST*).

3.5. Planning in Dynamic Environments

To achieve autonomous driving in urban scenes, a large number of uncertainties need to be considered. These uncertainties may come from sensor accuracy errors, positioning errors, environmental changes, errors in control execution, and movement changes of surrounding obstacles that have the greatest impact.

3.5.1. Decision Making Structures for Obstacle Avoidance

There is a method in DARPA: detect areas where collisions may occur along the path or intersection merge areas, use the trajectories of surrounding vehicles to calculate TTC, and stop if a collision is about to occur. This approach is relatively conservative. The defensive driving mentioned below also detects collision trajectories similarly. The advantage of this method is that the calculation is simple because the time dimension is ignored; however, it can only be applied to relatively simple scenarios.

The method based on POMDP can clearly explain various uncertainty factors in the movement of autonomous vehicles, including uncertainty in the self-vehicle control process and uncertainty in the surrounding environment and obstacles. However, POMDP is difficult to model and requires Discretize the state space.

3.5.2. Planning in Space-Time

In order to better consider the trajectory of obstacles, the time dimension needs to be introduced. For obstacle perception, it is relatively easy to obtain the instantaneous speed and position of a moving obstacle, but it is more difficult to obtain the future trajectory of the obstacle. The author here introduces the use of constant speed to estimate the trajectory of obstacles and the use of rapid iterative re-planning to reduce errors; it also introduces a relatively conservative strategy to expand the obstacle bounding box.

This picture shows the obstacle in ${\mathbf{R}}^2$ Visualization diagram of trajectory prediction in space-time space. Left 1 and Left 2 do not consider the uncertainty of the obstacle's future trajectory; the remaining two methods consider speed limits and acceleration limits respectively. This idea can also be used in boundary estimation of vehicle speed, acceleration and trajectory boundary estimation.

3.5.3. Control Space Obstacle Representations

Another way is to plan directly in the control space. Here the author mentioned an algorithm for multi-robot planning: Reciprocal Velocity Obstacles.

3.6. Planning Subject to Differential Constraints

Differential constraints! ! !

The motion planning problem will eventually become a high-level control problem. In previous methods, the importance of control is often ignored due to computational complexity and modeling complexity. This will cause large errors in the control part of the trajectory planned by Motion Planning, making the trajectory ineffective and even dangerous. . The error between the execution trajectory and the planned trajectory makes the collision detection in the planning part less effective and may cause danger.

Trajectories that are longer may take less time to execute than trajectories that are shorter .

For a system, the most important differential constraint is the progression of time, because time $t$ must move at a constant rate$\dot{t}=1$ growth

Robot differential constraints are mainly used in the following directions:

1. Differential constraints are used to generate velocity profiles, and the geometric path on this profile is performed in a decoupled manner. (In fact, it is the idea of ​​path-speed decoupling). The author gives two papers here. One of these articles should be the earliest source of the idea of ​​speed-path planning decoupling. 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. When used in the sampling algorithm, when building a tree, the direct integrated method is directly used to solve the connection of each segment in real time. The author in this part mainly recommends some kinodynamic random sampling algorithms.
3. Consider turning radius limits. Such as Dubins curve and Reeds-Shepp curve. There are also some heuristic algorithms.

The decoupling method of differential constraints usually has low solution efficiency and may not be able to find a solution ; direct integrated differential constraint can make up for these shortcomings, but the calculation is more complicated.

Sampling is made more efficient by limiting the states to be sampled to a set of states that start from the starting condition and can be reached in an obstacle-free space via trajectories that comply with kinodynamic constraints.

Applying Reachability Guidance (RG) and Nearest Neighbor (NN) search to state sampling can significantly improve efficiency. This cannot be achieved using traditional Euclidean spatial distance search.

This section introduces several analytical methods for applying reachability guidance strategies (RG) to motion planning. Among them, the reachable subspace is expressed as a hyperrectangle of a linear dynamics approximation system through the Ball Box Theorem, thereby extending the asymptotic optimality of the RRT* algorithm to the reachability guidance variant. Similar methods are applied in constraint-handling variants of PRM and FMT to demonstrate their asymptotic optimality. In addition, Goal-Rooted Feedback Motion Tree (GR-FMT) and analytical methods for solving two-point boundary value problems are also introduced, but they all have certain limitations.

There are also methods that use machine learning, such as SVM to find reachable states.

3.7. Incremental Planning and Replanning

Since in actual working conditions, the self-vehicle usually cannot observe the entire environment, but only parts of the environment, so incremental planning (incremental planning) is very necessary. At the same time, due to changes in the dynamic environment, the originally planned trajectory may no longer be applicable, so replanning is required.

Incremental planning needs to obtain a series of sub-goals or use a heuristic algorithm to score the trajectory library. The author introduced an algorithm that uses FSM to generate sub-goals. When the planned path is blocked, sub-goals are generated. The sub-goals are on a pre-defined path and are separated by a pre-defined distance.

In this part, the author introduces MOD (Mobility on Demands) again and mentions the importance of predefined paths in MOD: it can avoid local planning from falling into the minimum.

Considering that each planning cycle requires limited computational time and that the environment may change during this period, planning security mechanisms should also be carefully designed.

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 .
The above document also plans in a speed-path decoupled manner, specifying the degree of deceleration based on the proximity of the nearest obstacle measured from the weighted longitudinal and lateral offsets of the desired path (this method is called Dynamic Virtual Bumper). However, in this document, dynamic obstacles are only regarded as enlarged static obstacles.

In addition, there are some methods that comprehensively consider speed and spatial paths, such as Inevitable Collision State Avoidance (ICS-AVOID), Non-Linear Velocity Obstacles (NLVO), and Time-Varying Dynamic Window.

4. Control

Control represents the execution capability of the system, also known as motion control, which is the process of converting intentions into actions. Its purpose is to execute the planned intent by providing necessary inputs to the hardware level. Basic concepts such as Controllers, Measurements, and Models will not be explained in detail.

4.1. Classical Control

The basis of control theory will not be elaborated in detail.

Feedback control is a commonly used classical control method. A typical feedback control is PID.
$$u(t)=k_d\dot{e}+k_{p}e+k_i\int e(t)dt$$

Pure feedback control will have the following shortcomings:

1. The response to errors is delayed and will only respond when an error occurs.
2. The same mechanism is used to respond to interference, model errors, and measurement noise. This coupled approach is less logical than responding to these errors independently.

Adding feedforward controls can remedy these deficiencies.

Comparison of feedforward and feedback.

There are many control methods in this area based on the state space control method of modern control theory.

Typical linear state space model:
$$\begin{gathered} \dot{x(t)}=Ax(t)+Bu\left(t\right) \\ y(t)=Cx(t)+The\; Du\left(t\right) \end{gathered}$$
nonlinear state space model can be expressed by the following formula:

$$\begin{gathered} x_{t+1}=f(x_t,u_t)+{ϵ}_t \\ {y}_t=h(x_t)+d_t \end{gathered}$$

Among them, $x_t$Represents the state vector, $u_t$Represents the control vector, $y_t$represents the observation vector. $f$ is the state transition function,$h$ is the observation function,${ϵ}_t$和$d_t$is noise, usually assumed to be Gaussian white noise. This is the expression of a general nonlinear state space model, and there are also special nonlinear state space model forms.

2 degree of freedom controllers can also be applied to nonlinear systems. Feedforward is used to generate reference trajectories, while feedback is used to compensate for disturbances and errors .

4.2. Model Predictive Control

The three major elements of MPC: prediction model, feedback control, and rolling optimization.
For planning and prediction purposes, automatic control systems require the intervention of models. This has to mention the classic MPC. MPC combines model prediction and optimal control. By converting the control problem into an optimization problem, it combines the optimization of control actions in the future with the real-time adjustment of the optimal control strategy to achieve optimal control.

The core idea of ​​MPC is to predict future system states based on prediction models, and use these prediction results to determine control inputs to optimize control performance. Optimization is used to select the optimal control strategy, pre-calculate the future multi-step control action sequence, and then only execute the first control action. At the beginning of the next control cycle, the process will be repeated again. In this way, MPC can handle nonlinear and unsteady problems while being applicable to a variety of different types of control objectives and constraints.

The main advantages of MPC include:

• Able to handle nonlinear, time-varying and distributed parameter systems;
• Ability to handle various types of constraints, such as input, output, and state constraints;
• It has strong robustness and can cope with model uncertainties and disturbances;
• Able to optimize a variety of control objectives, such as tracking, stability, and energy consumption.

MPC Framework

However, MPC consumes more computing resources. If the computing power of on-board chips is greatly improved, the application value of MPC will be even higher.

MPC has been used in a variety of automotive control applications, including traction control, braking and steering, lane keeping, and more. Model predictive control technology has also been applied to various trajectory tracking control problems.

The general description form of the MPC problem is:

$t$ is the time series. $v(h|t)$ represents$v$ at timettThe predicted duration at$t$$h$ step. Equations (7) and (8) are descriptions of the discrete-time system model, and the sampling interval is$T_s$，$x\in {\mathbf{R}}^n$ is the state variable of the system,$u\in R^m$ is the control input,$y\in R^p$是系统输出。$U(t)=(u(0|t)，\cdot \cdot \cdot ，u(N-1|t))$ is the sequence of control inputs, and its optimal value needs to be solved,$N$ is the length of the prediction time domain. The cost function consists of the staged cost$L$ and terminal cost$F$ jointly constitute. $N_c$和$N_{cu}$are the time domain lengths of state constraints and output constraints respectively. $N_u$To control the length of the time domain.

MPC solves an optimal sequence U ∗ ( t ) U^*(t) at each round of time$U^{\ast }\left(t\right)$and output the first value of the sequence.

4.3. Trajectory Generation and Tracking

There are two main ways to generate trajectories based on known paths: one is to track the trajectory while generating the trajectory through optimization methods; the other is to separate the two parts.

4.3.1. Combined Trajectory Generation and Tracking

Combined Trajectory Generation and Tracking is usually aimed at applications in optimal time trajectories. For complex environments, its real-time performance may cause problems.

The author provides a document here: 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

The trajectory generation problem can be reduced to a two-point boundary value problem. Boundary conditions usually include the starting state $x(t_0)=x_0$and the final goal state $x\left(t_f\right)=x_f$Constraints, system dynamics $\dot{x}=f(x,u)$ as additional constraints. If a state trajectory x ( t ) x(t)satisfying the boundary conditions is given$x\left(t\right)$ has no control input$u\left(t\right)$ , then the trajectory is defined as infeasible.

There are two main methods of trajectory generation:

1. Based on sensors. Mainly used in the field of robotics, it relies on environmental perception to generate response trajectories without considering related dynamic factors.
2. Based on dynamics. There are many optimization algorithms in this area, such as applying genetic algorithms, gradient descent, etc. This type of method also has applications in assisted driving.

Trajectory Tracking

Compared with the path, the trajectory also describes the speed of movement (in fact, in more other information, the trajectory has more time $t$ this dimension).

There are two main methods:

1. Geometry-based methods.
2. Model-based approach. Model-based methods are usually divided into kinematics-based and dynamics-based methods; the former is suitable for scenes with low speed and small sudden changes in curvature; the latter is suitable for scenes with high speed and large curvature. Model-based methods typically require paths to be continuous but may be less robust to disturbances and large deviations .

Geometric Path Tracking

Pure pursuit

Geometry-based tracking algorithms use simple geometric relationships to derive steering control laws. These methods use look-ahead distances to estimate errors, and the complexity of the algorithms ranges from simple arc calculations to complex geometric theorems. Pure tracking algorithm is a typical geometry-based algorithm.

The input of a pure tracking algorithm is a series of path points rather than a continuous curve, so there is no need to consider the impact of discretization. However, the pure tracking algorithm is difficult to apply to high-speed scenes or scenes with sudden changes in curvature. At the same time, if the forward-looking distance is not selected well, there will be a steady-state error (essentially a P controller).

To avoid limiting how paths are generated, paths should be represented in a piecewise linear fashion. Through this principle, a smooth trajectory curve with continuous first-order and second-order derivatives should be represented by a series of dense points, so that it can be applied to more algorithms. (This part requires further understanding)

The following document is a variant of the pure tracking algorithm. The reference point is not selected on the rear axis, but at a distance $x_b$ee _Point$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.

There are also studies that prove that pure tracking algorithms can be made stable through the correct combination of look-ahead distance and system delay.

The pure tracking algorithm has an adjustable parameter look-ahead distance $L_{fw}$，$L_{fw}$The smaller it is, the closer it will be to the path during tracking, but it will cause oscillation; $L_{fw}$The larger it is, the tracking error will occur, but the tracking effect will be smoother.

The derivation process is relatively simple, just cut out the original text and use it directly.

Defects of the pure tracking algorithm : it does not take into account the curvature of the path where the target point is located and the heading angle of the target point. The pure tracking algorithm only calculates the geometric characteristics of the arc and ignores the lateral dynamic characteristics of the vehicle. Especially when the speed increases, the impact of lateral dynamics will be greater.

---

Stanley

Stanley's algorithm is also very classic.
Paper: 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's algorithm calculates steering commands through nonlinear control laws. This control law mainly takes into account the tracking error efa e_{fa} calculated from the front axle of the vehicle.$e_{fa}$and heading angle error $i_e$

$i_e=i-i_p$，$\theta$ is the heading angle of the vehicle,$i_p$is the road in $(x_p,y_p)$ is the angle formed by the tangent direction at the point.
In this way, the steering angle calculation formula can be obtained:
$$d\left(t\right)=i_e(t)+ta{n}^{-1}\left(\frac{e.g{\_}_{fa}(t)}{v_x\left(t\right)}\right)$$
$k$ is the gain coefficient,$v_x\left(t\right)$ is the longitudinal speed of the vehicle.

Advantages : Can converge to 0. Compared with the pure tracking algorithm, the tracking effect is better and there is no "cut cornor" problem. Good adaptability at high speed.
Disadvantages : Poor robustness to interference and more prone to oscillation. Need to provide paths with continuous curvature rather than discrete points.

Trajectory Tracking with a Model

De Luca, Oriolo, and Samson et al. proposed a control algorithm "Feedback control of a nonholonomic car-like robot" based on the kinematic bicycle model.

As shown in the figure, they describe the path with respect to the length ssfunction of$s$$i_p$for $x-$ axis sum$(x_p,y_p)$ the angle between the tangent directions. The heading angle error is:$i_e=i-i_p\left(s\right)$ , the road curvature is defined as:$k\left(s\right)=\frac{d{i}_p(s)}{ds}$, multiply both sides by $\dot{s}$ ,得${\dot{i}}_p(s)=k\left(s\right)\dot{s}$ .
$\stackrel{\dot{}and}{s}\_{\dot{e}}_{ra}$Definition:
$$\begin{array}{rl}\dot{s}& =vcos({\theta }_e)+{\dot{i}}_p{e}_{ra}\\ e_{ra}& =vsin(i_e)\end{array}$$

The inverse equation:
$$\left[\begin{array}{c}\dot{s}\\ {\dot{e}}_{ra}\\ {\dot{i}}_e\\ \dot{d}\end{array}\right]=\left[\begin{array}{c}\frac{cos(i_e)}{1-{\dot{e}}_{ra}k\left(s\right)}\\ sin(i_e)\\ (\frac{tan\delta }{L})-\frac{\kappa \left(s\right)cos(i_e)}{1-{\dot{e}}_{ra}k\left(s\right)}\\ 0\end{array}\right]v\left[\begin{array}{c}0\\ 0\\ 0\\ 1\end{array}\right]\dot{d}$$

---

MPC is widely used in path tracking control, but it is time-consuming. The author also provides several related literature on MPC based on dynamics/kinematics models.