Study Notes 11--Direct Construction of Local Trajectories

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

---

---

2.Car local trajectory planning

2.2 Local trajectory direct construction method

2.2.1 直线/Circular arc segment structure

Assumes that the cars are driven forward,Dubins曲线是在满足曲率约束和规定的始端和末端的切线方向的条件下,连接两个二维平面(即X-Y平面)的最短路径;If the car can also reverse driving,则路径为Reeds-Shepp曲线;

对于汽车,Simple kinematics model for：
$$\{\begin{array}{ll}& \dot{x}=v\cos \theta \\ & \dot{y}=v\sin \theta \\ & \dot{\theta }=u\end{array}\text{\hspace{1em}(1)}$$
其中：$(x,y)$Is the location of the car,$\theta$是航向,The car at a constant speed$v$移动,Turning speed control$u$是有界的;

Best path types can use similar to vehicle driving behavior"右转( R ),左转(L),直线(S)"Description to express;

The best path is always one of the six types：RSR,RSL,LSR,LSL,RLR,LRL;

实例说明： Consider for some given the initial position and end position and tangent,The best path is shown asRSR类型,Show car marching order：Turn right arc( R ),直线段(S),Turn right arc( R );The sequence formation of the shortest curve,将起始点$A$连接到终点$B$,And at each endpoint curve tangent condition,And size of curvature is less than the given value at that point;

2.2.2 Polynomial helix structure

• Represents the type of curvature arc length can be used(Corresponding path of$s$轴)A polynomial function of curves;

• 使用三阶(Cubic)Or five order(Quintic)多项式螺旋线,其曲率$\kappa$And the trajectory arc length$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$;

• Based on the use of third order(五阶)Spiral connection path,Its parameters by gradient descent(Gradient Descent)Rapid and efficient method to search;

• In the third order polynomial, for example：Considering initial stance from the car$q_{init}=(x_1,y_1,{\theta }_1,{\kappa }_1)$To the target position$q_{goal}=(x_G,y_G,{\theta }_G,{\kappa }_G)$,With continuous curvature and third-order helix;在初始状态时,Considering the curvature of the first derivative and second derivative are need to meet the initial state restrictions,可得：
  $$\begin{array}{rl}& {\kappa }_0={\kappa }_1\\ & {\kappa }_1=d\kappa (0)/ds\\ & {\kappa }_2=d^2\kappa (0)/d{s}^2\end{array}\text{\hspace{1em}(2)}$$

2.2.3 样条曲线

• Curve interpolation is after all the given points method,Curve fitting is generated curve approximation to the given point;
• For car motion planning and control system,Based on spline interpolation trajectory can make automobile control points,Increase the fitting curve and the degree of match the target trajectory;
• Spline curve by numerical calculation,Using the parameter adjustment curve to approximate polygon and get,Is based on the polygon edges and constructed the smooth curve points;The shape of a spline curve is influenced by the number and location of polygon vertices,Which is used to determine the spline curve of polygon is referred to as the characteristics of the spline curve polygon or control polygon,Polygon vertices is called spline curve control points,Spline curve in data fitting need through the given point type called value point;

Some commonly used curve structure：

1. BezierCurve structure

   一段$n$次BezierCurve expression is：
   $$P(u)=\sum _{i=0}^n{B}_i^n(u)P_i,u\in [0,1]\text{\hspace{1em}(3)}$$

   $$B_i^n(u)=\frac{n!}{i!(n-i)!}{u}^i(1-u{)}^{n-i},i\in \{0,1,\dots ,n\}\text{\hspace{1em}(4)}$$

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

   BezierCurve control points roughly describes the curve to the,By controlling the control points of curves can control the shape of the curve;All the control points of a polygon is called curve characteristics of poly;

   In the planning when the car path,The commonly used geometric properties：

   1. 端点性质

      1. Curve begins with the first control point,Ending at the last control point;
         $$P(0)=P_0,P(1)=P_n\text{\hspace{1em}(5)}$$

      2. The starting and end pointBezierCurve tangent to feature polygons respectively the first and last a while;
         $$T(0)=n(P_1-P_0),T(1)=n(P_n-P_{n-1})\text{\hspace{1em}(6)}$$

      3. BezierStarting and end point of the second derivative curve respectively, and the former only3A control point and the last3个控制点有关;
         $$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)$Is the control point$P_i$的凸线性组合,Curve on control points constitute the convex hull of;When the control points constitute the characteristics of the polygon is a convex,整条BezierCurve is convex,The curve without inflection point(拐点);

   3. 几何不变性

      If meet continuous position constraint condition,You need two control points,The curve of the1个和最后1个控制点;If meet the direction continuity conditions,则需要4个控制点,前2The last two points of control and;If meet the curvature constraints,则需要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)}$$
      若要实现BezierThe smooth transition curve of two line,BezierCurve at least6个控制点,由于$n$次BezierCurve has a$n+1$个控制点,则BezierThe number of curve at least as5次;

2. BSummary of spline curve

   • BFrom the bezier spline curve,But unlike Bessel curve,BSpline curve can adjust local,Avoid the bezier curve is not easy to local adjustment shortcoming,This feature is called local supportive;
   • BThe advantages of spline curve with continuous curvature,Especially at junctions between adjacent curve curvature is continuous;
   • Is commonly used in trajectory planning is the third orderB样条曲线,In order to meet the vehicle kinematics constraints;

   A control vertex$P_0,P_1,P_2,\dots ,P_n$,则$k$阶$(k-1)$次BThe mathematic expression of the spline curve as：
   $$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)$Each part is calledB样条;

   BB-spline basis function is a$k$阶$k-1$次分段多项式,也被称为$k$阶$k-1$次多项式样条;参数$t$Is a set of known as node vector of decreasing sequence;

   德布尔-Cox, a recursive definition：
   $$N_{i,1}(t)=\{\begin{array}{ll}1,& t_i\le t\le t_{i+1},\\ 0,& t\le t_i,t\ge t_{i+1}\end{array},k=1\text{\hspace{1em}(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$;

   The recurrence formula shows that：To determine the first$i$个$k$阶B样条$N_{i,k}(t)$,需要用$t_i,t_{i+1},\dots ,t_{i+k}$共$k+1$个结点;According to interval$[t_i,t_{i+k}]$为$N_{i,k}(t)$的支撑区间;

   In the curve equation,$n+1$个控制顶点$P_i(i=0,1,2,\dots ,n)$要用到$n+1$个$k$阶B样条基$N_{i,k}(t)$;Support interval and set defines a set ofBThe spline base node vector$T=[t_0,t_1,\dots ,t_{n+k}]$;

   Assuming control polygon vertices as$n$,阶数为$k$(次数为$k-1$),The node vector is$T=[t_0,t_1,\dots ,t_{n+k}]$;BSpline curve according to the distribution of nodes in the vector,划分成4中类型：

   • 均匀B样条曲线;这种BSpline curve equals the difference between the nodes in the vector,The equally spaced evenly distributed;
   • 准均匀B样条曲线;准均匀BIn two endpoints spline curve duplication$k$;With such characteristics node vector quasi uniform can be determinedB样条基;
   • 分段贝塞尔曲线;当BTwo endpoints spline node vector in the duplication$k$,And the duplication of all internal nodes as$k-1$时,This node vector segmentation can be determinedBernstein基;When the bezier curve is used to represent sectionB样条曲线后,每一段的BSpline curve can be individually adjusted without affecting other parts of the curve,Adjustment of a control point is also affect the control points involved in controlling the shape of the curve segment;
   • 非均匀Bezier曲线;In guarantee node sequence of decreasing the duplication is not more than two endpoints and$k$、Nodes in the duplication is not more than$k-1$的情况下,Node vector can be arbitrary;Such a set of node vector can determine the non-uniformB样条基;

   BSpline curve has the following properties：

   1. 局部性.参数为$t(t_i<t<t_{i+1})$的$k$阶BSpline curve on a bit can affect$k$个控制点,And with the rest of the control points do not produce contact;When adjusting a control point on the curve$P_i$的时候,Only can affect the interval$(t_i,t_{i+1})$In the shape of the curve,Impact on other curve;
   2. 连续性.假如BIn a certain node spline curve$r$重结点,Then the curve in the node continuous order is greater than or equal to$k-r-1$;
   3. 凸包性.BSpline curve is always located in the control polygon convex hull inside;
   4. 分段参数多项式.$P(t)$In each interval is about parameters$t$的多项式,And it's less than or equal to the number$k-1$;
   5. 变差缩减性.Plane there are a certain number of control points is used to constitute aBSpline curve control polygon,Arbitrary plane in a straight line withBSpline curve intersection points less than or equal to the number of the lines and the number of control polygon intersection point;
   6. 几何不变性.BThe shape of a spline curve and the change of position is not due to the coordinate system and change;
   7. 仿射不变性.在仿射变换下,BThe expression form of spline curve will not change;
   8. Modelling the flexibility of the.利用BSpline curve can construct linear end、尖点、Special cases such as tangent;