[發(fā)明專利]一種基于慣性技術(shù)的相對姿態(tài)測量方法

有效

• 申請?zhí)枺?/td>: 201610803806.3
• 申請日: 2016-09-06
• 公開（公告）號: CN107796388B
• 公開（公告）日: 2021-03-16
• 發(fā)明（設(shè)計(jì)）人: 馬濤;朱紅;李群;李永鋒;石志興;王根;劉沖;李瑞賢;李志勇
• 申請（專利權(quán)）人: 北京自動化控制設(shè)備研究所
• 主分類號: G01C21/16
• 分類號: G01C21/16
• 代理公司: 核工業(yè)專利中心 11007
• 代理人: 呂巖甲

1.一種基于慣性技術(shù)的相對姿態(tài)測量方法，其特征在于，該方法包括以下步驟：

坐標(biāo)系定義；

b₁坐標(biāo)系：艦船慣導(dǎo)設(shè)備載體坐標(biāo)系ox_b1y_b1z_b1，前上右坐標(biāo)系，x_b1軸指向艦船的右，y_b1軸指向艦船的前，z_b1軸指向艦船的上；

b₂坐標(biāo)系：相對姿態(tài)測量裝置載體坐標(biāo)系ox_b2y_b2z_b2，前上右坐標(biāo)系，x_b2軸指向測量裝置的右，y_b2軸指向測量裝置的前，z_b2軸指向測量裝置的上；

n坐標(biāo)系：導(dǎo)航坐標(biāo)系ox_ny_nz_n，東北天地理坐標(biāo)系，x_n軸指向東，y_n軸指向北，z_n軸指向天；

b₁系、b₂系和n系的計(jì)算坐標(biāo)系分別為和

記初始時刻為t₀，建立慣性坐標(biāo)系i₁、i₂和n₀，分別由b₁和b₂坐標(biāo)系在t₀時刻的位置凝固為慣性坐標(biāo)系i₁和i₂，系在t₀時刻的位置凝固為慣性坐標(biāo)系n₀，i₁和i₂系的計(jì)算坐標(biāo)系分別為和

步驟一、確定姿態(tài)轉(zhuǎn)換矩陣，并初始化卡爾曼濾波器；

記系到i₁系的姿態(tài)轉(zhuǎn)換矩陣為系到i₂系的姿態(tài)轉(zhuǎn)換矩陣為t₀時刻的和為姿態(tài)轉(zhuǎn)換矩陣；

根據(jù)艦船慣導(dǎo)設(shè)備輸出的姿態(tài)信息，實(shí)時重新計(jì)算系到系的姿態(tài)轉(zhuǎn)換矩陣t₀時刻的姿態(tài)轉(zhuǎn)換矩陣即為卡爾曼濾波器的狀態(tài)向量X為：

式中：Φ＝[Φ_x Φ_y Φ_z]^T為t₀時刻后任一時刻t的相對姿態(tài)向量，θ為船體動態(tài)變形引起的相對姿態(tài)角向量，單位rad；為θ的微分，θ₀為t₀時刻θ的值，θ_i為由陀螺漂移引起的i₁和i₂兩慣性系的誤差歐拉角向量，單位rad；ε₀為等效常值陀螺漂移，單位rad/s；ε_r為等效陀螺隨機(jī)漂移，單位rad/s；

步驟二、對步驟一中確定的姿態(tài)轉(zhuǎn)換矩陣進(jìn)行時間更新，并更新卡爾曼濾波狀態(tài)轉(zhuǎn)移矩陣；

將由船體動態(tài)變形引起的相對姿態(tài)變化看作是二階馬爾科夫過程，則其對應(yīng)的濾波器方程可表示為：

式中：j＝x，y，z，表示式(2)為坐標(biāo)系x，y，z的方程組；μ_2j為動態(tài)不規(guī)則系數(shù)，λ_j為動態(tài)變形的支配頻率，D_j為噪聲強(qiáng)度相關(guān)參數(shù)，w_j(t)為零均值單位高斯白噪聲；將艦船慣導(dǎo)設(shè)備和相對姿態(tài)測量裝置的陀螺漂移分為常值陀螺漂移ε₀₁、ε₀₂和陀螺隨機(jī)漂移ε_r1、ε_r2兩部分；

其中：

式中：ε₀₁和ε₀₂分別為艦船慣導(dǎo)設(shè)備和相對姿態(tài)測量裝置的常值陀螺漂移，單位rad/s；和分別為ε₀₁和ε₀₂的倒數(shù)；

用一階馬爾科夫模型近似艦船主慣導(dǎo)和相對姿態(tài)測量裝置的陀螺隨機(jī)漂移ε_r1和ε_r2，則：

式中：j′＝1x，1y，1z，2x，2y，2z，表示式(4)為坐標(biāo)系1x，1y，1z，2x，2y，2z的方程組；μ_1j′為一階馬爾科夫系數(shù)，為等效陀螺隨機(jī)漂移ε_r的倒數(shù)，ε_r＝ε_r2-ε_r1；σ_j′為一階馬爾科夫過程的驅(qū)動白噪聲，w_j′(t)為零均值單位高斯白噪聲；

由于陀螺誤差的存在，和會逐漸偏離真正的和即計(jì)算坐標(biāo)系和逐漸偏離坐標(biāo)系b₁和b₂；用來分別表示系與b₁系、系與b₂系之間的姿態(tài)轉(zhuǎn)換矩陣；將其對應(yīng)的姿態(tài)角向量分別定義為θ_ε1和θ_ε2；

由于陀螺誤差的存在，和會逐漸偏離真正的和還可以視為系和系逐漸偏離i₁系和i₂系；用來分別表示系與i₁系、系與i₂系之間的姿態(tài)轉(zhuǎn)換矩陣，同樣也代表了由于陀螺的測量誤差導(dǎo)致的計(jì)算偏差，將其對應(yīng)的姿態(tài)角向量分別定義為θ_i1和θ_i2，則：

式中：θ_ε1為系偏離b₁系的相對姿態(tài)角向量，單位rad；θ_ε2為系偏離b₂系的相對姿態(tài)角向量，單位rad；θ_i1為系偏離i₁系的相對姿態(tài)角向量，單位rad；θ_i2為系偏離i₂系的相對姿態(tài)角向量，單位rad；為b₁系到i₁系的姿態(tài)轉(zhuǎn)換矩陣，為b₂系到i₂系的姿態(tài)轉(zhuǎn)換矩陣；和分別為θ_i1和θ_i2的倒數(shù)；定義：

ε₀＝ε₀₂-ε₀₁ (9)

ε_r＝ε_r2-ε_r1 (10)

θ_ε＝θ_ε2-θ_ε1 (11)

θ_i＝θ_i2-θ_i1 (12)

式中：θ_ε為b系下計(jì)算偏差導(dǎo)致相對姿態(tài)角向量，單位rad；θ_ε1為系偏離b₁系的相對姿態(tài)角向量，單位rad；θ_ε2為系偏離b₂系的相對姿態(tài)角向量，單位rad；θ_i為i系下計(jì)算偏差導(dǎo)致相對姿態(tài)角向量，單位rad；θ_i1為系偏離i₁系的相對姿態(tài)角向量，單位rad；θ_i2為系偏離i₂系的相對姿態(tài)角向量，單位rad；

綜上，得到濾波模型的狀態(tài)矩陣為：

其中：

式中：和分別為β²在坐標(biāo)系x，y，z中的分量；μ₂為動態(tài)不規(guī)則系數(shù)，λ為動態(tài)變形的支配頻率；μ_2x、μ_2y和μ_2z分別為μ₂在坐標(biāo)系x，y，z中的分量；μ_e為等效陀螺隨機(jī)漂移的一階馬爾科夫系數(shù)，μ_ex、μ_ey和μ_ez分別為μ_e在坐標(biāo)系x,y,z中的分量；I_3×3為單位陣；

濾波模型的狀態(tài)噪聲矩陣為：

其中：

式中：D為噪聲強(qiáng)度相關(guān)參數(shù)，D_x、D_y和D_z分別為D在坐標(biāo)系x，y，z中的分量；σ_e為等效陀螺隨機(jī)漂移的一階馬爾科夫過程的驅(qū)動白噪聲，σ_ex、σ_ey和σ_ez分別為σ_e在坐標(biāo)系x，y，z中的分量；則濾波模型狀態(tài)方程為：

式中：為X的倒數(shù)；F為狀態(tài)矩陣；X為狀態(tài)向量；G為狀態(tài)噪聲矩陣；w為系統(tǒng)噪聲；

步驟三、計(jì)算觀測值，并更新觀測矩陣；

根據(jù)姿態(tài)矩陣變換關(guān)系有下式成立：

式中：為t時刻系到i₁系的姿態(tài)轉(zhuǎn)換矩陣；為單位姿態(tài)轉(zhuǎn)化矩陣；由t₀時刻艦船主慣導(dǎo)姿態(tài)輸出重建得到的系到系的姿態(tài)轉(zhuǎn)換矩陣；為系到n₀系的姿態(tài)轉(zhuǎn)換矩陣，由主慣導(dǎo)輸出的位置信息和當(dāng)前經(jīng)歷的時間實(shí)時計(jì)算得到；為系到系的姿態(tài)轉(zhuǎn)換矩陣，由艦船主慣導(dǎo)輸出的姿態(tài)信息實(shí)時重建得到；為系到i₂系的姿態(tài)轉(zhuǎn)換矩陣，利用相對姿態(tài)測量裝置的陀螺儀輸出角速率對其進(jìn)行實(shí)時更新；

t時刻的相對姿態(tài)體現(xiàn)在姿態(tài)轉(zhuǎn)換矩陣上，有下列關(guān)系：

式中：為b₂系到b₁系的姿態(tài)轉(zhuǎn)換矩陣；為b₁系到i₁系的姿態(tài)轉(zhuǎn)換矩陣；為i₂系到i₁系的姿態(tài)轉(zhuǎn)換矩陣；為b₂系到i₂系的姿態(tài)轉(zhuǎn)換矩陣；

考慮到計(jì)算坐標(biāo)系與理論坐標(biāo)系的誤差，有：

式中：為b₁系到系的姿態(tài)轉(zhuǎn)換矩陣；為系到b₂系的姿態(tài)轉(zhuǎn)換矩陣；為系到i₁系的姿態(tài)轉(zhuǎn)換矩陣；為i₂系到i₁系的姿態(tài)轉(zhuǎn)換矩陣；為系到i₂系的姿態(tài)轉(zhuǎn)換矩陣；

設(shè)t時刻的相對姿態(tài)角向量為則t₀時刻的相對姿態(tài)角向量為，姿態(tài)轉(zhuǎn)換矩陣和對應(yīng)的姿態(tài)角向量分別為θ_ε1和θ_ε2，則上述姿態(tài)角度均為小角度，忽略二階以上小量應(yīng)有：

式中：I為單位陣；表示向量的反對稱矩陣；表示向量的反對稱矩陣；[θ_ε1×]表示向量θ_ε1的反對稱矩陣；[θ_ε2×]表示向量θ_ε2的反對稱矩陣；

將以式(28)至式(31)代入考慮計(jì)算坐標(biāo)系誤差的相對姿態(tài)關(guān)系矩陣關(guān)系式(27)中，有：

整理得：

式中：I為單位陣；表示向量的反對稱矩陣；

式(33)為兩個方向余弦矩陣相等，則上式(33)展開將有9個等式，由于姿態(tài)歐拉角本身的約束特性，這9個等式中只有3個是獨(dú)立的；

若姿態(tài)轉(zhuǎn)換矩陣或?qū)?yīng)的姿態(tài)歐拉角向量為θ＝[θ_x θ_y θ_z]^T，定義其姿態(tài)轉(zhuǎn)換矩陣可由θ表示，如姿態(tài)轉(zhuǎn)換矩陣表示為：

記和分別為：

選取式(33)中非對角線上元素(1,2)、(3,1)和(3,2)對應(yīng)的3個等式，建立與

的關(guān)系：

令：

則：

式中：為t時刻的相對姿態(tài)角向量，為t₀時刻的相對姿態(tài)角向量，姿態(tài)轉(zhuǎn)換矩陣和對應(yīng)的姿態(tài)角向量分別為θ_ε1和θ_ε2；

觀測矩陣為：

式中：H為觀測矩陣；為b₂系到i₂系的姿態(tài)轉(zhuǎn)換矩陣；

步驟四、進(jìn)行卡爾曼濾波計(jì)算；

對連續(xù)的卡爾曼濾波狀態(tài)方程和系統(tǒng)方程進(jìn)行離散化，通過卡爾曼濾波計(jì)算獲得狀態(tài)向量X的相關(guān)參數(shù)，獲得相對姿態(tài)的測量。