[發明專利]基于加速度計和陀螺儀的載體轉速與轉角補償測量方法

在審

• 申請號: 201410175383.6
• 申請日: 2014-04-28
• 公開（公告）號: CN103913171A
• 公開（公告）日: 2014-07-09
• 發明（設計）人: 王衛江;高巍;榮政
• 申請（專利權）人: 北京理工大學
• 主分類號: G01C21/16
• 分類號: G01C21/16;G01C25/00
• 代理公司: 暫無信息
• 代理人: 暫無信息

1.基于加速度計和陀螺儀的載體轉速與轉角補償測量方法，其特征在于：具體包括如下步驟：

第一步：將傳感器和相關測量電路安裝于載體任意位置上，使得加速度計x、y軸平面與載體轉動平面平行即可，確定載體的初始轉角并記為θ_est(0)；

所述傳感器包括三軸加速度計、單軸陀螺儀；

所述相關測量電路包括信號采集電路和處理電路，采集電路與傳感器連接，處理電路與采集電路連接；

第二步：確定傳感器數據采樣周期T和互補濾波器數字濾波方法中的互補濾波器時間常數τ，令載體開始轉動，測量電路以采樣周期T讀取三軸加速度計和單軸陀螺儀的測量值，當前時刻n的測量值包括加速度計x軸方向上的加速度值a_x(n)、y軸方向上的加速度值a_y(n)、陀螺儀角速度測量值rate(n)，采用移動平均方法對采集到的加速度計和陀螺儀測量值進行平穩去噪；

第三步：利用加速度計測量值a_x(n)和a_y(n)計算加速度計所測載體轉角θ_acc(n)，利用陀螺儀測量值rate(n)計算陀螺儀所測載體轉角θ_gyro(n)；具體方法為：

所述的利用加速度計測量值計算載體轉角是利用重力加速度在x軸和y軸上分量的三角函數關系求得，當載體在非水平面內做旋轉運動時，重力加速度在加速度計的x軸和y軸上產生分量，在不考慮其他外力情況下，利用該x軸和y軸上的加速度分量根據三角關系求得：

tanθacc(n)=ax(n)ay(n),]]>

繼而得到：

θacc(n)=arctanax(n)ay(n);]]>

所述的利用陀螺儀測量值計算載體轉角是利用轉動角速度對時間積分求得；利用上一時刻轉角估計值以及當前時刻角速度測量值，并將積分運算離散化為求和運算以后可得：

θ_gyro(n)＝θ_est(n-1)+rate(n)×T；

第四步：根據傳感器數據采樣周期T和互補濾波器時間常數τ計算濾波器權重系數W，采用互補濾波器數字濾波方法計算載體轉角估計值θ_est(n)；互補濾波器數字濾波方法的具體過程為：

根據傳感器數據采樣周期和互補濾波器時間常數τ計算濾波器權重系數W，由該濾波器時間常數

τ=W×T1-W]]>

得到：

W=ττ+T;]]>

再利用互補濾波器將加速度計測量計算所得轉角θ_acc(n)和陀螺儀測量計算所得轉角θ_gyro(n)權重相加，得到載體轉角估計值：

θ_est(n)＝W×θ_gyro(n)+(1-W)×θ_acc(n)；

第五步：利用當前時刻載體轉角估計值θ_est(n)、上一時刻載體轉角估計值θ_est(n-1)和采樣周期T計算載體轉動角速度ω(n)；

所述的載體轉動角速度ω(n)的求解是利用載體轉角的估計值對時間微分得到，將微分運算離散化后得到：

ω(n)=θest(n)-θest(n-1)T.]]>