[發(fā)明專利]一種面向低速運動載體中MEMS級IMU的初始對準方法

有效

• 申請?zhí)枺?/td>: 202110080286.9
• 申請日: 2021-01-21
• 公開（公告）號: CN112902950B
• 公開（公告）日: 2022-10-21
• 發(fā)明（設計）人: 羅亞榮;游盛勇;胡建朗;郭遲
• 申請（專利權(quán)）人: 武漢大學
• 主分類號: G01C21/16
• 分類號: G01C21/16;G01C25/00
• 代理公司: 武漢科皓知識產(chǎn)權(quán)代理事務所(特殊普通合伙) 42222
• 代理人: 許蓮英

1.一種面向低速運動載體中MEMS級IMU的初始對準方法，其特征在于：

所述MEMS級IMU包括：微處理器、導航系統(tǒng)、IMU模塊；

所述微處理器分別與所述的導航系統(tǒng)、IMU模塊依次連接；

所述初始對準方法，包括以下步驟：

步驟1：導航系統(tǒng)獲取初始化時刻載體的緯度、初始化時刻載體的經(jīng)度，采集不同時刻載體的緯度、不同時刻載體的經(jīng)度，導航系統(tǒng)將初始化時刻載體的緯度、初始化時刻載體的經(jīng)度、不同時刻載體的緯度、不同時刻載體的經(jīng)度傳輸至微處理器；IMU模塊采集相鄰時刻的角速度增量信息、相鄰時刻的速度增量信息，IMU模塊將相鄰時刻的角速度增量信息、相鄰時刻的速度增量信息傳輸至微處理器；

步驟2：微處理器選取合適的采集時刻作為初始對準結(jié)束時刻，計算初始對準開始時刻到初始對準結(jié)束時刻載體的真實位置增量向量；

步驟3：微處理器將相鄰時刻的角度增量信息、速度增量信息通過捷聯(lián)慣性導航系統(tǒng)的機械編排算法從而獲得初始對準結(jié)束時刻載體的緯度估計值，初始對準結(jié)束時刻載體的經(jīng)度估計值，計算初始對準開始時刻到初始對準結(jié)束時刻載體的位置增量向量估計值；

步驟4：通過初始對準開始時刻到初始對準結(jié)束時刻載體的真實位置增量向量、初始對準開始時刻到初始對準結(jié)束時刻載體的位置增量向量估計值進行計算得到航向誤差角；

步驟5：微處理器將步驟4中獲得的航向誤差角反饋回捷聯(lián)慣性導航系統(tǒng)，修正初始的導航姿態(tài)，從而實現(xiàn)導航系統(tǒng)的初始對準；

步驟1所述初始對準時刻載體的緯度為：

其中，表示載體在t₀時刻的緯度；

步驟1所述初始對準時刻載體的經(jīng)度為：

其中，表示載體在t₀時刻的經(jīng)度；

步驟1所述不同時刻載體的緯度為：

i∈[1，N]

其中，表示載體在t_i時刻的緯度，N表示采集時刻的數(shù)量；

步驟1所述不同時刻載體的經(jīng)度為：

i∈[1，N]

其中，表示載體在t_i時刻的經(jīng)度，N表示采集時刻的數(shù)量；

步驟1所述相鄰時刻的角速度增量信息為：

其中，表示載體從t_i-1到t_i時刻的角速度增量信息；

步驟1所述相鄰時刻的速度增量信息為：

其中，表示載體從t_i-1到t_i時刻的速度增量信息；

步驟2所述計算t₀到t_k載體的真實位置增量向量為：

其中，t₀表示初始對準開始時刻，t_k表示初始對準結(jié)束時刻；表示導航系統(tǒng)采集的載體坐標系b系從t₀到t_k在導航坐標系n系中的載體的真實位置增量向量，為導航系統(tǒng)采集的載體坐標系b系在t₀的真實位置向量在導航坐標系n系中的表現(xiàn)，為導航系統(tǒng)采集的載體坐標系b系在t_k的真實位置向量在導航坐標系n系中的表現(xiàn)；

步驟3所述計算初始對準開始時刻到初始對準結(jié)束時刻載體的位置增量向量估計值為：

其中，t₀表示初始對準開始時刻，t_k表示初始對準結(jié)束時刻；是捷聯(lián)慣性導航系統(tǒng)的機械編排算法獲取的載體坐標系b系從t₀到t_k在導航坐標系n系中的載體的位置增量向量估計值；定義系為載體橫滾角和俯仰角為零時確定的b系，表示t₀時系到載體坐標系b系的姿態(tài)旋轉(zhuǎn)矩陣估計值；表示t_k時系到載體坐標系b系的姿態(tài)旋轉(zhuǎn)矩陣估計值；表示系到地心的地球半徑長度；是捷聯(lián)慣性導航系統(tǒng)解算得到的載體坐標系b系從t₀到t_k的姿態(tài)變化矩陣估計值；

通過鏈式法則表示為離散形式，具體定義為：

由羅德里格斯旋轉(zhuǎn)公式和“單子樣+前一周期”算法可得相鄰時刻載體的姿態(tài)變化矩陣為：

其中，為t_k-1的角度增量；為t_k的角度增量；為b系從t_k-1到t_k的等效旋轉(zhuǎn)矢量；是的模長；是的單位方向矢量；是單位方向矢量的反對稱矩陣；

步驟4所述航向誤差角的具體計算過程為：

將初始對準開始時刻到初始對準結(jié)束時刻載體的真實位置增量向量分解成三維向量形式：

其中，t₀表示初始對準開始時刻，t_k表示初始對準結(jié)束時刻；是載體的真實位置增量向量在正北方向的位置增量分量；是載體的真實位置增量向量在正東方向的位置增量分量；是載體的真實位置增量向量在豎直地向的位置增量分量；

將t₀到t_k載體的位置增量向量分解成三維向量形式：

其中，是載體的位置增量向量估計值在正北方向的位置增量分量；是載體的位置增量向量估計值在正北東方向的位置增量分量；是載體的位置增量向量估計值在豎直地向的位置增量分量；

定義n₀坐標系由t₀的導航坐標系n系確定，n₀坐標系是一個常值坐標系，不隨時間改變；

將通過正交投影方法投影在n₀系XY平面上，可得t₀到t_k載體的真實位置增量向量在n₀系XY平面上投影為：

將通過正交投影方法投影在n₀系XY平面上，可得t₀到t_k載體的位置增量向量估計值在n₀系XY平面上投影為：

等式可寫為三維游氏對準模型：

因為三維向量和第三維為零，于是重寫為二維向量

和則三維游氏對準模型可變?yōu)槎S游氏對準模型：

根據(jù)二維游氏對準模型可以求出航向誤差角Δψ，從而完成捷聯(lián)慣性導航系統(tǒng)的初始對準過程；

航向誤差角Δψ還可以由余弦定理：

其中表示向量內(nèi)積，和表示向量的模長。