CN112947068B - 一种自主水下航行器一体化声学定位与跟踪控制方法 - Google Patents

Abstract

本发明公开了一种自主水下航行器一体化声学定位与跟踪控制方法。本发明在水面布置三个发射周期性声学信号的浮标，根据三个信号达到AUV的时间，结合声速剖面对AUV位置进行估计。同时，采用扩展卡尔曼滤波处理由位置估计和洋流干扰引起的系统不确定性；为了有效处理AUV系统物理约束条件并实现一定的跟踪效果最优，考虑AUV水动力学模型，设计路径跟踪模型预测控制(MPC)器。由于定位与控制具有不同的采样时间，利用AUV名义模型对无声学定位信息的控制时间步进行航迹推算。考虑算法实时性，采用MPC框架下的持续线性化方法对系统模型进行近似处理，取得控制效果与计算效率的权衡。

Description

一种自主水下航行器一体化声学定位与跟踪控制方法

技术领域

本发明属于海洋移动平台智能化技术，涉及一种自主水下航行器 (Autonomousunderwater vehicles,AUVs)声学定位与路径跟踪控制一体化设计方法，尤其是针对存在于声学定位与环境干扰如洋流等的不确定性，提出一种基于扩展卡尔曼滤波的系统状态估计算法，解决AUV水下环境中定位难和运动易受洋流影响的问题。

背景技术

自主水下航行器(Autonomous underwater vehicles,AUVs)已广泛用于如军事、商业、海洋观测与资源探测等多种场合。与传统水下航行器相比，AUVs不需要人类驾驶员工作在危险和恶劣的水下环境中。与有缆远程控制水下航行器相比，AUVs能到达更广泛的海下空间，同时也避免了绳缆对航行器运动的妨碍。然而，水下环境中由于GPS(Globalpositioning system)信号的缺失，存在定位难的问题。同时，水下环境普遍存在洋流干扰，对AUV实现完全自主运动带来了更大的挑战。

水下定位技术通常分为两种，即与距离无关的定位技术和基于距离的定位技术。与距离无关的定位技术直接利用信标节点的拓扑结构对水下目标实现位置估计。基于位置的定位技术常见的包括接收信号强度指示方法、到达时间差方法 (time difference ofarrival,TDOA)和达到时间法(time of arrival,TOA)。这三种方法主要区别于距离的计算方法不同。接收信号强度方法的效果常受多径效应和强度衰减的影响。相比于TDOA方法，TOA方法需要更少的信标节点，计算和实现成本更低。各种基于基线定位的方法便是从TOA方法衍化而来。然而，TOA 方法需要各节点与AUV间实现高精度的时间同步。定位误差与噪音的处理也对定位准确性有一定影响。常用的定位误差处理方法包括最小二乘法和卡尔曼滤波方法。

AUV自主运动面临的另一个问题是洋流干扰的影响。各种区域海洋模型可以生成高精度的洋流场用于AUV环境仿真。考虑AUV在有障碍物或近海环境中的航行安全，可以对洋流的随机概率分布进行预测从而避免AUV发生碰撞等危险。洋流场也可以建模为一种时空分布的向量场，并进一步构建成一幅洋流欧拉地图。这种洋流欧拉地图便能对AUV的路径规划和控制模块提供输入已实现对能耗和路径长度等性能指标的优化。另一种处理洋流的方法是直接将洋流视为干扰，设计观测器对洋流产生的影响进行估计，并进行补偿。常用的洋流影响观测方法也包括卡尔曼滤波方法。

基于定位与环境洋流信息，AUV运动控制模块便能计算系统所需输入完成特定水下任务。由于实现方便，工程实际中应用最广泛的控制方法是比例-积分- 微分(Proportional-integral-derivative,PID)方法。PID方法长于增益调节或模糊逻辑等方法结合以处理非线性系统。李雅普诺夫方法如反步法、滑模控制等方法可以处理非线性和保障收敛，也经常用于AUV空中问题中。然而，这类方法通常需要反复的参数调节，当系统特性变化时便不再适用，且控制效果根据任务、环境变化很大。模型预测控制(Modelpredictive control,MPC)能有效处理系统约束条件，且实现一定的性能指标优化。MPC可用于AUV深度控制、路径跟踪控制等场景。但是，现有应用中多假设AUV定位信息完美已知，并不符合现实。

水下环境中GPS定位信号缺失，且水下环境普遍存在洋流干扰，对AUV实现完全自主运动带来较大挑战。现有技术多将AUV定位与控制问题分开解决，未考虑定位精度、环境干扰等因素对AUV运动控制影响。因此，研究AUV一体化声学定位与跟踪控制技术。

发明内容

本发明的目的是克服现有技术的不足，考虑在水面布置多个发射周期性声学信号的浮标，根据浮标信号达到AUV的时间，并结合声速剖面对AUV位置进行估计。同时，采用扩展卡尔曼滤波处理由位置估计和洋流干扰引起的系统不确定性。为了有效处理AUV系统物理约束条件并实现一定的跟踪效果最优，考虑 AUV水动力学模型，设计路径跟踪模型预测控制器。另一方面，由于定位与控制具有不同的采样时间，利用AUV名义模型对无声学定位信息的控制时间步进行航迹推算。考虑算法实时性，采用MPC框架下的持续线性化方法对系统模型进行近似处理，取得控制效果与计算效率的权衡。

本发明的技术方案如下：

本发明公开了一种自主水下航行器一体化声学定位与跟踪控制方法，其流程图如图1所示，具体包括如下步骤：

1)建立非线性带洋流干扰不确定性的AUV动力学模型；

2)部署三个已知位置的水面浮标，每个浮标带有一个声波发射器，每间隔

秒发出一次声音信号；根据声音信号到达AUV的时间，对AUV位置进行估计，获得AUV的状态；

3)考虑洋流干扰和声学观测中的不确定性，利用扩展卡尔曼滤波(EKF) 对步骤2)获得的AUV状态进行校正；校正后的AUV状态用于模型预测控制(MPC)的反馈；

所述模型预测控制(MPC)利用系统模型预测系统在未来一段有限时间内的轨迹，在每一个时间步在线求解基于预测轨迹的优化问题，得到最优控制序列反馈控制AUV，并以滚动时域的方式重复；

所述的系统模型为持续线性化预测模型，其通过对步骤1)所述动力学模型进行持续线性化获得。

附图说明

图1为本发明方法的流程示意图；

图2为AUV各自由度及自身坐标系和惯性坐标系和洋流影响；

图中，1.惯性坐标系，2.自身坐标系，3.洋流，4.AUV；

图3：基于水面三个固定浮标的AUV声学定位示意图；

图中，3-1.水面浮标1，3-2.水面浮标2，3-3.水面浮标3，3-4.海面，3-5. AUV，3-6.海底；

图4：水面三个浮标位置及AUV运行轨迹；

图5：从水面三个浮标的声信号到达AUV的时间；

图6：真实横向距离与二分法搜索估计的横向距离误差；

图7：AUV跟踪控制轨迹；

图8：AUV速度约束条件；

图9：AUV控制输入约束条件；

图10：算法求解耗费时间；

图11：无声学定位反馈的AUV跟踪控制。

具体实施方式

下面结合具体实施方式对本发明做进一步阐述和说明。本发明中各个实施方式的技术特征在没有相互冲突的前提下，均可进行相应组合。

为了对本发明做详细的阐述，以下分为AUV动力学及洋流建模、AUV声学定位、基于EKF和声学定位的AUV预测跟踪控制和仿真实验四部分对本发明实施例的具体内容和技术效果进行阐述。

一、AUV动力学及洋流建模

为实现AUV自主运动控制，首先对AUV进行动态建模。运动学模型表达 AUV的位移与速度关系，动力学模型根据牛顿第二定律，建立AUV在流体中运动时，作为刚体在外力、水动力的作用下质量、附加质量和加速度之间的关系。同时，AUV在水下环境中运动时，也不可避免的受到洋流的影响，如图2所示。

首先，对AUV模型作如下假设：1)AUV自身坐标系{b}的原点O_b与AUV 的重心重合；2)AUV在固定深度航行，所以AUV6自由度运动可以简化为水平面3自由度运动；3)AUV是右舷对称的；4)AUV浮力与重心重合。基于这些假设，AUV动力学模型可表示为：

这里η＝[x y ψ]^T表示惯性坐标系{n}中的位姿状态，v＝[u v r]^T表示自身坐标系{b}中的速度状态，该速度更精确的定义是点O_b相对于{b}表示在{n}中的速度。在自身坐标系{b}中还定义了洋流速度矢量v_c。被控力和力矩是τ＝[ξ Y_uuδu²δ N_uuδu²δ]。这里ξ和δ，即系统输入，分别是螺旋桨推力和舵角，Y_uuδ和N_uuδ是相应的水动力参数。旋转矩阵R(ψ)关联{n}和{b}两个坐标系中的系统状态：

其中ψ是航向角，且R满足

在(2)中，v_r＝v-v_c＝[u_r v_r r_r]^T是在{b}中AUV船体和洋流之间的相对速度。为了简化，我们考虑了无旋定速洋流，表示为b＝[V_c β_c]^T，其中V_c和β_c分别是{n}中的洋流速度和角度，

因此

刚体和附加质量矩阵分别表示为

和

同样，C_RB和C_A分别是刚体和附加科里奥利矩阵和向心矩阵。阻尼效应由D_L和非线性部分D_NL建模。附加和阻尼矩阵是由于海上航行器在流体中运动时的水动力效应所致。由于 AUV系统限制，AUV速度、螺旋桨最大推力和方向舵最大转角分别限制如下：

u_min≦u≦u_max，(4)

ξ_min≦ξ≦ξ_max，(5)

δ_min≦δ≦δ_max.(6)

AUV从指定起始点按照一定轨迹到达目的地。该区域的洋流信息，即V_c和β_c，通常可以由区域海洋模型预测，也可以由AUV自身携带的传感器粗略测量，但这些预测存在随机不确定性。因此，将洋流建模为

这里

在惯性坐标系中，

分别是预测的平均洋流速度和角度；

是反映预测精度的对应协方差，其中diag表示对角矩阵。

二、AUV声学定位

利用声音在水中的有限传播速度和海洋表面存在GPS或GNSS定位的特点，部署三个已知位置[x_n,y_n,z_n]^T,n＝1,2,3的水面浮标，如图3所示。然而，声波的水下的传播速度一般不是恒定的，而是随着水压、温度和电导率的变化而变化。因此，声波的传播路径实际上是弯曲的，从而导致信号到达时间(Time of arrival, TOA)存在不确定性。为了克服这一困难，在AUV声学定位问题中，利用等梯度声速剖面情况下的射线跟踪技术解决AUV水下定位问题。

三个浮标中的每一个浮标都带着一个声波发射器，每间隔

秒时间就发出一次声音信号。将AUV在时间t的位置表示为[x(t),y(t),z(t)]^T。为简化起见，假设：1)AUV在恒定的深度z米运动；2)三个水面浮标的位置大致固定，使用系线固定或使用动态定位方法确定自身位置，其深度为0米；3)对于水面浮标和AUV之间的射线跟踪问题，其中的声速剖面是等梯度的，声速仅取决于水深，表示为下式：

c(z)＝az+b (7)

其中c为声速，a为取决于水下环境的常数，b为水面的声速。接下来，在仿真模拟中可以将AUV处接收到三个浮标信号的TOA，表示为Δt_n，n＝1，2，3，具体计算步骤为：

X_n(t)＝z/d_n(t) (10)

Y_n(t)＝L_n(t)/X_n(t) (11)

α_n(t)＝arctan(Y_n(t)) (12)

β_n(t)＝arctan(X_n(t)) (13)

其中，d_n代表浮标n与AUV之间的实际水平距离；L_n、X_n和Y_n为中间变量；α_n为浮标n的实际声射线与直线路径之间的夹角；β_n为浮标n的水平方向与直线路径之间的夹角；最后，

和

分别为浮标n和AUV处的掠射角。

在仿真中，TOA即Δt_n，n＝1，2，3，可按式(16)计算，在实验中，可以利用已经同步过的时钟测量。然后，在已知TOA、AUV深度z和浮标固定位置的情况下，可以利用二元搜索等算法估计水平距离d_n。其主要思想是反复比较到达时间的中值点和实际传播时间，从而缩小搜索范围，直到找到足够精确的水平距离估计值

接下来，我们就可以利用

计算AUV的位置。如果在一个信号发射-接收周期内，AUV缓慢移动，可以假设AUV分别接收到1、2、3号浮标的三个信号时位置相同，那么我们可以得到一组非线性方程组：

通过对其求解，我们得到AUV的声学位置估计

需要注意的是，在实际中，只需要两个方程就可以得到

但是，为了提高浮标失效时的系统鲁棒性，部署了三个浮标。在实验中，由于接收可靠的声学信号通常是一个问题，因此在AUV声学定位中，冗余的备用浮标是必不可少的。

通常，AUV水平航向是由惯性测量单元等机载传感器测量的，记作

此外，由于存在于声速剖面模型、时钟同步误差、测量TOA、水面浮标位置 [x_n,y_n,z_n]^T以及航向角测量系统中的不确实性，假设AUV“测量”输出的

受到随机噪声ε～N(0,∑)的影响，其中ε为

向量，协方差矩阵满足

其中

和

为相应的标准差。因此，AUV水平姿态参数可以表述为：

这可以看作是系统的“测量”输出。

三、基于EKF和声学定位的AUV预测跟踪控制

利用声学定位的状态反馈信息和非线性带洋流干扰不确定性的AUV动力学模型，提出基于模型预测控制(Model predictive control,MPC)和EKF的轨迹跟踪控制器。MPC利用系统模型预测系统在未来一段有限时间内的轨迹。在每一个时间步在线求解基于预测轨迹的优化问题，并以滚动时域的方式重复此过程，因此也称为滚动时域控制。为了减轻采用(1)-(2)作为预测模型可能带来的计算负担，一种MPC框架下的持续线性化被应用于非线性模型(1)-(2)。此外，提出采用EKF方法处理洋流干扰和声学观测中的不确定性。EKF修正后的系统状态信息作为状态反馈用于预测跟踪控制器中。由于控制采样时间通常小于声学定位采样时间，在没有声学定位信息反馈的控制时间步则采用航迹推算进行状态反馈。

3.1持续线性化预测模型

持续线性化的基本思想是利用前一时间步的计算而未应用的最优控制输入序列，得到系统在整个预测时域上的线性化点。此方法区别于传统的仅将系统在单点进行线性化，一定程度上减小线性化误差，提高控制性能。

将AUV动力学模型(1)-(2)概括描述为：

其中

是一个光滑函数，x＝[η^T,v^T]^T是系统状态，

是控制输入向量，b于前文定义相同。为了方便数字仿真，连续模型采用零阶保持法进行离散化：

其中时间k与实际时间t的关系为：

为采样时间。注意，控制采样时间

不必与定位采样时间

相同，在每个控制时间步k，连续线性化的主要实现步骤如下：

1、移动来自前一个时间步的最优控制输入序列u^*(i∣k-1),i＝0,1,…,N_p-1，N_p是预测时域长度，用于求得“种子”控制输入轨迹u⁰(i∣k)：

u⁰(i∣k)＝u^*(i∣k-1)，i＝0,1,…,N_p-2,

u⁰(N_p-1∣k)＝u^*(N_p-2∣k-1)，i＝N_p

2、将u⁰(i∣k)应用到(20)得到“种子”状态轨迹x⁰(i∣k),i＝0,1,…,N_p-1。起始状态设置为x⁰(0∣k)＝x(k)，x(k)是当前系统状态。初始干扰值设置为：

方便起见其中

恒定为

2、将非线性动力学模型在(x⁰(i∣k),u⁰(i∣k),b⁰(i∣k))进行线性化，i＝0,1,…,N_p-1

x(i+1∣k)＝x⁰(i+1∣k)+A(i∣k)Δx(i∣k)+

B(i∣k)Δu(i∣k)+E(i∣k)Δb(i∣k) (22)

其中

分别是x⁰(i∣k),u⁰(i∣k),b⁰(i∣k)周围的扰动，A(i∣k),B(i∣k)，E(i∣k)是 x⁰(i∣k),u⁰(i∣k),b⁰(i∣k)处相应的雅可比矩阵。

3.2基于声学定位信息的EKF

卡尔曼滤波器是一种广泛使用的正确预测公式，用于处理速度估计和随机环境干扰。EKF通过线性化进一步将系统动力学扩展到非线性类型，以传播状态估计和协方差的近似值。基于模型(18)，(20)和(22)，便可进行EKF设计。

假设

且n≥2。在控制时间步k，当有新的声学定位信息时，即在定位步k/n，通过下式校正AUV状态。

其中

是输出矩阵，

是忽略不确定因素时的非线性模型的计算得到的状态，即：

其中f_d代表(20)的离散化结果。注意，带有上划线的参数

代表系统的名义状态信息，即不考虑不确定性时的信息。卡尔曼增益K(k)为：

误差

的协方差矩阵

通过以下递归方法计算：

P(k)代表估计误差的协方差。

通过此标准卡尔曼滤波程序，就可以通过声学“测量”输出y(k)校正名义系统状态

修正的状态

可以用于MPC反馈控制的设计。但是，如前所述，控制周期通常比定位周期短，

这表示y(k)不一定在每个时间k内都可用。当声学测量无法得到时，将利用(25)中的名义状态

用于反馈，也通常称为航位推算技术。

3.3预测跟踪技术

在每个时间步k，MPC会基于当前系统状态在预测时域N_p上计算未来的控制序列。以使将预测的系统轨迹接近给定的参考值，优化计算得到的控制序列的第一个元素被应用到系统。在下一个时间步k+1，将使用新测量的系统状态重复此过程。对于AUV跟踪问题，施加了系统约束(4)–(6)。接下来，提出 MPC方法，利用EKF的校正信息或来自航位推算技术的信息作为反馈来解决 AUV跟踪问题。

考虑系统参考轨迹如下：

其中ψ_d＝arctan((y_d-y₀)/(x_d-x₀))然后通过求解带约束条件的有限时域上的二次规划问题：

约束条件为：

u_min≤u(i∣k)≤u_max,i＝0,1,…,N_p

ξ_min≤ξ(i∣k)≤ξ_max,i＝0,1,…,N_p-1

δ_min≤δ(i∣k)≤δ_max,i＝0,1,…,N_p-1

其中

目标函数(27)旨在通过以最小的代价实现最小的跟踪误差。其中三项分别是终端跟踪误差成本，阶段跟踪误差成本和控制输入成本。Q_f,Q,R是各个成本项的正定权矩阵，可以进行调整以设置收敛速度和控制成本之间的优先级。由于系统状态和控制输入的限制，优化问题没有解析解，需要使用新的“测量”系统状态和时变预测模型在线重复地求解，但是由于连续线性化，上述优化问题变成凸优化问题从而可以得到快速的求解。

四、仿真实验

通过仿真实验验证所提AUV跟踪控制算法的有效性。AUV起始点位于[100， 100，300]m处，要求以1m/s的参考速度沿着参考路径到达目的地[600，600， 300]m处。三个水面浮标分别位于[0，0，0]m，[500，800，0]m和[1000，0， 0]m处。声速剖面的参数设置为：a＝-0.05和b＝1540m/s。在这个区域中，洋流的平均速度预测为0.5m/s，角度为π/6。然而，预测值存在不确定性，服从协方差∑_b＝diag([0.1m²/s²0.01rad²])的正态分布。不确定声学测量的协方差∑＝diag([0.1m²0.1m²0.01rad²])。控制器参数设置如下:预测地平Np＝10 权重参数

R＝diag([100 100])；采样时间

系统约束条件为：u_max＝-u_min＝2m/s，ξ_max＝-ξ_min＝86N，δ_max＝-δmin＝13.6π/180rad。所有算法均在Intel(R)Core(TM)i3-7100 CPU@ 3.70GHz的平台上实现求解。

4.1.基于声学定位的跟踪控制

4.1.1.声学定位结果

三个浮标位置如图4所示，通过(8)-(16)计算的三个浮标信号的TOAs如图5 所示。可以看出，随着AUV从初始位置移向目的地，AUV与浮标1之间的距离逐渐增大，与浮标2和浮标3的距离逐渐减小。因此，图5中浮标1的TOA轨迹上升，而其他两条轨迹下降。需要注意的是，AUV比浮标3更快地接近浮标 2，浮标2的TOA轨迹下降的速度也比浮标3更快。

图6进一步显示了用二分搜索法算法计算的实际水平距离d_i(t)和估计水平距离

之间的距离误差，可以看到该算法能实现较小的估计误差。

4.1.2.跟踪控制结果

图7表明，所提算法可以实现轨迹跟踪目标，并控制AUV沿路径从初始位置移动到目的地。在图7中，总共绘制了四条轨迹：1)在已知的不确定洋流条件下，根据(1)-(2)计算得到的真实AUV轨迹x(t)。2)从EKF估计的AUV轨迹(24)，该轨迹是通过使用声学定位反馈信息，即“已测量的”输出

校正航迹推算轨迹获得的。3)通过忽略系统不确定性，从(25)中计算出航迹推算轨迹

4)在每个声学定位步骤中，通过根据(17)从TOAs计算结果中添加随机不确定性，从(18)中获得测量输出y(t)。总体而言，这些轨迹之间的差异很小。因此，对置(300， 300)m附近处进行了放大。AUV的跟踪均方根误差为5.72米，适用于大面积海洋测绘等应用。

图8和图9分别显示了速度和控制输入轨迹。通过优化问题求解的控制输入，即螺旋桨推力和舵角，均满足系统约束。但是，对于速度轨迹，航迹推算速度轨迹很好地满足了约束条件，但AUV实际速度轨迹和估计轨迹的在某些时刻超过了约束条件。这是因为服从优化问题和航位推算轨迹都使用没有不确定性的名义系统动力学模型。如果优化问题中的所有约束条件都满足，航迹推算轨迹也将满足约束。但是，与航迹推算轨迹相比，实际轨迹进一步包含洋流的不确定性，而估计轨迹进一步包含测量的不确定性。这些不确定性将已经在约束边界上的航迹推算轨迹推离了可行区域。需要注意的是，估算速度轨迹与航迹推算航速轨迹的差异较小，两者的均方根值为0.1m/s,这个差异证明了EKF的校正能力。

为了减轻MPC在线优化问题的潜在计算负担，使用了持续线性化的预测模型。因此，只需在线求解凸规划问题。图10显示了所提跟踪控制器的求解时间。在整个仿真过程中，求解时间大都在10ms至20ms之间，且小于系统控制采样时间

这表明了该算法在实时应用中的潜力。

4.2.无声学定位的跟踪控制

为了演示声学定位对于水下机器人跟踪控制的作用，我们还进行了一组没有声学定位的仿真。此时，用于MPC反馈的系统状态完全由航迹推算模块提供。在水下环境中，当水下机器人没有配备任何定位装置时，就会出现水下机器人偏离甚至无法到达目的地的情况。因此，我们将模拟时间设置为与声学定位相同，跟踪结果如图11所示。可以观察到轨迹偏离了参考路径，跟踪任务失败。这意味着纯航迹推算的跟踪控制算法在实际应用中是不可靠的。注意，因为没有使用声学定位反馈，所以没有测量的轨迹，并且估计的轨迹与航迹推算轨迹一致。此外，由于控制器完全基于不受干扰的航迹推算状态，因此航迹推算轨迹可以很好地跟踪参考路径。

以上所述实施例仅表达了本发明的几种实施方式，其描述较为具体和详细，但并不能因此而理解为对本发明专利范围的限制。应当指出的是，对于本领域的普通技术人员来说，在不脱离本发明构思的前提下，还可以做出若干变形和改进，这些都属于本发明的保护范围。因此，本发明专利的保护范围应以所附权利要求为准。

Claims (7)

1.一种自主水下航行器一体化声学定位与跟踪控制方法，其特征在于，包括如下步骤：

1)建立非线性带洋流干扰不确定性的AUV动力学模型；

所述的步骤1)具体为：

建立AUV动力学模型，其表示为：

其中，η＝[x y ψ]^T表示惯性坐标系{n}中的位姿状态，v＝[u μ r]^T表示自身坐标系{b}中沿前进方向、侧向的速度及绕z轴的旋转角速度，v_c为{b}中洋流速度矢量；τ＝[ξ Y_uuδu²δN_uuδu²δ]是被控力和力矩；这里ξ和δ，即系统输入，分别是螺旋桨推力和舵角，Y_uuδ和N_uuδ是相应的水动力参数；R(ψ)为旋转矩阵；S^T为矩阵S的转置，其中

v_r＝v-v_c＝[u_r μ_r r_r]^T是在{b}中AUV船体和洋流之间的相对速度；

和

分别表示刚体和附加质量矩阵，C_RB和C_A分别是刚体和附加科里奥利矩阵和向心矩阵；D_L和D_NL分别表示流体中航行时的线性和非线性阻尼矩阵；

2)部署三个已知位置的水面浮标，每个浮标带有一个声信号发射器，每间隔

秒发出一次声信号；根据声信号到达AUV的时间，对AUV位置进行估计，获得AUV的位置状态；

3)考虑洋流干扰和声学观测中的不确定性，利用扩展卡尔曼滤波(EKF)对步骤2)获得的AUV状态进行估计和校正；校正后的AUV状态用于模型预测控制(MPC)的反馈；

所述模型预测控制(MPC)利用系统模型预测系统在未来一段有限时间内的轨迹，在每一个时间步在线求解基于预测轨迹的优化问题，得到最优控制序列应用于AUV，并以滚动时域的方式重复此过程；

所述的系统模型为持续线性化所得的预测模型，其通过对步骤1)所述动力学模型进行持续线性化获得；

对步骤1)所述动力学模型进行持续线性化具体为：

将AUV动力学模型概括描述为：

其中f:

是一个光滑函数，x＝[η^T,v^T]^T是系统状态，

是控制输入向量，b为洋流干扰；

采用零阶保持法进行离散化：

其中时间k与实际时间t的关系为：

为控制采样时间；

在每个控制时间步k，持续线性化实现步骤如下：

2.1)移动来自前一个时间步的最优控制输入序列u^*(i∣k-1),i＝0,1,…,N_p-1，N_p是预测时域长度，用于求得“种子”控制输入轨迹u⁰(i∣k)：

2.2)将u⁰(i∣k)应用到式(20)得到“种子”状态轨迹x⁰(i∣k),i＝0,1,…,N_p-1；起始状态设置为x⁰(0∣k)＝x(k)，x(k)是当前系统状态；

2.3)将非线性动力学模型在(x⁰(i∣k),u⁰(i∣k),b⁰(i∣k))进行线性化，i＝0,1,…,N_p-1；b⁰(i∣k)为初始干扰值，

x(i+1∣k)＝x⁰(i+1∣k)+A(i∣k)Δx(i∣k)+B(i∣k)Δu(i∣k)+E(i∣k)Δb(i∣k) (22)

其中

分别是x⁰(i∣k),u⁰(i∣k),b⁰(i∣k)附近的扰动，A(i∣k),B(i∣k)，E(i∣k)是x⁰(i∣k),u⁰(i∣k),b⁰(i∣k)处相应的雅可比矩阵。

2.根据权利要求1所述的自主水下航行器一体化声学定位与跟踪控制方法，其特征在于，所述洋流考虑为无旋定速洋流，洋流表示为b＝[V_c β_c]^T，其中V_c和β_c分别是{n}中的洋流速度和角度，

因此

3.根据权利要求1所述的自主水下航行器一体化声学定位与跟踪控制方法，其特征在于，所述的步骤2)具体为：

部署三个已知位置[x_n,y_n,z_n]^T,n＝1,2,3的水面浮标，三个浮标中的每一个浮标都带着一个声信号发射器，每间隔

秒时间就发出一次声信号，获得AUV处接收到三个浮标信号的到达时间TOA(Time of arrival)；在已知TOA、AUV深度和浮标固定位置的情况下，估计浮标n与AUV之间的实际水平距离d_n；利用水平距离估计值

计算AUV的位置。

4.根据权利要求3所述的自主水下航行器一体化声学定位与跟踪控制方法，其特征在于，

假设AUV分别接收到1、2、3号浮标的三个信号时位置相同，可以得到一组非线性方程组：

通过对其求解，得到AUV的声学位置估计

AUV水平航向是由机载传感器测量的，记作

由于存在于声速剖面模型、时钟同步误差、测量TOA、水面浮标位置[x_n,y_n,z_n]^T以及航向角测量系统中的不确实性，假设AUV“测量”输出的

受到随机噪声ε～N(0,∑)的影响，

则AUV水平姿态参数可以表述为：

5.根据权利要求4所述的自主水下航行器一体化声学定位与跟踪控制方法，其特征在于，所述的步骤3)中，考虑洋流干扰和声学观测中的不确定性，利用扩展卡尔曼滤波(EKF,Extended Kalman Filter)对步骤2)获得的AUV状态进行校正，具体为：

假设

且n≥2；在控制时间步k，当有新的声学定位信息时，即在定位步k/n，通过下式校正AUV状态；

其中

是输出矩阵，

是忽略不确定因素时的非线性模型的计算得到的状态，即：

其中f_d代表式(20)的离散化结果，带有上划线的参数

代表系统的名义状态信息，即不考虑不确定性时的信息；

卡尔曼增益K(k)为：

误差

的协方差矩阵

通过以下递归方法计算：

P(k)代表估计误差的协方差。

6.根据权利要求5所述的自主水下航行器一体化声学定位与跟踪控制方法，其特征在于，在所述的步骤3)中，由于控制周期通常比定位周期短，

这表示y(k)不一定在每个时间k内都可用；因此，当声学测量无法得到时，利用式(25)中的名义状态

用于反馈。

7.根据权利要求6所述的自主水下航行器一体化声学定位与跟踪控制方法，其特征在于，所述步骤3)中，在每一个时间步在线求解基于预测轨迹的优化问题，得到最优控制序列反馈控制AUV，具体为：

在每个时间步k，模型预测控制(MPC)会基于当前系统状态在预测时域N_p上计算未来的控制序列，以使将预测的系统轨迹接近给定的参考值，优化计算得到的控制序列的第一个元素被应用到系统，在下一个时间步k+1，将使用新测量的系统状态重复此过程；

考虑系统参考轨迹如下：

其中ψ_d＝arctan((y_d-y₀)/(x_d-x₀))；然后，通过求解带约束条件的有限时域上的二次规划问题：

约束条件为：

u_min≤u(i∣k)≤u_max,i＝0,1,…,N_p

ξ_min≤ξ(i∣k)≤ξ_max,i＝0,1,…,N_p-1

δ_min≤δ(i∣k)≤δ_max,i＝0,1,…,N_p-1

其中

约束条件中的下标min表示对应参数允许的最小值，max表示对应参数允许的最大值，式(27)中的三项分别是终端跟踪误差成本、阶段跟踪误差成本和控制输入成本，Q_f,Q,R是各个成本项的正定权矩阵。

Images