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

权利要求书

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是积分增益。