CN109116851B - 一种基于地图切换的巡检机器人进出库算法 - Google Patents

Abstract

本发明公开了一种基于地图切换的巡检机器人进出库算法，机器人上电自检完成后，打开充电屋房门，加载已经构好的充电屋地图，打开激光进行匹配定位，执行出库流程；当机器人行驶至定好的切换点，关闭激光定位，关闭充点屋房门，加载已经构好巡检地图，打开激光进行匹配定位，执行巡检任务；巡检过程电量低于定值后，加载已经构好的巡检地图，打开激光进行匹配定位，执行入库流程；当机器人返回切换点，关闭激光定位，打开充点屋房门，加载充电屋地图，打开激光进行匹配定位；对接充电桩以后，关闭激光定位、充电屋房门和机器人。本发明中机器人根据电量的多少自主的进行出入库流程，不需人为的去监测，减少了人力成本。

Description

一种基于地图切换的巡检机器人进出库算法

技术领域

本发明属于机器人控制领域，具体涉及一种基于地图切换的巡检机器人进出库算法。

背景技术

移动机器人(Robot)是自动执行工作的机器装置，它既可以接受人类指挥，又可以运行预先编排的程序，也可以根据以人工智能技术制定的原则纲领行动，能够协助或取代人类完成危险的工作，例如在山区变电站这种极端环境下就可以投入移动机器人。因此引起了国内外学者越来越多的重视。移动机器人在进行巡检时，需要在出入充电屋进行巡检和充电，但是由于充电屋内外环境不同，需要创建不同的环境地图，为了避免在充电屋这种封闭的小环境中发生碰撞现象，现有技术通过人工控制进出库，自动化程度低。

发明内容

本发明的目的在于提供一种基于地图切换的巡检机器人进出库算法。

实现本发明目的的技术解决方案为：一种基于地图切换的巡检机器人进出库算法，具体内容如下：

机器人上电自检完成后，打开充电屋房门，加载已经构好的充电屋地图，打开激光进行匹配定位，执行出库流程；当机器人行驶至定好的切换点，关闭激光定位，关闭充点屋房门，加载已经构好巡检地图，打开激光进行匹配定位，执行巡检任务；

巡检过程电量低于定值后，加载已经构好的巡检地图，打开激光进行匹配定位，执行入库流程；当机器人返回切换点，关闭激光定位，打开充点屋房门，加载充电屋地图，打开激光进行匹配定位；对接充电桩以后，关闭激光定位、充电屋房门和机器人。

本发明与现有技术相比，其显著优点为：1)本发明中机器人根据电量的多少自主的进行出入库流程，不需人为的去监测，减少了人力成本；2)本发明对运行过程中发生的滑动、碰撞等随机噪声具有更强的鲁棒性，即对于未知环境具有更强的适应性。

附图说明

图1是本发明基于地图切换的巡检机器人进出库算法的流程图。

具体实施方式

下面结合附图和具体实施方式进一步说明本发明的原理和方案。

如图1所示，基于地图切换的巡检机器人进出库定位算法，具体内容如下：

机器人上电自检完成后，打开充电屋房门，加载已经构好的充电屋地图，打开激光进行匹配定位，执行出库流程；当机器人行驶至定好的切换点，关闭激光定位，关闭充点屋房门，加载已经构好巡检地图，打开激光进行匹配定位，执行巡检任务；

巡检过程电量低于定值后，加载已经构好的巡检地图，打开激光进行匹配定位，执行入库流程；当机器人返回切换点，关闭激光定位，打开充点屋房门，加载充电屋地图，打开激光进行匹配定位；对接充电桩以后，关闭激光定位、充电屋房门和机器人。

由于所巡检的环境和充电房不存在合作路标，地图的匹配和机器人的自定位只能依靠地图模型或自定义路标的匹配来完成。本发明采用的是路标匹配的方法，路标是指地图中特征线段所在直线的交点，下面分别介绍特征线段、路标提取，路标匹配和定位。

步骤1、获取特征线段

机器人用激光雷达探测周围的环境，得到障碍物方向和距离的信息。这些激光雷达采集到的点在结构化的环境中提供环境的信息很大一部分是冗余的，可以把这些特征提取出来，建立基于简单线段特征的地图，实质上就是对激光雷达的原始数据进行了压缩。

要想获得特征线段，首先，将激光雷达探测到的离散点分成不同的簇，用集合C表示连续的障碍物表面，这里C＝{c_i},i＝1,2,3,…,N，c_i满足|c_ij-c_i(j+1)|<D_thr，其中c_ij表示c_i中第j个点到激光雷达的距离，D_thr为自适应阈值；其次，点簇中的点不一定在同一条直线上，需要把点簇c_i再细分为：c_i＝{s_ij}，其中s_ij中的点都处在同一条直线上。最后，将所有s_ij中的点拟合直线，找出起始点和终点，得到地图的特征线段。

步骤2、提取路标

根据特征线段就可以进行路标的提取，特征线段的交点就是路标的坐标信息，其中全局路标集用M＝{x₁,…,x_n}表示，x_i＝(x,y)表示从全局地图中生成的路标，(x,y)是横纵轴坐标，局部路标集用S＝{S₁,...S_n}表示，S_k＝{x,y}表示局部地图中的路标；路标之间的相互几何关系用矩阵R表示，其中R_ij为路标x_i与路标x_j或者S_i与S_j的集合距离。这样就完成了路标的提取，接下来就是进行匹配。

步骤3、路标匹配

进行路标的提取以后，需要将全局路标和局部路标进行比较，找出匹配的路标。首先找出

(M为全局路标集，S为局部路标集)，使得路标匹配对(x_i，s_i)∈X_m是全局地图下和局部地图下相同的路标，设在全局路标相关矩阵R_M和局部路标相关矩阵R_S中，R_M的行(列)数为n_M，R_S的行(列)为n_S。定义R_M的第p行(列)与R_S的第q行(列)相等的元素(在一定的误差范围内，实际应用中确定误差阈值)的个数为全局地图中的路标P与局部地图中的路标q的匹配度，用η(p,q)表示。

将局部地图中的所有路标与全局地图中的路标依次进行比较，即比较R_M的每一行(列)与R_S的每一行(列)，因为在全局地图和局部地图中，只有相互对应的路标关联的行(列)有最大的匹配度，找各个路标之间的匹配度，进行匹配度寻优，据以下评判标准找出最优的匹配对：

匹配判定标准1：

匹配判定标准2：η(p,q)≥n_s-3,其中p＝1,2,3,…,n_M,q＝1,2,3,…,n_s.

标准1保证了匹配对是最优的匹配对.考虑到全局路标和局部路标只是部分相关，标准2要求配度必须大于局部地图路标数n_s-3，确保了匹配的正确性，根据以上判定标准，找出所有的路标匹配对X_m＝{(x_i,s_j)}。

步骤4、机器人定位

完成匹配后就是对机器人的定位，需要知道两个信息，一个是机器人的位姿，第二个就是机器人位姿的误差，知道这两个信息以后就可以完成定位。

路标匹配对(x_i,s_j)∈X_m是同一个路标分别在全局坐标系和局部坐标系的表示。这两个坐标系的关系可通过平移和旋转相转换，平移和旋转的值即机器人位姿X_R＝(x^R,y^R,θ)^T。将局部地图中路标s_i转换到全局坐标

中可得

其中：

对于如何求解R(θ)和T，本发明采用奇异值分解的算法。奇异值分解(SVD)是矩阵分析中的一个有效方法，对最小二乘问题有重要的应用，是比较成熟的算法，因此只给出结果：

其中

x_gi为全局地图下的路标，x_1i为全局地图下的路标，

为局部地图下路标的均值，

为全局地图下路标的均值。矩阵H的奇异值分解为H＝UΛV^T,则旋转矩阵为：R^*＝VU^T，偏移

机器人位姿的误差用机器人估计位姿的协方差矩阵表示，Borges在他的文章里对此有详细论述，在此直接引用其结果，机器人估计位姿的协方差矩阵为：

其中

为全局坐标系中机器人的位姿，

C_G,C_L,C_res分别为全局坐标的误差、局部坐标的误差以及残差的误差，其中Λ_G,Λ_L为全局路标和局部路标的协方差矩阵，Λ_r为变残差协方差矩阵，r_i为残差向量，分别用下式表示：

应用以上得到的信息，把局部地图按照机器人位姿平移旋转，转换到全局坐标系中，全局地图中特征线段用l^G＝(ρ^G,α^G)^T表示，局部地图中特征线段用l^L＝(ρ^L,α^L)^T表示，则线段参数转换可以表示如下：

其中

完成特征线段参数转换以后，将协方差矩阵C_R和特征线段l^G代入误差传递公式里，可以解决误差问题，最终完成机器人的定位。完成定位以后配合里程计，机器人到达指定的地图切换点。

Claims (8)

1.基于地图切换的巡检机器人进出库算法，其特征在于，具体内容如下：

机器人上电自检完成后，打开充电屋房门，加载已经构好的充电屋地图，打开激光进行匹配定位，执行出库流程；当机器人行驶至定好的切换点，关闭激光定位，关闭充电屋房门，加载已经构好巡检地图，打开激光进行匹配定位，执行巡检任务；

巡检过程电量低于定值后，加载已经构好的巡检地图，打开激光进行匹配定位，执行入库流程；当机器人返回切换点，关闭激光定位，打开充电屋房门，加载充电屋地图，打开激光进行匹配定位；对接充电桩以后，关闭激光定位、充电屋房门和机器人；

采用路标匹配的方法进行激光匹配定位，具体方法为：

步骤1、获取特征线段：激光雷达探测环境信息，将探测数据点分成不同的点簇，使点簇中的点都处在同一条直线上，即得地图的特征线段；

步骤2、提取路标：根据地图的特征线段确定全局路标集和局部路标集，进而确定全局路标集和局部路标集的相关矩阵；

步骤3、路标匹配：比较全局路标集和局部路标集的相关矩阵，找出全局路标集和局部路标集的路标匹配对；

步骤4、机器人定位：根据路标匹配确定机器人位姿和位姿的估计误差，完成定位。

2.根据权利要求1所述的基于地图切换的巡检机器人进出库算法，其特征在于，步骤1中，探测数据点分成用一条直线表示的点簇的方法为：

用集合C代表激光雷达探测到的连续的障碍物表面，根据设定的距离阈值，将集合中C中的离散点分成不同的簇，即C＝{c_i}，i＝1，2，3，…，N，c_i满足|c_ij-c_i(j+1)|＜D_thr，其中c_ij表示c_i中第j个点到激光雷达的距离，D_thr为自适应阈值；

由于点簇c_i中的点不一定在同一条直线上，把点簇c_i再细分，直到点簇的点能够用一条直线表示，即c_i＝{s_ij}，其中s_ij中的点都处在同一条直线上。

3.根据权利要求2所述的基于地图切换的巡检机器人进出库算法，其特征在于，步骤2中，确定地图的特征线段的交点即得路标的全局坐标和局部坐标，设全局路标集用M＝{x₁，…，x_n}表示，x_i＝(x，y)表示从全局地图中生成的路标，(x，y)是横纵轴坐标，局部路标集用S＝{s₁，...s_n}表示，s_k＝{x，y}表示局部地图中的路标，路标之间的相互几何关系用矩阵R表示，R_ij为第i个路标与第j个路标的集合距离，对全局路标集和局部路标集求路标的集合距离，即得全局路标相关矩阵R_M和局部路标相关矩阵R_S，这样就完成了路标的提取。

4.根据权利要求3所述的基于地图切换的巡检机器人进出库算法，其特征在于，步骤3中，找出匹配的路标即从全局路标集M和局部路标集S中找出子集

使得路标匹配对(x_i，s_i)∈X_m是全局地图下和局部地图下相同的路标，具体方法是：设全局路标相关矩阵R_M和局部路标相关矩阵R_S中，R_M的行/列数为n_M，R_S的行/列为n_S，定义R_M的第p行/列与R_S的第q行/列元素差值小于设定误差阈值的个数为全局地图中的路标P与局部地图中的路标Q的匹配度，用η(p，q)表示，找出满足如下两个判定标准的匹配度，即得路标匹配对，判定标准具体为：

匹配判定标准1：

匹配判定标准2：η(p，q)≥n_s-3，其中p＝1，2，3，…，n_M，q＝1，2，3，…，n_s.

即得，所有的路标匹配对：X_m＝{(x_i，s_i)}。

5.根据权利要求2所述的基于地图切换的巡检机器人进出库算法，其特征在于，步骤4中，将路标匹配对局部地图中的路标s_i转换到全局地图中的路标表示

即：

其中

分别表示坐标系转化的旋转值和平移值，θ表示机器人位姿中的角度，即得机器人位姿X_R＝(x^R，y^R，θ)^T。

6.根据权利要求5所述的基于地图切换的巡检机器人进出库算法，其特征在于，步骤4中，采用奇异值分解的方法确定旋转值和平移值，设

其中

x_gi为全局地图下的路标，x_1i为局部地图下的路标，

为局部地图下路标的均值，

为全局地图下路标的均值，矩阵H的奇异值分解为H＝UΛV^T，则旋转矩阵为：R^*＝VU^T，平移值

7.根据权利要求2所述的基于地图切换的巡检机器人进出库算法，其特征在于，步骤4中，用机器人估计位姿的协方差矩阵表示机器人位姿的误差，表示为：

其中

为全局坐标系中机器人的位姿，

C_G，C_L，C_res分别为全局坐标的误差、局部坐标的误差以及残差的误差，Λ_G，Λ_L为全局路标和局部路标的协方差矩阵，Λ_r为变残差协方差矩阵，r_i为残差向量，分别用下式表示：

其中，x_i为全局地图中的路标，s_i为局部地图中的路标，N表示自然数。

8.根据权利要求1所述的基于地图切换的巡检机器人进出库算法，其特征在于，步骤4中，把局部特征线段按照机器人位姿平移旋转转换到全局坐标系中，再将机器人位姿的误差和转换后的特征线段代入误差传递公式，最终完成机器人的定位。

Images