1、Simple Car模型
如下图所示,Simple Car模型是一个表达车辆运动的简易模型。Simple Car模型将车辆看做平面上的刚体运动,刚体的原点位于车辆后轮的中心;x轴沿着车辆主轴方向,与车辆运动方向相同;车辆在任意一个时刻的姿态可以表述为(x, y, )。车辆的运动速度为s;方向盘的转角为,它与前轮的转角相同;前轮和后轮中心的距离为L;如果方向角的转角固定,车辆会在原地转圈,转圈的半径为。
在一个很短的时间内,可以认为车辆沿着后轮指向的方向前进,当趋于0时,有:
根据数学定义:
将2) 和3)代入1)中,得到:
显然,和是5)式的一个解,两侧乘以速度s等式仍然满足。因此有:
用表示车辆前进的距离,则有:
根据三角几何,有:
将9)式代入8)式,得到:
8)式两侧同除以dt, 并根据,得到:
至此得到了车辆的运动模型(Motion Model)。
然后引入Action变量,假设车辆运动速度s和方向盘转角由Action变量和指定,得到:
2、Dubins曲线
假设车辆按照常量速度运行: ,最大转向角度为,最小转弯半径,起点为, 终点为,我们目标是求解从起点到终点的最短行驶距离。求解最短距离的过程就是优化如下Cost的过程。
是到达所需的时间,,当不可达时,。
由于速度是恒定的,根据前面提到的车辆的运动模型:
其中:。将13)和14)代入12),可看到,最短行驶距离只与时间有关。
令S为车辆直行的Motion Primitive,L和R分别为车辆左转和右转的Motion Primitive,可以证明,任意起点到终点的Dubins最短路径可以由不超过三个Motion Primitives构成。由三个Motion Primitives构成的序列称为一个Word。由于两个连续的、相同的Motion Primitive可以合并为一个Motion Primitive,因此所有可能的Word有10中组合,Dubins证明最优的Word组合只能是如下6个组合之一:
其中,,,这里注意,大于,如果小于,一定有其它的序列优于该序列。
3、Dubins计算过程推导
3.1 基于向量的切点计算
假设两个最小转弯半径构成的Circle为 和,半径分别为和,圆心分别为和。
1)首先构造C1和C2的圆心到的向量。
2)构造C1和C2的外切线切点构成的向量。
3)构造垂直于的单位法向量n,修改的使其平行于。
根据法向量的定义:,得到:
根据单位向量的定义:,代入上式得到:
- 16)式两侧同除以D,得到:
注意,这里实际是将向量单位化。
根据向量点乘的数学定义:
因此:
等于向量与法向量n的夹角的余弦。为了方便书写,定义一个常量。
等式17)中只有n是未知数。
5)将向量旋转角度C就得到向量n。假设,根据向量旋转的数学定义:
6)计算出n之后,就可以很方便的计算出外切线的切点和。从C1的圆心出发,沿着向量n的方向,距离为的位置即为切点,亦然。
3.2 计算CSC类型的行驶曲线
RSR、LSL、RSL、LSR是CSC类型的行驶曲线,该类型曲线首先计算两个圆的切点,然后车辆沿着最小转弯半径构成的圆周行驶到第一个圆的切点,然后直行到第二个圆的切点,再沿着最小转弯半径构成的圆周行驶到目的地。下面我们以RSR轨迹为例看看如何计算行驶曲线。
假设起点和终点,最小转弯半径为。然后我们计算起点和终点的圆心。
起点的圆心为:
终点的圆心为:
得到起点和终点的圆心之后,可以利用3.1小节的切点计算方法,得到切点和。然后就可以得到车辆的行驶轨迹,该轨迹分为三段:start到的圆周弧;和的直线距离;到Goal的圆周弧。至此我们得到了RSR的行驶曲线。
3.3 计算CCC类型的行驶曲线
如下图所示,和的圆心为和,是与和相切的圆,圆心为。
根据数学关系,可得到:
记为与的夹角,已知三角形的三个边的长度,根据余弦定理,有:
最终可得到:
注意此处为LRL模式时,需要加上;为RLR模式时,需要减去。
然后计算和计算就变得很容易。定义向量,将向量缩放到。
最后可以得到交点。按照同样的过程可以计算得到。然后就可以得到start到的圆周弧;和的圆周弧;到Goal的圆周弧的三段轨迹组成的行驶曲线。