CN108932736B - 二维激光雷达点云数据处理方法以及动态机器人位姿校准方法 - Google Patents

Abstract

本发明公开了一种二维激光雷达点云数据处理方法，通过激光雷达扫描采集环境数据，进行点云数据预处理，将以激光雷达为坐标点的极坐标系转换为机器人位姿校准时使用的直角坐标系，然后采用区域分割、特征提取、点云分割、二维激光雷达和摄像头融合检测算法实现对环境信息的有效检测识别，利用摄像头的孔针模型与激光雷达进行三维坐标融合，使机器人根据偏差校准自身位姿。本发明同时公开了一种动态机器人位姿校准的方法，采用摄像头作为辅助检测传感器，融合激光雷达获取机器人实时位姿状态；与期望位姿状态进行比较，获得校准系统的调节量，并不断进行校准，直到激光雷达检测到的机器人位姿与期望位姿的偏差量小于某一阈值，则完成整个闭环位姿校准过程。

Description

二维激光雷达点云数据处理方法以及动态机器人位姿校准 方法

技术领域

本发明涉及一种二维激光雷达点云数据处理方法/算法，及其应用于动态机器人位姿校准系统的设计。

背景技术

就目前的发展状况来看，智能机器人技术还面临着种种难题，为此出现了很多机器人赛事，其考查的重点之一就是智能机器人环境建模问题，环境建模是智能机器人实现环境信息交流的唯一渠道，因此技术突破势在必行。近年来，我国在交通、电力传输、建筑等行业应用的驱动下，激光雷达技术发展迅猛。激光雷达采集到的丰富的点云数据，信息量庞大，常用数据存储格式不统一，存储数据时存在种种不足，数据精度很难保证，因此，激光雷达点云数据的后处理一直是一个重要课题。

为了克服二维激光雷达信息量庞大，常用数据存储格式不统一，存储数据时存在种种不足，数据精度很难保证的问题，研究二维激光雷达作业流程，优化设计作业方案，是非常有意义的。

目前，二维激光雷达利用测距原理，能够准确感知机器人所处环境信息，在智能机器人导航、定位、避障、路径规划等研究领域受到了极大关注。研究点云数据处理的算法，能够更高效、实用地应用于机器人对环境信息的感知，为完成路径规划、地图构建、机器定位等功能提供技术支持。

发明内容

本发明针对现有技术不足，提出了一种二维激光雷达点云数据的有效处理方法，并采用二维激光雷达与摄像头融合的方法，实现二维激光雷达动态机器人位姿校准。

本发明所采用的技术方案：

一种二维激光雷达点云数据处理方法，对通过激光雷达扫描采集的环境原始数据首先进行点云数据预处理，将以激光雷达为坐标点的极坐标系转换为机器人位姿校准时使用的直角坐标系，然后采用区域分割算法、特征提取算法、点云分割算法、二维激光雷达和摄像头融合检测算法实现对环境信息的有效检测识别，所述的区域分割算法的使用判定方法为：如果连续的两个扫描点距离小于某一阈值，则将这两个扫描点归于同一个区块；对比连续的两个扫描点，如果它们的距离大于某一阈值，连续的数据就从这个扫描点分割开；最后，把一帧完整的点云数据分割成为几个区块；所述的特征提取算法包括Split-and-Merge的直线特征提取算法和霍夫变换的圆弧特征提取算法，利用霍夫变换获取圆心坐标；所述的二维激光雷达和摄像头融合检测算法，利用摄像头的孔针模型与激光雷达进行三维坐标融合；根据此模型获取转换矩阵，再将参数矩阵在程序中与激光雷达圆心数据进行融合，使激光雷达得到的圆心坐标在摄像头图像坐标系中找到对应的像素点；摄像头根据此中心坐标获取的静态偏差，使机器人根据偏差校准自身位姿。

一种利用所述二维激光雷达点云数据处理方法实现动态机器人位姿校准的方法，首先将激光雷达与摄像头与机器人固连，且摄像头与雷达相对位置不变；其次，通过以下步骤进行动态机器人位姿校准：1)利用激光雷达扫描周围环境，获取DSM并使用算法处理获取机器人实时位姿状态；2)摄像头图像融合，即摄像头作为辅助检测传感器，融合于激光雷达以获取更精确的机器人实时位姿状态；3)将检测获取的机器人实时位姿状态数据，与期望位姿状态进行比较，获得校准系统的调节量；4)利用机器人运动或发射机构，执行位姿校准，得到一个新的机器人位姿；5)激光雷达实时检测当前位姿状态，反馈至执行机构再一次进行校准；6)如此循环，令机器人不断进行校准，直到激光雷达检测到的机器人位姿与期望位姿的偏差量小于某一阈值，则完成整个闭环位姿校准过程，最终达到期望位姿。

本发明的有益效果：

1、本发明二维激光雷达点云数据处理方法/算法，结合二维激光雷达，研究了各种算法对点云数据进行去噪滤波、区域分割、特征提取、点云分割处理，解决了原始数据复杂和精度低的问题。

2、本发明开发设计了一个基于二维激光雷达的机器人位姿校准系统，封装、集成了点云数据处理模块、算法，能够实时检测机器人位姿，并及时反馈修正，效率高，能够更高效、实用地应用于机器人对环境信息的感知；并且保证在有雾，有霾等天气机器人能够获取位姿信息。

3、本发明二维激光雷达点云数据处理方法/算法，直线特征提取采用了动态阈值的解决方案，减小了下述情况的出现的概率：阈值d_T影响特征提取效果，阈值d_T过大会导致特征点提取遗漏。阈值d_T过小会导致分割过密，二者都会影响直线拟合的效果，同时也加大了运算量。

4、本发明二维激光雷达点云数据处理方法/算法，包括Split-and-Merge的直线特征提取算法和霍夫变换的圆弧特征提取算法，对直线和圆弧检测精度很高，利用霍夫变换成功的检测出了柱子并获取圆心坐标，并且其他非圆弧信息并未被误判，算法适应性较强。

附图说明

图1为本发明二维激光雷达点云数据处理方法/算法流程图；

图2为特征提取算法中Split-and-Merg直线特征提取步骤；

图3为激光雷达和摄像头坐标融合建模的针孔模型的坐标示意图；

图4为本发明动态机器人位姿校准系统设计硬件平台总体框图；

图5为本发明动态机器人位姿校准方法流程图。

具体实施方式

下面通过具体实施方式，对本发明的技术方案做进一步的详细描述。

实施例1

参见图1，本发明二维激光雷达点云数据处理方法，对通过激光雷达扫描采集的环境原始数据首先进行点云数据预处理，将以激光雷达为坐标点的极坐标系转换为机器人位姿校准时使用的直角坐标系，然后采用区域分割算法、特征提取算法、点云分割算法、二维激光雷达和摄像头融合检测算法实现对环境信息的有效检测识别，其过程包括：

所述的区域分割算法的使用判定方法为：如果连续的两个扫描点距离小于某一阈值，则将这两个扫描点归于同一个区块；对比连续的两个扫描点，如果它们的距离大于某一阈值，连续的数据就从这个扫描点分割开；最后，把一帧完整的点云数据分割成为几个区块；一般情况下，扫描点的分布呈非均匀分布，距离激光雷达近的扫描点更加密集，而距离激光雷达远的扫描点密度更小；

所述的特征提取算法包括Split-and-Merge的直线特征提取算法和霍夫变换的圆弧特征提取算法，利用霍夫变换获取圆心坐标；并且其他非圆弧信息未被误判，算法适应性较强；

所述的二维激光雷达和摄像头融合检测算法，利用摄像头的孔针模型与激光雷达进行三维坐标融合；根据此模型获取转换矩阵，再将参数矩阵在程序中与激光雷达圆心数据进行融合，使激光雷达得到的圆心坐标在摄像头图像坐标系中找到对应的像素点；摄像头根据此中心坐标获取的静态偏差，使机器人根据偏差校准自身位姿；摄像头根据此中心坐标获取对方飞盘在着陆台上的静态偏差，使机器人根据偏差校准自身位姿，以瞄准对方飞盘进行击盘任务。

实施例2

参见图1、图2，本实施例的二维激光雷达点云数据处理方法，与实施例1的不同之处在于：所述点云数据预处理算法，包括滤波算法、排除异常值和坐标变换；滤波算法、排除异常值能有效地消除环境中的干扰信息；坐标变换将以激光雷达为坐标点的极坐标系转换为机器人位姿校准时使用的直角坐标系；

1)采用的滤波算法如下：

由于环境的干扰，激光雷达返回的扫描数据中包括噪声。使用合适的滤波算法能有效地消除噪音。选定窗口宽度为7的中值滤波器，能将离散的点云数据中的噪声有效滤除，并且保留有用的原始数据边缘信息，从而使其周围的数据都能更加靠近真实值，以便于下一步算法处理，计算公式如下：

x_mid＝med{x_i-s,...x_i-1,x_i,x_i+1,...,x_i+s}

其中s代表窗口大小，med{}代表取中值操作函数，x_i代表激光雷达探测的单个数据，x_mid代表中值滤波之后的数据。

2)坐标变换：

由于激光雷达采集得到的点云数据是以激光雷达为原点的极坐标点，而机器人位姿校准使用的是直角坐标系，通过以下关系式计算得到目标坐标：

X＝ρ cosΘ

Y＝ρ sinΘ

式中，ρ表示激光雷达扫描目标物体所测量到的目标点的距离信息，Θ表示其角度信息；(X，Y)表示目标物体在以机器人为中心的直角坐标系中的坐标，单位mm。

实施例3

参见图1，本实施例的二维激光雷达点云数据处理方法，与实施例2的不同之处在于：所述区域分割算法，设当某一扫描点A距离激光雷达中心点O为D时，设定分割距离阈值为d，则当扫描点距离激光雷达中心为3D时，阈值调整为3d；根据实际应用情况也可以选用非线性函数来定义自适应分割阈值；采用的区域分割算法如下：

a、计算点集中连续两点间的距离

b、判断Dj和阈值θ的关系，

如果Dj,大于阈值θ,则认为点(x,y)是两个区域的分割点，阈值的选择一般按照动态阈值的方式。按照上述步骤一步步进行后，最终可以分割出N个相互间没有连接的区域{Al，A2...AN}；

c、判断每个区域内数据点的个数,如果某个区域包含数据点的个数小于等于N个,那么该区域被视为噪声区域,舍弃这些噪声点，不进行处理；

激光雷达有效测距距离为A，角度分辨率为B，则相邻扫描点之间的距离最小为C：

C＝2×A×sinB

根据该值设定合适的分隔阈值；

利用OpenCV对不同的区域用不同的颜色进行显示分割(同一种颜色并不代表在同一区域，只是颜色有限，几种颜色在循环使用)，不同特征的物体，被分别分割出来，为后续的提取做好铺垫。

实施例4

参见图1、图2，本实施例的二维激光雷达点云数据处理方法，与前述各实施例的不同之处在于：所述特征提取算法包括直线提取算法和圆弧特征提取算法：

1)直线提取算法

直线特征提取中采取Split-and-Merge算法，设区域ai的第一个点和最后一个点分别为ks(Xs,Ys)和ke(Xe,Ye)；两点之间的连线为L，连线长度为da；区域ali内点Mk到L的距离记dk，设点到L距离的阈值为dT,如果dk>dT，则以点pk为分割点将区域ali分成两部分；否则，不进行分割，并采用最小二乘法对ali中的激光数据点进行直线拟合，求取直线参数(k,b)；重复上述步骤，直到这个区域内的所有直线特征提取完成。

阈值d_T影响特征提取效果，阈值d_T过大会导致特征点提取遗漏。阈值d_T过小会导致分割过密，二者都会影响直线拟合的效果，同时也加大了运算量。本发明采用了动态阈值的解决方案，将会减小上述情况的出现的概率。

实线表示区域点集，选取L＝d_T；根据提取特征的首末点长度，判断是否拟合区域内数据点是否可以拟合为一条线段；区域内的所有数据点都均匀分布于其周围，线段拟合结果较好；而在区域内选取L＝d_T，相对于同一条拟合直线区域内点的分布太分散导致拟合线段的结果较差。综上所述，如果根据激光雷达数据点集区域端点之间连线长度d_L的大小来选取动态的阈值d_T，在一定程度能提高算法线段拟合的效果；

2)圆弧特征提取算法

完成区域分割后，将激光雷达扫描的数据分为几个块区，每个块区主要包含撕裂点(BreakPoint)、角点(Corner)、直线、圆弧几个重要的特征；

Hough变换，采用一种类似于投票的机制，某一参数获得的票数越多，则存在圆弧的概率越大。利用前文已将点云图进行区域分割，分成的多个点集，分别进行Hough变换，可减少处理数据，方便地计算圆弧的位置与半径。

设点云图中有一点(X，Y)，存在无数个圆会经过该点，每个圆对应一组(a，b，R)参数；设立一个票箱，将所有可能的(a，b，R)参数放入其中，当某组参数出现一次，票箱中的票数+1；将所有点都扫描后，查看票箱数目，票数最多的点即为圆出现概率最大的点。设定一个阈值，若最多的票数小于该阈值，则认为此区域不存在圆，否则认为圆存在。该方法准确，实用性强。

实施例5

参见图1、图2，本实施例的二维激光雷达点云数据处理方法，与实施例4的不同之处在于：所述的点云分割算法包括聚类分割算法和随机抽样一致性算法：

1)聚类分割算法：

聚类分割的基本原理为：假设存在m个数据点，在m维空间内，设定点与点之间的一个特征距离；假如m个数据点分为n类(n≤m)，同时将距离最小的两类合并；然后再计算类与类之间的距离；迭代上述过程，最后完成分割的时候，需要满足任意两类间的距离都大于设定的阈值，或者类的个数小于指定的数目；

聚类的分割方法一般为两步：首先根据点云数据特征识别聚类模式，然后将具有相同模式的点云分割在一起；在聚类方法中每个点都与一个特征向量相关联，特征向量又包含了若干个几何或者辐射度量值；然后，在特征空间中通过聚类的方法(如K-mean法、最大似然方法和模糊聚类法)分割点云数据。

2)随机抽样一致性算法：

随机抽样一致性算法的基本思想：针对具体的问题进行参数估计的时候，首先需要设定一个判断准则，再利用判断准则不断删除与估计的参数不一致的数据，迭代使用判断准则，然后通过剔除之后的有效输入数据来估计准确的参数，它规定在满足一定的置信概率时，基本子集的最小抽样数M与取得至少一个正确子集的概率P(P>ε)满足如下关系:

P＝(1-(1-ε)^m)^M

其中，ε为错误的数据概率，m为参数估计的最小数据量；

RANSAC方法在点云分割中主要是应用在区域生长的方法中，嵌入这种方法减小噪声影响，节省时间。对于给定数目为m的点集Q，随机选取能够构造种子面最低参数要求的n(n≤m)个数据点，建立种子面参数模型S，以平面模型为例：

(1)计算最小抽样数M；

(2)利用求概率P公式从数据集合中选择m个点(m＝3)，计算模型ax+by+cz+d＝0的参数；

(3)根据点到平面模型的距离d＝|ax+by+cz+d|，设定一个阈值δ＝|d_t|；

(4)计算所有点到平面模型的距离，如果d满足0≤|d|≤|d_t|，将点归为有效点，否则归为无效点；

(5)重复(2)到(4)的步骤M次，统计每次分类后有效点的数目，选择有效点最多时对应的平面参数为最终的平面分割结果。

实施例6

参见图1、图2、图3，本实施例的二维激光雷达点云数据处理方法，与前述各实施例的不同之处在于：所述的二维激光雷达与摄像头融合算法，过程如下：

以激光雷达的坐标作为世界坐标，利用摄像头的孔针模型将物体的世界坐标转换到摄像头的图像坐标；首先将摄像头与激光雷达固定在一个支架上，通过移动目标物体或是支架，读取图像上的着陆台中心坐标和激光雷达测得的柱子圆心坐标，然后通过解超定方程组求出M矩阵；保持之前摄像头和激光雷达的相对位置关系，将激光雷达圆心坐标与M矩阵进行运算即可得到当前柱子中心坐标在摄像头图像上的像素坐标；

采用针孔模型方法,实现将激光雷达所在的三维空间坐标系中的物体投影到成像平面的转换；建立激光雷达与摄像头坐标系，设一目标物体为点P(Xl,Yl,Zl)，其在摄像头视图坐标系下，坐标表示为P(Xc,Yc,Zc)；在摄像头成像坐标系中，用坐标P(X,Y)表示，设摄像头成像系统焦距为f；整个成像过程就是物体三维空间坐标在以上坐标系之间的转换过程；

物体点P从激光雷达坐标系到摄像头像素坐标系的整体变换公式如下：

其中，K为摄像机内参数矩阵，[R t]为摄像机外参数矩阵，M为投影矩阵；

利用摄像头的理想变换模型，可以简化得到如下转换关系式：

即求得M矩阵后，激光雷达采集到的目标物体坐标经上式便可得到其在摄像头上的对应像素坐标，完成两传感器坐标融合，实现激光雷达为摄像头提供检测ROI区域与图像中心坐标。

实施例7

参见图4，图5。本实施例为利用前述二维激光雷达点云数据处理方法实现动态机器人位姿校准的方法的具体实施方式，实现动态机器人位姿校准。

如图4所示，一块稳压电源板输出稳定电压24V、12V、5V，分别为底盘电机、miniPC和激光雷达、主控板和摄像头提供驱动电源。摄像头与激光雷达分别使用USB和以太网口与miniPC进行数据通信。miniPC负责完成激光雷达单独检测机器人位姿程序运行，同时也负责摄像头检测飞盘并与激光雷达融合检测输出击打飞盘机器人所需位姿状态。miniPC通过有线串口与主控进行数据通信，由此主控可获得激光雷达与融合检测的机器人位姿调节量。

主控根据位姿调节量，校准机器人位姿。设计步骤如下：

首先将激光雷达与摄像头与机器人固连，且摄像头与雷达相对位置不变；其次，通过以下步骤进行动态机器人位姿校准：

1)利用激光雷达扫描周围环境，获取DSM(数字表面模型)并使用算法处理获取机器人实时位姿状态；

2)摄像头图像融合，即摄像头作为辅助检测传感器，融合于激光雷达以获取更精确的机器人实时位姿状态；

3)将检测获取的机器人实时位姿状态数据，与期望位姿状态进行比较，获得校准系统的调节量；

4)利用机器人运动或发射机构，执行位姿校准，得到一个新的机器人位姿；

5)激光雷达实时检测当前位姿状态，反馈至执行机构再一次进行校准；

6)如此循环，令机器人不断进行校准，直到激光雷达检测到的机器人位姿与期望位姿的偏差量小于某一阈值，则完成整个闭环位姿校准过程，最终达到期望位姿。

实施例8

参见图4、图5，本实施例的动态机器人位姿校准的方法，与实施例7不同的是：

步骤1)包括两个步骤：

a、二维激光雷达点云数据采集，即雷达采集原始点云数据发送到电脑/终端；

b、点云数据处理，即：基于步骤a雷达采集的DSM(数字表面模型)，进行去噪滤波、区域分割、点云分割、特征提取。得到初步机器人实时位姿信息；

所述步骤2)包括两个步骤：

c、数据采集；

d、坐标转换，即以激光雷达的坐标作为世界坐标，利用摄像头的孔针模型将物体的世界坐标转换到摄像头的图像坐标，并解超定方程组求出M矩阵。

为了保证步骤c数据的准确，摄像头的分辨率要在960p以上。

为了保证步骤d数据的准确，理论上6个标定点就可以求出M矩阵各元素值。实际求取过程中，由于标定存在一定的误差，使用6个以上标定点进行标定，通过解超定方程组降低误差。

Claims (8)

1.一种二维激光雷达点云数据处理方法，对通过激光雷达扫描采集的环境原始数据首先进行点云数据预处理，将以激光雷达为坐标点的极坐标系转换为机器人位姿校准时使用的直角坐标系，然后采用区域分割算法、特征提取算法、点云分割算法、二维激光雷达和摄像头融合检测算法实现对环境信息的有效检测识别，其特征在于：

所述的区域分割算法的使用判定方法为：如果连续的两个扫描点距离小于某一阈值，则将这两个扫描点归于同一个区块；对比连续的两个扫描点，如果它们的距离大于某一阈值，连续的数据就从这个扫描点分割开；最后，把一帧完整的点云数据分割成为几个区块；

所述的特征提取算法包括Split-and-Merge的直线特征提取算法和霍夫变换的圆弧特征提取算法，利用霍夫变换获取圆心坐标；

所述的二维激光雷达和摄像头融合检测算法，利用摄像头的孔针模型与激光雷达进行三维坐标融合；根据此模型获取转换矩阵，再将参数矩阵在程序中与激光雷达圆心数据进行融合，使激光雷达得到的圆心坐标在摄像头图像坐标系中找到对应的像素点；摄像头根据此圆心坐标获取的静态偏差，使机器人根据偏差校准自身位姿。

2.根据权利要求1所述的二维激光雷达点云数据处理方法，其特征在于：所述点云数据预处理算法，包括滤波算法、排除异常值和坐标变换；滤波算法、排除异常值能有效地消除环境中的干扰信息；坐标变换将以激光雷达为坐标点的极坐标系转换为机器人位姿校准时使用的直角坐标系；

1)采用的滤波算法如下：

选定窗口宽度为7的中值滤波器，能将离散的点云数据中的噪声有效滤除，并且保留有用的原始数据边缘信息，从而使其周围的数据都能更加靠近真实值，以便于下一步算法处理，计算公式如下：

x_mid＝med{x_i-s,...x_i-1,x_i,x_i+1,...,x_i+s}

其中s代表窗口大小，med{}代表取中值操作函数，x_i代表激光雷达探测的单个数据，x_mid代表中值滤波之后的数据；

2)坐标变换：

由于激光雷达采集得到的点云数据是以激光雷达为原点的极坐标点，而机器人位姿校准使用的是直角坐标系，通过以下关系式计算得到目标坐标：

X＝ρ cosΘ

Y＝ρ sinΘ

式中，ρ表示激光雷达扫描目标物体所测量到的目标点的距离信息，Θ表示其角度信息；(X，Y)表示目标物体在以机器人为中心的直角坐标系中的坐标，单位mm。

3.根据权利要求2所述的二维激光雷达点云数据处理方法，其特征在于：设当某一扫描点距离激光雷达中心点O为D时，设定分割距离阈值为d，则当扫描点距离激光雷达中心为3D时，阈值调整为3d；根据实际应用情况也可以选用非线性函数来定义自适应分割阈值；采用的区域分割算法如下：

a、计算点集中连续两点间的距离

b、判断Dj和阈值θ的关系，

如果Dj,大于阈值θ,则认为点(x,y)是两个区域的分割点，阈值的选择一般按照动态阈值的方式；

按照上述步骤一步步进行后，最终可以分割出N个相互间没有连接的区域{Al，A2...AN}；

c、判断每个区域内数据点的个数,如果某个区域包含数据点的个数小于等于N个,那么该区域被视为噪声区域,舍弃这些噪声点，不进行处理；

激光雷达有效测距距离为A，角度分辨率为B，则相邻扫描点之间的距离最小为C：

C＝2×A×sinB

根据该值设定合适的分隔阈值；

利用OpenCV对不同的区域用不同的颜色进行显示分割，不同特征的物体，被分别分割出来，为后续的提取做好铺垫。

4.根据权利要求1、2或3所述的二维激光雷达点云数据处理方法，其特征在于：所述特征提取算法包括直线提取算法和圆弧特征提取算法：

1)直线提取算法

直线特征提取中采取Split-and-Merge算法，设区域ali的第一个点和最后一个点分别为ks(Xs,Ys)和ke(Xe,Ye)；两点之间的连线为L，连线长度为da；区域ali内点Mk到L的距离记dk，设点到L距离的阈值为dT,如果dk>dT，则以点pk为分割点将区域ali分成两部分；否则，不进行分割，并采用最小二乘法对ali中的激光数据点进行直线拟合，求取直线参数；

重复上述步骤，直到这个区域内的所有直线特征提取完成；

实线表示区域点集，选取L＝dT；根据提取特征的首末点长度，判断拟合区域内数据点是否可以拟合为一条线段；区域内的所有数据点都均匀分布于其周围，线段拟合结果较好；而在区域内选取L＝dT，相对于同一条拟合直线区域内点的分布太分散导致拟合线段的结果较差；根据激光雷达数据点集区域端点之间连线长度da的大小来选取动态的阈值dT，在一定程度能提高算法线段拟合的效果；

2)圆弧特征提取算法

完成区域分割后，将激光雷达扫描的数据分为几个块区，每个块区包含撕裂点、角点、直线、圆弧几个重要的特征；

设点云图中有一点，存在无数个圆会经过该点，每个圆对应一组(a，b，R)参数；设立一个票箱，将所有可能的(a，b，R)参数放入其中，当某组参数出现一次，票箱中的票数+1；将所有点都扫描后，查看票箱数目，票数最多的点即为圆出现概率最大的点；设定一个阈值，若最多的票数小于该阈值，则认为此区域不存在圆，否则认为圆存在。

5.根据权利要求1所述的二维激光雷达点云数据处理方法，其特征在于：所述的点云分割算法包括聚类分割算法和随机抽样一致性算法：

1)聚类分割算法：

假设存在m个数据点，在m维空间内，设定点与点之间的一个特征距离；假如m个数据点分为n类，所述n≤m，同时将距离最小的两类合并；然后再计算类与类之间的距离；迭代上述过程，最后完成分割的时候，需要满足任意两类间的距离都大于设定的阈值，或者类的个数小于指定的数目；

聚类的分割方法一般为两步：首先根据点云数据特征识别聚类模式，然后将具有相同模式的点云分割在一起；在聚类方法中每个点都与一个特征向量相关联，特征向量又包含了若干个几何或者辐射度量值；然后，在特征空间中通过聚类的方法分割点云数据；

2)随机抽样一致性算法：

针对具体的问题进行参数估计的时候，首先需要设定一个判断准则，再利用判断准则不断删除与估计的参数不一致的数据，迭代使用判断准则，然后通过剔除之后的有效输入数据来估计准确的参数，它规定在满足一定的置信概率时，基本子集的最小抽样数M与取得至少一个正确子集的概率P满足如下关系，P>ε：

P＝(1-(1-ε)^m)^M

其中，ε为错误的数据概率，m为参数估计的最小数据点；

对于给定数目为m的点集Q，随机选取能够构造种子面最低参数要求的n个数据点，n≤m，建立种子面参数模型S，以平面模型为例：

(1)计算最小抽样数M；

(2)利用求概率P公式从数据集合中选择m个点，m＝3，计算模型Ax+By+Cz+d＝0的参数；

(3)根据点到平面模型的距离d＝|Ax+By+Cz+d|，设定一个阈值δ＝|d_t|；

(4)计算所有点到平面模型的距离，如果d满足0≤|d|≤|d_t|，将点归为有效点，否则归为无效点；

(5)重复(2)到(4)的步骤M次，统计每次分类后有效点的数目，选择有效点最多时对应的平面参数为最终的平面分割结果。

6.一种利用权利要求1所述二维激光雷达点云数据处理方法实现动态机器人位姿校准的方法，首先将激光雷达与摄像头与机器人固连，且摄像头与雷达相对位置不变；其次，通过以下步骤进行动态机器人位姿校准：

1)利用激光雷达扫描周围环境，获取DSM并使用算法处理获取机器人实时位姿状态；

2)摄像头图像融合，即摄像头作为辅助检测传感器，融合于激光雷达以获取更精确的机器人实时位姿状态；

3)将检测获取的机器人实时位姿状态数据，与期望位姿状态进行比较，获得校准系统的调节量；

4)利用机器人运动或发射机构，执行位姿校准，得到一个新的机器人位姿；

5)激光雷达实时检测当前位姿状态，反馈至执行机构再一次进行校准；

6)如此循环，令机器人不断进行校准，直到激光雷达检测到的机器人位姿与期望位姿的偏差量小于某一阈值，则完成整个闭环位姿校准过程，最终达到期望位姿。

7.根据权利要求6所述的动态机器人位姿校准的方法，其特征在于：步骤1)包括：

a、二维激光雷达点云数据采集，即雷达采集原始点云数据发送到电脑/终端；

b、点云数据处理，即：基于步骤a雷达采集的DSM，进行去噪滤波、区域分割、点云分割、特征提取；得到初步机器人实时位姿信息。

8.根据权利要求6或7所述的动态机器人位姿校准的方法，其特征在于：步骤2)包括：

c、数据采集；

d、坐标转换，即以激光雷达的坐标作为世界坐标，利用摄像头的孔针模型将物体的世界坐标转换到摄像头的图像坐标，并解超定方程组求出投影矩阵。

Images