水下机器人三维编队及避障方法技术

本发明专利技术涉及一种水下机器人三维编队及避障方法，包括：采用虚拟结构法定义几何队形，并根据队形中心参考轨迹，构建水下机器人的三维编队系统数学模型；无障碍自由环境下，基于Lyapunov稳定性原理计算各水下机器人的参考状态向量；计算单个障碍对水下机器人运动的影响，以修正各水下机器人的参考运动速度；根据零空间策略，考虑所有障碍物对水下机器人运动的影响，获得多障碍环境下的各水下机器人参考状态向量；各水下机器人根据各自的参考状态向量，利用非线性模型预测控制器进行编队控制问题求解，获得最优控制输入。本发明专利技术基于修正零空间模型预测考虑所有障碍物对水下机器人运动的影响，同时保证机器人稳定编队航行与安全避障，适应于复杂环境。

【技术实现步骤摘要】
水下机器人三维编队及避障方法
本专利技术属于水下机器人导航制导与控制
，尤其涉及一种基于修正零空间模型预测控制的水下机器人三维编队及避障方法。
技术介绍
近些年来，随着水下机器人逐渐在海洋环境监控、海底资源探测、搜索救援等领域展开应用，国内外学者针对如何提升水下机器人的自主航行作业能力与环境适应能力进行了大量研究。其中，编队控制技术是水下机器人协同执行具体作业任务的基础关键技术，它要求控制多艘机器人组成并保持一定的几何队形。国内外学者针对编队问题提出了一系列控制结构，主要包括长机-僚机法、基于行为的方法、以及虚拟结构法，它们适用于不同环境且各有优缺点：长机-僚机法原理简单，但存在编队误差累计、鲁棒性差等问题；基于行为的方法适应环境的能力强，但存在系统设计困难等缺陷；虚拟结构法的编队精度高，但灵活性差。水下机器人在三维复杂环境下编队航行时，往往会遇到各类突发障碍物或威胁，此时还要求机器人能够安全躲避障碍物，并避免机器人间发生碰撞。上述约束大大增加了编队问题的复杂度，而模型预测控制方法可灵活处理各种约束，鲁棒性高，因此将模型预测控制与本文档来自技高网...

【技术保护点】
1.一种水下机器人三维编队及避障方法，其特征在于，包括以下步骤：/n(S1)构建水下机器人的三维编队系统数学模型：采用虚拟结构法定义几何队形，并根据队形中心参考轨迹，构建水下机器人的三维编队系统数学模型；/n(S2)无障碍自由环境下，基于Lyapunov稳定性原理计算各水下机器人的参考状态向量；/n(S3)计算单个障碍对水下机器人运动的影响，以修正各水下机器人的参考运动速度；/n(S4)根据零空间策略，考虑所有障碍物对水下机器人运动的影响，获得多障碍环境下的各水下机器人参考状态向量；/n(S5)各水下机器人根据各自的参考状态向量，利用非线性模型预测控制器进行编队控制问题求解，获得最优控制输入。...

【技术特征摘要】
1.一种水下机器人三维编队及避障方法，其特征在于，包括以下步骤：
(S1)构建水下机器人的三维编队系统数学模型：采用虚拟结构法定义几何队形，并根据队形中心参考轨迹，构建水下机器人的三维编队系统数学模型；
(S2)无障碍自由环境下，基于Lyapunov稳定性原理计算各水下机器人的参考状态向量；
(S3)计算单个障碍对水下机器人运动的影响，以修正各水下机器人的参考运动速度；
(S4)根据零空间策略，考虑所有障碍物对水下机器人运动的影响，获得多障碍环境下的各水下机器人参考状态向量；
(S5)各水下机器人根据各自的参考状态向量，利用非线性模型预测控制器进行编队控制问题求解，获得最优控制输入。


2.根据权利要求1所述的水下机器人三维编队及避障方法，其特征在于，步骤(S1)采用虚拟结构法定义几何队形，并根据队形中心参考轨迹，构建水下机器人的三维编队系统数学模型的方法为：
不考虑洋流场影响，将第i个水下机器人Ri在惯性坐标系O-xyz下的质点运动模型定义为：



其中，si＝[xi,yi,zi,vi,ψi,γi]T和ui＝[uxi,uyi,uzi]T分别表示惯性坐标系O-xyz下第i个水下机器人的状态和控制输入向量，(xi,yi,zi)为机器人位置坐标，vi、ψi、γi分别为机器人速率、艏向角、纵倾角，uxi、uyi、uzi分别表示水下机器人在前进、艏向、纵倾三个方向上的过载分量；且满足ux,min≤uxi≤ux,max、uy,min≤uyi≤uy,max、uz,min≤uzi≤uz,max、zmin≤zi≤zmax、vmin≤vi≤vmax、γmin≤γi≤γmax；
将所有水下机器人组队看作一个虚拟刚体，采用虚拟结构法定义几何队形，且其几何中心点为Or，机器人编队的期望运动轨迹即为队形中心Or的参考运动轨迹，则参考运动轨迹定义为：



其中,惯性坐标系O-xyz下第i个水下机器人Ri和中心点Or的位置坐标分别为Pi＝[xi,yi,zi]T和Pr＝[xr,yr,zr]T，虚拟中心点Or的期望速率、方位角、纵倾角分别为vr、ψr、γr，ωr为转弯角速率；
以虚拟中心点Or的速度矢量在水平面内的投影为xr轴，以垂直向上方向为zr轴方向，建立三维编队坐标系Or-xryrzr，则将编队坐标系Or-xryrzr下第i个水下机器人Ri的位置Pir＝[xir,yir,zir]T表示为：



将式(3)求导，得到：



根据期望的几何队形，确定水下机器人Ri在编队坐标系Or-xryrzr下的期望位置Pdir＝[xdir,ydir,zdir]T，则编队误差表示为：
Pie＝[xie,yie,zie]T＝Pir-Pdir＝[xir-xdir,yir-ydir,zir-zdir]T(5)
根据式(1)-(5),构建水下机器人Ri的三维编队系统数学模型为：



其中，第i个水下机器人Ri在编队坐标系Or-xryrzr下的状态向量为si＝[xie,yie,zie,vi,ψi,γi]T。


3.根据权利要求2所述的水下机器人三维编队及避障方法，其特征在于，步骤(S2)无障碍自由环境下，基于Lyapunov稳定性原理计算各水下机器人的参考状态向量的方法为：
定义Lyapunov距离函数：



对式(7)求导，得到：



其中，β1、β2、β3为大于0的常系数；vi、ψi、γi分别作为水下机器人Ri的期望速率vir、艏向角ψir、纵倾角γir，期望编队误差为0...

【专利技术属性】
技术研发人员：姚鹏，魏欣，邱立艳，刘玉会，
申请(专利权)人：中国海洋大学，
类型：发明
国别省市：山东;37