一種結合地形和環境特征的水下導航定位方法
【專利摘要】本發明公開了一種結合地形和環境特征的水下導航定位方法,根據地形信息量判斷水下潛器的規劃航跡中各個子區域是否可匹配,若可匹配則采用地形輔助慣性導航系統實現定位,若不可匹配則采用同步定位與構圖算法輔助主慣性導航系統實現定位。本發明通過對導航區域地形信息量的計算,將海底地形劃分為地形可匹配區域及地形不可匹配區域。針對不同的區域,采用不同的導航算法實現對主慣性導航系統的位置誤差修正,具有更高的自主性。
【專利說明】一種結合地形和環境特征的水下導航定位方法
【技術領域】
[0001]本發明涉及水下導航【技術領域】,具體地說是設計一種能夠滿足水下潛器長時間高精度自主導航定位要求的方法。
【背景技術】
[0002]慣性導航系統不需要任何外來信息,也不會向外輻射任何信息,僅靠慣性導航系統本身就能在全天候條件下,在全球范圍內和任何介質環境里進行連續的三維定位和導航,這種同時具備自主性、隱蔽性和能獲取載體完備運動信息的獨特優點是諸如無線電導航、衛星導航和天文導航等其他導航系統無法比擬的。但是,慣性導航系統最大弱點是其系統誤差隨時間積累,時間越長,誤差越大。為了確保水下潛器的安全航行和武器的準確發射,必須利用外界位置信息對慣性導航系統進行周期的調整與校正。
[0003]地形輔助導航系統(Terrain-Aided Navigat1n, TAN),實質是由慣性導航系統(給出實時圖)與傳感器(實時圖與基準圖的聯系和紐帶)和數字地圖(給出基準圖)構成的組合導航系統,它作為一種高精度導航定位技術,只需要地形信息含量足夠豐富并增加存儲器的容量就能將導航精度提高近一個數量級,達到十幾米的定位精度。地形輔助導航具有自主、隱蔽、連續、全天候工作,導航定位誤差不隨時間積累的優點,是水下潛器理想的輔助導航定位手段。
[0004]典型的基于ICCP算法的地形輔助導航系統可以在具有先驗地形圖且地形變化明顯的海域,修正主慣性導航系統的位置誤差。然而,地形輔助導航需要有先驗的地形圖,且要求有明顯的地形變化,對于那些尚未測繪或者地形變化過于平緩,地形特征不明顯的海域,用地形輔助導航方法來降低主慣性導航系統隨時間積累的誤差是十分困難的。
[0005]此時,可利用對具有時間穩定性的其它水下特征諸如水下建筑、沉船等的觀測,采用基于海洋環境特征的同步定位與地圖構建(Simultaneous Localizat1n andMapping, SLAM)算法來作為地形輔助導航的替代方法,以實時修正水下潛器的位置,降低導航誤差的積累,改善導航系統的定位精度。
【發明內容】
[0006]要解決的技術問題:針對現有技術的不足,本發明提出一種結合地形和環境特征的水下導航定位方法,針對海底先驗地形圖不可得或者是地形信息量不豐富的海域,無法利用地形輔助導航系統來降低主慣性導航系統隨時間積累的位置誤差的技術問題。
[0007]技術方案:為解決上述技術問題,本發明采用以下技術方案:
[0008]一種結合地形和環境特征的水下導航定位方法,根據地形信息量判斷水下潛器的規劃航跡中各個子區域是否可匹配,若可匹配則采用地形輔助慣性導航系統實現定位,若不可匹配則采用同步定位與構圖算法輔助主慣性導航系統實現定位。
[0009]地形輔助導航方法具有自主、隱蔽等優點,在地形信息量豐富的可匹配海域,一般是基于ICCP算法,根據先驗地形圖來修正慣性導航系統的誤差,修正方法和修正過程均為現有技術,可以很好的輔助主慣性導航系統實現水下潛器的導航定位;而在地形不可匹配區域,同步定位與構圖算法,發揮了自身對具有時間穩定性的環境特征進行觀測來實現定位,并修正主慣性導航系統位置誤差的特點,有效地彌補了地形輔助導航系統的不足;兩種方法配合使用,使得水下潛器的導航定位具有更好的自主性和精確性。
[0010]進一步的,在本發明中,判斷水下潛器的規劃航跡中的各個子區域是否可匹配的方法如下:
[0011]步驟1.1:將海底地形高程采用網格矩陣的方式劃分成多塊地形候選區域,水下潛器的規劃航跡上經過其中L塊地形候選區域,設定某一塊地形候選區域的經緯度跨度為MXN網格,并且網格點坐標為(i, j)處的地形高程值為height (i,j),這里i = 1,2,...,M,j = I, 2,..., N ;
[0012]步驟1.2:利用移動計算窗口法計算地形候選區域的參數,具體方法為:
[0013]定義一個大小為mXn的局部移動計算窗口,并計算局部移動計算窗口內的地形
高度平均值
【權利要求】
1.一種結合地形和環境特征的水下導航定位方法,其特征在于:根據地形信息量判斷水下潛器的規劃航跡中各個子區域是否可匹配,若可匹配則采用地形輔助慣性導航系統實現定位,若不可匹配則采用同步定位與構圖算法輔助主慣性導航系統實現定位。
2.根據權利要求1所述的結合地形和環境特征的水下導航定位方法,其特征在于:判斷水下潛器的規劃航跡中的各個子區域是否可匹配的方法如下: 步驟1.1:將海底地形高程采用網格矩陣的方式劃分成多塊地形候選區域,水下潛器的規劃航跡上經過其中L塊地形候選區域,設定某一塊地形候選區域的經緯度跨度為MXN網格,并且網格點坐標為(i, j)處的地形高程值為height (i, j),這里i = I, 2,..., M, j =I, 2,..., N ; 步驟1.2:利用移動計算窗口法計算地形候選區域的參數,具體方法為: 定義一個大小為mXn的局部移動計算窗口,并計算局部移動計算窗口內的地形高度平均值
,當局部移動計算窗口的中心在各地形候選區域的全部網格點上移動一遍后,可以得到各地形候選區域的地形標準差σ (I)、經度方向上的地形相關系數Rlmgitude⑴以及緯度方向上的地形相關系數Rlatitude (I),其中I = 1,2,...,L,下標longitude表示經度,latitude表示纟韋度,地形候選區域的參數具體計算公式如下:
步驟1.3:針對規劃航跡上的L塊地形候選區域,分別判斷每一塊地形候選區域的地形標準差和經緯度方向地形相關系數是否同時滿足σ⑴>4σ。且Rlmgitude⑴<0.7且Rlatitude(I) < 0.7,若滿足,則對應的地形候選區域為可匹配區域,否則為不可匹配區域,其中,σ。為測深傳感器測量誤差的標準差。
3.根據權利要求1所述的結合地形和環境特征的水下導航定位方法,其特征在于:所述的同步定位與構圖算法包括以下步驟: 步驟3.1:系統非線性過程模型的建立 步驟3.1.1:水下潛器狀態方程的建立 選取東北天系為導航坐標系,載體坐標系X軸沿水下潛器橫軸指向右舷,載體坐標系I軸沿水下潛器縱軸指向前,載體坐標系ζ軸垂直于X軸和y軸所確定的平面構成右手坐標系;則k時刻水下潛器狀態方程如下:
其中: P°Svehicle (k)表不k時刻水下潛器的位置, veIvehicle (k)表示k時刻水下潛器的速度, quavehicle(k)表示k時刻水下潛器的姿態四元數, 上標G表示導航坐標系, 上標B表示載體坐標系, At表示離散采樣時間間隔, CB2e表示從載體坐標系變換到導航坐標系的方向余弦矩陣, fB(k)表示k時刻加速度計輸出, g表示重力加速度, (々)表示由陀螺儀測得的角速率構成的四元數, ?)表示四元數乘法; 步驟3.1.2:地圖狀態的建立 設定k時刻觀測到了 Nm個新的環境特征,則k時刻地圖狀態為:
其中: (dm示k時刻第ml個環境特征的狀態, 4^re_,?2(k)表示k時刻第m2個環境特征的狀態,
衰示k時刻第mNm個環境特征的狀態, 上標T表示轉置; 將水下環境特征建模為點特征,則環境特征在導航坐標系下的位置如下:
其中:表示在k時刻得到的第mi個環境特征在導航坐標系中的位置,
表示在k-Ι時刻得到的第mi個環境特征在導航坐標系中的位置, 表示水下潛器上的方位/距離傳感器到水下潛器中心的桿臂效應補償量在載體坐標系的分量, 下標S表示方位/距離傳感器坐標系, Cs2b表示方位/距離傳感器坐標系變換到載體坐標系的方向余弦矩陣,
表示方位/距離傳感器測得的第mi個環境特征與水下潛器之間的相對位置,其中,d表示方位/距離傳感器到環境特征之間的距離,Phi表示環境特征與方位/距離傳感器之間的方位角,theta表示環境特征與方位/距離傳感器之間的高度角; 步驟3.2:系統非線性觀測模型的建立 步驟3.2.1:根據方位/距離傳感器獲得環境特征與水下潛器的相對位置觀測,從而建立系統非線性觀測模型如下:
z (k) = h (X (k)) + V (k) 其中: z(k)表示對環境特征的觀測值, h(.)為非線性觀測函數, x(k)為系統狀態矢量,包括水下潛器的位置、速度、姿態四元數以及環境特征的位置, V (k)為系統觀測噪聲; k時刻對第mi個環境特征的觀測只與該環境特征和水下潛器狀態相關,因此有:
zmi (k) = h (Xvehicle (k),Xfeaturejli (k)) + V mi (k) 其中: Zffli (k)表示k時刻對第mi個環境特征的觀測, XvehiCle (k)表示水下潛器在k時刻的狀態, Xfeature_mi (k)表示第Hli個環境特征在k時刻的狀態, vffli(k)是對第mi個環境特征的觀測噪聲; 步驟3.2.2:設定
則k時刻對第mi個環境特征的觀測為:
其中: d表示方位/距離傳感器到環境特征之間的距離, phi表示環境特征與方位/距離傳感器之間的方位角, theta表示環境特征與方位/距離傳感器之間的高度角; 步驟3.3:系統狀態增廣過程 水下潛器航行過程中,方位/距離傳感器觀測到環境特征,若該環境特征為已觀測到過的特征,則直接執行步驟3.5 ;若該環境特征為新出現的特征,則對系統狀態進行增廣并順序執行步驟3.4和步驟3.5,增廣后的系統狀態變為:
其中: XvehiCle (k)表示k時刻水下潛器狀態,包括水下潛器位置、速度和姿態四元數, Xfeature(k)表示k時刻環境特征的狀態, fvehicle (.)為非線性狀態轉換函數; 步驟3.4:同步定位與構圖算法預測過程 通過步驟3.1建立的系統非線性過程模型和步驟3.2建立的系統非線性觀測模型對k時刻的系統狀態以及狀態協方差矩陣進行預測,有:
X(kIk-1) = f(x(k-1Ik-1))
其中: X (k I k-1)是根據k-1時刻的系統狀態對k時刻的一步預測狀態量, X (k-11 k-Ι)是k-1時刻的系統狀態估計值, Pcov (k I k-Ι)是根據k-Ι時刻的系統狀態對k時刻的預測協方差, Pcov(k-1 I k-1)是k-1時刻的狀態協方差估計值,
為系統狀態方程的雅可比矩陣, τ代表轉置, Q(k)為噪聲ω (k)的協方差; 步驟3.5:同步定位與構圖算法更新過程 對于觀測得到的環境特征,結合步驟3.2和步驟3.4對系統狀態值進行更新,具體公式如下:
新息:vimTOV (k) = z (k) -h (X (k I k-1)) 濾波增益:
狀態估計值:x(k|k) = X (k I k-1)+Kgain (k).Vinnov (k)
狀態協方差:
其中:
Vinnov (k)為新息, z (k)為k時刻觀測值, h (x (k I k-1))為k時刻觀測預測值, KgainOO為k時刻濾波增益,
為系統觀測方程的雅可比矩陣, R(k)為V (k)的協方差, x(k|k)為k時刻的系統狀態估計值, Pcov (k I k)為k時刻的系統狀態協方差; 步驟3.6:從x(k|k)中得到的p0Svehic;le即為修正后的主慣性導航位置信息。
【文檔編號】G01C21/16GK104075715SQ201410320791
【公開日】2014年10月1日 申請日期:2014年7月7日 優先權日:2014年7月7日
【發明者】徐曉蘇, 湯郡郡, 李佩娟, 張濤, 岳增陽 申請人:東南大學