《一种自主式水下航行器主从式协同控制方法.pdf》由会员分享，可在线阅读，更多相关《一种自主式水下航行器主从式协同控制方法.pdf（11页珍藏版）》请在专利查询网上搜索。
1、10申请公布号CN104076688A43申请公布日20141001CN104076688A21申请号201410339264X22申请日20140717G05B13/04200601G05D1/1020060171申请人山东省科学院海洋仪器仪表研究所地址266001山东省青岛市浙江路28号72发明人袁健周忠海牟华李俊晓54发明名称一种自主式水下航行器主从式协同控制方法57摘要本发明公开了一种自主式水下航行器主从式协同控制方法，其包括以下步骤考虑外部洋流的影响，将外部洋流视为外部扰动并对其进行数学建模；建立系统惯性坐标系，将两台水下航行器之间的相对速度沿着它们形心之间连线以及垂直连线方向进行矢。
2、量分解，建立系统的协同控制运动模型；通过反馈线性化方法将原系统运动模型转化为带外部扰动的线性系统模型；为了抑制外部扰动的影响设计了转化后系统运动模型的前馈反馈最优控制器。本发明可以在洋流等外扰动存在下，以较高跟踪精度和较小的驱动能量控制一台自主水下航行器以一定的角度和距离跟踪另一个自主水下航行器，能够实现自主水下航行器的协同控制。51INTCL权利要求书1页说明书4页附图5页19中华人民共和国国家知识产权局12发明专利申请权利要求书1页说明书4页附图5页10申请公布号CN104076688ACN104076688A1/1页21一种自主式水下航行器主从式协同控制方法，其特征在于，其包括以下步骤考。
3、虑外部洋流的影响，将外部洋流视为外部扰动并对其进行数学建模；建立系统惯性坐标系，将两台水下航行器之间的相对速度沿着它们形心之间连线以及垂直连线方向进行矢量分解，建立系统的协同控制运动模型；通过反馈线性化方法将原系统运动模型转化为带外部扰动的线性系统模型；为了抑制外部扰动的影响设计了转化后系统运动模型的前馈反馈最优控制器。2如权利要求1所述的自主式水下航行器主从式协同控制方法，其特征在于，所述外部洋流扰动的数学建模为具有持续扰动的外系统模型，由式描述，其中GR22为已知常量矩阵，由洋流扰动特性所决定。所以水下洋流扰动可以描述为满足下列条件的扰动矩阵G的所有特征值满足REIG中FIORELLI等在。
4、MONTEREY海湾进行了多AUVS的协同控制和自适应采样研究的实验，其使用了基于电缆的模块化多AUVS的快速行驶及避障控制，受限于电缆的长度并非完全自主控制。文献中采用了集中式的虚拟结构协同控制方法，采用了虚拟结构编队框架实现了机器人的协同控制，但是虚拟结构为假想存在，现实中并不存在，限制了实际工程应用。DO在文献中研究了通讯受限情况下陆地机器人的协同控制问题，但其并没有考虑水下洋流对系统的扰动影响。发明内容0003本发明所要解决的技术问题是提供一种自主式水下航行器主从式协同控制方法，其可以在洋流等外扰动存在下，以较高跟踪精度和较小的驱动能量控制一台自主水下航行器以一定的角度和距离跟踪另一个。
5、自主水下航行器，能够实现自主水下航行器的协同控制。0004本发明是通过下述技术方案来解决上述技术问题的一种自主式水下航行器主从式协同控制方法，其特征在于，其包括以下步骤考虑外部洋流的影响，将外部洋流视为外部扰动并对其进行数学建模；建立系统惯性坐标系，将两台水下航行器之间的相对速度沿着它们形心之间连线以及垂直连线方向进行矢量分解，建立系统的协同控制运动模型；通过反馈线性化方法将原系统运动模型转化为带外部扰动的线性系统模型；为了抑制外部扰动的影响设计了转化后系统运动模型的前馈反馈最优控制器。0005优选地，所述外部洋流扰动的数学建模为具有持续扰动的外系统模型，由式描述，其中GR22为已知常量矩阵，。
6、由洋流扰动特性所决定。所以水下洋流扰动可以描述为满足下列条件的扰动矩阵G的所有特征值满足REIG0，I1，2；外扰动的初始条件是未知的。说明书CN104076688A2/4页40006优选地，所述建立系统惯性坐标系下协同控制运动模型的具体步骤如下将两台水下航行器之间的相对速度沿着它们形心之间连线以及垂直连线方向进行矢量分解，建立系统的协同控制运动模型，具体为记AI为主机器人，AJ为从机器人，AJ以一定的距离和角度跟踪AI，通过设计合适的协同控制方法，使之形成两个机器人之间的主从式队形；AI和AJ在惯性坐标系下的坐标分别表示为PIXIYIIT和PJXJYJJT；VI和VJ分别为AI和AJ的实际速。
7、度，LIJ为AJ到AI的距离，为AI速度方向和二者连线的夹角，D为参考点到AJ重心的距离，F1和F2为平面内水下扰动分量；将相对速度分别沿着连线方向以及垂直连线方向进行分解，得000700080009为后续处理方便，将上式写为矢量形式，定义UIVIIT，FF1F2T，UJVJJT，IJIJ，则其中0010本发明的积极进步效果在于本发明可以在洋流等外扰动存在下，以较高跟踪精度和较小的驱动能量控制一台自主水下航行器以一定的角度和距离跟踪另一个自主水下航行器，能够实现自主水下航行器的协同控制。附图说明0011图1是本发明专利所提出的两个航行器协同控制系统模型的示意图；0012图2是能有效抑制外扰动的。
8、主从式协同控制方法仿真图；0013图3是两航行器之间距离LIJ变化趋势的示意图；0014图4是两航行器之间的变化趋势的示意图；0015图5是航行器AJ的控制器输入量的示意图。具体实施方式0016下面结合附图给出本发明较佳实施例，以详细说明本发明的技术方案。0017本发明包括以下步骤考虑外部洋流的影响，将外部洋流视为外部扰动并对其进行数学建模；建立系统惯性坐标系，将两台水下航行器之间的相对速度沿着它们形心之间连线以及垂直连线方向进行矢量分解，建立系统的协同控制运动模型；通过反馈线性化方法将原系统运动模型转化为带外部扰动的线性系统模型；为了抑制外部扰动的影响设计了转化后系统运动模型的前馈反馈最优控。
9、制器。说明书CN104076688A3/4页50018所述外部洋流扰动的数学建模为具有持续扰动的外系统模型，由式描述，其中GR22为已知常量矩阵，由洋流扰动特性所决定。所以水下洋流扰动可以描述为满足下列条件的扰动矩阵G的所有特征值满足REIG0，I1，；外扰动的初始条件是未知的。由于水下机器人储能有限，综合考虑系统跟踪误差以及驱动能量消耗问题，选取协调控制系统的最优化性能指标为通过求解RICCATI矩阵方程，得到前馈反馈最优控制律。0019如图1所示，下面举例对本发明做进一步说明，具体步骤为00201考虑到洋流的运动特点，将外部洋流扰动建模为具有持续扰动的外系统模型。由式描述，其中GR22为已。
10、知常量矩阵，由洋流扰动特性所决定。00212建立惯性坐标系，将两台水下航行器之间的相对速度沿着它们形心之间连线以及垂直连线方向进行矢量分解，建立系统的协同控制运动模型。具体为记AI为主机器人，AJ为从机器人。AJ以一定的距离和角度跟踪AI，通过设计合适的协同控制方法，使之形成两个机器人之间的主从式队形。AI和AJ在惯性坐标系下的坐标分别表示为PIXIYIIT和PJXJYJJT；VI和VJ分别为AI和AJ的实际速度，LIJ为AJ到AI的距离，为AI速度方向和二者连线的夹角，D为参考点到AJ重心的距离，F1和F2为平面内水下扰动分量。将相对速度分别沿着连线方向以及垂直连线方向进行分解，得如下式10。
11、02200230024为后续处理方便，将上式写为矢量形式。我们定义UIVIIT，FF1F2T，UJVJJT，IJIJ，则其中00253对协同运动模型1采用输入输出线性化方法，通过设置辅助控制输入和变量代换方法使协同运动模型1转化为线性系统，如下式200260027其中XX1X2T，UVWT，这里F为水下扰动向量且其动态特性由式描述，GR22为已知常量矩阵。说明书CN104076688A4/4页600284由于水下机器人储能有限，综合考虑系统跟踪误差以及驱动能量消耗问题，选取协调控制系统的最优化性能指标为通过求解RICCATI矩阵方程，得到前馈反馈最优控制律为如下式300290030其中P为RI。
12、CCATI矩阵方程如下式40031ATPPAPSPQ040032的唯一正定解，为矩阵方程的唯一解，其中SBR1BT。0033实现协同控制的控制策略为AI通过加速度传感器和陀螺仪检测自身的速度VI、角速度I、驶向角J以及相对驶向角信息，并通过水声通信方式向AJ发送数据AJ结合自身声纳传感器测量的LIJ以及计算所得的IJ进行控制，这样就可以实现AJ以期望距离和角度跟踪AJ，从而实现二者的主从式协同控制。0034本发明的具体实例如下0035惯性坐标系下主航行器的通过自身的各种传感器测量其初始位置和姿态状态。初始状态为PI0XI0YI0I0T2M1M/6RADT，通过加速度和陀螺仪测量其速度和角速度分。
13、别为V35M/S，0RAD/S并且保持不变。从航行器通过自身的各种传感器测量其初始位置和姿态状态，其初始状态为PJ0XJ0YJ0J0T18M12M0RADT，期望间距期望跟踪角度则水下扰动相关矩阵为如下式500360037性能指标相关矩阵为如下式600380039仿真步长005S，仿真时间T200S；K11，K21，D02M。0040通过与传统最优控制进行了仿真比较，表明基于前馈和反馈的最优控制能抑制外扰动对系统的影响，仿真结果如错误未找到引用源。错误未找到引用源。所示。0041上述说明并非是对本发明的限制，本发明并不仅限于上述举例，本技术领域的普通技术人员在本发明的实质范围内所做出的变化、改型、添加或替换，也应属于本发明的保护范围。说明书CN104076688A1/5页7图1说明书附图CN104076688A2/5页8图2说明书附图CN104076688A3/5页9图3说明书附图CN104076688A4/5页10图4说明书附图CN104076688A105/5页11图5说明书附图CN104076688A11。