[发明专利]一种水下机器人平面轨迹跟踪控制的方法

权利要求书

1.一种基于有限时间扰动观测器的水下机器人平面轨迹跟踪控制的方法，其特征在于：包括以下步骤：

A、设计轨迹跟踪误差系统

将水下机器人水平面三自由度数学模型，描述如下：

式中：惯性坐标系下水下机器人的纵向位移x、横向位移y和航向角ψ，记为η＝[x y ψ]^T；附体坐标系下水下机器人纵向线速度u、横向线速度v和航向角速度r，记为υ＝[u v r]^T；惯性坐标系E-XYZ以大地为坐标原点，EZ指向地心，EX、EY和EZ相互垂直；附体坐标系O-xyz以水下机器人中心为坐标原点，O-x指向水下机器人的运动前方，O-y指向水下机器人的右侧，O-z垂直于xOy面指向水下机器人的下方；；将纵向控制输入τ₁、横向控制输入τ₂和航向角控制输入τ₃记为τ＝[τ₁ τ₂ τ₃]^T，d表示外界干扰，令d＝MJ^T(η)δ(t)，δ(t)＝[δ₁ δ₂ δ₃]^T，其中δ₁表示纵向所受干扰，δ₂表示横向所受干扰，δ₃表示航向角所受干扰，J(η)表示惯性坐标系与附体坐标系转换矩阵，描述为：

且有J^T(η)＝J^-1(η)，其中R(r)描述为：

M表示质量和附加质量矩阵，且有M＝M^T＞0，C(υ)表示科氏及向心力矩阵，且有C(υ)＝-C^T(υ)，D(υ)表示阻尼矩阵，具体描述为：

式中，m为水下机器人的质量，I_z为转动惯量，分别为纵向、横向和航向角的水动力导数，X_u,Y_v,N_r分别为纵向、横向和航向角的一阶阻尼系数，X_u|u|,Y_v|v|,N_r|r|分别为纵向、横向和航向角的二阶阻尼系数；

基于以上模型，得到平面三自由度期望轨迹的数学模型，描述如下：

式中：η_d＝[x_d y_d ψ_d]^T表示期望轨迹的位置，υ_d＝[u_d v_d r_d]^T表示期望轨迹的速度，τ_d＝[τ_d1 τ_d2 τ_d3]^T表示期望轨迹的控制输入；

根据上述三自由度数学模型，为了方便设计，定义状态变量，描述如下：

式中：Θ＝[Θ₁ Θ₂ Θ₃]^T，Θ_d＝[Θ_d1 Θ_d2 Θ_d3]^T；

则，三自由度期望轨迹的数学模型改写为：

式中：

同样，期望轨迹的数学模型改写为：

式中：

Ω＝-J(η)M^-1(C(J^-1(η)Θ)+D(J^-1(η)Θ))J^-1(η)Θ+R(Θ₃)Θ

Ω_d＝-J(η_d)M^-1(C(J^-1(η_d)Θ_d)+D(J^-1(η_d)Θ_d))J^-1(η_d)Θ_d+R(Θ_d3)Θ_d

由式(9)-(10)得位置和速度跟踪误差系统，描述如下：

式中：Θ_e＝Θ-Θ_d，Ω_e＝Ω-Ω_d-J(η_d)M^-1τ_d；

B、设计控制律

针对外界扰动设计有限时间扰动观测器，假设外界扰动满足：

||δ_d||≤E (12)

式中：E是有界常数且为正；

为了提高轨迹跟踪控制系统的抗干扰能力，设计有限时间扰动观测器，描述如下：

其中：

式中：z_i＝[z_i1 z_i2 z_i3]^T，i＝0,1,2，其中z₀为矩阵Mυ的估计值，z₁为外界扰动δ_d的估计值，z₂为的估计值；

ζ_j＝[ζ_j1 ζ_j2 ζ_j3]^T为有限时间扰动观测器的状态观测值，j＝0,1,2；

k＝1,2,3，L＝diag(l₁,l₂,l₃)是有限时间扰动观测器参数，为常数；

根据式(11)，设计非奇异终端滑模面，描述如下：

s＝η_e+βΘ_e^q/p (15)

式中：β为常数且大于零，q,p均为正奇数，且1＜q/p＜2；

对上式求导得：

结合跟踪误差系统(11)，将上式改写为：

式中：diag(·)表示对角矩阵；

根据以上描述，设计有限时间扰动观测器的非奇异终端滑模控制律，描述如下：

式中：表示非奇异终端滑模的幂次趋近律，Λ＝diag(κ₁,κ₂,κ₃)表示常数对角矩阵，其中κ_i＞0，i＝1,2,3，α为常数，且0＜α＜1，sgn(s)＝[sgn(s₁) sgn(s₂) sgn(s₃)]^T。