[發明專利]基于UKF的無人機氣壓計異常數據處理的高度控制方法

有效

• 申請號: 201710743039.6
• 申請日: 2017-08-25
• 公開（公告）號: CN107577238B
• 公開（公告）日: 2020-12-18
• 發明（設計）人: 康騰;黃濤;劉國良;費鵬;曹鵬蕊
• 申請（專利權）人: 深圳禾苗通信科技有限公司
• 主分類號: G05D1/04
• 分類號: G05D1/04
• 代理公司: 深圳市卓科知識產權代理有限公司 44534
• 代理人: 邵妍;張金玲

1.一種基于UKF的無人機氣壓計異常數據處理的高度控制方法，其特征在于：利用加速度計的Z方向的加速度信息以及加速度計和氣壓計第一次UKF濾波出來的速度后對其差分得到的加速度信息做差,利用差值來量化氣流擾動對氣壓計的影響,具體包括帶加速度計和氣壓計的飛行器，加速度計通過降噪處理獲得加速度信息，氣壓計通過降噪處理獲得高度信息，后經第一次UKF濾波，第一次UKF濾波后更新氣壓計數據，更新后進行第二次UKF濾波，第二次UKF濾波后將第二次濾波位置信息以及第二次濾波速度信息傳遞給飛控位置控制模塊；

具體步驟如下：

步驟一、構建無人機高度方向的系統方程和量測方程：

在只考慮氣壓計(Barometer)和加速度計(Accelerometer)的情況下，構造無人機高度方向的系統方程和量測方程為：

系統方程：

量測方程：

式中，h_k、ve_k、acc_bias_k分別為k時刻Z方向的位置、速度和加速度偏移量；Ts為時間步長；為加速度計經旋轉矩陣后得到的NED坐標系下Z方向的加速度；Z_k為k時刻氣壓計的高度數據，在實際使用中由于氣壓計數據噪聲太大，會首先對其進行一階低通濾波處理；W_k-1、V_k分別為為系統方程和量測方程的協方差矩陣，在實際使用過程中會通過調節協方差矩陣的大小來使得濾波出來的結果跟隨氣壓計的特性；

步驟二、第一次UKF濾波：

對于離散時間非線性系統：

式中，x_k是n×1維的狀態向量；y_k是m×1維的量測向量，B_k-1是n×l維的系統噪聲分配矩陣；ω_k-1是l×1維的系統噪聲向量，ν_k-1是m×1維的量測噪聲向量，f(·)和h(·)均為時間參數離散而狀態連續的非線性向量函數，無人機系統常用的擴展卡爾曼濾波算法和無跡卡爾曼濾波算法，整體框架一樣：

式中，K_k為卡爾曼增益；P_x,k/k-1、P_y,k/k-1、P_xy,k/k-1為系統和量測協方差矩陣；為系統變量和量測變量預測值；P_x,k為系統變量和協方差更新值；

在UKF濾波算法中，需要對狀態預測和量測預測分別進行UT變換來更新系統狀態；

狀態預測UT變換：

量測預測UT變換：

步驟三、高度Z的更新：

(一)由步驟2中解算得到的速度差分得到氣壓計和加速度計濾波得到高度方向的加速度AccZ__acc+baro；再與加速度計獲取的高度方向的加速度AccZ__acc做差值；得到加速度差值AccZ_err,以此參數來量化氣流擾動對氣壓計的影響；

(二)通過AccZ_err的曲線變化，設定合適的閾值AccZ__threshold，當當前時刻的AccZ_err的絕對值小于閾值時，用氣壓計數據更新高度；而當當前時刻的AccZ_err的絕對值大于閾值時，則認為此時氣壓計受氣流擾動較大，此時氣壓計數據不準確，則用前一時刻的氣壓計數據更新高度；

步驟四、第二次UKF濾波；

利用步驟三中更新的高度信息Z_new作為量測值來更新UKF的狀態和協方差信息；

此時，量測值yk＝Z_new,系統狀態Xk和協方差Pk的信息依舊由UT變換得到，最后，將第二次UKF濾波位置解算得到的高度方向的位置、速度信息輸入給飛控位置控制模塊，從而使得高度控制更為準確，飛行體驗大大提高；

其中，bias_k：氣壓計高度估計的偏置量；

γ：狀態方差量的加權值；

Q_k-1：狀態方差量的協方差矩陣；

R_k：量測方差量的協方差矩陣。