CN103412565B - 一种具有全局位置快速估计能力的机器人定位方法 - Google Patents

Abstract

本发明提供了一种具有全局位置快速估计能力的机器人，其包括一机器人平台、运动传感器以及3D扫描装置；其定位过程为：事先对环境空间进行3D扫描得到完整的全局地图，将环境空间栅格化，计算每个栅格到与该栅格最近的障碍物点的距离值；启动所述3D扫描装置完成一帧3D扫描帧，同时采用粒子滤波框架模拟一定量的粒子按照所述机器人平台的动力学模型运动，将3D扫描帧按照各粒子的位置和姿态对环境地图进行模拟扫描得到各粒子的激光扫描点，利用栅格图计算得到激光扫描点与机器人激光扫描点最近的粒子，该粒子位置即为机器人全局位置最终的估计结果。本发明具有抗干扰性强，成本低且精度高等优点。

Description

一种具有全局位置快速估计能力的机器人定位方法

技术领域

本发明主要应用于室内外机器人平台的自主运动控制领域，尤其是一种通过3D扫描技术以及三维距离变换完成对机器人全局位置的估计技术。

背景技术

随着机器人技术的发展，其人被广泛的运用在各个领域，机器人通过对自身全局位置的估计，实现对机器人的自主导航、实现目标任务，以及其他用到全局位置作为反馈的控制系统。

实现机器人定位的方法很多，包括广泛应用的GPS定位，但此方法目前仅适用于室外环境，对干扰敏感且精度不高；里程计可以实现轮式平台的二维定位，惯性定位技术依靠与物质惯性有关的物理原理实现相对定位，但由于相对定位在原理上误差累积，因此长时间误差发散，此外高精度的惯性器件成本较高；全局位置估计还可利用2D激光扫描实现，但是如果在2D平面中的环境特征不明显（相似，单一）的情况下，其估计结果误差大。

发明内容

为了解决上述问题，本发明提供了一种具有全局位置快速估计能力的机器人，其包括一机器人平台、运动传感器以及3D扫描装置，所述机器人平台携带所述运动传感器以及所述3D扫描装置并移动，所述3D扫描装置用于对环境轮廓进行3D点云数据扫描，所述运动传感器测量所述机器人平台的具体位置。

较佳地，所述3D扫描装置包括2D激光扫描传感器、转台以及码盘，所述2D激光扫描传感器安装在所述转台上，所述2D激光扫描传感器用于逐帧扫描可测量半圆周内传感器中心到环境障碍物的距离，所述转台沿激光扫描平面垂直的方向旋转，所述码盘测量转台所在位置相对转角零位的转角。

较佳地，所述运动传感器包括里程计、加速度计和陀螺仪，所述里程计用于测量所述机器人平台的位置增量，所述加速度计用于测量机器人平台的加速度，所述陀螺仪用于测量机器人平台的航向角。

本发明还提供了一种包括以下步骤：

事先对环境空间进行3D扫描得到完整的全局地图，所述全局地图上的每个点代表一个障碍物点；

将环境空间以一定尺度栅格化为的栅格空间，计算每个栅格到与该栅格最近的障碍物点的距离值；

运行所述机器人平台，同时启动所述3D扫描装置完成一帧3D扫描帧；

采用粒子滤波框架，模拟一定量的粒子按照所述机器人平台的动力学模型运动；

将所述3D扫描帧按照所述各粒子的位置和姿态投影到栅格图中，计算各粒子的激光扫描点与机器人的激光扫描点位置之间的距离，得到激光扫描点与机器人激光扫描点最近的粒子，其位置即为机器人全局位置最终的估计结果；

重新获得粒子群对机器人获得的下一个扫描帧的位置进行估计。

较佳地，计算栅格与其最近障碍物点距离的过程采用了欧式距离变换以及基于最优KD-Tree的最近邻点的查找。

较佳地，所述机器人运动的动力学模型根据所述运动传感器测得。

较佳地，所述粒子模拟所述运动传感器所测得的机器人运动数据以及所述运动传感器的测量误差。

较佳地，激光扫描点与机器人激光扫描点最近的粒子为一个或多个。

较佳地，所述3D扫描装置的扫描过程为：转台获取逐帧2D激光扫描数据及对应的转台码盘测角，通过旋转坐标系法则将逐帧2D测量折算成一帧3D扫描结果。

本发明具有以下有益效果：

（1）相对于其他的全局定位方案，本发明所述方案几乎适用于任复杂环境，且抗环境干扰能力强；

（2）本发明提供的定位装置结构简单，算法巧妙的利用了三维距离变换技术，大大提高了后续运行过程中的计算效率，该算法对于部分遮挡等环境异常也有很好的鲁棒性；

（3）在同等位置估计精度条件下，本方案硬件成本远低于采用GPS方案或惯性导航方案的成本；在同等硬件成本条件下，本方案的位置估计精度由于其他方案。

当然，实施本发明的任一产品并不一定需要同时达到以上所述的所有优点。

附图说明

图1为本发明提供的机器人的全局位置快速估计方法流程图;

图2为本发明实施例提供的通过旋转坐标系法则将逐帧2D测量折算成一帧3D扫描原理图。

具体实施方式

下面结合附图说明本发明的优选实施例。

为解决现有技术所存在的缺陷，本发明提供了一种具有全局位置快速估计能力的机器人，其包括一机器人平台、运动传感器以及3D扫描装置，所述机器人平台携带所述运动传感器以及所述3D扫描装置并移动，所述3D扫描装置用于对环境轮廓进行3D点云数据扫描，所述运动传感器测量所述机器人平台的具体位置。

本发明实施例所采用的3D扫描装置包括2D激光扫描传感器、转台以及码盘，所述2D激光扫描传感器安装在所述转台上，所述2D激光扫描传感器用于逐帧扫描可测量半圆周内传感器中心到环境障碍物的距离，所述转台沿激光扫描平面垂直的方向旋转，所述码盘测量转台所在位置相对转角零位的转角。其还包括一滑环，所述滑环激光扫描器供电及数据通过滑环与外部连接。

本实施例采用的运动传感器包括里程计、加速度计及陀螺仪等测量装置，其中所述里程计用于测量所述机器人平台的位置增量，所述加速度计用于测量机器人平台的加速度，所述陀螺仪用于测量机器人平台的航向角。本发明还可以采用其他位置测量装置，并不局限于本实施例。根据运动传感器可以测量出机器人的具体位置，同时也可以计算得到机器人的运动动力学模型。

如图1所示，本发明还提供了机器人的全局位置快速估计方法，其包括以下步骤：

事先对环境空间进行3D扫描得到完整的全局地图，所述全局地图上的每个点代表一个障碍物点；

将环境空间以一定尺度栅格化为的栅格空间，计算每个栅格到与该栅格最近的障碍物点的距离值；

运行所述机器人平台，同时启动所述3D扫描装置完成一帧3D扫描帧；所述3D扫描装置的扫描过程为：转台获取逐帧2D激光扫描数据及对应的转台码盘测角，通过旋转坐标系法则将逐帧2D测量折算成一帧3D扫描结果，如图2所示，每帧2D测量为若干组角度/距离[γ_i,d_i]|_i=1,..,n,γ∈[-π/2,π/2]数据对，转台码盘测角为α_j∈[-π,π]，折算后点的3D坐标[x_k,y_k,z_k]按以下公式计算

x_(i,j)=d·cosγ_i·cosα_j

y_(i,j)=d·cosγ_i·sinα_j

z(i,j)=d·sinγ_i；

采用粒子滤波框架，模拟一定量的粒子按照所述机器人平台的动力学模型运动；本例采用的粒子滤波（ParticleFilter）框架，按照机器人平台动力学模型及传感器噪声模型仿真一定数量的粒子，每个粒子由三元组表示其全局位置坐标及朝向；

将所述3D扫描帧按照所述各粒子的位置和姿态投影到栅格图中，计算各粒子的激光扫描点与机器人的激光扫描点位置之间的距离，得到激光扫描点与机器人激光扫描点最近的粒子，其位置即为机器人全局位置最终的估计结果；激离栅格图中

Z_k=z_k

由于预先计算并存储了距离数据，可以实现快速的查找各个激光扫描点在环境地图中的距离值即到最近障碍物的距离，计算得到激光扫描点与机器人激光扫描点最近的粒子。该过程称为距离评价。

当机器人获得另一扫描帧并需要进行定位时，粒子滤波框架重新获得粒子群对机器人获得的下一个扫描帧的位置进行估计，具体步骤和获得上一帧机器人所在位置的定位方法一致。重新采样获得粒子群，有利于保证粒子群的健壮性，使得定位结果更加准确。

其中，环境地图由事先激光扫描，借助运动传感器测得的数据，将多帧激光拼接成完整的全局地图，其中每个激光扫描点代表该位置有障碍物；

本方法中，采用欧式距离变换（EDT）表示距离，三维空间中两点之间欧氏

距离表示为

$d=\sqrt{{(x1-x2)}^2+{(y1-y2)}^2+{(z1-z2)}^2},$

将特定三维空间以一定尺度栅格化为M×N×P的栅格空间,计算每个栅格点到最近障碍物点的距离值，一般采用基于最优KD-Tree来进行最近邻点的查找获得栅格点与其最近的障碍物点。

由于运动传感器在测量时具有一定的误差，根据运动传感器得到的动力学模型也与机器人实际运动模型具有一定的误差，粒子在模拟机器人按其动力学模型进行运动时，粒子同时模拟运动传感器所得到的动力学模型以及所述运动传感器的测量误差。具体误差可以表示为：

其中Δt为更新的时间增量；η_Δ为里程计速度量化误差，ε_od为里程高斯噪声，ε_b为陀螺仪高斯噪声，此处传感器误差可以用更复杂/精确的模型替代。

相对于其他的全局定位方案，本发明所述方案几乎适用于任意复杂环境，且抗环境干扰能力强；本发明提供的定位装置结构简单，算法巧妙的利用了三维距离变换技术，大大提高了后续运行过程中的计算效率，该算法对于部分遮挡等环境异常也有很好的鲁棒性；在同等位置估计精度条件下，本方案硬件成本远低于采用GPS方案或惯性导航方案的成本；在同等硬件成本条件下，本方案的位置估计精度由于其他方案。

以上公开的本发明优选实施例只是用于帮助阐述本发明。优选实施例并没有详尽叙述所有的细节，也不限制该发明仅为所述的具体实施方式。显然，根据本说明书的内容，可作很多的修改和变化。本说明书选取并具体描述这些实施例，是为了更好地解释本发明的原理和实际应用，从而使所属技术领域技术人员能很好地理解和利用本发明。本发明仅受权利要求书及其全部范围和等效物的限制。

Claims (6)

1.一种机器人全局位置快速估计方法，所述机器人包括一机器人平台、运动传感器以及3D扫描装置，所述机器人平台携带所述运动传感器以及所述3D扫描装置并移动，所述3D扫描装置用于对环境轮廓进行3D点云数据扫描，所述运动传感器测量所述机器人平台的具体位置；其特征在于，包括以下步骤：

事先对环境空间进行3D扫描得到完整的全局地图，所述全局地图上的每个点代表一个障碍物点；

将环境空间以一定尺度栅格化为M×N×P的栅格空间，计算每个栅格到与该栅格最近的障碍物点的距离值；

运行所述机器人平台，同时启动所述3D扫描装置完成一帧3D扫描帧；

采用粒子滤波框架，模拟一定量的粒子按照所述机器人平台的动力学模型运动；

将所述3D扫描帧按照各粒子的位置和姿态投影到栅格图中，计算各粒子的激光扫描点与机器人的激光扫描点位置之间的距离，得到激光扫描点与机器人激光扫描点最近的粒子，其位置即为机器人全局位置最终的估计结果；

重新获得粒子群对机器人获得的下一个扫描帧的位置进行估计。

2.如权利要求1所述的机器人全局位置快速估计方法，其特征在于，计算栅格与其最近障碍物点距离的过程采用了欧式距离变换以及基于最优KD-Tree的最近邻点的查找。

3.如权利要求1所述的机器人全局位置快速估计方法，其特征在于，所述机器人运动的动力学模型根据所述运动传感器测得。

4.如权利要求3所述的机器人全局位置快速估计方法，其特征在于，所述粒子模拟所述运动传感器所测得的机器人运动数据以及所述运动传感器的测量误差。

5.如权利要求1所述的机器人全局位置快速估计方法，其特征在于，激光扫描点与机器人激光扫描点最近的粒子为一个或多个。

6.如权利要求1所述的机器人全局位置快速估计方法，其特征在于，所述3D扫描装置的扫描过程为：转台获取逐帧2D激光扫描数据及对应的转台码盘测角，通过旋转坐标系法则将逐帧2D测量折算成一帧3D扫描结果。