[发明专利]一种融合SSD目标识别和里程计信息的移动机器人定位方法

权利要求书

1.一种融合SSD目标识别和里程计信息的移动机器人定位方法，其特征在于，该方法包括以下步骤：

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

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

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

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

所述步骤四中求出用于修正图像透视畸变的修正矩阵N的具体过程为：

在地面放置两个正方形板，两个正方形板的八个顶点坐标分别为[X_i Y_i 1]^T，i＝1,2…8，八个顶点在图像平面内的投影点坐标分别为[u_i v_i 1]^T，i＝1,2…8，世界坐标系中点和图像坐标系中点的关系为：

即估计出用于修正图像透视畸变的修正矩阵N的8个值，得到修正矩阵n₁₁是矩阵中的元素；

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

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

所述步骤五的具体过程为：

将采集的待定位移动机器人的运动图像输入训练好的SSD网络进行处理，根据输出的矩形包络框得到待定位移动机器人上的H和S两个ArUco标志物在运动图像中的位置分别为和其中：和分别为H和S两个ArUco标志物在图像中的位置坐标；

根据修正矩阵N求出待定位移动机器人的H和S两个ArUco标志物在地面坐标系中的坐标为：和

根据待定位移动机器人上的H和S两个ArUco标志物与中心相对位置关系计算出待定位移动机器人中心位姿，P是两个ArUco标志物H和S连线的中点，设PM＝L₁，HS＝L₂，PM和PS夹角为θ₀，点P坐标为(x_p,y_p)，M点坐标为(x_m,y_m)，根据几何关系可以得到：

其中：L₁和L₂分别代表PM和HS的长度；

则旋转角θ_r表示为：θ_r＝arctan(y_sc-y_hc,x_sc-x_hc)+θ₀

得到USB相机测量出的待定位移动机器人位姿为Y_k＝[x_m y_m θ_r]^T；

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

所述步骤六的具体过程为：

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

所述选取时间戳最近的信息作为同一时刻采集的待定位移动机器人的信息，其具体过程为：

将USB相机采集的运动图像与HUSKY传送的里程计信息进行匹配，得到待定位移动机器人每一时刻对应的USB相机采集的运动图像和HUSKY传送的里程计信息；

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

所述步骤七的具体过程为：

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

其中：X_k＝[x_k y_k θ_k]^T是待定位移动机器人的位姿变量，且位姿变量的三个元素分别为移动机器人的X轴坐标，Y轴坐标，待定位移动机器人轴线与X坐标轴的夹角；

X_k-1＝[x_k-1 y_k-1 θ_k-1]^T是上一时刻的待定位移动机器人的位姿变量，为里程计的测量信息，为里程计的测量噪声，Y_k为USB相机测量出的待定位移动机器人位姿，为USB相机的测量噪声；

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

Z_k＝IX_k

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