CN104076688A - 一种自主式水下航行器主从式协同控制方法 - Google Patents

Abstract

本发明公开了一种自主式水下航行器主从式协同控制方法，其包括以下步骤：考虑外部洋流的影响，将外部洋流视为外部扰动并对其进行数学建模；建立***惯性坐标系，将两台水下航行器之间的相对速度沿着它们形心之间连线以及垂直连线方向进行矢量分解，建立***的协同控制运动模型；通过反馈线性化方法将原***运动模型转化为带外部扰动的线性***模型；为了抑制外部扰动的影响设计了转化后***运动模型的前馈-反馈最优控制器。本发明可以在洋流等外扰动存在下，以较高跟踪精度和较小的驱动能量控制一台自主水下航行器以一定的角度和距离跟踪另一个自主水下航行器，能够实现自主水下航行器的协同控制。

Description

一种自主式水下航行器主从式协同控制方法

技术领域

本发明涉及一种控制方法，特别是涉及一种自主式水下航行器主从式协同控制方法。

背景技术

多自主式水下航行器(AUVs)的协同控制对于海洋科学考察和海洋开发等具有重要意义。多AUVs协协同控制可以显著提高AUV的包括海洋采样、成像、监视和通信在内众多应用方面的能力。与陆地多机器人(或多智能体)协同控制相比，多AUVs协同控制特别困难。虽然近年来多AUVs协同控制问题受到了广泛的重视，但多AUVs协同控制研究不像陆地多机器人(或多智能体)协同控制那样成熟。关于多AUVs协同控制的相关研究刚刚起步，其研究结果主要是借鉴了陆地多机器人(或多智能体)协同控制策略。文献<Multi-AUV control and adaptive sampling in Monterey Bay>中Fiorelli等在Monterey海湾进行了多AUVs的协同控制和自适应采样研究的实验，其使用了基于电缆的模块化多AUVs的快速行驶及避障控制，受限于电缆的长度并非完全自主控制。文献<High precision formation control of mobile robots usingvirtual structure approach>中采用了集中式的虚拟结构协同控制方法，采用了虚拟结构编队框架实现了机器人的协同控制，但是虚拟结构为假想存在，现实中并不存在，限制了实际工程应用。Do在文献<Formation tracking control ofunicycle-type mobile robots with limited sensing ranges>中研究了通讯受限情况下陆地机器人的协同控制问题，但其并没有考虑水下洋流对***的扰动影响。

发明内容

本发明所要解决的技术问题是提供一种自主式水下航行器主从式协同控制方法，其可以在洋流等外扰动存在下，以较高跟踪精度和较小的驱动能量控制一台自主水下航行器以一定的角度和距离跟踪另一个自主水下航行器，能够实现自主水下航行器的协同控制。

本发明是通过下述技术方案来解决上述技术问题的：一种自主式水下航行器主从式协同控制方法，其特征在于，其包括以下步骤：考虑外部洋流的影响，将外部洋流视为外部扰动并对其进行数学建模；建立***惯性坐标系，将两台水下航行器之间的相对速度沿着它们形心之间连线以及垂直连线方向进行矢量分解，建立***的协同控制运动模型；通过反馈线性化方法将原***运动模型转化为带外部扰动的线性***模型；为了抑制外部扰动的影响设计了转化后***运动模型的前馈-反馈最优控制器。

优选地，所述外部洋流扰动的数学建模为具有持续扰动的外***模型，由式描述，其中G∈R^2×2为已知常量矩阵，由洋流扰动特性所决定。所以水下洋流扰动可以描述为满足下列条件的扰动：矩阵G的所有特征值满足Re[λ_i(G)]<0，i＝1，2；外扰动的初始条件是未知的。

优选地，所述建立***惯性坐标系下协同控制运动模型的具体步骤如下：将两台水下航行器之间的相对速度沿着它们形心之间连线以及垂直连线方向进行矢量分解，建立***的协同控制运动模型，具体为：记A_i为主机器人，A_j为从机器人，A_j以一定的距离和角度跟踪A_i，通过设计合适的协同控制方法，使之形成两个机器人之间的主从式队形；A_i和A_j在惯性坐标系下的坐标分别表示为p_i＝[x_i y_i θ_i]^T和p_j＝[x_j y_j θ_j]^T；v_i和v_j分别为A_i和A_j的实际速度，l_ij为A_j到A_i的距离，为A_i速度方向和二者连线的夹角，d为参考点到A_j重心的距离，f₁和f₂为平面内水下扰动分量；将相对速度分别沿着连线方向以及垂直连线方向进行分解，得：

为后续处理方便，将上式写为矢量形式，定义u_i＝[v_i ω_i]^T，f＝[f₁ f₂]^T，u_j＝[v_j ω_j]^T，β_ij＝θ_i-θ_j，则 其中 $G_1=\left[\begin{array}{cccc}c& {\mathrm{\&gamma;}}_{i}o& s_{j}d& \mathrm{\&gamma;}\\ -s& {\mathrm{\&gamma;}}_{i}i/l& n_j& d_i\mathrm{\&gamma;}/j\end{array}\right],$ $D_1=\left[\begin{array}{cc}{d}_{11}& d_{12}\\ d_{21}& d_{22}\end{array}\right].$

本发明的积极进步效果在于：本发明可以在洋流等外扰动存在下，以较高跟踪精度和较小的驱动能量控制一台自主水下航行器以一定的角度和距离跟踪另一个自主水下航行器，能够实现自主水下航行器的协同控制。

附图说明

图1是本发明专利所提出的两个航行器协同控制***模型的示意图；

图2是能有效抑制外扰动的主从式协同控制方法仿真图；

图3是两航行器之间距离l_ij变化趋势的示意图；

图4是两航行器之间的变化趋势的示意图；

图5是航行器A_j的控制器输入量的示意图。

具体实施方式

下面结合附图给出本发明较佳实施例，以详细说明本发明的技术方案。

本发明包括以下步骤：考虑外部洋流的影响，将外部洋流视为外部扰动并对其进行数学建模；建立***惯性坐标系，将两台水下航行器之间的相对速度沿着它们形心之间连线以及垂直连线方向进行矢量分解，建立***的协同控制运动模型；通过反馈线性化方法将原***运动模型转化为带外部扰动的线性***模型；为了抑制外部扰动的影响设计了转化后***运动模型的前馈-反馈最优控制器。

所述外部洋流扰动的数学建模为具有持续扰动的外***模型，由式描述，其中G∈R^2×2为已知常量矩阵，由洋流扰动特性所决定。所以水下洋流扰动可以描述为满足下列条件的扰动：矩阵G的所有特征值满足Re[λ_i(G)]<0，i＝1，；外扰动的初始条件是未知的。由于水下机器人储能有限，综合考虑***跟踪误差以及驱动能量消耗问题，选取协调控制***的最优化性能指标为通过求解Riccati矩阵方程，得到前馈-反馈最优控制律。

如图1所示，下面举例对本发明做进一步说明，具体步骤为：

(1)考虑到洋流的运动特点，将外部洋流扰动建模为具有持续扰动的外***模型。由式描述，其中G∈R^2×2为已知常量矩阵，由洋流扰动特性所决定。

(2)建立惯性坐标系，将两台水下航行器之间的相对速度沿着它们形心之间连线以及垂直连线方向进行矢量分解，建立***的协同控制运动模型。具体为：记A_i为主机器人，A_j为从机器人。A_j以一定的距离和角度跟踪A_i，通过设计合适的协同控制方法，使之形成两个机器人之间的主从式队形。A_i和A_j在惯性坐标系下的坐标分别表示为p_i＝[x_i y_i θ_i]^T和p_j＝[x_j y_j θ_j]^T；v_i和v_j分别为A_i和A_j的实际速度，l_ij为A_j到A_i的距离，为A_i速度方向和二者连线的夹角，d为参考点到A_j重心的距离，f₁和f₂为平面内水下扰动分量。将相对速度分别沿着连线方向以及垂直连线方向进行分解，得如下式(1)：

为后续处理方便，将上式写为矢量形式。我们定义u_i＝[v_i ω_i]^T，f＝[f₁ f₂]^T，u_j＝[v_jω_j]^T，β_ij＝θ_i-θ_j，则 ${\stackrel{\&CenterDot;}{\mathrm{\&beta;}}}_{\mathrm{ij}}={\mathrm{\&omega;}}_i-{\mathrm{\&omega;}}_j,$ 其中 $D_1=\left[\begin{array}{cc}{d}_{11}& d_{12}\\ d_{21}& d_{22}\end{array}\right].$

(3)对协同运动模型(1)采用输入输出线性化方法，通过设置辅助控制输入和变量代换方法使协同运动模型(1)转化为线性***，如下式(2)：

$\stackrel{\&CenterDot;}{x}=\mathrm{Ax}+\mathrm{Bu}+\mathrm{Df}\&CenterDot;\&CenterDot;\&CenterDot;\left(2\right)$

其中x＝[x₁ x₂]^T，u＝[v w]^T，这里 $A=\left[\begin{array}{cc}{-k}_1& 0\\ 0& {-k}_2\end{array}\right],$ $B=\left[\begin{array}{cc}1& 0\\ 0& 1\end{array}\right],$ $D=\left[\begin{array}{cc}{-d}_{11}& {-d}_{12}\\ {-d}_{21}& {-d}_{22}\end{array}\right],$ f为水下扰动向量且其动态特性由式描述，G∈R^2×2为已知常量矩阵。

(4)由于水下机器人储能有限，综合考虑***跟踪误差以及驱动能量消耗问题，选取协调控制***的最优化性能指标为通过求解Riccati矩阵方程，得到前馈-反馈最优控制律为如下式(3)：

$u^{*}\left(t\right)=-R^{-1}{B}^T[\mathrm{Px}\left(t\right)+\stackrel{\_}{P}f]\&CenterDot;\&CenterDot;\&CenterDot;\left(3\right)$

其中P为Riccati矩阵方程如下式(4)：

A^TP+PA-PSP+Q＝0…………………………………………………(4)

的唯一正定解，为矩阵方程的唯一解，其中S＝BR^-1B^T。

实现协同控制的控制策略为A_i通过加速度传感器和陀螺仪检测自身的速度v_i、角速度ω_i、驶向角θ_j以及相对驶向角信息，并通过水声通信方式向A_j发送数据A_j结合自身声纳传感器测量的l_ij以及计算所得的γ_ij进行控制，这样就可以实现A_j以期望距离和角度跟踪A_j，从而实现二者的主从式协同控制。

本发明的具体实例如下：

惯性坐标系下主航行器的通过自身的各种传感器测量其初始位置和姿态状态。初始状态为p_i(0)＝[x_i(0) y_i(0) θ_i(0)]^T＝[2m 1m π/6rad]^T，通过加速度和陀螺仪测量其速度和角速度分别为v＝3.5m/s，ω＝0rad/s并且保持不变。从航行器通过自身的各种传感器测量其初始位置和姿态状态，其初始状态为p_j(0)＝[x_j(0) y_j(0) θ_j(0)]^T＝[1.8m -1.2m 0rad]^T，期望间距期望跟踪角度则水下扰动相关矩阵为如下式(5)：

$D=\left[\begin{array}{cc}-0.5& 0\\ 1& 1\end{array}\right],G=\left[\begin{array}{cc}-1& 10\\ -10& -2\end{array}\right]\&CenterDot;\&CenterDot;\&CenterDot;\left(5\right)$

性能指标相关矩阵为如下式(6)：

$Q=\left[\begin{array}{cc}2& 0\\ 0& 1\end{array}\right],R=\left[\begin{array}{cc}4& 0\\ 0& 2\end{array}\right]\&CenterDot;\&CenterDot;\&CenterDot;\left(6\right)$

仿真步长τ＝0.05s，仿真时间T＝200s；k₁＝1，k₂＝1，d＝0.2m。

通过与传统最优控制进行了仿真比较，表明基于前馈和反馈的最优控制能抑制外扰动对***的影响，仿真结果如错误！未找到引用源。-错误！未找到引用源。所示。

上述说明并非是对本发明的限制，本发明并不仅限于上述举例，本技术领域的普通技术人员在本发明的实质范围内所做出的变化、改型、添加或替换，也应属于本发明的保护范围。

Claims (3)

1.一种自主式水下航行器主从式协同控制方法，其特征在于，其包括以下步骤：考虑外部洋流的影响，将外部洋流视为外部扰动并对其进行数学建模；建立***惯性坐标系，将两台水下航行器之间的相对速度沿着它们形心之间连线以及垂直连线方向进行矢量分解，建立***的协同控制运动模型；通过反馈线性化方法将原***运动模型转化为带外部扰动的线性***模型；为了抑制外部扰动的影响设计了转化后***运动模型的前馈-反馈最优控制器。

2.如权利要求1所述的自主式水下航行器主从式协同控制方法，其特征在于，所述外部洋流扰动的数学建模为具有持续扰动的外***模型，由式描述，其中G∈R^2×2为已知常量矩阵，由洋流扰动特性所决定。所以水下洋流扰动可以描述为满足下列条件的扰动：矩阵G的所有特征值满足Re[λ_i(G)]<0，i＝1，；外扰动的初始条件是未知的。

3.如权利要求1所述的自主式水下航行器主从式协同控制方法，其特征在于，所述建立***惯性坐标系下的协同控制运动模型的具体步骤如下：将两台水下航行器之间的相对速度沿着它们形心之间连线以及垂直连线方向进行矢量分解，建立***的协同控制运动模型，具体为：记A_i为主机器人，A_j为从机器人，A_j以一定的距离和角度跟踪A_i，通过设计合适的协同控制方法，使之形成两个机器人之间的主从式队形；A_i和A_j在惯性坐标系下的坐标分别表示为p_i＝[x_i y_i θ_i]^T和p_j＝[x_j y_j θ_j]^T；v_i和v_j分别为A_i和A_j的实际速度，l_ij为A_j到A_i的距离，为A_i速度方向和二者连线的夹角，d为A_j前端参考点到A_j重心的距离，f₁和f₂为平面内水下扰动分量；将相对速度分别沿着连线方向以及垂直连线方向进行分解，得：

为后续处理方便，将上式写为矢量形式，定义u_i＝[v_i ω_i]^T，f＝[f₁ f₂]^T，u_j＝[v_j ω_j]^T，β_ij＝θ_i-θ_j，则 其中 $G_1=\left[\begin{array}{cccc}c& {\mathrm{\&gamma;}}_{i}o& s_{j}d& \mathrm{\&gamma;}\\ -s& {\mathrm{\&gamma;}}_{i}i/l& n_j& d_i\mathrm{\&gamma;}/j\end{array}\right],$ $D_1=\left[\begin{array}{cc}{d}_{11}& d_{12}\\ d_{21}& d_{22}\end{array}\right].$