CN112147598A - 一种基于直角墙面的激光标定方法 - Google Patents

Abstract

本发明涉及一种基于直角墙面的激光标定方法，求激光雷达的安装误差角：S1、使小车正面和侧面分别与直角墙面的两面墙面平行；S2、激光雷达采集小车正面墙面的激光点云数据，并做直线拟合将所述激光点云数据拟合成一条直线，求出该直线的斜率角；求激光雷达中心到小车中心的距离和激光雷达中心在小车坐标系中的偏移角：A1、小车原地旋转，记录旋转过程中激光雷达在墙面坐标系下的坐标值；A2、将激光雷达在墙面坐标系下的运动轨迹点进行圆拟合，并求出半径；A3、当激光雷达到小车正面墙面的距离最小时，记录小车的旋转角，即为激光雷达中心在小车坐标系中的偏移角。本方法通过旋转AGV小车即可实现标定，无需特定的标定物，方法具有可靠精度。

Description

一种基于直角墙面的激光标定方法

技术领域

本发明属于自主导航小车技术领域，具体涉及一种基于直角墙面的激光标定方法。

背景技术

基于激光测距传感的自主导航小车(AGV)被广泛应用于柔性生产系统(FMS)中,可以用于现代化工厂或仓库中的柔性搬运,是现代物流系统和工业生产中的关键设备之一。在小车运动模型中,一般将小车的中心点作为小车质点，激光雷达与车体刚性连接,两者间的相对姿态和位置固定不变,为了将激光雷达的点云数据从激光雷达坐标转换至车体坐标,需要对激光雷达标定。

SLAM(simultaneous localization and mapping，同时定位与建图)是实现无人车技术的关键，高质量的激光点云数据是保证快速特征匹配、高精度末端校准以及机动性避障的前提，因此在激光SLAM系统，高精度简易性的激光雷达标定工作一直成为研究者关注的重点。

现有的激光雷达标定算法大多是依靠特定的标定物进行标定，如高精度标定台，基于标定物的激光标定方法虽然可以保障较高精度的标定结果，但步骤过于复杂，标定物不方便携带，同时带来了不必要的成本。

发明内容

本发明的目的在于提供一种基于直角墙面的激光标定方法，本方法通过旋转AGV小车即可实现标定，无需特定的标定物。

为实现上述目的，本发明所采取的技术方案是：

一种基于直角墙面的激光标定方法，其特征在于包括求激光雷达的安装误差角、激光雷达中心到小车中心的距离和激光雷达中心在小车坐标系中的偏移角；

求激光雷达的安装误差角：

S1、将小车移动至直角墙面的墙角处，使小车正面和侧面分别与直角墙面的两面墙面平行；

S2、激光雷达采集小车正面墙面的激光点云数据，并做直线拟合将所述激光点云数据拟合成一条直线，求出该直线的斜率角，所述斜率角即为激光雷达的安装误差角；

求激光雷达中心到小车中心的距离和激光雷达中心在小车坐标系中的偏移角：

A1、控制小车原地旋转，记录旋转过程中激光雷达在墙面坐标系下的坐标值；

A2、将激光雷达在墙面坐标系下的运动轨迹点进行圆拟合，并求出半径，所述半径即为激光雷达中心到小车中心的距离；

A3、当激光雷达到小车正面墙面的距离最小时，记录小车的旋转角，所述旋转角即为激光雷达中心在小车坐标系中的偏移角。

进一步地，所述步骤S2包括：

S201、激光雷达采集小车正面墙面的激光点云数据，对该激光点云数据先进行RANSAC估计滤噪，再进行最小二乘直线拟合，求出拟合直线的斜率角；

S202、重复步骤S201，至少得到三组斜率角，对所有斜率角取平均斜率角值，所述平均斜率角值即为激光雷达的安装误差角。

进一步地，所述步骤A1中控制小车原地旋转，记录旋转过程中激光雷达在墙面坐标系下的坐标值：

(1)将直角墙面构建成墙面坐标系o-xy，墙角为坐标系原点，激光雷达扫描平面与墙面坐标系重合；

(2)小车原地旋转过程中，记录激光雷达旋转轨迹在墙面坐标系o-xy上的表示，建立激光雷达旋转模型1η(ω)_x和模型2η(ω)_y，η(ω)_x表示垂直于墙面1的激光束的方位角，η(ω)_y表示垂直墙面2的激光束的方位角；

η(ω)_x＝(δ-ω)·180/π+90 η,δ,ω∈(-π，π)

η(ω)_y＝(δ-ω)·180/π+180 η,δ,ω∈(-π，π)

模型1和模型2中，ω为车体累计旋转角，取车头方向为正，用弧度值表示；δ激光安装误差角，取车头方向为正，用弧度值表示；η表示垂直于墙面激光束的方位角，取激光雷达0°角方向为正，用角度值表示；

(3)获取每一帧点云集合，集合包括集合内所有点云到激光雷达的距离ρ，建立模型3；

模型3：i＝θ·n/360

ρi＝N[i]

模型3中，N为一帧点云集合，n为N内点云的数量，θ为点云的方位角，i为点云在N中的索引值，ρi为点云i到激光雷达的距离；

(4)将模型1和模型2分别带入模型3，求出垂直于墙面1的目标点云到激光雷达的距离ρx和垂直于墙面2的目标点云到激光雷达的距离ρy，在墙面坐标系o-xy下ρx表示激光雷达运行轨迹点的x值，ρy表示激光雷达运行轨迹点的y值，记录每一帧的(ρx,ρy)，将激光雷达运动轨迹构建一个运动轨迹集合∑ρxy，则∑ρxy反映了激光雷达在墙面坐标系o-xy下的运行轨迹；

其中，ρ_jx、ρ_jy为第j帧激光点云到墙壁1、2的垂直距离。

进一步地，所述步骤A2中激光雷达在墙面坐标系下的运动轨迹点采用RANSAC估计进行圆拟合：

(1)将激光雷达运动轨迹上所有点的坐标值构成一个运动轨迹集合，在集合中随机选择三个点构建模型；

(2)设容错值e为0.02m，计算集合中剩余点到圆心的距离R'，e'＝|R-R'|，当e'<e时，表示该点为局内点，累计局内点个数；

(3)迭代重复步骤(1)和(2)，当当前模型局内点数比上一次的更多时，更新当前模型为最优模型，并更新最优模型的局内点集合；

(4)选择最优模型的局内点重新进行RANSAC估计，并设容错值e为0.01m；

(5)最后根据重新估计的模型计算得到半径R。

进一步地，所述步骤A3包括：遍历所述集合，寻找最小Y轴值，并记录最小Y轴值时小车的旋转角，即为激光雷达中心在小车坐标系中的偏移角。

本发明的有益效果为：本方法通过旋转AGV小车即可实现标定，无需特定的标定物，方法具有可靠精度。

附图说明

图1是激光雷达与小车坐标系的关系示意图。

图2是激光雷达与小车与直角墙面的关系示意图。

图3是小车旋转时与激光雷达的关系示意图。

图4是激光雷达的运动轨迹与墙面坐标系的关系示意图。

图5是小车的旋转角度与激光雷达的运动轨迹的关系示意图。

图6是二维激光雷达测量原理示意图。

图7是墙壁2点云拟合直线示意图。

图8是集合∑ρxy的点云示意图。

图9是激光雷达旋转轨迹点拟合圆示意图。

具体实施方式

为了更好地理解本发明，下面结合实施例和附图对本发明的技术方案做进一步的说明。

如图6所示，二维激光雷达测量原理：

二维激光雷达在其扫描平面上的扫描范围为-180°～+180°,并在扫描范围内均匀的分散着若干激光束。其中0°扫描激光束平行于激光雷达的底面指向正前方,其角度大小随逆时针方向旋转而增加。二维激光雷达发射激光束探测外界物体，同时以极坐标(ρ’,α)的方式记录着探测点M到激光描平面上激光发射中心O的距离值ρ’，和经过M点的扫描激光束距0°扫描激光束的角度值α。二维激光雷达扫描平面有自身的二维坐标系o-xy，其中x轴的正方向与激光雷达0°扫描激光束的发射方向相同,y轴的正方向与激光雷达90°扫描激光束的发射方向相同，点o是激光雷达的扫描中心。由此将扫描平面上某点的极坐标形式转换成直角坐标：x＝ρ’cosα，y＝ρ’sinα。

如图1所示，激光雷达与小车坐标系关系

自主导航小车(下文简称AGV或小车)，为了实现在车间内安全高效地自动化搬运功能，除了车体本身，核心传感器通常包括Lidar(激光雷达)、IMU(惯性测量单元)、轮式编码器。其中，激光雷达坐标系与车体坐标系之间的变换关系是本实施例研究的内容。

2D激光导航小车系统设计到三个坐标系：地图坐标系Ow-XwYwZw，用(Xw,Yw,Zw)表示；小车坐标系Or-XrYrZr，用(Xr,Yr,Zr)表示；激光雷达坐标系Os-XsYsZs，用(Xs,Ys,Zs)表示。本实施例所用的小车为四麦轮驱动型，车体中心位于四轮中央，以车体中心为原点，车头方向为X轴正方向，Y轴垂直于X轴并朝向左侧，Z轴垂直于XY轴朝上，构成小车坐标系Or-XrYrZr；以激光雷达中心为原点，零度角方向为X轴正方向，Y轴垂直于X轴并朝向左侧，Z轴垂直于XY轴朝上，构成激光雷达坐标系Os-XsYsZs。

在2D激光自然导航系统中，激光雷达默认水平安装在车体上，Os-XsYs平面平行于Or-XrYr平面，俯仰角(pitch)、横滚角(roll)默认为0，又因为Z轴方向平移参数不影响定位结果计算，因此求解由xy方向平移参数和绕Z轴的旋转参数δ组成的3自由度参数。

如图1，其中小车坐标系原点Or为小车中心点，并将Xr所指的方向定义为小车的航向；激光雷达坐标系坐标原点为Os，取Xs为激光雷达正方向；δ为小车坐标系与激光雷达坐标系的夹角；Or和Os之间的距离为R，夹角β为Or和Os连线与小车坐标系Xr的夹角(即激光雷达中心在小车坐标系中的偏移角)。激光外参数标定结果是求激光雷达中心到小车中心x方向偏移、y方向偏移、二者夹角yaw的过程，因此需要通过δ、R、β进一步求得x、y、yaw：yaw＝δ，y＝Rsinβ，x＝Rcosβ，即本实施例最终要求的是δ、R、β。

一种基于直角墙面的激光标定方法，包括求激光雷达的安装误差角δ、激光雷达中心到小车中心的距离R和激光雷达中心在小车坐标系中的偏移角β；

如图2所示，求激光雷达的安装误差角δ：

S1、将小车移动至直角墙面的墙角处，使小车正面和侧面分别与直角墙面的墙面2和墙面1平行；

S2、激光雷达采集墙面2的激光点云数据，保存为点云集合P，对集合P先进行RANSAC估计降噪，再对降噪后的点云数据进行最小二乘直线拟合，并求出拟合直线的斜率角ψ(如图7所示)；

S3、重复步骤S201，至少得到三组斜率角ψ，对所有斜率角取平均斜率角值；如图2所示，由于拟合直线的斜率角ψ与激光雷达的安装误差角δ相等，所以平均斜率角值即为激光雷达的安装误差角δ。

如图3、4、5所示，求激光雷达中心到小车中心的距离R和激光雷达中心在小车坐标系中的偏移角β：

A1、控制小车原地旋转，记录旋转过程中激光雷达在墙面坐标系下的坐标值(ρx，ρy)，其中ρx和ρy分别为激光雷达中心到墙面1和墙面2的垂直距离；

(1)将直角墙面构建成墙面坐标系o-xy，墙角为坐标系原点，激光雷达扫描平面与墙面坐标系重合，如图5所示；

(2)小车原地旋转过程中，为了记录激光雷达旋转轨迹在墙面坐标系o-xy上的表示，建立激光雷达旋转模型1η(ω)_x和模型2η(ω)_y，η(ω)_x表示垂直于墙面1的激光束的方位角(即激光束到激光0°角的偏角)，η(ω)_y表示垂直墙面2的激光束的方位角(规定逆时针旋转为正)；

η(ω)_x＝(δ-ω)·180/π+90 η,δ,ω∈(-π，π)

η(ω)_y＝(δ-ω)·180/π+180 η,δ,ω∈(-π，π)

其中，ω为车体累计旋转角，取车头方向为正，用弧度值表示；δ激光安装误差角，取车头方向为正，用弧度值表示；η表示垂直于墙面激光束的方位角(即激光束到激光0°角的偏角)，取激光雷达0°角方向为正，用角度值表示；

(3)获取每一帧点云集合，集合包括所有点云到激光雷达的距离ρ；按照角度等分原则建立模型3，通过模型3可以求得一帧点云集合中任何角度θ所对应的距离ρ；

模型3：i＝θ·n/360

ρi＝N[i]

其中，N为一帧点云集合，n为N内点云的数量，θ为点云的方位角，i为点云在N中的索引值，ρi为点云i到激光雷达的距离；

(4)将模型1和模型2分别带入模型3，可推算出垂直于墙面1的目标点云到激光雷达的距离ρx和垂直于墙面2的目标点云到激光雷达的距离ρy，在墙面坐标系o-xy下ρx表示激光雷达运行轨迹点的x值，ρy表示激光雷达运行轨迹点的y值，记录每一帧的(ρx,ρy)，将激光雷达运动轨迹构建一个运动轨迹集合∑ρxy，则∑ρxy反映了激光雷达在墙面坐标系o-xy下的运行轨迹(如图8所示)；

其中，ρ_jx、ρ_jy为第j帧激光点云到墙壁1、2的垂直距离；

A2、将激光雷达在墙面坐标系下的运动轨迹点采用RANSAC估计进行圆拟合(如图9所示)，并求出半径；

(1)RANSAC估计圆拟合算法：将激光雷达运动轨迹上所有点的坐标值构成一个运动轨迹集合∑ρxy，在集合中随机选择三个点(x1,y1)，(x2,y2)，(x3,y3)，通过三点确定圆心和半径，令圆心坐标为(x0,y0)，半径为R，构建圆模型M为：

Ax²+Ay²+Bx+Cy+D＝0 (1)

将三个点带入公式(1)，并提取方程未知变量系数，得：

推出：

圆心(x0,y0)及半径R为：

(2)设容错值e为0.02m，计算集合∑ρxy中剩余点到圆心的距离R'，e'＝|R-R'|，当e'<e时，表示该点(适应于当前估计的模型M)为局内点，累计局内点个数；

(3)迭代重复步骤(1)和(2)，当当前模型局内点数比上一次的更多时，更新当前模型为最优模型，并更新最优模型的局内点集合；

(4)选择最优模型的局内点重新进行RANSAC估计，并设容错值e为0.01m(减小容错值可进一步提高当前估计的精度)；

(5)最后根据重新估计的模型计算得到半径R，即为激光雷达中心到小车中心的距离R；

A3、遍历所述集合∑ρxy，寻找最小Y轴值ρy，并记录最小Y轴值ρy时小车的旋转角σ，如图5，激光雷达中心在小车坐标系中的偏移角β与旋转角σ相等。

RANSAC算法介绍：RANSAC算法的输入是一组包含噪声的观测数据，通过反复选取数据中的一组随机子集来完成模型参数估计，被选取的子集假设为局内点，构建一个适应于假设的局内点的模型M，用模型M去测试剩余数据，如果某个点适应于该模型，则认为点为局内点。经过多次“随机选点--模型估计--模型测试”迭代后，数据适应性最高的那组模型为最优模型，最优模型可认为是该数据集的最终表示。

以上说明仅为本发明的应用实施例而已，当然不能以此来限定本发明之权利范围，因此依本发明申请专利范围所作的等效变化，仍属本发明的保护范围。

Claims (5)

1.一种基于直角墙面的激光标定方法，其特征在于包括求激光雷达的安装误差角、激光雷达中心到小车中心的距离和激光雷达中心在小车坐标系中的偏移角；

求激光雷达的安装误差角：

S1、将小车移动至直角墙面的墙角处，使小车正面和侧面分别与直角墙面的两面墙面平行；

S2、激光雷达采集小车正面墙面的激光点云数据，并做直线拟合将所述激光点云数据拟合成一条直线，求出该直线的斜率角，所述斜率角即为激光雷达的安装误差角；

求激光雷达中心到小车中心的距离和激光雷达中心在小车坐标系中的偏移角：

A1、控制小车原地旋转，记录旋转过程中激光雷达在墙面坐标系下的坐标值；

A2、将激光雷达在墙面坐标系下的运动轨迹点进行圆拟合，并求出半径，所述半径即为激光雷达中心到小车中心的距离；

A3、当激光雷达到小车正面墙面的距离最小时，记录小车的旋转角，所述旋转角即为激光雷达中心在小车坐标系中的偏移角。

2.根据权利要求1所述的一种基于直角墙面的激光标定方法，其特征在于，所述步骤S2包括：

S201、激光雷达采集小车正面墙面的激光点云数据，对该激光点云数据先进行RANSAC估计滤噪，再进行最小二乘直线拟合，求出拟合直线的斜率角；

S202、重复步骤S201，至少得到三组斜率角，对所有斜率角取平均斜率角值，所述平均斜率角值即为激光雷达的安装误差角。

3.根据权利要求1所述的一种基于直角墙面的激光标定方法，其特征在于，所述步骤A1中控制小车原地旋转，记录旋转过程中激光雷达在墙面坐标系下的坐标值：

(1)将直角墙面构建成墙面坐标系o-xy，墙角为坐标系原点，激光雷达扫描平面与墙面坐标系重合；

(2)小车原地旋转过程中，记录激光雷达旋转轨迹在墙面坐标系o-xy上的表示，建立激光雷达旋转模型1η_(ω)x和模型2η_(ω)y，η_(ω)x表示垂直于墙面1的激光束的方位角，η_(ω)y表示垂直墙面2的激光束的方位角；

η(ω)_x＝(δ-ω)·180/π+90 η,δ,ω∈(-π，π)

η(ω)_y＝(δ-ω)·180/π+180 η,δ,ω∈(-π，π)

模型1和模型2中，ω为车体累计旋转角，取车头方向为正，用弧度值表示；δ激光安装误差角，取车头方向为正，用弧度值表示；η表示垂直于墙面激光束的方位角，取激光雷达0°角方向为正，用角度值表示；

(3)获取每一帧点云集合，集合包括集合内所有点云到激光雷达的距离ρ，建立模型3；

模型3：i＝θ·n/360

ρi＝N[i]

模型3中，N为一帧点云集合，n为N内点云的数量，θ为点云的方位角，i为点云在N中的索引值，ρi为点云i到激光雷达的距离；

(4)将模型1和模型2分别带入模型3，求出垂直于墙面1的目标点云到激光雷达的距离ρx和垂直于墙面2的目标点云到激光雷达的距离ρy，在墙面坐标系o-xy下ρx表示激光雷达运行轨迹点的x值，ρy表示激光雷达运行轨迹点的y值，记录每一帧的(ρx,ρy)，将激光雷达运动轨迹构建一个运动轨迹集合∑ρxy，则∑ρxy反映了激光雷达在墙面坐标系o-xy下的运行轨迹；

其中，ρjx、ρjy为第j帧激光点云到墙壁1、2的垂直距离。

4.根据权利要求1或3所述的一种基于直角墙面的激光标定方法，其特征在于，所述步骤A2中激光雷达在墙面坐标系下的运动轨迹点采用RANSAC估计进行圆拟合：

(1)将激光雷达运动轨迹上所有点的坐标值构成一个运动轨迹集合，在集合中随机选择三个点构建模型；

(2)设容错值e为0.02m，计算集合中剩余点到圆心的距离R'，e'＝|R-R'|，当e'<e时，表示该点为局内点，累计局内点个数；

(3)迭代重复步骤(1)和(2)，当当前模型局内点数比上一次的更多时，更新当前模型为最优模型，并更新最优模型的局内点集合；

(4)选择最优模型的局内点重新进行RANSAC估计，并设容错值e为0.01m；

(5)最后根据重新估计的模型计算得到半径R。

5.根据权利要求3所述的一种基于直角墙面的激光标定方法，其特征在于，所述步骤A3包括：遍历所述集合，寻找最小Y轴值，并记录最小Y轴值时小车的旋转角，即为激光雷达中心在小车坐标系中的偏移角。

Images