CN108196445B - 一种基于双闭环自抗扰的水下机器人姿态与位置控制方法 - Google Patents

Abstract

本发明公开了一种基于双闭环自抗扰的水下机器人姿态与位置控制方法，首先对水下机器人运用动量和矩定理并利用大地坐标系和机体坐标系间的转换关系，建立动力学模型；设计水下机器人的速度环扩张状态观测器观测未建模和外部扰动并实时补偿抑制，使得速度环的抗扰性能大大增加，提高系统的鲁棒性；针对速度环高频震颤的特点，利用扰动观测值构建基于非线性函数反馈速度环自抗扰控制器；基于非线性函数fhan，设计目标位置信号的跟踪微分器，给出位置信号的过渡过程和目标速度估计值，避免了系统响应的突变，解决了快速性和超调的矛盾；设计基于位置偏差反馈的水下机器人位置环比例‑微分控制器，适于工程应用。

Description

一种基于双闭环自抗扰的水下机器人姿态与位置控制方法

技术领域

本发明属于水下机器人控制领域，涉及水下机器人的姿态和位置控制方法，具体涉及一种基于双闭环自抗扰的水下机器人姿态与位置控制方法。

背景技术

随着人类对海洋资源的开发越来越深入，水下机器人由于其下潜深度大，工作时效久，取代了人工潜水，为人类进行深海资源的研究与开发提供了强有力的工具，因此受到国内外的广泛关注；受自身复杂动力学特性的影响以及水文扰流的影响，水下机器人弹性机体、推进系统以及结构动态之间的耦合更强，模型的非线性度也更高；此外，受水流的粘性阻力、兴波阻力和波浪力影响，水下机器人对外界条件非常敏感。

当前针对水下机器人控制大都集中在基于偏差反馈的非线性控制的设计上；自抗扰是一种预知扰动，通过非线性反馈在线主动抑制扰动的新型控制技术。自抗扰控制器主要包括三个部分，跟踪微分器，扩张状态观测器和非线性状态误差反馈。顾名思义，跟踪微分器用来安排过渡过程和产生跟踪信号的微分信号。安排过渡过程的目的是降低目标值和当前值的偏差，特别是当跟踪信号存在阶跃信号时，可以有效避免系统震荡，使当前值无超调情况下最快达到目标值。扩张状态观测器是将系统外部扰动和模型内部扰动作为一个扩张的状态进行观测，实际上，可以充分利用标称模型中的已知非线性耦合，以获得更高的精度。PID利用偏差及其各阶导数的线性组合进行反馈，然而，这种线性反馈往往不是最优的；非线性状态误差反馈将偏差及其导数进行非线性组合，非线性饱和函数fal(·)和fhan(·)的使用使控制器能够满足工程实际中“小偏差，大增益；大偏差，小增益”的调参经验。

发明内容

本发明的目的在于提供一种基于双闭环自抗扰的水下机器人姿态与位置控制方法，以克服现有非线性控制器的运算负荷大，不利于工程实现和抗扰能力差的不足，本发明通过建模将水下机器人分为内环速度环和外环角度环，内环使用自抗扰控制器，外环使用PD控制器；内环速度量变化快，并且受误差影响大，使用自抗扰控制器以获得更强的鲁棒性和更高的精度，位置量一般变化缓慢，使用PD控制就可以获得足够的鲁棒性；而且控制器设计简单，计算机运行负载低。

为达到上述目的，本发明采用如下技术方案：

一种基于双闭环自抗扰的水下机器人姿态与位置控制方法，包括以下步骤：

步骤一：对水下机器人运用动量矩定理和坐标系间的转换关系，建立动力学模型；

步骤二：设计水下机器人的速度环扩张状态观测器；

步骤三：利用矿长状态观测器给出的扰动观测值构建基于非线性函数反馈速度环控制器；

步骤四：基于非线性函数，设计目标位置信号的跟踪微分器，给出位置信号的过渡过程和目标速度估计值；

步骤五：设计水下机器人位置环控制器，对水下机器人姿态与位置进行控制。

进一步地，步骤一具体为：水下机器人在大地坐标下的位置量为

其中x、y和z是机器人的位置坐标，φ、θ和

是机器人的姿态角度；机体坐标系下，机器人的速度量为v＝[μ υ ω p q r]^T，其中，μ、υ和ω为机器人沿机体坐标轴的线速度，p、q和r是机器人绕机体坐标轴的角速度，方向满足右手螺旋定理，η和ν都是时间t的函数；

为沿大地坐标轴的速度向量，通过转换矩阵J(η)，能够获得

与ν的转换关系：

对水下机器人运用动量与动量矩定理，能够建立如下运动方程：

其中，M(η)是该模型的惯性矩阵，C(ν)是科氏力矩阵，D(ν)代表系统阻尼矩阵，g(η)是负浮力，τ表示控制量；

系统(1)和系统(2)联立得到：

其中，M_η(η)＝J^-T(η)MJ^-1(η)，为广义惯性矩阵，C_η(ν,η)＝J^-T(η)[C(ν)-MJ^-1(η)]J^-1(η)，是广义科氏力矩阵，g_η(η)＝J^-T(η)g(η)，为等价负浮力，D_η(ν,η)＝J^-T(η)D(ν)J^-1(η)，为等价阻尼矩阵，τ_C＝J^-T(η)τ，为等价控制量，F_η(ν,η)是未建模部分；

定义位置量x₀＝η，速度量

控制系数b₀＝I，其中I为合适维数的单位矩阵，模型已知耦合非线性扰动量

未建模扰动量

控制器向量

水下机器人系统(3)能够写成以下串级积分形式：

其中，b在一阶速度系统(4)中为单位矩阵，将未建模动态扩张为新的状态x₂(t)，即，x₂(t)＝f(t)，水下机器人串级积分模型(4)被重写为：

x₂(t)＝ω(t) (6)

其中ω(t)是x₂(t)的导数。

进一步地，步骤二具体为：针对系统(3)设计的扩张状态观测器如下：

其中，z₁(t)和z₂(t)分别是x₁(t)和x₂(t)的观测值，e₁(t)＝x₁(t)-z₁(t)，β₁，β₂和δ₁是正的可调参数，非线性奇函数fal(·)的构成如下：

其中，sign(·)为符号函数：

进一步地，步骤三具体为：设计一个非线性状态误差反馈控制器使得内环能跟踪外环的输出，即目标角速度，令误差信号为：

ζ(t)＝ν(t)-z₁(t)

其中，ζ(t)是速度误差，ν(t)是大地坐标下水下机器人当前的实时速度；

构造一个非线性状态误差反馈控制器：

u_i(t)＝(α₁fal(ζ(t),σ₁,δ₂)-z₂(t))/b

其中，α₁是非线性状态误差反馈控制器的增益，σ₁和δ₂是可调参数，z₂(t)/b补偿水下机器人系统(4)中的未建模项f(t)。

进一步地，步骤四具体为：针对系统(3)的输入目标位置和角度，设计跟踪微分器：

其中，s₁(t)跟踪目标位置信号η_d(t)，s₂(t)跟踪目标位置的导数信号

r₀和h₀是两个可调的正数，r₀越大，跟踪速度越快；h₀越小，跟踪误差越小；非线性函数fhan(x₁(t),x₂(t),r,h)构成如下：

进一步地，步骤五具体为：设计一个PD控制器作为水下机器人控制系统的外环位置量控制器，输入到水下机器人系统的外环位置量为跟踪微分器安排的过渡过程s₁(t)，令位置目标值与实际值的偏差为ε(t)，并且满足ε(t)＝s₁(t)-η(t)，外环控制律设计为：

其中，k_p是比例增益，T_D是积分增益。

与现有技术相比，本发明具有以下有益的技术效果：

本发明将水下机器人的姿态与位置控制分为内外两个控制环，针对速度环高频震颤的特点设计自抗扰算法，位置环设计基于误差的PD控制器；速度环使用扩张状态观测器观测未建模和外部扰动并实时补偿抑制，速度环的抗扰性能大大增加，提高了系统的鲁棒性；位置环使用了跟踪微分器，使得位置偏差不会突变，避免了系统响应的突变，解决了快速性和超调的矛盾。

附图说明

图1为本发明的流程图。

具体实施方式

下面对本发明作进一步详细描述：

基于扩张状态观测器的自抗扰控制技术和双闭环控制架构，设计水下机器人的姿态与位置控制方法，通过以下步骤实现：

第一步：构建水下机器人动力学模型:

水下机器人在大地坐标下的位置量为

其中x、y和z是机器人的位置坐标，φ、θ和

是机器人的姿态角度。机体坐标系下，机器人的速度量为ν＝[μ υ ω pq r]^T，其中，μ、υ和ω为机器人的沿机体坐标轴的线速度，p、q和r是机器人绕机体坐标轴的角速度，方向满足右手螺旋定理，η和ν都是时间t的函数，在不引起歧义情况下，下文将忽略t。

为沿大地坐标轴的速度向量，通过转换矩阵J(η)，可以获得

与ν的转换关系：

对水下机器人运用动量与动量矩定理，可以建立如下的运动方程：

其中，M(η)是该模型的惯性矩阵，C(ν)是科氏力矩阵，D(ν)代表系统阻尼矩阵，g(η)是负浮力，τ表示控制量。系统(1)和系统(2)联立可得：

其中，M_η(η)＝J^-T(η)MJ^-1(η)，为广义惯性矩阵，C_η(ν,η)＝J^-T(η)[C(ν)-MJ^-1(η)]J^-1(η)，是广义科氏力矩阵，g_η(η)＝J^-T(η)g(η)，为等价负浮力，D_η(ν,η)＝J^-T(η)D(ν)J^-1(η)，为等价阻尼矩阵，τ_C＝J^-T(η)τ，为等价控制量，F_η(ν,η)是未建模部分。定义位置量x₀＝η，速度量

控制系数b₀＝I，其中I为合适维数的单位矩阵，模型已知耦合非线性扰动量

未建模扰动量

控制器向量

水下机器人系统(3)可以写成以下串级积分形式：

其中，b在一阶速度系统(4)中为单位矩阵，然而，在实际应用中它是可以调整的参数，实践证明b＝(1±30％)b₀系统都处于稳定状态。在水下机器人系统(3)中，未建模动态主要包括控制器扰动和水下机器人模型简化误差。在工程实际中，未建模动态连续可导并且有界，设计扩张状态观测器来估计未建模扰动。将未建模动态扩张为新的状态x₂(t)，即，x₂(t)＝f(t)，水下机器人串级积分模型(4)被重写为：

x₂(t)＝ω(t) (6)

其中ω(t)是x₂(t)的导数，注意ω(t)在实际应用中是有界的。

第二步：构建水下机器人速度环扩张状态观测器:

针对系统(3)设计的扩张状态观测器如下：

其中，z₁(t)和z₂(t)分别是x₁(t)和x₂(t)的观测值，e₁(t)＝x₁(t)-z₁(t)，β₁，β₂和δ₁是正的可调参数。其中非线性奇函数fal(·)的构成如下：

其中，sign(·)为符号函数：

其中，s₁(t)跟踪目标位置信号η_d(t)，s₂(t)跟踪目标位置的导数信号

r₀和h₀是两个可调的正数，基本上r₀越大，跟踪速度越快；h₀越小，跟踪误差越小，实际中r₀一般取为执行器饱和上限略小，h₀取为采样时长。非线性函数fhan(x₁(t),x₂(t),r,h)构成如下：

第五步：构建水下机器人位置环控制器:

设计一个PD控制器作为水下机器人控制系统的外环位置量控制器，输入到水下机器人系统的外环位置量为跟踪微分器安排的过渡过程s₁(t)，令位置目标值与实际值的偏差为ε(t)，并且满足ε(t)＝s₁(t)-η(t)。外环控制律设计为：

其中，k_p是比例增益，T_D是积分增益。

本发明考虑标称模型的不确定性，控制量的未知扰动和环境扰动，采用扩张状态观测器实时抑制扰动。通过对目标姿态和位置安排过渡过程和采用非线性状态误差反馈，以获得更高的控制精度和更强的鲁棒性。外环PD控制技术的使用，利于工程实现和降低计算复杂性。

下面结合具体实施例对本发明做进一步详细描述：

本发明如图1所示，通过以下步骤实现：

第一步：构建水下机器人动力学模型:

水下机器人在大地坐标下的位置量为

其中x、y和z是机器人的位置坐标，φ、θ和

是机器人的姿态角度。机体坐标系下，机器人的速度量为ν＝[μ υ ω pq r]^T，其中，μ、υ和ω为机器人的沿机体坐标轴的线速度，p、q和r是机器人绕机体坐标轴的角速度，方向满足右手螺旋定理。

为沿大地坐标轴的速度向量，通过转换矩阵J(η)，可以获得

与ν的转换关系：

对水下机器人运用动量与动量矩定理，可以建立如下的运动方程：

其中，M(η)是该模型的惯性矩阵，C(ν)是科氏力矩阵，D(ν)代表系统阻尼矩阵，g(η)是负浮力，τ_C表示控制量。系统(1)和系统(2)联立可得：

其中，M_η(η)＝J^-T(η)MJ^-1(η)，C_η(ν,η)＝J^-T(η)[C(ν)-MJ^-1(η)]J^-1(η)，g_η(η)＝J^-T(η)g(η)，C_η(ν,η)＝J^-T(η)[C(ν)-MJ^-1(η)]J^-1(η)，D_η(ν,η)＝J^-T(η)D(ν)J^-1(η)和τ_C＝J^-T(η)τ，F_η(ν,η)是未建模部分。定义位置量x₀＝η，速度量

控制系数b₀＝I，模型已知耦合非线性扰动量

未建模扰动量

控制器向量

相关的参数定义如下：

r_B＝[x_B,y_B,z_B]^T＝[0,0,0]^T，r_G＝[x_G,y_G,z_G]^T＝[0,0,0.05]^T，m＝125，

C(ν)＝C_A(ν)+C_RB(ν)，

D(ν)＝diag{48|μ|,48|υ|,48|ω|,110|p|,110|q|,110|r|}，

其中，x_B，y_B和z_B是浮心坐标，x_G，y_G和z_G表示质心坐标，m表示质量，I₀是转动惯量矩阵，ν₁＝[μ υ ω]^T和ν₂＝[p q r]^T是速度ν的平移速度和角速度分量，C_A(ν)和C_RB(ν)分别是所排流体的运动所具有的柯氏矩阵和柯氏矩阵。

水下机器人系统(3)可以写成以下串级积分形式：

其中，b在一阶速度系统(4)中为单位矩阵，然而，在实际应用中它是可以调整的参数，实践证明b＝(1±30％)b₀系统都处于稳定状态。在水下机器人系统(3)中，未建模动态主要包括控制器扰动和水下机器人模型简化误差。在工程实际中，未建模动态连续可导并且有界，设计扩张状态观测器来估计未建模扰动。将未建模动态扩张为新的状态x₂(t)，即，x₂(t)＝f(t)，水下机器人串级积分模型(4)被重写为：

x₂(t)＝ω(t) (6)

其中ω(t)是x₂(t)的导数，注意ω(t)在实际应用中是有界的。可调参数b的取值为：b＝diag{0.9 0.9 1 0.4 0.5 0.7}

第二步：构建水下机器人速度环扩张状态观测器:

针对系统(3)设计的扩张状态观测器如下：

其中，z₁(t)和z₂(t)分别是x₁(t)和x₂(t)的观测值。其中非线性奇函数fal(·)的构成如下：

其中，sign(·)为符号函数：

在本实例中，β₁和β₂的取值为：

β₁＝diag{100 100 300 200 300 150}

β₂＝diag{10000 10000 20000 10000 20000 20000}

第三步：构建水下机器人速度环控制器:

设计一个非线性状态误差反馈控制器使得内环能跟踪外环的输出，即目标角速度。令误差信号为：

ζ(t)＝ν(t)-z₁(t)

其中，ν(t)是大地坐标下水下机器人当前的实时速度。构造一个非线性状态误差反馈控制器：

u_i(t)＝(α₁fal(ζ(t),σ₁,δ₂)-z₂(t))/b

其中，α₁是非线性状态误差反馈控制器的增益，z₂(t)/b补偿水下机器人系统(4)中的未建模项f(t)。可调参数α₁的取值为：

α₁＝diag{7.6 7.6 8 7.6 7.6 8}

第四步：构建水下机器人位置环跟踪微分器:

针对系统(3)的输入目标位置和角度。设计跟踪微分器：

其中，s₁(t)跟踪目标位置信号η_d(t)，s₂(t)跟踪目标位置的导数信号

r₀和h₀是两个可调的正数，基本上r₀越大，跟踪速度越快；h₀越小，跟踪误差越小，实际中r₀一般取为执行器饱和上限略小，h₀取为采样时长。非线性函数fhan(x₁(t),x₂(t),r,h)构成如下：

可调参数r₀和h₀的取值为：

r₀＝diag{1000 1000 1200 1000 1000 1000}

h₀＝diag{0.005 0.005 0.005 0.005 0.005 0.005}

第五步：构建水下机器人位置环控制器:

设计一个PD控制器作为水下机器人控制系统的外环位置量控制器，输入到水下机器人系统的外环位置量为跟踪微分器安排的过渡过程s₁(t)，令位置目标值与实际值的偏差为ε(t)，并且满足ε(t)＝s₁(t)-η(t)。外环控制律设计为：

其中，k_p和T_D是可调参数，具体取值如下：

k_p＝diag{0.2 0.2 0.21 0.2 0.2 0.21}

T_D＝diag{0.47 0.47 0.5 0.47 0.47 0.5}。

Claims (1)

1.一种基于双闭环自抗扰的水下机器人姿态与位置控制方法，其特征在于，包括以下步骤：

步骤一：对水下机器人运用动量矩定理和坐标系间的转换关系，建立动力学模型；

具体为：水下机器人在大地坐标下的位置量为

其中x、y和z是机器人的位置坐标，f 、θ和

是机器人的姿态角度；机体坐标系下，机器人的速度量为ν＝[μ υ ωp q r]^T，其中，μ、υ和ω为机器人沿机体坐标轴的线速度，p、q和r是机器人绕机体坐标轴的角速度，方向满足右手螺旋定理，η和ν都是时间t的函数；

为沿大地坐标轴的速度向量，通过转换矩阵J(η)，能够获得

与ν的转换关系：

对水下机器人运用动量与动量矩定理，能够建立如下运动方程：

其中，M(η)是该模型的惯性矩阵，C(ν)是科氏力矩阵，D(ν)代表系统阻尼矩阵，g(η)是负浮力，τ表示控制量；

系统(1)和系统(2)联立得到：

其中，M_η(η)＝J^-T(η)MJ^-1(η)，为广义惯性矩阵，C_η(ν,η)＝J^-T(η)[C(ν)-MJ^-1(η)]J^-1(η)，是广义科氏力矩阵，g_η(η)＝J^-T(η)g(η)，为等价负浮力，D_η(ν,η)＝J^-T(η)D(ν)J^-1(η)，为等价阻尼矩阵，τ_C＝J^-T(η)τ，为等价控制量，F_η(ν,η)是未建模部分；

定义位置量x₀＝η，速度量

控制系数b₀＝I，其中I为合适维数的单位矩阵，模型已知耦合非线性扰动量

未建模扰动量

控制器向量

水下机器人系统(3)能够写成以下串级积分形式：

其中，b在一阶速度系统(4)中为单位矩阵，将未建模动态扩张为新的状态x₂(t)，即，x₂(t)＝f(t)，水下机器人串级积分模型(4)被重写为：

x₂(t)＝ω(t) (6)

其中ω(t)是x₂(t)的导数；

步骤二：设计水下机器人的速度环扩张状态观测器；

具体为：针对系统(3)设计的扩张状态观测器如下：

其中，z₁(t)和z₂(t)分别是x₁(t)和x₂(t)的观测值，e₁(t)＝x₁(t)-z₁(t)，β₁，β₂和δ₁是正的可调参数，非线性奇函数fal(·)的构成如下：

其中，sign(·)为符号函数：

步骤三：利用扩张 状态观测器给出的扰动观测值构建基于非线性函数反馈速度环控制器；

具体为：设计一个非线性状态误差反馈控制器使得内环能跟踪外环的输出，即目标角速度，令误差信号为：

ζ(t)＝ν(t)-z₁(t)

其中，ζ(t)是速度误差，ν(t)是大地坐标下水下机器人当前的实时速度；

构造一个非线性状态误差反馈控制器：

u_i(t)＝(α₁fal(ζ(t),σ₁,δ₂)-z₂(t))/b

其中，α₁是非线性状态误差反馈控制器的增益，σ₁和δ₂是可调参数，z₂(t)/b补偿水下机器人系统(4)中的未建模项f(t)；

步骤四：基于非线性函数，设计目标位置信号的跟踪微分器，给出位置信号的过渡过程和目标速度估计值；

具体为：针对系统(3)的输入目标位置和角度，设计跟踪微分器：

其中，s₁(t)跟踪目标位置信号η_d(t)，s₂(t)跟踪目标位置的导数信号

r₀和h₀是两个可调的正数，r₀越大，跟踪速度越快；h₀越小，跟踪误差越小；非线性函数fhan(x₁(t),x₂(t),r,h)构成如下：

步骤五：设计水下机器人位置环控制器，对水下机器人姿态与位置进行控制

具体为：设计一个PD控制器作为水下机器人控制系统的外环位置量控制器，输入到水下机器人系统的外环位置量为跟踪微分器安排的过渡过程s₁(t)，令位置目标值与实际值的偏差为ε(t)，并且满足ε(t)＝s₁(t)-η(t)，外环控制律设计为：

其中，k_p是比例增益，T_D是积分增益。

Images