Dubins曲线的计算与实现

Dubins曲线计算与实现

1.dubins曲线简介
dubins曲线是在满足曲率约束和规定的始端和末端的切线（进入方向）的条件下，连接两个二维平面的最短路径，而且限制目标只能向前行进。

下图给出一个dubins曲线的例子，其中a表示起始位置，该出的箭头表示当前的速度方向，b表示目标位置，箭头表示结束方向，物体会沿着圆弧于东后，再经过一段直线到，进入b的圆弧。这里两个圆的半径相同，均为目标的最小转弯半径。dubins曲线的规划的目的是找到一条从起始点到目标点的最短距离，并且需要满足转弯半径、前进方向、初始相对位置和速度方向的曲线。

2.dubins曲线的分类

针对上述的问题，在满足限制下，会有多条曲线可行。而问题的最优解在于寻找到路径最短的曲线。因而我们需要计算每种情况下最优曲线，找到其中路径最短的，作为最优解。
针对于起始点Pi和终止点Pf，采用three path segment的方法。将路径分为三段。分别是起始点到切线点1段，从切线点1到切线点2段，从切点2到终止点段。依据每段路径的方向，dubins集合 D = {LSL， RSR， RSL， LSR， RLR， LRL}。L表示从沿逆时针方向的圆弧运动，R表示沿顺时针方向圆弧运动，L表示沿直线运动。

3.针对不同类型的dubins方法，计算路径长度
输入数据：
pi(x, y, alpha), pf(x, y, beta), r;
输出：
杜宾斯路径长度与路径点
（1）.数据处理：
进行坐标系转换，将原始的直角坐标系转换为，以pi到pf为x轴，垂直于该方向向上为y轴。

经过坐标系旋转后，
起始点pi的坐标为（0,0，alpha - theta）,Pf(d, 0, beta-theta);
其中theta为从pi到pf向量的角度



用D/r单位化，可以将问题化简。即转弯半径r = 1，这样处理可以直接使用弧度来表示路径的长度，可大大简化后续的计算工作
（2）计算dubins路径长度过程
将整个路径长度L分为三段：
L= t + p + q分别对应于上述的分割段
1）LSL型的路径

解上述方程可以得到分阶段的路径长度

2).RSR型路径

解方程得到：

3）.RSL型路径

解方程

同上述的三个类型路径的计算方法，这里不再赘述
具体的计算方法可参考
Classification of the Dubins set. Shkel, A.M. 1 ; Lumelsky, V. 1
https://www.engineeringvillage.com/share/document.url?mid=cpx_2666204&database=cpx&view=abstract