CN103400416A - 一种基于概率多层地形的城市环境机器人导航方法 - Google Patents

Abstract

本发明公开了一种基于概率多层地形的城市环境机器人导航方法，首先通过安装在云台上的低成本激光传感器对地形进行扫描、滤波和概率变换以获得全局坐标下的概率化点云表示；其次通过网格化地形划分方法，把点云数据关联到相应的若干网格之中；然后对网格内关联的点云数据进行概率融合，形成可表示多层次地形的堆数据，完成地形的建模过程；最后对得到的多层概率地形进行穿越性分析以形成安全有效的机器人行走路径。本发明在保证实时性的前提下，通过概率化处理和引入多层堆结构实现了对城市地形环境的精确建模和导航，对于城市立体化导航服务、侦察救援、GIS系统等相关领域均具有非常重要的经济价值和应用前景。

Description

一种基于概率多层地形的城市环境机器人导航方法

技术领域

本发明涉及一种基于概率多层地形的城市环境机器人导航方法，特别是大尺度城市化环境移动机器人的地形精确建模和导航方法，属于城市立体化导航服务、地面或空中无人自主平台技术、地理信息系统（GIS）等应用领域。

背景技术

随着无人自主驾驶、军事侦察和救援、无人机、城市立体化导航服务等应用领域的日益扩展，针对于特定城市环境中移动机器人的地形建模和导航能力的相关应用研究正日益成为业界的热点问题，并逐渐产生了较为广泛的经济需求。这些需求不同于现有的室内移动机器人建模方法，对于非结构化环境的精确表达和实时导航能力提出了更高的要求，其安全性和效率指标更难得到满足。

目前常用的室外三维环境感知方法多采用声纳、激光、单目视觉、立体视觉等方式解决，其中二维扫描激光由于其低成本、高精度、处理快等突出特点受到业界的广泛关注和青睐，围绕其开发的地形建模方法主要包括点云图、voxel图和高程地图等。其中点云图直接采用原始地形测量信息表示，方法简明直接，但只适用于稀疏特征地形表示，且不便于后期导航处理；voxel图在点云图的基础上作了一定的压缩处理操作，可产生致密地形表示，但地形的创建和更新处理速度将随环境尺度的增大而急剧下降；数字高程地图来自地理信息领域，通过二维网格划分对原始地形数据进行融合处理以降低计算复杂度，但相应地形表达方式较粗糙，所能反映的地形特征极为有限，无法用于复杂的城市环境。

总的来说，上述常用的室外地形环境建模方法在应用与城市化环境导航中存在以下问题：1）未考虑激光传感器测量过程中的各种不确定性因素影响，因而导致所建立的地形模型和相应导航方法缺乏容错性，易受环境干扰，只能针对特定应用实施；2）所采用的地形环境过于简单，无法深刻反映现实环境的复杂性和多样性，特别是无法对城市环境中所存在的各种垂直结构（墙体等）或多层次结构（例如树、桥梁、山洞等）进行精确描述，大范围导航性能受到制约，甚至无法找到正确有效的导航路径。

发明内容

本发明的目的是为了克服已有的技术缺陷，解决特定城市化环境中移动机器人的建模和自主导航方法所存在的易受各种不确定性干扰影响、对城市环境的表达不充分等缺陷导致的相应自主导航能力弱、缺乏容错性和鲁棒性的问题，提出了一种基于概率多层地形的城市环境机器人导航方法，实现城市地形环境的安全可靠导航。

本发明采用的技术方案为：一种基于概率多层地形的城市环境机器人导航方法，首先通过安装在云台上的低成本激光传感器对地形进行扫描、滤波和概率变换以获得全局坐标下的概率化点云表示。其次通过网格化地形划分方法，把点云数据关联到相应的若干网格之中。然后对网格内关联的点云数据进行概率融合，形成可表示多层次地形的堆数据，完成地形的建模过程。最后对得到的多层概率地形进行穿越性分析以形成安全有效的机器人行走路径。

（1）三维地形信息的获取和概率化点云的生成：首先通过安装在激光云台上的二维激光传感器获得机器人周围环境的测量信息；考虑到激光测量数据、里程计和GPS测量数据的误差，把激光的测量数据、里程计和GPS测量数据进行高斯模型概率化；然后采用卡尔曼滤波的方法对里程计和GPS数据进行融合，以获得机器人当前位置的全局坐标；最后通过结合机器人当前的全局坐标，通过坐标变换得到测量值在全局坐标下的概率化点云表示。

（2）地形网格的划分和相应点云数据的关联：首先把地面划分成网格；然后对已经得到的测量值在全局坐标下的概率表示，采用高斯分布的3σ值作为不确定性边界关联到多个网格中并且计算出测量值属于这个网格的概率大小。

（3）多层地形堆结构的融合和更新：每个网格上可能关联多个测量值，首先计算每个测量值在网格上的高度的高斯概率分布，其次把测量值分配到堆上，通过判断已有的堆与刚分配堆之间的垂直距离大小来进行融合。

（4）地形可穿越性分析和导航路径生成：在构建出多层地形的基础上，对每层的地形进行平坦性分析，考虑到机器人自身机械结构特征和其穿越能力分割出可行进区域与障碍区域，最后得到机器人的行走路径。

有益效果：本发明采用低成本的激光扫描装置有效地解决了特定城市化环境中大尺度地形环境的表示和导航问题，通过概率化多层地形模型的创建和相应穿越性能的分析，提高了地形建模的精度，保证了整体算法在实际应用中的实时性。此方法简单高效，能够满足移动机器人在城市立体化导航、军事侦察和救援、无人机、GIS地理信息系统等领域的广泛需求，具有广阔的应用前景和良好的经济效益。

附图说明

图1为：算法总体流程图；

图2为：三维地形数据获取流程图；

图3为：激光扫描感知示意图；

图4为：数据关联到网格流程图；

图5为：高斯椭球投影示意图；

图6为：多层结构堆示意图；

图7为：堆的处理和融合流程图；

图8-11为：堆融合示意图；

具体实施方式

下面结合附图和具体实施方式对本发明做进一步说明。

图1为本专利所提出的基于概率多层地形的城市环境机器人导航方法的总体流程图，算法的具体步骤如下：

1、三维地形信息的获取和概率化点云的生成

环境三维数据的获取是进行后续地形建模的前提，首先通过二维激光传感器获得周围环境的距离信息；其次把激光的测量数据、里程计和GPS数据通过高斯模型进行概率化；然后采用卡尔曼滤波的方法，利用里程计和GPS数据对机器人进行融合定位，以获得机器人当前位置的全局坐标；最后通过结合机器人当前的全局坐标，通过坐标变换得到测量值在全局坐标下的概率化点云表示。算法的流程图如图2所示，具体步骤包括：

第一步：获得距离信息。由安装在激光云台上的二维激光传感器，通过激光云台的俯仰、旋转获得地形感知的原始距离数据。

第二步：激光数据的高斯概率化。由于激光传感器本身的测量误差，为提高地形建模的精度，对这些测量信息用高斯模型建模。假设激光传感器得到的距离信息为r， $\hat{r}=r+N(0,{\mathrm{\σ}}_r^2),$ 其中

表示方差，

表示真实值。

第三步：里程计和GPS测量数据的概率化。由于里程计和GPS测量数据的误差的存在，对这些测量信息也用高斯模型建模。里程计和GPS得到的测量信息为x,y,θ，L,B,H，记为p，

,其中表示真实值。

第四步：机器人的定位。采用里程计和GPS进行Kalman滤波组合定位。GPS数据消除里程计定位的累计误差。机器人存在平动速度v和转动速度ω，其位姿状态量为p_r=(x,y,θ)^T，则可建立其离散的运动学模型为：

$p_{r,k+1}=f\left(p_{r,k}\right)=\left(\begin{array}{c}{x}_k+v_k\mathrm{\Δ}t\cos {\mathrm{\θ}}_k\\ y_k+v_k\mathrm{\Δ}t\sin {\mathrm{\θ}}_k\\ {\mathrm{\θ}}_k+{\mathrm{\ω}}_k\mathrm{\Δt}\end{array}\right)---\left(1\right)$

第五步：转化成三维坐标。如图3所示，测量数据转化成三维坐标涉及到多个坐标变换。首先求得测量数据在X₁O₁Y₁中的坐标(x₁,y₁)，然后将(x₁,y₁)变换到O_RX_RY_RZ_R坐标中(x_R,y_R,z_R)，最后利用里程计和GPS得到的机器人在全局坐标中的坐标，最终得到测量点的全局坐标(x_w,y_w,z_w)用x表示，转化过程记为：

$x=f(\hat{p},\hat{r})---\left(2\right)$

第六步：计算测量值的概率化点云表示。对上式（1）进行线性化可得：

$x\≈f(p,r)+J_{\hat{p}}(p,r)\mathrm{\Δp}+J_{\hat{r}}(p,r)\mathrm{\Δr}---\left(3\right)$

其中和

表示f相对于

的雅克比矩阵。由此测量点的三维坐标的概率表示近似为高斯分布

$(\hat{x},P)=(f(p,r),J_{\hat{p}}(p,r)Q_p{J}_{\hat{p}}^T(p,r)+J_{\hat{r}}(p,r)Q_r{J}_{\hat{r}}^T(p,r))---\left(4\right)$

其中Q_P和Q_r分别表示p和r的均方差矩阵。

2、地形网格的划分和相应点云数据的关联

首先把地面划分成网格；然后对已经得到的测量值在全局坐标下的概率表示，采用高斯分布的3σ值作为不确定性边界关联到多个网格中并且计算出测量值属于这个网格的概率大小。算法的流程图见图4，具体步骤包括：

第一步：划分网格。将地面划分成大小为cell_size的方格，每个网格用中心点坐标表示即为(x_i,y_i)。划分网格时要综合考虑路径规划的精度、实时性等要求。网格的划分不宜太大，否则影响精度，而如果划分太小又会影响实时性。

第二步：计算高斯椭球在平面上的投影。计算(x_w,y_w)的联合概率分布，(x_w,y_w)直接从(x_w,y_w,z_w)中得到，可以从P中取左上角的2×2维矩阵得到。

第三步：确定x,y方向的边界，取3σ作为其投影的边界。 $x\∈({\hat{x}}_w-3{\mathrm{\σ}}_x,{\hat{x}}_w+3{\mathrm{\σ}}_x),y\∈({\hat{y}}_w-3{\mathrm{\σ}}_y,{\hat{y}}_w+3{\mathrm{\σ}}_y),$ 如图5所示。

第四步：根据边界确定关联的网格。关联的网格用集合S表示。

$S=\left\{(x_i,y_i)\right|{\hat{x}}_w-3{\mathrm{\σ}}_x\≤x_i\≤{\hat{x}}_w+3{\mathrm{\σ}}_x,{\hat{y}}_w-3{\mathrm{\σ}}_y\≤y_i\≤{\hat{y}}_w+3{\mathrm{\σ}}_y\}---\left(5\right)$

第五步：针对每一个网格计算关联概率。计算概率用黎曼和（Riemann Sum）近似，即所求的概率p为网格的面积乘以椭球面在网格中心处投影高度。

3、多层地形堆结构的融合和更新

多层结构的堆示意图如图6所示，每个网格上可能关联多个测量值，首先计算每个测量值在网格上的高度的高斯概率分布，其次把测量值分配到堆上，然后通过判断已有的堆与刚分配堆之间的垂直距离大小来进行融合。算法的流程图如图7所示，具体步骤包括：

第一步：计算测量值高度的概率分布。每个测量值属于不同网格的概率，其表述为

那么这个测量数据在某个网格上的高度的概率分布计算实际上是一个条件分布，即已知(x_w,y_w)属于某个网格，求h的分布。所求的h的分布为

计算式如下：

$\hat{h}=E\left[{\hat{z}}_w\right|x_w,y_w]={\hat{z}}_w+P{\left(P^{x_w{y}_w}\right)}^{-1}[\frac{1}{2}\left(\begin{array}{c}{x}_i+x_{i-1}\\ y_i+y_{i-1}\end{array}\right)-\left(\begin{array}{c}{\hat{x}}_w\\ {\hat{y}}_w\end{array}\right)]---\left(6\right)$

其协方差为：

${\mathrm{\σ}}_h^2=P^{z_w}-P{\left(P^{x_w{y}_w}\right)}^{-1}P---\left(7\right)$

第二步：测量分配到堆上。测量值分配到堆上要考虑测量值与该网格上已经存在的堆的高度关系。激光测量与网格中所有高度堆之间的融合方式可以分为以下四种情况：

1）测量值恰好在某堆中。

如图8所示，P₁表示网格已存在的高度堆，P₁对应的高度值为u₁，厚度值为d1，此时测量的高度估计h恰好分布在堆中间，因此无需对多层信息进行更新。

2）测量与某一堆融合

如图9所示，P₁和P₁表示网格已存在的高度堆，P₁对应的高度值为u₁，厚度值为d1，P₂对应的高度值为u₂，厚度值为d2。此时测量的高度估计h不在任何堆中。其中Δh₁大于设定的阈值h_m（h_m的设定要考虑机器人的高度），而Δh₂小于设定的阈值，这时将p₂与h进行融合成一个堆。新堆的高度为

厚度为d₂+Δh₂。

3）测量与某两堆融合

如图10所示，P₁和P₁表示网格已存在的高度堆，P₁对应的高度值为u₁，厚度值为d1，P₂对应的高度值为u₂，厚度值为d2。此时测量的高程估计h并不在任何堆中。其中Δh₁和Δh₂都小于设定的阈值h_m，这时将这三个堆融合成一个堆。新堆的高度为u₁，厚度为d₁+d₂+Δh₁+Δh₂。

4）测量单独成堆

如图11所示，P₁表示网格已存在的高度堆，P₁对应的高度值为u₁，厚度值为d1，P₂对应的高度值为u₂，厚度值为d2。此时测量的高程估计h并不在堆中，其中Δh₁和Δh₂都大于设定的阈值h_m，这时不进行任何的融合。测量所对应的堆的高度为

厚度为

4、地形可穿越性分析和导航路径生成

在构建出多层地形的基础上，对每层的地形进行平坦性分析，考虑到机器人自身机械结构特征和其穿越能力分割出可行进区域与障碍区域。

第一步：计算某一网格梯度。网格的梯度采用式（8）进行计算。

$g_{(m,n)}\≈\max \left\{\right|z_{(m,n)}-z_{(m+i,n+j)}|i,j=-\mathrm{1,0,1}\}---\left(8\right)$

其中g_(m,n)代表在一高度层中网格（m,n）上相应地形高度变化梯度的模，即对一个3×3的栅格区域进行分析，并取邻近高度差最大变化值作为近似的梯度模值。

第二步：计算某一网格的可穿越性。网格的可穿越性分析采用式（9）。

${\mathrm{map}}_{(m,n)}=\left\{\begin{array}{ccc}1,& \mathrm{if}& g_{(m,n)}>z_m\\ 0,& \mathrm{if}& g_{(m,n)}\≤z_m\end{array}\right.---\left(9\right)$

在上述分析的基础上运用式（9）对环境的可行进区域与障碍区域进行估计，其中map_(m,n)表示地形的可穿越性，map_(m,n)=0表示该网格是可以穿越，map_(m,n)=1表示该网格不可穿越。z_m是根据机器人的高度设定的阈值。

第三步：形成行走路径。计算出所有网格的可穿越性之后，采用A^*算法进行路径的规划形成机器人的最短有效行走路径。

应当指出，对于本技术领域的普通技术人员来说，在不脱离本发明原理的前提下，还可以做出若干改进和润饰，这些改进和润饰也应视为本发明的保护范围。本实施例中未明确的各组成部分均可用现有技术加以实现。

Claims (5)

1.一种基于概率多层地形的城市环境机器人导航方法，其特征在于：包括以下步骤：

（1）通过安装在云台上的低成本激光传感器对地形进行扫描、滤波和概率变换以获得全局坐标下的概率化点云表示；

（2）通过网格化地形划分方法，把点云数据关联到相应的若干网格之中；

（3）对网格内关联的点云数据进行概率融合，形成可表示多层次地形的堆数据，完成地形的建模过程；

（4）对得到的多层概率地形进行穿越性分析以形成安全有效的机器人行走路径。

2.根据权利要求1所述的一种基于概率多层地形的城市环境机器人导航方法，其特征在于：所述步骤（1）具体为：首先通过安装在激光云台上的二维激光传感器获得机器人周围环境的测量信息；考虑到激光测量数据、里程计和GPS测量数据的误差，把测量数据进行高斯模型概率化；然后采用卡尔曼滤波对里程计和GPS数据进行融合，以获得机器人当前位置的全局坐标；最后通过结合机器人当前的全局坐标，通过坐标变换得到测量值在全局坐标下的概率化点云表示。

3.根据权利要求1所述的一种基于概率多层地形的城市环境机器人导航方法，其特征在于：所述步骤（2）具体为：首先把地面划分成网格；然后对已经得到的测量值在全局坐标下的概率表示，采用高斯分布的3σ值作为不确定性边界关联到多个网格中并且计算出测量值属于这个网格的概率大小。

4.根据权利要求1所述的一种基于概率多层地形的城市环境机器人导航方法，其特征在于：所述步骤（3）具体为：每个网格上可能关联多个测量值，首先计算每个测量值在网格上的高度的高斯概率分布，其次把测量值分配到堆上，通过判断已有的堆与刚分配堆之间的垂直距离大小来进行融合。

5.根据权利要求1所述的一种基于概率多层地形的城市环境机器人导航方法，其特征在于：所述步骤（4）具体为：在构建出多层地形的基础上，对每层的地形进行平坦性分析，考虑到机器人自身机械结构特征和其穿越能力分割出可行进区域与障碍区域，从而形成机器人的行走路径。

Images