CN105205859B - 一种基于三维栅格地图的环境特征的相似性度量方法 - Google Patents

Abstract

本发明是一种基于三维栅格地图的环境特征的相似性度量方法，对于已经建好的三维栅格地图，针对三维栅格地图中的障碍物及其周边空白环境，进行特征分析与建模，相似的环境具有相似的环境特征，本发明提出了一种环境特征的相似性度量方法，做为环境特征识别的基础。本方法可以应用于机器人的环境识别、机器人的路径规划与自主运动，为机器人在实际环境中的游戏应用、机器人清洁等方面提供支撑。

Description

一种基于三维栅格地图的环境特征的相似性度量方法

技术领域

本发明涉及人工智能、模式识别与Fourier变换、Walsh变换等离散正交变换技术，特别是机器人对复杂环境的三维环境建模与识别的方法，应用于机器人的环境识别与自主运动方面。

背景技术

随着近些年工业机器人的发展，带动了服务机器人行业的逐渐掘起，同时从2014年开始的智能硬件领域也开始突起，根据国际机器人联盟的统计，2015年服务机器人销售额将达85亿美元，并且保持较高的20％～30％增长率，在智能硬件领域，据艾瑞研究，2014年全球智能硬件装机量达到60亿台，预计2017年将超过140亿台。

在市场高速发展的背后，问题同样明显，一方面市场的潜力还远未挖掘出来，另一方面，机器人及智能硬件进入服务行业也存在着一些技术难点，特别是三维环境的特征建模与识别技术，例如机器人进入实际的家庭环境中，进行环境识别与安全性地自主移动等，目前来看都还有一定的技术难点。

发明目的

本发明的主要目的就是解决基于三维栅格地图的环境建模后的环境相似性判断及相似程度度量，它提供一种方法，辅助环境识别，为其他实际应用提供技术支撑。可以但不限于应用在面向家庭的机器人游戏及机器人清洁方面。

技术方案

本发明的目的是这样实现的：通过相关设备及算法，例如三维激光雷达等，已经获取了实际环境的三维栅格地图信息，并在栅格地图中做了环境特征建模，其包括以下步骤：

(1)在环境特征建模时，考虑到计算的方便，我们设定一个影响距离σ，即障碍物体元对周边体元的影响范围，σ可以是但不限于欧氏距离、曼哈顿(Manhattan)距离等。同时也为了计算方便，把体元x的所受影响都平均离散到体元x的八个偏航面上[δ₀，δ_π/4，δ_π/2，δ_3π/4，δ_π，δ_5π/4，δ_3π/2，δ_7π/4]，再把每个偏航面δ_i内的影响分量都离散到偏航面δ_i内的八个俯仰角上[η₀，η_π/12，η_π/4，η_5π/12，η_7π/12，η_3π/4，η_11π/12，η_π]，形成体元x的所受影响矩阵f(x)，并且计算体元x的所受影响矩阵f(x)的特征矩阵与特征谱，这里给出两种特征矩阵及特征谱的计算方法，

(2)在环境特征建模时，把每个空闲体元的影响矩阵的每个元素初始化为0，逐个分析每个障碍物体元对σ距离内的所有体元的影响，例如，在分析的过程中，障碍物体元ξ相对体元x的偏航角度为θ，俯仰角度为β，距离为d，d≤σ，则障碍物体元ξ对体元x的影响函数可以是但不限于f(x_β，θ，d)＝1/d，如果θ恰好在体元x的某一偏航面上，则直接在这个偏航面上叠加即可，如果θ在某两个偏航面之间，例如θ∈(π/4，π/2)，则按照向量分解，把f(x_β，θ，d)分解投影到偏航面δ_π/4上的向量f_a与偏航面δ_π/2上的向量f_b(两个投影向量f_a与f_b同Z轴夹角相等，且与f(x_β，θ，d)共面)，按同样的向量分解方法再分别把向量f_a与f_b分解投影到各偏航面内的相邻两个俯仰角方向上，当对所有的障碍物体元分析完成后，每一个体元的影响矩阵已经计算完成，并存储在栅格中，记为：

(3)在环境特征建模时，考虑到计算的方便，我们求取f(x)的特征矩阵F(x)，并存储在体元中：

方法一、通过二维Fourier变换求取F(x)，令变换算子

根据快速二维Fourier变换计算得，

方法二，通过Walsh变换求取F(x)，令变换算子根据快速二维Walsh Hadama变换算得，

上述公式中的8x8矩阵，是8维的Walsh Hadama变换矩阵，不同的维数，所对应的Walsh Hadama变换矩阵是不相同的，其中：

(4)在环境特征建模时，考虑到计算的方便，我们求取特征矩阵F(x)的特征谱P(x)，并存储在栅格中：

方法一、根据快速二维Fourier变换求出的特征矩阵F(x)，我们有，

通过求模运算，特征谱元素P(x)[i][j]：|F(x)[i][j]|＝|F_i+1，j+1|，i，j∈[0，7]

方法二、根据快速二维Walsh Hadama变换求出的特征矩阵F(x)，我们有，

(5)在环境特征相似性度量系统中，我们设定阈值deg ree^threshold，另一个环境体元y的影响矩阵根据以上步骤使用二维Walsh Hadama变换方法，求取对应的特征谱P(y)，并比对相应的特征谱，记向量P(x)与向量P(y)之间的夹角为∠(P(x)，P(y))≤deg ree^threshold，计算如果cos∠(P(x)，P(y))≥cos(deg ree^threshold)，就表示体元x与目标体元y在实际环境中是相似的，

(6)在环境特征相似性度量系统中，对两个相似的环境体元x与环境体元y，如果：

考虑到i∈[1，n]，λ_i有等于零的情况，我们取

令，则我们定义环境体元x到环境体元y的相似比为λ

(7)在环境特征相似性度量系统中，按照上一步骤中的相似比进行归一化，令对两个相似的环境体元x与环境体元y，根据它们的特征矩阵F(x)与F(y)来考察其平移不变性、旋转不变性与镜像对称性，

方法一、根据快速Fourier变换，令

当时

p＝1则F(x)与F(y)有垂直镜像的关系；

q＝1则F(x)与F(y)有水平镜像的关系；

u＞0则F(x)与F(y)有循环上移——平移的关系；

l＞0则F(x)与F(y)有循环左移——平移的关系；

当时

p＝1，q＝0则F(x)与F(y)有左旋π/2的关系；

p＝0，q＝1则F(x)与F(y)有右旋π/2的关系；

方法二、根据快速二维Walsh Hadama变换，令：

D＝diag(1，-1，-1，1，-1，1，1，-1)

当时

p＝1则F(x)与F(y)有垂直镜像的关系；

q＝1则F(x)与F(y)有水平镜像的关系；

u＞0则F(x)与F(y)有循环上移——平移的关系；

l＞0则F(x)与F(y)有循环左移——平移的关系；

当时

p＝1，q＝0则F(x)与F(y)有左旋π/2的关系；

p＝0，q＝1则F(x)与F(y)有右旋π/2的关系：

(8)经过环境特征相似性度量系统，我们能成功的度量出相似环境的相似比、平移性、旋转性、对称性。

本发明所使用的系统组成如下：相似性判断系统、相似比度量系统、不变性度量系统。这三个系统是按照功能设置的软件系统，各子系统具体功能如下：

*相似性判断系统：在已建模后的栅格地图上，分析比对特征谱向量，判断环境栅格的相似性，

*相似比度量系统：对相似的环境计算其相似比，

*不变性度量系统：对相似的环境计算其旋转性、平移性、对称性。

附图说明：

图1是本发明方法所用系统组成图

图2(a)是三维栅格地图，(b)是体元的影响向量及分解方法，(c)是偏航面分解示意图

图3(a)是体元的偏航面与俯仰角的离散化，(b)是离散化后的矩阵

图4是一扇门的示意图

图5是旋转不变性与镜像对称性示意图

具体实施方式

下面结合附图，说明本发明的实施方式。

本发明方法所用的系统整体结构可参考图1，它有三个子系统组成，具体包含如下步骤：

第一步

首先，相似性判断系统，定义距离为欧氏距离，设定影响距离σ＝4，即障碍物体元只影响周边距离4以内的体元，如图2(a)是环境的三维栅格地图，图2(b)障碍物体元的影响向量及分解方法：

f(x_β，θ，d)＝f₁+f₂

＝f₁+f₃+f₄(f₃与f₄是f₂在两个相邻偏航面上的分量)

＝f′₃+f′₄+f₃+f₄(f₁按f₃与f₄的模长比例分解成f′₃与f′₄)

＝(f′₃+f₃)+(f′₄+f₄)

＝f_a+f_b(f_a与f_b同Z轴夹角相等)

按图3(a)所示把体元的影响离散到八个偏航面与八个俯仰角上，离散后的矩阵如图3(b)所示。

其次，相似性判断系统，设定deg ree^threshold＝5°。

第二步

在环境特征建模中，初始化每个体元的影响矩阵的元素f_ij＝0，i，j∈[1，8]，逐个扫描三维栅格地图中的每一个体元，对每一个障碍物体元全面计算对周边体元的影响。

例如，如图4所示，障碍物体元(1，1，1)对体元(2，1，1)的影响f(x_{π/2，π，1})＝1，对该影响离散化到体元(2，1，1)的偏航面与俯仰角上，易知f(x_{π/2，π，1})恰好在偏航面δ_π上，且在俯仰角η_sπ/12与η_7π/12之间，所以按向量分解的方式，只需把f(x_{π/2，π，1})投影到俯仰角η_sπ/12与η_7π/12上即可，由图2与图3知f(x_{π/2，π，1})分解以后得：

同理，障碍物体元(1，2，1)对体元(2，1，1)的影响离散到偏航面δ_3π/4上的俯仰角η_sπ/12与η_7π/12上，分解后得：

障碍物体元(1，3，1)对体元(2，1，1)的影响离散到偏航面δ_3π/4与偏航面δ_π/2上后分别为：

再把f_a与f_b投影到所属偏航面上的俯仰角η_5π/13与η_7π/12上，分解后得：

障碍物体元(1，1，2)对体元(2，1，1)的影响离散到偏航面δ_π上的俯仰角η_π/4上，分解后得：

障碍物体元(1，2，2)对体元(2，1，1)的影响离散到偏航面δ_3π/4上的俯仰角η_π/4与η_sπ/12上，分解后得：

障碍物体元(1，3，2)对体元(2，1，1)的影响离散到偏航面δ_3π/4与偏航面δ_π/2上后分别为(在各偏航面内的坐标表示，不考虑方向性)：

再把f_a与f_b投影到所属偏航面上的俯仰角η_sπ/12与η_π/4上，如下：

得分解后得：

障碍物体元(1，1，3)对体元(2，1，1)的影响离散到偏航面δ_π上的俯仰角η_sπ/12与η_π/4上后得：

障碍物体元(1，2，3)对体元(2，1，1)的影响离散到偏航面δ_3π/4上的俯仰角η_π/4与η_π/12上，分解后得：

障碍物体元(1，3，3)对体元(2，1，1)的影响离散到偏航面δ_3π/4与偏航面δ_π/2上后分别为：

再把f_a与f_b投影到所属偏航面上的俯仰角η_sπ/12与η_π/4上，如下：

得分解后得：

综上，体元(2，1，1)所受的影响矩阵

第三步

根据快速Walsh Hadama变换求出体元(2，1，1)的影响矩阵f(2，1，1)的特征矩阵F(2，1，1)，我们有：

第四步

根据快速Walsh Hadama变换求出体元(2，1，1)的特征谱P(2，1，1)，由上一步的特征矩阵F(2，1，1)我们有：

P(2，1，1)＝[0.0051，0.0001，0.0057，0.0325]

第五步

在相似性判断系统里，例如我们有另一环境的影响矩阵，如下：

同理我们有：

第六步

通过环境相似性判断系统，我们比较P(2，1，1)与P(door)

cos∠(P(2，1，1)，P(door))＝0.999625≥cos(deg ree^threshold)＝0.996195

同理，我们有另一环境的影响矩阵如下：

同样我们有：

易知：cos∠(P(door)，P_T(door))＝1≥cos(deg ree^threshold)＝0.996195；

在相似比度量系统里，容易计算：P(door)＝I_λ·P_T(door)，得λ＝1；

在不变性判断系统里，根据方法二之快速二维Walsh Hadama变换，算得知环境f(door)经过平移——循环上移6步再循环左移6步即可与环境f_T(door)各向同性，这与实际环境是相符的。

同理，我们有另一环境的影响矩阵如下：

同样我们有：

易知：cos∠(P(door)，P_M(door))＝1≥cos(deg ree^threshold)＝0.996195；

在相似比度量系统里，容易计算：P(door)＝I_λ·P_M(door)，得λ＝1；

在不变性判断系统里，根据方法二之快速二维Walsh Hadama变换，算得F_M(door)＝F(door)D，知环境f(door)经过水平镜像即可与环境f_M(door)各向同性，这与实际环境是相符的。

同理，我们有另一环境的影响矩阵如下：

同样我们有：

易知：cos∠(P(door)，P_R(door))＝1≥cos(degree^threshold)＝0.996195；

在相似比度量系统里，容易计算：P(door)＝I_λ·P_R(door)，得λ＝1；

在不变性判断系统里，根据方法二之快速二维Walsh Hadama变换，算得F_R(door)＝D(F(door))，知环境f(door)经过左旋即可与环境f_R(door)各向同性，这与实际环境是相符的。

综上，我们可以看到通过环境相似性度量系统，能把相似的环境分离出来，并且可以量化分析相似环境之间的关系，通过相似比运算与平移不变性、旋转不变性、对称不变性运算，揭示相似环境之间的内在本质，对环境特征的识别，即大大减少了穷举比对次数，又做了更精细化区分。

Claims (5)

1.一种基于三维栅格地图的环境特征的相似性度量方法，该方法是通过比对两个相似环境的特征矩阵，以确定两个相似环境的关系，或者镜像对称——称之为镜像对称性，或者循环移位——称之为平移不变性，或者旋转对称——称之为旋转不变性，该方法中的三维栅格地图如下定义：将环境空间抽象为三维直角坐标系O：xyz下的栅格空间，空间的全集为Ω，Ω内的每个元素称为体元，用c_x，y，z表示，(x，y，z)即为该体元的三维坐标，每个体元是一个边长为μ的正方体，正方体的每条边都与空间坐标轴平行，根据实际环境有无物体占据，来确定或概率意义上确定相应体元的占空值，基于此形成的地图称为三维栅格地图，μ称为三维栅格地图的分辨率；本方法中的环境建模及特征量、相似性、相似比、镜像对称性、平移不变性、旋转不变性按如下步骤给出：

(1)通过相关的设备及算法，建立与实际环境对应的三维栅格地图，这里的设备包括三维激光雷达或双目视觉传感器；

(2)在栅格地图上逐个分析每个障碍物体元对周边空闲体元的影响，如下所示：

障碍物体元对体元x的影响记作f(x_β，θ，d)，β∈[0，π]表示障碍物体元相对体元x的俯仰角，选取三维栅格地图的Z轴方向做为俯仰角0度起始方向，θ∈[0，2π)表示障碍物体元相对体元x的偏航角，选取三维栅格地图的X轴方向做为偏航角0度起始方向，d表示障碍物体元相对体元x的距离，按照β，θ的递增顺序，体元x所受的影响记为：

(3)f(x)的离散化：

首先，对体元x所受的影响按角度θ离散到有限的偏航面内[δ₁，δ₂，...，δ_s]，s∈N，每个偏航面δ表示一个以体元x所在的Z轴方向为边界的半平面，其次，在每个偏航面δ内，按角度β离散到有限的俯仰角上[η₁，η₂，...，η_n]，n∈N，方法如下：

某障碍物体元对体元x的影响当δ_i≤θ_k≤δ_i+1，i∈[1，s-1]，k∈[1，s]，则把按向量分解投影到偏航面δ_i与偏航面δ_i+1上，然后再分别把偏航面δ_i与偏航面δ_i+1内的分量，分别按向量分解投影到η_j与η_j+1两个俯仰角上，离散化后，

(4)根据体元x的所受影响f(x)，提炼出其特征矩阵F(x)，

是一种变换，这里的变换使用二维Fourier变换或二维Walsh变换；

(5)根据体元x的特征矩阵F(x)，计算出其特征谱P(x)，针对上一步骤中的变换，其特征谱分别是相应变换的功率谱，特征谱即为体元的环境特征；

(6)特征谱比对：

体元x的影响矩阵f(x)对应的特征谱为P(x)，体元y的影响矩阵f(y)对应的特征谱为P(y)，我们定义一阈值角度degree^threshold，若向量P(x)与向量P(y)之间的夹角∠(P(x)，P(y))≤degree^threshold，或者cos∠(P(x)，P(y))≥cos(degree^threshold)，就表示矩阵f(x)与f(y)是相似的，也就是体元x与体元y在实际环境中是相似的，利用向量运算法则，我们可以计算：

与cos(degree^threshold)比较后，我们即可得出相似性，

相似的向量P(x)与向量P(y)是同向的，他们之间的转化因子即为相似比；

(7)环境的不变性：

对于n×s阶矩阵f(x)，考察f(y)所表示的实际环境，都归一化之后，如果：

f(y)[i][j]＝f(x)[(i+u)％n][(j+l)％s]对j∈[0，s-1]，l∈N使得等式成立，则称环境f(x)与f(y)具有平移不变性；

如果矩阵f(y)[i][j]＝f(x)[j][s-1-i]对j∈[0，n-1]使得等式成立，则称环境f(x)左旋之后等同f(y)，称它们具有旋转不变性；

如果矩阵f(y)[i][j]＝f(x)[n-1-j][i]对j∈[0，n-1]使得等式成立，则称环境f(x)右旋之后等同f(y)，称它们具有旋转不变性；

如果对j∈[0，s-1]使得三个等式中有一个成立，则称环境f(x)与f(y)具有镜像对称性；

通过特征矩阵F(x)与F(y)的计算，即可确定其镜像对称性、平移不变性及旋转不变性；

(8)对实际环境，通过特征矩阵与特征谱计算，我们即可找出实际环境体元x与体元y的相似性与相似比，以及镜像对称性、平移不变性、旋转不变性，这样就达到了环境特征相似性度量的目的。

2.如权利要求1所述的一种基于三维栅格地图的环境特征的相似性度量方法，其特征在于，通过特征谱判断两个环境的相似性。

3.如权利要求1所述的一种基于三维栅格地图的环境特征的相似性度量方法，其特征在于，通过相似环境的相似比来表示相似程度。

4.如权利要求1所述的一种基于三维栅格地图的环境特征的相似性度量方法，其特征在于，通过相似环境之间的镜像对称性、平移不变性与旋转不变性来表示两个环境的相似性。

5.如权利要求1所述的一种基于三维栅格地图的环境特征的相似性度量方法，其特征在于，为了计算方便把栅格的影响矩阵通过变换求取特征矩阵，通过特征矩阵计算相似环境之间的镜像对称性、平移不变性与旋转不变性，这里的变换使用Fourier变换或Walsh变换。