全驱动式自主水下航行器协同控制方法
【专利摘要】本发明公开了一种全驱动式自主水下航行器(FAAUV)协同控制方法，包括以下步骤：首先建立FAAUV的二阶离散运动模型和动力学模型；其次由于每个水下机器人导航定位设备的定位误差不同，导致其拥有的航路信息具有不一致性；将航路信息视为虚拟领航者信息，其由一系列连续的航路点构成，提供一种将有限时间控制方法和一致性结合起来的有限时间一致性控制器，对FAAUV的速度和位置进行有限时间内的协同控制；每个水下机器人利用测速仪和陀螺仪测定自身的航速和运动姿态，并定时广播其自身的虚拟领航者信息和自身的速度信息。本发明计算出多个FAAUV的不同轴的驱动控制力，实现多个水下航行器在有限的时间内达到预定的编队形状。
【专利说明】全驱动式自主水下航行器协同控制方法
【技术领域】
[0001]本发明涉及一种控制方法，特别是涉及一种全驱动式自主水下航行器协同控制方法。
【背景技术】
[0002]多自主式水下航行器(FAAUVs)的协同控制对于海洋科学考察和海洋开发等具有重要意义。多FAAUVs协协同编队控制可以显著提高FAAUV的包括海洋采样、成像、监视和通信在内众多应用方面的能力。与陆地多机器人(或多智能体)协同控制相比，多FAAUVs协同控制特别困难。虽然近年来多FAAUVs协同控制问题受到了广泛的重视，但多FAAUVs协同控制研究不像陆地多机器人(或多智能体)协同控制那样成熟。当前关于自主水下航行器的编队控制的研究，多采用分散控制器的设计思路，而忽略了自主水下航行器间的耦合关系；或者将多FAAUVs建模为多智能体，而忽略了其姿态特性(横滚角、俯仰角和偏航角)。文献< Mult1-AUV control and adaptive sampling in Monterey Bay > 中 F1relli等在Monterey海湾进行了多AUVs的协同控制和自适应采样研究的实验，其使用了基于电缆的模块化多AUVs的快速行驶及避障控制，受限于电缆的长度并非完全自主控制。文献〈High precis1n format1n control of mobile robots using virtual structureapproach)中采用了集中式的虚拟结构协同控制方法，采用了虚拟结构编队框架实现了机器人的协同控制，但是虚拟结构为假想存在，现实中并不存在，限制了实际工程应用。Do在文献〈Format1n tracking control of unicycle-type mobile robots with limitedsensing ranges)中研究了通讯受限情况下陆地机器人的协同控制问题，但其并没有考虑水下洋流对系统的扰动影响。
[0003]当前的控制方法多是采用无限时间上的轨迹跟踪控制方法，不能保证轨迹在有限的时间内形成，而多机协调控制要求多个机器人能在有限的时间内形成预定的轨迹形状。
【发明内容】
[0004]本发明所要解决的技术问题是提供一种全驱动式自主水下航行器协同控制方法，其通过对FAAUV运动学和动力学的建模，实现多个FAAUV在有限的时间内形成预定的编队轨迹形状。
[0005]本发明是通过下述技术方案来解决上述技术问题的:一种全驱动式自主水下航行器协同控制方法，其特征在于，所述全驱动式自主水下航行器协同控制方法包括以下步骤:首先建立FAAUV的二阶离散运动模型和动力学模型；其次由于每个水下机器人导航定位设备的定位误差不同，导致其拥有的航路信息具有不一致性；将航路信息视为虚拟领航者信息，其由一系列连续的航路点构成，提供一种将有限时间控制方法和一致性结合起来的有限时间一致性控制器，对FAAUV的速度和位置进行有限时间内的一致性协同控制；然后每个水下机器人利用测速仪和陀螺仪测定自身的航速和运动姿态，并且定时向外广播其自身的虚拟领航者信息和自身的速度信息，接收并获得的其他机器人的水声通信广播信息，得到其他机器人的虚拟领航者信息和速度信息，利用协同控制方法求得协同后的速度和姿态；每个机器人都执行上述工作，于是最终形成一致的速度和位姿；最后得到一致性协同控制后的速度和位置，并将速度和位置代入到每个FAAUV的动力学方程中，计算出每个机器人的驱动力和力矩，根据推进器的布局，将其分配到对应轴的推进电机上，从而驱动相应的电动机并带动螺旋桨按照给定的驱动力和力矩转动。
[0006]优选地，所述建立FAAUV的二阶离散运动模型FAAUV i在惯性坐标系下的t时刻运动模型表示为下列二阶系统的形式:
【权利要求】
1.一种全驱动式自主水下航行器协同控制方法，其特征在于，所述全驱动式自主水下航行器协同控制方法包括以下步骤:首先建立FAAUV的二阶离散运动模型和动力学模型；其次由于每个水下机器人导航定位设备的定位误差不同，导致其拥有的航路信息具有不一致性；将航路信息视为虚拟领航者信息，其由一系列连续的航路点构成，提供一种将有限时间控制方法和一致性结合起来的有限时间一致性控制器，对FAAUV的速度和位置进行有限时间内的一致性协同控制；然后每个水下机器人利用测速仪和陀螺仪测定自身的航速和运动姿态，并且定时向外广播其自身的虚拟领航者信息和自身的速度信息，接收并获得的其他机器人的水声通信广播信息，得到其他机器人的虚拟领航者信息和速度信息，利用协同控制方法求得协同后的速度和姿态；每个机器人都执行上述工作，于是最终形成一致的速度和位姿；最后得到一致性协同控制后的速度和位置，并将速度和位置代入到每个FAAUV的动力学方程中，计算出每个机器人的驱动力和力矩，根据推进器的布局，将其分配到对应轴的推进电机上，从而驱动相应的电动机并带动螺旋桨按照给定的驱动力和力矩转动。
2.如权利要求1所述的全驱动式自主水下航行器协同控制方法，其特征在于，所述建立FAAUV的二阶离散运动模型FAAUV i在惯性坐标系下的t时刻运动模型表示为下列二阶系统的形式:
其中Xi⑴表示FAAUV i重心在惯性系下的位置和姿态，Vi⑴表示FAAUV i在惯性坐标系下的速度矢量，Ui (t)为需要设计的FAAUV i的一致性控制矢量，其中，i表示第i个FAAUV，i的数量根据需要协同编队的机器人的数量而定，对上式进行前向差分离散化得:
Xi (k+Ι) = Δ Tvi (k) +Xi (k)
Vi(k+1) = Δ Tui (k) +Vi(k) 其中k表示设备的采样时刻，k = 0,1,2-, Λ T表示采样周期。
3.如权利要求1所述的全驱动式自主水下航行器协同控制方法，其特征在于，所述虚拟领航者信息表示每个FAAUV存储在控制器中的期望领航者位置信息，每个FAAUV利用其自身的位置矢量与其到自身虚拟领航者的距离信息之差来进行一致性协同，可以实现所有FAAUV在惯性坐标系下的线速度、角速度和角位移矢量在有限时间内达到一致，并且使每个FAAUV所有用的虚拟领航者的位置矢量信息有限时间达到一致，所提出的有限时间一致性控制方法为:
其中％为与FAAUV i有通信联系的FAAUVs的集合，Iii为FAAUV i到虚拟领航者的距离矢量，.</00表示k时刻FAAUVi所拥有的虚拟领航者的位置矢量信息，ViGO表示k时刻FAAUV i在惯性坐标系下的速度矢量；0 < a，b < I为与收敛性速度相关的系数；0 < χ<I为与收敛性速度相关的系数；11表示矢量每个元素的绝对值，sigO为矢量函数。
4.如权利要求1所述的全驱动式自主水下航行器协同控制方法，其特征在于，所述动力学模型的建立如下=FAAUV i要实现六自由度运动，安装三对具有正反推力的推进器，沿着三个坐标轴的方向进行布局。沿着Ε-ξ、Ε-η和Ε-ζ轴方向上的合力分别为g(k)、g(k)和巧(k)，作用于Ε-ξ、Ε-η和Ε-ζ轴上的合力矩分别为M^(k)、M；:(k)和O)，分别表示如下:
其中为FAAUV i第ε个推进器和与之平行的坐标轴之间的垂直距离，FAAUV i的第ε个推进器产生的推力为厂、(k)，ε = 1，2…6。
5.如权利要求1所述的全驱动式自主水下航行器协同控制方法，其特征在于，所述全驱动式自主水下航行器协同控制方法求出各个FAAUV的推进器的驱动合力和合力矩，将协同控制方法求得协同后的速度和姿态代入到每个FAAUV的动力学方程中，计算出每个机器人的驱动力和力矩，根据推进器的布局，将其分配到对应轴的推进电机上，从而驱动相应的电动机并带动螺旋桨按 照给定的驱动力和力矩转动。
【文档编号】G05B13/04GK104076689SQ201410339282
【公开日】2014年10月1日 申请日期:2014年7月17日 优先权日:2014年7月17日
【发明者】袁健, 周忠海, 牟华, 李俊晓, 张风丽 申请人:山东省科学院海洋仪器仪表研究所