[發明專利]改進EKF及模糊PID雙閉環控制的無人艇抗擾系統及使用方法

有效

• 申請號: 202110373936.9
• 申請日: 2021-04-07
• 公開（公告）號: CN113093527B
• 公開（公告）日: 2021-11-23
• 發明（設計）人: 魯仁全;滕達;饒紅霞;林明;徐雍
• 申請（專利權）人: 廣東工業大學
• 主分類號: G05B11/42
• 分類號: G05B11/42
• 代理公司: 佛山市禾才知識產權代理有限公司 44379
• 代理人: 單蘊倩;朱培祺

本發明公開了改進EKF及模糊PID雙閉環控制的無人艇抗擾系統及使用方法,包括數據采集單元、數據處理單元、控制單元和執行單元，執行單元包括平衡水箱、導軌和電機，數據處理單元用于根據采集的所述無人艇的姿態數據，通過改進EKF姿態解算算法得到無人艇的實時橫滾角θ_i，控制單元用于將所述橫滾角的偏差角θ_e輸入至模糊PID雙閉環控制器，得到對電機的PWM控制信號。采用基于雙閉環模糊PID控制，能夠使無人艇橫滾角發生較大偏移時，將無人艇姿態及時調整回平穩狀態，而不是等外力結束后通過自身結構進行調整，更為安全，并改進EKF姿態解算算法以應對不同的磁場干擾情況，提高橫滾角的解算精度。

技術領域

本發明涉及無人艇技術領域，尤其涉及改進EKF及模糊PID雙閉環控制的無人艇抗擾系統及使用方法。

背景技術

無人艇行駛的過程中，在遭遇到風浪等外力干擾時，艇身會發生傾斜。當傾斜角度過大時，就可能發生翻船事故。當外力消失后，船體能平穩地回到原來的漂浮狀態的能力，稱之為船體的穩性。當前大部分船體的穩性都是等外力消失后，通過使用不倒翁結構來使其恢復到平穩狀態，使得船體具有一定的抗傾斜干擾能力。

然而，船體的穩性的好壞并不是簡單的靠這種復原能力的好壞的評斷的。需要結合自身和環境因素，使船體保持一個適當的穩性。這種方法并不能解決外力對船體姿態的干擾。當外力存在時，對小型無人艇的姿態的干擾足以造成對無人艇控制的影響，甚至會導致翻船。當前對于無人艇的姿態平穩控制的研究相對較少，這也受限于無人艇的結構。其次，現有的EKF姿態解算算法也無法應對不同磁場所帶來的干擾。但在無人艇的行駛中，保持其有一個平穩的姿態是極其有必要的，特別是對于抗傾斜能力稍弱的中小型無人艇。

發明內容

本發明的一個目的在于提出一種改進EKF及模糊PID雙閉環控制的無人艇抗擾系統及使用方法，采用基于雙閉環模糊PID控制，能夠使無人艇橫滾角發生較大偏移時，將無人艇姿態及時調整回平穩狀態，而不是等外力結束后通過自身結構進行調整，更為安全，并改進EKF姿態解算算法以應對不同的磁場干擾情況，提高橫滾角的解算精度。

為達此目的，本發明采用以下技術方案：

一種改進EKF及模糊PID雙閉環控制的無人艇抗擾系統，包括數據采集單元、數據處理單元、控制單元和執行單元；

所述執行單元包括平衡水箱、導軌和電機，所述導軌安裝于無人艇，所述導軌的左右兩端分別設有所述電機，所述平衡水箱通過所述電機傳動在所述導軌左右移動；

所述數據采集單元用于通過加速度計、陀螺儀和磁力計進行所述無人艇的姿態數據采集；

所述數據處理單元用于根據采集的所述無人艇的姿態數據，通過改進EKF姿態解算算法得到所述無人艇的實時橫滾角θ_i，并將所述實時橫滾角θ_i與預設的橫滾角期望值θ_p進行做差分析，得到所述橫滾角的偏差角θ_e；

所述控制單元用于將所述橫滾角的偏差角θ_e輸入至模糊PID雙閉環控制器，得到對電機的PWM控制信號，所述PWM控制信號用于驅動平衡水箱移動以調節所述無人艇的實時橫滾角θ_i，直至所述無人艇的實時橫滾角θ_i在預設范圍內，所述無人艇達到平衡為止。

優選地，所述改進EKF姿態解算算法依次包括先驗誤差協方差計算、第一階段加速度計數據校正、第二階段加速度計數據校正和所述無人艇的實時橫滾角θ_i計算；

所述先驗誤差協方差計算包括根據所述陀螺儀的三軸角速度數據，計算系統的離散轉移矩陣其中，n為世界坐標系，b為無人艇機體坐標系，k為系統時刻，T為周期時間,w_x、w_y和w_z分別代表所述陀螺儀測量的x軸方向、y軸方向和z軸方向的角速度；

計算四元數q的先驗估計值其中為四元數q的系統k-1時刻的先驗估計值；