[發明專利]一種基于微慣性和地磁技術的自適應三維姿態定位方法有效
| 申請號： | 201010231211.8 | 申請日： | 2010-07-14 |
| 公開（公告）號： | CN101915580A | 公開（公告）日： | 2010-12-15 |
| 發明（設計）人： | 杜清秀;弭鵬 | 申請（專利權）人： | 中國科學院自動化研究所 |
| 主分類號： | G01C21/16 | 分類號： | G01C21/16;G01C21/08;G01C21/20 |
| 代理公司： | 中科專利商標代理有限責任公司 11021 | 代理人： | 梁愛榮 |
| 地址： | 100080 *** | 國省代碼： | 北京;11 |
| 權利要求書： | 查看更多 | 說明書： | 查看更多 |
| 摘要： | |||
| 搜索關鍵詞： | 一種 基于 慣性 地磁 技術 自適應 三維 姿態 定位 方法 | ||
1.一種基于微慣性和地磁技術的自適應三維姿態定位方法，其特征在于，包括以下步驟：
步驟S1：利用集成了三軸微陀螺傳感器、三軸微加速度傳感器和三軸磁場傳感器的微慣性組合測量裝置來感應運動載體的運動姿態：三軸微陀螺傳感器感應運動載體沿軸向的角速度信號，三軸微加速度傳感器感應運動載體加速度信號，三軸磁場傳感器感應地球磁場信號；
步驟S2：設置微慣性組合測量裝置的初始姿態值和全局坐標系下的三軸微加速度傳感器的加速度數據和三軸磁場傳感器的地球磁場數據：在確保三軸微陀螺傳感器、三軸微加速度傳感器和三軸磁場傳感器坐標系對準的情況下，保持微慣性組合測量裝置固定不動，采集三軸微加速度傳感器的加速度數據和三軸磁場傳感器感應的地球磁場數據；設置微慣性組合測量裝置初始時刻即i＝0時的四元數形式姿態值q0＝[1?0?0?0]T、三軸微加速度傳感器矢量nYA＝[a0x?a0y?a0z]T和三軸磁場傳感器矢量nYM＝[m0xm0ym0z]T，對三軸微加速度傳感器矢量nYA和三軸磁場傳感器矢量nYM進行單位化，得到單位三軸微加速度傳感器矢量nSA和單位三軸磁場傳感器矢量nSM，所述單位化公式為：
nSA＝nYA/||nYA||和nSM＝nYM/||nYM||，
式中，a0x、a0y、a0z表示初始時刻即i＝0時的全局坐標系下的三軸微加速度傳感器的加速度數據，m0x、m0y、m0z表示初始時刻即i＝0時的全局坐標系下的是三軸磁場傳感器的地球磁場數據，下標0表示的是初始時刻，下標x、y、z表示的是每個傳感器坐標系下的三個坐標軸；上標n表示的是全局坐標系下的傳感器數據、下標A和M分別表示的是三軸微加速度傳感器和三軸磁場傳感器的數據；S表示的是單位化后相應傳感器數據；上標T表示矢量的轉置；
步驟S3：對微慣性組合測量裝置的姿態值進行求解：利用當前時刻即i＝t時的三軸微陀螺傳感器的角速度數據、三軸微加速度傳感器的加速度數據和三軸磁場傳感器的地球磁場數據，當前時刻t的姿態值qt以及步驟2獲得的初始姿態值q0、三軸微加速度傳感器的加速度數據nYA、nSA和三軸磁場傳感器的地球磁場數據nYM、nSM，來獲取微慣性組合測量裝置下一時刻即i＝t+Δt時的姿態值qt+Δt；
步驟S4：利用三軸微陀螺傳感器數據對i＝t+Δt時刻微慣性組合測量裝置的姿態值進行預測：將當前時刻t的三軸微陀螺傳感器的角速度數據表示為當前姿態下的旋轉角速度四元數QG＝[0?ωtx?ωty?ωtz]T，當前時刻t的三軸微加速度傳感器的加速度數據和三軸磁場傳感器的磁場強度數據分別為bYA＝[atx?aty?atz]T和bYM＝[mtx?mty?mtz]T，其中G、b為標記號，ωtx、ωty、ωtz表示的是當前時刻t的三軸陀螺傳感器的角速度數據，atx、aty、atz表示的是當前時刻t的三軸微加速度傳感器的加速度數據，mtx、mty、mtz表示的是當前時刻t的三軸磁場傳感器的地球磁場數據；
利用微慣性組合測量裝置當前時刻t的姿態值qt和當前時刻t的三軸微陀螺傳感器的旋轉角速度四元數QG，對i＝t+Δt時刻的微慣性組合測量裝置的姿態值進行預測，由于兩個時刻之間的間隔Δt較小，假設認為t和t+Δt兩個時刻之間三軸微陀螺傳感器輸出沒有變化，從而獲得姿態預測方程：
式中
對預測后的四元數進行單位化：
式中，qt為當前時刻t的微慣性組合測量裝置的姿態值，為下一時刻預測姿態值，Δt為采樣時間間隔，為四元數形式的當前時刻t全局姿態下的旋轉角速度；
步驟S5：對步驟S4獲得的三軸微加速度傳感器的加速度數據和三軸磁場傳感器地球磁場數據進行置信判斷，判定周圍環境是否存在電磁場干擾或瞬時加速度干擾；若干擾過大，則放棄該時刻采集的三軸磁場傳感器的地球磁場數據或三軸微加速度傳感器的加速度數據，否則以測得的周圍環境噪聲的大小設置自適應因子，該自適應因子是表征不存在較大的電磁干擾或是瞬時加速度情況下的三軸微加速度傳感器和三軸磁場傳感器的噪聲大小，從而基于該自適應因子對三軸微加速度傳感器的加速度數據或是三軸磁場傳感器的地球磁場數據的影響進行權重設置如下：
式中，bSM為調整后的三軸磁場傳感器矢量、bSA為調整后的三軸微加速度傳感器矢量；kA為三軸微加速度傳感器自適應因子，kM為三軸磁場傳感器自適應因子；εA為三軸微加速度傳感器噪聲上界，εM為三軸磁場傳感器噪聲上界；所述干擾過大是超過公式(4)中的三軸微加速度傳感器噪聲上界εA或三軸磁場傳感器噪聲上界εM，εA、εM的取值范圍是(0-1)，周圍環境噪聲越大，該噪聲上界εA、εM值應取值越小，具體取值應依據在現場環境中的實驗決定；
步驟S6：利用步驟S5調整后的三軸微加速度傳感器的加速度數據和三軸磁場傳感器的地球磁場數據獲得微慣性組合測量裝置的姿態誤差值該姿態誤差值通過高斯-牛頓方法獲得：
式中：
式中：λ表示為μ表示為λx、λy、λz為向量λ的三個分量，μx、μy、μz為向量μ的三個分量，q0、q1、q2、q3為四元數的四個分量，為四元數轉換成的旋轉矩陣；矩陣Ξ只與初始標定時刻的三軸微加速度傳感器的加速度數據和三軸磁場傳感器的地球磁場數據有關，所以在姿態方法中只要進行一次標定，獲取Ξ矩陣及其逆矩陣，無需在其他時刻進行矩陣逆運算；
步驟S7：基于步驟S4得到的姿態預測信息、步驟S6得到微慣性組合測量裝置的姿態誤差值和步驟S5得到的自適應因子進行融合，獲得t+Δt時刻微慣性組合測量裝置姿態值：
然后再對其單位化qt+Δt＝qt+Δt/||qt+Δt||；其中表示的是四元數的乘法運算；
步驟S8：輸出姿態信息：根據四元數和歐拉角之間的轉換關系，將四元數轉換為具有直觀意義的、自適應三維姿態的俯仰角α、橫滾角β和航向角γ如下：
該專利技術資料僅供研究查看技術是否侵權等信息，商用須獲得專利權人授權。該專利全部權利屬于中國科學院自動化研究所，未經中國科學院自動化研究所許可，擅自商用是侵權行為。如果您想購買此專利、獲得商業授權和技術合作，請聯系【客服】
本文鏈接：http://www.szxzyx.cn/pat/books/201010231211.8/1.html，轉載請聲明來源鉆瓜專利網。
