前文
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} ight) s(xi,yi,αi) ，终点为 g ( x g , y g , β g ) gleft(x_{g}, y_{g}, eta_{g} ight) g(xg,yg,βg) ， 先坐标变换将起点平移至原点，并旋转 θ heta θ 角，则终点也落在 x mathrm{x} x 轴上，起点和终点的坐标为 s ( 0 , 0 , α ) , g ( d , 0 , β ) s(0,0, alpha), g(d, 0, eta) 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) ag{1} egin{gathered} heta=operatorname{atan} 2left(frac{y_{g}-y_{i}}{x_{g}-x_{i}} ight) mod{2pi} \ D=sqrt{left(x_{i}-x_{g} ight)^{2}+left(y_{i}-y_{g} ight)^{2}} \ d=D / R \ alpha= left(alpha_{i}- heta ight) mod{2pi} \ eta= left(eta_{g}- heta ight) mod{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)
几点说明：
其中 θ heta θ 为起点和终点航向角度差，上面的角度都在 [ 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 π = π mod(3pi,2pi)=3pi mod 2 pi=pi mod(3π,2π)=3πmod2π=π。下述的 β ( m o d 2 π ) eta(mod 2 pi) β(mod2π)也是这个意思，即 β eta β对 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) ag{2} egin{aligned} &t_{l s l}=-alpha+arctan frac{cos eta-cos alpha}{d+sin alpha-sin eta}{mod 2 pi} \ &p_{l s l}=sqrt{2+d^{2}-2 cos (alpha-eta)+2 d(sin alpha-sin eta)} \ &q_{l s l}=eta-arctan frac{cos eta-cos alpha}{d+sin alpha-sin eta}{mod 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) ag{3} mathcal{L}_{l s l}=t_{l s l}+p_{l s l}+q_{l s l}=-alpha+eta+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 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仓库
由于在自动驾驶中算法实现一般使用C++，所以我也使用C++实现了相关功能，代码结构与python代码实现类似，这边就不再做相关代码解释了。完整代码详见另一个github仓库。