Dubins 曲线

本文最后更新于：2025年7月1日下午

Dubins 曲线是在无障碍的情况下，提供一条最优的前向路径，本文看记录 Dubins 曲线的推导与实现。

简介

Dubins 曲线是在无障碍的情况下，提供一条最优的前向路径，与A* ，RRT等那些搜索算法相比，还有一个优势就是规划的路径是满足车辆运动学要求的。

dubins曲线是在满足曲率约束和规定的始端和末端的切线（进入方向）的条件下，连接两个二维平面的最短路径，而且限制目标只能向前行进。

上图为Dubins曲线的示意图，给定起始点A、终止点B位置以及运动方向，生成一条从A点到B点的Dubins曲线。该曲线满足给定的运动曲率约束，即图中的转弯半径大于等于给定的半径R。

Dubins曲线由Dubins在1957年的文章《On curves of minimal length with a constraint on average curvature and with prescribed initial and terminal positions and tangents》中提出。Dubins曲线如下图所示。

分类

针对上述的问题，在满足限制下，会有多条曲线可行。而问题的最优解在于寻找到路径最短的曲线。因而我们需要计算每种情况下最优曲线，找到其中路径最短的，作为最优解。

针对起始点Pi，终止点Pf，以及引入的两个切线点1，切线点2中间点，可以将整个路径分成三段，分别是从起始点到切线点1段，切线点1到切线点2段，切线点2到终止点段。

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

根据每一段的运动方向，dubins集合D={LSL，RSR，RSL，LSR，RLR，LRL}六个。

其中L表示绕逆时针转动的圆弧段，S表示直线段，R表示绕顺时针转动的圆弧段。

RLR，LRL 中间转角必定大于 $\pi$ ，否则一定有其它的序列优于该序列。

车辆模型

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

在一个很短的时间 $\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)中，得到：
$$
-cos\theta sin\theta+sin\theta cos\theta=0 \tag{4}
$$

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

显然，$\dot{x}=\cos \theta, \dot{y} = \sin \theta$ 是 4) 式的一个解，两侧乘以速度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}
$$
10) 式两侧同除以 $dt$，并根据 $\dot{\omega}=s$，得到：

$$ \begin{array}{c} \frac {d\theta}{dt}=\frac{tan\phi}{L}\frac{d\omega}{dt}\\ \dot{\theta}= \frac {s}{L} \tan \phi \end{array} \tag{11} $$

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

基于向量的切点计算

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

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

$$ D=\sqrt{(x_2-x_1)^2+(y_2-y_1)^2} $$ 构造C1和C2的外切线切点构成的向量 $$ V_{2}=p_{ot2}-p_{ot1} $$

构造垂直于 $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$ 是单位向量，代入上式得到：
$$
n\cdot V_1=r_2-r_1
$$
两侧同除以D，得到：
$$
\frac{V_1}{D}\cdot n=\frac{r_2-r_1}{D}
$$

其中只有n是未知数。

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

根据向量点乘的数学定义：
$$
\vec{A}\cdot\vec{B}=|A||B|cos(\theta)
$$
因此 $\frac{r_2-r_1}{D}$ 等于向量 $V_1$ 与法向量 $n$ 的夹角的余弦。

至此计算出 $n$ 后，则可以方便地计算出两个圆的外切点。

计算CSC类型的行驶曲线

RSR、LSL、RSL、LSR是CSC类型的行驶曲线，该类型曲线首先计算两个圆的切点，然后车辆沿着最小转弯半径构成的圆周行驶到第一个圆的切点，然后直行到第二个圆的切点，再沿着最小转弯半径构成的圆周行驶到目的地。

假设起点 $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))
$$

得到起点和终点的圆心之后，计算切点 $p_{ot1}, p_{ot2}$ ，然后就可以得到车辆的行驶轨迹，该轨迹分为三段：

start 到 $p_{ot1}$ 的圆周弧
$p_{ot1}$ 和 $p_{ot2}$ 的直线距离
$p_{ot2}$ 到Goal的圆周弧。至此我们得到了RSR的行驶曲线。

计算CCC类型的行驶曲线

$C_1$ 和 $C_2$ 的圆心为 $p_1$ 和 $p_2$，$C_3$ 是与 $C_1$ 和 $C_2$ 相切的圆，圆心为 $p3$。

根据数学关系，可得到：

$$ \begin{aligned}&|\overline{p_{1}p_{2}}|=D=\sqrt{(x_{2}-x_{1})^{2}+(y_{2}-y_{1})^{2}}\\&|\overline{p_{1}p_{3}}|=2r_{min}\\&|\overline{p_{2}p_{3}}|=2r_{min}\end{aligned} $$

记 $\theta$ 为 $p_1p_2$与$p_1p_3$的夹角，已知三角形的三个边的长度，根据余弦定理，有：

$$ \begin {array}{c} p_1p_2^2 + p_1p_3^2 - p_2p_3^2 = 2 \cdot p_1p_2 \cdot p_1p_3 \cos \theta\\ D^2+(2r_{min})^2-(2r_{min})=2D(2r_{min})\cos \theta\\ D=4r_{min} \cos \theta \\ \theta = \arg \cos (\frac{D}{4r_{min}}) \end{array} $$

最终可得到：

$$ p_3=(x_1+2r_{min}cos(\theta),y_1+2r_{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_{t1}$和 $p_{t2}$的圆周弧； $p_{t2}$到Goal的圆周弧的三段轨迹组成的行驶曲线。

路径长度

对于大多数情况，即给定起始点以及终止点的位置以及运动方向信息，可以获得多条Dubins曲线，那肯定只能选择一条轨迹。选择的依据就是路径的长度。下面就接着介绍路径长度的计算。

输入数据：$𝑃_𝑖(𝑥_1,𝑦_1,𝛼) ， 𝑃_𝑓(𝑥_2,𝑦_2,𝛽)$
输出数据：路径长度 𝐿 ，以及路标点即切线点1、切线点2的坐标及运动方向

首先进行坐标系的变换，连接 $𝑃_𝑖$ 与 $𝑃_𝑓$ ，以该直线为 𝑥′ 轴，$𝑃_𝑖$ 点为原点，垂直于 𝑥′ 轴建立 𝑦′ 轴。新的坐标系就建立了，如上图示。

变换之后的新坐标为 $𝑃_𝑖(0,0,𝛼_2) ， 𝑃_𝑓(d,0,𝛽_2)$

其中:

$$ \begin{array}{c} 𝛼_2=𝛼−𝜃,𝛽_2=𝛽−𝜃\\ 𝜃=𝑎𝑟𝑐𝑡𝑎𝑛(\frac {𝑦_2−𝑦_1}{𝑥_2−𝑥_1})\\ 𝑑=\sqrt{(𝑥_2−𝑥_1)^2+(𝑦_2−𝑦_1)^2} \end{array} $$

LSL型的路径

假设从起始点 $𝑃_𝑖$ 到切线点1的圆弧长度为 $𝑙_1$ ，切线点1到切线点2的直线长度为 $𝑙_2$ ，切线点2到终止点 $𝑃_𝑓$ 的圆弧长度为 $𝑙_3$ ，以及起始点、终止点的坐标如上节坐标变换所述。

$$ \begin{array}{l}A+\pi=\alpha_2\\C+D=A\\B+D=\pi\\C+E=\beta_2\end{array} $$

利用 $𝑃_𝑖 $与 $𝑃_𝑓$ 的横坐标之差为 $d$，纵坐标相等，可以得到两个方程，即:

$$ R\cdot sinA+l_{2}\cdot cosC+R\cdot sin\beta_{2}=d\\-R\cdot cosA+l_{2}\cdot sinC-R\cdot cos\beta_{2}=0 $$ 对上面的方程依据边角关系进行变形，便可以得到 $$ \begin{aligned}&-R\cdot sin\alpha_2+l_2cos\left(\alpha_2+B\right)+R\cdot sin\beta_2=d\\&R\cdot cos\alpha_2+l_2sin\left(\alpha_2+B\right)-R\cdot cos\beta_2=0\\&\alpha_{2}+B+E=\beta_{2}\:mod\:2\pi\end{aligned} $$ 这个便是推导的原始公式，对上述公式求解便可以得到三段路径的长度分别为 $$ \begin{aligned}&B=-\alpha_{2}+arctan\left(\frac{cos\beta_{2}-cos\alpha_{2}}{d/R+sin\alpha_{2}-sin\beta_{2}}\right)\\&l_{2}=\sqrt{d^2+2R^2-2R^2cos\left(\alpha_2-\beta_2\right)+2dR\left(sin\alpha_2-sin\beta_2\right)}\end{aligned} $$ 则可以得到 $$ \begin{array}{l}l_1=R\cdot B\\l_3=R\cdot\beta_2-R\cdot(\alpha_2+B)\left\{mod\left(2\pi\right)\right\}\end{array} $$ 最终路径总长度 $$ L=l_1+l_2+l_3=R\cdot(-\alpha_2+\beta_2)+l_2=R\cdot(-\alpha+\beta)+l_2 $$