参考资料

• Classification of the Dubins set
• Dubins 曲线计算笔记
• Dubins path planning

---

前文

Dubins 曲线推导(基于向量的方法)

前文已经从向量的角度进行了Dubins曲线公式的推导，这篇博客主要基于这篇论文《Classification of the Dubins set》介绍基于几何的关系下的dubins曲线公式，论文每一种情况都有详细的推导，因此这里只给出每种情况的公式，推导过程大家参考论文即可。

---

1. Dubins 曲线计算

dubins路径集合为 { L S L ， R S R ， R S L ， L S R ， R L R ， L R L } \{LSL， RSR， RSL， LSR， RLR， LRL\} {LSL，RSR，RSL，LSR，RLR，LRL}。$L$表示向左转的圆弧运动，$R$表示向右转的圆弧运动，$S$表示沿直线运动。

1.1 坐标变换

设起点为 $s\left(x_i,y_i,{\alpha }_i\right)$ ，终点为 $g\left(x_g,y_g,{\beta }_g\right)$ ， 先坐标变换将起点平移至原点，并旋转 $\theta$ 角，则终点也落在 $\mathrm{x}$ 轴上，起点和终点的坐标为 $s(0,0,\alpha ),g(d,0,\beta )$ ，其中:
θ = atan ⁡ 2 ( y g − y i x g − x i )   m o d   { 2 π } D = ( x i − x g ) 2 + ( y i − y g ) 2 d = D / R α = ( α i − θ )   m o d   { 2 π } β = ( β g − θ )   m o d   { 2 π } (1) \tag{1}

θ=atan2(xg​−xi​yg​−yi​​)mod{2π}D=(xi​−xg​)2+(yi​−yg​)2 ​d=D/Rα=(αi​−θ)mod{2π}β=(βg​−θ)mod{2π}​(1)

几点说明：

• 其中 $\theta$ 为起点和终点航向角度差，上面的角度都在 $[0,2\pi ]$ 之间。

• 上面用 $D$ 除上$R$这样处理可以使每个最小转弯半径$R$都为 1 ，由角度计算弧长时更方便，弧长即等于角度的弧度，因此在后面看到的 $\cos (\alpha )$ ，其实是前面省略了 $R$ 。

• $mod()$是取模运算，例如：$\mathrm{m}\mathrm{o}\mathrm{d}(3\pi ,2\pi )=3\pi \mathrm{m}\mathrm{o}\mathrm{d}2\pi =\pi$。下述的$\beta (\mathrm{m}\mathrm{o}\mathrm{d}2\pi )$也是这个意思，即$\beta$对$2\pi$取模。python实现方式很简单，如下：

  def mod2pi(theta):
      """对2pi取模运算
      """
      return theta - 2.0 * math.pi * math.floor(theta / 2.0 / math.pi)
  
  1
  2
  3
  4
  5

坐标变换的python实现很简单，如下：

    # 坐标变换
    dx = g_x-s_x
    dy = g_y-s_y
    D = math.hypot(dx, dy)
    d = D * curvature

    theta = mod2pi(math.atan2(dy, dx))
    alpha = mod2pi(s_yaw - theta)
    beta = mod2pi(g_yaw - theta)
1
2
3
4
5
6
7
8
9

设车辆在起始圆上走过的长度为$t$ ，直线段长度为$p$ ，第二个圆上的圆弧长度为$q$，整个路径长度$L=t+p+q$。下面依次给出6种情况的轨迹公式。

1.2 LSL路径

$LSL$路径的轨迹长度公式如下:
t l s l = − α + arctan ⁡ cos ⁡ β − cos ⁡ α d + sin ⁡ α − sin ⁡ β {   m o d   2 π } p l s l = 2 + d 2 − 2 cos ⁡ ( α − β ) + 2 d ( sin ⁡ α − sin ⁡ β ) q l s l = β − arctan ⁡ cos ⁡ β − cos ⁡ α d + sin ⁡ α − sin ⁡ β {   m o d   2 π } (2) \tag{2}

​tlsl​=−α+arctand+sinα−sinβcosβ−cosα​{mod2π}plsl​=2+d2−2cos(α−β)+2d(sinα−sinβ) ​qlsl​=β−arctand+sinα−sinβcosβ−cosα​{mod2π}​(2)
总长度等于:
$${\mathcal{L}}_{lsl}=t_{lsl}+p_{lsl}+q_{lsl}=-\alpha +\beta +p_{lsl}\text{\hspace{1em}(3)}$$

python实现如下：

def left_straight_left(alpha, beta, d):
    """LSL路径
    """
    sa = math.sin(alpha)
    sb = math.sin(beta)
    ca = math.cos(alpha)
    cb = math.cos(beta)
    c_ab = math.cos(alpha - beta)

    tmp0 = d + sa - sb

    mode = ["L", "S", "L"]
    p_squared = 2 + (d * d) - (2 * c_ab) + (2 * d * (sa - sb))
    if p_squared < 0:
        return None, None, None, mode
    tmp1 = math.atan2((cb - ca), tmp0)
    t = mod2pi(-alpha + tmp1)
    p = math.sqrt(p_squared)
    q = mod2pi(beta - tmp1)

    return t, p, q, mode
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21

1.3 RSR路径

$RSR$路径的轨迹长度公式如下:

t r s r = α − arctan ⁡ cos ⁡ α − cos ⁡ β d − sin ⁡ α + sin ⁡ β {   m o d   2 π } p r s r = 2 + d 2 − 2 cos ⁡ ( α − β ) + 2 d ( sin ⁡ β − sin ⁡ α ) q r s r = − β (   m o d   2 π ) + arctan ⁡ cos ⁡ α − cos ⁡ β d − sin ⁡ α + sin ⁡ β {   m o d   2 π } (4) \tag{4}

trsr​prsr​qrsr​​=α−arctand−sinα+sinβcosα−cosβ​{mod2π}=2+d2−2cos(α−β)+2d(sinβ−sinα) ​=−β(mod2π)+arctand−sinα+sinβcosα−cosβ​{mod2π}​(4)

总长度等于:
$${\mathcal{L}}_{rsr}=t_{rsr}+p_{rsr}+q_{rsr}=\alpha -\beta +p_{rsr}\text{\hspace{1em}(5)}$$

python实现如下：

def right_straight_right(alpha, beta, d):
    """RSR路径
    """
    sa = math.sin(alpha)
    sb = math.sin(beta)
    ca = math.cos(alpha)
    cb = math.cos(beta)
    c_ab = math.cos(alpha - beta)

    tmp0 = d - sa + sb
    mode = ["R", "S", "R"]
    p_squared = 2 + (d * d) - (2 * c_ab) + (2 * d * (sb - sa))
    if p_squared < 0:
        return None, None, None, mode
    tmp1 = math.atan2((ca - cb), tmp0)
    t = mod2pi(alpha - tmp1)
    p = math.sqrt(p_squared)
    q = mod2pi(-mod2pi(beta) + tmp1)

    return t, p, q, mode
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20

1.4 RSL路径

$RSL$路径的轨迹长度公式如下:

t r s l = α − arctan ⁡ ( cos ⁡ α + cos ⁡ β d − sin ⁡ α − sin ⁡ β ) + arctan ⁡ ( 2 p r s l ) {   m o d   2 π } p r s l = d 2 − 2 + 2 cos ⁡ ( α − β ) − 2 d ( sin ⁡ α + sin ⁡ β ) q r s l = β (   m o d   2 π ) − arctan ⁡ ( cos ⁡ α + cos ⁡ β d − sin ⁡ α − sin ⁡ β ) + arctan ⁡ ( 2 p r s l ) {   m o d   2 π } (6) \tag{6}

trsl​prsl​qrsl​​=α−arctan(d−sinα−sinβcosα+cosβ​)+arctan(prsl​2​){mod2π}=d2−2+2cos(α−β)−2d(sinα+sinβ) ​=β(mod2π)−arctan(d−sinα−sinβcosα+cosβ​)+arctan(prsl​2​){mod2π}​(6)

总长度等于:
$${\mathcal{L}}_{rsl}=t_{rsl}+p_{rsl}+q_{rsl}=-\alpha +\beta +2t_{rsl}+p_{rsl}\text{\hspace{1em}(7)}$$

python实现如下：

def right_straight_left(alpha, beta, d):
    """RSL路径
    """
    sa = math.sin(alpha)
    sb = math.sin(beta)
    ca = math.cos(alpha)
    cb = math.cos(beta)
    c_ab = math.cos(alpha - beta)

    p_squared = (d * d) - 2 + (2 * c_ab) - (2 * d * (sa + sb))
    mode = ["R", "S", "L"]
    if p_squared < 0:
        return None, None, None, mode
    p = math.sqrt(p_squared)
    tmp2 = math.atan2((ca + cb), (d - sa - sb)) - math.atan2(2.0, p)
    t = mod2pi(alpha - tmp2)
    q = mod2pi(mod2pi(beta) - tmp2)

    return t, p, q, mode
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19

1.5 LSR路径

$LSR$路径的轨迹长度公式如下:

t l s r = ( − α + arctan ⁡ ( − cos ⁡ α − cos ⁡ β d + sin ⁡ α + sin ⁡ β ) − arctan ⁡ ( − 2 p l s r ) ) {   m o d   2 π } p l s r = − 2 + d 2 + 2 cos ⁡ ( α − β ) + 2 d ( sin ⁡ α + sin ⁡ β ) q l s r = − β (   m o d   2 π ) + arctan ⁡ ( − cos ⁡ α − cos ⁡ β d + sin ⁡ α + sin ⁡ β ) − arctan ⁡ ( − 2 p l s r ) {   m o d   2 π } (8) \tag{8}

tlsr​plsr​qlsr​​=(−α+arctan(d+sinα+sinβ−cosα−cosβ​)−arctan(plsr​−2​)){mod2π}=−2+d2+2cos(α−β)+2d(sinα+sinβ) ​=−β(mod2π)+arctan(d+sinα+sinβ−cosα−cosβ​)−arctan(plsr​−2​){mod2π}​(8)

总长度等于:
$${\mathcal{L}}_{lsr}=t_{lsr}+p_{lsr}+q_{lsr}=\alpha -\beta +2t_{lsr}+p_{lsr}\text{\hspace{1em}(9)}$$

python实现如下：

def left_straight_right(alpha, beta, d):
    """LSR路径
    """
    sa = math.sin(alpha)
    sb = math.sin(beta)
    ca = math.cos(alpha)
    cb = math.cos(beta)
    c_ab = math.cos(alpha - beta)

    p_squared = -2 + (d * d) + (2 * c_ab) + (2 * d * (sa + sb))
    mode = ["L", "S", "R"]
    if p_squared < 0:
        return None, None, None, mode
    p = math.sqrt(p_squared)
    tmp2 = math.atan2((-ca - cb), (d + sa + sb)) - math.atan2(-2.0, p)
    t = mod2pi(-alpha + tmp2)
    q = mod2pi(-mod2pi(beta) + tmp2)

    return t, p, q, mode
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19

1.6 RLR路径

$RLR$路径的轨迹长度公式如下:
t r l r = α − arctan ⁡ ( cos ⁡ α − cos ⁡ β d − sin ⁡ α + sin ⁡ β ) + p r l r 2 {   m o d   2 π } p r l r = arccos ⁡ 1 8 ( 6 − d 2 + 2 cos ⁡ ( α − β ) + 2 d ( sin ⁡ α − sin ⁡ β ) ) q r l r = α − β − t r l r + p r l r {   m o d   2 π } (10) \tag{10}

​trlr​=α−arctan(d−sinα+sinβcosα−cosβ​)+2prlr​​{mod2π}prlr​=arccos81​(6−d2+2cos(α−β)+2d(sinα−sinβ))qrlr​=α−β−trlr​+prlr​{mod2π}​(10)

总长度等于:
$${\mathcal{L}}_{rlr}=t_{rlr}+p_{rlr}+q_{rlr}=\alpha -\beta +2p_{rlr}\text{\hspace{1em}(11)}$$

python实现如下：

def right_left_right(alpha, beta, d):
    """RLR路径
    """
    sa = math.sin(alpha)
    sb = math.sin(beta)
    ca = math.cos(alpha)
    cb = math.cos(beta)
    c_ab = math.cos(alpha - beta)

    mode = ["R", "L", "R"]
    tmp_rlr = (6.0 - d * d + 2.0 * c_ab + 2.0 * d * (sa - sb)) / 8.0
    if abs(tmp_rlr) > 1.0:
        return None, None, None, mode

    p = mod2pi(math.acos(tmp_rlr))
    t = mod2pi(alpha - math.atan2(ca - cb, d - sa + sb) + mod2pi(p / 2.0))
    q = mod2pi(alpha - beta - t + p)
    return t, p, q, mode

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19

上述实现方式得不到最优结果，很奇怪，下面这种写法倒是可以得到最优结果：

def right_left_right(alpha, beta, d):
    """RLR路径，这部分的实现与公式不一致，改成与公式一致后反而得不到最优路径
    """
    sa = math.sin(alpha)
    sb = math.sin(beta)
    ca = math.cos(alpha)
    cb = math.cos(beta)
    c_ab = math.cos(alpha - beta)

    mode = ["R", "L", "R"]
    tmp_rlr = (6.0 - d * d + 2.0 * c_ab + 2.0 * d * (sa - sb)) / 8.0
    if abs(tmp_rlr) > 1.0:
        return None, None, None, mode

    p = mod2pi(2 * math.pi - math.acos(tmp_rlr))
    t = mod2pi(alpha - math.atan2(ca - cb, d - sa + sb) + mod2pi(p / 2.0))
    q = mod2pi(alpha - beta - t + mod2pi(p))
    return t, p, q, mode
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18

1.7 LRL路径

$LRL$路径的轨迹长度公式如下:
t l r l = ( − α + arctan ⁡ ( − cos ⁡ α + cos ⁡ α d + sin ⁡ α − sin ⁡ β ) + p l r l 2 ) {   m o d   2 π } p l r l = arccos ⁡ 1 8 ( 6 − d 2 + 2 cos ⁡ ( α − β ) + 2 d ( sin ⁡ α − sin ⁡ β ) ) {   m o d   2 π } q l r l = β (   m o d   2 π ) − α + 2 p l r l {   m o d   2 π } (12) \tag{12}

​tlrl​=(−α+arctan(d+sinα−sinβ−cosα+cosα​)+2plrl​​){mod2π}plrl​=arccos81​(6−d2+2cos(α−β)+2d(sinα−sinβ)){mod2π}qlrl​=β(mod2π)−α+2plrl​{mod2π}​(12)

总长度等于:
$${\mathcal{L}}_{lrl}=t_{lrl}+p_{lrl}+q_{lrl}=-\alpha +\beta +2p_{lrl}.\text{\hspace{1em}(13)}$$
python实现如下：

def left_right_left(alpha, beta, d):
    """LRL路径
    """
    sa = math.sin(alpha)
    sb = math.sin(beta)
    ca = math.cos(alpha)
    cb = math.cos(beta)
    c_ab = math.cos(alpha - beta)

    mode = ["L", "R", "L"]
    tmp_lrl = (6.0 - d * d + 2.0 * c_ab + 2.0 * d * (- sa + sb)) / 8.0
    if abs(tmp_lrl) > 1:
        return None, None, None, mode
    p = mod2pi(math.acos(tmp_lrl))
    t = mod2pi(-alpha - math.atan2(ca - cb, d + sa - sb) + p / 2.0)
    q = mod2pi(mod2pi(beta) - alpha +2*p)

    return t, p, q, mode
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18

同样上述实现方式得不到最优结果，下面这种写法可以得到最优结果：

def left_right_left(alpha, beta, d):
    """LRL路径，这部分的实现与公式不一致，改成与公式一致后反而得不到最优路径
    """
    sa = math.sin(alpha)
    sb = math.sin(beta)
    ca = math.cos(alpha)
    cb = math.cos(beta)
    c_ab = math.cos(alpha - beta)

    mode = ["L", "R", "L"]
    tmp_lrl = (6.0 - d * d + 2.0 * c_ab + 2.0 * d * (- sa + sb)) / 8.0
    if abs(tmp_lrl) > 1:
        return None, None, None, mode
    p = mod2pi(2 * math.pi - math.acos(tmp_lrl))
    t = mod2pi(-alpha - math.atan2(ca - cb, d + sa - sb) + p / 2.0)
    q = mod2pi(mod2pi(beta) - alpha - t + mod2pi(p))

    return t, p, q, mode
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18

以上完整代码文件见github仓库