直观的了解下Dubins曲线,一个理想条件下,不挂倒挡一把实现移车入库的老司机。
如下图所示,Simple Car模型是一个表达车辆运动的简易模型。Simple Car模型将车辆看做平面上的刚体运动,刚体的原点位于车辆后轮的中心;x轴沿着车辆主轴方向,与车辆运动方向相同;车辆在任意一个时刻的姿态可以表述为(x, y, \theta )。车辆的运动速度为s;方向盘的转角为\phi ,它与前轮的转角相同;前轮和后轮中心的距离为L;如果方向角的转角固定,车辆会在原地转圈,转圈的半径为\rho 。
Planning Algorithm-http://planning.cs.uiuc.edu/node658.html
在一个很短的时间\Delta t 内,可以认为车辆沿着后轮指向的方向前进,当\Delta t
趋于0时,有:
根据数学定义:
将2) 和3)代入1)中,得到:
显然,\dot{x}=cos{\theta} 和\dot{y}=sin{\theta} 是5)式的一个解,两侧乘以速度s等式仍然满足。因此有:
用\omega 表示车辆前进的距离,则有:
根据三角几何,有:
将9)式代入8)式,得到:
8)式两侧同除以dt, 并根据\dot{\omega} = s ,得到:
至此得到了车辆的运动模型(Motion Model)。
然后引入Action变量,假设车辆运动速度s和方向盘转角\phi 由Action变量u_s 和u_{\phi}
指定,得到:
假设车辆按照常量速度运行: \mu_s=1 ,最大转向角度为\phi_{max} ,最小转弯半径\rho_{min} ,起点为q_I , 终点为q_G ,我们目标是求解从起q_I 点到终点q_G 的最短行驶距离。求解最短距离的过程就是优化如下Cost的过程。
t_F是到达q_G 所需的时间,q=(x, y, \theta) ,当q_G 不可达时,L(\widetilde{q}, \widetilde{u}) = \infty 。由于速度u_s 是恒定的,根据前面提到的车辆的运动模型:
其中:u \in [-tan{\phi_{max}}, tan{\phi_{max}}] 。将13)和14)代入12),可看到,最短行驶距离只与时间d_F 有关。
令S为车辆直行的Motion Primitive,L和R分别为车辆左转和右转的Motion Primitive,可以证明,任意起点到终点的Dubins最短路径可以由不超过三个Motion Primitives构成。由三个Motion Primitives构成的序列称为一个Word。由于两个连续的、相同的Motion Primitive可以合并为一个Motion Primitive,因此所有可能的Word有10中组合,Dubins证明最优的Word组合只能是如下6个组合之一:
来源:Planning Algorithm-http://planning.cs.uiuc.edu/node658.html
其中,\alpha, \gamma \in [0, 2 \pi) ,\beta \in (\pi, 2\pi) ,这里注意,\beta 大于\pi ,如果小于\pi ,一定有其它的序列优于该序列。
假设两个最小转弯半径构成的Circle为 C_1 和C_2 ,半径分别为r_1 和r_2 ,圆心分别为p_1=(x_1, y_1) 和p_2=(x_2, y_2) 。
来源:A Comprehensive, Step-by-Step Tutorial on Computing Dubin’s Curves
1)首先构造C1和C2的圆心p_1 到p_2 的向量V_1 = (x_2-x_1, y_2-y_1) 。D=\sqrt{(x_2 - x_1)^2 + (y_2 - y_1)^2}
2)构造C1和C2的外切线切点构成的向量V_2 = p_{ot2} - p_{ot1} 。
来源:A Comprehensive, Step-by-Step Tutorial on Computing Dubin’s Curves
3)构造垂直于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 \cdot n = 1 ,代入上式得到:
注意,这里\frac{V_1}{D} 实际是将向量V_1 单位化。
根据向量点乘的数学定义:
因此:\frac{r_2 - r_1}{D} 等于向量V_1 与法向量n的夹角的余弦。为了方便书写,定义一个常量C= \frac{r_2 - r_1}{D} 。等式17)中只有n是未知数。
5)将向量V_1 旋转角度C就得到向量n。假设n=(n_x, n_y) ,根据向量旋转的数学定义:
6)计算出n之后,就可以很方便的计算出外切线的切点p_{ot1} 和p_{ot2} 。从C1的圆心出发,沿着向量n的方向,距离为r_1 的位置即为切点p_{ot1} ,p_{ot2} 亦然。
RSR、LSL、RSL、LSR是CSC类型的行驶曲线,该类型曲线首先计算两个圆的切点,然后车辆沿着最小转弯半径构成的圆周行驶到第一个圆的切点,然后直行到第二个圆的切点,再沿着最小转弯半径构成的圆周行驶到目的地。下面我们以RSR轨迹为例看看如何计算行驶曲线。
假设起点s = (x_1, y_1, \theta_1) 和终点g=(x_2, y_2, \theta_2) ,最小转弯半径为r_{min} 。
然后我们计算起点和终点的圆心。
起点的圆心为:
终点的圆心为:
来源:A Comprehensive, Step-by-Step Tutorial on Computing Dubin’s Curves
得到起点和终点的圆心之后,可以利用3.1小节的切点计算方法,得到切点p_{ot1} 和p_{ot2} 。然后就可以得到车辆的行驶轨迹,该轨迹分为三段:start到p_{ot1} 的圆周弧;p_{ot1} 和p_{ot2} 的直线距离;p_{ot2} 到Goal的圆周弧。至此我们得到了RSR的行驶曲线。
如下图所示,C_1 和C_2 的圆心为p_1 和p_2 ,C_3 是与C_1 和C_2 相切的圆,圆心为p_3 。
来源:A Comprehensive, Step-by-Step Tutorial on Computing Dubin’s Curves
根据数学关系,可得到:
记\theta 为p_1 p_2 与p_1 p_3 的夹角,已知三角形的三个边的长度,根据余弦定理,有:
最终可得到:
注意此处为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}
最后可以得到交点pt1=p1+V2。按照同样的过程可以计算得到。然后就可以得到start到的圆周弧;到的圆周弧;到Goal的圆周弧的三段轨迹组成的车辆行驶曲线。
1、A Comprehensive, Step-by-Step Tutorial on Computing Dubin’s Curves (https://gieseanw.files.wordpress.com/2012/10/dubins.pdf)
2、Planning Algorithm (http://planning.cs.uiuc.edu/node1.html)