[發明專利]基于多MEMS慣性傳感器的偏航角測量方法

有效

• 申請號: 202110464133.4
• 申請日: 2021-04-26
• 公開（公告）號: CN113126642B
• 公開（公告）日: 2022-12-02
• 發明（設計）人: 陳冰冰;陽建仁;溫治強;利劍清;陳盛宏
• 申請（專利權）人: 廣東華南計算技術研究所
• 主分類號: G05D1/08
• 分類號: G05D1/08;G05D1/10;G01C21/18
• 代理公司: 廣州大象飛揚知識產權代理有限公司 44745
• 代理人: 趙娜

1.一種基于多MEMS慣性傳感器的偏航角測量方法，其特征在于，具體包括以下步驟：

S1，將多個六軸MEMS慣性傳感器分別標記并保持水平狀態，對慣性傳感器進行標定，測出準確的常值偏移、坐標軸不正交以及不對齊的誤差、標度因子誤差以及溫度誤差參數；

S2，測定六軸MEMS慣性傳感器的所有軸的噪聲方差；

S3，將初始偏航角y設為0度，并初始化姿態解算算法所需的Q值和R值，姿態解算算法的狀態變量的方差P值的初始值P(0)設為姿態解算算法中的觀測變量的方差R值的五倍，狀態值s的初始值即s(0)設為0；

S4，依次讀取六軸MEMS慣性傳感器的測量數據，并對原始測量數據進行預處理，消除常值偏移、坐標軸未對齊的誤差、標度因子誤差以及溫度誤差；

S5，采用姿態解算算法融合六軸MEMS慣性傳感器的數據，輸出載體的Z軸角速度w；

S6，讀取數據的采樣時間T，則偏航角Δy＝w*T，使用Δy更新初始偏航角y，重復S4步驟；

步驟S5采用姿態解算算法融合六軸MEMS慣性傳感器U3、U2、U4、U5、U6的數據，輸出載體的Z軸角速度w具體包括以下步驟:

S501，采集測量數據，設經過S104中對測量數據的預處理之后，U3測得的載體的Z軸角速度為g3，U2測得的載體的Z軸角速度為g2，U4測得的載體的Z軸角速度為g4，U5測得的載體的Z軸角速度為g5，U6測得的載體的Z軸角速度為g6，U2測得的其中心位置的X軸加速度為a2，U4測得的其中心位置的Y軸加速度為a4，U5測得的其中心位置的X軸加速度為a5，U6測得的其中心位置的Y軸角速度為a6；

S502，確定載體Z軸角加速度m以及卡爾曼濾波中的觀測值h，載體Z軸角加速度為m，的計算公式為m＝(-a6+a5+a4-a2)/(4*R)；對于測量系統中的所有陀螺儀來說，測量的各軸的角速度是一致的，h＝(g3+g2+g4+g5+g6)/5；

S503，進行卡爾曼濾波計算，卡爾曼濾波在整個系統中是循環運行的，設k次迭代后得到的狀態值為s(k)，k-1迭代后得到的狀態值為s(k-1)，k次迭代中先驗估計方程得到的狀態值為s(k-)，k次迭代中要使用的載體Z軸角加速度為m(k)，k次迭代后的得到的狀態變量的方差為P(k)，k次迭代中要使用的觀測值為h(k)，k-1次迭代后得到的狀態變量的方差為P(k-1)，k次迭代中先驗估計方程得到的狀態變量的方差為P(k-)，如上所述采樣時間為T，則狀態先驗估計方程為s(k-)＝s(k-1)+m(k)*T，P(k-)＝P(k-1)+Q，卡爾曼增益K的計算方程為K＝P(k-)/(P(k-)+R)，觀測方程為s(k)＝s(k-)+K*(h(k)-s(k-))，P(k)＝P(k-)-KP(k-)，s(k)為輸出的載體的Z軸角速度；

步驟S3中初始化姿態解算算法所需的Q值和R值具體包括以下步驟：

對于U2中心位置的X軸加速度、U4中心位置的Y軸加速度、U5中心位置的X軸加速度、U6中心位置的Y軸加速度，噪聲方差的最大值設為qa，采樣時間為T，則Q＝T*T*qa/(4*R*R)，同樣的，對于U3中心位置的Z軸角速度、U2中心位置的Z軸角速度、U4中心位置的Z軸角速度、U5中心位置的Z軸角速度、U6中心位置的Z軸角速度，噪聲方差的最大值設為qg，則R＝qg/5。

2.根據權利要求1所述的一種基于多MEMS慣性傳感器的偏航角測量方法，其特征在于，步驟S1中六軸MEMS慣性傳感器的個數為5，六軸MEMS慣性傳感器包括一個三軸陀螺儀和一個三軸加速度計。

3.根據權利要求2所述的一種基于多MEMS慣性傳感器的偏航角測量方法，其特征在于，步驟S1中，六軸MEMS慣性傳感器U3、U2、U4、U5、U6位于同一水平面上，其坐標系互相平行，六軸MEMS慣性傳感器U3的中心與載體的中心重合。