海洋智能无人系统技术：路径跟随算法

虽然REMUS AUV和Sea Fox USV都是带有专用自动驾驶仪的商用航行器，但它们都提供通信接口，允许实验传感器和计算机有效载荷通过高级命令改写自动驾驶仪设置。例如，REMUSRECON接口与图11.2和图11.3所示的增强型自动驾驶仪非常相似（尽管直接对执行器输入仅适用于螺旋桨和交叉体推进器设置）。对于完全超越控制，有效载荷模块必须定期发送包含以下所有内容的有效命令：①所需深度或高度；②所需航行器或螺旋桨速度；③所需航向、转弯率或航路点位置。开发的轨迹生成器输出参考轨迹为空间曲线中每个坐标的参数化表达式加上在遍历该曲线时要使用的速度系数。使用这些表达式作为参考轨迹，早期开发的3D路径跟踪控制器可以计算将航行器驱动至（和沿）期望轨迹所需的转弯率和俯仰率。然而，RECON接口不接受俯仰速率命令（出于航行器安全原因）。因此，为了使用上述路径跟踪控制器跟踪REMUS AUV的三维轨迹，控制器输出必须分为在下节所述的水平（转弯率）和垂直（深度或高度）命令（显然，Sea Fox USV仅使用转弯率）。

1）水平面

考虑图11.17中描述的二维几何问题，它定义了惯性｛I｝帧，Serret-Frenet｛F｝误差帧和机身固定参考帧｛b｝。航行器的运动模型式（11.2）、式（11.3）简化为式（11.33）。

图11.17　水平路径跟随运动学

用动力学描述为式（11.34）。

通过构造，局部轨迹规划器生成空间轨迹各分量的解析表达式，pc（τ）作为虚弧长的函数。还可以分别计算空间轨道的一阶导数（τ）和二阶导数（τ）的解析表达式。利用图11.17中的关系，误差可以用Serret-Frenet帧｛F｝表示为式（11.35）。

其中，=［T，N，B］T是一个旋转矩阵，由Serret Frenet误差帧｛F｝的正切向量、法向向量和副法向向量构成。根据轨迹一阶导数的表达式计算出切线向量，表示为式（11.36）。

对于二维问题，可以直接从切线向量分量Nx=-Ty和Ny=Tx中计算法向量分量。此外，轨迹的有符号曲率可以使用轨迹的一阶导数和二阶导数的表达式计算为式（11.37）。

利用qF的时间导数，得到了误差运动学的状态空间表示（即船体固定帧｛b｝相对于Serret-Frenet帧｛F｝的位置和航向，该框架遵循所需的轨迹），如式（11.38）。

其中l是所需空间曲线的路径长度，描述虚拟目标沿此曲线前进的速度。

使航行器的位置误差（qF）和航向误差（ψe）为零。这将航行器驶入指令轨迹位置（pc）并将其速度矢量与轨迹的切线矢量（T）对齐。现在必须选择控制信号uψ以渐渐地将航行器位置和速度矢量驱动到指令轨道上。选择候选Lyapunov函数，表示为式（11.39）。

(www.xing528.com)

式中δψ是控制航行器接近路径方式的成形函数，如式（11.40）。

其中d＞0且为任意常数。

使用一些代数，选择下面的控制条件确保＜0

在式（11.41）中，K1，K2和d可作为增益来调整路径跟踪控制器的闭环性能。

2）垂直面

现在考虑使用高度命令在垂直平面上操纵REMUS AUV。在测量作业中，REMUS通常被设定为遵循由所需侧扫声呐范围决定的海底以上恒定高度的割草机模式。由于ADCP／DVL传感器持续测量海床以上的航行器高度，此操作模式确保在坡度高达45°的起伏海床上安全操作。需要进行避障演习进而安全通过更陡的斜坡、阶梯状的地形特征（如沙洲或珊瑚头）或在海底的物体。像前面说的那样，由于REMUS FLS以固定的方向安装，因此在执行操作时它可能会监测到新的障碍物以避免当前的障碍物威胁。定期或基于监测地重新规划可以处理这些情况。图11.1从概念上说明了这种情况。

当通过一个因探测时声呐阻塞而无法确定其范围的山脊或沙洲时，可能不希望沿着计划的垂直轨迹实现。在规划迭代之间，一种简单但更安全的方法是一旦航行器到达监测到的物体边界正上方的位置，就恢复到恒定的高度控制。可以使用上面描述的3D边界框交叉测试的2.5D版本来检查此条件。图11.18说明了REMUS FLS在最大声呐范围内探测海脊前缘的模拟。图像处理算法计算对象的距离（80 m）及其宽度（W）和其在海底以上的高度，但无法确定山脊的长度，因为它被自己的前缘遮挡。因此，障碍物监测算法生成一个测量宽W（m）×长1.0 m（假设）×高5.5 m的三维边界框。

IDVD方法规划器在NED坐标系下生成垂直轨迹，在浅水中，以高度控制模式操作航行器更安全。因此，将垂直坐标轨迹从深度平面转换为高度平面需要假设规划水平面上的恒定水深并利用式（11.42）关系式：

式中　D——在计划初始化时计算的水深；

hnom——标称高度设定点；

hcmd（τ）——通过RECON接口发送给自动驾驶仪的高度命令。

图11.18　使用AUV高度控制模式的垂直OA的模拟结果

由此产生的规划高度Δhplan（τ）与标称任务段高度的偏差如图11.18所示。如图所示，一旦ADCP／DVL传感器测量到航行器在山脊上方的真实高度，直接向航行器自动驾驶仪发送该高度指令将在高度剖面上引起不希望的跳跃。相反，一旦航行器到达山脊，将高度命令切换到标称任务段高度将产生所需的高度剖面。尽管图11.18描述了当航行器通过山脊，同时命令标称任务段高度时，高度突然下降，但实际上航行动态将确保AUV平稳过渡回其标称测量高度。