基于UKF的ArUco码和轮式编码器融合定位算法*

许佳斌，吴永明，梁济民，王卫军，韩彰秀，李 根

(1.广东工业大学机电工程学院，广州 510006；2.广州先进技术研究所，广州 511458)

0 引言

相比于自动导引车(AGV)，移动机器人(autonomous mobile robot，AMR)[1]能提供更大的柔性，从而实现工作空间全范围的自主导航和物料搬运，是物流数字化和制造智能化的关键设备。通常来讲，移动机器人要实现自主导航，首先需要定位，即确定自身在环境中的位置和姿态，进而采用路径规划和运动控制算法实现导航[2]。

由于工业现场有移动的人和货物，且环境复杂，为实现鲁棒且高精度的定位，研究者们尝试了不同传感器，例如激光雷达、视觉、UWB/蓝牙等。

激光定位精度高，实时性好，但需要搭载成本较高的激光雷达，当周围环境变化后或在地图场景相似的情况下定位可靠性不高[3]。超宽带(ultra-wide band，UWB)是一种利用纳秒级的非正弦波脉冲传输的无线载波通信技术[4]。具有脉冲间隔时间短，系统复杂度低，定位精度高的优点，但他只适用于视距传播条件下，在非视距条件下或有障碍物遮挡时会大大降低它的定位精度[5-6]。视觉定位通过视觉传感器采集周围环境的图像信息来实现定位，具有信息丰富、结构简单、成本低廉、安装方便和无传感器探测距离限制的优点，因此受到广泛的关注。

视觉定位可进一步分为基于环境特征的定位和基于人造特征的定位[7]。最近提出的基于环境特征点视觉定位方法。有ORB-SLAM2[8]、LDSO[9]、Openvslam[10]等方法。但由于光照变化，运动模糊、尺度未知及感知混叠问题，基于环境特征点的定位方法不够鲁棒。基于二维码的人工路标相比于自然路标具有信息稳定，制作容易、检测快速和仅靠单目相机就可以识别深度信息的优点。王家恩等[11]提出一种基于QR码视觉和惯性传感器的复合导航方法，利用已知QR码的位置构建地面栅格地图实现全局定位，但是此方法需要预知QR码的位置，使用非常受限且把二维码贴在地下很容易受到污损，后期维护成本高。ZHENG等[12]提出基于扩展卡尔曼滤波(EKF)融合里程计和ArUco信息进行定位，该方法适用于动态环境的定位，但该定位系统是得到的是相对位姿且构建的地图误差会影响机器人的位姿推算，估计误差将随时间累积。CHIOU等[13]等提出了一种惯性测量单元(IMU)与二维码相结合的定位方案，该方法通过二维码定位信息可以降低惯性测量单元定位累计误差，但由于机器人位姿是通过IMU加速度计的双重积分进行推算，长时间累计误差大，且高精度IMU价格昂贵。

综上所述，为了实现工业环境下的鲁棒高精度导航，本文在视觉二维码定位的基础上，提出基于UKF融合轮速编码器的方法。

(1)提出了使用轮速编码器进行运动预测，利用二维码重投影误差进行更新的方法，解决了纯视觉定位抖动和在没有ArUco码时定位丢失的问题。

(2)采用UKF融合ArUco码轮式编码器和视觉信息，提高机器人的定位鲁棒性。

(3)通过自主搭建的机器人进行实验，对比了视觉ArUco、轮速编码器和UKF融合定位算法，验证了本文所提算法的精度和鲁棒性。

1 ArUco特征地图构建

1.1 ArUco码介绍

图1 ArUco码

ArUco码[14](简称marker)是一个增强现实的开源库，如图1所示它是一种正方形的基准标记(squared fiducial marker)，由黑色边框和内部二进制编码组成，黑色为0，白色为1，通过识别marker解码为二进制编码与字典库进行匹配，可以得到唯一的ID号，以marker为中心建立坐标系可以得到4个角点的坐标，识别边框4个角点的像素坐标可以计算marker与相机的相对位姿。ArUco码相比于其他路标具有检测精度高和检测速度快的优点[15]。

1.2 地图构建

(1)

图2 ArUco码建图原理

2 基于UKF的数据融合定位算法

在ArUco码特征地图的基础上，本文提出的定位算法可以分为4个部分：定位初始化、运动模型、观测模型和基于UKF的数据融合。

不失一般性，运动模型和观测模型可以描述为：

(2)

式中，xt为机器人当前时刻的位姿(位置和姿态)；μt为机器人编码器读数或者控制量；zt,j为传感器对yj路标点的观测值；wt和vt,j为运动噪声和观测噪声，满足零均值的高斯分布。

只给定图像数据，定位初始化用于确定机器人在地图坐标系下的初始位姿，作为后续迭代位姿估计的初始输入；运动模型的输入为上一个时刻的定位结果及轮式编码器数据，输出为当前时刻机器人的位姿分布；给定一个位姿假设和当前观测数据，传感器模型用于评价在该位姿假设下的观测数据是否符合实际的观测数据；一旦具备运动模型和观测模型，即可用UKF框架构建轮式里程计和相机数据相融合的定位算法，从而避免单纯里程计定位导致的累计误差和单纯图像定位时模糊、低对比度图像造成的大偏差。

2.1 定位初始化

图3 坐标变换示意图

如果一帧图像检测到多个marker，对每个marker的4个角点进行重投影误差：

(3)

2.2 运动模型

机器人底盘采用两轮差速装置，由两个驱动轮和4个支撑轮组成，运动学模型示意图如图4所示。

图4 机器人运动学模型

机器人左右两边的车轮分别装有一个带增量式光电编码器的伺服电机，可以分别控制左右轮的行驶速度，实现机器人转弯和直行。光电编码器可以计算出特定时间Δt内机器人的左右轮位移，进而推算出两轮中心的位移和旋转角度。计算表达式为：

(4)

(5)

式中，Δsl、Δsr为机器人左右轮位移增量；Δθ、Δs为机器人中心单位时间内的位移和旋转角度；b为轮距。

(6)

2.3 观测模型

如图5所示为相机针孔成像模型，根据成像模型建立观测方程。

图5 相机针孔成像模型

将空间中的一点P=(Xw,Yw,Zw,1)T通过相机内参和外参转换到像素坐标系下p=(u,v)T，s为空间点在相机坐标系下的深度值，通过相机标定可以获得相机内参K。这一成像模型可以描述为：

(7)

给定机器的位姿为(xt,yt,θt)，可以构建成像模型的外参矩阵。

(8)

联合式(7)、式(8)，可以得到观测模型为：

(9)

2.4 UKF融合定位

由于UKF采用无迹变换后的sigma点实现对高斯分布的近似表达，将分布转变为了加权样本点，因此不需要对运动模型和观测模型进行线性化，理论上精度高于EKF[18]。

(10)

协方差Σt-1为：

(11)

式中，Q为控制噪声协方差，即编码器的噪声协方差，与左右轮位移成正比；R为观测噪声协方差，即相机的观测噪声协方差。

在这里假设编码器的标准差与Δt时间内的位移成正比，观测的标准差与该帧图片观测的marker数量m成反比，可得：

(12)

(13)

(14)

带入运动方程进行预测，得到：

(15)

(16)

计算得到卡尔曼增益Kt，更新机器人位姿的均值和协方差，得到后验估计值，有：

(17)

3 实验结果及分析

3.1 实验平台及环境

本文搭建了如图6所示的两轮差速机器人实验平台进行实验测试，实验平台主要由显示器、工控机和运动底盘组成。使用索尼IMX317的USB摄像头，分辨率为1920×1080，帧率为30 Hz。使用研华的ARK-2250L-U3A2工控机，CPU为i3-6100U处理器，8 G运行内存，工控机的上的操作系统为Ubuntu18.04版本，安装了Melodic版本的ROS系统，通过Rviz显示定位轨迹。

工控机与两个电机直接采用串口进行连接，实现工控机对电机的直接控制，电机通过串口反馈编码器信息。

实验区域为一块10 m×2.5 m的空地，如图7所示。由于灯光不均匀和遮挡，从图上可以明显看出环境的明暗变化。在该区域的天花板上均匀粘贴了ArUco码，摄像头与天花板距离2.5 m左右。

图6 机器人实验平台 图7 实验环境

3.2 实验结果及分析

通过两个实验进行定位测试，采用ROS系统的Rviz可视化工具显示运动轨迹信息。

实验1通过键盘控制小车进行移动定位，将仅靠编码器定位、纯视觉定位和UKF融合定位的3个算法轨迹进行比较，实验结果如图8所示。

图8 定位精度对比

从图中可以看出：由于累计误差，仅靠编码器定位的运动轨迹会越来越偏离实际轨迹(参考UKF的轨迹)。仅靠视觉信息进行定位的结果在运动过程中会有高频抖动，一方面是由明暗变化导致，另一方面是由运动模糊导致，特别在机器人转弯过程中，由于机器人运动幅度过大会出现较大的定位波动。采用UKF融合后的算法可以有效解决机器人定位过程中的抖动和累计误差的问题，使定位轨迹更平滑。

实验2分为2个步骤：

步骤1：在地图范围内人工规划一条直线轨迹；

步骤2：启动机器人的导航功能，自主跟随直线轨迹，重复该条轨迹20次。在跟随过程中记录3种定位算法给出的定位结果。

特别的，由于自主导航需要定位系统给出定位结果，为了轨迹跟随效果稳定，我们采用了UKF给出的结果用于自主导航。20次自主轨迹跟随实验定位结果如图9所示。

图9 导航定位精度对比

整体来看，基于UKF融合定位算法给出的轨迹定位的分布(灰色曲线簇)是最集中的，意味其随机误差最小。另外，可以发现A点，编码器给出了大偏转且单纯的视觉定位给出了较大的随机误差，这是因为在A点处，机器人运动路面不平，轮子发生了打滑，车体发生了抖动，而基于UKF定位算法在A点获得了较为精确、平滑的定位结果，验证了本文所提算法的鲁棒性。

4 结论

为解决移动机器人的精确，鲁棒、且低成本的定位问题，本文采用了ArUco人工标识；传感器采用单目相机和轮式编码器；定位算法上设计了基于UKF的数据融合定位算法， 通过实验表明，本文提出的基于UKF的ArUco和轮式编码器融合定位算法，相比于单个传感器的定位方法，可以有效改善移动机器人在定位过程中姿态偏离和定位抖动的问题，有效降低了定位的随机误差，从而提高了定位精度和鲁棒性，对于室内移动机器人的高精度和鲁棒性定位方面具有良好的借鉴价值。