一种基于物体语义路标的移动机器人视觉定位方法

1.本发明涉及移动机器人的技术领域，尤其涉及到一种基于物体语义路标的移动机器人视觉定位方法。


背景技术：

2.自主导航是移动机器人技术领域的一个核心技术问题，自主导航系统一般包含定位、路径规划以及控制等三个主要模块，其中定位模块是实时控制模块和局部路径规划模块的信息输入，作为信息反馈环节在自主导航系统起着至关重要作用。定位模块的实时性、精准度以及鲁棒性对自主导航系统性能有着决定性因素。
3.现有基于视觉传感器的定位系统主要依赖于视觉特征关键点的匹配，即当前帧图像中的特征关键点与特征地图数据库中的关键点进行匹配关联。在匹配成功的情况下，借助地图数据库中关键点的三维坐标，通过相机投射投影几何模型估计出当前帧图像对应的相机坐标系的三维位姿，即等同于估计出传感器移动平台的位姿，从而实现定位功能。在定位过程中，视觉特征关键点匹配是精准位姿估计的前提，特征关键点的匹配主要基于描述向量提取算法，其本质上依赖于图像局部像素强度分布特性。在外界环境的纹理信息、光照条件严格相同的条件下，图像局部像素强度分布特性会受到相机捕获视角的影响，致使在同一场景下，因为观察角度和地图构建过程时不一致而导致特征点匹配失败，进而造成机器人定位系统无法可靠工作。


技术实现要素：

4.针对现有基于视觉方案的移动机器人全局定位技术中由于视觉特征点的提取及匹配算法的光照不变性和视角不变性不足的问题，本发明提出一种基于物体语义路标的移动机器人视觉定位方法，旨在摆脱定位系统对机器人运动姿态的限制要求，提升定位系统对外界环境条件的鲁棒性，最终提升移动机器人的自主性和灵活性。
5.为实现上述目的，本发明所提供的技术方案为：
6.一种基于物体语义路标的移动机器人视觉定位方法，包括如下步骤：
7.s1、构建离线的物体路标全局地图；
8.s2、构建在线的物体路标局部地图；
9.s3、以图结构为数学模型对物体路标局部地图和物体路标全局地图进行数学表达，并以子图匹配问题作为地图匹配数学方程，以子地图匹配求解结果实现物体路标局部地图在物体路标全局地图中的配准估计，最终实现移动机器人的视觉定位。
10.进一步地，所述步骤s1构建离线的物体路标全局地图的具体过程如下：
11.s1
‑
1、获取输入图像，并对输入图像进行预处理；
12.s1
‑
2、利用训练好的卷积神经网络实现多目标物体检测，并最终以2d矩形包络框的形式输出，输出图像中的所有目标物体的类别向量c
i
及其包络框在像素坐标系中的位置x
i
，y
i
及尺寸信息w
i
，h
i
，即{c
i
,x
i
,y
i
,w
i
,h
i
,i∈n}，w
i
为高度，h
i
为高度；
13.s1
‑
3、利用光流法对当前图像与上一帧图像进行像素级别配准，通过最小化两帧间配准的像素的灰度值代价函数，估计相机运动位姿：
[0014][0015]
式(1)中，(
·
)^符号表示将李代数se(3)中的6维向量变换成4
×
4的矩阵形式，即而exp(
·
)是对李代数se(3)的指数映射，即exp(ξ^)∈se(3)为相邻帧的刚体运动位姿，se(3)是特殊欧几里得群。p1和p2为对应相机的投影矩阵，x
i,1
和x
i,2
为空间中对应的三维点在对应相机坐标系下的表达，i1(
·
)和i2(
·
)为对应像素点的像素灰度值；
[0016]
s1
‑
4、利用对偶椭球曲面对物体进行包络作为物体路标地图表达，先对图像物体检测框进行内切椭圆拟合并得到椭圆的对偶矩阵形式，对于视图i而言，在其相机姿态(r
i
，t
i
)和相机内参矩阵k已经求得的情况下，三维空间中的对偶椭球曲面q
*
在该视图中的投影轮廓通过投影矩阵p
i
＝k
·
[r
i t
i
]具有如下关系：
[0017][0018]
式(2)中，标量表明方程在相差一个尺度下具有等价性。
[0019]
s1
‑
5、在齐次坐标系中，椭球的对偶形式q
*
为一个4x4的对称矩阵，而椭圆的对偶形式c
*
为一个3x3的对称矩阵，即q
*
和c
*
分别有10个和6个独立元素；方程(2)是一个二次型方程，将p
i
进行二次型向量表达，记为b
i
，原方程线性化表达成：
[0020][0021]
s1
‑
6、对于多个视图，联立多个方程(3)构建线性方程组，对于过约束方程的求解问题是线性最小二乘问题，通过svd进行求解出变量将其恢复成对称矩阵可得到椭球原对偶形式：
[0022][0023]
完成物体包络椭球的重建，形成物体路标全局地图。
[0024]
进一步地，所述步骤s1
‑
1中，预处理过程包括：滤波去噪、灰度图转化以及尺寸缩放，以适应神经网络输入维度。
[0025]
进一步地，所述步骤s2构建在线的物体路标局部地图的过程如下：
[0026]
对视觉图像进行预处理后，进行物体检测和数据关联；接着利用光流法进行位姿跟踪，即实现视觉里程计功能，该视觉里程计融合上一时刻优化位姿，以最大程度消除累计误差；然后利用多视图构建局部物体路标地图，并采用滑动窗口式的局部地图规模控制策略，并对物体路标包络面坐标系进行刚体变换，在相机坐标系下进行表达，最终形成以相机
坐标系相关联的规模可控的物体路标局部地图。
[0027]
进一步地，所述步骤s3的具体过程如下：
[0028]
对物体路标局部地图与物体路标全局地图进行匹配，匹配问题从数学上看为一个子图匹配问题，将物体路标全局地图表示为图结构g＝(v,e)，其中v为节点，包含每个路标i的三维坐标和标签，e为边，表示两个路标之间的相互三维关系；同理将局部地图表示为图结构h＝(v
′
,e
′
)，v
′
为节点，e
′
为边；
[0029]
则地图匹配问题具体定义为：给定具有n个节点的图g＝(v,e)和具有m个节点的图h＝(v
′
,e
′
)，在图g和图h寻求节点对应关系：
[0030]
x∈{0,1}
n
×
m
[0031]
以最大化图属性和结构的一致性：
[0032][0033]
其中v表示图g的i1节点和图h的i2节点的一致性度量，该度量来自路标的三维尺度一致性以及语义标签的一致性；d表示图g的i1‑
j1边和图h的i2‑
j2边的一致性度量，该度量来自地图中的不同路标之间三维位置距离一致性；利用语义信息对两个图中的相同标签的节点进行匹配，减少优化迭代次数，最后获得最优匹配解，确定两个地图之间路标的一一对应关系；
[0034]
完成物体路标局部地图对物体路标全局地图的匹配之后，以物体路标全局地图中的路标三维信息作为基准进行位姿传播，即匹配上的全局地图路标三维信息
‑
局部地图路标三维信息
‑
局部地图的参考坐标系——相机坐标系在世界坐标系中的变换t
wc
∈se(3)，最后获得当前时刻机器人相对于地图世界坐标系的位姿：
[0035]
t
wr
＝t
wc
t
cr
∈se(3)
[0036]
式中，t
cr
∈se(3)是机器人相对相机坐标系的变换矩阵,t
wc
∈se(3)是相机坐标系相对世界坐标系的变换矩阵，根据变换矩阵链式法则，二者相乘做矩阵运算可以得到机器人相对世界坐标系的变换矩阵t
wr
∈se(3)，即机器人的定位姿态信息。
[0037]
与现有技术相比，本方案原理及优点如下：
[0038]
本方案首先构建离线的物体路标全局地图，然后构建在线的物体路标局部地图，接着、以图结构为数学模型对物体路标局部地图和物体路标全局地图进行数学表达，并以子图匹配问题作为地图匹配数学方程，以子地图匹配求解结果实现物体路标局部地图在物体路标全局地图中的配准估计，最终实现移动机器人的视觉定位。本方案基于物体语义路标进行机器人视觉定位，利用三维空间的几何结构具有视角不变性的性质，摆脱定位系统对机器人运动姿态的限制要求，提升定位系统对外界环境条件的鲁棒性，最终提升移动机器人的自主性和灵活性。
附图说明
[0039]
为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施例或现有技术描述中所需要使用的服务作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以
根据这些附图获得其他的附图。
[0040]
图1为本发明一种基于物体语义路标的移动机器人视觉定位方法的原理流程图；
[0041]
图2为实施例中基于椭球包络曲面表征的物体路标全局地图；
[0042]
图3为实施例中滑动窗口策略下的物体路标局部地图。
具体实施方式
[0043]
下面结合具体实施例对本发明作进一步说明：
[0044]
如图1所示，本实施例所述的一种基于物体语义路标的移动机器人视觉定位方法，包括以下步骤：
[0045]
s1、构建离线的物体路标全局地图，过程如下：
[0046]
s1
‑
1、获取输入图像，并对输入图像进行预处理，具体包括：滤波去噪、灰度图转化以及尺寸缩放，以适应神经网络输入维度；
[0047]
s1
‑
2、利用训练好的卷积神经网络实现多目标物体检测，并最终以2d矩形包络框的形式输出，输出图像中的所有目标物体的类别向量c
i
及其包络框在像素坐标系中的位置x
i
，y
i
及尺寸信息w
i
，h
i
，即{c
i
,x
i
,y
i
,w
i
,h
i
,i∈n}，w
i
为高度，h
i
为高度；
[0048]
s1
‑
3、利用光流法对当前图像与上一帧图像进行像素级别配准，通过最小化两帧间配准的像素的灰度值代价函数，估计相机运动位姿：
[0049][0050]
式(1)中，(
·
)^符号表示将李代数se(3)中的6维向量变换成4
×
4的矩阵形式，即而exp(
·
)是对李代数se(3)的指数映射，即exp(ξ^)∈se(3)为相邻帧的刚体运动位姿，se(3)是特殊欧几里得群。p1和p2为对应相机的投影矩阵，x
i,1
和x
i,2
为空间中对应的三维点在对应相机坐标系下的表达，i1(
·
)和i2(
·
)为对应像素点的像素灰度值；
[0051]
s1
‑
4、利用对偶椭球曲面对物体进行包络作为物体路标地图表达，先对图像物体检测框进行内切椭圆拟合并得到椭圆的对偶矩阵形式，对于视图i而言，在其相机姿态(r
i
，t
i
)和相机内参矩阵k已经求得的情况下，三维空间中的对偶椭球曲面q
*
在该视图中的投影轮廓通过投影矩阵p
i
＝k
·
[r
i t
i
]具有如下关系：
[0052][0053]
式(2)中，标量表明方程在相差一个尺度下具有等价性。
[0054]
s1
‑
5、在齐次坐标系中，椭球的对偶形式q
*
为一个4x4的对称矩阵，而椭圆的对偶形式c
*
为一个3x3的对称矩阵，即q
*
和c
*
分别有10个和6个独立元素；方程(2)是一个二次型方程，将p
i
进行二次型向量表达，记为b
i
，原方程线性化表达成：
[0055][0056]
s1
‑
6、对于多个视图，联立多个方程(3)构建线性方程组，对于过约束方程的求解
问题是线性最小二乘问题，通过svd进行求解出变量将其恢复成对称矩阵可得到椭球原对偶形式：
[0057][0058]
完成物体包络椭球的重建，形成物体路标全局地图，如图2所示。
[0059]
s2、构建在线的物体路标局部地图；
[0060]
本步骤同步骤s1，对视觉图像进行预处理后，进行物体检测和数据关联；接着利用光流法进行位姿跟踪，即实现视觉里程计功能，该视觉里程计融合上一时刻优化位姿，以最大程度消除累计误差；然后利用多视图构建局部物体路标地图，并采用滑动窗口式的局部地图规模控制策略，并对物体路标包络面坐标系进行刚体变换，在相机坐标系下进行表达，最终形成以相机坐标系相关联的规模可控的物体路标局部地图，如图3所示。
[0061]
s3、物体路标局部地图和物体路标全局地图进行匹配，最终实现移动机器人的视觉定位。
[0062]
本步骤的具体过程如下：
[0063]
对物体路标局部地图与物体路标全局地图进行匹配，匹配问题从数学上看为一个子图匹配问题，将物体路标全局地图表示为图结构g＝(v,e)，其中v为节点，包含每个路标i的三维坐标和标签，e为边，表示两个路标之间的相互三维关系；同理将局部地图表示为图结构h＝(v
′
,e
′
)，v
′
为节点，e
′
为边；
[0064]
则地图匹配问题具体定义为：给定具有n个节点的图g＝(v,e)和具有m个节点的图h＝(v
′
,e
′
)，在图g和图h寻求节点对应关系：
[0065]
x∈{0,1}
n
×
m
[0066]
以最大化图属性和结构的一致性：
[0067][0068]
其中v表示图g的i1节点和图h的i2节点的一致性度量，该度量来自路标的三维尺度一致性以及语义标签的一致性；d表示图g的i1‑
j1边和图h的i2‑
j2边的一致性度量，该度量来自地图中的不同路标之间三维位置距离一致性；利用语义信息对两个图中的相同标签的节点进行匹配，减少优化迭代次数，最后获得最优匹配解，确定两个地图之间路标的一一对应关系；
[0069]
完成物体路标局部地图对物体路标全局地图的匹配之后，以物体路标全局地图中的路标三维信息作为基准进行位姿传播，即匹配上的全局地图路标三维信息
‑
局部地图路标三维信息
‑
局部地图的参考坐标系——相机坐标系在世界坐标系中的变换t
wc
∈se(3)，最后获得当前时刻机器人相对于地图世界坐标系的位姿：
[0070]
t
wr
＝t
wc
t
cr
∈se(3)
[0071]
式中，t
cr
∈se(3)是机器人相对相机坐标系的变换矩阵,t
wc
∈se(3)是相机坐标系
相对世界坐标系的变换矩阵，根据变换矩阵链式法则，二者相乘做矩阵运算可以得到机器人相对世界坐标系的变换矩阵t
wr
∈se(3)，即机器人的定位姿态信息。
[0072]
本实施例基于物体语义路标进行机器人视觉定位，利用三维空间的几何结构具有视角不变性的性质，摆脱定位系统对机器人运动姿态的限制要求，提升定位系统对外界环境条件的鲁棒性，最终提升移动机器人的自主性和灵活性。
[0073]
以上所述之实施例子只为本发明之较佳实施例，并非以此限制本发明的实施范围，故凡依本发明之形状、原理所作的变化，均应涵盖在本发明的保护范围内。