CN112506053A - 载人潜水器的运动控制方法 - Google Patents

Abstract

本发明公开了一种载人潜水器的运动控制方法，包括以下步骤：根据载人潜水器的位置信息、姿态信息以及受到的复合干扰信息建立载人潜水器的动力学模型；对载人潜水器的动力学模型进行简化；并构建扩张状态观测器，利用扩张状态观测器获取复合干扰信息的估计值；采用全阶非奇异终端滑模设计滑模控制器；根据滑模控制器和二次规划方法对载人潜水器的推力进行控制分配。本发明能够有效提高潜水器运行控制的鲁棒性并避免抖振现象，同时能够很好地进行潜水器的推力控制分配并降低能耗。

Description

载人潜水器的运动控制方法

技术领域

本发明涉及载人潜水器的运动控制技术领域，具体涉及一种载人潜水器的 运动控制方法。

背景技术

现有的载人潜水器通常采用PID控制方法进行运动控制，并通过直接求取 控制矩阵的伪逆对控制作用进行推力分配，但是上述控制方法的鲁棒性不强， 而具有较强鲁棒性的滑模控制算法又存在抖振现象的问题，并且上述推力分配 方法能耗较大，易造成资源的浪费，无法满足使用需求。

发明内容

本发明要解决的技术问题是提供一种载人潜水器的运动控制方法，能够有 效提高潜水器运行控制的鲁棒性并避免抖振现象，同时能够很好地进行潜水器 的推力控制分配并降低能耗。

为了解决上述技术问题，本发明提供的技术方案如下：

一种载人潜水器的运动控制方法，其特征在于，包括以下步骤：

1)根据载人潜水器的位置信息、姿态信息以及受到的复合干扰信息建立载 人潜水器的动力学模型；

2)对载人潜水器的动力学模型进行简化；并构建扩张状态观测器，利用扩 张状态观测器获取复合干扰信息的估计值；

3)采用全阶非奇异终端滑模设计滑模控制器；

4)根据所述滑模控制器和二次规划方法对载人潜水器的推力进行控制分配。

在其中一个实施方式中，步骤1)中所述载人潜水器的动力学模型为：

其中，η为载人潜水器的位置姿态矩阵，J(η)为坐标转换矩阵,v为速度矩 阵；M为惯性矩阵，C(v)为科氏力及向心力矩阵，D(v)为流体阻力矩阵， G(η)为由重力和浮力组成的回复力矩阵，τ为推进器总推力矩阵，d_l为载人 潜水器受到的复合干扰信息；

其中，τ＝Bu，B为行满秩矩阵，其列数大于行数，u为推进器实际推力。

在其中一个实施方式中，步骤2)中对载人潜水器的动力学模型进行简化 的方法包括以下步骤：

A1)对动力学模型进行变换得到以下公式：

其中，v_a＝J(η)v，

M₀表示M的标称值，C₀(v)为C(v)的标称值，D₀(v) 为D(v)的标称值,G₀(v)为G(v)的标称值；

A2)令x＝v_a，f(x)＝-M₀ ^-1C₀(v)v-M₀ ^-1D₀(v)v-M₀ ^-1G₀(v)，g₁(x)＝M₀ ^-1B， g₂(x)＝M₀ ^-1，则将公式

转换为以下仿射非线性系统 方程：

在其中一个实施方式中，所述步骤2)扩张状态观测器为：

其中，

表示x₁的估计值，

表示x₂的估计值，

表示载人潜水器受到的 复合干扰信息d_l的估计值。

在其中一个实施方式中，所述步骤3)中采用全阶非奇异终端滑模设计滑 模控制器的方法包括以下步骤：

B1)定义e_η＝η-η_d和

并设计全阶非奇异终端滑模面s：

其中，e_η表示载人潜水器位置的跟踪误差，η_d载人潜水器位置的期望位置， e_v表示载人潜水器速度的跟踪误差，Λ_v表示速度的正定对角矩阵，Λ_η表示位置 的正定对角矩阵，a_v表示速度指数，a_η表示位置指数；

B2)根据全阶非奇异终端滑模面s和

得出全阶非奇异终端滑模控制律：

τ＝τ_eq+τ_n；

τ_n+Λ_lfτ_n＝κ；

κ＝-Λ_s sgn(s)；

其中，τ_eq表示等效控制量，τ_n表示切换控制量，Λ_lf表示低通滤波参数 的正定对角矩阵，Λ_s表示滑模面的正定对角矩阵。

在其中一个实施方式中，所述步骤4)中对载人潜水器的推力进行控制分 配的方法包括以下步骤：

C1)建立以下二次规划问题模型：

|u_i|≤u_bound,i＝1,2,.....8

其中，τ为推进器总推力矩阵，B为行满秩矩阵，其列数大于行数，u为 推进器实际推力，H和Q均为正定对角矩阵，σ为松弛变量，u_i为u中第i个元 素，表示第i个推进器的推力，u_bound表示边界值；

C2)使用内点法对步骤C1)中的二次规划问题模型进行求解，从而完成对 载人潜水器的推进器推力的配置。

本发明具有以下有益效果：本发明的载人潜水器的运动控制方法，实现了 对载人潜水器的双层控制，内层采用滑模控制器，不仅能够使载人潜水器跟踪 预定的轨迹运动，而且能够处理由外界干扰和模型参数摄动等引起的复合干扰 的影响，提高了潜水器运行控制系统的鲁棒性；外层为基于二次规划算法的控 制分配器，不仅能够实现将期望的力和力矩映射到推进器实际推力的功能，还 能兼顾控制系统能耗最小等约束条件，能够很好地降低能耗。

附图说明

图1是本发明的载人潜水器的运动控制方法的控制示意图；

图2是载人潜水器的推进器分布侧视图；

图3是载人潜水器的推进器分布俯视图；

图4是载人潜水器的各推进器的推力分配图；

具体实施方式

下面结合附图和具体实施例对本发明作进一步说明，以使本领域的技术人 员可以更好地理解本发明并能予以实施，但所举实施例不作为对本发明的限定。

参阅图1，本实施例公开了一种载人潜水器的运动控制方法，包括以下步 骤：

1)根据载人潜水器的位置信息、姿态信息以及受到的复合干扰信息建立载 人潜水器的动力学模型；

2)对载人潜水器的动力学模型进行简化；并构建扩张状态观测器，利用扩 张状态观测器获取复合干扰信息的估计值；

3)采用全阶非奇异终端滑模设计滑模控制器；

4)根据滑模控制器和二次规划方法对载人潜水器的推力进行控制分配。

上述方法结合潜水器受到的复合干扰信息来建立动力学模型并构建扩张状 态观测器，以及采用全阶非奇异终端滑模设计滑模控制器，能够有效提升系统 鲁棒性，同时降低滑模控制存在的抖振问题；针对潜水器的过驱动特性，利用 二次规划方法作为最优算法来进行潜水器的推力控制分配，优化了推进器推力 的分配，大幅度降低了能耗，起到很好的节能降耗的目的。

在其中一个实施方式中，步骤1)中载人潜水器的动力学模型为：

其中，η为载人潜水器的位置姿态矩阵，包含三个方向的位置量和三个旋 转方向的姿态角度量,J(η)为坐标转换矩阵,v为速度矩阵，包括三个方向的线 速度和三个旋转方向的角速度；M为惯性矩阵，C(v)为科氏力及向心力矩阵， D(v)为流体阻力矩阵，G(η)为由重力和浮力组成的回复力矩阵，τ为推进 器总推力矩阵，d_l为载人潜水器受到的复合干扰信息；

其中，τ＝Bu，B为行满秩矩阵，其列数大于行数，u为推进器实际推力。

上述动力学模型是依据牛顿-欧拉方程建立的六自由度动力学模型，对潜水 器受到的外界干扰和模型不确定性等形成的复合干扰信息也考虑在内，利于增 加后续滑模控制的鲁棒性。

在其中一个实施方式中，步骤2)中对载人潜水器的动力学模型进行简化 的方法包括以下步骤：

A1)对动力学模型进行变换得到以下公式：

为简化表达形式，将公式(4)中

简写为

和

则有：

其中，v_a＝J(η)v，

M₀表示M的标称值，C₀(v)为C(v)的标称值，D₀(v) 为D(v)的标称值,G₀(v)为G(v)的标称值；

为v_a的一阶导数；

为η的一阶导数；

A2)令x＝v_a，f(x)＝-M₀ ^-1C₀(v)v-M₀ ^-1D₀(v)v-M₀ ^-1G₀(v)，g₁(x)＝M₀ ^-1B，g₂(x)＝M₀ ^-1，则将公式

转换为以下仿射非线性系统 方程：

在其中一个实施方式中，步骤2)扩张状态观测器为：

其中，

表示x₁的估计值，

表示x₂的估计值，

表示载人潜水器受到的 复合干扰信息d_l的估计值。

为x的一阶导数。

在其中一个实施方式中，步骤3)中采用全阶非奇异终端滑模设计滑模控 制器的方法包括以下步骤：

B1)定义e_η＝η-η_d和

并设计全阶非奇异终端滑模面s：

其中，e_η表示载人潜水器位置的跟踪误差，η_d载人潜水器位置的期望位置， e_v表示载人潜水器速度的跟踪误差，Λ_v表示速度的正定对角矩阵，Λ_η表示位置 的正定对角矩阵，a_v表示速度指数，a_η表示位置指数；

B2)根据全阶非奇异终端滑模面s和复合干扰估计值

得出全阶非奇异 终端滑模控制律：

τ＝τ_eq+τ_n；

τ_n+Λ_lfτ_n＝κ；

κ＝-Λ_s sgn(s)；

其中，τ_eq表示等效控制量，τ_n表示切换控制量，Λ_lf为正定对角矩阵， 表示低通滤波参数，Λ_s表示滑模面的正定对角矩阵。

上述控制律，可以使潜水器系统状态在较短时间内达到滑模面，并且在较 短间内使得系统的控制误差收敛到零。

在其中一个实施方式中，步骤4)中对载人潜水器的推力进行控制分配的 方法包括以下步骤：

C1)建立以下二次规划问题模型：

|u_i|≤u_bound,i＝1,2,.....8

其中，τ为推进器总推力矩阵，B为行满秩矩阵，其列数大于行数，u为 推进器实际推力，H和Q均为正定对角矩阵，σ为松弛变量，u_i为u中第i个元 素，表示第i个推进器的推力，u_bound表示边界值；

C2)使用内点法对步骤C1)中的二次规划问题模型进行求解，从而完成对 载人潜水器的推进器推力的配置。

载人潜水器的实际的控制作用是由分布在潜水器上的各推进器产生的合力 与合力矩产生的，各推进器的分布情况参阅图2-图3。通过上述控制分配方法， 在所规划的不同期望路径下，我们只需要按照给定的加权矩阵对潜水器系统进 行基于二次规划的最优分配，就可以完成对各推进器的控制力与力矩的优化配 置。

下面结合具体的仿真实验来说明本实施例的控制分配方法的有效性：

各参数选取如下：

干扰观测器参数β₁＝diag{60,...,60}，β₂＝diag{900,...,900}，即干扰观测器的带宽选择为ω₀＝30rad，滑模控制器参数Λ_v＝diag{3,...,3}，Λ_η＝diag{2,...,2}；

选取两个指数参数为：α_v＝11/17和α_η＝11/23；

τ_n中的控制增益为κ＝diag{0.001,...,0.001}，低通滤波参数为 Λ_lf＝diag{50,...,50}；

控制分配中的权重矩阵分别为：H＝diag{101105101051}及 Q＝diag{1000,...,1000}。

载人潜水器的复合干扰由20％的参数不确定性及外部海洋干扰组成，其中 外界干扰为:

载人潜水器共有8个推进器，利用上述参数并依据本实施例的上述控制分 配方法最终获取各推进器的推力，各推进器的实际推力分配参阅图4；

采用上述实施例的控制分配方法与传统的广义逆控制分配方法所消耗的能 量比较如表1所示：

表1两种控制分配方法所消耗能量比较表

由表1可知，以本实施例的上述二次规划控制分配方法得到的控制过程相 较于传统的广义逆控制的分配方法，在不影响控制效果的前提下所消耗能量显 著下降。

另外，通过仿真实验表明，上述实施例所设计的扩张状态观测器能够有效 地对潜水器系统中存在的复合干扰进行估计，而且整个运动控制方式能够使潜 水器以较高的精度对期望位置轨迹以及姿态角轨迹进行跟踪，并有效抑制控制 器抖振。潜水器系统起步时，由于潜水器的起始状态与期望值之间偏差较大， 控制器所给出的控制量将超出执行器阈值，而本实施例的运动控制方式在控制 分配中给出的控制量约束对控制量的幅值进行了有效限制。

本实施例的载人潜水器的运动控制方法，实现了对载人潜水器的双层控制， 内层采用鲁棒控制技术的滑模控制器，不仅能够使载人潜水器跟踪预定的轨迹 运动，而且能够处理由外界干扰和模型参数摄动等引起的复合干扰的影响，提 高了潜水器运行控制系统的鲁棒性；外层为基于二次规划算法的控制分配器， 不仅能够实现将期望的力和力矩映射到推进器实际推力的功能，还能兼顾控制 系统能耗最小等约束条件，能够很好地降低能耗。

以上所述实施例仅是为充分说明本发明而所举的较佳的实施例，本发明的 保护范围不限于此。本技术领域的技术人员在本发明基础上所作的等同替代或 变换，均在本发明的保护范围之内。本发明的保护范围以权利要求书为准。

Claims (6)

1.一种载人潜水器的运动控制方法，其特征在于，包括以下步骤：

1)根据载人潜水器的位置信息、姿态信息以及受到的复合干扰信息建立载人潜水器的动力学模型；

2)对载人潜水器的动力学模型进行简化；并构建扩张状态观测器，利用扩张状态观测器获取复合干扰信息的估计值；

3)采用全阶非奇异终端滑模设计滑模控制器；

4)根据所述滑模控制器和二次规划方法对载人潜水器的推力进行控制分配。

2.如权利要求1所述的载人潜水器的运动控制方法，其特征在于，步骤1)中所述载人潜水器的动力学模型为：

其中，η为载人潜水器的位置姿态矩阵，J(η)为坐标转换矩阵,v为速度矩阵；M为惯性矩阵，C(v)为科氏力及向心力矩阵，D(v)为流体阻力矩阵，G(η)为由重力和浮力组成的回复力矩阵，τ为推进器总推力矩阵，d_l为载人潜水器受到的复合干扰信息；

其中，τ＝Bu，B为行满秩矩阵，其列数大于行数，u为推进器实际推力。

3.如权利要求2所述的载人潜水器的运动控制方法，其特征在于，步骤2)中对载人潜水器的动力学模型进行简化的方法包括以下步骤：

A1)对动力学模型进行变换得到以下公式：

其中，v_a＝J(η)v，

M₀表示M的标称值，C₀(v)为C(v)的标称值，D₀(v)为D(v)的标称值,G₀(v)为G(v)的标称值；

A2)令x＝v_a，f(x)＝-M₀ ^-1C₀(v)v-M₀ ^-1D₀(v)v-M₀ ^-1G₀(v)，g₁(x)＝M₀ ^-1B，g₂(x)＝M₀ ^-1，则将公式

转换为以下仿射非线性系统方程：

4.如权利要求3所述的载人潜水器的运动控制方法，其特征在于，所述步骤2)扩张状态观测器为：

其中，

表示x₁的估计值，

表示x₂的估计值，

表示载人潜水器受到的复合干扰信息d_l的估计值。

5.如权利要求4所述的载人潜水器的运动控制方法，其特征在于，所述步骤3)中采用全阶非奇异终端滑模设计滑模控制器的方法包括以下步骤：

B1)定义e_η＝η-η_d和

并设计全阶非奇异终端滑模面s：

其中，e_η表示载人潜水器位置的跟踪误差，η_d载人潜水器位置的期望位置，e_v表示载人潜水器速度的跟踪误差，Λ_v表示速度的正定对角矩阵，Λ_η表示位置的正定对角矩阵，a_v表示速度指数，a_η表示位置指数；

B2)根据全阶非奇异终端滑模面s和

得出全阶非奇异终端滑模控制律：

τ＝τ_eq+τ_n；

τ_n+Λ_lfτ_n＝κ；

κ＝-Λ_ssgn(s)；

其中，τ_eq表示等效控制量，τ_n表示切换控制量，Λ_lf表示低通滤波参数的正定对角矩阵，Λ_s表示滑模面的正定对角矩阵。

6.如权利要求5所述的载人潜水器的运动控制方法，其特征在于，所述步骤4)中对载人潜水器的推力进行控制分配的方法包括以下步骤：

C1)建立以下二次规划问题模型：

|u_i|≤u_bound,i＝1,2,.....8

其中，τ为推进器总推力矩阵，B为行满秩矩阵，其列数大于行数，u为推进器实际推力，H和Q均为正定对角矩阵，σ为松弛变量，u_i为u中第i个元素，表示第i个推进器的推力，u_bound表示边界值；

C2)使用内点法对步骤C1)中的二次规划问题模型进行求解，从而完成对载人潜水器的推进器推力的配置。

Images