張婕,龍川,殷飛,楊元(1.重慶市勘測(cè)院,重慶 40000; .重慶市地理信息云服務(wù)企業(yè)工程技術(shù)研究中心,重慶 40000; .重慶數(shù)字城市科技有限公司,重慶 40000)

基于粒子群算法的地面移動(dòng)測(cè)量平臺(tái)系統(tǒng)標(biāo)定方法研究

張婕1?,龍川2,殷飛3,楊元3
(1.重慶市勘測(cè)院,重慶 400020; 2.重慶市地理信息云服務(wù)企業(yè)工程技術(shù)研究中心,重慶 400020; 3.重慶數(shù)字城市科技有限公司,重慶 400020)

摘 要:基于激光掃描儀的地面移動(dòng)測(cè)量技術(shù)是近幾年剛剛發(fā)展起來的新型測(cè)繪技術(shù),該技術(shù)集成激光掃描儀、INS (GNSS+IMU)、DMI、相機(jī)等傳感器,可以快速獲取大面積的高精度點(diǎn)云與影像數(shù)據(jù)。本文研究了系統(tǒng)的標(biāo)定方法,在已有數(shù)學(xué)模型的基礎(chǔ)上,采用粒子群算法對(duì)標(biāo)定計(jì)算過程進(jìn)行優(yōu)化,簡(jiǎn)化了計(jì)算機(jī)算法實(shí)現(xiàn),增強(qiáng)了方法的魯棒性,全局收斂性好。通過實(shí)驗(yàn),系統(tǒng)誤差達(dá)到10 cm左右,證明該方法的有效性。

關(guān)鍵詞:地面移動(dòng)測(cè)量系統(tǒng);LiDAR;POS;系統(tǒng)標(biāo)定;粒子群算法

1　引 言

激光掃描技術(shù)又稱LiDAR(Light Detecting and Ranging),是近幾年發(fā)展起來的一種新型三維空間信息獲取技術(shù)[1]。它集光學(xué)、機(jī)械、電子和計(jì)算機(jī)等多個(gè)學(xué)科的最新研究成果,可以大面積,高精度低獲取三維空間坐標(biāo)信息。地面移動(dòng)測(cè)量系統(tǒng)是把LiDAR、INS、DMI、相機(jī)等傳感器集成到地面移動(dòng)車輛平臺(tái)上,可以快速獲取道路周邊的精確點(diǎn)云和影像數(shù)據(jù)的一種高新技術(shù)。近年來,國(guó)內(nèi)外地面移動(dòng)測(cè)量系統(tǒng)是繼機(jī)載LiDAR逐步成熟后的又一個(gè)研究熱點(diǎn),相關(guān)產(chǎn)品相繼出現(xiàn)。其精度主要受POS精度與系統(tǒng)標(biāo)定精度的影響。目前系統(tǒng)標(biāo)定通常使用最小二乘法進(jìn)行求解[1,2]。本文在傳統(tǒng)系統(tǒng)標(biāo)定原理的基礎(chǔ)上,研究使用粒子群算法求解移動(dòng)測(cè)量平臺(tái)中LiDAR設(shè)備與POS坐標(biāo)系的旋轉(zhuǎn)與平移參數(shù)。

2　地面移動(dòng)測(cè)量坐標(biāo)轉(zhuǎn)換原理

2.1問題描述

激光雷達(dá)、GNSS、IMU傳感器安裝在同一剛性平臺(tái)上,整個(gè)平臺(tái)安裝在移動(dòng)車輛上。在前進(jìn)過程中,同步采集激光點(diǎn)云數(shù)據(jù),GNSS原始數(shù)據(jù),IMU的三軸加速度與三軸角速度,各種數(shù)據(jù)通過時(shí)間進(jìn)行嚴(yán)格同步。

在數(shù)據(jù)處理過程中[5],把GNSS原始數(shù)據(jù)與IMU的原始數(shù)據(jù)首先進(jìn)行POS解算,獲得一定時(shí)間間隔周期的POS數(shù)據(jù)(位置與姿態(tài)數(shù)據(jù),包括WGS84坐標(biāo)與該時(shí)刻平臺(tái)的姿態(tài))。因?yàn)榧す饫走_(dá)采集的數(shù)據(jù)是用極坐標(biāo)表示的相對(duì)于激光雷達(dá)本身設(shè)備坐標(biāo)系的點(diǎn)云坐標(biāo),因此,需要把激光雷達(dá)坐標(biāo)系下的點(diǎn)云坐標(biāo)轉(zhuǎn)換為WGS84坐標(biāo)系下的坐標(biāo)或者東北天坐標(biāo)系下的坐標(biāo)。在這一轉(zhuǎn)換過程中,需要已知激光雷達(dá)相對(duì)于POS坐標(biāo)系的旋轉(zhuǎn)矩陣與平移向量,本文研究通過已有控制點(diǎn)求解激光雷達(dá)坐標(biāo)系相對(duì)于POS坐標(biāo)系的旋轉(zhuǎn)與平移參數(shù)的計(jì)算方法。

2.2坐標(biāo)轉(zhuǎn)換模型

LiDAR采集的點(diǎn)為設(shè)備坐標(biāo)系,INS系統(tǒng)的坐標(biāo)系為POS坐標(biāo)系,而最終要獲得的點(diǎn)云坐標(biāo)為東北天坐標(biāo)系或其他地理坐標(biāo)系。本節(jié)描述了各個(gè)坐標(biāo)系的定義,并建立從設(shè)備坐標(biāo)系到東北天坐標(biāo)系的轉(zhuǎn)換公式[3]。

(1)激光掃描儀坐標(biāo)系

激光掃描儀坐標(biāo)系定義,原點(diǎn):以激光雷達(dá)發(fā)射中心為原點(diǎn)O;X軸:激光掃描平面0°對(duì)應(yīng)的軸;Y軸:激光掃描平面90°對(duì)應(yīng)的軸;Z軸:垂直XOY平面,并且與X、Y軸構(gòu)成右手坐標(biāo)系;激光掃描在X-O-Y平面,從上往下觀察,激光掃描方向?yàn)槟鏁r(shí)針旋轉(zhuǎn)。

圖1　設(shè)備坐標(biāo)系

圖1中,X-O-Y平面上一個(gè)激光點(diǎn)的極坐標(biāo)表示為(σ,θ),則該點(diǎn)在激光雷達(dá)坐標(biāo)系下的坐標(biāo)為(xL, yL,zL)= [σcos(θ),σsin(θ),0]。

(2)POS(GPS/ IMU)坐標(biāo)系

原點(diǎn):GPS天線相位中心作為原點(diǎn);

坐標(biāo)系三個(gè)軸與IMU內(nèi)部的三個(gè)陀螺儀軸平行;

X軸控制俯仰;Y軸控制側(cè)滾,Z軸控制航向;

(3)WGS-84大地坐標(biāo)系

原點(diǎn):地球質(zhì)心;

X軸:指向BIH1984.0定義的地區(qū)極(CTP)方向;

Z軸:指向BIH1984.0的零子午面和CTP赤道的交點(diǎn);

Y軸:與Z,X軸構(gòu)成的右手坐標(biāo)系。

GNSS坐標(biāo)采用的是WGS-84經(jīng)緯度坐標(biāo),為了計(jì)算各種坐標(biāo)系的轉(zhuǎn)換,需要把GNSS的經(jīng)緯度坐標(biāo)轉(zhuǎn)換為東北天坐標(biāo)系。

(4)東北天坐標(biāo)系:

以選定的點(diǎn)為原點(diǎn),X軸:指向東;Y軸:指向北;Z 軸:過原點(diǎn)垂直水平面向上。

東北天坐標(biāo)系又名站心坐標(biāo)系,首先選定一個(gè)點(diǎn)作為原點(diǎn),在該點(diǎn)按照三軸方向建立坐標(biāo)系,用于表示局部小范圍的笛卡兒坐標(biāo)系。使用GNSS采集的WGS84坐標(biāo)可以通過計(jì)算公式在WGS84坐標(biāo)系與東北天坐標(biāo)系之間進(jìn)行坐標(biāo)互轉(zhuǎn)。

(5)數(shù)學(xué)模型

把激光掃描儀坐標(biāo)系下的坐標(biāo)轉(zhuǎn)換到東北天坐標(biāo)系下需要進(jìn)行兩次旋轉(zhuǎn)與平移運(yùn)算[2,4]。第一次轉(zhuǎn)換:把激光掃描儀坐標(biāo)系轉(zhuǎn)換到POS坐標(biāo)系,即:

其中(x,y,z)T為激光掃描儀坐標(biāo)系下的點(diǎn);RLP為激光掃描儀到POS坐標(biāo)系的旋轉(zhuǎn)矩陣,(X0,Y0,Z0)T為激光掃描儀原點(diǎn)到POS坐標(biāo)系原點(diǎn)的偏移。(X,Y, Z)T為激光點(diǎn)在POS坐標(biāo)系下的坐標(biāo)。

第二次轉(zhuǎn)換:把POS坐標(biāo)系下的坐標(biāo)轉(zhuǎn)換到東北天坐標(biāo)系下,即:

其中(X,Y,Z)T為POS坐標(biāo)系下的坐標(biāo);RPg為POS坐標(biāo)系到東北天坐標(biāo)系的旋轉(zhuǎn)矩陣,(ˉXP,ˉYP, ˉZP)T為POS坐標(biāo)系原點(diǎn)在東北天坐標(biāo)系下的偏移。(ˉX,ˉY,ˉZ)T為東北天坐標(biāo)系下的坐標(biāo)。

合并式(1)和式(2),得到掃描儀坐標(biāo)系到東北天坐標(biāo)系的轉(zhuǎn)換公式為:

在系統(tǒng)標(biāo)定過程中通過3對(duì)以上的控制點(diǎn)求取RLP和(X0,Y0,Z0)T即激光掃描儀坐標(biāo)系在POS坐標(biāo)系中的旋轉(zhuǎn)矩陣與偏移向量。在點(diǎn)云坐標(biāo)解算過程中帶入點(diǎn)云坐標(biāo),POS解算后的heading、pitch、roll三個(gè)角度以及(ˉXP,ˉYP,ˉZP)T坐標(biāo)即可把點(diǎn)云坐標(biāo)轉(zhuǎn)換為東北天坐標(biāo)系中,完成點(diǎn)云坐標(biāo)解算。

3　粒子群算法系統(tǒng)標(biāo)定原理

3.1粒子群算法

粒子群算法(Particle Swarm Optimization,簡(jiǎn)稱PSO),是一種仿生學(xué)優(yōu)化算法。其理論基礎(chǔ)是以單一粒子來做為鳥類族群之中的單一個(gè)體,于算法中賦予該個(gè)體擁有記憶性,并能夠透過與其他粒子之間的互動(dòng)而尋求到最適解。任一個(gè)體粒子皆可用有自身移動(dòng)過程中所產(chǎn)生的記憶與經(jīng)驗(yàn),當(dāng)個(gè)體移動(dòng)的同時(shí),能依靠自身的經(jīng)驗(yàn)與記憶來學(xué)習(xí)調(diào)整自身的移動(dòng)方向,由于在粒子群算法中,多個(gè)粒子是同時(shí)移動(dòng)的,且同時(shí)以自身經(jīng)驗(yàn)與其他粒子所提供的經(jīng)驗(yàn)進(jìn)行比對(duì)找尋最適當(dāng)?shù)慕?并使自己處于最適解中,該粒子群算法的特性使得粒子不單單受自身演化的影響,同時(shí)會(huì)對(duì)群體間的演化擁有學(xué)習(xí)性、記憶性,并使粒子本身達(dá)到最佳調(diào)整。其特點(diǎn)是實(shí)現(xiàn)簡(jiǎn)單、魯莽性強(qiáng)、不需要嚴(yán)格初始化、全局收斂性好。

3.2目標(biāo)函數(shù)建模

已知M個(gè)控制點(diǎn),Pj=(xj,yj,zj)(1≤j≤M)表示激光掃描儀坐標(biāo)系中的點(diǎn)坐標(biāo),Pj點(diǎn)在東北天坐標(biāo)系中的坐標(biāo)為ˉPj= (ˉxj,ˉyj,ˉzj)。Pj點(diǎn)采集時(shí)刻平臺(tái)的POS為POSj。

式(4)描述了把激光雷達(dá)坐標(biāo)系中的點(diǎn)(x,y,z)轉(zhuǎn)換到東北天坐標(biāo)系下,其中RPg由POSj中的三個(gè)角度分量按歐拉角構(gòu)造的旋轉(zhuǎn)矩陣;RLP由Qi的三個(gè)角度分量按歐拉角構(gòu)造的旋轉(zhuǎn)矩陣;(x,y,z)T為Pj, (X0,Y0,Z0)T為Qi的前三個(gè)坐標(biāo)分量;(ˉXP,ˉYP,ˉZP)T為POSj中的前三個(gè)位置分量。成本函數(shù)定義:

式(5)中ˉPj為已知條件中控制點(diǎn)在東北天坐標(biāo)系下的坐標(biāo)。成本函數(shù)的物理含義:通過式(2)與變量的值計(jì)算的控制點(diǎn)在東北天坐標(biāo)系下的坐標(biāo)與實(shí)測(cè)的坐標(biāo)之間的距離平方和。如果該值越小,則表示變量的值接近真實(shí)的解。

3.3算法步驟

使用粒子群算法求解本問題的基本步驟如下:

步驟1:初始化N個(gè)變量,每個(gè)變量記為Qi=(xi, yi,zi,θi,ωi,ψi)(1≤i≤N),前三個(gè)分量為位置分量,后三個(gè)分量為旋轉(zhuǎn)角度分量,迭代的次數(shù)n=0;

步驟2:初始化N個(gè)速度變量Vi=(Vxi,Vyi,Vzi,Vθi, Vωi,Vψi);

步驟3:初始化N個(gè)局部最優(yōu)解變量Qbesti=Qi;及對(duì)應(yīng)的成本Fi=+∞;

步驟4:計(jì)算每個(gè)變量Qi的成本函數(shù)Fnewi=∑Mj=1| f (Pj,POSj,Qi)-ˉPj|2;如果Fnewi＜Fi,則Fi=Fnewi,Qbesti=Qi;

步驟5:計(jì)算最小成本Fm= mini∈[1,N]{Fi},既第m個(gè)變量Qm為目前的全局最優(yōu)解,則Qbest= Qbestm,Fbest= Fm;如果Fbest＜ε則退出,Qbest即為解;

步驟6:更新速度Vi:

步驟7:更新變量狀態(tài):Qi=Qi+Vi,n=n+1轉(zhuǎn)步驟4;

3.4速度更新算法

粒子群算法中非常重要的步驟就是更新速度(第6步),具體算法如下:

步驟6.1:△Vi=c1r1(Qbesti-Qi)+c2r2(Qbest-Qi);

步驟6.2:計(jì)算搜索步長(zhǎng)因子:

步驟6.3:VPi=λ·norm[norm(VPi)+rand(1,c5)·norm(△VPi)],其中VPi為Vi的前三個(gè)位置分量,△VPi為△Vi的前三個(gè)位置分量,norm()為歸一化函數(shù),rand (1,c3)表示1到c3的隨機(jī)數(shù);

步驟6.4:VAi=λ·norm[norm(VAi)+rand(1,c5)· norm(△VAi)],其中VAi為Vi的后三個(gè)角度分量,△VAi為△Vi的后三個(gè)角度分量;

步驟6.5:用VPi和VAi更新Vi。

3.5參數(shù)說明

步驟5中,ε為退出條件,取值根據(jù)精度需要而定,取1e-6。

步驟6.1中,1≤c1≤c2≤2,c1=1.5,c2=2;r1,r2∈[0, 1]的隨機(jī)數(shù)。該步驟根據(jù)每一個(gè)變量的局部最優(yōu)解、全局最優(yōu)解和當(dāng)前變量值構(gòu)造一個(gè)速度變化值△Vi。

步驟6.2中,步長(zhǎng)因子表示速度變化的大小,Fbest越小步長(zhǎng)越小,有利于對(duì)全局最優(yōu)解進(jìn)行小范圍搜索;反之,有利于從較大范圍進(jìn)行搜索。當(dāng)?shù)拇螖?shù)增加時(shí),搜索步長(zhǎng)中的sin部分會(huì)使步長(zhǎng)周期性變化,當(dāng)sin的值變大時(shí),可以擴(kuò)大搜索范圍,變小時(shí),會(huì)對(duì)最優(yōu)解局部進(jìn)行搜索。c3,c4為常數(shù),1≤c3≤10,50≤c4≤200,c3取2,c4取100。

步驟6.3和步驟6.4對(duì)速度的位置和角度分量分別進(jìn)行計(jì)算,其中1≤c5≤10,c5取1.5。

4　實(shí) 驗(yàn)

4.1參數(shù)獲取實(shí)驗(yàn)

實(shí)驗(yàn)使用重慶市勘測(cè)院與重慶數(shù)字城市科技有限公司聯(lián)合研發(fā)的吉信移動(dòng)測(cè)量系統(tǒng)作為實(shí)驗(yàn)平臺(tái)。該系統(tǒng)集成3臺(tái)激光掃描儀,1臺(tái)POS,Trimble GNSS接收機(jī)以及DMI。

圖2　吉信移動(dòng)測(cè)量系統(tǒng)

我們?cè)谥貞c市國(guó)際博覽中心前建立標(biāo)定場(chǎng),使用全站儀在道路兩邊50 m范圍內(nèi)共設(shè)置了20個(gè)點(diǎn)位,在其中14個(gè)點(diǎn)位上放置了反射靶標(biāo)。并采集了沿線道路若干電桿和房屋角點(diǎn)作為精度評(píng)定點(diǎn)。針對(duì)右測(cè)激光掃描儀的實(shí)驗(yàn)數(shù)據(jù)如表1所示。

實(shí)測(cè)控制點(diǎn) 表1

POS數(shù)據(jù) 表2

求解后,得到Qbest= (0.205, - 0.043,0.162, 89.872,- 90.752, - 0.852)。在硬件上測(cè)量的值為(0.20,-0.04,0.16,90,-90,0),差別較小?？梢钥闯?該算法可以用來解決該問題。

4.2系統(tǒng)精度實(shí)驗(yàn)

為了驗(yàn)證以上結(jié)果的正確性,進(jìn)行了精度實(shí)驗(yàn)。實(shí)驗(yàn)區(qū)為重慶市國(guó)博中心大道。使用以上結(jié)果進(jìn)行點(diǎn)云解算,把點(diǎn)云坐標(biāo)與RTK實(shí)測(cè)坐標(biāo)進(jìn)行比較。一共選取了13個(gè)地物點(diǎn),原始數(shù)據(jù)與對(duì)比結(jié)果如表3所示。

從表3中,可以獲得平面中誤差為0.066 m,點(diǎn)位中誤差為0.093 m。

系統(tǒng)精度實(shí)驗(yàn)數(shù)據(jù) 表3

5　結(jié) 語

本文介紹了地面移動(dòng)測(cè)量系統(tǒng)標(biāo)定的基本原理與數(shù)學(xué)模型,在此基礎(chǔ)上提出使用粒子群算法對(duì)標(biāo)定過程進(jìn)行優(yōu)化,并給出了具體實(shí)現(xiàn)算法與速度更新算法。并通過實(shí)驗(yàn)證明計(jì)算結(jié)果與實(shí)測(cè)值非常吻合,并通過精度實(shí)驗(yàn)驗(yàn)證了標(biāo)定結(jié)果可以使系統(tǒng)達(dá)到較高的精度。說明該方法可以用于求解地面移動(dòng)測(cè)量系統(tǒng)的標(biāo)定問題。另外,還可以根據(jù)不同的約束條件對(duì)該方法進(jìn)行改進(jìn),可以引進(jìn)面擬合等方法,只需要對(duì)成本函數(shù)進(jìn)行適當(dāng)修改,這也是我們后續(xù)要進(jìn)行的研究與改進(jìn)。理論上,模擬退火、遺傳算法等優(yōu)化算法對(duì)此類問題同樣適用。

參考文獻(xiàn)

[1] 楊長(zhǎng)強(qiáng).激光掃描儀檢校及車載激光點(diǎn)云的分類與矢量化研究[D].山東:山東科技大學(xué),2010.

[2] 胡競(jìng).車載三維激光移動(dòng)建模系統(tǒng)總體檢校方法研究[D].北京:首都師范大學(xué),2011.

[3] 張小紅.機(jī)載激光雷達(dá)測(cè)量技術(shù)理論與方法[M].武漢:武漢大學(xué)出版社,2007.

[4] 吳華意,宋愛紅,李新科.機(jī)載激光雷達(dá)系統(tǒng)的應(yīng)用與數(shù)據(jù)后處理技術(shù)[J].測(cè)繪與空間地理信息,2006,29(3): 59～63.

[5] 沈嚴(yán),李磊,阮友田.車載激光測(cè)繪技術(shù)[J].紅外與激光工程,2009,38(3):437～439.

Ground Mobile Measurement Platform System Based on Particle Swarm Optimization (PSO) Method Research

Zhang Jie1,Long Chuan2,Yin Fei3,Yang Yuan3
(1.Chongqing Surveying Institute,Chongqing 400020,China; 2.Engineering research center for geographic information cloud service enterprises in chongqing,Chongqing 400020,China; 3.Chongqing Cybercity Sci-tech Co.,Ltd,Chongqing 400020,China)

Abstract:Ground mobile measurement technique based on laser scanner is just developed a new type of surveying and mapping technology in recent years,the technology integration laser scanner,INS (GNSS and IMU),DMI,cameras and other sensors,can quickly obtain a large area of high precision point cloud and image data.The system application in domestic still in its infancy,there are many problems need to study in the process of data collection.This article focuses on the calibration method of the system,on the basis of the existing mathematical model,adopting the particle swarm algorithm to optimize a calibration calculation process,simplify the computer algorithm implementation,enhances the method of recklessness,good global convergence.Through the experiment,proved the effectiveness of the proposed method.

Key words:ground mobile measurement system;LiDAR;POS;system calibration;PSO

文章編號(hào):1672-8262(2015)01-97-05中圖分類號(hào):P225.7

文獻(xiàn)標(biāo)識(shí)碼:A

收稿日期:?2014—05—21

作者簡(jiǎn)介:張婕(1979—),女,工程師,主要從事GIS、地圖編制、地面移動(dòng)測(cè)量系統(tǒng)的應(yīng)用研究。

基金項(xiàng)目:重慶市科委:重慶市科技人才培養(yǎng)計(jì)劃(cstc2013kjrc-tdjs-40001)