CN111421548A - 一种移动机器人定位方法及系统 - Google Patents

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

Info

Publication number
CN111421548A
CN111421548A CN202010317589.3A CN202010317589A CN111421548A CN 111421548 A CN111421548 A CN 111421548A CN 202010317589 A CN202010317589 A CN 202010317589A CN 111421548 A CN111421548 A CN 111421548A
Authority
CN
China
Prior art keywords
particle
particles
pose
robot
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.)
Granted
Application number
CN202010317589.3A
Other languages
English (en)
Other versions
CN111421548B (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 BDA0002460128850000025
根据所述预测能量值和真实能量值确定粒子的权重，根据粒子的位姿和粒子的权重，获取机器人的当前位姿；
步骤S4、根据粒子的权重在粒子集Xt中进行粒子采样，得到粒子集XL，根据每个单元格的能量进行粒子采样，得到粒子集XG，由粒子集XL和粒子集XG组成新的粒子集；
步骤S5、根据新的粒子集，重复执行步骤S2-S4，得到机器人在移动中的位姿。
进一步地，将所述二维占用单元格地图转换为三维单元格地图，具体包括，将方向空间等分为K＝360·N/β个区间，将x、y两个维度构成的二维占用单元格地图，转换为由x、y、θ三个维度构成的三维单元格地图，其中，K为空间中的区间个数，β为机器人上的激光雷达的测量角度，N为激光雷达的测量数据量。
进一步地，获取所述三维单元格地图中每个单元格的能量值，具体包括，根据公式
Figure BDA0002460128850000021
获取所述三维单元格地图中每个单元格的能量值，其中，Ei为第i个单元格的能量值，(xi,yi,θi)为第i个单元格在三维单元格地图的坐标，dn为预测机器人到达单元格对应位姿时的激光雷达测量值，dmax为激光雷达测量值中的最大值。
进一步地，根据所述运动信息采集粒子，具体包括，根据均值Δμ和协方差矩阵R的多维高斯分布对粒子进行采样，其中，Δμ为机器人相对于上一时刻运动的差值，R为采集机器人运动信息时里程计误差的协方差矩阵。
进一步地，所述更新粒子的位姿，具体包括，根据公式
Figure BDA0002460128850000022
更新粒子的位姿，其中，
Figure BDA0002460128850000023
为第n个粒子的位姿，p(·)表示概率分布，xt表示本次位姿矢量，xt-1上一次的位姿矢量，μt为机器人的控制矢量。
进一步地，所述获取机器人的真实能量值
Figure BDA0002460128850000024
具体包括，根据公式
Figure BDA0002460128850000031
所述获取机器人的真实能量值
Figure BDA0002460128850000032
其中，
Figure BDA0002460128850000033
为激光雷达对周围障碍物的产生的距离数据。
进一步地，所述根据所述预测能量值和真实能量值确定粒子的权重，具体包括，计算所述预测能量值与真实能量值差的绝对值，以所述差的绝对值作为自变量，以均值为0、以方差为σerr，获取高斯函数的函数值作为粒子的权重，其中，σerr为常数。
进一步地，根据粒子的位姿和粒子的权重，获取机器人的当前位姿，具体包括，根据公式
Figure BDA0002460128850000034
获取机器人的当前位姿，xt为机器人当前位姿，
Figure BDA0002460128850000035
为第n个粒子的位姿，
Figure BDA0002460128850000036
为第n个粒子的权重。
进一步地，所述根据粒子的权重在粒子集Xt中进行粒子采样，得到粒子集XL，根据每个单元格的能量进行粒子采样，得到粒子集XG，具体包括：
根据粒子的权重在粒子集Xt中进行NL次粒子采样，权重越大的粒子被采样到的概率越大，得到NL个粒子组成的粒子集XL；
遍历整个三维单元格地图，提取每个单元格的能量值，若
Figure BDA0002460128850000037
则单元格i位于等能量区域中，在等能量区域中按照均匀分布进行NG次采样得到粒子集XG；
其中，Ei为单元格i对应的能量值，δ为常数，NL＝α·NT，NG＝NT-NL，NT为粒子集Xt中的粒子个数，α为粒子的分配系数。
另一方面，本发明提供了一种根据上述任一技术方案所述的移动机器人定位方法的系统，包括单元格能量值及粒子集初始化模块、粒子采集及位姿更新模块、机器人位姿获取模块、粒子集更新模块和迭代执行模块；
所述单元格能量值及粒子集初始化模块，用于获取机器人移动时生成的二维占用单元格地图，将所述二维占用单元格地图转换为三维单元格地图，获取所述三维单元格地图中每个单元格的能量值，以粒子表示机器人的位姿，初始化粒子对应的位姿，确定粒子集Xt；
所述粒子采集及位姿更新模块，用于采集机器人的运动信息，根据所述运动信息采集粒子，并更新粒子的位姿；
所述机器人位姿获取模块，用于获取粒子所在的单元格，将所在单元格对应的能量值赋值给粒子，得到粒子的预测能量值，获取机器人的真实能量值
Figure BDA0002460128850000041
根据所述预测能量值和真实能量值确定粒子的权重，根据粒子的位姿和粒子的权重，获取机器人的当前位姿；
所述粒子集更新模块，用于根据粒子的权重在粒子集Xt中进行粒子采样，得到粒子集XL，根据每个单元格的能量进行粒子采样，得到粒子集XG，由粒子集XL和粒子集XG组成新的粒子集；
所述迭代执行模块，用于使所述粒子集更新模块不断获取新的粒子集，使所述粒子采集及位姿更新模块不断更新粒子的位姿，并使所述机器人位姿获取模块获取机器人在移动中的位姿。
与现有技术相比，本发明的有益效果包括：通过获取机器人移动时生成的二维占用单元格地图，将所述二维占用单元格地图转换为三维单元格地图，获取所述三维单元格地图中每个单元格的能量值，以粒子表示机器人的位姿，初始化粒子对应的位姿，确定粒子集Xt；采集机器人的运动信息，根据所述运动信息采集粒子，并更新粒子的位姿；获取粒子所在的单元格，将所在单元格对应的能量值赋值给粒子，得到粒子的预测能量值，获取机器人的真实能量值
Figure BDA0002460128850000042
根据所述预测能量值和真实能量值确定粒子的权重，根据粒子的位姿和粒子的权重，获取机器人的当前位姿；根据粒子的权重在粒子集Xt中进行粒子采样，得到粒子集XL，根据每个单元格的能量进行粒子采样，得到粒子集XG，由粒子集XL和粒子集XG组成新的粒子集；根据新的粒子集，不断更新粒子的位姿，得到机器人在移动中的位姿；实现了机器人在移动中位姿的确定，并适用于各种可视角度激光雷达，对各种工况具有较好的适应性。
附图说明
图1是本发明实施例1所述的移动机器人定位方法的流程示意图；
图2是本发明实施例1所述的粒子集的最大权重和粒子的分配系数的关系示意图；
图3是本发明实施例2所述的二维占用单元格地图；
图4是本发明实施例2所述的等能量区域示意图；
图5是本发明实施例2所述的位姿示意图。
具体实施方式
为了使本发明的目的、技术方案及优点更加清楚明白，以下结合附图及实施例，对本发明进行进一步详细说明。应当理解，此处所描述的具体实施例仅仅用以解释本发明，并不用于限定本发明。
实施例1
本发明实施例提供一种移动机器人定位方法，其流程示意图，如图1所示，其包括以下步骤：
步骤S1、获取机器人移动时生成的二维占用单元格地图，将所述二维占用单元格地图转换为三维单元格地图，获取所述三维单元格地图中每个单元格的能量值，以粒子表示机器人的位姿，初始化粒子对应的位姿，确定粒子集Xt；
步骤S2、采集机器人的运动信息，根据所述运动信息采集粒子，并更新粒子的位姿；
步骤S3、获取粒子所在的单元格，将所在单元格对应的能量值赋值给粒子，得到粒子的预测能量值，获取机器人的真实能量值
Figure BDA0002460128850000051
根据所述预测能量值和真实能量值确定粒子的权重，根据粒子的位姿和粒子的权重，获取机器人的当前位姿；
步骤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 BDA0002460128850000061
一个具体实施例中，粒子滤波器使用粒子的集合来表示机器人位姿，将粒子的集合(粒子集)记为Xt，所述粒子集Xt含有数量为NT的粒子，每个粒子都有一个位姿，初始时刻所有粒子的位姿表示为
Figure BDA0002460128850000062
优选的，将所述二维占用单元格地图转换为三维单元格地图，具体包括，将方向空间等分为K＝360·N/β个区间，将x、y两个维度构成的二维占用单元格地图，转换为由x、y、θ三个维度构成的三维单元格地图，其中，K为空间中的区间个数，β为机器人上的激光雷达的测量角度，N为激光雷达的测量数据量。所述激光雷达的测量数据量即为激光雷达激光束的个数。
优选的，获取所述三维单元格地图中每个单元格的能量值，具体包括，根据公式
Figure BDA0002460128850000063
获取所述三维单元格地图中每个单元格的能量值，其中，Ei为第i个单元格的能量值，(xi,yi,θi)为第i个单元格在三维单元格地图的坐标，dn为预测机器人到达单元格对应位姿时的激光雷达测量值，dmax为激光雷达测量值中的最大值。
优选的，根据所述运动信息采集粒子，具体包括，根据均值Δμ和协方差矩阵R的多维高斯分布对粒子进行采样，其中，Δμ为机器人相对于上一时刻运动的差值，R为采集机器人运动信息时里程计误差的协方差矩阵。
具体实施时，通过里程计采集机器人的运动信息，得到机器人相对于上一时刻运动了Δμ＝(Δx，Δy，Δθ)，而里程计存在误差，且误差服从高斯分布，其协方差矩阵为
Figure BDA0002460128850000064
优选的，所述更新粒子的位姿，具体包括，根据公式
Figure BDA0002460128850000065
更新粒子的位姿，其中，
Figure BDA0002460128850000066
为第n个粒子的位姿，p(·)表示概率分布，xt表示本次位姿矢量，xt-1上一次的位姿矢量，μt为机器人的控制矢量。
具体实施时，根据均值为Δμ，协方差为R的多维高斯分布对粒子进行采样，更新每个粒子的位姿，对于第n个粒子，其位姿
Figure BDA0002460128850000067
根据以下公式得到；
Figure BDA0002460128850000071
其中，p(·)表示概率分布，xt表示本次位姿矢量，xt-1上一次的位姿矢量，μt为机器人的控制矢量，为已知量。
进行粒子采样后，需要对粒子进行能量赋值，即获取每个粒子所在的单元格，并把单元格对应的能量值赋值给对应的粒子；
对于粒子n，其位姿为
Figure BDA0002460128850000072
对应的找到其所在的单元格celli，其坐标为(xi，yi，θi)，其中保存的能量值为Ei，则粒子n的能量值为
Figure BDA0002460128850000073
需要说明的是，此时获取的粒子的能量值为预测能量值；
优选的，所述获取机器人的真实能量值
Figure BDA0002460128850000074
具体包括，根据公式
Figure BDA0002460128850000075
所述获取机器人的真实能量值
Figure BDA0002460128850000076
其中，
Figure BDA0002460128850000077
为激光雷达对周围障碍物的产生的距离数据。
优选的，所述根据所述预测能量值和真实能量值确定粒子的权重，具体包括，计算所述预测能量值与真实能量值差的绝对值，以所述差的绝对值作为自变量，以均值为0、以方差为σerr，获取高斯函数的函数值作为粒子的权重，其中，σerr为常数。
优选的，根据粒子的位姿和粒子的权重，获取机器人的当前位姿，具体包括，根据公式
Figure BDA0002460128850000078
获取机器人的当前位姿，xt为机器人当前位姿，
Figure BDA0002460128850000079
为第n个粒子的位姿，
Figure BDA00024601288500000710
为第n个粒子的权重。
具体实施时，通过机器人上激光雷达对周围障碍物的产生的距离数据
Figure BDA00024601288500000711
计算机器人的真实能量值
Figure BDA00024601288500000712
Figure BDA00024601288500000713
将每个粒子的预测能量值与
Figure BDA00024601288500000714
比较，并借助高斯函数计算误差的似然值，对于粒子n，首先计算
Figure BDA00024601288500000715
与
Figure BDA00024601288500000716
差的绝对值
Figure BDA00024601288500000717
按照均值为0，方差为σerr的高斯函数，计算当自变量为errn时的高斯函数值，作为粒子n的权重
Figure BDA0002460128850000081
得出所有权重后，进行归一化，找出最大权重
Figure BDA0002460128850000082
然后通过加权平均计算机器人的当前位姿，
Figure BDA0002460128850000083
优选的，所述根据粒子的权重在粒子集Xt中进行粒子采样，得到粒子集XL，根据每个单元格的能量进行粒子采样，得到粒子集XG，具体包括：
根据粒子的权重在粒子集Xt中进行NL次粒子采样，权重越大的粒子被采样到的概率越大，得到NL个粒子组成的粒子集XL；
遍历整个三维单元格地图，提取每个单元格的能量值，若
Figure BDA0002460128850000084
则单元格i位于等能量区域中，在等能量区域中按照均匀分布进行NG次采样得到粒子集XG；
其中，Ei为单元格i对应的能量值，δ为常数，NL＝α·NT，NG＝NT-NL，NT为粒子集Xt中的粒子个数，α为粒子的分配系数。
另一个具体实施例中，粒子集的最大权重和粒子的分配系数的关系示意图，如图2所示，横坐标为粒子集Xt中的最大权重
Figure BDA0002460128850000085
纵坐标为粒子的分配系数α，该关系示意图是通过实验获取，可将粒子集的最大权重和粒子的分配系数的关系记为规则f，则
Figure BDA0002460128850000086
通过粒子的最大权重
Figure BDA0002460128850000087
可获得粒子的分配系数α；
将粒子集Xt划分为粒子集合，分别为XL与XG，XL中的粒子数量为NL＝α·NT，XG中的粒子数量为NG＝NT-NL；在粒子集Xt中根据每个粒子的权重
Figure BDA0002460128850000088
进行采样，权重越大的粒子被采样到的概率越大，进行NL次，得到NL个粒子组成集合XL；遍历整个G3D，提取每个单元格的能量，对于单元格i，若
Figure BDA0002460128850000089
则将符合该条件的单元格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 BDA0002460128850000091
生成粒子集合Xt，其中含有数量为NT的粒子，此处选择NT＝100；每个粒子都有一个位姿，初始时刻所有粒子的位姿表示为
Figure BDA0002460128850000092
里程计可安装在机器人的底部，通过里程计采集机器人的运动信息，得到机器人相对于上一时刻运动了Δμ＝(Δx，Δy，Δθ)，而里程计存在误差，且误差服从高斯分布，其协方差矩阵为
Figure BDA0002460128850000093
例如，此时读取里程计数值发现，Δμ＝(0.1，0.1，0.02)，且里程计各个维度方差为σx＝0.05，σy＝0.05，σθ＝0.1，这是一个三维的高斯分布，则根据这个概率分布由程序进行采样，更新每个粒子的位姿；
获取找到每个粒子所在的单元格，并把单元格对应能量值赋值给粒子；对于粒子n，其位姿为
Figure BDA0002460128850000094
对应的找到其所在的单元格celli，其坐标为(xi，yi，θi)，且其中保存的能量值为Ei，则粒子n的能量值为
Figure BDA0002460128850000095
示例性的，对于采样得到位姿为(0.02 0.07 0.02)T的粒子n，由于x、y方向的分辨率为0.05m/cell，θ维度的分辨率为0.5deg/cell，因此在G3D中的单元格i坐标为(0，1，360)，则根据
Figure BDA0002460128850000096
获取粒子的预测能量值；
获取激光雷达的产生的距离数据
Figure BDA0002460128850000097
并计算其真实的能量值
Figure BDA0002460128850000098
Figure BDA0002460128850000099
这里计算得到
Figure BDA00024601288500000910
将每个粒子的能量值(预测能量值)与
Figure BDA00024601288500000911
比较，并借助高斯函数计算误差的似然值；对于粒子n，首先计算
Figure BDA00024601288500000912
与
Figure BDA00024601288500000913
差的绝对值
Figure BDA00024601288500000914
按照均值为0，方差为σerr的高斯函数，计算当自变量为errn时的函数值，作为粒子n的权重
Figure BDA00024601288500000915
并找出最大权重
Figure BDA00024601288500000916
若
Figure BDA00024601288500000917
取σerr＝0.5，则对应的σerr＝0.05，
Figure BDA00024601288500000918
以此类推；得出所有权重后，进行归一化，找出最大权重
Figure BDA0002460128850000101
此时得到最大权重为0.383；
通过加权平均计算机器人的当前位姿；
Figure BDA0002460128850000102
可计算出机器人位姿为xt＝(0.38497 1.39872 0.14311)T；
由于
Figure BDA0002460128850000103
根据规则f得出此时的α＝0.497；则用于局部的粒子数量NL＝49，用于全局的粒子数量NG＝51；
在粒子集Xt中根据每个粒子的权重
Figure BDA0002460128850000104
进行采样，权重越大的粒子被采样到的概率越大，进行NL＝49次得到集合XL；
遍历整个G3D，提取每个单元格的能量，对于单元格i，若
Figure BDA0002460128850000105
则将单元格i标记为等能量区域(Similar Energy Region，SER)；
例如，当
Figure BDA0002460128850000106
时，等能量区域示意图，如图4所示，图中网格表示尺寸；需要说明的是，等能量区域是在G3D中标记的，而G3D是一个三维地图，无法在二维图像中表示，而G3D中的三个维度分别为x、y、θ，因此使用黑色箭头在二维图中表示，每个箭头的尾端点表示坐标(x，y)，箭头指向的方向表示θ；
在SER中按照均匀分布进行NG次采样得到粒子集XG；例如在
Figure BDA0002460128850000107
的等能量区域中进行51次随机采样，得到的51个位姿，其位姿示意图，如图5所示，每个黑色箭头表示一个位姿；得到新的粒子集Xt，可进行下一轮的机器人位姿计算；从而得到机器人在移动中的位姿；需要说明的是，激光雷达使用视角为270°的2D激光雷达，该雷达每帧能产生540个数据，最大有效测量距离为30m。
实施例3
本发明实施例还提供了一种根据所述任一实施例所述的移动机器人定位方法的系统，包括单元格能量值及粒子集初始化模块、粒子采集及位姿更新模块、机器人位姿获取模块、粒子集更新模块和迭代执行模块；
所述单元格能量值及粒子集初始化模块，用于获取机器人移动时生成的二维占用单元格地图，将所述二维占用单元格地图转换为三维单元格地图，获取所述三维单元格地图中每个单元格的能量值，以粒子表示机器人的位姿，初始化粒子对应的位姿，确定粒子集Xt；
所述粒子采集及位姿更新模块，用于采集机器人的运动信息，根据所述运动信息采集粒子，并更新粒子的位姿；
所述机器人位姿获取模块，用于获取粒子所在的单元格，将所在单元格对应的能量值赋值给粒子，得到粒子的预测能量值，获取机器人的真实能量值
Figure BDA0002460128850000111
根据所述预测能量值和真实能量值确定粒子的权重，根据粒子的位姿和粒子的权重，获取机器人的当前位姿；
所述粒子集更新模块，用于根据粒子的权重在粒子集Xt中进行粒子采样，得到粒子集XL，根据每个单元格的能量进行粒子采样，得到粒子集XG，由粒子集XL和粒子集XG组成新的粒子集；
所述迭代执行模块，用于使所述粒子集更新模块不断获取新的粒子集，使所述粒子采集及位姿更新模块不断更新粒子的位姿，并使所述机器人位姿获取模块获取机器人在移动中的位姿。
需要说明的是，实施例1-3未重复描述之处可互相借鉴。
本发明公开了一种移动机器人定位方法及系统，通过获取机器人移动时生成的二维占用单元格地图，将所述二维占用单元格地图转换为三维单元格地图，获取所述三维单元格地图中每个单元格的能量值，以粒子表示机器人的位姿，初始化粒子对应的位姿，确定粒子集Xt；采集机器人的运动信息，根据所述运动信息采集粒子，并更新粒子的位姿；获取粒子所在的单元格，将所在单元格对应的能量值赋值给粒子，得到粒子的预测能量值，获取机器人的真实能量值
Figure BDA0002460128850000112
根据所述预测能量值和真实能量值确定粒子的权重，根据粒子的位姿和粒子的权重，获取机器人的当前位姿；根据粒子的权重在粒子集Xt中进行粒子采样，得到粒子集XL，根据每个单元格的能量进行粒子采样，得到粒子集XG，由粒子集XL和粒子集XG组成新的粒子集；根据新的粒子集，不断更新粒子的位姿，得到机器人在移动中的位姿；实现了机器人在移动中位姿的确定，通过二维占用单元格地图进行预处理，将二维地图转换成三维地图，使得本发明提供的技术方案可以预测各个方向的能量值，进而使得本发明的技术方案适用于各种可视角度的激光雷达；本发明技术方案根据最大权重来决定划分粒子的方式，将粒子集分配成两个部分，分别用于全局定位与局部定位，且使用了动态的分配方法，使得本发明技术方案对各种工况具有较好的适应性。
以上所述本发明的具体实施方式，并不构成对本发明保护范围的限定。任何根据本发明的技术构思所做出的各种其他相应的改变与变形，均应包含在本发明权利要求的保护范围内。

Claims (10)

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

Cited By (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 合肥科大智能机器人技术有限公司 一种移动机器人定位方法

Cited By (1)

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

Also Published As

Publication number Publication date
CN111421548B (zh) 2021-10-19

Similar Documents

Publication Publication Date Title
CN109001711B (zh) 多线激光雷达标定方法
Hähnel et al. Map building with mobile robots in populated environments.
Kümmerle et al. Monte carlo localization in outdoor terrains using multilevel surface maps
EP2808842B1 (en) An apparatus and method for tracking and reconstructing three-dimensional objects
US8705792B2 (en) Object tracking using linear features
CN109798896A (zh) 一种室内机器人定位与建图方法及装置
CN105856230A (zh) 一种可提高机器人位姿一致性的orb关键帧闭环检测slam方法
Park et al. Radar localization and mapping for indoor disaster environments via multi-modal registration to prior LiDAR map
CN112923933A (zh) 一种激光雷达slam算法与惯导融合定位的方法
CN113483747A (zh) 基于具有墙角信息的语义地图改进amcl定位方法及机器人
CN114119920A (zh) 三维点云地图构建方法、系统
CN110187337B (zh) 一种基于ls和neu-ecef时空配准的高机动目标跟踪方法及系统
CN111739066B (zh) 一种基于高斯过程的视觉定位方法、系统及存储介质
Schadler et al. Multi-resolution surfel mapping and real-time pose tracking using a continuously rotating 2D laser scanner
CN114299318A (zh) 一种快速点云数据处理和目标图像匹配的方法及系统
CN112379393A (zh) 一种列车碰撞预警方法及其装置
CN113985435A (zh) 一种融合多激光雷达的建图方法及系统
CN111421548B (zh) 一种移动机器人定位方法及系统
CN114066773B (zh) 一种基于点云特征与蒙特卡洛扩展法的动态物体去除
CN115131514A (zh) 一种同时定位建图的方法、装置、系统及存储介质
CN114047766B (zh) 面向室内外场景长期应用的移动机器人数据采集系统及方法
CN106097277B (zh) 一种基于视觉测量的绳索物质点跟踪方法
CN112802195B (zh) 一种基于声呐的水下机器人连续占据建图方法
CN111765883B (zh) 机器人蒙特卡罗定位方法、设备及存储介质
Liu et al. A localizability estimation method for mobile robots based on 3d point cloud feature

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