CN114035567A - 一种水面无人艇航控系统 - Google Patents

Abstract

本发明公开了一种水面无人艇航控系统，属于无人艇航控技术领域，解决仅涉及单项技术的控制系统的无人艇在执行任务时存在局限性问题。本发明的一种水面无人艇航控系统包括：路径跟踪模块、虚拟引导模块、动力定位模块和底层控制器；路径跟踪模块用于根据接收到的任务路径，生成航行规划路径信息发送给底层控制器；虚拟引导模块根据任务移动引导点，生成航行规划移动引导信息发送给底层控制器；动力定位模块根据任务固定定位点，生成航行规划动力定位信息发送给底层控制器；底层控制器用于根据路径跟踪模块、虚拟引导模块和动力定位模块，结合当前位姿信息获取期望舵角和期望油门，对无人艇进行控制。本发明适用于欠驱动无人艇的自主控制系统。

Description

一种水面无人艇航控系统

技术领域

本申请涉及无人艇航控技术领域，尤其涉及一种水面无人艇航控系统。

背景技术

当前的相关技术，均只考虑路径跟踪，动力定位，虚拟引导这三大基本任务的其中一种，忽略了三者的系统性。随着自主技术的提升和广泛应用，无人艇被要求在越来越复杂的任务场景中执行任务。然而，相关的论文及专利往往仅针对上述三大基本任务中的一种进行分析和讨论，仅涉及单项技术的控制系统毫无疑问的使得无人艇在执行任务时存在局限性。

发明内容

针对此问题，为设计一个包含上述三大基本控制技术的控制系统，以此提升无人艇的自主能力，本发明提供了一种水面无人艇航控系统。

一种水面无人艇航控系统，所述系统包括：路径跟踪模块、虚拟引导模块、动力定位模块和底层控制器；

所述路径跟踪模块用于根据接收到的任务路径，生成航行规划路径信息发送给底层控制器，所述航行规划路径信息包括所述路径跟踪模块的期望艏向和期望航速；

所述虚拟引导模块根据任务移动引导点，生成航行规划移动引导信息发送给底层控制器，所述航行规划移动引导信息包括所述虚拟引导模块的期望艏向和期望航速；

所述动力定位模块根据任务固定定位点，生成航行规划动力定位信息发送给底层控制器，所述航行规划动力定位信息包括所述动力定位模块的期望舵角和期望油门；

所述底层控制器用于根据所述路径跟踪模块、所述虚拟引导模块和所述动力定位模块，并结合当前位姿信息获取期望舵角和期望油门，进而对无人艇进行控制；

所述底层控制器包括艏向控制器和航速控制器；

所述艏向控制器根据期望艏向和当前艏向，获取期望舵角，并根据所述期望舵角对无人艇进行艏向的控制；

所述航速控制器根据期望航速和当前航速，获取期望油门，并根据所述期望油门对无人艇进行航速的控制。

进一步地，所述路径跟踪模块包括制导律计算模块、干扰观察器、越位判断模块、缓冲带模块和艏向自适应状态引导模块；

所述制导律计算模块将跟踪点的概念改为跟踪面，并根据根据横侧偏差的大小调整收敛速度，提出基于自适应边界层的制导律；

所述干扰观察器将艏向侧滑角的偏置为一种不确定干扰，同时集总艏向跟踪误差，对该集总的不确定性进行实时的估计；

越位判断模块作为补充，根据期望目标点与无人艇组成的向量和期望路径向量的相对角关系，决定是否换点；

缓冲带用于提高路径切换时的跟踪精度，具体为：

若无人艇投影还未进入期望路径，或者已进入期望路径但还处在前置缓冲带内，则采用前置缓冲速度；

若无人艇的投影已超出期望路径，或者已进入期望路径且处在终端缓冲带，则采用终端缓冲速度；

艏向自适应状态引导模块用于保证无人艇能够时刻航行在期望路径上，以无人艇艏向作为航行趋向参考值，以横侧偏差作为精度参考值，在牺牲航速的前提下以提高航行时的安全性。

进一步地，所述虚拟引导模块包括制导控制模块和层级航速规划器模块；

所述制导控制模块根据目标艏向、无人艇与目标相对艏向和无人艇至目标航线距离，获取制导律；

所述层级航速规划器模块根据无人艇至目标距离，获取指令航速与期望航速的关系。

进一步地，所述动力定位模块包括位姿保持器、环境力预测模块、环境力前馈控制模块和推力分配模块；

所述位姿保持器采取比例-微分控制；

所述环境力预测模块根据风速，风向，波幅，波长，波向获取风力系数和浪系数；

所述环境力前馈控制模块所述风力系数和所述浪系数，通过环境力前馈控制器结算抗干扰控制输出；

所述推力分配模块在期望的控制力与可达控制力之间的误差最小化的基础上，使得各执行器均使用最小控制力。

进一步地，所述制导律计算模块用于计算直线制导律和圆弧制导律；

所述直线制导律为：

式中，ψ_d为期望艏向，φ_p为上一个期望路径点和当前期望路径点的连线于正北坐标轴的夹角，k_i＞0(i＝1,2,3)为控制参数，δ_l表示直线跟踪横侧偏差：

δ_l＝-(n_t-n_k)sinφ_p+(e_t-e_k)cosφ_p

式中，p_t＝(n_t,e_t)表示当前无人艇的位置，p_k＝(n_k,e_k)表示期望直线路径上的起点，另外，φ_p的表达式为：

φ_p＝atan2(n_k+1-n_k,e_k+1-e_k)

式中，p_k+1＝(n_k+1,e_k+1)表示期望直线路径上的终点；

所述圆弧制导律为：

式中，k_i＞0(i＝4,5,6)为控制参数，tan(π²/4k₄)/k₅＞δ^*，其中δ^*≥_sup|δ_c|为最大横侧偏差，有圆弧跟踪横侧偏差为：

式中，p_c＝(n_c,e_c)表示期望圆弧路径的圆心，r表示期望圆弧路径的半径，φ_c为期望路径的圆心和无人艇当前位置的连线与正北坐标轴的夹角，φ_c的表达式为：

φ_c＝atan2(n_t-n_k,e_t-e_k)。

进一步地，所述制导控制模块的制导律为：

式中，_supδ为正向控制参数，

为目标艏向，ψ_p为无人艇与目标相对艏向，δ为无人艇至目标航线距离。

进一步地，所述层级航速规划器模块，具体包括：

根据无人艇至目标距离：

获取指令航速与期望航速的关系为：

式中，ε_i,i＝1,2为航速调整参数，l_i,i＝1,2为层级距离，u_d为期望航速，u_user为指令航速。

进一步地，所述位姿保持器采取比例-微分控制：

式中，τ为解算出来的控制力矩，K₁,K₂为正定对角矩阵控制参数，η_t＝η-η_d为位姿误差，其中η＝[n,e,ψ]^T为在大地坐标系下的当前位姿，η_d＝[n_d,e_d,ψ_d]^T为在大地坐标系下的期望定位点，J(η)为旋转矩阵，具体公式有：

进一步地，所述艏向控制器通过对比期望艏摇速度和当前艏摇速度，引入S面函数对比例项进行修正，得到：

式中，λ＞0表示最大修正幅值，

表示艏摇误差，

为艏向误差，关于期望艏摇速度 r_d，采用线性函数进行预测：

式中，ω＞0表示最大期望艏摇幅值。

进一步地，所述航速控制器根据指令速度查表，有：

若

则令i＝j

式中，j表示搜索指针，i为指令航速对应表中位置，

表示航速表，u_d表示期望航速。

通过插值法获得基础油门量P_base：

以航速和航速误差为参考值，通过积分S面函数，加之上述基础油门量，给出以下航速控制器：

式中，k₅为比例项，k_i,i＝1,2,3为正向控制参数，

为航速一阶导，

为航速误差：

本发明的有益效果：首先，本发明分别对路径跟踪模块、虚拟引导模块和动力定位模块进行了设计，并将底层控制器根据这三个模块进行相应设计，使底层控制器可以根据这三个模块对无人艇进行控制；其次，本申请分别对路径跟踪模块、虚拟引导模块和动力定位模块进行了改进，使得对无人艇的控制更加精确。

本发明适用于欠驱动无人艇的自主控制系统。

附图说明

为了更清楚地说明本申请的技术方案，下面将对实施例中所需要使用的附图作简单地介绍，显而易见地，对于本领域普通技术人员而言，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。

图1为一种水面无人艇航控系统结构示意图；

图2为路径跟踪模块的原理图；

图3为安全阀的流程图；

图4为虚拟引导模块的原理图；

图5为虚拟点跟踪原理示意图；

图6为动力定位模块的原理图；

图7为推力输出逻辑图；

图8为艏向控制流程图；

图9为航速控制流程图；

图10为工况一跟踪路径结果图；

图11为工况一跟踪误差结果图。

具体实施方式

图1所示，一种水面无人艇航控系统，该系统包括：路径跟踪模块、虚拟引导模块、动力定位模块和底层控制器；

所述路径跟踪模块用于根据接收到的任务路径，生成航行规划路径信息发送给底层控制器，所述航行规划路径信息包括所述路径跟踪模块的期望艏向和期望航速；

所述虚拟引导模块根据任务移动引导点，生成航行规划移动引导信息发送给底层控制器，所述航行规划移动引导信息包括所述虚拟引导模块的期望艏向和期望航速；

所述动力定位模块根据任务固定定位点，生成航行规划动力定位信息发送给底层控制器，所述航行规划动力定位信息包括所述动力定位模块的期望舵角和期望油门；

所述底层控制器用于根据所述路径跟踪模块、所述虚拟引导模块和所述动力定位模块，并结合当前位姿信息获取期望舵角和期望油门，进而对无人艇进行控制；

所述底层控制器包括艏向控制器和航速控制器；

所述艏向控制器根据期望艏向和当前艏向，获取期望舵角，并根据所述期望舵角对无人艇进行艏向的控制；

所述航速控制器根据期望航速和当前航速，获取期望油门，并根据所述期望油门对无人艇进行航速的控制。

本发明分别对路径跟踪模块、虚拟引导模块和动力定位模块进行了改进，使得对无人艇的控制更加精确。

所述路径跟踪模块包括制导律计算模块、干扰观察器、越位判断模块、缓冲带模块和艏向自适应状态引导模块，具体包括：

路径跟踪，即任务下达一条路径，要求无人艇稳定航行在路径上。在实际工程中，为实现欠驱动无人艇的路径跟踪控制，往往使用基于视线法的制导律，加之PID控制作为底层控制律。这种传统的控制算法易受洋流、波浪和风等海洋环境扰动的影响，从而产生跟踪偏差和振荡问题。由于问题的根源来自视线法本身所产生的期望角度在受到干扰后无法于期望航线对齐，上述的问题不能简单地通过在艏向控制器中添加积分项来解决。另一方面，当无人艇在航行时受到的漂移力能够很明显的表现在速度的变化上，在路径跟踪时，该影响进而体现在航向角与艏向角的差值，即侧滑角ψ_b。由于传统视线法往往忽略该侧滑角的影响，因此其横测偏差的稳定误差往往随着航速的提高而增大。为解决该问题，大多偏于实践应用的论文以航向角为跟踪目标去设计制导律。上述研究往往任务航向控制器具备理论性能，即航向跟踪误差始终为0，数学表述为

然而在复杂海洋环境下的航向控制器往往难以实现这一理论性能。在高速航行时，该略微的航向偏差会产生较大的精度影响。同时若根据几何关系直接采用ψ_b＝atan2(v/u)作为侧滑角，式中的速度均为上一时刻的值，因此无法满足实时性要求。针对上述问题，本发明提出一种结合视线法和向量场理论的制导律，即自适应边界层，将跟踪点的概念改变成跟踪面，以削弱无人艇在受到干扰时引起的振荡，优化制导律输出的期望艏向的平滑程度。同时，本发明将艏向侧滑角的偏置作为一种不确定干扰，同时集总跟踪误差，对集总的不确定性进行实时估计。

路径跟踪原理如图2所示，图中点池轮询器用于期望点轮询的相关操作，安全阀用于判断当前无人艇是否到达容许误差内，进而判断是否进行换点；越位判断作为补充，根据期望目标点与无人艇组成的向量和期望路径向量的相对角关系，决定是否换点：

所述制导律计算模块将跟踪点的概念改为跟踪面，并根据根据横侧偏差的大小调整收敛速度，提出基于自适应边界层的制导律，具体为：

基于传统视线法的路径跟踪易受洋流、波浪和风等环境扰动的影响。特别是当无人艇受到环境扰动时，产生的路径偏差和振荡问题。这个缺点不能通过简单地在艏向控制器上添加积分动作来解决，因为问题的根源来自视线法本身所产生的期望角度在受到干扰后无法于航线方向对齐。因此为提高视线法的抗干扰能力，本节结合向量场理论，将跟踪点的概念改变成跟踪面，以削弱无人艇在受到干扰时引起的振荡，优化期望艏向平滑程度，同时根据横侧偏差的大小调整收敛速度以权衡收敛速度和控制精度之间的取舍，提出基于自适应边界层的制导律。有直线制导律为：

式中，ψ_d为期望艏向，φ_p为上一个期望路径点和当前期望路径点的连线于正北坐标轴的夹角，k_i＞0(i＝1,2,3)为控制参数。δ_l表示直线跟踪横侧偏差：

δ_l＝-(n_t-n_k)sinφ_p+(e_t-e_k)cosφ_p (2)

式中，p_t＝(n_t,e_t)表示当前无人艇的位置，p_k＝(n_k,e_k)表示期望直线路径上的起点，另外，φ_p为上一个期望点和当前坐标点的连线与正北坐标轴的夹角，具体表达式为：

φ_p＝atan2(n_k+1-n_k,e_k+1-e_k) (3)

式中，p_k+1＝(n_k+1,e_k+1)表示期望直线路径上的终点。

有圆弧制导律为：

式中，ψ_d为期望艏向，φ_c为期望圆心和无人艇当前位置的连线于正北坐标轴的夹角， k_i＞0(i＝4,5,6)为控制参数，tan(π²/4k₄)/k₅＞δ^*，其中δ^*≥_sup|δ_c为最大横侧偏差。有圆弧跟踪横侧偏差为：

式中，p_c＝(n_c,e_c)表示期望圆弧路径的圆心，r表示期望圆弧路径的半径。φ_c为期望路径的圆心和无人艇当前位置的连线与正北坐标轴的夹角，类似于公式(3)地，有其具体表达式为φ_c＝atan2(n_t-n_k,e_t-e_k)

所述干扰观察器将艏向侧滑角的偏置为一种不确定干扰，同时集总艏向跟踪误差，对该集总的不确定性进行实时的估计，具体为：

根据牛顿第二定律可知，无人艇在航行时受到的漂移力能够很明显的表现在速度的变化上，在路径跟踪时，该影响进而体现在航向角与艏向角的差值，即侧滑角ψ_b。由于传统的视线法往往忽略该侧滑角的影响，因此其横侧偏差的收敛易受外界干扰的影响。为解决该问题，大多偏于实践应用的论文采用航向角作为跟踪目标去设计制导律。上述研究往往认为航向控制器具备理论性能，即

然而无人艇在复杂海洋环境下的航向控制器不可能实时满足该理论性能。在高速航行时，该略微的航向偏差会产生较大的精度影响，这是无法从控制器解决的问题。同时若根据几何关系直接采用ψ_b＝atan2(v/u)作为侧滑角，式中的速度均为上一时刻的值，无法满足实时性要求。因此本文考虑艏向侧滑角的偏置为一种不确定干扰，同时集总艏向跟踪误差，对该集总的不确定性进行实时的估计：

直线干扰观测器：为给出简洁观测器公式，介绍辅助变量σ₁及非奇异性滑模σ₂

式中，f₁,p,q均为正奇整数，满足p＞q。

在此基础，我们给出非奇异滑模观测器：

式中，f₂,γ为正常数，其中0＜γ＜1。

表示对不确定项一阶导的估计值，

表示关于

的非线性函数，有：

有对不确定项一阶导W的自适应律：

其中，f₃为正常数。

圆弧跟踪干扰观测器：考虑不确定项观测器的辅助变量及其滑模面为：

式中，f₁ ^*,p^*,q^*均为正奇整数，满足p^*＞q^*。

在此基础，我们给出类似于直线制导律中采用的非奇异滑模观测器：

式中，

为正常数，其中0＜γ^*＜1。由于不确定项一阶导为未知变量，并非是人为给定参数，因此这里仍采用W表示。同时

仍表示对W的估计值，

仍表示关于

的相关非线性函数，有：

式中，f₃ ^*为正常数。

安全阀：安全阀流程如图3所示，其中，假设当前无人艇在NED坐标中的当前位置为p_t，上一时刻位置为p_t-1，跟踪的期望目标点为p₁，上一个期望目标点为p₀。

越位判断模块作为补充，根据期望目标点与无人艇组成的向量和期望路径向量的相对角关系，决定是否换点，具体为：

越位判断：假设当前无人艇在NED坐标中的当前位置为p_t，跟踪的期望目标点为p_k+1，上一个期望目标点为p_k。通常情况下无人艇到达容许误差内进行换点的依据有：

如果

可得结论p_k+1,p_k＝p_k+2,p_k+1 (13)

式中，R_safe表示容许误差。

考虑无人艇在切换路径时由于航速与航向直接的耦合产生侧漂的问题，提出无人艇是否越位的可补充判据，以提高航行时的安全性。无人艇的投影在当前期望航线的延长线上，且以超出期望目标点时换点。有无人艇的投影：

式中，

为无人艇投影在大地坐标系下的位置。在获得投影坐标后，根据横侧偏差δ的特性我们可得：

正常情况下采用第一种方式即可，然而意外情况有可能导致无人艇无法到达容许误差内，从而产生的无人艇在期望目标点周围绕圈，亦或者不期望的掉头现象，因此可补充越位判断以避免上述情况的发生。

缓冲带用于提高路径切换时的跟踪精度，具体为：

若无人艇投影还未进入期望路径，或者已进入期望路径但还处在前置缓冲带内，则采用前置缓冲速度；

若无人艇的投影已超出期望路径，或者已进入期望路径且处在终端缓冲带，则采用终端缓冲速度；

缓冲带进一步具体包括：

缓冲带：为提高路径切换时的跟踪精度，本文提出缓冲带的概念，根据公式(14)中的两向量是否共向作为判断依据：

若无人艇在前置缓冲带，则有：

式中，l₁为前置缓冲带的长度。该式表示若无人艇投影还未进入期望路径，或者已进入期望路径但还处在前置缓冲带内，则采用前置缓冲速度u_fop。

若无人艇在终端缓冲带，则有：

式中，l₂为终端缓冲带的长度。该式表示若无人艇的投影已超出期望路径，或者已进入期望路径且处在终端缓冲带，则采用终端缓冲速度u_fop。

艏向自适应状态引导模块用于保证无人艇能够时刻航行在期望路径上，以无人艇艏向作为航行趋向参考值，以横侧偏差作为精度参考值，在牺牲航速的前提下以提高航行时的安全性，具体为：

艏向自适应状态引导：为保证无人艇能够时刻航行在期望路径上，不过大偏离路径，通过自适应状态引导，以无人艇航向作为航行趋向参考值，以横侧偏差作为精度参考值，在牺牲航速的前提下以提高航行时的安全性：

在直线跟踪时：

表示直线跟踪时的航向误差：

式中，φ_p表示期望直线与N轴的夹角，p_k＝(n_k,e_k)表示期望直线路径的起点，p_k+1＝(n_k+1,e_k+1) 表示期望直线路径上的终点。

δ_l表示直线跟踪横侧偏差有：

δ_l＝-(n_t-n_k)sinφ_p+(e_t-e_k)cosφ_p (20)

式中，p_t＝(n_t,e_t)表示当前无人艇的位置。

只有在无人艇航向和横侧偏差均在期望范围内时，才以指令航速进行航行，否则以最优姿态调整航度进行航行：

如果

且(|δ_l|＜δ_ld)，则u＝u_d。否则，u＝u_op (21)

式中φ_d,δ_ld为正向控制参数，分别表示期望航向误差范围和期望横侧偏差范围，u_op为最优姿态调整航速，u_d为指令航速。

在圆弧跟踪时：可得δ_c圆弧跟踪横侧偏差：

其中，p_c＝(n_c,e_c)表示期望圆弧路径的圆心，r表示期望圆弧路径的半径。

在圆弧跟踪时，由于期望艏向时刻变化，难以作为参考值，因此仅以横侧偏差作为参考。

如果((|δ_c|＜δ_max))且(|δ_c|＞δ_min)，则u＝u_op。否则，u＝u_d。 (23)

式中，δ_max,δ_min为正向控制参数，该判据相当于在圆弧轨道上添置缓速带。

性能描述：

①对于Z型折线航路的转折处应该有多种处理方案，可以在不同情况下选择不同的通过方式，保证整体的跟踪精度。针对Z型转折切换有以下几种方式。第一种：在路径跟踪开始时，以最优跟踪速度对艏向和位姿进行调整，待调整完毕后以指令速度进行跟踪。第二种：调整容许误差，使用指令速度进行跟踪。第三种：每次切换路径前后，以最优跟踪速度进行转艏，中间部分采取指令速度跟踪。第四种：实时监测无人艇与跟踪路径的相对位置，当超出可容许偏差时，转入艏向位姿调整模式，待调整完毕，再切换成正常跟踪模式。第五种：对每段路径采取分段处理，根据情况采用对应的最优航速层级化跟踪。

②对于圆弧航线的环绕方向、环绕次数、圆心角大小参数应该能够在线配置。在已设定任务速度的前提下能够自动调整无人艇局部航速，以保证路径跟踪精度。具有安全航行策略，航行过程中能够不超出设定的最大允许路径跟踪误差范围。三级及以下海况下路径跟踪平均误差≤5m。

③针对安全航行策略，航行过程中能够不超出设定的最大允许路径跟踪误差范围的问题和时间约束功能，能够指定路径中特定航路点的到达时间的问题，上述路径跟踪实现中已加入考虑，但在试验过程中未实施过。

航速优化针对时间限制，解算处总航程和指定到达时间对应的所需航速，再与指令航速以及缓冲带，航向位姿势修正器共同决定期望航速，在指令航速进行跟踪的前提下，给出以下几种可补充的优化方式，①在切换路径前(后，前后)一段距离内采取最优跟踪速度以保证换点时的跟踪精度。②根据限定时间和剩余航程解算出补偿航速。③判断当前横侧偏差与航行趋势判断是否以慢速优先调整艏向位姿。④是否在每段路径起始阶段以慢速调整艏向和位姿进入期望航程。

所述虚拟引导模块包括制导控制模块和层级航速规划器模块，具体包括：

虚拟引导，即任务下达一个移动引导点，要求无人艇和引导点保持给定的相对方位角和相对距离。针对这一任务，本发明同样采用自适应边界层的路径跟踪制导律解算期望艏向，以期实现相对方位角的稳定保持。同时设计层级航速规划器，以期获得相对距离的稳定保持。在这种设计下，仅层级航速规划器是额外设计的算法。另一方面，当对制导律的设计不满意时，可选择分别修改，亦或者仅修改路径跟踪制导律以实现控制系统的优化。这种模块化的设计赋予整个控制系统轻便性和易修改的优点。

虚拟引导跟踪原理如图4所示，图中障碍物过滤器用于过滤无关障碍物信息，层级航速规划器用于保证无人艇与虚拟点的距离：

所述制导控制模块根据目标艏向、无人艇与目标相对艏向和无人艇至目标航线距离，获取制导律，具体为：

制导控制：虚拟点跟踪原理如图5所示，假设目标当前时刻位置为

目标上一时刻位置为

无人艇当前时刻位置为p_t(n_t,e_t)，无人艇上一时刻位置为 p_t-1(n_t-1,e_t-1)。有目标航向

有无人艇与目标相对航向ψ_p：

有无人艇至目标航线距离δ：

根据上述所求变量，给出制导律：

式中，ψ_d为期望艏向，_supδ为正向控制参数，

为目标航向，ψ_p为无人艇与目标相对航向，δ为无人艇至目标航线距离。

所述层级航速规划器模块根据无人艇至目标距离，获取指令航速与期望航速的关系，具体为：

层级航速规划器：值得注意的是，在航速控制环中存在层级航速规划器，因此期望航速 u_d不同于指令航速u_user。有无人艇至目标距离：

从而得指令航速与期望航速的关系为：

式中，ε_i,i＝1,2为航速调整参数，l_i,i＝1,2为层级距离。

所述动力定位模块包括位姿保持器、环境力预测模块、环境力前馈控制模块和推力分配模块，具体包括：

动力定位，即任务下达一个固定定位点，要求无人艇始终以一个给定期望艏向保持在该定位点。在工程试验中，欠驱无人艇的横向位置控制权重相对较低，这往往导致所需的控制力和力矩向调节横测偏差倾斜。欠驱动执行器的配置往往难以满足这一倾斜，从而影响动力定位的控制性能。从外场试验得出，积分项的实现能减小稳定误差，但对减轻横测偏差的作用不大，因此在位姿保持控制中，本发明设计比例-微分的位姿控制。同时，进行环境力预测，据此给出环境力前馈补偿信号。

动力定位原理如图6所示，图中环境力预测器用于进行环境干扰力预测，环境力前馈控制器用于输出抗干扰力，位姿保持器用于输出控制力。

所述位姿保持器采取比例-微分控制，具体为：

位姿保持器：为便于工程实现同时考虑到双体艇的横向位置控制权重相对较低，这往往导致所需的控制力和力矩向调节横侧偏差倾斜，从而导致执行器的配置难以实现，从而影响整体的动力定位性能。从外场试验得出，由于积分项的实现加大了控制输出，但对减轻横侧偏差的作用不大，因此位姿保持器仅采取比例-微分控制：

式中，τ为解算出来的控制力矩，K₁,K₂为正定对角矩阵控制参数。η_t＝η-η_d为位姿误差，其中η＝[n,e,ψ]^T为在大地坐标系下的当前位姿，η_d＝[n_d,e_d,ψ_d]^T为在大地坐标系下的期望定位点。J(η)为旋转矩阵，具体公式有：

所述环境力预测模块根据风速，风向，波幅，波长，波向获取风力系数和浪系数，具体为：

环境力预测：根据风速V_rw，风向γ_rw，波幅ω_w，波长λ_w，波向γ_w解算分别计算风力系数和浪系数。首先计算攻角

及表面风速

式中，u_rw＝V_rwcos(γ_rw-ψ)为相对纵向风速,v_rw＝V_rwsin(γ_rw-ψ)为相对横向风速，ψ为无人艇当前艏向。考虑到动力定位时，无人艇处于静态，有航速u,v→0。风动压q_rw为：

式中，ρ_a＝1.2为空气密度，V_rw为相对风速。风力系数C...(·)为：

式中，c_x,c_y,c_z为预测系数。浪动压为：

式中，ρ＝1.025表示海水密度，g＝9.8表示重力加速度，ω_w表示平均波幅。波浪力系数 D_...(·)为：

式中，λ_w表示波长，L表示船长。

所述环境力前馈控制模块所述风力系数和所述浪系数，通过环境力前馈控制器结算抗干扰控制输出，具体为：

环境力前馈控制：根据环境力预测所得的风浪系数，通过环境力前馈控制器结算抗干扰控制输出

式中，A_Fw和A_Lw分别表示正面和侧面的投影风区，L表示船长。

所述推力分配模块在期望的控制力与可达控制力之间的误差最小化的基础上，使得各执行器均使用最小控制力，具体为：

推力分配：具体推力输出逻辑如图7所示，为提高能源的使用率，将推力分配看作是最优化问题，即在期望的控制力与可达控制力之间的误差最小化的基础上，使得各执行器均使用最小控制力。

根据拉格朗日方程，可得执行器权重转换矩阵T_W：

T_W＝W^-1T^T(TW^-1T^T)^-1 (45)

式中，W为权重矩阵，当无人艇的左右舷侧对称且具有相同执行机构，则有

k表示执行器的个数。T为转换矩阵，有

式中，l_xi,l_yi分别表示第i个执行器在纵向和横向与重心的距离。

输入至执行机构的力f：

f＝[F_x1,F_y1,...F_xi,F_yi,...,F_xk,F_yk]^T＝T_wτ (47)

执行机构期望方位角δ_i与期望推力T_i为

考虑到线性执行器的物理限制，每个螺旋桨具备方位角范围δ≤|δ_max|。然而，它们都具备倒车能力。针对该问题，设计推力输出逻辑：若分配方案请求一个无法实现的角度，则将推力设置为零；若需要δ∈[δ_max-180,180-δ_max]的角度，则将该推进器置为倒车模式，采用方位角δ＝δ+180。

所述底层控制器包括艏向控制器和航速控制器，具体包括：

底层控制器包括艏向控制和航速控制，根据上述的制导律求出相应期望艏向和期望速度后，结合当前位姿信息求解推力和舵角。本发明提出的底层控制器仅根据状态反馈信息，不需要任何模型就能求解合适的推力和舵角，因此十分适用于实际工程的控制器部署。

所述艏向控制器根据期望艏向和当前艏向，获取期望舵角，并根据所述期望舵角对无人艇进行艏向的控制，具体为：

艏向控制器：艏向控制流程如图8示，其中坐标转化模块为通过将差分量转化为算法可用的归一化误差。参数分离器通过无人艇当前的航速、姿态以及期望的航速，考虑到控制性能需求以及安全性需求，调用合适控制参数。限制器为出于安全性以及设备耐久性考虑，对控制器输出进行辨识，优化，处理输出抖振以及过饱和问题：

在采用归一化的增量式比例-微分-积分控制器进行艏向控制时，发现在浪流作用下，若期望角度是和浪流作用垂直的那个区域，在接近期望角度的时候往往出现积分累积值较大，从而导致比例和微分作用变小，以至于经常出现控制滞后的现象。考虑积分饱和只会在给出最大舵角后还存在静态误差的情况下出现，即可忽略积分上限。再者，考虑在艏向误差收敛至0的过程中，积分累积处若在衰减过程，则可减少积分的作用，故有：

式中，An是辅助变量，σ为积分死区参数。t₁表示满舵的时刻，即有积分只累积至满舵时刻，

为艏向误差，k_i为控制参数。

在上述改进的基础上，考虑以艏摇速度为依据实时修正比例项。这里通过对比期望艏摇速度和当前艏摇速度，引入S面函数对比例项k₅进行修正，得到：

式中，λ＞0表示最大修正幅值，

表示艏摇误差。关于期望艏摇速度r_d，本节采用线性函数进行预测：

式中，ω＞0表示最大期望艏摇幅值。

所述航速控制器根据期望航速和当前航速，获取期望油门，并根据所述期望油门对无人艇进行航速的控制，具体为：

航速控制器：航速控制器原理如图9所示，拟采用基于先验信息的增量式控制方法，其中的先验信息为经试验获得的油门-航速标定信息，通过以该先验信息作为基准油门，在此基础上采用归一化的增量式比例-微分控制器，以获得航速的快速响应。通过指令处理器对累增油门进行控制，以实现开环控制。限制器为出于安全性以及设备耐久性考虑，对控制器输出进行辨识，优化，处理输出抖振以及过饱和问题。

首先根据指令速度查表，有：

若

则令i＝j(52)

式中，j表示搜索指针，i为指令航速对应表中位置，

表示航速表，u_d表示期望航速。

通过插值法获得基础油门量P_base：

式中，

表示油门表。

最后，以航速和航速误差为参考值，通过积分S面函数，加之上述基础油门量，给出以下航速控制器：

式中，P为期望油门，k_i,i＝1,2,3为正向控制参数，

为航速一阶导，

为航速误差：

为完成风浪环境中的运动控制稳定性及精度考核，本测试进行了包括定向定速，直线路径跟踪，圆弧路径跟踪，虚拟引导及定点控位在内的五项控制测试内容。

首先给出误差公式：

(1)直线路径跟踪横侧偏差计算

假设一共有i个直线期望目标点P＝{p_i|p_i＝(n_i,e_i)}。当前船体位置表示为p_t＝(n_t,e_t)。测试总时间表示为T。有直线横侧偏差δ_t计算公式:

式中，

表示第t时刻，当前船体与第i个和第i-1个期望目标点形成的直线的横侧偏差，χ_p＝atan2(p_i-1,p_i)表示该直线与大地坐标系北向夹角的角度。再对

遍历取最小值，即有

最后对δ_t求其均方根RMS，有

(2)圆弧路径跟踪横侧偏差计算

假设有期望圆轨中心点为p_t＝(n_t,e_t)，跟踪半径为r。当前船体位置为p_t＝(n_t,e_t)。测试总时间表示为T。有圆弧横侧偏差δ_t计算公式：

取第一次振荡点作为临界点，即第一次δ_t＞δ_t-1的时刻(后文以t＝1表示该时刻)，震荡前的偏差为趋近过程不做考虑，因此从t＝1时刻开始对δ_t求其均方根RMS。

(3)虚拟引导误差计算

假设有期望引导点为p_v＝(n_v,e_v)，期望距离为l，期望方位为θ。当前船体位置为p_t＝(n_t,e_t)。测试总时间表示为T。有距离误差l_e的计算公式为：

有方位误差θ_e的计算公式为：

θ_e＝atan2(e_t-e_v,n_t-n_v)-θ (62)

(4)定位误差计算

假设有期望定位点为p_d＝(n_d,e_d)，期望定位艏向为ψ_d。当前船体位置为p_t＝(n_t,e_t)，实际艏向为ψ，定位指标为Δ，测试总时间表示为T。有定位误差δ_t计算公式：

取第一次进入定位区域为临界点，即第一次δ_t＜Δ的时刻(后文以t＝1表示该时刻)，进入区域前的偏差为趋近过程不做考虑，因此从t＝1时刻开始对δ_t求其均方根RMS判断是否超出定位指标Δ

有艏向误差计算公式为：

ψ_e＝ψ-ψ_d (66)

其次进行路径跟踪运动控制测试：

三级海况

基于定速定向控制的控制精度及响应情况，完成路径跟踪制导律。为测试路径跟踪精度及稳定性，首先在三级海况下测试单项路径跟踪精度，具体工况如表1所示。

表1路径跟踪单项测试工况

路径跟踪单项测试工况一的跟踪路径结果如图10所示，跟踪误差结果如图11所示

从图10的路径图可知，12节航速下总体跟踪精度良好。从图11中(a)所示的航向跟踪曲线可知，航向跟踪效果良好，说明制导律产生的期望艏向能够良好的匹配航向环的响应时间。从图11中(b)所示的航速跟踪曲线可知，由于海况较差航速虽有震荡，但震荡范围稳定在2节以内。从图11中(c)所示的横侧偏差曲线可知，在整个跟踪过程中，稳定段的横侧偏差(除开切换线段过程)超调不超过2米。分析数据可知，12节航速下的横侧偏差均方根(RMS)值为0.4米，每次切换路径后闭环系统稳定时间均在20s以内。

Claims (10)

1.一种水面无人艇航控系统，其特征在于，所述系统包括：路径跟踪模块、虚拟引导模块、动力定位模块和底层控制器；

所述路径跟踪模块用于根据接收到的任务路径，生成航行规划路径信息发送给底层控制器，所述航行规划路径信息包括所述路径跟踪模块的期望艏向和期望航速；

所述虚拟引导模块根据任务移动引导点，生成航行规划移动引导信息发送给底层控制器，所述航行规划移动引导信息包括所述虚拟引导模块的期望艏向和期望航速；

所述动力定位模块根据任务固定定位点，生成航行规划动力定位信息发送给底层控制器，所述航行规划动力定位信息包括所述动力定位模块的期望舵角和期望油门；

所述底层控制器用于根据所述路径跟踪模块、所述虚拟引导模块和所述动力定位模块，并结合当前位姿信息获取期望舵角和期望油门，进而对无人艇进行控制；

所述底层控制器包括艏向控制器和航速控制器；

所述艏向控制器根据期望艏向和当前艏向，获取期望舵角，并根据所述期望舵角对无人艇进行艏向的控制；

所述航速控制器根据期望航速和当前航速，获取期望油门，并根据所述期望油门对无人艇进行航速的控制。

2.根据权利要求1所述的一种水面无人艇航控系统，其特征在于，所述路径跟踪模块包括制导律计算模块、干扰观察器、越位判断模块、缓冲带模块和艏向自适应状态引导模块；

所述制导律计算模块将跟踪点的概念改为跟踪面，并根据根据横侧偏差的大小调整收敛速度，提出基于自适应边界层的制导律；

所述干扰观察器将艏向侧滑角的偏置为一种不确定干扰，同时集总艏向跟踪误差，对该集总的不确定性进行实时的估计；

越位判断模块作为补充，根据期望目标点与无人艇组成的向量和期望路径向量的相对角关系，决定是否换点；

缓冲带用于提高路径切换时的跟踪精度，具体为：

若无人艇投影还未进入期望路径，或者已进入期望路径但还处在前置缓冲带内，则采用前置缓冲速度；

若无人艇的投影已超出期望路径，或者已进入期望路径且处在终端缓冲带，则采用终端缓冲速度；

艏向自适应状态引导模块用于保证无人艇能够时刻航行在期望路径上，以无人艇艏向作为航行趋向参考值，以横侧偏差作为精度参考值，在牺牲航速的前提下以提高航行时的安全性。

3.根据权利要求1所述的一种水面无人艇航控系统，其特征在于，所述虚拟引导模块包括制导控制模块和层级航速规划器模块；

所述制导控制模块根据目标艏向、无人艇与目标相对艏向和无人艇至目标航线距离，获取制导律；

所述层级航速规划器模块根据无人艇至目标距离，获取指令航速与期望航速的关系。

4.根据权利要求1所述的一种水面无人艇航控系统，其特征在于，所述动力定位模块包括位姿保持器、环境力预测模块、环境力前馈控制模块和推力分配模块；

所述位姿保持器采取比例-微分控制；

所述环境力预测模块根据风速，风向，波幅，波长，波向获取风力系数和浪系数；

所述环境力前馈控制模块所述风力系数和所述浪系数，通过环境力前馈控制器结算抗干扰控制输出；

所述推力分配模块在期望的控制力与可达控制力之间的误差最小化的基础上，使得各执行器均使用最小控制力。

5.根据权利要求2所述的一种水面无人艇航控系统，其特征在于，所述制导律计算模块用于计算直线制导律和圆弧制导律；

所述直线制导律为：

式中，ψ_d为期望艏向，φ_p为上一个期望路径点和当前期望路径点的连线于正北坐标轴的夹角，k_i＞0(i＝1,2,3)为控制参数，δ_l表示直线跟踪横侧偏差：

δ_l＝-(n_t-n_k)sinφ_p+(e_t-e_k)cosφ_p

式中，p_t＝(n_t,e_t)表示当前无人艇的位置，p_k＝(n_k,e_k)表示期望直线路径上的起点，φ_p的表达式为：

φ_p＝atan2(n_k+1-n_k,e_k+1-e_k)

式中，p_k+1＝(n_k+1,e_k+1)表示期望直线路径上的终点；

所述圆弧制导律为：

式中，ψ_d为期望艏向，φ_c为期望圆心和无人艇当前位置的连线于正北坐标轴的夹角，k_i＞0(i＝4,5,6为控制参数，tan(π²/4k₄)/k₅＞δ^*，其中δ^*≥_sup|δ_c|为最大横侧偏差，有圆弧跟踪横侧偏差为：

式中，p_c＝(n_c,e_c)表示期望圆弧路径的圆心，r表示期望圆弧路径的半径，φ_c为期望路径的圆心和无人艇当前位置的连线与正北坐标轴的夹角，φ_c的表达式为：

φ_c＝atan2(n_t-n_k,e_t-e_k)。

6.根据权利要求3所述的一种水面无人艇航控系统，其特征在于，

所述制导控制模块的制导律为：

式中，ψ_d为期望艏向，_supδ为正向控制参数，

为目标艏向，ψ_p为无人艇与目标相对艏向，δ为无人艇至目标航线距离。

7.根据权利要求3所述的一种水面无人艇航控系统，其特征在于，所述层级航速规划器模块，具体包括：

根据无人艇至目标距离：

获取指令航速与期望航速的关系为：

式中，ε_i,i＝1,2为航速调整参数，l_i,i＝1,2为层级距离，u_d为期望航速，u_user为指令航速。

8.根据权利要求4所述的一种水面无人艇航控系统，其特征在于，所述位姿保持器采取比例-微分控制：

式中，τ为解算出来的控制力矩，K₁,K₂为正定对角矩阵控制参数，η_t＝η-η_d为位姿误差，其中η＝[n,e,ψ]^T为在大地坐标系下的当前位姿，η_d＝[n_d,e_d,ψ_d]^T为在大地坐标系下的期望定位点，J(η)为旋转矩阵，具体公式有：

9.根据权利要求1所述的一种水面无人艇航控系统，其特征在于，

所述艏向控制器通过对比期望艏摇速度和当前艏摇速度，引入S面函数对比例项进行修正，得到：

式中，k₅为比例项，λ＞0表示最大修正幅值，

表示艏摇误差，

为艏向误差，关于期望艏摇速度r_d，采用线性函数进行预测：

式中，ω＞0表示最大期望艏摇幅值。

10.根据权利要求1所述的一种水面无人艇航控系统，其特征在于，

所述航速控制器根据指令速度查表，有：

若

则令i＝j

式中，j表示搜索指针，i为指令航速对应表中位置，T_· ^u表示航速表，u_d表示期望航速。

通过插值法获得基础油门量P_base：

以航速和航速误差为参考值，通过积分S面函数，加之上述基础油门量，给出以下航速控制器：

式中，P为期望油门，k_i,i＝1,2,3为正向控制参数，

为航速一阶导，

为航速误差：

Images