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

有效

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

1.一種改進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時刻的先驗估計值；

和計算先驗誤差協方差其中Q_k-1為系統k-1時刻的過程噪聲的協方差；

所述無人艇的實時橫滾角θ_i計算為根據方向余弦矩陣和四元數旋轉矩陣的對于關系，帶入以下公式，得到所述無人艇的實時橫滾角θ_i的值γ、俯仰角的值θ和偏航角的值ψ：

其中，q₀、q₁、q₂、q₃均為實數，且q₁、q₂、q₃構成旋轉向量。

2.根據權利要求1所述的改進EKF及模糊PID雙閉環控制的無人艇抗擾系統，其特征在于：

所述第一階段加速度計數據校正包括計算四元數q的第一雅可比矩陣

計算第一卡爾曼增益

其中第一噪聲矩陣V_k1為單位矩陣，R_k1為加速度計誤差協方差矩陣；記錄所述加速度計的測量數據z_k1＝a_x，a_y，a_z，其中a_x為機體加速度在機體坐標系下x軸方向的分量，a_y為機體加速度在機體坐標系下y軸方向的分量，a_z為機體加速度在機體坐標系下z軸方向的分量；

計算加速度理論值：

根據卡爾曼濾波公式，計算四元數q的第一修正量：

并將q_∈1，3置零,更新四元數q為其中為四元數q的第一修正量的先驗估計值；

和計算后驗誤差協方差

所述第二階段加速度計數據校正包括計算四元數q的第二雅可比矩陣

計算第二卡爾曼增益

其中第二噪聲矩陣V_k2為單位矩陣，R_k2為磁力計誤差協方差矩陣；

記錄所述磁力計的測量數據z_k2＝m_x，m_y，m_z，其中m_x為磁場方向在機體坐標系下x軸方向的分量，m_y為磁場方向在機體坐標系下y軸方向的分量，m_z為磁場方向在機體坐標系下z軸方向的分量；

計算磁力計理論值：

根據卡爾曼濾波公式，計算四元數q的第二修正量：

將q_∈2，1和q_∈2，2置零,更新四元數q為

和更新后驗誤差協方差為P_k＝(I-K_k2H_k2)P_k1。