融合初值校準(zhǔn)與二階逼近的單星測(cè)頻定位算法*

張金藝，王 彬，秦 奮,，朱浤濤

(1. 上海大學(xué) 通信與信息工程學(xué)院， 上海 200444； 2. 上海大學(xué) 微電子研究與開發(fā)中心， 上海 200072；3. 上海航天電子技術(shù)研究所， 上海 201109)

深空探測(cè)對(duì)人類研究宇宙、開發(fā)太空資源具有重大的意義。進(jìn)入21世紀(jì)以來，各國對(duì)于外太空的探索愈發(fā)白熱化。探測(cè)器定位是深空探測(cè)中一個(gè)重要的組成部分，在地外星體探測(cè)中，圍繞特定星體構(gòu)建全球定位系統(tǒng)的成本難以估量。單星定位系統(tǒng)借助其成本低、體積小、結(jié)構(gòu)簡(jiǎn)單的優(yōu)勢(shì)，可以迅速發(fā)射部署，為目標(biāo)實(shí)現(xiàn)定位服務(wù)[1]。單星定位可以分為單星測(cè)向定位和單星測(cè)頻定位。單星測(cè)向定位通過干涉儀求解相位模糊得到定位目標(biāo)到衛(wèi)星的方向角和俯仰角，并結(jié)合地球模型實(shí)現(xiàn)單脈沖瞬時(shí)定位[2]。然而干涉儀天線結(jié)構(gòu)復(fù)雜，使得方向角和俯仰角求解困難，其定位誤差達(dá)幾十千米[3]。單星測(cè)頻定位利用多普勒效應(yīng)，由衛(wèi)星與定位目標(biāo)相對(duì)運(yùn)動(dòng)造成的多普勒頻移來反演地面定位目標(biāo)的位置[4]，其誤差為幾千米，因而單星測(cè)頻定位相對(duì)于單星測(cè)向定位適用性更高。然而單星測(cè)頻定位具有定位固有信息不足的缺陷，使得定位精度難以達(dá)到實(shí)用的要求，近年來，借助外部輔助信息來提升定位精度是一種常用的手段。

為提高單星測(cè)頻定位的精度，陸安南等提出一種測(cè)頻測(cè)相位差組合定位算法[5]，定位精度相對(duì)單一方法較高，達(dá)到千米級(jí)，但是該方法需多次測(cè)量信號(hào)的頻率，僅適用于長(zhǎng)時(shí)間通信信號(hào)的定位。Zhu等提出了消除二階導(dǎo)數(shù)的影響從而提高多普勒頻率測(cè)量精度的算法[6]，但是沒有在方程求解層面解決定位誤差問題。徐義、Liang等提出一種利用網(wǎng)格搜索法求解最優(yōu)值的單星測(cè)頻定位算法[7-8]，網(wǎng)格搜索法簡(jiǎn)單易行，但是實(shí)際運(yùn)用中計(jì)算量大而且精度受限。Yang、狄慧等提出一種基于徑向加速度的單星定位算法[9-10]，利用運(yùn)動(dòng)學(xué)原理通過到達(dá)脈沖提取徑向加速度，之后采用粒子群優(yōu)化(Particle Swarm Optimization, PSO)算法求解定位目標(biāo)位置，在一定程度上減小了定位誤差，但該算法對(duì)徑向加速度估計(jì)精度要求很高，且計(jì)算復(fù)雜度較高。Zhao等提出一種基于神經(jīng)網(wǎng)絡(luò)的波束掃描單星定位算法[11]，通過獲取波束的坐標(biāo)信息近似預(yù)測(cè)目標(biāo)位置，利用反向傳播(Back Propagation, BP)神經(jīng)網(wǎng)絡(luò)學(xué)習(xí)波束信息以得到定位結(jié)果，但是波束掃描對(duì)衛(wèi)星發(fā)射機(jī)功率要求過高，因此該算法實(shí)際應(yīng)用效果不佳。

對(duì)于單星測(cè)頻定位而言，傳統(tǒng)的求解算法由于初值選取問題導(dǎo)致誤差過大，而將優(yōu)化方案引入定位求解又會(huì)增加額外的計(jì)算開銷。為了解決這個(gè)問題，本文提出了融合初值校準(zhǔn)與二階逼近的單星測(cè)頻定位算法，利用初值校準(zhǔn)降低算法迭代次數(shù)，通過二階逼近提高定位精度。

1 多普勒單星定位問題剖析

1.1 基于多普勒效應(yīng)的單星測(cè)頻定位算法

單星測(cè)頻定位基于多普勒效應(yīng)，當(dāng)衛(wèi)星經(jīng)過地面靜止定位目標(biāo)的上空時(shí)，由于衛(wèi)星與定位目標(biāo)具有相對(duì)運(yùn)動(dòng)，衛(wèi)星接收信號(hào)頻率與定位目標(biāo)發(fā)射信號(hào)頻率存在偏差。通過數(shù)字信號(hào)處理從接收信號(hào)中得到多普勒變化率，進(jìn)而反演出目標(biāo)源的位置。單星測(cè)頻定位只需要一顆衛(wèi)星多次測(cè)量即可實(shí)現(xiàn)，具有結(jié)構(gòu)簡(jiǎn)單、功耗低的優(yōu)點(diǎn)。

(1)

將衛(wèi)星速度、位置以及待求定位目標(biāo)位置代入式(1)可得定位方程：

(2)

式(2)中，衛(wèi)星的位置和速度信息已知，發(fā)射頻率和接收頻率可測(cè)，待求量只有定位目標(biāo)的位置(x，y，z)。理論上，對(duì)于3個(gè)未知量，聯(lián)立3個(gè)不同時(shí)刻的定位方程可求解出定位結(jié)果。為求解聯(lián)立的非線性方程組，可以通過將方程線性化、選取合適的初值迭代來實(shí)現(xiàn)。多普勒單星定位算法在整體求解過程中，存在初值選取和定位方程求解2個(gè)問題。

圖1 單星測(cè)頻定位原理圖Fig.1 Schematic of single-satellite localization from frequency measurements

1.2 初值選取問題剖析

(3)

但是式(3)所示的非線性方程組求解困難，為了快速得到初值，多普勒單星定位算法選取t0時(shí)刻衛(wèi)星星下點(diǎn)作為迭代求解的初值。假設(shè)t0時(shí)刻衛(wèi)星的位置為(X0，Y0，Z0)，衛(wèi)星的軌道高度為h，則初值的坐標(biāo)可以表示為：

(4)

然而在實(shí)際應(yīng)用中，定位目標(biāo)并不一定與衛(wèi)星軌道在同一平面內(nèi)(如圖2所示)。當(dāng)定位目標(biāo)與衛(wèi)星軌道平面垂直距離增大時(shí)，星下點(diǎn)距真實(shí)位置的誤差變大，以星下點(diǎn)為初值會(huì)造成算法復(fù)雜度增加甚至迭代算法不收斂。

圖2 星下點(diǎn)與定位目標(biāo)不匹配示意Fig.2 Mismatch between sub-satellite point and localization target

1.3 定位方程求解問題剖析

定位方程求解決定了定位精度，在多普勒單星定位算法中，非線性方程組難以直接求解，一般通過泰勒展開將定位方程組線性化，把星下點(diǎn)(x0，y0，z0)作為迭代初值，迭代求解線性化的定位方程。

為便于分析，由式(2)，將定位方程寫為如下形式：

fi=gi(x,y,z)

(5)

將其在(x0，y0，z0)點(diǎn)泰勒展開并略去高階項(xiàng)可以得到：

gi(x0,y0,z0)+gi(x0,y0,z0)

(6)

進(jìn)一步簡(jiǎn)化可以得到：

(7)

式中，Δfi=fi-gi(x0,y0,z0)為ti時(shí)刻的多普勒頻移，Δx=x-x0、Δy=y-y0和Δz=z-z0分別表示迭代前后3個(gè)坐標(biāo)軸的估計(jì)差值。聯(lián)立3個(gè)時(shí)刻的線性化定位方程組求解估計(jì)差值，或者聯(lián)立3個(gè)以上的方程組通過最小二乘法獲取估計(jì)差值最優(yōu)解，并設(shè)定閾值或迭代次數(shù)得到最終定位結(jié)果。

將定位方程泰勒展開的本質(zhì)是用多項(xiàng)式去逼近原有的定位方程。然而在其線性化的過程中，復(fù)雜的高階項(xiàng)被舍去，僅含一階級(jí)數(shù)的泰勒展開式往往無法較好地?cái)M合原有定位方程，一些復(fù)雜的定位信息在舍去高階項(xiàng)的過程中被略去，造成單星定位的誤差始終無法減小。在多普勒單星定位算法中，算法的缺陷使提高頻率測(cè)量精度等手段對(duì)定位精度的提高有限，本文將對(duì)定位方程的求解提出新的思路。

2 高精度快速單星測(cè)頻定位算法

多普勒單星定位算法中的初值選取問題和定位方程求解問題極大地影響了定位求解的速度和精度。通過觀察單星測(cè)頻定位方程式(2)可以發(fā)現(xiàn)，定位方程右邊是由常函數(shù)、指數(shù)函數(shù)、冪函數(shù)等基本函數(shù)經(jīng)過數(shù)次有理運(yùn)算得到的初等函數(shù)，其在零點(diǎn)附近區(qū)域連續(xù)。同時(shí)，在物理世界中衛(wèi)星收到了目標(biāo)源發(fā)射的信號(hào)，因此定位方程必有解。在這兩個(gè)條件下，由收斂性可知當(dāng)初始值位于零點(diǎn)附近區(qū)域時(shí)，迭代法的結(jié)果必定收斂。本文提出的融合初值校準(zhǔn)與二階逼近的單星測(cè)頻定位算法通過初值校準(zhǔn)使得初始值更加逼近定位目標(biāo)的真實(shí)位置(即定位方程的根)，進(jìn)而減少迭代次數(shù)；通過定位方程泰勒展開式保留二階定位信息，更好地?cái)M合原有方程，提高定位精度，從而實(shí)現(xiàn)高精度快速單星測(cè)頻定位算法。

2.1 初值校準(zhǔn)

圖3 初值校準(zhǔn)模型Fig.3 Initial value calibration model

由t0時(shí)刻定位目標(biāo)和衛(wèi)星的幾何關(guān)系可以得到，衛(wèi)星的速度矢量V0與由地心O、位置P0以及定位目標(biāo)位置S構(gòu)成的平面垂直，因此可以很容易地得出下列3個(gè)方程。其中式(8)表示該時(shí)刻衛(wèi)星運(yùn)動(dòng)方向和定位目標(biāo)與地心的連線垂直，式(9)為△OSP0的余弦定理，式(10)表示定位目標(biāo)處于球體表面。

(8)

(9)

x2+y2+z2=r2

(10)

式(8)和式(9)為三元一次方程，聯(lián)立式(8)和式(9)可以得到x0和y0關(guān)于z0的表達(dá)式，將其代入式(10)可以得到一個(gè)關(guān)于z0的一元二次方程。本文所提出的初值計(jì)算方法解決了多普勒單星定位算法初值選取困難的問題，相比優(yōu)化法引入大量計(jì)算代價(jià)的做法降低了定位解算時(shí)間、提高了定位效率。在實(shí)際應(yīng)用中，直接獲得多普勒頻移為零時(shí)的測(cè)距信息可能存在一定的困難，可以在多普勒頻移由正到零到負(fù)的時(shí)間段(即衛(wèi)星靠近定位目標(biāo)、衛(wèi)星與定位目標(biāo)相對(duì)距離最近、衛(wèi)星開始遠(yuǎn)離定位目標(biāo))不間斷地測(cè)距，選取多普勒頻移絕對(duì)值最小時(shí)刻的位置、速度以及測(cè)距信息代入上面方程組，求解出校準(zhǔn)的初值。

在初值校準(zhǔn)模型中，求解初值方程組時(shí)會(huì)得到兩個(gè)實(shí)數(shù)解。其中S點(diǎn)為定位目標(biāo)真實(shí)點(diǎn)，S′為定位目標(biāo)關(guān)于衛(wèi)星軌道平面的鏡像點(diǎn)。一般可以通過兩種方法去除鏡像點(diǎn)：①定位過程中目標(biāo)點(diǎn)總是位于軌道平面的一側(cè)。利用這個(gè)特點(diǎn)，將衛(wèi)星接收天線的主瓣朝向目標(biāo)點(diǎn)一側(cè)，將副瓣朝向鏡像點(diǎn)一側(cè)，在定位期間判斷接收信號(hào)的來波方向確定目標(biāo)點(diǎn)位置。②在不增加新的硬件和設(shè)備信息的前提下，可以通過衛(wèi)星的軌道特性來解決。通常單星測(cè)頻定位的衛(wèi)星為低軌道衛(wèi)星，隨著地球的自轉(zhuǎn)，衛(wèi)星軌道平面在地球坐標(biāo)系下的位置將發(fā)生變化，將兩次單星測(cè)頻定位結(jié)果作對(duì)比，其交集即為目標(biāo)點(diǎn)。

2.2 含二階定位信息的方程求解

牛頓迭代法求解定位方程組的本質(zhì)是通過泰勒展開，用一個(gè)有限的多項(xiàng)式去逼近原有的方程，使得方程的求解簡(jiǎn)單化。在多普勒單星定位算法中，泰勒展開的高階項(xiàng)被舍去，定位方程簡(jiǎn)化為一個(gè)線性方程，算法復(fù)雜度大大降低。然而，高階項(xiàng)的舍去也造成了一定的后果，即多項(xiàng)式方程組與原定位方程擬合度不夠。在一些其他的非線性方程組求解中，高階項(xiàng)的舍去不會(huì)造成比較大的偏差，然而在多普勒單星定位方程中，待求的坐標(biāo)點(diǎn)S(x，y，z)可以由cosθi演繹獲得，那么定位方程的展開實(shí)質(zhì)是對(duì)cosθi的泰勒展開。當(dāng)θi的誤差在1°左右時(shí)，cosθi的誤差在可忽略的數(shù)量級(jí)，但星球體的巨大半徑(例如在地球上，經(jīng)度每相差1°，實(shí)際距離相差在111 km左右)會(huì)使得最終的定位結(jié)果產(chǎn)生數(shù)千米甚至數(shù)十千米級(jí)的誤差。如果單星測(cè)頻定位方程組的泰勒展開項(xiàng)不足以逼近原有的方程，那么在求解過程中就人為地引入了千米級(jí)的誤差，這也是單星測(cè)頻定位誤差難以減小的原因。為了解決這個(gè)缺陷，將泰勒展開的二階級(jí)數(shù)平方項(xiàng)保留，并提出了求解算法。

將式(2)泰勒展開，保留二階級(jí)數(shù)平方項(xiàng)：

(11)

式(11)中含有平方項(xiàng)，難以直接求解。在本文提出的算法中，初值校準(zhǔn)使得初始點(diǎn)位置接近目標(biāo)的真實(shí)位置，因此每次迭代后，x、y、z三個(gè)方向上的差量較小，平方項(xiàng)和一次項(xiàng)對(duì)于最終定位結(jié)果的影響在同一級(jí)別。為了簡(jiǎn)化迭代過程，令Δu=Δx2，Δv=Δy2，Δw=Δz2，將式(11)化為線性方程：

(12)

其中，Δfi為ti時(shí)刻多普勒頻移，Δx、Δy和Δz分別表示前后兩次3個(gè)坐標(biāo)軸的估計(jì)差值，Δu、Δv、Δw分別表示前后兩次3個(gè)坐標(biāo)軸的估計(jì)差值的平方。選取6個(gè)或以上時(shí)刻的觀測(cè)數(shù)據(jù)代入式(12)，可以得到定位方程組：

(13)

在線性方程組(13)中，令

(14)

(15)

(16)

將方程組改為矩陣形式：

F=GS

(17)

對(duì)于式(17)，當(dāng)觀測(cè)數(shù)據(jù)超過6個(gè)時(shí)用最小二乘法求解：

S=(GTG)-1GTF

(18)

3 仿真與結(jié)果分析

3.1 STK仿真平臺(tái)搭建

本文利用衛(wèi)星工具包(Satellite Tool Kit, STK)軟件來搭建仿真平臺(tái)。為了驗(yàn)證所提算法的適用性，選取仿真環(huán)境為火星，設(shè)置衛(wèi)星軌道為圓軌道，高度為800 km，軌道傾角為45°，載頻為4.5 GHz，信號(hào)采樣時(shí)間為1 s。在衛(wèi)星通信的最大范圍內(nèi)隨機(jī)選取10個(gè)定位點(diǎn)，利用STK的access報(bào)告獲取衛(wèi)星星歷信息、速度位置信息以及多普勒頻移信息，作為算法的輸入?yún)?shù)。

3.2 實(shí)驗(yàn)結(jié)果分析

在3.1小節(jié)搭建的仿真平臺(tái)中，分別設(shè)置外部測(cè)距的誤差為1 km和0 km，設(shè)置衛(wèi)星接收機(jī)采樣頻率為1 Hz，利用STK獲取多普勒頻移為0時(shí)的衛(wèi)星位置及運(yùn)動(dòng)狀態(tài)數(shù)據(jù)。在仿真過程中，由式(1)可以得到定位期間接收頻率隨時(shí)間減小的特性，在選取觀測(cè)量時(shí)只需判斷其是否在前一秒和后一秒觀測(cè)量值組成的區(qū)間內(nèi)來避免觀測(cè)異常值對(duì)結(jié)果的影響。為了保證仿真結(jié)果的嚴(yán)謹(jǐn)性，選取多普勒頻移為0前180 s、120 s、60 s以及多普勒頻移為0后60 s、120 s、180 s共6組接收頻率數(shù)據(jù)，對(duì)多普勒單星定位算法[12]和本文算法各自建立6個(gè)方程聯(lián)立的定位方程組，從初值誤差、迭代收斂次數(shù)和定位誤差方面與多普勒單星定位算法進(jìn)行對(duì)比，從定位誤差方面與基于神經(jīng)網(wǎng)絡(luò)的單星波束掃描定位算法[13]進(jìn)行對(duì)比。

3.2.1 初值誤差分析

在3.1小節(jié)的仿真平臺(tái)中，將獲得的衛(wèi)星位置及運(yùn)動(dòng)狀態(tài)數(shù)據(jù)代入2.1小節(jié)中的初值方程組進(jìn)行求解，并與多普勒單星定位算法的星下點(diǎn)初值進(jìn)行對(duì)比，結(jié)果如圖4所示。

圖4 初值誤差對(duì)比Fig.4 Comparison of initial value error

從圖4可以看出，多普勒單星定位算法的初值平均誤差在18.77 km左右，而本文提出算法的初值平均誤差在測(cè)距誤差為1 km的情況下為6.23 km，減小了66.8%左右。從圖中還可以看到，測(cè)距誤差為0 km時(shí)，初值平均誤差與測(cè)距誤差在1 km情況下的非常接近，這是因?yàn)樵陔x地800 km的高度，初值校準(zhǔn)的結(jié)果對(duì)1 km級(jí)別的測(cè)距誤差并不敏感，這也印證了2.1小節(jié)中闡述的“初值校準(zhǔn)對(duì)測(cè)距誤差要求很低”的說法。精確的初值不僅可以減少后續(xù)迭代算法的次數(shù)，同時(shí)還可避免迭代不收斂情況的發(fā)生。

3.2.2 迭代收斂次數(shù)與定位誤差分析

為了驗(yàn)證“初值校準(zhǔn)”和“二階逼近”對(duì)于定位效果的影響，在前文的10個(gè)初值基礎(chǔ)上設(shè)置4組實(shí)驗(yàn)進(jìn)行對(duì)照實(shí)驗(yàn)。第一組為多普勒單星定位算法；第二組為測(cè)距誤差1 km下的二階逼近算法；第三組為測(cè)距誤差0 km下的二階逼近算法；第四組為多普勒單星定位算法初值代入二階逼近迭代算法。記錄以上4組實(shí)驗(yàn)的迭代收斂次數(shù)和定位誤差，結(jié)果如表1和表2所示。同時(shí)，將所提出的算法與基于神經(jīng)網(wǎng)絡(luò)的單星波束掃描定位算法在定位誤差方面進(jìn)行對(duì)比，結(jié)果如圖5所示。

表1 迭代收斂次數(shù)對(duì)比

表2 定位誤差對(duì)比

聯(lián)合表1和表2進(jìn)行綜合比對(duì)：比較第一組和第三組可以得到，多普勒單星定位算法的迭代收斂次數(shù)在6～8，平均誤差在2.247 km左右，而本文提出的算法迭代收斂次數(shù)為3～5，平均誤差在0.935 km左右，本文算法在迭代收斂次數(shù)和定位誤差上均有較大提升；比較第二組和第三組可以得到，本文算法在外部測(cè)距誤差為1 km和 0 km 情況下的迭代收斂次數(shù)和定位誤差一致，說明最終的定位誤差不受測(cè)距誤差的影響；比較第三組和第四組可以得到，第四組的迭代收斂次數(shù)高于第三組，第三、第四兩組的定位誤差一致，說明初值校準(zhǔn)降低了算法迭代次數(shù)，并且避免了定位結(jié)果不收斂情況的產(chǎn)生；比較第一組和第四組可以得到，兩者的迭代收斂次數(shù)基本相同，第四組的定位誤差低于第一組，說明二階逼近實(shí)現(xiàn)了定位誤差的降低，提高了定位精度。

圖5 定位誤差對(duì)比Fig.5 Comparison of localization error

由圖5可以得到，本文提出的算法的平均誤差為0.935 km，與基于神經(jīng)網(wǎng)絡(luò)的單星波束掃描定位算法平均0.880 km的誤差十分接近。仿真表明，本文提出的算法在迭代次數(shù)和定位誤差方面相比多普勒單星定位算法分別降低了49.5%和58.4%左右，同時(shí)杜絕了定位結(jié)果不收斂的情形，對(duì)于傳統(tǒng)算法而言是一次較大的突破。其定位誤差與基于神經(jīng)網(wǎng)絡(luò)的算法十分接近，并且由于計(jì)算量低，具有更好的適用性。

4 結(jié)論

針對(duì)單星測(cè)頻定位誤差較大的問題，提出了一種融合初值校準(zhǔn)與二階逼近的單星測(cè)頻定位算法，利用測(cè)距得到多普勒頻移為零時(shí)衛(wèi)星與定位目標(biāo)的距離信息，代入軌道平面與定位目標(biāo)的幾何關(guān)系解算出校準(zhǔn)的定位初值，將校準(zhǔn)初值代入含二階定位信息的定位方程泰勒展開式，聯(lián)立6個(gè)時(shí)刻的定位方程組迭代求解出最終結(jié)果。本文提出的算法無論在迭代次數(shù)還是定位精度上，相比多普勒單星定位算法都有很大的提高。同時(shí)，算法改進(jìn)使得定位方程組數(shù)量增加，在實(shí)際應(yīng)用中選取多普勒頻移零時(shí)刻前后各3個(gè)時(shí)間點(diǎn)列出定位方程，能夠有效地利用測(cè)量數(shù)據(jù)，降低頻率測(cè)量的隨機(jī)誤差對(duì)定位結(jié)果的影響。