一种同时定位与地图创建方法及装置制造方法
【专利摘要】本申请提供一种同时定位与地图创建方法及装置，通过对机器人的位姿初始值进行高斯概率分布、离散近似，生成该机器人在下一时刻的多个位姿采样，计算各个位姿采样的权重并获取与预先设置的采样权重范围相匹配的权重作为目标权重，将与目标权重对应的位姿采样确定为目标位姿采样，并获取与位姿采样相应的感知信息，利用感知信息更新预先设定的全局地图中与所述目标位姿采样相对应的位置相关联的路标，使得能够更快速的实现收敛，以较少的粒子就能达到较高的定位精度，并且定位精度基本不受粒子数的影响。
【专利说明】一种同时定位与地图创建方法及装置
【技术领域】
[0001]本申请涉及机器人定位与地图创建【技术领域】，特别是涉及一种同时定位与地图创
建方法及装置。
【背景技术】
[0002]机器人定位和地图创建问题是移动机器人导航研究中的关键问题，可靠导航是自主机器人最基本、最重要的一项功能，对移动机器人的研究有非常重要的意义。
[0003]在对机器人进行定位和地图创建的过程中主要是通过粒子滤波器来进行计算的，粒子滤波器中的新位置粒子是从提议分布-运动模型中抽取的，如果提议分布和实际的后验分布的形状相似，那么根据提议分布抽取的粒子在利用权重函数进行补偿后能够很好的表示后验分布，但是由于提议分布没有考虑当前的感知信息，如果后验分布位于提议分布尾部较窄的一块区域内，则在权重函数取值较大区域对应的采样。且粒子滤波器往往存在早熟问题，即在经过若干次迭代之后，大多数的粒子权重都趋于零，从而只有少数的粒子真正对系统状态的估计起作用。
[0004]现有技术中，为了使粒子能够更好的表示系统的后验分布，Thrun等人提出了混合形式的提议分布。按照这种提议分布抽取的粒子能更好的表示系统的后验分布，但是这种方法使采样阶段的计算量大大增加。
[0005]为了减少粒子滤波的计算量，Fox提出了自适应粒子滤波器，这种新型的粒子滤波器能够根据系统状态的不确定性自适应的调整采样数的多少，但是这种滤波器更容易产生早熟现象。
[0006]为了防止粒子滤波器的早熟现象，Milstein等人提出了基于聚类的粒子滤波器，这种滤波器将采样分成不同的类，并通过保持各个类的采样数不变而防止采样聚于某个局部区域，但是这种方法为了解决上述粒子滤波器存在的缺陷，失去了粒子滤波器将主要的计算量集中于系统状态最有可能的区域的优点。
[0007]因此，亟需一种同时定位与地图创建方法及装置，能够更快速的实现收敛，以较少的粒子就能达到较高的定位精度，并且定位精度基本不受粒子数的影响。
【发明内容】
[0008]有鉴于此，本申请实施例提供一种同时定位与地图创建方法及装置，能够更快速的实现收敛，以较少的粒子就能达到较高的定位精度，并且定位精度基本不受粒子数的影响。
[0009]为了实现上述目的，本申请实施例提供的技术方案如下:
[0010]一种同时定位与地图创建方法，包括:
[0011]对机器人的位姿初始值进行高斯概率分布、离散近似，生成该机器人在下一时刻的多个位姿采样；
[0012]分别计算各个所述位姿采样的权重；[0013]在各个所述权重中获取与预先设置的采样权重范围相匹配的权重，作为目标权重；
[0014]将与所述目标权重对应的位姿采样确定为目标位姿采样，获取与所述目标位姿采样相应的感知信息；
[0015]利用所述感知信息，更新预先设定的全局地图中与所述目标位姿采样相对应的位置相关联的路标。
[0016]优选的，还包括:接收用户输入的机器人的位姿初始值。
[0017]优选的，当利用所述感知信息，更新预先设定的全局地图中与所述目标位姿采样相对应的位置相关联的路标之后，还包括:分别将各个所述目标位姿采样的值作为位姿初始值，执行如上述权利要求1所述的同时定位与地图创建方法。
[0018]优选的，在分别计算各个所述位姿采样的权重之前还包括:
[0019]计算当前位姿采样集的近似后验值；
[0020]根据所述近似后验值以及预先设置的阈值，判断是否需要进行重新采样。
[0021]优选的，还包括:将机器人的当前感知信息融合到提议分布中。
[0022]优选的，通过无偏变换对提议分布采样粒子，控制所述粒子向后验分布的高概率区域移动。
[0023]优选的，所述利用所述感知信息，更新预先设定的全局地图中与所述目标位姿采样相对应的位置相关联的路标，包括:
[0024]利用所述感知信息以及预先设置的无偏卡尔曼滤波器，更新预先设定的全局地图中与所述目标位姿采样相对应的位置相关联的路标。
[0025]优选的，还包括:根据所述感知信息，判断是否需要更新预先设定的全局地图中与所述目标位姿采样相对应的位置相关联的路标。
[0026]一种同时定位与地图创建装置，包括:位姿采样单元、权重计算单元、重新采样单元、感知信息获取单元以及路标更新单元，其中，
[0027]所述位姿采样单元用于对机器人的位姿初始值进行高斯概率分布、离散近似，生成该机器人在下一时刻的多个位姿米样；
[0028]所述权重计算单元与所述位姿采样单元相连接，用于分别计算各个所述位姿采样的权重；
[0029]所述重新采样单元与所述权重计算单元相连接，用于在各个所述权重中获取与预先设置的采样权重范围相匹配的权重，作为目标权重；
[0030]所述感知信息获取单元与所述重新采样单元相连接，用于将所述目标权重对应的位姿采样确定为目标位姿采样，获取与所述目标位姿采样相应的感知信息；
[0031]所述路标更新单元与所述感知信息获取单元相连接，用于利用所述感知信息，更新预先设定的全局地图中与所述目标位姿采样相对应的位置相关联的路标。
[0032]优选的,还包括:位姿初始值接收单元，
[0033]所述位姿初始值接收单元与位姿采样单元相连接，用于接收用户输入的机器人的位姿初始值。
[0034]本申请提供一种同时定位与地图创建方法及装置，通过对机器人的位姿初始值进行高斯概率分布、离散近似，生成该机器人在下一时刻的多个位姿采样，计算各个位姿采样的权重并获取与预先设置的采样权重范围相匹配的权重作为目标权重，将与目标权重对应的位姿采样确定为目标位姿采样，并获取与位姿采样相应的感知信息，利用感知信息更新预先设定的全局地图中与所述目标位姿采样相对应的位置相关联的路标，使得能够更快速的实现收敛，以较少的粒子就能达到较高的定位精度，并且定位精度基本不受粒子数的影响。
【专利附图】
【附图说明】
[0035]为了更清楚地说明本申请实施例或现有技术中的技术方案，下面将对实施例或现有技术描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本申请中记载的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。 [0036]图1为本申请实施例一提供的一种同时定位与地图创建方法流程图；
[0037]图2为本申请实施例一提供的一种问题的图形模式示意图；
[0038]图3为本申请实施例二提供的一种同时定位与地图创建方法流程图；
[0039]图4为本申请实施例三提供的一种同时定位与地图创建装置的结构示意图。
【具体实施方式】
[0040]为了使本【技术领域】的人员更好地理解本申请中的技术方案，下面将结合本申请实施例中的附图，对本申请实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本申请一部分实施例，而不是全部的实施例。基于本申请中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都应当属于本申请保护的范围。
[0041]实施例一:
[0042]图1为本申请实施例一提供的一种同时定位与地图创建方法流程图。
[0043]如图1所示，该方法包括:
[0044]S101、对机器人的位姿初始值进行高斯概率分布、离散近似，生成该机器人在下一时刻的多个位姿采样。
[0045]在本申请实施例中，对机器人的位姿初始值进行高斯概率分布、离散近似，生成该机器人在下一时刻的多个位姿米样的过程如下:
[0046]在本申请实施例中，当机器人穿过一个未知环境时，设t时刻机器人位姿St= [xt, yt, 0 t]T,已经观测到的地图为M,其中mk表示第k个路标,K表示已经观测的路标数，kte {I,-,N}表示t时刻感知到的路标索引号。系统的完整状态可以表示为xt=[s1:t，M]τ，其中Spt=S1, S2,…，St表示机器人从I到t时刻的运动路径，同样，IV1表示t-ι到t时刻的运动控制信息，Zt表示机器人的当前感知信息。机器人从位姿Stl开始通过控制命令序列U0, U1,, IV1移动，随着机器人的移动，附近的路标被感知到，时刻t = I,感知到路标Hl1,并获得测量数据Z1 (包括距离和方向)，时刻t = 2，感知到路标m2，并在时刻t=3，重新感知到路标Hi1，现在已经形成的地图为同时定位与地图创建的输入信息是，路标观测信息z1:t，以及运动控制信息同时定位与地图创建的目的是，根据输入信息估计机器人运动路径s1:t以及地图M。[0047]在本申请实施例中，首先将当前的感知信息Zt加入到传统提议分布p(st| Sllt-/1), Zt)中，其中，St表示t时刻机器人位姿，Sllt=S1, S2,…，St表示机器人的运动路径，表示机器人的运动控制信息，Zt表示机器人的当前感知信息。通过将当前感知信息Zt加入到传统提议分布中，使从先验分布采样的粒子向后验高概率区域移动。
[0048]应用无偏变换计算感知变量Zt的均值I；和方差C *精度可以达到二阶泰勒扩展，
设L是St的维数，主要分为以下几步:
[0049]
【权利要求】
1.一种同时定位与地图创建方法，其特征在于，包括: 对机器人的位姿初始值进行高斯概率分布、离散近似，生成该机器人在下一时刻的多个位姿采样； 分别计算各个所述位姿采样的权重； 在各个所述权重中获取与预先设置的采样权重范围相匹配的权重，作为目标权重； 将与所述目标权重对应的位姿采样确定为目标位姿采样，获取与所述目标位姿采样相应的感知信息； 利用所述感知信息，更新预先设定的全局地图中与所述目标位姿采样相对应的位置相关联的路标。
2.根据权利要求1所述的方法，其特征在于，还包括:接收用户输入的机器人的位姿初始值。
3.根据权利要求1所述的方法，其特征在于，当利用所述感知信息，更新预先设定的全局地图中与所述目标位姿采样相对应的位置相关联的路标之后，还包括:分别将各个所述目标位姿采样的值作为位姿初始值，执行如上述权利要求1所述的同时定位与地图创建方法。
4.根据权利要求1所述的方法，其特征在于，在分别计算各个所述位姿采样的权重之前还包括: 计算当前位姿采样集的近似`后验值； 根据所述近似后验值以及预先设置的阈值，判断是否需要进行重新采样。
5.根据权利要求1所述的方法，其特征在于，还包括:将机器人的当前感知信息融合到提议分布中。
6.根据权利要求5所述的方法，其特征在于，通过无偏变换对提议分布采样粒子，控制所述粒子向后验分布的高概率区域移动。
7.根据权利要求1所述的方法，其特征在于，所述利用所述感知信息，更新预先设定的全局地图中与所述目标位姿采样相对应的位置相关联的路标，包括: 利用所述感知信息以及预先设置的无偏卡尔曼滤波器，更新预先设定的全局地图中与所述目标位姿采样相对应的位置相关联的路标。
8.根据权利要求7所述的方法，其特征在于，还包括:根据所述感知信息，判断是否需要更新预先设定的全局地图中与所述目标位姿采样相对应的位置相关联的路标。
9.一种同时定位与地图创建装置，其特征在于，包括:位姿采样单元、权重计算单元、重新采样单元、感知信息获取单元以及路标更新单元，其中， 所述位姿采样单元用于对机器人的位姿初始值进行高斯概率分布、离散近似，生成该机器人在下一时刻的多个位姿采样； 所述权重计算单元与所述位姿采样单元相连接，用于分别计算各个所述位姿采样的权重； 所述重新采样单元与所述权重计算单元相连接，用于在各个所述权重中获取与预先设置的采样权重范围相匹配的权重，作为目标权重； 所述感知信息获取单元与所述重新采样单元相连接，用于将所述目标权重对应的位姿采样确定为目标位姿采样，获取与所述目标位姿采样相应的感知信息；所述路标更新单元与所述感知信息获取单元相连接，用于利用所述感知信息，更新预先设定的全局地图中与所述目标位姿采样相对应的位置相关联的路标。
10.根据权利要求9所述的装置，其特征在于，还包括:位姿初始值接收单元， 所述位姿初始值接收单元与位姿采样单元相连接，用于接收用户输入的机器人的位姿初 始值。
【文档编号】G05D1/02GK103631264SQ201310646196
【公开日】2014年3月12日 申请日期:2013年12月4日 优先权日:2013年12月4日
【发明者】厉茂海, 林睿, 王振华, 陈国栋, 孙荣川 申请人:苏州大学张家港工业技术研究院