本發(fā)明涉及一種基于慣性測量匹配的非線性撓曲變形估計(jì)方法，可用于飛機(jī)、艦船以及車輛的撓曲變形測量及撓曲變形角測量。

背景技術(shù)：

船體、機(jī)體的撓曲變形使得各個(gè)節(jié)點(diǎn)處的運(yùn)動參數(shù)存在較大差異，如不對撓曲變形進(jìn)行測量和補(bǔ)償，這種差異將嚴(yán)重影響各個(gè)節(jié)點(diǎn)處運(yùn)動參數(shù)的測量精度。因此要實(shí)現(xiàn)多個(gè)節(jié)點(diǎn)處運(yùn)動參數(shù)的高精度測量，必須對撓曲變形進(jìn)行測量和補(bǔ)償。

撓曲變形的測量方法主要有兩類，一類是基于光學(xué)傳感器的光學(xué)測量法，另一類是基于分布式位置姿態(tài)測量系統(tǒng)(Position and Orientation System,POS)的測量法。光學(xué)測量法系統(tǒng)安裝復(fù)雜，且要求光束收發(fā)無阻斷，容易受天氣等外部因素影響。分布式POS由一個(gè)主POS和多個(gè)慣性測量單元(Inertial Measurement Unit，IMU)(子IMU)組成，主POS的GPS天線和主IMU通過濾波的方法實(shí)現(xiàn)慣性/衛(wèi)星組合導(dǎo)航，子IMU通過與主POS進(jìn)行傳遞對準(zhǔn)測量子節(jié)點(diǎn)運(yùn)動參數(shù)。基于分布式POS的測量法需要建立包含主、子系統(tǒng)姿態(tài)誤差、速度誤差、位置誤差、慣性器件誤差、安裝誤差、撓曲變形等多種誤差方程，并進(jìn)行包含主子系統(tǒng)捷聯(lián)解算、主系統(tǒng)慣性/衛(wèi)星組合導(dǎo)航、子系統(tǒng)傳遞對準(zhǔn)等多種復(fù)雜運(yùn)算，且需要引入GPS，增加了變形測量系統(tǒng)的成本。

技術(shù)實(shí)現(xiàn)要素：

本發(fā)明要解決的技術(shù)的問題是：針對現(xiàn)有技術(shù)的不足，提供了一種基于慣性測量匹配的非線性撓曲變形估計(jì)方法，在充分利用慣性測量數(shù)據(jù)的同時(shí)，既降低了測量系統(tǒng)成本，又減小了運(yùn)算量。

本發(fā)明解決上述技術(shù)問題采用的技術(shù)方案為：一種基于慣性測量匹配的非線性撓曲變形估計(jì)方法，具體步驟如下：

步驟(1)、建立包括安裝誤差角、撓曲變形、撓曲變形一階導(dǎo)、撓曲變形角、撓曲變形角一階導(dǎo)、加速度計(jì)常值和隨機(jī)偏置以及陀螺儀常值和隨機(jī)漂移的非線性系統(tǒng)狀態(tài)方程；

步驟(2)、將主、子節(jié)點(diǎn)的加速度計(jì)測量值之差和陀螺儀測量值之差作為量測，建立系統(tǒng)的非線性系統(tǒng)測量方程；

步驟(3)、采用非線性濾波方法——中心差分卡爾曼濾波估計(jì)方法，估計(jì)出t_k時(shí)刻子節(jié)點(diǎn)處的撓曲變形和撓曲變形角，k＝1,2,…,N，不斷重復(fù)本步驟，直至主子節(jié)點(diǎn)慣性測量數(shù)據(jù)結(jié)束。

本發(fā)明的原理：在船體、機(jī)體、船體等的各測量節(jié)點(diǎn)處安裝IMU，利用撓曲變形引起的各節(jié)點(diǎn)處慣性測量量的差異，建立包括子節(jié)點(diǎn)安裝誤差角、撓曲變形、撓曲變形角、主子節(jié)點(diǎn)加速度計(jì)常值和隨機(jī)偏置以及主子節(jié)點(diǎn)陀螺儀常值和隨機(jī)漂移的非線性系統(tǒng)狀態(tài)方程，以及包含主、子節(jié)點(diǎn)加速度計(jì)測量值之差和陀螺儀測量值之差的非線性量測方程，最后通過非線性濾波方法估計(jì)撓曲變形和撓曲變形角。

本發(fā)明與現(xiàn)有技術(shù)相比的優(yōu)點(diǎn)在于：本發(fā)明彌補(bǔ)了基于分布式POS的測量法模型復(fù)雜、運(yùn)算量大以及成本高的不足，利用各節(jié)點(diǎn)處IMU測量量即可實(shí)現(xiàn)撓曲變形測量，且建立的系統(tǒng)非線性狀態(tài)方程和量測方程準(zhǔn)確，計(jì)算精度高。

附圖說明

圖1為基于慣性測量匹配的非線性撓曲變形估計(jì)方法流程圖；

圖2為撓曲變形示意圖，其中，地球慣性坐標(biāo)系為O_ix_iy_iz_i，主IMU坐標(biāo)系為O_mx_my_mz_m，初始時(shí)刻子IMU坐標(biāo)系為O_s0x_s0y_s0z_s0，主子節(jié)點(diǎn)之間桿臂r₀，動態(tài)過程中子IMU坐標(biāo)系為O_sx_sy_sz_s，主子節(jié)點(diǎn)之間桿臂為r，對應(yīng)的撓曲變形為△r，撓曲變形角為θ，R_m和R_s分別表示動態(tài)過程中O_ix_iy_iz_i坐標(biāo)系下主子節(jié)點(diǎn)的位置矢量；

圖3為x軸方向的撓曲角以及y軸方向的初始桿臂引起的撓曲變形示意圖，其中，θ_x為x軸方向撓曲角，線段O_mA表示初始時(shí)刻主IMU坐標(biāo)系下y軸方向的桿臂分量，桿臂經(jīng)過撓曲變形后為圓弧O_mA′。

具體實(shí)施方式

下面結(jié)合附圖以及具體實(shí)施方式進(jìn)一步說明本發(fā)明。

如說明書附圖1所示，本發(fā)明的具體實(shí)施包括以下步驟：

1、建立包括安裝誤差角、撓曲變形、撓曲變形一階導(dǎo)、撓曲變形角、撓曲變形角一階導(dǎo)、加速度計(jì)常值和隨機(jī)偏置以及陀螺儀常值和隨機(jī)漂移的非線性系統(tǒng)狀態(tài)方程，具體為：

(1)建立固定安裝誤差角數(shù)學(xué)模型

如附圖1系統(tǒng)撓曲變形示意圖中所示，相關(guān)坐標(biāo)系包括：地球慣性坐標(biāo)系O_ix_iy_iz_i；主節(jié)點(diǎn)處慣性測量單元(Inertial Measurement Unit,IMU)坐標(biāo)系O_mx_my_mz_m；子節(jié)點(diǎn)處IMU坐標(biāo)系O_sx_sy_sz_s。

子節(jié)點(diǎn)固定安裝誤差角在系統(tǒng)安裝之后就已確定且為常值，其數(shù)學(xué)模型為：

其中，μ＝[μ_x μ_y μ_z]^T為子節(jié)點(diǎn)相對主節(jié)點(diǎn)的固定安裝誤差角，μ_x、μ_y和μ_z表示子IMU坐標(biāo)系O_sx_sy_sz_s的x軸、y軸和z軸的安裝誤差角；

(2)彈性變形角數(shù)學(xué)模型

子節(jié)點(diǎn)處的彈性變形角θ的變化符合二階馬爾科夫過程，其微分方程為：

式中，θ＝[θ_x θ_y θ_z]^T，θ_i表示子IMU坐標(biāo)系O_sx_sy_sz_s的i軸方向的彈性變形角，β_i＝2.146/τ_i，τ_i為二階馬爾科夫過程相關(guān)時(shí)間，η_i為白噪聲，其方差滿足其中為彈性變形角θ_i的方差，β_i和為描述彈性變形角的二階馬爾科夫過程參數(shù)；

(3)彈性變形數(shù)學(xué)模型

如附圖1系統(tǒng)撓曲變形示意圖中所示，相關(guān)坐標(biāo)系包括：地球慣性坐標(biāo)系O_ix_iy_iz_i；主節(jié)點(diǎn)處慣性測量單元(Inertial Measurement Unit,IMU)坐標(biāo)系O_mx_my_mz_m；子節(jié)點(diǎn)處IMU坐標(biāo)系O_sx_sy_sz_s。初始時(shí)刻主子節(jié)點(diǎn)之間桿臂在主IMU坐標(biāo)系O_mx_my_mz_m表示為撓曲角為θ₀＝0_3×1，撓曲變形動態(tài)過程中，主子節(jié)點(diǎn)之間桿臂為r，對應(yīng)的撓曲變形為△r，撓曲變形角為θ。附圖2為x軸方向的撓曲角分量θ_x以及y軸方向的初始桿臂值分量r_y0引起的撓曲變形示意圖。線段O_mA表示初始時(shí)刻主IMU坐標(biāo)系下y軸方向的桿臂分量，撓曲變形后為圓弧O_mA′，且圓弧O_mA′和線段O_mA的長度均為r_y0。由此線段AB即為y軸方向撓曲變形量線段O_mC即為z軸方向撓曲變形量其表達(dá)式為：

同理可得θ_x和r_z0引起y軸方向撓曲變形量和z軸方向的撓曲變形量的表達(dá)式為：

則θ_x引起的y軸方向的撓曲變形和z軸方向的撓曲變形表達(dá)式為：

同理可得，θ_y引起的x軸方向的撓曲變形和z軸方向的撓曲變形θ_z引起的x軸方向的撓曲變形和y軸方向的撓曲變形

最終得到撓曲變形的表達(dá)式為：

對上式進(jìn)行求導(dǎo)即可得到表達(dá)式：

對上式再進(jìn)行求導(dǎo)，并將撓曲角的二階表達(dá)式帶入其中即可得到撓曲變形的二階導(dǎo)數(shù)的表達(dá)式：

(4)加速度計(jì)常值和隨機(jī)偏置數(shù)學(xué)模型

由于主系統(tǒng)的慣性器件精度要高于子系統(tǒng)，因此在建立狀態(tài)方程式不考慮主系統(tǒng)的加速度計(jì)誤差，則子IMU加速度計(jì)常值偏置微分方程為：

式中為子節(jié)點(diǎn)加速度計(jì)常值偏置，分別為子IMU坐標(biāo)系下的分量。

子IMU加速度計(jì)隨機(jī)偏置由一階馬爾科夫過程表示：

式中，為一階馬爾科夫過程參數(shù)，γ_si為白噪聲。

(5)陀螺儀常值和隨機(jī)漂移數(shù)學(xué)模型

子IMU陀螺儀常值漂移微分方程為：

式中ε_s＝[ε_sx ε_sy ε_sz]^T為子節(jié)點(diǎn)陀螺儀常值漂移，ε_sx，ε_sy，ε_mz分別為子IMU坐標(biāo)系下的分量。

子IMU陀螺儀隨機(jī)漂移由一階馬爾科夫過程表示：

式中，κ_si為一階馬爾科夫過程參數(shù)，λ_si為白噪聲。

(6)非線性系統(tǒng)狀態(tài)方程建立

系統(tǒng)的狀態(tài)量為X＝[X₁ X₂ X₃ X₄]^T，其中包括9維安裝誤差角和撓曲變形角6維撓曲變形量6維子系統(tǒng)加速度計(jì)誤差變量以及6維子系統(tǒng)陀螺儀誤差變量X₄＝[ε_sx ε_sy ε_sz ε′_sx ε′_sy ε′_sz]^T。

系統(tǒng)狀態(tài)方程的表達(dá)形式如下：

式中，X(t)＝[X₁(t) X₂(t) X₃(t) X₄(t)]^T為系統(tǒng)狀態(tài)量；為系統(tǒng)噪聲，非線性函數(shù)F[X(t),t]的表達(dá)式可根據(jù)(1)-(5)得到。G表達(dá)式如下：

2、將主、子節(jié)點(diǎn)的加速度計(jì)測量值之差和陀螺儀測量值之差作為量測，建立系統(tǒng)的非線性系統(tǒng)測量方程，具體過程如下：

(1)陀螺儀輸出匹配量測方程

主IMU陀螺儀輸出值和子IMU陀螺儀輸出值之間的非線性關(guān)系可以表示為：

式中，分別為主子IMU的陀螺漂移，包括常值漂移和隨機(jī)漂移兩部分。為撓曲角變化率，為主IMU與子IMU之間的坐標(biāo)轉(zhuǎn)換矩陣，其表達(dá)式可以表示為：

由此得到陀螺儀輸出匹配的非線性量測方程：

(2)加速度計(jì)輸出匹配量測方程

撓曲桿臂產(chǎn)生的加速度表達(dá)式為：

式中，為主系統(tǒng)陀螺儀輸出角速度，r^m、和分別為主IMU坐標(biāo)系下?lián)锨鷹U臂及其一階導(dǎo)數(shù)和二階導(dǎo)數(shù)，表達(dá)式已經(jīng)在步驟2中給出。

主IMU加速度計(jì)輸出值和子IMU加速度計(jì)輸出值之間的非線性關(guān)系可以表示為：

由此得到加速度計(jì)輸出匹配的非線性量測方程：

(3)非線性系統(tǒng)量測方程建立

系統(tǒng)的量測量為Z＝[Z_Ω Z_f]^T，其中包括3維主、子節(jié)點(diǎn)陀螺儀輸出匹配量Z_Ω和3維主、子節(jié)點(diǎn)加速度計(jì)輸出匹配量Z_f。

系統(tǒng)量測方程的表達(dá)形式如下：

Z(t)＝H(X,t)+v(t)

式中為量測噪聲，其中為加速度計(jì)量測噪聲，為陀螺儀量測噪聲，非線性函數(shù)H(X,t)的表達(dá)式可根據(jù)(1)-(2)得到。