CN107168329B - 基于跟随领航者法的航速航向协同控制编队航行控制方法 - Google Patents

Abstract

本发明公开了一种基于跟随领航者法的航速航向协同控制编队航行控制方法，与常规的编队航行控制方法相比，航速航向控制具有明确的控制任务分工，物理意义明确，克服了人工势场法不能解决队形保持的控制问题，具有很强的工程操作性和实施性，无需人工干预，可广泛用以解决于无人值守环境下的船舶编队航行控制问题，尤其适用于水面靶场、高危水域等场合的船舶编队航行需要，具体地通过在编队形成过程中加入速度和航向控制约束，适用于编队控制中的队形形成和队形保持控制，具有很强的工程操作性和实施性，无需人工干预，可广泛用以解决于无人值守环境下的船舶编队航行控制问题，尤其适用于水面靶场、高危水域等场合的船舶编队航行需要。

Description

基于跟随领航者法的航速航向协同控制编队航行控制方法

技术领域

本发明涉及一种基于跟随领航者法的航速航向协同控制编队航行控制方法，属于船舶自动控制技术领域。

背景技术

多智能体的编队协同控制问题，是机器人和船舶智能控制领域的研究热点。编队控制包括编队方法、队形形成控制和队形保持控制三方面内容。编队方法解决了多智能体以何种队形同步航行的问题。队形形成控制是将多只船从开始的随机位置形成规定的整体队形的过程。队形保持控制是整体队形形成后，如何实现队形不再错乱的控制过程。

目前，编队航行控制尚处于理论研究阶段，对于多机器人编队问题的研究主要有以下方法：Leader-follower法(跟随领航者法)、人工势函数法等。其中，Leader-follower法是一种编队方法，不解决如何形成编队的问题。人工势函数法主要用于解决队形形成控制，无法队形保持控制。

而常规的Leader-Follower法和人工势场法，从控制机理上决定了仅能实现船舶位置收敛控制，而无法在期望位置点上实现领航者和跟随者的航速航向一致性同步控制，导致无法以稳定编队状态同步航行，因此只能实现船舶编队的队形形成控制，而无法实现后续的队形保持控制，其控制过程与控制机理如图1所示。

如图1所示，人工势场法首先由Khatib提出，其基本思想是借鉴物理方面的概念，目标对船舶产生吸引力，从而引导船舶向目标运动。以跟随者R₂为例，在编队航行过程中，受到编队位置T₂的吸引，从而驱动船舶向T₂点驶去。由于驱动方向始终由船舶当前位置点R₂指向T₂，人工势场法可以实现控制船舶逼近T₂点，但未能提供有效的航速和航向约束，因此无法实现以特定速度和航向到达T₂点，也就无法实现后续的队形保持控制。另外，当船舶R₂到达目标点T₂附近时，由于持续的吸引力，将极可能导致船舶超越目标点，使之出现目标点位于船舶身后的情况，诱发船舶发生极大的转向动作或不可预知的航行动作，安全隐患很大。

综上所述，当前编队控制算法研究主要集中于编队方法和编队形成的算法，忽视了编队形成后的队形保持控制。人工势场法是一种十分有效的编队形成控制方法，但无法同步实现队形形成和队形保持两方面的控制需求。

发明内容

为解决现有技术的不足，本发明的目的在于提供一种基于跟随领航者法的航速航向协同控制编队航行控制方法，解决目前编队形成后的队形保持控制方法不完善，队形保持效果很差的问题。

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

一种基于跟随领航者法的航速航向协同控制编队航行控制方法，其特征是，所述船舶编队包括领航者和跟随者，领航者的速度和航向记为(V₁,ψ₁)，跟随者的速度和航向记为(V₂,ψ₂)，其中：

领航者航行控制方法包括如下步骤：

步骤1-1)设置领航者需要跟踪(T₁₁,T₁₂)航迹点序列，其中，T₁₁的坐标记为(x_d1,y_d1)，T₁₂的坐标记为(x_d2,y_d2)，并计算目标航线角ψ_d；

步骤1-2)计算领航者距离目标航线T₁₁-T₁₂的垂直距离d₁₁；

步骤1-3)通过GPS及航姿测量设备获取船舶的当前航向角ψ₁，通过航向角ψ₁与目标航线角ψ_d之间的偏差，计算出舵角控制回路的期望舵角δ_c；

步骤1-4)通过舵角传感器获取船舶的舵角信息，计算当前舵角值与步骤1-3)计算得到的期望舵角之间的偏差，再利用船舶上的舵角执行机构调整船舶舵的方位；

跟随者控制方法包括如下步骤：

步骤2-1)跟随者接收领航者的位置、航向角ψ₁和速度V₁信息；

步骤2-2)根据领航者的位置和跟随者自身的位置信息，根据编队队形形状计算跟随者距离期望位置T₂₁的横向距离误差d₂₁和纵向距离误差d₂₂，无需计算T₂₁的具体坐标值；

步骤2-3)计算跟随者的驱动速度V₂，其中，V₂是与纵向距离误差d₂₂和V₁有关的函数，当d₂₂→0时，V₂→V₁；

步骤2-4)计算跟随者的驱动航向ψ₂，其中，ψ₂是与横向距离误差d₂₁和ψ₁有关的函数，当d₂₁→0时，ψ₂→ψ₁；

步骤2-5)通过控制油门和舵角使d₂₂→0和d₂₁→0，跟随者的航速和航向将随之收敛于(V₁,ψ₁)，即跟随者的速度和航向与领航者保持一致，从而实现编队航行队形形成和保持控制。

进一步地，所述步骤1-1)中目标航线角ψ_d的计算公式为 其中，为目标航线角ψ_d的过渡计算变量，x_d1为目标点T₁₁的经度值，y_d1为目标点T₁₁的纬度值，x_d2为目标点T₁₂的经度值，y_d2为目标点T₁₂的纬度值，c为船舶目标点T₁₁、目标点T₁₂与地心所构成的地心角，cos(c)为其余弦值。

进一步地，所述步骤1-1)中需要根据船舶目标点T₁₂与T₁₁的相对位置关系进行象限转换，转换方式如下：

以目标点T₁₁为原点，建立直角坐标系，其中正北方(纬度方向)为纵轴，与纵轴垂直的正东方(经度方向)为横轴。

若目标点T₁₂位于该直角坐标系的第一象限，则

若目标点T₁₂位于该直角坐标系的第二象限，则

若目标点T₁₂位于该直角坐标系的第三四象限，则

进一步地，所述步骤1-3)中计算出舵角控制回路的期望舵角δ_c时利用S函数进行计算，其中S函数如下：其中，y为控制量，e为控制误差，k为曲线斜率，MAX代表控制最大值，其中k、MAX的取值可根据实际控制机构的性能以及经验值确定。

进一步地，所述步骤1-3)中期望舵角δ_c＝f₁+f₂，其中，f₁为主舵角控制量；f₂为航迹收敛控制舵角修正量，在航向控制的基础上，控制船舶向期望航线T₁₁-T₁₂收敛。

进一步地，所述步骤1-3)中f₁＝f(ψ_d-ψ₁)，采用经典PID控制方法进行计算。

进一步地，所述步骤1-3)中f₂采用S函数计算：

进一步地，所述步骤2-3)中跟随者的驱动速度d₂₂→0，Δ_v→0，V₂→V₁。

进一步地，所述步骤2-4)中跟随者的驱动航向d₂₁→0，Δ_ψ→0，ψ₂→ψ₁。

本发明所达到的有益效果：通过在编队形成过程中加入速度和航向控制约束，适用于编队控制中的队形形成和队形保持控制，具有很强的工程操作性和实施性，无需人工干预，可广泛用以解决于无人值守环境下的船舶编队航行控制问题，尤其适用于水面靶场、高危水域等场合的船舶编队航行需要。

附图说明

图1是现有技术中人工势场法的原理图；

图2是本发明的控制原理图；

图3是S函数不同斜率下的仿真结果；

图4是船舶编队控制工作原理图。

具体实施方式

下面结合附图对本发明作进一步描述。以下实施例仅用于更加清楚地说明本发明的技术方案，而不能以此来限制本发明的保护范围。

要实现船舶的稳定编队航行控制，无论采用哪种具体的控制方法，均需通过调节船舶的航速和航向，最终实现位置、航速、航向等多种状态的同步收敛控制。只有在同一时刻实现位置、航速和航向的同步收敛控制，才能实现多船由无绪状态到有绪状态，并保持有绪状态航行的控制过程。Leader-Follower法为例，在编队形成过程中，需要跟随者到达期望队形位置时，应具有与领航者相同的速度和航向，方能实现后续的队形保持控制。

针对当前主流编队控制方法所存在的使用局限性，即忽视了队形保持控制的重要性和实现方法，而且在编队航行过程中，队形保持一般是历时时间最长、最为重要的一种稳定航行状态。

编队航行控制的本质控制问题，是在同一时刻实现位置、航速和航向的同步收敛控制，基于此分析本发明将编队控制过程分解为速度控制回路和航向控制回路，位置纵向误差和航速调节由速度控制回路负责，位置侧向误差和航向调节由航向控制回路解决，以避免常规编队控制方法在队形保持过程中航向控制参与调节位置纵向误差的危险动作，导致无法实现队形保持甚至引发极大的转向动作或不可预知的航行动作等危险情况。

本发明专利是针对高速靶船的编队航行控制需求而提出的。在军用靶船领域，为逼真模拟实际战场环境的复杂电磁环境特性和被打击对象的运动特性，需要由目标靶船、辐射源船、干扰源船等组成靶船编队，以逼真模拟敌航母、驱逐舰、护卫舰等航母战斗群的编队航行运动状态，以及敌舰载雷达武器装备的辐射特性和干扰特性。其中，目标靶船为演练中被攻击摧毁的靶船，辐射源船和干扰源船为任务配试船只，搭载贵重的任务设备(如辐射源、干扰源、图像及通信设备等)，在靶试训练中重复使用，以降低靶试成本。同样地，本发明也适用于常规船舶、机器人等多智能体的编队协同控制问题，是一种通用的编队控制方法。

区别于常规的Leader-Follower法，领航者、跟随者各自向预定好的期望位置驱动控制，本发明将跟随者的期望位置根据领航者实时确定，从航行开始即让跟随者始终接受编队队形的约束作用，快速进入编队状态，并以预定队形的方式共同向目标点T₁₁前进。在实际工程应用中，目标点由一系列点组成，在不影响对专利内容理解的基础上，为表述方便，以单点代表目标点序列。

如图2所示，本案中，船舶编队由一个领航者R1、多个跟随者(R2、R3…,RN)组成，所有跟随者的控制方法步骤完成相同，在本发明中以单个跟随者R2表示。领航者的速度和航向记为(V₁,ψ₁)，跟随者的速度和航向记为(V₂,ψ₂)。对于每一个跟随者，将当前位置与目标位置的误差分解为位置纵向误差和位置侧向误差，第i个船舶的位置纵向误差为d_i2(i＝1,2,…,N)，位置侧向误差为d_i1(i＝1,2,…,N)，N为船舶总个数。在本权利要求中，对于第i个船舶的位置纵向误差为d_i2(i＝1,2,…,N)以及位置侧向误差为d_i1(i＝1,2,…,N)，以d₂₁和d₂₂为说明，也就是以第2个船舶进行了说明，对于之后的第n个船舶，其原理与第2个完全一样。

在本方案中，采用了一种斜率幅度可调的S函数，该函数可以限定控制边界并提供正确的趋势控制响应，所提出的S函数如下：其中，y为控制量，e为控制误差，k为曲线斜率，MAX代表控制最大值。

以MAX＝1为，k分别取0.01和0.1时的函数曲线如图3所示。

在进行具体控制时：

领航者可以固定速度V₁(V₁<V_max)航行，通过进行航向控制，实现航迹收敛，V_max表示领航者的限速上限，其航行控制方法包括以下计算步骤：

步骤1-1)假设领航者需要跟踪(T₁₁,T₁₂)航迹点序列，T₁₁的坐标记为(x_d1,y_d1)，T₁₂的坐标记为(x_d2,y_d2)，首先计算目标航线角ψ_d：

计算公式为： 其中，x_d1为目标点T₁₁的经度值，y_d1为目标点T₁₁的纬度值；x_d2为目标点T₁₂的经度值，y_d2为目标点T₁₂的纬度值，c为船舶目标点T₁₁、目标点T₁₂与地心所构成的地心角；为目标航线角ψ_d的过渡计算变量，还需要根据船舶目标点T₁₂与T₁₁的相对位置关系进行象限转换：

以目标点T₁₁为原点，建立直角坐标系，其中正北方(纬度方向)为纵轴，与纵轴垂直的正东方(经度方向)为横轴。

若目标点T₁₂位于该直角坐标系的第一象限，则

若目标点T₁₂位于该直角坐标系的第二象限，则

若目标点T₁₂位于该直角坐标系的第三四象限，则

步骤1-2)：计算领航者R1距离目标航线T₁₁-T₁₂的垂直距离d₁₁，该计算方法为常规算法，比如直接通过坐标进行计算；

步骤1-3)：通过GPS及航姿测量设备获取船舶的当前航向角ψ₁，通过航向角与目标航线角ψ_d之间的偏差，并利用S函数计算出舵角控制回路的期望舵角δ_c；其中，f₁为主舵角控制量，可选用经典PID控制方法或其它类似控制方法；f₂为航迹收敛控制舵角修正量，在航向控制的基础上，控制船舶向期望航线T₁₁-T₁₂收敛，f₂采用S函数计算；

步骤1-4)通过舵角传感器获取船舶的舵角信息，计算当前舵角值与步骤1-3)计算得到的期望舵角之间的偏差，再利用船舶上的舵角执行机构调整船舶舵的方位。

以跟随者R2为例，通过调节速度V₂和ψ₂，使其位置点收敛至T₂₁，同时将速度V₂和航向ψ₂分别收敛至(V₁,ψ₁)，跟随者控制方法包括以下计算步骤：

步骤2-1)跟随者R2接收领航者R1的位置、航向ψ₁和速度V₁信息；

步骤2-2)根据领航者R1的位置和跟随者R2自身的位置信息，计算跟随者R2距离期望位置T₂₁的横向距离误差d₂₁和纵向距离误差d₂₂，具体计算均为简单的几何运算，此处不再赘述；

步骤2-3)计算跟随者R2的驱动速度，如下：d₂₂→0，Δ_v→0，V₂→V₁；

步骤2-4)计算跟随者R2的驱动航向，如下：d₂₁→0，Δ_ψ→0，ψ₂→ψ₁。

步骤2-5)通过控制油门和舵角使跟随者R2的航速和航向分别收敛于(V₁,ψ₁)。

以上公式中，→表示收敛的意思，在船舶控制领域，属于通用属于，进一步地指代将对应的航向角或者速度向某一值趋近直至一样或者在误差范围内。

如图4所示，为实现本发明所提供的编队航行控制算法，需要领航者与跟随者之间具备无线通信能力，领航者向跟随者发送领航者位置、参考速度和参考航向等信息。无论对于领航者还是跟随者，一种典型但不限于此的控制系统配置包括无线通信电台、航行控制计算机、航向航速控制板、发动机、舵角执行机构、GPS及航姿测量设备、舵以及舵角传感器。

领航者R1之航行控制计算机通过无线通信电台接收期望航迹点序列和航速等远程控制指令，以实现指定速度和航迹领队航行；

跟随者R2之航行控制计算机通过无线通信电台接收领航者R1的当前位置、参考速度和参考航向，实时计算驱动速度和驱动航向，控制跟随者以预设队形与领航者同步航行；跟随者R3同理控制；

航行控制计算机通过RS232/RS422串口接收GPS及航姿测量设备输出的船舶位置及航向信息，并进行航向控制回路解算和航速控制回路解算；

航速航向控制板通过网络接收航行控制机输出的航速和航向指令，并采集舵角反馈信息，计算当前舵角值与计算得到的期望舵角之间的偏差，解算后通过控制总线输出油门模拟电压和启动指令至发动机，并发送舵角执行指令至舵角执行机构，由舵角执行机构控制舵的方向。

以上所述仅是本发明的优选实施方式，应当指出，对于本技术领域的普通技术人员来说，在不脱离本发明技术原理的前提下，还可以做出若干改进和变形，这些改进和变形也应视为本发明的保护范围。

Claims (7)

1.一种基于跟随领航者法的航速航向协同控制编队航行控制方法，其特征是，船舶编队包括领航者和跟随者，领航者的速度和航向记为(V₁,ψ₁)，跟随者的速度和航向记为(V₂,ψ₂)，其中：

领航者的航行控制方法包括如下步骤：

步骤1-1)设置领航者需要跟踪航迹点序列(T₁₁,T₁₂)，航迹点序列中的点T₁₁，T₁₂为船舶的目标点，其中，T₁₁的坐标记为(x_d1,y_d1)，T₁₂的坐标记为(x_d2,y_d2)，并计算目标航线角ψ_d；

步骤1-2)计算领航者距离目标航线T₁₁-T₁₂的垂直距离d₁₁；

步骤1-3)获取船舶的当前航向角ψ₁，通过航向角ψ₁与目标航线角ψ_d之间的偏差，计算出舵角控制回路的期望舵角δ_c；

步骤1-4)通过舵角传感器获取船舶的舵角信息，计算当前舵角值与步骤1-3)计算得到的期望舵角之间的偏差，再利用船舶上的舵角执行机构调整船舶舵的方位；

跟随者控制方法包括如下步骤：

步骤2-1)跟随者接收领航者的位置、航向角ψ₁和速度V₁信息；

步骤2-2)根据领航者的位置和跟随者自身的位置信息，根据编队队形形状计算跟随者距离期望位置T₂₁的横向距离误差d₂₁和纵向距离误差d₂₂，无需计算T₂₁的具体坐标值；

步骤2-3)计算跟随者的驱动速度V₂，其中，V₂是与纵向距离误差d₂₂和V₁有关的函数，当d₂₂→0时，V₂→V₁；

步骤2-4)计算跟随者的驱动航向ψ₂，其中，ψ₂是与横向距离误差d₂₁和ψ₁有关的函数，当d₂₁→0时，ψ₂→ψ₁；

步骤2-5)通过控制油门和舵角使d₂₂→0和d₂₁→0，跟随者的航速和航向将随之收敛于(V₁,ψ₁)，即跟随者的速度和航向与领航者保持一致，从而实现编队航行队形形成和保持控制；

所述步骤2-3)中跟随者的驱动速度 驱动d₂₂→0，Δ_v→0使得V₂→V₁；

所述步骤2-4)中跟随者的驱动航向 驱动d₂₁→0，Δ_ψ→0使得ψ₂→ψ₁；

k为曲线斜率，MAX代表控制最大值，其中k、MAX的取值可根据实际控制机构的性能以及经验值确定。

2.根据权利要求1所述的一种基于跟随领航者法的航速航向协同控制编队航行控制方法，其特征是，所述步骤1-1)中目标航线角ψ_d的过渡计算变量的计算公式为 其中，c为船舶目标点T₁₁、目标点T₁₂与地心所构成的地心角，cos(c)为其余弦值。

3.根据权利要求2所述的一种基于跟随领航者法的航速航向协同控制编队航行控制方法，其特征是，所述步骤1-1)中需要根据船舶目标点T₁₂与T₁₁的相对位置关系进行象限转换，转换方式如下：

以目标点T₁₁为原点，建立直角坐标系，其中纬度方向为纵轴，经度方向为横轴；

若目标点T₁₂位于该直角坐标系的第一象限，则

若目标点T₁₂位于该直角坐标系的第二象限，则

若目标点T₁₂位于该直角坐标系的第三四象限，则

4.根据权利要求1所述的一种基于跟随领航者法的航速航向协同控制编队航行控制方法，其特征是，所述步骤1-3)中计算出舵角控制回路的期望舵角δ_c时利用S函数进行计算，其中S函数如下：其中，y为控制量，e为控制误差，k为曲线斜率，MAX代表控制最大值，其中k、MAX的取值可根据实际控制机构的性能以及经验值确定。

5.根据权利要求4所述的一种基于跟随领航者法的航速航向协同控制编队航行控制方法，其特征是，所述步骤1-3)中期望舵角δ_c＝f₁+f₂，其中，f₁为主舵角控制量；f₂为航迹收敛控制舵角修正量，在航向控制的基础上，控制船舶向期望航线T₁₁-T₁₂收敛。

6.根据权利要求5所述的一种基于跟随领航者法的航速航向协同控制编队航行控制方法，其特征是，所述步骤1-3)中f₁＝f(ψ_d-ψ₁)，采用经典PID控制方法进行计算。

7.根据权利要求5所述的一种基于跟随领航者法的航速航向协同控制编队航行控制方法，其特征是，所述步骤1-3)中f₂采用S函数计算：