一种模仿人类记忆的移动机器人目标搜索方法

1.本发明涉及一种模仿人类记忆的移动机器人目标搜索方法，一种基于概率拓扑语义地图的模仿人类记忆的机器人目标搜索方法，属于移动机器人环境感知、建模与目标搜索技术领域。


背景技术：

2.服务机器人需要执行诸如端茶递水的协助性任务，而移动机器人工作在动态环境中，水杯等物体的位置会随时发生改变。常用于机器人导航的二维或三维栅格地图仅能满足避障所需，没有水杯等目标所在位置的信息，更无法使机器人应对物体位置的动态变化，通常导致目标搜索失败，所以机器人在进行目标搜索时需要获取语义信息在地图坐标系的映射。而且，根据最短路径、最短预期时间、最大可靠性等单一指标生成的搜索路径，往往不具有最优性。例如，将最短路径或最短运行时间作为优化指标生成的搜索路径，忽略了物体在环境中动态变化的因素；将最大可靠性作为优化指标应用于不确定性目标搜索过程中，忽略了搜索过程中的时间代价。当执行较为紧急的任务，如送药时，需要在预期搜索时间与物体存在概率之间进行综合考虑生成搜索路径。
3.目前已有一种移动机器人的目标搜索观测点自动生成方法(公开号cn112711256a)，通过rrt算法生成随机树tree并根据每一个树节点的观测区域对于整个地图可视化的贡献值以特殊方式进行筛选，自动生成能够观测到整个地图环境可行区域的观测点，可以满足自主移动机器人目标搜索任务需求，可用于自主移动机器人目标搜索任务规划。上述现有技术与本发明的区别在于：本发明先让移动机器人对环境进行自主探索，用于对当前环境结构信息以及语义信息进行感知。后自适应生成拓扑节点，进行拓扑地图的构建，用于简化地图结构；再在拓扑地图顶点与语义信息之间建立归属关系；最后，将仿人记忆机制的概率衰减模型应用于语义信息与拓扑节点之间归属关系的概率更新中。在目标搜索时，将目标在各个在拓扑节点感知区域内存在的概率以及预期搜索时间进行综合考虑，生成用于机器人搜索的最终路径。并对两个优化指标进行加权，以使机器人能够在应对时间紧迫的任务以及非时间紧迫任务之间进行动态调整。


技术实现要素：

4.本发明的目的是针对现有技术的不足，提供一种模仿人类记忆的移动机器人目标搜索方法，将人类的目标搜索过程映射至机器人目标搜索的过程中。
5.为实现上述目的，本发明的技术方案是：
6.一种模仿人类记忆的移动机器人目标搜索方法，包括以下步骤：
7.步骤一：语义信息提取；
8.一个描述当前环境语义信息的元组表示为《p,c,si,t》。
9.p为物体在全局坐标系下的三维坐标。c为物体的类别。si为当前物体所属的区域。t为识别到物体时的时间戳，表示从伦敦时间1970年1月1日0点开始到现在所经过的毫秒
数。
10.通过目标检测算法对视觉传感器的rgb信息进行检测，得到具有单个或多个物体的2d包围框。将像素平面上坐标映射到栅格地图坐标系，需经过以下转换关系。首先，将像素坐标系下坐标转化到成像平面。
[0011][0012]
像素坐标系与成像平面之间只存在单纯的平移关系，而且度量单位不同。经过公式(1)进行转换，其中，(u0,v0)代表成像平面原点在像素坐标系中的坐标位置，dx与dy代表每个像素在u,v轴方向上的物理尺寸。(u,v,1)代表像素坐标系下的齐次坐标，(x,y,1)代表成像平面坐标系下的对应坐标。
[0013]
从成像平面到相机坐标系之间的映射需要辅助信息zc，zc代表物体的深度，由深度相机获取。经过下式运算，将成像平面坐标系的坐标(x,y,1)映射到相机坐标系下的相应坐标(xc,yc,zc)，其中f为相机焦距，即为相机坐标系原点到成像平面的距离。
[0014][0015]
相机坐标系与机器人坐标系之间存在平移和旋转关系，经过下式运算即可求得相机坐标系坐标(xc,yc,zc)在机器人坐标系下的表示(xr,yr,zr)。其中，旋转矩阵r和平移矩阵t属于相机外参的一部分。
[0016][0017]rm
和tm为机器人在地图坐标系下相对旋转和平移矩阵，由自适应蒙特卡洛定位方法估计得出。经过以下运算求得机器人坐标系的坐标(xr,yr,zr)在地图坐标系下的表示(xm,ym,zm)。
[0018][0019]
因此，目标检测算法所获取的2d物体包围框通过下式，转换到栅格地图坐标系。
[0020][0021]
简化为：
[0022][0023]
其中，
[0024][0025]
为相机内参，由标定得到。
[0026]
步骤二：拓扑地图构建；
[0027]
以机器人为圆心，以传感器探测范围为半径构建机器人感知范围模型，并基于快速随机探索树(rapid random exploration tree,rrt)算法，在机器人工作环境中构建无碰撞的探索树，将树节点之间的连线删除；然后，从起始节点开始，获取机器人3/4倍感知范围内的树节点，并求解形心，计算如下。将式(8)作为损失函数，通过不断迭代，求解合适的形心，使得损失函数j最小。
[0028][0029]
其中，xn代表该当前机器人3/4倍感知范围内的树节点空间坐标，n为3/4倍感知范围内的树节点总数，n∈(1,n)，求解使得j最小的形心u0。
[0030][0031]
最后，以形心u0为圆心，以机器人最大感知范围为半径作圆，将圆内除圆心以外的树节点删减。通过循环删减，直至对树节点删减完毕。
[0032]
步骤三：仿人记忆的概率更新
[0033]
人类学习新知识后，会在短时间内保持对该知识的记忆，此时若不进行重新学习，则会逐渐将知识遗忘。基于此规律，赋予机器人以人类的记忆模型，以s形曲线描述物体在某个位置的概率变化关系。在某个区域识别到物体后，物体在该区域的概率会先保持在一个较高的状态，若没有在该位置重新发现该物体，则概率会逐渐衰减到0。公式如下所示：
[0034][0035]
其中，a代表遗忘率，b代表记忆半衰期，t为识别到物体时的时间戳，表示从伦敦时
间1970年1月1日0点开始到现在所经过的毫秒数，n代表重复发现的次数。其中，a越大，则概率衰减的越快，b越大则代表概率保持在50％以上的时间越久。
[0036]
人类在多次学习一个知识之后，对该知识的遗忘速度会变慢。同样的，当机器人多次在一个区域发现同一物体后，对该物体的遗忘率减小，使物体在该区域存在的概率衰减速度变慢。同时，记忆半衰期应同时延长，使概率长时间保持在一个较高的水平。
[0037]
对遗忘率的调整规律如下式，
[0038]an
＝a
0-nδa0ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(11)
[0039]
其中，an为记过n次重新发现后的遗忘率，a0为初始遗忘率，δa0代表遗忘率的变化率。
[0040]
对记忆半衰期的调整规律如下式，
[0041]bn
＝b0 nδb0ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(12)
[0042]
其中，bn为记过n次重新发现后的记忆半衰期，b0为初始记忆半衰期，δb0代表记忆半衰期的变化率。
[0043]
步骤四：概率-拓扑-语义信息融合
[0044]
拓扑地图代表了机器人当前工作环境的简化结构，在语义信息与拓扑节点之间建立归属关系。建立规则如下，将所有物体位置信息标记在度量地图中，获取在每个拓扑节点感知范围内物体的坐标，并将其添加进语义信息中，代表物体归属于某个拓扑节点。同时，从语义信息确定开始，仿人记忆机制对物体与拓扑节点归属关系进行概率衰减，若在同一个拓扑节点探测范围内多次发现该物体，则根据公式(11)-(12)对遗忘率以及记忆半衰期进行自适应调整，并根据公式(10)进行概率计算。经过以上步骤，实现对概率-拓扑-语义信息的融合。
[0045]
步骤五：基于概率-拓扑-语义地图的目标搜索策略
[0046]
对目标的查找过程如下：首先，给定需要查找的目标以及对应的roi，并对其sift特征进行提取；对于给定的目标，根据步骤四的融合地图，找出其所属的各个拓扑节点。再通过dijkstra算法向目标所对应的拓扑节点规划出一条实际可行的路径，求得机器人按照路径运行的预期时间tm，
[0047][0048]
其中，lm为从机器人当前位置到中目标所在拓扑节点的路径长度，v为机器人匀速运动的速度。并对预期时间进行归一化，求出从机器人当前位置向目标所在区域运动的归一化时间
[0049][0050]
然后，根据公式(10)获取目标物体m在各个拓扑节点的概率pm，并对概率进行归一化，求出目标在各个拓扑节点对应区域存在的归一化概率
[0051][0052]
由于机器人工作在动态环境中，物体位置分布是不确定的。所以，在进行目标搜索时，需要对物体所属拓扑节点概率与机器人当前位置到拓扑节点的预期时间同时进行考虑，用于生成搜索路径。作为机器人搜索的目标区域，不仅具有较大的目标存在概率，而且相对于机器人所在位置具有较短的预期运动时间。也就是当一个拓扑节点中物体存在概率不会小于其他拓扑节点，而且预期运动时间不会大于其余拓扑节点。即对于物体可能所在位置对应的m个拓扑节点，当
[0053][0054]
且
[0055][0056]
机器人会优先在物体存在概率以及预期搜索时间都较优的拓扑节点i进行搜索，而非拓扑节点j。因此，将物体存在概率以及预期搜索时间进行综合考虑，使用加权代价作为拓扑节点访问序列生成的指标。当物体存在概率越大，且预期时间越短，则加权代价越小；当概率越小，而预期时间越长，则加权代价越大。加权代价costm的计算公式为
[0057][0058]
式中，α为时间权重系数，用于调整执行时间紧迫任务的预期时间权重。当执行时间紧迫任务时，α增大；当执行非时间紧迫任务时，减小α，使机器人的目标搜索达到最优。
[0059]
步骤六：目标相似度计算
[0060]
对于视觉传感器捕捉的rgb信息进行目标检测，筛选出属于同类物体的roi，再对其关键点特征进行提取，计算其与目标物体特征点的欧氏距离；最后，判断欧氏距离是否小于阈值，若小于则认为当前识别物体即为目标物体，若大于阈值，则继续读取rgb信息，进行判断。
[0061]
与现有技术相比，本发明的有益效果在于：
[0062]
目标检测算法采用轻量化的网络结构，对计算能力的要求较低，能够在移动机器人计算资源有限的平台上进行实时运行。所构建的语义地图相对于三维点云语义地图具有极小的空间占用。拓扑地图的构建是自适应构建过程，没有人为因素的介入，保证了机器人的自主性，同时提高了机器人在不同环境的适应能力。同时，所生成的拓扑节点具有完备性，即通过访问少数节点，即可获取环境的完整感知。将语义地图与度量地图以及拓扑地图进行独立维护，能够各自进行增、删、改等更新维护操作。在进行目标搜索时，综合考虑物体存在概率以及预期搜索时间，避免了仅将物体存在概率或预期搜索时间作为优化指标的局限性。能够根据所执行任务的时间紧迫性，对预期搜索时间的权重进行动态调整，使机器人能够适应不同的搜索环境。
附图说明
[0063]
为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施例或现
有技术描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动性的前提下，还可以根据这些附图获得其他的附图。
[0064]
图1为本发明模仿人类记忆的移动机器人目标搜索方法的物体坐标转换示意图；
[0065]
图2为本发明模仿人类记忆的移动机器人目标搜索方法的机器人感知范围示意图；
[0066]
图3为本发明模仿人类记忆的移动机器人目标搜索方法的概率随时间衰减示意图；
[0067]
图4为本发明模仿人类记忆的移动机器人目标搜索方法的不同遗忘率以及记忆半衰期的概率随时间衰减示意图；
[0068]
图5为本发明模仿人类记忆的移动机器人目标搜索方法的流程图。
具体实施方式
[0069]
下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。
[0070]
本实例采用p3-dx先锋机器人实验平台，搭载深度相机realsense d435i以及激光雷达rp lidara2，执行语义拓扑地图的构建：
[0071]
通过目标检测算法对视觉传感器的rgb信息进行检测，得到具有单个或多个物体的2d包围框。将像素平面上坐标映射到栅格地图坐标系，需经过以下转换关系，如附图1所示。首先，将像素坐标系下坐标转化到成像平面。
[0072][0073]
像素坐标系与成像平面之间只存在单纯的平移关系，而且度量单位不同。经过公式(1)进行转换，其中，(u0,v0)代表成像平面原点在像素坐标系中的坐标位置，dx与dy代表每个像素在u,v轴方向上的物理尺寸。(u,v,1)代表像素坐标系下的齐次坐标，(x,y,1)代表成像平面坐标系下的对应坐标。
[0074]
从成像平面到相机坐标系之间的映射需要辅助信息zc，zc代表物体的深度，由深度相机获取。经过下式运算，将成像平面坐标系的坐标(x,y,1)映射到相机坐标系下的相应坐标(xc,yc,zc)，其中f为相机焦距，即为相机坐标系原点到成像平面的距离。
[0075]
[0076]
相机坐标系与机器人坐标系之间存在平移和旋转关系，经过下式运算即可求得相机坐标系坐标(xc,yc,zc)在机器人坐标系下的表示(xr,yr,zr)。其中，旋转矩阵r和平移矩阵t属于相机外参的一部分。
[0077][0078]rm
和tm为机器人在地图坐标系下相对旋转和平移矩阵，由自适应蒙特卡洛定位方法估计得出。经过以下运算求得机器人坐标系的坐标(xr,yr,zr)在地图坐标系下的表示(xm,ym,zm)。
[0079][0080]
因此，目标检测算法所获取的2d物体包围框通过下式，转换到栅格地图坐标系。
[0081][0082]
简化为：
[0083][0084]
其中，由标定得到内参为
[0085]
表一相机内参标定取值(单位：pixel)
[0086]
参数f
xfy
u0v0取值612.795612.789325.436236.364
[0087]
步骤二：拓扑地图构建；
[0088]
以机器人为圆心，以传感器探测范围为半径构建机器人感知范围模型，并基于快速随机探索树(rapid random exploration tree,rrt)算法，在机器人工作环境中构建无碰撞的探索树，将树节点之间的连线删除；然后，从起始节点开始，获取机器人3/4倍感知范围内的树节点，并求解形心，计算如下。将式(7)作为损失函数，通过不断迭代，求解合适的形心，使得损失函数j最小。
[0089][0090]
其中，xn代表该当前机器人3/4倍感知范围内的树节点空间坐标，n为3/4倍感知范围内的树节点总数，n∈(1,n)，求解使得j最小的形心u0。
[0091][0092]
最后，以形心u0为圆心，以机器人最大感知范围为半径作圆，将圆内除圆心以外的树节点删减。通过循环删减，直至对树节点删减完毕。
[0093]
步骤三：仿人记忆的概率更新
[0094]
人类学习新知识后，会在短时间内保持对该知识的记忆，此时若不进行重新学习，则会逐渐将知识遗忘。基于此规律，赋予机器人以人类的记忆模型，以s形曲线描述物体在某个位置的概率变化关系。在某个区域识别到物体后，物体在该区域的概率会先保持在一个较高的状态，若没有在该位置重新发现该物体，则概率会逐渐衰减到0。公式如下所示：
[0095][0096]
其中，a代表遗忘率，b代表记忆半衰期，t为识别到物体时的时间戳，表示从伦敦时间1970年1月1日0点开始到现在所经过的毫秒数，n代表重复发现的次数。其中，a越大，则概率衰减的越快，b越大则代表概率保持在50％以上的时间越久。
[0097]
人类在多次学习一个知识之后，对该知识的遗忘速度会变慢。同样的，当机器人多次在一个区域发现同一物体后，对该物体的遗忘率减小，使物体在该区域存在的概率衰减速度变慢。同时，记忆半衰期应同时延长，使概率长时间保持在一个较高的水平。
[0098]
对遗忘率的调整规律如下式，
[0099]an
＝a
0-nδa0ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(10)
[0100]
其中，an为记过n次重新发现后的遗忘率，a0为初始遗忘率，δa0代表遗忘率的变化率。
[0101]
对记忆半衰期的调整规律如下式，
[0102]bn
＝b0 nδb0ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(11)
[0103]
其中，bn为记过n次重新发现后的记忆半衰期，b0为初始记忆半衰期，δb0代表记忆半衰期的变化率。
[0104]
表二遗忘率、记忆半衰期初值及变化率设置
[0105]
参数a0δa0b0δb0取值20.1210
[0106]
步骤四：概率-拓扑-语义信息融合
[0107]
拓扑地图代表了机器人当前工作环境的简化结构，在语义信息与拓扑节点之间建立归属关系。建立规则如下，将所有物体位置信息标记在度量地图中，获取在每个拓扑节点感知范围内物体的坐标，并将其添加进语义信息中，代表物体归属于某个拓扑节点。同时，从语义信息确定开始，仿人记忆机制对物体与拓扑节点归属关系进行概率衰减，若在同一个拓扑节点探测范围内多次发现该物体，则根据公式(10)-(11)对遗忘率以及记忆半衰期进行自适应调整，并根据公式(9)进行概率计算。经过以上步骤，实现对概率-拓扑-语义信息的融合。
[0108]
步骤五：基于概率-拓扑-语义地图的目标搜索
[0109]
对目标的查找过程如下：先给定需要查找的目标以及对应的roi，并对其sift特征
进行提取；然后对于给定的目标，根据步骤四的融合地图，找出其所属的各个拓扑节点。再通过dijkstra算法向目标所对应的拓扑节点规划出一条实际可行的路径，求得机器人按照路径运行的预期时间tm，
[0110][0111]
其中，lm为从机器人当前位置到中目标所在拓扑节点的路径长度，v为机器人匀速运动的速度。并对预期时间进行归一化，求出从机器人当前位置向目标所在区域运动的归一化时间
[0112][0113]
然后，根据公式(10)获取目标物体m在各个拓扑节点的概率pm，并对概率进行归一化，求出目标在各个拓扑节点对应区域存在的归一化概率
[0114][0115]
由于机器人工作在动态环境中，物体位置分布是不确定的。所以，在进行目标搜索时，需要对物体所属拓扑节点概率与机器人当前位置到拓扑节点的预期时间同时进行考虑，用于生成搜索路径。作为机器人搜索的目标区域，不仅具有较大的目标存在概率，而且相对于机器人所在位置具有较短的预期运动时间。也就是当一个拓扑节点中物体存在概率不会小于其他拓扑节点，而且预期运动时间不会大于其余拓扑节点。即对于物体可能所在位置对应的m个拓扑节点，当
[0116][0117]
且
[0118][0119]
机器人会优先在物体存在概率以及预期搜索时间都较优的拓扑节点i进行搜索，而非拓扑节点j。因此，将物体存在概率以及预期搜索时间进行综合考虑，使用加权代价作为拓扑节点访问序列生成的指标。当物体存在概率越大，且预期时间越短，则加权代价越小；当概率越小，而预期时间越长，则加权代价越大。加权代价costm的计算公式为
[0120][0121]
式中，α为时间权重系数，用于调整执行时间紧迫任务的预期时间权重。当执行时间紧迫任务时，α增大；当执行非时间紧迫任务时，减小α，使机器人的目标搜索达到最优。
[0122]
步骤六：目标相似度计算
[0123]
在路径上进行搜索时，对于视觉传感器捕捉的rgb信息进行目标检测，筛选出属于同类物体的roi，再对其关键点特征进行提取，计算其与目标物体特征点的欧氏距离；最后，判断欧氏距离是否小于阈值18000，若小于则认为当前识别物体即为目标物体，若大于阈
值，则继续读取rgb信息，进行判断。
[0124]
以上结合附图对本发明的实施方式作了详细说明，但本发明不限于所描述的实施方式。对于本领域的技术人员而言，在不脱离本发明原理和精神的情况下，对这些实施方式进行多种变化、修改、替换和变型，仍落入本发明的保护范围内。