1.Mathematically, the motion planning problem is a high-dimensional non-convex optimization problem that is subject to static and dynamic obstacle constraints, kinematic constraints (e.g. curvature bounds), dynamic constraints (e.g. speed limit, acceleration limit, jerk limit), a nonlinear time-varying dynamic system model, traffic rules, smoothness requirements, and most importantly realtime requirements.

2.Wenda Xu在其博士论文Motion Planning for Autonomous Vehicles in Urban Scenarios: A Sequential Optimization Approach中的规划框架：

First, we use a vertical cell decomposition method to handle static obstacles,where the free-space is partitioned into a set of cells. Each cell is either a trapezoid or a triangle. The corridor is built by a list of connected convex cells.

Second, given the path corridor, a convex path optimization is applied to obtain a collision-free smooth path that also obeys the kinematic model. Based on Differential Flatness theory, the path optimization is formulated as a constrained convex optimization problem, which guarantees a global optimum. The curvature constraints are handled by an iterative optimization approach.

Third, we propose a space-time vertical cell decomposition method to handle dynamic obstacles. Compared to the normal vertical cell decomposition method, additional constraints need to be considered. Any feasible solution should be no faster than the minimum-time solution. Moreover, time should be monotonically increasing.

Fourth, given the coarse collision-free speed profile and the S-T corridor, a novel speed optimizer is proposed. Compared to previous methods, it can handle constraints on the station dimension such as velocity limit and stop line and constraints on the time dimension such as moving obstacles simultaneously based on iterative quadratic programming (QP). It can handle cases such as yielding to other traffic as well as merging in front of other vehicles.

3.轨迹生成的两种方法
There are two types of trajectory generation methods: forward kinematics and inverse kinematics. In forward kinematics, the initial state of the robot and control input are given, and the final state can be obtained through integrating the control input. Typical numerical integration methods include the Euler, Simpson, and Runge-Kutta methods. In inverse kinematics, the initial and final state of the robot are given, and the control input is to be determined. It usually requires solving a system of algebraic equations if the trajectory can be represented by polynomials or a trajectory optimization problem for general nonlinear systems.

4.运动学自行车模型

5.微分平坦性
微分平坦的定义：

车辆的状态（x,y,θ,δ）都可以用x和y相关的量来表示：


控制量(v,ω)也可以用x和y相关的量来表示：


由此可见，如果车辆轨迹是三阶可导的，那自然就满足非完整性的运动学约束。

6.关于曲率




7.拓扑路径搜索

论文Robust Real-time UAV Replanning Using Guided Gradient-based Optimization and Topological Paths中提出一种拓扑路径搜索方法。

引入两种不同的图节点，即guards和connectors，类似于PRM的可视性。

guards负责探索自由空间的不同部分，并且任意两个guards ，g1和g2彼此不可见(g1g2连接线处于碰撞中)。

在主循环之前，在起点s和终点g创建两个guards。

每次一个采样点对所有其他guards不可见时，在该点创建一个新的guard(第6-7行)。

为了形成路线图的路径，connectors用于连接不同的guards(第7-19行)。

当一个采样点正好对两个guards可见时，创建一个新的connectors，或者连接guards以形成拓扑上不同的连接(第19-20行)，或者替换现有的connectors以形成更短的路径(第16-17行)。

设置时间限制(tmax)或采样数限制(Nmax)以终止循环。

图示：

参考文献：
①详解TopologyPRM
②fast-planner-PRM算法解析-topo_prm.cpp