基于路标信息与多传感器数据融合的机器人重定位方法与流程
本发明涉及人工智能领域，更具体的，涉及一种基于路标信息与多传感器数据融合的机器人重定位方法。
背景技术：
传统的定位算法，如hantenr[1]等人提出的基于粒子滤波，使用预测值和观测值进行扫描匹配从而更新定位算法，虽然在一定程度上能够完成机器人较为精准的定位，并且在里程计信息较为准确，周围环境物体特征比较明显时，可以自适应完成小幅度的位姿修正，但是对里程计信息依赖程度太高，对周围环境变化的反应灵敏度也不够。碰上累计误差过大，位置漂移，人为移动这些意外情况时的容错性并不高，从而导致在各类情况发生的时候出现定位大幅度失真。
目前，已经有很多团队提出利用路标作为参照物，反向修正机器人的位姿的算法，如frintrops[2]等人提出的基于vslam的位姿图优化算法，利用了视觉自动检测出路标信息，在闭环情况下反复检测和连续追踪，直接解算路标的位姿数据，结合图优化的方式修正位姿，这样虽能在一定程度上完成机器人位置的修正，但是由于用视觉获取深度和角度信息受光线、角度等多方面的因素影响，很容易产生偶然误差，修正效果往往不如人意，必须要在十分特定的简单环境下才能达到较好的效果。
schusterf[3]等人为了能适应更复杂多变环境下自主行驶汽车，所提出的建立标志性路标并且利用激光数据进行图优化的更高效精确的定位方法，但是该文提出的方法只是利用了激光数据，而激光数据的特征并不是那么明显，在进行识别和匹配时都有较大误差，无法做到精确识别和准确修正。
技术实现要素：
本发明克服了现有机器人在已知环境中定位技术的缺点与不足，主要是针对slam同步即时建图与定位中出现的定位误差问题，提出一种融合激光感知数据和视觉识别信息，并利用路标信息来重新修正定位以消除定位误差的方法。实现根据视觉识别，赋予给各路标物体相应的语义标签，再结合对应的激光簇数据，进行信息融合，得到激光语义标签，制定一套路标物体名称和与其对应坐标信息的关联映射表。接着在导航过程中，如果机器人出现位置偏移，利用视觉识别出相应的路标，根据识别出的路标名称找到其位置坐标，再结合实时的激光数据和imu转角信息推算出机器人当前实际位姿，最后修正机器人的定位偏差。以提高机器人产生位姿误差时的重定位水平。
为实现以上发明目的，采用的技术方案是：
基于路标信息与多传感器数据融合的机器人重定位方法，包括以下步骤：
步骤s1：构建地图，并在构建已知地图的过程中同时设置并确定几个特征明显的物体作为系统中的路标，在路标物体的选择和确定路标物体在当前环境的位置要符合本方法的基本原则；
步骤s2：利用聚类算法将落在同一路标物体上的激光点聚成一个激光点簇，形成激光数据；
步骤s3：通过深度学习和图像匹配的方式对视觉识别的物体产生视觉语义信息；
步骤s4：利用标定参数和几何模型，将激光数据和视觉语义信息进行信息融合，产生语义激光数据，给环境中每个路标物体对应的激光簇数据赋予视觉语义标签；
步骤s5：基于语义激光数据，构建带有各路标名称的语义地图，并且根据一一对应的路标名称和位置坐标建立关联映射表；
步骤s6：当定位出现偏差时，导航过程中利用视觉识别附近路标物体的分类及语义名称；
步骤s7：根据路标的语义信息查找映射表中对应坐标；
步骤s8：根据路标的坐标信息、激光实时感知数据和imu记录角度信息利用转换公式反向推算机器人当前实际位置。
优选的，所述的步骤s1的具体步骤如下：
步骤s101：地标物体的选择和设置
建立一个自带地标物体的环境地图，完成对地标物体的设定，而其设定要符合如下两个基本原则：
(1)挑选特征较为明显或者自带辨识信息的物体作为路标；
(2)路标为固定的静态物体，如果是位置或者形状在动态变化的物体则拒绝选择；
步骤s102：确定地标物体在当前环境的位置
选定好地标物体之后，将路标放置在当前环境中，放置地点满足以下两个约束条件：
(1)由于路标的状态是长期静止不动的，为了不影响机器人和其他事物的运动，所以路标的放置位置应在靠墙或者角落；
(2)各个路标的摆放位置要覆盖到整个地图环境；
步骤s103：构建地图
在选择好的地标物体按照放置原则确定安放在地图环境当中之后，便可构建地图，获取精准的地图信息。
优选的，所述的步骤s6中所述的视觉识别路标物体的方法采用tiny-yolo方法。在该步骤中，当前场景是机器人已经在构建好的带有地标的语义地图中进行自主导航了，若机器人出现了位置偏移，首先找到附近地标物体，获取相应的路标信息。所以要进行物体检测，由于用来位姿修正的数据来源之一是激光数据，数据获取的实时性要求较高，输入激光数据的速度在10hz或以上时在实际应用中才能获得较好的效果；而深度卷积神经网络是由多个隐含层组成的神经网络模型，每个隐含层提取图像不同层次的特征，网络层数越深，提取的特征越抽象，表征能力也就越强，但是同时检测速度会被严重拖慢，影响实时性。因此，在选择模型时需要在速度和效果上进行权衡。所以在本发明中使用的物体检测方法是基于tiny-yolo模型实现。tiny-yolo是目前检测速度最快的目标检测方法之一。tiny-yolo是基于yolov2模型的精简模型，而yolov2是一个端到端(end-to-end)的深度卷积神经网络模型，并将目标检测问题作为回归问题进行求解。与rcnn系列网络不同，yolov2的训练和检测均在一个网络模型中进行，不需要独立求解物体的候选框，所以在训练过程中不需要像rcnn系列网络一样采用分离模块求解物体的候选框。并且yolov2将目标检测问题作为回归问题求解，每次检测只需要进行一次的前向过程(inference)即可以同时得到物体位置和物体分类信息，而不需要像rcnn系列网络一样将物体位置和物体分类作为两部分求解。所以本发明直接使用这类目标检测模型得到输出结果。
优选的，所述的步骤s8的具体步骤如下：
假设机器人所用传感器为激光雷达，雷达建立的坐标系以极坐标表示，雷达扫描激光数据点间隔是0.5度，扫描范围是正前方的180度，则可以扫描360个数据点；而雷达的测量数据点是按角度顺序排列的，则可知第i个数据点在雷达为中心的极坐标系中的角度θi为：
其中，θi表示第i个数据点所在角度，π表示180°的弧度值，可通过公式(2)(3)将极坐标转化成笛卡尔坐标：
x′i＝ρi·cosθi(2)
其中，x′i表示第i个数据点横坐标，ρi表示机器人到第i个数据点的距离，cosθi表示第i个数据点的余弦值；
y′i＝ρi·sinθi(3)
其中，y′i表示第i个数据点纵坐标，ρi表示机器人到第i个数据点的距离，sinθi表示第i个数据点的正弦值；
将第i个数据点在笛卡尔坐标系中的点m点看作是算法当中的地标物体，通过映射表已知其世界坐标
设o为世界坐标系的原点；o’为传感器数据源，即机器人的实时位置，以其为原点构建了一个局部坐标系，机器人当前相对世界坐标系的位置坐标(x0,y0)可表示为：
其中，x0表示机器人位置横坐标，
其中，y0表示机器人位置纵坐标，
lx1＝x′i·cosα(6)
其中，x′i表示第i个数据点横坐标，cosα表示机器人偏转角的余弦，lx1表示数据点投影的总长；
lx2＝y′i·sinα(7)
其中，y′i表示第i个数据点纵坐标，sinα表示机器人偏转角的正弦，lx2表示数据点投影的部分长度；
ly1＝y′i·cosα(8)
其中，y′i表示第i个数据点纵坐标，cosα表示机器人偏转角的余弦，ly1表示数据点纵轴投影的部分长度；
ly2＝x′i·sinα(9)
其中，x′i表示第i个数据点横坐标，sinα表示机器人偏转角的正弦，ly2表示数据点纵轴投影的部分长度；
综合公式(4)(5)(6)(7)(8)(9)可推出机器人相对世界坐标系的坐标为：
其中，x0表示机器人位置横坐标，
其中，y0表示机器人位置纵坐标，
即当机器人由于航程推演方法缺陷、地面环境不均匀、人为移动机器人位置而产生位姿偏差时，可以通过以上算法推算机器人实际位置。
与现有技术相比，本发明的有益效果是：
本发明利用视觉感知信息赋予地标的语义标签，能让地标物体更加具化，在机器人思维中的存在更加明显；利用建图过程中激光数据正推导得到的地标位置信息和实时激光扫描信息，让相对的位置关系更加精准，有效提高定位精度和重定位效率，充分利用了视觉的识别作用和激光测量的准确性；同时，本发明对于位姿修正效果更加准确且对各类复杂环境都具有较强的自适应能力。
附图说明
图1为本发明的流程图。
图2表示以激光数据源(机器人)为原点建立的局部坐标系的坐标信息以及内部转换关系。
图3表示的是世界坐标系和机器人局部坐标系的相对关系以及坐标信息的变换。
具体实施方式
附图仅用于示例性说明，不能理解为对本专利的限制；
以下结合附图和实施例对本发明做进一步的阐述。
实施例1
如图1、图2、图3所示，基于路标信息与多传感器数据融合的机器人重定位方法，包括以下步骤：
步骤s1：构建地图，并在构建已知地图的过程中同时设置并确定几个特征明显的物体作为系统中的路标，在路标物体的选择和确定路标物体在当前环境的位置要符合本方法的基本原则；
步骤s2：利用聚类算法将落在同一路标物体上的激光点聚成一个激光点簇，形成激光数据；
步骤s3：通过深度学习和图像匹配的方式对视觉识别的物体产生视觉语义信息；
步骤s4：利用标定参数和几何模型，将激光数据和视觉语义信息进行信息融合，产生语义激光数据，给环境中每个路标物体对应的激光簇数据赋予视觉语义标签；
步骤s5：基于语义激光数据，构建带有各路标名称的语义地图，并且根据一一对应的路标名称和位置坐标建立关联映射表；
步骤s6：当定位出现偏差时，导航过程中利用视觉识别附近路标物体的分类及语义名称；
步骤s7：根据路标的语义信息查找映射表中对应坐标；
步骤s8：根据路标的坐标信息、激光实时感知数据和imu记录角度信息利用转换公式反向推算机器人当前实际位置。
优选的，所述的步骤s1的具体步骤如下：
步骤s101：地标物体的选择和设置
建立一个自带地标物体的环境地图，完成对地标物体的设定，而其设定要符合如下两个基本原则：
(1)挑选特征较为明显或者自带辨识信息的物体作为路标；
(2)路标为固定的静态物体，如果是位置或者形状在动态变化的物体则拒绝选择；
步骤s102：确定地标物体在当前环境的位置
选定好地标物体之后，将路标放置在当前环境中，放置地点满足以下两个约束条件：
(1)由于路标的状态是长期静止不动的，为了不影响机器人和其他事物的运动，所以路标的放置位置应在靠墙或者角落；
(2)各个路标的摆放位置要覆盖到整个地图环境；
步骤s103：构建地图
在选择好的地标物体按照放置原则确定安放在地图环境当中之后，便可构建地图，获取精准的地图信息。
优选的，所述的步骤s6中所述的视觉识别路标物体的方法采用tiny-yolo方法。在该步骤中，当前场景是机器人已经在构建好的带有地标的语义地图中进行自主导航了，若机器人出现了位置偏移，首先找到附近地标物体，获取相应的路标信息。所以要进行物体检测，由于用来位姿修正的数据来源之一是激光数据，数据获取的实时性要求较高，输入激光数据的速度在10hz或以上时在实际应用中才能获得较好的效果；而深度卷积神经网络是由多个隐含层组成的神经网络模型，每个隐含层提取图像不同层次的特征，网络层数越深，提取的特征越抽象，表征能力也就越强，但是同时检测速度会被严重拖慢，影响实时性。因此，在选择模型时需要在速度和效果上进行权衡。所以在本发明中使用的物体检测方法是基于tiny-yolo模型实现。tiny-yolo是目前检测速度最快的目标检测方法之一。tiny-yolo是基于yolov2模型的精简模型，而yolov2是一个端到端(end-to-end)的深度卷积神经网络模型，并将目标检测问题作为回归问题进行求解。与rcnn系列网络不同，yolov2的训练和检测均在一个网络模型中进行，不需要独立求解物体的候选框，所以在训练过程中不需要像rcnn系列网络一样采用分离模块求解物体的候选框。并且yolov2将目标检测问题作为回归问题求解，每次检测只需要进行一次的前向过程(inference)即可以同时得到物体位置和物体分类信息，而不需要像rcnn系列网络一样将物体位置和物体分类作为两部分求解。所以本发明直接使用这类目标检测模型得到输出结果。
优选的，所述的步骤s8的具体步骤如下：
如图2所示，假设机器人所用传感器为激光雷达，雷达建立的坐标系以极坐标表示，雷达扫描激光数据点间隔是0.5度，扫描范围是正前方的180度，则可以扫描360个数据点；而雷达的测量数据点是按角度顺序排列的，则可知第i个数据点在雷达为中心的极坐标系中的角度θi为：
其中，θi表示第i个数据点所在角度，π表示180°的弧度值，可通过公式(2)(3)将极坐标转化成笛卡尔坐标：
x′i＝ρi·cosθi(2)
其中，x′i表示第i个数据点横坐标，ρi表示机器人到第i个数据点的距离，cosθi表示第i个数据点的余弦值；
y′i＝ρi·sinθi(3)
其中，y′i表示第i个数据点纵坐标，ρi表示机器人到第i个数据点的距离，sinθi表示第i个数据点的正弦值；
将第i个数据点在笛卡尔坐标系中的点m点看作是算法当中的地标物体，通过映射表已知其世界坐标
如图3所示，设o为世界坐标系的原点；o’为传感器数据源，即机器人的实时位置，以其为原点构建了一个局部坐标系，机器人当前相对世界坐标系的位置坐标(x0,y0)可表示为：
其中，x0表示机器人位置横坐标，
其中，y0表示机器人位置纵坐标，
lx1＝x′i·cosα(6)
其中，x′i表示第i个数据点横坐标，cosα表示机器人偏转角的余弦，lx1表示数据点投影的总长；
lx2＝y′i·sinα(7)
其中，y′i表示第i个数据点纵坐标，sinα表示机器人偏转角的正弦，lx2表示数据点投影的部分长度；
ly1＝y′i·cosα(8)
其中，y′i表示第i个数据点纵坐标，cosα表示机器人偏转角的余弦，ly1表示数据点纵轴投影的部分长度；
ly2＝x′i·sinα(9)
其中，x′i表示第i个数据点横坐标，sinα表示机器人偏转角的正弦，ly2表示数据点纵轴投影的部分长度；
综合公式(4)(5)(6)(7)(8)(9)可推出机器人相对世界坐标系的坐标为：
其中，x0表示机器人位置横坐标，
其中，y0表示机器人位置纵坐标，
即当机器人由于航程推演方法缺陷、地面环境不均匀、人为移动机器人位置而产生位姿偏差时，可以通过以上算法推算机器人实际位置。
显然，本发明的上述实施例仅仅是为清楚地说明本发明所作的举例，而并非是对本发明的实施方式的限定。对于所属领域的普通技术人员来说，在上述说明的基础上还可以做出其它不同形式的变化或变动。这里无需也无法对所有的实施方式予以穷举。凡在本发明的精神和原则之内所作的任何修改、等同替换和改进等，均应包含在本发明权利要求的保护范围之内。
参考文献
[1]hantenr,bucks,ottes,etal.vector-amcl:vectorbasedadaptivemontecarlolocalizationforindoormaps[j].2016.
[2]frintrops,jensfeltp,christensenhi.attentionallandmarkselectionforvisualslam[c]//ieee/rsjinternationalconferenceonintelligentrobotsandsystems.ieee,2006:2582-2587.
[3]schusterf,kellercg,rappm,etal.landmarkbasedradarslamusinggraphoptimization[c]//ieee,internationalconferenceonintelligenttransportationsystems.ieee,2016:2559-2564。
- 还没有人留言评论。精彩留言会获得点赞！