基于移動(dòng)最小二乘法的軌跡擬合切線方位角計(jì)算

姚連璧， 錢瑾斐

(同濟(jì)大學(xué) 測(cè)繪與地理信息學(xué)院，上海 200092)

雖然通過單天線全球?qū)Ш叫l(wèi)星系統(tǒng)(GNSS)接收機(jī)無法直接獲得方位角信息，但是可以對(duì)一段連續(xù)的GNSS坐標(biāo)點(diǎn)軌跡擬合求切線，間接求得該段所對(duì)應(yīng)的方位角，即軌跡擬合方位角.由于擬合方位角由軌跡擬合得到，不會(huì)受到儀器安置誤差的影響，因此可以用來粗略校正慣性傳感單元方位角(航向角)的安置誤差[1].

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

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

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

實(shí)際上，對(duì)于最終要擬合的結(jié)果，只需相應(yīng)地離散坐標(biāo)點(diǎn)所對(duì)應(yīng)的擬合坐標(biāo)以及軌跡擬合方位角，因此擬合過程中的擬合曲線是否連續(xù)或者擬合結(jié)果是否能夠組成一條連續(xù)的軌跡并不是必須的.根據(jù)上述常用方法的問題以及軌跡擬合方位角的特性，嘗試采用移動(dòng)最小二乘法來擬合軌跡，獲得軌跡的方位角.

1 移動(dòng)最小二乘理論

移動(dòng)最小二乘法是形成無網(wǎng)格方法逼近函數(shù)的方法之一 , 已在無網(wǎng)格方法中得到廣泛應(yīng)用[4-5].移動(dòng)最小二乘法最初由Shepard[6]以最低階的形式發(fā)展成為一種逼近方法，并由Lancaster等[7]擴(kuò)展到更高階的形式.移動(dòng)最小二乘法的優(yōu)點(diǎn)有很好的數(shù)學(xué)理論支持，對(duì)于每個(gè)固定點(diǎn)，移動(dòng)最小二乘法即為通常的最小二乘法[8].

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

(1)

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

xi)=min

(2)

使Ωx是以δ為約束參數(shù)的一個(gè)鄰域，則式(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，δ的基數(shù).最后，由M(α)=0可得

生鮮農(nóng)產(chǎn)品具有時(shí)效短和鮮活易腐等特點(diǎn)，新鮮度和安全性成為消費(fèi)者選擇的首要依據(jù)，因此生鮮農(nóng)產(chǎn)品的庫存成本除前三者外還有價(jià)格折扣成本和廢棄成本。即生鮮農(nóng)產(chǎn)品庫存成本=訂貨成本+存儲(chǔ)成本+缺貨成本+價(jià)格折扣成本+廢棄成本。

(4)

即

(5)

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

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

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

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

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

3 算法測(cè)試

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

a 四平路實(shí)驗(yàn)

b 逸仙路實(shí)驗(yàn)圖2 四平路實(shí)驗(yàn)及逸仙路實(shí)驗(yàn)軌跡Fig.2 Trajectory of Siping road experiment and Yixian road experiment

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

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

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

項(xiàng)目四平路實(shí)驗(yàn)掉頭處逸仙路實(shí)驗(yàn)Δx/mΔy/m方位角差/(°)Δx/mΔy/m方位角差/(°)平均值007.096000.859標(biāo)準(zhǔn)差0.0050.0077.11900.0010.511

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

分別以2、3、4、5、10 m緊支系數(shù)δ等權(quán)計(jì)算逸仙路實(shí)驗(yàn)數(shù)據(jù)，與實(shí)測(cè)數(shù)據(jù)做差，對(duì)比不同緊支系數(shù)δ取值對(duì)坐標(biāo)及方位角擬合結(jié)果的影響，如表2和表3所示.

表2 緊支系數(shù)對(duì)坐標(biāo)擬合的影響Tab.2 Effect of dilatation parameter on coordinate fitting mm

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

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

(5)

式中：vmax為移動(dòng)測(cè)量系統(tǒng)的最大車速，m·s-1；f為傳感器數(shù)據(jù)時(shí)間同步后的頻率，Hz.最后，選擇δ=4 m，分別以等權(quán)及不等權(quán)的方式計(jì)算逸仙路實(shí)驗(yàn)數(shù)據(jù)，與實(shí)測(cè)數(shù)據(jù)做差值，對(duì)比不同的權(quán)函數(shù)對(duì)擬合結(jié)果的影響.等權(quán)為在由緊支系數(shù)δ確定的范圍內(nèi)，所以測(cè)量數(shù)據(jù)的權(quán)都為1，而不等權(quán)為根據(jù)測(cè)量點(diǎn)至擬合點(diǎn)之間的距離，由權(quán)函數(shù)分配不同的權(quán)值給測(cè)量點(diǎn)，使得離擬合點(diǎn)較近的測(cè)量值對(duì)擬合的影響大，遠(yuǎn)離擬合點(diǎn)的測(cè)量值對(duì)擬合的影響較小.

在本文中，分別構(gòu)造以下3種不等權(quán)的權(quán)函數(shù)模型：

(6)

(7)

W3(Δs)=

(8)

式中:Δs為測(cè)量點(diǎn)至擬合點(diǎn)的距離.將4種權(quán)函數(shù)擬合軌跡與方位角的計(jì)算結(jié)果與實(shí)測(cè)數(shù)據(jù)做差值，對(duì)比不同權(quán)函數(shù)模型對(duì)坐標(biāo)及方位角擬合結(jié)果的影響，如表4和表5所示.

表4 權(quán)函數(shù)對(duì)坐標(biāo)擬合的影響Tab.4 Effect of weight functions on coordinate fitting mm

表5 權(quán)函數(shù)對(duì)方位角擬合的影響Tab.5 Effect of weight functions on azimuth fitting (°)

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

4 結(jié)論

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

(2)隨著緊支系數(shù)δ的增大，移動(dòng)最小二乘坐標(biāo)擬合的精度(標(biāo)準(zhǔn)差)逐漸下降，而方位角擬合的精度逐漸上升.

(3)權(quán)函數(shù)能夠提高坐標(biāo)擬合的精度，但對(duì)方位角擬合的作用十分有限.

綜上所述，在選擇緊支系數(shù)δ時(shí)，本文建議充分考慮實(shí)驗(yàn)的具體情況.根據(jù)實(shí)驗(yàn)中移動(dòng)測(cè)量系統(tǒng)的最大車速、GNSS的頻率，選擇滿足整體最小二乘擬合要求的緊支系數(shù)δ，且不宜過大;然后,配以適當(dāng)?shù)臋?quán)函數(shù)模型，對(duì)彎曲程度較小的軌跡切線方位角進(jìn)行擬合計(jì)算，以此進(jìn)行IMU粗對(duì)準(zhǔn)標(biāo)定等相關(guān)應(yīng)用.