基于多传感器信息融合的双足机器人自主定位

王立玲，梁 亮，马 东，王洪瑞，刘秀玲

（1.河北大学 电子信息工程学院，保定 071002；2.河北省数字医疗工程重点实验室，保定 071002）

在现今高效率高智能化的社会中，智能服务型机器人相关的技术研究一直是科学技术的前沿领域，尤其在室内环境下实现精确的定位，是服务型机器人融入日常工作生活、执行复杂任务的先决条件。其中双足仿人机器人具有高自由度的优点，相比轮式履带式机器人平台，可以实现在不同地面、多层次复杂的室内环境下自主移动。然而，双足机器人摇晃的行走方式会造成相机采集的图像模糊，以及快速地移动和旋转，均会导致基于视觉的同步定位与地图构建（SLAM）定位失败。脚下的滑动和关节电机齿轮之间的传递误差，也会导致惯性测量单元（IMU）和基于电机编码器计算出的位置信息累积误差较大。

目前国内外对于双足机器人室内定位导航的主要解决方案是依靠安装在机器人身上的视觉传感器或距离传感器作为数据来源。Hornung等人[1]应用蒙特卡罗定位法，并整合2D激光传感器的测量数据和关节编码器的数据，在3D地图中追踪NAO机器人的6D位姿。在后续研究中又整合深度相机数据到观察模型[2]，维护八叉树表示的3D动态地图，用于实时路径规划，然而其需要非常准确的初始静态地图。Nefti-Meziani等人[3]提出在NAO上搭建双目视觉来恢复深度估计，使用OpenCV库和OpenGL库分别进行特征点检测和3D地图重构，但并没有提供机器人定位导航所需的位姿计算。Chandra等人[4]则基于双目视觉，使用OpenCV库实现NAO机器人位姿估计。另外也常用扩展卡尔曼滤波（EKF）来融合多个传感器数据，如Oriolo等人[5]通过差分运动学计算来异步更新EKF预测模型，并整合并行跟踪和映射（PTAM）视觉位姿估计和IMU数据来执行EKF的更新步骤。同样Xinjilefu等人[6]把运动学计算应用于基于稳态卡尔曼滤波器求解状态向量来估计Atlas双足机器人实时位姿。冯国虎等人[7]采用卡尔曼滤波实现单目视觉和惯导系统时间同步，以二者的速度差作为观察量校正系统误差，并同时估计单目尺度因子误差。

综合上述研究，本文采用成本低廉且便携的单目相机，由相较于文献[5]中的PTAM具有更高精度和稳定性的单目ORB-SLAM系统估计相机位姿[8]。经绝对尺度恢复和坐标系校正后输出位置信息，差分后通过扩展卡尔曼滤波算法与由基于编码器数据计算的位置信息和IMU数据融合，在视觉SLAM失效时，仍然能够提供可靠的位姿估计，并且解决了编码器和IMU精度低、累积误差大的问题。从而实现双足机器人在未知室内环境下，不使用额外的如激光、深度相机等传感器，进行更加精确稳定的自主定位。本系统不局限于特定的机器人类型，具有可扩展性。

1 单目ORB-SLAM中的位姿估计

由 R.Mur-Artal等人[9]提出的基于特征识别的单目 ORB-SLAM 系统，增加了闭环检测，统一了特征类型，相较于其他单目SLAM系统具有很高的精度和运行效率[10]。

ORB-SLAM分为三个并行线程：Tracking、Local Mapping和Loop Closing。Tracking线程提供每个新图像帧的相机位姿估计，同时还负责关键帧的筛选。Tracking线程流程图如图1所示。

图1 Tracking线程流程图Fig.1 Flowchart of tracking processes

具体实现过程如下：

首先对相机内参及畸变参数进行标定，从当前图像帧中提取ORB（ORiented BRIEF）特征点。词袋模型（Bag of Words，BoW）是指构建一个词汇树，对二进制描述符空间进行离散化，使用该树加速几何验证的对应关系，基于BoW对ORB特征描述符进行分组，加速后续特征匹配过程。

初始化相机位姿：根据场景中是否出现明显视差，分为非平面模型和平面模型，由启发式来选择所适应的模型。如果为非平面模型，则采用基于RANSAC（Random Sample Consensus）方案的归一化直接线性变换（DTL）算法和八点法来求解基础矩阵Fcr；如果为平面模型，并行地以同样的方法计算单应矩阵 Hcr，如式(1)(2)所示：

式中，xc、xr分别为当前帧与初始帧匹配成功的特征点。

由基础矩阵 Fcr和单应矩阵 Hcr来恢复相机位姿，得到初始两帧图像之间的相对位移以及初始点云地图，相机标定矩阵为K，平面法向量为n，旋转矩阵和平移向量分别表示为 Rcr、tcr，公式如下：

其中，引入∧符号，表示从向量到反对称矩阵的运算：

特征匹配期间使用恒速模型（即旋转矩阵和平移向量与上一帧相同）跟踪对应的特征点，每个特征点与投影到下一帧中的附近位置的特征相匹配，避免了在每一帧图像中检测和提取特征点，加速匹配过程。如果恒速模型失效，则在参考帧匹配模式下，基于BoW搜索当前帧和关键帧之间的关键点匹配关系，再利用匹配到的特征点来确定位姿。如果依然失效，则进入全局重定位模式，根据BoW相似性分数筛选若干关键帧作为候选帧，同上述方法搜索匹配点集，利用 RANSAC迭代EPnP（Perspective-n-Point）算法求解相机位姿。

系统获取初始位姿后不断地采用光束平差法对所估计位姿实时地进行局部优化，并且在存储的关键帧和关键点中搜索可能的重定位与闭环检测，从而进行全局优化。

将机器人头部单目相机坐标系记为 c系，文中使用下标()c·表示，机器人躯干中心载体坐标系记为b系，文中使用下标()b·表示，相机与躯干为固定空间位置关系，通过标定获得，之间的相对位姿表示为变换矩阵。选取初始时刻的载体坐标系为导航坐标系，记为w系，文中以下标(·)w表示。ORB-SLAM 估计的相机位姿是以初始化成功时的初始帧相机坐标系为参考坐标系，记此参考坐标系为cw系，文中以下标(·)cw表示。SLAM系统输出相机位姿的变换矩阵表示为。为校正单目视觉参考坐标系与导航坐标系之间的偏差，将转换到导航坐标系下得到，校正算法如下：

则经过坐标系校正后得到ORB-SLAM输出的在导航坐标系下的位姿估计。同时单目视觉存在无法确定绝对尺度因子的问题，本文利用机器人向前行进过程中，分别获得固定时间间隔内视觉SLAM估算的移动距离以及由编码器计算得出的移动距离xb，从而实时估计尺度因子实现单目视觉尺度恢复。

2 IMU及由编码器计算位置信息

NAO是由法国 Aldebaran公司研发的双足机器人，本文基于NAO V5版本。其配备相机、IMU、声呐等多个传感器，全身拥有25个自由度，支持多种开发语言，具有高度的可编程性和智能性。头部装有两个CMOS相机，分别位于额头和下颚位置，可以采集高达30fps 1280×960的图像，视野角度为72.6°DFOV，便于应用机器人视觉的相关技术。通过自身操作系统NAOqi的API接口可以调用惯性传感器及关节磁旋转编码器等传感器的数据。NAO机器人还支持基于ROS（Robot Operating System）开发。ROS是开源的机器人操作和控制系统软件开发框架，节点之间通过主题和服务通信，遵循发布者/订阅者协议，还提供图形化模拟环境RViz等工具和库，可以轻松实现功能包扩展复用，而不拘于特定的机器人平台，为其开发和交互提供了一个公共平台。

NAO机器人的IMU包含一个三轴陀螺仪和一个三轴加速度计，位于机器人的躯干中心。三轴陀螺仪精度为 20 (°)/s ，测量范围约为±500 (°)/s，三轴加速度计精度为0.02g(g为重力加速度)，测量范围约为±2 g 。惯性单元处理器Aldebaran Robotics算法是通过整合加速度计的数据来较正陀螺仪的角度误差，完成机器人躯干方向的计算。默认IMU坐标系与载体坐标系重合，由于加速度数据噪声较大无法使用，所以由IMU提供机器人的偏航角θ及其角速度ω的测量。

NAOqi API getPosition获取关节编码器的数据，并基于机器人的 3D运动学模型求解得出机器人的位姿信息。机器人所有关节的编码器数据以向量形式表示为然后通过s构建函数关系可以求解出 NAO机器人在导航坐标系下的位置及姿态，即变换矩阵其中姿态数据累积误差较大，使用位置估计作为位置测量

3 基于扩展卡尔曼滤波的多信息融合

针对依据单一传感器信息估计机器人位姿存在精度和稳定性等问题，本文提出了基于扩展卡尔曼滤波以松耦合的方式融合单目视觉、编码器计算的位置估计和IMU多传感器信息的方法，系统整体框图如图2所示。

图2 多信息融合算法整体框图Fig.2 Overall diagram of multi-information fusion algorithm

系统方程以及量测方程为：

其中，wk-1表示协方差为Q的过程噪声，vk表示协方差为R的观测噪声，为互不相关的零均值高斯白噪声。

位置、速度以及偏航测量对应的观测噪声协方差如下：

其中，Rp对角线元素单位均为m2，Rv对角线元素单位均为(m/s)2，Rq对角线元素单位分别为rad2、(rad/s)2。

1）系统状态初始化

首先，当系统接收到第一个测量值，对系统状态量和状态协方差进行初始化，得到初始状态量和初始系统状态协方差如下：

其中：P0矩阵对角线元素单位分别m2、m2、(m/s)2、(m/s)2、rad2、(rad/s)2。

2）预测过程

式中，xk-1为上一时刻系统状态量。Δt为当前时刻与上一时刻之间的采样时间间隔，状态转移矩阵Φ（k,k-1）由牛顿力学运动学模型得出：

由状态转移函数一阶泰勒展开式得到雅克比矩阵F，通过F和过程噪声协方差Q预测当前系统状态协方差。

3）更新过程

在更新过程中，系统增益矩阵K由观测矩阵H、观测噪声协方差R和预测的Pk来计算更新，然后根据K更新系统状态量xk以及系统状态协方差矩阵 Pk，如式(18)～(20)所示：

其中，对应测量模型得到观测矩阵H，将H矩阵中测量值与状态向量对应的子矩阵赋值为单位矩阵。

4 实验验证与分析

4.1 实验设计

实验硬件平台为双足机器人NAO。采用Nature-Point公司的高速动作捕捉系统Motive，记录机器人头部mark点的移动轨迹为真值。在ROS Kinetic软件开发平台下，以Python和C++为开发语言。实验过程中NAO机器人平台及单目视觉ORB-SLAM定位与地图构建结果（以轨迹1为例）如图3所示。

调用NAOqi API moveto使NAO分别以以下三种轨迹行走，这三种行走轨迹可以模拟双足机器人在室内全部的基本原始运动[11]：

1）沿x方向（导航坐标系下）长1 m的直线；

2）x方向长1 m，y方向宽0.5 m的矩形；

3）目标点坐标(0, 0.8)，偏航角0°，半径为0.2 m的Dubins曲线，Dubins曲线是指平面内满足最小转弯半径约束，由直线和圆弧段组成的两个位姿矢量间的最短路径。

4.2 实验结果与分析

在室内环境下控制NAO机器人以4.1节描述的三种轨迹行走，并分别获取由编码器计算的位置数据（图中Robot odometry）、EKF融合结果以及动捕系统记录的机器人的真实位置，所得的实验结果移动轨迹对比如图4所示。

图4(a) 轨迹1实验结果对比图Fig.4(a) Comparison on experimental results of Trajectory 1

图4(b) 轨迹2实验结果对比图Fig.4(b) Comparison on experimental results of Trajectory 2

图4(c) 轨迹3实验结果对比图Fig.4(c) Comparison on experimental results of Trajectory 3

对比分析所得实验结果相对真值的y方向误差如图5所示。可见由编码器得出的位置数据误差不断累积，轨迹1中误差由0累积达到0.12 m，在轨迹2中出现0.41 m的较大误差，并且轨迹形状与真实轨迹不符，在轨迹3实验中，误差逐渐累积到0.2 m。而三个行走轨迹情景下扩展卡尔曼滤波融合所得的结果 y方向误差均低于0.12 m，与由编码器得出的位置数据相比，误差明显降低。

图5(a) 轨迹1中的误差曲线对比Fig.5(a) Comparison of error curves in Trajectory 1

图5(b) 轨迹2中的误差曲线对比Fig.5(b) Comparison of error curves in Trajectory 2

图5(c) 轨迹3中的误差曲线对比Fig.5(c) Comparison of error curves in Trajectory 3

为验证本文提出的方法对传统位姿传感器定位精度的提升性能，针对不同的传感器组合方案：由编码器计算出的位置、单目视觉、融合IMU与位置信息以及本文提出的EKF融合三种传感器方法。对比分析上述方案在三种行走轨迹下的均方根误差（RMSE），其中性能评价函数和 ex分别为x和 y方向上相对于真值的均方根误差。结果如表1所示，单位为m，可见位置信息与IMU融合后，定位精度得到一定提升，并且由于单目视觉的精确定位性能，EKF融合三种传感器信息后，均方根误差明显降低，均优于其他组合方案。

相机的快速旋转移动造成图像帧之间跟踪的特征点数量和质量快速变化，对于单目视觉 SLAM极具挑战性，并且视觉 SLAM严重依赖环境条件。为验证本文方法对上述单目视觉缺点的改进性能，在一般实验室环境及日常光照条件下进行了50次重复实验，单目视觉ORB-SLAM与EKF融合算法失效情况统计如表2所示。

表1 不同传感器组合方案均方根误差比较Tab.1 Comparison of RMSEs of different sensor combination schemes

由表2可知，ORB-SLAM在矩形和Dubins曲线转弯过程中出现多次跟踪失败，然而扩展卡尔曼滤波融合多信息算法在单目视觉失效时仍能够实现稳定可靠的位姿跟踪，相比于单目视觉，失效率至少降低(17−2)/17=88%。在单目视觉失效情况下，对比其估计的位姿与EKF融合结果如图6所示。

表2 EKF融合算法与单目视觉失效情况统计Tab.2 EKF filter fusion algorithm and monocular vision failure statistics

图6 单目视觉失效情况下的位姿估计与融合结果对比Fig.6 Comparison between visual pose estimation and fusion results under monocular visual failure

综上实验，所提出的基于扩展卡尔曼滤波融合多信息的自主定位方法，能够有效地抑制传统位姿传感器的误差累积，提升定位精度，改善单目视觉SLAM在双足机器人转弯过程中易失效的问题。

5 结 论

本文提出了一种应用于双足机器人平台的多传感器信息融合定位方法。通过扩展卡尔曼滤波融合ORB-SLAM的视觉位姿估计、由编码器数据计算的位置以及IMU偏航测量数据。以NAO机器人为研究平台设计了多组的实验，实验结果表明本文所提出的方法可以有效降低传统位姿传感器的累积误差，并且降低单目视觉SLAM的失效率。实现了在无外置传感器反馈下，对机器人进行更加精确稳定的定位，能够满足室内双足机器人自主定位的需求。