运动规划之参数曲线

1 运动学约束

最大曲率限制：因为车辆有最小转弯半径，意味着车辆能走的路径的曲率不能超过最大值
为了计算简便，通过对曲线上几个点的曲率进行限制达到约束整条曲线的目的。

2 参数化曲线

原本y=y(x)的曲线写成x(s),y(s)的形式，x，y的值都由参数s来确定，通过s来遍历整条曲线。
参数s可以是弧长，也可以是无量纲的[0,1] 0代表起点,1代表终点
x ( s ) = α 3 s 3 + α 2 s 2 + α 1 s + α 0 y ( s ) = β 3 s 3 + β 2 s 2 + β 1 s + β 0 \begin{array}{l} x(s)=\alpha_{3} s^{3}+\alpha_{2} s^{2}+\alpha_{1} s+\alpha_{0} \\ y(s)=\beta_{3} s^{3}+\beta_{2} s^{2}+\beta_{1} s+\beta_{0} \end{array} x(s)=α3​s3+α2​s2+α1​s+α0​y(s)=β3​s3+β2​s2+β1​s+β0​​

3 路径优化

通过代价函数f来优化路径，参数化曲线允许在参数空间优化
min ⁡ f ( r ( u ) )  s.t.  { c ( r ( u ) ) ≤ α , ∀ u ∈ [ 0 , 1 ] r ( 0 ) = β 0 r ( u f ) = β f \min f(\mathbf{r}(u)) \text { s.t. }\left\{\begin{array}{c} c(\mathbf{r}(u)) \leq \alpha, \quad \forall u \in[0,1] \\ \mathbf{r}(0)=\beta_{0} \\ \mathbf{r}\left(u_{f}\right)=\beta_{f} \end{array}\right. minf(r(u)) s.t. ⎩ ⎨ ⎧​c(r(u))≤α,∀u∈[0,1]r(0)=β0​r(uf​)=βf​​

4 路径参数化Example

五次多项式

三次螺旋线

5 五次样条

x,y都是u的5次多项式，对于边界条件存在解析解(closed form solution)
x ( u ) = α 5 u 5 + α 4 u 4 + α 3 u 3 + α 2 u 2 + α 1 u + α 0 y ( u ) = β 5 u 5 + β 4 u 4 + β 3 u 3 + β 2 u 2 + β 1 u + β 0 u ∈ [ 0 , 1 ] \begin{array}{c} x(u)=\alpha_{5} u^{5}+\alpha_{4} u^{4}+\alpha_{3} u^{3}+\alpha_{2} u^{2}+\alpha_{1} u+\alpha_{0} \\ y(u)=\beta_{5} u^{5}+\beta_{4} u^{4}+\beta_{3} u^{3}+\beta_{2} u^{2}+\beta_{1} u+\beta_{0} \\ u \in[0,1] \end{array} x(u)=α5​u5+α4​u4+α3​u3+α2​u2+α1​u+α0​y(u)=β5​u5+β4​u4+β3​u3+β2​u2+β1​u+β0​u∈[0,1]​
五次样条曲率计算

κ ( u ) = x ′ ( u ) y ′ ′ ( u ) − y ′ ( u ) x ′ ′ ( u ) ( x ′ ( u ) 2 + y ′ ( u ) 2 ) 3 2 \kappa(u)=\frac{x^{\prime}(u) y^{\prime \prime}(u)-y^{\prime}(u) x^{\prime \prime}(u)}{\left(x^{\prime}(u)^{2}+y^{\prime}(u)^{2}\right)^{\frac{3}{2}}} κ(u)=(x′(u)2+y′(u)2)23​x′(u)y′′(u)−y′(u)x′′(u)​

6 三次螺旋线

螺旋线定义曲线曲率为弧长的函数，定义使得曲率约束检查变得简单,曲率可以通过简单在曲线上采样一些点进行约束即可获得良好的曲率性能
k ( s ) k(s) k(s)为曲率方程
θ ( s ) θ(s) θ(s)可以理解为相对于起点增加的航向角delta heading
κ ( s ) = a 3 s 3 + a 2 s 2 + a 1 s + a 0 θ ( s ) = θ 0 + ∫ 0 s a 3 s ′ 3 + a 2 s ′ 2 + a 1 s ′ + a 0 d s ′ = θ 0 + a 3 s 4 4 + a 2 s 3 3 + a 1 s 2 2 + a 0 s x ( s ) = x 0 + ∫ 0 s cos ⁡ ( θ ( s ′ ) ) d s ′ y ( s ) = y 0 + ∫ 0 s sin ⁡ ( θ ( s ′ ) ) d s ′ \begin{array}{l} \kappa(s)=a_{3} s^{3}+a_{2} s^{2}+a_{1} s+a_{0}\\ \begin{array}{c} \theta(s)=\theta_{0}+\int_{0}^{s} a_{3} s^{\prime 3}+a_{2} s^{\prime 2}+a_{1} s^{\prime}+a_{0} d s^{\prime} \\ =\theta_{0}+a_{3} \frac{s^{4}}{4}+a_{2} \frac{s^{3}}{3}+a_{1} \frac{s^{2}}{2}+a_{0} s \\ x(s)=x_{0}+\int_{0}^{s} \cos \left(\theta\left(s^{\prime}\right)\right) d s^{\prime} \\ y(s)=y_{0}+\int_{0}^{s} \sin \left(\theta\left(s^{\prime}\right)\right) d s^{\prime} \end{array} \end{array} κ(s)=a3​s3+a2​s2+a1​s+a0​θ(s)=θ0​+∫0s​a3​s′3+a2​s′2+a1​s′+a0​ds′=θ0​+a3​4s4​+a2​3s3​+a1​2s2​+a0​sx(s)=x0​+∫0s​cos(θ(s′))ds′y(s)=y0​+∫0s​sin(θ(s′))ds′​​
螺旋线位置(x，y)没有一个严格的解析解，只能通过数值方法-辛普森法则来近似菲涅尔积分。