[發明專利]一種基于雙閉環自抗擾的水下機器人姿態與位置控制方法

有效

• 申請號: 201711311540.1
• 申請日: 2017-12-11
• 公開（公告）號: CN108196445B
• 公開（公告）日: 2021-02-02
• 發明（設計）人: 袁源;成磊;袁建平;朱占霞;孫沖;張博
• 申請（專利權）人: 西北工業大學
• 主分類號: G05B13/04
• 分類號: G05B13/04
• 代理公司: 西安通大專利代理有限責任公司 61200
• 代理人: 齊書田

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