CN109341688A - 一种基于构造顺序的地图调用定位算法 - Google Patents

Abstract

本发明公开了一种基于构造顺序的地图调用算法，步骤为：激光雷达采集环境数据，转化为笛卡尔系下的全局坐标；根据全局坐标进行区域分块、线段分割和线段表示，创建局部地图；寻找相邻局部地图得相关线段，定位巡检机器人的位姿信息；根据定位信息、规划路径和里程计数据，判断是否到达局部地图边缘，当到达局部地图边缘时，调用下一张局部地图，执行巡检任务。本发明减少了位姿估计所需的环境信息量，提高了位姿估计的效率；并且一次只需要融合两张子地图，提高了融合的效率和精确度。

Description

一种基于构造顺序的地图调用定位算法

技术领域

本发明属于机器人领域，涉及一种基于构造顺序的地图调用定位算法。

背景技术

移动机器人(Robot)是自动执行工作的机器装置，既可以接受人类指挥，又可以运行预先编排的程序，也可以根据人工智能技术制定的原则纲领行动。在山区变电站这种极端环境下可以投入移动机器人，长期来看，使用移动机器人可以节约人力成本，移动机器人***的研究引起了国内外学者越来越多的重视。

机器人想实现未知环境中探索和导航，需要先完成同步定位与地图创建。现有技术大都是创建较小或中等规模的环境地图，再进行定位。然而，随着环境规模的不断扩大，基于较小或中等规模环境地图的定位方法在精度、鲁棒性以及计算效率等方面性能较差。

发明内容

本发明所解决的技术问题在于提供一种基于构造顺序的地图调用定位算法，提高了定位的精度、效率和对环境的鲁棒性。

实现本发明目的的技术解决方案为：一种基于构造顺序的地图调用算法，包括如下步骤：

步骤1、采集环境数据：激光雷达采集环境数据，转化为笛卡尔系下的全局坐标；

步骤2、创建局部地图：根据全局坐标进行区域分块、线段分割和线段表示，创建局部地图；

步骤3、定位机器人：寻找相邻局部地图得相关线段，定位巡检机器人的位姿信息；

步骤4、调用地图：根据定位信息、规划路径和里程计数据，判断是否到达局部地图边缘，当到达局部地图边缘时，调用下一张局部地图，执行巡检任务。

本发明与现有技术相比，其显著优点为：1)本发明在激光雷达坐标系下，将环境数据聚类为障碍物，利用障碍物的全局与局部中心坐标特征来估计机器人的位姿，减少了位姿估计所需的环境信息量，提高了位姿估计的效率；2)本发明一次只需要融合两张子地图，提高了融合的效率和精确度；3)相比ICP算法直接使用距离数据点进行对应匹配，本算法计算量较小，收敛速度较快。

附图说明

图1是本发明基于构造顺序的地图调用定位算法的流程图。

图2是本发明中激光雷达获得的原始数据图。

图3是本发明经过区域分块、线段分割和线段表示得到的局部地图。

图4是本发明中线段范围的示意图。

具体实施方式

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

如图1所示，基于构造顺序的地图调用定位算法，包括如下步骤：

步骤1、采集环境数据

激光雷达LMS511采集环境数据，具体从左往右进行180度扫描，间隔0.5度，采集各个点与扫描仪的相对位置，并用激光雷达中心为极点的极坐标系表示局部坐标。

为了处理的方便，把这些以极坐标表示的数据转化为笛卡尔系下的全局坐标：

式中，d_i是激光雷达得到的极坐标系数据，(X_l,Y_l,α_l)是激光雷达中心在地图中的坐标，X_l是激光雷达在地图中的横坐标，Y_l是激光雷达再地图中的纵坐标，α_l是激光雷达主轴与X轴的夹角，激光雷达采集到的原始数据图如图2所示。

步骤2、创建局部地图

采用“聚合-分割-聚合”的方法，通过区域分块、线段分割和线段表示三个步骤，将激光雷达采集到的离散点的数据信息处理成用来表示环境信息的线段，完成局部地图的创建，具体是：

步骤2.1、区域分块

之前通过坐标转换，可以得到361个离散点的笛卡尔坐标，这361个点为一个区域，自左往右起点是第一个点，终点是第361个点，按照连续两点间的距离进行区域分块，步骤如下:

步骤2.1.1、计算连续两个点之间的距离D_j，公式如下:

如果D_j大于预先设定的阈值δ，则认为这两个点是不连续的，以后者为分割点将区域分成两块。为了取得较为精确的分块效果，采用动态阈值的方法来分块，例如扫描点到激光雷达中心的距离小于D，选取的阈值为δ，如果距离为大于D且小于2D，选取的阈值为2δ。

步骤2.1.2、判断每个区域内的扫描点的个数，如果每个区域包含点的个数小于3，该区域被视为噪声区域，舍弃这些噪声点。

步骤2.2、线段分割

经过区域分块，得到了可以用线段表示的区域B_i(i＝1,2…N)，这些区域可以由一条或者多条线段来表示，对这些区域进行线性分割，得到均可以用一条线段表示的线性区域，步骤如下：

步骤2.2.1、在区域块B_i中，以第一个点P_s(X_s,Y_s)和最后一个点P_E(X_e,Y_e)作一条直线L，计算其余各点到这条直线的距离，选出距离最大的一个点，若这个距离大于阈值α，以对应的点为分割点将这块区域分成两部分，同样采用动态阈值的方法进行线性分割，阈值α可选为直线长度L的1/10；

步骤2.2.2、重复步骤2.2.1，可以得到M个可以用一条线段表示的线性区域L_i(i＝1,2…M)，其中M不大于J。

步骤2.3、线段表示

通过步骤2.1和2.2，扫描得到的361个点被分成了J个区域分块和M个线性区域，需要构造一条线段来最大程度地描述这些点之间的线性关系。假设线性区域L_i含有的扫描点的个数为N，用W(W₁,W₂…W_N-1)来表示权重矢量，若相邻两点间的距离为σ_i,i+1，起始点到终止点的长度为L，则W_i＝(σ_i-1,i+σ_i+1,i)/2L，这样可以得到区域L_i的权重矢量W(W₁,W₂…W_N-1)。通过下面五个参数表征线段，即得描述线性区域的局部地图：

(1)权重中心：P_m(m_x,m_y)，其中

(2)协方差阵：其中 (x_i,y_i)为线性区域内扫描点的坐标；

(3)线段和X轴之间的夹角：θ＝0.5*arctan(2μ₀₁/(μ₀₀-μ₁₁))；

(4)线段的长度：L_i＝|d_x/cosθ|，其中d_x＝X_s-X_e；

(5)线段的起点和终点：

(x_s,y_s)＝(m_x-L_icosθ/2,m_y-L_isinθ/2)

(x_e,y_e)＝(m_x+L_icosθ/2,m_y+L_isinθ/2)。

经过区域分块、线段分割和线段表示三个步骤就转化为局部地图(连续的线段)，局部地图如图3所示。

步骤3、调用地图

局部子地图构好以后，巡检机器人通过自身***可以得到自己的位姿，机器人就可以在各子地图里分别进行巡检任务。由于机器人是按照一定的路径进行任务的，机器人装有里程计(可以计算行走了多少距离)，当行走定好距离时，机器就能判断其已到达地图边缘，从而调用下一张局部子地图。为使得机器人在下一张地图中巡检，我们必须知道机器人此时的精确位姿。

假设当前所在的局部子地图为A，下一张地图为B，A和B融合后的地图为全局地图，地图A和地图B之间必然存在相同的环境，因此对相同环境的扫描得到的线段应该是重合的，由于机器人硬件***得到的位姿存在误差，线段不可能完全重合，这样的线段就是相关线段。只要找到这些相关线段我们就能在地图B中确定机器人的精确位姿，具体是：

步骤3.1、确定线段范围

在寻找地图A和地图B相关线段的过程中，为了减少检查的线段数量，提出了“线段范围”的方法。如图4所示，L_G为全局地图中的一条线段，以L_G的两个端点和激光雷达中心为定点圈定一个范围R，对于给定的范围R，局部地图中的线段可分为以下四种情况：

(1)线段的两个端点都在R外部而且与R无交；

(2)线段的两个端点都在R内部；

(3)线段的一个端点在R内部，一个端点在R外部；

(4)线段的两个端点都在R外部而且与R有两个交点。

当寻找相关线段时，只考虑这些在R区域范围内的线段(如线段2，3，4)，“线段范围”缩短了寻找相关线段耗费的时间。取出全局地图的一条线段L_G，按照“线段范围”的方法，得出局部地图中符合条件的线段，如果不存在，则取出下一条全局地图的线段，再次计算，直到找到符合条件的线段L_C或取完所有全局线段为止。

步骤3.2、局部地图中的线段和全局地图中的线段是否相关

相关测试的两个条件：

Δθ＝|θ_L-θ_G|<δ_θ，其中θ_L是局部地图中线段和X轴的夹角，θ_G是全局地图中线段L_G和X轴的夹角，δ_θ是一个阈值；

线段L_C的重心到线段L_G的垂直距离D_L小于一个阈值δ_L，即D₁＝|(m_GY-m_LY)cosθ_G-(m_GX-m_LX)sinθ_G|<δ_L,其中(m_GX，m_GY)为线段L_G的重心，(m_LX，m_LY)为线段L的重心。

如果满足以上两个条件，则这两条线段是相关的。从同时满足以上两个条件的局部地图线段中，选择置信度最高，即(Δθ,D₁)最小的作为线段L_G的相关线段；重复相关判断步骤，找出所有相关的线段对，并计算得到每一对相关线段的(Δθ,D₁)。

步骤3.4、定位机器人

由于机器人硬件***存在的误差，自身***得到的位姿是不准确的，因此我们要根据求得的相关线段来完成机器人的精确定位，得到新的位姿。通过机器人自身***得到的机器人位姿(X,Y,θ)，其中(X,Y)是机器人的坐标，θ是机器人与X轴的夹角，结合求得的相关线段计算新位姿，包括：

新角度：

θ'₁＝θ+θ_G-θ_L；

L_C的新重心(m_LX'，m_LY')：

则新重心到线段L_G的距离为：

D_L＝(m_GY-m_LY')cosθ_G-(m_GX-m_LX')sinθ_G

机器人的新位姿(X'₁,Y'₁,θ'₁)为：

X'₁＝X+D_Lcosθ'₁,Y'₁＝Y+D_Lsinθ'₁；

任意选取另外三对相关线段，计算新位姿，这样总共得到四组机器人的新位姿(X_i,Y_i,θ_i)，其中i＝1,2,3,4。采用最小平方法确定机器人的精确位姿，使得这四组新位姿到精确位姿的误差的平方和最小，即满足条件：

其中，(X',Y',θ')就是机器人的精确位姿。

步骤3.5、调用地图

由于机器人是按照特定的路径来进行巡检任务的，当机器人行走一定距离并且通过里程计知道到达地图A的边缘以后开始调用地图B，通过之前的定位步骤我们知道了机器人此时的精确位姿，就可以在地图B中开始巡检，之后的地图调用是同样的方法，机器人到达地图B边缘时，调用地图C并进行精确定位，获得精确位姿，然后就可以在地图C中进行巡检任务，以此类推。

Claims (6)

1.一种基于构造顺序的地图调用算法，其特征在于，包括如下步骤：

步骤1、采集环境数据：激光雷达采集环境数据，转化为笛卡尔系下的全局坐标；

步骤2、创建局部地图：根据全局坐标进行区域分块、线段分割和线段表示，创建局部地图；

步骤3、定位机器人：寻找相邻局部地图得相关线段，定位巡检机器人的位姿信息；

步骤4、调用地图：根据定位信息、规划路径和里程计数据，判断是否到达局部地图边缘，当到达局部地图边缘时，调用下一张局部地图，执行巡检任务。

2.根据权利要求1所述的基于构造顺序的地图调用定位算法，其特征在于，步骤1中，进行180度扫描，扫描方式从左往右，扫描间隔为0.5度，采集各个点与扫描仪的相对位置，以激光雷达中心为极点的极坐标表示。

3.根据权利要求1所述的基于构造顺序的地图调用定位算法，其特征在于，步骤1中，从极坐标系转化到笛卡尔系的公式为：

式中，d_i是激光雷达得到的极坐标系数据，(X_l,Y_l,α_l)是激光雷达中心在地图中的坐标，X_l是激光雷达在地图中的横坐标，Y_l是激光雷达再地图中的纵坐标，α_l是激光雷达主轴与X轴的夹角。

4.根据权利要求1所述的基于构造顺序的地图调用定位算法，其特征在于，步骤2中，采用“聚合-分割-聚合”的方法创建局部地图，具体步骤如下：

步骤2.1、区域分块：计算连续两个点之间的距离D_j，公式如下：

如果D_j大于预先设定的阈值，则认为这两个点是不连续的，以后者为分割点将区域分成两块，为了取得较为精确的分块效果，采用动态阈值的方法来分块，扫描点到激光雷达中心的距离小于D，选取的阈值为δ，如果距离为大于D且小于2D，选取的阈值为2δ；

判断每个区域内的扫描点的个数，如果每个区域包含点的个数小于3，该区域被视为噪声区域，舍弃这些噪声点，最终得到可以用一条或者多条线段表示的区域B_i(i＝1,2…J)；

步骤2.2、线段分割：在每个区域分块B_i，以第一个点P_s(X_s,Y_s)和最后一个点P_E(X_e,Y_e)作一条直线L，计算其余各点到这条直线的距离，选出离直线距离最远的一个点，若最大距离大于阈值α，以对应的点为分割点将区域分成两块，阈值α选为直线长度L的1/10，最终得到用一条线段表示的线性区域L_i(i＝1,2…M)；

步骤2.3、线性表示：构造一条线段来最大程度地描述线性区域扫描点之间的线性关系，假设线性区域L_i含有的扫描点的个数为N，对应的权重矢量为W(W₁,W₂…W_N-1)，若相邻两点间的距离为σ_i,i+1，起始点到终止点的长度为L，则W_i＝(σ_i-1,i+σ_i+1,i)/2L，通过下面五个参数表征线段，即得描述线性区域的局部地图：

(1)权重中心：P_m(m_x,m_y)，其中

(2)协方差阵：其中 (x_i,y_i)为线性区域内扫描点的坐标；

(3)线段和X轴之间的夹角：θ＝0.5*arctan(2μ₀₁/(μ₀₀-μ₁₁))；

(4)线段的长度：L_i＝|d_x/cosθ|，其中d_x＝X_s-X_e；

(5)线段的起点和终点：

(x_s,y_s)＝(m_x-L_icosθ/2,m_y-L_isinθ/2)

(x_e,y_e)＝(m_x+L_icosθ/2,m_y+L_isinθ/2)。

5.根据权利要求1所述的基于构造顺序的地图调用定位算法，其特征在于，步骤3中，假设当前所在的局部子地图为A，下一张地图为B，A和B融合后的地图为全局地图，寻找融合地图相关线段的步骤为：

首先，确定线段范围：取全局地图中的一条线段L_G，以L_G的两个端点和激光雷达中心为定点圈定一个范围R，对于给定的范围R，局部地图中的线段可分为以下四种情况：

(1)线段的两个端点都在R外部而且与R无交；

(2)线段的两个端点都在R内部；

(3)线段的一个端点在R内部，一个端点在R外部；

(4)线段的两个端点都在R外部而且与R有两个交点；

选择在R区域范围内的局部地图线段，即第二种情况，用于后续寻找相关线段，如果不存在符合条件的局部地图线段，则取出下一条全局地图的线段重新确定线段范围，直到选到符合条件的局部地图线段或取完所有全局线段；

然后，判断局部地图中的线段和全局地图中的线段是满足如下两个条件：

(1)Δθ＝|θ_L-θ_G|<δ_θ，其中θ_L是局部地图中线段和X轴的夹角，θ_G是全局地图中线段L_G和X轴的夹角，δ_θ是一个阈值；

(2)线段L_C的重心到线段L_G的垂直距离D_L小于一个阈值δ_L，即D₁＝|(m_GY-m_LY)cosθ_G-(m_GX-m_LX)sinθ_G|<δ_L,其中(m_GX，m_GY)为线段L_G的重心，(m_LX，m_LY)为线段L_C的重心；

从同时满足以上两个条件的局部地图线段中，选择置信度最高，即(Δθ,D₁)最小的作为线段L_G的相关线段；

重复上述步骤，找出所有相关的线段对，计算得到每一对相关线段的(Δθ,D₁)。

6.根据权利要求1所述的基于构造顺序的地图调用定位算法，其特征在于，步骤3中，机器人定位的步骤为：

首先，通过机器人自身***得到机器人位姿(X,Y,θ)，其中(X,Y)是机器人的坐标，θ是机器人与X轴的夹角，结合求得的相关线段计算新位姿，包括：

新角度：

θ'₁＝θ+θ_G-θ_L

L_C的新重心(m_LX'，m_LY')：

然后，计算新重心到线段L_G的距离：

D_L＝(m_GY-m_LY')cosθ_G-(m_GX-m_LX')sinθ_G

接着，计算机器人的新位姿(X'₁,Y'₁,θ'₁)：

X'₁＝X+D_Lcosθ'₁,Y'₁＝Y+D_Lsinθ'₁

最后，任意选取另外三对相关线段，计算新位姿，这样总共得到四组机器人的新位姿(X_i,Y_i,θ_i)，其中i＝1,2,3,4，采用最小平方法确定机器人的精确位姿，使得这四组新位姿到精确位姿的误差的平方和最小，即满足条件：

其中，(X′，Y′，θ′)就是机器人的精确位姿。