[發(fā)明專利]一種基于卡爾曼濾波的IMU機(jī)載視覺姿態(tài)融合的方法

在審

• 申請?zhí)枺?/td>: 201910317443.6
• 申請日: 2019-04-19
• 公開（公告）號: CN110032201A
• 公開（公告）日: 2019-07-19
• 發(fā)明（設(shè)計(jì)）人: 熊洪睿;黃捷;馮山;韓嬋;鄒陽;王新華
• 申請（專利權(quán)）人: 成都飛機(jī)工業(yè)（集團(tuán)）有限責(zé)任公司
• 主分類號: G05D1/08
• 分類號: G05D1/08;G05D1/10
• 代理公司: 暫無信息
• 代理人: 暫無信息

1.一種基于卡爾曼濾波的IMU機(jī)載視覺姿態(tài)融合的方法，其特征在于，主要包括步驟S300：基于卡爾曼濾波的IMU/機(jī)載視覺姿態(tài)融合；采集IMU實(shí)驗(yàn)數(shù)據(jù)并經(jīng)過四元數(shù)解算得到姿態(tài)角數(shù)據(jù)，同時對圖像采集的圖片進(jìn)行處理得到相機(jī)的內(nèi)、外參數(shù)；采用卡爾曼濾波融合姿態(tài)角數(shù)據(jù)和外參數(shù)。

2.根據(jù)權(quán)利要求1所述的一種基于卡爾曼濾波的IMU機(jī)載視覺姿態(tài)融合的方法，其特征在于，所述步驟S300主要包括以下步驟：

步驟S301：IMU數(shù)據(jù)的采集頻率為200Hz；

步驟S302：通過四元數(shù)解算姿態(tài)角：

步驟S303：相機(jī)內(nèi)參數(shù)標(biāo)定，相機(jī)的內(nèi)部參數(shù)包括相機(jī)焦距f、主點(diǎn)坐標(biāo)、像素大小；采用棋盤標(biāo)定法間隙標(biāo)定；

步驟S304：相機(jī)外參數(shù)解算：外參數(shù)求解就是旋轉(zhuǎn)矩陣R與平移矩陣t的求解，然后進(jìn)行坐標(biāo)的變換；

步驟S305：將IMU數(shù)據(jù)和機(jī)載視覺數(shù)據(jù)進(jìn)行融合：以實(shí)驗(yàn)采集數(shù)據(jù)模擬IMU和視覺信號，在拍攝時，變換攝像頭姿態(tài)；相對于背景來說，有上視視角、右視視角和正面視角，采集連續(xù)的圖片及對應(yīng)的IMU數(shù)據(jù)，通過時間戳對齊，在matlab平臺以及VS2013平臺下，解算姿態(tài)角。

3.根據(jù)權(quán)利要求2所述的一種基于卡爾曼濾波的IMU機(jī)載視覺姿態(tài)融合的方法，其特征在于，所述步驟S302主要包括以下步驟：

R'＝qRq'

式中，R表示被旋轉(zhuǎn)的矢量，R'表示經(jīng)四元數(shù)旋轉(zhuǎn)之后的矢量，q表示四元數(shù)，q'表示其轉(zhuǎn)置，四元數(shù)的標(biāo)量部分表示轉(zhuǎn)角的余弦值，其矢量部分表示瞬時轉(zhuǎn)軸n的方向；下式表示矢量R相對參考坐標(biāo)系旋轉(zhuǎn)一個轉(zhuǎn)角θ，旋轉(zhuǎn)軸的方向由四元數(shù)的虛部確定，cosα、cosβ、cosγ表示旋轉(zhuǎn)軸n與參考坐標(biāo)系軸間的方向余弦值：

q＝λ+p₁i+p₂j+p₃k

式中，λ表示四元數(shù)的實(shí)部，p₁、p₂和p₃表示四元數(shù)的虛部；

四元數(shù)法姿態(tài)矩陣計(jì)算如下：

①初始四元數(shù)的確定，其輸入為初始的姿態(tài)角：

θ₀、ψ₀和γ₀代表俯仰角、偏航角、滾轉(zhuǎn)角的初始值；

②四元數(shù)標(biāo)量部分與矢量部分λ、p₁、p₂、p₃的實(shí)時計(jì)算：輸入信號為陀螺儀的數(shù)字輸出信號其中i為x、y、z，ω_ib為三軸角速率，Δθ為陀螺儀積分得到的角度增量；計(jì)算方法采用二階龍格庫塔法：

T為時間間隔，t代表當(dāng)前時刻，Ω_b(t)由陀螺儀角速率構(gòu)成的反對稱矩陣，K₁、K₂和Y是中間變量，式子表示的是下一個時刻的四元數(shù)值由上一個時刻的四元數(shù)值和一個估算的斜率的乘積所決定；

③姿態(tài)矩陣的實(shí)時計(jì)算：確定姿態(tài)矩陣輸入為λ(n)、p₁(n)、p₂(n)和p₃(n)：

式中，表示大地系轉(zhuǎn)機(jī)體系旋轉(zhuǎn)矩陣；

④載體姿態(tài)角計(jì)算，以確定姿態(tài)角θ、ψ和γ，輸入為T₁₁(n)、T₁₂(n)、T₁₃(n)、T₂₃(n)和T₃₃(n)：

θ＝-arcsin(T₁₃(n))

方向余弦法姿態(tài)矩陣的計(jì)算與四元數(shù)法的區(qū)別主要是姿態(tài)矩陣的描述不同，其描述如式所示：

其解算方向余弦矩陣微分方程為得到方向余弦矩陣后可提取姿態(tài)角；其中Ω為旋轉(zhuǎn)角速度的斜對稱矩陣。