CN111421548B - 一种移动机器人定位方法及*** - Google Patents

一种移动机器人定位方法及*** Download PDF

Info

Publication number
CN111421548B
CN111421548B CN202010317589.3A CN202010317589A CN111421548B CN 111421548 B CN111421548 B CN 111421548B CN 202010317589 A CN202010317589 A CN 202010317589A CN 111421548 B CN111421548 B CN 111421548B
Authority
CN
China
Prior art keywords
particle
particles
robot
pose
cell
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Active
Application number
CN202010317589.3A
Other languages
English (en)
Other versions
CN111421548A (zh
Inventor
王志红
王奔
邹斌
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Wuhan University of Technology WUT
Original Assignee
Wuhan University of Technology WUT
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Wuhan University of Technology WUT filed Critical Wuhan University of Technology WUT
Priority to CN202010317589.3A priority Critical patent/CN111421548B/zh
Publication of CN111421548A publication Critical patent/CN111421548A/zh
Application granted granted Critical
Publication of CN111421548B publication Critical patent/CN111421548B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
    • B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
    • B25J9/00—Programme-controlled manipulators
    • B25J9/16—Programme controls
    • B25J9/1656—Programme controls characterised by programming, planning systems for manipulators
    • B25J9/1664—Programme controls characterised by programming, planning systems for manipulators characterised by motion, path, trajectory planning

Landscapes

  • Engineering & Computer Science (AREA)
  • Robotics (AREA)
  • Mechanical Engineering (AREA)
  • Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)

Abstract

本发明公开一种移动机器人定位方法及***，属于移动机器人定位技术领域，解决了现有技术无法适应视角小于360°的激光雷达及对不同工况适应性不强的问题。移动机器人定位方法，包括：获取二维占用单元格地图，将二维占用单元格地图转换为三维单元格地图，获取三维单元格地图中单元格的能量值，确定粒子集Xt；更新粒子的位姿，获取粒子的预测能量值，获取机器人的真实能量值
Figure DDA0002460128870000011
根据预测能量值和真实能量值确定粒子的权重，获取机器人的当前位姿；由粒子集XL和粒子集XG组成新的粒子集；根据新的粒子集，不断更新粒子的位姿，得到机器人在移动中的位姿。实现了机器人在移动中位姿的确定，并适用于各种可视角度激光雷达，对各种工况具有较好的适应性。

Description

一种移动机器人定位方法及***
技术领域
本发明涉及移动机器人定位技术领域，尤其是涉及一种移动机器人定位方法及***。
背景技术
移动机器人定位技术一直是研究热门，移动机器人要实现功能，首先必须知道它在全局地图中的位置。定位***通过传感器的信息与地图数据得到机器人在地图中的坐标，2D激光雷达是一种距离传感器，能够获得周围障碍物的距离与方位信息，探测距离能够达到数十米，最大误差能够控制在厘米级别。
基于已知地图的定位，目前有自适应蒙特卡洛定位算法(Self-Adaptive MonteCarlo Localization，SAMCL)，这种算法基于蒙特卡洛定位算法；蒙特卡洛定位算法使用大量带有权重的粒子描述机器人在地图中的概率分布。通过简单的地图预处理降低了实时运算量，使用静态的粒子分配，将粒子分为两个集合，分别用于全局定位与局部定位，从而应对绑架问题；但这种定位算法仍存在较大缺陷，一是，无法适应视角小于360°的激光雷达，SAMCL在预处理过程中没有考虑方向问题，利用360°雷达的数据，在二维地图中的每个单元格只计算一个能量值，如果使用视角小于360°的雷达则难以计算位姿，而市面上的大多数2D激光雷达视角都小于360°，二是，对不同工况的适应性不强，为了应对绑架问题，SAMCL判定当绑架问题发生时则将粒子分成全局与局部两个部分，否则不划分，而由于粒子的分配方式是提前设定好的，遇到任何情况都只能使用一种划分方式，对于不同工况没有足够的区分度。
发明内容
本发明的目的在于至少克服上述一种技术不足，提出一种移动机器人定位方法及***。
一方面，本发明提供了一种移动机器人定位方法，包括以下步骤：
步骤S1、获取机器人移动时生成的二维占用单元格地图，将所述二维占用单元格地图转换为三维单元格地图，获取所述三维单元格地图中每个单元格的能量值，以粒子表示机器人的位姿，初始化粒子对应的位姿，确定粒子集Xt；
步骤S2、采集机器人的运动信息，根据所述运动信息采集粒子，并更新粒子的位姿；
步骤S3、获取粒子所在的单元格，将所在单元格对应的能量值赋值给粒子，得到粒子的预测能量值，获取机器人的真实能量值
Figure GDA0003157968140000021
根据所述预测能量值和真实能量值确定粒子的权重，根据粒子的位姿和粒子的权重，获取机器人的当前位姿；
步骤S4、根据粒子的权重在粒子集Xt中进行粒子采样，得到粒子集XL，根据每个单元格的能量进行粒子采样，得到粒子集XG，由粒子集XL和粒子集XG组成新的粒子集；
步骤S5、根据新的粒子集，重复执行步骤S2-S4，得到机器人在移动中的位姿。
进一步地，将所述二维占用单元格地图转换为三维单元格地图，具体包括，将方向空间等分为K＝360·N/β个区间，将x、y两个维度构成的二维占用单元格地图，转换为由x、y、θ三个维度构成的三维单元格地图，其中，K为空间中的区间个数，β为机器人上的激光雷达的测量角度，N为激光雷达的测量数据量。
进一步地，获取所述三维单元格地图中每个单元格的能量值，具体包括，根据公式
Figure GDA0003157968140000022
获取所述三维单元格地图中每个单元格的能量值，其中，Ei为第i个单元格的能量值，θi为单元格i对应的θ坐标，dn为预测机器人到达单元格对应位姿时的激光雷达测量值，dmax为激光雷达测量值中的最大值。
进一步地，根据所述运动信息采集粒子，具体包括，根据均值Δμ和协方差矩阵R的多维高斯分布对粒子进行采样，其中，Δμ为机器人相对于上一时刻运动的差值，R为采集机器人运动信息时里程计误差的协方差矩阵。
进一步地，所述更新粒子的位姿，具体包括，根据公式
Figure GDA0003157968140000023
更新粒子的位姿，其中，
Figure GDA0003157968140000024
为第n个粒子的位姿，p(·)表示概率分布，xt表示本次位姿矢量，xt-1上一次的位姿矢量，μt为机器人的控制矢量。
进一步地，所述获取机器人的真实能量值
Figure GDA0003157968140000025
具体包括，根据公式
Figure GDA0003157968140000031
所述获取机器人的真实能量值
Figure GDA0003157968140000032
其中，
Figure GDA0003157968140000033
为激光雷达对周围障碍物的产生的距离数据。
进一步地，所述根据所述预测能量值和真实能量值确定粒子的权重，具体包括，计算所述预测能量值与真实能量值差的绝对值，以所述差的绝对值作为自变量，以均值为0、以方差为σerr，获取高斯函数的函数值作为粒子的权重，其中，σerr为常数。
进一步地，根据粒子的位姿和粒子的权重，获取机器人的当前位姿，具体包括，根据公式
Figure GDA0003157968140000034
获取机器人的当前位姿，xt为机器人当前位姿，
Figure GDA0003157968140000035
为第n个粒子的位姿，
Figure GDA0003157968140000036
为第n个粒子的权重。
进一步地，所述根据粒子的权重在粒子集Xt中进行粒子采样，得到粒子集XL，根据每个单元格的能量进行粒子采样，得到粒子集XG，具体包括：
根据粒子的权重在粒子集Xt中进行NL次粒子采样，权重越大的粒子被采样到的概率越大，得到NL个粒子组成的粒子集XL；
遍历整个三维单元格地图，提取每个单元格的能量值，若
Figure GDA0003157968140000037
则单元格i位于等能量区域中，在等能量区域中按照均匀分布进行NG次采样得到粒子集XG；
其中，Ei为单元格i对应的能量值，δ为常数，NL＝α·NT，NG＝NT-NL，NT为粒子集Xt中的粒子个数，α为粒子的分配系数。
另一方面，本发明提供了一种根据上述任一技术方案所述的移动机器人定位方法的***，包括单元格能量值及粒子集初始化模块、粒子采集及位姿更新模块、机器人位姿获取模块、粒子集更新模块和迭代执行模块；
所述单元格能量值及粒子集初始化模块，用于获取机器人移动时生成的二维占用单元格地图，将所述二维占用单元格地图转换为三维单元格地图，获取所述三维单元格地图中每个单元格的能量值，以粒子表示机器人的位姿，初始化粒子对应的位姿，确定粒子集Xt；
所述粒子采集及位姿更新模块，用于采集机器人的运动信息，根据所述运动信息采集粒子，并更新粒子的位姿；
所述机器人位姿获取模块，用于获取粒子所在的单元格，将所在单元格对应的能量值赋值给粒子，得到粒子的预测能量值，获取机器人的真实能量值
Figure GDA0003157968140000041
根据所述预测能量值和真实能量值确定粒子的权重，根据粒子的位姿和粒子的权重，获取机器人的当前位姿；
所述粒子集更新模块，用于根据粒子的权重在粒子集Xt中进行粒子采样，得到粒子集XL，根据每个单元格的能量进行粒子采样，得到粒子集XG，由粒子集XL和粒子集XG组成新的粒子集；
所述迭代执行模块，用于使所述粒子集更新模块不断获取新的粒子集，使所述粒子采集及位姿更新模块不断更新粒子的位姿，并使所述机器人位姿获取模块获取机器人在移动中的位姿。
与现有技术相比，本发明的有益效果包括：通过获取机器人移动时生成的二维占用单元格地图，将所述二维占用单元格地图转换为三维单元格地图，获取所述三维单元格地图中每个单元格的能量值，以粒子表示机器人的位姿，初始化粒子对应的位姿，确定粒子集Xt；采集机器人的运动信息，根据所述运动信息采集粒子，并更新粒子的位姿；获取粒子所在的单元格，将所在单元格对应的能量值赋值给粒子，得到粒子的预测能量值，获取机器人的真实能量值
Figure GDA0003157968140000042
根据所述预测能量值和真实能量值确定粒子的权重，根据粒子的位姿和粒子的权重，获取机器人的当前位姿；根据粒子的权重在粒子集Xt中进行粒子采样，得到粒子集XL，根据每个单元格的能量进行粒子采样，得到粒子集XG，由粒子集XL和粒子集XG组成新的粒子集；根据新的粒子集，不断更新粒子的位姿，得到机器人在移动中的位姿；实现了机器人在移动中位姿的确定，并适用于各种可视角度激光雷达，对各种工况具有较好的适应性。
附图说明
图1是本发明实施例1所述的移动机器人定位方法的流程示意图；
图2是本发明实施例1所述的粒子集的最大权重和粒子的分配系数的关系示意图；
图3是本发明实施例2所述的二维占用单元格地图；
图4是本发明实施例2所述的等能量区域示意图；
图5是本发明实施例2所述的位姿示意图。
具体实施方式
为了使本发明的目的、技术方案及优点更加清楚明白，以下结合附图及实施例，对本发明进行进一步详细说明。应当理解，此处所描述的具体实施例仅仅用以解释本发明，并不用于限定本发明。
实施例1
本发明实施例提供一种移动机器人定位方法，其流程示意图，如图1所示，其包括以下步骤：
步骤S1、获取机器人移动时生成的二维占用单元格地图，将所述二维占用单元格地图转换为三维单元格地图，获取所述三维单元格地图中每个单元格的能量值，以粒子表示机器人的位姿，初始化粒子对应的位姿，确定粒子集Xt；
步骤S2、采集机器人的运动信息，根据所述运动信息采集粒子，并更新粒子的位姿；
步骤S3、获取粒子所在的单元格，将所在单元格对应的能量值赋值给粒子，得到粒子的预测能量值，获取机器人的真实能量值
Figure GDA0003157968140000051
根据所述预测能量值和真实能量值确定粒子的权重，根据粒子的位姿和粒子的权重，获取机器人的当前位姿；
步骤S4、根据粒子的权重在粒子集Xt中进行粒子采样，得到粒子集XL，根据每个单元格的能量进行粒子采样，得到粒子集XG，由粒子集XL和粒子集XG组成新的粒子集；
步骤S5、根据新的粒子集，重复执行步骤S2-S4，得到机器人在移动中的位姿。
一个具体实施例中，获取机器人在检测环境中行走时生成的二维占用单元格地图并保存，所述的检测环境是指可以获取二维占用单元格地图的环境，接着对二维占用单元格地图进行预处理；
机器人上设有激光雷达，将激光雷达测量锥角度记作β，对应的测量距离数据量为N，最大测量距离为dmax，对二维占用单元格地图记作由I个单元格mi组成的集合{mi}；
将方向空间等分为K＝360·N/β个区间(对于多数激光雷达K为整数)，则可以将原有由x、y两个维度构成的二维占用单元格地图，转换成由x、y、θ组成的三维单元格地图G3D；对G3D中的每个单元格(xi，yi，θi)，可以根据地图信息，预测机器人到达该位姿时的雷达测量值{d1，…，dN}，dmax为雷达测量值中的最大值，将第i个单元格的能量值记作Ei，则G3D中的每一个单元格都具有一个能量值，
Figure GDA0003157968140000061
一个具体实施例中，粒子滤波器使用粒子的集合来表示机器人位姿，将粒子的集合(粒子集)记为Xt，所述粒子集Xt含有数量为NT的粒子，每个粒子都有一个位姿，初始时刻所有粒子的位姿表示为
Figure GDA0003157968140000062
优选的，将所述二维占用单元格地图转换为三维单元格地图，具体包括，将方向空间等分为K＝360·N/β个区间，将x、y两个维度构成的二维占用单元格地图，转换为由x、y、θ三个维度构成的三维单元格地图，其中，K为空间中的区间个数，β为机器人上的激光雷达的测量角度，N为激光雷达的测量数据量。所述激光雷达的测量数据量即为激光雷达激光束的个数。
优选的，获取所述三维单元格地图中每个单元格的能量值，具体包括，根据公式
Figure GDA0003157968140000063
获取所述三维单元格地图中每个单元格的能量值，其中，Ei为第i个单元格的能量值，θi为单元格i对应的θ坐标，dn为预测机器人到达单元格对应位姿时的激光雷达测量值，dmax为激光雷达测量值中的最大值。
优选的，根据所述运动信息采集粒子，具体包括，根据均值Δμ和协方差矩阵R的多维高斯分布对粒子进行采样，其中，Δμ为机器人相对于上一时刻运动的差值，R为采集机器人运动信息时里程计误差的协方差矩阵。
具体实施时，通过里程计采集机器人的运动信息，得到机器人相对于上一时刻运动了Δμ＝(Δx，Δy，Δθ)，而里程计存在误差，且误差服从高斯分布，其协方差矩阵为
Figure GDA0003157968140000064
优选的，所述更新粒子的位姿，具体包括，根据公式
Figure GDA0003157968140000065
更新粒子的位姿，其中，
Figure GDA0003157968140000066
为第n个粒子的位姿，p(·)表示概率分布，xt表示本次位姿矢量，xt-1上一次的位姿矢量，μt为机器人的控制矢量。
具体实施时，根据均值为Δμ，协方差为R的多维高斯分布对粒子进行采样，更新每个粒子的位姿，对于第n个粒子，其位姿
Figure GDA0003157968140000067
根据以下公式得到；
Figure GDA0003157968140000071
其中，p(·)表示概率分布，xt表示本次位姿矢量，xt-1上一次的位姿矢量，μt为机器人的控制矢量，为已知量。
进行粒子采样后，需要对粒子进行能量赋值，即获取每个粒子所在的单元格，并把单元格对应的能量值赋值给对应的粒子；
对于粒子n，其位姿为
Figure GDA0003157968140000072
对应的找到其所在的单元格celli，其坐标为(xi，yi，θi)，其中保存的能量值为Ei，则粒子n的能量值为
Figure GDA0003157968140000073
需要说明的是，此时获取的粒子的能量值为预测能量值；
优选的，所述获取机器人的真实能量值
Figure GDA0003157968140000074
具体包括，根据公式
Figure GDA0003157968140000075
所述获取机器人的真实能量值
Figure GDA0003157968140000076
其中，
Figure GDA0003157968140000077
为激光雷达对周围障碍物的产生的距离数据。
优选的，所述根据所述预测能量值和真实能量值确定粒子的权重，具体包括，计算所述预测能量值与真实能量值差的绝对值，以所述差的绝对值作为自变量，以均值为0、以方差为σerr，获取高斯函数的函数值作为粒子的权重，其中，σerr为常数。
优选的，根据粒子的位姿和粒子的权重，获取机器人的当前位姿，具体包括，根据公式
Figure GDA0003157968140000078
获取机器人的当前位姿，xt为机器人当前位姿，
Figure GDA0003157968140000079
为第n个粒子的位姿，
Figure GDA00031579681400000710
为第n个粒子的权重。
具体实施时，通过机器人上激光雷达对周围障碍物的产生的距离数据
Figure GDA00031579681400000711
计算机器人的真实能量值
Figure GDA00031579681400000712
Figure GDA00031579681400000713
将每个粒子的预测能量值与
Figure GDA00031579681400000714
比较，并借助高斯函数计算误差的似然值，对于粒子n，首先计算
Figure GDA00031579681400000715
与
Figure GDA00031579681400000716
差的绝对值
Figure GDA00031579681400000717
按照均值为0，方差为σerr的高斯函数，计算当自变量为errn时的高斯函数值，作为粒子n的权重
Figure GDA0003157968140000081
得出所有权重后，进行归一化，找出最大权重
Figure GDA0003157968140000082
然后通过加权平均计算机器人的当前位姿，
Figure GDA0003157968140000083
优选的，所述根据粒子的权重在粒子集Xt中进行粒子采样，得到粒子集XL，根据每个单元格的能量进行粒子采样，得到粒子集XG，具体包括：
根据粒子的权重在粒子集Xt中进行NL次粒子采样，权重越大的粒子被采样到的概率越大，得到NL个粒子组成的粒子集XL；
遍历整个三维单元格地图，提取每个单元格的能量值，若
Figure GDA0003157968140000084
则单元格i位于等能量区域中，在等能量区域中按照均匀分布进行NG次采样得到粒子集XG；
其中，Ei为单元格i对应的能量值，δ为常数，NL＝α·NT，NG＝NT-NL，NT为粒子集Xt中的粒子个数，α为粒子的分配系数。
另一个具体实施例中，粒子集的最大权重和粒子的分配系数的关系示意图，如图2所示，横坐标为粒子集Xt中的最大权重
Figure GDA0003157968140000085
纵坐标为粒子的分配系数α，该关系示意图是通过实验获取，可将粒子集的最大权重和粒子的分配系数的关系记为规则f，则
Figure GDA0003157968140000086
通过粒子的最大权重
Figure GDA0003157968140000087
可获得粒子的分配系数α；
将粒子集Xt划分为粒子集合，分别为XL与XG，XL中的粒子数量为NL＝α·NT，XG中的粒子数量为NG＝NT-NL；在粒子集Xt中根据每个粒子的权重
Figure GDA0003157968140000088
进行采样，权重越大的粒子被采样到的概率越大，进行NL次，得到NL个粒子组成集合XL；遍历整个G3D，提取每个单元格的能量，对于单元格i，若
Figure GDA0003157968140000089
则将符合该条件的单元格i标记为等能量区域(SimilarEnergy Region，SER)；在SER中按照均匀分布进行NG次采样得到粒子集XG；如果NG为0，则这一步可不进行；组合两个粒子集XG与XL，组成新的粒子集Xt。
实施例2
本发明实施例还提供一种移动机器人定位方法，包括以下步骤：
获取机器人在检测环境中行走时生成的二维占用单元格地图，所述二维占用单元格地图，如图3所示，其分辨率为0.05m/cell；
对地图进行预处理；其中N＝540，β＝270，K＝360·N/β＝720，将二维占用单元格地图转换为由x、y、θ组成的三维单元格地图G3D，其中θ维度共有720个区间；
计算G3D中每个单元格的能量值；此处dmax＝30，θi表示单元格i对应的θ坐标，dn表示，若机器人位姿角度为θ时，算法预测雷达上第n条光束测量到的距离为dn；
Figure GDA0003157968140000091
生成粒子集合Xt，其中含有数量为NT的粒子，此处选择NT＝100；每个粒子都有一个位姿，初始时刻所有粒子的位姿表示为
Figure GDA0003157968140000092
里程计可安装在机器人的底部，通过里程计采集机器人的运动信息，得到机器人相对于上一时刻运动了Δμ＝(Δx，Δy，Δθ)，而里程计存在误差，且误差服从高斯分布，其协方差矩阵为
Figure GDA0003157968140000093
例如，此时读取里程计数值发现，Δμ＝(0.1，0.1，0.02)，且里程计各个维度方差为σx＝0.05，σy＝0.05，σθ＝0.1，这是一个三维的高斯分布，则根据这个概率分布由程序进行采样，更新每个粒子的位姿；
获取找到每个粒子所在的单元格，并把单元格对应能量值赋值给粒子；对于粒子n，其位姿为
Figure GDA0003157968140000094
对应的找到其所在的单元格celli，其坐标为(xi，yi，θi)，且其中保存的能量值为Ei，则粒子n的能量值为
Figure GDA0003157968140000095
示例性的，对于采样得到位姿为(0.02 0.07 0.02)T的粒子n，由于x、y方向的分辨率为0.05m/cell，θ维度的分辨率为0.5deg/cell，因此在G3D中的单元格i坐标为(0，1，360)，则根据
Figure GDA0003157968140000096
获取粒子的预测能量值；
获取激光雷达的产生的距离数据
Figure GDA0003157968140000097
并计算其真实的能量值
Figure GDA0003157968140000098
Figure GDA0003157968140000099
这里计算得到
Figure GDA00031579681400000910
将每个粒子的能量值(预测能量值)与
Figure GDA00031579681400000911
比较，并借助高斯函数计算误差的似然值；对于粒子n，首先计算
Figure GDA00031579681400000912
与
Figure GDA00031579681400000913
差的绝对值
Figure GDA00031579681400000914
按照均值为0，方差为σerr的高斯函数，计算当自变量为errn时的函数值，作为粒子n的权重
Figure GDA00031579681400000915
并找出最大权重
Figure GDA00031579681400000916
若
Figure GDA00031579681400000917
取σerr＝0.5，则对应的σerr＝0.05，
Figure GDA00031579681400000918
以此类推；得出所有权重后，进行归一化，找出最大权重
Figure GDA0003157968140000101
此时得到最大权重为0.383；
通过加权平均计算机器人的当前位姿；
Figure GDA0003157968140000102
可计算出机器人位姿为xt＝(0.38497 1.39872 0.14311)T；
由于
Figure GDA0003157968140000103
根据规则f得出此时的α＝0.497；则用于局部的粒子数量NL＝49，用于全局的粒子数量NG＝51；
在粒子集Xt中根据每个粒子的权重
Figure GDA0003157968140000107
进行采样，权重越大的粒子被采样到的概率越大，进行NL＝49次得到集合XL；
遍历整个G3D，提取每个单元格的能量，对于单元格i，若
Figure GDA0003157968140000104
则将单元格i标记为等能量区域(Similar Energy Region，SER)；
例如，当
Figure GDA0003157968140000105
时，等能量区域示意图，如图4所示，图中网格表示尺寸；需要说明的是，等能量区域是在G3D中标记的，而G3D是一个三维地图，无法在二维图像中表示，而G3D中的三个维度分别为x、y、θ，因此使用黑色箭头在二维图中表示，每个箭头的尾端点表示坐标(x，y)，箭头指向的方向表示θ；
在SER中按照均匀分布进行NG次采样得到粒子集XG；例如在
Figure GDA0003157968140000106
的等能量区域中进行51次随机采样，得到的51个位姿，其位姿示意图，如图5所示，每个黑色箭头表示一个位姿；得到新的粒子集Xt，可进行下一轮的机器人位姿计算；从而得到机器人在移动中的位姿；需要说明的是，激光雷达使用视角为270°的2D激光雷达，该雷达每帧能产生540个数据，最大有效测量距离为30m。
实施例3
本发明实施例还提供了一种根据所述任一实施例所述的移动机器人定位方法的***，包括单元格能量值及粒子集初始化模块、粒子采集及位姿更新模块、机器人位姿获取模块、粒子集更新模块和迭代执行模块；
所述单元格能量值及粒子集初始化模块，用于获取机器人移动时生成的二维占用单元格地图，将所述二维占用单元格地图转换为三维单元格地图，获取所述三维单元格地图中每个单元格的能量值，以粒子表示机器人的位姿，初始化粒子对应的位姿，确定粒子集Xt；
所述粒子采集及位姿更新模块，用于采集机器人的运动信息，根据所述运动信息采集粒子，并更新粒子的位姿；
所述机器人位姿获取模块，用于获取粒子所在的单元格，将所在单元格对应的能量值赋值给粒子，得到粒子的预测能量值，获取机器人的真实能量值
Figure GDA0003157968140000111
根据所述预测能量值和真实能量值确定粒子的权重，根据粒子的位姿和粒子的权重，获取机器人的当前位姿；
所述粒子集更新模块，用于根据粒子的权重在粒子集Xt中进行粒子采样，得到粒子集XL，根据每个单元格的能量进行粒子采样，得到粒子集XG，由粒子集XL和粒子集XG组成新的粒子集；
所述迭代执行模块，用于使所述粒子集更新模块不断获取新的粒子集，使所述粒子采集及位姿更新模块不断更新粒子的位姿，并使所述机器人位姿获取模块获取机器人在移动中的位姿。
需要说明的是，实施例1-3未重复描述之处可互相借鉴。
本发明公开了一种移动机器人定位方法及***，通过获取机器人移动时生成的二维占用单元格地图，将所述二维占用单元格地图转换为三维单元格地图，获取所述三维单元格地图中每个单元格的能量值，以粒子表示机器人的位姿，初始化粒子对应的位姿，确定粒子集Xt；采集机器人的运动信息，根据所述运动信息采集粒子，并更新粒子的位姿；获取粒子所在的单元格，将所在单元格对应的能量值赋值给粒子，得到粒子的预测能量值，获取机器人的真实能量值
Figure GDA0003157968140000112
根据所述预测能量值和真实能量值确定粒子的权重，根据粒子的位姿和粒子的权重，获取机器人的当前位姿；根据粒子的权重在粒子集Xt中进行粒子采样，得到粒子集XL，根据每个单元格的能量进行粒子采样，得到粒子集XG，由粒子集XL和粒子集XG组成新的粒子集；根据新的粒子集，不断更新粒子的位姿，得到机器人在移动中的位姿；实现了机器人在移动中位姿的确定，通过二维占用单元格地图进行预处理，将二维地图转换成三维地图，使得本发明提供的技术方案可以预测各个方向的能量值，进而使得本发明的技术方案适用于各种可视角度的激光雷达；本发明技术方案根据最大权重来决定划分粒子的方式，将粒子集分配成两个部分，分别用于全局定位与局部定位，且使用了动态的分配方法，使得本发明技术方案对各种工况具有较好的适应性。
以上所述本发明的具体实施方式，并不构成对本发明保护范围的限定。任何根据本发明的技术构思所做出的各种其他相应的改变与变形，均应包含在本发明权利要求的保护范围内。

Claims (10)

1.一种移动机器人定位方法，其特征在于，包括以下步骤：
步骤S1、获取机器人移动时生成的二维占用单元格地图，将所述二维占用单元格地图转换为三维单元格地图，获取所述三维单元格地图中每个单元格的能量值，以粒子表示机器人的位姿，初始化粒子对应的位姿，确定粒子集Xt；
步骤S2、采集机器人的运动信息，根据所述运动信息采集粒子，并更新粒子的位姿；
步骤S3、获取粒子所在的单元格，将所在单元格对应的能量值赋值给粒子，得到粒子的预测能量值，获取机器人的真实能量值
Figure FDA0003157968130000011
根据所述预测能量值和真实能量值确定粒子的权重，根据粒子的位姿和粒子的权重，获取机器人的当前位姿；
步骤S4、根据粒子的权重在粒子集Xt中进行粒子采样，得到粒子集XL，根据每个单元格的能量进行粒子采样，得到粒子集XG，由粒子集XL和粒子集XG组成新的粒子集；
步骤S5、根据新的粒子集，重复执行步骤S2-S4，得到机器人在移动中的位姿。
2.根据权利要求1所述的移动机器人定位方法，其特征在于，将所述二维占用单元格地图转换为三维单元格地图，具体包括，将方向空间等分为K＝360·N/β个区间，将x、y两个维度构成的二维占用单元格地图，转换为由x、y、θ三个维度构成的三维单元格地图，其中，K为空间中的区间个数，β为机器人上的激光雷达的测量角度，N为激光雷达的测量数据量。
3.根据权利要求2所述的移动机器人定位方法，其特征在于，获取所述三维单元格地图中每个单元格的能量值，具体包括，根据公式
Figure FDA0003157968130000012
获取所述三维单元格地图中每个单元格的能量值，其中，Ei为第i个单元格的能量值，θi为单元格i对应的θ坐标，dn为预测机器人到达单元格对应位姿时的激光雷达测量值，dmax为激光雷达测量值中的最大值。
4.根据权利要求1所述的移动机器人定位方法，其特征在于，根据所述运动信息采集粒子，具体包括，根据均值Δμ和协方差矩阵R的多维高斯分布对粒子进行采样，其中，Δμ为机器人相对于上一时刻运动的差值，R为采集机器人运动信息时里程计误差的协方差矩阵。
5.根据权利要求1所述的移动机器人定位方法，其特征在于，所述更新粒子的位姿，具体包括，根据公式
Figure FDA0003157968130000021
更新粒子的位姿，其中，
Figure FDA0003157968130000022
为第n个粒子的位姿，p(·)表示概率分布，xt表示本次位姿矢量，xt-1上一次的位姿矢量，μt为机器人的控制矢量。
6.根据权利要求3所述的移动机器人定位方法，其特征在于，所述获取机器人的真实能量值
Figure FDA0003157968130000023
具体包括，根据公式
Figure FDA0003157968130000024
所述获取机器人的真实能量值
Figure FDA0003157968130000025
其中，
Figure FDA0003157968130000026
为激光雷达对周围障碍物的产生的距离数据。
7.根据权利要求1所述的移动机器人定位方法，其特征在于，所述根据所述预测能量值和真实能量值确定粒子的权重，具体包括，计算所述预测能量值与真实能量值差的绝对值，以所述差的绝对值作为自变量，以均值为0、以方差为σerr，获取高斯函数的函数值作为粒子的权重，其中，σerr为常数。
8.根据权利要求1所述的移动机器人定位方法，其特征在于，根据粒子的位姿和粒子的权重，获取机器人的当前位姿，具体包括，根据公式
Figure FDA0003157968130000027
获取机器人的当前位姿，xt为机器人当前位姿，
Figure FDA0003157968130000028
为第n个粒子的位姿，
Figure FDA0003157968130000029
为第n个粒子的权重。
9.根据权利要求1所述的移动机器人定位方法，其特征在于，所述根据粒子的权重在粒子集Xt中进行粒子采样，得到粒子集XL，根据每个单元格的能量进行粒子采样，得到粒子集XG，具体包括：
根据粒子的权重在粒子集Xt中进行NL次粒子采样，权重越大的粒子被采样到的概率越大，得到NL个粒子组成的粒子集XL；
遍历整个三维单元格地图，提取每个单元格的能量值，若
Figure FDA00031579681300000210
则单元格i位于等能量区域中，在等能量区域中按照均匀分布进行NG次采样得到粒子集XG；
其中，Ei为单元格i对应的能量值，δ为常数，NL＝α·NT，NG＝NT-NL，NT为粒子集Xt中的粒子个数，α为粒子的分配系数。
10.一种根据权利要求1-9任一所述的移动机器人定位方法的***，其特征在于，包括单元格能量值及粒子集初始化模块、粒子采集及位姿更新模块、机器人位姿获取模块、粒子集更新模块和迭代执行模块；
所述单元格能量值及粒子集初始化模块，用于获取机器人移动时生成的二维占用单元格地图，将所述二维占用单元格地图转换为三维单元格地图，获取所述三维单元格地图中每个单元格的能量值，以粒子表示机器人的位姿，初始化粒子对应的位姿，确定粒子集Xt；
所述粒子采集及位姿更新模块，用于采集机器人的运动信息，根据所述运动信息采集粒子，并更新粒子的位姿；
所述机器人位姿获取模块，用于获取粒子所在的单元格，将所在单元格对应的能量值赋值给粒子，得到粒子的预测能量值，获取机器人的真实能量值
Figure FDA0003157968130000031
根据所述预测能量值和真实能量值确定粒子的权重，根据粒子的位姿和粒子的权重，获取机器人的当前位姿；
所述粒子集更新模块，用于根据粒子的权重在粒子集Xt中进行粒子采样，得到粒子集XL，根据每个单元格的能量进行粒子采样，得到粒子集XG，由粒子集XL和粒子集XG组成新的粒子集；
所述迭代执行模块，用于使所述粒子集更新模块不断获取新的粒子集，使所述粒子采集及位姿更新模块不断更新粒子的位姿，并使所述机器人位姿获取模块获取机器人在移动中的位姿。
CN202010317589.3A 2020-04-21 2020-04-21 一种移动机器人定位方法及*** Active CN111421548B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202010317589.3A CN111421548B (zh) 2020-04-21 2020-04-21 一种移动机器人定位方法及***

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202010317589.3A CN111421548B (zh) 2020-04-21 2020-04-21 一种移动机器人定位方法及***

Publications (2)

Publication Number Publication Date
CN111421548A CN111421548A (zh) 2020-07-17
CN111421548B true CN111421548B (zh) 2021-10-19

Family

ID=71556572

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202010317589.3A Active CN111421548B (zh) 2020-04-21 2020-04-21 一种移动机器人定位方法及***

Country Status (1)

Country Link
CN (1) CN111421548B (zh)

Families Citing this family (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN112014854A (zh) * 2020-08-31 2020-12-01 华通科技有限公司 一种基于粒子群定位算法的定位方法

Citations (12)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN103631264A (zh) * 2013-12-04 2014-03-12 苏州大学张家港工业技术研究院 一种同时定位与地图创建方法及装置
CN104180799A (zh) * 2014-07-15 2014-12-03 东北大学 一种基于自适应蒙特卡罗定位的机器人定位方法
EP3112969A1 (en) * 2015-07-03 2017-01-04 Korea Advanced Institute of Science and Technology Method and apparatus for relocation of mobile robot in indoor environment
CN108645413A (zh) * 2018-06-06 2018-10-12 江苏海事职业技术学院 一种移动机器人的同时定位与地图创建的动态纠正方法
CN108931245A (zh) * 2018-08-02 2018-12-04 上海思岚科技有限公司 移动机器人的局部自定位方法及设备
CN109459033A (zh) * 2018-12-21 2019-03-12 哈尔滨工程大学 一种多重渐消因子的机器人无迹快速同步定位与建图方法
CN109682382A (zh) * 2019-02-28 2019-04-26 电子科技大学 基于自适应蒙特卡洛和特征匹配的全局融合定位方法
CN109798896A (zh) * 2019-01-21 2019-05-24 东南大学 一种室内机器人定位与建图方法及装置
CN109974701A (zh) * 2017-12-28 2019-07-05 深圳市优必选科技有限公司 机器人的定位方法及装置
CN110007670A (zh) * 2019-02-14 2019-07-12 四川阿泰因机器人智能装备有限公司 移动机器人定位建图方法
CN110340877A (zh) * 2019-07-11 2019-10-18 深圳市杉川机器人有限公司 移动机器人及其定位方法和计算机可读存储介质
CN110927740A (zh) * 2019-12-06 2020-03-27 合肥科大智能机器人技术有限公司 一种移动机器人定位方法

Patent Citations (12)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN103631264A (zh) * 2013-12-04 2014-03-12 苏州大学张家港工业技术研究院 一种同时定位与地图创建方法及装置
CN104180799A (zh) * 2014-07-15 2014-12-03 东北大学 一种基于自适应蒙特卡罗定位的机器人定位方法
EP3112969A1 (en) * 2015-07-03 2017-01-04 Korea Advanced Institute of Science and Technology Method and apparatus for relocation of mobile robot in indoor environment
CN109974701A (zh) * 2017-12-28 2019-07-05 深圳市优必选科技有限公司 机器人的定位方法及装置
CN108645413A (zh) * 2018-06-06 2018-10-12 江苏海事职业技术学院 一种移动机器人的同时定位与地图创建的动态纠正方法
CN108931245A (zh) * 2018-08-02 2018-12-04 上海思岚科技有限公司 移动机器人的局部自定位方法及设备
CN109459033A (zh) * 2018-12-21 2019-03-12 哈尔滨工程大学 一种多重渐消因子的机器人无迹快速同步定位与建图方法
CN109798896A (zh) * 2019-01-21 2019-05-24 东南大学 一种室内机器人定位与建图方法及装置
CN110007670A (zh) * 2019-02-14 2019-07-12 四川阿泰因机器人智能装备有限公司 移动机器人定位建图方法
CN109682382A (zh) * 2019-02-28 2019-04-26 电子科技大学 基于自适应蒙特卡洛和特征匹配的全局融合定位方法
CN110340877A (zh) * 2019-07-11 2019-10-18 深圳市杉川机器人有限公司 移动机器人及其定位方法和计算机可读存储介质
CN110927740A (zh) * 2019-12-06 2020-03-27 合肥科大智能机器人技术有限公司 一种移动机器人定位方法

Also Published As

Publication number Publication date
CN111421548A (zh) 2020-07-17

Similar Documents

Publication Publication Date Title
CN109001711B (zh) 多线激光雷达标定方法
EP2808842B1 (en) An apparatus and method for tracking and reconstructing three-dimensional objects
CN112923933A (zh) 一种激光雷达slam算法与惯导融合定位的方法
CN106886980B (zh) 一种基于三维激光雷达目标识别的点云密度增强的方法
CN111145227B (zh) 一种地下隧道空间多视点云的可迭代整体配准方法
CN111273312B (zh) 一种智能车辆定位与回环检测方法
CN113108773A (zh) 一种融合激光与视觉传感器的栅格地图构建方法
CN112347550A (zh) 耦合式室内三维语义建图及建模方法
CN110196031B (zh) 一种三维点云采集***的标定方法
CN113066162A (zh) 一种用于电磁计算的城市环境快速建模方法
CN113674412B (zh) 基于位姿融合优化的室内地图构建方法、***及存储介质
Ruhnke et al. Highly accurate maximum likelihood laser mapping by jointly optimizing laser points and robot poses
CN114119920A (zh) 三维点云地图构建方法、***
CN110187337A (zh) 一种基于ls和neu-ecef时空配准的高机动目标跟踪方法及***
CN111421548B (zh) 一种移动机器人定位方法及***
CN114299318A (zh) 一种快速点云数据处理和目标图像匹配的方法及***
CN115131514A (zh) 一种同时定位建图的方法、装置、***及存储介质
CN114066773B (zh) 一种基于点云特征与蒙特卡洛扩展法的动态物体去除
CN112802195B (zh) 一种基于声呐的水下机器人连续占据建图方法
CN111765883B (zh) 机器人蒙特卡罗定位方法、设备及存储介质
CN112581610B (zh) 从多波束声呐数据中建立地图的鲁棒优化方法和***
CN113985435A (zh) 一种融合多激光雷达的建图方法及***
Gujski et al. Machine learning clustering for point clouds optimisation via feature analysis in Cultural Heritage
Liu et al. A localizability estimation method for mobile robots based on 3d point cloud feature
CN114636981B (zh) 基于雷达回波的在线深度学习台风中心定位***

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
SE01 Entry into force of request for substantive examination
SE01 Entry into force of request for substantive examination
GR01 Patent grant
GR01 Patent grant