| by YoungTimes | No comments

自动驾驶运动规划-Dubins曲线

直观的了解下Dubins曲线,一个理想条件下,不挂倒挡一把实现移车入库的老司机。

来源:公众号:半杯茶的小酒杯

1、Simple Car模型

如下图所示,Simple Car模型是一个表达车辆运动的简易模型。Simple Car模型将车辆看做平面上的刚体运动,刚体的原点位于车辆后轮的中心;x轴沿着车辆主轴方向,与车辆运动方向相同;车辆在任意一个时刻的姿态可以表述为(x, y, $\theta$)。车辆的运动速度为s;方向盘的转角为$\phi$,它与前轮的转角相同;前轮和后轮中心的距离为L;如果方向角的转角固定,车辆会在原地转圈,转圈的半径为$\rho$。

Planning Algorithm-http://planning.cs.uiuc.edu/node658.html

在一个很短的时间$\Delta t$内,可以认为车辆沿着后轮指向的方向前进,当$\Delta t$趋于0时,有:

$tan \theta = dy / dx \tag{1}$

根据数学定义:

$dy / dx = \dot{y} / \dot{x} \tag{2}$

$tan \theta = sin{\theta} / cos{\theta} \tag{3}$

将2) 和3)代入1)中,得到:

$-\dot{x}sin{\theta} + \dot{y}cos{\theta} = 0 \tag{5}$

显然,$\dot{x}=cos{\theta}$和$\dot{y}=sin{\theta}$是5)式的一个解,两侧乘以速度s等式仍然满足。因此有:

$\dot{x} = s cos{\theta} \tag{6}$

$\dot{y} = s sin{\theta} \tag{7}$

用$\omega$表示车辆前进的距离,则有:

$d \omega = \rho d \theta \tag{8}$

根据三角几何,有:

$\rho = L / tan{\phi} \tag{9}$

将9)式代入8)式,得到:

$d {\theta} = \frac{tan{\phi}}{L} d {\omega} \tag{10}$

8)式两侧同除以dt, 并根据$\dot{\omega} = s$,得到:

$\dot{\theta} = \frac{s}{L} tan{\phi} \tag{11}$

至此得到了车辆的运动模型(Motion Model)。

然后引入Action变量,假设车辆运动速度s和方向盘转角$\phi$由Action变量$u_s$和$u_{\phi}$指定,得到:

$\dot{x} = u_s cos{\theta}$

$\dot{y} = u_s sin{\theta}$

$\dot{\theta} = \frac{u_s}{L} tan{\phi}$

2、Dubins曲线

假设车辆按照常量速度运行: $\mu_s=1$,最大转向角度为$\phi_{max}$,最小转弯半径$\rho_{min}$,起点为$q_I$, 终点为$q_G$,我们目标是求解从起$q_I$点到终点$q_G$的最短行驶距离。求解最短距离的过程就是优化如下Cost的过程。

$L(\widetilde{q}, \widetilde{u}) = \int^{t_F}_{0} \sqrt{\dot{x}^2(t) + \dot{y}^2(t)} dt \tag{12}$

$t_F$是到达$q_G$所需的时间,$q=(x, y, \theta)$,当$q_G$不可达时,$L(\widetilde{q}, \widetilde{u}) = \infty$。

由于速度$u_s$是恒定的,根据前面提到的车辆的运动模型:

$\dot{x} = u_s cos{\theta} \tag{13}$

$\dot{y} = u_s sin{\theta} \tag{14}$

$\dot{\theta} = \frac{u_s}{L} tan{\phi}$

其中:$u \in [-tan{\phi_{max}}, tan{\phi_{max}}]$。将13)和14)代入12),可看到,最短行驶距离只与时间$d_F$有关。

令S为车辆直行的Motion Primitive,L和R分别为车辆左转和右转的Motion Primitive,可以证明,任意起点到终点的Dubins最短路径可以由不超过三个Motion Primitives构成。由三个Motion Primitives构成的序列称为一个Word。由于两个连续的、相同的Motion Primitive可以合并为一个Motion Primitive,因此所有可能的Word有10中组合,Dubins证明最优的Word组合只能是如下6个组合之一:

${L_{\alpha}R_{\beta}L_{\gamma}, R_{\alpha}L_{\beta}R_{\gamma}, L_{\alpha}S_dL_{\gamma}, L_{\alpha}S_dR_{\gamma}, R_{\alpha}S_dL_{\gamma}, R_{\alpha}S_dR_{\gamma}}$

来源:A Comprehensive, Step-by-Step Tutorial on Computing Dubin’s Curves

其中,$\alpha, \gamma \in [0, 2 \pi)$,$\beta \in (\pi, 2\pi)$,这里注意,$\beta$大于$\pi$,如果小于$\pi$,一定有其它的序列优于该序列。

3、Dubins计算过程推导

3.1 基于向量的切点计算

假设两个最小转弯半径构成的Circle为 $C_1$和$C_2$,半径分别为$r_1$和$r_2$,圆心分别为$p_1=(x_1, y_1)$和$p_2=(x_2, y_2)$。

来源:A Comprehensive, Step-by-Step Tutorial on Computing Dubin’s Curves

1)首先构造C1和C2的圆心$p_1$到$p_2$的向量$V_1 = (x_2-x_1, y_2-y_1)$。

$D=\sqrt{(x_2 – x_1)^2 + (y_2 – y_1)^2}$

2)构造C1和C2的外切线切点构成的向量$V_2 = p_{ot2} – p_{ot1}$。

来源:A Comprehensive, Step-by-Step Tutorial on Computing Dubin’s Curves

3)构造垂直于$V_2$的单位法向量n,修改$V_1$的使其平行于$V_2$。

$V_1 -= (r_2 – r_1) \cdot n$

根据法向量的定义:$V_2 \cdot n = 0$,得到:

$n \cdot (V_1 – (r_2 – r_1) \cdot n) = 0$

根据单位向量的定义:$n \cdot n = 1$,代入上式得到:

$n \cdot V_1 = r_2 – r_1 \tag{16}$

4) 16)式两侧同除以D,得到:

$\frac{V_1}{D} \cdot n = \frac{r_2 – r_1}{D} \tag{17}$

注意,这里$\frac{V_1}{D}$实际是将向量$V_1$单位化。

根据向量点乘的数学定义:

$\vec{A} \cdot \vec{B} = |A||B|cos(\theta)$

因此:

$\frac{r_2 – r_1}{D}$等于向量$V_1$与法向量n的夹角的余弦。为了方便书写,定义一个常量$C= \frac{r_2 – r_1}{D}$。

等式17)中只有n是未知数。

5)将向量$V_1$旋转角度C就得到向量n。假设$n=(n_x, n_y)$,根据向量旋转的数学定义:

$n_x = V_{1x} * C – V_{1y} * \sqrt{1-C^2}$

$n_y = V_{1x} * \sqrt{1-C^2} + V_{1y} * C$

6)计算出n之后,就可以很方便的计算出外切线的切点$p_{ot1}$和$p_{ot2}$。从C1的圆心出发,沿着向量n的方向,距离为$r_1$的位置即为切点$p_{ot1}$,$p_{ot2}$亦然。

3.2 计算CSC类型的行驶曲线

RSR、LSL、RSL、LSR是CSC类型的行驶曲线,该类型曲线首先计算两个圆的切点,然后车辆沿着最小转弯半径构成的圆周行驶到第一个圆的切点,然后直行到第二个圆的切点,再沿着最小转弯半径构成的圆周行驶到目的地。下面我们以RSR轨迹为例看看如何计算行驶曲线。

假设起点$s = (x_1, y_1, \theta_1)$和终点$g=(x_2, y_2, \theta_2)$,最小转弯半径为$r_{min}$。然后我们计算起点和终点的圆心。

起点的圆心为:

$p_{c1} = (x_1 + r_{min} * cos(\theta_1 – \pi / 2), y_1 + r_{min} * sin(\theta_1 – \pi / 2))$

终点的圆心为:

$p_{c2} = (x_2 + r_{min} * cos(\theta_2 – \pi / 2), y_2 + r_{min} * sin(\theta_2 – \pi / 2))$

来源:A Comprehensive, Step-by-Step Tutorial on Computing Dubin’s Curves

得到起点和终点的圆心之后,可以利用3.1小节的切点计算方法,得到切点$p_{ot1}$和$p_{ot2}$。然后就可以得到车辆的行驶轨迹,该轨迹分为三段:start到$p_{ot1}$的圆周弧;$p_{ot1}$和$p_{ot2}$的直线距离;$p_{ot2}$到Goal的圆周弧。至此我们得到了RSR的行驶曲线。

来源:公众号:半杯茶的小酒杯

3.3 计算CCC类型的行驶曲线

如下图所示,$C_1$和$C_2$的圆心为$p_1$和$p_2$,$C_3$是与$C_1$和$C_2$相切的圆,圆心为$p_3$。

来源:A Comprehensive, Step-by-Step Tutorial on Computing Dubin’s Curves

根据数学关系,可得到:

$|\overline{p_1 p_2}| = D = \sqrt{(x_2 – x_1)^2 + (y_2 – y_1)^2}$

$|\overline{p_1 p_3}| = 2 r_{min}$

$|\overline{p_2 p_3}| = 2 r_{min}$

记$\theta$为$p_1 p_2$与$p_1 p_3$的夹角,已知三角形的三个边的长度,根据余弦定理,有:

$\theta = cos^{-1}(\frac{D}{2 r_{min}}) \tag{18}$

最终可得到:

$p_3 = (x_1 + 2 r_{min} cos(\theta), y_1 + 2 r_{min} sin(\theta))$

注意此处为LRL模式时,$\theta$需要加上$atan(V_1)$;为RLR模式时,$\theta$需要减去$atan(V_1)$。

然后计算$p_{t1}$和计算$p_{t2}$就变得很容易。定义向量$V_2=p_3 – p_1$,将向量缩放到$r_{min}$。

$V_2 = \frac{V_2}{||V_2||} * r_{min}$

最后可以得到交点$p_{t1} = p_1 + V_2$。按照同样的过程可以计算得到$p_{t2}$。然后就可以得到start到$p_{ot1}$的圆周弧;$p_{ot1}$和$p_{ot2}$的圆周弧;$p_{ot2}$到Goal的圆周弧的三段轨迹组成的行驶曲线。

来源:公众号:半杯茶的小酒杯

参考文章

1、A Comprehensive, Step-by-Step Tutorial on
Computing Dubin’s Curves
(https://gieseanw.files.wordpress.com/2012/10/dubins.pdf)

2、Planning Algorithm
(http://planning.cs.uiuc.edu/node1.html)

除非注明,否则均为[半杯茶的小酒杯]原创文章,转载必须以链接形式标明本文链接

本文链接:http://www.banbeichadexiaojiubei.com/index.php/2020/03/15/%e8%87%aa%e5%8a%a8%e9%a9%be%e9%a9%b6%e8%bf%90%e5%8a%a8%e8%a7%84%e5%88%92-dubins%e6%9b%b2%e7%ba%bf/