一种融合SSD目标识别和里程计信息的移动机器人定位方法与流程

本发明属于移动机器人视觉定位领域，具体涉及一种移动机器人定位方法。

背景技术：

随着机器人领域的研究与发展，室内移动机器人视觉定位在工业，服务业应用越来越广泛，移动机器人的鲁棒，快速，高精度定位是移动机器人控制，避障，任务规划的基础，同时视觉定位具有信息量大，成本低的特点而被广泛应用在服务机器人，仓储机器人，工业移动机器人。将相机安装在室内的棚顶处可以对室内环境进行观测，将标志物固定到移动机器人上，移动机器人在运动过程中标志物与环境融为一体，需要通过ssd对图像中机器人上的标志物进行快速识别进而获得机器人的位姿，但是由于ssd识别标志物位置存在噪声，单纯的运用视觉定位的结果不具有鲁棒性，而且移动机器人携带的里程计由于机器人运动打滑等因素会存在积累误差，因此，这些因素将会影响移动机器人定位结果的鲁棒性和精确度。

技术实现要素：

本发明的目的是为解决ssd识别标志物位置存在噪声、以及移动机器人运动打滑影响移动机器人定位结果的鲁棒性和精确度的问题。

本发明为解决上述技术问题采取的技术方案是：

一种融合ssd目标识别和里程计信息的移动机器人定位方法，该方法包括以下步骤：

步骤一、将用于定位的aruco标志物固定在移动机器人上，控制移动机器人在室内地面行走，并在室内棚顶安装usb相机用来采集移动机器人的运动图像；

步骤二、利用开源视觉库opencv识别采集的图像上的aruco标志，得到aruco标志的矩形包络框，根据矩形包络框位置做出采集图像的标签，对打标签后的图像进行增广，并将增广结果分为训练集和测试集两部分；

步骤三、将训练集输入到ssd网络进行训练，测试集用于验证训练的ssd网络，直至测试集上的定位准确率不低于q时，得到训练好的ssd网络；

步骤四、采集待定位移动机器人的运动图像，求出用于修正图像透视畸变的修正矩阵n，并利用修正矩阵n来修正图像透视畸变；

步骤五、将修正透视畸变后的运动图像输入训练好的ssd网络进行处理，得到usb相机测量出的标志物位姿，再根据标志物和待定位移动机器人的几何关系求出usb相机测量出的待定位移动机器人位姿；

步骤六、对齐usb相机采集的待定位移动机器人的运动图像信息和pc端读取的里程计信息；

步骤七、在h-infinity滤波框架下融合usb相机测量出的待定位移动机器人位姿和pc端读取的里程计信息，得到待定位移动机器人位姿的估计值。

本发明的有益效果是：本发明提供了一种融合ssd目标识别和里程计信息的移动机器人定位方法，本发明构建的在h-infinite滤波框架下融合ssd的定位信息和里程计的运动信息的方法，利用需要的标志物对ssd网络进行训练，进而通过usb相机获得标志物的位置，在滤波框架下融合里程计信息后输出的移动机器人位姿更加具有鲁棒性，在usb相机未识别出来目标的情况下，可以通过里程计信息估计出位姿状态，滤波算法滤除了usb相机的噪声，使定位误差在1.5cm以内，角度误差在4度以内。

附图说明

图1为本发明的视觉定位执行框图；

图2为本发明对齐移动机器人的运动图像信息和里程计信息的示意图；

图3为本发明的标志物和移动机器人的几何关系示意图；

其中：markerh代表标志物h，markers代表标志物s；o代表坐标原点，x和y分别代表x轴和y轴；

具体实施方式

具体实施方式一：本实施方式所述的一种融合ssd目标识别和里程计信息的移动机器人定位方法，该方法具体包括以下步骤：

步骤一、将用于定位的aruco标志物固定在移动机器人上，控制移动机器人在室内地面行走，并在室内棚顶安装usb相机用来采集移动机器人的运动图像；

步骤二、利用开源视觉库opencv识别采集的图像上的aruco标志物，得到aruco标志物的矩形包络框，根据矩形包络框位置做出采集图像的标签，对打标签后的图像进行增广，并将增广结果分为训练集和测试集两部分；

步骤三、将训练集输入到ssd网络进行训练，测试集用于验证训练的ssd网络，直至测试集上的定位准确率不低于q时，得到训练好的ssd网络；

步骤四、求出用于修正图像透视畸变的修正矩阵n，并利用usb相机采集待定位移动机器人的运动图像；

步骤五、将采集的待定位移动机器人的运动图像输入训练好的ssd网络进行处理，得到aruco标志物在运动图像中的位置，利用修正矩阵n求出aruco标志物在地面坐标系中的坐标；

再根据aruco标志物和待定位移动机器人的几何关系求出usb相机测量出的待定位移动机器人位姿；

步骤六、对齐usb相机采集的待定位移动机器人的运动图像和pc端读取的里程计信息；

步骤七、在h-infinity滤波框架下融合usb相机测量出的待定位移动机器人位姿和pc端读取的里程计信息，得到待定位移动机器人位姿的估计值。

具体实施方式二：本实施方式与具体实施方式一不同的是：步骤一的具体过程为：

将两种用于定位的aruco标志物h和s固定在移动机器人上，控制移动机器人在室内地面行走，并在室内棚顶安装usb相机用来采集移动机器人的运动图像，pc端记录采集的图像，记录w张图像。

具体实施方式三：本实施方式与具体实施方式二不同的是：所述步骤二中对打标签后的图像进行增广，其具体方法包括：在图像中添加随机椒盐噪声、在图像中添加高斯噪声或将带有aruco标志物的局部图像放大和旋转。

具体实施方式四：本实施方式与具体实施方式三不同的是：步骤三的具体过程为：

ssd网络的前端是基础的vgg-16分类网络，后端接有5个不同尺度的特征层(特征层的尺度不是固定的，可以根据实际需要调整)，本文中需要训练ssd识别用于定位的两种aruco标志物，ssd网络前端的vgg-16分类网络导入已经预训练好的参数，后端的特征层的训练利用步骤二制作好的训练集；

利用步骤二制作好的训练集训练ssd网络，设置学习率为0.001，动量参数为0.9，batch_size为32，权重衰减为0.0005，不断进行迭代训练，调整ssd网络的卷积层的权重值以及偏置值，直到loss小于1.6时，用测试集验证训练的ssd网络，直至测试集上的定位准确率不低于q时停止训练，得到训练好的ssd网络。其中：q的值不小于95％。

具体实施方式五：本实施方式与具体实施方式四不同的是：步骤四中求出用于修正图像透视畸变的修正矩阵n的具体过程为：

在室内棚顶安装摄像头，由于安装问题导致地面的图像出现透视畸变，通过一个3×3大小的矩阵n修正透视畸变；

在地面放置两个正方形板，两个正方形板的八个顶点坐标分别为[xiyi1]^t，i＝1，2...8，八个顶点在图像平面内的投影点坐标分别为[uivi1]^t，i＝1，2...8，世界坐标系中点和图像坐标系中点的关系为：

即估计出用于修正图像透视畸变的修正矩阵n的8个值，得到修正矩阵n11、n12、n13、n21、n22、n23、n31和n32均是矩阵中的元素。

具体实施方式六：本实施方式与具体实施方式五不同的是：所述步骤五的具体过程为：将采集的待定位移动机器人的运动图像输入训练好的ssd网络进行处理，根据输出的矩形包络框得到待定位移动机器人上的h和s两个aruco标志物在运动图像中的位置分别为和其中：科分别为h和s两个aruco标志物在图像中的位置坐标；

根据修正矩阵n求出待定位移动机器人的h和s两个aruco标志物在地面坐标系中的坐标为：和

根据待定位移动机器人上的h和s两个aruco标志物与中心相对位置关系计算出待定位移动机器人中心位姿，如图3所示，p是两个aruco标志物h和s连线的中点，设pm＝l1，hs＝l2，pm和ps夹角为θ0，点p坐标为(xp，yp)，m点坐标为(xm，ym)，根据几何关系可以得到：

其中：l1和l2分别代表pm和hs的长度；

则旋转角θr表示为：θr＝arctan(ysc-yhc，xsc-xhc)+θ0

得到usb相机测量出的待定位移动机器人位姿为yk＝[xmymθr]^t。

具体实施方式七：本实施方式与具体实施方式六不同的是：所述步骤六的具体过程为：

pc端接收usb相机传送的待定位移动机器人的运动图像，利用ros系统，在pc端读取husky传送的里程计信息，选取时间戳最近的信息作为同一时刻采集的待定位移动机器人的信息。

如图2所示，usb相机采集的运动图像与pc端通过usb连接，pc端读取husky传送的里程计信息，将usb相机采集的运动图像与husky传送的里程计信息进行匹配，得到待定位移动机器人每一时刻对应的usb相机采集的运动图像和husky传送的里程计信息。

具体实施方式八：本实施方式与具体实施方式七不同的是：所述步骤七的具体过程为：

根据运动模型构建待定位移动机器人的运动方程和观测方程为：

其中：xk＝[xkykθk]^t是待定位移动机器人的位姿变量，且位姿变量的三个元素分别为移动机器人的x轴坐标，y轴坐标，待定位移动机器人轴线与x坐标轴的夹角；

xk-1＝[xk-1yk-1θk-1]^t是上一时刻的待定位移动机器人的位姿变量，为里程计的测量信息，为里程计的测量噪声，yk为usb相机测量出的待定位移动机器人位姿，为usb相机的测量噪声；

则待估计的目标：即待定位移动机器人的位姿变量本身，即：

zk＝ixk

其中：zk为待估计量，i为单位矩阵；在h-infinite滤波框架下求解出zk的估计值，即得到待定位移动机器人位姿的估计值。

当步骤六中获得待定位移动机器人每一时刻对应的usb相机采集的运动图像和husky传送的里程计信息后，将运动图像对应的位姿与同一时刻的里程计信息在h-infinity滤波框架下融合，得到该时刻下的待定位移动机器人位姿的估计值。

如图1所示，定位出移动机器人的当前位置后，判断移动机器人是否到达目的地，若到达目的地，则停止移动机器人，若未到达目的地，则继续进行路径规划，生成控制指令控制移动机器人移动，直至判断移动机器人到达目的地。

本发明还可有其它多种实施例，在不背离本发明精神及其实质的情况下，本领域技术人员当可根据本发明作出各种相应的改变和变形，但这些相应的改变和变形都应属于本发明所附的权利要求的保护范围。