一種約束魯棒加權(quán)最小二乘靜默定位算法

楊月怡 鄢社鋒 李 璇 毛琳琳

（1.中國(guó)科學(xué)院聲學(xué)研究所，北京 100190；2.中國(guó)科學(xué)院大學(xué)，北京 100049）

1 引言

隨著海洋戰(zhàn)略地位的提升和水聲技術(shù)的發(fā)展，水下定位技術(shù)引起了越來(lái)越多的關(guān)注，并在環(huán)境觀測(cè)、搜救捕撈等方面發(fā)揮了重要作用［1-4］。以北斗、GPS 為代表的陸地導(dǎo)航定位系統(tǒng)發(fā)展較為成熟，但目前適用于水下傳感網(wǎng)絡(luò)的定位方案仍相對(duì)較少，水下定位成為近年來(lái)的熱門研究課題［5-8］。

為獲取準(zhǔn)確的定位信息，現(xiàn)有的定位算法大多需要各傳感節(jié)點(diǎn)間保持嚴(yán)格的時(shí)鐘同步，但水下時(shí)鐘同步對(duì)軟件和硬件都提出了極高的要求。2008年，Cheng 等人首次提出水下靜默定位算法（Underwater Positioning Scheme，UPS）［9］，該方法無(wú)須待定位節(jié)點(diǎn)主動(dòng)通信且免時(shí)鐘同步，僅需四個(gè)參考節(jié)點(diǎn)即可構(gòu)造定位方程組，十分適合注重節(jié)能的水下傳感網(wǎng)絡(luò)中的定位。然而定位方程的解算并不十分簡(jiǎn)單，在參考節(jié)點(diǎn)布放陣型不佳時(shí)，容易產(chǎn)生較大的定位誤差。一般對(duì)定位方程的求解有迭代法和解析法。文獻(xiàn)［10］在TDOA/AOA 定位中提出一種基于泰勒展開的迭代約束加權(quán)最小二乘，有效降低了噪聲的影響，提高了定位精度。文獻(xiàn)［11］通過(guò)引入中間變量將方程轉(zhuǎn)化為約束加權(quán)最小二乘估計(jì)問題，然后根據(jù)牛頓迭代法求解問題。迭代法需要進(jìn)行多次迭代計(jì)算，復(fù)雜度高，且對(duì)初值的選擇和步長(zhǎng)敏感，還可能會(huì)面臨收斂問題。解析法因其不存在不收斂的問題，也被研究學(xué)者們廣泛關(guān)注。文獻(xiàn)［12］將定位問題轉(zhuǎn)換為二次規(guī)劃問題，首先通過(guò)拉格朗日法求解方程，然后進(jìn)一步在約束條件下進(jìn)行泰勒展開以優(yōu)化解析解。文獻(xiàn)［13］提出一種利用時(shí)頻差聯(lián)合定位水下運(yùn)動(dòng)目標(biāo)的方法，該方法使用兩步加權(quán)最小二乘估計(jì)目標(biāo)節(jié)點(diǎn)位置，首先求出運(yùn)動(dòng)節(jié)點(diǎn)位置、速度的初始估計(jì)值，然后對(duì)中間變量進(jìn)行泰勒展開，并帶回第一步中以消除忽略二次噪聲帶來(lái)的誤差，修正初始估計(jì)值。上述算法在觀測(cè)誤差有限的情況下可以達(dá)到較高的定位精度，但在復(fù)雜的水下環(huán)境中容易受噪聲干擾導(dǎo)致定位精度嚴(yán)重下降，且算法復(fù)雜度較高。為在有限觀測(cè)量的情況下提高定位精度，本文提出一種結(jié)合卡爾曼濾波的約束穩(wěn)健加權(quán)最小二乘算法。該方法首先將定位方程組進(jìn)行偽線性化，然后引入輔助變量并利用其與待求變量之間的關(guān)系對(duì)方程進(jìn)行約束進(jìn)而求出目標(biāo)節(jié)點(diǎn)的位置估計(jì)值。在對(duì)目標(biāo)節(jié)點(diǎn)的動(dòng)態(tài)定位中，通過(guò)對(duì)定位結(jié)果的卡爾曼濾波進(jìn)一步消除定位誤差，提高定位精度。仿真及海試數(shù)據(jù)處理結(jié)果表明，本文所提算法可有效提高定位精度。

2 靜默定位模型

假設(shè)水下三維空間中分布著4個(gè)已知自身位置的參考節(jié)點(diǎn)，第i個(gè)參考節(jié)點(diǎn)Bi的坐標(biāo)為[xi，yi，zi]T，i=1，2，3，4，并將第一個(gè)參考節(jié)點(diǎn)設(shè)為主參考節(jié)點(diǎn)。待定位的目標(biāo)節(jié)點(diǎn)S坐標(biāo)為[x，y，z]T。UPS 算法首先需計(jì)算各節(jié)點(diǎn)之間的距離，令

水下靜默定位算法過(guò)程如下：首先，主參考節(jié)點(diǎn)B1每隔T秒周期性廣播定位信號(hào)，發(fā)送信號(hào)中包括參考節(jié)點(diǎn)自身位置和時(shí)鐘信息等。在n1時(shí)刻，待定位節(jié)點(diǎn)S接收到來(lái)自參考節(jié)點(diǎn)B1廣播的信標(biāo)信號(hào)。在參考節(jié)點(diǎn)B2接收到來(lái)自參考節(jié)點(diǎn)B1的信標(biāo)信號(hào)一段時(shí)間Δ2后，開始廣播其信標(biāo)信號(hào)，除自身位置信息外，還包含處理時(shí)間Δ2，待定位節(jié)點(diǎn)S在n2時(shí)刻接收到來(lái)自參考節(jié)點(diǎn)B2的信標(biāo)信號(hào)。同樣，節(jié)點(diǎn)B3、B4在收到來(lái)自B1的信標(biāo)信號(hào)后開始廣播包含其位置信息和處理時(shí)間的信標(biāo)信號(hào)。待定位節(jié)點(diǎn)S分別在時(shí)刻n3、n4接收到來(lái)自信標(biāo)節(jié)點(diǎn)B3、B4的信標(biāo)信號(hào)。定位過(guò)程如圖1 所示。令v為水下聲速，Δni=ni-n1，i=2，3，4 為目標(biāo)節(jié)點(diǎn)本地時(shí)鐘記錄的定位信號(hào)到達(dá)時(shí)間差。由此，可得到距離方程：

圖1 UPS算法過(guò)程Fig.1 UPS positioning process

對(duì)方程進(jìn)行簡(jiǎn)化：

由上述公式可得：待定位節(jié)點(diǎn)僅利用自身時(shí)鐘記錄各定位信號(hào)達(dá)到時(shí)間即可獲得參考節(jié)點(diǎn)與自身的距離。利用圖2的幾何關(guān)系即可得到待定位目標(biāo)的位置求解方程組：

圖2 幾何定位模型Fig.2 Geometric positioning model

UPS 算法采用降階法求解上述方程，通過(guò)簡(jiǎn)單的數(shù)學(xué)計(jì)算即可得到目標(biāo)位置的解析解。這種解法無(wú)須迭代，計(jì)算復(fù)雜度低，但定位誤差偏大。

本文基于上述靜默定位模型，提出一種結(jié)合卡爾曼濾波的約束穩(wěn)健加權(quán)最小二乘算法。該算法首先將靜默定位問題轉(zhuǎn)化為二次規(guī)劃問題，然后利用拉格朗日乘子法和廣義奇異值分解求出目標(biāo)節(jié)點(diǎn)的坐標(biāo)。

3 基于靜默定位模型的算法改進(jìn)

3.1 約束穩(wěn)健加權(quán)最小二乘定位算法

在實(shí)際應(yīng)用中，待定位節(jié)點(diǎn)的深度信息可由壓力計(jì)等其他外部設(shè)備得到，作為已知信息。此時(shí)，可將非線性方程組轉(zhuǎn)化為偽線性方程組，并利用最小二乘（Least Square，LS）方法進(jìn)行求解。具體來(lái)說(shuō)，將式（5）整理為偽線性方程組：

LS 算法中假設(shè)X中的變量x，y，d1s是相互獨(dú)立的，然而目標(biāo)節(jié)點(diǎn)與參考節(jié)點(diǎn)B1的距離d1s為輔助變量且存在。因此，利用這一相關(guān)性，本節(jié)提出一種約束穩(wěn)健加權(quán)最小二乘（Constrained robust weighted least square，CRWLS）算法，對(duì)定位結(jié)果進(jìn)行改進(jìn)。將靜默定位問題轉(zhuǎn)化為線性約束最優(yōu)化問題，則可以構(gòu)建目標(biāo)函數(shù)：

即將靜默定位問題轉(zhuǎn)換為帶有二次約束的二次規(guī)劃問題。式中Xa=[x1，y1，0]T，Σ=diag(1，1，-1)，c=-(z-z1)2，W為加權(quán)矩陣。當(dāng)W為單位陣時(shí)，各觀測(cè)量權(quán)值相同。

利用拉格朗日乘子法求解上述方程，即：

式中λ為拉格朗日乘子。對(duì)式（8）中X求導(dǎo)并令其為0，可得：

將式（9）代回約束條件可得拉格朗日乘子λ的解。然后利用廣義奇異值分解（Generalized singular value decomposition，GSVD）的方法將ATW-1AΣ對(duì)角化為：

式中Λ=diag(ζ1，ζ2，ζ3)為對(duì)角矩陣，因此式（8）可重新寫作：

式中p=[p1，p2，p3]T=UTΣ(ATW-1b-ATW-1AXa)，q=[q1，q2，q3]T=U-1(ATW-1b-ATW-1AXa)。可通過(guò)卷積計(jì)算簡(jiǎn)化式（11），并對(duì)多項(xiàng)式進(jìn)行求根運(yùn)算，考慮到實(shí)際意義，僅保留方程的實(shí)數(shù)根。并將實(shí)數(shù)解依次帶入式（9）得到對(duì)應(yīng)的估計(jì)值，再將帶入目標(biāo)函數(shù)J(X)，找到使目標(biāo)函數(shù)最小的X即為最優(yōu)解。

當(dāng)各觀測(cè)量中的噪聲都為高斯分布時(shí)，CRWLS算法中的權(quán)值矩陣取單位陣即可得到較為準(zhǔn)確的目標(biāo)節(jié)點(diǎn)位置估計(jì)值。然而由于水下聲速分布的非均勻性，聲波在水下并非沿直線傳播，在不對(duì)聲線彎曲進(jìn)行修正的情況下，若僅采用固定聲速來(lái)計(jì)算水聲信號(hào)的傳播距離將產(chǎn)生較大誤差［14］。本節(jié)通過(guò)穩(wěn)健加權(quán)方法，根據(jù)殘差賦予各觀測(cè)量權(quán)值。權(quán)函數(shù)在平差過(guò)程中計(jì)算殘差，殘差越大，認(rèn)為其可信度越低，所對(duì)應(yīng)的權(quán)函數(shù)越小，經(jīng)過(guò)多次迭代可使殘差小的觀測(cè)量占比更大，減少噪聲對(duì)定位精度的影響。構(gòu)建殘差方程為：

求解上式可得殘差V的估計(jì)值，并以此構(gòu)建權(quán)值矩陣W，并帶回方程，迭代更新。常見的等價(jià)權(quán)函數(shù)有Huber 權(quán)函數(shù)、IGG3 權(quán)函數(shù)、Tukey 權(quán)函數(shù)等［15］，不論形式如何變化，權(quán)函數(shù)總在平差過(guò)程中隨殘差改變，實(shí)際應(yīng)用時(shí)可根據(jù)實(shí)際情況選取，本節(jié)選擇IGG3權(quán)函數(shù)，其權(quán)重因子為：

通常k0取值為1～1.5，k1取3。式中，|分別為系數(shù)矩陣與第i個(gè)觀測(cè)量的殘差為權(quán)值因子。等價(jià)權(quán)函數(shù)隨殘差的增大而減小，對(duì)噪聲較大的觀測(cè)數(shù)據(jù)進(jìn)行降權(quán)處理，從而減小了觀測(cè)信息中的噪聲對(duì)定位精度的影響。

根據(jù)上述公式，最終得到約束穩(wěn)健加權(quán)最小二乘算法迭代求解步驟如表1所示。

表1 約束穩(wěn)健加權(quán)最小二乘算法流程Tab.1 Constrained robust weighted least squares algorithm flow

3.2 卡爾曼濾波算法

水下定位系統(tǒng)中，觀測(cè)量的精度決定了系統(tǒng)的定位精度。然而由于水下復(fù)雜的物理環(huán)境，觀測(cè)量往往受到各種噪聲的影響，從而存在一定誤差［16］。當(dāng)待定位節(jié)點(diǎn)處于運(yùn)動(dòng)狀態(tài)中時(shí)，卡爾曼濾波算法因其可以利用更多測(cè)量信息，相比每時(shí)刻獨(dú)立定位能夠獲得更好的定位估計(jì)精度。因此，為進(jìn)一步提高定位精度，可利用運(yùn)動(dòng)狀態(tài)的連續(xù)性，進(jìn)一步擴(kuò)展UPS 定位算法，使用卡爾曼濾波對(duì)定位結(jié)果進(jìn)行預(yù)測(cè)，提高定位精度。

利用k時(shí)刻UPS 解算的定位結(jié)果構(gòu)造k時(shí)刻的狀態(tài)向量為X(k)=[xk，yk，vk，vy]T，分別為k時(shí)刻目標(biāo)節(jié)點(diǎn)在x方向、y方向上的坐標(biāo)和速度，然后建立狀態(tài)方程和觀測(cè)方程：

式中F為狀態(tài)轉(zhuǎn)移矩陣；Z(k)=[xk，yk]T為k時(shí)刻觀測(cè)量；M(k)為系統(tǒng)噪聲；H為觀測(cè)矩陣；V(k)為觀測(cè)噪聲。其中，M(k)和V(k)是互不相關(guān)的零均值高斯分布序列，其協(xié)方差矩陣分別為Q(k)、R(k)。假設(shè)目標(biāo)節(jié)點(diǎn)做勻速直線運(yùn)動(dòng)，將速度的變化視為水體的擾動(dòng)，則：

由式（14）可得狀態(tài)向量X(k)的進(jìn)一步預(yù)測(cè)和k時(shí)刻的預(yù)測(cè)誤差協(xié)方差矩陣為：

利用上一節(jié)中提出的算法得到目標(biāo)節(jié)點(diǎn)位置信息后，可更新觀測(cè)值Z(k)。此時(shí)k時(shí)刻狀態(tài)變量的估計(jì)可由上一時(shí)刻的預(yù)測(cè)值和觀測(cè)量Z(k)聯(lián)合表示為：

其對(duì)應(yīng)的協(xié)方差矩陣為：

其中，K(k)為卡爾曼濾波增益：

4 仿真分析

為了驗(yàn)證文章所提算法的有效性和可靠性首先進(jìn)行仿真實(shí)驗(yàn)。為模擬水下環(huán)境，在仿真實(shí)驗(yàn)中以Munk 聲速剖面為背景，利用射線聲學(xué)原理模擬聲波在水下的傳播軌跡，進(jìn)而求出聲波的傳播時(shí)間，并進(jìn)行后續(xù)實(shí)驗(yàn)。實(shí)驗(yàn)中設(shè)置四個(gè)參考節(jié)點(diǎn)，按矩形設(shè)置基陣位置，參考節(jié)點(diǎn)位置分布如表2所示。

表2 參考節(jié)點(diǎn)布放參數(shù)Tab.2 Reference node layout parameters

4.1 測(cè)時(shí)誤差影響分析

UPS定位算法在本地記錄來(lái)自各個(gè)參考節(jié)點(diǎn)定位信號(hào)的到達(dá)時(shí)間，并以此計(jì)算出各參考節(jié)點(diǎn)與目標(biāo)節(jié)點(diǎn)之間的距離，從而得到目標(biāo)節(jié)點(diǎn)的位置估計(jì)。這種方法免去了時(shí)鐘同步從而不存在時(shí)鐘差誤差［17］，但聲波在水下的傳播會(huì)發(fā)生能量衰減，使定位信號(hào)被淹沒在噪聲中，同時(shí)多徑效應(yīng)、水面反射、多普勒效應(yīng)［18］等仍會(huì)影響測(cè)時(shí)準(zhǔn)確性。為分析測(cè)時(shí)誤差的影響，假設(shè)測(cè)時(shí)誤差為高斯分布，誤差范圍0～4 ms，參考節(jié)點(diǎn)坐標(biāo)如表2所示，目標(biāo)節(jié)點(diǎn)位置選擇接近陣中心處，坐標(biāo)為[1500，1500，100]T。進(jìn)行1000 次蒙特卡洛仿真實(shí)驗(yàn)，并用均方根誤差RMSE評(píng)估，如式（22）。

圖3 不同測(cè)時(shí)誤差時(shí)的定位結(jié)果均方根誤差Fig.3 RMSE under different delay measurement errors

4.2 聲速誤差影響分析

水下聲速是UPS 算法中的基本參量之一，UPS中距離信息是由時(shí)延差和聲速的乘積獲取，故聲速的變化很大程度上會(huì)影響UPS 定位精度。由于海水的分層特性，水下存在聲速?gòu)澢鷨栴}，通常認(rèn)為海水中的平均聲速約等于1500 m/s。在不能獲取水下聲速剖面的情況下，等效聲速的設(shè)置將直接影響UPS定位的準(zhǔn)確性。為分析不同聲速下UPS算法的定位誤差，將等效聲速分別在1489 m/s～1519 m/s 處取值，節(jié)點(diǎn)位置與上述實(shí)驗(yàn)相同，并用均方根誤差RMSE 評(píng)估定位性能。圖4 給出了在不同等效聲速下，經(jīng)典LS算法和本文CRWLS算法的均方誤差，可以看到，在等效聲速設(shè)置的較為合適時(shí)，LS 算法與CRWLS算法估計(jì)誤差相近，但在等效聲速估計(jì)誤差較大的情況下，LS 算法定位誤差迅速增加，導(dǎo)致算法失效，而CRWLS 算法的定位誤差增長(zhǎng)速度明顯小于LS 算法，相比之下更為穩(wěn)健。這是因?yàn)長(zhǎng)S 算法在對(duì)目標(biāo)節(jié)點(diǎn)進(jìn)行定位時(shí)將目標(biāo)節(jié)點(diǎn)位置與輔助變量視為相互獨(dú)立的變量，從而對(duì)定位方程進(jìn)行偽線性化，而CRWLS 算法考慮了待求變量中誤差的相關(guān)性，且穩(wěn)健權(quán)函數(shù)可以有效降低觀測(cè)信息中噪聲的影響。

圖4 不同聲速下的定位結(jié)果均方根誤差Fig.4 RMSE under different sound velocity

4.3 靜態(tài)定位誤差分析

首先進(jìn)行靜態(tài)定位實(shí)驗(yàn)，參考節(jié)點(diǎn)位置如表3所示，深度保持100 m 不變。圖5 給出了LS 算法和CRWLS 算法的定位結(jié)果，可以看出本文提出的CRWLS 算法相比于經(jīng)典LS 算法定位誤差更小，尤其在陣的邊緣位置處，LS算法得到的位置估計(jì)已明顯偏移目標(biāo)節(jié)點(diǎn)真實(shí)位置，算法失效。仿真實(shí)驗(yàn)中LS 算法的估計(jì)誤差達(dá)74.8 m，而本文所提出的CRWLS算法誤差為6.5 m，定位誤差明顯下降。

表3 參考節(jié)點(diǎn)布放參數(shù)Tab.3 Reference node layout parameters

圖5 陣內(nèi)不同位置處定位結(jié)果的均方根誤差Fig.5 The RMSE for different position within the array

4.4 動(dòng)態(tài)定位誤差分析

進(jìn)一步地，本節(jié)進(jìn)行水下動(dòng)態(tài)定位實(shí)驗(yàn)。UPS算法的觀測(cè)量中必然存在著一些誤差，使用卡爾曼濾波可在一定程度上消除噪聲的影響，有效跟蹤目標(biāo)節(jié)點(diǎn)的運(yùn)動(dòng)。動(dòng)態(tài)定位的仿真實(shí)驗(yàn)環(huán)境設(shè)置與靜態(tài)定位實(shí)驗(yàn)相同。假定目標(biāo)節(jié)點(diǎn)在x方向上移動(dòng)的速度為1.5 m/s，y方向上移動(dòng)速度為2 m/s，20 s后轉(zhuǎn)向，保持x方向速度不變，y方向速度降至1.6 m/s，目標(biāo)節(jié)點(diǎn)深度始終保持100 m 不變。根據(jù)CRWLS 算法得到目標(biāo)位置觀測(cè)值后，利用卡爾曼濾波算法進(jìn)一步改進(jìn)定位結(jié)果，定位誤差如表4所示?？梢?，卡爾曼濾波進(jìn)一步減小了定位誤差。

表4 卡爾曼濾波前后誤差對(duì)比Tab.4 Error comparison before and after Kalman filter

5 試驗(yàn)驗(yàn)證

為進(jìn)一步驗(yàn)證所提算法有效性，利用在青島膠州灣展開的水下靜默定位實(shí)驗(yàn)中獲取到的數(shù)據(jù)對(duì)所提算法進(jìn)行驗(yàn)證?？紤]到試驗(yàn)的便捷性，令待定位節(jié)點(diǎn)與試驗(yàn)?zāi)复３诌B接，利用10 m長(zhǎng)線纜將其吊放入水并在其上方水面處架設(shè)GPS 天線，為定位提供位置參考。實(shí)際定位時(shí)會(huì)停船測(cè)試，保持GPS和吊放設(shè)備相對(duì)靜止。實(shí)驗(yàn)設(shè)備如圖6所示。

圖6 試驗(yàn)中所用設(shè)備及節(jié)點(diǎn)布放過(guò)程Fig.6 Equipments used in sea trial and node deployment process

將定位精度較高的GPS 給出的運(yùn)動(dòng)軌跡作為位置參考，將解算出的目標(biāo)軌跡與GPS 定位軌跡進(jìn)行對(duì)比，對(duì)所提出的定位算法進(jìn)行驗(yàn)證，定位結(jié)果如圖7所示。圖中黑色實(shí)線為GPS給出的目標(biāo)節(jié)點(diǎn)運(yùn)動(dòng)軌跡，本文將其作為目標(biāo)節(jié)點(diǎn)在水下的實(shí)際運(yùn)動(dòng)軌跡；紅色虛線為經(jīng)典LS算法估計(jì)出的目標(biāo)節(jié)點(diǎn)運(yùn)動(dòng)軌跡；藍(lán)色點(diǎn)劃線為本文所提出的CRWLS 算法解算出的運(yùn)動(dòng)軌跡；紫色點(diǎn)虛線為卡爾曼濾波后的CRWLS 算法。可以看出，由LS 算法解算出的目標(biāo)節(jié)點(diǎn)定位軌跡已明顯偏移真實(shí)運(yùn)動(dòng)軌跡；而本文所提CRWLS 算法得出的目標(biāo)節(jié)點(diǎn)定位軌跡已較為接近GPS 給出的運(yùn)動(dòng)軌跡，進(jìn)一步地，經(jīng)過(guò)卡爾曼濾波后，解出的目標(biāo)節(jié)點(diǎn)運(yùn)動(dòng)軌跡更為光滑，定位誤差也更小，精度更高。仿真和海試實(shí)驗(yàn)都表明本文所提出的約束穩(wěn)健加權(quán)最小二乘算法得到的定位結(jié)果更接近目標(biāo)的實(shí)際運(yùn)動(dòng)軌跡，而卡爾曼濾波的加入進(jìn)一步使得定位軌跡更為平滑，更接近實(shí)際運(yùn)動(dòng)規(guī)律。本文所提算法求出的目標(biāo)節(jié)點(diǎn)運(yùn)動(dòng)軌跡與真實(shí)位置仍存在一定誤差，這是由于海水是非均勻介質(zhì)，聲波在水下傳播時(shí)會(huì)發(fā)生折射反射現(xiàn)象，而并非沿直線行進(jìn)。UPS 算法將聲波在水下的傳播速度設(shè)為常數(shù)，結(jié)合測(cè)得的傳播時(shí)間直接計(jì)算出定位信號(hào)的傳播距離，并未考慮聲波在水下的傳播特性，因此存在較大的測(cè)量誤差。要解決這一問題可射線聲學(xué)原理對(duì)定位結(jié)果進(jìn)行修正［19］。

6 結(jié)論

針對(duì)水下靜默定位算法精度較差的問題，本文提出一種結(jié)合卡爾曼濾波的約束穩(wěn)健加權(quán)最小二乘算法。該算法將目標(biāo)節(jié)點(diǎn)到參考節(jié)點(diǎn)之間的距離作為輔助變量并利用其與目標(biāo)節(jié)點(diǎn)坐標(biāo)之間的相關(guān)性，對(duì)方程進(jìn)行約束。通過(guò)引入拉格朗日乘子法和廣義奇異值分解，本文給出了目標(biāo)節(jié)點(diǎn)坐標(biāo)的解析解。通過(guò)對(duì)觀測(cè)數(shù)據(jù)進(jìn)行穩(wěn)健加權(quán)，對(duì)噪聲較大的數(shù)據(jù)進(jìn)行降權(quán)處理，CRWLS算法可有效提高對(duì)噪聲的魯棒性，提高靜默定位算法的定位精度。在對(duì)目標(biāo)節(jié)點(diǎn)進(jìn)行動(dòng)態(tài)定位時(shí)，本文進(jìn)一步利用卡爾曼濾波對(duì)定位結(jié)果進(jìn)行處理以消除噪聲的影響。仿真與試驗(yàn)數(shù)據(jù)處理結(jié)果表明，相比于經(jīng)典最小二乘算法，約束穩(wěn)健加權(quán)最小二乘算法可以獲得更加準(zhǔn)確且更穩(wěn)健的定位結(jié)果。