机器人平台免疫遮挡的定法方法

1.本发明属于机器人平台定位导航的技术领域，具体涉及一种机器人平台免疫遮挡的定法方法。


背景技术：

2.传统环境建模得到的栅格地图、拓扑地图和特征地图因缺少对环境中物体的语义描述，导致移动机器人不能很好的理解环境，也无法完成一些高级复杂的任务，而语义地图能帮助机器人更好的理解环境，能大大提高了机器人人机交互的能力和智能化的水平。定位技术是目前智能移动机器人研究的难点和热点，当前主流的定位算法在静态无遮挡环境下运行效果较好，但在有障碍物遮挡的环境中运行效果较差。当语义物体被遮挡时，语义定位将无法实现或出现多个语义定位位姿，导致机器人在几何相似环境下粒子无法快速收敛，定位效率大大下降。因此，需要开发一种能够在物体被遮挡时，仍能精准定位的机器人平台定位方法。


技术实现要素：

3.本发明的目的在于针对现有技术的不足之处，提供一种机器人平台免疫遮挡的定法方法，该方法在物体被遮挡时，仍能完成机器人精确的重定位，提高了机器人重定位的鲁棒性。
4.为解决上述技术问题，本发明采用如下技术方案：
5.一种机器人平台免疫遮挡的定法方法，所述机器人平台包括机器人平台底盘以及设置在机器人平台底盘上的主控机、激光雷达传感器、深度相机，主控机分别与机器人平台、激光雷达传感器、深度相机连接；
6.定位方法包括如下步骤：
7.步骤1：主控机控制机器人平台在室内运动，激光雷达传感器实时采集室内信息并传输至所述主控机，所述主控机根据激光雷达传感器采集的室内信息通过地图构建方法得到二维环境栅格地图，并将得到的二维环境栅格地图结合深度相机采集的视觉语义信息进行语义映射得到二维栅格语义地图，对二维栅格语义地图进行语义预处理生成语义链表；
8.步骤2：主控机在建图过程中对深度相机获取的满足要求的关键帧进行保存，当主控机检测到机器人平台在室内运动遇到障碍物时，其将当前帧与建图时保存的关键帧进行相似度匹配，选出相似度评分最高的关键帧作为相似帧，并将当前帧包含的语义信息与相似帧语义链进行对比以确定被遮挡物体的类别；
9.步骤3：在在确认被遮挡物体的类别后，主控机利用机器人平台自身获取的语义信息类别与语义链表进行比较来确定机器人平台所处的区域，并且主控机为被遮挡物体的语义物体赋低置信度，为深度相机识别到的语义物体赋高置信度，使用高置信度的语义信息进行语义定位，通过语义定位结合机器人平台所处区域确定机器人平台的估计位姿和方向；
10.步骤4：主控机根据步骤3得到的估计位姿结合amcl激光定位算法进行位姿的迭代更新获得机器人平台最终的真实位姿，并在相似几何环境遮挡和相似语义环境下进行对比验证。
11.进一步地，二维栅格语义地图构建方法为：
12.主控机控制机器人平台在室内行走获取室内地图，通过地图构建方法将室内环境构建得到环境栅格地图，基于ssd目标检测算法和deeplabv2语义分割算法提取语义信息，并结合激光雷达传感器采集的激光信息进行坐标转换和语义映射得到语义地图，再对生成的语义地图进行膨胀腐蚀处理二维语义栅格地图。
13.进一步地，语义信息的映射的计算方法为：
14.l
t,i
＝l
t-1,i-l0+l
inv,i
；
15.式中，l
t,i
表示t时刻第i个语义栅格的状态，l
t-1,i
是t-1时刻第i个语义栅格的状态，l0表示先验概率，l
inv,i
为相机的逆观测模型。
16.进一步地，生成语义链表的方法为：
17.绘制出语义地图中语义信息的最小包围轮廓，并求出各个语义信息的投影中心坐标，取每个区域关键物体作为基层语义信息，提取关键物体周围的语义信息形成语义链，提取范围为r，其计算公式为：
[0018][0019]
其中，θs为深度相机的水平视场角，ds为语义信息提取的最大视距。
[0020]
进一步地，步骤2中关键帧满足条件为：
[0021]
(1)当前帧和上一关键帧之间必须有预设的序列间隔；
[0022]
(2)当前帧与之前关键帧相互的共视区域低于预设的角度范围；
[0023]
(3)关键帧包含多于一种的语义信息和特征。
[0024]
进一步地，步骤2中相似度匹配的方法为：
[0025]
构建包含多种场景信息的字典，从包含障碍物的关键帧中使用orb提取特征，在字典树中逐层查找，找到特征对应的单词w_x，并以词袋向量的形式描述；提取当前帧中的特征，确定特征对应bow中的单词并确定当前帧中单词的使用情况，将当前帧转换为词袋向量描述；
[0026]
通过比较关键帧和当前帧的词袋向量来确定两者之间的相似程度，相似度计算如式：
[0027][0028]
其中，ηn表示wn的权重，wn表示单词，vi表示关键帧的词袋向量，vj表示当前帧的词袋向量，s(v
i-vj)表示当前帧和关键帧之间的相似度，v
ix
表示第x幅关键帧图片的向量特征，v
jx
表示第x幅当前帧图片的向量特征。
[0029]
进一步地，单词权重的计算方法为：
[0030]
采用tf-idf的加权方式计算单词的权重；其中，在建立字典时计算idf，具体为：统计某个子叶节点wi中的特征数量相对于所有特征数量的比例作为idf部分，如下式所示：
[0031][0032]
其中，n为所有特征的数量，ni为wi的特征数量；
[0033]
tf部分是指某个特征在某帧图像中出现的频率，计算公式如下：
[0034][0035]
其中，mi为图像fi中单词w2出现的次数，m为一共出现的次数，单词wi的权重计算如下式：
[0036]
ηi＝tfi×
idfi[0037]
对于某一时刻的关键帧fi，它的特征点可对应到许多单词wi，构成的bow如下：
[0038][0039]
进一步地，确定机器人所处的区域的方法为：
[0040]
首先，计算当前t时刻环境中的其它语义物体与被遮挡之间的欧式距离计算语义链表中相似区域的语义物体对应的欧式距离d
0i
，求其与当前t时刻的欧氏距离差值的绝对值δdj，比较δdj大小来确定机器人所在的区域，选择δdj最小的区域为机器人所在的区域，具体计算公式为：
[0041][0042][0043]
式中，表示当前t时刻识别的被遮挡物体坐标，表示当前t时刻识别到的其它语义物体的中心坐标。
[0044]
进一步地，确定机器人平台估计位姿的方法为：
[0045]
首先，使用高置信度的语义信息进行语义定位，其计算公式为：
[0046][0047][0048]
其中，r1表示深度相机获取的其中一高置信度语义物体到机器人平台的距离，r2表示深度相机获取的另一高置信度语义物体到机器人平台的距离，d表示高置信度物体之间的距离，h表示机器人平台的可能位姿到高置信物体中心连线的距离，a表示其中一高置信语义物体到其垂足之间的距离；当表示目前使用的语义信息无法完成机器人平台的定位，需要移动机器人平台继续移动，让机器人平台识别到更多的语义信息来进行语义定位；当表示两语义物体画圆的交点只有一个，该点可能为机器人平台真实位置；当表示机器人可能位置有两个点；
[0049]
其次，求解机器人精准的语义定位位置，其计算公式为：
[0050]
p
pose
＝c1+an
||
±
hn
⊥
[0051][0052]
其中，定义一个两个高置信语义物体中线连线的单位方向向量n
||
，n
⊥
为n
||
的单位法向量，c1表示其中一高置信语义物体的中心距离，c2分别表示另一高置信语义物体的中心距离；
[0053]
求出机器人平台的可能位置后，通过确定该点是否在二维栅格语义地图中的栅格空闲区域来判断该点是否为机器人的实际位姿(x
t
,y
t
)；若该点在空闲位置，则该点为机器人平台的实际位姿；反之则排除。
[0054]
进一步地，机器人平台的方向获取方法为：
[0055]
机器人的方向通过机器人平台的偏航角θ
t
确定，其需要通过机器人平台与语义物体的夹角θ1以及语义物体与地图坐标系x轴的夹角θ2来求解，具体计算公式为：
[0056]
θ
t
＝θ
2-θ1[0057]
θ1＝θ-90
[0058]
其中，θ为映射到机器人坐标系下语义物体中心与正x轴的夹角。
[0059]
进一步地，步骤4中采用amcl激光定位算法进行位姿的迭代更新，具体方法为：
[0060]
(1)初始化粒子集χ0，粒子数量为n，粒子权重为1/n；
[0061]
(2)根据机器人平台当前t时刻得到的运动数据计算出t-1时刻到t时刻的机器人平台运动量进行位姿估计得到粒子的状态分布χ
t
；
[0062]
(3)先根据状态分布χ
t
结合步骤3机器人位姿匹配的置信度、激光雷达采集数据、二维栅格语义地图的匹配置信度来更新粒子权重，得到t时刻粒子权重w
t
，然后结合步骤3中获得的机器人平台估计位姿对粒子权重更新，再遍历整个粒子集，加权粒子集所有粒子，得到权重更新后粒子集
[0063]
(4)对权重更新后粒子集进行重采样并进行权重的归一化处理，反复迭代粒子聚集收敛，获得机器人的实际位姿。
[0064]
与现有技术相比，本发明的有益效果为：本发明针对语义定位过程中存在障碍物遮挡语义物体，语义定位失败导致无法正确获得机器人位姿的问题，提出了一种机器人平台免疫遮挡的定位方法，在重定位过程中，首先通过kinectv2深度相机获取观测数据，利用ssd目标检测进行语义识别，当检测到障碍物时就与建图时保存的关键帧进行相似度匹配，确定相似关键帧，其次把当前帧包含的语义信息与关键帧语义链进行加权做差来确定被遮挡物体的类别，将机器人所识别到的语义信息类别与语义地图的语义链进行比较来确定机器人所处的区域，通过未被遮挡的高置信度语义物体的深度信息来进行语义定位，最后同步结合amcl算法与环境地图匹配的方法实现更精确、更快速的定位效果，并提高了机器人重定位的鲁棒性。
附图说明
[0065]
图1为本发明实施例机器人平台免疫遮挡的定法方法的流程图；
[0066]
图2为本发明实施例构建的地图，其中，(a)实验环境，(b)二维栅格地图；
[0067]
图3为本发明实施例的ssd目标检测效果图；
[0068]
图4为本发明实施例语义分割结果图，其中，(a)深度相机采集的视觉图，(b)为(a)的语义分割效果图；
[0069]
图5为本发明实施例语义映射图，其中，(a)墙角映射图，(b)其它语义映射图，(c)语义映射颜色对应图；
[0070]
图6为本发明实施例构建的二维栅格语义地图；
[0071]
图7为本发明实施例语义信息搜索图，其中，(a)语义搜索区域，(b)环境栅格地图，(c)搜索区域划分及语义标签；
[0072]
图8为本发明实施例不同环境下的关键帧图；
[0073]
图9为本发明实施例位姿求解图，其中，(a)机器人位姿；(b)机器人视角图；
[0074]
图10为本发明实施例航向角求解图，其中(a)二维栅格语义地图中航向角；(b)机器人坐标系下映射图；
[0075]
图11为本发明实施例遮挡垃圾桶的语义定位图，其中，(a)识别到障碍物图，(b)关键帧图，(c)确定被障碍物遮挡的语义物体类别，(d)语义定位，(e)语义定位结果；
[0076]
图12为本发明实施例遮挡椅子的语义定位图，其中，(a)识别到障碍物图，(b)关键帧图，(c)确定被障碍物遮挡的语义物体类别，(d)语义定位，(e)语义定位结果；
[0077]
图13为本发明实施例遮挡墙角的语义定位图，其中，(a)识别到障碍物图，(b)关键帧图，(c)确定被障碍物遮挡的语义物体类别，(d)语义定位，(e)语义定位结果；
[0078]
图14为本发明实施例相似语义真实环境；
[0079]
图15为本发明实施例相似环境下地图，其中，(a)二维环境栅格地图，(b)语义映射地图(c)二维栅格语义地图；
[0080]
图16为本发明实施例相似环境下语义定位图，其中，(a)机器人所处位置，(b)目标检测图，(c)相似语义，(d)机器人平台的位姿求解结果，(e)初步语义定位；
[0081]
图17为本发明实施例相似环境下语义定位结果图，其中，(a)语义物体与墙角之间的距离，(b)语义估计位姿，(c)amcl定位的真实位姿。
具体实施方式
[0082]
下面将结合本发明实施例对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有作出创造性劳动的前提下所获得的所有其他实施例，都属于本发明保护的范围。
[0083]
需要说明的是，在不冲突的情况下，本发明中的实施例及实施例中的特征可以相互组合。
[0084]
下面结合具体实施例对本发明作进一步说明，但不作为本发明的限定。
[0085]
本发明实施例公开了一种机器人平台免疫遮挡的定法方法，其中，机器人平台包括机器人平台底盘以及设置在机器人平台底盘上的主控机、激光雷达传感器、深度相机，主控机分别与机器人平台、激光雷达传感器、深度相机通过电线连接；在本实施例中，主控机选用的是酷睿m4i7-d迷你主机；机器人平台底盘选用的是pibot品牌的arduino驱动板底盘；激光雷达传感器选用性能稳定的sick lms111激光雷达；深度相机选用的是kinect v2。
[0086]
如图1所示，定位方法包括如下步骤：
[0087]
步骤1：主控机控制机器人平台在室内运动，激光雷达传感器实时采集室内信息并传输至所述主控机，所述主控机根据激光雷达传感器采集的室内信息通过地图构建方法得到二维环境栅格地图，并将得到的二维环境栅格地图结合深度相机采集的视觉语义信息进行语义映射得到二维栅格语义地图，对二维栅格语义地图进行语义预处理生成语义链表；
[0088]
主控机控制机器人平台在室内运动行走，在行走时，激光雷达传感器实时采集室内物体与机器人平台的距离、室内物体与机器人平台的方向角，并传输至主控机，主控机将室内物体与机器人平台的距离、室内物体与机器人平台的方向角通过地图构建方法得到二维环境栅格地图(如图2)，与此同时，深度相机实时采集室内环境的视觉信息并传递给主控机，主控机将采集的视觉信息基于ssd(single shot multibox detector)目标检测算法和deeplabv2语义分割算法提取语义信息，在本实施例中，即对室内物体包括椅子、柜子、垃圾桶和门等进行分割得到语义分割效果图，ssd目标检测效果图如图3，deeplabv2图像分割效果图如图4；之后进行多传感器融合坐标转换和语义映射得到墙角语义映射图和其他语义信息映射图(图5)，最后将之前构建的二维环境栅格地图与语义映射图进行融合生成语义地图，并对生成的语义地图进行膨胀腐蚀处理二维栅格语义地图(如图6)。其中，语义地图中语义信息的映射计算公式为：
[0089]
l
t,i
＝l
t-1,i-l0+l
inv,i
；
[0090]
式中，l
t,i
表示t时刻第i个语义栅格的状态，l
t-1,i
是t-1时刻第i个语义栅格的状态，l0表示先验概率，设移动机器人平台启动时刻语义栅格的占用概率和空闲概率都为0.5，即l0＝0，l
inv,i
为相机的逆观测模型。
[0091]
对二维栅格语义地图进行语义预处理生成语义链表，该语义链表用于后续免疫遮挡定位研究，具体方法为：利用opencv库中函数绘制出语义地图中语义信息的最小包围轮廓(如图7)，并求出各个语义信息的投影中心坐标。由于每个区域一定包含墙角语义信息，取每个区域关键墙角作为基层语义信息，提取墙角周围的语义信息形成语义链表，如表1所示，提取范围为r(m)，计算公式为：
[0092][0093]
其中，θs为相机的水平视场角，ds为语义信息提取的最大视距，本实施例中使用kinectv2相机的最佳观测距离；
[0094]
表1为语义链表
[0095][0096][0097]
步骤2：主控机在建图过程中对深度相机获取的满足要求的关键帧进行保存，当主
控机检测到机器人平台在室内运动遇到障碍物时，其将当前帧与建图时保存的关键帧进行相似度匹配，选出相似度评分最高的关键帧作为相似帧，并将当前帧包含的语义信息与相似帧语义链进行对比以确定被遮挡物体的类别；
[0098]
主控机控制机器人平台在室内实时运动，kinectv2深度相机实时获取观测数据，主控机对深度相机获取的满足关键帧选择规则的帧作为关键帧(如图8)储存下来，在本实施例中，关键帧的选择规则为：
[0099]
(1)当前帧和上一关键帧之间必须有一定的序列间隔，在本实施例中，序列时间间隔大于30s；
[0100]
(2)当前帧与之前关键帧相互的共视区域低于一定范围，具体的范围根据实际的实验环境而定；
[0101]
(3)关键帧有足够的语义信息和特征，即具备一种以上的语义信息和特征，比如在一帧图片中，可以同时拥有椅子、柜子、垃圾桶和墙角，而不是单单只有椅子和柜子或者只有椅子，以保证特征的丰富性。
[0102]
当检测到障碍物时，主控机将深度相机获取的当前帧与建图时保存的关键帧进行相似度匹配，选出相似度评分最高的关键帧作为相似帧，然后将当前帧与相似帧的语义链表进行对比来确定被遮挡物体的类别；
[0103]
本实施例选择基于词袋模型的方法进行相似度匹配，具体为：
[0104]
创建了一个包含多种场景信息的字典，规模为k＝10，d＝5。从包含障碍物的关键帧中使用oriented fast and rotated brie算法(orb)提取特征，在字典树中逐层查找，找到特征对应的单词w
x
，以向量的形式描述，例如一帧图像有“人”、“车”、“猫”等单词，可记为下式：
[0105]fi
＝1
·
w1+1
·
w2+0
·
w3[0106]
其中，fi表示第i时刻的一帧图像，w1、w2、w3分别对应单词“人”，“车”，“猫”。
[0107]
当某个单词在一副图像中出现的频率较高或者频率较低，它所占的权重也会相应不同，本实施例运用了tf-idf的加权方式(tf部分的思想是某单词在关键帧中经常出现，它的区分度就高；idf的思想是某单词在字典中出现的频率越低，分类图像时区分度越高)。在建立字典时计算idf：统计某个子叶节点wi中的特征数量相对于所有特征数量的比例作为idf部分，如下式所示：
[0108][0109]
其中，n为所有特征的数量，ni为wi的特征数量；
[0110]
tf部分是指某个特征在某帧图像中出现的频率，计算公式如下：
[0111][0112]
其中，mi为图像fi中单词w2出现的次数，m为一共出现的次数，单词wi的权重计算如下式：
[0113]
ηi＝tfi×
idfi[0114]
对于某一时刻的关键帧fi，它的特征点可对应到许多单词wi，构成的词袋模型bow如下：
[0115][0116]
通过词袋向量vi表示关键帧fi；再采用orb对当前帧进行特征提取，并确定特征对应bow中的单词，再确定当前帧中单词的使用情况，以将当前帧转换为一个词袋向量描述；比较关键帧和当前帧的词袋向量来确定两者之间的相似程度，相似度计算如式：
[0117][0118]
其中，ηn表示wn的权重，vi表示关键帧的词袋向量，vj表示当前帧的词袋向量，s(v
i-vj)表示当前帧和关键帧之间的相似度，v
ix
表示第x幅关键帧图片的向量特征，v
jx
表示第x幅当前帧图片的向量特征。
[0119]
通过上述方法选出相似度评分最高的关键帧作为相似帧，在确定相似帧后，将相似帧和当前帧包含的语义链信息转换为向量的形式，给每个物体赋不同的权重(在本实施例中，corner＝1、door＝10、trash＝100、chair＝1000、cabinet＝10000)，进行语义链的对比，确定被遮挡的语义物体类别，相似帧和当前帧计算公式为：
[0120]
a＝1
·
door+0
·
chair+1
·
corner+0
·
trash+1
·
cabinet
[0121]
b＝1
·
door+0
·
chair+0
·
corner+0
·
trash+1
·
cabinet
[0122]
t＝a-b
[0123]
式中，a代表相似帧，b代表当前帧；按照上式t的值来确定被遮挡的物体类别，当t＝0时，表示没有语义物体被遮挡；当t＝1时，被遮挡的物体类别为墙角；当t＝10时，被遮挡的物体类别为门；当t＝100时，被遮挡的物体类别为垃圾桶；当t＝1000时，被遮挡的物体类别为椅子；当t＝10000时，被遮挡的物体类别为柜子。
[0124]
步骤3：在确认被遮挡物体的类别后，主控机利用机器人平台自身获取的语义信息类别与语义链表进行比较来确定机器人平台所处的区域，并且主控机为被遮挡物体的语义物体赋低置信度，为深度相机识别到的语义物体赋高置信度，使用高置信度的语义信息进行语义定位，通过语义定位结合机器人平台所处区域确定机器人平台的估计位姿和方向；
[0125]
在本实施例中，确定机器人平台所处区域的方法为：
[0126]
首先，计算当前t时刻环境中的其它语义物体与墙角之间的欧式距离计算语义链表中可能相似区域的语义物体对应的欧式距离d
0i
，求其与当前t时刻的欧氏距离差值的绝对值δdj，比较δdj大小来确定机器人所在的区域，并选择δdj最小的区域为机器人所在的区域，计算公式为：
[0127][0128][0129]
式中，表示当前t时刻识别的墙角坐标，表示当前t时刻识别到的其它语义物体的中心坐标。
[0130]
在步骤2确认被遮挡语义物体的类别后，给该被遮挡语义物体的语义信息赋低置
信度，并给通过深度相机采集的视觉信息直接识别到的语义物体的语义信息赋高置信度，使用高置信度的语义信息进行语义定位。如图9中，当墙角被遮挡时，使用门和柜子进行语义定位，求解两圆的交点即为机器人的位置，其中c1为查语义链表获取的门的中心坐标，r1是通过深度相机获取的机器人到门的相对距离值，c2为柜子的中心坐标，r2是深度相机获取的机器人到柜子的相对距离值，其半径的几何关系式如式计算公式为：
[0131][0132][0133]
并转换为a和h的表达式，来判断对应的语义信息以求解出机器人的位姿，转换公式如下：
[0134][0135][0136]
其中，r1和r2表示高置信度语义物体到机器人的距离，d表示高置信度物体之间的距离，h表示机器人的可能位姿到高置信物体中心连线的距离，a表示其中一高置信语义物体到其垂足之间的距离。当表示目前使用的语义信息无法完成机器人的定位，需要移动机器人继续移动，让机器人识别到更多的语义信息来进行语义定位；当表示两语义物体画圆的交点只有一个，该点可能为机器人真实位置；当表示机器人可能位置有两个点。
[0137]
求解机器人精准的语义定位位置的计算公式为：
[0138]
p
pose
＝c1+an
||
±
hn
⊥
[0139][0140]
其中，定义一个两个高置信语义物体中线连线的单位方向向量n
||
，n
⊥
为n
||
的单位法向量。
[0141]
求出机器人的可能位置pose1和pose2后(如图9)，通过确定该点是否在二维栅格语义地图中的栅格空闲区域来判断该点是否为机器人的估计位姿(x
t
,y
t
)。若该点在空闲位置，则该点为机器人的估计位姿；反之则排除。
[0142]
在确定机器人平台的估计位姿后，需要确定机器人平台的方向，即确定机器人平台的偏航角θ
t
，其需要通过机器人平台与语义物体的夹角θ1以及语义物体与地图坐标系x轴的夹角θ2来求解(如图10)，其计算公式为：
[0143]
θ
t
＝θ
2-θ1[0144]
θ1＝θ-90
[0145]
其中，θ为映射到机器人坐标系下语义物体中心与正x轴的夹角。
[0146]
步骤4：主控机根据步骤3得到的估计位姿和方向结合amcl激光定位算法进行位姿的迭代更新，并在相似几何环境遮挡和相似语义环境下进行对比实验验证，获得机器人平
台最终的真实位姿，并在相似几何环境遮挡和相似语义环境下进行对比验证。
[0147]
在本实施例中，主控机采用amcl激光定位算法对机器人平台进行位姿和方向的迭代更新的方法为：
[0148]
(1)初始化粒子集：由于机器人平台无任何先验信息，在二维栅格语义地图中机器人平台的位姿完全未知，定义一个初始时刻大小为n的粒子集在二维栅格语义地图中随机撒n个粒子，且每个粒子权重相等均为其中表示初始时刻粒子集χ0的第i个粒子代表的状态估计；
[0149]
(2)粒子位姿预测：根据机器人平台当前t时刻得到的运动数据计算出t-1时刻到t时刻的机器人平台运动量，并将其从机器人平台坐标系转换到世界坐标系下，利用t-1时刻粒子状态估计和运动量u
t
对当前t时刻粒子进行预测得到其状态估计即p(x
t
|x
t-1
,u
t
)，具体表示为：
[0150][0151]
式中，p(x
t
|x
t-1
,u
t
)表示当前t时刻粒子的状态估计，bel(x
t-1
)表示t-1时刻粒子先验，表示t时刻粒子先验；
[0152]
(3)粒子权重更新：利用输入激光雷达的观测数据z
t
对粒子的权重进行修正，其过程为：将t时刻激光雷达观测数据z
t
分别根据当前各个粒子的位姿转换到地图坐标系中，将当前观测数据与二维栅格语义地图进行匹配，返回每个粒子匹配的得分，根据得分更新粒子权重，遍历完整个粒子集后，加权粒子集所有粒子，得到权重更新后粒子集
[0153]
(4)粒子重采样：粒子集中粒子的权重都已得到了更新，为了使后验概率分布更加接近移动机器人平台的真实位姿，使初始随机分布的粒子收敛，采用轮盘赌法，先将粒子集中所有粒子按照权重比例占用0～1区间，然后产生随机数，计算随机数对应的区间，然后复制随机数区间对应的粒子，最后对粒子进行权重的归一化处理。在重采样的过程中，权重大的粒子被多次复制，对后验概率密度贡献小的粒子则被逐渐淘汰，进行多次重采样，粒子逐渐聚集收敛，完成机器人的定位过程。
[0154]
在获得机器人平台的真实位姿后，在相似几何环境遮挡下完成对比实验，结合步骤中构建的二维栅格语义地图和语义链在相似几何环境下对本实施例得到的真实位姿进行实验验证，当存在动态障碍物遮挡语义物体时运用本实施例方法，首先通过目标检测识别到障碍物如图11(a)，其次通过与之前保存的关键帧进行基于词袋模型的相似度匹配，当前帧与关键帧3相似度匹配得分最高，初步判断相似帧为关键帧3，如图11(b)所示，通过对当前帧和关键帧包含的语义链信息进行匹配检验来判断相似帧是否匹配正确，当前帧包含了柜子和墙角，相似帧也包含了柜子和墙角，故确定相似匹配正确；再次将关键帧和当前帧的语义链进行赋值加权，依据式步骤3中公式计算t＝100，因此确定被障碍物遮挡的语义物体为垃圾桶，如图11(c)；然后通过查找语义链表图8来查找包含墙角、柜子和垃圾桶语义的区域，确定机器人处在3区域；最后在进行精确的语义定位位姿求解，由于垃圾桶被遮挡了，其深度信息并不准确，把垃圾桶的定位置信度设为0，通过墙角和柜子进行语义定位如图11(d)，语义定位结果如图11(e)；当遮挡椅子时应用本实施例方法得到最终语义定位结果(图
12)；当遮挡墙角时应用本本实施例方法得到最终语义定位结果(图13)；
[0155]
将得到的真实位姿在相似语义环境下进行对比。为了验证本本实施例方法构建的语义链表能够提高移动机器人平台在相似语义环境下定位的鲁棒性，在如图14所示环境中进行语义定位实验，其中有两块区域同时包含了椅子和柜子语义，构成了相似语义场景，运用步骤1得到相似环境下环境栅格地图、语义映射图和二维栅格语义地图(图15)，根据该实验环境运用步骤2得到该环境下语义链表，见表2。
[0156]
表2为实验环境下的语义链表
[0157][0158]
主控机控制机器人平台在上述环境中移动进行语义重定位，当机器人平台处在图16(a)所示的位置时，基于目标检测，机器人识别到了墙角、柜子和椅子等语义信息，如图16(b)所示；但机器人平台在2
○
墙角附近也能识别到相同的语义信息，如图16(c)所示；基于语义物体到机器人平台的相对距离进行机器人平台的位姿求解，如图16(d)所示，机器人平台的位姿在两个区域均有可能存在；初步语义定位如图16(e)所示，两个语义定位位姿均有可能为机器人平台的真实位姿。结合生成语义链表(表2)计算可能位姿附近的墙角与其包含的语义物体之间的距离，如图17(a)所示的柜子与墙角之间的距离d
01
和椅子与墙角之间的距离d
04
，计算当前t时刻机器人平台通过目标检测识别到的语义物体与墙角之间的距离和进而确定机器人平台处在7
○
号墙角附近，语义定位位姿如17(b)所示，将语义定位位姿结合到amcl中，更新当前粒子集权重，增大当前语义定位位姿附近粒子的权重，迭代更新，最后粒子收敛，如图17(c)所示。
[0159]
以上仅为本发明较佳的实施例，并非因此限制本发明的实施方式及保护范围，对于本领域技术人员而言，应当能够意识到凡运用本发明说明书内容所作出的等同替换和显而易见的变化所得到的方案，均应当包含在本发明的保护范围内。

技术特征：
1.一种机器人平台免疫遮挡的定法方法，其特征在于，所述机器人平台包括机器人平台底盘以及设置在机器人平台底盘上的主控机、激光雷达传感器、深度相机，主控机分别与机器人平台、激光雷达传感器、深度相机连接；定位方法包括如下步骤：步骤1：主控机控制机器人平台在室内运动，激光雷达传感器实时采集室内信息并传输至所述主控机，所述主控机根据激光雷达传感器采集的室内信息通过地图构建方法得到二维环境栅格地图，并将得到的二维环境栅格地图结合深度相机采集的视觉语义信息进行语义映射得到二维栅格语义地图，对二维栅格语义地图进行语义预处理生成语义链表；步骤2：主控机在建图过程中对深度相机获取的满足要求的关键帧进行保存，当主控机检测到机器人平台在室内运动遇到障碍物时，其将当前帧与建图时保存的关键帧进行相似度匹配，选出相似度评分最高的关键帧作为相似帧，并将当前帧包含的语义信息与相似帧语义链进行对比以确定被遮挡物体的类别；步骤3：在确认被遮挡物体的类别后，主控机利用机器人平台自身获取的语义信息类别与语义链表进行比较来确定机器人平台所处的区域，并且主控机为被遮挡物体的语义物体赋低置信度，为深度相机识别到的语义物体赋高置信度，使用高置信度的语义信息进行语义定位，通过语义定位结合机器人平台所处区域确定机器人平台的估计位姿和方向；步骤4：主控机根据步骤3得到的估计位姿结合amcl激光定位算法进行位姿的迭代更新获得机器人平台最终的真实位姿，并在相似几何环境遮挡和相似语义环境下进行对比验证。2.根据权利要求1所述的机器人平台免疫遮挡的定法方法，其特征在于，二维栅格语义地图构建方法为：主控机控制机器人平台在室内行走获取室内地图，通过地图构建方法将室内环境构建得到环境栅格地图，基于ssd目标检测算法和deeplabv2语义分割算法提取语义信息，并结合激光雷达传感器采集的激光信息进行坐标转换和语义映射得到语义地图，再对生成的语义地图进行膨胀腐蚀处理二维语义栅格地图。3.根据权利要求2所述的机器人平台免疫遮挡的定法方法，其特征在于，语义信息的映射的计算方法为：l
t,i
＝l
t-1,i-l0+l
inv,i
；式中，l
t,i
表示t时刻第i个语义栅格的状态，l
t-1,i
是t-1时刻第i个语义栅格的状态，l0表示先验概率，l
inv,i
为相机的逆观测模型。4.根据权利要求2所述的机器人平台免疫遮挡的定法方法，其特征在于，生成语义链表的方法为：绘制出语义地图中语义信息的最小包围轮廓，并求出各个语义信息的投影中心坐标，取每个区域关键物体作为基层语义信息，提取关键物体周围的语义信息形成语义链，提取范围为r，其计算公式为：其中，θ
s
为深度相机的水平视场角，d
s
为语义信息提取的最大视距。5.根据权利要求1所述的机器人平台免疫遮挡的定法方法，其特征在于，步骤2中关键
帧满足条件为：(1)当前帧和上一关键帧之间必须有预设的序列间隔；(2)当前帧与之前关键帧相互的共视区域低于预设的角度范围；(3)关键帧包含多于一种的语义信息和特征。6.根据权利要求1所述的机器人平台免疫遮挡的定法方法，其特征在于，步骤2中相似度匹配的方法为：构建包含多种场景信息的字典，从包含障碍物的关键帧中使用orb提取特征，在字典树中逐层查找，找到特征对应的单词w
x
，并以词袋向量的形式描述；提取当前帧中的特征，确定特征对应bow中的单词并确定当前帧中单词的使用情况，将当前帧转换为词袋向量描述；通过比较关键帧和当前帧的词袋向量来确定两者之间的相似程度，相似度计算如式：其中，η
n
表示w
n
的权重，w
n
表示单词，v
i
表示关键帧的词袋向量，v
j
表示当前帧的词袋向量，s(v
i-v
j
)表示当前帧和关键帧之间的相似度，v
ix
表示第x幅关键帧图片的向量特征，v
jx
表示第x幅当前帧图片的向量特征。7.根据权利要求1所述的机器人平台免疫遮挡的定法方法，其特征在于，确定机器人所处的区域的方法为：首先，计算当前t时刻环境中的其它语义物体与被遮挡之间的欧式距离计算语义链表中相似区域的语义物体对应的欧式距离d
0i
，求其与当前t时刻的欧氏距离差值的绝对值δd
j
，比较δd
j
大小来确定机器人所在的区域，选择δd
j
最小的区域为机器人所在的区域，具体计算公式为：域，具体计算公式为：式中，表示当前t时刻识别的被遮挡物体坐标，表示当前t时刻识别到的其它语义物体的中心坐标。8.根据权利要求1所述的机器人平台免疫遮挡的定法方法，其特征在于，确定机器人平台估计位姿的方法为：首先，使用高置信度的语义信息进行语义定位，其计算公式为：首先，使用高置信度的语义信息进行语义定位，其计算公式为：其中，r1表示深度相机获取的其中一高置信度语义物体到机器人平台的距离，r2表示深度相机获取的另一高置信度语义物体到机器人平台的距离，d表示高置信度物体之间的距离，h表示机器人平台的可能位姿到高置信物体中心连线的距离，a表示其中一高置信语义
物体到其垂足之间的距离；当表示目前使用的语义信息无法完成机器人平台的定位，需要移动机器人平台继续移动，让机器人平台识别到更多的语义信息来进行语义定位；当表示两语义物体画圆的交点只有一个，该点可能为机器人平台真实位置；当表示机器人可能位置有两个点；其次，求解机器人精准的语义定位位置，其计算公式为：p
pose
＝c1+an
||
±
hn
⊥
其中，定义一个两个高置信语义物体中线连线的单位方向向量n
||
，n
⊥
为n
||
的单位法向量，c1表示其中一高置信语义物体的中心距离，c2分别表示另一高置信语义物体的中心距离；求出机器人平台的可能位置后，通过确定该点是否在二维栅格语义地图中的栅格空闲区域来判断该点是否为机器人的实际位姿(x
t
,y
t
)；若该点在空闲位置，则该点为机器人平台的实际位姿；反之则排除。9.根据权利要求1所述的机器人平台免疫遮挡的定法方法，其特征在于，机器人平台的方向获取方法为：机器人的方向通过机器人平台的偏航角θ
t
确定，其需要通过机器人平台与语义物体的夹角θ1以及语义物体与地图坐标系x轴的夹角θ2来求解，具体计算公式为：θ
t
＝θ
2-θ1θ1＝θ-90其中，θ为映射到机器人坐标系下语义物体中心与正x轴的夹角。10.根据权利要求1所述的机器人平台免疫遮挡的定法方法，其特征在于，步骤4中采用amcl激光定位算法进行位姿的迭代更新，具体方法为：(1)初始化粒子集χ0，粒子数量为n，粒子权重为1/n；(2)根据机器人平台当前t时刻得到的运动数据计算出t-1时刻到t时刻的机器人平台运动量进行位姿估计得到粒子的状态分布χ
t
；(3)先根据状态分布χ
t
结合步骤3机器人位姿匹配的置信度、激光雷达采集数据、二维栅格语义地图的匹配置信度来更新粒子权重，得到t时刻粒子权重w
t
，然后结合步骤3中获得的机器人平台估计位姿对粒子权重更新，再遍历整个粒子集，加权粒子集所有粒子，得到权重更新后粒子集(4)对权重更新后粒子集进行重采样并进行权重的归一化处理，反复迭代粒子聚集收敛，获得机器人的实际位姿。

技术总结
本发明公开了一种机器人平台免疫遮挡的定法方法，包括：主控机控制机器人平台在室内运动，并结合激光雷达传感器和深度相机采集的信息得到二维语义地图，对二维语义地图进行语义预处理生成语义链表；通过深度相机获取观测数据，并进行语义识别，当检测到障碍物时就与建图时保存的关键帧进行相似度匹配，确定相似关键帧；把当前帧包含的语义信息与关键帧语义链进行比较确定被遮挡物体的类别，将主控机识别的语义信息类别与语义链表进行比较来确定机器人平台所处的区域，通过未被遮挡的高置信度语义物体的深度信息来进行语义定位，最后同步结合AMCL算法与环境地图匹配得到机器人平台的真实位姿。本发明定位准确，提高了机器人重定位的鲁棒性。重定位的鲁棒性。重定位的鲁棒性。


技术研发人员：蒋林 罗焱 左建朋 唐振宇
受保护的技术使用者：武汉科技大学
技术研发日：2023.05.16
技术公布日：2023/9/7