基于移動最小二乘法的軌跡擬合切線方位角計算

姚連璧， 錢瑾斐

(同濟大學 測繪與地理信息學院，上海 200092)

雖然通過單天線全球導航衛星系統(GNSS)接收機無法直接獲得方位角信息，但是可以對一段連續的GNSS坐標點軌跡擬合求切線，間接求得該段所對應的方位角，即軌跡擬合方位角.由于擬合方位角由軌跡擬合得到，不會受到儀器安置誤差的影響，因此可以用來粗略校正慣性傳感單元方位角(航向角)的安置誤差[1].

在數學上，符合一定條件的動點所形成的圖形，或者說，符合一定條件的點的全體所組成的集合，叫做滿足該條件的點的軌跡.移動測量系統的軌跡是由GNSS接收機測得的坐標點的集合.一般來說，車輛的行駛軌跡沒有規律，即在不同區域內，它們的形狀可以完全不相關，而且軌跡的累積距離也相當長，因而無法直接通過高次擬合函數對車輛的行駛軌跡進行逼近.在分段插值中，分段線性插值在分段點上僅連續但不可導，分段埃爾米特插值只有連續的一階導數，光滑度不高，而樣條函數可以同時解決這兩個問題，使插值函數既是低階分段函數,又是光滑函數，并且只需在區間端點提供某些導數信息[2].

樣條插值的特點是擬合的曲線通過插值點，即通過GNSS軌跡點.然而，GNSS軌跡點含有1～2 cm左右的平面誤差，直接插值的做法會將誤差傳播到樣條函數的參數中，進而對切線結果(即方位角)產生一定的影響.因此，需要以擬合的方式計算方位角.

根據分段得到的一段軌跡點，可以擬合得到一個函數關系式.然而，在這個函數關系式上，擬合的精度不是均勻的，通常中部的精度稍高，兩頭的精度較差，容易產生“跳變”.因此，將分段擬合得到的曲線簡單地拼接起來作為擬合的結果勢必是不理想的.姚連璧等[3]提出整體樣條擬合的方法，將連接處的連續性作為條件方程，以附有條件的間接平差來進行整體擬合.當軌跡累積距離太長，參與計算的軌跡點太多時，采用該方法計算會形成一個巨大的矩陣，嚴重時甚至超過計算機內存容量的限制而導致無法計算，因此這種方法也不適用于移動測量系統的軌跡擬合.

實際上，對于最終要擬合的結果，只需相應地離散坐標點所對應的擬合坐標以及軌跡擬合方位角，因此擬合過程中的擬合曲線是否連續或者擬合結果是否能夠組成一條連續的軌跡并不是必須的.根據上述常用方法的問題以及軌跡擬合方位角的特性，嘗試采用移動最小二乘法來擬合軌跡，獲得軌跡的方位角.

1 移動最小二乘理論

移動最小二乘法是形成無網格方法逼近函數的方法之一 , 已在無網格方法中得到廣泛應用[4-5].移動最小二乘法最初由Shepard[6]以最低階的形式發展成為一種逼近方法，并由Lancaster等[7]擴展到更高階的形式.移動最小二乘法的優點有很好的數學理論支持，對于每個固定點，移動最小二乘法即為通常的最小二乘法[8].

設在某一個子域Ω?Rd上存在參數未知的多項式函數u，并已知該函數關系式在一系列自變量x(x∈Ω)上的對應函數值，則可以通過這些對應值逼近多項式函數u[9].作為一種逼近方法，移動最小二乘法由三部分組成：基函數、權函數、待求系數.對于一個自變量x，可以得到

(1)

式中:P(x)=(p1(x),p2(x),…,pm(x))T為一個m項基函數；α為待求參數，其值通過最小二乘法解得.令

xi)=min

(2)

使Ωx是以δ為約束參數的一個鄰域，則式(2)可以寫成

(3)

式中：α=(α1,α2,…,αm)T；UΩx=(u(xi)|i∈Ix,δ)T∈Rcx；PΩx=(pj(xi))i∈Ix,δ,j∈(1,2,…,m)∈Rm×cx；WΩx=diag(w(x-xi)|i∈Ix，δ)∈Rcx×cx；cx為Ix，δ的基數.最后，由M(α)=0可得

生鮮農產品具有時效短和鮮活易腐等特點，新鮮度和安全性成為消費者選擇的首要依據，因此生鮮農產品的庫存成本除前三者外還有價格折扣成本和廢棄成本。即生鮮農產品庫存成本=訂貨成本+存儲成本+缺貨成本+價格折扣成本+廢棄成本。

(4)

即

(5)

式(5)中有m個未知參數，則x的鄰域Ωx應該滿足求得最小二乘解的條件.為防止求解運算中矩陣出現奇異，Ωx鄰域內點的數量應該大于基函數P(x)的項數.

2 軌跡擬合切線方位角算法

在擬合中，緊支系數δ作為計算取值范圍的約束，決定了納入擬合計算的坐標點的范圍.由于軌跡為線性數據，則δ以距離約束量的方式約束擬合數據點的選取，即選擇擬合目標點前后±δ里程范圍內的點進行擬合計算.在擬合計算中，權函數決定了各坐標點對擬合結果的影響大小，一般分為等權與不等權，等權時將各坐標點“一視同仁”，而不等權時則一般是離目標點近的點的權重較大，在取值范圍邊界處的點的權重較小.不等權的權函數有多種取法，下文中將對其做比較.

基于移動最小二乘法的軌跡擬合切線方位角的算法流程如圖1所示.

圖1 算法流程Fig.1 Flow chart of the algorithm

3 算法測試

算法測試的主要目的有以下兩點：①測試算法的可行性與適用性；②測試緊支系數δ的不同取值及權函數的不同選擇對擬合結果的影響.測試數據來源于同濟大學測繪與地理信息學院移動測量車的實驗數據，實驗的時間地點分別為2015年2月5日于上海市四平路大連路至國順路段(包括四平路隧道)及2015年5月19日于上海市逸仙路高架曲陽路至軍工路段，軌跡如圖2所示.選取其中觀測精度質量較好的部分作為測試數據.

a 四平路實驗

b 逸仙路實驗圖2 四平路實驗及逸仙路實驗軌跡Fig.2 Trajectory of Siping road experiment and Yixian road experiment

對于算法的可行性與適用性，主要測試算法在不同軌跡形狀下的適用性.本文選擇的逸仙路實驗軌跡主要由直線段與曲率較小(半徑達到100 m以上)的路段構成，而四平路隧道部分則提取車輛掉頭轉彎處曲率較大(轉彎半徑在幾米到幾十米)的軌跡進行計算，如圖3所示.

從表1可以看出，在軌跡形狀彎曲變化程度較小的路段，移動最小二乘法的擬合效果較好.在逸仙路實驗中，坐標差的平均值都為0 m，標準差分別為0 m及0.001 m，與實測數據幾乎一致；方位角差的平均值為0.859°，標準差為0.511°，說明在該段IMU存在著約0.859°的對準誤差.在四平路實驗掉頭處，坐標擬合結果同樣優異，坐標差的平均值都為0 m，標準差分別為0.005 m與0.007 m，而方位角擬合結果不甚理想，差值的平均值達7.096°,標準差達7.119°.如圖4所示，在車輛轉彎半徑較小的地方，IMU方位角與軌跡的切線方向存在著較大的不同.這主要是由于二輪驅動的桑塔納在轉彎時，其前進方向與IMU反映的車體方向存在一定的夾角，從而導致轉彎處擬合角和IMU測量值的偏差較大.

圖3 車輛掉頭處軌跡Fig.3 Trajectory of vehicle turning around表1 實測軌跡擬合誤差Tab.1 Fitting deviation of measured trajectory

項目四平路實驗掉頭處逸仙路實驗Δx/mΔy/m方位角差/(°)Δx/mΔy/m方位角差/(°)平均值007.096000.859標準差0.0050.0077.11900.0010.511

圖4 轉彎處IMU方位角與擬合方位角之差Fig.4 Difference between IMU azimuth and fitting azimuth around the bend

分別以2、3、4、5、10 m緊支系數δ等權計算逸仙路實驗數據，與實測數據做差，對比不同緊支系數δ取值對坐標及方位角擬合結果的影響，如表2和表3所示.

表2 緊支系數對坐標擬合的影響Tab.2 Effect of dilatation parameter on coordinate fitting mm

表3 緊支系數對方位角擬合的影響Tab.3 Effect of dilatation parameter on azimuth fitting (°)

從表2和表3可以看到，緊支系數δ確定了參與擬合計算的軌跡點的范圍.當緊支系數δ越小時，坐標擬合的標準差越小，隨著緊支系數δ的增大，坐標擬合的標準差逐漸增大.緊支系數δ的增大導致了擬合距離的增大，雖然軌跡接近于光滑的弧線，但是會導致擬合曲線與實際軌跡的差異變大，即方位角標準差逐漸增大；對于方位角，隨著緊支系數δ的增大，IMU方位角與擬合方位角差的均值不變，而標準差逐漸減小，說明緊支系數δ越大，擬合方位角的離散程度越小.需要注意的是，由緊支系數δ所確定范圍內的點的數量應滿足最小二乘求解的條件，因而緊支系數δ不能取得太小，需滿足

(5)

式中：vmax為移動測量系統的最大車速，m·s-1；f為傳感器數據時間同步后的頻率，Hz.最后，選擇δ=4 m，分別以等權及不等權的方式計算逸仙路實驗數據，與實測數據做差值，對比不同的權函數對擬合結果的影響.等權為在由緊支系數δ確定的范圍內，所以測量數據的權都為1，而不等權為根據測量點至擬合點之間的距離，由權函數分配不同的權值給測量點，使得離擬合點較近的測量值對擬合的影響大，遠離擬合點的測量值對擬合的影響較小.

在本文中，分別構造以下3種不等權的權函數模型：

(6)

(7)

W3(Δs)=

(8)

式中:Δs為測量點至擬合點的距離.將4種權函數擬合軌跡與方位角的計算結果與實測數據做差值，對比不同權函數模型對坐標及方位角擬合結果的影響，如表4和表5所示.

表4 權函數對坐標擬合的影響Tab.4 Effect of weight functions on coordinate fitting mm

表5 權函數對方位角擬合的影響Tab.5 Effect of weight functions on azimuth fitting (°)

從表4和表5可以看出：權函數對坐標擬合標準差的影響較大，根據所采用權函數的不同，擬合的坐標約有5%至25%的提升；軌跡擬合方位角受權函數的影響則較小，平均值在精度范圍內未發生變化，標準差的變化也較小.這說明權函數主要影響擬合出的線型，而對該處的切線方向——方位角影響不大.

4 結論

(1)軌跡形狀測試說明本文算法能處理小曲率及大曲率半徑下的軌跡擬合.在大曲率半徑下，擬合方位角與IMU方位角有較大的差異，所以這種情況下不建議使用該方法擬合的數據；在軌跡彎曲不大的路線上，該算法能夠較好地逼近軌跡并計算方位角，為移動測量車的IMU方位角標定提供一個比較精準的原始值.

(2)隨著緊支系數δ的增大，移動最小二乘坐標擬合的精度(標準差)逐漸下降，而方位角擬合的精度逐漸上升.

(3)權函數能夠提高坐標擬合的精度，但對方位角擬合的作用十分有限.

綜上所述，在選擇緊支系數δ時，本文建議充分考慮實驗的具體情況.根據實驗中移動測量系統的最大車速、GNSS的頻率，選擇滿足整體最小二乘擬合要求的緊支系數δ，且不宜過大;然后,配以適當的權函數模型，對彎曲程度較小的軌跡切線方位角進行擬合計算，以此進行IMU粗對準標定等相關應用.