Dubins曲线学习笔记

帅里帅气的LY

已于 2022-12-15 00:26:48 修改

阅读量2k

点赞数 7

分类专栏： dubins曲线文章标签：学习算法

于 2022-12-15 00:24:24 首次发布

原文链接：https://gieseanw.wordpress.com/2012/10/21/a-comprehensive-step-by-step-tutorial-to-computing-dubins-paths/

版权

dubins曲线专栏收录该内容

1 篇文章 1 订阅

订阅专栏

Dubins曲线学习笔记

1 Dubins曲线

1.1 Dubins曲线的构成

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

dubins路径示意

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

符号	含义	绕单位圆
L	左转	逆时针
R	右转	顺时针
S	直走	直走

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

符号	曲线
CCC	{LRL RLR}
CSC	{LSL RSR LSR RSL}

其中的符号含义为：

符号	含义
C	单位圆弧
S	一条直线段

如下图所示：
dubins曲线两种形式

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

1.2 CSC轨迹

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

1.3 CCC轨迹

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

2 Andy G’s Blog的计算思路

2.1 计算的关键

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

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

2.2 CSC轨迹推导计算

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

2.2.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，即可求出切点。
外切点计算（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的坐标。