基于SFLOS的水面无人艇路径跟踪控制技术研究

张爱国 孙宏放

摘要：相比直线路径跟踪控制，曲线路径跟踪控制往往由于操纵性约束和外界干扰，导致水面无人艇难以精确、稳定跟踪期望路径。本文通过引入Serret-Frenet坐标系，采用带有前视距离的视线导航法，然后设计出目标点的路径参数跟踪率，将对曲线路径跟踪转化为对一系列目标点的跟踪，最后再根据前视距离以及横向误差计算出期望艏向角，以此推导出SFLOS引导算法的具体表达式并进行仿真实验。实验结果表明，基于Serret-Frenet下的视线制导法与传统方法相比，尤其是在曲线路径跟踪上，在控制误差、稳定性和抗干扰性等方面性能均有提升，能快速收敛到目标轨迹。

关键词：水面无人艇;改进视线制导（SFLOS）;路径跟踪

引言

水面无人艇（unmanned surface vehicle，USV）集合了人工智能、自动化控制等多项技术，是一个复杂的系统。从上世纪九十年代至今，USV进入一个飞速发展的过程。在军事和民用方面应用广泛。其中，路径跟踪控制是USV技术发展的核心。路径跟踪控制是指USV从任意位置出发，能够按照规定的路径行驶并到达预定位置。路径跟踪控制是USV可以实现其他功能的前提，具有重要的实用价值。

在视线法中，在目标轨迹上定义一个虚拟的节点，并控制USV跟随该节点，从而使得 USV 位于目标轨迹上。视线法简单易用，但抗干扰性差。近年来，学者们提出了多种改进方法，以解决实际运用中遇到的难题。其中，柳晨光等[1]提出了考虑水流干扰条件下的自适应视距制导算法，通过在室外水池环境下的模型船路径跟踪控制试验平台，验证了其具有更高的路径跟踪控制精度和可靠性。陈霄等[2]出了一种基于改进视线导引算法，与传统的视线导引算法相比，改进的导引算法通过引入自适应观测器能够实现对漂角的實时估计和补偿，同时时变前视距离的设计使得无人艇的操纵更加灵活。Lei W等[3]提出了一种改进的无人机路径跟踪积分光瞄准制导律，其最大的特点是相比较传统制导律，该积分制导律可以适应不同航速。董早鹏等[4]提出了一种非对称模型下的改进视线法导向路径跟踪控制方法，不仅能够实现直线路径跟踪导向，还能实现一般曲线路径跟踪的视线导向，同时该方法在收敛速度和系统稳定性方面体现了一定的优越性，具有全局渐进稳定性。瞿洋等[5]提出了一种 ILOS 引导律的改进方式，消除了缓变环境载荷对船舶位置所产生的横向偏离。Wang H等[6]提出了基于相对速度模型设计新型的ILOS制导律，并证明了控制器的稳定性。

针对曲线路径跟踪，本文引入Serret-Frenet坐标体系，对原有的LOS进行改进，设计了将SFLOS导引算法与航速控制器、航向控制器相结合设计曲线路径跟踪控制算法。用SIMULINK对LOS导引算法以及SFLOS导引算法的路径跟踪控制系统进行路径点以及正弦函数跟踪的仿真试验，提高了无人艇在路径跟踪下的稳定性。

1 改进视线制导法（SFLOS）

1.1 Serret-Frenet坐标框架

视线导航法的原理是根据船舶自身位置与目标路径之间的距离，引入前视距离，使船舶的航向角对准LOS期望航向角，当艏向角误差趋向于零时表示船舶的实际航行路径趋向于目标路径，视线导航法多用于直线路径跟踪，本节的SFLOS导引算法解决曲线路径跟踪。首先引入Serret-Frenet坐标系，采用带有前视距离的视线导航法，然后设计出目标点的路径参数跟踪率，将对曲线路径跟踪转化为对一系列目标点的跟踪，最后再根据前视距离以及横向误差计算出期望艏向角[7]。

本节引入Serret-Frenet坐标框架，SF坐标框架来自微分几何学中，通过旋转坐标系以及应用微分同胚变换，将大地坐标系的位置误差转换为SF坐标系下的位置误差，再经过对船舶艏向角以及航迹角、船舶纵向速度以及横向速度夹角的几何分析，最终获得船舶的期望艏向角。

假设l为一条可微曲线，以弧长w为参数并且任意点处的曲率不为零，曲线l所在坐标系为大地坐标系，曲线l上任意点为l（ω）=（x（ω），y（ω）），在任意点处的单位切向量如下式：

将该向量顺时针旋转90°，为该点的单位法向量，如下式：

以该点为原点，其切向量以及法向量方向为横轴和纵轴，此为曲线l在l=（ω）下的Serret-Frenet坐标框架。

1.2 建立位置误差系统

假定无人艇所跟踪的路径为水平面一条以θ为参数的自由曲线Pp（θ），取该曲线上任意一点为[xp（θ），yp（θ）]T，设该点为目标点，引领无人艇沿着跟踪路径行驶，充当“虚拟向导”，该点的速度为。

先作如下假设：期望路径Pp（θ）存在一阶以及二阶导数并且有界，且满足，其中。

如图1.2，P为无人艇跟踪的期望轨迹，以Pp（θ）为原点构造Serret-Frenet坐标系，以无人艇重心为原点构造附体坐标系，以及船舶初始位置为原点构造大地坐标系，大地坐标系与Serret-Frenet坐标系之间的相对旋转角度为Ψp（θ），表达式如下： （1-3）

定义在Serret-Frenet坐标系下无人艇位姿误差为，大地坐标系下位姿误差为ηen，经过微分同胚变换可知： （1-4）

其中为旋转矩阵，如下式（1-5）：

其中Ψw=Ψ+β为无人艇的航迹角。

根据微分同胚变换的原理，如果无人艇的位姿误差在Serret-Frenet坐标系下收敛，则无人艇的位姿误差在大地坐标系下仍收敛，此时只需考虑令。

对式（1-4）进行求导，得到跟踪误差的微分形式：

现得到在Serret-Frenet坐标系下位姿跟踪误差的导数形式，如式（1-12）：

1.3 SFLOS导引算法设计

定义在Serret-Frenet坐标系下无人艇的位置误差为，控制目标为：

根据上文的位置跟踪误差，设计路径跟踪控制器，构造李雅普诺夫函数：

为使纵向位置误差收敛，设计路径参数跟踪率为：

由SFLOS导引原理图可知表示在Serret-Frenet坐标系下无人艇的期望速度与Serret-Frenet坐标系横轴夹角。

根据图1.2，由几何关系可知，无人艇期望艏向角为：

其中Δ为虚拟导航点沿着期望路径切向方向上的前视距离，合适的前视距离可以避免控制器出现饱和现象，为艏向误差。

由此，基于李雅普诺夫完成了SFLOS导引算法，该算法主要分为两部分：一是设计了期望路径的参数更新率，建立期望路径目标点与无人艇航行时的动态关系，二是根据SFLOS原理图的几何关系设计了期望艏向角，SFLOS导引算法的具体表达式如下：

2 路径跟踪仿真实验

在曲线路径跟踪控制中，在静水水域中分别进行路径跟踪仿真，设定无人艇的初始状态为位于大地坐标系原点且艏向角为零，期望速度同样设定为0.5m/s，航向控制器与航速控制器的参数与直线路径跟踪仿真试验中的一致，曲线跟踪路径如下：

SFLOS中的设计参数为：kx=1，前视距离为10m，仿真步长0.02。仿真结果如下：

由图3.1.6偏航距离曲线可以看出无人艇能较为准确地完成正弦函数的路径跟踪，误差在0.2m之内，在跟踪过程中没有出现偏航距离过大的现象，但根据图3.1.6和图3.1.8可以看出当无人艇进行大转角时，偏航距离达到最大值，最终无人艇实际航行路径能收敛到期望路径上，且无人艇的航速开始快速增加最终在航速控制器下稳定在0.5m/s左右，曲线路径跟踪控制满足路径跟踪的要求，对比上文基于LOS导引算法的直线路径跟踪控制的偏航距离误差达到1m，表明SFLOS导引算法的跟踪控制效果更好，性能更优。

3 结语

本文通过引入SF坐标系，设计了期望路径的参数更新率，建立期望路径目标点与无人艇航行时的动态关系，根据SFLOS原理图的几何关系设计了期望艏向角，将SFLOS导引算法与航速控制器、航向控制器相结合设计曲线路径跟踪控制算法。最后，用SIMULINK对LOS导引算法以及SFLOS导引算法的路径跟踪控制系统进行路径点以及正弦函数跟踪的仿真试验，验证相关算法的有效性以及优越性。

参考文献

[1]柳晨光 ，初秀民 ，毛庆洲 ，谢朔.无人船自适应路径跟踪控制系统[J].机械工程学报，2020，56（08）：216-227.

[2]陈霄，刘忠，罗亚松，等.海洋环境下欠驱动无人艇航迹跟踪控制算法[J].哈尔滨工业大学学报，2018，50（10）：116-123.

[3]Wan L ，Su Y ，Zhang H ，et al.An improved integral light-of-sight guidance law for path following of unmanned surface vehicles[J].Ocean Engineering，2020，205：107302.

[4]董早鵬，万磊，廖煜雷，等.基于非对称模型的欠驱动USV路径跟踪控制[J].中国造船，2016（1）：116-126.

[5]瞿洋，徐海祥，等.基于ILOS的欠驱船舶循迹控制[J].武汉理工大学学报（交通科学与工程版），2016，40（5）：834-838.

[6]WANG H，WANG D，PENG Z H.Adaptive neural control for cooperative path following of marine surface vehicles：State and output feedback[J].International Journal of Systems Science，2016，47（2）：343-359.

[7]周利.基于扰动补偿的无人艇路径跟踪控制算法研究[D].哈尔滨：哈尔滨工程大学，2017.

作者简介：

孙宏放（1978.8），男汉族 安徽桐城人 学历：博士研究生 职称：副研究员，从事科技创新、人才引进、成果转化和产业化等工作。

张爱国（1978.4），男汉族 山东巨野人 学历：博士研究生 职称：助理研究员，从事平台搭建、创业孵化、成果转化和产业推广等工作。