CN115437251A - 一种带有输入量化的usv路径规划与模糊自适应跟踪控制方法 - Google Patents

Abstract

本发明提供一种带有输入量化的USV路径规划与模糊自适应跟踪控制方法，包括：获取周围环境及周围其他船舶的海况信息，设置参数；采用模拟退火算法改进传统人工势场法对海况信息下的本船进行合理规避障碍物的路径规划；对规划的路径作曲线拟合得出优化后的路径，并将优化后的路径用一条函数曲线表示，将曲线作为船舶运动控制系统的期望输入；设计基于扩张状态观测器的运动学制导律和自适应模糊量化控制器，对船舶规划路径进行跟踪。本发明采用模拟退火算法改进的人工势场法进行船舶路径规划，将规划的优化路径作为船舶运动控制系统的期望输入，基于扩张状态观测器设计船舶运动学跟踪控制率，通过设计自适应模糊量化器解决了欠驱动船舶路径跟踪问题。

Description

一种带有输入量化的USV路径规划与模糊自适应跟踪控制 方法

技术领域

本发明涉及人工智能技术领域，具体而言，尤其涉及一种带有输入量化的USV路径规划与模糊自适应跟踪控制方法。

背景技术

近年来，随着物联网、大数据、云计算等技术的快速发展，智能航海受到广泛关注。一种小型化、多用途、智能化并以遥控或自主方式航行的无人海洋运载平台，无人船(Unmanned Surface Vehicle，USV)应运而生，它可以自主在海洋环境中开展复杂的海洋作业，具有广阔的发展空间与应用前景。

USV的路径规划和路径跟踪是密切联系的，路径规划需要结合USV运动控制的特性，并且规划的路径需要通过路径跟踪来实现。USV的路径规划、跟踪及控制一直以来都是USV研究的重要内容。

发明内容

根据上述提出现有船舶路径规划及跟踪控制所存在的技术问题，提供一种带有输入量化的USV路径规划与模糊自适应跟踪控制方法。本发明采用模拟退火算法优化传统人工势场法的斥力函数，有效解决了传统人工势场法目标不可达和容易陷入局部极小值的缺陷。在运动学制导层，将规划的优化路径作为船舶运动控制系统的期望输入，设计了基于扩张状态观测器(ESO)的运动学制导律，实现了对期望路径的跟踪以及海流所引起的运动学偏移的补偿。在动力学控制层，通过使用模糊逻辑系统实现对模型不确定及外界干扰的逼近，采用一种线性解析模型来描述输入量化过程，在量化参数未知的假设下所设计的自适应模糊量化控制器能够稳定跟踪运动学制导信号。基于输入到状态稳定性理论证明了闭环系统的稳定性，仿真实验验证了所提算法的有效性，实现了欠驱动船舶的高精度路径规划及跟踪控制。

本发明采用的技术手段如下：

一种带有输入量化的USV路径规划与模糊自适应跟踪控制方法，包括如下步骤：

S1、获取周围环境及周围其他船舶的海况信息，设置参数；

S2、采用模拟退火算法改进传统人工势场法对所述海况信息下的本船进行合理规避障碍物的路径规划；

S3、对规划的路径作曲线拟合得出优化后的路径，并将优化后的路径用一条函数曲线表示，将曲线作为船舶运动控制系统的期望输入；

S4、设计基于扩张状态观测器的运动学制导律和自适应模糊量化控制器，对船舶规划路径进行跟踪；

S5、基于输入的状态稳定性理论证明闭环系统的稳定性，通过仿真实验验证算法的有效性。

进一步地，所述步骤S1具体包括：

S11、建立正方形环境地图；

S12、在环境地图中建随机建立n个黑色圆形障碍物，障碍物的圆心用一个n*2的向量表示；

S13、建立欧式坐标系，设置船舶起始点的位置和目标点的位置；

S14、设置人工势场法的步长、引力增益系数、斥力增益系数、障碍影响距离；设置模拟退火算法中的初始温度、终止温度，并设置循环次数；在对比实验中，传统人工势场法与改进人工势场法采用相同的参数；

S15、考虑USV控制系统，设置理想化的规划路径，使得USV沿着拟合后的时变参数化轨迹航行；选取USV的参数，设置USV的初始状态，设置底层控制器参数，设置量化参数。

进一步地，所述步骤S2具体包括：

S21、设本船所在位置为(x,y)^Τ，X为本船位置向量，X_d为目标点位置向量，则本船在势场中受到目标点的引力为F_att(X)，所受到的障碍物的斥力为F_rep(X)，引力与斥力的合力为F_total(X)，合力F_total(X)的方向为本船改变的目标航向；

S22、根据人工势场法原理，目标点对本船产生的引力F_att(X)会随着本船与目标点间的距离的增加而单调递增，将本船的引力势场函数表示为：

其中，K_a为引力势场常量，(X-X_d)²为本船到目标点的相对距离；

S23、重新定义一个斥力函数，假定以障碍物为中心的斥力势场影响范围为ρ₀，本船与障碍物的距离为ρ，新的势场函数中考虑了船舶与目标点之间的距离，在原有的斥力函数中添加一个系数项：|X-X_d|ⁿ，改进后的斥力函数为：

其中，U_rep(X)为障碍物的斥力场，k_r则表示斥力势场常数，是正数，ρ为船舶在空间位置中与障碍物间距离，ρ₀为以障碍物为中心的斥力势场影响范围，X为船舶位置向量，X_d为目标点位置向量，ρ(X,X_g)表示船舶到障碍物的欧几里得距离，n为一个大于零的任意实数。

S24、利用模拟退火算法突跳性对模型进行局部最优优化；在程序中，给定一个初始温度以及一个初始状态，并给定合适的退火速度，随着本船的行进，初始温度降低，在温度降低的过程中，模拟退火算法对每一个状态下的势场强度进行计算，同时，在每一个状态下给定一个随机扰动，并计算新状态下势场强度与初始状态势场强度之差，并对差值进行分析；

S25、按照Metropolis准则，若差值小于等于零，则程序会接受新状态；若差值大于零，则以一定的概率接受新状态，反之会保留原初始状态；如此往复，避免本船在行进中产生局部极小值和目标不可达的情况，使船舶安全到达目标点，规划出合理避障的最优路径。

进一步地，所述步骤S3具体包括：

对规划出的最优路径进行平滑处理，并作为输入信号进行路径跟踪；使用MATLAB中ployfit函数对最优路径进行曲线拟合，拟合函数如下所示：

y＝a₁xⁿ+a₂x^n-1+a₃x^n-2+...+a_nx+a_n+1

上式为一个多项式，在拟合过程中，需要对不同的多项式次数进行拟合；拟合次数少，路径拟合效果比较差，拟合次数过多，会出现龙格现象，只有当误差最小时，才会出现理想的拟合效果。

进一步地，所述步骤S4具体包括：

S41、基于扩张状态观测器，对期望路径的跟踪以及海流所引起的运动学偏移补偿，设计船舶运动学制导律；

S42、采用模糊逻辑系统实现对模型不确定及外界干扰的逼近及建立线性解析模型来描述输入量化过程的方法，设计船舶动力学自适应模糊量化控制器。

进一步地，所述步骤S41具体包括：

S411、构建船舶平面运动模型；

S412、将船舶的运动学方程表示为：

其中，x,y,

分别为惯性坐标系下USV的横向位置、纵向位置和艏摇角；u,v,r为附体坐标系下USV的前向速度、横向速度和艏揺速度；u_c,

分别表示海流的速度及流向；

S413、定义USV误差为：

其中，旋转矩阵

表示为：

S414、对误差进行求导，可得：

其中，

S415、为解决船舶欠驱动的问题，设置误差转移方程为：

其中，δ₀∈R为正的常值；

S416、将误差转移方程带入求导的误差中可得：

其中，h＝diag{d,δ₀}，

S417、假设未知项σ满足

其中σ^*为正常数；航海实践中USV的速度及加速度均有上界，因此假设合理；

S418、假设海流流速u_c与流向

未知，因此σ为未知项，采用扩张状态观测器进行估计：

其中，

为ESO的估计误差；

S419、根据步骤S416和步骤S418可得：

进一步可表达为：

其中，

S4110、基于ESO的分布式运动学制导律设计为：

其中，

及

为正的常数。

进一步地，所述步骤S42具体包括：

S421、基于牛顿力学和拉格朗日力学，欠驱动USV动力学非线性数学模型可表述为：

其中，

为USV质量，

为水动力导数项，I_z为关于z轴的惯性矩；f_u(·),f_v(·),f_r(·)为向心力、科氏力和水动力阻尼构成的非线性项；τ_uw,τ_vw,τ_rw为未知的环境干扰；Q(τ_u),Q(τ_r)为控制输入τ_u,τ_r的量化值；

S422、均值量化器可表述为：

其中，o_u,o_r为量化参数，经过量化器后，控制信号τ_i变成Q(τ_i)；

S423、根据欠驱动USV动力学非线性数学模型可得：

动力学子系统的控制目标为：

其中，δ₁,δ₂为小的正常数；

S424、将步骤S423的公式进行如下转换：

其中，

S425、令Q(τ_u)＝q_1u(t)τ_u+q_2u(t),Q(τ_r)＝q_1r(t)τ_r+q_2r(t)，且：

其中，q_1u(t)和q_1r(t)未知，由于量化过程符号不变，可知q_1u(t)＞0，q_1r(t)＞0，当|τ_u(t)|＜a,|τ_r(t)|＜a时，Q(τ_u(t)),Q(τ_r(t))是有界的，因此q_2u(t),q_2r(t)是有界的，取

S426、定义积分滑模面为：

其中，s＝[s_u,s_r]^T,c_u＞0,c_r＞0；

S427、对积分滑模面求导可得：

S428、令Δ_u(X)＝d_u+F_u,Δ_r(X)＝d_r+F_r,可得：

S429、由于Δ_u(X)为未知项，根据万能逼近定理，对于任意小的常数ε_N，存在模糊逻辑系统

使得

其中

为模糊系统的理想权重；h_u(X)为模糊基向量，ε_u为模糊系统的逼近误差，满足|ε_u|≤ε_N，令

为Δ_u(X)的估计值，且

其中

为理想权重

的估计值，且

S4210、定义：

其中，l_u,η_u为正常值，且满足η_u≥ε_N+η_d,η_d＞0；由于：

可得：

其中，

S4211、

可得：

其中，

为Δ_r(X)的估计值；

为模糊系统的理想权重；

为W_r的估计值，且

ε_r为模糊系统的逼近误差，满足|ε_r|≤ε_N；l_r,η_r为正的常值，满足η_r≥ε_N+η_d,η_d＞0；

S4212、由于q_1u(t)和q_1r(t)时变且未知，需要进行自适应估计，采用对其下界进行估计的方法，防止估计值为零时引发的奇异问题，定义时变增益μ_u＝1/q_1u(t)_min,μ_r＝1/q_1r(t)_min，其中q_1u(t)_min,q_1r(t)_min分别为q_1u(t),q_1r(t)的下界；

S4213、设计自适应动力学控制律和自适应律为：

其中，γ₁,γ₂,γ₃,γ₄,ρ_u,ρ_r,

为正常值；

S4214、基于

s_u,s_r,

所构成的误差动态系统可表述为：

进一步地，所述步骤S5具体包括：

S51、运动学误差子系统稳定性证明；

S52、动力学误差子系统输入状态稳定性证明。

进一步地，所述步骤S51具体包括：

S511、考虑如下李雅普诺夫函数：

其中，P为正定矩阵，满足：

A^TP+P^TA≤I

S512、对式

求导可得：

由于

使得：

其中，

运动学子系统是输入状态稳定的，且：

其中，

进一步地，所述步骤S52具体包括：

S521、将动力学误差子系统看作一个状态为s_u,s_r,

输入为D_u,D_r,

的系统，构建李雅普诺夫函数：

其中，

S522、对其求导可得：

S523、将步骤S4211和步骤S4212中的

带入到步骤S521中，可得：

S524、将动力学控制律、自适应律、带入S522式中可得：

S525、考虑到：

q_1u≥q_1umin＝1/μ_u＞0,q_1r≥q_1rmin＝1/μ_r＞0,-η_u|s_u|+ε_us_u≤0,-η_r|s_r|+ε_rs_r≤0,

可得：

基于步骤S525中的公式，以及步骤S524中的不等式可写为：

其中，

S526、将动力学误差动态带入式S525中可得：

定义

可得：

其中，

因此动力学误差子系统是输入状态稳定的，且：

其中，

且：

P_c＝diag{1,1/2γ₂μ_u,1/2γ₄μ_r,1/2γ₁,1/2γ₃}

S527、考虑由USV运动学模型、动力学模型、运动学制导律、动力学底层控制律、自适应律、构成的闭环系统，基于级联系统稳定性理论，结合S51和S52稳定性证明，由运动学误差子系统和动力学误差子系统构成的级联系统是输入状态稳定的；由S512和S526中的||E₁(t)||、||E₂(t)||可得，||E₂(t)||是有界的，即：

因此，闭环系统是稳定的。

较现有技术相比，本发明具有以下优点：

1、本发明提供的带有输入量化的USV路径规划与模糊自适应跟踪控制方法，针对船舶路径规划问题，采用模拟退火算法优化传统人工势场法的斥力函数，既有效解决了传统人工势场法目标不可达和容易陷入局部极小值的缺陷，也解决了现有改进人工势场法只能优化传统人工势场法目标不可达或局部极小值问题。

2、本发明提供的带有输入量化的USV路径规划与模糊自适应跟踪控制方法，针对改进人工势场法的规划航线作为参考跟踪路径，通过多项式拟合，使规划航线变成了一条光滑连续的曲线，作为船舶运动学控制子系统的参考输入信号；

3、本发明提供的带有输入量化的USV路径规划与模糊自适应跟踪控制方法，在USV运动学子系统，设计了基于扩张状态观测器(ESO)的运动学制导律，实现了对期望路径的跟踪以及海流所引起的运动学偏移的补偿，同时解决了船舶的欠驱动问题；

4、本发明提供的带有输入量化的USV路径规划与模糊自适应跟踪控制方法，在USV动力学子系统，引入模糊逻辑系统实现对系统模型不确定及风、浪干扰的逼近。通过一种线性解析模型来描述输入量化过程，在量化参数未知的假设下所设计的自适应模糊量化控制器能够稳定跟踪运动学制导信号。

5、本发明提供的带有输入量化的USV路径规划与模糊自适应跟踪控制方法，基于输入到状态稳定性理论证明了闭环系统的稳定性，仿真实验验证了所提算法的有效性，实现了欠驱动船舶的高精度路径规划及跟踪控制。

基于上述理由本发明可在人工智能等领域广泛推广。

附图说明

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

图1为本发明方法流程图。

图2为本发明实施例提供的船舶运动模型图。

图3为本发明实施例提供的模拟退火算法改进的人工势场法流程图。

图4为本发明实施例提供的简单障碍物环境下传统人工势场法(a)和改进人工势场法(b)路径规划对比图。

图5本发明实施例提供的复杂障碍物环境下传统人工势场法(a)和改进人工势场法(b)路径规划对比图。

图6为本发明实施例提供的一种改进人工势场法路径拟合曲线。

图7为本发明实施例提供的实际海况船舶路径跟踪航线。

图8为本发明实施例提供的一种位置和角度的跟踪实验结果仿真曲线。

图9为本发明实施例提供的一种输入信号变化实验结果仿真曲线。

图10为本发明实施例提供的一种动力学控制力实验结果仿真曲线。

图11为本发明实施例提供的一种动力学系统控制力矩实验结果仿真曲线。

具体实施方式

需要说明的是，在不冲突的情况下，本发明中的实施例及实施例中的特征可以相互组合。下面将参考附图并结合实施例来详细说明本发明。

为使本发明实施例的目的、技术方案和优点更加清楚，下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。以下对至少一个示例性实施例的描述实际上仅仅是说明性的，决不作为对本发明及其应用或使用的任何限制。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。

需要注意的是，这里所使用的术语仅是为了描述具体实施方式，而非意图限制根据本发明的示例性实施方式。如在这里所使用的，除非上下文另外明确指出，否则单数形式也意图包括复数形式，此外，还应当理解的是，当在本说明书中使用术语“包含”和/或“包括”时，其指明存在特征、步骤、操作、器件、组件和/或它们的组合。

除非另外具体说明，否则在这些实施例中阐述的部件和步骤的相对布置、数字表达式和数值不限制本发明的范围。同时，应当清楚，为了便于描述，附图中所示出的各个部分的尺寸并不是按照实际的比例关系绘制的。对于相关领域普通技术人员己知的技术、方法和设备可能不作详细讨论，但在适当情况下，所述技术、方法和设备应当被视为授权说明书的一部分。在这里示出和讨论的所有示例中，任向具体值应被解释为仅仅是示例性的，而不是作为限制。因此，示例性实施例的其它示例可以具有不同的值。应注意到：相似的标号和字母在下面的附图中表示类似项，因此，一旦某一项在一个附图中被定义，则在随后的附图中不需要对其进行进一步讨论。

在本发明的描述中，需要理解的是，方位词如“前、后、上、下、左、右”、“横向、竖向、垂直、水平”和“顶、底”等所指示的方位或位置关系通常是基于附图所示的方位或位置关系，仅是为了便于描述本发明和简化描述，在未作相反说明的情况下，这些方位词并不指示和暗示所指的装置或元件必须具有特定的方位或者以特定的方位构造和操作，因此不能理解为对本发明保护范围的限制：方位词“内、外”是指相对于各部件本身的轮廓的内外。

为了便于描述，在这里可以使用空间相对术语，如“在……之上”、“在……上方”、“在……上表面”、“上面的”等，用来描述如在图中所示的一个器件或特征与其他器件或特征的空间位置关系。应当理解的是，空间相对术语旨在包含除了器件在图中所描述的方位之外的在使用或操作中的不同方位。例如，如果附图中的器件被倒置，则描述为“在其他器件或构造上方”或“在其他器件或构造之上”的器件之后将被定位为“在其他器件或构造下方”或“在其位器件或构造之下”。因而，示例性术语“在……上方”可以包括“在……上方”和“在……下方”两种方位。该器件也可以其他不同方式定位(旋转90度或处于其他方位)，并且对这里所使用的空间相对描述作出相应解释。

此外，需要说明的是，使用“第一”、“第二”等词语来限定零部件，仅仅是为了便于对相应零部件进行区别，如没有另行声明，上述词语并没有特殊含义，因此不能理解为对本发明保护范围的限制。

如图1所示，本发明提供了一种带有输入量化的USV路径规划与模糊自适应跟踪控制方法，包括如下步骤：

S1、获取周围环境及周围其他船舶的海况信息，设置参数；

S2、采用模拟退火算法改进传统人工势场法对所述海况信息下的本船进行合理规避障碍物的路径规划；

S3、对规划的路径作曲线拟合得出优化后的路径，并将优化后的路径用一条函数曲线表示，将曲线作为船舶运动控制系统的期望输入；

S4、设计基于扩张状态观测器的运动学制导律和自适应模糊量化控制器，对船舶规划路径进行跟踪；

S5、基于输入的状态稳定性理论证明闭环系统的稳定性，通过仿真实验验证算法的有效性。

具体实施时，作为本发明优选的实施方式，所述步骤S1具体包括：

S11、建立长和宽均为[-2,12]的正方形环境地图；

S12、在环境地图中建随机建立n个黑色圆形障碍物，障碍物的圆心用一个n*2的向量表示，障碍物的半径为0.5；

S13、建立欧式坐标系，设置船舶起始点的位置为(0，0)，目标点的位置为(10，10)；

S14、设置人工势场法的步长为0.1，引力增益系数为5，斥力增益系数为15，障碍影响距离为2。设置模拟退火算法中的初始温度、终止温度，并设置循环次数为100次。在对比实验中，传统人工势场法与改进人工势场法采用相同的参数；

S15、考虑USV控制系统,令理想化的规划路径x_d＝t，USV沿着拟合后的时变参数化轨迹p_d＝[t,a₁t¹⁰+a₂t⁹+a₃t⁸+...+a₉t²+a₁₀+a₁₁]^T航行。USV参数选取如下：

m_u＝25.8kg

m_v＝33.8kg

m_r＝2.76kg·m²

f_u(·)＝-5.87u³-1.33|u|u-0.72u+m_vvr+1.0948r²

f_v(·)＝-36.5|v|v-0.8896v-0.805v|r|-m_uur

f_r(·)＝-0.75|r|r-1.90r+0.08|v|r+(m_u-m_v)uv-1.0948ur。

USV初始状态设为p₀＝[0,0]^T，底层控制器参数c_u＝c_r＝10，l_u＝l_r＝30，η_u＝η_r＝2，ρ_u＝ρ_r＝0.02，

γ₁＝γ₃＝3,γ₂＝γ₄＝2，量化参数o_u＝o_r＝0.2；

具体实施时，作为本发明优选的实施方式，所述步骤S2具体包括：

S21、设本船所在位置为(x,y)^Τ，X为本船位置向量，X_d为目标点位置向量，则本船在势场中受到目标点的引力为F_att(X)，所受到的障碍物的斥力为F_rep(X)，引力与斥力的合力为F_total(X)，合力F_total(X)的方向为本船改变的目标航向；随着本船的位置不断变化，合力F_total(X)的方向也随之变化，只要本船在的航向随着合力的方向而改变，本船就可以在航行中避开障碍物到达目标点；

S22、根据人工势场法原理，目标点对本船产生的引力F_att(X)会随着本船与目标点间的距离的增加而单调递增，将本船的引力势场函数表示为：

其中，K_a为引力势场常量，(X-X_d)²为本船到目标点的相对距离；

S23、在势场中，为了避免本船与障碍物发生碰撞，障碍物会对本船产生一定的斥力F_rep(X)，且斥力的大小会随着本船与障碍物间距离的变大而单调递减。按照传统人工势场法中定义的引力函数和斥力函数，此时障碍物产生的斥力将远远大于船舶受到的引力，进而船舶将不会到达目标点。为解决目标不可达的问题，需要重新定义一个斥力函数，这个函数需要满足当船舶向目标点航行并靠近时，船舶受到的斥力尽可能地小至趋于零，那么目标点所在的位置将是整个势场的全局最小点，从而解决目标不可达的问题。假定以障碍物为中心的斥力势场影响范围为ρ₀，本船与障碍物的距离为ρ，新的势场函数中考虑了船舶与目标点之间的距离，在原有的斥力函数中添加一个系数项：|X-X_d|ⁿ，改进后的斥力函数为：

其中，U_rep(X)为障碍物的斥力场，k_r则表示斥力势场常数，是正数，ρ为船舶在空间位置中与障碍物间距离，ρ₀为以障碍物为中心的斥力势场影响范围，X为船舶位置向量，X_d为目标点位置向量，ρ(X,X_g)表示船舶到障碍物的欧几里得距离，n为一个大于零的任意实数；其中，船舶与目标点间的相对距离|X-X_d|ⁿ＝|(x-x_d)ⁿ|+|(y-y_d)|ⁿ，在原有的斥力函数中引入了船舶与目标点之间的相对距离，保障了目标点在整个势场中处于全局最小的位置。

S24、针对势场中可能存在多个局部极小值点的问题，人工势场法中加入模拟退火算法，主要是利用模拟退火算法突跳性对模型进行局部最优优化；在程序中，给定一个初始温度以及一个初始状态，并给定合适的退火速度，随着本船的行进，初始温度降低，在温度降低的过程中，模拟退火算法对每一个状态下的势场强度进行计算，同时，在每一个状态下给定一个随机扰动，并计算新状态下势场强度与初始状态势场强度之差，并对差值进行分析；

S25、按照Metropolis准则，若差值小于等于零，则程序会接受新状态；若差值大于零，则以一定的概率接受新状态，反之会保留原初始状态；如此往复，避免本船在行进中产生局部极小值和目标不可达的情况，使船舶安全到达目标点，规划出合理避障的最优路径。

具体实施时，作为本发明优选的实施方式，所述步骤S3具体包括：

对规划出的最优路径进行平滑处理，并作为输入信号进行路径跟踪；使用MATLAB中ployfit函数对最优路径进行曲线拟合，拟合函数如下所示：

y＝a₁xⁿ+a₂x^n-1+a₃x^n-2+...+a_nx+a_n+1

上式为一个多项式，在拟合过程中，需要对不同的多项式次数进行拟合；拟合次数少，路径拟合效果比较差，拟合次数过多，会出现龙格现象，只有当误差最小时，才会出现理想的拟合效果。

具体实施时，作为本发明优选的实施方式，所述步骤S4具体包括：

S41、基于扩张状态观测器，对期望路径的跟踪以及海流所引起的运动学偏移补偿，设计船舶运动学制导律；

S411、构建船舶平面运动模型；

S412、将船舶的运动学方程表示为：

其中，x,y,

分别为惯性坐标系下USV的横向位置、纵向位置和艏摇角；u,v,r为附体坐标系下USV的前向速度、横向速度和艏揺速度；u_c,

分别表示海流的速度及流向；

S413、定义USV误差为：

其中，旋转矩阵

表示为：

S414、对误差进行求导，可得：

其中，

S415、为解决船舶欠驱动的问题，设置误差转移方程为：

其中，δ₀∈R为正的常值；

S416、将误差转移方程带入求导的误差中可得：

其中，h＝diag{d,δ₀}，

S417、假设未知项σ满足

其中σ^*为正常数；航海实践中USV的速度及加速度均有上界，因此假设合理；

S418、假设海流流速u_c与流向

未知，因此σ为未知项，采用扩张状态观测器进行估计：

其中，

为ESO的估计误差；

S419、根据步骤S416和步骤S418可得：

进一步可表达为：

其中，

S4110、基于ESO的分布式运动学制导律设计为：

其中，

及

为正的常数。

S42、采用模糊逻辑系统实现对模型不确定及外界干扰的逼近及建立线性解析模型来描述输入量化过程的方法，设计船舶动力学自适应模糊量化控制器。

S421、基于牛顿力学和拉格朗日力学，欠驱动USV动力学非线性数学模型可表述为：

其中，

为USV质量，

为水动力导数项，I_z为关于z轴的惯性矩；f_u(·),f_v(·),f_r(·)为向心力、科氏力和水动力阻尼构成的非线性项；τ_uw,τ_vw,τ_rw为未知的环境干扰；Q(τ_u),Q(τ_r)为控制输入τ_u,τ_r的量化值；

S422、均值量化器可表述为：

其中，o_u,o_r为量化参数，经过量化器后，控制信号τ_i变成Q(τ_i)，这个过程中会产生量化误差，相当于丢失部分控制信息，继而导致控制系统性能下降；为了保证USV动力学子系统能够有效跟踪制导输入，必须设计一种具有足够精度且较低通信速率的量化控制方案；

S423、根据欠驱动USV动力学非线性数学模型可得：

动力学子系统的控制目标为：

其中，δ₁,δ₂为小的正常数；

S424、将步骤S423的公式进行如下转换：

其中，

S425、令Q(τ_u)＝q_1u(t)τ_u+q_2u(t),Q(τ_r)＝q_1r(t)τ_r+q_2r(t)，且：

其中，q_1u(t)和q_1r(t)未知，由于量化过程符号不变，可知q_1u(t)＞0，q_1r(t)＞0，当|τ_u(t)|＜a,|τ_r(t)|＜a时，Q(τ_u(t)),Q(τ_r(t))是有界的，因此q_2u(t),q_2r(t)是有界的，取

S426、定义积分滑模面为：

其中，s＝[s_u,s_r]^T,c_u＞0,c_r＞0；

S427、对积分滑模面求导可得：

S428、令Δ_u(X)＝d_u+F_u,Δ_r(X)＝d_r+F_r,可得：

S429、由于Δ_u(X)为未知项，根据万能逼近定理，对于任意小的常数ε_N，存在模糊逻辑系统

使得

其中

为模糊系统的理想权重；h_u(X)为模糊基向量，ε_u为模糊系统的逼近误差，满足|ε_u|≤ε_N，令

为Δ_u(X)的估计值，且

其中

为理想权重

的估计值，且

S4210、定义：

其中，l_u,η_u为正常值，且满足η_u≥ε_N+η_d,η_d＞0；由于：

可得：

其中，

S4211、

可得：

其中，

为Δ_r(X)的估计值；

为模糊系统的理想权重；

为W_r的估计值，且

ε_r为模糊系统的逼近误差，满足|ε_r|≤ε_N；l_r,η_r为正的常值，满足η_r≥ε_N+η_d,η_d＞0；

S4212、由于q_1u(t)和q_1r(t)时变且未知，需要进行自适应估计，采用对其下界进行估计的方法，防止估计值为零时引发的奇异问题，定义时变增益μ_u＝1/q_1u(t)_min,μ_r＝1/q_1r(t)_min，其中q_1u(t)_min,q_1r(t)_min分别为q_1u(t),q_1r(t)的下界；

S4213、设计自适应动力学控制律和自适应律为：

其中，γ₁,γ₂,γ₃,γ₄,ρ_u,ρ_r,

为正常值；

S4214、基于

s_u,s_r,

所构成的误差动态系统可表述为：

具体实施时，作为本发明优选的实施方式，所述步骤S5具体包括：

S51、运动学误差子系统稳定性证明；

S511、考虑如下李雅普诺夫函数：

其中，P为正定矩阵，满足：

A^TP+P^TA≤I

S512、对式

求导可得：

由于

使得：

其中，

运动学子系统是输入状态稳定的，且：

其中，

S52、动力学误差子系统输入状态稳定性证明。

S521、将动力学误差子系统看作一个状态为s_u,s_r,

输入为D_u,D_r,

的系统，构建李雅普诺夫函数：

其中，

S522、对其求导可得：

S523、将步骤S4211和步骤S4212中的

带入到步骤S521中，可得：

S524、将动力学控制律、自适应律、带入S522式中可得：

S525、考虑到：

q_1u≥q_1umin＝1/μ_u＞0,q_1r≥q_1rmin＝1/μ_r＞0,-η_u|s_u|+ε_us_u≤0,-η_r|s_r|+ε_rs_r≤0,

可得：

基于步骤S525中的公式，以及步骤S524中的不等式可写为：

其中，

S526、将动力学误差动态带入式S525中可得：

定义

可得：

其中，

因此动力学误差子系统是输入状态稳定的，且：

其中，

且：

P_c＝diag{1,1/2γ₂μ_u,1/2γ₄μ_r,1/2γ₁,1/2γ₃}

S527、考虑由USV运动学模型、动力学模型、运动学制导律、动力学底层控制律、自适应律、构成的闭环系统，基于级联系统稳定性理论，结合S51和S52稳定性证明，由运动学误差子系统和动力学误差子系统构成的级联系统是输入状态稳定的；由S512和S526中的||E₁(t)||、||E₂(t)||可得，||E₂(t)||是有界的，即：

因此，闭环系统是稳定的。

实施例

为了验证模拟退火算法改进的人工势场法进行船舶路径规划的有效性及欠驱动无人船对期望航线的路径跟踪和控制器的有效性，本发明采用MATLAB/Simulink进行路径规划及跟踪控制仿真，参数设置参见步骤S1。

如图4-5所示，采用对比试验，通过简单障碍物环境下传统人工势场法与采用模拟退火算法改进的人工势场法路径规划的实验，及为较复杂障碍物环境下传统人工势场法与采用模拟退火算法改进的人工势场法路径规划的实验对比可以得出：在简单障碍物条件下，传统人工势场法与改进人工势场法都可规划出一条由起点到终点的有效路径，但改进的人工势场法路径更平滑，且具有可行性；在较为复杂的障碍物条件下，传统人工势场法的路径规划出现了目标不可达的缺陷，而采用改进斥力函数并采用模拟退火算法改进的人工势场法克服了传统人工势场法的缺陷，对于较复杂的路径规划更加有效。

如图6-9所示，图中对拟合出来的曲线进行路径跟踪。可以看出，船舶在跟踪路径时，在外界干扰,系统不确定性,量化误差等多重影响下，位置和航向角与规划航线没有较大偏差，船舶在扩张观测器的观测下，可以实现对期望路径跟踪以及海流所引起的运动学偏移的补偿，完成从起始点到目标点的跟踪任务。并且θ_d在区间(-π/2,π/2)内，具有良好的跟踪效果。

如图10-11所示，设计一种自适应模糊量化控制器，跟踪运动学制导律给出的期望速度，来解决在真实的海洋环境航行时，无人船在流对船舶造成定向偏移情况下，受到海洋环境的干扰和系统存在不确定性，对船舶航行造成影响的问题。

仿真实验结果表明，采用模拟退火算法改进传统人工势场法对船舶路径进行规划，改善了传统人工势场法目标不可达和陷入局部极小值的问题；针对改进人工势场法的规划航线作为参考跟踪路径，通过多项式拟合，使规划航线变成可以表达的多项式，作为船舶运动学控制子系统的参考输入信号；考虑对期望路径的跟踪以及海流所引起的运动学偏移的补偿，基于扩张观测器设计船舶运动学制导律，即前向速度引导律、转艏角速度引导律；考虑船舶动力学子系统的特性，研究了船舶动力学控制问题，通过引入模糊逻辑系统实现对系统模型不确定及风、浪干扰的逼近，通过一种线性解析模型来描述输入量化过程,在量化参数未知的假设下，设计了一种自适应模糊量化控制器，实现了对运动学制导信号的稳定跟踪。验证了欠驱动无人船在考虑流对船舶造成定向偏移情况下，采用带有输入量化器的模糊控制器可以较好的跟踪规划的最优路径，完成从起始点到目标点的跟踪任务，实现欠驱动无人船的路径规划与跟踪控制。

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

Claims (10)

1.一种带有输入量化的USV路径规划与模糊自适应跟踪控制方法，其特征在于，包括如下步骤：

S1、获取周围环境及周围其他船舶的海况信息，设置参数；

S2、采用模拟退火算法改进传统人工势场法对所述海况信息下的本船进行合理规避障碍物的路径规划；

S3、对规划的路径作曲线拟合得出优化后的路径，并将优化后的路径用一条函数曲线表示，将曲线作为船舶运动控制系统的期望输入；

S4、设计基于扩张状态观测器的运动学制导律和自适应模糊量化控制器，对船舶规划路径进行跟踪；

S5、基于输入的状态稳定性理论证明闭环系统的稳定性，通过仿真实验验证算法的有效性。

2.根据权利要求1所述的带有输入量化的USV路径规划与模糊自适应跟踪控制方法，其特征在于，所述步骤S1具体包括：

S11、建立正方形环境地图；

S12、在环境地图中建随机建立n个黑色圆形障碍物，障碍物的圆心用一个n*2的向量表示；

S13、建立欧式坐标系，设置船舶起始点的位置和目标点的位置；

S14、设置人工势场法的步长、引力增益系数、斥力增益系数、障碍影响距离；设置模拟退火算法中的初始温度、终止温度，并设置循环次数；在对比实验中，传统人工势场法与改进人工势场法采用相同的参数；

S15、考虑USV控制系统，设置理想化的规划路径，使得USV沿着拟合后的时变参数化轨迹航行；选取USV的参数，设置USV的初始状态，设置底层控制器参数，设置量化参数。

3.根据权利要求1所述的带有输入量化的USV路径规划与模糊自适应跟踪控制方法，其特征在于，所述步骤S2具体包括：

S21、设本船所在位置为(x,y)^Τ，X为本船位置向量，X_d为目标点位置向量，则本船在势场中受到目标点的引力为F_att(X)，所受到的障碍物的斥力为F_rep(X)，引力与斥力的合力为F_total(X)，合力F_total(X)的方向为本船改变的目标航向；

S22、根据人工势场法原理，目标点对本船产生的引力F_att(X)会随着本船与目标点间的距离的增加而单调递增，将本船的引力势场函数表示为：

其中，K_a为引力势场常量，(X-X_d)²为本船到目标点的相对距离；

S23、重新定义一个斥力函数，假定以障碍物为中心的斥力势场影响范围为ρ₀，本船与障碍物的距离为ρ，新的势场函数中考虑了船舶与目标点之间的距离，在原有的斥力函数中添加一个系数项：|X-X_d|ⁿ，改进后的斥力函数为：

其中，U_rep(X)为障碍物的斥力场，k_r则表示斥力势场常数，是正数，ρ为船舶在空间位置中与障碍物间距离，ρ₀为以障碍物为中心的斥力势场影响范围，X为船舶位置向量，X_d为目标点位置向量，ρ(X,X_g)表示船舶到障碍物的欧几里得距离，n为一个大于零的任意实数；

S24、利用模拟退火算法突跳性对模型进行局部最优优化；在程序中，给定一个初始温度以及一个初始状态，并给定合适的退火速度，随着本船的行进，初始温度降低，在温度降低的过程中，模拟退火算法对每一个状态下的势场强度进行计算，同时，在每一个状态下给定一个随机扰动，并计算新状态下势场强度与初始状态势场强度之差，并对差值进行分析；

S25、按照Metropolis准则，若差值小于等于零，则程序会接受新状态；若差值大于零，则以一定的概率接受新状态，反之会保留原初始状态；如此往复，避免本船在行进中产生局部极小值和目标不可达的情况，使船舶安全到达目标点，规划出合理避障的最优路径。

4.根据权利要求1所述的带有输入量化的USV路径规划与模糊自适应跟踪控制方法，其特征在于，所述步骤S3具体包括：

对规划出的最优路径进行平滑处理，并作为输入信号进行路径跟踪；使用MATLAB中ployfit函数对最优路径进行曲线拟合，拟合函数如下所示：

y＝a₁xⁿ+a₂x^n-1+a₃x^n-2+...+a_nx+a_n+1

上式为一个多项式，在拟合过程中，需要对不同的多项式次数进行拟合；拟合次数少，路径拟合效果比较差，拟合次数过多，会出现龙格现象，只有当误差最小时，才会出现理想的拟合效果。

5.根据权利要求1所述的带有输入量化的USV路径规划与模糊自适应跟踪控制方法，其特征在于，所述步骤S4具体包括：

S41、基于扩张状态观测器，对期望路径的跟踪以及海流所引起的运动学偏移补偿，设计船舶运动学制导律；

S42、采用模糊逻辑系统实现对模型不确定及外界干扰的逼近及建立线性解析模型来描述输入量化过程的方法，设计船舶动力学自适应模糊量化控制器。

6.根据权利要求5所述的带有输入量化的USV路径规划与模糊自适应跟踪控制方法，其特征在于，所述步骤S41具体包括：

S411、构建船舶平面运动模型；

S412、将船舶的运动学方程表示为：

其中，x,y,

分别为惯性坐标系下USV的横向位置、纵向位置和艏摇角；u,v,r为附体坐标系下USV的前向速度、横向速度和艏揺速度；u_c,

分别表示海流的速度及流向；

S413、定义USV误差为：

其中，旋转矩阵

表示为：

S414、对误差进行求导，可得：

其中，

S415、为解决船舶欠驱动的问题，设置误差转移方程为：

其中，δ₀∈R为正的常值；

S416、将误差转移方程带入求导的误差中可得：

其中，h＝diag{d,δ₀}，

S417、假设未知项σ满足

其中σ^*为正常数；航海实践中USV的速度及加速度均有上界，因此假设合理；

S418、假设海流流速u_c与流向

未知，因此σ为未知项，采用扩张状态观测器进行估计：

其中，

为ESO的估计误差；

S419、根据步骤S416和步骤S418可得：

进一步可表达为：

其中，

S4110、基于ESO的分布式运动学制导律设计为：

其中，

及

为正的常数。

7.根据权利要求5所述的带有输入量化的USV路径规划与模糊自适应跟踪控制方法，其特征在于，所述步骤S42具体包括：

S421、基于牛顿力学和拉格朗日力学，欠驱动USV动力学非线性数学模型可表述为：

其中，

为USV质量，

为水动力导数项，I_z为关于z轴的惯性矩；f_u(·),f_v(·),f_r(·)为向心力、科氏力和水动力阻尼构成的非线性项；τ_uw,τ_vw,τ_rw为未知的环境干扰；Q(τ_u),Q(τ_r)为控制输入τ_u,τ_r的量化值；

S422、均值量化器可表述为：

其中，o_u,o_r为量化参数，经过量化器后，控制信号τ_i变成Q(τ_i)；

S423、根据欠驱动USV动力学非线性数学模型可得：

动力学子系统的控制目标为：

其中，δ₁,δ₂为小的正常数；

S424、将步骤S423的公式进行如下转换：

其中，

S425、令Q(τ_u)＝q_1u(t)τ_u+q_2u(t),Q(τ_r)＝q_1r(t)τ_r+q_2r(t)，且：

其中，q_1u(t)和q_1r(t)未知，由于量化过程符号不变，可知q_1u(t)＞0，q_1r(t)＞0，当|τ_u(t)|＜a,|τ_r(t)|＜a时，Q(τ_u(t)),Q(τ_r(t))是有界的，因此q_2u(t),q_2r(t)是有界的，取

S426、定义积分滑模面为：

其中，s＝[s_u,s_r]^T,c_u＞0,c_r＞0；

S427、对积分滑模面求导可得：

S428、令Δ_u(X)＝d_u+F_u,Δ_r(X)＝d_r+F_r,可得：

S429、由于Δ_u(X)为未知项，根据万能逼近定理，对于任意小的常数ε_N，存在模糊逻辑系统

使得

其中

为模糊系统的理想权重；h_u(X)为模糊基向量，ε_u为模糊系统的逼近误差，满足|ε_u|≤ε_N，令

为Δ_u(X)的估计值，且

其中

为理想权重

的估计值，且

S4210、定义：

其中，l_u,η_u为正常值，且满足η_u≥ε_N+η_d,η_d＞0；由于：

可得：

其中，

S4211、

可得：

其中，

为Δ_r(X)的估计值；

为模糊系统的理想权重；

为W_r的估计值，且

ε_r为模糊系统的逼近误差，满足|ε_r|≤ε_N；l_r,η_r为正的常值，满足η_r≥ε_N+η_d,η_d＞0；

S4212、由于q_1u(t)和q_1r(t)时变且未知，需要进行自适应估计，采用对其下界进行估计的方法，防止估计值为零时引发的奇异问题，定义时变增益μ_u＝1/q_1u(t)_min,μ_r＝1/q_1r(t)_min，其中q_1u(t)_min,q_1r(t)_min分别为q_1u(t),q_1r(t)的下界；

S4213、设计自适应动力学控制律和自适应律为：

其中，γ₁,γ₂,γ₃,γ₄,ρ_u,ρ_r,

为正常值；

S4214、基于

s_u,s_r,

所构成的误差动态系统可表述为：

8.根据权利要求1所述的带有输入量化的USV路径规划与模糊自适应跟踪控制方法，其特征在于，所述步骤S5具体包括：

S51、运动学误差子系统稳定性证明；

S52、动力学误差子系统输入状态稳定性证明。

9.根据权利要求8所述的带有输入量化的USV路径规划与模糊自适应跟踪控制方法，其特征在于，所述步骤S51具体包括：

S511、考虑如下李雅普诺夫函数：

其中，P为正定矩阵，满足：

A^TP+P^TA≤I

S512、对式

求导可得：

由于

使得：

其中，

运动学子系统是输入状态稳定的，且：

其中，

10.根据权利要求8所述的带有输入量化的USV路径规划与模糊自适应跟踪控制方法，其特征在于，所述步骤S52具体包括：

S521、将动力学误差子系统看作一个状态为s_u,s_r,

输入为D_u,D_r,

的系统，构建李雅普诺夫函数：

其中，

S522、对其求导可得：

S523、将步骤S4211和步骤S4212中的

带入到步骤S521中，可得：

S524、将动力学控制律、自适应律、带入S522式中可得：

S525、考虑到：

q_1u≥q_1umin＝1/μ_u＞0,q_1r≥q_1rmin＝1/μ_r＞0,-η_u|s_u|+ε_us_u≤0,-η_r|s_r|+ε_rs_r≤0,

可得：

基于步骤S525中的公式，以及步骤S524中的不等式可写为：

其中，

S526、将动力学误差动态带入式S525中可得：

定义

可得：

其中，

因此动力学误差子系统是输入状态稳定的，且：

其中，

且：

P_c＝diag{1,1/2γ₂μ_u,1/2γ₄μ_r,1/2γ₁,1/2γ₃}

S527、考虑由USV运动学模型、动力学模型、运动学制导律、动力学底层控制律、自适应律、构成的闭环系统，基于级联系统稳定性理论，结合S51和S52稳定性证明，由运动学误差子系统和动力学误差子系统构成的级联系统是输入状态稳定的；由S512和S526中的||E₁(t)||、||E₂(t)||可得，||E₂(t)||是有界的，即：

因此，闭环系统是稳定的。

Images