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