2. 上海师范大学信息与机电工程学院, 上海 200234
2. College of Information, Mechanical and Electrical Engineering, Shanghai Normal University, Shanghai 200234, China
0 引言
移动机器人的自定位问题是机器人研究领域的经典问题，研究者们致力于开发精度更高、成本更低、更加方便实用的定位算法，里程计、航位推算等相关技术在其中担任着重要的角色[1]. 移动机器人的里程计一般由一种或多种低成本传感器计算得到，短时间内精度较高，但存在累计误差，如果不对该误差进行修正，里程计的性能将快速下降. 所以，里程计的模型误差估计与修正也一直是移动机器人领域人们研究的热点问题.
传统的轮式机器人里程计模型参数估计和误差修正方法有UMBmark校准法[2]、最小二乘法[3]、路径比较法(path comparison，PC)[4]等，这些方法都是基于轮式机器人的里程计模型，通过校正模型参数以减小系统误差且是离线运行的. 为了满足不断发展的机器人应用需求，一些更加高效、对模型参数依赖更小的误差修正方法相继被提出. 文[5]和文[6]将人工神经网络引入到移动机器人里程计中，直接对里程计模型进行估计，更具有鲁棒性和普适性；Yun等[7]提出一种利用家庭清洁机器人已知的固定定位信息用自适应扩展卡尔曼滤波(augmented extended Kalman filter，AEKF)来估计系统误差参数的方法.
仿人机器人被认为是机器人技术发展最高水平的体现，其外形、结构及步行方式与人类相似，更适合在生活和工作环境中与人类协同工作[8]. 仿人机器人双足行走的运动方式更加灵活多变，但其里程计的系统模型更加复杂，误差修正也更加困难，仅仅依靠简单的编码器和惯性测量传感器(inertial measurement unit，IMU)是不够的. 随着硬件计算能力的不断提高，基于图像流的视觉里程计(visual odometry，VO)和视觉SLAM(simultaneous localization and mapping)技术发展迅速.
ORB-SLAM(oriented FAST and rotated BRIEF SLAM)[9]通过非线性优化和回环检测修正了视觉里程计的累计误差. SVO(semi-direct visual odometry)[10]结合了特征点法和直接法，进一步提高了视觉里程计的实时性. 进一步地，为了弥补单目视觉里程计尺度不定等问题，研究者开始在视觉里程计加入IMU，形成视觉惯性里程计(VIO)，典型的代表有OKVIS(open keyframe-based visual-inertial SLAM)[11]、visual-inertial ORBSLAM[12]、VINS-Mono(visual-inertial system monocular)[13]等. 视觉传感器能采集丰富的环境信息且成本低廉，在仿人机器人机器人领域开始广泛利用视觉信息来修正里程计的定位误差. Oriolo等[14]通过拓展卡尔曼算法将单目视觉里程计和基于机器人运动学得到的里程计数据进行融合，利用VSLAM有效地修正了里程计的误差. 王立玲等[15]将机器人状态定义为机器人位姿和速度，分别用运动学里程计、单目ORB-SLAM和IMU进行测量，通过拓展卡尔曼的状态测量更新步骤完成多传感器的融合. 基于自然特征点视觉信息修正里程计的方法虽然精度较高，但在纹理较少或动态的环境中，点特征容易缺失或误匹配. 为了解决这个问题，在最新的研究中，人们将更高层次的环境特征如线、面、物体等加入到系统中以提高鲁棒性[16-17]. 但这些方法在特征提取、数据关联和位姿优化等模块中计算量较大，还不能应用于实时嵌入式系统中.
针对上述存在的问题，本文提出一种基于环境物体路标的仿人机器人实时里程计算法，算法流程如图 1所示. 由正运动学计算得到行走里程计，此时若视觉信息有效，则通过无迹卡尔曼滤波用视觉测量对里程计进行修正，同时把里程计修正前后的数据加入训练集；若视觉信息无效，则根据训练集数据通过广义回归神经网络(generalized regression neural network，GRNN)更新回归模型，使用该模型直接对里程计进行修正. 主要研究内容及贡献有：
|
| 图 1 算法流程图 Fig.1 Algorithm flow chart |
1) 算法直接以环境中的物体作路标，通过色彩分割和形态学处理识别并定位路标；并提出一种应用于仿人机器人特征点三维坐标数值求解及机器人全局位姿估计方法.
2) 提出多路标组合定位方法，将多个路标的测量信息进行组合，并对无迹卡尔曼滤波(unscented Kalman filter，UKF)框架进行拓展，以实现机器人运动学里程计数据与多组测量信息的融合，完成对里程计数据的测量修正.
3) 为进一步提高系统鲁棒性，提出一种基于广义回归神经网络模型的里程计实时修正方法. 当视觉测量无效时，短时间内替代滤波修正.
本文以仿人机器人NAO为实验平台通过一系列仿真和实体实验验证了本算法的有效性.
1 视觉位姿测量 1.1 选取路标物体为了降低机器人对工作环境的要求，避免对环境进行改动，算法直接在环境中选取合适的物体作为路标. 选取原则是：作为路标的物体色彩在环境中应较为突出，便于识别；物体中心在同一水平面不同方位的投影点与该物体投影的几何中心重合，即路标物体应具有对称结构，如球体、柱体等. 如图 2所示，可选取场景中的玩偶和苹果作为路标. 环境路标物体确定之后，记录所选物体的位置、色彩、形状、尺寸等信息作为路标的先验信息.
|
| 图 2 环境路标选取 Fig.2 Selection of environment landmarks |
机器人在行走时采集的图像中存在运动模糊且含有大量噪声和无用的环境信息，如图 3(a)所示. 需要对原始图像进行处理，消除图像模糊的影响和清除多余的环境信息，再对处理后的图像进行路标的识别和中心坐标的计算.
|
| 图 3 相机图像处理过程 Fig.3 Camera image processing |
仿人机器人在行走过程中，头部晃动较大，容易产生运动模糊. 运动模糊对图像细节部分影响较大，在传统的基于特征角点的视觉里程计中，会导致特征丢失，跟踪失败. 但其对于色块的影响相对较小，故将对特征角点的识别定位转化为对路标物体中心的定位可一定程度消除图像运动模糊的影响.
根据路标选取的规则可知，路标物体的色彩在场景环境中较为突出，故根据路标的先验色彩信息进行图像分割，分成路标区域和背景环境，分割的效果如图 3(b)所示. 从图中可以看出，图像分割基本去除了背景环境且对于存在运动模糊的图像依然可以有效地提取到路标，但图像中还存在一些环境噪声且路标物体的轮廓粗糙，形状较不规则. 为进一步去除噪声，平滑路标物体区域轮廓，对分割图像再进行形态学处理. 开运算消除图像中孤立的噪点，闭运算平滑路标区域外轮廓，同时填补区域中的小孔使其完整平滑，处理后的效果如图 3(c)所示.
1.2.2 路标物体的识别经过色彩分割和形态学处理后的图像中只含有圆滑的路标块和少许不规则形状的干扰块，此时可通过标识物体的先验信息如物体的形状、大小及位置信息等对色块进行筛选，定位标识.
首先对分割图像进行粗略的扫描，得到色块的大致位置，再对候选位置周围进行精细的扫描以确定色块的面积与形状轮廓，根据路标形状大小过滤候选色块，通过式(1) 求剩余候选色块的中心：
|
(1) |
|
(2) |
其中，U，D，L和R为标识区域框边界；p向量为(u，v)T处像素值，定义标识为1，背景为0. 最后，根据机器人先验位姿估计和路标在世界坐标系中的坐标对路标进行重投影，距离重投影点最近的色块中心即为路标中心的测量值(u0，v0)T，如图 3(d)所示.
1.3 机器人位姿求解已知路标物体中心在图像中的像素坐标(u，v)T及其世界坐标系坐标(xw，yw，zw)T，机器人的位姿解算问题就是一个n点透视问题(perspective-n-point，PnP)[18]. 传统求解PnP的方法，如P3P(perspective-three-point)、EPnP(efficient PnP)[19]，往往需要求解高阶方程组来计算相机三维位姿(x，y，z，α，β，θ)且至少需要3对特征点，求解过程繁琐，运算量大. 一般地，仿人机器人在行走过程中，质心高度基本保持不变，机器人(相机)的运动可以看作在一个二维水平面上的运动，其位姿只有3个自由度：二维位置和姿态角. 本文根据简化的机器人模型和路标点之间的几何关系对机器人位姿进行估计，算法总体时间复杂度为O(1)，相比传统方法，更加快速高效.
1.3.1 2D-3D点求解如图 4所示，建立相机坐标系∑c和图像坐标系∑p，物体路标中心M所在水平面与相机平面的垂直高度差为h，M在相机坐标系下的坐标为(xc，yc，zc)T，在成像平面的投影坐标为(xp，yp)T. 机器人的结构参数，相机的焦距f，特征点的世界坐标(xw，yw，zw)T及其投影坐标(u，v)T均为已知量.
|
| 图 4 相机、图像坐标系与标识物体及投影点的几何关系 Fig.4 Geometric relationship among camera coordinate system, image, landmark and its projection point |
中心M点的像素坐标为(u，v)T，将之转化为图像坐标：
|
(3) |
其中，(W，H)为图像的尺寸，FOV(field of view)为相机视场角.
一般地，仿人机器人头部存在pitch、yaw两个自由度，机器人的头部可能会转动，如图 5所示，需要对相机进行旋转矫正. 旋转矩阵为
|
| 图 5 相机的旋转修正 Fig.5 Correction for camera rotation |
|
(4) |
其中，ϕ为yaw角，φ为pitch角.
根据头部的旋转矩阵对M投影点的图像坐标进行修正：
|
(5) |
相机距离地面的高度hc可由机器人结构参数经过正运动学计算得到，即zc，根据图中的三角形相似关系可得到物体路标中心M在相机坐标系下的3D坐标(xc，yc，zc)T：
|
(6) |
通过1.3.1小节中2D-3D点的转换，可以求出两个路标的中心点在相机坐标系下的3D坐标M1(x1c，y1c，z1c)T、M2(x2c，y2c，z2c)T. 机器人可近似为在平面运动，将场景投影到二维平面，如图 6所示，其中M1、M2为路标的中心点投影，以点M1为原点，M1M2为x轴，建立路标坐标系∑m，∑w、∑c分别为世界坐标系和相机坐标系，T为转换矩阵.
|
| 图 6 坐标系之间的转换关系 Fig.6 Transformation relationship between coordinates |
已知M1、M2在世界坐标系中的坐标分别为(x1w，y1w)T和(x2w，y2w)T，∑m在∑w中旋转角度：
|
(7) |
世界坐标系到路标坐标系的转换矩阵为
|
(8) |
同理，已知M1、M2在相机坐标系中的坐标为(x1c，y1c)T、(x2c，y2c)T，可得
|
(9) |
其中，
|
(10) |
相机坐标系到机器人坐标系的转换矩阵为Tcb，由机器人结构参数得到，世界坐标系到机器人坐标系的转换矩阵为
|
(11) |
求得仿人机器人在二维世界坐标系下的位姿.
与VSLAM通常采用的基于集束调整优化求解相机位姿的方法[13]相比，本文所提出的基于几何模型与坐标转换的位姿求解方法，不需要众多的特征点对，计算复杂度小，实时性高，但位姿估计的精度不及视觉SLAM算法.
2 基于UKF的测量修正 2.1 行走里程计仿人机器人移动方式特殊，无轮式机器人的编码器来提供里程计信息，但可通过正运动学计算机器人姿态变化进而得到行走里程计.
一般地，仿人机器人下半身可简化为图 7所示的模型，其中，∑b、∑f分别表示机器人身体(胸部)坐标系和支撑脚坐标系，j为关节，l为关节长度；Torso为机器人躯干，即质心位置；FSR为机器人脚底压力传感器. 单腿具有6个自由度，可将之看作以脚为基座的机械臂，建立D-H坐标系，关节ji到关节ji+1的转换矩阵为
|
| 图 7 仿人机器人行走简化模型 Fig.7 Simplified walking model of humanoid robot |
|
(12) |
其中，θ、d、a和α为关节的D-H参数. 根据链式法则，机器人脚底坐标系到胸部的转换矩阵为
|
(13) |
机器人胸部坐标系k到k+1时刻的位姿偏移为
|
(14) |
得到机器人行走里程计增量，进而得到行走里程计数据.
2.2 滤波修正算法根据运动学计算得到的机器人里程计由于是由位姿增量积分得到，其短时精度较高但存在累计误差，而由路标计算得到的视觉测量可以提供全局的位姿估计，二者特性互补，所以可使用滤波的方法使用视觉信息来修正运动学里程计.
将机器人位姿估计的不确定性用概率分布(μ，Σ)来表示，整个系统满足马尔可夫假设，里程计增量ΔT作为系统输入，视觉测量信息作为系统观测Z，通过滤波的方式将二者进行融合，以视觉观测值来修正行走里程计. 机器人系统是非线性的，本文融合方法采用无迹卡尔曼滤波[20]，通过采样的方式对系统进行线性近似，相比扩展卡尔曼滤波具有更高的近似精度.
系统状态变量定义为机器人在世界坐标系下的位姿，即X =(p，θ)T=(x，y，θ)T，状态转移方程可用式(15)表示：
|
(15) |
其中，
测量方程可表示为
|
(16) |
其中，H为单位阵；δ (k)为高斯随机噪声，方差为Q (k).
无迹卡尔曼滤波算法根据k-1时刻机器人位姿X (k-1) 和k时刻里程计增量Δ T (k)通过式(15)计算状态转移，再根据视觉测量Z (k)通过式(16)完成状态测量得到机器人后验位姿X (k)~(μ，Σ) X (k).
无迹卡尔曼滤波融合算法具体步骤如算法1所示.
在实际应用中，视觉测量的精度很大程度上取决于路标的识别与提取，若只依靠单组路标，路标识别的误差对最后的结果影响较大. 另一方面，当这组路标被遮挡或不在机器人视野范围内时，视觉信息将失去作用. 因此，为提高视觉测量的鲁棒性，本算法在环境中选取多个路标，并将所有路标进行组合进而得到多组测量结果，加入到滤波算法中参与融合修正. 根据视觉测量模块可知，要得到机器人的位姿需要2个路标，一个作为路标坐标系的原点用于解算机器人位置，另一个提供坐标系x轴方向用于解算机器人姿态. 多个物体作为路标，保证机器人视野中始终存在2个及以上路标以确保视觉定位的有效性.
当视野中出现2个以上路标时，为充分利用路标信息，同时保持算法的实时性，采用以下方式处理所有路标：1) 假设在环境中选取了N个物体路标，其中机器人视野中存在n个，以每个路标为原点，以图像中距离原点最远的路标为x轴方向，建立n个路标坐标系，如图 8所示，因为，当确定x轴方向时，距离原点越远的路标，其由测量带来的误差对方向影响越小. 2) 进一步可以解算出n个机器人位姿，在滤波算法的测量更新步骤中，以上一个测量融合后的结果作为本次测量的先验，进行融合得到本次测量的后验，再作为下一个测量的先验，直至完成n次测量融合.
|
| 图 8 多路标坐标系建立 Fig.8 Establishment of coordinate for multi-landmarks |
多路标融合算法如算法2所示.
| 算法2 多路标融合算法 |
1. 运动更新： |
| 2. 测量更新： |
3.  |
| 4. else： |
5.  |
| 6. for i from 1 to n do |
7.  |
8.  |
| 9. end |
10.  |
| 11. end |
| 12. 返回后验估计X(k) |
算法2中，首先根据里程计增量完成机器人状态的运动更新. 若机器人视野中路标少于2个，则直接输出运动更新后状态；若存在2个及以上路标，则根据上述方法用测量得到的n个机器人状态对预测状态进行修正完成测量更新. 其中，第1步的状态转移和第7步的测量更新的计算参照2.2小节中的无迹卡尔曼滤波算法.
3 基于GRNN的模型修正虽然环境中选取了多个路标，但在实际应用过程中难免会出现路标缺失的情况，视觉测量信息无效，此时，可以使用由之前的有效数据得到的回归模型直接对里程计原始数据进行修正.
3.1 回归问题直接根据机器人运动学得到的里程计中含有系统误差和随机误差，即：
|
(17) |
其中，
|
(18) |
但当视觉测量Z无效，如机器人视野中只有一个或没有路标时，滤波修正便不再适用.
为了解决这个问题，考察里程计先验数据
|
(19) |
在实时系统中，数据更新频率较快，在较短时间内系统误差γ (t)和随机误差η (t)基本保持不变，即在t变化较小时将里程计近似为时不变系统：
|
(20) |
修正问题转变为回归预测问题. 由于里程计数据包含机器人3个维度的状态，所以里程计修正是一个多输入多输出的非线性回归预测问题.
3.2 GRNN模型修正算法当视觉测量有效时，当前时刻的
广义回归神经网络(GRNN)[21]是径向基神经网络的一种，具有很强的非线性映射能力，而且在学习样本数据少时，预测效果很好，还可以处理不稳定数据，非常适合应用在嵌入式系统中处理这种实时性要求较高的回归问题.
GRNN具有4层网络结构，如图 9所示，分别是输入层、模式层、求和层和输出层，每一层神经元的数目与样本的维度和训练样本的数量有关. GRNN是一个前馈网络，不需要反向传播训练连接权重，是一个运算复杂度相对较小的网络.
|
| 图 9 GRNN网络结构 Fig.9 GRNN network structure |
输入层神经元的数目等于训练样本中输入向量的维数，在本模型中就是里程计原始数据
|
(21) |
即计算测试样本与训练样本中的每一个样本的高斯函数的取值；求和层由2个部分组成，第1部分只有一个神经元，是对所有模式层神经元的输出进行求和：
|
(22) |
第2部分神经元数目与输出向量维度一致，传递函数是对模式层神经元进行加权求和，权值为Xij，表示第i个训练样本Xi的第j个元素：
|
(23) |
输出层的神经元数目等于输出向量的维数，分别对应(x，y，θ)，传递函数为
|
(24) |
当视觉测量有效时，使用滤波算法对里程计进行修正，同时将数据作为训练样本更新到训练集中，为了保证系统近似时不变，训练集窗口n不宜过大，经过实验测试，n取500/v左右时较为合适，其中v为采样频率(单位Hz)，即保持训练集数据的时效为500 ms，超过这一时效的数据时容易引入异常点，导致模型的精度下降. 当视觉信息无效时，可直接利用训练集数据通过GRNN模型修正运动学里程计.
4 实验验证与结果分析为验证提出的仿人机器人高精度里程计的有效性和实际应用性能，本文以NAO仿人机器人为实验平台，在室内环境中设计了一系列仿真和实体实验.
4.1 机器人硬件配置本文实验对象是V5版NAO仿人机器人，其全身具有25个自由度，高56.4 cm，肩宽27.5 cm，质量5.4 kg. NAO的核心处理器型号为Intel ATOM Z530，主频1.6 GHz，RAM 1 GB，闪存2 GB，支持C + +和Python编程.
机器人的前额与下颚配有两个单目CMOS(complementary metal oxide semiconductor)相机，采集图像帧率最高可达30帧/s，具有72°的对角线视野范围，两个相机视野之间几乎没有重叠. NAO机器人每个关节处配备有磁旋转编码器用以测量关节角度，精度约为0.1°；还在脚底配置有压力传感器.
4.2 仿真实验验证本文首先在SimRobot仿真平台上进行了跟踪实验对本里程计的可行性和精度进行验证.
4.2.1 单组路标验证实验首先验证本算法的可行性并测试个模块精度，在仿真环境中摆放一组路标，机器人依靠该组路标和自身传感器进行定位，并完成小范围的路径跟踪任务. 在实验中，控制机器人的头部使路标始终保持在机器人视野中心.
图 10为在仿真平台上搭建的室内环境，在房间中心位置摆放两个小圆柱体作为环境标识，并建立图中所示坐标系作为世界坐标系. 环境标识的先验信息包括标识的色彩范围、形状尺寸和在世界坐标系中位置坐标.
|
| 图 10 单组路标路径跟踪实验仿真环境 Fig.10 Simulation environment of tracking experiment with one set of landmark |
实验中，机器人以最高0.2 m/s的速度行走. 规划路径为800 mm×800 mm的矩形，机器人位姿用(x，y，θ)表示，路径为(1 500，-400，180)T→(700，-400，180)T→(700，400，180)T→(1 500，400，180)T→(1 500，-400，180)T.
图 11、图 12分别为路径跟踪实验的各算法位姿估计结果和误差曲线图. 图 11(a)是由机器人关节角通过正运动学直接得到的行走里程计数据，结果表明，里程计数据在x轴和y轴方向上都存在一定的误差，且随运动距离的增大而增大，这是因为用里程计估计位姿是一个不断积分的过程，误差也会不断累积. 从图 11(b)可看出，机器人在行走过程中，由视觉数据得到的位姿测量在真实值的左右震荡，并且距离路标物体越远，震荡幅度越大. 这种现象再次证明仿人机器人双足行走这种移动方式对视觉定位有较大的影响. 里程计具有很好的局部性能，而视觉测量具有很好全局性，两者正好互补，通过无迹卡尔曼滤波用视觉测量来修正运动学里程计的累计误差，修正后的里程计位姿估计精度较高，如图 11(c)所示，机器人在该位姿的反馈下，顺利完成了图中矩形路径的跟踪任务. 图 11(d)为以修正前后里程计数据为训练集，直接使用GRNN模型对当前运动学里程计做修正后得到的结果. 从图 11可以看出，直接使用模型修正得到里程计结果与根据视觉测量的滤波修正的结果相比，精度略低，但在没有视觉信息的情况下同样有效的修正了运动学里程计的累计误差，说明在短时间内，如果视觉测量无效，可以使用模型来对运动学里程计进行修正.
|
| 图 11 单组路标路径跟踪实验结果 Fig.11 Results of one set landmarks path tracking experiment |
|
| 图 12 路径跟踪实验误差曲线 Fig.12 Errors curve of path tracking experiment |
表 1为实验绝对值误差数据的统计结果. 在1.2 m2×1.2 m2的工作空间中，运动学里程计数据在机器人位置估计上存在很大的误差，x和y方向上的误差均值分别为空间长度的7.94%和9.78%，y方向最大值为23.3 cm. 视觉测量在位置上的误差均值分别是整个空间长度的1.17%和2.43%，但是其最大值却达到了5.65%和16.65%，在y轴方向的标准差也较大. 机器人在运动过程中姿态角基本不变，所以其测量误差都较小. 滤波修正算法结合了运动学里程计和视觉测量的优势，各方面数据在三者中都是最优的，位置误差均值分别是空间长度的1.10%和1.07%，最大误差不到机器人肩宽的1/4，可以满足机器人在路径跟踪任务的精度需求. 另外，GRNN模型在没有视觉测量的情况下对运动学里程计进行修正后，里程计的误差均值减少了大约80%，误差的最大值和标准差也有了显著的下降.
| 算法 | 维度 | 均值 | 最大值 | 标准差 |
| 运动学里程计 | x /mm | 95.28 | 150.39 | 44.62 |
| y /mm | 117.40 | 232.96 | 84.87 | |
| θ /(°) | 1.20 | 3.62 | 1.23 | |
| 视觉测量 | x /mm | 14.01 | 67.85 | 8.64 |
| y /mm | 29.14 | 199.85 | 32.34 | |
| θ /(°) | 1.30 | 7.52 | 1.21 | |
| 滤波修正 | x /mm | 13.20 | 46.35 | 7.10 |
| y /mm | 12.83 | 60.63 | 12.37 | |
| θ /(°) | 0.61 | 3.04 | 0.49 | |
| 模型修正 | x /mm | 24.09 | 114.90 | 21.92 |
| y /mm | 20.23 | 93.49 | 19.45 | |
| θ /(°) | 1.35 | 4.60 | 1.07 |
单路标实验验证了本里程计算法的可行性，并证明了滤波修正后的里程计对机器人的位姿估计具有很高的精度，同时也证明了基于GRNN的模型修正方法能在短时间内直接有效修正里程计以填补视觉测量无效时的空缺.
4.2.2 多路标应用实验在实际应用中，由于机器人的工作空间较大，只依靠单组路标难以保证机器人在执行任务时路标始终在视野中，所以需要在工作环境中选取多个路标. 为验证机器人在多路标的室内环境中执行具体任务时，本文提出的高精度里程计算法能够为机器人提供有效的位姿反馈，设计了如图 13所示的实验：在室内环境中设置(选取)了7个均匀分布在房间四周的物体路标(M 1，M 2，…，M 7). 机器人起始位姿为门(4 000，800，180)T，需要执行的任务是，从起始位姿依次走到沙发(2 000，-800，210)T、床(-200，-400，180)T、书桌(-1 000，800，180)T、圆桌(1 000，800，0)T.
|
| 图 13 多路标仿真实验环境 Fig.13 Simulation environment of multiple landmarks experiment |
图 14为实验结果，其中橙色为机器人实际位姿，黑色为本里程计的位姿估计结果. 表 2是实验中的定位误差绝对值统计结果. 从实验结果上看，当机器人4 m×6 m的空间中执行任务时，高精度里程计对机器人位姿的估计与真实值之间的位置误差均值分别是2.8 cm和2.2 cm，占工作空间长度的0.47%和0.55%，位置误差的最大值分别是8.4 cm和9.4 cm，是机器人肩宽(27.5 cm)的1/3左右. 而姿态角误差均值为2.05°，最大角度误差不超过10°. 定位误差的标准差也比较小，说明算法的位姿估计相对比较稳定，不确定性小.
|
| 图 14 多路标应用实验结果 Fig.14 Experiment result of multiple landmarks application experiment |
| 维度 | 均值 | 最大值 | 标准差 |
| x /mm | 28.39 | 84.32 | 22.34 |
| y /mm | 21.67 | 94.06 | 17.46 |
| θ /(°) | 2.05 | 9.12 | 1.63 |
图 15为实验中3个维度定位误差的变化曲线，可以看到3个维度的误差变化趋势基本一致. 当姿态角误差较小时，如在18 s或30 s处，位置误差也在0附近且相对平稳；当姿态角误差较大，如在10 s或50 s处，两个方向的位置误差也较大. 这也说明了定位结果对路标坐标系方向的比较敏感. 因此，在路标组合时，选择视野中最远的路标以尽量减少路标定位误差给机器人姿态估计带来的影响.
|
| 图 15 多路标应用实验误差数据 Fig.15 Error data of multiple landmarks application experiment |
从表 3可以看出，整个算法中，最耗时的模块为视觉测量模块且其和滤波修正模块都与视野中存在的路标个数呈线性相关. 即使视野中同时存在6个路标，即视觉模块有6组测量结果，整个算法的总耗时也不超过10 ms. 实验结果表明，本算法在普通的处理器设备上依然具有很好的实时性.
在实验中，机器人依靠定位算法的位姿反馈，顺利完成了定点移动的任务.
4.2.3 实时性测试实验为了测试本算法的实时性，在仿真环境中，测量机器人在静止站立和行走时各个模块的平均耗时. 仿真实验在个人电脑上完成，配置为Intel i5-8265U处理器，8 G内存，无GPU. 实验模型修正模块n取15. 统计结果如表 3所示，单位为ms.
| 状态 | 视野路标数量 | 视觉测量模块/ms | 里程计模块/ms | 滤波修正模块/ms | 模型修正模块/ms | 总耗时/ms |
| 站立 | 2 | 1.427 | 1.724 | 0.696 | - | 3.847 |
| 4 | 2.857 | 1.826 | 1.352 | - | 6.035 | |
| 6 | 4.438 | 1.891 | 1.925 | - | 8.254 | |
| 0 | 0.916 | 1.751 | 0.007 | 0.029 | 2.703 | |
| 行走 | 2 | 1.581 | 1.820 | 0.676 | - | 4.077 |
| 4 | 3.066 | 1.773 | 1.292 | - | 6.131 | |
| 6 | 4.897 | 1.759 | 1.821 | - | 8.477 | |
| 0 | 1.046 | 1.809 | 0.009 | 0.032 | 2.896 |
仿真实验中验证了算法的可行性与有效性，本文在实体机器人上也设计了相应的实验，进一步验证所提出的高精度里程计在真实环境中的性能.
4.3.1 视觉测量实验首先验证视觉测量算法在实体环境中的有效性，实体实验环境如图 16所示，以玩偶和水果作为物体路标(M1~M4)，同仿真实验相同，事先需测量如表 1所示的路标物体先验信息. 地板上均匀分布的白色贴纸标记用于估计机器人的实际位置，标记之间的间隔为500 mm.
|
| 图 16 实体实验环境 Fig.16 Real experiment environment |
视觉测量实验中，机器人保持静止站立，手动将机器人摆放到固定位姿中，共20组实验，每组实验测量100次取均值作为测量结果. 在实体机器人上，机器人的采样周期为33 ms，视觉定位模块平均耗时6.49 ms，里程计模块平均耗时0.11 ms，滤波融合模块耗时1.34 ms，定位程序共耗时7.94 ms.
视觉测量实验的结果如图 17所示，图 18是测量误差在x方向和y方向上的均值与标准差. 从18图可以看出，视觉测量在x方向上的测量误差与不确定性随着与路标物体距离的增大而增大，因为标识中心的位置测量存在一定误差，这个误差对于定位结果的影响随着距离的增大而增大. 另外，y方向上视觉定位的测量不确定性比x方向大，测量误差也随着路标物体距离的增大而增大且估计位姿与真值相比都向路标物体中心靠拢. 实验误差统计结果如表 4所示，视觉测量在位置上误差的绝对值均值分别是2.6 cm和3.5 cm，实验的空间大小为2 m×2 m，所以误差占空间长度的1.3%和1.75%，视觉定位中，100次测量的标准差相对较大，说明纯视觉测量的不确定性较大，这点与仿真实验的结果相一致. 视觉测量实验结果表明，本文所提出视觉测量的相关算法可以应用在实体环境中，具有良好的精度和较好实时性.
|
| 图 17 实体视觉定位实验结果 Fig.17 Result of visual localization in real experiment |
|
| 图 18 测量误差均值与标准差 Fig.18 Mean and standard deviation of error measuring |
| 维度 | 均值 | 最大值 | 标准差 |
| x /mm | 26.23 | 116.26 | 32.02 |
| y /mm | 35.75 | 107.71 | 25.87 |
| θ /(°) | 0.036 | 0.107 | 0.026 |
RoboCup机器人世界杯是目前规模最大、影响力最大的机器人赛事之一，在标准平台组中，以NAO机器人为硬件平台，进行5V5的足球比赛. 在比赛过程中，由于机器人离线运行且环境动态多变，其精准的实时自定位一直是一个难题. 为进一步验证算法的实际应用性能，以机器人足球赛为背景，设计了以下定位、对球及踢球实验.
对球是指机器人在踢球前根据球的位置、踢球方向、距离调整自身位姿的过程. 在机器人足球赛中，对球的精度除了与轨迹规划和跟踪算法有关，很大程度上取决于机器人自定位的精度. 当踢球动作固定时，对球的好坏将直接决定踢球的结果. 因为缺乏能够直接实时测量机器人真实位姿的工具，所以本实验以最后球的落点来间接反映机器人对球的好坏及里程计定位的精度.
如图 19所示，在机器人足球赛场中，以开球点，中圈与中场线交点及罚球点作为物体路标，根据场地信息可以得到这些路标的先验知识. 在路标识别中，先对相机图像进行颜色分割，再进行线提取，根据先验路标特征完成路标的识别与定位. 进而再应用本算法可完成机器人的自定位. 以该定位结果为反馈，设计相应的轨迹规划与跟踪算法完成对球与踢球，并记录球的落点.
|
| 图 19 实体对球踢球实验环境 Fig.19 Environment of ball alignment experiment |
本文一共做了20组实验，其中10组中机器人从固定位姿(3 000，0，0)出发，另外10组中机器人从随机位姿出发. 对球踢球实验中，踢球结果如图 20和图 21所示. 图中蓝色点为将机器人摆放在理想位姿踢球时球的落点，红色为机器人自行对球踢球的结果. 从图 20和图 21可以看出机器人理想位姿下踢球的有效射程大约为4 m，机器人根据位姿反馈自行对球踢球时球的落点大多数都在理想落点半径1 m以内，经人工测量，实验中机器人实际对球位姿误差不超过2 cm，满足比赛中正常的解围、射门等需求，且对球的结果基本与机器人的起始位置无关.
在机器人足球赛中，机器人的自定位是其行为决策的基础，但其面临3个难题：1) 比赛用机器人处理器运算能力有限；2) 机器人自身的运动速度尤其头部运动较快，易产生运动模糊；3) 比赛时场地上动态“障碍物”较多，环境多变. 实验结果证明，本文所提出的算法实时性高、能够克服运动模糊和动态“障碍物”的影响且精度较高、鲁棒性强，满足机器人足球赛的应用需求，相比其他算法具有一定的优势.
|
| 图 20 对球踢球实验1结果 Fig.20 Result of alignment and kicking experiment 1 |
|
| 图 21 对球踢球实验2结果 Fig.21 Result of alignment and kicking experiment 2 |
本文提出了一种基于环境物体路标的仿人机器人实时的高精度里程计，通过无迹卡尔曼滤波算法根据单目视觉测量结果对仿人机器人运动学里程计进行修正，消除累计误差. 同时，为提高算法的鲁棒性，提出了多路标的组合测量及修正方法，及在视觉测量无效时，基于广义回归神经网络模型的直接修正方法. 仿真实验结果表明，机器人在4 m×6 m的室内空间中执行任务时，本里程计平均误差不超过2.8 cm，仅是机器人肩宽的1/10. 在实体实验中，机器人静止状态下，视觉测量平均误差约占整个测试空间的1.7%；运动实验中，机器人能够在本里程计的反馈下顺利完成路径跟踪任务和机器人足球赛中的精准对球任务，并且本里程计能够很好地满足机器人的实时性要求. 仿真和实体实验结果证明，本文所提出的高精度里程计算法具有较高的精度和实时性，能够有效地解决仿人机器人在室内环境中的自主定位问题.
为了进一步提高算法的实用性和定位精度，后续将开展以下工作：1) 研究基于更多特征的图像分割算法以提高路标识别与定位的准确性，提高视觉测量对不同环境的鲁棒性；2) 利用惯性传感器进一步提高里程计的估计精度；3) 将机器人在行走过程中身体晃动的影响考虑进位姿估计模型当中，进一步提高测量精度.
| [1] | Dogruer C U. Estimation of odometer parameters with MMAE and LSE[C]//International Conference on Advanced Intelligent Mechatronics. Piscataway, NJ, USA: IEEE, 2014: 1728-1733. |
| [2] | Dang Y, Wang H, Xu W. Calibration of iRobot create using UMBmark[C]//IEEE International Workshop on Advanced Motion Control. Piscataway, NJ, USA: IEEE, 2016: 97-102. |
| [3] | Antonelli G, Chiaverini S, Fusco G. A calibration method for odometry of mobile robots based on the least-squares technique: Theory and experimental validation[J]. IEEE Transactions on Robotics, 2005, 21(5): 994–1004. DOI:10.1109/TRO.2005.851382 |
| [4] | Doh N L, Choset H, Chung W K. Relative localization using path odometry information[J]. Autonomous Robots, 2006, 21(2): 143–154. DOI:10.1007/s10514-006-6474-8 |
| [5] | Xu H, Collins J J. Estimating the odometry error of a mobile robot by neural networks[C]//International Conference on Machine Learning and Applications. Piscataway, NJ, USA: IEEE, 2009: 378-385. |
| [6] | Piñeiro T J, Jose D, Arnay R, et al. Improving odometric accuracy for an autonomous electric cart[J]. Sensors, 2018, 18(2). DOI:10.3390/s180/0200 |
| [7] | Yun Y, Park B, Chung W K. Odometry calibration using home positioning function for mobile robot[C]//IEEE International Conference on Robotics and Automation. Piscataway, NJ, USA: IEEE, 2008: 2116-2121. |
| [8] |
梶田秀司.
仿人机器人[M]. 北京: 清华大学出版社, 2007: 23-25.
Shuuji K. Humanoid robotics[M]. Beijing: Tsinghua University Press, 2007: 23-25. |
| [9] | Murartal R, Montiel J M, Tardos J D, et al. ORB-SLAM: A versatile and accurate monocular SLAM system[J]. IEEE Transactions on Robotics, 2015, 31(5): 1147–1163. DOI:10.1109/TRO.2015.2463671 |
| [10] | Forster C, Pizzoli M, Scaramuzza D, et al. SVO: Fast semi-direct monocular visual odometry[C]//International Conference on Robotics and Automation. Piscataway, NJ, USA: IEEE, 2014: 15-22. |
| [11] | Leutenegger S, Lynen S, Bosse M, et al. Keyframe-based visual-inertial odometry using nonlinear optimization[J]. The International Journal of Robotics Research, 2014, 34(3): 314–334. |
| [12] | Mur-Artal R, Tardós J D. Visual-inertial monocular SLAM with map reuse[J]. IEEE Robotics and Automation Letters, 2017, 2(2): 796–803. DOI:10.1109/LRA.2017.2653359 |
| [13] | Tong Q, Peiliang L, Shaojie S. VINS-Mono: A robust and versatile monocular visual-inertial state estimator[J]. IEEE Transactions on Robotics, 2018, 34(4): 1004–1020. DOI:10.1109/TRO.2018.2853729 |
| [14] | Oriolo G, Paolillo A, Rosa L, et al. Humanoid odometric localization integrating kinematic, inertial and visual information[J]. Autonomous Robots, 2016, 40(5): 867–879. DOI:10.1007/s10514-015-9498-0 |
| [15] |
王立玲, 梁亮, 马东, 等.
基于多传感器信息融合的双足机器人自主定位[J]. 中国惯性技术学报, 2018, 26(5): 75–80.
Wang L L, Liang L, Ma D, et al. Autonomous positioning of bipedal robot based on multi-sensor information fusion[J]. China Journal of Inertial Technology, 2018, 26(5): 75–80. |
| [16] |
王丹, 黄鲁, 李垚.
基于点线特征的单目视觉同时定位与地图构建算法[J]. 机器人, 2019, 41(3): 392–403.
Wang D, Huang L, Li Y. Monocular vision simultaneous location and map building algorithm based on point and line features[J]. Robot, 2019, 41(3): 392–403. |
| [17] | Yang S, Scherer S. Monocular object and plane SLAM in structured environments[J]. IEEE Robotics and Automation Letters, 2019, 4(4): 3145–3152. DOI:10.1109/LRA.2019.2924848 |
| [18] | Tarek H, Claude S. Riccati observers for the non-stationary PnP problem[J]. IEEE Transactions on Automatic Control, 2018, 63(6): 726–741. |
| [19] | Lepetit V, Moreno-Noguer F, Fua P. EPnP: An accurate O(n) solution to the PnP problem[J]. International Journal of Computer Vision, 2009, 81(2): 155. DOI:10.1007/s11263-008-0152-6 |
| [20] | Hartmann G, Huang F, Klette R. Landmark initialization for unscented Kalman filter sensor fusion in monocular camera localization[J]. International Journal of Fuzzy Logic & Intelligent Systems, 2013, 13(1): 1–10. |
| [21] | Chasset P O. GRNN: General regression neural network[J]. Revue de Physique Appliquée, 2013, 4(6): 1321–1325. |