[發明專利]一種田間作業車輛外部加速度辨識與姿態同步估計方法

在審

• 申請號: 201811029509.3
• 申請日: 2018-09-05
• 公開（公告）號: CN109141414A
• 公開（公告）日: 2019-01-04
• 發明（設計）人: 張智剛;黃培奎;羅錫文;張健;何杰;劉兆朋;王輝;岳斌斌;高維煒
• 申請（專利權）人: 華南農業大學
• 主分類號: G01C21/16
• 分類號: G01C21/16
• 代理公司: 廣州市華學知識產權代理有限公司 44245
• 代理人: 李斌

1.一種田間作業車輛外部加速度辨識與姿態同步估計方法，其特征在于，包括下述步驟：

S1、基于三軸陀螺儀與三軸加速度計的測量模型，推導外部加速度的一階低通濾波模型，融合到卡爾曼濾波器；

S2、將三軸陀螺儀測量量ω_X、ω_Y、ω_Z作為卡爾曼濾波器的狀態輸入量；

S3、將三軸加速度計的測量量a_X、a_Y、a_Z作為濾波器的觀測輸入向量，進行外部加速度的修正；

S4、建立包含3個狀態向量與3個觀測向量的系統卡爾曼濾波狀態方程和測量方程，精準測量田間作業車輛姿態角與辨識外部加速度。

2.根據權利要求1所述的田間作業車輛外部加速度辨識與姿態同步估計方法，其特征在于，在步驟S1之前，還包括下述步驟：

搭建硬件平臺，所述硬件平臺包括3軸加速度計和3軸陀螺儀，所述3軸加速度計和3軸陀螺儀的輸出頻率不小于100Hz。

3.根據權利要求1所述的田間作業車輛外部加速度辨識與姿態同步估計方法，其特征在于，在步驟S1中，

三軸陀螺儀測量模型為：y_G＝ω+n_G，

三軸加速度計測量模型為：y_A＝g+α+n_A，

外部加速度的一階低通模型為：α_t＝kα_t-1+ε_t。

4.根據權利要求3所述的田間作業車輛外部加速度辨識與姿態同步估計方法，其特征在于，在步驟S3中，

外部加速度估計誤差為：

基于方向余弦矩陣的姿態角解算為：

其中y_G，y_A分別是三軸陀螺儀與三軸加速度計測量值，由此可估算外部加速度。

5.根據權利要求1所述的田間作業車輛外部加速度辨識與姿態同步估計方法，其特征在于，在步驟S4中，所述系統卡爾曼濾波狀態方程具體如下：

系統的狀態向量為[c₃₁,c₃₂,c₃₃]；

系統狀態狀態轉移矩陣為

系統狀態噪聲矩陣為

系統狀態噪聲協方差矩陣為

6.根據權利要求1所述的田間作業車輛外部加速度辨識與姿態同步估計方法，其特征在于，在步驟S4中，所述測量方程如下：

陀螺儀測量噪聲協方差矩陣為

系統觀測矩陣為

系統輸入矩陣為H＝gI；

系統觀測噪聲矩陣為

系統觀測噪聲協方差矩陣為R_t-1＝∑_α+∑_A；

外部加速度模型的測量誤差協方差矩陣為

加速度計測量噪聲協方差矩陣為

t表示本次計算的時序，Δt表示融合時間。