刘海平，战强

(北京航空航天大学 a.中法工程师学院； b. 机械工程及自动化学院, 北京100191)

0 引言

随着机器人技术的快速发展，越来越多的智能家居走进人们的日常生活。但移动机器人在室内的定位与导航精度的问题仍然是推广家庭服务机器人所需解决的关键点之一。不同机器人平台因所采用传感器、处理器等硬件的多样性，需依靠开发者重复开发控制系统。与此同时，导航系统和相关软件却不可重复利用。其次，现代机器人系统开发的规模和复杂程度与日俱增，在这样的大环境下，WillowGarage公司于2010年公布了开源的机器人操作系统ROS(robotoperatingsystem)。

首先介绍ROS的基础概念以及导航相关的功能包；然后按照实验的研究步骤分别介绍了定位方法、机器人运动模型、卡尔曼滤波和机器人控制策略；进而论述实现定位的算法和相关函数；最后分析在Rovio移动机器人平台上所获得的测试数据和结果，论证研究方法的有效性和算法优化的思路。

1 机器人操作系统

机器人操作系统(ROS)是一套用于机器人设计与控制的操作系统。ROS不仅提供了丰富的操作功能，如底层设备控制、进程间的消息传递，还整合了主流的库函数，如OpenGL、OpenCV，并且拥有很多传感器的驱动程序，包括Kinect、激光测距仪等[1]。该系统可概括为以下2个特点：

1) 点对点的通讯方式：每个进程以节点(Node)的形式存在，并且节点可运行分布在网络各端口的多台计算机上。节点管理器负责存储所有节点的主题和服务信息[2]，并在两个节点间建立连接查询表。点对点的通讯方式可以提高移动机器人导航过程数据传输的效率。

2) 跨编程语言开发：软件主要使用C++和Python，同时兼容Java、Lisp等编程语言。ROS在编译时统一定义节点所传入传出的消息(Message)结构，并采用中立的接口转译给不同的开发语言。节点将消息发送到指定的话题(Topic)从而完成发布。若一个节点对某数据感兴趣，它只需订阅相关的话题即可。另外，节点间也可以通过服务(Service)实现请求和调用。

定位与导航的算法实现过程中使用了ROS提供的开源库函数：RVIZ可将机器人转化为可视的三维模型；TF是坐标转换库，提供系统中任一个点在坐标系之间坐标转换的查询和变换服务。

2 机器人导航技术和控制策略

移动机器人可以自行组织运行和自主规划，其基础在于定位精度和导航准确性。通过导航定位可获得机器人所在的位置、方向，再分析实验数据，建立运动模型，从而完成路径的规划和目标跟踪。

2.1 定位方法

在导航过程中，定位是实现自主能力的核心问题之一，其功能是获得机器人相对于坐标系的位置。机器人定位一般可划分为相对定位和绝对定位。

相对定位主要依靠内部传感器，包括里程计(odometry)、陀螺仪等。通过测量相对于机器人初始位姿的平面距离和转动方向，从而估计机器人位置。该方法计算量小[3]，但有累加特性。

绝对定位主要依靠环境标识、路标或匹配地图进行定位。绝对定位的精度高，但处理速度较慢，设备成本高。

2.2 机器人运动模型

机器人运动模型如图1所示，将机器人的运动分解为平移与旋转两种方式。假设机器人在第i时刻的位姿为{x(i),y(i),θ(i)}，以此为原点建立局部坐标系，以机器人的行进方向作为x轴。假设在下一时刻i+1，机器人相对时刻i的位姿变化量为：{Δx(i),Δy(i),Δθ(i)}，则根据坐标变换公式可知机器人在第i+1时刻的全局坐标系下的位姿{x(i+1),y(i+1),θ(i+1)}为：

图1 Rovio机器人运动模型

(1)

其中：{Δx(i),Δy(i),Δθ(i)}来自里程计的观测值，{Ωx(i),Ωy(i),Ωθ(i)}是传感器噪声。

2.3 卡尔曼滤波

卡尔曼滤波(kalmanfilter,KF)通过递归的方法，迭代更新估计值来减小误差[4]。KF的优点在于计算过程只需要上一时刻的估计值和当前时刻的观测值，极大地减少运算内存的消耗。

实验用一个添加了噪声干扰的马尔可夫链来表示卡尔曼滤波的动态过程[5](图2)。采用Rovio位姿作为状态向量X=(x y θ)，控制向量U=(v Ω)是Rovio的线速度和角速度，Z=(xdyd)是Rovio的坐标观测值。

图2 卡尔曼滤波器的结构示意图

实验是在规划路径下的定位与导航，所以假设整个系统是线性的，其KF的状态描述方程

X=A×X+B×U+ωk

(2)

A为状态变换矩阵，B为数据输入变换矩阵，ω为机器人运动过程的噪声元素，满足以下协方差矩阵Q(实验数据结果)：

ωk～N(0,Q)

(3)

测量方程

Z=H×X+vk

(4)

H为观测矩阵，v为测量噪声，满足以下协方差矩阵R(实验数据)：

vk～N(0,R)

(5)

(6)

(7)

然后，KF利用对当前时刻系统状态的观测值来更新在上一阶段的预测值，从而获得一个平均误差更小的估计值，测量更新过程如下：

(8)

(9)

PK=(I-KkHk)Rk|k-1

(10)

2.4 控制分层

结合上述运动模型和Rovio机器人实验平台，在机器人操作平台中机器人的控制策略可划分为以下3个层次：

1) 机器人的电机驱动部分：Rovio控制器接收ROS发送的命令，转化cmd_vel的速度为轮子的期望速度，对轮子分别进行PID控速。等时间间隔地读取电机码盘值，转化为万向轮速度再上传给Rovio控制器，并用航迹推演法计算Rovio当前的速度。

2)ROS与Rovio的通信部分：ROS发出速度指令至Rovio平台，Rovio将当前实际速度值发布到Odom主题，以提供给其他节点订阅。

3) 机器人的定位：决策层，具有不同的地图建模和定位导航策略。这一部分调用ROS相应的程序包，如move_base，AMCL，map_server等。

3 ROS平台上的定位算法实现

利用ROS分布式框架的特点，在Rovio机器人只设置惯性导航数据采集以及部分预处理的节点，在上位机完成定位、导航以及可视化的节点。上位机和Rovio之间各节点使用无线网络进行数据通信[6]。

3.1 导航规划层算法

参考ROS功能包move_base中的局部规划层和全局规划层，程序中创建了Nav_to_goal和Nav_points2个节点。

Nav_to_goal初步实现将机器人导航到目标点。算法假设当前位置与目标点之间没有障碍物。将机器人的速度和转向控制分为3个步骤：首先调整机器人位姿使之面向目标点；然后驱动机器人接近目标点，用插值法平滑运动过程的速度；到达目标之后旋转机器人朝向以满足下一步骤的准备。在这个过程，节点使用tf库的TransformStamped函数，将Rovio当前的位置转化为其在target_frame的坐标。

Nav_points提供根据规划的路径完成Rovio平台和ROS之间控制命令的传输。算法用geometry_msgs::PoseArray表示规划的路径轨迹，并进行RVIZ可视化处理，在模拟的几何地图上实时显示机器人的运动。

3.2 Rovio的检测与定位

实验室利用AxisPTZ相机，通过图像检测Rovio的像素重心，然后由坐标变换函数转化为摄像头姿态(pan,tilt)。通过支持向量机算法间接实现(pan,tilt)和固定坐标系(x,y)的转换。具体定位步骤如下：

1) 节点detect的作用为检测Rovio在图像中的位置：利用OpenCv将图片采集到ROS中，并根据颜色特征选择HSV值范围。然后算法对检测结果进行滤噪处理，计算检测所得到的闭环区域重心，作为目标的图像坐标。然后通过pantiltzoom方程结合摄像头当前的参数信息，将Rovio的像素坐标转化为(pan,tilt)坐标，也就是将目标置于摄像头图像中心(x/2, y/2)时，其对应摄像头的横纵转角。最后将当前摄像头检测到的所有机器人的坐标转化为(pan,tilt)数组，保存并发布到话题PanTilts。

2) 节点Tracker的作用为控制摄像头实时跟踪Rovio。该节点可读取话题PanTilts的信息并发布一系列控制摄像头移动的指令。Tracker提供3种模式，跟踪模式：摄像头实时跟踪机器人Rovio的移动，当监控图像中检测到多个机器人时，计算摄像头当前横纵角坐标与(pan,tilt)的最小差值；自动搜寻模式：控制摄像头按一定角度范围和增减规律遍历；扫描跟踪模式，摄像头先按照搜寻模式遍历，当检测到Rovio时切换为跟踪模式。

3) 节点sampler的作用为采集数据样本，通过机器学习得到(pan,tilt)和(x,y)之间的变换关系。算法引入支持向量机(supportvectormachines,SVM)，并构造最优化超平面从而获得x和y的预估方程。

x=pred1(pan,tilt)

(11)

y=pred2(pan,tilt)

通过观察节点Sampler所得到的数据集，其中数据的分布并非线性。SVM通过一个映射函数，将低维的输入空间Rn映射到高维的特征空间H。实现过程中，本文使用了径向基核函数(radialbasisfunction)，其中最常用的RBF为高斯核函数:

K(x,y)=exp(-y‖x-y‖2)

(12)

对于一个基于RBF核函数的SVM，其划分效果是由参数(C,γ)决定， C为惩罚因子，γ为核参数。实验中用k折交叉验证法来评估SVM优化结果的推广能力。

3.3 多目标的聚类与跟踪

当摄像头传递对应位置的点云信息时候，多目标的点云信息会形成有限个点云集合。聚类分析的目的是：经过图像检测的结果为点云图，从中提取一定数目标点来表示机器人所处位置。

为了结构化点云数据，选取聚类数目以及获得时间不变性的标注信息，算法在增长型神经元(growingneuralgas，GNG)模型基础上修改聚类数目判断的标准，从而避免了增长型神经网络出现网络过度冗余的情况，最终获得结构更为简单、泛化能力强、学习速度快、性能稳定的聚类算法。增长性神经网络可以根据信息处理的需要，实时改变神经网络的拓扑结构与神经元个数[7]。在GNG过程中，新增marker点引入的误差为：

(13)

(14)

由式(13)和式(14)可以推出

∀ω

(15)

将T的选择添加到GNG算法流程中的判断增长终止与否的条件，从而优化了GNG算法，提高聚类分析的速度。

4 实验结果

Rovio机器人是美国WowWee公司发布的一款wifi遥控的移动摄像机器人，配备有640×480的webcam摄像头以及声音采集和扩散设备，并具有完整的控制软件和开发工具。

4.1 卡尔曼滤波

实验测试过程中以视觉定位的初步聚类结果作为Rovio位姿的观测值，通过KF多次迭代后使位姿相比于处理前的数据更加精确，误差更小，如图3所示。从图3可以观察出Rovio的转角预测与真实值之间的差值由初始的剧烈波动到趋于稳定，证明了卡尔曼滤波对隐藏变量的预估作用。

图3 KF的预测值与真实值的对比图

4.2 SVM构造最优化超平面

实验通过采集样本来训练SVM以构造最优化超平面从而获得x和y的预测方程，如图4、图5所示。

图4 x坐标的预估方程

图5 y坐标的预估方程

在该实验中，算法使用RBF核函数，其中C=120，gamma=0.02。用10组真实数据交叉验证得到经验误差为x=0.012, y=0.009；真实误差值为ΔE= 1.21。

4.3 多目标的聚类

利用优化后的GNG算法对3个Rovio机器人进行检测和跟踪。相比原GNG算法，在ROS平台上虽然仍不能达到实时快速跟踪，但聚类速度有着明显的提升。实验结果认可了GNG算法通过对判断条件的改善可加快聚类速度的思路，如图6所示。

图6 优化后GNG算法的聚类效果

5 结语

文中基于机器人操作系统对室内机器人的定位和导航技术进行研究和实现。利用ROS提供的开源功能包，在Rovio平台实现定位导航算法。该算法避免传统方法所需要的大量计算，提高了定位和导航效率。实验结果证明了利用SVM研究摄像机定标问题的可行性。优化判断标准的GNG算法对数据点云聚类，在保证聚类效果的前提下，取得了速度上的提升。最后卡尔曼滤波的应用也得到了实验结果积极的反馈，下一步的工作可以研究融合多数据来源提高定位精度。