[發(fā)明專利]一種面向低速運(yùn)動(dòng)載體中MEMS級(jí)IMU的初始對(duì)準(zhǔn)方法

有效

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

1.一種面向低速運(yùn)動(dòng)載體中MEMS級(jí)IMU的初始對(duì)準(zhǔn)方法，其特征在于：

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

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

所述初始對(duì)準(zhǔn)方法，包括以下步驟：

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

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

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

步驟4：通過(guò)初始對(duì)準(zhǔn)開(kāi)始時(shí)刻到初始對(duì)準(zhǔn)結(jié)束時(shí)刻載體的真實(shí)位置增量向量、初始對(duì)準(zhǔn)開(kāi)始時(shí)刻到初始對(duì)準(zhǔn)結(jié)束時(shí)刻載體的位置增量向量估計(jì)值進(jìn)行計(jì)算得到航向誤差角；

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

步驟1所述初始對(duì)準(zhǔn)時(shí)刻載體的緯度為：

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

步驟1所述初始對(duì)準(zhǔn)時(shí)刻載體的經(jīng)度為：

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

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

i∈[1，N]

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

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

i∈[1，N]

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

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

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

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

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

步驟2所述計(jì)算t₀到t_k載體的真實(shí)位置增量向量為：

其中，t₀表示初始對(duì)準(zhǔn)開(kāi)始時(shí)刻，t_k表示初始對(duì)準(zhǔn)結(jié)束時(shí)刻；表示導(dǎo)航系統(tǒng)采集的載體坐標(biāo)系b系從t₀到t_k在導(dǎo)航坐標(biāo)系n系中的載體的真實(shí)位置增量向量，為導(dǎo)航系統(tǒng)采集的載體坐標(biāo)系b系在t₀的真實(shí)位置向量在導(dǎo)航坐標(biāo)系n系中的表現(xiàn)，為導(dǎo)航系統(tǒng)采集的載體坐標(biāo)系b系在t_k的真實(shí)位置向量在導(dǎo)航坐標(biāo)系n系中的表現(xiàn)；

步驟3所述計(jì)算初始對(duì)準(zhǔn)開(kāi)始時(shí)刻到初始對(duì)準(zhǔn)結(jié)束時(shí)刻載體的位置增量向量估計(jì)值為：

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

通過(guò)鏈?zhǔn)椒▌t表示為離散形式，具體定義為：

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

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

步驟4所述航向誤差角的具體計(jì)算過(guò)程為：

將初始對(duì)準(zhǔn)開(kāi)始時(shí)刻到初始對(duì)準(zhǔn)結(jié)束時(shí)刻載體的真實(shí)位置增量向量分解成三維向量形式：

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

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

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

定義n₀坐標(biāo)系由t₀的導(dǎo)航坐標(biāo)系n系確定，n₀坐標(biāo)系是一個(gè)常值坐標(biāo)系，不隨時(shí)間改變；

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

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

等式可寫(xiě)為三維游氏對(duì)準(zhǔn)模型：

因?yàn)槿S向量和第三維為零，于是重寫(xiě)為二維向量

和則三維游氏對(duì)準(zhǔn)模型可變?yōu)槎S游氏對(duì)準(zhǔn)模型：

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

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

其中表示向量?jī)?nèi)積，和表示向量的模長(zhǎng)。