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

1、Simple Car模型

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

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

在一个很短的时间 $\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}}$

其中， $\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)$ 。

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}$ 。

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}$

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))$

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

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