CN103674015B - 一种无轨化定位导航方法及装置 - Google Patents

Abstract

本发明提供一种无轨化定位导航方法及装置，通过预先设置的航位推算公式以及测量得到的相邻时间点t‑1到t之间机器人沿朝向方向运动过的距离以及在水平面上的航向变化率计算得到机器人的位置预测值，然后提取激光传感器测量得到的激光数据中的局部特征信息，并利用SLAM技术将局部特征信息融合成一个统一的全局特征地图，进而实现对机器人位置的估计以及生成拓扑图的过程，使得在降低成本及运维费用的基础上，提高定位导航的稳定性和可靠性，满足机器人在自然环境中运动的需要。

Description

一种无轨化定位导航方法及装置

技术领域

本申请涉及机器人自主运动控制技术领域，特别是涉及一种无轨化定位导航方法及装置。

背景技术

在变电站内智能巡检机器人的技术方案经过几年的发展和推广后，变的日趋成熟，现有技术中，变电站内机器人定位导航所采用的技术主要有以下几种：

1、磁条引导加RFID(radio frequency identification，射频识别)标签定位方法，该方法主要依靠安装于机器人底盘前部的磁传感器阵列检测机器人相对于磁条轨迹的偏离，通过控制使机器人保持沿磁条中心的轨迹运动，并将RFID标签用作机器人的巡检点停靠。该方法虽然导航重复度好、抗干扰能力强，但是磁条轨迹的铺设涉及大量现场施工、磁条成本及磁条维护成本、一旦磁条铺设完工则巡检路线不能灵活改变、底盘受限于磁传感器检测距离，使得机器人只能在平地上运动，对于草地等自然地形不能适用。

2、基于GPS和磁罗盘的定位方法，该方法主要采用高精度的差分GPS定位系统作定位，用磁罗盘传感器检测地磁来确定机器人的朝向。该方法虽然规避了前一种方法的大部分缺点，但高精度GPS成本高，且由于变电站为高电场高磁场环境，而GPS和磁罗盘传感器都易受电磁干扰影响，所以该方法并不能在稳定性和可靠性上得到保证。

3、基于激光传感器和反光标识的定位导航方法，该方法通过在巡检路径两侧安装固定几何形状的标识物，利用激光传感器检测环境中的反光标识，至少检测到3个反光标识就可以计算得到与反光标识(反光标识坐标已知)同一全局坐标系下传感器所处位置和朝向。该方法虽然可靠而且精度好，机器人也能实现草地等全地形的运动，但是该方法需要设立反光标识物，且反光标识的全局位置需要事前已知，反光标志物全局位置的误差直接影响机器人最终的定位导航精度，虽然可灵活改变机器人路线，但需要在路线两边设置反光标识因此仍然存在一定的运维费用。

因此，亟需一种定位导航方法，在降低成本及运维费用的基础上，提高定位导航的稳定性和可靠性，满足机器人在自然环境中运动的需要。

发明内容

有鉴于此，本申请实施例提供一种无轨化定位导航方法，在降低成本及运维费用的基础上，提高定位导航的稳定性和可靠性，满足机器人在自然环境中运动的需要。

为了实现上述目的，本申请实施例提供的技术方案如下：

一种无轨化定位导航方法，包括：

测量相邻时间点t-1到t之间机器人沿朝向方向运动过的距离以及在水平面上的航向变化率；

根据所述距离、所述航向变化率以及预先设置的航位推算公式计算t时刻机器人的位置预测值；

提取激光传感器测量得到的激光数据中的局部特征信息；

利用同时定位与地图构建SLAM技术将所述局部特征信息融合成一个统一的全局特征地图；

利用所述全局特征地图、所述位置预测值以及预先设置的特征匹配算法计算t时刻机器人的位置估计值；

采用状态映射的方法将基于所述全局特征地图的矢量图转换为拓扑图，以实现对机器人的无轨化定位导航。

优选的，还包括：利用所述位置估计值对利用SLAM技术将所述局部特征信息融合成一个统一的全局特征地图的过程进行修正。

优选的，包括：

通过里程计测量相邻时间点t-1到t之间机器人沿朝向方向运动过的距离。

优选的，包括：

通过惯性测量单元IMU测量机器人在水平面上的航向变化率。

优选的，所述激光数据为激光传感器通过扫描式测量得到的二维平面180°或270°范围内机器人与各个障碍物之间的实际距离。

优选的，提取激光传感器测量得到的激光数据中的局部特征信息，包括：

提取激光传感器测量得到的激光数据中的线段特征；

判断是否完成提取线段特征的过程；

当是时，在所述激光数据中提取线段特征后剩余的数据集中提取圆弧特征。

优选的，还包括：

接收用户输入的机器人的目标位置信息；

获取机器人的当前位置信息；

在所述拓扑图中搜索与所述当前位置信息以及目标位置信息相对应的路径。

优选的，还包括：

在基于全局特征地图的矢量图中显示所述路径。

一种无轨化定位导航装置，包括：测量单元、位置预测单元、局部特征信息提取单元、全局特征地图生成单元、位置估计单元以及拓扑图生成单元，其中，

所述测量单元用于测量相邻时间点t-1到t之间机器人沿朝向方向运动过的距离以及在水平面上的航向变化率；

所述位置预测单元与所述测量单元相连接，用于根据所述距离、所述航向变化率以及预先设置的航位推算公式计算t时刻机器人的位置预测值；

所述局部特征信息提取单元用于提取激光传感器测量得到的激光数据中的局部特征信息；

所述全局特征地图生成单元与所述局部特征信息提取单元相连接，用于利用SLAM技术将所述局部特征信息融合成一个统一的全局特征地图；

所述位置估计单元的一端与所述位置预测单元相连接，另一端与所述全局特征地图生成单元相连接，用于利用所述全局特征地图、所述位置预测值以及预先设置的特征匹配算法计算t时刻机器人的位置估计值；

所述拓扑图生成单元与所述全局特征地图生成单元相连接，用于采用状态映射的方法将基于所述全局特征地图的矢量图转换为拓扑图，以实现对机器人的无轨化定位导航。

优选的，所述局部特征信息提取单元包括：线段特征提取单元、判断单元以及圆弧特征提取单元，其中，

所述线段特征提取单元用于提取激光传感器测量得到的激光数据中的线段特征；

所述判断单元与所述线段特征提取单元相连接，用于判断是否完成提取线段特征的过程；

所述圆弧特征提取单元的一端与所述线段特征提取单元相连接，另一端与所述判断单元相连接，用于在判断出已完成对线段特征的提取过程后，在所述激光数据中提取线段特征后剩余的数据集中提取圆弧特征。

本发明提供一种无轨化定位导航方法及装置，通过预先设置的航位推算公式以及测量得到的相邻时间点t-1到t之间机器人沿朝向方向运动过的距离以及在水平面上的航向变化率计算得到机器人的位置预测值，然后提取激光传感器测量得到的激光数据中的局部特征信息，并利用SLAM技术将局部特征信息融合成一个统一的全局特征地图，进而实现对机器人位置的估计以及生成拓扑图的过程，使得在降低成本及运维费用的基础上，提高定位导航的稳定性和可靠性，满足机器人在自然环境中运动的需要。

附图说明

为了更清楚地说明本申请实施例或现有技术中的技术方案，下面将对实施例或现有技术描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本申请中记载的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。

图1为本申请实施例一提供的一种无轨化定位导航方法流程图；

图2为本申请实施例一提供的一种特征匹配原理图；

图3为本申请实施例一提供的一种基于Bezier曲线的矢量图；

图4为本申请实施例一提供的针对图3所示的基于Bezier曲线的矢量图所生成的拓扑图；

图5为本申请实施例二提供的一种无轨化定位导航装置的结构示意图。

具体实施方式

为使本发明的上述目的、特征和优点能够更加明显易懂，下面结合附图和具体实施方式对本发明作进一步详细的说明。

实施例一：

图1为本申请实施例一提供的一种无轨化定位导航方法流程图。

如图1所示，该方法包括：

S101、测量相邻时间点t-1到t之间机器人沿朝向方向运动过的距离以及在水平面上的航向变化率。

在本申请实施例中，通过里程计测量相邻时间点t-1到t之间机器人沿朝向方向运动过的距离，并通过IMU(Inertial Measurement Unit，惯性测量单元)测量机器人在水平面上的航向变化率。

S102、根据所述距离、所述航向变化率以及预先设置的航位推算公式计算t时刻机器人的位置预测值。

在本申请实施例中，设通过里程计测量得到的相邻时间点t-1到t之间机器人沿朝向方向运动过的距离为Δd，通过IMU测量得到的机器人在水平面上的航向变化率为ω，且设时间点t-1到t之间的时间增量为Δt，t-1时刻机器人位姿O_t-1＝(x_t-1,y_t-1,θ_t-1)^T，则t时刻机器人位姿O_t＝(x_t,y_t,θ_t)^T按照航位推算公式计算为：

S103、提取激光传感器测量得到的激光数据中的局部特征信息。

在本申请实施例中，激光传感器可以通过扫描式测量二维平面180°或270°范围内机器人与各个障碍物之间的实际距离，进而生成激光数据。

在生成激光数据后，首先提取该激光数据中的线段特征，该线段特征的提取过程为：1)、利用霍夫变换初步确定哪些点属于哪些线段；2)、利用同线性判断合并大致重合的线段；3)、利用最小二乘拟合精确的直线参数。每一个特征线段最后可由f＝(c,θ,l,P,σ)五项参数描述，其中c,θ为直线的参数方程参数，直线方程表示为：

xcosθ+ysinθ+c＝0

其中，σ为线段拟合残差，表示线段特征的不确定性，P＝(x_c,y_c)^T为线段中心点位置，参数l为线段长度属性。

然后判断是否已经完成对线段特征的提取过程，当是时，在激光数据中提取线段特征后剩余的数据集中提取圆弧特征，提取圆弧特征的过程为：1)、聚类，将数据集分割成几组类内点距离变化小、角度相邻的点；2)、最小二乘拟合圆弧；3)、根据已知圆柱直径取值范围过滤出符合直径要求的圆弧；每一个特征圆弧最后可由c＝(P,r,θ_s,θ_e,l,σ)六项参数描述，其中P＝(x_c,y_c)^T为圆心位置，r为半径，确定了圆弧所在圆的方程：

(x-x_c)²+(y-y_c)²＝r²

θ_s,θ_e为圆弧起始位置和终止位置对应的圆心角，参数l为圆弧长度属性，σ为圆弧的拟合残差，表示圆弧特征的不确定性。

S104、利用同时定位与地图构建SLAM技术将所述局部特征信息融合成一个统一的全局特征地图。

当提取了提取激光传感器测量得到的激光数据中的局部特征信息后，利用SLAM(Simultaneous Localization And Mapping，同时定位与地图构建)技术将所述局部特征信息融合成一个统一的全局特征地图，该全局特征地图包括线段特征和圆弧特征的集合，

FMap＝{f_i,i＝1,...,n；c_k,k＝1,...,m}.

其中，n为全局特征地图中所包含的线段特征数，m为全局特征地图中所包含的圆弧特征数。

S105、利用所述全局特征地图、所述位置预测值以及预先设置的特征匹配算法计算t时刻机器人的位置估计值。

在本申请实施例中，一次性激光扫描的数据由K条独立的射线概率联合组成，即

其中，为第k条射线的概率，其意义为：机器人在估计位置时，该条射线在距离激光传感器发射原点的位置遇到地图上所标识障碍物的概率。首先对地图上的线段特征和圆弧特征构建观测概率图，如图2所示的特征匹配原理图，以直线段特征为例采用零均值高斯概率分布，空间中距离特征线段越近的位置观测概率越大，图中的颜色越亮。只有在正确的估计位置，所有的激光射线才能与地图特征有很好的相合程度，此时射线联合概率会得到较高的概率值，因此求解一个使函数取最大值的参数作为t时刻机器人的位置估计值。

S106、采用状态映射的方法将基于所述全局特征地图的矢量图转换为拓扑图，以实现对机器人的无轨化定位导航。

采用状态映射的方法将基于所述全局特征地图的矢量图转换为拓扑图，以实现对机器人的无轨化定位导航的过程为：

1)、将全局特征地图看成一个基于Bezier(贝塞尔)曲线的矢量图，如图3所示的基于Bezier曲线的矢量图，该图中的Node的意思为节点，该矢量图包括的基本元素有：节点V＝{v_i,i＝1,...,n；}.其中v_i＝(x,y)^T为节点在全局特征地图中的坐标，2阶Bezier曲线的边E＝{e_i,i＝1,...,n；}.，其中e_i＝[p_s,p_c1,p_c2,p_e]由4个点指定一条曲线。

2)、考虑机器人有一定的运动约束和实际使用中的人为约束，在路径规划中不能将机器人视为一个质点，而看成一个矢量。约定：(1)机器人在任何时候不能脱离边E运动；(2)在一条边上机器人保持一种运动状态；(3)机器人在有限的节点处允许原地掉转头。

3)、机器人在每一条边上都有4种运动状态，如图4所示的是针对图3所示的基于Bezier曲线的矢量图所生成的拓扑图，其中，在每一个节点处根据实际情况/人为要求，确定这些运动状态之间是否能够连通，例如E1状态和E5状态机器人的运动状态一致可以连通，而E1和E8这两种状态之间只有当允许在节点原地旋转的条件下才能连通，否则这两种状态不连通。

根据这样的规律将每条Bezier边扩展成4种状态，形成集合EI＝{E_i,i＝1,...,4*n；}.并根据各个节点处实际需求标记集合EI中元素的连通性。

进一步的，在本申请实施例提供的一种无轨化定位导航方法中还包括：

接收用户输入的机器人的目标位置信息；获取机器人的当前位置信息；在所述拓扑图中搜索与所述当前位置信息以及目标位置信息相对应的路径。

在本申请实施例中，可以接收用户输入的机器人的目标位置信息，该目标位置信息为集合EI中的某一个元素EI_end，获取机器人的当前位置信息，该当前位置信息也可以标记为集合EI中的某一个元素EI_start，然后在EI所构成的拓扑图中搜索EI_start到EI_end的一条路径。

进一步的，在本申请实施例一提供的一种无轨化定位导航方法中还包括：

在基于全局特征地图的矢量图中显示所述路径。

在本申请实施例中，将全局特征地图看成一个基于Bezier曲线的矢量图，如果搜索到一条通路P＝{E_i,i∈[1,4n]}.需要将通路P映射回原来Bezier矢量地图的边的通路，即p＝{e_i,i∈[1,n]}.，并在基于全局特征地图的矢量图中显示该路径。

本发明提供一种无轨化定位导航方法，通过预先设置的航位推算公式以及测量得到的相邻时间点t-1到t之间机器人沿朝向方向运动过的距离以及在水平面上的航向变化率计算得到机器人的位置预测值，然后提取激光传感器测量得到的激光数据中的局部特征信息，并利用SLAM技术将局部特征信息融合成一个统一的全局特征地图，进而实现对机器人位置的估计以及生成拓扑图的过程，使得在降低成本及运维费用的基础上，提高定位导航的稳定性和可靠性，满足机器人在自然环境中运动的需要。

实施例二：

图5为本申请实施例二提供的一种无轨化定位导航装置的结构示意图。

如图5所示，该装置包括：测量单元1、位置预测单元2、局部特征信息提取单元3、全局特征地图生成单元4、位置估计单元5以及拓扑图生成单元6，其中，

所述测量单元1用于测量相邻时间点t-1到t之间机器人沿朝向方向运动过的距离以及在水平面上的航向变化率。

所述位置预测单元2与所述测量单元1相连接，用于根据所述距离、所述航向变化率以及预先设置的航位推算公式计算t时刻机器人的位置预测值。

所述局部特征信息提取单元3用于提取激光传感器测量得到的激光数据中的局部特征信息。

所述全局特征地图生成单元4与所述局部特征信息提取单元3相连接，用于利用SLAM技术将所述局部特征信息融合成一个统一的全局特征地图。

所述位置估计单元5的一端与所述位置预测单元2相连接，另一端与所述全局特征地图生成单元4相连接，用于利用所述全局特征地图、所述位置预测值以及预先设置的特征匹配算法计算t时刻机器人的位置估计值。

所述拓扑图生成单元6与所述全局特征地图生成单元4相连接，用于采用状态映射的方法将基于所述全局特征地图的矢量图转换为拓扑图，以实现对机器人的无轨化定位导航。

进一步的，在本申请实施例二提供的无轨化定位导航装置中，所述局部特征信息提取单元3包括：包括：线段特征提取单元、判断单元以及圆弧特征提取单元，其中，

所述线段特征提取单元用于提取激光传感器测量得到的激光数据中的线段特征。

所述判断单元与所述线段特征提取单元相连接，用于判断是否完成提取线段特征的过程。

所述圆弧特征提取单元的一端与所述线段特征提取单元相连接，另一端与所述判断单元相连接，用于在判断出已完成对线段特征的提取过程后，在所述激光数据中提取线段特征后剩余的数据集中提取圆弧特征。

本发明提供一种无轨化定位导航装置，包括：测量单元、位置预测单元、局部特征信息提取单元、全局特征地图生成单元、位置估计单元以及拓扑图生成单元，位置预测单元通过预先设置的航位推算公式以及测量单元测量得到的相邻时间点t-1到t之间机器人沿朝向方向运动过的距离以及在水平面上的航向变化率计算得到机器人的位置预测值，然后局部特征信息提取单元提取激光传感器测量得到的激光数据中的局部特征信息，并由全局特征地图生成单元利用SLAM技术将局部特征信息融合成一个统一的全局特征地图，进而通过位置估计单元以及拓扑图生成单元实现对机器人位置的估计以及生成拓扑图的过程，使得在降低成本及运维费用的基础上，提高定位导航的稳定性和可靠性，满足机器人在自然环境中运动的需要。

本说明书中各个实施例采用递进的方式描述，每个实施例重点说明的都是与其他实施例的不同之处，各个实施例之间相同相似部分互相参见即可。对于实施例公开的装置而言，由于其与实施例公开的方法相对应，所以描述的比较简单，相关之处参见方法部分说明即可。

以上仅是本申请的优选实施方式，使本领域技术人员能够理解或实现本申请。对这些实施例的多种修改对本领域的技术人员来说将是显而易见的，本文中所定义的一般原理可以在不脱离本申请的精神或范围的情况下，在其它实施例中实现。因此，本申请将不会被限制于本文所示的这些实施例，而是要符合与本文所公开的原理和新颖特点相一致的最宽的范围。

Claims (9)

1.一种无轨化定位导航方法，其特征在于，包括：

测量相邻时间点t-1到t之间机器人沿朝向方向运动过的距离以及在水平面上的航向变化率；

根据所述距离、所述航向变化率以及预先设置的航位推算公式计算t时刻机器人的位置预测值；

提取激光传感器测量得到的激光数据中的局部特征信息；

利用同时定位与地图构建SLAM技术将所述局部特征信息融合成一个统一的全局特征地图；

利用所述全局特征地图、所述位置预测值以及预先设置的特征匹配算法计算t时刻机器人的位置估计值；

采用状态映射的方法将基于所述全局特征地图的矢量图转换为拓扑图，以实现对机器人的无轨化定位导航；

利用所述位置估计值对利用SLAM技术将所述局部特征信息融合成一个统一的全局特征地图的过程进行修正。

2.根据权利要求1所述的方法，其特征在于，包括：

通过里程计测量相邻时间点t-1到t之间机器人沿朝向方向运动过的距离。

3.根据权利要求1所述的方法，其特征在于，包括：

通过惯性测量单元IMU测量机器人在水平面上的航向变化率。

4.根据权利要求1所述的方法，其特征在于，所述激光数据为激光传感器通过扫描式测量得到的二维平面180°或270°范围内机器人与各个障碍物之间的实际距离。

5.根据权利要求1所述的方法，其特征在于，提取激光传感器测量得到的激光数据中的局部特征信息，包括：

提取激光传感器测量得到的激光数据中的线段特征；

判断是否完成提取线段特征的过程；

当是时，在所述激光数据中提取线段特征后剩余的数据集中提取圆弧特征。

6.根据权利要求1所述的方法，其特征在于，还包括：

接收用户输入的机器人的目标位置信息；

获取机器人的当前位置信息；

在所述拓扑图中搜索与所述当前位置信息以及目标位置信息相对应的路径。

7.根据权利要求6所述的方法，其特征在于，还包括：

在基于全局特征地图的矢量图中显示所述路径。

8.一种无轨化定位导航装置，其特征在于，包括：测量单元、位置预测单元、局部特征信息提取单元、全局特征地图生成单元、位置估计单元以及拓扑图生成单元，其中，

所述测量单元用于测量相邻时间点t-1到t之间机器人沿朝向方向运动过的距离以及在水平面上的航向变化率；

所述位置预测单元与所述测量单元相连接，用于根据所述距离、所述航向变化率以及预先设置的航位推算公式计算t时刻机器人的位置预测值；

所述局部特征信息提取单元用于提取激光传感器测量得到的激光数据中的局部特征信息；

所述全局特征地图生成单元与所述局部特征信息提取单元相连接，用于利用SLAM技术将所述局部特征信息融合成一个统一的全局特征地图；

所述位置估计单元的一端与所述位置预测单元相连接，另一端与所述全局特征地图生成单元相连接，用于利用所述全局特征地图、所述位置预测值以及预先设置的特征匹配算法计算t时刻机器人的位置估计值；

所述拓扑图生成单元与所述全局特征地图生成单元相连接，用于采用状态映射的方法将基于所述全局特征地图的矢量图转换为拓扑图，以实现对机器人的无轨化定位导航；利用所述位置估计值对利用SLAM技术将所述局部特征信息融合成一个统一的全局特征地图的过程进行修正。

9.根据权利要求8所述的装置，其特征在于，所述局部特征信息提取单元包括：线段特征提取单元、判断单元以及圆弧特征提取单元，其中，

所述线段特征提取单元用于提取激光传感器测量得到的激光数据中的线段特征；

所述判断单元与所述线段特征提取单元相连接，用于判断是否完成提取线段特征的过程；

所述圆弧特征提取单元的一端与所述线段特征提取单元相连接，另一端与所述判断单元相连接，用于在判断出已完成对线段特征的提取过程后，在所述激光数据中提取线段特征后剩余的数据集中提取圆弧特征。