基于单目摄像头的人形机器人实时导航系统

王军华，魏衡华（中国科学技术大学　自动化系，安徽　合肥　230026）

基于单目摄像头的人形机器人实时导航系统

王军华，魏衡华
（中国科学技术大学自动化系，安徽合肥230026）

提出一种用于室内环境下人形机器人的实时导航系统。首先基于颜色特征，把单目图像的像素点分类为地面和障碍物，并对各个障碍物的像素点进行聚类，以选择出它们最底部的像素点作为特征点。然后基于SVM（Support Vector Machine）算法，构造摄像头中实际点和像素点坐标的映射关系，并用均值场理论辅助SVM的学习过程。根据映射关系和特征像素点，就可以估计出障碍物与机器人的距离。最后根据距离信息，在离散化的运动空间中，一步一步规划出机器人的行走路径。在人形机器人DARWIN上对提出的方法进行了实现，实验结果显示了该方法的有效性。

人形机器人；导航；颜色信息；支持向量机；路径规划

0　引言

近年来，研究人员对人形机器人开展了大量研究工作，并取得了巨大进步。人形机器人的机械结构使得它们更适合人类的生活环境，比如家里和办公室等室内环境。而人形机器人能自由行动的关键在于其自主导航能力。人形机器人的自主导航涉及很多基础研究领域，比如计算机视觉、人工智能、运动规划等。

目前，已有一些针对不同硬件平台的人形机器人的自主导航方法。陈卫东等人［1］将障碍物和目标的位置信息模糊化，建立模糊规则并对其求解以实现自主导航；CHESTNUTT J等人［2］为Honda ASIMO人形机器人设计了脚步规划器，基于有限的状态独立行为，使用A*算法搜索最优脚步序列；ZANNATHA J M等人［3］设计了人形足球机器人的视觉自定位系统，通过图像处理算法及几何分析，进行3D单目重建，估计机器人在球场内的位置和方向；WANG Y T等人［4］实现了人形机器人的定位和建图，其使用SURF进行图像特征检测和选择，并基于扩展的卡尔曼滤波继续图像深度计算和特征状态估计；BALTES J等人［5］以视觉信息作为反馈，使用粒子滤波对机器人定位，据此规划出机器人的行走路径。

前人的工作中较多采用的是双目视觉系统，或者配合声纳等设备，虽然这样能使测量和控制更加精准，但机器人的体积及质量往往较大。由于单目摄像头成本较低、结构简单，越来越多的机器人用其作为自主导航检测系统的传感器。本文针对配置一个摄像头的人形机器人以及在室内地面行走的任务，设计了一个实用的实时导航系统，此系统使机器人在实验环境中能够从当前位置到达预定的目的地。本文的工作为基于单目的人形机器人在更加复杂的环境中顺利行走奠定了基础。

1　系统概述

此导航系统分为3部分，分别为障碍物的检测、障碍物的定位和机器人路径规划。

检测障碍物时，基于颜色特征，把单目图像中的所有像素点分类为地面和障碍物。对属于障碍物的像素点进行聚类，以区分开不同的障碍物。选择每个障碍物最底部的像素点作为此障碍物的特征点，用于估计此障碍物和机器人之间的相对位置。

定位障碍物时，先用SVM（Support Vector Machine）算法构造摄像头中实际点和像素点坐标的映射关系，并用均值场理论来辅助SVM的学习过程。基于此映射关系，还原出特征像素点的3D信息，就可以估计出障碍物与机器人的距离。

根据机器人的行走特点，离散化机器人的运动空间，然后根据距离信息，一步一步规划出机器人的行走路径。

2　系统设计描述

2.1障碍物检测

机器人在自主运动的过程中，需要避开地面上的障碍物。可对机器人行走的室内地面环境进行一些合理假设：地面平坦且颜色一致；障碍物都在地面上没有悬空；障碍物的颜色与地面的颜色不同；障碍物的形状比较简单且有规则。

本文选择颜色特征作为识别障碍物的主要线索。使用机器人的摄像头对机器人行走的地面进行多次随机采样，以获取较完备的地面参考信息。将摄像头输出的RGB颜色空间的图像转换到HIS颜色空间，计算出所有地面采样图像的平均色调和强度直方图，并用简单均值滤波器进行滤波处理。

机器人行走时，用5×5高斯滤波器对输入图像进行滤波，以降低图像中的噪声。将过滤后的图像转换到HIS颜色空间。

将图像中的每个像素的色调、强度与参考数据相比较，如果满足以下两个条件之一，则该像素被判定属于障碍物：（1）该像素色调值在直方图中的频数低于某个阈值；（2）该像素强度值在直方图中的频数低于某个阈值。

基于前文所描述的方法，对图1（a）中地面上的圆柱体进行识别，识别结果如图1（b）为所示，此实验验证了障碍物检测方法的有效性。

图1　障碍物识别结果示例

2.2障碍物定位

识别出视野内地面上的障碍物后，还需要弄清障碍物和机器人的相对位置，才能进一步确定机器人的行走方向。根据前文的假设，由于障碍物都比较简单且有规则，可以选择障碍物最底部的点作为障碍物的特征点，用于度量障碍物和机器人的距离。

对单摄像头采集的2D图像，无法直接得到其中的像素点对应实际点的真实坐标，需要建立它们之间的映射关系。本文利用SVM［6］回归分析，求得摄像头模型参数。均值场理论［7］用于辅助SVM算法的学习过程。

本文采用针孔透视投影模型，假设真实点M（X，Y，Z）对应图像像素点m（x，y），用式（1）表示这种投影关系：

式（1）的图形化表示如图2所示，可以看出，3个输出分支之间存在耦合，因此无法对其单独优化。

图2　齐次变换的图形化表示

使用线性核的SVM回归规则表示输出为：

为克服回归输出之间存在的耦合问题，在优化支持向量回归机时，可以使用梯度下降法对伸缩因子s进行优化。

用放置在地面上的黑白相间的方格棋盘来构造训练数据集和测试数据集，以机器人在地面投影的中心为原点构造真实世界坐标系，如图3所示。选择方块的角作为特征点，用其中60个点的数据作为训练数据，另外20个点的数据作为测试数据。可以通过角点检测计算出所有特征点的像素坐标。对被选入训练集合的特征点，它们在坐标系O-xyz中的3D坐标为输入，它们所对应的像素点在图像平面上的像素坐标为输出。

图3　训练场景示意图

给定伸缩因子初始值后，就可以开始训练映射函数。用平均场方法优化图3中的回归分支，用梯度下降法更新伸缩因子，重复这两个过程直到式（4）达到最小。

训练结束后，投影矩阵的估计值为：

测试时，输入测试集中特征点的像素坐标，得到对应真实坐标的估计值，3D重建的准确度用均方根误差来衡量：

2.3路径规划

机器人的运动机制比较复杂，使得其路径规划也很困难，一种简单的处理办法就是离散化机器人的运动空间。针对机器人的机械结构及步态特点，本文为人形机器人设计了一种树形路径规划算法。对机器人的下一个位置，设计了5种不同的选择，如图4所示。

图4　运动空间离散化

对每个位置，由式（7）计算出一个值，作为选择下一个位置的依据：

其中，j∈｛1，2，…，5｝，dmax为距离的最大值，本文中取dmax=300cm。

计算出每个位置对应的fj后，再根据式（8）确定下一个位置的最优选择：

在图4中，此位置为m3。当机器人到达下一位置后，不断重复以上路径规划过程直到机器人走到目的地。

3　实验结果

本文以韩国ROBOTIS公司制造的小型人形机器人DARWIN为验证平台，对所提出的导航方法进行了实现。实验场景如图5（a）所示，地面上的红色小球为目标地点，行进的路径上有4个障碍物。

图5（a）、（b）分别为机器人在起点处的场景图和视野图，机器人看到预设的4个障碍和两个目标小球；图5（c）、（d）分别为机器人到达第一个目标地点处的场景图和视野图，机器人避开障碍物并准确到达目的地，此时视野中还有两个预设的障碍物以及一个目标小球；图5（e）、（f）为机器人到达第二个目标地点处的场景图和视野图，机器人依然成功避开障碍物并准确到达目的地，此时视野中无目标，机器人停止行走。

图5　机器人自主导航的快照

机器人顺利完成设定的避障导航任务，验证了本文所述方法的有效性。

4　结论

本文为小型人形机器人提出一种实用的实时导航系统。此系统包括3部分：（1）基于颜色特征的障碍物检测系统；（2）基于SVM的障碍物定位系统；（3）基于运动空间离散化的树形路径规划系统。障碍物检测系统通过区别障碍物和地面的颜色特征，能够比较准确地识别出在给定地面上的有别于地面颜色的障碍物，并选择出用于计算障碍物和机器人距离的特征点。经过训练的障碍物定位系统对测试数据表现良好，能够比较精确地根据图像中的像素坐标还原出对应实际点的真实坐标。树形路径规划方法将机器人的运动空间离散化为树形结构，能根据当前障碍物和机器人的相对位置关系，从当前位置节点的子节点中选择下一个行进位置，使机器人一步一步抵达目标位置。在DARWIN机器人上实现了整个系统，并通过实验验证，机器人顺利避开障碍物到达设定的目标位置，显示了本文方法的有效性。

［1］陈卫东，朱奇光.基于模糊算法的移动机器人路径规划［J］.电子学报，2011，39（4）：971-974，980.

［2］CHESTNUTT J，LAU M，CHEUNG G，et al.Footstep planning for the Honda ASIMO humanoid［C］.Proceedings of the 2005 IEEE International Conference on Robotics and Automation，ICRA 2005，IEEE，2005：629-634.

［3］ZANNATHAJM I，CISNEROSLIMÓN R，GÓMEZ SÁNCHEZ A D，et al.Monocular visual self-localization for humanoid soccer robots［C］.2011 21st International Conference on Electrical Communications and Computers，.IEEE，2011：100-107.

［4］WANG Y T，HUNG D Y，CHENG S H.Monocular SLAM for a small-size humanoid robot［J］.Tamkang Journal of Science and Engineering，2011，14（2）：123-129.

［5］BALTES J，CHENG C T，LAU M C.Real-time navigation fora humanoid robotusing particle filter［J］.Applied Mechanics and Materials，2013，284：1914-1918.

［6］VAPNIK V.The nature of statistical learning theory［M］.New York：Springer，1995.

［7］OPPER M，WINTHER O.Gaussian processes for classification：mean field algorithms［J］.Neural Computation，2000，12（11）：2655-2684.

Study on real-time navigation for humanoid robot with monocular

Wang Junhua，Wei Henghua
（Department of Automation，University of Science and Technology of China，Hefei 230026，China）

This paper presents a real-time navigation system.Based on color information，pixels from monocular image are classified into ground and obstacles，and pixels from the same obstacle are clustered to choose the bottom ones as features.Then SVM（Support Vector Machine）is used to establish a mapping between the real-world coordinates of points and their corresponding image pixels，and Field Mean theory is used to assist the learning procedure.Distances between obstacles and robot can be estimated according to the mapping and features.At last，the path is planned step by step in the discretized motion space with the distances information.The navigation method is implemented on humanoid robot DARWIN and the results of the experiment show its effectiveness.

humanoid robots；navigation；color information；SVM；path planning

TP391

A

1674-7720（2015）16-0049-03

王军华，魏衡华.基于单目摄像头的人形机器人实时导航系统［J］.微型机与应用，2015，34（16）：49-51，54.

2015-03-20）

王军华（1988-），男，硕士研究生，主要研究方向：控制理论与控制工程。

魏衡华（1955-），男，硕士，副教授，硕士生导师，主要研究方向：智能控制，伺服控制，运动控制。