CN104950882A - 全局一致渐进路径跟踪引导控制方法 - Google Patents

Abstract

本发明公开了一种全局一致渐进路径跟踪引导控制方法，其特征在于，包括：读入当前船舶姿态、当前航路点和下一个航路点；判断是否达到下一个路航点；若是，则调用引导系统模块算法，更新航路点坐标和目标路径，计算实时期望航向角；若否，则调用引导系统模块算法，保持当前目标路径，计算实时期望航向角；调用控制系统模块算法，计算船舶相应的转艏力矩和纵向力。本发明的有益效果是：根据上面所提到的控制器方法和引导系统的两种方法的结合，这种串联的结合驱使船舶渐进地追踪期望的航向和估计外部扰动，并使得船舶保持的目标航线，实现高精度定位。

Description

全局一致渐进路径跟踪引导控制方法

技术领域

本发明属于海洋工程水面船舶舰艇动力定位控制领域，具体涉及一种基于自适应变积分增益视野线法（ILOS）的全局一致渐进路径跟踪引导控制算法。

背景技术

随着海洋资源不断地被探索，人类在海洋中的活动也日益广泛活跃，目前我国政策也大力支持海上交通的发展，在这些背景下，对船舶控制模块的精度和性能要求越来越高。而在海洋工程中，船舶动力定位系统是船舶在海洋作业和海上交通运输的重要支持系统之一，动力定位系统包括控制系统，引导系统，导航系统和测量系统等系统。随着我们自主研发的北斗导航的广泛应用，利用导航系统建立引导系统已成为海洋工程领域的热门研究对象。本发明更加强调利用导航系统反馈的船舶位置和角度数据，根据提出的引导控制算法驱使船舶驶向目标航线和保持航行在目标航线上。

船舶或者其他低速水面交通工具在低速航行，特别是远洋航行的时候，需要船舶通过一系列预设的航路点，甚至是期望要求船舶能够追踪某特定的直线，以此来节省船舶的燃料成本、维护成本和人力成本。由于海洋环境扰动具有一定的变化缓慢的特性和船舶的自身性能具有一定的局限性，因此面对常值扰动（或者是慢变扰动），研究一种平稳、实际可行的引导系统和控制系统是最基本要求也是最亟需解决的问题。

目前国内外已经有许多关于船舶路径追踪的研究和应用，该类研究成果主要是局部渐进的控制算法和引导系统中的传统LOS法或积分LOS法（简称ILOS），尚未有将积分器增益自适应调节的引导算法和全局一直渐进稳定的控制器算法相结合起来得到全局渐进稳定的串联系统。现有的研究中，部分得到控制输入是船舶本身由于自身局限性而无法产生满足稳定性和渐进性所需的转艏力矩、纵向力等，那么结合船舶的自身条件和约束性，本发明从引导系统算法着手，通过自适应调节积分器增益，即可以使得船舶以尽可能短的时间和距离到达目标航向，又可以避免积分所可能引起的饱和效应。同时，本发明利用对船舶横向跟踪误差积分以使船舶即使在目标航线上稳定航行时船头航向仍与目标航线保持一定的角度，以此容许船舶产生一定的横向力以抵消使得船舶偏离航线的外部扰动的作用力。另外，本发明还利用了积分器反演法（integrator backstepping）设计控制器算法，从而控制器算法保留了船舶的非线性和大惯性等有用特性。

发明内容

本发明的主要目的在于，针对上述现有技术中的不足，提供一种全局一致渐进路径跟踪引导控制方法。

本发明解决现有技术问题所采用的技术方案是：一种全局一致渐进路径跟踪引导控制方法，包括：

读入当前船舶姿态、当前航路点和下一个航路点；

判断是否达到下一个路航点；

若是，则调用引导系统模块算法，更新航路点坐标和目标路径，计算实时期望航向角；

若否，则调用引导系统模块算法，保持当前目标路径，计算实时期望航向角；

调用控制系统模块算法，计算船舶相应的转艏力矩和纵向力。

进一步地，所述计算实时期望航向角包括步骤：

（1）在引导系统中，对轨迹追踪横向误差进行积分并自适应改变积分增益；

（2）利用（1）实时在线计算船舶期望航向角度，同时对船舶进行侧滑角补偿：

假设船舶的操纵人员通过人机界面或者其他输入设备确定一系列航路点 ，，要求船舶以某种角度航行通过上述航路点或者以尽可能接近上述航路点时再转向，同时，两个航路点之间要求船舶以直线的方式航行；

首先，定义追踪误差：，若船舶当前位置是，那么有 ，；其中，是纵向追踪误差， 是横向追踪误差，；在该说明书中，我们假定：无论船舶偏离目标航线，还是恰好航行在目标航线上，我们都要求利用视野线法进行引导控制；在传统的视野线法（line-of-sight，简称LOS）是：，其中是横向跟踪误差，是根据船舶长度而设定的预设值；此时，要求船舶的航向能够与目标航线相一致，为此，在常值扰动下对船舶的期望艏向角度进行补偿：，，其中是目标航线的角度，u是船舶纵向速度，v是船舶横荡速度，当在缺少测量横荡速度v的条件下，利用改进的视野线法—自适应变积分增益视野线法进行侧滑角补偿，即该方法表达式： ${\mathrm{\ψ}}_{\mathrm{mILOS}}\stackrel{\mathrm{\Δ}}{=}-\arctan (K_{p}e+e^{-\mathrm{\ρ}\left|e\right|}{y}_e),$ ，，根据前者可以看出积分增益是根据纵向追踪误差自适应调节增益值的，则有期望航向角是：。

进一步地，所述计算船舶相应的转艏力矩和纵向力包括步骤：

在将期望艏向输入控制系统前，要求信号通过三个串联的低通滤波器以得到平滑可微的期望艏相角和高阶微分与，设船舶当前姿态为η＝[x,y,ψ]^T∈R²×[-π,π]，欠驱动船舶的三个自由度模型为： ，；

，船舶的船体固定坐标系中速度向量是ν＝[u,v,r]^T∈R³；

进行坐标转换： $z_0\stackrel{\mathrm{\Δ}}{=}\mathrm{\ψ}-{\mathrm{\ψ}}_d=h^T\mathrm{\η}-{\mathrm{\ψ}}_d,$  ， $Z\stackrel{\mathrm{\Δ}}{=}{[z_1,z_2,z_3]}^T=v-\mathrm{\α},$ ，其中 $h\stackrel{\mathrm{\Δ}}{=}{\left[\mathrm{0,0,1}\right]}^T$ 是映射向量，α＝[α₁,α₂,α₃]^T∈R³是后面设计到的镇定函数。由于和，对求得时间导数：。

设计第一个控制李雅普诺夫函数（CLF）：，对求时间导数： ，将镇定函数进行关于的线性化，即可得：，其中。所以，；

为了镇定第一个控制李雅普诺夫函数和自适应估计外部扰动，设计第二个控制李雅普诺夫函数：，其中估计误差，是对扰动的估计并且慢变扰动有，Γ＝Γ^T∈R^3×3＞0 是设计正定矩阵；

由和船舶三自由度数学模型，可得到的时间导数是：

将和代入上式可以得到：

通过选择，使得该方程右边第三项为0；

选择控制输入：

或者

选择时，，

选择时，，

其中， 是另一个设计正定矩阵；

至此，由于 正定和控制输入或， 负定，所以平衡点是全局一直渐进稳定。

为了实现船舶能够按照期望的速度航向，假设镇定函数，是所期望的船舶纵向前进速度，由欠驱动船舶的执行器物理机构，可以知道转艏力矩和横向力具有比例关系，为了使得船舶的控制输入能够适应此物理关系，利用中的关系可以得到：

 ，那么通过对动态变量积分求解可以得到镇定函数项，此时镇定函数向量的三个参数已经确定；

对横向追踪误差进行求时间导数：

，该表达式可以变换表达形式：，利用三角函数的变换关系，得到：

设：，

其中，，是上文所提到的控制系统；

显然，对于无强制系统，通过设计CLF：可以得出是全局一致渐进有界的，而驱动项显然是以上文的控制系统-子系统为输入的；根据控制领域已有定理，可以知道完整系统-系统是全局一致渐进稳定的；至此，在假设可以测量纵荡速度v的前提下，对侧滑角β进行了补偿，并得到了引导系统和控制系统串联结合而成的全局一致渐进追踪算法，在此基础之上，如果不能直接补偿侧滑角度β，通过改进传统的LOS引导算法，得到自适应变积分增益的积分型LOS引导算法： ${\mathrm{\ψ}}_{\mathrm{ILOS}}\stackrel{\mathrm{\Δ}}{=}-\arctan (K_{p}e+e^{-\mathrm{\ρ}\left|e\right|}{y}_e),$ ，，其中应该特别指出的是积分项的增益是关于的自然指数函数，和；自然指数函数在本发明中占居十分重要的地位，其随着的绝对值变大而呈指数减小，这意味着在船舶距离目标航线较远时能够使得引导系统算法中的积分项变为次要地位而比例项却占有重要作用以引导船舶在自身性能容许下尽快航向目标航线，从而避免了积分所引起的船舶执行器饱和效应；当船舶已经保持在目标航线时，即，积分增益消失变为1但保留着我们所需要的积分运算，以此实现了船舶在目标航线航行时仍然具有“侧滑角补偿”相类似的效果；为了说明该自适应变积分增益算法的稳定性，我们设计第三个CLF：；

因为，那么将控制李雅普诺夫函数进行时间微分，得：，所以可以得出--系统在处是全局一致渐进和全局局部指数稳定的平衡点。

本发明的有益效果是：根据上面所提到的控制器方法和引导系统的两种方法的结合，这种串联的结合驱使船舶渐进地追踪期望的航向和估计外部扰动，并使得船舶保持的目标航线，实现高精度定位。

附图说明

图1是LOS引导算法参数示意图；

图2是船舶自适应变积分增益LOS方法程序的流程图；

图3是船舶有横荡速度测量情况下，对侧滑角补偿后所得船舶实际艏向角；

图4是船舶有横荡速度测量情况下，对侧滑角补偿后船舶航行的实际轨迹；

图5是船舶有横荡速度测量情况下，对侧滑角补偿后船舶实际艏向角与引导算法所得期望艏向角之间误差；

图6是船舶在无横荡速度测量情况下，利用自适应变积分增益LOS引导算法后船舶实际艏向角；

图7是船舶在无横荡速度测量情况下，利用自适应变积分增益LOS引导算法后船舶航行的实际轨迹；

图8是船舶在无横荡速度测量情况下，利用自适应变积分增益LOS引导算法后对侧滑角补偿后船舶实际艏向角与引导算法所得期望艏向角之间误差。

本发明目的的实现、功能特点及优点将结合实施例，参照附图做进一步说明。

具体实施方式

以下将结合附图及具体实施例详细说明本发明的技术方案，以便更清楚、直观地理解本发明的发明实质。

参照图1至图8所示，本发明实施例提供了一种全局一致渐进路径跟踪引导控制方法，包括以下步骤：

步骤一：读入当前船舶姿态、当前航路点和下一个航路点；

步骤二：判断是否达到下一个路航点；

步骤三：若是，则调用引导系统模块算法，更新航路点坐标和目标路径，计算实时期望航向角；

若否，则调用引导系统模块算法，保持当前目标路径，计算实时期望航向角；

步骤四：调用控制系统模块算法，计算船舶相应的转艏力矩和纵向力。

具体的，所述计算实时期望航向角包括步骤：

（1）在引导系统中，对轨迹追踪横向误差进行积分并自适应改变积分增益；

（2）利用（1）实时在线计算船舶期望航向角度，同时对船舶进行侧滑角补偿：

假设船舶的操纵人员通过人机界面或者其他输入设备确定一系列航路点，，要求船舶以某种角度航行通过上述航路点或者以尽可能接近上述航路点时再转向，同时，两个航路点之间要求船舶以直线的方式航行；

首先，定义追踪误差，若船舶当前位置是，那么有 ，；其中，是纵向追踪误差，是横向追踪误差，；在该说明书中，我们假定：无论船舶偏离目标航线，还是恰好航行在目标航线上，我们都要求利用视野线法进行引导控制；在传统的视野线法（line-of-sight，简称LOS）是：，其中是横向跟踪误差，是根据船舶长度而设定的预设值；此时，要求船舶的航向能够与目标航线相一致，为此，在常值扰动下对船舶的期望艏向角度进行补偿：，，其中是目标航线的角度，u是船舶纵向速度，v是船舶横荡速度，当在缺少测量横荡速度v的条件下，利用改进的视野线法—自适应变积分增益视野线法进行侧滑角补偿，即该方法表达式： ${\mathrm{\ψ}}_{\mathrm{mILOS}}\stackrel{\mathrm{\Δ}}{=}-\arctan (K_{p}e+e^{-\mathrm{\ρ}\left|e\right|}{y}_e),$ ，，根据前者可以看出积分增益是根据纵向追踪误差自适应调节增益值的，则有期望航向角是：。

具体的，所述计算船舶相应的转艏力矩和纵向力包括步骤：

在将期望艏向输入控制系统前，要求信号通过三个串联的低通滤波器以得到平滑可微的期望艏相角和高阶微分与，设船舶当前姿态为η＝[x,y,ψ]^T∈R²×[-π,π]，欠驱动船舶的三个自由度模型为： ，；

，船舶的船体固定坐标系中速度向量是ν＝[u,v,r]^T∈R³；

进行坐标转换： $z_0\stackrel{\mathrm{\Δ}}{=}\mathrm{\ψ}-{\mathrm{\ψ}}_d=h^T\mathrm{\η}-{\mathrm{\ψ}}_d,$  ， $Z\stackrel{\mathrm{\Δ}}{=}{[z_1,z_2,z_3]}^T=v-\mathrm{\α},$ ，其中 $h\stackrel{\mathrm{\Δ}}{=}{\left[\mathrm{0,0,1}\right]}^T$ 是映射向量，是后面设计到的镇定函数。由于和，对求得时间导数：。

设计第一个控制李雅普诺夫函数（CLF）：，对求时间导数： ，将镇定函数进行关于的线性化，即可得：，其中。所以，；

为了镇定第一个控制李雅普诺夫函数和自适应估计外部扰动，设计第二个控制李雅普诺夫函数：，其中估计误差，是对扰动的估计并且，Γ＝Γ^T∈R^3×3＞0 是设计正定矩阵；

由和船舶三自由度数学模型，可得到的时间导数是：

将和代入上式可以得到：

通过选择，使得该方程右边第三项为0；

选择控制输入：

或者

选择时，，

选择时，，

其中， 是另一个设计正定矩阵；

至此，由于 正定和控制输入或， 负定，所以平衡点是全局一直渐进稳定。

为了实现船舶能够按照期望的速度航向，假设镇定函数，是所期望的船舶纵向前进速度，由欠驱动船舶的执行器物理机构，可以知道转艏力矩和横向力具有比例关系，为了使得船舶的控制输入能够适应此物理关系，利用中的关系可以得到：

 ，那么通过对动态变量积分求解可以得到镇定函数项，此时镇定函数向量的三个参数已经确定；

对横向追踪误差进行求时间导数：

，该表达式可以变换表达形式：，利用三角函数的变换关系，得到：

设：，

其中，，是上文所提到的控制系统；

显然，对于无强制系统，通过设计CLF：可以得出是全局一致渐进有界的，而驱动项显然是以上文的控制系统-子系统为输入的；根据控制领域已有定理，可以知道完整系统-系统是全局一致渐进稳定的；至此，在假设可以测量纵荡速度v的前提下，对侧滑角β进行了补偿，并得到了引导系统和控制系统串联结合而成的全局一致渐进追踪算法，在此基础之上，如果不能直接补偿侧滑角度β，通过改进传统的LOS引导算法，得到自适应变积分增益的积分型LOS引导算法： ${\mathrm{\ψ}}_{\mathrm{ILOS}}\stackrel{\mathrm{\Δ}}{=}-\arctan (K_{p}e+e^{-\mathrm{\ρ}\left|e\right|}{y}_e),$ ，，其中应该特别指出的是积分项的增益是关于的自然指数函数，和；自然指数函数在本发明中占居十分重要的地位，其随着的绝对值变大而呈指数减小，这意味着在船舶距离目标航线较远时能够使得引导系统算法中的积分项变为次要地位而比例项却占有重要作用以引导船舶在自身性能容许下尽快航向目标航线，从而避免了积分所引起的船舶执行器饱和效应；当船舶已经保持在目标航线时，即，积分增益消失变为1但保留着我们所需要的积分运算，以此实现了船舶在目标航线航行时仍然具有“侧滑角补偿”相类似的效果；为了说明该自适应变积分增益算法的稳定性，我们设计第三个CLF：；

因为，那么将控制李雅普诺夫函数进行时间微分，得：，所以可以得出--系统在处是全局一致渐进和全局局部指数稳定的平衡点。

在C++环境下，可以根据流程图（图2）进行编译实现。

利用已知欠驱动船舶数学模型，在Matlab/simulink环境中进行仿真和验证。通过采用本发明的控制器算法，得出船舶应该输出的力和力矩的大小，驱使船舶驶向目标航线。针对已知数学模型，应该设计出能适应船舶自身条件和性能的引导系统，以使得引导系统输出的期望转艏角度是船舶能够达到的，这样需要调节ρ以得到合适的引导系统输出，同时设计其他的设计参数和设计矩阵。

设定某船舶的初始位置和初始艏向是：，该船舶运行在固定风速和常值海流的扰动，其扰动合作用力是：。

第一种情景：假设船舶的横荡速度v和纵荡速度u均可知，在这种环境下，我们要求船舶能够按照倾斜率是π/3并且通过航路点等航路点，同时要求船舶的纵向速度是7.8节每小时。如图3所示船舶的航向（角度单位为弧度制），在船舶完全追踪在目标航线时艏向与目标航线成一定角度（实际艏向0.8rad<π/3rad），此时船舶与目标航线保持一定角度以应对外界环境扰动的作用力。图4是船舶的实际轨迹和航行姿态。图5是船舶实际艏向和基于补偿LOS引导算法所产生的期望艏向之间的误差值。

第二种情景：假设横荡速度和纵荡速度未知或者作为不必要条件，在这种环境下，我们要求船舶能够按照倾斜率是π/3并且通过航路点等航路点，同时要求船舶的纵向速度是7.8节每小时。如图6所示船舶的航向（角度单位为弧度制），在船舶完全追踪在目标航线时艏向与目标航线成一定角度（实际艏向0.8rad<π/3rad），此时船舶与目标航线保持一定角度以应对外界环境扰动的作用力。图7是船舶的实际轨迹和航行姿态。图8是船舶实际艏向和自适应变积分增益LOS引导算法所产生的期望艏向之间的误差值。

事实证明本发明中的自适应变积分增益LOS引导算法与反演法相结合可以实现全局一致渐进稳定性，并驱使船舶平稳地驶向目标航线。

图3—图8所示的两个例子说明了本发明具有的平稳、高精度的引导和控制算法，另外仿真2000s只需进行不足0.01s的时间，这也从侧面反映出本发明的计算简单。在环境扰动是缓慢变化的条件下，船舶能够稳定地趋向目标航线，这在实际工程应用中具有很高的应用开发价值。

根据上面所提到的控制器方法和引导系统的两种方法的结合，这种串联的结合驱使船舶渐进地追踪期望的航向和估计外部扰动，并使得船舶保持的目标航线，实现高精度定位。

以上所述仅为本发明的优选实施例，并非因此限制其专利范围，凡是利用本发明说明书及附图内容所作的等效结构或等效流程变换，直接或间接运用在其他相关的技术领域，均同理包括在本发明的专利保护范围内。

Claims (3)

1.一种全局一致渐进路径跟踪引导控制方法，其特征在于，包括：

读入当前船舶姿态、当前航路点和下一个航路点；

判断是否达到下一个路航点；

若是，则调用引导系统模块算法，更新航路点坐标和目标路径，计算实时期望航向角；

若否，则调用引导系统模块算法，保持当前目标路径，计算实时期望航向角；

调用控制系统模块算法，计算船舶相应的转艏力矩和纵向力。

2.根据权利要求1所述的全局一致渐进路径跟踪引导控制方法，其特征在于，所述计算实时期望航向角包括步骤：

（1）在引导系统中，对轨迹追踪横向误差进行积分并自适应改变积分增益；

（2）利用（1）实时在线计算船舶期望航向角度，同时对船舶进行侧滑角补偿：

假设船舶的操纵人员通过人机界面或者其他输入设备确定一系列航路点 ，，要求船舶以某种角度航行通过上述航路点或者以尽可能接近上述航路点时再转向，同时，两个航路点之间要求船舶以直线的方式航行；

首先，定义追踪误差：，若船舶当前位置是，那么有 ，；其中，是纵向追踪误差，是横向追踪误差，；在该说明书中，我们假定：无论船舶偏离目标航线，还是恰好航行在目标航线上，我们都要求利用视野线法进行引导控制；在传统的视野线法（line-of-sight，简称LOS）是：，其中是横向跟踪误差，是根据船舶长度而设定的预设值；此时，要求船舶的航向能够与目标航线相一致，为此，在常值扰动下对船舶的期望艏向角度进行补偿：，，其中是目标航线的角度，u是船舶纵向速度，v是船舶横荡速度，当在缺少测量横荡速度v的条件下，利用改进的视野线法—自适应变积分增益视野线法进行侧滑角补偿，即该方法表达式：，根据前者可以看出积分增益是根据纵向追踪误差自适应调节增益值的，则有期望航向角是：。

3.根据权利要求1所述的全局一致渐进路径跟踪引导控制方法，其特征在于，所述计算船舶相应的转艏力矩和纵向力包括步骤：

在将期望艏向输入控制系统前，要求信号通过三个串联的低通滤波器以得到平滑可微的期望艏相角和高阶微分与，设船舶当前姿态为η＝[x,y,ψ]^T∈R²×[-π,π]，欠驱动船舶的三个自由度模型为： ，；

，船舶的船体固定坐标系中速度向量是ν＝[u,v,r]^T∈R³；

进行坐标转换： ，其中是映射向量，是后面设计到的镇定函数，

由于和，对求得时间导数：，设计第一个控制李雅普诺夫函数（CLF）：，对求时间导数： ，将镇定函数进行关于的线性化，即可得：，其中，

所以，；

为了镇定第一个控制李雅普诺夫函数和自适应估计外部扰动，设计第二个控制李雅普诺夫函数：，其中估计误差，是对扰动的估计并且，Γ＝Γ^T∈R^3×3＞0 是设计正定矩阵；

由和船舶三自由度数学模型，可得到的时间导数是：

将和代入上式可以得到：

通过选择，使得该方程右边第三项为0；

选择控制输入：

或者

选择时，，

选择时，，

其中， 是另一个设计正定矩阵；

至此，由于 正定和控制输入或， 负定，所以平衡点是全局一直渐进稳定，为了实现船舶能够按照期望的速度航向，假设镇定函数，是所期望的船舶纵向前进速度，由欠驱动船舶的执行器物理机构，可以知道转艏力矩和横向力具有比例关系，为了使得船舶的控制输入能够适应此物理关系，利用中的关系可以得到：

 ，那么通过对动态变量积分求解可以得到镇定函数项，此时镇定函数向量的三个参数已经确定；

对横向追踪误差进行求时间导数：

，该表达式可以变换表达形式：，利用三角函数的变换关系，得到：

设：，

其中，，是上文所提到的控制系统；

显然，对于无强制系统，通过设计CLF：可以得出是全局一致渐进有界的，而驱动项显然是以上文的控制系统-子系统为输入的；根据控制领域已有定理，可以知道完整系统-系统是全局一致渐进稳定的；至此，在假设可以测量纵荡速度v的前提下，对侧滑角β进行了补偿，并得到了引导系统和控制系统串联结合而成的全局一致渐进追踪算法，在此基础之上，如果不能直接补偿侧滑角度β，通过改进传统的LOS引导算法，得到自适应变积分增益的积分型LOS引导算法：，其中应该特别指出的是积分项的增益是关于的自然指数函数，和；自然指数函数在本发明中占居十分重要的地位，其随着的绝对值变大而呈指数减小，这意味着在船舶距离目标航线较远时能够使得引导系统算法中的积分项变为次要地位而比例项却占有重要作用以引导船舶在自身性能容许下尽快航向目标航线，从而避免了积分所引起的船舶执行器饱和效应；当船舶已经保持在目标航线时，即，积分增益消失变为1但保留着我们所需要的积分运算，以此实现了船舶在目标航线航行时仍然具有“侧滑角补偿”相类似的效果；为了说明该自适应变积分增益算法的稳定性，我们设计第三个CLF：；

因为，那么将控制李雅普诺夫函数进行时间微分，得：，所以可以得出--系统在处是全局一致渐进和全局局部指数稳定的平衡点。