[發明專利]基于AHRS的精對準算法

有效

• 申請號: 201310439123.0
• 申請日: 2013-09-24
• 公開（公告）號: CN103471594A
• 公開（公告）日: 2013-12-25
• 發明（設計）人: 李玉柏;王堅;李桓
• 申請（專利權）人: 成都市星達通科技有限公司
• 主分類號: G01C21/20
• 分類號: G01C21/20
• 代理公司: 成都頂峰專利事務所(普通合伙) 51224
• 代理人: 楊軍

1.基于AHRS的精對準算法，其特征在于，包括以下步驟：

（1）采集當前的緯度信息和重力加速度信息；

（2）根據AHRS中加速度計和陀螺儀的輸出測得當前的姿態轉移矩陣C_bp；

（3）測算出地球相對于慣性坐標系的旋轉角速度w_ie；

（4）確定當前X軸向的角度誤差、Y軸向的角度誤差、Z軸向的角度誤差、陀螺儀在X軸向的常值偏移、陀螺儀在Y軸向的常值偏移、陀螺儀在Z軸向的常值偏移、加速度計在X軸向的常值偏移和加速度計在Y軸向的常值偏移的被估計值；

（5）根據步驟（4）中被估計值前后兩個時刻的關系得出連續卡爾曼濾波下的狀態轉移矩陣A，并根據公式F＝eye(8)+A×t將其轉換為離散卡爾曼濾波下的狀態轉移矩陣，其中，t為濾波間隔；

（6）根據當前噪聲和觀測值前后兩個時刻間的關系求出噪聲驅動矩陣G，根據被估計值和觀測值之間的關系求出觀測矩陣H，根據陀螺儀的隨機誤差確定AHRS中的系統噪聲方差矩陣，并根據加速度計的隨機誤差確定觀測噪聲方差矩陣；

（7）將被估計值的初始濾波值設置為零；

（8）開始卡爾曼濾波，每次取兩個方向上的加速度計的輸出和陀螺儀的三個方向上的輸出作為觀測值；

（9）根據求得的濾波結果更新觀測噪聲方差矩陣和被估計值，并再次取觀測值進行卡爾曼濾波；

（10）返回步驟（8），直至兩次濾波結果小于用戶設定的閾值；

（11）使用求得的X軸向、Y軸向和Z軸向上的姿態角的誤差對姿態轉移矩陣C_bp進行糾正。

2.根據權利要求1所述的基于AHRS的精對準算法，其特征在于，所述步驟（2）具體包括以下步驟：

（2a）根據AHRS中加速度計和陀螺儀的輸出得出重力加速度在地理坐標系下的投影和地球繞慣性坐標系自轉角速度在地理坐標系下的投影

（2b）根據公式rt→=g→g→×w→ie(g→×w→ie)×g→=r1t→r2t→r3t→]]>構造坐標系下的三個向量和

（2c）根據方程式r1b→=Ctbr1t→r2b→=Ctbr2t→r3b→=Ctbr3t→]]>解得矩陣其中，t為時間間隔，將以平臺坐標系表示；

（2d）根據公式計算出姿態轉移矩陣C_bp。