Dubins曲线学习笔记

Dubins曲线学习笔记

1 Dubins曲线

1.1 Dubins曲线的构成

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

---

dubins曲线可以表示成3个运动基本动作的组合（即左转L、右转R、直行S）。

dubins曲线给出了充分的路经集合，该集合包含的曲线叫做最佳路径。由Dubins证得最短路径只从6条曲线中选取：{LRL LSL LSR RLR RSR RSL}。
进一步可简化为：

其中的符号含义为：

如下图所示：

综上所述，dubins曲线路径定义为：
在最大曲率限制下，平面内两个有方向的点间的最短可行路径是CSC或CCC，或是两者的子集。

1.2 CSC轨迹

对于CSC轨迹来说，他包括：{LSL RSR LSR RSL}，对于四组圆的组合：RR,LL,RL,LR，他们之间的切线如下图所示，蓝色箭头表示开始的运动方向，绿色箭头表示结束时的运动方向。

1.3 CCC轨迹

CCC轨迹是另一组完全不同的轨迹，包括LRL和RLR。它们由一个方向的转弯组成，然后朝相反的方向组成，然后再进行另一个转弯，如下图所示的RLR轨迹。

2 Andy G’s Blog的计算思路

2.1 计算的关键

对于给定的两个圆的圆心位置，如何计算切点位置。

• 对于CSC类型的组合，其关键是根据起终点出发的两个圆，计算出一条切线，由于起终点的方向性，这条切线唯一。
• 对于CCC类型的组合，其关键是计算过渡圆的位置。

2.2 CSC轨迹推导计算

• 首先通过当前点和目标点的位姿计算出两个最大曲率圆的圆心（由最小转弯半径决定）。
• 计算切点，对于RSR和LSL曲线要计算曲率圆的外切线切点，对于RSL和LSR曲线要计算曲率圆的内切线切点。
• 通过得到的切点计算弧长以及切线长度，弧长通过L=Rθ计算，但注意，此时轨迹中可能分优弧劣弧。

2.2.1 切点计算思路

1. 内切点计算（RSL和LSR）

   • 几何法

     • 首先，以两个曲率圆的圆心之间的连线（即向量v1）距离为直径，连线中点为圆心构造圆C3；
     • 以r1+r2为半径，p1为圆心构造圆c4。圆c3和圆c4交点为pt（上下各一个，分别用来求内切线，下图仅标注了上方的那个）；
     • 三角形p1p3pt的三边均已知，利用余弦定理求出角ptp1p3，利用actan(v1)可求向量v1与x轴的夹角，该夹角加上角ptp1p3，即可求出向量v2（ptp1）与x轴的夹角θ；
     • 由向量v2的长度为r1+r2，可以通过x和y轴投影，以及p1点的坐标，可以计算出pt点的坐标；
     • 由pt点的坐标，可以计算出p_it1的坐标，也就是第一条内切线在圆c1上的切点；
     • 由于该切线等价于向量ptp2，已知pt、p2、p_it1的坐标，可以求出该切线在圆c2上的切点，利用同样的方法可以求出另一条内切线的切点。
       

   • 向量法

     • 由向量v5长为r1+r2，所以由cos(p2p1p_it1)=(r2+r1)/D，再加上向量v1以及圆心p1，即可求出切点。
       

2. 外切点计算（RSR和LSL）

   • r1=r2时

     • 向量v1和v2间的夹角为90度，再加上actan(v1)，可以得到向量v2与x轴的夹角，从而求出第一条外切线位于c1上的切点，同理可求c2上的切点，由v1与v3等价可得。
       

   • 向量法

     • 假设r1>r2，假设与向量v2垂直的单位向量n，单位向量n与向量v3、v4、v5方向相同，可知向量n与向量v6垂直；
     • v5由（r1-r2）乘以单位向量n表示，单位向量n与向量v6点积为0，可得到cos(p2p1p_it1)=(r2-r1)/D；
     • 由p1点坐标可知切点p_it1的坐标，利用v2与v6等价，便可得到切点p_it2的坐标。
       

2.3 CCC轨迹推导计算

• 通过余弦定理求出∠p2p1p3，然后通过actan(v1)求出向量v1距离x轴的角度θ；
• 对于RLR轨迹，应该在θ的基础上加上∠p2p1p3，对于LRL轨迹，应该在θ的基础上减去∠p2p1p3，从而得到向量p3p1距离x轴的夹角，从而计算出p3坐标；
• 由p3坐标即可求出pt2与pt1的坐标。
  

注意：

• 使用CCC时，当前点与目标点之间的距离必须小于4r，否则其余四种方法更有效。

2.4 首尾圆弧计算

弧长通过L=Rθ计算，但需要判断是优弧还是劣弧，可通过actan(v2)-actan(v1)计算，结果带正负，再结合轨迹的方向判断是否要加上或减去2pi，来得到真实的角度θ，从而得到弧长，如图所示。

3 论文Dubins_Set_Robotics_2001的计算思路

该论文中介绍了另一种求解思路：

• 在计算前进行坐标变换
  

注意：这里将最小转弯半径化为1，这样子方便计算。

• 对每种轨迹，均单独进行分析，寻找之间的几何关系，列等式，解方程组，直接计算出每种轨迹对应的三段轨迹的长度。
  

详细情况可参考：论文配套笔记
得到了三段轨迹的长度后，还需要计算出三段轨迹之间的两个衔接点，依然采用几何分析的方法，结论如下：

即可画出dubins曲线。

4 参考文献

1. A Comprehensive, Step-by-Step Tutorial to Computing Dubin’s Paths – Andy G’s Blog
2. Dubins_Set_Robotics_2001