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

有效

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

技術領域

本發明涉及一種導航領域的對準算法，具體地說，是涉及一種基于AHRS的精對準算法。

背景技術

傳統的精對準技術觀測量比較少，大都只用東向和北向的速度做觀測值或只用東向和北向加速度計做觀測值，該種對準方式雖然對姿態角的誤差有一定的估計作用，但是收斂很慢，且誤差估計精度不高。現有技術中大都采用多傳感器的輸出做觀測值、采用東向和北向的速度和陀螺儀的三個方向的角速度共五個值做觀測值，以此提高收斂速度和精度，但這種方式的對準度仍然較為有限，不能滿足人們需求。

發明內容

本發明的目的在于提供一種基于AHRS的精對準算法，主要解決現有技術中存在的精對準技術收斂較慢、誤差估計精度不高，不能滿足人們需求的問題。

為了實現上述目的，本發明采用的技術方案如下：

基于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）具體包括以下步驟：

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