word怎么根据点画曲线_自动驾驶运动规划-Dubins曲线

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，修改

的使其平行于

。

根据法向量的定义：

，得到：

根据单位向量的定义：

，代入上式得到：

1. 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的圆周弧的三段轨迹组成的行驶曲线。

参考文章

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)

注：本文首发于微信公众号，转载请注明出处，谢谢！

公众号:半杯茶的小酒杯

个人博客地址:

http://www.banbeichadexiaojiubei.com​www.banbeichadexiaojiubei.com

推荐阅读：

半杯茶的小酒杯：未知环境下的Lidar概率占位栅格图(Occupancy Grid Map) Python代码实现​zhuanlan.zhihu.com

半杯茶的小酒杯：自动驾驶Mapping-占位栅格图(Occupancy Grid Map)​zhuanlan.zhihu.com

半杯茶的小酒杯：自动驾驶中的车辆运动学模型​zhuanlan.zhihu.com

半杯茶的小酒杯：自动驾驶定位算法(十五)基于多传感器融合的状态估计(muti-Sensors Fusion)​zhuanlan.zhihu.com

自动驾驶路径规划器-Lattice Planner详解​www.banbeichadexiaojiubei.com 半杯茶的小酒杯：自动驾驶定位算法(十四)-递归贝叶斯滤波(Bayes Filter)​zhuanlan.zhihu.com

半杯茶的小酒杯：自动驾驶定位算法(十三)-粒子滤波(Particle Filter)​zhuanlan.zhihu.com

半杯茶的小酒杯：自动驾驶硬件系统(十二)-激光雷达(Lidar)测量模型​zhuanlan.zhihu.com

自动驾驶定位算法(九)-直方图滤波(Histogram Filter)定位​www.banbeichadexiaojiubei.com