Ahrs高精度姿態數據計算方法
2023-09-18 09:06:55 1
Ahrs高精度姿態數據計算方法
【專利摘要】本發明公開了一種AHRS高精度姿態數據計算方法，主要解決了現有技術中存在的由於加速度計和陀螺儀的精度有限，且存在累積誤差，因而所得到的姿態數據誤差較大，不能滿足技術需求的問題。其包括初始化傳感器和歸一化坐標系轉換矩陣；計算載體的姿態數據和此時的坐標系轉換矩陣R1；計算該時刻的坐標系轉換矩陣R2；確定該時刻最終的坐標系轉換矩陣，並對最終的坐標系轉換矩陣進行歸一化，得到該時刻的歸一化坐標系轉換矩陣；根據步驟（4）中得出的歸一化坐標系轉換矩陣求解出該時刻最終的姿態數據；返回步驟（2），進行下一時刻的姿態數據計算。通過上述方案，本發明達到了便於實現、精度較高的目的，具有很高的實用價值和推廣價值。
【專利說明】AHRS高精度姿態數據計算方法
【技術領域】
[0001]本發明涉及一種導航領域的姿態數據計算方法，具體地說，是涉及一種AHRS高精度姿態數據計算方法。
【背景技術】
[0002]眾所周知，慣性導航主要涉及兩方面的內容:定位和定姿，定位可通過全球衛星定位系統(如GPS、北鬥等)完成；定姿主要將加速度計輸出的初始角度信息和陀螺儀輸出的三軸角速度信息通過積分計算得到當前的姿態數據(主要包括方向角、俯仰角和翻滾角)。由於加速度計和陀螺儀的精度有限，且存在累積誤差，因而所得到的姿態數據誤差較大，不能滿足技術需求。
【發明內容】
[0003]本發明的目的在於提供一種AHRS高精度姿態數據計算方法，主要解決現有技術中存在的由於加速度計和陀螺儀的精度有限，且存在累積誤差，因而所得到的姿態數據誤差較大，不能滿足技術需求問題。
[0004]為了實現上述目的，本發明採用的技術方案如下:
[0005]AHRS高精度姿態數據計算方法，包括以下步驟:
[0006](I)將AHRS中的傳感器和歸一化坐標系轉換矩陣進行初始化處理；
[0007](2)讀取AHRS中加速度計和磁力計的數據，計算載體的姿態數據和此時的坐標系轉換矩陣R1 ;
[0008](3)讀取AHRS中陀螺儀的數據，根據上一時刻的歸一化坐標系轉換矩陣計算該時刻的坐標系轉換矩陣R2 ；
[0009](4)根據坐標系轉換矩陣R1和坐標系轉換矩陣R2確定該時刻最終的坐標系轉換矩陣，並對最終的坐標系轉換矩陣進行歸一化，得到該時刻的歸一化坐標系轉換矩陣；
[0010](5)根據步驟(4)中得出的歸一化坐標系轉換矩陣求解出該時刻最終的姿態數據；
[0011](6)返回步驟(2)，進行下一時刻的姿態數據計算。
[0012]所述步驟(I)具體包括:
[0013](Ia)配置傳感器的各項參數；
[0014](Ib)根據此時加速度計和磁力計的輸出數據初始化歸一化坐標系轉換矩陣
D
normalized°
[0015]所述步驟(Ib)包括以下步驟:
[0016](Ibl)從加速度計上讀取載體坐標系在X軸向的加速度accel_x、在Y軸向的加速度accel_y、在Z軸向的加速度accel_z ;從磁力計上讀取載體坐標系在X軸向的磁場強度magnetom_x、在Y軸向的磁場強度magnetom_y、在Z軸向的磁場強度magnetom_z ；[0017](lb2)根據公式
【權利要求】
1.AHRS高精度姿態數據計算方法，其特徵在於，包括以下步驟: (1)將AHRS中的傳感器和歸一化坐標系轉換矩陣進行初始化處理； (2)讀取AHRS中加速度計和磁力計的數據，計算載體的姿態數據和此時的坐標系轉換矩陣R1 ; (3)讀取AHRS中陀螺儀的數據，根據上一時刻的歸一化坐標系轉換矩陣計算該時刻的坐標系轉換矩陣R2 ； (4)根據坐標系轉換矩陣R1和坐標系轉換矩陣R2確定該時刻最終的坐標系轉換矩陣，並對最終的坐標系轉換矩陣進行歸一化，得到該時刻的歸一化坐標系轉換矩陣； (5)根據步驟(4)中得出的歸一化坐標系轉換矩陣求解出該時刻最終的姿態數據； (6)返回步驟(2)，進行下一時刻的姿態數據計算。
2.根據權利要求1所述的AHRS高精度姿態數據計算方法，其特徵在於，所述步驟(1)具體包括: (Ia)配置傳感器的各項參數； (Ib)根據此時加速度計和磁力計的輸出數據初始化歸一化坐標系轉換矩陣RnOTmalized。
3.根據權利要求2所述的AHRS高精度姿態數據計算方法，其特徵在於，所述步驟(Ib)包括以下步驟: (Ibl)從加速度計上讀取載體坐標系在X軸向的加速度accel_X、在Y軸向的加速度accel_y、在Z軸向的加速度acce`l_z ;從磁力計上讀取載體坐標系在X軸向的磁場強度magnetom_x、在Y軸向的磁場強度magnetom_y、在Z軸向的磁場強度magnetom_z ；ctcccl X (lb2)根據公式tan(妁=—1.2 ~.2計算俯仰角Θ ;根據公式
^jaccelj + accel_z
QCCCl Vtan(妁=-^計算翻滾角Φ ;根據公式
accel_z
magjK - magnetonijK * cos(沒)+ magnctom^y * sin(^) * sin(沒)
+ magnetotn_z * cos(^) * sin(i?)
mag_\' — magnetom_y * cos(^) - magnetom—z * sin(參)計算方向角 ψ ;.、 mag V
tan(i//) =---
mag^x 相應地，所述歸一化坐標系轉換矩陣Rnmializral通過公式
cos^cosi// sin ^sin ^cosi// - cos ^ si η ψ cos ^ sin ^cos/// + sin ^sin y/
R = cos^sin/// sin^sin^sin*// + cos^cos;// cos^sin^sin/// - sin^cos?// 完成豐刀始
-sinsin 彳 cos Θcos φ cos Θ化。
4.根據權利要求3所述的AHRS高精度姿態數據計算方法，其特徵在於，所述步驟(2)具體包括以下步驟: (2a)從加速度計上讀取載體坐標系在X軸向的加速度accel_X、在Y軸向的加速度accel_y、在Z軸向的加速度accel_z ;從磁力計上讀取載體坐標系在X軸向的磁場強度magnetom_x、在Y軸向的磁場強度magnetom_y、在Z軸向的磁場強度magnetom_z ；
(2b)根據公式
5.根據權利要求4所述的AHRS高精度姿態數據計算方法，其特徵在於，所述步驟(3)具體包括以下步驟: (3a)從陀螺儀上讀取載體坐標系在X軸向的角速度ωχ、在Y軸向的角速度coy、在Z軸向的角速度ωζ ；
(3b)根據公式聲和公式
6.根據權利要求5所述的AHRS高精度姿態數據計算方法，其特徵在於，所述步驟(4)具體包括以下步驟: (4a)根據計算出的坐標系轉換矩陣R1和坐標系轉換矩陣R2計算該時刻最終的坐標系轉換矩陣R=R^X (R2-R1),其中，λ為坐標系轉換矩陣校正因子，且O≤λ ≤ 1 ； (4b)根據以下公式:
7.根據權利要求6所述的AHRS高精度姿態數據計算方法，其特徵在於，所述步驟(5)中該時刻最終的姿態數據通過該時刻的最終俯仰角Θ、翻滾角Φ和方向角Ψ確定，該時刻的最終俯仰角Θ、翻滾角Φ和方向角Ψ根據公式
【文檔編號】G01C21/20GK103499348SQ201310440327
【公開日】2014年1月8日 申請日期:2013年9月24日 優先權日:2013年9月24日
【發明者】李玉柏, 王堅, 李桓 申請人:成都市星達通科技有限公司