一种基于激光雷达数据的移动机器人位姿推算方法及系统

摘要

本发明公开了一种基于激光雷达数据的移动机器人位姿推算方法，首先接收与预处理激光雷达数据；分割聚类环境数据；然后从激光雷达数据中选择簇群；并对目标簇群进行跟踪；获取簇群中每个子簇群的顶点在局部坐标系下的坐标数据，获取簇群的顶点的坐标和每个连接连线的倾角；将起始点和终点组成连接直线并计算偏转角度；通过滑动滤波计算机器人位置坐标变化后的位置坐标；最后初始化起始点的位姿和采样位姿点。本发明提供的移动机器人位姿推算方法是使用精度较高的二维激光雷达，无需对环境进行任何修改；机器人的位姿信息相对于具体环境客观描述，更便捷将位姿信息集成到其它移动机器人的业务模块中，提高系统的可靠性、易实现性并降低成本。

说明书

技术领域

本发明涉及移动机器人及智能车辆的局部导航领域，特别涉及一种基于激光雷达数据的移动机器人位姿推算方法。

背景技术

机器人作为一种新型的生产工具，表现出极大的优越性。在减轻劳动强度，提高生产效率，改变生产模式等方面可将人们从危险、恶劣、繁重的工作环境中解放出来。然而随着机器人的不断发展，人们逐渐发现这些固定于某一位置操作的机器人对很多应用需求均不能满足。因此在20世纪80年代后期，许多国家开始有计划地开展了移动机器人技术的研究。

对于移动机器人而言，导航是一个重要的方向。而移动机器人的导航的前提就是对机器人的定位和姿态识别。精确的位姿推算对于移动机器人的定位、自动地图生成、路径规划和控制、目标检测和跟踪等具有重要意义。在二维的平面坐标系下，移动机器人的位姿就是机器人在全局坐标系中的位置以及机器人的车体方向，即表示为：(x,y,θ)。机器人的位置可以用机器人质心坐标点表示为，而用车体前方偏向全局坐标系X轴的夹角来表示车体的方向。在本文中，定义方向角为：以X轴0度，顺时针方向为负，逆时针方向为正；如图1所示，图1机器人的位姿模型；图中X_GO_GY_G为机器所在的全局坐标系；X_RO_RY_R为机器人所在的局部坐标系；(x,y)为机器人质心的坐标；θ为机器人相对于起始点的位姿偏转角。应用在移动机器人的位姿推算方法一般可分为两种：航位推算(Dead-Reckoning)是一种非常经典的相对位姿推算方法，通过安装在移动机器人轮子上的光电编码器输出的轮速与移动机器人的运动学模型，累积每个采样周期下的移动机器人位移以及角度偏转。航位推算的优点是方法简单、价格低廉，但从其本质上上可以看出，航位推算方法的精度容易受轮速的标定、车轮与地面的打滑、颠簸等因素造成积分误差而大大降低，因此误差较大。惯性导航系统INS(InertialNavigation>

无论是航位推算还是惯性导航系统，都存在一个共同的缺点：存在积分误差，移动机器人的行程越远，该积分误差越大。因此现有的位姿推算方法并不适用于对机器人位姿信息要求较高的场合。如何实现更加精确的且不受地面情况限制的位姿推算方法，是一个很重要的问题。

因此，需要一种基于激光雷达数据的移动机器人位姿描述方法。

发明内容

本发明的目的就是提供一种基于激光雷达数据的移动机器人位姿描述方法。

本发明的目的是通过这样的技术方案实现的：

本发明提供的一种基于激光雷达数据的移动机器人位姿推算方法，包括以下步骤：

Step1：接收与预处理激光雷达数据；

Step2：分割聚类环境数据；

Step3：从激光雷达数据中选择簇群；

Step4：目标簇群的跟踪；获取簇群中每个子簇群的顶点在局部坐标系下的坐标数据，获取簇群的顶点的坐标和每个连接连线的倾角；

Step5：将起始点和终点组成连接直线并计算簇群中连接直线的偏转角度；

Step6：通过滑动滤波计算机器人位置坐标变化后的位置坐标；

Step7：初始化起点的位姿并存储位置坐标。

进一步，所述步骤Step1中的接收与预处理激光雷达数据的具体步骤如下：

通过与计算机连接的二维扫描型激光雷达传感器获取环境数据；

将环境数据以数组形式存储于计算机中，所述环境数据包括距离信息、激光雷达扫描范围和角度分辨率；

预处理距离数据，包括去除有效范围外的数据点、滤除孤立的噪声点及对激光雷达测量机制的缺陷补偿；

将环境数据从激光雷达的极坐标系下的数据转化为机器人或智能车辆的局部直角坐标系坐标。

进一步，所述步骤Step2中的按照以下步骤来分割聚类环境数据：

循环比较距离数据的每两个相邻数据点差值，标记差值大于分割阈值的数据点为分割点；

存储分割点的角标；

进一步，所述步骤Step3中从激光雷达数据中选择簇群的具体步骤如下：

计算簇群中相对应起始点和终点连接直线；

计算簇群中数据点与连接直线的距离；

获取距离直线最远的数据点为顶点；

计算顶点坐标；

计算连接直线的倾角。

进一步，所述步骤Step5中的连接直线及偏转角度按照以下步骤来实现：

获取机器人相对于起始位置的A'位置坐标和角度信息φ；

对机器人的角度信息进行滑动滤波；

对簇群的角度信息进行滑动滤波；

得到B'位置机器人相对起始位置的角度信息；

通过对角度信息进行均值滤波得到机器人在B'位置的偏转角度：

${\theta }_{B^{\prime }}=\frac{\underset{i=1}{\overset{n}{\Sigma }}(\phi +{\theta }_{i_{(j+1)*\gamma }}-{\theta }_{i_{j*\gamma }})}{n};$

其中，θ_B′表示机器人在B'位置的偏转角度；n表示一帧环境数据中经过分割算法分割出来的簇群总数；i表示n个簇群中第i个子簇群；表示第i个子簇群在j*γ时刻的位姿偏转角度；表示第i个子簇群在(j+1)*γ时刻的位姿偏转角度；φ表示机器人在A'位置的偏转角度。

进一步，所述步骤Step6中机器人位置坐标变化后的位置坐标按照以下步骤来计算：

获取机器人相对应起始点的A'位置坐标(x,y)；

通过以下公式来计算机器人在B'位置的坐标：

$\left(\begin{array}{c}{x}_{B^{\prime }}=x+\left|\right|BC\left|\right|+\frac{L_{ROBOT}}{2}*COS\left({\theta }_{B^{\prime }}\right)\\ y_{B^{\prime }}=y+\left|\right|CF\left|\right|-\frac{L_{ROBOT}}{2}*COS\left({\theta }_{B^{\prime }}\right)\end{array}\right)$

其中，L_ROBOT为机器人长度，x_B′表示机器人在B'位置的横坐标；y_B′表示机器人在B'位置的纵坐标；||CF||表示机器人质心从A'位置到B'位置的纵坐标距离；||BC||表示机器人质心从A'位置到B'位置的横坐标距离。

本发明还提供了一种基于激光雷达数据的移动机器人位姿推算系统，包括预处理模块、分割聚类模块、簇群选择模块、簇群跟踪模块、偏转角度计算模块、位置坐标计算模块和存储模块；

所述预处理模块，用于接收与预处理激光雷达数据；

所述分割聚类模块，用于分割聚类环境数据；

所述簇群选择模块，用于从激光雷达数据中选择簇群；

所述簇群跟踪模块，用于获取簇群中每个子簇群的顶点在局部坐标系下的坐标数据，获取簇群的顶点的坐标和每个连接连线的倾角；

所述偏转角度计算模块，用于将起始点和终点组成连接直线并计算簇群中连接直线的偏转角度；

所述位置坐标计算模块，用于通过滑动滤波计算机器人位置坐标变化后的位置坐标；

所述存储模块，用于初始化起点的位姿并存储位置坐标。

进一步，所述预处理模块中的接收与预处理激光雷达数据的具体步骤如下：

通过与计算机连接的二维扫描型激光雷达传感器获取环境数据；

将环境数据以数组形式存储于计算机中，所述环境数据包括距离信息、激光雷达扫描范围和角度分辨率；

预处理距离数据，包括去除有效范围外的数据点、滤除孤立的噪声点及对激光雷达测量机制的缺陷补偿；

将环境数据从激光雷达的极坐标系下的数据转化为机器人或智能车辆的局部直角坐标系坐标。

进一步，所述簇群选择模块中从激光雷达数据中选择簇群的具体步骤如下：

计算簇群中相对应起始点和终点连接直线；

计算簇群中数据点与连接直线的距离；

获取距离直线最远的数据点为顶点；

计算顶点坐标；

计算连接直线的倾角。

所述偏转角度计算模块中的连接直线及偏转角度按照以下步骤来实现：

获取机器人相对于起始位置的A'位置坐标和角度信息φ；

对机器人的角度信息进行滑动滤波；

对簇群的角度信息进行滑动滤波；

得到B'位置机器人相对起始位置的角度信息；

通过对角度信息进行均值滤波得到机器人在B'位置的偏转角度：

${\theta }_{B^{\prime }}=\frac{\underset{i=1}{\overset{n}{\Sigma }}(\phi +{\theta }_{i_{(j+1)*\gamma }}-{\theta }_{i_{j*\gamma }})}{n};$

其中，θ_B′表示机器人在B'位置的偏转角度；n表示一帧环境数据中经过分割算法分割出来的簇群总数；i表示n个簇群中第i个子簇群；表示第i个子簇群在j*γ时刻的位姿偏转角度；表示第i个子簇群在(j+1)*γ时刻的位姿偏转角度；φ表示机器人在A'位置的偏转角度。

进一步，所述位置坐标计算模块中机器人位置坐标变化后的位置坐标按照以下步骤来计算：

获取机器人相对应起始点的A'位置坐标(x,y)；

通过以下公式来计算机器人在B'位置的坐标：

$\left(\begin{array}{c}{x}_{B^{\prime }}=x+\left|\right|BC\left|\right|+\frac{L_{ROBOT}}{2}*COS\left({\theta }_{B^{\prime }}\right)\\ y_{B^{\prime }}=y+\left|\right|CF\left|\right|-\frac{L_{ROBOT}}{2}*COS\left({\theta }_{B^{\prime }}\right)\end{array}\right)$

其中，L_ROBOT为机器人长度，x_B′表示机器人在B'位置的横坐标；y_B′表示机器人在B'位置的纵坐标；||CF||表示机器人质心从A'位置到B'位置的纵坐标距离；||BC||表示机器人质心从A'位置到B'位置的横坐标距离。

由于采用了上述技术方案，本发明具有如下的优点：

本发明提供的移动机器人位姿推算方法是基于移动机器人自身的信息得出的位姿信息，使用激光雷达信息来描述移动机器人的位姿；二维激光雷达具有较高的精度而无需对环境进行任何修改；机器人的位姿信息相对于具体环境客观描述，使用基于激光雷达的位姿估计可以更便捷的将位姿信息集成到其它移动机器人的业务模块中，例如避障路径规划，而省去任何标定过程，从而提高系统的可靠性、易实现性并降低成本。可广泛使用于对环境描述精度较高的二维激光雷达控制中。

本发明的其他优点、目标和特征在某种程度上将在随后的说明书中进行阐述，并且在某 种程度上，基于对下文的考察研究对本领域技术人员而言将是显而易见的，或者可以从本发明的实践中得到教导。本发明的目标和其他优点可以通过下面的说明书和权利要求书来实现和获得。

附图说明

本发明的附图说明如下。

图1为本发明的机器人的位姿模型示意图。

图2为本发明的ABD算法示意图。

图3a为本发明的顶点在连接直线一侧的簇群示意图。

图3b为本发明的顶点在连接直线另一侧的簇群示意图。

图4a为在第j*γ个时刻起点和终点连线示意图。

图4b为在第(j+1)*γ个时刻起点和终点连线示意图。

图4c为机器人在A'位置和B'位置变化示意图。

图5为本发明的激光雷达数据预处理流程图。

图6为本发明的ABD分割算法流程图。

图7为本发明的位姿推算方法流程图。

具体实施方式

下面结合附图和实施例对本发明作进一步说明。

实施例1

如图所示，本实施例提供的一种基于激光雷达数据的移动机器人位姿推算方法，包括以下步骤：

Step1：接收与预处理激光雷达数据；

Step2：分割聚类环境数据；

Step3：从激光雷达数据中选择簇群；

Step4：目标簇群的跟踪；获取簇群中每个子簇群的顶点在局部坐标系下的坐标数据，获取簇群的顶点的坐标和每个连接连线的倾角；

Step5：将起始点和终点组成连接直线并计算簇群中连接直线的偏转角度；

Step6：通过滑动滤波计算机器人位置坐标变化后的位置坐标；

Step7：初始化起点的位姿并存储位置坐标。

所述步骤Step1中的接收与预处理激光雷达数据的具体步骤如下：

通过与计算机连接的二维扫描型激光雷达传感器获取环境数据；

将环境数据以数组形式存储于计算机中，所述环境数据包括距离信息、激光雷达扫描范围和角度分辨率；

预处理距离数据，包括去除有效范围外的数据点、滤除孤立的噪声点及对激光雷达测量机制的缺陷补偿；

将环境数据从激光雷达的极坐标系下的数据转化为机器人或智能车辆的局部直角坐标系坐标。

所述步骤Step2中的按照以下步骤来分割聚类环境数据：

循环比较距离数据的每两个相邻数据点差值，标记差值大于分割阈值的数据点为分割点；

存储分割点的角标；

所述步骤Step3中从激光雷达数据中选择簇群的具体步骤如下：

计算簇群中相对应起始点和终点连接直线；

计算簇群中数据点与连接直线的距离；

获取距离直线最远的数据点为顶点；

计算顶点坐标；

计算连接直线的倾角。

所述步骤Step5中的连接直线及偏转角度按照以下步骤来实现：

获取机器人相对于起始位置的A'位置坐标和角度信息φ；

对机器人的角度信息进行滑动滤波；

对簇群的角度信息进行滑动滤波；

得到B'位置机器人相对起始位置的角度信息；

通过对角度信息进行均值滤波得到机器人在B'位置的偏转角度：

${\theta }_{B^{\prime }}=\frac{\underset{i=1}{\overset{n}{\Sigma }}(\phi +{\theta }_{i_{(j+1)*\gamma }}-{\theta }_{i_{j*\gamma }})}{n};$

其中，θ_B′表示机器人在B'位置的偏转角度；n表示一帧环境数据中经过分割算法分割 出来的簇群总数；i表示n个簇群中第i个子簇群；表示第i个子簇群在j*γ时刻的位姿偏转角度；表示第i个子簇群在(j+1)*γ时刻的位姿偏转角度；φ表示机器人在A'位置的偏转角度。

所述步骤Step6中机器人位置坐标变化后的位置坐标按照以下步骤来计算：

获取机器人相对应起始点的A'位置坐标(x,y)；

通过以下公式来计算机器人在B'位置的坐标：

$\left(\begin{array}{c}{x}_{B^{\prime }}=x+\left|\right|BC\left|\right|+\frac{L_{ROBOT}}{2}*COS\left({\theta }_{B^{\prime }}\right)\\ y_{B^{\prime }}=y+\left|\right|CF\left|\right|-\frac{L_{ROBOT}}{2}*COS\left({\theta }_{B^{\prime }}\right)\end{array}\right)$

其中，L_ROBOT为机器人长度，x_B′表示机器人在B'位置的横坐标；y_B′表示机器人在B'位置的纵坐标；||CF||表示机器人质心从A'位置到B'位置的纵坐标距离；||BC||表示机器人质心从A'位置到B'位置的横坐标距离。

本实施例提供的一种基于激光雷达数据的移动机器人位姿推算系统，包括预处理模块、分割聚类模块、簇群选择模块、簇群跟踪模块、偏转角度计算模块、位置坐标计算模块和存储模块；

所述预处理模块，用于接收与预处理激光雷达数据；

所述分割聚类模块，用于分割聚类环境数据；

所述簇群选择模块，用于从激光雷达数据中选择簇群；

所述簇群跟踪模块，用于获取簇群中每个子簇群的顶点在局部坐标系下的坐标数据，获取簇群的顶点的坐标和每个连接连线的倾角；

所述偏转角度计算模块，用于将起始点和终点组成连接直线并计算簇群中连接直线的偏转角度；

所述位置坐标计算模块，用于通过滑动滤波计算机器人位置坐标变化后的位置坐标；

所述存储模块，用于初始化起点的位姿并存储位置坐标。

所述预处理模块中的接收与预处理激光雷达数据的具体步骤如下：

通过与计算机连接的二维扫描型激光雷达传感器获取环境数据；

将环境数据以数组形式存储于计算机中，所述环境数据包括距离信息、激光雷达扫描范围和角度分辨率；

预处理距离数据，包括去除有效范围外的数据点、滤除孤立的噪声点及对激光雷达测量机制的缺陷补偿；

将环境数据从激光雷达的极坐标系下的数据转化为机器人或智能车辆的局部直角坐标系坐标。

所述簇群选择模块中从激光雷达数据中选择簇群的具体步骤如下：

计算簇群中相对应起始点和终点连接直线；

计算簇群中数据点与连接直线的距离；

获取距离直线最远的数据点为顶点；

计算顶点坐标；

计算连接直线的倾角。

所述偏转角度计算模块中的连接直线及偏转角度按照以下步骤来实现：

获取机器人相对于起始位置的A'位置坐标和角度信息φ；

对机器人的角度信息进行滑动滤波；

对簇群的角度信息进行滑动滤波；

得到B'位置机器人相对起始位置的角度信息；

通过对角度信息进行均值滤波得到机器人在B'位置的偏转角度：

${\theta }_{B^{\prime }}=\frac{\underset{i=1}{\overset{n}{\Sigma }}(\phi +{\theta }_{i_{(j+1)*\gamma }}-{\theta }_{i_{j*\gamma }})}{n};$

其中，θ_B′表示机器人在B'位置的偏转角度；n表示一帧环境数据中经过分割算法分割出来的簇群总数；i表示n个簇群中第i个子簇群；表示第i个子簇群在j*γ时刻的位姿偏转角度；表示第i个子簇群在(j+1)*γ时刻的位姿偏转角度；φ表示机器人在A'位置的偏转角度。

所述位置坐标计算模块中机器人位置坐标变化后的位置坐标按照以下步骤来计算：

获取机器人相对应起始点的A'位置坐标(x,y)；

通过以下公式来计算机器人在B'位置的坐标：

$\left(\begin{array}{c}{x}_{B^{\prime }}=x+\left|\right|BC\left|\right|+\frac{L_{ROBOT}}{2}*COS\left({\theta }_{B^{\prime }}\right)\\ y_{B^{\prime }}=y+\left|\right|CF\left|\right|-\frac{L_{ROBOT}}{2}*COS\left({\theta }_{B^{\prime }}\right)\end{array}\right)$

其中，L_ROBOT为机器人长度，x_B′表示机器人在B'位置的横坐标；y_B′表示机器人在B'位置的纵坐标；||CF||表示机器人质心从A'位置到B'位置的纵坐标距离；||BC||表示机器人质心从A'位置到B'位置的横坐标距离。

实施例2

本实施例提供的基于激光雷达数据的移动机器人位姿推算方法，首先，接收和预处理激光雷达数据，具体如下：

将二维扫描型激光雷达传感器与计算机进行连接获取环境数据，并以数组形式存储于计算机中，环境数据主要表现为距离信息D＝{d₁，d₂，d₃，...，d_i，...，d_N}。然后对距离数据进行预处理，包括去除有效范围外的数据点、滤除孤立的噪声点及对激光雷达测量机制的缺陷补偿。另外将环境数据从激光雷达的极坐标系下的数据转化为机器人或智能车辆的局部直角坐标系坐标，即将数据集合表示为：bg＝{P₁,P₂,P₃,......,P_i,......P_N}，其中P_i表示为激光雷达第i线激光束反映的信息，即P_i＝(x_i,y_i)，(x_i,y_i)为第i线激光束检测到的环境在局部坐标系下的坐标。

对环境数据的分割聚类：

平面ABD分割聚类算法在实时性和分割效果上均满足，本发明的要求，因此采用此算法对环境数据进行分割聚类，方法如下：

循环比较距离数据的每两个相邻点之间的差值，当大于分割阈值时，则认为找到了分割点，公式如下：

|d_n-d_n-1|＞D_max

其中：d_n为当前扫描点的距离，d_n-1为前一扫描点的距离，D_max为分割阈值。此处D_max描述如下：

$D_{max}={\gamma }_{n-1}*\frac{sin\left(\Delta \phi \right)}{sin(\lambda -\Delta \phi )}+3\delta$

其中：γ_n-1为d_n和d_n-1取得最小值，δ为余量。λ和Δφ如下图所示，图中φ_n和φ_n-1为点P_n和点P_n-1相当于激光雷达极坐标的角度值。

图2ABD算法示意图，图中：点P_n和点P_n-1为扫描点中连续的两个点；d_n为当前扫描点的距离；d_n-1为前一扫描点的距离；γ_n-1为d_n和d_n-1取得最小值；D_max为分割阈值；Δφ为两个点与坐标原点形成向量的夹角；λ为分割圆的辅助角。如果点P_n和点P_n-1根据上述算法检测得出的结果都是分割点时，则暂时标记点P_n和点P_n-1为分割点，然后利用一个阈值ε(d_n)进 行二次检测，ε(d_n)＝k*d_n，其中k为常系数。为了使分割点间的距离不至于太远，本发明对阈值进行限定，规定ε(d_n)≤μ。若|d_n-d_n-1|＞ε(d_n)，则认为点P_n和点P_n-1为分割点，反之，则不是分割点，取消之前分割的标记。

对一帧激光雷达的N个距离数据进行优化型ABD算法聚类分析后将得p个分割点，即将环境分成p类。并将每个分割点的角标存储在计算机内。

对不同时刻进行目标簇群跟踪

①在采样时刻t时，寻找具有特征点的簇群C＝{C₁,C₂,...,C_n}。

寻找采样时刻t的激光雷达数据中具有顶点的簇群，如下图所示。规则为连接该簇群的起始点和终点形成直线，计算除了这两点外的其他所有点距离该直线的距离。距离直线最远的即为顶点。

图3具有顶点的簇群示意图

基于上述选择的簇群，求出顶点坐标，记为：

$P=\left(\begin{array}{c}{P}_1\\ P_2\\ .\\ .\\ .\\ P_n\end{array}\right)$

将每个簇群的起始点与终点连线形成的直线，并计算每条直线的倾角，记为：

$L=\left(\begin{array}{c}{L}_1\\ L_2\\ .\\ .\\ .\\ L_n\end{array}\right),\theta =\left(\begin{array}{c}{\theta }_1\\ {\theta }_2\\ .\\ .\\ .\\ {\theta }_n\end{array}\right)$

②目标簇群的跟踪

每经过γ个采样时刻时，获取上述簇群集合C内每个子簇群的顶点在局部坐标系下的坐标数据，在经过m*γ时刻后，所有子簇群的顶点的坐标和每个子簇群起点和终点连线的倾角分别表示为：

$P=\left(\begin{array}{ccccc}{P}_{1_{t+\gamma }}& P_{1_{t+2*\gamma }}& P_{1_{t+3*\gamma }}& \mathrm{...}& P_{1_{t+m*\gamma }}\\ P_{2_{t+\gamma }}& P_{2_{t+2*\gamma }}& P_{2_{t+3*\gamma }}& \mathrm{...}& P_{2_{t+m*\gamma }}\\ & .& & & \\ & .& & & \\ & .& & & \\ P_{n_{t+\gamma }}& P_{n_{t+2*\gamma }}& P_{n_{t+3*\gamma }}& \mathrm{...}& P_{m_{t+m*\gamma }}\end{array}\right)$

$\theta =\left(\begin{array}{ccccc}{\theta }_{1_{t+\gamma }}& {\theta }_{1_{t+2*\gamma }}& {\theta }_{1_{t+3*\gamma }}& \mathrm{...}& {\theta }_{1_{t+m*\gamma }}\\ {\theta }_{2_{t+\gamma }}& {\theta }_{2_{t+2*\gamma }}& {\theta }_{2_{t+3*\gamma }}& \mathrm{...}& {\theta }_{2_{t+m*\gamma }}\\ & .& & & \\ & .& & & \\ & .& & & \\ {\theta }_{n_{t+\gamma }}& {\theta }_{n_{t+2*\gamma }}& {\theta }_{n_{t+3*\gamma }}& \mathrm{...}& {\theta }_{m_{t+m*\gamma }}\end{array}\right)$

基于滑动滤波的位姿推算

对于第i个子簇群C_i，从第j*γ个时刻到第(j+1)*γ时刻，该簇群的变化示意图如下所示：图4不同时刻机器人位姿变化示意图，图a中：L_j*γ为某簇群在第j*γ个时刻起点和终点连线；图b中：L_(j+1)*γ为某簇群在第(j+1)*γ个时刻起点和终点连线；图c中：θ_B'为B'位置的偏转角度；φ为机器人在A'位置相对于起始位置的角度。

由图4(a)与图4(b)可知，机器人从A'运动到B'位置时，机器人偏转的角度差就是某一个簇群偏转的角度差，该角度差等于L_j*γ与L_(j+1)*γ倾角的变化程度，即(θ_(j+1)*γ-θ_j*γ)。

在此设A'位置相对于起始位置，机器人的角度为φ。对机器人位姿中的角度信息进行滑动滤波。将滑动窗口宽度设置为k*γ长度(k＜m)，经过对上述θ矩阵内n个簇群进行(m-k)次滑动滤波后，得到B'位置机器人相对起始位置的角度为：

$\theta =\left(\begin{array}{c}\phi +{\theta }_{1(j+1)*\gamma }-{\theta }_{1_{j*\gamma }}\\ \phi +{\theta }_{2(j+1)*\gamma }-{\theta }_{2_{j*\gamma }}\\ .\\ .\\ .\\ \phi +{\theta }_{n_{(j+1)*\gamma }}-{\theta }_{n_{j*\gamma }}\end{array}\right)$

对以上n个(j+1)*γ时刻的位姿偏转角度进行均值滤波后，得到在B'位置的偏转角度为：

${\theta }_{B^{\prime }}=\frac{\underset{i=1}{\overset{n}{\Sigma }}(\phi +{\theta }_{i_{(j+1)*\gamma }}-{\theta }_{i_{j*\gamma }})}{n}$

由图4(a)可以很明显看出，当机器人从A'运动到B'位置时，视野里的局部环境也发生了变化。设在A'位置时，机器人的位置坐标为(x,y)，图(c)将变化定量后，有几何知识可以得到：在B'位置时，机器人的坐标即为：

$\left(\begin{array}{c}{x}_{B^{\prime }}=x+\left|\right|BC\left|\right|+\frac{L_{ROBOT}}{2}*COS\left({\theta }_{B^{\prime }}\right)\\ y_{B^{\prime }}=y+\left|\right|CF\left|\right|-\frac{L_{ROBOT}}{2}*COS\left({\theta }_{B^{\prime }}\right)\end{array}\right)$

其中L_ROBOT为机器人长度，其余相关参数见图4(c)所示。

由图(c)可知，D点即为该簇群的顶点，当机器人到达B'位置时，此时D点对应的激光雷达线数也是确定了的，假设此时的线数为p，那么图(c)上其中ξ为二维激光雷达的角度分辨率。

由：

得到：

||BC||＝||BD||*sin(p*ξ-90°+θ_B')

CF的长度也可以根据几何关系得到，即：

得到：

||CF||＝||DF||-||BD||*cos(p*ξ-90°+θ_B')

因此可以得到机器人在B'位置处的坐标为：

对机器人位姿中的位置信息进行滑动滤波。将滑动窗口宽度设置为k*γ长度(k＜m)，经过对上述P矩阵内n个簇群进行(m-k)次滑动滤波后，得到B'位置机器人相对起始位置的 位置为：

$P=\left(\begin{array}{c}{P}_{1_{(j+1)*\gamma }}\\ P_{2_{(j+1)*\gamma }}\\ .\\ .\\ .\\ P_{n_{(j+1)*\gamma }}\end{array}\right)=\left(\begin{array}{c}(x_{1_{B^{\prime }}},y_{1_{B^{\prime }}})\\ (x_{2_{B^{\prime }}},y_{2_{B^{\prime }}})\\ .\\ .\\ .\\ (x_{n_{B^{\prime }}},y_{n_{B^{\prime }}})\end{array}\right)$

对以上n个(j+1)*γ时刻的位置进行均值滤波后，得到在B'位置的位置坐标为：

$\left(\begin{array}{c}{x}_{B^{\prime }}=\frac{\underset{i=1}{\overset{n}{\Sigma }}\left(x_{i_{B^{\prime }}}\right)}{n}\\ y_{B^{\prime }}=\frac{\underset{i=1}{\overset{n}{\Sigma }}\left(y_{i_{B^{\prime }}}\right)}{n}\end{array}\right)$

根据上面的公式可以从起始点进行叠加，叠加次数与采样周期有关。因此得到机器人在任何时刻的位姿。

实施例3

本实施例详细描述基于激光雷达数据的移动机器人位姿推算方法的实施方式，具体包括以下步骤：

Step1：激光雷达数据的接收与预处理

将二维扫描型激光雷达传感器与计算机进行连接获取环境数据，并以数组形式存储于计算机中，环境数据主要表现为距离信息D＝{d₁，d₂，d₃，...，d_i，...，d_N}，N常取值为500，对应激光雷达的扫描范围180°，角度分辨率ξ＝0.36°。然后对距离数据进行预处理，包括去除有效范围外的数据点、滤除孤立的噪声点及对激光雷达测量机制的缺陷补偿。另外将环境数据从激光雷达的极坐标系下的数据转化为机器人或智能车辆的局部直角坐标系坐标，即将数据集合表示为：bg＝{P₁,P₂,P₃,......,P_i,......P_N}，其中P_i表示为激光雷达第i线激光束反映的信息，即P_i＝(x_i,y_i)，(x_i,y_i)为第i线激光束检测到的环境在局部坐标系下的坐标。

Step2：对环境数据的分割聚类

平面ABD分割聚类算法在实时性和分割效果上均满足，本发明的要求，因此采用此算 法对环境数据进行分割聚类，方法如下：

循环比较距离数据的每两个相邻点之间的差值，当大于分割阈值时，则认为找到了分割点，公式如下：

|d_n-d_n-1|＞D_max

其中：d_n为当前扫描点的距离，d_n-1为前一扫描点的距离，D_max为分割阈值。此处D_max描述如下

其中：r_n-1为d_n和d_n-1取得最小值，δ为余量。λ和如下图所示，图中和为点P_n和点P_n-1相当于激光雷达极坐标的角度值。

图2为ABD算法示意图，如果点P_n和点P_n-1根据上述算法检测得出的结果都是分割点时，则暂时标记点P_n和点P_n-1为分割点，然后利用一个阈值ε(d_n)进行二次检测，ε(d_n)＝k*d_n，其中k为常系数。为了使分割点间的距离不至于太远，本发明对阈值进行限定，规定ε(d_n)≤μ。若|d_n-d_n-1|＞ε(d_n)，则认为点P_n和点P_n-1为分割点，反之，则不是分割点，取消之前分割的标记。其中，在本实施例中，k＝0.15，μ＝0.1m，δ＝0.03m。

对一帧激光雷达的N＝500个距离数据进行优化型ABD算法聚类分析后将得p个分割点，即将环境分成p类。并将每个分割点的角标存储在计算机内。

Step3：在采样时刻t时，寻找具有特征点的簇群C＝{C₁，C₂，…，C_n}。

寻找采样时刻t的激光雷达数据中具有顶点的簇群，如下图所示。规则为连接该簇群的起始点和终点形成直线，计算除了这两点外的其他所有点距离该直线的距离。距离直线最远的即为顶点。

图3为具有顶点的簇群示意图，基于上述选择的簇群，求出顶点坐标，记为：

$P=\left(\begin{array}{c}{P}_1\\ P_2\\ .\\ .\\ .\\ P_n\end{array}\right)$

将每个簇群的起始点与终点连线形成的直线，并计算每条直线的倾角，记为：

$L=\left(\begin{array}{c}{L}_1\\ L_2\\ .\\ .\\ .\\ L_n\end{array}\right),\theta =\left(\begin{array}{c}{\theta }_1\\ {\theta }_2\\ .\\ .\\ .\\ {\theta }_n\end{array}\right)$

Step4：目标簇群的跟踪

每经过γ＝1个采样时刻(采样周期一般取T＝100ms)时，获取上述簇群集合C内每个子簇群的顶点在局部坐标系下的坐标数据，在经过m*γ个采样周期后，所有子簇群的顶点的坐标和每个子簇群起点和终点连线的倾角分别表示为：

$P=\left(\begin{array}{ccccc}{P}_{1_{t+\gamma }}& P_{1_{t+2*\gamma }}& P_{1_{t+3*\gamma }}& \mathrm{...}& P_{1_{t+m*\gamma }}\\ P_{2_{t+\gamma }}& P_{2_{t+2*\gamma }}& P_{2_{t+3*\gamma }}& \mathrm{...}& P_{2_{t+m*\gamma }}\\ & .& & & \\ & .& & & \\ & .& & & \\ P_{n_{t+\gamma }}& P_{n_{t+2*\gamma }}& P_{n_{t+3*\gamma }}& \mathrm{...}& P_{m_{t+m*\gamma }}\end{array}\right)$

$\theta =\left(\begin{array}{ccccc}{\theta }_{1_{t+\gamma }}& {\theta }_{1_{t+2*\gamma }}& {\theta }_{1_{t+3*\gamma }}& \mathrm{...}& {\theta }_{1_{t+m*\gamma }}\\ {\theta }_{2_{t+\gamma }}& {\theta }_{2_{t+2*\gamma }}& {\theta }_{2_{t+3*\gamma }}& \mathrm{...}& {\theta }_{2_{t+m*\gamma }}\\ & .& & & \\ & .& & & \\ & .& & & \\ {\theta }_{n_{t+\gamma }}& {\theta }_{n_{t+2*\gamma }}& {\theta }_{n_{t+3*\gamma }}& \mathrm{...}& {\theta }_{m_{t+m*\gamma }}\end{array}\right)$

其中，在本实施例中，m＝10。

Step5：基于滑动滤波的偏转角度推算

对于第i个子簇群C_i，从第j*γ个时刻到第(j+1)*γ时刻，该簇群的变化示意图如下所示：

图4为不同时刻机器人位姿变化示意图，由图4(a)与图4(b)可知，机器人从A'运动到B'位置时，机器人偏转的角度差就是某一个簇群偏转的角度差，该角度差等于L_j*γ与L_(j+1)*γ倾角的变化程度，即(θ_(j+1)*γ-θ_j*γ)。

在此设A'位置相对于起始位置，机器人的角度为φ。对机器人位姿中的角度信息进行滑动滤波。将滑动窗口宽度设置为k*γ长度，其中k＝5，经过对上述θ矩阵内n个簇群进行(m-k)＝5次滑动滤波后，得到B'位置机器人相对起始位置的角度为：

$\theta =\left(\begin{array}{c}\phi +{\theta }_{1_{(j+1)*\gamma }}-{\theta }_{1_{j*\gamma }}\\ \phi +{\theta }_{2_{(j+1)*\gamma }}-{\theta }_{2_{j*\gamma }}\\ .\\ .\\ .\\ \phi +{\theta }_{n_{(j+1)*\gamma }}-{\theta }_{n_{j*\gamma }}\end{array}\right)$

对以上n个(j+1)*γ时刻的位姿偏转角度进行均值滤波后，得到在B'位置的偏转角度为：

${\theta }_{B^{\prime }}=\frac{\underset{i=1}{\overset{n}{\Sigma }}(\phi +{\theta }_{i_{(j+1)*\gamma }}-{\theta }_{i_{j*\gamma }})}{n}$

Step6：基于滑动滤波的位置坐标推算

由图4a可以很明显看出，当机器人从A'运动到B'位置时，视野里的局部环境也发生了变化。设在A'位置时，机器人的位置坐标为(x,y)，图4c将变化定量后，有几何知识可以得到：在B'位置时，机器人的坐标即为：

$\left(\begin{array}{c}{x}_{B^{\prime }}=x+\left|\right|BC\left|\right|+\frac{L_{ROBOT}}{2}*COS\left({\theta }_{B^{\prime }}\right)\\ y_{B^{\prime }}=y+\left|\right|CF\left|\right|-\frac{L_{ROBOT}}{2}*COS\left({\theta }_{B^{\prime }}\right)\end{array}\right)$

其中L_ROBOT为机器人长度，本实施例采用的机器人长度为114cm，即L_ROBOT＝114cm，其余相关参数见图4c所示。

由图4c可知，D点即为该簇群的顶点，当机器人到达B'位置时，此时D点对应的激光雷达线数也是确定了的，假设此时的线数为p，那么图4c上其中ξ为二维激光雷达的角度分辨率，本实施例中取ξ＝0.36°。

由：

得到：

||BC||＝||BD||*sin(0.36°*p-90°+θ_B')

CF的长度也可以根据几何关系得到，即：

得到：

||CF||＝||DF||-||BD||*cos(0.36°*p-90°+θ_B')

因此可以得到机器人在B'位置处的坐标为：

对机器人位姿中的位置信息进行滑动滤波。将滑动窗口宽度设置为k*γ长度(k＜m)，经过对上述P矩阵内n个簇群进行(m-k)次滑动滤波后，得到B'位置机器人相对起始位置的位置为：

$P=\left(\begin{array}{c}{P}_{1_{(j+1)*\gamma }}\\ P_{2_{(j+1)*\gamma }}\\ .\\ .\\ .\\ P_{n_{(j+1)*\gamma }}\end{array}\right)=\left(\begin{array}{c}(x_{1_{B^{\prime }}},y_{1_{B^{\prime }}})\\ (x_{2_{B^{\prime }}},y_{2_{B^{\prime }}})\\ .\\ .\\ .\\ (x_{n_{B^{\prime }}},y_{n_{B^{\prime }}})\end{array}\right)$

对以上n个(j+1)*γ时刻的位置进行均值滤波后，得到在B'位置的位置坐标为：

$\left(\begin{array}{c}{x}_{B^{\prime }}=\frac{\underset{i=1}{\overset{n}{\Sigma }}\left(x_{i_{B^{\prime }}}\right)}{n}\\ y_{B^{\prime }}=\frac{\underset{i=1}{\overset{n}{\Sigma }}\left(y_{i_{B^{\prime }}}\right)}{n}\end{array}\right)$

Step7：初始化起点的位姿，即(x,y,θ)＝(0,0,0)，每经过γ＝1的采样周期时，对位姿进行累积。因此得到机器人在任何时刻的位姿。

最后说明的是，以上实施例仅用以说明本发明的技术方案而非限制，尽管参照较佳实施例对本发明进行了详细说明，本领域的普通技术人员应当理解，可以对本发明的技术方案进行修改或者等同替换，而不脱离本技术方案的宗旨和范围，其均应涵盖在本发明的权利要求范围当中。