CN113313827A - 一种基于连续置信度分布的机器人建图方法 - Google Patents

一种基于连续置信度分布的机器人建图方法 Download PDF

Info

Publication number
CN113313827A
CN113313827A CN202110678866.8A CN202110678866A CN113313827A CN 113313827 A CN113313827 A CN 113313827A CN 202110678866 A CN202110678866 A CN 202110678866A CN 113313827 A CN113313827 A CN 113313827A
Authority
CN
China
Prior art keywords
grid
mutual information
map
measurement
robot
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
CN202110678866.8A
Other languages
English (en)
Other versions
CN113313827B (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.)
Zhejiang University ZJU
Original Assignee
Zhejiang University ZJU
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 Zhejiang University ZJU filed Critical Zhejiang University ZJU
Priority to CN202110678866.8A priority Critical patent/CN113313827B/zh
Publication of CN113313827A publication Critical patent/CN113313827A/zh
Application granted granted Critical
Publication of CN113313827B publication Critical patent/CN113313827B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T17/00—Three dimensional [3D] modelling, e.g. data description of 3D objects
    • G06T17/05—Geographic models
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T7/00—Image analysis
    • G06T7/70—Determining position or orientation of objects or cameras
    • G06T7/73—Determining position or orientation of objects or cameras using feature-based methods
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T2207/00—Indexing scheme for image analysis or image enhancement
    • G06T2207/30—Subject of image; Context of image processing
    • G06T2207/30244—Camera pose

Landscapes

  • Engineering & Computer Science (AREA)
  • Physics & Mathematics (AREA)
  • Geometry (AREA)
  • Software Systems (AREA)
  • General Physics & Mathematics (AREA)
  • Theoretical Computer Science (AREA)
  • Remote Sensing (AREA)
  • Computer Graphics (AREA)
  • Computer Vision & Pattern Recognition (AREA)
  • Image Analysis (AREA)
  • Manipulator (AREA)

Abstract

本发明公开了一种基于连续置信度分布的机器人建图方法，包括步骤：初始化占据网格地图及置信度分布，初始化波束混合传感模型；基于机载传感器的距离测量信息计算地图的连续置信度分布；针对传统基于占据网格地图的互信息未考虑地图网格单元之间的依赖性，导致互信息估计精度和运算效率不平衡的问题，提出一种基于连续置信度分布的互信息度量，显式地建模了同时刻同一测量锥内的网格单元间的测量依赖性，得到每个被观测网格的互信息；通过近似计算方法，更新整个区域在新测量下的互信息，得到互信息实值地图。本发明的互信息建图方法允许在线实现，可在非结构化和杂乱的环境中使机器人对未知区域和模糊区域表现出良好的探索行为。

Description

一种基于连续置信度分布的机器人建图方法
技术领域
本发明属于机器人感知技术领域，主要涉及一种基于连续置信度分布的机器人建图方法。
背景技术
机器人的主动探测目的在于控制机器人绘制更多未知区域的地图，同时保证地图的精度、时间成本、行程距离、能量节约等其他指标，在军事和民用领域有着广阔的应用前景，如未知行星地面探测和军事侦察。目前，在相关技术中，基于信息的探测方法更受关注，因为该方法容许更快的探测，并且较为容易扩展包含更多不规则和小边界的三维场景。这些方法利用互信息(Mutual information,MI)等信息论度量来构造报酬函数，并选择最优控制策略，以最小化二维和三维地图的不确定性。
目前较为流行的基于信息的探测方法一般基于查询和更新较快的占据网格地图(Occupancy Grid Map，OGM)计算MI，也称为占据网格互信息(Occupancy Grid MutualInformation，OGMI)，因为在很多应用场景中OGM足以实现无碰撞路径规划和导航[1-3]。然而，这些代表性的OGMI方法使用了许多来自OGM的理想假设，如OGM中常用的反演传感模型(Inverse Sensor Model，ISM)需要根据所处环境和特定任务手动调节参数，从而限制了OGMI在不同场景和任务的性能。此外，独立更新的网格单元占据值会导致地图和传感器测量之间产生不一致估计的冲突，具体来说，噪声测量、传感器的可视范围和环境中的几何结构等带来的内在依赖性和耦合性在互信息的计算中被忽略，而这将会导致基于信息的探测方法在一些杂乱、非结构化、以及大尺度场景中可能会使机器人规划出次优甚至不理想的路径，从而无法在给定指标下完成探测任务。传统占据网格地图的置信度一般为伯努利分布，对环境不确定性的描述能力也不如连续性概率分布。
基于以上认识，在占据网格地图的基础上，本发明提出了一种基于连续置信度分布的机器人建图方法。本发明在不显著增加计算量的前提下，考虑了网格单元之间的测量相关性和空间相关性，提高了地图探测的精度，保证了精度和效率的平衡；基于占据网格地图的连续置信度分布提出了置信度增强互信息(Confidence-rich Mutual Information，CRMI)概念，基于传感因果模型(Sensor Cause Model，SCM)在每个时间步对同一测量锥内的网格单元之间的测量依赖性进行显式建模，得到每个波束上每个相交地图单元的置信度增强互信息，通过近似计算得到整个区域的置信度增强互信息。本发明提出的置信度增强互信息构建的实值地图可使机器人在非结构化和杂乱环境中对未知区域和模糊区域表现出良好的探索行为，从而完成未知环境的探测任务。
[1]Julian B J,Karaman S,D Rus.On mutual information-based control ofrange sensing robots for mapping applications[J].International Journal ofRobotics Research,2014,33(10):5156-5163.
[2]Zhang Z,Henderson T,Karaman S,et al.FSMI:Fast computation ofShannon mutual information for information-theoretic mapping[J].TheInternational Journal of Robotics Research,2020,39(9):1155-1177.
[3]Charrow B,Liu S,Kumar V,et al.Information-Theoretic Mapping UsingCauchy-Schwarz Quadratic Mutual Information[C]//2015IEEE InternationalConference on Robotics and Automation(ICRA),2015,pp.4791–4798.
发明内容
本发明的目的在于，针对传统基于占据网格地图的互信息方法未考虑地图网格单元之间的依赖性，且基于反演传感模型的互信息会导致精度和效率不平衡的问题，基于占据网格地图的连续置信度分布，提出一种称为置信度增强互信息的信息论度量；基于因果传感模型，在每个时间步对同一测量锥内的网格单元之间的测量依赖性进行显式建模，得到每个波束上每个与地图相交的网格单元的置信度增强互信息，通过近似计算并更新整个区域在新测量下的置信度增强互信息，并构建实值地图。本发明的置信度增强互信息建图方法允许在线实现，可使机器人在非结构化和杂乱的环境中对未知区域和模糊区域表现出良好的探索行为，为机器人完成主动探测任务提供准确的信息论度量。
为了解决上述问题，本发明提供了如下技术方案：
本发明公开了一种基于连续置信度分布的机器人建图方法，其包括以下步骤：
步骤1：定义一个n维占据网格向量M＝{M[1],M[2],...,M[n]}，初始化定义在网格上的占据值向量m＝{m[1],...,m[n]}，其中m[i]∈[0,1]表示单元i的占据水平；初始化占据值向量m对应的连续置信度分布
Figure BDA0003122060950000031
由n个边缘化连续概率分布
Figure BDA0003122060950000032
组成，其中z1:k＝{z1,...,zk}和x1:k＝{x1,...,xk}分别表示t＝1时刻到t＝k时刻的机载传感器测量和全局位置信息；为便于置信度分布的更新计算和实现，将
Figure BDA0003122060950000033
离散化为向量
Figure BDA0003122060950000034
λm为离散分辨率；初始化混合波束传感模型p(z|xk,m)；
步骤2：机器人搭载的传感器获取k时刻周围环境的距离测量信息zk后，使用光线投射法计算得到第j个波束
Figure BDA0003122060950000035
和占据网格向量M相交的网格单元集合
Figure BDA0003122060950000036
其中j＝1,...,nz，
Figure BDA0003122060950000037
内网格单元的索引i按照k时刻各个网格单元和机器人位置之间的距离disti由近到远升序排列，
Figure BDA0003122060950000038
步骤3：计算k时刻的置信度分布
Figure BDA0003122060950000039
步骤4：根据过往观测和位姿信息，利用k-1时刻的置信度分布
Figure BDA00031220609500000310
计算地图的信息熵：
Figure BDA00031220609500000311
其中
Figure BDA00031220609500000312
为
Figure BDA00031220609500000313
的离散化向量；
步骤5：通过近似方法，计算并更新地图和测量之间的互信息I，包括以下子步骤：
5.1对于第j个波束
Figure BDA00031220609500000314
遍历
Figure BDA00031220609500000315
内所有网格单元，将该波束的距离测量值
Figure BDA00031220609500000316
离散化为
Figure BDA00031220609500000317
其中离散分辨率为λz；
5.2对于第
Figure BDA00031220609500000318
个网格单元，计算该波束在当前取值z对应的测量先验值p(z|z1:k-1,x1:k-1)：
Figure BDA00031220609500000319
其中
Figure BDA0003122060950000041
为指示变量，当且仅当第j个波束在k时刻为最大距离读数zMAX时等于1，否则为0，p(z|ck,xk)即混合波束传感模型，
Figure BDA0003122060950000042
mbin＝0表示该条波束上的网格单元占据值均为0，
Figure BDA0003122060950000043
为当前波束的测量锥内的地图网格单元数量；
5.3计算当前取值z下的地图条件熵：
Figure BDA0003122060950000044
5.4根据互信息公式计算并更新每个网格单元的互信息I[i]：
I[i]＝h(M[i]|zk＝z,z1:k-1,x1:k-1)-h(M[i]|z1:k-1,x1:k-1)；
5.5假设
Figure BDA0003122060950000045
内网格单元之间是独立的，则单个波束的互信息可由每个网格单元的互信息累加近似，其中M[i]表示：
Figure BDA0003122060950000046
5.6假设波束之间是相互独立的，于是测量zk对应的互信息可通过累加每个波束的互信息进行近似：
Figure BDA0003122060950000047
最终得到基于连续置信度分布的互信息实值地图，可用于基于信息报酬函数的机器人探测任务。
5.7运算复杂度分析：在本发明中，一次新的测量到达后，地图构建和MI计算的复杂度分别为
Figure BDA0003122060950000048
和
Figure BDA0003122060950000049
而OGM的地图更新和MI计算复杂度分别为O(nc log n)和
Figure BDA00031220609500000410
因为传感器测量距离有限且波束开角较小，故nc＜＜n，且
Figure BDA00031220609500000411
一般取较小的值即可达到较高精度，因此本发明的复杂度未显著提升。
作为本发明的优选，所述的步骤3具体为：
3.1使用k-1时刻的置信度分布
Figure BDA00031220609500000412
计算更新
Figure BDA00031220609500000413
内所有网格单元的期望占据值
Figure BDA0003122060950000051
Figure BDA0003122060950000052
3.2计算第j个波束的因果传感模型vscm；
3.3对于
Figure BDA0003122060950000053
内第i个网格单元，其离散化置信度分布
Figure BDA0003122060950000054
的更新方式为：
Figure BDA0003122060950000055
3.4遍历k时刻测量zk的nz个波束，即可更新机器人感知范围内所有网格单元的离散化置信度分布
Figure BDA0003122060950000056
作为本发明的优选，所述的步骤3.2具体为：
3.2.1初始化vscm为0向量，大小为nc，初始化中间变量p1＝1，p2＝1，遍历所有可能导致测量的诱因
Figure BDA0003122060950000057
记为i；
3.2.2若当前波束
Figure BDA0003122060950000058
的测量读数为最大值zMAX，则对于每个诱因i，其因果传感模型的值按如下方式更新：
Figure BDA0003122060950000059
3.2.3若当前波束
Figure BDA00031220609500000510
的测量读数不是最大值zMAX，则对于每个
Figure BDA00031220609500000511
其因果传感模型的值按如下方式更新：对于非首个元素的网格单元i，计算
Figure BDA00031220609500000512
然后根据每个网格单元i和传感器之间的距离disti，通过混合波束传感模型计算得到测量似然
Figure BDA00031220609500000513
从而得到
Figure BDA00031220609500000514
最后对vscm归一化处理。
与现有技术相比，本发明的有益效果为：
本发明实现了一种基于连续置信度分布的机器人建图方法，优点在于：采用基于波束的因果传感模型(SCM)得到的占据地图的连续置信度分布，定义了每个网格单元的CRMI，相对于反演传感模型(ISM)定义的互信息OGMI，采用CRMI的机器人可以在大尺度非结构化、杂乱和受限环境中表现出更好的探测行为；为了获得更精确的MI估计值，本发明在CRMI中显式地建模了每个时间步在同一测量锥内的地图网格单元之间的测量依赖性，在大的杂乱和非结构化环境中更具表现力和适用性；同时，本发明在提高互信息精度的同时，采用近似计算方法，确保置信度更新和CRMI的时间复杂度最差为平方复杂度，类似于OGMI，可允许算法的在线实现。
附图说明
图1为本发明的实施例中的传感器单个波束测量示意图。
图2为本发明的实施例1中的场景及机器人轨迹示意图。
图3为本发明的实施例1中的置信度增强互信息CRMI示意图。
图4为本发明的实施例1中的占据网格互信息OGMI示意图。
图5为本发明的实施例1中的地图熵变化量示意图。
图6为本发明的实施例2中的场景地图真实值示意图。
图7为本发明的实施例2中的置信度增强互信息CRMI示意图。
图8为本发明的实施例2中的占据网格互信息OGMI示意图。
图9为本发明的实施例3中的地面场景的卫星地图。
图10为本发明的实施例3中的置信度增强互信息CRMI示意图。
具体实施方式
下面结合附图对本发明方法的实施方式做详细说明。
参考图1—图10，一种基于连续置信度分布的机器人建图方法，具体步骤包括：
步骤1：定义一个n维占据网格向量M＝{M[1],M[2],...,M[n]}，初始化定义在网格上的占据值向量m＝{m[1],...,m[n]}，其中m[i]∈[0,1]表示单元i的占据水平；初始化占据值向量m对应的连续置信度分布
Figure BDA0003122060950000061
由n个边缘化连续概率分布
Figure BDA0003122060950000062
组成，其中z1:k＝{z1,...,zk}和x1:k＝{x1,...,xk}分别表示t＝1时刻到t＝k时刻的机载传感器测量和全局位置信息；为便于置信度分布的更新计算和实现，将
Figure BDA0003122060950000063
离散化为向量
Figure BDA0003122060950000064
λm为离散分辨率；初始化混合波束传感模型p(z|xk,m)；
在本发明的实施例中，作为本发明的优选，采用如下由基于高斯分布、均匀分布、指数分布等传感模型组成的混合波束传感模型：
Figure BDA0003122060950000071
其中z*为实际测量值：
Figure BDA0003122060950000072
步骤2：机器人搭载的传感器获取k时刻周围环境的距离测量信息zk后，使用光线投射法计算得到第j个波束
Figure BDA0003122060950000073
和占据网格向量M相交的网格单元集合
Figure BDA0003122060950000074
其中j＝1,...,nz，
Figure BDA0003122060950000075
内网格单元的索引i按照k时刻各个网格单元和机器人位置之间的距离disti由近到远升序排列，
Figure BDA0003122060950000076
步骤3：计算k时刻的置信度分布
Figure BDA0003122060950000077
作为本发明的优选，所述的步骤3具体为：
3.1使用k-1时刻的置信度分布
Figure BDA0003122060950000078
计算更新
Figure BDA0003122060950000079
内所有网格单元的期望占据值
Figure BDA00031220609500000710
Figure BDA00031220609500000711
3.2计算第j个波束的因果传感模型vscm；
3.3对于
Figure BDA00031220609500000712
内第i个网格单元，其离散化置信度分布
Figure BDA00031220609500000713
的更新方式为：
Figure BDA00031220609500000714
3.4遍历k时刻测量zk的nz个波束，即可更新机器人感知范围内所有网格单元的离散化置信度分布
Figure BDA0003122060950000081
作为本发明的优选，所述的步骤3.2具体为：
3.2.1初始化vscm为0向量，大小为nc，初始化中间变量p1＝1，p2＝1，遍历所有可能导致测量的诱因
Figure BDA0003122060950000082
记为i；
3.2.2如图1所示，C(x)表示当前波束与地图相交得到的测量锥，|C(x)|为测量锥内网格单元数量，即nc。若当前波束
Figure BDA0003122060950000083
的测量读数为最大值zMAX，即最大距离情况，则对于每个诱因i，其因果传感模型的值按如下方式更新：
Figure BDA0003122060950000084
3.2.3若当前波束
Figure BDA0003122060950000085
的测量读数不是最大值zMAX，即图1中的非最大距离情况，则对于每个
Figure BDA0003122060950000086
其因果传感模型的值按如下方式更新：对于非首个元素的网格单元i，计算
Figure BDA0003122060950000087
然后根据每个网格单元i和传感器之间的距离disti，通过混合波束传感模型计算得到测量似然
Figure BDA0003122060950000088
从而得到
Figure BDA0003122060950000089
最后对vscm归一化处理。
步骤4：根据过往观测和位姿信息，利用k-1时刻的置信度分布
Figure BDA00031220609500000810
计算地图的信息熵：
Figure BDA00031220609500000811
其中
Figure BDA00031220609500000812
为
Figure BDA00031220609500000813
的离散化向量；
步骤5：通过近似方法，计算并更新地图和测量之间的互信息I，包括以下子步骤：
5.1对于第j个波束
Figure BDA00031220609500000814
遍历
Figure BDA00031220609500000815
内所有网格单元，将该波束的距离测量值
Figure BDA00031220609500000816
离散化为
Figure BDA00031220609500000817
其中离散分辨率为λz；
5.2对于第
Figure BDA00031220609500000818
个网格单元，计算该波束在当前取值z对应的测量先验值p(z|z1:k-1,x1:k-1)：
Figure BDA0003122060950000091
其中
Figure BDA0003122060950000092
为指示变量，当且仅当第j个波束在k时刻为最大距离读数zMAX时等于1，否则为0，p(z|ck,xk)即混合波束传感模型，
Figure BDA0003122060950000093
mbin＝0表示该条波束上的网格单元占据值均为0，
Figure BDA0003122060950000094
为当前波束的测量锥内的地图网格单元数量；
5.3计算当前取值z下的地图条件熵：
Figure BDA0003122060950000095
5.4根据互信息公式计算并更新每个网格单元的互信息I[i]：
I[i]＝h(M[i]|zk＝z,z1:k-1,x1:k-1)-h(M[i]|z1:k-1,x1:k-1)； (9)
5.5假设
Figure BDA0003122060950000096
内网格单元之间是独立的，则单个波束的互信息可由每个网格单元的互信息累加近似，其中M[i]表示：
Figure BDA0003122060950000097
5.6假设波束之间是相互独立的，于是测量zk对应的互信息可通过累加每个波束的互信息进行近似：
Figure BDA0003122060950000101
最终得到基于连续置信度分布的互信息实值地图，可用于基于信息报酬函数的机器人探测任务。
5.7运算复杂度分析：在本发明中，一次新的测量到达后，地图构建和MI计算的复杂度分别为
Figure BDA0003122060950000102
和
Figure BDA0003122060950000103
而OGM的地图更新和MI计算复杂度分别为O(nc logn)和
Figure BDA0003122060950000104
因为传感器测量距离有限且波束开角较小，故nc＜＜n，且
Figure BDA0003122060950000105
一般取较小的值即可达到较高精度，因此本发明的复杂度未显著提升。
下面将上述实施过程，应用于机器人在小尺度仿真环境、办公场所环境以及大尺度校园场景环境的信息论探测任务，并使用参考文献[1]的OGMI方法作为对比，以验证本发明的有效性。表1所示为本发明所有实施例共同使用的参数，其余参数根据实施例不同而单独设定。
表1基本参数设置
Figure BDA0003122060950000106
实施例1：小尺度狭窄、杂乱仿真环境
在本实施例中，仿真机器人安装了二维全向激光雷达，可均匀发出60条波束，最大测量距离zMAX＝8m，在预设轨迹上一共采集了1144帧扫描数据。如图2所示，仿真环境大小为33m×33m，地图分辨率为λM＝0.33m。本实施例的置信度增强互信息CRMI结果如图3所示，MI的值均在[0,1]范围内。可以看出，边界、自由区域、模糊区域和未探测区域的CRMI区分十分明显，且顺序递增。边界和机器人自身周围的CRMI最小(深黑色)，这主要因为SCM计算过程中空置值(即1减去占据值)的累乘计算以及波束传感模型导致这些区域的SCM和对应的置信度更高。被多个波束在一个时间步内同时测量到的地图单元会被多次计算，从而得到更低的CRMI。未探测区域的CRMI最大(白色)，因为该区域内网格单元的占据值分布更加均匀。类似地，模糊区域的CRMI(灰白混合)比自由区域(灰色)的CRMI高，因为前者得到的观测相对更少。
图4为本实施例中的占据网格OGMI示意图，可以看出，CRMI在自由区域的均值约在0.55，而OGMI则为0.7左右，这主要因为OGMI在得到观测信息后下降更快并很快收敛到一个较高的值。具体地，OGMI的反演传感模型ISM一般采用Log-Odds更新，会加速OGM二元占据值的概率更快趋向0或1。这种短时间内的明显差异意味着在每个被观测单元上的分布变得更加“自信”。同时，OGMI的边际效应降低较快，使得条件熵在接收到少量观测后，对MI的减少贡献不大。而SCM的计算依赖于波束上每个单元的占据值估计，而每个单元上的置信度分布(概率密度函数)将根据SCM计算的参数逐渐过渡变化，而不是在新测量到达时突然发生变化。因此，根据公式(9)，自由区域内OGMI大于CRMI。
图5为两种互信息度量下环境地图信息熵的变化量。可以看出，OGMI很快收敛到较高值，其地图熵减少量比CRMI小约49.5％。此外，OGMI在小物体和窄过道等区域一致性更差，可能会给在线路径规划器带来潜在风险。
CRMI还表现了期望的探索行为。以一个由信息控制器驱动的机器人为例，基于CRMI的报酬函数使得机器人对未知空间和未知空间具有吸引行为，而对边界和当前姿态具有排斥行为，表现出类似[1]中的OGMI的探索行为，这意味着机器人在未来的某一时刻会主动探索所有未知区域，完成探测任务。
实施例2：办公场所环境实验
本实施例采用了公开数据集Intel Research Lab(Seattle)，该数据集含有910个激光雷达扫描，此处设定地图大小为30m×30m，地图分辨率为λM＝0.2m，最大测量距离设定为zMAX＝8m，λz＝0.1m。图6为该数据集真实地图，图7和图8分别为该区域CRMI和OGMI的示意图。可以看出，CRMI对该杂乱环境描述了更多的细节，如角落、小物体和房间等，而OGMI在这些区域都较为模糊，且在走廊和墙壁表现出“过于自信”，这也将导致采用信息控制器的机器人在这些区域产生强烈的排斥行为，可能会导致机器人快速掠过该区域，难以完成该狭小、受限制区域的地图构建和探索任务。
实施例3：大尺度校园场景环境实验
本实施例采用了含有2008个激光雷达扫描的公开数据集Freiburg Campus，该场景为大尺度非结构化户外环境，如图9的卫星地图所示。本实施例地图大小设置为250m×250m，地图分辨率为λM＝0.33m，最大测量距离设定为zMAX＝50m。如图10所示，CRMI表现出和实施例1和2类似的性能，尤其在模糊区域、杂乱区域和未探测区域的估计更加保守，同时又对小物体表现出精细化估计。表2的时间消耗对比也证明了本发明可在不显著增加运算复杂度的前提下，提高MI的估计精度，显示了本发明的优越性。
表2时间消耗对比(MI计算时间/总时间，单位：秒)
时间消耗 实施例1 实施例2 实施例3
CRMI 17.33/23.49 195.52/218.58 2588.34/2750.92
OGMI 18.48/23.66 188.9/211.75 2321.93/2463.6
上面结合附图对本发明的实施例进行了描述，但是本发明并不局限于上述的具体实施方式，上述的具体实施方式仅仅是示意性的，而不是局限性的，本领域的普通技术人员在本发明的启示下，在不脱离本发明宗旨和权利要求所保护的范围情况下，还可做出很多形式，这些均属于本发明的保护范围之内。

Claims (5)

1.一种基于连续置信度分布的机器人建图方法，其特征在于，包括以下步骤：
步骤1：定义一个n维占据网格向量M＝{M[1],M[2],...,M[n]}，初始化定义在网格上的占据值向量m＝{m[1],...,m[n]}，其中m[i]∈[0,1]表示单元i的占据水平；初始化占据值向量m对应的连续置信度分布
Figure FDA0003122060940000011
由n个边缘化连续概率分布
Figure FDA0003122060940000012
组成，其中z1:k＝{z1,...,zk}和x1:k＝{x1,...,xk}分别表示t＝1时刻到t＝k时刻的机载传感器测量和全局位置信息；为便于置信度分布的更新计算和实现，将
Figure FDA0003122060940000013
离散化为向量
Figure FDA0003122060940000014
λm为离散分辨率；初始化混合波束传感模型；
步骤2：机器人搭载的传感器获取k时刻周围环境的距离测量信息zk后，使用光线投射法计算得到第j个波束
Figure FDA0003122060940000015
和占据网格向量M相交的网格单元集合
Figure FDA0003122060940000016
其中j＝1,...,nz，
Figure FDA0003122060940000017
内网格单元的索引i按照k时刻各个网格单元和机器人位置之间的距离disti由近到远升序排列，
Figure FDA0003122060940000018
步骤3：计算k时刻的置信度分布
Figure FDA0003122060940000019
步骤4：根据过往观测和位姿信息，利用k-1时刻的置信度分布
Figure FDA00031220609400000110
计算地图的信息熵：
Figure FDA00031220609400000111
其中
Figure FDA00031220609400000112
为
Figure FDA00031220609400000113
的离散化向量；
步骤5：通过近似方法，计算并更新地图和测量之间的互信息I，包括以下子步骤：
5.1对于第j个波束
Figure FDA00031220609400000114
遍历
Figure FDA00031220609400000115
内所有网格单元，将该波束的距离测量值
Figure FDA00031220609400000116
离散化为
Figure FDA00031220609400000117
其中离散分辨率为λz；
5.2对于第
Figure FDA00031220609400000118
个网格单元，计算该波束在当前取值z对应的测量先验值p(z|z1:k-1,x1:k-1)：
Figure FDA0003122060940000021
其中
Figure FDA0003122060940000022
为指示变量，当且仅当第j个波束在k时刻为最大距离读数zMAX时等于1，否则为0，p(z|ck,xk)即混合波束传感模型，
Figure FDA0003122060940000023
mbin＝0表示该条波束上的网格单元占据值均为0，
Figure FDA0003122060940000024
为当前波束的测量锥内的地图网格单元数量；
5.3计算当前取值z下的地图条件熵：
Figure FDA0003122060940000025
5.4根据互信息公式计算并更新每个网格单元的互信息：
I[i]＝h(M[i]|zk＝z,z1:k-1,x1:k-1)-h(M[i]|z1:k-1,x1:k-1)；
5.5假设
Figure FDA0003122060940000026
内网格单元之间是独立的，则单个波束的互信息可由每个网格单元的互信息累加近似，其中M[i]表示：
Figure FDA0003122060940000027
5.6假设波束之间是相互独立的，于是测量zk对应的互信息可通过累加每个波束的互信息进行近似：
Figure FDA0003122060940000028
最终得到基于连续置信度分布的互信息实值地图，可用于基于信息报酬函数的机器人探测任务。
2.根据权利要求1所述的一种基于连续置信度分布的机器人建图方法，其特征在于，所述的步骤3包括以下子步骤：
3.1使用k-1时刻的置信度分布
Figure FDA0003122060940000029
计算更新
Figure FDA00031220609400000210
内所有网格单元的期望占据值
Figure FDA00031220609400000211
Figure FDA00031220609400000212
3.2计算第j个波束的因果传感模型vscm；
3.3对于
Figure FDA00031220609400000213
内第i个网格单元，其离散化置信度分布
Figure FDA00031220609400000214
的更新方式为：
Figure FDA0003122060940000031
3.4遍历k时刻测量zk的nz个波束，即可更新机器人感知范围内所有网格单元的离散化置信度分布
Figure FDA0003122060940000032
3.根据权利要求2所述的一种基于连续置信度分布的机器人建图方法，其特征在于，所述的步骤3.2计算第j个波束的因果传感模型vscm，包括以下子步骤：
3.2.1初始化vscm为0向量，大小为nc，初始化中间变量p1＝1，p2＝1，遍历所有可能导致测量的诱因
Figure FDA0003122060940000033
记为i；
3.2.2若当前波束
Figure FDA0003122060940000034
的测量读数为最大值zMAX，则对于每个诱因i，其因果传感模型的值按如下方式更新：
Figure FDA0003122060940000035
3.2.3若当前波束
Figure FDA0003122060940000036
的测量读数不是最大值zMAX，则对于每个
Figure FDA0003122060940000037
其因果传感模型的值按如下方式更新：对于非首个元素的网格单元i，计算
Figure FDA0003122060940000038
然后根据每个网格单元i和传感器之间的距离disti，通过混合波束传感模型计算得到测量似然
Figure FDA0003122060940000039
从而得到
Figure FDA00031220609400000310
最后对vscm归一化处理。
4.根据权利要求1所述的一种基于连续置信度分布的机器人建图方法，其特征在于，所述的步骤1中的机载传感器为距离测量传感器。
5.根据权利要求4所述的一种基于连续置信度分布的机器人建图方法，其特征在于，所述的距离测量传感器包括激光雷达、超声波传感器、深度视觉相机、声呐。
CN202110678866.8A 2021-06-18 2021-06-18 一种基于连续置信度分布的机器人建图方法 Active CN113313827B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202110678866.8A CN113313827B (zh) 2021-06-18 2021-06-18 一种基于连续置信度分布的机器人建图方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202110678866.8A CN113313827B (zh) 2021-06-18 2021-06-18 一种基于连续置信度分布的机器人建图方法

Publications (2)

Publication Number Publication Date
CN113313827A true CN113313827A (zh) 2021-08-27
CN113313827B CN113313827B (zh) 2022-02-22

Family

ID=77379265

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202110678866.8A Active CN113313827B (zh) 2021-06-18 2021-06-18 一种基于连续置信度分布的机器人建图方法

Country Status (1)

Country Link
CN (1) CN113313827B (zh)

Cited By (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN115022808A (zh) * 2022-06-21 2022-09-06 北京天坦智能科技有限责任公司 一种通信机器人的即时定位和无线电地图构建方法

Citations (7)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US20140350839A1 (en) * 2013-05-23 2014-11-27 Irobot Corporation Simultaneous Localization And Mapping For A Mobile Robot
US20180189565A1 (en) * 2015-08-28 2018-07-05 Imperial College Of Science, Technology And Medicine Mapping a space using a multi-directional camera
US20180210087A1 (en) * 2017-01-26 2018-07-26 The Regents Of The University Of Michigan Localization Using 2D Maps Which Capture Vertical Structures In 3D Point Data
CN108344999A (zh) * 2018-01-09 2018-07-31 浙江大学 一种用于水下机器人导航的声呐地图构建与重定位方法
US20200237176A1 (en) * 2018-01-05 2020-07-30 Irobot Corporation System for spot cleaning by a mobile robot
US20210103290A1 (en) * 2019-10-02 2021-04-08 Mitsubishi Electric Research Laboratories, Inc. System and Method for Probabilistic Multi-Robot SLAM
CN112802195A (zh) * 2020-12-30 2021-05-14 浙江大学 一种基于声呐的水下机器人连续占据建图方法

Patent Citations (8)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US20140350839A1 (en) * 2013-05-23 2014-11-27 Irobot Corporation Simultaneous Localization And Mapping For A Mobile Robot
US20180189565A1 (en) * 2015-08-28 2018-07-05 Imperial College Of Science, Technology And Medicine Mapping a space using a multi-directional camera
CN108369743A (zh) * 2015-08-28 2018-08-03 帝国科技及医学学院 使用多方向相机地图构建空间
US20180210087A1 (en) * 2017-01-26 2018-07-26 The Regents Of The University Of Michigan Localization Using 2D Maps Which Capture Vertical Structures In 3D Point Data
US20200237176A1 (en) * 2018-01-05 2020-07-30 Irobot Corporation System for spot cleaning by a mobile robot
CN108344999A (zh) * 2018-01-09 2018-07-31 浙江大学 一种用于水下机器人导航的声呐地图构建与重定位方法
US20210103290A1 (en) * 2019-10-02 2021-04-08 Mitsubishi Electric Research Laboratories, Inc. System and Method for Probabilistic Multi-Robot SLAM
CN112802195A (zh) * 2020-12-30 2021-05-14 浙江大学 一种基于声呐的水下机器人连续占据建图方法

Non-Patent Citations (3)

* Cited by examiner, † Cited by third party
Title
L.GAN 等: "Bayesian spatial kernel smoothing for scalable dense semantic mapping", 《IEEE ROBOT》 *
吴志志: "三轮全向移动机器人的同时定位与地图创建", 《中国优秀博硕士学位论文全文数据库(硕士)信息科技辑》 *
郑荣濠: "多自主移动机器人的编队控制及稳定性分析", 《中国优秀博硕士学位论文全文数据库(硕士)信息科技辑》 *

Cited By (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN115022808A (zh) * 2022-06-21 2022-09-06 北京天坦智能科技有限责任公司 一种通信机器人的即时定位和无线电地图构建方法
CN115022808B (zh) * 2022-06-21 2022-11-08 北京天坦智能科技有限责任公司 一种通信机器人的即时定位和无线电地图构建方法

Also Published As

Publication number Publication date
CN113313827B (zh) 2022-02-22

Similar Documents

Publication Publication Date Title
Thrun Learning occupancy grid maps with forward sensor models
Lang et al. Adaptive Non-Stationary Kernel Regression for Terrain Modeling.
Hähnel et al. Map building with mobile robots in populated environments.
Asgharivaskasi et al. Active bayesian multi-class mapping from range and semantic segmentation observations
Saulnier et al. Information theoretic active exploration in signed distance fields
CN111476286A (zh) 一种移动机器人构建地图方法
O'Callaghan et al. Continuous occupancy mapping with integral kernels
Senanayake et al. Spatio-temporal hilbert maps for continuous occupancy representation in dynamic environments
Kaufman et al. Bayesian occupancy grid mapping via an exact inverse sensor model
US11574480B2 (en) Computerized device for driving assistance
Santana et al. An approach for 2d visual occupancy grid map using monocular vision
Torroba et al. PointNetKL: Deep inference for GICP covariance estimation in bathymetric SLAM
Kuang et al. Ir-mcl: Implicit representation-based online global localization
Wu et al. Object slam-based active mapping and robotic grasping
CN113313827B (zh) 一种基于连续置信度分布的机器人建图方法
Sun et al. Dense 3-d mapping with spatial correlation via gaussian filtering
Hou et al. Volumetric next best view by 3D occupancy mapping using Markov chain Gibbs sampler for precise manufacturing
Dhawale et al. Efficient Parametric Multi-Fidelity Surface Mapping.
Wang et al. Virtual maps for autonomous exploration with pose SLAM
Tatavarti et al. Towards real-time segmentation of 3D point cloud data into local planar regions
Moreno et al. Differential Evolution Markov Chain filter for global localization
Sharma et al. Proxmap: Proximal occupancy map prediction for efficient indoor robot navigation
Joubert Adaptive occupancy grid mapping with measurement and pose uncertainty
CN111765883B (zh) 机器人蒙特卡罗定位方法、设备及存储介质
Pedrosa et al. A non-linear least squares approach to SLAM using a dynamic likelihood field

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