謝心月，鄒德岳**，劉源

（1.大連理工大學信息與通信工程學院，遼寧 大連 116024；2.中國計量科學研究院，北京 100000）

0 引言

隨著車聯(lián)網技術的不斷發(fā)展，智能交通監(jiān)測成為解決交通擁堵、提高交通安全和效率的重要手段，而對車輛位置和姿態(tài)的準確監(jiān)測成為實現(xiàn)智能交通和車輛互聯(lián)的關鍵[1-2]。然而，在城市中的一些復雜封閉環(huán)境，如城市峽谷、隧道、地下停車場等，GNSS（Global Navigation Satellite System，全球導航衛(wèi)星系統(tǒng)）信號受到阻隔，導致定位效果不佳，甚至完全失效。其他可行的定位方案，如Wi-Fi和超寬帶雷達，往往在城市環(huán)境中部署困難[3-4]。此外，純粹依靠慣性導航的定位方法容易隨時間累積誤差[5-7]。這些問題限制了車聯(lián)網技術的發(fā)展和應用，也給城市交通管理帶來了挑戰(zhàn)。為了解決這些問題，提出將VLP（Visible Light Positioning，可見光定位）技術[8-9]應用于車聯(lián)網中，以解決車輛定位問題。VLP 技術不依賴于GNSS，可以在復雜城市環(huán)境中獨立工作，提供持續(xù)穩(wěn)定的定位服務。本研究旨在探索VLP 技術在車聯(lián)網智能交通監(jiān)測中的應用，為解決城市交通問題提供技術支持。

目前存在許多VLP 算法，但難以應用在上述的封閉環(huán)境里：指紋算法適用于固定的室內場景，且無法實現(xiàn)姿態(tài)角的跟蹤[10-11]；AOA（Angle of Arrival，到達角度）算法則依賴于接收器對入射光角的檢測精度和光源數(shù)量[12-13]；TOA（Time of Arrival，到達時間）算法則需要高度精確的時間同步[14-15]；RSS（Received Signal Strength，信號接收強度）算法取決于接收器對光強的準確識別，以及準確的光傳播信道模型，且容易受到燈光反射等因素的影響[16-17]。總之，這些算法在光信號傳播距離較短且接收穩(wěn)定的室內環(huán)境中表現(xiàn)出色，但在城市環(huán)境中的效果不盡如人意。因此，為實現(xiàn)高精度的車輛位姿測量，設計了一種多傳感器數(shù)據(jù)融合方案，以提高VLP 在車輛位姿測量環(huán)境下的定位精度和魯棒性。

最近的學術研究對VLP 算法提出了一些改進方案，但無法完全解決VLP 在復雜環(huán)境下的定位挑戰(zhàn)：將EKF（Extend Kalman Filter，擴展卡爾曼濾波器）應用到VLP 中以提高定位的魯棒性，但VLP 系統(tǒng)的非線性特點會導致復雜的雅可比矩陣計算[18-19]；在UKF（Unscented Kalman Filter，無噪聲卡爾曼濾波器）中使用RSS 模型會繼承RSS 算法易受環(huán)境影響的缺點[20]。因此，為實現(xiàn)高精度的車輛定位，在之前的研究[21]的基礎上，提出了一種改進的VLP 算法，使用單目相機和IMU（Inertial Measurement Unit，慣性測量單元），基于剛體運動模型來使用UKF 融合單目相機的圖像數(shù)據(jù)和IMU 的數(shù)據(jù)計算得到無人載具的位置和姿態(tài)角。通過仿真模擬，驗證了該算法在精度和魯棒性方面的出色性能。

1 理論分析

1.1 投影模型

投影模型如下圖所示（見圖1）：被監(jiān)測車輛攜帶三個可見光光源（以下簡稱LED）以及IMU，三個LED 分別標記為此外，使用相機捕獲車輛上LED 的圖像，它的光心記為Oc。LED 排布不受限制，但需確定各個LED 燈之間的相對位置。

圖1 投影模型

在圖1 中，建立四個坐標系統(tǒng)：CCS（Camera Coordinate System，相機坐標系統(tǒng)），該坐標系統(tǒng)的原點位于相機的光心，其Z 軸指向被監(jiān)測車輛；ICS（Image Coordinate System，圖像坐標系統(tǒng)），該坐標系統(tǒng)的原點位于相機成像平面的中心，用于描述LED 在圖像上的坐標；PCS（Pixel Plane Coordinate System，像素平面坐標系統(tǒng)），該坐標系統(tǒng)的原點位于成像平面的左上角，用于描述LED 的像素坐標；VCS（Vehicle Coordinate System，車輛坐標系統(tǒng)），該坐標系統(tǒng)的原點位于LED 圖案中心，且初始化的各軸方向與CCS 坐標系相同，用于描述無人船的位置和姿態(tài)。在CCS、ICS 和PCS 中，坐標軸Xc、Xi和Up相 互 平 行，Yc、YI和Vp相 互 平 行。Oc到OI的 距離即為相機焦距。其中，為各個LED 在VCS 中的坐標；為 各個LED 在CCS 中的坐標；為各個LED 在ICS 中的坐標；為各個LED 在PCS 中的坐標，這些坐標可以從相機的圖像中獲得。

1.2 VLP算法

（1）傳感器輸入

對于過程方程的輸入，車輛攜帶一個IMU，可以跟蹤VCS 相對CCS 在X 軸、Y 軸和Z 軸上的姿態(tài)角變化,分別記為γ、β和α。但是，由于β存在累積誤差，該算法不將β作為輸入，而將其作為未知數(shù)進行估計，這樣既利用了慣性測量傳感器的數(shù)據(jù),又盡量避免了累積誤差的影響。對于觀測方程的輸入，則使用相機的圖像數(shù)據(jù)中的

（2）狀態(tài)向量

在算法中，第k-1 個計算周期對應的狀態(tài)向量為：

（3）過程方程

過程方程(2)的作用是從第k-1個計算周期的狀態(tài)預測第k個計算周期的狀態(tài)，計算公式如下：

其中ωk為預測噪聲，Rk表示CCS 與VCS 之間的旋轉矩陣，計算公式如下：

在本文中，將式(3)簡寫為式(4)：

（4）測量方程

（5）無跡卡爾曼濾波器

在本節(jié)中，將在UKF 中使用第（1）～（4）點的內容，使得UKF 可以正確地計算狀態(tài)向量。

在第k計算周期，使用式(7)計算第k-1 周期的最優(yōu)狀態(tài)的sigma點，其中n表示狀態(tài)元素的數(shù)量，Xk-1表示k-1 周期的最優(yōu)狀態(tài)，表示k-1 周期的最優(yōu)狀態(tài)的sigma 點。

其中，Qk-1為Xk-1的協(xié)方差矩陣，λ是對角線因子，計算公式如下：

其中，L為測量噪聲的協(xié)方差矩陣。

進一步地，就可以聯(lián)合式(17) 和式(18) 計算出卡爾曼增益Kk，計算公式如下所示：

2 算法仿真與結果分析

2.1 仿真參數(shù)和方法介紹

在該算法的測試中，相機的分辨率為2 280×1 080，單位為像素；相機的內參矩陣Ain已提前標定完成，如下所示：

算法初始化時，VCS 與CCS 方向一致并位于CCS的Z 軸正前方3 m 處。測試時，生成一個隨機的車輛軌跡，LED 坐標隨著車輛的在軌跡中的位置和姿態(tài)也隨機變化，且保證每次的軌跡與相機的距離從0 m 開始直至最大120 m。

通過兩個誤差指標來評估算法性能：定位誤差和姿態(tài)角誤差，分別對應狀態(tài)向量中的和β。定位誤差定義為算法狀態(tài)向量中的與隨機軌跡中的的歐氏距離，而姿態(tài)角誤差則定義為算法狀態(tài)向量中的β與隨機軌跡中的β的累積差值。在測試中，將LED 組成的圖案固定為正三角形，設置不同的邊長（單位：米）和圖像噪聲（單位：像素）水平對這兩個誤差的影響，以探討不同環(huán)境條件下算法性能的變化。

2.2 仿真結果與分析

首先，設定不同的LED 圖案邊長L，并且將像素噪聲設置為服從均值為0，標準差為1 的高斯分布N(0,1)，以測試不同L對算法的定位誤差和姿態(tài)角誤差的影響。在0 到120 m 的隨即軌跡中，在L的值設定為3 m 時，從定位誤差的CDF（Cumulative Distribution Functions，累積分布函數(shù)）可以看出，如圖2，定位誤差主要集中在0 到1.5 米范圍內。當L的值減小時，誤差有明顯的擴大。

圖2 不同L下的定位誤差

此外，如圖3 中姿態(tài)角誤差的CDF 所示，當L等于3 米時，誤差分布主要集中在-1 至1 度的范圍內。盡管隨著L的減小，這種分布無明顯變化。以上數(shù)據(jù)說明，增大LED 之間的距離可以減小算法的定位誤差，提升算法性能，但對姿態(tài)角誤差無明顯影響。

圖3 不同L下的姿態(tài)角誤差

其次，設定L為3 m，改變標準差(σ)來評估圖像噪聲對算法性能的影響。同時，將本文的算法（Mono+UKF Pro）與之前的算法[13]（Mono+UKF）比較。如圖4 和圖5 所示，Mono+UKF Pro 的CDF 表明，越大的σ會導致定位誤差和姿態(tài)角誤差越大；并且與Mono+UKF 相比，Mono+UKF Pro 的角度誤差和位置誤差有明顯的降低，說明了該算法性能的提升。然而，盡管增大σ會導致定位誤差和姿態(tài)角誤差的增加，但相對噪聲等級σ的增加并未明顯加劇這兩種誤差。這表明了該算法具有出色的魯棒性，能夠相對穩(wěn)定地應對噪聲等級的變化。

圖4 不同σ下的定位誤差

圖5 不同σ下的姿態(tài)角誤差

3 結束語

本文提出了一種基于VLP 的車輛位姿跟蹤算法，專注于封閉環(huán)境下的定位應用。通過詳細的仿真實驗，驗證了該算法在定位誤差和姿態(tài)角誤差方面的出色性能，而且即使在距離光源較遠的情況下（120 m），算法仍能保持優(yōu)異的表現(xiàn)。

值得注意的是，所提出的算法對高斯噪聲表現(xiàn)出強大的魯棒性。尤其在姿態(tài)角誤差方面，該算法的魯棒性尤為顯著，即使圖像噪聲水平逐漸增加，也未對性能產生根本性的不良影響。

所提出的算法不僅在位姿跟蹤性能和魯棒性方面表現(xiàn)出色，還在受到圖像噪聲干擾和大尺度距離范圍的河流流量測量中展現(xiàn)了其潛力。更為重要的是，該算法的應用領域不僅僅局限于車輛位姿測量，還適用于GNSS 信號不可用的環(huán)境，可擴展到機器人導航、工業(yè)規(guī)劃等類似場景。

然而，該算法也存在局限性，特別是在LED 點移動超出圖像邊界時性能受到嚴重影響的問題。未來的工作將重點解決這一挑戰(zhàn)，并探索將該算法融入到全局位姿跟蹤技術中，例如將其整合到SLAM（同步定位與地圖構建）中，以提升適用范圍和性能。