CN112947068B - 一种自主水下航行器一体化声学定位与跟踪控制方法 - Google Patents
一种自主水下航行器一体化声学定位与跟踪控制方法 Download PDFInfo
- Publication number
- CN112947068B CN112947068B CN202110111390.XA CN202110111390A CN112947068B CN 112947068 B CN112947068 B CN 112947068B CN 202110111390 A CN202110111390 A CN 202110111390A CN 112947068 B CN112947068 B CN 112947068B
- Authority
- CN
- China
- Prior art keywords
- auv
- time
- control
- model
- acoustic
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Active
Links
- 238000000034 method Methods 0.000 title claims abstract description 69
- XLYOFNOQVPJJNP-UHFFFAOYSA-N water Substances O XLYOFNOQVPJJNP-UHFFFAOYSA-N 0.000 claims abstract description 15
- 238000001914 filtration Methods 0.000 claims abstract description 9
- 238000005070 sampling Methods 0.000 claims abstract description 9
- 239000011159 matrix material Substances 0.000 claims description 19
- 238000005457 optimization Methods 0.000 claims description 15
- 238000005259 measurement Methods 0.000 claims description 13
- 238000005096 rolling process Methods 0.000 claims description 4
- 238000013016 damping Methods 0.000 claims description 3
- 239000012530 fluid Substances 0.000 claims description 3
- 238000005312 nonlinear dynamic Methods 0.000 claims description 2
- 238000004422 calculation algorithm Methods 0.000 abstract description 11
- 230000000694 effects Effects 0.000 abstract description 11
- 238000004364 calculation method Methods 0.000 abstract description 7
- 238000012545 processing Methods 0.000 abstract description 5
- 230000003287 optical effect Effects 0.000 abstract description 2
- 230000000737 periodic effect Effects 0.000 abstract description 2
- 230000033001 locomotion Effects 0.000 description 10
- 238000004088 simulation Methods 0.000 description 10
- 230000004807 localization Effects 0.000 description 9
- 238000013461 design Methods 0.000 description 4
- 238000005516 engineering process Methods 0.000 description 4
- 230000007613 environmental effect Effects 0.000 description 3
- 230000005236 sound signal Effects 0.000 description 3
- 238000013459 approach Methods 0.000 description 2
- 238000012937 correction Methods 0.000 description 2
- 238000010586 diagram Methods 0.000 description 2
- 238000002474 experimental method Methods 0.000 description 2
- 230000005484 gravity Effects 0.000 description 2
- 230000001133 acceleration Effects 0.000 description 1
- 229960001948 caffeine Drugs 0.000 description 1
- 230000007423 decrease Effects 0.000 description 1
- 230000007547 defect Effects 0.000 description 1
- 230000001419 dependent effect Effects 0.000 description 1
- 238000006073 displacement reaction Methods 0.000 description 1
- 238000005265 energy consumption Methods 0.000 description 1
- 238000005562 fading Methods 0.000 description 1
- 231100001261 hazardous Toxicity 0.000 description 1
- 238000013507 mapping Methods 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 238000003672 processing method Methods 0.000 description 1
- 238000010845 search algorithm Methods 0.000 description 1
- 230000001360 synchronised effect Effects 0.000 description 1
- RYYVLZVUVIJVGH-UHFFFAOYSA-N trimethylxanthine Natural products CN1C(=O)N(C)C(=O)C2=C1N=CN2C RYYVLZVUVIJVGH-UHFFFAOYSA-N 0.000 description 1
Images
Classifications
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05B—CONTROL OR REGULATING SYSTEMS IN GENERAL; FUNCTIONAL ELEMENTS OF SUCH SYSTEMS; MONITORING OR TESTING ARRANGEMENTS FOR SUCH SYSTEMS OR ELEMENTS
- G05B13/00—Adaptive control systems, i.e. systems automatically adjusting themselves to have a performance which is optimum according to some preassigned criterion
- G05B13/02—Adaptive control systems, i.e. systems automatically adjusting themselves to have a performance which is optimum according to some preassigned criterion electric
- G05B13/04—Adaptive control systems, i.e. systems automatically adjusting themselves to have a performance which is optimum according to some preassigned criterion electric involving the use of models or simulators
- G05B13/042—Adaptive control systems, i.e. systems automatically adjusting themselves to have a performance which is optimum according to some preassigned criterion electric involving the use of models or simulators in which a parameter or coefficient is automatically adjusted to optimise the performance
Landscapes
- Engineering & Computer Science (AREA)
- Health & Medical Sciences (AREA)
- Artificial Intelligence (AREA)
- Computer Vision & Pattern Recognition (AREA)
- Evolutionary Computation (AREA)
- Medical Informatics (AREA)
- Software Systems (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Automation & Control Theory (AREA)
- Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
- Measurement Of Velocity Or Position Using Acoustic Or Ultrasonic Waves (AREA)
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动力学模型;
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}的原点Ob与AUV 的重心重合;2)AUV在固定深度航行,所以AUV6自由度运动可以简化为水平面3自由度运动;3)AUV是右舷对称的;4)AUV浮力与重心重合。基于这些假设,AUV动力学模型可表示为:
这里η=[x y ψ]T表示惯性坐标系{n}中的位姿状态,v=[u v r]T表示自身坐标系{b}中的速度状态,该速度更精确的定义是点Ob相对于{b}表示在{n}中的速度。在自身坐标系{b}中还定义了洋流速度矢量vc。被控力和力矩是τ=[ξ Yuuδu2δ Nuuδu2δ]。这里ξ和δ,即***输入,分别是螺旋桨推力和舵角,Yuuδ和Nuuδ是相应的水动力参数。旋转矩阵R(ψ)关联{n}和{b}两个坐标系中的***状态:
在(2)中,vr=v-vc=[ur vr rr]T是在{b}中AUV船体和洋流之间的相对速度。为了简化,我们考虑了无旋定速洋流,表示为b=[Vc βc]T,其中Vc和βc分别是{n}中的洋流速度和角度,
因此
刚体和附加质量矩阵分别表示为和同样,CRB和CA分别是刚体和附加科里奥利矩阵和向心矩阵。阻尼效应由DL和非线性部分DNL建模。附加和阻尼矩阵是由于海上航行器在流体中运动时的水动力效应所致。由于 AUV***限制,AUV速度、螺旋桨最大推力和方向舵最大转角分别限制如下:
umin≦u≦umax,(4)
ξmin≦ξ≦ξmax,(5)
δmin≦δ≦δmax.(6)
AUV从指定起始点按照一定轨迹到达目的地。该区域的洋流信息,即Vc和βc,通常可以由区域海洋模型预测,也可以由AUV自身携带的传感器粗略测量,但这些预测存在随机不确定性。因此,将洋流建模为这里在惯性坐标系中,分别是预测的平均洋流速度和角度;是反映预测精度的对应协方差,其中diag表示对角矩阵。
二、AUV声学定位
利用声音在水中的有限传播速度和海洋表面存在GPS或GNSS定位的特点,部署三个已知位置[xn,yn,zn]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,表示为Δtn,n=1,2,3,具体计算步骤为:
Xn(t)=z/dn(t) (10)
Yn(t)=Ln(t)/Xn(t) (11)
αn(t)=arctan(Yn(t)) (12)
βn(t)=arctan(Xn(t)) (13)
其中,dn代表浮标n与AUV之间的实际水平距离;Ln、Xn和Yn为中间变量;αn为浮标n的实际声射线与直线路径之间的夹角;βn为浮标n的水平方向与直线路径之间的夹角;最后,和分别为浮标n和AUV处的掠射角。
在仿真中,TOA即Δtn,n=1,2,3,可按式(16)计算,在实验中,可以利用已经同步过的时钟测量。然后,在已知TOA、AUV深度z和浮标固定位置的情况下,可以利用二元搜索等算法估计水平距离dn。其主要思想是反复比较到达时间的中值点和实际传播时间,从而缩小搜索范围,直到找到足够精确的水平距离估计值
通过对其求解,我们得到AUV的声学位置估计需要注意的是,在实际中,只需要两个方程就可以得到但是,为了提高浮标失效时的***鲁棒性,部署了三个浮标。在实验中,由于接收可靠的声学信号通常是一个问题,因此在AUV声学定位中,冗余的备用浮标是必不可少的。
通常,AUV水平航向是由惯性测量单元等机载传感器测量的,记作此外,由于存在于声速剖面模型、时钟同步误差、测量TOA、水面浮标位置 [xn,yn,zn]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)概括描述为:
1、移动来自前一个时间步的最优控制输入序列u*(i∣k-1),i=0,1,…,Np-1,Np是预测时域长度,用于求得“种子”控制输入轨迹u0(i∣k):
u0(i∣k)=u*(i∣k-1),i=0,1,…,Np-2,
u0(Np-1∣k)=u*(Np-2∣k-1),i=Np
2、将u0(i∣k)应用到(20)得到“种子”状态轨迹x0(i∣k),i=0,1,…,Np-1。起始状态设置为x0(0∣k)=x(k),x(k)是当前***状态。初始干扰值设置为:
2、将非线性动力学模型在(x0(i∣k),u0(i∣k),b0(i∣k))进行线性化,i=0,1,…,Np-1
x(i+1∣k)=x0(i+1∣k)+A(i∣k)Δx(i∣k)+
B(i∣k)Δu(i∣k)+E(i∣k)Δb(i∣k) (22)
其中
分别是x0(i∣k),u0(i∣k),b0(i∣k)周围的扰动,A(i∣k),B(i∣k),E(i∣k)是 x0(i∣k),u0(i∣k),b0(i∣k)处相应的雅可比矩阵。
3.2基于声学定位信息的EKF
卡尔曼滤波器是一种广泛使用的正确预测公式,用于处理速度估计和随机环境干扰。EKF通过线性化进一步将***动力学扩展到非线性类型,以传播状态估计和协方差的近似值。基于模型(18),(20)和(22),便可进行EKF设计。
其中
P(k)代表估计误差的协方差。
通过此标准卡尔曼滤波程序,就可以通过声学“测量”输出y(k)校正名义***状态修正的状态可以用于MPC反馈控制的设计。但是,如前所述,控制周期通常比定位周期短,这表示y(k)不一定在每个时间k内都可用。当声学测量无法得到时,将利用(25)中的名义状态用于反馈,也通常称为航位推算技术。
3.3预测跟踪技术
在每个时间步k,MPC会基于当前***状态在预测时域Np上计算未来的控制序列。以使将预测的***轨迹接近给定的参考值,优化计算得到的控制序列的第一个元素被应用到***。在下一个时间步k+1,将使用新测量的***状态重复此过程。对于AUV跟踪问题,施加了***约束(4)–(6)。接下来,提出 MPC方法,利用EKF的校正信息或来自航位推算技术的信息作为反馈来解决 AUV跟踪问题。
考虑***参考轨迹如下:
其中ψd=arctan((yd-y0)/(xd-x0))然后通过求解带约束条件的有限时域上的二次规划问题:
约束条件为:
umin≤u(i∣k)≤umax,i=0,1,…,Np
ξmin≤ξ(i∣k)≤ξmax,i=0,1,…,Np-1
δmin≤δ(i∣k)≤δmax,i=0,1,…,Np-1
其中目标函数(27)旨在通过以最小的代价实现最小的跟踪误差。其中三项分别是终端跟踪误差成本,阶段跟踪误差成本和控制输入成本。Qf,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.1m2/s20.01rad2])的正态分布。不确定声学测量的协方差∑=diag([0.1m20.1m20.01rad2])。控制器参数设置如下:预测地平Np=10 权重参数R=diag([100 100]);采样时间 ***约束条件为:umax=-umin=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更快。
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轴的旋转角速度,vc为{b}中洋流速度矢量;τ=[ξ Yuuδu2δNuuδu2δ]是被控力和力矩;这里ξ和δ,即***输入,分别是螺旋桨推力和舵角,Yuuδ和Nuuδ是相应的水动力参数;R(ψ)为旋转矩阵;ST为矩阵S的转置,其中
vr=v-vc=[ur μr rr]T是在{b}中AUV船体和洋流之间的相对速度;和分别表示刚体和附加质量矩阵,CRB和CA分别是刚体和附加科里奥利矩阵和向心矩阵;DL和DNL分别表示流体中航行时的线性和非线性阻尼矩阵;
3)考虑洋流干扰和声学观测中的不确定性,利用扩展卡尔曼滤波(EKF)对步骤2)获得的AUV状态进行估计和校正;校正后的AUV状态用于模型预测控制(MPC)的反馈;
所述模型预测控制(MPC)利用***模型预测***在未来一段有限时间内的轨迹,在每一个时间步在线求解基于预测轨迹的优化问题,得到最优控制序列应用于AUV,并以滚动时域的方式重复此过程;
所述的***模型为持续线性化所得的预测模型,其通过对步骤1)所述动力学模型进行持续线性化获得;
对步骤1)所述动力学模型进行持续线性化具体为:
将AUV动力学模型概括描述为:
采用零阶保持法进行离散化:
在每个控制时间步k,持续线性化实现步骤如下:
2.1)移动来自前一个时间步的最优控制输入序列u*(i∣k-1),i=0,1,…,Np-1,Np是预测时域长度,用于求得“种子”控制输入轨迹u0(i∣k):
2.2)将u0(i∣k)应用到式(20)得到“种子”状态轨迹x0(i∣k),i=0,1,…,Np-1;起始状态设置为x0(0∣k)=x(k),x(k)是当前***状态;
2.3)将非线性动力学模型在(x0(i∣k),u0(i∣k),b0(i∣k))进行线性化,i=0,1,…,Np-1;b0(i∣k)为初始干扰值,
x(i+1∣k)=x0(i+1∣k)+A(i∣k)Δx(i∣k)+B(i∣k)Δu(i∣k)+E(i∣k)Δb(i∣k) (22)
其中
分别是x0(i∣k),u0(i∣k),b0(i∣k)附近的扰动,A(i∣k),B(i∣k),E(i∣k)是x0(i∣k),u0(i∣k),b0(i∣k)处相应的雅可比矩阵。
5.根据权利要求4所述的自主水下航行器一体化声学定位与跟踪控制方法,其特征在于,所述的步骤3)中,考虑洋流干扰和声学观测中的不确定性,利用扩展卡尔曼滤波(EKF,Extended Kalman Filter)对步骤2)获得的AUV状态进行校正,具体为:
其中
卡尔曼增益K(k)为:
P(k)代表估计误差的协方差。
7.根据权利要求6所述的自主水下航行器一体化声学定位与跟踪控制方法,其特征在于,所述步骤3)中,在每一个时间步在线求解基于预测轨迹的优化问题,得到最优控制序列反馈控制AUV,具体为:
在每个时间步k,模型预测控制(MPC)会基于当前***状态在预测时域Np上计算未来的控制序列,以使将预测的***轨迹接近给定的参考值,优化计算得到的控制序列的第一个元素被应用到***,在下一个时间步k+1,将使用新测量的***状态重复此过程;
考虑***参考轨迹如下:
其中ψd=arctan((yd-y0)/(xd-x0));然后,通过求解带约束条件的有限时域上的二次规划问题:
约束条件为:
umin≤u(i∣k)≤umax,i=0,1,…,Np
ξmin≤ξ(i∣k)≤ξmax,i=0,1,…,Np-1
δmin≤δ(i∣k)≤δmax,i=0,1,…,Np-1
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202110111390.XA CN112947068B (zh) | 2021-01-27 | 2021-01-27 | 一种自主水下航行器一体化声学定位与跟踪控制方法 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202110111390.XA CN112947068B (zh) | 2021-01-27 | 2021-01-27 | 一种自主水下航行器一体化声学定位与跟踪控制方法 |
Publications (2)
Publication Number | Publication Date |
---|---|
CN112947068A CN112947068A (zh) | 2021-06-11 |
CN112947068B true CN112947068B (zh) | 2022-08-23 |
Family
ID=76237822
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN202110111390.XA Active CN112947068B (zh) | 2021-01-27 | 2021-01-27 | 一种自主水下航行器一体化声学定位与跟踪控制方法 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN112947068B (zh) |
Families Citing this family (1)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN114839883B (zh) * | 2022-07-04 | 2022-09-20 | 海南浙江大学研究院 | 非线性欠驱动水下航行器定深抗干扰鲁棒l1控制方法 |
Citations (2)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN109343350A (zh) * | 2018-11-20 | 2019-02-15 | 清华大学 | 一种基于模型预测控制的水下机器人路径跟踪控制方法 |
CN110865333A (zh) * | 2019-11-19 | 2020-03-06 | 浙江大学 | 洋流影响下水下滑翔机单信标无源声学定位方法 |
-
2021
- 2021-01-27 CN CN202110111390.XA patent/CN112947068B/zh active Active
Patent Citations (2)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN109343350A (zh) * | 2018-11-20 | 2019-02-15 | 清华大学 | 一种基于模型预测控制的水下机器人路径跟踪控制方法 |
CN110865333A (zh) * | 2019-11-19 | 2020-03-06 | 浙江大学 | 洋流影响下水下滑翔机单信标无源声学定位方法 |
Non-Patent Citations (5)
Title |
---|
A. Caiti 等.localization of autonomous underwater vehicles by floating acoustic buoys: a set-membership approach.《IEEE》.2005, * |
Ding Zhang 等.Localization and Tracking Control Using Hybrid Acoustic–Optical Communication for Autonomous Underwater Vehicles.《IEEE》.2020, * |
严浙平等.模型辅助和洋流估计下的UUV导航定位方法.《水下无人***学报》.2017,第25卷(第04期),参见第319-324页. * |
张广洁等.基于模型预测控制的欠驱动AUV直线路径跟踪.《水下无人***学报》.2017,第25卷(第01期),全文. * |
曹方方等.基于EKF的UUV单信标水声导航定位方法研究.《计算机测量与控制》.2011,(第09期),全文. * |
Also Published As
Publication number | Publication date |
---|---|
CN112947068A (zh) | 2021-06-11 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
Wu et al. | Depth control of model-free AUVs via reinforcement learning | |
Wu | Coordinated path planning for an unmanned aerial-aquatic vehicle (UAAV) and an autonomous underwater vehicle (AUV) in an underwater target strike mission | |
Meng et al. | A visual/inertial integrated landing guidance method for UAV landing on the ship | |
Miller et al. | Autonomous underwater vehicle navigation | |
CN111596333B (zh) | 一种水下定位导航方法及*** | |
Xu et al. | A novel self-adapting filter based navigation algorithm for autonomous underwater vehicles | |
CN110906933B (zh) | 一种基于深度神经网络的auv辅助导航方法 | |
CN103970021A (zh) | 一种基于模型预测控制的松弛动力定位控制*** | |
Zhang et al. | NavNet: AUV navigation through deep sequential learning | |
Braginsky et al. | Development of an autonomous surface vehicle capable of tracking autonomous underwater vehicles | |
CN109901402B (zh) | 一种基于航向平滑技术的自主水下机器人路径跟踪方法 | |
CN112068440B (zh) | 基于模型预测控制的auv回收对接动力定位控制方法 | |
Harris et al. | Preliminary evaluation of cooperative navigation of underwater vehicles without a dvl utilizing a dynamic process model | |
Xu et al. | Accurate two-step filtering for AUV navigation in large deep-sea environment | |
Zhang et al. | Sliding mode prediction control for 3d path following of an underactuated auv | |
CN112947068B (zh) | 一种自主水下航行器一体化声学定位与跟踪控制方法 | |
Zhang et al. | An integrated navigation method for small-sized AUV in shallow-sea applications | |
Raimondi et al. | Hierarchical fuzzy/lyapunov control for horizontal plane trajectory tracking of underactuated AUV | |
Osborn et al. | AUV state estimation and navigation to compensate for ocean currents | |
Qin et al. | Robust interactive multimodel INS/DVL intergrated navigation system with adaptive model set | |
Annamalai et al. | A robust navigation technique for integration in the guidance and control of an uninhabited surface vehicle | |
CN113670303B (zh) | 一种基于rbf神经网络的sins/dvl组合导航流速补偿方法 | |
Zhou et al. | Geomagnetic matching algorithm based on the probabilistic neural network | |
Jayasiri et al. | AUV‐Based Plume Tracking: A Simulation Study | |
Ma et al. | A robust fusion terrain-aided navigation method with a single Beam Echo Sounder |
Legal Events
Date | Code | Title | Description |
---|---|---|---|
PB01 | Publication | ||
PB01 | Publication | ||
SE01 | Entry into force of request for substantive examination | ||
SE01 | Entry into force of request for substantive examination | ||
GR01 | Patent grant | ||
GR01 | Patent grant |