- 参考资料
- 1. Dubins 曲线计算
- 1.1 坐标变换
- 1.2 LSL路径
- 1.3 RSR路径
- 1.4 RSL路径
- 1.5 LSR路径
- 1.6 RLR路径
- 1.7 LRL路径
- 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 L L表示向左转的圆弧运动， R R R表示向右转的圆弧运动， S S S表示沿直线运动。
1.1 坐标变换设起点为
s
(
x
i
,
y
i
,
α
i
)
sleft(x_{i}, y_{i}, alpha_{i}right)
s(xi,yi,αi) ，终点为
g
(
x
g
,
y
g
,
β
g
)
gleft(x_{g}, y_{g}, beta_{g}right)
g(xg,yg,βg) ， 先坐标变换将起点平移至原点，并旋转
θ
theta
θ 角，则终点也落在
x
mathrm{x}
x 轴上，起点和终点的坐标为
s
(
0
,
0
,
α
)
,
g
(
d
,
0
,
β
)
s(0,0, alpha), g(d, 0, beta)
s(0,0,α),g(d,0,β) ，其中:
θ
=
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} begin{gathered} theta=operatorname{atan} 2left(frac{y_{g}-y_{i}}{x_{g}-x_{i}}right) bmod{2pi} \ D=sqrt{left(x_{i}-x_{g}right)^{2}+left(y_{i}-y_{g}right)^{2}} \ d=D / R \ alpha= left(alpha_{i}-thetaright) bmod{2pi} \ beta= left(beta_{g}-thetaright) bmod{2pi} end{gathered}
θ=atan2(xg−xiyg−yi)mod{2π}D=(xi−xg)2+(yi−yg)2
d=D/Rα=(αi−θ)mod{2π}β=(βg−θ)mod{2π}(1)
几点说明：
-
其中 θ theta θ 为起点和终点航向角度差，上面的角度都在 [ 0 , 2 π ] [0,2 pi] [0,2π] 之间。
-
上面用 D D D 除上 R R R这样处理可以使每个最小转弯半径 R R R都为 1 ，由角度计算弧长时更方便，弧长即等于角度的弧度，因此在后面看到的 cos ( α ) cos (alpha) cos(α) ，其实是前面省略了 R R R 。
-
m o d ( ) mod() mod()是取模运算，例如： m o d ( 3 π , 2 π ) = 3 π m o d 2 π = π bmod(3pi,2pi)=3pi bmod 2 pi=pi mod(3π,2π)=3πmod2π=π。下述的 β ( m o d 2 π ) beta(bmod 2 pi) β(mod2π)也是这个意思，即 β beta β对 2 π 2pi 2π取模。python实现方式很简单，如下：
def mod2pi(theta): """对2pi取模运算 """ return theta - 2.0 * math.pi * math.floor(theta / 2.0 / math.pi)
坐标变换的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)
设车辆在起始圆上走过的长度为 t t t ，直线段长度为 p p p ，第二个圆上的圆弧长度为 q q q，整个路径长度 L = t + p + q L=t+p+q L=t+p+q。下面依次给出6种情况的轨迹公式。
1.2 LSL路径
L
S
L
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} begin{aligned} &t_{l s l}=-alpha+arctan frac{cos beta-cos alpha}{d+sin alpha-sin beta}{bmod 2 pi} \ &p_{l s l}=sqrt{2+d^{2}-2 cos (alpha-beta)+2 d(sin alpha-sin beta)} \ &q_{l s l}=beta-arctan frac{cos beta-cos alpha}{d+sin alpha-sin beta}{bmod 2 pi} end{aligned}
tlsl=−α+arctand+sinα−sinβcosβ−cosα{mod2π}plsl=2+d2−2cos(α−β)+2d(sinα−sinβ)
qlsl=β−arctand+sinα−sinβcosβ−cosα{mod2π}(2)
总长度等于:
L
l
s
l
=
t
l
s
l
+
p
l
s
l
+
q
l
s
l
=
−
α
+
β
+
p
l
s
l
(3)
tag{3} mathcal{L}_{l s l}=t_{l s l}+p_{l s l}+q_{l s l}=-alpha+beta+p_{l s l}
Llsl=tlsl+plsl+qlsl=−α+β+plsl(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.3 RSR路径
R S R 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} begin{aligned} t_{r s r} &=alpha-arctan frac{cos alpha-cos beta}{d-sin alpha+sin beta}{bmod 2 pi} \ p_{r s r} &=sqrt{2+d^{2}-2 cos (alpha-beta)+2 d(sin beta-sin alpha)} \ q_{r s r} &=-beta(bmod 2 pi)+arctan frac{cos alpha-cos beta}{d-sin alpha+sin beta}{bmod 2 pi} \ end{aligned} trsrprsrqrsr=α−arctand−sinα+sinβcosα−cosβ{mod2π}=2+d2−2cos(α−β)+2d(sinβ−sinα) =−β(mod2π)+arctand−sinα+sinβcosα−cosβ{mod2π}(4)
总长度等于:
L
r
s
r
=
t
r
s
r
+
p
r
s
r
+
q
r
s
r
=
α
−
β
+
p
r
s
r
(5)
tag{5} mathcal{L}_{r s r} =t_{r s r}+p_{r s r}+q_{r s r}=alpha-beta+p_{r s r}
Lrsr=trsr+prsr+qrsr=α−β+prsr(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.4 RSL路径
R S L 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} begin{aligned} t_{r s l} &=alpha-arctan left(frac{cos alpha+cos beta}{d-sin alpha-sin beta}right)+arctan left(frac{2}{p_{r s l}}right){bmod 2 pi} \ p_{r s l} &=sqrt{d^{2}-2+2 cos (alpha-beta)-2 d(sin alpha+sin beta)} \ q_{r s l} &=beta(bmod 2 pi)-arctan left(frac{cos alpha+cos beta}{d-sin alpha-sin beta}right)+arctan left(frac{2}{p_{r s l}}right){bmod 2 pi} \ end{aligned} trslprslqrsl=α−arctan(d−sinα−sinβcosα+cosβ)+arctan(prsl2){mod2π}=d2−2+2cos(α−β)−2d(sinα+sinβ) =β(mod2π)−arctan(d−sinα−sinβcosα+cosβ)+arctan(prsl2){mod2π}(6)
总长度等于:
L
r
s
l
=
t
r
s
l
+
p
r
s
l
+
q
r
s
l
=
−
α
+
β
+
2
t
r
s
l
+
p
r
s
l
(7)
tag{7} mathcal{L}_{r s l} =t_{r s l}+p_{r s l}+q_{r s l}=-alpha+beta+2 t_{r s l}+p_{r s l}
Lrsl=trsl+prsl+qrsl=−α+β+2trsl+prsl(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.5 LSR路径
L S R 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} begin{aligned} t_{l s r} &=left(-alpha+arctan left(frac{-cos alpha-cos beta}{d+sin alpha+sin beta}right)-arctan left(frac{-2}{p_{l s r}}right)right){bmod 2 pi} \ p_{l s r} &=sqrt{-2+d^{2}+2 cos (alpha-beta)+2 d(sin alpha+sin beta)} \ q_{l s r} &=-beta(bmod 2 pi)+arctan left(frac{-cos alpha-cos beta}{d+sin alpha+sin beta}right)-arctan left(frac{-2}{p_{l s r}}right){bmod 2 pi} \ end{aligned} tlsrplsrqlsr=(−α+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)
总长度等于:
L
l
s
r
=
t
l
s
r
+
p
l
s
r
+
q
l
s
r
=
α
−
β
+
2
t
l
s
r
+
p
l
s
r
(9)
tag{9} mathcal{L}_{l s r} =t_{l s r}+p_{l s r}+q_{l s r}=alpha-beta+2 t_{l s r}+p_{l s r}
Llsr=tlsr+plsr+qlsr=α−β+2tlsr+plsr(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.6 RLR路径
R
L
R
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} begin{aligned} &t_{r l r}=alpha-arctan left(frac{cos alpha-cos beta}{d-sin alpha+sin beta}right)+frac{p_{r l r}}{2}{bmod 2 pi} \ &p_{r l r}=arccos frac{1}{8}left(6-d^{2}+2 cos (alpha-beta)+2 d(sin alpha-sin beta)right) \ &q_{r l r}=alpha-beta-t_{r l r}+p_{r l r}{bmod 2 pi} \ end{aligned}
trlr=α−arctan(d−sinα+sinβcosα−cosβ)+2prlr{mod2π}prlr=arccos81(6−d2+2cos(α−β)+2d(sinα−sinβ))qrlr=α−β−trlr+prlr{mod2π}(10)
总长度等于:
L
r
l
r
=
t
r
l
r
+
p
r
l
r
+
q
r
l
r
=
α
−
β
+
2
p
r
l
r
(11)
tag{11} mathcal{L}_{r l r}=t_{r l r}+p_{r l r}+q_{r l r}=alpha-beta+2 p_{r l r}
Lrlr=trlr+prlr+qrlr=α−β+2prlr(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
上述实现方式得不到最优结果，很奇怪，下面这种写法倒是可以得到最优结果：
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.7 LRL路径
L
R
L
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} begin{aligned} &t_{l r l}=left(-alpha+arctan left(frac{-cos alpha+cos alpha}{d+sin alpha-sin beta}right)+frac{p_{l r l}}{2}right){bmod 2 pi} \ &p_{l r l}=arccos frac{1}{8}left(6-d^{2}+2 cos (alpha-beta)+2 d(sin alpha-sin beta)right){bmod 2 pi} \ &q_{l r l}=beta(bmod 2 pi)-alpha+2 p_{l r l}{bmod 2 pi} end{aligned}
tlrl=(−α+arctan(d+sinα−sinβ−cosα+cosα)+2plrl){mod2π}plrl=arccos81(6−d2+2cos(α−β)+2d(sinα−sinβ)){mod2π}qlrl=β(mod2π)−α+2plrl{mod2π}(12)
总长度等于:
L
l
r
l
=
t
l
r
l
+
p
l
r
l
+
q
l
r
l
=
−
α
+
β
+
2
p
l
r
l
.
(13)
tag{13} mathcal{L}_{l r l}=t_{l r l}+p_{l r l}+q_{l r l}=-alpha+beta+2 p_{l r l} .
Llrl=tlrl+plrl+qlrl=−α+β+2plrl.(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
同样上述实现方式得不到最优结果，下面这种写法可以得到最优结果：
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
以上完整代码文件见github仓库
