一种从多线激光雷达点云数据中识别圆柱面的方法与流程

本发明属于环境感知领域，尤其涉及一种从多线激光雷达点云数据识别圆柱面的方法。

背景技术：

早先的激光雷达，由于购买成本和使用条件的限制，并没有得到广泛的关注和应用。但近年来，随着激光雷达技术的飞速发展，其应用领域不断拓宽，特别是在环境感知领域，利用激光雷达与相机的融合感知外部环境更是成为了无人系统使用的主流方法。激光雷达与相机的异质融合首先需要使用标定物进行联合外标定，标定物的选取直接影响着融合效果。现有联合外标定文献中最常见的标定物就是黑白相间的棋盘格，但使用棋盘格作为标定物需要在稳定的实验环境中进行离线联合外标定。

从保证联合外标定精度的角度出发，由于现实环境中干扰因素的存在，随着时间的推移，联合外标定精度不可避免的降低，这就需要在现实环境中寻找一种可以作为激光雷达与相机进行在线联合外标定的标定物。而在现实环境中无法有效地找到类似于棋盘格特征的物体，但现实环境中存在着较多的具有圆柱面特征的物体，如建筑中的圆柱体、桥墩、水上灯塔以及圆柱面型浮标等物体。利用现实环境中的圆柱面作为联合外标定中的标定物较少被人提及，主要是激光雷达得到的是稀疏不完整的圆柱面点云数据，现有圆柱面拟合的研究文献是基于高精度高分辨率的三维激光扫描仪获得大致完整的圆柱面点云数据的基础上进行的，对于稀疏不完整点云圆柱面的拟合少有研究。为了解决环境感知领域中，多线激光雷达在与相机联合外标定时将圆柱面作为标定物遇到的圆柱面识别问题，本发明基于上述事实，提出一种用于多线激光雷达点云数据识别圆柱面方法。

技术实现要素：

本发明为了解决上述问题，提出一种从多线激光雷达点云数据中识别圆柱面的方法，其特征在于包含以下步骤：

步骤1：设定圆柱面数学模型

由圆柱面的几何特性可得，圆柱面上的点到其轴线的距离恒等于半径r0，即：

其中，p(x,y,z)^t为圆柱面上任意一点，p0(x0,y0,z0)^t为圆柱轴线上一点，l(a,b,c)^t为圆柱轴线单位向量，r0为圆柱底圆半径。圆柱轴线上一点(x0,y0,z0)^t，圆柱轴线单位向量(a,b,c)^t，圆柱底圆半径r0，这七个参数可以唯一确定一个圆柱面。

步骤2：圆柱面拟合预处理

将激光雷达扫描圆柱面得到的不完整圆柱面点云数据通过ransac方法进行平面拟合，假设平面方程为：

f(x,p)＝x^tp+d＝0(2)

式(2)中，x＝(x,y,z)^t为任意点云坐标；p＝(k,m,n)^t为平面单位法向量；d为原点至平面的距离，单位：mm。ransa方法依据测量点至平面距离平方和满足阈值的原则求取最佳平面方程参数p和d,因此误差方程为：

ei＝f(xi,p)(3)

对所有点列出误差方程，利用阈值判断条件(4)，找寻满足判断条件最多的点数，然后确定参数p,d；

ei<v(4)

式(4)中，v代表的是距离误差阈值，单位mm；也是最后的拟合平面的平整度,i点在平面上的投影点的坐标(xpi,ypi,zpi)^t为：

求出投影坐标后，每一线激光雷达的点云投影是一段三维椭圆弧，对于三维椭圆拟合其方程复杂难以有效求解，可以经过坐标转换将其转换为二维椭圆拟合，经过坐标转换的椭圆弧方程其参数不会改变。

对投影点进行坐标转换，使平面单位法向量(k,m,n)^t变换为平行于z轴的向量即(0,0,1)^t。设旋转矩阵为r3x3，要使：

即

将旋转矩阵与投影点坐标(xpi,ypi,zpi)^t相乘,得到新的坐标(x′i,y′i,z′i)^t,将新坐标中的x′i、y′i取出得(x′,y′)^t。(x′,y′)^t为平行于xoy平面的平面上的坐标，根据(x′,y′)^t可进行平面椭圆拟合。

步骤3：圆柱面拟合

由于激光雷达的自身以及现场环境的影响，获得的圆柱面点云数据不可避免地带有噪声，这些噪声会影响圆柱面的拟合精度，所以需要对得到的圆柱面点云数据进行降噪处理。激光雷达的噪声类似于高斯噪声，所以本发明的点云降噪方法首先是去除远离圆柱主体部分的漂移点，之后对于每一线剩下的点云数据使用移动平均滤波法。计算公式如下：

其中，r′(j)为第j个滤波之后的激光雷达到圆柱面的距离点，单位：mm；r(j)为滤波之前第j个激光雷达到圆柱面的测量距离点，单位：mm；队列长度j根据实验值设定。滤波后通过激光雷达竖直和水平方向的角度设定规则计算成点云坐标(x,y,z)^t。

由于雷达每一线激光在圆柱体上的点云是椭圆弧特征，且经过降噪之后激光雷达的噪声依旧存在，本发明使用一种对于噪声或者遮挡情况的椭圆的有效拟合方法。利用激光雷达在圆柱面上形成的椭圆弧在轴线与激光雷达中心连线的方向上对称的这一几何特性，采取不将所有点都列为拟合点的基于代数距离的最小平方短轴倾角椭圆拟合方法。

二维平面椭圆拟合方法步骤可总结为：

1)依据椭圆弧几何对称这一特性，利用下式随机选取拟合点；

其中(x′sel,y′sel)^t表示选取的待拟合点集，(x′s,y′s)^t第s个旋转之后的点，tot是(x′,y′)^t的元素总数，s代表的是选取间隔。

2)(x′sel,y′sel)^t为进行椭圆拟合的坐标点，将椭圆方程中常数项归化为1后，椭圆方程可表示为：

ax²+bxy+cy²+dx+ey+1＝0(10)

令u＝[a,b,c,d,e,1]^t,v＝[x²,xy,y²,x,y,1]^t,则优化目标为：

其中

根据拉格朗日乘子法，引入拉格朗日因子λ，构造拉格朗日函数

l(u)＝u^tvv^tu-(uthu-1)(12)

对等式(12)求导得零

vv^tu＝λhu(13)

通过求解等式(13)中的广义特征值问题得到式(11)的最优解。

从而求得椭圆的几何特征参数为：

椭圆中心坐标(单位：mm)：

长短半轴平方(单位：mm²)：

短轴倾角：

3)将s依次按照1,2,3,5,7,9设定值重复执行步骤1)-步骤2),选取min‖θ‖²对应的椭圆系数u′＝[a′,b′,c′,d′,e′,1]^t作为最终的椭圆拟合效果。

对于激光雷达每一线激光束得到的圆柱点云数据执行上述的步骤1)、2)、3)之后，可以获得γ组椭圆的几何特征参数，对γ个椭圆中心坐标进行奇异值分解,可以得到圆柱面的轴线单位向量(a,b,c)^t，圆柱轴线上一点(x0,y0,z0)^t设为第γ-1线激光束圆柱面点云所在平面与轴线的交点；圆柱面底圆半径通过式(17)得到。

其中,γ为使用的多线激光雷达的激光线束个数；βi是第i线激光束扫描形成的平面法向量与单位向量(0,0,1)^t的夹角，单位是rad；aci是第i个椭圆拟合计算得到的短轴，单位是mm。

本发明，与现有圆柱面拟合技术相比，简单高效具有较高的实时性和可实现性，能够快速识别圆柱面；本发明提出的方法对圆柱面的识别具有较高的精度，能够为使用多线激光雷达的无人系统提高外部环境感知能力。

附图说明：

图1是一种从多线激光雷达点云数据中识别圆柱面的方法的流程图。

具体实施方式：

以下是本发明的优选实施例并结合附图，对本发明的技术方案作进一步的描述，但本发明不限于实施例。

如图1所示，本发明的从多线激光雷达点云数据中识别圆柱面的方法包括以下步骤：

步骤1：设定圆柱面数学模型

由圆柱面的几何特性可得，圆柱面上的点到其轴线的距离恒等于半径r0，即：

其中，p(x,y,z)^t为圆柱面上任意一点，p0(x0,y0,z0)^t为圆柱轴线上一点，l(a,b,c)^t为圆柱轴线单位向量，r0为圆柱底圆半径。圆柱轴线上一点(x0,y0,z0)^t，圆柱轴线单位向量(a,b,c)^t，圆柱底圆半径r0，这七个参数可以唯一确定一个圆柱面。

步骤2：圆柱面拟合预处理

将激光雷达扫描圆柱面得到的不完整圆柱面点云数据通过ransac方法进行平面拟合，假设平面方程为：

f(x,p)＝x^tp+d＝0(2)

式(2)中，x＝(x,y,z)^t为任意点云坐标；p＝(k,m,n)^t为平面单位法向量；d为原点至平面的距离，单位：mm。ransa方法依据测量点至平面距离平方和满足阈值的原则求取最佳平面方程参数p和d,因此误差方程为：

ei＝f(xi,p)(3)

对所有点列出误差方程，利用阈值判断条件(4)，找寻满足判断条件最多的点数，然后确定参数p,d；

ei<v(4)

式(4)中，v代表的是距离误差阈值，单位mm；也是最后的拟合平面的平整度,i点在平面上的投影点的坐标(xpi,ypi,zpi)^t为：

求出投影坐标后，每一线激光雷达的点云投影是一段三维椭圆弧，对于三维椭圆拟合其方程复杂难以有效求解，可以经过坐标转换将其转换为二维椭圆拟合，经过坐标转换的椭圆弧方程其参数不会改变。

对投影点进行坐标转换，使平面单位法向量(k,m,n)^t变换为平行于z轴的向量即(0,0,1)^t。设旋转矩阵为r3x3，要使：

即

将旋转矩阵与投影点坐标(xpi,ypi,zpi)^t相乘,得到新的坐标(x′i,y′i,z′i)^t,将新坐标中的x′i、y′i取出得(x′,y′)^t。(x′,y′)^t为平行于xoy平面的平面上的坐标，根据(x′,y′)^t可进行平面椭圆拟合。

步骤3：圆柱面拟合

由于激光雷达的自身以及现场环境的影响，获得的圆柱面点云数据不可避免地带有噪声，这些噪声会影响圆柱面的拟合精度，所以需要对得到的圆柱面点云数据进行降噪处理。激光雷达的噪声类似于高斯噪声，所以本发明的点云降噪方法首先是去除远离圆柱主体部分的漂移点，之后对于每一线剩下的点云数据使用移动平均滤波法。计算公式如下：

其中，r′(j)为第j个滤波之后的激光雷达到圆柱面的距离点，单位：mm；r(j)为滤波之前第j个激光雷达到圆柱面的测量距离点，单位：mm；队列长度j根据实验值设定。滤波后通过激光雷达竖直和水平方向的角度设定规则计算成点云坐标(x,y,z)^t。

由于雷达每一线激光在圆柱体上的点云是椭圆弧特征，且经过降噪之后激光雷达的噪声依旧存在，本发明使用一种对于噪声或者遮挡情况的椭圆的有效拟合方法。利用激光雷达在圆柱面上形成的椭圆弧在轴线与激光雷达中心连线的方向上对称的这一几何特性，采取不将所有点都列为拟合点的基于代数距离的最小平方短轴倾角椭圆拟合方法。

二维平面椭圆拟合方法步骤可总结为：

2)依据椭圆弧几何对称这一特性，利用下式随机选取拟合点；

其中(x′sel,y′sel)^t表示选取的待拟合点集，(x′s,y′s)^t第s个旋转之后的点，tot是(x′,y′)^t的元素总数，s代表的是选取间隔。

2)(x′sel,y′sel)^t为进行椭圆拟合的坐标点，将椭圆方程中常数项归化为1后，椭圆方程可表示为：

ax²+bxy+cy²+dx+ey+1＝0(10)

令u＝[a,b,c,d,e,1]^t,v＝[x²,xy,y²,x,y,1]^t,则优化目标为：

其中

根据拉格朗日乘子法，引入拉格朗日因子λ，构造拉格朗日函数

l(u)＝u^tvv^tu-(u^thu-1)(12)

对等式(12)求导得零

vv^tu＝λhu(13)

通过求解等式(13)中的广义特征值问题得到式(11)的最优解。

从而求得椭圆的几何特征参数为：

椭圆中心坐标(单位：mm)：

长短半轴平方(单位：mm²)：

短轴倾角：

3)将s依次按照1,2,3,5,7,9设定值重复执行步骤1)-步骤2),选取min‖θ‖²对应的椭圆系数u′＝[a′,b′,c′,d′,e′,1]^t作为最终的椭圆拟合效果。

对于激光雷达每一线激光束得到的圆柱点云数据执行上述的步骤1)、2)、3)之后，可以获得γ组椭圆的几何特征参数，对γ个椭圆中心坐标进行奇异值分解,可以得到圆柱面的轴线单位向量(a,b,c)^t，圆柱轴线上一点(x0,y0,z0)^t设为第γ-1线激光束圆柱面点云所在平面与轴线的交点；圆柱面底圆半径通过式(17)得到。

其中,γ为使用的多线激光雷达的激光线束个数；βi是第i线激光束扫描形成的平面法向量与单位向量(0,0,1)^t的夹角，单位是rad；aci是第i个椭圆拟合计算得到的短轴，单位是mm。