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

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

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

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

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

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

1　引 言

激光掃描技術(shù)又稱LiDAR(Light Detecting and Ranging),是近幾年發(fā)展起來的一種新型三維空間信息獲取技術(shù)[1]。它集光學(xué)、機(jī)械、電子和計算機(jī)等多個學(xué)科的最新研究成果,可以大面積,高精度低獲取三維空間坐標(biāo)信息。地面移動測量系統(tǒng)是把LiDAR、INS、DMI、相機(jī)等傳感器集成到地面移動車輛平臺上,可以快速獲取道路周邊的精確點(diǎn)云和影像數(shù)據(jù)的一種高新技術(shù)。近年來,國內(nèi)外地面移動測量系統(tǒng)是繼機(jī)載LiDAR逐步成熟后的又一個研究熱點(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ǔ)上,研究使用粒子群算法求解移動測量平臺中LiDAR設(shè)備與POS坐標(biāo)系的旋轉(zhuǎn)與平移參數(shù)。

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

2.1問題描述

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

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

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é)描述了各個坐標(biāo)系的定義,并建立從設(shè)備坐標(biāo)系到東北天坐標(biāo)系的轉(zhuǎn)換公式[3]。

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

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

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

圖1中,X-O-Y平面上一個激光點(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)系三個軸與IMU內(nèi)部的三個陀螺儀軸平行;

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),為了計算各種坐標(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)系,首先選定一個點(diǎn)作為原點(diǎn),在該點(diǎn)按照三軸方向建立坐標(biāo)系,用于表示局部小范圍的笛卡兒坐標(biāo)系。使用GNSS采集的WGS84坐標(biāo)可以通過計算公式在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對以上的控制點(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三個角度以及(ˉ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,簡稱PSO),是一種仿生學(xué)優(yōu)化算法。其理論基礎(chǔ)是以單一粒子來做為鳥類族群之中的單一個體,于算法中賦予該個體擁有記憶性,并能夠透過與其他粒子之間的互動而尋求到最適解。任一個體粒子皆可用有自身移動過程中所產(chǎn)生的記憶與經(jīng)驗(yàn),當(dāng)個體移動的同時,能依靠自身的經(jīng)驗(yàn)與記憶來學(xué)習(xí)調(diào)整自身的移動方向,由于在粒子群算法中,多個粒子是同時移動的,且同時以自身經(jīng)驗(yàn)與其他粒子所提供的經(jīng)驗(yàn)進(jìn)行比對找尋最適當(dāng)?shù)慕?并使自己處于最適解中,該粒子群算法的特性使得粒子不單單受自身演化的影響,同時會對群體間的演化擁有學(xué)習(xí)性、記憶性,并使粒子本身達(dá)到最佳調(diào)整。其特點(diǎn)是實(shí)現(xiàn)簡單、魯莽性強(qiáng)、不需要嚴(yán)格初始化、全局收斂性好。

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

已知M個控制點(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)采集時刻平臺的POS為POSj。

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

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

3.3算法步驟

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

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

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

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

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

步驟5:計算最小成本Fm= mini∈[1,N]{Fi},既第m個變量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:計算搜索步長因子:

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

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

步驟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ù)每一個變量的局部最優(yōu)解、全局最優(yōu)解和當(dāng)前變量值構(gòu)造一個速度變化值△Vi。

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

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

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

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

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

圖2　吉信移動測量系統(tǒng)

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

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

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

求解后,得到Qbest= (0.205, - 0.043,0.162, 89.872,- 90.752, - 0.852)。在硬件上測量的值為(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ū)為重慶市國博中心大道。使用以上結(jié)果進(jìn)行點(diǎn)云解算,把點(diǎn)云坐標(biāo)與RTK實(shí)測坐標(biāo)進(jìn)行比較。一共選取了13個地物點(diǎn),原始數(shù)據(jù)與對比結(jié)果如表3所示。

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

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

5　結(jié) 語

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

參考文獻(xiàn)

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

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

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

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

[5] 沈嚴(yán),李磊,阮友田.車載激光測繪技術(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

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

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

收稿日期:?2014—05—21

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

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