参考资料

网上有许多dubins曲线的介绍,但我看了都迷迷糊糊的,所以我还是参考一些博客和论文自己再手推一遍,加深理解。

前言——车辆简化运动学模型

为了更好地阐述Dubins 曲线,这里我们简单地介绍一种车辆简化运动学模型。关于详细的车辆运动学模型介绍可以参考前文

设车辆的转弯半径为:
R = L tan ⁡ δ f R=\frac{L}{\tan {\delta_{f}}} R=tanδfL
其中 L L L 为前轴到后轴的长度, δ f \delta_{f} δf 为最大前轮转角。

而简化的运动学模型方程可以表示为:
x ˙ = v cos ⁡ θ y ˙ = v sin ⁡ θ θ ˙ = ω = v R \begin{aligned} &\dot{x}=v \cos {\theta} \\ &\dot{y}=v \sin {\theta} \\ &\dot{\theta}=\omega=\frac{v}{R} \end{aligned} x˙=vcosθy˙=vsinθθ˙=ω=Rv
其中 θ \theta θ 为偏航角, v v v为纵向速度, ω \omega ω为角速度。

将上述运动学方程离散化,取采样时间为 d t dt dt,则有
x new  = x p r e v + v ⋅ cos ⁡ ( θ ) ⋅ d t y new  = y p r e v + v ⋅ sin ⁡ ( θ ) ⋅ d t θ new  = θ p r e v + v R ⋅ d t \begin{aligned} &x_{\text {new }}=x_{\mathrm{prev}}+v \cdot \cos(\theta) \cdot dt \\ &y_{\text {new }}=y_{\mathrm{prev}}+v \cdot \sin(\theta) \cdot dt \\ &\theta_{\text {new }}=\theta_{\mathrm{prev}}+\frac{v}{R}\cdot dt \end{aligned} xnew =xprev+vcos(θ)dtynew =yprev+vsin(θ)dtθnew =θprev+Rvdt

显然,取 d t dt dt越小,得到的离散点坐标越多。令 Δ s = v d t \Delta s=v dt Δs=vdt ,则有:
x new  = x p r e v + Δ s ⋅ cos ⁡ ( θ ) y new  = y p r e v + Δ s ⋅ sin ⁡ ( θ ) θ new  = θ p r e v + Δ s R \begin{aligned} &x_{\text {new }}=x_{\mathrm{prev}}+\Delta s \cdot \cos (\theta) \\ &y_{\text {new }}=y_{\mathrm{prev}}+\Delta s \cdot \sin (\theta) \\ &\theta_{\text {new }}=\theta_{\mathrm{prev}}+\frac{\Delta s}{R} \end{aligned} xnew =xprev+Δscos(θ)ynew =yprev+Δssin(θ)θnew =θprev+RΔs

1. Dubins 曲线

1.1 基本概念

Dubins 曲线不考虑车辆后退(汽车只能朝前开),且不允许出现尖瓣。Dubins曲线是在满足曲率约束和规定的始端和末端的切线方向的条件下,连接两个二维平面(即X-Y平面)的最短路径。Dubins曲线如下图所示。

图片来源:https://blog.csdn.net/robinvista/article/details/95137143

Dubins曲线可以表示成3个运动基本动作的组合(即左转 L L L、右转 R R R、直行 S S S),一般将一种组合称之为一种word

符号含义绕单位圆
L左转逆时针
R右转顺时针
S直走直走

Dubins 给出了充分的路径集合,该集合里所包含的曲线叫做最佳路径。但是这个充分集合很小,对于每种特定终点情况下的集合中,最多只有6条可选曲线,分别表示如下:
{ L R L L S L L S R R L R R S R R S L } (1) \tag{1} \left\{\begin{array}{lllllll} L R L & L S L & L S R & R L R & R S R & R S L \end{array}\right\} {LRLLSLLSRRLRRSRRSL}(1)

为什么是6条可选的曲线?

由于连续两次做同一种基本运动和做一次基本运动是等效的,所以 word经过排列组合后的有效种类有12个(除了上述6个还有其余6个: S L S , S R S , S L R , S R L , R L S , L R S SLS, SRS, SLR, SRL, RLS, LRS SLS,SRS,SLR,SRL,RLS,LRS)。论文又证明最短路径只在12种中的6种word中即公式(1)去选取。

Dubins 证明了,一个最优路径一定是由分段圆弧 (单位圆) 和线段组成的平滑曲线,且最多 3 部分组成。可以进一步简化表示为如下形式:

C C C → { L R L R L R } C S C → { L S L R S R L S R R S L } (2) \tag{2} \begin{array}{llllllll} C C C & \rightarrow & \{ & L R L & R L R & \}\\ C S C & \rightarrow & \{ & L S L & R S R & L S R & R S L & \} \end{array} CCCCSC{{LRLLSLRLRRSR}LSRRSL}(2)

等式 (2) 中符号含义如下:

符号含义
C单位圆弧
S一条直线段

一个word表示相应的一类路径,对于 L L L R R R,其下标表示旋转的角度;对于 S S S,其下标表示行驶的直线距离。比如下图的 ( R α S d L γ ) (R_{\alpha}S_{d}L_{\gamma}) (RαSdLγ)

综上,Dubins曲线路径被定义为:

在最大曲率限制下,平面内两个有方向的点间的最短可行路径是 C L C CLC CLC路径或 C C C CCC CCC路径,或是他们的子集,其中 C C C表示圆弧段, L L L表示与 C C C相切的直线段。

可能的疑惑

  • 两点之间不是直线最短吗,为什么要加入圆弧

    因为这是在带有方向的初始位置 (起始位姿点)和终止位置 (终止位姿点)间寻找最短路径,所以这是两个位姿点间的最短路径,而不是单纯的两个坐标点间的最短路径。

  • 最短路径在最小半径的时候取到的前提是在无障碍的情况下

1.2 CSC轨迹

对于 C S C CSC CSC轨迹来说,它包括: R S R , L S R , R S L , 和 L S R RSR, LSR, RSL, 和 LSR RSR,LSR,RSL,LSR四种情况,下图是 R S R RSR RSR的轨迹情况:

对于四组圆的组合: R R , L L , R L , L R RR, LL, RL, LR RR,LL,RL,LR,他们之间的切线如下图所示,其中蓝色箭头表示开始的运动方向,绿色箭头表示结束时的运动方向。

1.3 CCC轨迹

C C C CCC CCC轨迹是另一组完全不同的轨迹,包括 L R L 和 R L R L R L 和R L R LRLRLR。它们由一个方向的转弯组成,然后朝相反的方向组成,然后再进行另一个转弯,如下图所示的RLR轨迹。

2. Dubins曲线推导计算

计算的关键

对于给定的两个圆的圆心位置,如何计算切点位置。

  • 对于 C S C CSC CSC类型的组合,其关键是根据起终点出发的两个圆,计算出一条切线,由于起终点的方向性,这条切线唯一。

  • 对于 C C C CCC CCC类型的组合,其关键是计算过渡切圆的位置。

2.1 CSC轨迹推导计算

2.1.1 已知圆心位置和半径,求切点

1. RSR和LSL

如图, L S L LSL LSL R S R RSR RSR的示意图如下:

RSR
LSL

由于 L S L LSL LSL R S R RSR RSR的推导是完全一致的,因此以上面任意一幅图为例进行推导都行。

为了推导方便,我们这里将起点圆和终点圆的最小转弯半径分别用了两个符号表示,并且画出的起点和终点圆的大小不同,但实际上最小转弯半径是一样的。

假设两个最小转弯半径构成的圆,半径大小分别为 r 1 r_1 r1 r 2 r_2 r2,圆心分别为 o 1 o_1 o1 o 2 o_2 o2,两个切点分别为 t 1 t_1 t1 t 2 t_2 t2。两个圆心间的连线构成向量 V 1 V_1 V1,两个切点之间的连线(即切线)构成向量 V 2 V_2 V2,切线的单位法向量 n n n,他们的方向分别如图所示。

显然,单位法向量 n n n与两圆心到切点的连线(即 o 1 t 1 o_1t_1 o1t1 o 2 t 2 o_2t_2 o2t2)是平行的。

我们需要求解的问题是:已知圆心位置和半径的前提下,求切点。

根据已知条件, V 2 ⊥ n V_2 \perp n V2n,显然有
V 2 ⋅ n = 0 V_2 \cdot n =0 V2n=0

在四边形 o 1 o 2 t 2 t 1 o_1o_2t_2t_1 o1o2t2t1中,根据向量的几何关系(注意向量定义的方向)有

V 2 = − o 1 t 1 ⃗ + V 1 + o 2 t 2 ⃗ (3) \tag{3} V_2=-\vec{o_1t_1}+V_1+\vec{o_2t_2} V2=o1t1 +V1+o2t2 (3)

由图几何关系可知
o 1 t 1 ⃗ = r 1 ⋅ n o 2 t 2 ⃗ = r 2 ⋅ n (4) \tag{4} \vec{o_1t_1}=r_1\cdot n\\ \vec{o_2t_2}=r_2\cdot n o1t1 =r1no2t2 =r2n(4)

所以
V 2 = V 1 + ( r 2 − r 1 ) ⋅ n (5) \tag{5} V_2=V_1+(r_2-r_1)\cdot n V2=V1+(r2r1)n(5)

等式(5)两边右乘单位法向量 n n n,得
V 2 ⋅ n = ( V 1 + ( r 2 − r 1 ) ⋅ n ) ⋅ n ⇓ 0 = ( V 1 + ( r 2 − r 1 ) ⋅ n ) ⋅ n ⇓ 0 = V 1 ⋅ n + r 2 − r 1 ⇓ V 1 ⋅ n = r 1 − r 2 (6) \tag{6} \begin{aligned} V_2\cdot n &=(V_1+(r_2-r_1)\cdot n)\cdot n\\ & \Downarrow\\ 0 &=(V_1+(r_2-r_1)\cdot n)\cdot n\\ & \Downarrow\\ 0 &=V_1\cdot n+r_2-r_1\\ & \Downarrow\\ V_1\cdot n &=r_1-r_2 \end{aligned} V2n00V1n=(V1+(r2r1)n)n=(V1+(r2r1)n)n=V1n+r2r1=r1r2(6)

进一步地,记 V 1 V_1 V1模长为 D D D,将单位化后的 V 1 V_1 V1记为 V 1 D = V 1 n \frac{V_1}{D}=V_{1n} DV1=V1n,将等式(6)两边同除以 D D D,由于 V 1 n V_{1n} V1n n n n都是单位向量,故得
V 1 ⋅ n D = r 1 − r 2 D ⇓ V 1 n ⋅ n = r 1 − r 2 D ⇓ V 1 n ⋅ n = c ⇓ V 1 n ⋅ n = ∣ ∣ V 1 n ∣ ∣ ⋅ ∣ ∣ n ∣ ∣ ⋅ cos ⁡ θ = cos ⁡ θ = c (7) \tag{7} \begin{aligned} \frac{V_1\cdot n}{D} &=\frac{r_1-r_2}{D}\\ & \Downarrow\\ V_{1n}\cdot n&=\frac{r_1-r_2}{D}\\ & \Downarrow\\ V_{1n}\cdot n&=c\\ & \Downarrow\\ V_{1n}\cdot n&=||V_{1n}||\cdot||n||\cdot \cos{\theta}=\cos{\theta}=c\\ \end{aligned} DV1nV1nnV1nnV1nn=Dr1r2=Dr1r2=c=∣∣V1n∣∣∣∣n∣∣cosθ=cosθ=c(7)

式中—— c = r 1 − r 2 D = cos ⁡ θ c=\frac{r_1-r_2}{D}=\cos{\theta} c=Dr1r2=cosθ θ \theta θ V 1 V_1 V1 n n n的夹角,显然 sin ⁡ θ = 1 − c 2 \sin{\theta}=\sqrt{1-c^2} sinθ=1c2

假设 V 1 n = ( v 1 n x , v 1 n y ) V_{1n}=(v_{1nx},v_{1ny}) V1n=(v1nx,v1ny) n = ( n x , n y ) n=(n_{x},n_{y}) n=(nx,ny),因为向量 V 1 n V_{1n} V1n逆时针旋转角度 θ \theta θ就得到了向量 n n n,所以根据向量旋转的计算方式可得
[ n x n y ] = [ cos ⁡ θ − sin ⁡ θ sin ⁡ θ cos ⁡ θ ] [ v 1 n x v 1 n y ] (7-1) \tag{7-1} \begin{bmatrix} n_x\\ n_y\\ \end{bmatrix}= \begin{bmatrix} \cos \theta&-\sin\theta\\ \sin\theta&\cos \theta\\ \end{bmatrix} \begin{bmatrix} v_{1nx}\\ v_{1ny}\\ \end{bmatrix} [nxny]=[cosθsinθsinθcosθ][v1nxv1ny](7-1)

根据 c c c θ \theta θ的关系,式(7-1)可化为
{ n x = v 1 n x ∗ c − v 1 n y ∗ 1 − c 2 n y = v 1 n y ∗ c + v 1 n x ∗ 1 − c 2 (8-1) \tag{8-1} \left\{\begin{array}{l} n_{x}=v_{1 {nx}} * c-v_{1 {ny}} * \sqrt{1-c^{2}} \\ n_{y}=v_{1 {ny}} * c+v_{1 {nx}} * \sqrt{1-c^{2}} \end{array} \right. {nx=v1nxcv1ny1c2 ny=v1nyc+v1nx1c2 (8-1)

计算出 n n n之后,就可以很方便的计算出两个切点:从圆心 o 1 o_1 o1出发,沿着向量 n n n的方向,走过半径 r 1 r_1 r1的距离即为切点 t 1 t_1 t1 t 2 t_2 t2同理。

即起点圆的切点坐标 ( x t 1 , y t 1 ) (x_{t1},y_{t1}) (xt1,yt1)为:
x t 1 = x o 1 + r 1 ∗ n x y t 1 = y o 1 + r 1 ∗ n y   (8-2) \tag{8-2} x_{t1}=x_{o1}+r_1*n_x\\ y_{t1}=y_{o1}+r_1*n_y\\\ xt1=xo1+r1nxyt1=yo1+r1ny (8-2)
终点圆的切点坐标 ( x t 2 , y t 2 ) (x_{t2},y_{t2}) (xt2,yt2)为:
x t 2 = x o 2 + r 2 ∗ n x y t 2 = y o 2 + r 2 ∗ n y   (8-3) \tag{8-3} x_{t2}=x_{o2}+r_2*n_x\\ y_{t2}=y_{o2}+r_2*n_y\\\ xt2=xo2+r2nxyt2=yo2+r2ny (8-3)

2. RSL和LSR

R S L RSL RSL L S R LSR LSR的推导过程与上一小节完全类似。

RSL
LSR

由于 R S L RSL RSL L S R LSR LSR的推导是完全一致的,因此以上面任意一幅图为例进行推导。在四边形 o 1 t 1 t 2 o 2 o_1t_1t_2o_2 o1t1t2o2中,根据向量的几何关系有

V 2 = − o 1 t 1 ⃗ + V 1 + o 2 t 2 ⃗ V_2=-\vec{o_1t_1}+V_1+\vec{o_2t_2} V2=o1t1 +V1+o2t2

由图中几何关系可知
o 1 t 1 ⃗ = − r 1 ⋅ n o 2 t 2 ⃗ = r 2 ⋅ n \vec{o_1t_1}=-r_1\cdot n\\ \vec{o_2t_2}=r_2\cdot n o1t1 =r1no2t2 =r2n

所以
V 2 = V 1 + ( r 2 + r 1 ) ⋅ n (9) \tag{9} V_2=V_1+(r_2+r_1)\cdot n V2=V1+(r2+r1)n(9)

等式(9)两边右乘 n n n,得
V 2 ⋅ n = ( V 1 + ( r 2 + r 1 ) ⋅ n ) ⋅ n ⇓ 0 = ( V 1 + ( r 2 + r 1 ) ⋅ n ) ⋅ n ⇓ 0 = V 1 ⋅ n + r 2 + r 1 ⇓ V 1 ⋅ n = − r 1 − r 2 (10) \tag{10} \begin{aligned} V_2\cdot n &=(V_1+(r_2+r_1)\cdot n)\cdot n\\ & \Downarrow\\ 0 &=(V_1+(r_2+r_1)\cdot n)\cdot n\\ & \Downarrow\\ 0 &=V_1\cdot n+r_2+r_1\\ & \Downarrow\\ V_1\cdot n &=-r_1-r_2 \end{aligned} V2n00V1n=(V1+(r2+r1)n)n=(V1+(r2+r1)n)n=V1n+r2+r1=r1r2(10)

进一步地,记 V 1 V_1 V1模长为 D D D,将单位化后的 V 1 V_1 V1记为 V 1 D = V 1 n \frac{V_1}{D}=V_{1n} DV1=V1n,将等式(10)两边同除以 D D D,由于 V 1 n V_{1n} V1n n n n都是单位向量,故得
V 1 ⋅ n D = − r 1 − r 2 D ⇓ V 1 n ⋅ n = − r 1 − r 2 D ⇓ V 1 n ⋅ n = c ⇓ V 1 n ⋅ n = ∣ ∣ V 1 n ∣ ∣ ⋅ ∣ ∣ n ∣ ∣ ⋅ cos ⁡ θ = cos ⁡ θ = c (11) \tag{11} \begin{aligned} \frac{V_1\cdot n}{D} &=\frac{-r_1-r_2}{D}\\ & \Downarrow\\ V_{1n}\cdot n&=\frac{-r_1-r_2}{D}\\ & \Downarrow\\ V_{1n}\cdot n&=c\\ & \Downarrow\\ V_{1n}\cdot n&=||V_{1n}||\cdot||n||\cdot \cos{\theta}=\cos{\theta}=c\\ \end{aligned} DV1nV1nnV1nnV1nn=Dr1r2=Dr1r2=c=∣∣V1n∣∣∣∣n∣∣cosθ=cosθ=c(11)

式中 c = − r 1 − r 2 D < 0 c=\frac{-r_1-r_2}{D}<0 c=Dr1r2<0 θ \theta θ V 1 V_1 V1 n n n的夹角,显然 sin ⁡ θ = 1 − c 2 \sin{\theta}=\sqrt{1-c^2} sinθ=1c2 ,且 θ \theta θ大于90°。

假设 V 1 n = ( v 1 n x , v 1 n y ) V_{1n}=(v_{1nx},v_{1ny}) V1n=(v1nx,v1ny) n = ( n x , n y ) n=(n_{x},n_{y}) n=(nx,ny),同理利用向量旋转的关系,化简后可求出 n n n
{ n x = v 1 n x ∗ c + v 1 n y ∗ 1 − c 2 n y = v 1 n y ∗ c − v 1 n x ∗ 1 − c 2 (12-1) \tag{12-1} \left\{\begin{array}{l} n_{x}=v_{1 {nx}} * c+v_{1 {ny}} * \sqrt{1-c^{2}} \\ n_{y}=v_{1 {ny}} * c-v_{1 {nx}} * \sqrt{1-c^{2}} \end{array}\right. {nx=v1nxc+v1ny1c2 ny=v1nycv1nx1c2 (12-1)

因为向量 V 1 n V_{1n} V1n旋转角度 θ \theta θ就得到了向量 n n n,所以根据向量旋转的数学计算也可得出。

计算出 n n n之后,就可以很方便的计算出两个切点:从圆心 o 1 o_1 o1出发,沿着向量 o 1 t 1 ⃗ \vec{o_1t_1} o1t1 的方向(即 n n n的反方向),走过半径 r 1 r_1 r1的距离即为切点 t 1 t_1 t1 t 2 t_2 t2同理。

即起点圆的切点坐标 ( x t 1 , y t 1 ) (x_{t1},y_{t1}) (xt1,yt1)为:
x t 1 = x o 1 − r 1 ∗ n x y t 1 = y o 1 − r 1 ∗ n y   (12-2) \tag{12-2} x_{t1}=x_{o1}-r_1*n_x\\ y_{t1}=y_{o1}-r_1*n_y\\\ xt1=xo1r1nxyt1=yo1r1ny (12-2)
终点圆的切点坐标 ( x t 2 , y t 2 ) (x_{t2},y_{t2}) (xt2,yt2)为:
x t 2 = x o 2 + r 2 ∗ n x y t 2 = y o 2 + r 2 ∗ n y   (12-3) \tag{12-3} x_{t2}=x_{o2}+r_2*n_x\\ y_{t2}=y_{o2}+r_2*n_y\\\ xt2=xo2+r2nxyt2=yo2+r2ny (12-3)

2.1.2 已知起点终点位姿,求圆心坐标

假设已知起点 s = ( x 1 , y 1 , θ 1 ) s=\left(x_{1}, y_{1}, \theta_{1}\right) s=(x1,y1,θ1) 和终点 g = ( x 2 , y 2 , θ 2 ) g=\left(x_{2}, y_{2}, \theta_{2}\right) g=(x2,y2,θ2) θ 1 , θ 2 \theta_1,\theta_2 θ1,θ2表示航向角。然后我们计算起点圆和终点圆的圆心

(1) 当车辆右转时

如上图所示,根据几何关系,圆心 o 1 o_1 o1的坐标 ( x o 1 , y o 1 ) (x_{o1},y_{o1}) (xo1,yo1)可以表示为

{ x o 1 = x 1 + r 1 ∗ cos ⁡ ( 180 ° − α 1 ) y o 1 = y 1 − r 1 ∗ s i n ( 180 ° − α 1 ) (13) \tag{13} \left\{\begin{array}{l} x_{o1}=x_1+r_1*\cos{(180°-\alpha_1)} \\ y_{o1}=y_1-r_1*sin{(180°-\alpha_1)} \end{array}\right. {xo1=x1+r1cos(180°α1)yo1=y1r1sin(180°α1)(13)

α 1 \alpha_1 α1 θ 1 \theta_1 θ1的关系为
90 ° − θ 1 + α 1 = 180 ° 90°-\theta_1+\alpha_1=180° 90°θ1+α1=180°

α 1 = 90 ° + θ 1 (14) \tag{14} \alpha_1=90°+\theta_1 α1=90°+θ1(14)

将公式(14)代入公式(13),使用三角函数公式化简后得到起点圆的圆心为:
o 1 = ( x 1 + r 1 ∗ sin ⁡ θ 1 , y 1 − r 1 ∗ cos ⁡ θ 1 ) (15) \tag{15} o_{1}=\left(x_{1}+r_{1} * \sin{\theta_1}, y_{1}-r_{1} * \cos{\theta_1}\right) o1=(x1+r1sinθ1,y1r1cosθ1)(15)
同理可得,终点圆的圆心为:
o 2 = ( x 2 + r 2 ∗ sin ⁡ θ 2 , y 2 − r 2 ∗ cos ⁡ θ 2 ) (16) \tag{16} o_{2}=\left(x_{2}+r_{2} * \sin{\theta_2}, y_{2}-r_{2} * \cos{\theta_2}\right) o2=(x2+r2sinθ2,y2r2cosθ2)(16)

(2) 当车辆左转时

类似的分析,如上图所示,根据几何关系,圆心 o 1 o_1 o1的坐标 ( x o 1 , y o 1 ) (x_{o1},y_{o1}) (xo1,yo1)可以表示为

{ x o 1 = x 1 + r 1 ∗ cos ⁡ ( 180 ° − α 1 ) y o 1 = y 1 − r 1 ∗ s i n ( 180 ° − α 1 ) (17) \tag{17} \left\{\begin{array}{l} x_{o1}=x_1+r_1*\cos{(180°-\alpha_1)} \\ y_{o1}=y_1-r_1*sin{(180°-\alpha_1)} \end{array}\right. {xo1=x1+r1cos(180°α1)yo1=y1r1sin(180°α1)(17)

α 1 \alpha_1 α1 θ 1 \theta_1 θ1的关系为
360 ° − 90 ° − θ 1 + α 1 = 180 ° 360°-90°-\theta_1+\alpha_1=180° 360°90°θ1+α1=180°

θ 1 = 90 ° + α 1 (18) \tag{18} \theta_1=90°+\alpha_1 θ1=90°+α1(18)

将公式(18)代入公式(17),化简后得到起点圆的圆心为:
o 1 = ( x 1 − r 1 ∗ sin ⁡ θ 1 , y 1 + r 1 ∗ cos ⁡ θ 1 ) (19) \tag{19} o_{1}=\left(x_{1}-r_{1} * \sin{\theta_1}, y_{1}+r_{1} * \cos{\theta_1}\right) o1=(x1r1sinθ1,y1+r1cosθ1)(19)
同理可得,终点圆的圆心为:
o 2 = ( x 2 − r 2 ∗ sin ⁡ θ 2 , y 2 + r 2 ∗ cos ⁡ θ 2 ) (20) \tag{20} o_{2}=\left(x_{2}-r_{2} * \sin{\theta_2}, y_{2}+r_{2} * \cos{\theta_2}\right) o2=(x2r2sinθ2,y2+r2cosθ2)(20)

2.1.3 求行驶轨迹

C S C CSC CSC类型曲线轨迹求解流程

首先得到起点圆和终点圆的圆心,然后计算出两个圆的切点,计算出切点后,车辆便可以沿着最小转弯半径构成的圆周行驶到第一个圆的切点,然后直行到第二个圆的切点,再沿着最小转弯半径构成的圆周行驶到目的地。

通过2.1.2小节计算得到起点和终点圆的圆心之后,利用2.1.1小节的切点计算方法,便可以得到切点。然后就可以得到车辆的行驶轨迹,该轨迹分为三段:start到切点 t 1 t_1 t1的圆周弧; t 1 t_1 t1 t 2 t_2 t2的直线距离; t 2 t_2 t2到End的圆周弧。至此我们便得到了 C S C CSC CSC的行驶曲线。

2.1.4 python代码实现

下面参考资料2的matlab程序,写了python代码,并没有考虑最优路径。


import numpy as np
import matplotlib.pyplot as plt

"""Dubins Curve CSC型

"""

#目标定义
#定义起终点[x y dir]
S = np.array([1, 1, 7 * np.pi / 4])
G = np.array([6, 8, 3 * np.pi / 4])
#定义转弯半径
ri = 1
rg = 1

#组合

i = -1 # 1:右转,-1:左转 ---起点圆
j = 1 # 1:右转,-1:左转 ---终点圆
k = i*j # 1:RSR/LSL, -1: RSL/LSR

"""计算首尾圆心坐标"""
xi = S[0] + ri * i * np.sin(S[2])
yi = S[1] - ri * i * np.cos(S[2])
xg = G[0] + rg * j * np.sin(G[2])
yg = G[1] - rg * j * np.cos(G[2])

"""计算法向量"""
#起终点圆圆心之间的向量V1=[v1x,v1y]
v1x = xg - xi
v1y = yg - yi
# V1模长
D = np.sqrt(v1x * v1x + v1y * v1y)
# 单位化
v1x = v1x / D
v1y = v1y / D
#计算法向量n
c = (k * ri - rg) / D
nx = v1x * c - j * v1y * np.sqrt(1 - c * c)
ny = v1y * c + j * v1x * np.sqrt(1 - c * c)



"""计算起终点圆的切点"""
xit = xi + k * ri * nx
yit = yi + k * ri * ny
xgt = xg + rg * nx
ygt = yg + rg * ny

# print(xgt-xg,ygt-yg)
# print(nx,ny)

"""绘图"""
# # 画起终点的初始方向
xiDir = np.array([S[0], S[0]+ri*np.cos(S[2])])
yiDir = np.array([S[1], S[1]+ri*np.sin(S[2])])
xgDir = np.array([G[0], G[0]+rg*np.cos(G[2])])
ygDir = np.array([G[1], G[1]+rg*np.sin(G[2])])


#切点连线即切线
tangent_x = np.array([xit, xgt])
tangent_y = np.array([yit, ygt])

# 画出首尾圆
t = np.arange(0, 2 * np.pi+0.01, 0.01)

# t = np.arange(S[2] - np.pi / 2, 3 * np.pi / 2 - np.arctan(ny / nx)+0.01, 0.01)
circle_xi = xi + ri * np.cos(t)
circle_yi = yi + ri * np.sin(t)

# t = np.arange(- np.pi / 2 - np.arctan(ny / nx),G[2] - np.pi / 2+0.01,0.01)
circle_xg = xg + rg * np.cos(t)
circle_yg = yg + rg * np.sin(t)


plt.plot(S[0], S[1], 'go', G[0], G[1], 'go', xiDir, yiDir, '-g', xgDir, ygDir, '-g', xi, yi,
         'k*', xg, yg, 'k*', circle_xi, circle_yi, '-r', circle_xg, circle_yg, '-r', [xi,xit], [yi,yit], '-b', [xg,xgt],[yg,ygt], '-b', tangent_x, tangent_y, '-r')

plt.axis('equal')
plt.show()

这是 L S R LSR LSR曲线的效果:

2.2 CCC轨迹推导计算

RLR
LRL

2.2.2 推导过程

C C C CCC CCC轨迹的推导比较纯几何,假设圆心 o 1 , o 2 o_1,o_2 o1,o2和三个圆的半径已知,则表明三角形 Δ o 1 o 3 o 2 \Delta o_1o_3o_2 Δo1o3o2的边长均已知,首先先求出 V 12 V_{12} V12与水平方向的夹角 β \beta β,然后通过余弦定理可以求出 θ \theta θ,继而根据三角关系,用 o 1 o_1 o1坐标计算 o 3 o_3 o3坐标。

我们假设 o 1 , o 2 , o 3 o_1,o_2,o_3 o1,o2,o3的半径分别为 r 1 , r 2 , r m i d r_{1},r_{2},r_{mid} r1,r2,rmid

根据数学关系,可得到:
{ ∣ o 1 o 2 ‾ ∣ = D = ( x o 2 − x o 1 ) 2 + ( y o 2 − y o 1 ) 2 ∣ o 1 o 3 ‾ ∣ = r 1 + r m i d ∣ o 2 o 3 ‾ ∣ = r 2 + r m i d (21) \tag{21} \left\{\begin{array}{l} \left|\overline{o_{1} o_{2}}\right|=D=\sqrt{\left(x_{o2}-x_{o1}\right)^{2}+\left(y_{o2}-y_{o1}\right)^{2}} \\ \left|\overline{o_{1} o_{3}}\right|=r_{1 }+r_{mid } \\ \left|\overline{o_{2} o_{3}}\right|=r_{2}+r_{mid} \end{array}\right. o1o2=D=(xo2xo1)2+(yo2yo1)2 o1o3=r1+rmido2o3=r2+rmid(21)

根据余弦定理有:
θ = cos ⁡ − 1 ( ∣ o 1 o 3 ‾ ∣ 2 + ∣ o 1 o 2 ‾ ∣ 2 − ∣ o 2 o 3 ‾ ∣ 2 2 ⋅ ∣ o 1 o 3 ‾ ∣ ⋅ ∣ o 1 o 2 ‾ ∣ ) (22) \tag{22} \theta=\cos ^{-1}\left( \frac{\left|\overline{o_{1} o_{3}}\right|^2+\left|\overline{o_{1} o_{2}}\right|^2-\left|\overline{o_{2} o_{3}}\right|^2}{2\cdot \left|\overline{o_{1} o_{3}}\right| \cdot \left|\overline{o_{1} o_{2}}\right|}\right) θ=cos1(2o1o3o1o2o1o32+o1o22o2o32)(22)

最终可得到:
o 3 = ( x 1 + ∣ o 1 o 3 ‾ ∣ ⋅ cos ⁡ ( β − θ ) , y 1 + ∣ o 1 o 3 ‾ ∣ ⋅ sin ⁡ ( β − θ ) ) (23) \tag{23} o_{3}=\left(x_{1}+ \left|\overline{o_{1} o_{3}}\right| \cdot \cos (\beta-\theta), y_{1}+\left|\overline{o_{1} o_{3}}\right| \cdot \sin (\beta-\theta)\right) o3=(x1+o1o3cos(βθ),y1+o1o3sin(βθ))(23)

解出 o 3 o_3 o3坐标后,只需从 o 1 o_1 o1出发,沿 V 13 V_{13} V13 V 13 = o 3 − o 1 V_{13}=o_3-o_1 V13=o3o1,其他同理)走过半径 r 1 r_1 r1的距离,即可得到第一个切点,同理沿 V 32 V_{32} V32方向,可得第二个切点位置。

定义向量 V 13 = o 3 − o 1 V_{13}=o_{3}-o_{1} V13=o3o1 ,则切点 t 1 t_1 t1的坐标为 。
t 1 = o 1 + V 13 ∥ V 13 ∥ ∗ r 1 (24) \tag{24} t_1=o_{1}+\frac{V_{13}}{\left\|V_{13}\right\|} * r_{1} t1=o1+V13V13r1(24)
按照同样的过程可以计算得到 t 2 t_2 t2 。然后就可以得到起点到切点 t 1 t_1 t1的圆周弧; t 1 t_1 t1 t 2 t_2 t2之间的圆周弧; t 2 t_2 t2到终点的圆周弧的三段轨迹组成的行驶曲线。


说明


一般地,对于最优情况来说,我们假设 o 1 , o 2 , o 3 o_1,o_2,o_3 o1,o2,o3具有最小转弯半径 r m i n r_{min} rmin

根据数学关系,可得到:
{ ∣ o 1 o 2 ‾ ∣ = D = ( x o 2 − x o 1 ) 2 + ( y o 2 − y o 1 ) 2 ∣ o 1 o 3 ‾ ∣ = 2 r min ⁡ ∣ o 2 o 3 ‾ ∣ = 2 r min ⁡ (25) \tag{25} \left\{\begin{array}{l} \left|\overline{o_{1} o_{2}}\right|=D=\sqrt{\left(x_{o2}-x_{o1}\right)^{2}+\left(y_{o2}-y_{o1}\right)^{2}} \\ \left|\overline{o_{1} o_{3}}\right|=2 r_{\min } \\ \left|\overline{o_{2} o_{3}}\right|=2 r_{\min } \end{array}\right. o1o2=D=(xo2xo1)2+(yo2yo1)2 o1o3=2rmino2o3=2rmin(25)
此时三角形 Δ o 1 o 3 o 2 \Delta o_1o_3o_2 Δo1o3o2为等腰三角形,根据余弦定理,有:
θ = cos ⁡ − 1 ( D 2 2 r min ⁡ ) = cos ⁡ − 1 ( D 4 r min ⁡ ) (26) \tag{26} \theta=\cos ^{-1}\left(\frac{\frac{D}{2}}{2 r_{\min }}\right)=\cos ^{-1}\left(\frac{D}{4 r_{\min }}\right) θ=cos1(2rmin2D)=cos1(4rminD)(26)
最终可得到:
o 3 = ( x 1 + 2 r min ⁡ cos ⁡ ( β − θ ) , y 1 + 2 r min ⁡ sin ⁡ ( β − θ ) ) (27) \tag{27} o_{3}=\left(x_{1}+2 r_{\min } \cos (\beta-\theta), y_{1}+2 r_{\min } \sin (\beta-\theta)\right) o3=(x1+2rmincos(βθ),y1+2rminsin(βθ))(27)

参考资料A Comprehensive, Step-by-Step Tutorial to Computing Dubin’s Paths 中的这一段没理解,先放在这,有知道的朋友们欢迎帮忙解释一下。


2.2.4 python代码实现

下面参考资料2的matlab程序,写了python代码,并没有考虑最优路径。


import numpy as np
import matplotlib.pyplot as plt


"""
Dubins Curve CCC型
"""


#目标定义
#定义起终点[x y psi]
S = np.array([1, 1, 7 * np.pi / 4])
G = np.array([4, 5, 3 * np.pi / 4])
#定义转弯半径
ri = 1
rg = 1
rmid = 1


i = -1 # 1:右转,-1:左转


"""计算首尾圆心坐标及其连线向量V12"""
xi = S[0] + ri * i * np.sin(S[2])
yi = S[1] - ri * i * np.cos(S[2])
xg = G[0] + rg * i * np.sin(G[2])
yg = G[1] - rg * i * np.cos(G[2])

V12 = np.array([xg - xi,yg - yi])
angleV12 = np.arctan(V12[1] / V12[0])

"""计算中间圆坐标及三圆心连线向量V13、V32"""
d12 = np.sqrt((xg - xi) ** 2 + (yg - yi) ** 2)
rmid = np.max(np.array([rmid, (d12 - ri - rg) * 0.5]))
d13 = ri + rmid
d32 = rmid + rg
angleP213 = np.arccos((d12 ** 2 + d13 ** 2 - d32 ** 2) / (2 * d12 * d13)) # 余弦定理
xmid = xi + d13 * np.cos(angleV12 - angleP213)
ymid = yi + d13 * np.sin(angleV12 - angleP213)
V13 = np.array([xmid - xi,ymid - yi])
V32 = np.array([xg - xmid,yg - ymid])

Vn13 = V13 / d13  # 归一化
Vn32 = V32 / d32
"""计算切点坐标"""
xt1 = xi + ri * Vn13[0]
yt1 = yi + ri * Vn13[1]
xt2 = xmid + rmid * Vn32[0]
yt2 = ymid + rmid * Vn32[1]

"""绘图"""
# # 画起终点的初始方向
xiDir = np.array([S[0], S[0]+ri*np.cos(S[2])])
yiDir = np.array([S[1], S[1]+ri*np.sin(S[2])])
xgDir = np.array([G[0], G[0]+rg*np.cos(G[2])])
ygDir = np.array([G[1], G[1]+rg*np.sin(G[2])])

# 画出首尾圆
t = np.arange(0, 2 * np.pi+0.01, 0.01)

circle_xi = xi + ri * np.cos(t)
circle_yi = yi + ri * np.sin(t)

circle_xg = xg + rg * np.cos(t)
circle_yg = yg + rg * np.sin(t)

circle_xmid = xmid + rmid * np.cos(t)
circle_ymid = ymid + rmid * np.sin(t)

#绘图
plt.plot(S[0],S[1],'go',G[0],G[1],'go',xiDir,yiDir,'-g',xgDir,ygDir,'-g',xi,yi,'k*',xg,yg,'k*',xmid,ymid,'k*',xt1,yt1,'bo',xt2,yt2,'bo',circle_xi,circle_yi,'-r',circle_xg,circle_yg,'-r',circle_xmid,circle_ymid,'-r')
plt.axis('equal')
plt.show()

这是 L R L LRL LRL曲线的实现效果:

3. 后记

  • Dubins曲线是开环的,考虑到实际车辆行驶中的不确定性, Dubins还存在动态性的问题。
  • Dubins曲线能够用于RRT等路径规划算法中,在原始RRT中,树枝的生长是直线生长(找到最近点,建立直线连接),对于汽车来说,不可能一直都能实现沿直线树枝行驶,因此在新节点和整棵随机树的关联上,可以选择使用Dubins曲线,使得各节点之间有一个连续、光滑、满足车辆运动学的连接。
  • Dubins曲线是不对称的,从A点到B点的最短距离,并不一定等于B点到A点的最短距离。相对的,Reeds Shepp曲线是对称的。
Logo

开放原子开发者工作坊旨在鼓励更多人参与开源活动,与志同道合的开发者们相互交流开发经验、分享开发心得、获取前沿技术趋势。工作坊有多种形式的开发者活动,如meetup、训练营等,主打技术交流,干货满满,真诚地邀请各位开发者共同参与!

更多推荐