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

本发明涉及人工智能、模式识别与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)各向同性，这与实际环境是相符的。

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