Motion planning with environment

建立车辆模型：

对于汽车而言，质点模型，刚体模型都是不够的，汽车模型除了二维平面里的刚体涉及到的X，Y和θ外，还多了一个转向的变量。

可以将汽车四个*轮抽象成两个*，使汽车运动模型简化为自行车模型。模型如下图所示：

• κ：后轮转向半径R的倒数
• ω：方向盘转动角度
• L：前轮中心和后轮中心的距离

曲线坐标系SL

SL坐标系也叫做frenet frame，如下图所示：

它以道路中心线为参考，S表示道路中心线的方向，L表示与道路中心线垂直的方向。在结构化道路上行驶的时候，SL坐标系比XY坐标系更加贴合实际需求。

• SL坐标系到XY坐标系的转换：
  给定SL坐标系时，每一个点的S坐标本身对应一个（x-r，y-r）坐标，根据该点的横向偏移距离，可以求出给定点在世界坐标系中的XY位置。如下图所示：

• XY坐标系到SL坐标系的转化：
  SL坐标系并不是唯一的，XY会在曲线上产生很多投影，投影点是经过XY坐标，且垂直于曲线的线段与曲线的交点。
  

• signchange
  

• 超平面分割定理：判断两个动态障碍物是否相交。如果两个障碍物不相交，一定可以找到一个超平面将其分隔开。
  

多项式

在轨迹上以等距离的方式随机选择一些点，然后用高阶多项插值的方式来近似表示轨迹，对多项式进行优化。
缺点：高阶多项式不能用于平滑，因为高阶的多项式抖动太大，没有办法控制幅度，即龙格现象。

Bezier Spline

Bezier Spline 曲线是由一系列控制点定义的，例如P0 到 Pn，其中n代表曲线的阶数。如图所示，分别给出1阶、2 阶、3 阶 Bezier Spline 曲线的表示形式。通过对它们做平滑，得到平滑的曲线，例如二阶平滑保证曲线的曲率平滑。
缺点：除了起始点和终点，其它控制点不能保证会被得到的曲线经过。

生成光滑曲线

生成一条光滑的曲线，涉及到两方面，一方面是目标，另一方面是工具。

• How do we define smoothness?
  
  怎么定义平滑呢？最简单的方法就是最短路径，但是路径最短还不能保证平滑性，因此会对其不同阶导数进行 Minimize 求解，保证导数空间的连续，这就是 Smoothing Spline 最初的思想。

• Smoothing Spline 的特殊性质
  在给定边界的条件下，它是一个多项式，可以找到最优解。前提是它的 Boundary Constraint 只考虑了起点和终点，如果中间有障碍物就不是最优解。这种情况下可以使用 Piecewise Polynomial（分段多项式）来处理。
  一个 Piecewise Polynomial 是一维的函数，描述二维曲线是不够的，这时候就有一个 Spline 2D，假设我们把曲线分成 N 截，每节曲线段它的 X 坐标是一个 Polynomial ，Y 坐标也是一个 Polynomial 。

• spline 2D

把曲线分为n节线段，每一节都是一个polynomial函数，在内部是 N 阶可导的，但是如何保证节点处是平滑的呢？我们让它在 X 坐标上的变化率，也就是三阶导的平方是最小的，Y 上的变化率三阶导也是最小的，代价函数就是这两个变化率的和。代价函数的求解就是一个二次规划问题，我把这种 Loss Function 定义成这种形式是因为平方的积分能够给计算带来便利。端点的约束条件要保证连接点之间，对x方向的导数和对y方向的导数都要是相等的，一般要求到三阶导都是相等的，包括它的 X，Y 点的值也完全相等，此时就能保证三阶导连续。

• 螺旋曲线：通过一个极坐标形式定义，比如说沿着一条曲线，如果一个点 S 的曲率是知道的，假设它的原点在 （0，0）的位置，可以唯一定义出一条经过 S 的曲线，也就是 Spiral Path 。那么可以让 Spiral Path 满足起点、终点约束条件生成一条螺旋曲线。
  

• Spiral Path 和 Spline 2D 的区别
  任何的曲线在足够密的时候都可以用Piecewise Spiral path 或者是 Piecewise Polynomial 表示。
  区别：出发点不一样
  Polynomial 计算很快很简单，Spline 2D 是一个凸空间里面生成一个 Spline 曲线。Spiral Path 是从 Configuration Space 出发。理论上来讲，螺旋曲线生成的线是要比 Spiral Path 更好处理，对一些极端情况处理更好。