CN114924485A - 一种过驱动水下机器人动力定位自适应容错控制架构及方法 - Google Patents

Abstract

本发明提供一种过驱动水下机器人动力定位自适应容错控制架构及方法，属于水下机器人控制领域。所述自适应容错控制架构由非线性扰动观测器模块、非线性幂次滑模控制模块、自适应推力分配模块及推进器效率在线估计模块组成。对应的自适应容错控制方法包括：S1：结合非线性扰动观测器的非线性幂次滑模控制器输出各自由度控制量，再通过推力分配以获取各推进器指令；S2：通过动力学模型推算状态值与实际状态值的偏差构建二次规划问题并获得推进器估计效率因子；S3：根据估计效率因子重构分配矩阵，以实现故障工况下的自适应容错控制。所设计架构及方法可以实现环境干扰、推进器故障的同时估计并完成过驱动水下机器人自适应容错动力定位任务。

Description

一种过驱动水下机器人动力定位自适应容错控制架构及方法

技术领域

本发明涉及水下机器人控制技术领域，具体来说是一种在推进器故障工况下，过驱动水下机器人采用自适应推力分配实现动力定位任务下的容错控制架构及方法，一体化的容错控制架构及方法有效地简化机器人动力定位任务下的容错控制流程。

背景技术

21世纪人类逐渐将开发目光转向浩瀚的海洋，世界范围内越来越多的国家将开发海洋作为基本国策，同时大力发展海洋技术，确保能够合理开发海洋资源，维护海洋权益乃至国家安全。水下机器人作为一种智能型水下设备，可以实现在大深度、高危险、被污染、可见度较低等极端环境下代替潜水员开展水下作业。

作业级水下机器人一般为过驱动配置，即该机器人执行机构数量大于所需要控制自由度的数量，这种冗余配置大大提高了机器人的可靠性与灵活性，但是在实际作业过程中，外界扰动、推进器故障等因素仍不可避免。水下机器人一旦发生推进器故障，会引起推进器实际输出推力与运动控制器的期望控制量之间产生偏差，严重故障甚至会导致机器人的任务失败或者影响其安全性。因此有必要设计一套适用于水下复杂工况下的容错控制架构方法。

对于水下机器人推进器故障工况下的容错控制方法，主要分为主动容错和被动容错两种。其中，被动容错本质上是一种鲁棒控制方法，将故障引起的系统特性变化视为参数摄动，并对其进行实时估计和补偿，但是被动容错往往存在控制输出不连续的问题而影响到系统的稳定性。主动容错则是先进行故障诊断(本发明中即为推进器的效率估计)得到一定的故障模型，进而适应性地调整控制参数，从而实现不改变原有控制器地情况下实现容错控制。在主动容错方法中，容错控制效果往往受到故障诊断效果的影响，故障诊断效果则会受到外界环境干扰的影响，传统的容错方法中往往无法兼顾推进器故障和外界环境干扰，从而导致推进器故障诊断效果的下降，因此有必要针对环境干扰和推进器故障同时存在的情况下展开水下机器人容错控制系统的深入研究。

发明内容

本发明所要解决的问题是：针对同时存在外界环境扰动和执行器故障的复杂水下工况，采用自适应推力分配方法实现水下机器人的容错控制。

为实现上述目的，本发明首先基于滑模方法进行水下机器人动力定位运动控制器的设计，同时引入幂次趋近律以及饱和边界层的方法削弱滑模控制“抖振”现象。其次，采用非线性扰动观测器对外界环境干扰进行逼近并作为非线性幂次滑模控制器的补偿输入。再次，为了实现非线性幂次滑模控制器相应自由度控制量至各推进器控制指令之间的分配，以分配偏差和能量消耗作为优化目标，通过二次规划的方法构建推力分配器输出各推进器的期望控制指令。然后结合外界扰动估计值、推进器期望控制指令进行水下机器人的状态量推算，并将其与推进器故障工况下的实际状态对比以获取状态偏差序列，进而依据状态偏差序列构建二次规划问题，对其求解获得各推进器效率因子估计值以重构推力分配矩阵，最后实现环境干扰、推进器故障的同时估计并完成过驱动水下机器人自适应容错动力定位任务。

附图说明

图1为过驱动水下机器人自适应推力分配容错控制架构整体框图；

图2为故障工况下自适应推力分配的动力定位轨迹曲线；

图3为故障工况下不采取自适应推力分配的动力定位轨迹曲线；

图4为任务过程中水下机器人的位姿变化曲线；

图5为任务过程中水下机器人的速度变化曲线；

图6为观测复合扰动与实际环境扰动对比曲线；

图7为故障工况下各推进器效率观测曲线；

图8为各自由度期望控制力(力矩)与实际控制力(力矩)对比曲线；

图9为各推进器期望推力与实际推力大小对比；

图10为“BlueROV2 Heavy”过驱动水下机器人及其推进器布置示意图；

图11为“FUSION”过驱动水下机器人及其推进器布置示意图。

具体实施方式

为了使本发明的目的、技术方案及优点更加清楚明白，以下结合附图及实施例，对本发明进行进一步详细说明。应当理解，此处所描述的具体实施例仅仅用以解释本发明，并不用于限定本发明。

针对本文所提出的自适应容错控制架构，主要通过非线性扰动观测模块进行外界环境扰动的在线估计，非线性幂次滑模控制模块求解相应自由度控制量，自适应推力分配模块求解各推进器推力指令，推进器效率估计模块进行推进器效率修正。

下面结合过驱动水下机器人水平面动力定位任务以介绍具体实施过程：

在设计水下机器人自适应容错控制方法之前，首先考虑水下机器人在水平面三自由度数学模型，可以做如下表示

其中，

为空间位置向量，x为机器人纵荡位置，y为横荡位置，

为艏摇角，J(η)为运动学转换矩阵，v＝[u；v；r]为线速度与角速度向量，u为机器人纵荡速度，v为横荡速度，r为艏摇角速度，M∈R^3×3为惯性矩阵，且满足M＝M^T，C(v)∈R^3×3为科氏力及向心力矩阵，D(v)∈R^3×3为水阻尼矩阵，d＝τ_rao+(τ-τ_d)∈R³为水下机器人所受到的复合扰动项，其中τ_rao为外界环境扰动项，τ为推进器实时状态下的真实作用力，τ_d为非线性幂次滑模控制器输出期望控制作用力，所以此处将τ-τ_d作为内部扰动项，当有效估计推进器状态，理论上τ_d会趋向于τ，即此时d会趋向于τ_rao。

在水下机器人的实际工作中，需要将期望控制作用力τ_d转化为各推进器的推力指令，可以根据推进器的布置建立水平面上的总推力(力矩)和各推进器推力关系如下：

τ_d＝TKf

式中，T为水下机器人的推力分配矩阵，f为推进器指令向量；K为推进器的效率矩阵，K＝diag[K₁,K₂,K₃,K₄]，其中K_i,i＝1,2,3,4为描述各推进器真实状态的效率因子，K_i＝1时代表推进器正常工作，实际推力与标称推力相同，0＜K_i＜1代表推进器出现部分故障，K_i＝0代表推进器出现完全故障，即不能输出推力。

过驱动水下机器人动力定位自适应容错控制方法的具体实现如下：

步骤一：非线性幂次滑模控制器设计

定义水下机器人跟踪误差e＝η-η_d∈R³，

之后通过误差构建如下线性滑模面：

其中，s＝[s₁,s₂,s₃]为滑模面向量，其中s₁,s₂,s₃为相应自由度的滑模面具体表达形式，c∈R^3×3为待设计的正定对角矩阵，η和η_d分别为水下机器人实时位姿和期望位姿。

选取李雅普诺夫函数为：

对其进行求导可得：

针对复合扰动基于非线性扰动观测器进行估计补偿，构造如下非线性扰动观测器：

式中，

为复合扰动估计项，定义估计误差

ξ∈R³为观测器状态量，K₀∈R^3×3为待选取的正定对角矩阵。

对复合扰动进行估计补偿后，采用线性化反馈技术对无关项进行抵消，可设计非线性幂次滑模控制器如下：

其中，τ_d为三自由期望控制力/力矩向量，

为外界环境干扰估计值，v为线速度与角速度向量，C为科氏力与向心力矩阵；D为水阻尼矩阵；M为惯性矩阵；

为水下机器人期望位姿的二阶导数；c、k、k_ε均为待设计的正定对角矩阵；

为跟踪误差的一阶导数；S_α＝diag(|s₁|^α,|s₂|^α,|s₃|^α)为幂次滑模面，α为满足0＜α＜1的待选取幂，sat(s)＝[sat(s₁),sat(s₂),sat(s₃)]为所引入的用以削弱抖振现象的切换项，其定义如下：

其中，sign为符号函数，tanh为双曲正切函数，δ为待设计的边界层厚度。

步骤二：自适应推力分配设计

在步骤一中已经获取水平面三自由度下的控制量τ_d，下面则需要通过推力分配器将τ_d转化为水平面各推进器的推力指令f。

在推力分配器中，构建推力分配优化目标函数如下：

s.t.τ_d-TKf＝σ

f_min≤f≤f_max

其中，上式目标函数中的f^THf为实现消耗功率最小，σ^TWσ为实现实际推力输出与期望控制指令间的误差最小。σ为推力分配偏差，T为水下机器人的推力分配矩阵，f_min、f_max分别为推进器推力可达的上下界限。H为功耗项加权系数矩阵，W为误差项加权系数矩阵，由于在实际作业任务中需要实现指令误差尽量小，因此W取值远大于H。K为推进器当前效率，在下一步中对其进行实时估计以实现针对推进器故障的自适应分配。

步骤三：推进器效率估计方法设计

在步骤二中，推力系数矩阵K根据推进器实际状态确定，但是在水下实际作业过程中，各推进器输出推力及效率也无法直接测量获得，因此在本文中采取当前状态下的干扰估计值序列与系统状态偏差序列构建二次规划问题进行各推进器效率因子的在线估计以获取推进器估计效率矩阵

通过水下机器人的运动学与动力学公式，可以建立如下关系：

式中，

分别为t时刻的估计位姿、速度状态量，其它项具体表达式如下：

式中t为当前时刻值，Δt为时间步长大小，

为运动学转换矩阵，I为对角单位矩阵，

为推进器在线估计效率矩阵，

为前文中非线性扰动观测器的时变复合扰动估计值。

为实现推进器的效率矩阵估计，定义效率矩阵增量如下：

式中

其中

i＝1,2,3,4为所代表的推进器效率该时刻增量。

通过实际状态与估计状态可以求解状态偏差S_x如下：

式中，X(t)为t时刻测量真实状态值，

为t时刻估计状态值。

为获取推进器真实效率矩阵，此时可以依据状态偏差序列以及效率矩阵增量构建如下二次规划优化求解问题：

……

其中，上式目标函数中的第一项为实现状态偏差序列最小，第二项为实现推进器效率增量最小，M为状态偏差项加权系数矩阵，N推进器效率增量加权系数矩阵；S_x,i,i＝1,2...m构成状态偏差序列，m为选取的偏差序列的大小；限制函数中为状态偏差序列计算方法。需要注意的是在计算偏差序列过程中各式效率矩阵均采用

求解此问题即可获取当前时刻效率矩阵增量

进一步可以获得推进器当前的推进器效率估计值。

采用推进器效率估计值对上一步中的推力分配模型进行重构，最终即可获取推进器模型重构后各推进器的直接控制指令。

实施案例：

为验证本发明中所述控制方法的效果，采用某过驱动水下机器人水平面模型为仿真对象开展仿真试验，仿真过程中参数设置如下：

设置机器人的初始位置：

跟踪过程分时间先后跟踪两个目标点，描述为如下公式：

引入外界环境干扰：

各推进器推力限制范围-40N＜f_i＜40N。

推进器在跟踪过程中发生两次故障，初始时刻推进器正常工作，50s之后1号推进器发生完全故障，170s之后在之前故障的基础上3号推进器损失一半效率，整个过程可以描述为：

根据发明实施方式可以归纳容错控制系统仿真实现流程如表1所示，

表1自适应容错控制方法仿真流程

仿真结果如下，图2为水下机器人在任务过程中采取容错控制策略的水平面运动轨迹，先后抵达目标点1和目标点2；图3为未采取容错控制策略的水平面运动轨迹，其作为对照试验，在故障发生后呈现发散趋势；图4与图5分别为水下机器人的位姿与速度变化曲线；图6为非线性扰动观测器对复合扰动估计值与实际外界环境扰动的对比曲线；图7为在线估计的推进器效率因子变化曲线；图8为系统的期望控制力(力矩)与实际控制力(力矩)对比曲线；图9为任务过程中各推进器期望推力与实际推力大小对比曲线。

对于本文所提出的自适应容错控制方法，可以有效地适用于多型过驱动水下机器人平台之中。如图10所示是一种开源低成本过驱动水下机器人“BlueROV2 Heavy”及其推进器布置示意图，图11所示是一种商用过驱动水下机器人“FUSION”及其推进器布置示意图，其它类似推进器布置的过驱动水下机器人均可以作为本文自适应容错控制方法的应用平台。

Claims (8)

1.一种过驱动水下机器人动力定位自适应容错控制方法，其特征在于：所述自适应容错控制方法基于自适应容错控制架构进行控制；

该自适应容错控制架构由非线性扰动观测模块、非线性幂次滑模控制模块、自适应推力分配模块及推进器效率在线估计模块组成，其中非线性扰动观测模块用于外界环境干扰估计，非线性幂次滑模控制模块输出控制量，并将其作为自适应推力分配模块的输入，进而得到各推进器推力指令并作用于机器人，推进器效率在线估计模块实时监控推进器状态并对自适应推力分配模块中的分配矩阵进行重构，以实现推进器故障损失推力的补偿，从而实现外界环境干扰、推进器故障的同时估计并完成过驱动水下机器人自适应容错动力定位任务；

所述自适应容错控制方法具体如下：

S1：首先结合非线性扰动观测模块构建非线性幂次滑模控制模块，实现外界环境干扰的估计并输出水平面纵荡、横荡、艏摇三个自由度的期望控制力/力矩；为使三自由度控制量转化为各推进器的控制指令，随后基于控制偏差最小和能耗最小为原则构建推力分配优化目标函数，实现在保证控制精度的同时消耗较低的能量；

S2：建立推进器故障模型，将S1中所得到的外界环境干扰估计值以及各推进器推力指令代入机器人动力学模型获取状态估计值，随后将其与机器人实际状态值对比即获取状态偏差序列以构建二次规划问题，求解该问题后可实现推进器在线效率估计，得出推进器效率估计因子；

S3：在不改变非线性幂次滑模控制模块的情况下，将S2中推进器效率估计因子引入S1，进行分配矩阵重构实现自适应推力分配以完成容错控制。

2.如权利要求1所述的一种过驱动水下机器人动力定位自适应容错控制方法，其特征在于：

非线性扰动观测模块用以实现外界环境干扰的在线估计，并对非线性幂次滑模控制模块的输出控制量进行补偿；

非线性幂次滑模控制模块通过幂次趋近律结合以及饱和边界层的方法设计滑模控制器，通过当前机器人的跟踪目标与当前状态进行水平面纵荡、横荡、艏摇三个自由度控制量的初步解算；结合非线性扰动干扰模块的控制补偿量得到最终期望控制量并将其输入至自适应推力分配模块；

自适应推力分配模块的输入量为运动控制模块传递而来的各自由度期望控制量，输出为水平面各推进器的指令，分配原则基于分配偏差最小以及能量消耗最少的原则；

推进器在线效率估计模块是对推进器状态进行实时监测的模块，其输入量为机器人的实际状态、推力指令、干扰实时估计值等；通过机器人运动学与动力学模型推算获得的机器人预期状态与实际状态形成状态偏差序列，对其进行分析获得各推进器的效率估计值，最后依据效率估计值进行推力分配模块中的效率矩阵的重构。

3.如权利要求1所述的一种过驱动水下机器人动力定位自适应容错控制方法，其特征在于，在所述S1中，

结合非线性扰动观测模块构建非线性幂次滑模控制模块，控制器如下：

其中，τ_d为三自由期望控制力/力矩向量，

为外界环境干扰估计值，v为线速度与角速度向量，C为科氏力与向心力矩阵；D为水阻尼矩阵；M为惯性矩阵；

为水下机器人期望位姿的二阶导数；c、k、k_ε均为待设计的正定对角矩阵；

为跟踪误差的一阶导数；s＝[s₁,s₂,s₃]为构建的幂次滑模面，其中s₁,s₂,s₃为相应自由度的滑模面具体表达形式，S_α＝diag(|s₁|^α,|s₂|^α,|s₃|^α)为幂次滑模面，α为满足0＜α＜1的待选取幂，sat(s)＝[sat(s₁),sat(s₂),sat(s₃)]为所引入的用以削弱抖振现象的切换项，其定义如下：

其中，sign为符号函数，tanh为双曲正切函数，δ为待设计的边界层厚度。

4.如权利要求1所述的一种过驱动水下机器人动力定位自适应容错控制方法，其特征在于，在所述S1中，

基于分配偏差最小以及能量消耗最少的原则构建推力分配优化目标函数如下：

s.t.τ_d-Tf＝σ

f_min≤f≤f_max

其中，上式目标函数中的第一项

为实现消耗功率最小，f为推进器指令向量；第二项σ^TWσ为实现实际推力输出与期望控制指令间的误差最小，σ为推力分配偏差，τ_d为各自由度期望控制量；H为功耗项加权系数矩阵，W为误差项加权系数矩阵；限制函数中，T为水下机器人的推力分配矩阵，f_min、f_max推进器推力可达的上下界限。

5.如权利要求1所述的一种过驱动水下机器人动力定位自适应容错控制方法，其特征在于，在所述S2中，

根据水下机器人动力学模型实现推进器效率的估计，相对于通过电流、功率、转速等信息监测推进器状态的方法能够减少相关传感器的投资成本；对于推进器故障，其能表征出来的影响只有推力的部分损失或者完全损失,此时可以通过引入故障系数如下：

K＝diag[K₁,K₂,K₃,K₄]

其中K₁,K₂,K₃,K₄分别为1、2、3、4号推进器的效率因子；

其故障工况下具体含义表示如下：

①当K_i＝1,i＝1,2,3,4时，代表正常工作，该推进器实际输出推力与标称输出推力一致；

②当0＜K_i＜1,i＝1,2,3,4时，代表发生部分故障，该推进器实际输出推力是标称输出推力的K_i倍；

③当K_i＝0,i＝1,2,3,4时，代表发生完全故障，该推进器实际输出推力为零。

6.如权利要求4所述的一种过驱动水下机器人动力定位自适应容错控制方法，其特征在于，所述S2中的推进器在线效率估计包括:

提出了估计各推进器效率因子的方法，依据状态偏差序列以及效率矩阵增量构建如下二次规划优化求解问题：

……

其中，S_x,i为该时间点对应的状态偏差；M、N为权重系数矩阵，

其中

i＝1,2,3,4为代表的推进器效率该时刻变化量；X(t)、

分别为该时刻的实际状态量与推算状态量，t为当前时刻值，Δt为时间步长大小，m为选取的偏差序列的大小；得到优化后的效率变化量

之后，即可推算得到推进器估计效率因子

7.如权利要求6所述的一种过驱动水下机器人动力定位自适应容错控制方法，其特征在于，所述S3中，

在不改变控制器的情况下引入自适应推力分配方法实现容错控制，将推进器效率估计因子

引入推力分配约束条件τ_d-Tf＝σ中即可得到：

其中，

为修正后的推进器效率矩阵，当获取的

相对真实故障相对准确时，通过自适应推力分配获得的推力指令即可以形成相对准确的实际控制力/力矩。

8.一种过驱动水下机器人动力定位自适应容错控制架构，其特征在于：所述自适应容错架构包括非线性扰动观测模块、非线性幂次滑模控制模块、自适应推力分配模块及推进器效率在线估计模块，

所述非线性扰动观测模块用以实现外界环境干扰的在线估计，并对非线性幂次滑模控制模块的输出控制量进行补偿；

所述非线性幂次滑模控制模块通过幂次趋近律结合以及饱和边界层的方法设计滑模控制器，通过当前机器人的跟踪目标与当前状态进行水平面纵荡、横荡、艏摇三个自由度控制量的初步解算；结合非线性扰动干扰模块的控制补偿量得到最终期望控制量并将其输入至自适应推力分配模块；

所述自适应推力分配模块的输入量为运动控制模块传递而来的各自由度期望控制量，输出为水平面各推进器的指令，分配原则基于分配偏差最小以及能量消耗最少的原则；

所述推进器在线效率估计模块是对推进器状态进行实时监测的模块，其输入量为机器人的实际状态、推力指令、干扰实时估计值等；通过机器人运动学与动力学模型推算获得的机器人预期状态与实际状态形成状态偏差序列，对其进行分析获得各推进器的效率估计值，最后依据效率估计值进行推力分配模块中的效率矩阵的重构。

Images