本發(fā)明涉及一種旋翼擾動下慣性/GPS組合導航外桿臂效應誤差補償方法，屬于組合導航系統(tǒng)測量技術(shù)領域。

背景技術(shù)：

無人直升機的高機動、多模態(tài)的飛行特點對導航系統(tǒng)的精度和可靠性提出了極高的要求。由于無人直升機的主旋翼通常安裝在機體中心位置，如果按照傳統(tǒng)固定翼無人機的天線安裝方式，在機體中心的上表面位置安裝GPS天線，那么GPS導航信號到GPS天線間的傳播路徑會受到旋翼的周期性切割，導致接收機不能對其載波相位進行穩(wěn)定跟蹤鎖定，大大減少了GPS信號接收的有效時間。因此，為了盡可能避免旋翼擾動的干擾，GPS天線的安裝位置通常要遠離旋翼擾動的位置，例如安裝在機頂、尾槳上端等位置。然而，SINS一般安裝在機艙內(nèi)，這就導致了兩者安裝位置的不重合，輸出的導航信息也就存在了安裝坐標系的差異，當無人直升機飛行中存在角度運動時，會產(chǎn)生外桿臂效應誤差。因此，非常有必要對旋翼擾動下慣性/GPS組合導航系統(tǒng)外桿臂效應誤差進行估計和補償。

對于外桿臂效應誤差，理想環(huán)境下，當無人直升機平飛時，SINS(捷聯(lián)慣導系統(tǒng))的速度與和加速度相位中心和GPS相同，SINS的位置也可以簡單的通過GPS天線中心的位置加上外桿臂距離求得；但是考慮到無人直升機旋翼轉(zhuǎn)動帶來的機體振動，以及機體本身存在的角度運動，無法以簡單的位置矢量疊加的形式獲得GPS測量的飛行器質(zhì)心的位置，從而產(chǎn)生外桿臂效應誤差。針對外桿臂效應誤差的相關研究算法比較少，因此，研究在旋翼擾動產(chǎn)生的機體振動環(huán)境下的慣性/GPS組合導航系統(tǒng)外桿臂效應誤差補償方法，從而提高無人直升機飛行過程中的導航系統(tǒng)精度，將具有突出的應用價值。

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

本發(fā)明提出了一種旋翼擾動下慣性/GPS組合導航外桿臂效應誤差補償方法，對外桿臂誤差進行建模，將外桿臂效應誤差擴展為系統(tǒng)狀態(tài)量，建立包含外桿臂效應誤差的濾波狀態(tài)方程和量測方程，基于卡爾曼濾波器實現(xiàn)了對外桿臂效應誤差的在線估計和補償。該方法能實現(xiàn)對外桿臂效應誤差的估計及補償，進一步提高慣性/GPS組合導航系統(tǒng)精度。

本發(fā)明為解決其技術(shù)問題采用如下技術(shù)方案：

一種旋翼擾動下慣性/GPS組合導航外桿臂效應誤差補償方法，包括以下步驟：

步驟1，分析外桿臂效應誤差原理，建立無人直升機旋翼擾動下的外桿臂效應誤差模型；

步驟2，在步驟1對無人直升機旋翼擾動下的外桿臂效應誤差建模的基礎上，建立包含外桿臂效應誤差的濾波狀態(tài)方程和量測方程；

步驟3，根據(jù)步驟2建立的濾波狀態(tài)方程和量測方程，利用GPS和慣導系統(tǒng)輸出信息，實現(xiàn)對外桿臂效應誤差的估計和補償。

步驟1所述的無人直升機旋翼擾動下的外桿臂效應誤差模型為：

ΔL＝r^b

ΔV＝ω×r^b

其中，r^b為機體坐標系下的外桿臂距離，定義為機體坐標系中GPS天線和SINS位置差；ΔL為機體坐標系下外桿臂效應引起的桿臂誤差，ΔV為機體坐標系下外桿臂效應引起的速度偏差，Δa為機體坐標系下外桿臂效應引起的加速度偏差；ω為機體的角速率，ω×(ω×r^b)為向心加速度，為切向加速度，同時有：

其中，為機體坐標系下的地球自轉(zhuǎn)角速率，為導航坐標系相對地球坐標系的角速度在機體坐標系上的分量，為機體坐標系相對于導航坐標系轉(zhuǎn)動的角速度；

步驟2所述的包含外桿臂效應誤差的濾波狀態(tài)方程為：

其中，X(t)為系統(tǒng)狀態(tài)變量，F(xiàn)(t)為系統(tǒng)矩陣，G(t)系統(tǒng)狀態(tài)噪聲系數(shù)矩陣，W(t)為系統(tǒng)噪聲矩陣，為系統(tǒng)狀態(tài)變量一階微分，濾波狀態(tài)方程表示為：

其中，φ＝[φ_E φ_N φ_U]^T為數(shù)學平臺誤差角，φ_E為東向平臺誤差角，φ_N為北向平臺誤差角，φ_U為天向平臺誤差角，δV＝[δV_E δV_N δV_U]^T為速度誤差，δV_E為東向速度誤差，δV_N為北向速度誤差，δV_U為天向速度誤差，δP＝[δL δλ δh]^T為位置誤差，δL為緯度位置誤差，δλ為經(jīng)度位置誤差，δh為高度位置誤差，δr＝[δr_x δr_y δr_z]^T為外桿臂誤差，δr_x為外桿臂誤差在X軸上的分量，δr_y為外桿臂誤差在y軸上的分量，δr_z為外桿臂誤差在z軸上的分量；為數(shù)學平臺誤差角變化量，為速度誤差變化量，為位置誤差變化量，為外桿臂誤差變化量，w_a＝[w_ax w_ay w_az]^T為加速度計白噪聲誤差，w_ax為X軸加速度計白噪聲誤差，w_ay為y軸加速度計白噪聲誤差，w_az為z軸加速度計白噪聲誤差，w_g＝[w_gx w_gy w_gz]^T為陀螺儀白噪聲誤差，w_gx為x軸陀螺儀白噪聲誤差，w_gy為y軸陀螺儀白噪聲誤差，w_gz為z軸陀螺儀白噪聲誤差。同時有：

其中，ω_ie為地球自轉(zhuǎn)角速率，V_E為速度誤差在導航坐標系中東向的分量，V_N為速度誤差在導航坐標系中北向的分量，V_U為速度誤差在導航坐標系中天向的分量，f_E為比力在導航坐標系中東向的分量，f_N為比力在導航坐標系中北向的分量，f_U為比力在導航坐標系中天向的分量，R為地球半徑，R_M為地球的子午圈曲率半徑，R_N為地球卯酉圈曲率半徑，L為無人直升機所處的緯度，h為無人直升機所處的高度。

步驟2所述的包含外桿臂效應誤差的濾波量測方程為：

Z(t)＝H(t)X(t)+V(t)

其中，H(t)為量測系數(shù)矩陣，V(t)為量測噪聲矩陣，Z(t)為量測狀態(tài)信息，量測方程表示為：

其中，[r^b×]和分別表示矢量r^b和的反對稱矩陣，v為量測噪聲矩陣，同時有：

其中，γ為無人直升機的橫滾角、θ為無人直升機的俯仰角，ψ為無人直升機的航向角。

量測信息由GPS和SINS的位置、速度誤差構(gòu)成，表示為：

式(18)中，P_GPS是導航坐標系中GPS天線位置，P_SINS是導航坐標系中SINS位置，V_GPS是導航坐標系中GPS天線速度，V_SINS是導航坐標系中SINS速度。

本發(fā)明具有以下有益效果：

本發(fā)明通過對外桿臂誤差進行建模，將外桿臂效應誤差擴展為系統(tǒng)狀態(tài)量，建立包含外桿臂效應誤差的濾波狀態(tài)方程和量測方程，能夠基于卡爾曼濾波器實現(xiàn)對外桿臂效應誤差的在線估計和補償，進一步提高慣性/GPS組合導航系統(tǒng)精度。

附圖說明

圖1是外桿臂效應誤差原理圖。

圖2(a)為綜合振動環(huán)境下外桿臂長度在X軸投影的估計結(jié)果與真實值對比圖；

圖2(b)為綜合振動環(huán)境下外桿臂長度在Y軸投影的估計結(jié)果與真實值對比圖；

圖2(c)為綜合振動環(huán)境下外桿臂長度在Z軸投影的估計結(jié)果與真實值對比圖。

圖3(a)為未經(jīng)外桿臂效應誤差濾波補償與經(jīng)補償修正的組合導航系統(tǒng)橫滾角誤差對比圖；圖3(b)為未經(jīng)外桿臂效應誤差濾波補償與經(jīng)補償修正的組合導航系統(tǒng)俯仰角誤差對比圖；圖3(c)為未經(jīng)外桿臂效應誤差濾波補償與經(jīng)補償修正的組合導航系統(tǒng)航向角誤差對比圖。

圖4(a)為未經(jīng)外桿臂效應誤差濾波補償與經(jīng)補償修正的組合導航系統(tǒng)東向速度誤差對比圖；圖4(b)為未經(jīng)外桿臂效應誤差濾波補償與經(jīng)補償修正的組合導航系統(tǒng)北向速度誤差對比圖；圖4(c)為未經(jīng)外桿臂效應誤差濾波補償與經(jīng)補償修正的組合導航系統(tǒng)天向速度誤差對比圖。

圖5(a)未經(jīng)外桿臂效應誤差濾波補償與經(jīng)補償修正的組合導航系統(tǒng)經(jīng)度位置誤差對比圖；圖5(b)未經(jīng)外桿臂效應誤差濾波補償與經(jīng)補償修正的組合導航系統(tǒng)緯度位置誤差對比圖；圖5(c)未經(jīng)外桿臂效應誤差濾波補償與經(jīng)補償修正的組合導航系統(tǒng)天向位置誤差對比圖。

具體實施方式

下面詳細描述本發(fā)明的實施方式，所述實施方式的示例在附圖中示出。下面通過參考附圖描述的實施方式是示例性的，僅用于解釋本發(fā)明，而不能解釋為對本發(fā)明的限制。

具體實施方式如下：

1、根據(jù)外桿臂效應誤差原理建立外桿臂效應誤差模型。

如圖1所示，在機體坐標系中，是GPS天線位置，是SINS位置，定義GPS天線和SINS位置差為外桿臂距離r^b：

式(1)中，r^b＝[r_x r_y r_z]^T，r_x為外桿臂距離在機體坐標系中X軸方向上的投影、r_y為外桿臂距離在機體坐標系中Y軸方向上的投影、r_z為外桿臂距離在機體坐標系中Z軸方向上的投影。

假設所有無人直升機導航設備都是固聯(lián)在機體上，則為機體坐標系下外桿臂長度的變化，為機體坐標系下外桿臂長度的變化加速度。則外桿臂效應誤差模型為：

ΔL＝r^b

ΔV＝ω×r^b

上述模型定義在機體坐標系中。式(2)中，r^b為機體坐標系下的外桿臂距離，定義為機體坐標系中GPS天線和SINS位置差；ΔL為機體坐標系下外桿臂效應引起的桿臂誤差，ΔV為機體坐標系下外桿臂效應引起的速度偏差，Δa為機體坐標系下外桿臂效應引起的加速度偏差；ω為機體的角速率，ω×(ω×r^b)為向心加速度，為切向加速度，同時有：

式(3)中，為機體坐標系下的地球自轉(zhuǎn)角速率，為導航坐標系相對地球坐標系的角速度在機體坐標系上的分量，為機體坐標系相對于導航坐標系轉(zhuǎn)動的角速度。

導航坐標系(東北天坐標系)中的外桿臂效應誤差模型為：

式(4)中，rⁿ為導航坐標系下的外桿臂距離；ΔLⁿ為導航坐標系下外桿臂效應引起的桿臂誤差，ΔVⁿ為導航坐標系下外桿臂效應引起的速度偏差，Δaⁿ為導航坐標系下外桿臂效應引起的加速度偏差，為導航坐標系下外桿臂長度的變化，為導航坐標系下外桿臂長度的變化加速度，為機體坐標系相對于導航坐標系轉(zhuǎn)動的角加速度；為機體坐標系b系(X,Y,Z-右,前,上)到導航坐標系n系(X,Y,Z-東,北,天)的轉(zhuǎn)換矩陣，且同時有：

其中：為陀螺儀的實際輸出，表示安裝在系的陀螺儀相對慣性空間的角速率在機體系上的投影；為導航坐標系相對慣性空間的角速度在機體坐標系上的分量；為導航坐標系相對慣性空間的角速度在導航坐標系上的分量；為導航坐標系下的地球自轉(zhuǎn)角速率；為導航坐標系相對地球坐標系的角速度在導航坐標系上的分量。

2、將外桿臂效應誤差為系統(tǒng)狀態(tài)量，建立卡爾曼濾波模型

(2.1)建立包含外桿臂效應誤差的濾波狀態(tài)方程：

式(6)中，X(t)為系統(tǒng)狀態(tài)變量，F(xiàn)(t)為系統(tǒng)矩陣，G(t)系統(tǒng)狀態(tài)噪聲系數(shù)矩陣，W(t)為系統(tǒng)噪聲矩陣，為系統(tǒng)狀態(tài)變量一階微分。濾波狀態(tài)方程表示為：

式(7)中，φ＝[φ_E φ_N φ_U]^T為數(shù)學平臺誤差角，φ_E為東向平臺誤差角，φ_N為北向平臺誤差角，φ_U為天向平臺誤差角，δV＝[δV_E δV_N δV_U]^T為速度誤差，δV_E為東向速度誤差，δV_N為北向速度誤差，δV_U為天向速度誤差，δP＝[δL δλ δh]^T為位置誤差，δL為緯度位置誤差，δλ為經(jīng)度位置誤差，δh為高度位置誤差，δr＝[δr_x δr_y δr_z]^T為外桿臂誤差，δr_x為外桿臂誤差在X軸上的分量，δr_y為外桿臂誤差在y軸上的分量，δr_z為外桿臂誤差在z軸上的分量。為數(shù)學平臺誤差角變化量，為速度誤差變化量，為位置誤差變化量，為外桿臂誤差變化量，w_a＝[w_ax w_ay w_az]^T為加速度計白噪聲誤差，w_ax為X軸加速度計白噪聲誤差，w_ay為y軸加速度計白噪聲誤差，w_az為z軸加速度計白噪聲誤差，w_g＝[w_gx w_gy w_gz]^T為陀螺儀白噪聲誤差，w_gx為x軸陀螺儀白噪聲誤差，w_gy為y軸陀螺儀白噪聲誤差，w_gz為z軸陀螺儀白噪聲誤差。同時有：

式(8)-(15)中，ω_ie為地球自轉(zhuǎn)角速率，V_E為速度誤差在導航坐標系中東向的分量，V_N為速度誤差在導航坐標系中北向的分量，V_U為速度誤差在導航坐標系中天向的分量，f_E為比力在導航坐標系中東向的分量，f_N為比力在導航坐標系中北向的分量，f_U為比力在導航坐標系中天向的分量，R為地球半徑，R_M為地球的子午圈曲率半徑，R_N為地球卯酉圈曲率半徑，L為無人直升機所處的緯度，h為無人直升機所處的高度。

(2.2)建立包含外桿臂長度誤差的濾波量測方程，量測方程為：

Z(t)＝H(t)X(t)+V(t) (16)

式(16)中，H(t)為量測系數(shù)矩陣，V(t)為量測噪聲矩陣，Z(t)為量測狀態(tài)信息，量測方程表示為：

式(17)中，[r^b×]和表示矢量r^b和的反對稱矩陣，v為量測噪聲矩陣。

量測信息由GPS和SINS的位置、速度誤差構(gòu)成，可表示為：

式(18)中，P_GPS是導航坐標系中GPS天線位置，P_SINS是導航坐標系中SINS位置，V_GPS是導航坐標系中GPS天線速度，V_SINS是導航坐標系中SINS速度。同時有：

式(19)中，γ為無人直升機的橫滾角、θ為無人直升機的俯仰角，ψ為無人直升機的航向角。

3、根據(jù)建立的濾波狀態(tài)方程和量測方程，實現(xiàn)對外桿臂效應誤差的估計和補償。首先，根據(jù)平臺角誤差、速度誤差和位置誤差的濾波方程可以估計得到外桿臂距離，然后將其代入系統(tǒng)方程中，在濾波過程中進行補償修正。

為了驗證本發(fā)明所提出的旋翼擾動下慣性/GPS組合導航外桿臂估計方法的有效性，采用本發(fā)明在Matlab仿真軟件中進行仿真驗證。

機體在受旋翼擾動的綜合振動影響下，對比仿真設置的真實外桿臂值和通過濾波估計得到的外桿臂值，對比結(jié)果如圖2所示，虛線代表設置的外桿臂長度真實值，實線代表外桿臂長度估計值。從圖2(a)、圖2(b)、圖2(c)可以看出，在無人直升機旋翼擾動帶來機體振動情況下，本發(fā)明外桿臂效應濾波估計方法能夠較為有效的實現(xiàn)對綜合振動環(huán)境下外桿臂長度的估計。

為了進一步驗證無人直升機慣性/GPS組合導航外桿臂效應補償算法，設計一組1360s包含垂直起飛、懸停、斜爬升、加速、平行、斜下降、垂直下降等多種運動特性的無人直升機航跡，補償校正從200s開始。將未經(jīng)過桿臂效應誤差濾波補償?shù)慕M合導航系統(tǒng)結(jié)果與經(jīng)過補償修正的組合導航結(jié)果進行對比，得到對比曲線如圖3-圖5所示，虛線為未經(jīng)濾波補償?shù)慕M合導航結(jié)果，實線為補償后的組合導航結(jié)果。從圖3(a)、圖3(b)可以看出經(jīng)過外桿臂效應誤差補償?shù)膶Ш较到y(tǒng)橫滾角姿態(tài)誤差、俯仰角姿態(tài)誤差有所減?。粡膱D4(a)、圖4(b)可以看出，經(jīng)過外桿臂效應誤差補償?shù)膶Ш较到y(tǒng)東向和北向速度誤差有所減??；從圖5(a)、圖5(b)、圖5(c)可以看出,經(jīng)過外桿臂效應誤差補償?shù)膶Ш较到y(tǒng)經(jīng)度、緯度和高度位置誤差有所減小，總體導航系統(tǒng)精度有所提高。

以上實施例僅為說明本發(fā)明的技術(shù)思想，不能以此限定本發(fā)明的保護范圍，凡是按照本發(fā)明提出的技術(shù)思想，在技術(shù)方案基礎上所做的任何改動，均落入本發(fā)明保護范圍之內(nèi)。