基于最小二乘優化的車輛位置估計算法

彭鑫，李仁發，楊柳，劉 驕

(湖南大學 計算機與通信學院，湖南 長沙 410082)

1 引言

近年來，道路交通安全已是智能交通系統(ITS,intelligent transportation systems)所要解決的頭號問題。車載自組網作為智能交通系統的重要基礎引起了學術界和工業界的極大興趣。

車載自組網是由車輛和布設在特定位置的路邊單元(RSU)所組成的開放式移動自組織網絡(MANET)[1]。它具有部署方便、成本低廉和結構開放等特點，可實現事故預警、輔助駕駛、交通信息發布、車間通信和 Internet接入等服務，有著廣闊的應用前景。位置服務是車載自組網諸多應用的基礎，比如車輛導航和碰撞避免。另外，車載自組網的路由也需要車輛位置信息[2,3]。目前，GPS導航以其精度高、費用低廉等特點在車載系統中得到了廣泛應用。雖然GPS在開闊地帶可以取得良好的定位性能，但由于衛星信號容易受到遮擋，GPS無法用于高樓林立的城市中心區和隧道中。現在采用的替代方案是以陀螺儀為代表的車載航位推算系統[4]。從航位推算系統目前的技術水平看，位置估計誤差為行駛距離的1%～2%。也就是說，一輛以60英里時速行駛的汽車，GPS信號只要中斷30s，車輛的位置估計誤差將會大于10m。這樣的精度無法滿足高安全性位置服務的需要。因此，車輛位置估計將是車載自組網研究的熱點問題之一。

本文提出一種基于最小二乘法的車載位置估計算法。提出的方案充分利用了車輛間的距離信息，通過對距離矩陣采用最小二乘法來提高估計精度并且避免了定位滯后問題，滿足車網環境下實時定位的性能要求。提出的算法可以直接得到車輛的相對位置分布，而且在有絕對位置信息的條件下可以獲得車輛的實際位置。最后通過仿真與文獻[5]的算法進行了對比。

本文第2節簡述國內外相關研究；第3節建立了車輛位置估計的模型和最小二乘估計方法；第 4節給出了車輛位置估計算法及其復雜度分析；第 5節是仿真和實驗分析；第6節是結束語。

2 相關研究

目前已提出的位置估計算法主要面向無線傳感器網絡。由于節點計算資源的限制，一些估計精度高但計算量較大的算法無法很好地用于無線傳感器網絡，而車載系統具有豐富的計算資源，有利于高精度位置估計算法的開發。一般來說，位置估計算法可分為粗粒度和細粒度2大類。粗粒度位置估計算法主要利用節點在網絡中的連通性關系來確定節點的大致位置[6,7]。細粒度算法通過距離測量等手段可提供更為精確的位置信息[8]。本文出于研究上的考慮，主要研究用于移動自組網環境下的細粒度位置估計算法。

文獻[9]提出的移動定位算法要求每個節點測得與一跳鄰居間的距離并且建立局部坐標系求出與一跳鄰居間的夾角，然后節點對局部坐標系進行變換得到全局坐標系。該算法的不足在于通信開銷過大并且擴展性低。文獻[10]提出的蒙特卡羅移動定位算法把運動節點的可能位置以加權樣本集的形式表示為后驗分布，用加權樣本集作為節點的位置估計。該算法在濾波階段需要錨節點直接支持，不能直接得到周圍節點的運動情況，制約了算法在車載環境下的應用。文獻[11]利用移動節點在網絡中添加采樣點，然后利用多維尺度變換來實現移動定位。文獻[12]提出一種基于點模式匹配的車輛位置估計算法。該算法維持一個地標拓撲數據庫，然后車輛對周圍環境的地標進行識別，用識別到的三角拓撲與庫中的拓撲進行匹配來確定車輛位置。該算法要求車輛配備激光掃描器，并且主要面向單臺車輛的自身位置估計。文獻[13]提出了一種車載定位算法，它根據定位需要進行建簇，然后利用三邊定位得到簇內車輛的相對位置。但是該算法在測距過程中存在明顯的誤差擴散問題無法保證估計精度。文獻[5]提出的分布式位置估計算法通過對初始位置估計采用最小二乘法來求得車輛位置分布。該算法在得到車輛位置分布的同時保證了較好的估計精度，是目前典型的車載分布式位置估計算法。

上述車輛位置估計方法有一個共同點，就是通過對運動車輛的瞬時估計把動態網絡轉化為靜態問題來求解。在一般移動自組織網絡環境下，這種方法是可行的，但是在車輛高速運動的車載網絡環境下，采用這種模式將會導致定位滯后問題，降低了估計精度，無法滿足實時性要求。

3 算法模型的建立

3.1 車輛位置估計模型

本文算法通過測量車輛間的距離信息求出車輛位置的最佳估計。令ρij表示車輛i和j之間的測量距離，Xn,m表示車輛的坐標矩陣，n表示車輛數，m表示空間維度，dij表示車輛i和j在m維空間X中的距離：

對于n階距離矩陣A，定義中心化映射：

其中，In表示 n階單位矩陣，令根據多維尺度變換[14]，有如下定理。

定理1 矩陣P為 m維空間中點間平方距離矩陣，當且僅當矩陣π(P)為對稱半正定矩陣，并且有分解式π(P)=XXT，其中X為n×m的空間點坐標矩陣，使得D2(X)=P。

定理2 如果矩陣P為 m維空間中點間平方距離矩陣，B=π(P)，r=rank(B)，λ1≥…≥λr≥λr+1=…=λn=0表示矩陣B的奇異值，Λ=diag(λ1…λn)，則有分解式B=QΛQT。在m維空間中，令Λm=diag(λ1…λm)，Qm表示矩陣Q的前m列，則X=QmΛm1/2。

上述定理實際上給出了由空間中點間距離計算各點坐標的方法。基于多維尺度變換的無線傳感器網絡定位算法[11,15]就是以上述結論為基礎。

從定理中不難看出，要得到網絡中節點的坐標，必須準確獲得各節點之間的距離，構成距離矩陣。這在靜態無線傳感器網絡中易于滿足，在移動自組織網絡中可以通過對移動節點的運行軌跡進行采樣，將動態網絡轉化為靜態網絡進行定位。但在車載自組網中，車輛運動速度非常快，如果仍然通過對車輛運動軌跡采樣的方式進行位置估計將造成車輛的估計位置滯后于實際位置，導致估計精度的下降，無法滿足實時性要求。本文算法將車輛的運行時間劃分為連續的時間段，從而把車輛位置估計問題轉化為多約束聯合優化問題，然后通過迭代最小二乘法求解。

下面考察2種交通模型。如圖1所示，在t0時刻，測得車輛i、j之間的距離為r0，與行駛方向間的夾角為α，車輛i、j的行駛速度分別為vi、vj。不難算出，圖1(a)中，t1=t0+τ時刻，車輛i、j之間的距離為

圖1 2種交通路段

圖1(b)中，t1=t0+τ時刻，車輛i、j之間的距離為

其中，μ=(r0sinαi-vjτsinθ)/sinθ，ω=(r0sinαj- viτsinθ)/sinθ，θ=π-αi-αj。在[t0,t1]時間段內，i、j之間的距離 δij∈[min(r0,r1), max(r0,r1)]，車輛距離矩陣滿足條件Δ0≤Δ≤Δ1，其中，

圖1的車輛運動模型適用于車輛由遠及近或由近及遠的情況。另一運動模型是在時間τ內，兩車因距離過近運動狀態發生了改變，即從由遠及近變為由近及遠，兩車間的最短距離出現在(t0, t1)內。由于本文選取的τ為很短的時長，僅當兩車距離很近時才有這種狀況發生，此時雖然但是兩者非常接近。為計算方便和統一模型，本文不考慮這種情況。

因此，根據以上分析，車輛位置估計問題可轉化為多約束聯合優化問題：

本文采用迭代最小二乘法來求解上述問題。

3.2 車輛位置估計的最小二乘法

根據問題(5)，分別定義最小二乘迭代函數f1(Δ)與f2(D)。

函數 f1(Δ)：

step2 對矩陣B進行奇異值分解。B=QΛQT，在 m 維空間中，令Λm=diag(λ1…λm)，Qm表示矩陣Q的前m列，則

這樣函數 f1和 f2構成了問題(5)的 2個迭代過程。接下來討論該迭代過程的收斂性。首先證明一個定理。

定理 3 距離矩陣約束條件 Sn=[Δ0,Δ1]是一個凸集。

不失一般性，假設r0≥r1。

令矩陣 M1=[m1ij], M2=[m2ij]，且 M1, M2∈Sn，即min(r0,r1)2≤m1, m2≤max(r0,r1)2。由不難得到

因此，Sn是一個凸集。

證畢。

令{Δk}為f1和f2迭代過程所產生的序列，En表示n階距離矩陣的集合。不難看出，函數f1完成了從Sn到En子集上的映射，函數f2完成了從En子集到Sn的映射。將式(2)定義的映射π應用于問題(5)，令 B=π[D(X)]，根據定理 1，式(5)可以表示為如下形式：

其中，P(n)表示n階半正定矩陣的集合。

令Δ∈Sn，b∈P(n)，將函數 f1的 step2～setp4 重新定義為函數D=φ(b)。由此構建問題(6)的迭代過程分別為 F1=πf1(Δ)，F2=f2φ(B)，并且 G(Δ)=F2F1(Δ)。不難看出，F1為距離矩陣所構成的凸集到半正定矩陣集合上的映射，F2為半正定矩陣到一個凸集上的映射。通過函數 G(Δ)可得到問題(6)的迭代序列{Δk}=G(Δ0)。對于?Δa∈Sn，有 Ba=F1(Δa)，Δα=F2(Ba)，Δα∈G(Δa)。因此可得到如下不等式：

由F2為半正定矩陣到凸集上的單射可知F2(Ba)是唯一的。如果F2(Ba)=Δa，則取

如果Δa∈G(Δa)，則取

在Sn上定義子集：

由定理3可知，Sn是一個閉凸集，所以集合N是有界的。由式(8)可知{Δk}=G(Δ0)?N，再由不等式(7)可得{Δk}=G(Δ0)在Sn上是收斂的。令Δn為序列{Δk}的收斂點，有Δn=G(Δn)。由于函數F1為距離矩陣所構成的凸集到半正定矩陣集合上的映射，因此，{Bk}=F1(Δ0)也是有界的并且為收斂序列。

綜上，由 F1，F2構成的問題(6)的迭代序列是收斂的并且有唯一收斂點(Δn,F1(Δn))。問題(6)與問題(5)是等價的，所以函數f1，f2所構成的迭代序列在問題(5)上也是收斂的。

4 基于迭代最小二乘的車輛定位算法

4.1 算法描述

本文提出的車輛位置估計算法描述如下。

step1 在t0時刻，每臺車輛測量與周圍鄰居間的距離 r0和夾角α，并廣播行駛速度 v，選取合適的τ值，根據式(3)或式(4)求得r1；

step2 通過Dijkstra算法求得車輛2跳鄰居間的距離和以及t0時刻的距離矩陣；

step4 令 k=1，由函數 f1(Δ0)得到矩陣 D(X0)；

step5 對矩陣 D(Xk-1)使用函數 f2(D(Xk-1))求得Δk；

step6 對矩陣Δk使用函數 f1(Δk)得到矩陣D(Xk)；

step7 若‖Δk-D(Xk)‖≤ε，轉到step8；否則，k=k+1，轉到step5；

step8 根據每臺車輛得到的相對位置Xk，逐步添加與其有最多共同節點的局部坐標圖，直至得到全網相對坐標；

step9 如果有3臺以上車輛已知絕對位置，則可由Householder變換將相對坐標Xk轉化為絕對坐標。

4.2 算法分析

根據駕駛行為規律，車輛在道路上會按特定方向定速巡航，而且τ本身也是較小的時間段。因此算法step1中，使用t0時刻的速度v來計算t1=t0+τ時刻的車輛間距是合理的，而且避免了車輛相對運動狀態的改變。由于車輛依據道路規劃行駛，而非隨機分布，因此 step2采用最短路徑長度來近似車輛距離可以保證較好的測距精度。為了保證實時性要求，本算法并未求出問題(5)的最優解，而是在step7中設定一個閾值ε，當問題(5)滿足ε條件時終止迭代。對于車輛的某些安全應用而言，比如碰撞避免，無需求得車輛絕對位置，只要在 step8中求出周圍車輛的相對位置即可。

如果車輛的連通度為c，車輛數為n，k為算法平均迭代次數，a為信標車輛的數目。算法 step1的復雜度為 O(c2)。step2的復雜度為 O(c3)。step3-step7的復雜度為 O(kc3)。step8的復雜度為O(c3n)。step9的復雜度為 O(a3+n)。因此算法總復雜度為O(c3n+kc3+a3)。

5 仿真分析

本文模擬2種交通環境。一種是直線路段，另一種是交叉路段。采用MATLAB7.1與文獻[5]的算法進行了比較。實驗中距離測量引入零均值高斯噪聲 N(0,σ2)。算法 step7的迭代終止條件ε=10-3。用均方根誤差(RMSE)作為性能評價指標:

其中，n是陰影區域內車輛總數。

5.1 仿真實驗1

如圖2所示，模擬2km長的雙向6車道路段，其中陰影區域1km長路段表示無法獲取GPS信號的區域，也就是需要進行位置估計的區域。陰影區域兩側各有 0.5km的路段，其中的車輛可以估計自身的絕對位置，從而為陰影區域內車輛的位置估計提供支持。仿真區域隨機分布300臺車輛，行駛速度保持 60km/h。上方路段由右向左行駛，即從右端隨機進入仿真區域，從左端離開仿真區域，下方路段則反之。

圖2 雙向6車道交通

GPS定位采用標準差為7m的高斯誤差模型，每秒刷新一次位置信息，取τ=1s，并且位置估計采用地圖匹配法，將車輛的估計位置限制在當前車道上。文獻[5]算法初始位置估計采用三邊測量法。車輛通信距離r=130m，車輛測距標準差σ=3m。記錄場景中某臺車輛的實驗結果如圖3所示，上方曲線為GPS的估計精度，中間曲線為文獻[5]算法的精度，下方曲線為本文算法的估計精度。從圖中可以看出，GPS外加地圖匹配的估計誤差約為6.9m，文獻[5]算法的估計誤差為4.7m，本文算法的位置估計誤差為3.4m。

圖3 3種位置估計算法的誤差比較

圖4 2種算法在不同通信距離下的誤差比較

在上述環境中，改變車輛通信半徑，比較2種算法在不同通信半徑下的估計精度。實驗結果取10次運行的平均值。如圖4所示，2種算法隨著通信半徑的增大估計誤差顯著下降。對文獻[5]算法而言，通信半徑越大周圍鄰居越多，在三邊測量階段能夠取得更好的精度，在求精階段可更準確地估計鄰居車輛的位置。本文算法，通信半徑越大，最短路徑距離近似方法引入的誤差也就越小，而且在最小二乘過程中的距離值也越精確。從圖中可以看出，在當前實驗環境中，文獻[5]算法當通信半徑大于150m時性能趨于穩定，本文算法當通信半徑大于120m時性能趨于穩定，并且比文獻[5]算法有更好的估計精度。

在相同環境中，改變車輛測距誤差比較2種算法在不同測距誤差下的性能。結果如圖 5所示。2種算法都以距離信息為基礎，因此在車輛測距誤差增大的情況下，算法的估計誤差都呈增大趨勢。由于本文算法可以通過最小二乘迭代過程求得一個凸集范圍內的最優解，因此取得了比文獻[5]算法更好的估計精度。

圖5 2種算法在不同測距誤差下的比較

5.2 仿真實驗2

如圖6所示，模擬2km長的雙向四車道路段，其中車道1和4各有一條長0.5km的輔道單向出口以評估算法在交叉路段的性能。圖中陰影區域的1km長路段表示進行位置估計的區域。陰影區域兩側各有0.5km的路段，其中的車輛可以獲知自身的絕對位置。仿真區域隨機分布220臺車輛，行駛速度保持60km/h，車道1和4的車輛從輔道離開。上方路段由右向左行駛，也就是右端隨機進入仿真區域，從左端離開仿真區域，下方路段則反之。

圖6 交叉路段

本次實驗采用和實驗1相同的參數。首先改變車輛通信半徑與文獻[5]比較，結果如圖 7所示。然后設置車輛通信半徑r=130m，改變車輛測距誤差，結果如圖8所示。從圖7和圖8不難看出，在交叉路段環境中，2種算法位置估計精度比直線路段略有下降。這是由于文獻[5]采用三邊測量法，在交叉路段環境中容易出現反褶的不定性。雖然該算法采用多次測距來進行彌補，但在交叉路段車輛的距離估計精度難以保證。本文采用最短路徑距離來估計2跳車輛間的距離，這種方法在直線路段可取得較好的結果，但在交叉路段精度有所降低。總體上看，本文算法在交叉路段環境中性能穩定，仍然取得了較好的精度。

圖7 交叉路段環境下，算法在不同通信距離的比較

圖8 交叉路段環境下，算法在不同測距誤差時的比較

6 結束語

針對車輛位置服務要求精度高、實時性強的特點提出了基于最小二乘法的車輛位置估計算法。該算法充分利用車輛行駛速度和車輛間的距離信息推導車輛距離矩陣所滿足的凸約束，然后采用最小二乘法進行車輛位置估計，從而避免了定位滯后問題引入的誤差。實驗分析表明，本文算法在滿足車輛實時位置估計的同時，能有效地提高估計精度，并且在不同道路環境下表現出較強的適應性和可靠性。

[1] BLUM J, ESKANDARIAN A, HOFFMAN L. Challenges of inter-vehicle ad hoc networks[J]. IEEE Transactions on Intelligent Transportation Systems, 2004, 5(4)∶ 347-351.

[2] BOUKERCHE A, OLIVEIRA H A B F, NAKAMURA E F. Vehicular ad hoc networks∶ a new challenge for localization-based systems[J].Computer Communications, 2008, 12(31)∶2838-2849.

[3] 常促宇,向勇,史美林.車載自組網的現狀與發展[J]. 通信學報, 2007,11(28)∶ 116-126.CHANG C Y, XIANG Y, SHI M L. Development and status of vehicular ad hoc networks[J]. Journal on Communications, 2007, 11(28)∶116-126.

[4] KING T, FUBLER H, TRANSIER M, et al. Dead-reckoning for position-based forwarding on highways[A]. Proceedings of the 3rd International Workshop on Intelligent Transportation[C]. Hamburg, Germany, 2006. 199-204.

[5] PARHER R, VALAEE S. Vehicle localization in vehicular networks[A]. IEEE 64th Vehicular Technology Conference[C]. Canada,2006.2713-2717.

[6] BULUSU N, HEIDEMANN J, ESTRIN D. GPS-less low cost outdoor localization for very small devices[J]. IEEE Personal Communications Magazine, 2000, 5(7), 28-34.

[7] DOHERTY L, GHAOUI L E, PISTER K. Convex position estimation in wireless sensor networks[A]. Proceedings of INFOCOMM[C]. Anchorage, USA, 2001. 1655-1663.

[8] RUDAFSHANI M, DATTA S. Localization in wireless sensor networks[A]. Proceedings of IPSN[C]. Cambridge, USA, 2007. 51-60.

[9] CAPKUN S, HAMDI M, HUBAUX J P. GPS-free positioning in mobile ad-hoc networks[A]. Proceedings of the 34th International Conference on System Sciences[C]. Hawaii, USA, 2001.9008-9018.

[10] BAGGIO A, LANGENDOEN K. Monte-carlo localization for mobile wireless sensor networks[J]. Ad Hoc Networks, 2008, 5(6)∶ 718-733.

[11] WU C H, SHENG W H, ZHANG Y. Mobile sensor networks self localization based on multi-dimensional scaling[A]. Proceedings of IEEE International Conference on Robotics and Automation[C]. Roma,Italy, 2007. 4038-4043.

[12] HAUNERT J, BRENNER C. Vehicle localization by matching triangulated point patterns[A]. Proceedings of the ACM International Conference on Advances in Geographic Information Systems[C]. Seattle,USA, 2009. 344-351.

[13] KUKSHYA V, KRISHNAN H, KELLUM C. Design of a system solution for relative positioning of vehicles using vehicle-to-vehicle radio communications during GPS outages[A]. Proceedings of IEEE Vehicular Technology Conference[C]. Dallas, USA, 2005. 1313-1317.

[14] BORG I, GROENEN P. Modern Multidimensional Scaling∶ Theory and Applications[M]. New York∶ Springer-Verlag, 1997.53-59.

[15] SHANG Y, RUML W, ZHANG Y. Localization from connectivity in sensor networks[J]. IEEE Transactions on Parallel and Distributed Systems, 2004, 15 (11)∶ 961-974.