慣導輔助下的偽距差值單星高動態定位方法
【專利摘要】本發明提出一種慣導輔助下的偽距差值單星高動態定位方法：利用三個時刻單星給出的偽距、以及第一時刻高度計輸出建立定位方程，利用慣導系統得到第二、三時刻與第一時刻定位目標在地心地固坐標系中的坐標變化量，將變化量代入定位方程中通過最小二乘法進行求解；建立目標動態過程的狀態空間模型，構建關于卡爾曼濾波的狀態方程以及量測方程；利用最小二乘結果作為卡爾曼濾波算法的初始值，利用卡爾曼濾波器不斷更新得到更加準確平滑的位置信息。本發明建立慣導輔助下的單星高動態定位算法模型并結合卡爾曼濾波算法，消除利用單星定位時由于目標的高動態特性帶來的定位精度下降的問題，提高了單星定位對高動態目標的定位精度低、精度維持性低的問題。
【專利說明】
慣導輔助下的偽距差值單星高動態定位方法
技術領域
[0001] 本發明是一種用于慣導輔助下的偽距差值單星高動態定位方法，特別涉及一種基 于偽距差值（pseudo range difference)的慣導系統（Inertial Navigation System)輔助 下的單星高動態的定位方法。
【背景技術】
[0002] 單星定位具有成本低、質量輕、體積小、研發周期短等優勢，在星座導航拒止環境 下，單星定位可以有效補充導航星座的缺失，實現對定位目標的定位，但當目標處于高動態 條件下，單星定位存在定位精度隨動態性增加而下降的問題。
[0003]如何提高單星定位在高動態下的定位精度是導航領域的一個研究熱點。目前單星 定位的主要方法有測頻單星定位方法，該方法利用衛星發射信號的測頻信息構建多個定位 輻射面實現單星定位，但當輻射面增大時，定位誤差會快速增長;測角測距單星定位方法， 該方法通過距離、角度并結合用戶終端速度輔助實現目標位置解算，但對衛星的有效載荷 復雜度要求比較高;基于偽距輔助的徑向加速度單星定位方法，該方法根據運動學原理得 到地面靜止輻射源相對衛星的徑向加速度從而實現定位，但精度較差;基于積分多普勒測 量的單星定位方法，該方法利用雙曲面和定位目標高程信息實現定位，其對衛星有效載荷 要求較低，但只適合對靜止目標進行定位;偽距率輔助的單星定位算法，該方法是在積分多 普勒測量的單星定位方法的基礎上進行了改進，定位精度更高，但同樣存在對動態目標定 位誤差大的情況。
【發明內容】
[0004] 積分多普勒測量的單星定位方法具有提高通道特性容錯率的特性，但無法有效地 解決高動態下的目標定位，因此，基于定位目標具有高動態特性單星定位方法的不足，本發 明設計了一種慣導輔助下的高動態單星定位方法，該方法利用慣導系統計算高動態下定位 目標的坐標偏移，可提高動態條件下利用積分多普勒測量進行單星定位時的精度;其次，結 合單星定位中積分多普勒測量值得到的偽距與慣導推算得到的偽距做偽距差，通過融合濾 波可提尚定位精度維持的性能。
[0005] 要解決的技術問題
[0006] 本發明的目的在于提供一種慣導輔助下的單星高動態定位算法，這種慣導輔助下 的單星高動態定位方案，實現難度低，可以有效提高利用單顆衛星對高動態目標進行定位 的定位精度，并且具有較高的定位精度維持能力。
[0007] 技術方案
[0008] 本發明的基于慣導輔助下的單星高動態定位方法結合了已經對準的慣導系統可 提供可靠的相對位移信息和積分多普勒測量的單星定位方法對衛星通道特性容錯率較高 的優點，本發明建立慣導輔助下的單星高動態定位算法模型并結合卡爾曼濾波算法，來消 除利用單星定位時由于目標的高動態特性帶來的定位精度下降的問題，提高了單星定位對 高動態目標的定位精度低、精度維持性低的問題。
[0009] 本發明的技術方案為：
[0010] 所述一種慣導輔助下的偽距差值單星高動態定位方法，其特征在于:包括以下步 驟：
[0011]步驟1:利用三個時刻單星給出的基于積分多普勒的單星與定位目標偽距，以及第 一時刻定位目標的高度hi，建立定位方程；
[0012] 步驟2:利用定位目標的慣導系統得到第二、三時刻與第一時刻定位目標在地心地 固坐標系中的坐標變化量;將坐標變化量代入步驟1建立的定位方程，并通過最小二乘法求 解定位方程，得到第一時刻定位目標的坐標估計值；
[0013] 步驟3:建立定位目標動態過程的狀態空間模型：
[0014] 狀態方程為：
[0015] X^FX + W
[0016] 其中X為狀態變量，F為動態矩陣，W為過程噪聲;選取狀態變量為：
[0017] X=[8Rx 8Ry 8RZ 8VX 8Vy 8VZ]T
[0018] SRx、SRy、SRz與SVx、SVy、SV z分別為地心地固坐標系中定位目標在三個方向上位置 偏差和速度偏差，動態矩陣為
[0020]將動態矩陣離散化得到狀態轉移矩陣為：
[0022] At為采樣間隔；
[0023] 建立定位目標動態過程的量測方程為：
[0024] Z = HX+V
[0025]選取定位目標慣導系統推算的偽距與單星測量得到的偽距之間的偽距差作為量 測量：
[0027] h表示第i時刻單星測量得到的單星和定位目標之間的偽距，R1>INS表示第i時刻定 位目標慣導系統推算的偽距，1 = 1，2,3，111,_表示第一時刻定位目標慣導系統推算的定位 目標高度；
[0028] 測量矩陣為：
[0030] eix，eiy，eiz分別為第i時刻的單位觀測矢量，i = 1，2，3:
[0034] Xi，Yi，Zi表示單星在地心地固坐標系中第i時刻的坐標，Xi，yi，Zi表示定位目標在 地心地固坐標系中第i時刻的坐標；
[0038] V為測量噪聲；
[0039] 步驟4:以步驟2得到的定位目標坐標估計值為初始值，采用卡爾曼濾波算法對步 驟3建立的定位目標動態過程狀態空間模型進行計算，得到定位目標坐標的最優估計值。
[0040] 有益效果
[0041 ]本發明通過利用慣導系統提供定位目標不同時刻的相對位置，改善了由于目標的 高動態特性而進行單星定位時定位誤差較大的缺點，并且結合最小二乘以及基于偽距差值 的卡爾曼濾波方法，使得定位精度具有較好的穩定性。
[0042] 本發明的附加方面和優點將在下面的描述中部分給出，部分將從下面的描述中變 得明顯，或通過本發明的實踐了解到。
【附圖說明】
[0043] 本發明的上述和/或附加的方面和優點從結合下面附圖對實施例的描述中將變得 明顯和容易理解，其中：
[0044] 圖1是本發明的慣導輔助下的單星高動態定位算法的系統結構圖。
[0045] Ri表示第一時刻衛星與定位目標之間的偽距
[0046] R2表示第二時刻衛星與定位目標之間的偽距
[0047] R3表示第三時刻衛星與定位目標之間的偽距
[0048] A X1表示第二時刻與第一時刻間定位目標在地心地固坐標系中在x軸的坐標變化 量
[0049] A yi表示第二時刻與第一時刻間定位目標在地心地固坐標系中在y軸的坐標變化 量
[0050] A Z1表示第二時刻與第一時刻間定位目標在地心地固坐標系中在z軸的坐標變化 量
[0051 ] a X2表示第三時刻與第一時刻間定位目標在地心地固坐標系中在X軸的坐標變化 量
[0052] A y2表示第三時刻與第一時刻間定位目標在地心地固坐標系中在y軸的坐標變化 量
[0053] A Z2表示第三時刻與第一時刻間定位目標在地心地固坐標系中在z軸的坐標變化 量
[0054]圖2是衛星軌道參數為遠地點為36000Km，近地點為300Km，回歸周期為24小時的大 橢圓軌道，在遠地點區域為定位目標提供定位。衛星載波頻率為2.4GHz，衛星位置誤差為 l〇m，定位目標的東向速度和北向速度為100m/s，天向速度為Om/s，多普勒積分測量誤差為 (^^，氣壓高度計測量誤差為^/暖導系統的初始姿態誤差為丨⑶^⑶^⑶'^陀螺儀零偏 為0.01° /h，陀螺儀隨機游走為0.001° /sqrt(h)，加速度計偏移為10yg，加速度計隨機游走 為20iig/sqrt(Hz)，300秒時長本發明所提出的算法和多普勒單星定位算法、偽距率輔助的 單星定位算法的定位誤差對比圖。實線帶圓點線為本發明的定位誤差曲線，實線帶方形線 為多普勒單星定位算法的定位誤差曲線，實線帶星形線為偽距率輔助的單星定位算的定位 誤差。
[0055] 圖3是與圖2中仿真條件相同，定位目標的東向速度和北向速度為lOm/s，天向速度 為0m/s時定位誤差對比圖。實線帶圓點線為本發明的定位誤差曲線，實線帶方形線為多普 勒單星定位算法的定位誤差曲線，實線帶星形線為偽距率輔助的單星定位算的定位誤差。
【具體實施方式】
[0056] 下面詳細描述本發明的實施例，所述實施例是示例性的，旨在用于解釋本發明，而 不能理解為對本發明的限制。
[0057] 本發明的基于慣導輔助下的單星高動態定位方法結合了已經對準的慣導系統可 提供可靠的相對位移信息和積分多普勒測量的單星定位方法對衛星通道特性容錯率較高 的優點，本發明建立慣導輔助下的單星高動態定位算法模型并結合卡爾曼濾波算法，來消 除利用單星定位時由于目標的高動態特性帶來的定位精度下降的問題，提高了單星定位對 高動態目標的定位精度低、精度維持性低的問題。本發明原理步驟如下：
[0058] a、利用三個時刻單星給出的基于積分多普勒的偽距、以及第一時刻的高度計輸出 進行定位方程建立。
[0059] b、利用慣導系統得到第二、三時刻與第一時刻定位目標在地心地固坐標系ECEF中 的坐標變化量。并將該變化量代入上一步所建立的定位方程中通過最小二乘法進行求解。
[0060] C、進行系統的模型建立，建立目標動態過程的狀態空間模型，并選取合適的狀態 變量與量測量，構建系統關于卡爾曼濾波的狀態方程以及量測方程。
[0061] d、利用步驟b中用最小二乘求出的值作為卡爾曼濾波算法的初始值，利用卡爾曼 濾波器不斷更新得到更加準確平滑的位置信息。
[0062] 整個慣導輔助下的單星高動態定位方法由慣導系統以及積分多普勒定位系統所 組成。
[0063]基于上述原理，本實施例整體結構如圖1所示，利用衛星所得到的基于積分多普勒 的偽距、慣導輔助得到的在地心地固ECEF坐標系下的位置變化以及高度計所得到的高程輔 助信息，結合最小二乘法與卡爾曼濾波算法求得不同時刻對高動態目標的定位位置，具體 步驟如下：
[0064] 1、利用三個時刻單星給出的基于積分多普勒的偽距、以及第一時刻的高度計輸出 進行定位方程建立。
[0065]利用衛星積分多普勒可以得到三個時刻的偽距值，列出如下三個等式：
[0069] 其中，Ri表示第i個時刻衛星和定位目標之間的偽距，Xi，Yi，Zi表示衛星在地心地 固坐標系ECEF(之后的坐標全在此坐標系下）中第i個時刻的坐標，該值可通過單星定位系 統的星歷得到，Xl，， Zl表示定位目標在第i個時刻的坐標，Stu為接收機鐘差。
[0070]利用氣壓高度計得到第一時刻定位目標的高程信息匕，并利用該高程信息構建方 程：
[0072] 2、利用慣導系統得到第二、三時刻與第一時刻定位目標在地心地固坐標系ECEF中 的坐標變化量。并將該變化量代入上一步所建立的定位方程中通過最小二乘法進行求解。
[0073] 步驟1中四個方程十個未知數無法求解，利用慣導系統得到定位目標第一時刻到 第二、三時刻的坐標變化量如下所示：
[0074] X2 = xi+ A xi,y2 = yi+ A yi,Z2 = zi+ A zi (5)
[0075] X3 = X1+A X2，y3 = yi+A y2，Z3 = zi+A Z2 (6)
[0076] 其中，A X1，A 71和A Z1、A X2, A y2和A Z2分別表示慣導模塊在第一個時刻和第二、 三個時刻之間定位目標在X軸，y軸和Z軸上的相對位移，分別將式(5)、（6)代入式(2)、（3)中 得到式(7)、（8)，如下所示：
[0079] 這樣減少了六個未知數，聯立式(1)、（4)、（7)、（8)利用最小二乘法進行求解。
[0080] 用k代表當前歷元正在進行的牛頓迭代次數，即k-1為當前歷元已經完成的迭代次 數。將四個非線性方程在第k-1次迭代結果[Xk+ROH] 1處進行泰勒展開，并略去高階項后 使之線性化，可以得到其線性化后的矩陣方程式為：
[0081 ] G ? A x = b (9)
[0082]其中，
[0086]利用最小二乘法對未知量進行求解，所得到的方程解AX使得式(9)的左邊的函數 值與右邊的實際輸出的測量值之差的平方和最小，求得Ax如下所示：
[0089] 在AX的長度| | AX| |滿足一定精度后停止迭代，得到第一時刻定位目標的坐標 xi,yi,zi〇
[0090] 3、進行系統的模型建立，建立定位目標動態過程的狀態空間模型，并選取合適的 狀態變量與量測量，構建系統關于卡爾曼濾波的狀態方程以及量測方程。
[0091 ]運動目標的動態過程用狀態空間模型表示：
[0092] X = FX + W (15)
[0093] 上式中，X為系統的狀態變量，F為系統的動態矩陣，W為系統過程噪聲。將F離散化 后得到離散化的狀態轉移矩陣，選取采樣間隔較小時，F可以認為是非時變的。
[0095]選取系統的狀態變量為：
[0096] X=[8Rx 8Ry 8RZ 8VX 8Vy 8VZ]T (17)
[0097] 其中，SRx、SRy、SRz與SVx、SV y、SVz分別為地心地固中的三個坐標系方向上運動目標 的位置偏差和速度偏差。所以動態矩陣表達式為：
[0099]離散化后的狀態轉移矩陣為：
[0101]選取慣導系統推算的偽距與單星測量得到的偽距之間的偽距差作為量測量：
[0103] Ri,INS表示第i時刻定位目標慣導系統推算的偽距，i = 1，2，3，hi, INS表示第一時刻 定位目標慣導系統推算的定位目標高度；
[0104] 測量矩陣為：
[0106] eix，eiy，eiz分別為第i時刻的單位觀測矢量，i = 1，2，3:
[0113] 量測方程為：
[0114] Z = HX+V (23)
[0115] V為系統測量噪聲。
[0116] 4、利用步驟2中用最小二乘求出的值作為卡爾曼濾波算法的初始值，利用卡爾曼 濾波器不斷更新得到更加準確平滑的位置信息。
[0117]卡爾曼濾波算法包含預測和校正兩部分，k表示當前正在進行的卡爾曼濾波歷元， k_l表示上一個卡爾曼濾波歷元，上標〃 A 〃表示為估計值，右上標〃-〃表示先驗值。預測部分 利用上一 (k_l)歷元的狀態估計值來預測當前(k)歷元的狀態值的估計值并作為先驗估計 值，用均方誤差陣來衡量所估計值的可靠性。利用步驟3用最小二乘求得的信息作為卡爾曼 濾波器的初始值。
[0120] 氧^:表示真實值Xk-1的最優估計值，鳥表示真實值Xk的先驗估計值，Pk為先驗估計 值的均方誤差陣。
[0121]校正部分利用當前歷元的實際測量值來校正預測部分得到的狀態先驗估計值，并 計算校正后的狀態量的可靠性。
[0125] Kk為卡爾曼濾波增益系數矩陣，尤表示真實值最優估計值，Pk為最優估計值的 均方誤差陣。
[0126] 卡爾曼濾波算法綜合的利用了先驗估計值和實際測量量兩方面的信息，使得測量 更新后的狀態估計值有最小的均方誤差。
[0127] 性能分析
[0128] 由圖2中可得，多普勒單星定位算法和偽距率輔助的單星定位算法的定位誤差隨 時間的增加呈指數性增長，當運行時間為300s時，定位誤差分別達到49Km和38Km，無法完成 有效定位，而本發明所提出算法在運行時間為300s時，定位誤差小于lKm，可以為定位目標 提供有效的定位信息。多普勒單星定位算法和偽距率輔助的單星定位算法主要針對靜態定 位，當定位目標處于動態環境中時會差生較大誤差，而本發明提出的速度輔助慣導的單星 定位方法則可以實現高動態下的目標定位，并有較高的精度維持性能。
[0129] 由圖3中也可以看出，在定位目標運行速度為lOm/s時，300s后多普勒單星定位算 法和偽距率輔助的單星定位算法的定位誤差分別達到4.7Km和4.4Km，而本發明的定位誤差 不到O.lKm。對比圖2和圖3,本發明所提出方法的定位誤差增量遠小于其他兩種方法，可以 相對有效地實現尚動態下的尚穩定度單星定位。
[0130] 盡管上面已經示出和描述了本發明的實施例，可以理解的是，上述實施例是示例 性的，不能理解為對本發明的限制，本領域的普通技術人員在不脫離本發明的原理和宗旨 的情況下在本發明的范圍內可以對上述實施例進行變化、修改、替換和變型。
【主權項】
1. 一種慣導輔助下的偽距差值單星高動態定位方法，其特征在于:包括以下步驟： 步驟1:利用三個時刻單星給出的基于積分多普勒的單星與定位目標偽距，以及第一時 刻定位目標的高度hi，建立定位方程； 步驟2:利用定位目標的慣導系統得到第二、三時刻與第一時刻定位目標在地心地固坐 標系中的坐標變化量;將坐標變化量代入步驟1建立的定位方程，并通過最小二乘法求解定 位方程，得到第一時刻定位目標的坐標估計值； 步驟3:建立定位目標動態過程的狀態空間模型： 狀態方程為： x 二 Fx+r 其中X為狀態變量，F為動態矩陣，W為過程噪聲;選取狀態變量為： X=[8Rx 8Ry 8RZ 8Vx 8Vy 8VZ]T SRx、SRy、SRz與SVx、SVy、SV z分別為地心地固坐標系中定位目標在三個方向上位置偏差 和速度偏差，動態矩陣為將動態矩陣尚散化得到狀態轉移矩陣為：At為采樣間隔； 建立定位目標動態過程的量測方程為： Z = HX+V 選取定位目標慣導系統推算的偽距與單星測量得到的偽距之間的偽距差作為量測量：Ri表示第i時刻單星測量得到的單星和定位目標之間的偽距，R1>INS表示第i時刻定位目 標慣導系統推算的偽距，1 = 1，2,3，111,1^表示第一時刻定位目標慣導系統推算的定位目標 高度； 測量矩陣為：eix，eiy，eiz分別為第i時刻的單位觀測矢量，i = 1，2，3:Xi，Yi，Zi表示單星在地心地固坐標系中第i時刻的坐標，Xi，yi，Zi表示定位目標在地心 地固坐標系中第i時刻的坐標；V為測量噪聲； 步驟4:以步驟2得到的定位目標坐標估計值為初始值，采用卡爾曼濾波算法對步驟3建 立的定位目標動態過程狀態空間模型進行計算，得到定位目標坐標的最優估計值。
【文檔編號】G01S19/47GK106054227SQ201610519027
【公開日】2016年10月26日
【申請日】2016年7月4日
【發明人】岳哲, 廉保旺, 唐成凱, 張怡, 郝軍芳
【申請人】西北工業大學