CN114019805A - 一种欠驱动auv的模型预测对接控制方法 - Google Patents

一种欠驱动auv的模型预测对接控制方法 Download PDF

Info

Publication number
CN114019805A
CN114019805A CN202111361802.1A CN202111361802A CN114019805A CN 114019805 A CN114019805 A CN 114019805A CN 202111361802 A CN202111361802 A CN 202111361802A CN 114019805 A CN114019805 A CN 114019805A
Authority
CN
China
Prior art keywords
auv
docking
butt joint
angle
formula
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.)
Granted
Application number
CN202111361802.1A
Other languages
English (en)
Other versions
CN114019805B (zh
Inventor
王晓伟
蒋晓刚
郑海娟
朱家梁
李遂意
陈晓华
黄卫萍
何钧钧
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Jiujiang Vocational and Technical College
Original Assignee
Jiujiang Vocational and Technical College
Priority date (The priority date 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 date listed.)
Filing date
Publication date
Application filed by Jiujiang Vocational and Technical College filed Critical Jiujiang Vocational and Technical College
Priority to CN202111361802.1A priority Critical patent/CN114019805B/zh
Publication of CN114019805A publication Critical patent/CN114019805A/zh
Application granted granted Critical
Publication of CN114019805B publication Critical patent/CN114019805B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

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
    • Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
    • Y02—TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
    • Y02P—CLIMATE CHANGE MITIGATION TECHNOLOGIES IN THE PRODUCTION OR PROCESSING OF GOODS
    • Y02P90/00—Enabling technologies with a potential contribution to greenhouse gas [GHG] emissions mitigation
    • Y02P90/02—Total factory control, e.g. smart factories, flexible manufacturing systems [FMS] or integrated manufacturing systems [IMS]

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)

Abstract

本发明公开了一种欠驱动auv的模型预测对接控制方法，包括：对接误差模型设计；对接控制器的设计，包括期望趋近角的设计和动力学控制器的设计。本发明针对一种常见的缺少横向和垂向驱动力的欠驱动AUV，采用USBL进行定位，获取喇叭口导向式对接装置上的四个应答器在载体坐标系中的坐标，并通过坐标变换得到AUV在固定坐标系中的位置和姿态，考虑USBL视角的约束，应用MPC优化了其对接过程中的期望趋近角，不仅实现了其三维空间内的对接控制，而且有效的缩短了对接距离。

Description

一种欠驱动auv的模型预测对接控制方法
技术领域
本发明涉及自主水下航行器技术，具体涉及一种欠驱动auv的模型预测对接控制方法。
背景技术
目前，自主水下航行器（AUV）在海洋开发中得到广泛的应用。当AUV要在水下执行给定任务时，首先通过母船对AUV进行布放，然后AUV对预先规划的路径进行跟踪控制来完成目标区域的勘察或探测，当任务完成后要对AUV进行回收，即返回母船进行能量补给、数据交换（下载探测数据或接受新的任务）和检修等工作。传统的回收方法是当AUV返回并靠近母船时，采用吊车等设备将其吊起进行回收，但是这种方法不仅会增加船员的工作量，而且自动化程度较低，所以如何实现AUV的自主回收是目前的一个研究热点。目前典型的回收方式主要有：平台式、导向式、捕捉式、鱼雷发射管式等，其中喇叭口导向式回收装置较简单，可靠性和实用性较好，可以实现AUV的自主回收，应用较为广泛。
AUV自主回收的难点主要有两个，一是AUV的导航定位问题，因为AUV在回收过程中要实时的获取自身的位置和姿态。导向式回收方式一般将回收分为回坞和对接两个步骤，回坞的目的是使AUV通过跟踪规划的路径回到对接区域，这一步控制精度要求不高，AUV采用自主导航即可。AUV进入对接区域后即启动对接程序，这一步控制精度要求较高。水声导航定位技术由于定位精度较高得到了广泛的应用，其中超短基线定位系统（USBL,Ultroshort Base Line）结构简单、体积小更受青睐。AUV自主回收的另一个难点是其控制问题。为了降低成本和提高可靠性，很多AUV设计为欠驱动的形式，即在某些自由度缺少驱动力，另外其运动模型很难精确获取、工作环境也存在各种干扰，这都给控制器的设计构成了困难。有文献针对AUV的对接问题，应用USBL进行定位，设计了一种改进的卡尔曼滤波算法，改善了信号的滞后和干扰问题。有文献针对一种全驱动AUV的对接问题，应用神经网络和滑模技术设计了控制器，实现了其对接过程中的姿态控制。模型预测控制（MPC）便于处理具有约束的控制问题，在AUV自主回收控制中也得到了大量的应用。有文献设计了一种基于MPC和模糊控制的回坞导引算法，实现了AUV的自主回收。有文献应用USBL进行定位，基于MPC设计了控制器，实现了一种全驱动AUV的对接控制。有文献应用MPC处理对接的约束问题，实现了一种动基座 UUV的自主回收。
以上文献主要解决的是AUV在水平面内的回收问题，在对接过程中基本未考虑深度误差的影响。本发明针对一种常见的欠驱动AUV（缺少横向和垂向驱动力），采用USBL进行定位，应用MPC优化了其对接过程中的期望趋近角，实现了其三维空间内的对接控制。
发明内容
本发明的主要目的在于提供一种欠驱动auv的模型预测对接控制方法。
本发明采用的技术方案是：一种欠驱动auv的模型预测对接控制方法，包括：对接误差模型设计；对接控制器的设计，包括期望趋近角的设计和动力学控制器的设计。
进一步地，所述对接误差模型设计包括：设置了两个坐标系，一个为载体坐标系
Figure 790117DEST_PATH_IMAGE001
，其原点定义在AUV的浮心O处，另一个为固定坐标系
Figure 219962DEST_PATH_IMAGE002
，其原点定义在导向式对接装置喇叭口的中心E处；载体坐标系的x轴指向前方、y轴指向右侧、z轴指向下方，固定坐标系的ξ轴指向前方与对接路径重合、η轴指向右侧的应答器2、ζ轴指向下方的应答器4；对接误差模型可以简化为
Figure DEST_PATH_IMAGE003
(1)
式（1）中，
Figure 133691DEST_PATH_IMAGE004
为AUV在
Figure 939973DEST_PATH_IMAGE005
中的位置和姿态，即对接误差，
Figure 583444DEST_PATH_IMAGE006
代表AUV在
Figure 590714DEST_PATH_IMAGE005
中的坐标、θ为AUV的纵倾角、ψ为AUV的艏摇角；
Figure 319636DEST_PATH_IMAGE007
代表AUV在
Figure 929609DEST_PATH_IMAGE008
中的速度，u为纵向速度、v为横向速度、w为垂向速度、q为纵倾角速度、r为艏摇角速度；
Figure 693165DEST_PATH_IMAGE009
为
Figure 340178DEST_PATH_IMAGE008
到
Figure 821975DEST_PATH_IMAGE005
的旋转变换矩阵，
Figure 970060DEST_PATH_IMAGE010
首先通过USBL接收器获取4个应答器在
Figure DEST_PATH_IMAGE011
中的坐标，它们与对接误差满足关系式（2），
Figure 994648DEST_PATH_IMAGE012
(2)
式（2）中，
Figure 937196DEST_PATH_IMAGE013
为USBL接收器获取的4个应答器的坐标，l为USBL接收器与浮心O的距离，
Figure 640710DEST_PATH_IMAGE014
为4个应答器在
Figure 467851DEST_PATH_IMAGE015
中的坐标，
Figure 940421DEST_PATH_IMAGE016
为4个应答器在
Figure 319450DEST_PATH_IMAGE017
中的坐标；
对接装置喇叭口的半径为1米，所以
Figure 510260DEST_PATH_IMAGE018
；通过关系式
Figure 875513DEST_PATH_IMAGE019
可得
Figure 468168DEST_PATH_IMAGE020
(3)
通过关系式
Figure 18098DEST_PATH_IMAGE021
可得
Figure 568641DEST_PATH_IMAGE022
(4)
通过关系式
Figure 862219DEST_PATH_IMAGE023
可得
Figure 309381DEST_PATH_IMAGE024
(5)。
更进一步地，所述期望趋近角的设计包括：设计导引律生成艏摇角和纵倾角的期望值，即期望趋近角；采用如下LOS导引律
Figure DEST_PATH_IMAGE025
(6)
式（6）中
Figure 171158DEST_PATH_IMAGE026
为垂直面趋近角，
Figure 336560DEST_PATH_IMAGE027
为水平面趋近角，
Figure 168250DEST_PATH_IMAGE028
和
Figure 610863DEST_PATH_IMAGE029
为前视距离；采用MPC设计导引律来对期望趋近角进行优化，纵倾角和艏摇角对期望趋近角的误差方程可以近似为以下微分方程
Figure 502596DEST_PATH_IMAGE030
(7)
式（7）中
Figure 889715DEST_PATH_IMAGE031
为可调时间常数，其中
Figure 525096DEST_PATH_IMAGE032
；
根据对接误差模型（1），横向和垂向的对接误差方程可以简化为
Figure 822216DEST_PATH_IMAGE033
(8)
式（8）中
Figure 884850DEST_PATH_IMAGE034
，当
Figure 759265DEST_PATH_IMAGE035
时可以简化为
Figure 932757DEST_PATH_IMAGE036
，当
Figure 349963DEST_PATH_IMAGE037
时可以简化为
Figure 583499DEST_PATH_IMAGE038
；将式（7）和式（8）合并，
则对接控制可以等价为如下误差的镇定问题
Figure 945210DEST_PATH_IMAGE039
(9)
将式（9）离散化可得对接误差的预测模型为
Figure 797759DEST_PATH_IMAGE040
(10)
式（10）中的下标k代表采用时间序列，
Figure 194106DEST_PATH_IMAGE041
Figure 598542DEST_PATH_IMAGE042
和
Figure 181970DEST_PATH_IMAGE043
分别为状态向量和输出向量，均为对接误差，
Figure 838211DEST_PATH_IMAGE044
为输入向量，即期望趋近角，T为采样周期；在k时刻，根据预测模型（10）可得将来k+1至
Figure DEST_PATH_IMAGE045
时刻对接误差的预测值为
Figure 620222DEST_PATH_IMAGE046
式中的
Figure DEST_PATH_IMAGE047
分别代表控制步长和预测步长，其中
Figure 336505DEST_PATH_IMAGE048
；则进一步可得预测的输出值为
Figure DEST_PATH_IMAGE049
预测输出值可以整理为以下矩阵形式
Figure 672808DEST_PATH_IMAGE050
(11)
Figure DEST_PATH_IMAGE051
Figure 135669DEST_PATH_IMAGE052
因为USBL接收器的视角是受限制的，所以考虑期望趋近角的约束条件为
Figure DEST_PATH_IMAGE053
(12)
Figure 647553DEST_PATH_IMAGE054
将（12）整理为以下线性矩阵不等式
Figure 393793DEST_PATH_IMAGE055
(13)
Figure 217392DEST_PATH_IMAGE056
符号
Figure DEST_PATH_IMAGE057
为克罗内克积；构造Lyapunov函数
Figure 215435DEST_PATH_IMAGE058
(14)
对式（14）求导可得
Figure 440880DEST_PATH_IMAGE059
式中，
Figure 358020DEST_PATH_IMAGE060
，只要满足
Figure DEST_PATH_IMAGE061
，则
Figure 544282DEST_PATH_IMAGE062
；
将稳定约束条件整理为以下线性矩阵不等式
Figure DEST_PATH_IMAGE063
(15)
Figure 736229DEST_PATH_IMAGE064
求解最优问题，定义如下代价函数
Figure DEST_PATH_IMAGE065
(16)
代价函数（16）中的加权矩阵
Figure 957126DEST_PATH_IMAGE066
,
Figure DEST_PATH_IMAGE067
,
Figure 310747DEST_PATH_IMAGE068
，
Figure 718726DEST_PATH_IMAGE070
均为正定矩阵，其中
Figure DEST_PATH_IMAGE071
，
Figure 714364DEST_PATH_IMAGE072
将（11）带入（16）可得
Figure DEST_PATH_IMAGE073
(17)
式（17）中
Figure 789767DEST_PATH_IMAGE074
， 因为
Figure DEST_PATH_IMAGE075
在每一个采用时刻为常值，所以考虑约束的MPC最优问题可以等价为对以下二次型的求解
Figure 314289DEST_PATH_IMAGE076
(18)
在每个采用周期通过求解式（18）可以得到将来
Figure DEST_PATH_IMAGE077
步最优期望趋近角的序列值
Figure 475143DEST_PATH_IMAGE078
，当然只要将
Figure 743314DEST_PATH_IMAGE078
的第一组值
Figure 532278DEST_PATH_IMAGE079
作为当前的期望趋近角即可，当进入下一个采用周期再重复以上计算。
更进一步地，所述动力学控制器的设计包括：实现期望纵向速度
Figure 100138DEST_PATH_IMAGE080
和趋近角
Figure 607343DEST_PATH_IMAGE081
的控制，其中纵向速度的期望值
Figure 413625DEST_PATH_IMAGE082
m.s-1，是通控制推进器的转速
Figure 932462DEST_PATH_IMAGE083
产生纵向力X来实现，趋近角
Figure 798787DEST_PATH_IMAGE084
是通过控制水平舵角
Figure 527708DEST_PATH_IMAGE085
和垂直舵角
Figure 278627DEST_PATH_IMAGE086
产生纵倾力矩M和艏摇力矩N来实现；动力学控制器采用如下PID控制器
Figure 776604DEST_PATH_IMAGE087
(19)
PID控制器的参数设置为kX p=20，kX i=5，kX d=5，kM p=2，kM i=3，kM d=0.1，kN p=2，kN i=3，kN d=0.1。
本发明的优点：本发明针对一种常见的缺少横向和垂向驱动力的欠驱动AUV，采用USBL进行定位，获取喇叭口导向式对接装置上的四个应答器在载体坐标系中的坐标，并通过坐标变换得到AUV在固定坐标系中的位置和姿态，考虑USBL视角的约束，应用MPC优化了其对接过程中的期望趋近角，不仅实现了其三维空间内的对接控制，而且有效的缩短了对接距离。
除了上面所描述的目的、特征和优点之外，本发明还有其它的目的、特征和优点。下面将参照图，对本发明作进一步详细的说明。
附图说明
构成本申请的一部分的附图用来提供对本发明的进一步理解，本发明的示意性实施例及其说明用于解释本发明，并不构成对本发明的不当限定。
图1是AUV三维对接示意图；
图2是本发明的AUV三维对接控制原理框图；
图3是本发明的AUV三维对接控制仿真对接三维示意图；
图4是本发明的AUV三维对接控制仿真对接水平面示意图；
图5是本发明的AUV三维对接控制仿真对接垂直面示意图；
图6是本发明的AUV三维对接控制仿真推进器转速曲线图；
图7是本发明的AUV三维对接控制仿真水平舵角曲线图；
图8是本发明的AUV三维对接控制仿真垂直舵角曲线图；
图9是本发明的AUV三维对接控制仿真纵向速度曲线图；
图10是本发明的AUV三维对接控制仿真对接纵向误差曲线图；
图11是本发明的AUV三维对接控制仿真对接横向误差曲线图；
图12是本发明的AUV三维对接控制仿真对接垂向误差曲线图；
图13是本发明的AUV三维对接控制仿真垂直面趋近角和纵倾角（MPC）曲线图；
图14是本发明的AUV三维对接控制仿真垂直面趋近角和纵倾角（LOS前视距离3m）曲线图；
图15是本发明的AUV三维对接控制仿真垂直面趋近角和纵倾角（LOS前视距离8m）曲线图；
图16是本发明的AUV三维对接控制仿真水平面趋近角和艏摇角（MPC）曲线图；
图17是本发明的AUV三维对接控制仿真水平面趋近角和艏摇角（LOS前视距离3m）曲线图；
图18是本发明的AUV三维对接控制仿真）水平面趋近角和艏摇角（LOS前视距离8m）曲线图。
具体实施方式
为了使本发明的目的、技术方案及优点更加清楚明白，以下结合附图及实施例，对本发明进行进一步详细说明。应当理解，此处所描述的具体实施例仅仅用以解释本发明，并不用于限定本发明。
参考图1至图2，一种欠驱动auv的模型预测对接控制方法，包括：对接误差模型设计；对接控制器的设计，包括期望趋近角的设计和动力学控制器的设计。
本发明的欠驱动AUV的航速是通过尾部的推进器控制纵向速度来实现，潜伏是通过水平舵控制纵倾角来间接实现，航向是通过垂直舵控制艏摇角来实现，AUV在横摇、横向和垂向没有驱动力，其三维对接原理见图1。为了便于建立对接误差模型。
所述对接误差模型设计包括：设置了两个坐标系，一个为载体坐标系
Figure 813830DEST_PATH_IMAGE088
，其原点定义在AUV的浮心O处，另一个为固定坐标系
Figure 170993DEST_PATH_IMAGE089
，其原点定义在导向式对接装置喇叭口的中心E处；载体坐标系的x轴指向前方、y轴指向右侧、z轴指向下方，固定坐标系的ξ轴指向前方与对接路径重合、η轴指向右侧的应答器2、ζ轴指向下方的应答器4；对接误差模型可以简化为
Figure 319078DEST_PATH_IMAGE090
(1)
式（1）中，
Figure 937141DEST_PATH_IMAGE091
为AUV在
Figure 145269DEST_PATH_IMAGE092
中的位置和姿态，即对接误差，
Figure 724149DEST_PATH_IMAGE093
代表AUV在
Figure 675924DEST_PATH_IMAGE092
中的坐标、θ为AUV的纵倾角、ψ为AUV的艏摇角；
Figure 414073DEST_PATH_IMAGE094
代表AUV在
Figure 668468DEST_PATH_IMAGE095
中的速度，u为纵向速度、v为横向速度、w为垂向速度、q为纵倾角速度、r为艏摇角速度；
Figure 859278DEST_PATH_IMAGE096
为
Figure 349165DEST_PATH_IMAGE097
到
Figure 941820DEST_PATH_IMAGE098
的旋转变换矩阵，
Figure 101537DEST_PATH_IMAGE099
首先通过USBL接收器获取4个应答器在
Figure 45223DEST_PATH_IMAGE100
中的坐标，它们与对接误差满足关系式（2），
Figure 338801DEST_PATH_IMAGE101
(2)
式（2）中，
Figure 661329DEST_PATH_IMAGE102
为USBL接收器获取的4个应答器的坐标，l为USBL接收器与浮心O的距离，
Figure 382160DEST_PATH_IMAGE103
为4个应答器在
Figure 281983DEST_PATH_IMAGE104
中的坐标，
Figure DEST_PATH_IMAGE105
为4个应答器在
Figure 81767DEST_PATH_IMAGE106
中的坐标；
对接装置喇叭口的半径为1米，所以
Figure 914593DEST_PATH_IMAGE107
；通过关系式
Figure 806326DEST_PATH_IMAGE108
可得
Figure 68811DEST_PATH_IMAGE109
(3)
通过关系式
Figure 704192DEST_PATH_IMAGE110
可得
Figure 125946DEST_PATH_IMAGE111
(4)
通过关系式
Figure 188580DEST_PATH_IMAGE112
可得
Figure 203940DEST_PATH_IMAGE113
(5)。
所述期望趋近角的设计包括：设计导引律生成艏摇角和纵倾角的期望值，即期望趋近角；采用如下LOS导引律
Figure 377433DEST_PATH_IMAGE114
(6)
式（6）中
Figure 653693DEST_PATH_IMAGE115
为垂直面趋近角，
Figure 762595DEST_PATH_IMAGE116
为水平面趋近角，
Figure DEST_PATH_IMAGE117
和
Figure 655464DEST_PATH_IMAGE118
为前视距离；采用MPC设计导引律来对期望趋近角进行优化，纵倾角和艏摇角对期望趋近角的误差方程可以近似为以下微分方程
Figure 508014DEST_PATH_IMAGE119
(7)
式（7）中
Figure 638781DEST_PATH_IMAGE120
为可调时间常数，其中
Figure DEST_PATH_IMAGE121
；
根据对接误差模型（1），横向和垂向的对接误差方程可以简化为
Figure 308797DEST_PATH_IMAGE122
(8)
式（8）中
Figure 33170DEST_PATH_IMAGE123
，当
Figure 814044DEST_PATH_IMAGE124
时可以简化为
Figure 64897DEST_PATH_IMAGE125
，当
Figure 515601DEST_PATH_IMAGE126
时可以简化为
Figure 851905DEST_PATH_IMAGE127
；将式（7）和式（8）合并，
则对接控制可以等价为如下误差的镇定问题
Figure 170891DEST_PATH_IMAGE128
(9)
将式（9）离散化可得对接误差的预测模型为
Figure 276250DEST_PATH_IMAGE129
(10)
式（10）中的下标k代表采用时间序列，
Figure 897855DEST_PATH_IMAGE130
Figure DEST_PATH_IMAGE131
和
Figure 987034DEST_PATH_IMAGE132
分别为状态向量和输出向量，均为对接误差，
Figure 844131DEST_PATH_IMAGE133
为输入向量，即期望趋近角，T为采样周期；在k时刻，根据预测模型（10）可得将来k+1至
Figure 942013DEST_PATH_IMAGE134
时刻对接误差的预测值为
Figure 124733DEST_PATH_IMAGE135
式中的
Figure 170049DEST_PATH_IMAGE136
分别代表控制步长和预测步长，其中
Figure 706204DEST_PATH_IMAGE137
；则进一步可得预测的输出值为
Figure 786155DEST_PATH_IMAGE138
预测输出值可以整理为以下矩阵形式
Figure 874197DEST_PATH_IMAGE139
(11)
Figure 406809DEST_PATH_IMAGE140
Figure DEST_PATH_IMAGE141
因为USBL接收器的视角是受限制的，所以考虑期望趋近角的约束条件为
Figure 277813DEST_PATH_IMAGE142
(12)
Figure 212271DEST_PATH_IMAGE143
将（12）整理为以下线性矩阵不等式
Figure 346581DEST_PATH_IMAGE144
(13)
Figure 632068DEST_PATH_IMAGE145
符号
Figure 634660DEST_PATH_IMAGE057
为克罗内克积；构造Lyapunov函数
Figure 298990DEST_PATH_IMAGE146
(14)
对式（14）求导可得
Figure 994414DEST_PATH_IMAGE147
式中，
Figure 501618DEST_PATH_IMAGE148
，只要满足
Figure DEST_PATH_IMAGE149
，则
Figure 183267DEST_PATH_IMAGE150
；
将稳定约束条件整理为以下线性矩阵不等式
Figure 357896DEST_PATH_IMAGE151
(15)
Figure 958642DEST_PATH_IMAGE152
求解最优问题，定义如下代价函数
Figure 828509DEST_PATH_IMAGE153
(16)
代价函数（16）中的加权矩阵
Figure 172902DEST_PATH_IMAGE154
,
Figure 202038DEST_PATH_IMAGE155
,
Figure 114631DEST_PATH_IMAGE156
，
Figure 330848DEST_PATH_IMAGE157
均为正定矩阵，其中
Figure 744512DEST_PATH_IMAGE158
，
Figure 240871DEST_PATH_IMAGE159
将（11）带入（16）可得
Figure 448999DEST_PATH_IMAGE160
(17)
式（17）中
Figure 418092DEST_PATH_IMAGE161
， 因为
Figure 979654DEST_PATH_IMAGE162
在每一个采用时刻为常值，所以考虑约束的MPC最优问题可以等价为对以下二次型的求解
Figure 717803DEST_PATH_IMAGE163
(18)
在每个采用周期通过求解式（18）可以得到将来
Figure 96832DEST_PATH_IMAGE164
步最优期望趋近角的序列值
Figure 287642DEST_PATH_IMAGE165
，当然只要将
Figure 918474DEST_PATH_IMAGE166
的第一组值
Figure 245550DEST_PATH_IMAGE167
作为当前的期望趋近角即可，当进入下一个采用周期再重复以上计算。
所述动力学控制器的设计包括：实现期望纵向速度
Figure 795480DEST_PATH_IMAGE168
和趋近角
Figure 348953DEST_PATH_IMAGE169
的控制，其中纵向速度的期望值
Figure 642531DEST_PATH_IMAGE170
m.s-1，是通控制推进器的转速
Figure 355272DEST_PATH_IMAGE171
产生纵向力X来实现，趋近角
Figure 685890DEST_PATH_IMAGE172
是通过控制水平舵角
Figure 851292DEST_PATH_IMAGE173
和垂直舵角
Figure 682982DEST_PATH_IMAGE174
产生纵倾力矩M和艏摇力矩N来实现；动力学控制器采用如下PID控制器
Figure 250230DEST_PATH_IMAGE175
(19)
PID控制器的参数设置为kX p=20，kX i=5，kX d=5，kM p=2，kM i=3，kM d=0.1，kN p=2，kN i=3，kN d=0.1。
欠驱动AUV的三维对接控制原理框图见图2。
仿真实验与分析
为了验证控制器的性能，接下了进行了三维对接控制仿真实验，图3为仿真结果。仿真中采用了欠驱动AUV REMUS-100的动力学仿真模型。AUV对接的初始位姿为
Figure 17328DEST_PATH_IMAGE176
。在水平和垂直方向，对接的允许误差均为±0.25 m，对接的期望趋近角分别采用LOS和MPC两种导引律生成，其中在LOS导引律中分别采用了3 m和8 m两种不同的前视距离。
图3为其对接三维示意图，图4为对接水平面示意图，图5为对接垂直面示意图。可以看到当LOS导引律的前视距离为3 m时，初始误差收敛最快，但垂向误差出现了超调现象，大约在对接装置前方40 m处，水平和垂直方向的对接误差均收敛到允许范围之内。当LOS导引律的前视距离增大为8 m时，初始误差收敛最慢，大约在对接装置前方30 m处，水平和垂直方向的对接误差才收敛到允许范围之内。当采用MPC导引律时，大约在对接装置前方50 m处，水平和垂直方向的对接误差就收敛到允许范围之内，所以采用MPC导引律时所需要的对接距离最短。
图6至图8分别为推进器的转速、水平舵角和垂直舵角曲线，可见控制信号都比较平稳。
通过图9可以看到纵向速度可以很好的稳定在期望值。
图10至图12为对接的位置误差曲线，可见所有位置误差均能收敛到允许范围之内，但是采用MPC导引律时所用的收敛时间最短。
图13至图18为对接的期望趋近角和姿态误差曲线，当LOS导引律的前视距离为3 m时，期望趋近角最大且出现了超出约束范围的情况，但是采用MPC导引律时期望趋近角均在约束范围内。
结论
本发明针对一种常见的缺少横向和垂向驱动力的欠驱动AUV，采用USBL进行定位，获取喇叭口导向式对接装置上的四个应答器在载体坐标系中的坐标，并通过坐标变换得到AUV在固定坐标系中的位置和姿态，考虑USBL视角的约束，应用MPC优化了其对接过程中的期望趋近角，不仅实现了其三维空间内的对接控制，而且有效的缩短了对接距离。
以上所述仅为本发明的较佳实施例，并不用以限制本发明，凡在本发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。

Claims (4)

1.一种欠驱动auv的模型预测对接控制方法，其特征在于，包括：对接误差模型设计；对接控制器的设计，包括期望趋近角的设计和动力学控制器的设计。
2.根据权利要求1所述的欠驱动auv的模型预测对接控制方法，其特征在于，所述对接误差模型设计包括：设置了两个坐标系，一个为载体坐标系
Figure 892697DEST_PATH_IMAGE001
，其原点定义在AUV的浮心O处，另一个为固定坐标系
Figure 338722DEST_PATH_IMAGE002
，其原点定义在导向式对接装置喇叭口的中心E处；载体坐标系的x轴指向前方、y轴指向右侧、z轴指向下方，固定坐标系的ξ轴指向前方与对接路径重合、η轴指向右侧的应答器2、ζ轴指向下方的应答器4；
对接误差模型可以简化为
Figure 938330DEST_PATH_IMAGE003
(1)
式（1）中，
Figure 811608DEST_PATH_IMAGE004
为AUV在
Figure 739244DEST_PATH_IMAGE005
中的位置和姿态，即对接误差，
Figure 723381DEST_PATH_IMAGE006
代表AUV在
Figure 443075DEST_PATH_IMAGE007
中的坐标、θ为AUV的纵倾角、ψ为AUV的艏摇角；
Figure 611888DEST_PATH_IMAGE008
代表AUV在
Figure 151454DEST_PATH_IMAGE009
中的速度，u为纵向速度、v为横向速度、w为垂向速度、q为纵倾角速度、r为艏摇角速度；
Figure 939281DEST_PATH_IMAGE010
为
Figure 388848DEST_PATH_IMAGE011
到
Figure 603929DEST_PATH_IMAGE012
的旋转变换矩阵，
Figure 630791DEST_PATH_IMAGE013
首先通过USBL接收器获取4个应答器在
Figure 222309DEST_PATH_IMAGE014
中的坐标，它们与对接误差满足关系式（2），
Figure 775650DEST_PATH_IMAGE015
(2)
式（2）中，
Figure 896053DEST_PATH_IMAGE016
为USBL接收器获取的4个应答器的坐标，l为USBL接收器与浮心O的距离，
Figure 675790DEST_PATH_IMAGE017
为4个应答器在
Figure 680786DEST_PATH_IMAGE018
中的坐标，
Figure 964000DEST_PATH_IMAGE019
为4个应答器在
Figure 786463DEST_PATH_IMAGE020
中的坐标；
对接装置喇叭口的半径为1米，所以
Figure 787917DEST_PATH_IMAGE021
；通过关系式
Figure 580292DEST_PATH_IMAGE022
可得
Figure 718013DEST_PATH_IMAGE023
(3)
通过关系式
Figure 711376DEST_PATH_IMAGE024
可得
Figure 637062DEST_PATH_IMAGE025
(4)
通过关系式
Figure 108495DEST_PATH_IMAGE026
可得
Figure 366301DEST_PATH_IMAGE027
(5)。
3.根据权利要求1所述的欠驱动auv的模型预测对接控制方法，其特征在于，所述期望趋近角的设计包括：设计导引律生成艏摇角和纵倾角的期望值，即期望趋近角；采用如下LOS导引律
Figure 655199DEST_PATH_IMAGE028
(6)
式（6）中
Figure 365666DEST_PATH_IMAGE029
为垂直面趋近角，
Figure 640790DEST_PATH_IMAGE030
为水平面趋近角，
Figure 18682DEST_PATH_IMAGE031
和
Figure 698056DEST_PATH_IMAGE032
为前视距离；
采用MPC设计导引律来对期望趋近角进行优化，纵倾角和艏摇角对期望趋近角的误差方程可以近似为以下微分方程
Figure 161398DEST_PATH_IMAGE033
(7)
式（7）中
Figure 974633DEST_PATH_IMAGE034
为可调时间常数，其中
Figure 331665DEST_PATH_IMAGE035
；
根据对接误差模型（1），横向和垂向的对接误差方程可以简化为
Figure 306575DEST_PATH_IMAGE036
(8)
式（8）中
Figure 257213DEST_PATH_IMAGE037
，当
Figure 749506DEST_PATH_IMAGE038
时可以简化为
Figure 836410DEST_PATH_IMAGE039
，当
Figure 513379DEST_PATH_IMAGE040
时可以简化为
Figure 685735DEST_PATH_IMAGE041
；将式（7）和式（8）合并，则对接控制可以等价为如下误差的镇定问题
Figure 965406DEST_PATH_IMAGE042
(9)
将式（9）离散化可得对接误差的预测模型为
Figure 906817DEST_PATH_IMAGE043
(10)
式（10）中的下标k代表采用时间序列，
Figure 489108DEST_PATH_IMAGE044
Figure 24126DEST_PATH_IMAGE045
和
Figure 982855DEST_PATH_IMAGE046
分别为状态向量和输出向量，均为对接误差，
Figure 44352DEST_PATH_IMAGE047
为输入向量，即期望趋近角，T为采样周期；在k时刻，根据预测模型（10）可得将来k+1至
Figure 63123DEST_PATH_IMAGE048
时刻对接误差的预测值为
Figure 334705DEST_PATH_IMAGE049
式中的
Figure 565966DEST_PATH_IMAGE050
分别代表控制步长和预测步长，其中
Figure 481969DEST_PATH_IMAGE051
；则进一步可得预测的输出值为
Figure 544079DEST_PATH_IMAGE052
预测输出值可以整理为以下矩阵形式
Figure 178322DEST_PATH_IMAGE053
(11)
Figure 213274DEST_PATH_IMAGE054
Figure 108418DEST_PATH_IMAGE055
因为USBL接收器的视角是受限制的，所以考虑期望趋近角的约束条件为
Figure 468992DEST_PATH_IMAGE056
(12)
Figure 590532DEST_PATH_IMAGE057
将（12）整理为以下线性矩阵不等式
Figure 429175DEST_PATH_IMAGE058
(13)
Figure 195137DEST_PATH_IMAGE059
符号
Figure 726612DEST_PATH_IMAGE060
为克罗内克积；构造Lyapunov函数
Figure 804290DEST_PATH_IMAGE061
(14)
对式（14）求导可得
Figure 836837DEST_PATH_IMAGE062
式中，
Figure 581939DEST_PATH_IMAGE063
，只要满足
Figure 18736DEST_PATH_IMAGE064
，则
Figure 724655DEST_PATH_IMAGE065
；
将稳定约束条件整理为以下线性矩阵不等式
Figure 905101DEST_PATH_IMAGE066
(15)
Figure 770289DEST_PATH_IMAGE067
求解最优问题，定义如下代价函数
Figure 502622DEST_PATH_IMAGE068
(16)
代价函数（16）中的加权矩阵
Figure 820470DEST_PATH_IMAGE069
,
Figure 804607DEST_PATH_IMAGE070
,
Figure 524301DEST_PATH_IMAGE071
，
Figure 178268DEST_PATH_IMAGE072
均为正定矩阵，其中
Figure 983413DEST_PATH_IMAGE073
，
Figure 771240DEST_PATH_IMAGE074
将（11）带入（16）可得
Figure 470075DEST_PATH_IMAGE075
(17)
式（17）中
Figure 419576DEST_PATH_IMAGE076
， 因为
Figure 446438DEST_PATH_IMAGE077
在每一个采用时刻为常值，所以考虑约束的MPC最优问题可以等价为对以下二次型的求解
Figure 916252DEST_PATH_IMAGE078
(18)
在每个采用周期通过求解式（18）可以得到将来
Figure 79380DEST_PATH_IMAGE079
步最优期望趋近角的序列值
Figure 465362DEST_PATH_IMAGE080
，当然只要将
Figure 369733DEST_PATH_IMAGE080
的第一组值
Figure 499363DEST_PATH_IMAGE081
作为当前的期望趋近角即可，当进入下一个采用周期再重复以上计算。
4.根据权利要求1所述的欠驱动auv的模型预测对接控制方法，其特征在于，所述动力学控制器的设计包括：实现期望纵向速度
Figure 516998DEST_PATH_IMAGE082
和趋近角
Figure 214826DEST_PATH_IMAGE083
的控制，其中纵向速度的期望值
Figure 216281DEST_PATH_IMAGE084
m.s-1，是通控制推进器的转速
Figure 884022DEST_PATH_IMAGE085
产生纵向力X来实现，趋近角
Figure 146376DEST_PATH_IMAGE086
是通过控制水平舵角
Figure 139740DEST_PATH_IMAGE087
和垂直舵角
Figure 362911DEST_PATH_IMAGE088
产生纵倾力矩M和艏摇力矩N来实现；动力学控制器采用如下PID控制器
Figure 99923DEST_PATH_IMAGE089
(19)
PID控制器的参数设置为kX p=20，kX i=5，kX d=5，kM p=2，kM i=3，kM d=0.1，kN p=2，kN i=3，kN d=0.1。
CN202111361802.1A 2021-11-17 2021-11-17 一种欠驱动auv的模型预测对接控制方法 Active CN114019805B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202111361802.1A CN114019805B (zh) 2021-11-17 2021-11-17 一种欠驱动auv的模型预测对接控制方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202111361802.1A CN114019805B (zh) 2021-11-17 2021-11-17 一种欠驱动auv的模型预测对接控制方法

Publications (2)

Publication Number Publication Date
CN114019805A true CN114019805A (zh) 2022-02-08
CN114019805B CN114019805B (zh) 2023-05-23

Family

ID=80064968

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202111361802.1A Active CN114019805B (zh) 2021-11-17 2021-11-17 一种欠驱动auv的模型预测对接控制方法

Country Status (1)

Country Link
CN (1) CN114019805B (zh)

Citations (7)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN105527967A (zh) * 2015-12-16 2016-04-27 西北工业大学 一种auv反演对接控制方法
CN106444794A (zh) * 2016-09-20 2017-02-22 哈尔滨工程大学 一种无参数欠驱动uuv垂直面路径跟踪滑模控制方法
CN106444806A (zh) * 2016-09-27 2017-02-22 哈尔滨工程大学 基于生物速度调节的欠驱动auv三维轨迹跟踪控制方法
CN108303079A (zh) * 2017-12-21 2018-07-20 中国船舶重工集团公司第七0七研究所 一种水下usbl反向应用的数据平滑方法
FR3080194A1 (fr) * 2018-04-12 2019-10-18 Cgg Services Sas Procede de guidage d'un vehicule sous-marin autonome et systeme associe d'acquisition de donnees d'analyse sous-marine
JP2021034050A (ja) * 2019-08-21 2021-03-01 哈爾浜工程大学 強化学習に基づくａｕｖ行動計画及び動作制御方法
CN112527018A (zh) * 2020-12-26 2021-03-19 九江职业技术学院 一种欠驱动自主水下航行器的三维镇定控制方法

Patent Citations (7)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN105527967A (zh) * 2015-12-16 2016-04-27 西北工业大学 一种auv反演对接控制方法
CN106444794A (zh) * 2016-09-20 2017-02-22 哈尔滨工程大学 一种无参数欠驱动uuv垂直面路径跟踪滑模控制方法
CN106444806A (zh) * 2016-09-27 2017-02-22 哈尔滨工程大学 基于生物速度调节的欠驱动auv三维轨迹跟踪控制方法
CN108303079A (zh) * 2017-12-21 2018-07-20 中国船舶重工集团公司第七0七研究所 一种水下usbl反向应用的数据平滑方法
FR3080194A1 (fr) * 2018-04-12 2019-10-18 Cgg Services Sas Procede de guidage d'un vehicule sous-marin autonome et systeme associe d'acquisition de donnees d'analyse sous-marine
JP2021034050A (ja) * 2019-08-21 2021-03-01 哈爾浜工程大学 強化学習に基づくａｕｖ行動計画及び動作制御方法
CN112527018A (zh) * 2020-12-26 2021-03-19 九江职业技术学院 一种欠驱动自主水下航行器的三维镇定控制方法

Non-Patent Citations (3)

* Cited by examiner, † Cited by third party
Title
姚绪梁;王晓伟;: "基于MPC导引律的AUV路径跟踪和避障控制" *
齐贝贝 等: "基于偶极势场的AUV模型预测对接控制算法" *
齐贝贝;齐仲里;王林林;: "基于模型预测控制的自主水下航行器模糊回坞导引算法", 国外电子测量技术 *

Also Published As

Publication number Publication date
CN114019805B (zh) 2023-05-23

Similar Documents

Publication Publication Date Title
CN106444806B (zh) 基于生物速度调节的欠驱动auv三维轨迹跟踪控制方法
Pshikhopov et al. Control system design for autonomous underwater vehicle
Huang et al. Vehicle-manipulator system dynamic modeling and control for underwater autonomous manipulation
Wang et al. A simultaneous planning and control method integrating APF and MPC to solve autonomous navigation for USVs in unknown environments
CN112650231B (zh) 一种实现避碰和避障的欠驱船编队控制系统
Du et al. Research on UUV recovery active disturbance rejection control based on LMNN compensation
Yan et al. Modeling, strategy and control of UUV for autonomous underwater docking recovery to moving platform
Raimondi et al. Hierarchical fuzzy/lyapunov control for horizontal plane trajectory tracking of underactuated AUV
Yan et al. Dynamic docking technology between AUV and mobile mothership
Liang et al. Path following control for underactuated AUV based on feedback gain backstepping
David et al. Unmanned underwater vehicle navigation and collision avoidance using fuzzy logic
Yakimenko et al. AUV rendezvous trajectories generation for underwater recovery
CN116027796A (zh) 一种多自治水下机器人编队控制系统及方法
CN114019805A (zh) 一种欠驱动auv的模型预测对接控制方法
CN113110458A (zh) 一种无人船虚拟目标跟踪控制系统
Shi et al. Advanced model predictive control for autonomous marine vehicles
JP7501946B1 (ja) 水中グライダのベクトル場における経路追跡の制御方法
Li et al. Experimental Research on Formation Control of UUVs Based on Behavior Rules
Chen et al. Improved multi-uuv formation control for artificial potential fields and virtual navigators
Min et al. Research on the Heading Control of Underwater Vehicle Under Hover Condition
Yeyi et al. AUV Dynamic Docking Based on Longitudinal Safety Speed
Chavez et al. An Approach for Trajectory Tracking Control of an Underactuated Autonomous Underwater Vehicle Considering Time Delay
Zhou et al. The Tracking control of Autonomous Underwater Vehicle Based on FA-Model Predictive Control
Zhang et al. Path following control for static docking recovery of an underactuated AUV in the presence of ocean current disturbances
Cao et al. Hovering Control of an Underwater Vehicle

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