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

1、 基于粒子群算法的地面移動測量平臺系統(tǒng)標定方法研究張 婕1，龍 川2，殷 飛3，楊 元3重慶市勘測院，重慶 400020；重慶市地理信息云服務(wù)企業(yè)工程技術(shù)研究中心，重慶 400020；3.重慶數(shù)字城市科技有限公司，重慶 400020）摘要：基于激光掃描儀的地面移動測量技術(shù)是近幾年剛剛發(fā)展起來的新型測繪技術(shù)，該技術(shù)集成激光掃描儀、INS（GNSS+IMU）、DMI、相機等傳感器，可以快速獲取大面積的高精度點云與影像數(shù)據(jù)。本文研究了系統(tǒng)的標定方法，在已有數(shù)學(xué)模型的基礎(chǔ)上，采用粒子群算法對標定計算過程進行優(yōu)化，簡化了計算機算法實現(xiàn)，增強了方法的魯莽性，全局收斂性好。通過實驗，系統(tǒng)誤差達到10cm左右

2、，證明該方法的有效性。關(guān)鍵詞：地面移動測量系統(tǒng)；LiDAR；POS；系統(tǒng)標定；粒子群算法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 infor

3、mation 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 technolog

4、y 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 articl

5、e 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. Throu

6、gh the experiment, proved the effectiveness of the proposed method.Key Words: Ground mobile measurement system; LiDAR; POS; system calibration; PSO1 引言激光掃描技術(shù)又稱LiDAR（Light Detecting and Ranging)，是近幾年發(fā)展起來的一種新型三維空間信息獲取技術(shù)1。它集光學(xué)、機械、電子和計算機等多個學(xué)科的最新研究成果，可以大面積，高精度低獲取三維空間坐標信息。地面移動測量系統(tǒng)是把LiDAR、INS、DMI、相機等傳感器集成到

7、地面移動車輛平臺上，可以快速獲取道路周邊的精確點云和影像數(shù)據(jù)的一種高新技術(shù)。近年來，國內(nèi)外地面移動測量系統(tǒng)是繼機載LiDAR逐步成熟后的有一個研究熱點，相關(guān)產(chǎn)品相繼出現(xiàn)。其精度主要受POS精度與系統(tǒng)標定精度的影響。目前系統(tǒng)標定通常使用最小二乘法進行求解1,2。本文在傳統(tǒng)系統(tǒng)標定原理的基礎(chǔ)上，研究使用粒子群算法求解移動測量平臺中LiDAR設(shè)備與POS坐標系的旋轉(zhuǎn)與平移參數(shù)。2 地面移動測量坐標轉(zhuǎn)換原理2.1問題描述激光雷達、GNSS、IMU傳感器安裝在同一剛性平臺上，整個平臺安裝在移動車輛上。在前進過程中，同步采集激光點云數(shù)據(jù)，GNSS原始數(shù)據(jù)，IMU的三軸加速度與三軸角速度，各種數(shù)據(jù)通過時間進

8、行嚴格同步。在數(shù)據(jù)處理過程中5，把GNSS原始數(shù)據(jù)與IMU的原始數(shù)據(jù)首先進行POS解算，獲得一定時間間隔周期的POS數(shù)據(jù)（位置與姿態(tài)數(shù)據(jù)，包括WGS84坐標與該時刻平臺的姿態(tài)）。因為激光雷達采集的數(shù)據(jù)是用極坐標表示的相對于激光雷達本身設(shè)備坐標系的點云坐標，因此，需要把激光雷達坐標系下的點云坐標轉(zhuǎn)換為WGS84坐標系下的坐標或者東北高坐標系下的坐標。在這一轉(zhuǎn)換過程中，需要已知激光雷達相對于POS坐標系的旋轉(zhuǎn)矩陣與平移向量，本文研究通過已有控制點求解激光雷達坐標系相對于POS坐標系的旋轉(zhuǎn)與平移參數(shù)的計算方法。2.2坐標轉(zhuǎn)換模型 LiDAR采集的點為設(shè)備坐標系，INS系統(tǒng)的坐標系為POS坐標系，而最

9、終要獲得的點云坐標為東北高坐標系或其它地理坐標系。本節(jié)描述了各個坐標系的定義，并建立從設(shè)備坐標系到東北高坐標系的轉(zhuǎn)換公式3。(1)激光掃描儀坐標系激光掃描儀坐標系定義，原點：以激光雷達發(fā)射中心為原點O；X軸：激光掃描平面0對應(yīng)的軸；Y軸：激光掃描平面90對應(yīng)的軸；Z軸：垂直XOY平面，并且與X、Y軸構(gòu)成右手坐標系；激光掃描在X-O-Y平面，從上往下觀察，激光掃描方向為逆時針旋轉(zhuǎn)。圖1 設(shè)備坐標系上圖中，X-O-Y平面上一個激光點的極坐標表示為()，則該點在激光雷達坐標系下的坐標為 ()=()。(2)POS（GPS/IMU）坐標系原點： GPS天線相位中心作為原點； 坐標系三個軸與IMU內(nèi)部的三

10、個陀螺儀軸平行；X軸控制俯仰；Y軸控制側(cè)滾，Z軸控制航向；(3)WGS-84大地坐標系原點：地球質(zhì)心，X軸：指向BIH1984.0定義的地區(qū)極（CTP）方向；Z軸：指向BIH1984.0的零子午面和CTP赤道的交點；Y軸：與Z，X軸構(gòu)成的右手坐標系。GNSS坐標采用的是WGS-84經(jīng)緯度坐標，為了計算各種坐標系的轉(zhuǎn)換，需要把GNSS的經(jīng)緯度坐標轉(zhuǎn)換為東北高坐標系。(4)東北高坐標系：以選定的點為原點，X軸：指向東；Y軸：指向北；Z軸：過原點垂直水平面向上。東北高坐標系又名站心坐標系，首先選定一個點作為原點，在該點按照三軸方向建立坐標系，用于表示局部小范圍的笛卡爾坐標系。使用GNSS采集的WGS

11、84坐標可以通過計算公式在WGS84坐標系與東北高坐標系之間進行坐標互轉(zhuǎn)。(5)數(shù)學(xué)模型把激光掃描儀坐標系下的坐標轉(zhuǎn)換到東北高坐標系下需要進行兩次旋轉(zhuǎn)與平移運算2,4。第一次轉(zhuǎn)換：把激光掃描儀坐標系轉(zhuǎn)換到POS坐標系，即： （1）其中為激光掃描儀坐標系下的點；為激光掃描儀到POS坐標系的旋轉(zhuǎn)矩陣，為激光掃描儀原點到POS坐標系原點的偏移。為激光點在POS坐標系下的坐標。第二次轉(zhuǎn)換：把POS坐標系下的坐標轉(zhuǎn)換到東北高坐標系下，即： （2）其中為POS坐標系下的坐標；為POS坐標系到東北高坐標系的旋轉(zhuǎn)矩陣，為POS坐標系原點在東北高坐標系下的偏移。為東北高坐標系下的坐標。合并公式（1）和公式（2）

12、，得到掃描儀坐標系到東北高坐標系的轉(zhuǎn)換公式為： （3）在系統(tǒng)標定過程中通過3對以上的控制點求取和即激光掃描儀坐標系在POS坐標系中的旋轉(zhuǎn)矩陣與偏移向量。在點云坐標解算過程中帶入點云坐標，POS解算后的heading、pitch、roll三個角度以及坐標即可把點云坐標轉(zhuǎn)換為東北高坐標系中，完成點云坐標解算。 3 粒子群算法系統(tǒng)標定原理3.1粒子群算法粒子群算法（Particle Swarm Optimization，簡稱PSO），是一種仿生學(xué)優(yōu)化算法。其理論基礎(chǔ)是以單一粒子來做為鳥類族群之中的單一個體，于算法中賦予該HYPERLINK /wiki/個體個體擁有記憶性，并能夠透過與其他粒子之間的互

13、動而尋求到最適解。任一個體粒子皆可用有自身移動過程中所產(chǎn)生的記憶與經(jīng)驗，當個體移動的同時，能依靠自身的經(jīng)驗與記憶來學(xué)習(xí)調(diào)整自身的移動方向，由于在粒子群算法中，多個粒子是同時移動的，且同時以自身經(jīng)驗與其他粒子所提供的經(jīng)驗進行比對找尋最適當?shù)慕?，并使自己處于最適解中，該粒子群算法的特性使得粒子不單單受自身演化的影響，同時會對群體間的演化擁有學(xué)習(xí)性、記憶性，并使粒子本身達到最佳調(diào)整。其特點是實現(xiàn)簡單、魯莽性強、不需要嚴格初始化、全局收斂性好。3.2 目標函數(shù)建模已知個控制點，（）表示激光掃描儀坐標系中的點坐標，點在東北高坐標系中的坐標為。點采集時刻平臺的POS為。 （4）公式（4）描述了把激光雷達坐

14、標系中的點轉(zhuǎn)換到東北高坐標系下，其中由中的三個角度分量按歐拉角構(gòu)造的旋轉(zhuǎn)矩陣；由的三個角度分量按歐拉角構(gòu)造的旋轉(zhuǎn)矩陣；為，為的前三個坐標分量；為中的前三個位置分量。成本函數(shù)定義： （5）公式（5）中為已知條件中控制點在東北高坐標系下的坐標。成本函數(shù)的物理含義：通過公式（2）與變量的值計算的控制點在東北高坐標系下的坐標與實測的坐標之間的距離平方和。如果該值越小，則表示變量的值接近真實的解。3.3 算法步驟使用粒子群算法求解本問題的基本步驟如下：步驟1:初始化個變量，每個變量記為()，前三個分量為位置分量，后三個分量為旋轉(zhuǎn)角度分量，迭代的次數(shù);步驟2:初始化個速度變量;步驟3:初始化個局部最優(yōu)解變

15、量;及對應(yīng)的成本；步驟4:計算每個變量的成本函數(shù);如果，則，；步驟5:計算最小成本，既第個變量為目前的全局最優(yōu)解，則，;如果則退出，即為解;步驟6:更新速度:步驟7：更新變量狀態(tài)：，轉(zhuǎn)步驟4;3.4速度更新算法粒子群算法中非常重要的步驟就是更新速度(第6步)，具體算法如下：步驟6.1:;步驟6.2:計算搜索步長因子：;步驟6.3： ，其中為的前三個位置分量，為的前三個位置分量，為歸一化函數(shù)，表示1到的隨機數(shù)；步驟6.4： ，其中為的后三個角度分量，為的后三個角度分量；步驟6.5: 用和更新；3.5參數(shù)說明步驟5中，為退出條件，取值根據(jù)精度需要而定，取1e-6。步驟6.1中，,、;的隨機數(shù)。該步

16、驟根據(jù)每一個變量的局部最優(yōu)解、全局最優(yōu)解和當前變量值構(gòu)造一個速度變化值。步驟6.2中，步長因子表示速度變化的大小，越小步長越小，有利于對全局最優(yōu)解進行小范圍搜索；反之，有利于從較大范圍進行搜索。當?shù)拇螖?shù)增加時，搜索步長中的sin部分會使步長周期性變化，當sin的值變大時，可以擴大搜索范圍，變小時，會對最優(yōu)解局部進行搜索。為常數(shù)，取2，取100。步驟6.3和步驟6.4對速度的位置和角度分量分別進行計算，其中,取1.5。4 實驗4.1 參數(shù)獲取實驗實驗使用重慶市勘測院與重慶數(shù)字城市科技有限公司聯(lián)合研發(fā)的吉信移動測量系統(tǒng)作為實驗平臺。該系統(tǒng)集成三臺激光掃描儀，一臺POS，Trimble GNSS

17、接收機，以及DMI。圖2 ：吉信移動測量系統(tǒng)我們在重慶市國際博覽中心前建立標定場，使用全站儀在道路兩邊50米范圍內(nèi)共設(shè)置了20個點位，在其中14個點位上放置了反射靶標。并采集了沿線道路若干電桿和房屋角點作為精度評定點。針對右測激光掃描儀的實驗數(shù)據(jù)如下。 實測控制點 表1序號激光設(shè)備坐標WGS84坐標13.3851.1840.0649188.9363289984.524232.480213.7611.0840.0649198.2023289979.950232.403324.0220.5890.0649206.3383289973.445232.520434.299-0.2610.0649216

18、.6233289970.810232.81053.5061.2010.0649204.5533290013.471232.368614.4191.4350.0649213.9333290007.955232.096724.730.390.0649223.2003290003.482232.544834.929-0.0910.0649231.4423289997.193232.8629-40.4333.0860.0649166.8343290036.289232.30310-28.9332.7770.0649158.6733289996.707232.43511-40.7463.2840.064

19、9147.7623290001.426232.42812-29.4854.1210.0649141.2403289967.276232.17113-42.8574.4710.0649130.5333289975.375232.42514-57.9826.5160.0649116.6793289981.623231.937 POS數(shù)據(jù) 表2序號姿態(tài)位置航行俯仰側(cè)滾129.0485-1.5414-0.75129.7192485106.5417776232.903 229.0268-1.5771-0.726229.7192509106.5417791232.901 329.2004-1.4807-0

20、.781829.7192342106.5417681232.915 429.0532-1.56-0.758829.7192483106.5417774232.903 526.9963-1.7339-0.699329.7195076106.5419409232.785 627.0272-1.7617-0.659429.719503106.5419381232.785 727.0695-1.8152-0.620329.7194974106.5419347232.786 827.2238-1.5385-0.410329.7194793106.5419236232.801 926.6258-1.682

21、3-0.611229.7195538106.5419686232.774 1029.2381-1.4831-0.73329.7192309106.5417659232.917 1129.3042-1.5007-0.726929.7192258106.5417626232.921 1230.4892-1.5648-1.370429.7189688106.5415904233.043 1330.0244-1.6282-1.372329.7189872106.541603233.032 1429.9414-1.6234-1.369129.7189921106.5416064233.029 求解后，得

22、到=(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)精度實驗為了驗證以上結(jié)果的正確性，進行了精度實驗。實驗區(qū)為重慶市國博中心大道。使用以上結(jié)果進行點云解算，把點云坐標與RTK實測坐標進行比較。一共選取了13個地物點，原始數(shù)據(jù)與對比結(jié)果見表3。 系統(tǒng)精度實驗數(shù)據(jù) 表3RTK坐標點云坐標誤差平面誤差絕對誤差X0Y0Z0XYZXYZ649204.553 3290013.471 232.180 649204.541 3290

23、013.553 232.133 -0.012 0.082 -0.047 0.083 0.095 649176.230 3290030.718 231.962 649176.257 3290030.746 231.925 0.027 0.028 -0.037 0.039 0.054 649152.432 3290042.653 231.935 649152.458 3290042.709 231.927 0.026 0.056 -0.008 0.062 0.062 649140.122 3290050.894 231.978 649140.117 3290050.940 231.918 -0.0

24、05 0.046 -0.060 0.046 0.076 649147.762 3290001.426 231.949 649147.773 3290001.477 231.908 0.011 0.051 -0.041 0.052 0.066 649134.617 3290009.024 232.033 649134.628 3290009.073 232.020 0.011 0.049 -0.013 0.050 0.052 649121.058 3290013.909 231.962 649121.000 3290013.891 231.942 -0.058 -0.018 -0.020 0.0

25、61 0.064 649141.240 3289967.276 232.205 649141.268 3289967.329 232.163 0.028 0.053 -0.042 0.060 0.073 649116.679 3289981.623 231.948 649116.659 3289981.606 231.949 -0.020 -0.017 0.001 0.026 0.026 649104.948 3289991.248 231.922 649104.930 3289991.210 231.888 -0.018 -0.038 -0.034 0.042 0.054 649176.690 3290028.713 231.119 649176.704 3290028.785 231.100 0.014 0.072 -0.019 0.073 0.076 649160.491 3289998.944 231.214 649160.508 3289998.992 231.210 0.017 0.048 -0.004 0.051 0.051 649345.216 3290375.605 229.997