CN105867373A - 一种基于激光雷达数据的移动机器人位姿推算方法及系统 - Google Patents
一种基于激光雷达数据的移动机器人位姿推算方法及系统 Download PDFInfo
- Publication number
- CN105867373A CN105867373A CN201610213132.1A CN201610213132A CN105867373A CN 105867373 A CN105867373 A CN 105867373A CN 201610213132 A CN201610213132 A CN 201610213132A CN 105867373 A CN105867373 A CN 105867373A
- Authority
- CN
- China
- Prior art keywords
- robot
- cluster
- data
- laser radar
- pose
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Granted
Links
- 238000000034 method Methods 0.000 title claims abstract description 33
- 230000007613 environmental effect Effects 0.000 claims description 49
- 230000011218 segmentation Effects 0.000 claims description 34
- 238000004422 calculation algorithm Methods 0.000 claims description 16
- 230000007547 defect Effects 0.000 claims description 8
- 238000005259 measurement Methods 0.000 claims description 8
- 238000002372 labelling Methods 0.000 claims description 7
- 238000000638 solvent extraction Methods 0.000 claims description 7
- 238000005070 sampling Methods 0.000 abstract description 12
- 238000001914 filtration Methods 0.000 abstract description 3
- 230000003247 decreasing effect Effects 0.000 abstract 1
- 238000007781 pre-processing Methods 0.000 abstract 1
- 238000010586 diagram Methods 0.000 description 14
- 230000001133 acceleration Effects 0.000 description 5
- 238000001514 detection method Methods 0.000 description 4
- 239000011159 matrix material Substances 0.000 description 4
- 230000000694 effects Effects 0.000 description 3
- 230000003287 optical effect Effects 0.000 description 3
- 238000004364 calculation method Methods 0.000 description 2
- 238000007621 cluster analysis Methods 0.000 description 2
- 235000013399 edible fruits Nutrition 0.000 description 2
- 238000005516 engineering process Methods 0.000 description 2
- 238000004519 manufacturing process Methods 0.000 description 2
- 230000001052 transient effect Effects 0.000 description 2
- 230000000007 visual effect Effects 0.000 description 2
- 101100379079 Emericella variicolor andA gene Proteins 0.000 description 1
- 238000009825 accumulation Methods 0.000 description 1
- 230000018109 developmental process Effects 0.000 description 1
- 238000006073 displacement reaction Methods 0.000 description 1
- 230000010354 integration Effects 0.000 description 1
- 230000035945 sensitivity Effects 0.000 description 1
Classifications
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
- G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
- G05D1/02—Control of position or course in two dimensions
- G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
- G05D1/0257—Control of position or course in two dimensions specially adapted to land vehicles using a radar
Landscapes
- Engineering & Computer Science (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Aviation & Aerospace Engineering (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Automation & Control Theory (AREA)
- Optical Radar Systems And Details Thereof (AREA)
- Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
Abstract
本发明公开了一种基于激光雷达数据的移动机器人位姿推算方法,首先接收与预处理激光雷达数据;分割聚类环境数据;然后从激光雷达数据中选择簇群;并对目标簇群进行跟踪;获取簇群中每个子簇群的顶点在局部坐标系下的坐标数据,获取簇群的顶点的坐标和每个连接连线的倾角;将起始点和终点组成连接直线并计算偏转角度;通过滑动滤波计算机器人位置坐标变化后的位置坐标;最后初始化起始点的位姿和采样位姿点。本发明提供的移动机器人位姿推算方法是使用精度较高的二维激光雷达,无需对环境进行任何修改;机器人的位姿信息相对于具体环境客观描述,更便捷将位姿信息集成到其它移动机器人的业务模块中,提高系统的可靠性、易实现性并降低成本。
Description
技术领域
本发明涉及移动机器人及智能车辆的局部导航领域,特别涉及一种基于激光雷达数据的移动机器人位姿推算方法。
背景技术
机器人作为一种新型的生产工具,表现出极大的优越性。在减轻劳动强度,提高生产效率,改变生产模式等方面可将人们从危险、恶劣、繁重的工作环境中解放出来。然而随着机器人的不断发展,人们逐渐发现这些固定于某一位置操作的机器人对很多应用需求均不能满足。因此在20世纪80年代后期,许多国家开始有计划地开展了移动机器人技术的研究。
对于移动机器人而言,导航是一个重要的方向。而移动机器人的导航的前提就是对机器人的定位和姿态识别。精确的位姿推算对于移动机器人的定位、自动地图生成、路径规划和控制、目标检测和跟踪等具有重要意义。在二维的平面坐标系下,移动机器人的位姿就是机器人在全局坐标系中的位置以及机器人的车体方向,即表示为:(x,y,θ)。机器人的位置可以用机器人质心坐标点表示为,而用车体前方偏向全局坐标系X轴的夹角来表示车体的方向。在本文中,定义方向角为:以X轴0度,顺时针方向为负,逆时针方向为正;如图1所示,图1机器人的位姿模型;图中XGOGYG为机器所在的全局坐标系;XRORYR为机器人所在的局部坐标系;(x,y)为机器人质心的坐标;θ为机器人相对于起始点的位姿偏转角。应用在移动机器人的位姿推算方法一般可分为两种:航位推算(Dead-Reckoning)是一种非常经典的相对位姿推算方法,通过安装在移动机器人轮子上的光电编码器输出的轮速与移动机器人的运动学模型,累积每个采样周期下的移动机器人位移以及角度偏转。航位推算的优点是方法简单、价格低廉,但从其本质上上可以看出,航位推算方法的精度容易受轮速的标定、车轮与地面的打滑、颠簸等因素造成积分误差而大大降低,因此误差较大。惯性导航系统INS(InertialNavigation Systems)常使用陀螺仪得到机器人在每个方向上的角加速度,经过对角加速度的积分就可以得到偏转角度,使用加速度传感器得到机器人的加速度信息,并结合角度信息,对加速度进行矢量积分获得机器人的位置信息,从而获得移动机器人在不同采样时刻的位姿信息。一般情况下,惯性导航系统的精度高于航位推算,但是其精度也要受陀螺仪漂移、标定误差、敏感度等问题影响。
无论是航位推算还是惯性导航系统,都存在一个共同的缺点:存在积分误差,移动机器人的行程越远,该积分误差越大。因此现有的位姿推算方法并不适用于对机器人位姿信息要求较高的场合。如何实现更加精确的且不受地面情况限制的位姿推算方法,是一个很重要的问题。
因此,需要一种基于激光雷达数据的移动机器人位姿描述方法。
发明内容
本发明的目的就是提供一种基于激光雷达数据的移动机器人位姿描述方法。
本发明的目的是通过这样的技术方案实现的:
本发明提供的一种基于激光雷达数据的移动机器人位姿推算方法,包括以下步骤:
Step1:接收与预处理激光雷达数据;
Step2:分割聚类环境数据;
Step3:从激光雷达数据中选择簇群;
Step4:目标簇群的跟踪;获取簇群中每个子簇群的顶点在局部坐标系下的坐标数据,获取簇群的顶点的坐标和每个连接连线的倾角;
Step5:将起始点和终点组成连接直线并计算簇群中连接直线的偏转角度;
Step6:通过滑动滤波计算机器人位置坐标变化后的位置坐标;
Step7:初始化起点的位姿并存储位置坐标。
进一步,所述步骤Step1中的接收与预处理激光雷达数据的具体步骤如下:
通过与计算机连接的二维扫描型激光雷达传感器获取环境数据;
将环境数据以数组形式存储于计算机中,所述环境数据包括距离信息、激光雷达扫描范围和角度分辨率;
预处理距离数据,包括去除有效范围外的数据点、滤除孤立的噪声点及对激光雷达测量机制的缺陷补偿;
将环境数据从激光雷达的极坐标系下的数据转化为机器人或智能车辆的局部直角坐标系坐标。
进一步,所述步骤Step2中的按照以下步骤来分割聚类环境数据:
循环比较距离数据的每两个相邻数据点差值,标记差值大于分割阈值的数据点为分割点;
存储分割点的角标;
进一步,所述步骤Step3中从激光雷达数据中选择簇群的具体步骤如下:
计算簇群中相对应起始点和终点连接直线;
计算簇群中数据点与连接直线的距离;
获取距离直线最远的数据点为顶点;
计算顶点坐标;
计算连接直线的倾角。
进一步,所述步骤Step5中的连接直线及偏转角度按照以下步骤来实现:
获取机器人相对于起始位置的A'位置坐标和角度信息φ;
对机器人的角度信息进行滑动滤波;
对簇群的角度信息进行滑动滤波;
得到B'位置机器人相对起始位置的角度信息;
通过对角度信息进行均值滤波得到机器人在B'位置的偏转角度:
其中,θB′表示机器人在B'位置的偏转角度;n表示一帧环境数据中经过分割算法分割出来的簇群总数;i表示n个簇群中第i个子簇群;表示第i个子簇群在j*γ时刻的位姿偏转角度;表示第i个子簇群在(j+1)*γ时刻的位姿偏转角度;φ表示机器人在A'位置的偏转角度。
进一步,所述步骤Step6中机器人位置坐标变化后的位置坐标按照以下步骤来计算:
获取机器人相对应起始点的A'位置坐标(x,y);
通过以下公式来计算机器人在B'位置的坐标:
其中,LROBOT为机器人长度,xB′表示机器人在B'位置的横坐标;yB′表示机器人在B'位置的纵坐标;||CF||表示机器人质心从A'位置到B'位置的纵坐标距离;||BC||表示机器人质心从A'位置到B'位置的横坐标距离。
本发明还提供了一种基于激光雷达数据的移动机器人位姿推算系统,包括预处理模块、分割聚类模块、簇群选择模块、簇群跟踪模块、偏转角度计算模块、位置坐标计算模块和存储模块;
所述预处理模块,用于接收与预处理激光雷达数据;
所述分割聚类模块,用于分割聚类环境数据;
所述簇群选择模块,用于从激光雷达数据中选择簇群;
所述簇群跟踪模块,用于获取簇群中每个子簇群的顶点在局部坐标系下的坐标数据,获取簇群的顶点的坐标和每个连接连线的倾角;
所述偏转角度计算模块,用于将起始点和终点组成连接直线并计算簇群中连接直线的偏转角度;
所述位置坐标计算模块,用于通过滑动滤波计算机器人位置坐标变化后的位置坐标;
所述存储模块,用于初始化起点的位姿并存储位置坐标。
进一步,所述预处理模块中的接收与预处理激光雷达数据的具体步骤如下:
通过与计算机连接的二维扫描型激光雷达传感器获取环境数据;
将环境数据以数组形式存储于计算机中,所述环境数据包括距离信息、激光雷达扫描范围和角度分辨率;
预处理距离数据,包括去除有效范围外的数据点、滤除孤立的噪声点及对激光雷达测量机制的缺陷补偿;
将环境数据从激光雷达的极坐标系下的数据转化为机器人或智能车辆的局部直角坐标系坐标。
进一步,所述簇群选择模块中从激光雷达数据中选择簇群的具体步骤如下:
计算簇群中相对应起始点和终点连接直线;
计算簇群中数据点与连接直线的距离;
获取距离直线最远的数据点为顶点;
计算顶点坐标;
计算连接直线的倾角。
所述偏转角度计算模块中的连接直线及偏转角度按照以下步骤来实现:
获取机器人相对于起始位置的A'位置坐标和角度信息φ;
对机器人的角度信息进行滑动滤波;
对簇群的角度信息进行滑动滤波;
得到B'位置机器人相对起始位置的角度信息;
通过对角度信息进行均值滤波得到机器人在B'位置的偏转角度:
其中,θB′表示机器人在B'位置的偏转角度;n表示一帧环境数据中经过分割算法分割出来的簇群总数;i表示n个簇群中第i个子簇群;表示第i个子簇群在j*γ时刻的位姿偏转角度;表示第i个子簇群在(j+1)*γ时刻的位姿偏转角度;φ表示机器人在A'位置的偏转角度。
进一步,所述位置坐标计算模块中机器人位置坐标变化后的位置坐标按照以下步骤来计算:
获取机器人相对应起始点的A'位置坐标(x,y);
通过以下公式来计算机器人在B'位置的坐标:
其中,LROBOT为机器人长度,xB′表示机器人在B'位置的横坐标;yB′表示机器人在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'位置的偏转角度:
其中,θB′表示机器人在B'位置的偏转角度;n表示一帧环境数据中经过分割算法分割出来的簇群总数;i表示n个簇群中第i个子簇群;表示第i个子簇群在j*γ时刻的位姿偏转角度;表示第i个子簇群在(j+1)*γ时刻的位姿偏转角度;φ表示机器人在A'位置的偏转角度。
所述步骤Step6中机器人位置坐标变化后的位置坐标按照以下步骤来计算:
获取机器人相对应起始点的A'位置坐标(x,y);
通过以下公式来计算机器人在B'位置的坐标:
其中,LROBOT为机器人长度,xB′表示机器人在B'位置的横坐标;yB′表示机器人在B'位置的纵坐标;||CF||表示机器人质心从A'位置到B'位置的纵坐标距离;||BC||表示机器人质心从A'位置到B'位置的横坐标距离。
本实施例提供的一种基于激光雷达数据的移动机器人位姿推算系统,包括预处理模块、分割聚类模块、簇群选择模块、簇群跟踪模块、偏转角度计算模块、位置坐标计算模块和存储模块;
所述预处理模块,用于接收与预处理激光雷达数据;
所述分割聚类模块,用于分割聚类环境数据;
所述簇群选择模块,用于从激光雷达数据中选择簇群;
所述簇群跟踪模块,用于获取簇群中每个子簇群的顶点在局部坐标系下的坐标数据,获取簇群的顶点的坐标和每个连接连线的倾角;
所述偏转角度计算模块,用于将起始点和终点组成连接直线并计算簇群中连接直线的偏转角度;
所述位置坐标计算模块,用于通过滑动滤波计算机器人位置坐标变化后的位置坐标;
所述存储模块,用于初始化起点的位姿并存储位置坐标。
所述预处理模块中的接收与预处理激光雷达数据的具体步骤如下:
通过与计算机连接的二维扫描型激光雷达传感器获取环境数据;
将环境数据以数组形式存储于计算机中,所述环境数据包括距离信息、激光雷达扫描范围和角度分辨率;
预处理距离数据,包括去除有效范围外的数据点、滤除孤立的噪声点及对激光雷达测量机制的缺陷补偿;
将环境数据从激光雷达的极坐标系下的数据转化为机器人或智能车辆的局部直角坐标系坐标。
所述簇群选择模块中从激光雷达数据中选择簇群的具体步骤如下:
计算簇群中相对应起始点和终点连接直线;
计算簇群中数据点与连接直线的距离;
获取距离直线最远的数据点为顶点;
计算顶点坐标;
计算连接直线的倾角。
所述偏转角度计算模块中的连接直线及偏转角度按照以下步骤来实现:
获取机器人相对于起始位置的A'位置坐标和角度信息φ;
对机器人的角度信息进行滑动滤波;
对簇群的角度信息进行滑动滤波;
得到B'位置机器人相对起始位置的角度信息;
通过对角度信息进行均值滤波得到机器人在B'位置的偏转角度:
其中,θB′表示机器人在B'位置的偏转角度;n表示一帧环境数据中经过分割算法分割出来的簇群总数;i表示n个簇群中第i个子簇群;表示第i个子簇群在j*γ时刻的位姿偏转角度;表示第i个子簇群在(j+1)*γ时刻的位姿偏转角度;φ表示机器人在A'位置的偏转角度。
所述位置坐标计算模块中机器人位置坐标变化后的位置坐标按照以下步骤来计算:
获取机器人相对应起始点的A'位置坐标(x,y);
通过以下公式来计算机器人在B'位置的坐标:
其中,LROBOT为机器人长度,xB′表示机器人在B'位置的横坐标;yB′表示机器人在B'位置的纵坐标;||CF||表示机器人质心从A'位置到B'位置的纵坐标距离;||BC||表示机器人质心从A'位置到B'位置的横坐标距离。
实施例2
本实施例提供的基于激光雷达数据的移动机器人位姿推算方法,首先,接收和预处理激光雷达数据,具体如下:
将二维扫描型激光雷达传感器与计算机进行连接获取环境数据,并以数组形式存储于计算机中,环境数据主要表现为距离信息D={d1,d2,d3,...,di,...,dN}。然后对距离数据进行预处理,包括去除有效范围外的数据点、滤除孤立的噪声点及对激光雷达测量机制的缺陷补偿。另外将环境数据从激光雷达的极坐标系下的数据转化为机器人或智能车辆的局部直角坐标系坐标,即将数据集合表示为:bg={P1,P2,P3,......,Pi,......PN},其中Pi表示为激光雷达第i线激光束反映的信息,即Pi=(xi,yi),(xi,yi)为第i线激光束检测到的环境在局部坐标系下的坐标。
对环境数据的分割聚类:
平面ABD分割聚类算法在实时性和分割效果上均满足,本发明的要求,因此采用此算法对环境数据进行分割聚类,方法如下:
循环比较距离数据的每两个相邻点之间的差值,当大于分割阈值时,则认为找到了分割点,公式如下:
|dn-dn-1|>Dmax
其中:dn为当前扫描点的距离,dn-1为前一扫描点的距离,Dmax为分割阈值。此处Dmax描述如下:
其中:γn-1为dn和dn-1取得最小值,δ为余量。λ和Δφ如下图所示,图中φn和φn-1为点Pn和点Pn-1相当于激光雷达极坐标的角度值。
图2ABD算法示意图,图中:点Pn和点Pn-1为扫描点中连续的两个点;dn为当前扫描点的距离;dn-1为前一扫描点的距离;γn-1为dn和dn-1取得最小值;Dmax为分割阈值;Δφ为两个点与坐标原点形成向量的夹角;λ为分割圆的辅助角。如果点Pn和点Pn-1根据上述算法检测得出的结果都是分割点时,则暂时标记点Pn和点Pn-1为分割点,然后利用一个阈值ε(dn)进行二次检测,ε(dn)=k*dn,其中k为常系数。为了使分割点间的距离不至于太远,本发明对阈值进行限定,规定ε(dn)≤μ。若|dn-dn-1|>ε(dn),则认为点Pn和点Pn-1为分割点,反之,则不是分割点,取消之前分割的标记。
对一帧激光雷达的N个距离数据进行优化型ABD算法聚类分析后将得p个分割点,即将环境分成p类。并将每个分割点的角标存储在计算机内。
对不同时刻进行目标簇群跟踪
①在采样时刻t时,寻找具有特征点的簇群C={C1,C2,...,Cn}。
寻找采样时刻t的激光雷达数据中具有顶点的簇群,如下图所示。规则为连接该簇群的起始点和终点形成直线,计算除了这两点外的其他所有点距离该直线的距离。距离直线最远的即为顶点。
图3具有顶点的簇群示意图
基于上述选择的簇群,求出顶点坐标,记为:
将每个簇群的起始点与终点连线形成的直线,并计算每条直线的倾角,记为:
②目标簇群的跟踪
每经过γ个采样时刻时,获取上述簇群集合C内每个子簇群的顶点在局部坐标系下的坐标数据,在经过m*γ时刻后,所有子簇群的顶点的坐标和每个子簇群起点和终点连线的倾角分别表示为:
基于滑动滤波的位姿推算
对于第i个子簇群Ci,从第j*γ个时刻到第(j+1)*γ时刻,该簇群的变化示意图如下所示:图4不同时刻机器人位姿变化示意图,图a中:Lj*γ为某簇群在第j*γ个时刻起点和终点连线;图b中:L(j+1)*γ为某簇群在第(j+1)*γ个时刻起点和终点连线;图c中:θB'为B'位置的偏转角度;φ为机器人在A'位置相对于起始位置的角度。
由图4(a)与图4(b)可知,机器人从A'运动到B'位置时,机器人偏转的角度差就是某一个簇群偏转的角度差,该角度差等于Lj*γ与L(j+1)*γ倾角的变化程度,即(θ(j+1)*γ-θj*γ)。
在此设A'位置相对于起始位置,机器人的角度为φ。对机器人位姿中的角度信息进行滑动滤波。将滑动窗口宽度设置为k*γ长度(k<m),经过对上述θ矩阵内n个簇群进行(m-k)次滑动滤波后,得到B'位置机器人相对起始位置的角度为:
对以上n个(j+1)*γ时刻的位姿偏转角度进行均值滤波后,得到在B'位置的偏转角度为:
由图4(a)可以很明显看出,当机器人从A'运动到B'位置时,视野里的局部环境也发生了变化。设在A'位置时,机器人的位置坐标为(x,y),图(c)将变化定量后,有几何知识可以得到:在B'位置时,机器人的坐标即为:
其中LROBOT为机器人长度,其余相关参数见图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'位置机器人相对起始位置的位置为:
对以上n个(j+1)*γ时刻的位置进行均值滤波后,得到在B'位置的位置坐标为:
根据上面的公式可以从起始点进行叠加,叠加次数与采样周期有关。因此得到机器人在任何时刻的位姿。
实施例3
本实施例详细描述基于激光雷达数据的移动机器人位姿推算方法的实施方式,具体包括以下步骤:
Step1:激光雷达数据的接收与预处理
将二维扫描型激光雷达传感器与计算机进行连接获取环境数据,并以数组形式存储于计算机中,环境数据主要表现为距离信息D={d1,d2,d3,...,di,...,dN},N常取值为500,对应激光雷达的扫描范围180°,角度分辨率ξ=0.36°。然后对距离数据进行预处理,包括去除有效范围外的数据点、滤除孤立的噪声点及对激光雷达测量机制的缺陷补偿。另外将环境数据从激光雷达的极坐标系下的数据转化为机器人或智能车辆的局部直角坐标系坐标,即将数据集合表示为:bg={P1,P2,P3,......,Pi,......PN},其中Pi表示为激光雷达第i线激光束反映的信息,即Pi=(xi,yi),(xi,yi)为第i线激光束检测到的环境在局部坐标系下的坐标。
Step2:对环境数据的分割聚类
平面ABD分割聚类算法在实时性和分割效果上均满足,本发明的要求,因此采用此算法对环境数据进行分割聚类,方法如下:
循环比较距离数据的每两个相邻点之间的差值,当大于分割阈值时,则认为找到了分割点,公式如下:
|dn-dn-1|>Dmax
其中:dn为当前扫描点的距离,dn-1为前一扫描点的距离,Dmax为分割阈值。此处Dmax描述如下
其中:rn-1为dn和dn-1取得最小值,δ为余量。λ和如下图所示,图中和为点Pn和点Pn-1相当于激光雷达极坐标的角度值。
图2为ABD算法示意图,如果点Pn和点Pn-1根据上述算法检测得出的结果都是分割点时,则暂时标记点Pn和点Pn-1为分割点,然后利用一个阈值ε(dn)进行二次检测,ε(dn)=k*dn,其中k为常系数。为了使分割点间的距离不至于太远,本发明对阈值进行限定,规定ε(dn)≤μ。若|dn-dn-1|>ε(dn),则认为点Pn和点Pn-1为分割点,反之,则不是分割点,取消之前分割的标记。其中,在本实施例中,k=0.15,μ=0.1m,δ=0.03m。
对一帧激光雷达的N=500个距离数据进行优化型ABD算法聚类分析后将得p个分割点,即将环境分成p类。并将每个分割点的角标存储在计算机内。
Step3:在采样时刻t时,寻找具有特征点的簇群C={C1,C2,…,Cn}。
寻找采样时刻t的激光雷达数据中具有顶点的簇群,如下图所示。规则为连接该簇群的起始点和终点形成直线,计算除了这两点外的其他所有点距离该直线的距离。距离直线最远的即为顶点。
图3为具有顶点的簇群示意图,基于上述选择的簇群,求出顶点坐标,记为:
将每个簇群的起始点与终点连线形成的直线,并计算每条直线的倾角,记为:
Step4:目标簇群的跟踪
每经过γ=1个采样时刻(采样周期一般取T=100ms)时,获取上述簇群集合C内每个子簇群的顶点在局部坐标系下的坐标数据,在经过m*γ个采样周期后,所有子簇群的顶点的坐标和每个子簇群起点和终点连线的倾角分别表示为:
其中,在本实施例中,m=10。
Step5:基于滑动滤波的偏转角度推算
对于第i个子簇群Ci,从第j*γ个时刻到第(j+1)*γ时刻,该簇群的变化示意图如下所示:
图4为不同时刻机器人位姿变化示意图,由图4(a)与图4(b)可知,机器人从A'运动到B'位置时,机器人偏转的角度差就是某一个簇群偏转的角度差,该角度差等于Lj*γ与L(j+1)*γ倾角的变化程度,即(θ(j+1)*γ-θj*γ)。
在此设A'位置相对于起始位置,机器人的角度为φ。对机器人位姿中的角度信息进行滑动滤波。将滑动窗口宽度设置为k*γ长度,其中k=5,经过对上述θ矩阵内n个簇群进行(m-k)=5次滑动滤波后,得到B'位置机器人相对起始位置的角度为:
对以上n个(j+1)*γ时刻的位姿偏转角度进行均值滤波后,得到在B'位置的偏转角度为:
Step6:基于滑动滤波的位置坐标推算
由图4a可以很明显看出,当机器人从A'运动到B'位置时,视野里的局部环境也发生了变化。设在A'位置时,机器人的位置坐标为(x,y),图4c将变化定量后,有几何知识可以得到:在B'位置时,机器人的坐标即为:
其中LROBOT为机器人长度,本实施例采用的机器人长度为114cm,即LROBOT=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'位置机器人相对起始位置的位置为:
对以上n个(j+1)*γ时刻的位置进行均值滤波后,得到在B'位置的位置坐标为:
Step7:初始化起点的位姿,即(x,y,θ)=(0,0,0),每经过γ=1的采样周期时,对位姿进行累积。因此得到机器人在任何时刻的位姿。
最后说明的是,以上实施例仅用以说明本发明的技术方案而非限制,尽管参照较佳实施例对本发明进行了详细说明,本领域的普通技术人员应当理解,可以对本发明的技术方案进行修改或者等同替换,而不脱离本技术方案的宗旨和范围,其均应涵盖在本发明的权利要求范围当中。
Claims (10)
1.一种基于激光雷达数据的移动机器人位姿推算方法,其特征在于:包括以下步骤:
Step1:接收与预处理激光雷达数据;
Step2:分割聚类环境数据;
Step3:从激光雷达数据中选择簇群;
Step4:目标簇群的跟踪;获取簇群中每个子簇群的顶点在局部坐标系下的坐标数据,获取簇群的顶点的坐标和每个连接连线的倾角;
Step5:将起始点和终点组成连接直线并计算簇群中连接直线的偏转角度;
Step6:通过滑动滤波计算机器人位置坐标变化后的位置坐标;
Step7:初始化起点的位姿并存储位置坐标。
2.如权利要求1所述的基于激光雷达数据的移动机器人位姿推算方法,其特征在于:所述步骤Step1中的接收与预处理激光雷达数据的具体步骤如下:
通过与计算机连接的二维扫描型激光雷达传感器获取环境数据;
将环境数据以数组形式存储于计算机中,所述环境数据包括距离信息、激光雷达扫描范围和角度分辨率;
预处理距离数据,包括去除有效范围外的数据点、滤除孤立的噪声点及对激光雷达测量机制的缺陷补偿;
将环境数据从激光雷达的极坐标系下的数据转化为机器人或智能车辆的局部直角坐标系坐标。
3.如权利要求1所述的基于激光雷达数据的移动机器人位姿推算方法,其特征在于:所述步骤Step2中的按照以下步骤来分割聚类环境数据:
循环比较距离数据的每两个相邻数据点差值,标记差值大于分割阈值的数据点为分割点;
存储分割点的角标。
4.如权利要求1所述的基于激光雷达数据的移动机器人位姿推算方法,其特征在于:所述步骤Step3中从激光雷达数据中选择簇群的具体步骤如下:
计算簇群中相对应起始点和终点连接直线;
计算簇群中数据点与连接直线的距离;
获取距离直线最远的数据点为顶点;
计算顶点坐标;
计算连接直线的倾角。
5.如权利要求1所述的基于激光雷达数据的移动机器人位姿推算方法,其特征在于:所述步骤Step5中的连接直线及偏转角度按照以下步骤来实现:
获取机器人相对于起始位置的A'位置坐标和角度信息φ;
对机器人的角度信息进行滑动滤波;
对簇群的角度信息进行滑动滤波;
得到B'位置机器人相对起始位置的角度信息;
通过对角度信息进行均值滤波得到机器人在B'位置的偏转角度:
其中,θB ′表示机器人在B'位置的偏转角度;n表示一帧环境数据中经过分割算法分割出来的簇群总数;i表示n个簇群中第i个子簇群;表示第i个子簇群在j*γ时刻的位姿偏转角度;表示第i个子簇群在(j+1)*γ时刻的位姿偏转角度;φ表示机器人在A'位置的偏转角度。
6.如权利要求1所述的基于激光雷达数据的移动机器人位姿推算方法,其特征在于:所述步骤Step6中机器人位置坐标变化后的位置坐标按照以下步骤来计算:
获取机器人相对应起始点的A'位置坐标(x,y);
通过以下公式来计算机器人在B'位置的坐标:
其中,LROBOT为机器人长度,xB ′表示机器人在B'位置的横坐标;yB ′表示机器人在B'位置的纵坐标;||CF||表示机器人质心从A'位置到B'位置的纵坐标距离;||BC||表示机器人质心从A'位置到B'位置的横坐标距离。
7.一种基于激光雷达数据的移动机器人位姿推算系统,其特征在于:包括预处理模块、分割聚类模块、簇群选择模块、簇群跟踪模块、偏转角度计算模块、位置坐标计算模块和存储模块;
所述预处理模块,用于接收与预处理激光雷达数据;
所述分割聚类模块,用于分割聚类环境数据;
所述簇群选择模块,用于从激光雷达数据中选择簇群;
所述簇群跟踪模块,用于获取簇群中每个子簇群的顶点在局部坐标系下的坐标数据,获取簇群的顶点的坐标和每个连接连线的倾角;
所述偏转角度计算模块,用于将起始点和终点组成连接直线并计算簇群中连接直线的偏转角度;
所述位置坐标计算模块,用于通过滑动滤波计算机器人位置坐标变化后的位置坐标;
所述存储模块,用于初始化起点的位姿并存储位置坐标。
8.如权利要求7所述的基于激光雷达数据的移动机器人位姿推算系统,其特征在于:所述预处理模块中的接收与预处理激光雷达数据的具体步骤如下:
通过与计算机连接的二维扫描型激光雷达传感器获取环境数据;
将环境数据以数组形式存储于计算机中,所述环境数据包括距离信息、激光雷达扫描范围和角度分辨率;
预处理距离数据,包括去除有效范围外的数据点、滤除孤立的噪声点及对激光雷达测量机制的缺陷补偿;
将环境数据从激光雷达的极坐标系下的数据转化为机器人或智能车辆的局部直角坐标系坐标。
9.如权利要求7所述的基于激光雷达数据的移动机器人位姿推算系统,其特征在于:所述簇群选择模块中从激光雷达数据中选择簇群的具体步骤如下:
计算簇群中相对应起始点和终点连接直线;
计算簇群中数据点与连接直线的距离;
获取距离直线最远的数据点为顶点;
计算顶点坐标;
计算连接直线的倾角。
所述偏转角度计算模块中的连接直线及偏转角度按照以下步骤来实现:
获取机器人相对于起始位置的A'位置坐标和角度信息φ;
对机器人的角度信息进行滑动滤波;
对簇群的角度信息进行滑动滤波;
得到B'位置机器人相对起始位置的角度信息;
通过对角度信息进行均值滤波得到机器人在B'位置的偏转角度:
其中,θB ′表示机器人在B'位置的偏转角度;n表示一帧环境数据中经过分割算法分割出来的簇群总数;i表示n个簇群中第i个子簇群;表示第i个子簇群在j*γ时刻的位姿偏转角度;表示第i个子簇群在(j+1)*γ时刻的位姿偏转角度;φ表示机器人在A'位置的偏转角度。
10.如权利要求7所述的基于激光雷达数据的移动机器人位姿推算系统,其特征在于:所述位置坐标计算模块中机器人位置坐标变化后的位置坐标按照以下步骤来计算:
获取机器人相对应起始点的A'位置坐标(x,y);
通过以下公式来计算机器人在B'位置的坐标:
其中,LROBOT为机器人长度,xB ′表示机器人在B'位置的横坐标;yB ′表示机器人在B'位置的纵坐标;||CF||表示机器人质心从A'位置到B'位置的纵坐标距离;||BC||表示机器人质心从A'位置到B'位置的横坐标距离。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201610213132.1A CN105867373B (zh) | 2016-04-07 | 2016-04-07 | 一种基于激光雷达数据的移动机器人位姿推算方法及系统 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201610213132.1A CN105867373B (zh) | 2016-04-07 | 2016-04-07 | 一种基于激光雷达数据的移动机器人位姿推算方法及系统 |
Publications (2)
Publication Number | Publication Date |
---|---|
CN105867373A true CN105867373A (zh) | 2016-08-17 |
CN105867373B CN105867373B (zh) | 2018-09-11 |
Family
ID=56636175
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN201610213132.1A Active CN105867373B (zh) | 2016-04-07 | 2016-04-07 | 一种基于激光雷达数据的移动机器人位姿推算方法及系统 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN105867373B (zh) |
Cited By (9)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN109633666A (zh) * | 2019-01-18 | 2019-04-16 | 广州高新兴机器人有限公司 | 室内动态环境下基于激光雷达的定位方法及计算机存储介质 |
CN109765569A (zh) * | 2017-11-09 | 2019-05-17 | 电子科技大学中山学院 | 一种基于激光雷达实现虚拟航迹推算传感器的方法 |
CN110197104A (zh) * | 2018-02-27 | 2019-09-03 | 杭州海康威视数字技术股份有限公司 | 基于车辆的测距方法及装置 |
CN111338338A (zh) * | 2020-02-20 | 2020-06-26 | 山东科技大学 | 基于路面特征聚类分析的机器人速度自适应控制方法 |
CN111344647A (zh) * | 2017-09-15 | 2020-06-26 | 艾耶股份有限公司 | 具有低延时运动规划更新的智能激光雷达系统 |
CN111457904A (zh) * | 2020-03-20 | 2020-07-28 | 力信测量(上海)有限公司 | 一种用于双护盾tbm视觉测量系统 |
CN111578894A (zh) * | 2020-06-02 | 2020-08-25 | 北京经纬恒润科技有限公司 | 一种确定障碍物航向角的方法及装置 |
CN112799055A (zh) * | 2020-12-28 | 2021-05-14 | 深圳承泰科技有限公司 | 一种探测被测车辆的方法、装置以及电子设备 |
CN114575289A (zh) * | 2022-05-05 | 2022-06-03 | 深圳市旗扬特种装备技术工程有限公司 | 一种潮汐车道隔离水马移动控制方法、装置及移动载具 |
Citations (3)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN102359783A (zh) * | 2011-07-22 | 2012-02-22 | 北京大学 | 基于视觉的移动机器人定位方法 |
CN104811683A (zh) * | 2014-01-24 | 2015-07-29 | 三星泰科威株式会社 | 用于估计位置的方法和设备 |
US9129211B2 (en) * | 2012-03-15 | 2015-09-08 | GM Global Technology Operations LLC | Bayesian network to track objects using scan points using multiple LiDAR sensors |
-
2016
- 2016-04-07 CN CN201610213132.1A patent/CN105867373B/zh active Active
Patent Citations (3)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN102359783A (zh) * | 2011-07-22 | 2012-02-22 | 北京大学 | 基于视觉的移动机器人定位方法 |
US9129211B2 (en) * | 2012-03-15 | 2015-09-08 | GM Global Technology Operations LLC | Bayesian network to track objects using scan points using multiple LiDAR sensors |
CN104811683A (zh) * | 2014-01-24 | 2015-07-29 | 三星泰科威株式会社 | 用于估计位置的方法和设备 |
Cited By (13)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN111344647A (zh) * | 2017-09-15 | 2020-06-26 | 艾耶股份有限公司 | 具有低延时运动规划更新的智能激光雷达系统 |
CN109765569A (zh) * | 2017-11-09 | 2019-05-17 | 电子科技大学中山学院 | 一种基于激光雷达实现虚拟航迹推算传感器的方法 |
CN110197104B (zh) * | 2018-02-27 | 2022-03-29 | 杭州海康威视数字技术股份有限公司 | 基于车辆的测距方法及装置 |
CN110197104A (zh) * | 2018-02-27 | 2019-09-03 | 杭州海康威视数字技术股份有限公司 | 基于车辆的测距方法及装置 |
CN109633666A (zh) * | 2019-01-18 | 2019-04-16 | 广州高新兴机器人有限公司 | 室内动态环境下基于激光雷达的定位方法及计算机存储介质 |
CN109633666B (zh) * | 2019-01-18 | 2021-02-02 | 广州高新兴机器人有限公司 | 室内动态环境下基于激光雷达的定位方法及计算机存储介质 |
CN111338338A (zh) * | 2020-02-20 | 2020-06-26 | 山东科技大学 | 基于路面特征聚类分析的机器人速度自适应控制方法 |
CN111338338B (zh) * | 2020-02-20 | 2024-01-16 | 山东科技大学 | 基于路面特征聚类分析的机器人速度自适应控制方法 |
CN111457904A (zh) * | 2020-03-20 | 2020-07-28 | 力信测量(上海)有限公司 | 一种用于双护盾tbm视觉测量系统 |
CN111578894A (zh) * | 2020-06-02 | 2020-08-25 | 北京经纬恒润科技有限公司 | 一种确定障碍物航向角的方法及装置 |
CN111578894B (zh) * | 2020-06-02 | 2021-10-15 | 北京经纬恒润科技股份有限公司 | 一种确定障碍物航向角的方法及装置 |
CN112799055A (zh) * | 2020-12-28 | 2021-05-14 | 深圳承泰科技有限公司 | 一种探测被测车辆的方法、装置以及电子设备 |
CN114575289A (zh) * | 2022-05-05 | 2022-06-03 | 深圳市旗扬特种装备技术工程有限公司 | 一种潮汐车道隔离水马移动控制方法、装置及移动载具 |
Also Published As
Publication number | Publication date |
---|---|
CN105867373B (zh) | 2018-09-11 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN105867373A (zh) | 一种基于激光雷达数据的移动机器人位姿推算方法及系统 | |
CN112083725B (zh) | 一种面向自动驾驶车辆的结构共用多传感器融合定位系统 | |
CN105866790B (zh) | 一种考虑激光发射强度的激光雷达障碍物识别方法及系统 | |
CN112083726B (zh) | 一种面向园区自动驾驶的双滤波器融合定位系统 | |
CN102538781B (zh) | 基于机器视觉和惯导融合的移动机器人运动姿态估计方法 | |
CN111426320B (zh) | 一种基于图像匹配/惯导/里程计的车辆自主导航方法 | |
Barjenbruch et al. | Joint spatial-and Doppler-based ego-motion estimation for automotive radars | |
CN108362288B (zh) | 一种基于无迹卡尔曼滤波的偏振光slam方法 | |
CN105573318A (zh) | 基于概率分析的环境构建方法 | |
CN114526745A (zh) | 一种紧耦合激光雷达和惯性里程计的建图方法及系统 | |
CN112923931A (zh) | 一种基于固定路线下的特征地图匹配与gps定位信息融合方法 | |
Han et al. | Robust ego-motion estimation and map matching technique for autonomous vehicle localization with high definition digital map | |
CN113252051A (zh) | 一种地图构建方法及装置 | |
CN111402328A (zh) | 一种基于激光里程计的位姿计算方法及装置 | |
CN115930977A (zh) | 特征退化场景的定位方法、系统、电子设备和可读存介质 | |
CN116642482A (zh) | 基于固态激光雷达和惯性导航的定位方法、设备和介质 | |
CN111123953A (zh) | 人工智能大数据下粒子化移动机器人组及其控制方法 | |
CN109387198A (zh) | 一种基于序贯检测的惯性/视觉里程计组合导航方法 | |
KR102618247B1 (ko) | 자율주행 차량의 측위 헤딩오차 보정 장치 및 그의동작 방법 | |
CN111208820B (zh) | 人工智能大数据下粒子化无人车组、控制方法及介质 | |
CN117075158A (zh) | 基于激光雷达的无人变形运动平台的位姿估计方法及系统 | |
CN117029870A (zh) | 一种基于路面点云的激光里程计 | |
CN112747752B (zh) | 基于激光里程计的车辆定位方法、装置、设备和存储介质 | |
CN113554705B (zh) | 一种变化场景下的激光雷达鲁棒定位方法 | |
CN110749327B (zh) | 一种合作环境下的车辆导航方法 |
Legal Events
Date | Code | Title | Description |
---|---|---|---|
C06 | Publication | ||
PB01 | Publication | ||
C10 | Entry into substantive examination | ||
SE01 | Entry into force of request for substantive examination | ||
GR01 | Patent grant | ||
GR01 | Patent grant | ||
TR01 | Transfer of patent right |
Effective date of registration: 20230208 Address after: 213399 room 5025, building B, 218 Hongkou Road, Kunlun Street, Liyang City, Changzhou City, Jiangsu Province Patentee after: Liyang Smart City Research Institute of Chongqing University Address before: 400044 No. 174 Sha Jie street, Shapingba District, Chongqing Patentee before: Chongqing University |
|
TR01 | Transfer of patent right |