一种水下滑翔机的编队控制方法技术

本发明专利技术涉及一种水下滑翔机的编队控制方法及系统，包括利用滑翔机自身数据和感知的流场数据进行滑翔机状态估计，用于多水下滑翔机编队控制；结合多机器人协同路径跟踪，用于多水下滑翔机协同速度控制和多水下滑翔机航点生成；调整多水下滑翔机组成的多边形位置关系及状态，用于输出多种编队控制行迹。本发明专利技术利用三台水下滑翔机进行三角形编队控制试验，试验测试的编队功能包括编队定向航行、顶点跟踪、中心跟踪、队形缩放、队形旋转、收敛控制和队形随动等，以及两种或多种功能组合，取得了正三角形编队的实际与期望边长误差标准差控制在1KM范围内，队形跟踪误差小于0.5KM的良好效果。效果。效果。

【技术实现步骤摘要】
一种水下滑翔机的编队控制方法


[0001]本专利技术涉及水下滑翔机控制
，具体地说是一种水下滑翔机的编队控制算法。

技术介绍

[0002]水下滑翔机是一种通过调节自身浮力和姿态来实现水中滑行的预编程水下潜航器，其编队协同控制不同于其他类型的水下机器人。由于水下滑翔机群仅能在异步出水后进行间接的交互以完成指令更新，是一种信息的异步交互，因此这类交互方式使得具有强时间、空间约束的同步行为采用传统实时协同的方法将难以实现。另一方面，水下滑翔机在水下运动的过程中节点之间不存在信息交互，信息交互的缺失意味着群体中的每个节点都无法获得用于实现行为协同所必备的邻居状态反馈信息，进而无法基于邻居信息实现对自身位姿与速度的调整，因此针对该问题需要结合控制与规划的思想来实现。
[0003]目前水下滑翔机多以指定航路点、指定航迹线、和指定艏向三种方式航行，对于多台水下滑翔机之间的相位性、同步性的控制能力不强。同时水下滑翔机集群监控系统中数据管理平台具备一定的开放性和兼容性，可以实时修改滑翔机的一系列参数，以使得基于监控中心的编队控制能力大大提高。

技术实现思路

[0004]针对水下滑翔机群这种特殊的协同运动控制，研究一种双层反馈协同控制机制，即“云端路径协同反馈环”+“水下路径跟踪反馈环”(如图1所示)，进而将协同控制置于可实现交互的云中心执行，研究水下滑翔机集群在海流影响下队形保持的协同运动控制技术，“云端路径协同反馈环”的计算在仿真计算服务器中运行；水下路径跟踪反馈环中主要研究基于海流干扰的自适应抗扰/规避/顺流运动控制技术，驱使每个单体各自跟踪云端路径协同反馈环中规划的信息，进而通过双层反馈环路实现在异步信息交互下的水下滑翔机协同运动控制，“水下路径跟踪反馈环”属于单体的运动控制，将由水下滑翔机集群监控系统传送指令给滑翔机执行。
[0005]本专利技术为实现上述目的所采用的技术方案是：
[0006]一种水下滑翔机的编队控制方法，包括：利用滑翔机自身数据和感知的流场与环境要素数据进行滑翔机状态估计，用于多水下滑翔机编队控制；结合多机器人协同路径跟踪，用于多水下滑翔机协同速度控制和多水下滑翔机航点生成；调整多水下滑翔机组成的多边形位置关系及状态，用于输出多种编队控制行迹。
[0007]所述该方法包括：
[0008]多水下滑翔机实时反馈各自的位置至上位机控制器；
[0009]根据编队队形需求、流场与环境要素数据以及实时监测的多水下滑翔机位置，按照协同编队控制算法计算协同指令；
[0010]根据协同指令计算多水下滑翔机各航点执行指令，用于控制各个滑翔机的水下航
点轨迹；
[0011]调整误差、重复迭代上述指令计算的步骤，调整多水下滑翔机组成的多边形位置关系及状态，输出多种编队控制行迹。
[0012]所述编队队形需求包括：形状、大小、群体运动方向；
[0013]所述协同指令包括：队形的路点位置序列、队形的速度、行进方向；
[0014]所述各航点执行指令包括：各个滑翔机的俯仰角、航向角、驱动浮力；
[0015]所述协同编队控制算法，包括：
[0016]对于滑翔机单个质点：
[0017]编队队形需求包括：形状与大小、群体运动方向；即，编队队形需求包括：形状与大小、群体运动方向；即，为队形需求；其中x
di
(θ
i
)与y
di
(θ
i
)代表每个滑翔机期望的水平位置，此位置信息即定义了期望队形的形状与大小，ψ
di
(θ
i
)代表第i个滑翔机期望的运动航向，即群体运动方向；θ为虚拟动点的路径参数；
[0018]A.设计虚拟动点的协同更新律，计算θ，
[0019]虚拟动点的协同更新律为：
[0020][0021][0022]其中，1
n
为n个1组成的列向量，L为拉普拉斯矩阵，控制参数为n个1组成的列向量，L为拉普拉斯矩阵，控制参数控制参数系统过程状态μ的向量表达为具体形式
[0023]B.把θ代入计算当前队形中第i个质点的队形需求η
di
(θ
i
)，
[0024]C.把η
di
(θ
i
)代入质点执行控制器公式，计算控制输入量τ＝[τ
u
，τ
v
，τ
r
]T
以驱动质点的运动，τ
u
，τ
v
为质点的驱动力，τ
r
为质点驱动力矩；
[0025]质点执行控制器公式为：
[0026]τ
i
＝z
i1-kz
i2
+r
i
J
i
Sυ
ic
[0027]其中，z
i1
为路径跟踪误差，z
i1
＝J
iT
(η
i-η
di
)，z
i2
为质点速度误差变量，z
i2
＝v
i-v
id
，η
i
为实时监测的多水下滑翔机位置，η
di
为队形需求η
di
(θ
i
)，v
i
为为体坐标系下的实际速度，v
id
为替代虚拟控制率的一阶滤波器状态(即动态面控制)；
[0028]r
i
为当前第i个质点的艏向角速度；J
i
为质点体坐标系与平面坐标系之间的转移矩阵为自变换矩阵，满足S＝-S
T
；
[0029]υ
ir
＝υ
i-υ
if
，υ
ir
为海流干扰下的合速度，υ
i
为体坐标系下的实际速度，υ
if
为体坐标系下的海流流速，υ
ic
＝J
i
υ
if
为平面坐标系下的海流速度，即流场与环境要素数据；
[0030]D.将τ
i
代入考虑海流后的质点模型，计算当前队形中第i个质点的路点(η
i
(t))；
[0031]考虑海流后的质点模型为
[0032][0033][0034]依次计算，得到队形的中各个质点的路点位置序列；
[0035]E.单个滑翔机质点的速度、行进方向包括：
[0036]其中，单个滑翔机质点的速度为：一段时间内的先后路点位置的距离与时间的比值；
[0037]单个滑翔机质点的行进方向为：一段时间内的先后路点位置的距离与实测GPS位置的差值的比值；
[0038]F.组成队形的速度、行进方向包括：
[0039]其中，组成队形的速度为：队形中所有单个滑翔机质点一段时间内先后路点位置本文档来自技高网...

【技术保护点】

【技术特征摘要】
1.一种水下滑翔机的编队控制方法，其特征在于，包括：利用滑翔机自身数据和感知的流场与环境要素数据进行滑翔机状态估计，用于多水下滑翔机编队控制；结合多机器人协同路径跟踪，用于多水下滑翔机协同速度控制和多水下滑翔机航点生成；调整多水下滑翔机组成的多边形位置关系及状态，用于输出多种编队控制行迹。2.根据权利要求1所述的一种水下滑翔机的编队控制方法，其特征在于，所述该方法包括：多水下滑翔机实时反馈各自的位置至上位机控制器；根据编队队形需求、流场与环境要素数据以及实时监测的多水下滑翔机位置，按照协同编队控制算法计算协同指令；根据协同指令计算多水下滑翔机各航点执行指令，用于控制各个滑翔机的水下航点轨迹；调整误差、重复迭代上述指令计算的步骤，调整多水下滑翔机组成的多边形位置关系及状态，输出多种编队控制行迹。3.根据权利要求2所述的一种水下滑翔机的编队控制方法，其特征在于，所述编队队形需求包括：形状、大小、群体运动方向；所述协同指令包括：队形的路点位置序列、队形的速度、行进方向；所述各航点执行指令包括：各个滑翔机的俯仰角、航向角、驱动浮力。4.根据权利要求3所述的一种水下滑翔机的编队控制方法，其特征在于，所述协同编队控制算法，包括：对于滑翔机单个质点：编队队形需求包括：形状与大小、群体运动方向；即，编队队形需求包括：形状与大小、群体运动方向；即，为队形需求；其中x
di
(θ
i
)与y
di
(θ
i
)代表每个滑翔机期望的水平位置，此位置信息即定义了期望队形的形状与大小，ψ
di
(θ
i
)代表第i个滑翔机期望的运动航向，即群体运动方向；θ为虚拟动点的路径参数；A.设计虚拟动点的协同更新律，计算θ，虚拟动点的协同更新律为：虚拟动点的协同更新律为：其中，1
n
为n个1组成的列向量，L为拉普拉斯矩阵，控制参数为n个1组成的列向量，L为拉普拉斯矩阵，控制参数控制参数系统过程状态μ的向量表达为具体形式B.把θ代入计算当前队形中第i个质点的队形需求η
di
(θ
i
)；C.把η
di
(θ
i
)代入质点执行控制器公式，计算控制输入量τ＝[τ
u
，τ
v
，τ
r
]
T
以驱动质点的运
动，τ
u
，τ
v
为质点的驱动力，τ
r
为质点驱动力矩；质点执行控制器公式为：τ
i
＝z
i1-kz
i2
+r
i
J
i
Sυ
ic
其中，z
i1
为路径跟踪误差，z
i1
＝J
iT
(η
i-η
di
)，z
i2
为质点速...

【专利技术属性】
技术研发人员：罗业腾，刘世杰，鞠晓龙，王瑾，谭智铎，
申请(专利权)人：中国科学院沈阳自动化研究所，
类型：发明
国别省市：