具体实施方式

为了使本申请的目的、技术方案及优点更加清楚明白，以下结合附图及实施例，对本申请进行进一步详细说明。

本具体实施例仅仅是对本申请的解释，其并不是对本申请的限制，本领域技术人员在阅读完本说明书后可以根据需要对本实施例做出没有创造性贡献的修改，但只要在本申请的权利要求范围内都受到专利法的保护。

本申请实施例公开一种机器人导航与定位方法，如图1和图2所示，所述方法的包括以下步骤。

S100，获取机器人的绝对位置信息和相对位置信息。

（1）通过RFID定位技术获取机器人的绝对位置信息。

在机器人上安装读写器及读写器天线，在地面安装无源超高频RFID标签作为参考标签。所有的参考标签的EPC及其所对应的世界坐标系下的位置作为已知的信息保存在机器人内，机器人在连续运动的过程中，控制RFID系统不断的读取参考标签，并通过从RFID系统中所提取的参考标签的可读性和相位相关信息进行定位。

如图3所示，所述方法的具体步骤包括：

S101，建立初始粒子集，其中，粒子对应所述机器人，一个粒子位姿对应一个机器人位姿。

设定所述粒子初始位姿，如公式（1）所示：

（1）

其中，Xi 0是在初始时刻粒子集中第i个粒子的初始位姿；R ₁，R ₂，R ₃是0到1之间的随机数；T _j = T ₁，T ₂，…，T _m ，T _j 是在初始时刻读到的参考标签，x _Tj 是在初始时刻读到的参考标签的横坐标，y _Tj 是在初始时刻读到的参考标签的纵坐标，r是参考标签的最大读取范围。

S102，对于每个粒子t-1时刻的位姿，采用航迹推算法预测每个粒子t时刻的位姿。

所述航迹推算（Dead Reckoning，DR）定位技术的关键是能测量出机器人单位时间间隔走过的距离，以及在这段时间间隔内机器人航向的变化。利用拖轮和加速度计分别测量出旋转率和加速率，再对测量结果进行积分，从而求解出机器人移动的距离以及航向的变化，再根据航迹推算法求得机器人的位姿。

S103，构建t时刻粒子总权重与可读性权重和相位差权重的关系式，获得t时刻的每个粒子的总权重。

首先对粒子可读性权重评估：设定粒子位姿判断条件，利用粒子位姿判断条件判定粒子集中每个粒子所代表的机器人位姿的合理性，通过合理性检验的粒子可读性权重设为ω _1,i,t =1，未通过合理性检验的粒子可读性权重设为ω _1,i,t =0。

计算粒子的相位差权重，具体公式如下：

（2）

（3）

（4）

其中，ω _2,i,t 第i个粒子在t时刻的相位差权重，△θ _t 是所述天线分别在t-1时刻和在t时刻测量的同一个RFID参考标签的观测相位的差值。△θ _i,t 是第i个粒子的预测相位差，σ是 相 位 测 量 噪 声 的 标 准 差，△d _t 是 t 时 刻 的 距 离 差，d1是在t-1时刻所述天线到RFID参考标签的距离，d2是在t时刻所述天线到RFID参考标签的距离，λ代表无线电信号的波长。

所述粒子总权重ω _i,t 表示为：

（5）

其中，ω _i,t-1是第i个粒子在t-1时刻的总权重，ω _1,i,t 是第i个粒子在t时刻的可读性权重，ω _2,i,t 是第i个粒子在t时刻的相位差权重。

S104，利用每个粒子的总权重和粒子位姿计算机器人t时刻的位姿，以此实现所述机器人的绝对定位。

通过估计粒子集的加权几何中心，计算出机器人的位姿，实现机器人定位：

（6）

其中，x _j 是预测位姿的横坐标，y _j 是预测位姿的纵坐标，th _j 是预测位姿的姿态角，x _i,t 是t时刻第i个粒子在世界坐标系下的横坐标，y _i,t 是t时刻第i个粒子在世界坐标系下的纵坐标，th _i,t 是t时刻第i个粒子在世界坐标系下的姿态角，N是粒子集中粒子的个数。

（2）通过相机获取机器人的相对位置信息。

本实施例中将安装在机器人的末端操纵器上，此时RGB相机获取的目标物体图像直接反映目标物体与机器人末端操纵器之间的相对位置和姿态关系。在跟踪目标物体前，先进行离线学习，将机器人控制到正确位置，以获取期望的目标物体图像，利用活动轮廓得到期望的目标物体边缘轮廓v ^*：

（7）

其中，x* j,y* j分别表示期望的目标物体边缘轮廓上j点的横坐标和纵坐标，n为期望的目标物体边缘轮廓上点的数量。

在线工作时，RGB相机实时采集目标物体图像，通过活动轮廓得到实时获取的目标物体边缘轮廓v：

（8）

其中，x* j,y* j分别表示实时获取的目标物体边缘轮廓上j点的横坐标和纵坐标。

由期望的目标物体边缘轮廓与实时获取的目标物体边缘轮廓的差值e=v-v ^*实时控制机器人的运动，直到消除该差值，或使该差值最小，此时机器人已到达正确位置。

图像边缘运动和RGB相机边缘运动之间的关系：

（9）

其中，τ _c 是RGB相机的运动速度：τ _c =(T _c ,W _c )，T _c =(T _cx ,T _cy ,T _cz )是平动速度，W _c =(W _cx ,W _cy ,W _cz )是转动速度，L是反映目标物体三维坐标(X _j ,Y _j ,Z _j )与图像坐标中对应点的坐标(x _j ,y _j )关系的矩阵，考虑透视投影，x _j =X _j /Z _j ，y _j =Y _j /Z _j ，则 L 由下式决定：

（10）

为了计算更加方便，在实际工作中Z _j 可取Z* j（即期望值）。

通过关系矩阵L得到目标物体的三维坐标(X _j ,Y _j ,Z _j )，从而得到机器人的相对位置信息。

S200，将所述机器人的绝对位置信息和相对位置信息融合，得到机器人的精确位置信息。

本实施例采取两种定位技术融合的方式来进行精确定位，采用视觉相对位置信息进行辅助定位。

具体的融合方式为：

将RFID定位过程中所使用的世界坐标系作为全局坐标系，将视觉解算出的相对位置信息经过空间变换，转换到世界坐标系下。构建一个层次型神经网络，将两个位置坐标输入到该神经网络中进行数据融合。

本实施例中采用基于BP算法的三层前馈网络模型，首先对建立的该三层前馈网络模型进行离线训练。训练算法以二进制矢量表示的感知信息作为输入矢量，对应的决策命令二进制矢量作为目标矢量，其中样本数据为先验知识范围内的感知信息类别。模型训练完成后将绝对位置信息和经过空间变换后的相对位置信息作为该三层前馈网络模型的输入，根据预先设置的融合参数，产生融合策略，从而完成机器人的绝对位置信息和相对位置信息融合，得到机器人的精确位置信息。

本实施例采用的神经网络具有较强的容错性和自组织、自学习及自适应能力，能够实现复杂的映射；神经网络还具有强大的非线性处理能力，能够很好的满足数据融合技术的要求。

S300，采集机器人的工作环境图像，并结合所述机器人的精确位置信息得到导航地图和障碍物地图。

（1）将机器人的当前精确位置信息作为导航的起始位置。采用窄基线立体相机对机器人脚部附近的环境进行实时摄像，得到的窄基线图像。采用宽基线立体相机对机器人脚部以外的远方环境进行实时摄像，得到的宽基线图像。

根据宽基线图像和窄基线图像所表示出来的光学特性信息（灰度值、颜色值等）、几何特性信息（外形、大小等）、空间位置信息（图像中的位置、方向等）及特征信息（边缘、角点的位置信息）等信息，将这些信息中的结构性特征予以组合形成稳定的特征向量，将对于图像的几何变形、光照变化等因素保持一定稳定性的特征向量作为不变量，将宽基线图像数据和窄基线图像数据进行匹配，根据近邻欧式距离比值准则提取两图像中匹配最稳定的极值特征区域对，利用顺序抽样一致性算法剔除误匹配特征区域对，估计两图像数据的外极几何关系，得到匹配结果。根据匹配结果并结合机器人的当前精确位置信息，获得以当前精确位置信息为导航起始点的导航地图。

（2）采用全景相机对周围环境进行拍摄得到全景图像，同时通过若干个激光雷达对周围环境中的障碍物进行扫描，得到包含若干障碍物的扫描地图。再通过对周围环境中的障碍物进行测量获得障碍物高度信息，并将障碍物高度信息标注到扫描地图中得到障碍物地图。

通过激光雷达测量障碍物高度信息的过程为：设地面为一个平面，障碍物的高低角α能够测出，根据无线电波的传播速度及其来回所用的时间，能够计算出雷达与障碍物顶点之间的倾斜距离d，这时障碍物的高度h为：

（11）

将计算得到的障碍物高度信息标注到所述全景扫描地图中，得到障碍物地图。

S400，将所述导航地图和障碍物地图进行融合，获得标注有障碍物高度信息的地形测绘图。

步骤401，对导航地图和障碍物地图进行特征点提取，然后对提取的特征点进行匹配，得到匹配点对。

步骤402，利用仿射变换方法，根据匹配点对获取导航地图和障碍物地图之间的旋转矩阵、尺度变量和平移矢量，计算相应地仿射变换矩阵T。

（12）

其中，Φ表示障碍物地图相较于导航地图旋转的角度，s表示融合比例，δ _x ,δ _y 分别表示障碍物地图相较于导航地图在x方向和y方向上的偏移量。

步骤403，利用旋转矩阵、尺度变量、平移矢量和仿射变换矩阵，对导航地图和障碍物地图进行融合，从而获得标注有障碍物高度信息的地形测绘图。

S500，根据所述地形测绘图中的障碍物高度信息判断机器人是否进行翻越或转向回避。

将地形测绘图中的障碍物高度信息与预先设置的障碍物阈值进行对比，以供机器人导航判断是否进行翻越或者转向回避。当障碍物高度信息小于预先设置的障碍物阈值时，说明机器人可以翻越障碍物的高度，则执行机器人翻越障碍物。当障碍物高度信息不小于预先设置的障碍物阈值时，说明机器人翻越不了障碍物的高度，则执行机器人进行转向避让，回避障碍物的阻挡。

本申请实施例还公开一种机器人导航与定位系统，如图2和图4所示，所述系统包括：

（1）定位模块，用于获取机器人的绝对位置信息和相对位置信息。

本实施例中，所述定位模块包括：

RFID子模块，用于通过读取RFID标签获取机器人的绝对位置信息。所述RFID子模块包括：RFID标签、读写器和读写天线。在机器人上安装读写器和读写器天线，在地面安装无源超高频RFID标签作为参考标签。读写器读取/写入RFID标签中的信息。读写器天线通过通信基站在所述RFID标签和读写器之间传递射频信号。

视觉定位子模块，用于获取机器人的相对位置信息。本实施例中的视觉定位子模块采用的是RGB相机，将该相机安装在机器人的末端操纵杆上，这样获取的物体图像直接反映目标物体与机器人末端操纵器之间的相对位置和姿态关系，从而得到机器人的相对位置信息。

（2）数据融合模块，用于将所述机器人的绝对位置信息和相对位置信息融合，得到机器人的精确位置信息。

（3）环境感知模块，用于采集机器人的工作环境图像，并结合所述机器人的精确位置信息得到导航地图和障碍物地图。

本实施例中，所述环境感知模块具体包括：

窄基线立体相机，用于实时获取对机器人脚部的环境进行采集得到的窄基线图像。

宽基线立体相机，用于实时获取对机器人脚部以外的远方环境进行采集得到的宽基线图像。

图像融合子模块，用于将宽基线图像和窄基线图像进行匹配，根据匹配结果并结合机器人的精确位置信息，获得以所述精确位置信息为导航起始点的导航地图。

全景扫描子模块，用于获取全景图像。所述全景扫描子模块可以是全景相机。

障碍物扫描子模块，用于扫描机器人工作环境中的障碍物，及障碍物高度信息。所述障碍物扫描子模块可以是激光扫描雷达。

障碍物标记子模块，用于将测量得到的障碍物高度信息标注到所述全景扫描地图中，得到障碍物地图。

（4）地图融合模块，用于将所述导航地图和障碍物地图进行融合，获得标注有障碍物高度信息的地形测绘图。

（5）避障导航模块，用于根据所述地形测绘图中的障碍物高度信息判断机器人是否进行翻越或转向回避。

（6）控制器，用于根据所述避障导航模块的判断结果输出控制指令。

（7）驱动器，用于根据所述控制器输出的控制指令驱动机器人移动。

本申请实施例还公开一种机器人导航与定位的存储介质，采用如下的技术方案：

一种计算机可读存储介质，所述计算机可读存储介质上存储有计算机程序，所述计算机程序被处理器运行时执行：

获取机器人的绝对位置信息和相对位置信息；

将所述机器人的绝对位置信息和相对位置信息融合，得到机器人的精确位置信息；

采集机器人的工作环境图像，并结合所述机器人的精确位置信息得到导航地图和障碍物地图；

将所述导航地图和障碍物地图进行融合，获得标注有障碍物高度信息的地形测绘图；

根据所述地形测绘图中的障碍物高度信息判断机器人是否进行翻越或转向回避的步骤。

本申请实施例还公开一种机器人导航与定位的设备，采用如下的技术方案：

一种计算机设备，包括存储器和处理器，所述存储器上存储有能够被处理器加载并执行的计算机程序，所述处理器执行所述计算机程序时实现：

获取机器人的绝对位置信息和相对位置信息；

将所述机器人的绝对位置信息和相对位置信息融合，得到机器人的精确位置信息；

采集机器人的工作环境图像，并结合所述机器人的精确位置信息得到导航地图和障碍物地图；

将所述导航地图和障碍物地图进行融合，获得标注有障碍物高度信息的地形测绘图；

根据所述地形测绘图中的障碍物高度信息判断机器人是否进行翻越或转向回避的步骤。

以上均为本申请的较佳实施例，并非依此限制本申请的保护范围，故：凡依本申请的结构、形状、原理所做的等效变化，均应涵盖于本申请的保护范围之内。