CN115562266A - 基于变参数视线法的无人船航迹控制方法及存储介质 - Google Patents

Abstract

本发明是一种基于变参数视线法的无人船航迹控制方法及存储介质，包括建立有限时间内的船舶数学模型，通过回转试验求取模型参数，将系统改造成最佳二阶系统来设计航向控制器；基于视线法实现船舶轨迹跟踪控制，根据当前坐标与目标航线的位置关系计算出船舶沿轨迹航行的视线角，将其与实际航向角作差，得到目标航向角，再通过控制器得到目标舵角，并向下层发送命令，控制船舶运动，消除航迹误差，使得船舶向着期望轨迹方向驶进。本发明根据船舶实际位置与路径目标点的距离确定起始航点，方便船舶试验；针对无人船无法正常切换航段的情况，调节算法的视线半径以及切换条件，减小跟踪误差，使得航迹控制更加精确。

Description

基于变参数视线法的无人船航迹控制方法及存储介质

技术领域

本发明涉及无人船的航迹跟踪控制技术领域，具体涉及一种基于变参数视线法的无人船航迹控制方法。

背景技术

无人船的系统是复杂系统，包含多种功能模块，航行控制是系统重要的模块之一。无人船在海上航行时从经济性和安全性两方面考虑，要求其能沿着特定的航线，即期望轨迹航行。但是在船舶航行过程中不可避免地会受到风浪流等随机干扰的影响，船舶将偏离计划航迹，这样不但会消耗更多的能量，甚至会造成航行事故。因此，设计合理的航迹控制器使船舶保持航迹尽快地到达目的地具有重要意义。

目前存在的技术问题有：

当湖泊、河流和海面上风平浪静时候，无人船航行一般保持直线或者由直线组成的折线航路，这样时间最短、燃料消耗也最少。船舶航行时总会受到外界环境因素的影响，规划航行期望的路线时可以将其规划为动态的曲线，此时传统的视线法无法满足跟踪期望航线的需求。

发明内容

本发明提出的一种基于变参数视线法的无人船航迹控制方法，可至少解决上述技术问题之一。

为实现上述目的，本发明采用了以下技术方案：

一种基于变参数视线法的无人船航迹控制方法，包括以下步骤，

首先建立有限时间内的船舶数学模型，通过回转试验求取模型参数，将系统改造成最佳二阶系统来设计航向控制器；

基于视线法实现船舶轨迹跟踪控制，根据当前坐标与目标航线的位置关系计算出船舶沿轨迹航行的视线角，将其与船舶航向角作差，得到目标航向角，进而得到目标舵角，并向下层发送命令，控制船舶运动，消除航迹误差，使得船舶向着期望轨迹方向驶进。

进一步的，具体包括以下步骤：

S1、建立有限时间内船舶模型；

S2、设计航向控制器；

S3、将不连续的节点映射到控制算法区间内；

S4、变参数视线法实现无人船航迹跟踪控制。

进一步的，所述S1、建立有限时间内船舶模型具体包括：

在有限时间内，将船舶当作一阶惯性模型，传递函数为

其中K为增益系数，T为时间常数；

上述模型的阶跃响应输出峰值为A，输入的阶跃响应系数为K₁，系统输出

则系统的放大系数K＝A/K₁；对一阶系统

的阶跃响应进行拉普拉斯反变换得到时域信号c(t)＝1-e^-t/T；

船舶以设定速度直线航行达到稳定后，将舵转到设定舵角并保持不变，此时船舶进入回转状态。实时采集输出角速度与输入舵角数据，并分别绘制它们与试验时间的关系曲线，由于角速度与舵角之间满足此种关系

即可计算出船舶在转弯时的模型参数K、T。

进一步的，所述S2、设计航向控制器具体包括，

标准二阶系统的开环传递函数为：

ξ为阻尼比，ω_n为自然频率，第一步建立的船舶模型为

再通过一个积分环节

得到航向角与输入舵角之间的关系

设计的控制器为W_c(s)，由于

控制器

如此构成一个闭环，输入为期望的航向角，输出为传感器测量的实际航向角。

进一步的，所述S3、将不连续的节点映射到控制算法区间内具体包括，

将不在控制范围的角度映射到

这个区间里，具体映射方法参照即

由DGPS获取船的经纬度，实时转换为XY平面坐标，并绘制在地图中；

利用串口读取DGPS信息，按照读取协议进行解析，获取经度、纬度、东向速度、北向速度以及航向角信息，用于航路规划、定位当前船舶位置、计算目标航向角，DGPS检测到的航向角的范围

在实际控制中需要将其映射到视线法的控制区间上，检测角与控制角坐标的转换如下：

进一步的，所述S4、变参数视线法实现无人船航迹跟踪控制具体包括：

航段识别通过航点号进行区分，通过目标航点号[x_d(k),y_d(k)]和当前航点号(x,y)确定当前目标航段，起始航点为1号点，后续航点号依次加一；设切换半径为R₀，转换条件如下：

当发现无人船进入到了以当前路径点[x_d(k),y_d(k)]为圆心，R₀为半径的圆时，判定为抵达目标航点；或者已经经过此航段已进入下一航段，但是未判别到进入上述圆中；目标航点切换为下一航点[x_d(k+1),y_d(k+1)]，当前航点号加一，此时目标航段变成[x_d(k),y_d(k)]→[x_d(k+1),y_d(k+1)]，期望的航向角变成

直到最后一个目标点；

当无人船航行的期望轨迹设计为曲线，此时将曲线按照切线进行分割，分成不同长度的线段，由此可对曲线进行跟踪；取曲线上一点作外切线，将一条曲线分成了n段直线段，此时根据船舶当前位置与期望航段的距离，以及航段的长度来调节视线半径R与切换半径R₀；

设船舶当前位置与期望航段的垂直距离为d，当目标航线未出现在无人船视线半径圆内，将此时的视线半径增大，令R'＝d，控制船舶向目标航线靠近，直到目标航线进入到视线圆内，此时视线半径等于初始设定值；当航段短于切换半径R₀时，将调节航段切换的条件，由于船舶惯性大的特点，切换半径最小值应不小于一个船长L，因此令R₀＝L。

另一方面，本发明还公开一种计算机可读存储介质，存储有计算机程序，所述计算机程序被处理器执行时，使得所述处理器执行如上述方法的步骤。

由上述技术方案可知，本发明的基于变参数视线法的无人船航迹控制方法，首先建立了有限时间内的船舶数学模型，通过回转试验求取模型参数，设计航向控制器以及航迹控制器，使无人船按照期望航迹航行。本发明的主要创新点在于：期望路线为两点间的曲线且时刻变化，故采用一种变参数的视线法对系统进行航迹跟踪控制。

首先建立有限时间内的船舶数学模型，通过回转试验求取模型参数，将系统改造成最佳二阶系统来设计航向控制器；基于视线法实现船舶轨迹跟踪控制，根据当前坐标与目标航线的位置关系计算出船舶沿轨迹航行的视线角，将其与船舶航向角作差，得到目标航向角，进而得到目标舵角，并向下层发送命令，控制船舶运动，消除航迹误差，使得船舶向着期望轨迹方向驶进。

本发明同现有技术相比，具有如下优点：

(1)针对无人船没有进入切换圆内但是已过切换航点的情况，调节算法的视线半径以及切换条件，避免船舶在航行过程中无法正常切换航段的问题，使得航迹控制更加精确。

(2)视线法要求期望航段为直线组成的折线段，而由于无人船航行受到风浪流的干扰，规划的目标航路为曲面上两点间的曲线，本发明采用的变参数视线法，可以对曲线轨迹进行跟踪。

(3)利用简单的回转试验获取船舶两个重要的参数，将无人船系统设计为最佳二阶系统，不需要大量的试验去调整控制参数，且控制的效果较好、稳定性较高。

附图说明

图1为视线法算法实现流程图；

图2为视线法示意图；

图3a为一阶系统阶跃响应框图；

图3b为一阶系统阶跃响应时域信号曲线图；

图4a为舵角-角速率关系曲线图，其中输入舵角＝34°；

图4b为舵角-角速率关系曲线图，其中输入舵角＝30°；

图5为航向控制工作原理图；

图6为航向控制器设计框图；

图7为系统整体设计框图；

图8为检测角与控制角坐标转换图；

图9为变参数视线法示意图；

图10为航段切换示意图；

图11为轨迹跟踪控制结果图。

具体实施方式

为使本发明实施例的目的、技术方案和优点更加清楚，下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例是本发明一部分实施例，而不是全部的实施例。

无人船在海上航行时从经济性和安全性两方面考虑，要求其能沿着特定的航线航行。但船舶不可避免地会受到风浪流等随机干扰的影响，使其偏离计划航迹，这样不但会消耗更多的能量，甚至会造成航行事故。因此，设计合理的航迹控制器使船舶保持航迹尽快地到达目的地具有重要意义。

对于无人船航迹控制问题，通常采用视线法。该算法的导航原理：如果使被控船舶的航向保持对准视线角，经过适当的控制就能使被控船舶到达期望的位置，达到航迹跟踪的效果。该算法将传统的控制量从3个自由度的船舶位置和航向角，减少到2个自由度的船舶航向角和航行速度。视线法的算法实现流程图如图1。

视线法示意图如图2。航路规划中的航线是从点P₁＝(x₁,y₁)到点P₂＝(x₂,y₂)的一个线段，航路外一点P₀＝(x₀,y₀)表示无人船的当前位置，速度是v，视线算法的半径R＝(3～5)倍船长，以R为半径向下一个目标航点画圆交点为P_LOS＝(x_LOS,y_LOS)，d表示当前位置到航线的距离，垂足坐标点为P_N＝(x_N,y_N)。

视线法要求期望航段为直线组成的折线段，而由于无人船航行受到风浪流的干扰，规划的目标航路为曲面上两点间的曲线，由于外部环境时刻变化，期望的航线不断变化，曲线的斜率改变导致期望的航线段改变，此时利用变参数视线法对无人船进行轨迹跟踪控制。并且将不连续的节点映射到算法的控制区间里，提高了算法的稳定性。

具体实施步骤如下：

第1步，建立有限时间内船舶模型。

在有限时间内，可以认为船舶模型参数不变。船舶是大惯性物体，因此可以将其当作一阶惯性模型，传递函数为

其中K为增益系数，T为时间常数。

如图3a)，上述模型的阶跃响应输出峰值为A，输入的阶跃响应系数为K₁，系统输出

则系统的放大系数K＝A/K₁。对一阶系统

的阶跃响应进行拉普拉斯反变换得到时域信号c(t)＝1-e^-t/T，如图3b)。可以看出，峰值的0.632倍点对应的时间t即为系统的时间常数T。

船舶以设定速度直线航行达到稳定后，将舵转到设定舵角并保持不变，此时船舶进入回转状态。实时采集输出角速度与输入舵角数据，并分别绘制它们与试验时间的关系曲线，由于角速度与舵角之间满足此种关系

即可计算出船舶在转弯时的模型参数K、T。

通过分析图4图例中的第3、4条曲线——命令舵角值与实际舵角反馈值，发现在向舵机发出命令后大概1秒的时间，才有实际的数据反馈。因此，舵机模型中存在延迟环节e^-τs。实际测得的时间常数T≈6s>3τ，因此可以不考虑舵机的延迟，将延迟环节等效为一个小的一阶惯性环节与舵机模型合并得到：

在左电机转速为696rpm、右电机转速为656rpm、实际船速约为2.4m/s的情况下，K≈0.079，T≈6.6；在左电机转速为793rpm、右电机转速为794rpm、实际船速约为3.4m/s的情况下，K≈0.085，T≈5.9。

通过在顺流和逆流不同船速下的测试，求取的参数都比较稳定。因此在小的时间范围内和小的扰动下可以认为船舶转弯的参数一定。

第2步，设计航向控制器。

航向控制工作原理图如图5所示。

航向控制工作原理：命令舵角与角速率的关系为一阶惯性环节，通过积分得到船舶的航向角。航向控制器输入为航向偏差，由输入的目标航向和传感器采集的当前航向角比较得到，然后输入到航向控制器，输出目标舵角δ_r(k)，之后传递给舵角控制器，舵机根据舵角指令信号转到所需角度。此时船舶改变航向，跟踪目标最终实现控制功能。

通过设计控制器，将整个系统改造成最佳二阶系统。与一阶系统相比，二阶系统的动静态性能都比较好，系统比较稳定容易控制。影响二阶系统的主要参数是频率比和阻尼比，当阻尼比在0.6～0.8时，综合超调量、上升时间、稳定时间这些指标，系统有较好的性能。因此在设计控制器时取阻尼比为0.707、自然频率为1，通过零极点配置达到综合性能最佳的系统。

控制器设计框图如图6所示。标准二阶系统的开环传递函数为：

(ξ为阻尼比，ω_n为自然频率)，第一步建立的船舶模型为

再通过一个积分环节

得到航向角与输入舵角之间的关系

设计的控制器为W_c(s)。由于

控制器

如此构成一个闭环，输入为期望的航向角，输出为传感器测量的实际航向角。

第3步，将不连续的节点映射到控制算法区间内。

由于视线法的控制区间在[-π,π]上，使得在-π和π这两个节点是不连续的。当船舶向正南方向航行时，航向角很多时候可能需要在-π和π这两个节点附近变化，就会出现在船舶跟踪期望航向点时，控制器发生误命令，使得船舶向相反方向绕大圈来跟踪期望航向点，这种情况非常不利于对船舶进行航迹跟踪控制。

当航向角偏差在控制区间即

时，控制器发出的指令是正确的，可以正常指导船舶跟踪目标航向点；当

时，控制器发出的指令与实际的航向角相差±360°，船舶实际航向角可能会从反方向向期望航向角收敛。所以需要将不在控制范围的角度映射到[-π,π]这个区间里即可解决该问题，具体映射方法参照即

系统整体设计框图如图7所示。系统整体设计分为两个部分：仿真部分与实际部分。框图中包含航海计算模块、传感器检测环节、位置解算环节、航向控制环、航迹控制环节。

航海计算模块，实现航线加载、航点号计算、抵达判断、目标航线航向角计算等功能。位置解算环节根据当前航向和速度，在航向角下对速度做x轴和y轴方向的分解并积分，得到XY位置。

航迹控制环节的功能是先通过航海计算环节，通过LOS算法，将接收到的无人艇的坐标值计算出的视线角与目标航线的航向角进行比较，计算出航迹误差，将其输入到航迹控制器，通过航迹控制算法得到航向纠正值，与航海计算环节中计算得到的目标航向角比较得出航向

将计算出的航向角传递给航向控制环，消除航迹误差，使得船舶向着期望轨迹方向驶进。

在模型仿真下，根据模型递推出的航向角以及速度进行位置解算，在仿真到达的位置与设定航点一定距离时实现航段切换，直到船舶到达最后一个航点，结束仿真。在实际试验中，是由DGPS获取船的经纬度，实时转换为XY平面坐标，并绘制在地图中，前期还要进行地图与实际GPS的校准。

利用串口读取DGPS信息，按照读取协议进行解析，获取经度、纬度、东向速度、北向速度以及航向角等信息，用于航路规划、定位当前船舶位置、计算目标航向角等。需要注意的是，DGPS检测到的航向角的范围

在实际控制中需要将其映射到视线法的控制区间上，检测角与控制角坐标的转换如图8所示。

第4步，变参数视线法实现无人船航迹跟踪控制。

在航行过程中要有一个转换条件来判断是否切换到下个路径点。航段识别通过航点号进行区分，通过目标航点号[x_d(k),y_d(k)]和当前航点号(x,y)确定当前目标航段。起始航点为1号点，后续航点号依次加一。设切换半径为R₀，转换条件如下：

当发现无人船进入到了以当前路径点[x_d(k),y_d(k)]为圆心，R₀为半径的圆时，判定为抵达目标航点；或者已经经过此航段已进入下一航段，但是未判别到进入上述圆中，避免了船舶未切换航段而导致无法跟踪目标航线的情况。目标航点切换为下一航点[x_d(k+1),y_d(k+1)]，当前航点号加一，此时目标航段变成[x_d(k),y_d(k)]→[x_d(k+1),y_d(k+1)]，期望的航向角变成

直到最后一个目标点。航段切换示意图如图9所示。

当无人船航行的期望轨迹设计为曲线，此时将曲线按照切线进行分割，分成不同长度的线段，由此可对曲线进行跟踪。由于外部环境时刻变化，期望的航线不断变化，曲线的斜率改变导致期望的航线段改变。取曲线上一点作外切线，将一条曲线分成了n段直线段，示意图如图10所示。此时根据船舶当前位置与期望航段的距离，以及航段的长度来调节视线半径R与切换半径R₀。

设船舶当前位置与期望航段的垂直距离为d，当目标航线未出现在无人船视线半径圆内(即d>R)，算法将此时的视线半径增大，令R'＝d，控制船舶向目标航线靠近，直到目标航线进入到视线圆内(d≤R)，此时视线半径等于初始设定值，即R'＝R；当航段短于切换半径R₀时，算法将调节航段切换的条件，由于船舶惯性大的特点，切换半径最小值应不小于一个船长L，因此令R₀＝L，改善了由于船舶转弯时的大惯性导致轨迹跟踪效果不好的情况。

在不同的水流环境下做测试，轨迹跟踪控制效果图如图11，控制效果良好，验证了控制算法的稳定性。

又一方面，本发明还公开一种计算机可读存储介质，存储有计算机程序，所述计算机程序被处理器执行时，使得所述处理器执行如上述任一方法的步骤。

再一方面，本发明还公开一种计算机设备，包括存储器和处理器，所述存储器存储有计算机程序，所述计算机程序被所述处理器执行时，使得所述处理器执行如上述任一方法的步骤。

在本申请提供的又一实施例中，还提供了一种包含指令的计算机程序产品，当其在计算机上运行时，使得计算机执行上述实施例中任一方法的步骤。

可理解的是，本发明实施例提供的系统与本发明实施例提供的方法相对应，相关内容的解释、举例和有益效果可以参考上述方法中的相应部分。

本领域普通技术人员可以理解实现上述实施例方法中的全部或部分流程，是可以通过计算机程序来指令相关的硬件来完成，所述的程序可存储于一非易失性计算机可读取存储介质中，该程序在执行时，可包括如上述各方法的实施例的流程。其中，本申请所提供的各实施例中所使用的对存储器、存储、数据库或其它介质的任何引用，均可包括非易失性和/或易失性存储器。非易失性存储器可包括只读存储器(ROM)、可编程ROM(PROM)、电可编程ROM(EPROM)、电可擦除可编程ROM(EEPROM)或闪存。易失性存储器可包括随机存取存储器(RAM)或者外部高速缓冲存储器。作为说明而非局限，RAM以多种形式可得，诸如静态RAM(SRAM)、动态RAM(DRAM)、同步DRAM(SDRAM)、双数据率SDRAM(DDRSDRAM)、增强型SDRAM(ESDRAM)、同步链路(Synchlink)DRAM(SLDRAM)、存储器总线(Rambus)直接RAM(RDRAM)、直接存储器总线动态RAM(DRDRAM)、以及存储器总线动态RAM(RDRAM)等。

以上实施例的各技术特征可以进行任意的组合，为使描述简洁，未对上述实施例中的各个技术特征所有可能的组合都进行描述，然而，只要这些技术特征的组合不存在矛盾，都应当认为是本说明书记载的范围。

以上实施例仅用以说明本发明的技术方案，而非对其限制；尽管参照前述实施例对本发明进行了详细的说明，本领域的普通技术人员应当理解：其依然可以对前述各实施例所记载的技术方案进行修改，或者对其中部分技术特征进行等同替换；而这些修改或者替换，并不使相应技术方案的本质脱离本发明各实施例技术方案的精神和范围。

Claims (7)

1.一种基于变参数视线法的无人船航迹控制方法，其特征在于，包括以下步骤，

首先建立有限时间内的船舶数学模型，通过回转试验求取模型参数，将系统改造成最佳二阶系统来设计航向控制器；

基于视线法实现船舶轨迹跟踪控制，根据当前坐标与目标航线的位置关系计算出船舶沿轨迹航行的视线角，将其与船舶航向角作差，得到目标航向角，进而得到目标舵角，并向下层发送命令，控制船舶运动，消除航迹误差，使得船舶向着期望轨迹方向驶进。

2.根据权利要求1所述的基于变参数视线法的无人船航迹控制方法，其特征在于：具体包括以下步骤：

S1、建立有限时间内船舶模型；

S2、设计航向控制器；

S3、将不连续的节点映射到控制算法区间内；

S4、变参数视线法实现无人船航迹跟踪控制。

3.根据权利要求2所述的基于变参数视线法的无人船航迹控制方法，其特征在于：所述S1、建立有限时间内船舶模型具体包括：

在有限时间内，将船舶当作一阶惯性模型，传递函数为

其中K为增益系数，T为时间常数；

上述模型的阶跃响应输出峰值为A，输入的阶跃响应系数为K₁，系统输出

则系统的放大系数K＝A/K₁；对一阶系统

的阶跃响应进行拉普拉斯反变换得到时域信号c(t)＝1-e^-t/T；

船舶以设定速度直线航行达到稳定后，将舵转到设定舵角并保持不变，此时船舶进入回转状态。实时采集输出角速度与输入舵角数据，并分别绘制它们与试验时间的关系曲线，由于角速度与舵角之间满足此种关系

即可计算出船舶在转弯时的模型参数K、T。

4.根据权利要求2所述的基于变参数视线法的无人船航迹控制方法，其特征在于：所述S2、设计航向控制器具体包括，

标准二阶系统的开环传递函数为：

ξ为阻尼比，ω_n为自然频率，第一步建立的船舶模型为

再通过一个积分环节

得到航向角与输入舵角之间的关系

设计的控制器为W_c(s)，由于

控制器

如此构成一个闭环，输入为期望的航向角，输出为传感器测量的实际航向角。

5.根据权利要求2所述的基于变参数视线法的无人船航迹控制方法，其特征在于：所述S3、将不连续的节点映射到控制算法区间内具体包括，

将不在控制范围的角度映射到

这个区间里，具体映射方法参照即

由DGPS获取船的经纬度，实时转换为XY平面坐标，并绘制在地图中；

利用串口读取DGPS信息，按照读取协议进行解析，获取经度、纬度、东向速度、北向速度以及航向角信息，用于航路规划、定位当前船舶位置、计算目标航向角，DGPS检测到的航向角的范围

在实际控制中需要将其映射到视线法的控制区间上，检测角与控制角坐标的转换如下：

6.根据权利要求2所述的基于变参数视线法的无人船航迹控制方法，其特征在于：所述S4、变参数视线法实现无人船航迹跟踪控制具体包括：

航段识别通过航点号进行区分，通过目标航点号[x_d(k),y_d(k)]和当前航点号(x,y)确定当前目标航段，起始航点为1号点，后续航点号依次加一；设切换半径为R₀，转换条件如下：

当发现无人船进入到了以当前路径点[x_d(k),y_d(k)]为圆心，R₀为半径的圆时，判定为抵达目标航点；或者已经经过此航段已进入下一航段，但是未判别到进入上述圆中；目标航点切换为下一航点[x_d(k+1),y_d(k+1)]，当前航点号加一，此时目标航段变成[x_d(k),y_d(k)]→[x_d(k+1),y_d(k+1)]，期望的航向角变成

直到最后一个目标点；

当无人船航行的期望轨迹设计为曲线，此时将曲线按照切线进行分割，分成不同长度的线段，由此可对曲线进行跟踪；取曲线上一点作外切线，将一条曲线分成了n段直线段，此时根据船舶当前位置与期望航段的距离，以及航段的长度来调节视线半径R与切换半径R₀；

设船舶当前位置与期望航段的垂直距离为d，当目标航线未出现在无人船视线半径圆内，将此时的视线半径增大，令R'＝d，控制船舶向目标航线靠近，直到目标航线进入到视线圆内，此时视线半径等于初始设定值；当航段短于切换半径R₀时，将调节航段切换的条件，由于船舶惯性大的特点，切换半径最小值应不小于一个船长L，因此令R₀＝L。

7.一种计算机可读存储介质，存储有计算机程序，所述计算机程序被处理器执行时，使得所述处理器执行如权利要求1至6中任一项所述方法的步骤。

Images