基于微分几何实现三维Dubins 路径

黄金芬

（西华大学计算机与软件工程学院，成都610039）

0 引言

无人机飞行路径规划是无人机自主控制中重点设计的一环，无人机的飞行路径需要考虑无人机的约束条件。Dubins 路径不仅符合无人机的飞行特性，而且是两个位姿点间的最短路径，因此，结合Dubins 路径来规划无人机飞行路径是有利的。当前，Dubins 路径主要用于无人机等高飞行路径规划，即类似于二维平面上的飞行，很少用于无人机变高飞行路径规划，即无人机在三维空间中的飞行。导致这种现象的一个重要原因就是三维Dubins 路径很难求解。当前常见的求解三维Dubins 路径的方法是先求出两个位姿点在二维平面上的Dubins 路径，然后结合螺旋线将二维Dubins 路径转为三维Dubins 路径，但这种方法存在使用范围受限的缺陷[1]。

Dubins 路径可被简单地定义为，在最大曲率限制下，两位姿点间的最短可行路径，可以是CSC 路径或CCC 路径，或是它们的子集（CS、CC、SC），其中C 表示圆弧段，S 表示与C 相切的直线段[2]。本文主要讨论CSC 型Dubins 路径。

Shanmugavel 等人做了大量关于Dubins 路径的研究[3-5]。文献[3]介绍了基于微分几何求解二维Dubins路径的方法；文献[4]介绍了基于微分几何求解带有Clothoid 曲线的Dubins 路径的方法；文献[5]提供了基于微分几何求解三维Dubins 路径的思路，但没有给出具体的求解方法。本文结合文献[5]提出的思路对基于微分几何求解三维Dubins 路径的方法进行详细解析。

1 基本知识

1.1 参数定义

计算Dubins 路径前需要指定一些参数信息，下面对涉及的参数在二维和三维中有区别的分二维和三维来描述，其余的统一描述。

二维路径中的参数定义如下：

（1）起始位姿点：ps(xs,ys,θs)。

（2）起始位姿点方向向量：ds，ds=[cosθssinθs]。

（3）终止位姿点：pf(xf,yf,θf)。

（4）终止位姿点方向向量：df，df=[cosθfsinθf]。

其中，下标s 表示起始位置的参数，f 表示终止位置的参数，θ表示位姿点的方向向量与x轴正方向的夹角。

三维路径中的参数定义如下：

（1）起始位姿点：ps(xs,ys,zs)。

（2）起始位姿点方向向量：ds(xds,yds,zds)。

（3）终止位姿点：pf(xf,yf,zf)。

（4）终止位姿点方向向量：df(xdf,ydf,zdf)。其余参数定义如下：

（1）起始转弯圆转向：turns，其值为1 表示向左转，为-1 表示向右转。

（2）起始转弯圆半径：rs，其中ks为起始转弯圆曲率。

（3）终止转弯圆转向：turnf，其值为1 表示向左转，为-1 表示向右转。

1.2 计算规则

下面对路径规划中所涉及的一些变量的计算方法进行讲解。

（1）二维平面

图1 二维平面上位姿点对应的两个转弯圆

根据图1 可知，位姿点的两个转弯圆圆心c1 和c2都在以位姿点p 为圆心的圆上，并且它们在圆p 上的夹角与θ相差是位姿点方向向量d 和向量d0的夹角。二维平面上圆上点坐标的求解方法如式（1）所示，其中ε为点在圆上的夹角，图1 中p0 是ε等于0时计算出来的点坐标，在二维平面上，向量d0 与x 轴正方向平行且同向，因此d 和d0 的夹角就等于参数中的θ。根据式（1）可知，当ε增大时，计算出来的点在原点的左边，当ε减小时，计算出来的点在原点的右边，因此，图1 中c1 在圆p 上的夹角为在圆p 上的夹角为反过来，p 在圆c1 上的夹角为在圆c2 上的夹角为

定义位姿点p 在转弯圆上夹角为ε，当点p 向左旋转v 度后对应的坐标如式（2）所示，当点p 向右旋转v 度后对应的坐标如式（3）所示。

位姿点(x,y,θ)沿当前方向直线前进长度v 后的坐标如式（4）所示。

（2）三维平面

三维平面由于其特殊性，需要定义描述圆转向的视角，若三维平面的法向量指向的是三维坐标轴中负轴的方向，则视线和法向量的指向保持一致，若法向量指向的是三维坐标轴中正轴的方向，则视线和法向量的指向相反。

式（5）是三维平面上圆上点坐标的求解公式。

其中，j 和k 的计算方法如式（6）所示。

其中，n 是圆所在三维平面的法向量，ε是点在圆上的夹角。根据式（6）可知，三维平面上圆上点坐标的计算公式与三维平面的法向量有关。设点的夹角ε递增，若法向量n 指向负轴的方向，则根据式（5）、式（6）得到的圆上点向右转，若法向量n 指向正轴的方向，则根据式（5）、式（6）得到的圆上点向左转。定义变量dire 来表示法向量n 下根据式（5）、式（6）得到的圆上点的转向，则dire 的值如式（7）所示，值为1 表示向左转，为-1 表示向右转。

式（7）表示的意思是当法向量中z 坐标值大于0时得到的圆上点向左转，z 坐标值小于0 时得到的圆上点向右转，若z 坐标值等于0，则判断y 坐标的值，y 坐标值大于0 时得到的圆上点向左转，y 坐标值小于0 时得到的圆上点向右转，若y 坐标的值也等于0，则再判断x 坐标的值，判断方法和前面一样。

图2 展示了三维平面上位姿点对应的两个转弯圆，可以看到，在三维平面上，位姿点也可以向左转或向右转，因此，三维平面上对应的CSC 型Dubins 路径也有四种子类型，即LSL、LSR、RSL、RSR。

图2 三维平面上位姿点对应的两个转弯圆

图2 中p0 是圆p 上的起点，即夹角ε为0 时根据式（5）、式（6）计算出来的圆上的点，α是位姿点方向向量d 和向量d0 的夹角。转弯圆圆心c1 和c2 在圆p上的夹角与α相差，具体的求解方法如式（8）所示。

式（8）表达的意思是如果转弯圆转向turn 和dire的值不相同，则圆心在圆p 上夹角为如果转弯圆转向turn 和dire 的值相同，则圆心在圆p 上夹角为与二维平面相比不同的是，这里还要考虑dire 的值。

三维空间中点(x,y,z)沿单位方向向量[m,n,q]前进长度v 后的坐标如式（9）所示。

2 计算二维Dubins路径

图3 给出了一条二维Dubins 路径。将Dubins 路径分成三段，并给每一段设置一个FS 坐标系，分别为起始坐标系es、连接坐标系ec和终止坐标系ef。

图3 二维平面上的Dubins路径

定义基坐标系e[t,n]，则es、ec和ef与e分别有如下关系：

其中，R(θs)、R(φ)、R(θf)表示对应角度下的坐标系的逆时针旋转矩阵。φ和θs存在如下关系。

当起始转弯圆向左转时：

当起始转弯圆向右转时：

其中，φs表示起始弧对应的角度。由图3 可知，二维平面上Dubins 路径中存在如下的矢量等式：

在基坐标系e 下，矢量c、as、af和ac可写为如下

形式：

其中c 是中心矢量c 的长度，u 是单位矢量，a 是切向量ac的长度。由图3 可知，中心矢量c 可由两个转弯圆圆心得到。

式（13）可表示为如下形式：

由于上式是一个旋转等式，因此左右两边的矢量幅值应相等：

所以，a 可以表达为：

其中，半径的正负号和转弯圆的转向有关，具体的对照关系可以查看表1。

表1 与路径类型对应的a 的计算公式

为了计算φ，式（15）可以写为如下形式：

其中

进而解出φ：

其中：

解出φ后就可以根据起始转弯圆的转向和式（11）、式（12）解出φs，得到φs的值后要进行如式（20）所示的取模运算，然后根据表2 即可求出终止弧对应的角度φf，得到φf的值后也要进行如式（21）所示的取模运算。

表2 路径类型与角度变换关系

到此，Dubins 路径的三个关键信息—起始弧对应的角度φs、切线段的长度a以及终止弧对应的角度φf都得到了，根据所需要的路径上点的个数、Dubins 路径的子类型以及式（2）、式（3）、式（4）便可以求得路径上点的坐标。

3 计算三维Dubins路径

求解三维Dubins 路径的主要思路是先将起始位姿点旋转到与终止位姿点共面的位置，然后求解三维平面上的Dubins 路径，最后将旋转路径和平面上的Dubins 路径组合起来构成指定两点之间的三维Dubins路径。

（1）旋转起始位姿点到与终止位姿点共面的位置。使用平行的向量一定共面这个理论来判断两位姿点是否共面以及寻找共面位置。图4 描述了空间中位姿点的转弯圆，可以看出，在三维空间中，从一个位姿点出发，可以向各个方向以圆的形式旋转，位姿点最终可以旋转到空间中任意方向。因此，一定存在一个转弯圆上的某个点的方向与终止位姿点平行。

根据法向量的定理可知，三维平面的法向量一定与平面上的向量垂直。若平面上的向量与终止位姿点的方向向量平行，则法向量也与终止位姿点的方向向量垂直。由图4 可知，圆上点的方向就是点的切向量，假设圆pc 是寻找的转弯圆，可以看到圆心pc 在圆ps上的切向量dc 就是圆pc 所在平面的法向量。dc、dv和ds 相互垂直，因此dc 等于dv 叉乘ds。结合式（5）、式（6）可以把圆心pc 用参数的形式表达出来，根据向量垂直的关系建立等式可以把参数解出来，反带入就得到pc 和dc 的值。

图4 位姿点的转弯圆

图5 是一个三维空间中的Dubins 路径，可以看到，转弯圆pc 上存在一点pt，pt 的切向量dt 与终止位姿点的方向df 平行。已知圆心pc 和圆所在平面的法向量dc，则pt 可以根据式（5）、式（6）用参数的形式表达出来。根据向量平行的关系建立等式可以求得参数的解，反带入就得到pt 和dt 的值，pt 和dt 就构成了和终止位姿点共面的新的起始位姿点。

图5 三维空间中的Dubins路径

（2）求解三维平面上Dubins 路径。三维平面上求解Dubins 路径的原理和步骤与二维平面上一样，主要是一些变量的求解方法存在区别，下面分析如何求解这些变量。

求解单位向量u。u 是二维向量，如果要在三维平面上求u，则要在三维平面上设计一个类似二维的坐标轴。图6 展示了一种构造三维平面上类二维坐标轴的方法。

图6 三维平面上的Dubins路径

以Dubins 路径中两个转弯圆圆心cirs 和cirf 连线的中点为圆心，作过cirs 和cirf 的圆，找到圆上的起点p0，当出现如下情况时，要对p0 进行一个变换。

式（22）表达的含义是如果p0 在圆心连线的右边，则将p0 转换到圆心连线的左边。连接cirs 和p0 的向量dp 就是类二维坐标系中的x 轴，将dp 向左旋转就得到了y 轴。因此，三维平面上u 的求解方法如式（23）所示。

求解对应二维平面上的θs和θf。对应二维平面上的θs和θf的计算方法如表3 所示。

表3 对应二维平面上的θs 和θf 的计算规则

其中γs和γf分别等于从dp 旋转到dtt、dff 的夹角。这里要注意向量之间旋转的方向，定义从dp 旋转到dtt、dff 的旋转方向为dirc，则dirc 的值如式（24）所示，值为1 表示向左转，为-1 表示向右转。

其中c2a 来自于式（22）。要注意，类似判断条件下式（24）得到的值和式（7）是不一样的。

以上就是三维平面上相比二维平面计算方法中主要存在差异的变量。到此，根据二维Dubins 路径的计算步骤就可以求得三维平面上Dubins 路径起始弧对应的角度φs、切线段的长度a以及终止弧对应的角度φf。起始旋转弧对应的角度可以通过求解两个向量之间夹角的方式得到。结合起始旋转弧段和三维平面上Dubins 路径就得到了最终的三维Dubins 路径。根据所需要的路径上点的个数、Dubins 路径的子类型以及式（5）、式（6）、式（8）、式（9）便可以求得路径上点的坐标。

4 仿真验证

使用MATLAB 软件对本文算法的有效性进行仿真验证。图7 是二维平面上Dubins 路径的仿真结果，图8 是三维空间中Dubins 路径的仿真结果，可以看到，不论是二维平面还是三维空间，本文算法都能成功计算出指定两点之间的Dubins 路径。

5 结语

本文对基于微分几何实现二维和三维Dubins 路径的原理和步骤进行了细致的阐述，仿真结果表明，本文提出的方法能够准确计算出二维平面内和三维空间中两位姿点间的CSC 型Dubins 路径。

图7 二维Dubins路径仿真结果

图8 三维Dubins路径仿真结果