上一期文章介绍了二维平面下的导航向量场计算方法，本期文章将介绍三维空间中，导航向量场及扩展。

本文主要介绍论文[1]中提出的一种基于导航向量场的航迹跟踪算法。此外，虽然本文以三维航迹为例进行介绍，但该方法可非常方便的拓展到高维曲线中。

在文章的评论区，给大家准备了相关文献和仿真代码的领取～

导航向量场优势

路径跟踪算法有多种不同的实现方法，主要可分为基于控制论的和基于几何的两大类。其中基于控制论的方法往往实现困难，在非完整约束下导出的控制律复杂，往往难以应用于实际工程中；而基于几何的方法主要有视线法和本文针对的向量场法两大类。

基于视线的方法通过不断将无人机航向与视线方向对齐来实现航迹的跟踪，而向量场法则根据期望航迹计算直接导出每个点对应的期望飞行方向。视线法可适用于任何轨迹，而向量场法则适用于可解析表达的轨迹。通过参数曲线拟合、分段等手段可大大拓展向量场法可使用的轨迹类型。有文章[2]表明，在这几类算法中，基于向量场的航迹跟踪算法所需要的控制消耗最少且跟踪精度最高。相较于其他基于导航向量场的航迹跟踪算法，本文所介绍的方法还具有计算量小、实现简单等优点。

三维导航向量场

有些向量场构建方式是计算无人机到曲线上距离最近的点，构建收敛项使得无人机收敛至期望轨迹。上一期文章中，我们也介绍过通过设计隐函数来表示期望轨迹，将坐标点带入轨迹函数算作距离误差，巧妙的避免的计算欧式距离、寻找最近点的繁琐计算。基于误差计算收敛项和传播项，构建导航向量场。接下来介绍三维空间如何表示期望轨迹和误差，以及导航向量场的构建。

期望轨迹和误差的表示

曲线定义：一条$n$维的曲线可表示为$n-1$个隐函数的零水平集的交集。以三维曲线为例：
P = { x : ϕ i ( x ) = 0 , i = 1 , 2 } \mathcal P = \{\pmb x:\phi_i(\pmb x)=0, i=1,2\} P={x:ϕi​(x)=0,i=1,2}
式中$x=[x,y,z]\in {\mathbb{R}}^3$表示三维空间中的点，${\varphi }_i(x)=0$表示${\varphi }_i$的零水平集，也即是一个曲面。

为更好的说明水平集的概念，举例：一个原点为$(0,0,0)$、半径为$10$的曲面可表示为：$\varphi (x)=\sqrt{x^2+y^2+z^2}-10=0$；而隐函数$\varphi (x)$的$k$水平集即是$\varphi (x)=k$时的解，显然$\varphi (x)$的不同水平集是互不相交的同心圆面，可以证明任意一个隐函数的不同水平集都是互不相交的。

误差定义：得益于水平集的概念，我们可以直接利用当前点所处的水平集来表示误差，从而避免了计算欧式距离误差。点$x$处的误差为：
$$e(x)=\left[\begin{array}{c}{e}_1(x),e_2(x)\end{array}\right]$$
式中$e_i(x)={\varphi }_i(x)$。

导航向量场的构建

导航向量场由传播项和收敛项两项组成：
$$\chi (x)={\chi }_t(x)+{\chi }_n(x)$$
式中${\chi }_t$为传播项，指引着无人机沿着期望轨迹的方向前进；${\chi }_n$为收敛项，为无人机提供靠近期望航迹的方向。

传播项${\chi }_t$即为曲线的切线方向：
$${\chi }_t=a\nabla {\varphi }_1\times \nabla {\varphi }_2$$
式中$a=1$或$-1$，决定无人机沿着曲线正向或者逆向运动。$\nabla {\varphi }_1$，$\nabla {\varphi }_2$为曲面的偏导。

收敛项${\chi }_n$可构建为：
$${\chi }_n=-\sum _{i=1}^2{k}_i{\varphi }_i\nabla {\varphi }_i$$
式中$k_i>0$为收敛项增益系数。

可以证明，无人机沿着导航向量场提供的方向飞行，最终可收敛到期望轨迹上。证明过程见《Path following control in 3D using a vector field》的Proposition 1。

有意思的是，${\chi }_n$和${\chi }_t$是互相垂直的。${\chi }_n$的方向是当前点所在的两个水平集指向各自的0水平集的法向量的矢量和，因此${\chi }_n$总是使得误差减小；而${\chi }_t$是当前点所在两水平集的公切线，沿着${\chi }_t$飞行并不会导致水平集的变化。两者相互作用各司其职，使得无人机在沿着期望轨迹方向飞行的同时误差逐渐收敛。

注意：

1. 导航向量场中可能存在奇异点，也就是$\chi (x)=0$的点。幸运的是这些点为不稳定点，可使无人机保持上一时刻的飞行方向，从而飞过奇异点。
2. 导航向量场仅提供期望飞行方向信息，不指示速度，因此可对其进行归一化后使用。

数值仿真

为便于分析算法效果，令：
$$\dot{x}=v\frac{\chi (x)}{||\chi (x)||}$$
进行数值仿真实验。式中$v$为设定的速度，本次仿真设定为1。

以一个球面和一个圆柱面相交形成的期望轨迹为例（具体参数参见附录的代码）。分别设置收敛项的增益系数为0.1（蓝色轨迹）和0.5（红色轨迹）。可见收敛项增益越大，曲线越快的收敛至期望轨迹；而增益越小，收敛越慢但曲线相对而言更加平缓。读者可根据自身需求选择合适的增益。

总结

本文所介绍的基于导航向量场的航迹跟踪方法可引导无人机平稳的收敛至期望轨迹并稳定跟踪。该方法计算量小、实现简单，并且该方法仅提供导航信息（期望飞行方向），因此使用者可根据所应用的具体对象的运动/动力学模型设计航向控制器，从而将GVF应用于不同模型。

不过，该方法有一个的缺点：期望轨迹局限性大。该方法中期望轨迹由两个曲面相交得到，实际应用场景中期望轨迹往往并不是解析表达的（比如多个航迹点形成的期望轨迹），这种情况下如何根据期望轨迹反推曲面是一个难题。一个直观的解决思路时利用分段的方法，将期望轨迹拟合为分段的解析表达形式，但该方法又将引入如何在不同段间切换向量场的难题。

后续将介绍该论文作者的后续工作[3]，该文章将期望轨迹进行参数化，从而解决向量场的奇异点问题。这种实现方法也为我们提供了另一个解决期望轨迹无法解析表达问题的可能性：用户可首先利用参数曲线对轨迹进行拟合，得到与原始轨迹十分接近的有解析表达式的参数曲线，再利用该方法构建导航向量场。敬请期待！

后台回复“GVF3d”，即可领取相关文献和仿真代码。

参考文献及链接

[1]Yao W. Path following control in 3D using a vector field[M]//Guiding Vector Fields for Robot Motion Control. Cham: Springer Nature Switzerland, 2023: 39-62.

[2]Sujit P B, Saripalli S, Sousa J B. Unmanned aerial vehicle path following: A survey and analysis of algorithms for fixed-wing unmanned aerial vehicless[J]. IEEE Control Systems Magazine, 2014, 34(1): 42-59.

[3]Yao W, de Marina H G, Lin B, et al. Singularity-free guiding vector field for robot navigation[J]. IEEE Transactions on Robotics, 2021, 37(4): 1206-1221.

END

后续会陆续带来规划、控制相关的文章、仿真分享。

迅翼SwiftWing致力于固定翼技术共享，汇聚固定翼领域技术极客，推动固定翼技术持续创新！