学习笔记11--局部轨迹直接构造法

本系列博客包括6个专栏，分别为：《自动驾驶技术概览》、《自动驾驶汽车平台技术基础》、《自动驾驶汽车定位技术》、《自动驾驶汽车环境感知》、《自动驾驶汽车决策与控制》、《自动驾驶系统设计及应用》。
此专栏是关于《自动驾驶汽车决策与控制》书籍的笔记.

---

---

2.汽车局部轨迹规划

2.2 局部轨迹直接构造法

2.2.1 直线/圆弧段构造

假设汽车只能向前行驶，Dubins曲线是在满足曲率约束和规定的始端和末端的切线方向的条件下，连接两个二维平面(即X-Y平面)的最短路径；如果汽车也可以反向行驶，则路径为Reeds-Shepp曲线；

对于汽车，系统的简单运动学模型为：
{ x ˙ = v cos ⁡ θ y ˙ = v sin ⁡ θ θ ˙ = u (1) {˙x=vcosθ˙y=vsinθ˙θ=u

\tag{1} ⎩ ⎨ ⎧​​x˙=vcosθy˙​=vsinθθ˙=u​(1)
其中：$(x,y)$是汽车的位置，$\theta$是航向，汽车以恒定速度$v$移动，转弯速度控制$u$是有界的；

最佳路径类型可以用与汽车驾驶行为相似的"右转( R )，左转(L)，直线(S)"描述来表达；

最佳路径总是六种类型之一：RSR，RSL，LSR，LSL，RLR，LRL；

实例说明： 考虑对于某些给定的初始位置和最终位置及切线，最佳路径显示为RSR类型，表明汽车行进顺序为：右转弧( R )，直线段(S)，右转弧( R )；这个序列形成最短的曲线，将起始点$A$连接到终点$B$，并在每个端点处曲线满足切线条件，且该处曲率不超过给定值大小；

2.2.2 多项式螺旋线构造

• 代表了一类曲率可以用弧长(对应轨迹中的$s$轴)的多项式函数来表示的曲线簇；

• 使用三阶(Cubic)或五阶(Quintic)多项式螺旋线，其曲率$\kappa$和轨迹弧长$s$的关系$\kappa (s)$的关系如下：

  • 三阶：$\kappa (s)={\kappa }_0+{\kappa }_{1}s+{\kappa }_2{s}^2+{\kappa }_3{s}^3$；
  • 五阶：$\kappa (s)={\kappa }_0+{\kappa }_{1}s+{\kappa }_2{s}^2+{\kappa }_3{s}^3+{\kappa }_4{s}^4+{\kappa }_5{s}^5$；

• 基于这种使用三阶(五阶)螺旋线连接的轨迹，其参数可以通过梯度下降(Gradient Descent)方法快速有效进行搜索；

• 以三阶多项式为例：考虑从汽车初始姿态$q_{init}=(x_1,y_1,{\theta }_1,{\kappa }_1)$到目标姿态$q_{goal}=(x_G,y_G,{\theta }_G,{\kappa }_G)$，且具有连续曲率的三阶螺旋线；在初始状态时，考虑曲率的一阶导数和二阶导数均需要满足初始状态的限制，可得：
  κ 0 = κ 1 κ 1 = d κ ( 0 ) / d s κ 2 = d 2 κ ( 0 ) / d s 2 (2) κ0=κ1κ1=dκ(0)/dsκ2=d2κ(0)/ds2

  \tag{2} ​κ0​=κ1​κ1​=dκ(0)/dsκ2​=d2κ(0)/ds2​(2)

2.2.3 样条曲线

• 曲线插值是经过所有给定的点的方法，曲线拟合是生成曲线逼近这些给定的点；
• 对于汽车运动规划和控制系统，需要基于样条插值使得汽车运动轨迹能经过控制点，增加拟合曲线与目标轨迹的匹配程度；
• 样条曲线可通过数值计算，利用参数调整曲线使之逼近多边形而得到，是基于多边形的边和点构造而成的光滑曲线；样条曲线的形状受多边形顶点的数目和位置影响，其中用以确定样条曲线的多边形被称为样条曲线的特征多边形或控制多边形，多边形的顶点被称为样条曲线的控制点，样条曲线在数据拟合中需要穿过的给定的点称为型值点；

一些常用曲线构造：

1. Bezier曲线构造

   一段$n$次Bezier曲线的表达式为：
   $$P(u)=\sum _{i=0}^n{B}_i^n(u)P_i,u\in [0,1]\text{\hspace{1em}(3)}$$

   B i n ( u ) = n ! i ! ( n − i ) ! u i ( 1 − u ) n − i , i ∈ { 0 , 1 , … , n } (4) B_{i}^n(u)=\frac{n!}{i!(n-i)!}u^i(1-u)^{n-i},i\in\{0,1,\dots,n\}\tag{4} Bin​(u)=i!(n−i)!n!​ui(1−u)n−i,i∈{0,1,…,n}(4)

   其中：$u$是位置参数，$P_i$是Bezier曲线的控制点；

   Bezier曲线的控制点描述了曲线的大体走向，通过控制曲线的控制点就可以控制曲线的形状；所有控制点组成的多边形称为曲线的特征多边形；

   在规划汽车路径时，常用的几何性质：

   1. 端点性质

      1. 曲线起始于第一个控制点，终止于最后一个控制点；
         $$P(0)=P_0，P(1)=P_n\text{\hspace{1em}(5)}$$

      2. 起点处和终点处的Bezier曲线分别相切于特征多边形的第一条边和最后一条边；
         $$T(0)=n(P_1-P_0)，T(1)=n(P_n-P_{n-1})\text{\hspace{1em}(6)}$$

      3. Bezier曲线起点处和终点处的二阶导数分别只与前3个控制点和最后3个控制点有关；
         $$S(0)=n(n-1)(P_2-2P_1+P_0)，S(1)=n(n-1)(P_n-2P_{n-1}+P_{n-2})\text{\hspace{1em}(7)}$$

   2. 凸包性

      Bezier曲线$P(u)$是各控制点$P_i$的凸线性组合，曲线落在控制点构成的凸包中；当各控制点构成的特征多边形是凸时，整条Bezier曲线也是凸的，即曲线没有反曲点(拐点)；

   3. 几何不变性

      若满足位置连续约束条件，则需要两个控制点，即曲线的第1个和最后1个控制点；若满足方向连续条件，则需要4个控制点，前2个和最后两个控制点；若满足曲率连续约束条件，则需要6个控制点，前3个和最后3个控制点；
      $$S(0)=n(n-1)(P_2-2P_1+P_0)c(u)=[P_x^{\prime }(u)P_y^{\prime \prime }(u)-P_x^{\prime \prime }(u)P_y^{\prime }(u)/||P^{\prime }(u)|{|}^3]\text{\hspace{1em}(8)}$$
      若要实现Bezier曲线对两线段的光滑过渡，Bezier曲线至少要有6个控制点，由于$n$次Bezier曲线具有$n+1$个控制点，则Bezier曲线的次数至少为5次；

2. B样条曲线概述

   • B样条曲线来自于贝塞尔曲线，但不同于贝塞尔曲线，B样条曲线可以进行局部调整，避免了贝塞尔曲线不容易局部调整的缺点，这种特性称为局部支撑性；
   • B样条曲线具有曲率连续的优点，特别是在相邻曲线段之间的结点处曲率也是连续的；
   • 轨迹规划中常用的是三阶B样条曲线，以满足汽车运动学约束；

   设有控制顶点$P_0,P_1,P_2,\dots ,P_n$，则$k$阶$(k-1)$次B样条曲线的数学表达式为：
   $$P(t)=\sum _{i=0}^n{N}_{i,k}(t)P_i\text{\hspace{1em}(9)}$$
   其中：$N_{i,k}(t)$是B样条基函数，$N_{i,k}(t)$中每一部分被称为B样条；

   B样条基函数是一个$k$阶$k-1$次分段多项式，也被称为$k$阶$k-1$次多项式样条；参数$t$是一组被称为结点矢量的非递减序列；

   德布尔-考克斯递推定义：
   N i , 1 ( t ) = { 1 , t i ≤ t ≤ t i + 1 , 0 , t ≤ t i ， t ≥ t i + 1 ， k = 1 (10) N_{i,1}(t)= {1,ti≤t≤ti+1,0,t≤ti，t≥ti+1

   ，k=1\tag{10} Ni,1​(t)={1,0,​ti​≤t≤ti+1​,t≤ti​，t≥ti+1​​，k=1(10)

   $$N_{i,k}=\frac{t-t_i}{t_{i-k-1}-t_i}{N}_{i,k-1}(t)+\frac{t_{i+k}-t}{t_{i+k}-t_{i+1}}{N}_{i+1,k-1}(t)，k\ge 2\text{\hspace{1em}(11)}$$

   约定：$\frac{0}{0}=0$；

   该递推公式表明：欲确定第$i$个$k$阶B样条$N_{i,k}(t)$，需要用$t_i,t_{i+1},\dots ,t_{i+k}$共$k+1$个结点；称区间$[t_i,t_{i+k}]$为$N_{i,k}(t)$的支撑区间；

   曲线方程中，$n+1$个控制顶点$P_i(i=0,1,2,\dots ,n)$要用到$n+1$个$k$阶B样条基$N_{i,k}(t)$；支撑区间的并集定义了这一组B样条基的结点矢量$T=[t_0,t_1,\dots ,t_{n+k}]$；

   假设控制多边形的顶点数为$n$，阶数为$k$(次数为$k-1$)，则结点矢量是$T=[t_0,t_1,\dots ,t_{n+k}]$；B样条曲线按结点矢量中结点的分布情况，划分成4中类型：

   • 均匀B样条曲线；这种B样条曲线结点矢量中结点间的差值相等，即等间距均匀分布；
   • 准均匀B样条曲线；准均匀B样条曲线在两端点处存在重复度$k$；具有这样特点的结点矢量可以确定准均匀的B样条基；
   • 分段贝塞尔曲线；当B样条曲线的结点矢量两端点处存在重复度$k$，而所有内部结点重复度为$k-1$时，这样的结点矢量可以确定分段的Bernstein基；当贝塞尔曲线用于表示分段的B样条曲线后，每一段的B样条曲线可以单独调整而不影响其他部分的曲线，调整某一控制点也只是影响该控制点参与控制的曲线段的形状；
   • 非均匀Bezier曲线；在保证结点序列非递减且两端点重复度不大于$k$、内结点重复度不大于$k-1$的情况下，结点矢量可以任意取；这样的一组结点矢量可以确定非均匀的B样条基；

   B样条曲线具有如下性质：

   1. 局部性。参数为$t(t_i<t<t_{i+1})$的$k$阶B样条曲线上一点最多能够影响$k$个控制点，而与其余的控制点不产生联系；当调整该曲线上某一控制点$P_i$的时候，仅仅能够影响区间$(t_i,t_{i+1})$内曲线的形状，对其他曲线不产生影响；
   2. 连续性。假如B样条曲线在某一结点处存在$r$重结点，那么曲线在该结点处连续阶大于或等于$k-r-1$；
   3. 凸包性。B样条曲线总是位于控制多边形的凸包内部；
   4. 分段参数多项式。$P(t)$在每一区间都是关于参数$t$的多项式，且它的次数小于或等于$k-1$；
   5. 变差缩减性。平面内存在一定数量的控制点用于构成B样条曲线的控制多边形，平面内任意一条直线与B样条曲线相交的点的数目小于或等于该直线与控制多边形相交的点的数目；
   6. 几何不变性。B样条曲线的形状和位置不因坐标系的改变而发生变化；
   7. 仿射不变性。在仿射变换下，B样条曲线的表达式形式不会发生改变；
   8. 造型的灵活性。利用B样条曲线可以构造直线端、尖点、切线等特殊情况；