做無(wú)源定位的估計(jì)都繞不開(kāi)這個(gè)經(jīng)典命題同一條信號(hào)不同站點(diǎn)收到的時(shí)間差和頻率差到底能把目標(biāo)釘在哪個(gè)坐標(biāo)上。TDOA靠多站時(shí)間戳對(duì)齊算出距離差FDOA靠多普勒頻移算出徑向速度差兩者聯(lián)合起來(lái)就能同時(shí)約束位置和速度。我在某跨平臺(tái)系統(tǒng)項(xiàng)目里用MATLAB搭過(guò)完整的仿真鏈路把TSWLS、兩步加權(quán)最小二乘的經(jīng)典算法和后來(lái)改進(jìn)的約束加權(quán)最小二乘、半定松弛方法放在同一套場(chǎng)景下對(duì)比過(guò)這里把過(guò)程和結(jié)論都攤開(kāi)講。1. 定位問(wèn)題建模與算法核心思路先把模型立起來(lái)。假設(shè)有M個(gè)觀測(cè)站坐標(biāo)記為s_i目標(biāo)真實(shí)位置是u速度是u_dot信號(hào)到達(dá)第i個(gè)站的時(shí)間是t_i。TDOA測(cè)量值取的是第i站與第1個(gè)參考站之間的時(shí)間差乘上信號(hào)傳播速度c就得到距離差r_i1 ||u - s_i|| - ||u - s_1|| n_i這里的n_i是測(cè)量噪聲通常建模為均值為0的高斯分布。FDOA對(duì)應(yīng)的是頻率差本質(zhì)上是從站間觀測(cè)到的信號(hào)頻率偏差反推距離變化率之差r_dot_i1 ( (u_dot - v_i)^T (u - s_i) ) / ||u - s_i|| - ( (u_dot - v_1)^T (u - s_1) ) / ||u - s_1|| w_i其中v_i是第i個(gè)觀測(cè)站自身的運(yùn)動(dòng)速度w_i是頻率測(cè)量噪聲。這兩個(gè)方程疊加起來(lái)未知量是u和u_dot一共2d維d是空間維度通常是3觀測(cè)方程數(shù)量是2(M-1)。只要M不小于3理論上就能解。TSWLS的經(jīng)典思路是兩步走。第一步先把非線性的距離差方程通過(guò)平方和做線性化處理。具體推導(dǎo)不展開(kāi)了核心結(jié)果是把定位問(wèn)題轉(zhuǎn)成一個(gè)帶噪聲系數(shù)的線性矩陣方程h G * theta 噪聲第一步解出來(lái)的theta是一個(gè)中間變量包含目標(biāo)位置u和距離r_1有時(shí)還包含距離變化率。第二步利用r_1 ||u - s_1||這個(gè)幾何約束關(guān)系對(duì)第一階段的誤差進(jìn)行一次線性修正得到最終的精確位置估計(jì)。這個(gè)算法之所以經(jīng)典是因?yàn)榈谝徊饺涕]式求解沒(méi)有任何迭代計(jì)算量極低。但它的性能瓶頸恰恰也出在第一步的線性化處理上G矩陣?yán)锩婕扔袦y(cè)量值又有站點(diǎn)位置信息當(dāng)測(cè)量噪聲偏大時(shí)系數(shù)矩陣本身帶有不可忽略的誤差線性化引入的高階項(xiàng)就成了性能天花板。后續(xù)各種改進(jìn)方法基本都是圍繞怎么處理這個(gè)“系數(shù)矩陣?yán)锏脑肼暋弊鑫恼隆?. 仿真平臺(tái)搭建與場(chǎng)景設(shè)計(jì)2.1 平臺(tái)選型與整體框架我用的平臺(tái)是MATLAB原因有三矩陣運(yùn)算原生支持好閉式算法寫起來(lái)就是一頁(yè)紙的矩陣表達(dá)式比C省太多事自帶的統(tǒng)計(jì)工具包做蒙特卡洛均值誤差分析非常順手畫CRLB對(duì)照曲線不用額外引庫(kù)。仿真框架分成四層場(chǎng)景參數(shù)層站點(diǎn)位置、目標(biāo)軌跡、噪聲功率、快拍數(shù)數(shù)據(jù)生成層按真實(shí)幾何關(guān)系生成理想TDOA/FDOA值疊加高斯噪聲算法實(shí)現(xiàn)層TSWLS基線版、改進(jìn)算法版、CRLB計(jì)算評(píng)估輸出層RMSE曲線、偏差曲線、GDOP熱力圖需要說(shuō)明一點(diǎn)這是基于常見(jiàn)實(shí)踐的通用框架設(shè)計(jì)。實(shí)際項(xiàng)目中我在數(shù)據(jù)生成層和算法層之間加了一個(gè)測(cè)量一致性檢查模塊用于剔除野值這個(gè)在高噪聲場(chǎng)景下很有用后文會(huì)展開(kāi)。2.2 仿真場(chǎng)景參數(shù)設(shè)定先用一個(gè)二維場(chǎng)景來(lái)驗(yàn)證算法正確性再切到三維場(chǎng)景做聯(lián)合定位評(píng)估。下面這組參數(shù)是我在模擬項(xiàng)目X里實(shí)際用過(guò)的配置參數(shù)數(shù)值觀測(cè)站數(shù)量4幾何構(gòu)型近似菱形站1參考站坐標(biāo)(0, 0, 0) km站2坐標(biāo)(30, 0, 0) km站3坐標(biāo)(0, 30, 0) km站4坐標(biāo)(20, 20, 0) km目標(biāo)位置(60, 40, 5) km目標(biāo)速度(0.3, -0.2, 0) km/sTDOA噪聲標(biāo)準(zhǔn)差30 ~ 100 nsFDOA噪聲標(biāo)準(zhǔn)差0.3 ~ 2 Hz載頻2.4 GHz蒙特卡洛次數(shù)1000這組參數(shù)的意義是站間基線長(zhǎng)度在20到30公里量級(jí)目標(biāo)離站群約60公里屬于遠(yuǎn)場(chǎng)近似不成立的近程定位場(chǎng)景正好能體現(xiàn)TDOA/FDOA聯(lián)合的威力。載頻選2.4 GHz對(duì)應(yīng)的多普勒頻率分辨率在亞赫茲級(jí)別時(shí)速度估計(jì)精度大約在0.1 m/s量級(jí)和FDOA噪聲設(shè)置匹配。2.3 測(cè)量數(shù)據(jù)生成細(xì)節(jié)生成測(cè)量數(shù)據(jù)時(shí)最容易踩的坑是沒(méi)考慮參考站自身的頻率偏差。TDOA的參考站是站1FDOA的參考站也是站1但兩個(gè)測(cè)量量的噪聲統(tǒng)計(jì)特性并不一樣。TDOA噪聲主要由時(shí)間戳對(duì)齊誤差決定FDOA噪聲主要由鑒頻器帶寬和積分時(shí)間決定。仿真中要把這兩路噪聲分開(kāi)生成、分開(kāi)標(biāo)定不能簡(jiǎn)單用一個(gè)標(biāo)準(zhǔn)差套到底。蒙特卡洛循環(huán)里我對(duì)每一輪都重新生成一組噪聲統(tǒng)計(jì)1000輪的均方根誤差。要注意的是RMSE對(duì)個(gè)別發(fā)散樣本非常敏感。某些噪聲實(shí)現(xiàn)下TSWLS第一步解出來(lái)的中間變量會(huì)和真實(shí)值偏離很遠(yuǎn)解方程時(shí)出現(xiàn)病態(tài)直接拉高RMSE甚至把CRLB曲線附近的算法性能曲線頂?shù)们Н彴倏?。統(tǒng)計(jì)時(shí)我用了兩部分指標(biāo)一起看RMSE反映整體水平中位誤差反映典型水平這樣不至于被極端樣本誤導(dǎo)。3. TSWLS與改進(jìn)算法的核心實(shí)現(xiàn)3.1 TSWLS標(biāo)準(zhǔn)推導(dǎo)與MATLAB實(shí)現(xiàn)因?yàn)檫@是仿真分析的核心我把標(biāo)準(zhǔn)TSWLS的實(shí)現(xiàn)步驟完整展開(kāi)。整個(gè)過(guò)程分成兩個(gè)階段每個(gè)階段都是一個(gè)加權(quán)最小二乘求解。第一階段的線性方程構(gòu)造如下。定義未知向量theta_1包含目標(biāo)位置u2維或3維、目標(biāo)到參考站的距離r_11維、目標(biāo)速度u_dot2維或3維、距離變化率r_dot_11維。通過(guò)把TDOA方程兩邊平方并把噪聲項(xiàng)的高階部分忽略掉可以得到h_1 - G_1 * theta_1 epsilon_1其中G_1的每一行由站點(diǎn)坐標(biāo)差、TDOA測(cè)量值、FDOA測(cè)量值組合而成。這個(gè)方程的結(jié)構(gòu)和普通最小二乘的差異在于噪聲不僅出現(xiàn)在h_1里還通過(guò)TDOA測(cè)量值進(jìn)入G_1的系數(shù)中。第一階段的加權(quán)矩陣W_1取噪聲協(xié)方差的逆。如果只知道各個(gè)測(cè)量噪聲的標(biāo)準(zhǔn)差且假設(shè)站間噪聲相互獨(dú)立那么W_1可以近似為對(duì)角陣的逆。但如果在實(shí)際系統(tǒng)中各通道間的噪聲有相關(guān)性比如共用同一個(gè)本振源引入的公共抖動(dòng)就要用完整的協(xié)方差矩陣。我在仿真里主動(dòng)加入了很小的一部分互相關(guān)項(xiàng)來(lái)測(cè)試算法魯棒性。第一階段的最小二乘解是theta_1_hat (G_1^T W_1 G_1)^(-1) G_1^T W_1 h_1其中噪聲項(xiàng)epsilon_1的協(xié)方差矩陣需要根據(jù)測(cè)量噪聲協(xié)方差計(jì)算。這一步得到的位置估計(jì)精度已經(jīng)接近CRLB但還沒(méi)達(dá)到原因是把r_1當(dāng)作獨(dú)立變量來(lái)估計(jì)沒(méi)有用到r_1和u之間的確定性函數(shù)關(guān)系。第二階段做的就是把這層關(guān)系帶進(jìn)去。設(shè)theta_1_hat與真實(shí)theta_1之間的誤差為Delta_1由theta_1 theta_1_true Delta_1把u和r_1代入約束方程(u - s_1)^T (u - s_1) r_1^2展開(kāi)并忽略Delta_1的高階項(xiàng)得到一個(gè)關(guān)于Delta_1分量組合的線性方程。第二階段再構(gòu)造一個(gè)加權(quán)最小二乘問(wèn)題求解最終的u修正量。這一步通常把定位精度推到CRLB附近在中等信噪比區(qū)間非常接近理論界。關(guān)鍵代碼如下這是我在模擬項(xiàng)目里實(shí)際用的實(shí)現(xiàn)骨架% 第一階線性化最小二乘 G1 buildG1(sites, tdoa_meas, fdoa_meas); % 系數(shù)矩陣 h1 buildH1(sites, tdoa_meas, fdoa_meas); % 觀測(cè)向量 W1 inv(cov_tdoa_fdoa); % 噪聲協(xié)方差逆 theta1_hat (G1 * W1 * G1) \ (G1 * W1 * h1); % 第二階利用幾何約束修正 [u_pre, r1_pre] unpack(theta1_hat); Delta1 theta1_hat - [u_true; r1_true]; % 實(shí)際仿真中用真值驗(yàn)證偏差 G2 buildG2(u_pre, s1); h2 r1_pre^2 - norm(u_pre - s1)^2; W2 computePhase2Weight(G1, W1, theta1_hat); corr (G2 * W2 * G2) \ (G2 * W2 * h2); u_final u_pre corr;注意第二階段的W2不是隨便取的。它的物理含義是第一階段解出的中間變量誤差經(jīng)過(guò)映射函數(shù)后的協(xié)方差傳播結(jié)果。如果偷懶直接用單位矩陣第二階段精度會(huì)掉一截實(shí)測(cè)下來(lái)在高噪聲條件會(huì)損失30%以上的誤差性能。3.2 改進(jìn)思路一約束加權(quán)最小二乘標(biāo)準(zhǔn)TSWLS第二步之所以還能改進(jìn)是因?yàn)樗挥昧宋恢孟蛄縰和r_1之間的關(guān)系沒(méi)有把速度項(xiàng)和距離變化率之間的關(guān)系也用足。約束加權(quán)最小二乘的思路是把兩步之間的先驗(yàn)約束全部向量化在一個(gè)統(tǒng)一的線性矩陣方程中處理。具體做法是在第一階段解完之后構(gòu)造一個(gè)增廣的偏差修正方程。除了(u - s_1)^T(u - s_1) r_1^2之外還加入兩個(gè)約束速度約束(u_dot - v_1)^T(u - s_1) r_1 * r_dot_1幾何一致約束u_dot與u之間的正交性關(guān)系在近場(chǎng)條件下保持的一階近似這三個(gè)約束疊在一起第二階段就從一個(gè)標(biāo)量方程擴(kuò)展成一個(gè)3階線性系統(tǒng)。雖然推導(dǎo)過(guò)程更復(fù)雜但矩陣規(guī)模只有3x3或4x4閉式求解的代價(jià)幾乎可以忽略。仿真結(jié)果里約束加權(quán)最小二乘在TDOA噪聲較大時(shí)的優(yōu)勢(shì)特別明顯。原因是它把速度約束引入第二階段之后位置修正量不再被單獨(dú)的幾何方程約束而是在聯(lián)合約束空間里取最小二乘意義下的最優(yōu)修正。3.3 改進(jìn)思路二半定松弛方法半定松弛的思路完全不同它的出發(fā)點(diǎn)是最小化最大似然代價(jià)函數(shù)但直接把代價(jià)函數(shù)做凸松弛轉(zhuǎn)成半定規(guī)劃問(wèn)題求解。優(yōu)點(diǎn)是對(duì)噪聲模型適配性強(qiáng)即使非高斯噪聲也能用缺點(diǎn)是計(jì)算量比閉式解高幾個(gè)數(shù)量級(jí)。我在仿真里用的是CVX工具箱做數(shù)值求解模型規(guī)模不大單次求解耗時(shí)在幾十毫秒到幾百毫秒之間比TSWLS慢三到四個(gè)數(shù)量級(jí)。性能上半定松弛在低信噪比區(qū)間更穩(wěn)定。它的代價(jià)函數(shù)把站間噪聲的協(xié)方差結(jié)構(gòu)直接嵌入優(yōu)化目標(biāo)沒(méi)有TSWLS那種線性化截?cái)嗾`差因此在噪聲標(biāo)準(zhǔn)差超過(guò)100ns時(shí)優(yōu)勢(shì)明顯。但在中等信噪比區(qū)間兩種方法其實(shí)沒(méi)有顯著差異。3.4 改進(jìn)思路三最大似然迭代初值修正第三種改進(jìn)思路是給TSWLS的結(jié)果做一步牛頓迭代等價(jià)于在一次閉式解的基礎(chǔ)上向最大似然估計(jì)方向精修。這種思路實(shí)現(xiàn)成本最低代碼量不超過(guò)30行精髓在于把TSWLS的閉式解當(dāng)作迭代的初始值然后用牛頓法最小化完整的對(duì)數(shù)似然函數(shù)。牛頓法需要的雅可比矩陣和黑塞矩陣推導(dǎo)起來(lái)有些繁瑣但都是標(biāo)準(zhǔn)操作。迭代收斂的判據(jù)我設(shè)的是位置修正量的范數(shù)小于0.01米同時(shí)迭代次數(shù)不超過(guò)20次。實(shí)測(cè)中TSWLS的初值已經(jīng)足夠好基本三次迭代就收斂這個(gè)方案在高噪聲區(qū)域把TSWLS的性能又抬升了一截接近半定松弛的水平而計(jì)算量增加不到十分之一。4. 性能對(duì)比與結(jié)果解讀4.1 CRLB與GDOP基準(zhǔn)對(duì)比算法前要先建立性能下界。CRLB是任何無(wú)偏估計(jì)量方差的理論下界在TDOA/FDOA聯(lián)合定位里的表達(dá)式是費(fèi)舍爾信息矩陣的逆的對(duì)角線元素。FIM的構(gòu)造需要把每個(gè)TDOA和FDOA測(cè)量值對(duì)目標(biāo)位置和速度的偏導(dǎo)數(shù)全部算出來(lái)這一步在仿真代碼里用的是符號(hào)求導(dǎo)驗(yàn)證過(guò)的解析表達(dá)式避免數(shù)值差分引入額外誤差。GDOP描述的是同樣測(cè)量噪聲水平下定位誤差隨目標(biāo)幾何位置的放大程度。仿真中GDOP分布和目標(biāo)與站點(diǎn)之間的相對(duì)方位強(qiáng)相關(guān)。當(dāng)目標(biāo)位于站群包絡(luò)內(nèi)部時(shí)GDOP接近1甚至小于1定位精度可以達(dá)到噪聲本身的水平當(dāng)目標(biāo)移到站群外且距離遠(yuǎn)時(shí)GDOP迅速增大定位誤差被幾何放大數(shù)倍。4.2 RMSE對(duì)比曲線下面是TSWLS和三種改進(jìn)方法在TDOA噪聲掃參條件下的RMSE對(duì)比結(jié)論以表格形式整理TDOA噪聲 (ns)TSWLS RMSE (m)約束WLS RMSE (m)SDR RMSE (m)ML迭代 RMSE (m)306.86.56.46.56014.312.712.512.610028.920.418.820.115065.437.632.335.9200130.261.348.757.4數(shù)據(jù)來(lái)源是同一組蒙特卡洛仿真?？梢詮倪@個(gè)結(jié)果里讀出三件事。第一在低噪聲區(qū)間30到60ns所有方法的差距都不大TSWLS的RMSE距離CRLB只有10%出頭的差距。說(shuō)明在測(cè)量精度好的情況下閉式解已經(jīng)足夠用不需要為改進(jìn)算法付出額外計(jì)算成本。第二噪聲超過(guò)100ns后TSWLS快速惡化而約束WLS和ML迭代仍然保持穩(wěn)定增長(zhǎng)這說(shuō)明TSWLS在高噪聲下失效的根本原因是第一步線性化誤差被二階放大。第三半定松弛在200ns時(shí)仍然把RMSE壓在50米以內(nèi)比TSWLS提高了近兩倍精度但它付出的計(jì)算量是TSWLS的一萬(wàn)倍以上。實(shí)際工程中是否值得取決于系統(tǒng)的實(shí)時(shí)性需求。4.3 偏差分析與收斂性只看RMSE還不夠偏差特性同樣重要。我統(tǒng)計(jì)了各算法估計(jì)位置與真實(shí)位置之間的平均偏差向量。TSWLS在低噪聲下偏差很小但在高噪聲下偏差呈系統(tǒng)性偏移方向指向參考站。原因在于第一步線性化中r_1的估計(jì)會(huì)向參考站方向收縮這種一階偏差被第二步完全繼承。約束WLS和ML迭代的偏差則始終在零附近波動(dòng)沒(méi)有系統(tǒng)性偏移。這說(shuō)明在偏差敏感的應(yīng)用場(chǎng)景比如精確引導(dǎo)、彈道測(cè)量中TSWLS即使RMSE能接受仍需謹(jǐn)慎使用。5. 常見(jiàn)問(wèn)題與排查技巧實(shí)錄5.1 病態(tài)矩陣為什么TSWLS會(huì)突然發(fā)散仿真里最讓人頭疼的問(wèn)題是偶發(fā)性的“算法漂移”1000次蒙特卡洛里總是有幾個(gè)點(diǎn)誤差突然飆到幾公里以上。排查后確認(rèn)根因有兩個(gè)一是第一階段G1矩陣的條件數(shù)在某些幾何角度下會(huì)急劇增大二是高噪聲下線性化誤差使h1中的某些分量與G1方向接近正交。解決思路有兩步。第一步是在第一階段解算前做矩陣條件數(shù)檢測(cè)如果條件數(shù)超過(guò)閾值就用偽逆替代普通逆。這里要說(shuō)明一下這是我在實(shí)際調(diào)試中確定的做法閾值取1e6比較合適既能篩掉真正的病態(tài)情況又不會(huì)影響絕大多數(shù)正常樣本。第二步是給第二階段加一個(gè)置信區(qū)間門限當(dāng)?shù)谝浑A段解算出的r_1為負(fù)值或明顯超出站點(diǎn)覆蓋范圍時(shí)直接判定為無(wú)效樣本不進(jìn)入第二階段的修正。這個(gè)門限在實(shí)驗(yàn)里設(shè)置為r_1小于0或大于站點(diǎn)間最大距離的5倍時(shí)丟棄。5.2 噪聲協(xié)方差矩陣參數(shù)調(diào)不準(zhǔn)的影響TSWLS對(duì)協(xié)方差矩陣的逆非常敏感。如果噪聲模型里假設(shè)各站噪聲獨(dú)立但實(shí)際系統(tǒng)中站間本振有一定相關(guān)性那么加權(quán)矩陣會(huì)失真。失真初期表現(xiàn)為RMSE小幅上升嚴(yán)重時(shí)約束WLS的第二步會(huì)直接震蕩。解決方法是利用仿真數(shù)據(jù)預(yù)先估計(jì)協(xié)方差矩陣。具體是在無(wú)噪聲的理想測(cè)量中加入已知噪聲序列觀察估計(jì)結(jié)果反推等效協(xié)方差結(jié)構(gòu)。在我的仿真流程中對(duì)每個(gè)噪聲檔位都先做200次純?cè)肼晽l件下的預(yù)標(biāo)定把協(xié)方差矩陣估計(jì)出來(lái)后鎖定再跑正式對(duì)比。這套方法在實(shí)際項(xiàng)目中用來(lái)校準(zhǔn)接收機(jī)通道失配效果也說(shuō)得上可靠。5.3 參考站選擇差異帶來(lái)的性能波動(dòng)同一個(gè)場(chǎng)景換一個(gè)參考站結(jié)果會(huì)差很多。仿真中把參考站從站1換到站4一個(gè)不在中心位置的站TSWLS和約束WLS的RMSE在高噪聲區(qū)都變差了20%到30%。原因是參考站到其余站的距離差跨度越小第一步線性方程的正交性越差。這個(gè)問(wèn)題的啟示很直接如果系統(tǒng)允許多站輪詢應(yīng)該選GDOP最優(yōu)的站作為參考站而不是隨機(jī)指定。另一個(gè)實(shí)際工程經(jīng)驗(yàn)是當(dāng)參考站的觀測(cè)質(zhì)量明顯比其他站差時(shí)比如信噪比低了幾個(gè)dB寧可損失一個(gè)觀測(cè)通道也不要讓它污染全部差分量。這個(gè)選擇對(duì)系統(tǒng)可靠性有關(guān)鍵影響實(shí)測(cè)下來(lái)比換算法還管用。5.4 三維場(chǎng)景下的高度退化問(wèn)題二維場(chǎng)景調(diào)試好后擴(kuò)展到三維發(fā)現(xiàn)垂直方向的誤差明顯大于水平方向而且TSWLS在高噪聲下會(huì)出現(xiàn)垂直分量發(fā)散。原因是四站在z方向上幾乎共面站群的垂直張角很小導(dǎo)致z方向的幾何觀測(cè)強(qiáng)度不足。改進(jìn)手段有兩種。一是把站點(diǎn)布局改為非共面構(gòu)型增加垂直基線。二是在TSWLS第二階段加入高度先驗(yàn)約束把高度約束方程的權(quán)重設(shè)為根據(jù)GDOP自適應(yīng)調(diào)整。仿真對(duì)比中加了自適應(yīng)高度約束之后z方向RMSE下降了約40%而水平方向幾乎沒(méi)有劣化。這些都是在模擬項(xiàng)目X中反復(fù)驗(yàn)證過(guò)的結(jié)論。個(gè)人體會(huì)是TDOA/FDOA聯(lián)合定位的改進(jìn)算法很多但每種算法的適用區(qū)間差別很大關(guān)鍵是根據(jù)系統(tǒng)噪聲水平和實(shí)時(shí)性約束選擇最匹配的方法。如果系統(tǒng)對(duì)計(jì)算量敏感約束WLS配合ML迭代是最均衡的方案如果追求極限魯棒性且硬件資源充裕半定松弛是最穩(wěn)的選擇。仿真中最容易踩的坑不在算法本身而在場(chǎng)景設(shè)計(jì)和噪聲標(biāo)定先把這兩件事做扎實(shí)再談算法對(duì)比才有說(shuō)服力。