基于粒子群算法的地面移动测量平台系统标定方法研究

张婕,龙川,殷飞,杨元(1.重庆市勘测院,重庆 40000; .重庆市地理信息云服务企业工程技术研究中心,重庆 40000; .重庆数字城市科技有限公司,重庆 40000)

基于粒子群算法的地面移动测量平台系统标定方法研究

张婕1∗,龙川2,殷飞3,杨元3
(1.重庆市勘测院,重庆 400020; 2.重庆市地理信息云服务企业工程技术研究中心,重庆 400020; 3.重庆数字城市科技有限公司,重庆 400020)

摘 要:基于激光扫描仪的地面移动测量技术是近几年刚刚发展起来的新型测绘技术,该技术集成激光扫描仪、INS (GNSS+IMU)、DMI、相机等传感器,可以快速获取大面积的高精度点云与影像数据。本文研究了系统的标定方法,在已有数学模型的基础上,采用粒子群算法对标定计算过程进行优化,简化了计算机算法实现,增强了方法的鲁棒性,全局收敛性好。通过实验,系统误差达到10 cm左右,证明该方法的有效性。

关键词:地面移动测量系统;LiDAR;POS;系统标定;粒子群算法

1　引 言

激光扫描技术又称LiDAR(Light Detecting and Ranging),是近几年发展起来的一种新型三维空间信息获取技术[1]。它集光学、机械、电子和计算机等多个学科的最新研究成果,可以大面积,高精度低获取三维空间坐标信息。地面移动测量系统是把LiDAR、INS、DMI、相机等传感器集成到地面移动车辆平台上,可以快速获取道路周边的精确点云和影像数据的一种高新技术。近年来,国内外地面移动测量系统是继机载LiDAR逐步成熟后的又一个研究热点,相关产品相继出现。其精度主要受POS精度与系统标定精度的影响。目前系统标定通常使用最小二乘法进行求解[1,2]。本文在传统系统标定原理的基础上,研究使用粒子群算法求解移动测量平台中LiDAR设备与POS坐标系的旋转与平移参数。

2　地面移动测量坐标转换原理

2.1问题描述

激光雷达、GNSS、IMU传感器安装在同一刚性平台上,整个平台安装在移动车辆上。在前进过程中,同步采集激光点云数据,GNSS原始数据,IMU的三轴加速度与三轴角速度,各种数据通过时间进行严格同步。

在数据处理过程中[5],把GNSS原始数据与IMU的原始数据首先进行POS解算,获得一定时间间隔周期的POS数据(位置与姿态数据,包括WGS84坐标与该时刻平台的姿态)。因为激光雷达采集的数据是用极坐标表示的相对于激光雷达本身设备坐标系的点云坐标,因此,需要把激光雷达坐标系下的点云坐标转换为WGS84坐标系下的坐标或者东北天坐标系下的坐标。在这一转换过程中,需要已知激光雷达相对于POS坐标系的旋转矩阵与平移向量,本文研究通过已有控制点求解激光雷达坐标系相对于POS坐标系的旋转与平移参数的计算方法。

2.2坐标转换模型

LiDAR采集的点为设备坐标系,INS系统的坐标系为POS坐标系,而最终要获得的点云坐标为东北天坐标系或其他地理坐标系。本节描述了各个坐标系的定义,并建立从设备坐标系到东北天坐标系的转换公式[3]。

(1)激光扫描仪坐标系

激光扫描仪坐标系定义,原点:以激光雷达发射中心为原点O;X轴:激光扫描平面0°对应的轴;Y轴:激光扫描平面90°对应的轴;Z轴:垂直XOY平面,并且与X、Y轴构成右手坐标系;激光扫描在X-O-Y平面,从上往下观察,激光扫描方向为逆时针旋转。

图1　设备坐标系

图1中,X-O-Y平面上一个激光点的极坐标表示为(σ,θ),则该点在激光雷达坐标系下的坐标为(xL, yL,zL)= [σcos(θ),σsin(θ),0]。

(2)POS(GPS/ IMU)坐标系

原点:GPS天线相位中心作为原点;

坐标系三个轴与IMU内部的三个陀螺仪轴平行;

X轴控制俯仰;Y轴控制侧滚,Z轴控制航向;

(3)WGS-84大地坐标系

原点:地球质心;

X轴:指向BIH1984.0定义的地区极(CTP)方向;

Z轴:指向BIH1984.0的零子午面和CTP赤道的交点;

Y轴:与Z,X轴构成的右手坐标系。

GNSS坐标采用的是WGS-84经纬度坐标,为了计算各种坐标系的转换,需要把GNSS的经纬度坐标转换为东北天坐标系。

(4)东北天坐标系:

以选定的点为原点,X轴:指向东;Y轴:指向北;Z 轴:过原点垂直水平面向上。

东北天坐标系又名站心坐标系,首先选定一个点作为原点,在该点按照三轴方向建立坐标系,用于表示局部小范围的笛卡儿坐标系。使用GNSS采集的WGS84坐标可以通过计算公式在WGS84坐标系与东北天坐标系之间进行坐标互转。

(5)数学模型

把激光扫描仪坐标系下的坐标转换到东北天坐标系下需要进行两次旋转与平移运算[2,4]。第一次转换:把激光扫描仪坐标系转换到POS坐标系,即:

其中(x,y,z)T为激光扫描仪坐标系下的点;RLP为激光扫描仪到POS坐标系的旋转矩阵,(X0,Y0,Z0)T为激光扫描仪原点到POS坐标系原点的偏移。(X,Y, Z)T为激光点在POS坐标系下的坐标。

第二次转换:把POS坐标系下的坐标转换到东北天坐标系下,即:

其中(X,Y,Z)T为POS坐标系下的坐标;RPg为POS坐标系到东北天坐标系的旋转矩阵,(¯XP,¯YP, ¯ZP)T为POS坐标系原点在东北天坐标系下的偏移。(¯X,¯Y,¯Z)T为东北天坐标系下的坐标。

合并式(1)和式(2),得到扫描仪坐标系到东北天坐标系的转换公式为:

在系统标定过程中通过3对以上的控制点求取RLP和(X0,Y0,Z0)T即激光扫描仪坐标系在POS坐标系中的旋转矩阵与偏移向量。在点云坐标解算过程中带入点云坐标,POS解算后的heading、pitch、roll三个角度以及(¯XP,¯YP,¯ZP)T坐标即可把点云坐标转换为东北天坐标系中,完成点云坐标解算。

3　粒子群算法系统标定原理

3.1粒子群算法

粒子群算法(Particle Swarm Optimization,简称PSO),是一种仿生学优化算法。其理论基础是以单一粒子来做为鸟类族群之中的单一个体,于算法中赋予该个体拥有记忆性,并能够透过与其他粒子之间的互动而寻求到最适解。任一个体粒子皆可用有自身移动过程中所产生的记忆与经验,当个体移动的同时,能依靠自身的经验与记忆来学习调整自身的移动方向,由于在粒子群算法中,多个粒子是同时移动的,且同时以自身经验与其他粒子所提供的经验进行比对找寻最适当的解,并使自己处于最适解中,该粒子群算法的特性使得粒子不单单受自身演化的影响,同时会对群体间的演化拥有学习性、记忆性,并使粒子本身达到最佳调整。其特点是实现简单、鲁莽性强、不需要严格初始化、全局收敛性好。

3.2目标函数建模

已知M个控制点,Pj=(xj,yj,zj)(1≤j≤M)表示激光扫描仪坐标系中的点坐标,Pj点在东北天坐标系中的坐标为¯Pj= (¯xj,¯yj,¯zj)。Pj点采集时刻平台的POS为POSj。

式(4)描述了把激光雷达坐标系中的点(x,y,z)转换到东北天坐标系下,其中RPg由POSj中的三个角度分量按欧拉角构造的旋转矩阵;RLP由Qi的三个角度分量按欧拉角构造的旋转矩阵;(x,y,z)T为Pj, (X0,Y0,Z0)T为Qi的前三个坐标分量;(¯XP,¯YP,¯ZP)T为POSj中的前三个位置分量。成本函数定义:

式(5)中¯Pj为已知条件中控制点在东北天坐标系下的坐标。成本函数的物理含义:通过式(2)与变量的值计算的控制点在东北天坐标系下的坐标与实测的坐标之间的距离平方和。如果该值越小,则表示变量的值接近真实的解。

3.3算法步骤

使用粒子群算法求解本问题的基本步骤如下:

步骤1:初始化N个变量,每个变量记为Qi=(xi, yi,zi,θi,ωi,ψi)(1≤i≤N),前三个分量为位置分量,后三个分量为旋转角度分量,迭代的次数n=0;

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

步骤3:初始化N个局部最优解变量Qbesti=Qi;及对应的成本Fi=+∞;

步骤4:计算每个变量Qi的成本函数Fnewi=∑Mj=1| f (Pj,POSj,Qi)-¯Pj|2;如果Fnewi＜Fi,则Fi=Fnewi,Qbesti=Qi;

步骤5:计算最小成本Fm= mini∈[1,N]{Fi},既第m个变量Qm为目前的全局最优解,则Qbest= Qbestm,Fbest= Fm;如果Fbest＜ε则退出,Qbest即为解;

步骤6:更新速度Vi:

步骤7:更新变量状态:Qi=Qi+Vi,n=n+1转步骤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()为归一化函数,rand (1,c3)表示1到c3的随机数;

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

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

3.5参数说明

步骤5中,ε为退出条件,取值根据精度需要而定,取1e-6。

步骤6.1中,1≤c1≤c2≤2,c1=1.5,c2=2;r1,r2∈[0, 1]的随机数。该步骤根据每一个变量的局部最优解、全局最优解和当前变量值构造一个速度变化值△Vi。

步骤6.2中,步长因子表示速度变化的大小,Fbest越小步长越小,有利于对全局最优解进行小范围搜索;反之,有利于从较大范围进行搜索。当迭代的次数增加时,搜索步长中的sin部分会使步长周期性变化,当sin的值变大时,可以扩大搜索范围,变小时,会对最优解局部进行搜索。c3,c4为常数,1≤c3≤10,50≤c4≤200,c3取2,c4取100。

步骤6.3和步骤6.4对速度的位置和角度分量分别进行计算,其中1≤c5≤10,c5取1.5。

4　实 验

4.1参数获取实验

实验使用重庆市勘测院与重庆数字城市科技有限公司联合研发的吉信移动测量系统作为实验平台。该系统集成3台激光扫描仪,1台POS,Trimble GNSS接收机以及DMI。

图2　吉信移动测量系统

我们在重庆市国际博览中心前建立标定场,使用全站仪在道路两边50 m范围内共设置了20个点位,在其中14个点位上放置了反射靶标。并采集了沿线道路若干电杆和房屋角点作为精度评定点。针对右测激光扫描仪的实验数据如表1所示。

实测控制点 表1

POS数据 表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系统精度实验

为了验证以上结果的正确性,进行了精度实验。实验区为重庆市国博中心大道。使用以上结果进行点云解算,把点云坐标与RTK实测坐标进行比较。一共选取了13个地物点,原始数据与对比结果如表3所示。

从表3中,可以获得平面中误差为0.066 m,点位中误差为0.093 m。

系统精度实验数据 表3

5　结 语

本文介绍了地面移动测量系统标定的基本原理与数学模型,在此基础上提出使用粒子群算法对标定过程进行优化,并给出了具体实现算法与速度更新算法。并通过实验证明计算结果与实测值非常吻合,并通过精度实验验证了标定结果可以使系统达到较高的精度。说明该方法可以用于求解地面移动测量系统的标定问题。另外,还可以根据不同的约束条件对该方法进行改进,可以引进面拟合等方法,只需要对成本函数进行适当修改,这也是我们后续要进行的研究与改进。理论上,模拟退火、遗传算法等优化算法对此类问题同样适用。

参考文献

[1] 杨长强.激光扫描仪检校及车载激光点云的分类与矢量化研究[D].山东:山东科技大学,2010.

[2] 胡竞.车载三维激光移动建模系统总体检校方法研究[D].北京:首都师范大学,2011.

[3] 张小红.机载激光雷达测量技术理论与方法[M].武汉:武汉大学出版社,2007.

[4] 吴华意,宋爱红,李新科.机载激光雷达系统的应用与数据后处理技术[J].测绘与空间地理信息,2006,29(3): 59～63.

[5] 沈严,李磊,阮友田.车载激光测绘技术[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

文献标识码:A

收稿日期:∗2014—05—21

作者简介:张婕(1979—),女,工程师,主要从事GIS、地图编制、地面移动测量系统的应用研究。

基金项目:重庆市科委:重庆市科技人才培养计划(cstc2013kjrc-tdjs-40001)