基于状态估计的服务机器人主动感知系统的研究*

袁 亮

(新疆大学 机械工程学院，乌鲁木齐市 830047)

基于状态估计的服务机器人主动感知系统的研究*

袁 亮

(新疆大学 机械工程学院，乌鲁木齐市 830047)

服务机器人在人机交互过程中，一个重要问题是如何在未知环境下确定人的位姿并能够快速准确的运动到人的面前。针对这一问题，文章设计了基于激光传感器的服务机器人主动式感知系统。在本系统中，服务机器人在没有先验信息的条件下，可以通过运动过程中检测、更新环境中的人物位姿来降低目标位置的不确定性，并且实时规划可以到达该人物正前方的最优路径。系统采用基于Adaboost的模板估计算法来估计人物位姿，采用基于不确定性预测的多目标优化算法来实现运动决策优化。在机器人轮椅平台上实现了该系统，并对系统的性能进行了评估。

服务机器人；主动感知；人物位姿估计；运动决策优化；不确定性推理

0 引言

人物感知和定位是服务型移动机器人研究领域的一项主要内容，是联系机器人和人的关键技术。早期的人物感知定位技术被较多地应用于服务机器人目标跟踪中。机器人往往是被动的接受环境信息，然后应用基于动态信息检测和跟踪滤波的方法对人物进行定位和跟踪[1]。基于动态信息的人物感知定位技术缺乏广泛适用性和稳定性，一旦环境信息变化或人物目标特征不完整，感知定位算法将丢失目标，机器人不能主动地寻找人物目标。这样的服务机器人是缺乏智能和自主性的。

目前，主动感知算法主要是解决机器人在非结构化的环境下，如何在主动获取环境信息来降低目标信息的不确定性的同时，尽快的完成任务。文献[2]较早提出了机器人主动感知问题，并且从概念上区分了主动感知和主动传感器的区别。主动传感器是一种传感器的工作原理，是传感器主动发送和采集环境数据。而机器人主动感知是一个系统问题，其中涉及传感器，目标优化，控制等问题。文献[3]总结了主动感知系统的系统框架和应用方法。主动感知系统主要包括三个部分，状态观测，状态滤波和决策优化。同时提出了将主动感知运用于机器人轨迹优化的问题中，通过对机器人运动进行建模，运用多目标规划和分析运动模型的协方差矩阵来达到使机器人快速准确到达目标位置。

文献[4]进一步扩展了主动感知问题的应用范围，将其应用到群机器人和传感器网络中去，使机器人在人机交互的过程中具备更高的智能性。当前，主动感知的算法和系统设计思想除了用于控制和规划外，更多地被应用于智能服务机器人领域，因为在这个领域，机器人对环境的智能感知能力往往要求更高。文献[5]介绍了综合多种传感器构建移动机器人的主动感知系统，使移动机器人可以综合多种感知手段观察和预测人的意图和行为。还介绍了如何运用主动感知来实时降低环境信息的不确定性和规划机器人的运动决策来达到最终的运动目标。

服务机器人能够顺利完成服务的前提就是如何准确的定位人物位置和姿态，并且规划出最优的运动轨迹。之前的研究主要将这两个问题分开处理[6-7]，即先利用传感器数据计算出人的位姿信息，然后离线规划计算最优轨迹。但由于忽略了观测位置和数据处理的不确定性，运动规划的结果存在较大偏差。基于状态估计的人物主动感知系统将传感器的静态感知能力和移动机器人的运动能力结合起来，实时同步地更新校准人物位姿信息来降低目标信息的不确定性和优化运动轨迹，使机器人可以在无任何人物位置先验信息的情况下，快速准确的到达人物的面前，进而完成后续的服务工作。

本文主要解决的问题是服务机器人如何在人机交互过程中主动快速的运动到人物面前。通过这项研究使服务机器人具备在复杂环境下能够自主地降低目标不确定性，检测和定位人的能力。本文提出了基于状态估计的人物主动感知系统。基于主动感知的思想，设计了新的人物位姿状态估计算法和运动决策优化算法。

1 人物主动感知系统整体设计

服务机器人对人的主动感知系统主要包含基于机器学习的人物位姿估计和运动决策优化等。主动感知系统的流程图如下图：

图1 主动感知系统流程图

基于机器学习的人物位姿估计部分首先检测并计算出人物的位置和姿态。运动决策部分利用人物位姿信息来预测机器人下一时刻不同的运动位置对降低人物姿态信息不确定性的影响，同时兼顾优化运动成本。人物位姿估计采用基于Adaboost的人物检测和基于模板匹配的位置姿态估计方法。运动决策优化中的人物姿态不确定性预测采用基于主观Bayes的不确定性估计，然后通过启发式和空间压缩算法来求解多目标优化函数。

2 人物位姿估计

基于激光测距仪的人物位姿估计算法主要包含人物检测和位姿计算两个部分。传统的人物检测方法主要基于固定特征的阈值判定方法[7]，不足是缺乏鲁棒性，参数需要经常调整。针对这些缺点，本文提出来基于机器学习的人物检测算法。该检测算法通过大量样本的学习可以对不同环境具有较好的适应性。对于符合人物特征的数据集，进一步运用基于模板的位姿计算方法。人物位姿估计的算法流程图如图2。

图2 人物位姿估计算法流程图

2.1 模板匹配

姿态估计算法首先将激光测距仪原始数据转化为距离直方图，然后从中提取出三种模板[11]，根据实验观察，这三种模板涵盖了人在激光测距仪前所有可能的成像。距离直方图是将点集中每个点的径向距离做成一张直方图。选择从距离直方图中提取人物姿态模板的原因是：

(1) 人相对于其背景环境是处于较突出的位置，人物位置更容易从背景环境中分离出来。

(2) 人的姿态在距离直方图中呈现出有限种模板，便于提取和分析。

根据实验观测，人物在激光测距仪前不同的站立姿态在距离直方图的中的成像可以分为三种模板：m1: 人侧身站立在机器人前;m2: 人双腿合并面对机器人站立;m3: 人双腿分开自然站立在机器人面前。

2.2 点集特征

人物检测的基本思想是从激光测距仪的原始数据中提取几何特征，然后依次做线性分类，这符合Adaboost 分类器的特点，所以利用Adaboost算法来选择最具区分度的特征和阈值。所有特征提取后，每个点集将拥有一个特征向量，该向量包含了一个点集的以上所有特征，所有点集的特征向量构成了Adaboost分类器的样本集。

通常，Adaboost中的弱分类器算法采用的是启发式优化算法来确定线性分类的阈值[10]，这类算法不足在于其解并非最优解，而且时间复杂度往往较高。为了克服以上缺点，提高人物检测算法的分类准确率和算法效率，本文设计了基于动态规划的弱分类器训练算法，可以得到理论上的最优阈值，而且有确定的时间复杂度下界。

变量说明：

rigPos=PosD，lefNeg= 0，

rigNeg=NegD，lefPos= 0

Fori=0...N

ifsi=1lefPos+=Distri,rigPos-=Distri

elsesi=-1rigNeg-=Distri,lefNeg+=Distr

if(max(lefPos+rigNeg,lefNeg+rigPos)>sumD)

sumD=max(lefPos+rigNeg,lefNeg+rigPos)，

其中N是所有样本的个数。同时记录当前的阈值划分位置循环结束，得到最优的线性分类阈值，使正确划分的样本权重最大。该算法可以将弱分类器的训练时间降低到O(nlogn)的复杂度，而且得到的是该维特征上最优的分类阈值。这样训练出的若干个弱分类器级联起来就组成了Adaboost强分类器。每一组由距离直方图中提取出的模板点集可以用该分类器判断其是否符合人物特征。

2.3 位姿估计

对于符合人物特征的点集，位姿估计的方法是利用模板数据的特征来估算人物的位姿。三种不同模板的位姿计算方法如下：

(1)m1：当人侧身站立在激光传感器前，该区间的距离直方图可以较明显的分为两个子集subS1和subS2，分别代表人物的双腿，各自包含的激光点的个数为numS1，numS2。提取两个子集的中心点坐标(x1,y1)和(x2,y2)，人物的位置和姿态计算方法如下，

(1)

(2)m2：当人双腿合并面对机器人站立，此时人的位置为该点集setM2的中心坐标，朝向和激光测距仪的径向平行，位置和姿态的计算方法为：

(2)

(3)m3：当人双腿分开自然站立在机器人面前，模板区间两条腿的特征明显，可以划分为两个子集，位姿计算方法同模板m1。

至此已完成了人物位姿估计系统的全部设计，该模块返回的结果是激光测距仪中提取出的人物位置，姿态和模板匹配类型。

3 运动决策优化

这里主要介绍主动感知系统得到人物的位置和姿态信息后，如何规划机器人的运动决策。主动感知利用观测对象即人的位姿估计算法为运动决策优化提供实时人物位姿信息，运动决策优化部分的算法流程图如图3，本章首先介绍多目标优化函数，然后分析优化函数中的目标信息不确定性和运动成本函数，最后介绍解空间搜索算法。

图3 运动决策优化算法流程图

3.1 多目标优化函数

主动感知的主要任务是在运动目标位置信息不完整的情况下，运动过程中如何兼顾快速到达目标和收集更多状态观测数据来降低目标信息不确定性，实时修正运动决策两个目标。

运动决策优化的目标函数为：

(3)

3.2 目标不确定性估计和运动成本分析

图4 机器人在不同位置的人物感知示意图

3.2.1 人物位姿不确定性表示

人物位姿不确定性包括位置不确定性upos和姿态不确定性uori。人物位置不确定性的表示如图4人物两腿间的阴影区域，在i观测时刻，人物位置估计存在一个疑似区域OSi，真实环境中人物的准确位置一定落在OSi区域内。机器人在不同时刻的OSi互相重合和叠加，可以不断缩小疑似区域，提高人物位置估计的精度。位置不确定性upos的表示：

(4)

upos表示为疑似区域的长宽比。双腿间直线距离在机器人不同观测位置局部坐标系内保持不变，所以upos值越趋近于零，人物疑似位置区域OSi在激光测距仪径向上的分量越小，水平分量计算出的人物位置的结果越精确。

人物姿态不确定性的表示首先利用实验统计数据来构造了人物姿态估计数据中的两个概率分布模型。

图5 p(θ)和p(Error)的概率分布

图5a表示人物检测算法计算出的姿态数据||θ||在(-90°～+90°)的概率密度函数p(θ)，可以看出该位姿估计算法在θ成(-40°～+40°)时性能较好。角度越大，在距离直方图中提取三种估计模板的难度越大，进而造成估算得到的姿态角的精度也越小。

图5b说明了姿态估计的误差Error的概率密度p(Error)，累计概率85%以上的误差分布在(0°～40°)内。姿态角的计算和对应的模板是有关联的，每种模板在距离直方图中的几何形状不同，不同特征提取的精度不同，所以其计算出的姿态角存在相应的概率分布 。姿态估计模块计算出的结果存在不确定性，表现在姿态角的误差和概率分布两方面。

(5)

其中Maxerr为归一化处理。P(μi|err)很难从大量实验数据中构造出模型，μi和当前人物姿态的估计值有关，当前的估计值θ也是存在不确定性的。所以目前的问题模型是在当前时刻存在不确定性的姿态估计值θ的基础上预测执行运动决策ai后人物姿态的不确性。

3.2.2 基于主观Bayes决策的人物姿态不确定性预测

本节将介绍应用主观Bayes决策方法来计算uori函数，应用主观Bayes方法的原因是因为其能够解决上文的两个问题：

(1)姿态估计中的条件概率p(mi|θ)，p(μi|err)等模型很难从实验和数据中建立出来。

(2)观测值，即人物姿态估计值θ存在不确定性，观测数据的不确定性会传递给μi和P(err|μi)的计算。

主观Bayes方法主要解决Bayes估计中难以取得的条件概率和证据不确定性的传递问题。

对于每一个人物姿态估计值θ，其对应的模板mi是确定的，利用主观Bayes方法，

(6)

LS1：为经验参数，代表mi对θ的支持程度。

(7)

由于θ存在不确定性，作为p(θ|mi)的观测值mi是确定的，则p(err|μi)可以利用p(θ|mi)=1 和p(θ|mi)=p(θ)两个特殊点的做线性插值，再结合计算得出。至此，姿态估计不确定性指标可以通过p(θ|mi)，p(err|f(μi))和主动Bayes不确定性推理方法计算得出。

3.2.3 运动成本分析

(8)

3.3 解空间搜索

离散运动决策优化目标函数的解空间为二维平面。首先利用离散化的思想，考虑交龙移动机器人在运动控制上的特性，将机器人当前位置的临域范围离散化为若干栅格，每个栅格为机器人运动可控的最小单位面积，同时将主动感知的每一步运动决策优化限定在一定范围内。但朴素的搜索算法在这样的解空间内，时间和空间复杂度上仍较大，所以需要设计一个高效的最优化方法。

目标函数的优化算法将利用分支限界和启发式搜索的思想。分支限界的节点扩展方程如式：

x(k)=rsin(k*β)

y(k)=rcos(k*β)，n*r≤maxr

(9)

式中k代表启发式搜索的方向，r代表每次运动决策中搜索算法的步长，这样将决策变量降维到目标位置的方向。maxr为一个规划周期内解空间的界限。

启发式搜索的两个启发函数定义为：

fup(k)=uori+upos+(x(k)-maxr)2+(y(k)-maxr)2

(10)

fdown(k)=step*r/maxr

(11)

其中step：当前搜索的层数。每一步运动优化决策，可以通过以上方法计算出最优决策。而且运动决策优化的目标函数是离散解空间上的目标函数，符合拟阵的传递性和可扩展性，整个目标函数适用于Greedy规则，可以证明每一步的最优决策可以保证运动决策优化目标函数的整体最优。

4 实验和分析

本文以机器人轮椅为平台，主动感知系统的评估实验主要包括两部分，首先是评估基于机器学习的人物检测算法，其次是评估整个主动感知系统在运动决策优化和降低目标信息不确定性方面的优越性。

4.1 人物检测算法评估实验

在办公室和家居环境下，各采集600帧激光测距仪数据。两种不同背景环境下的样本集可以提高训练后分类算法的适用范围，降低其误识别率。在数据采集过程中，环境中有静止站立的人，也有走动的人，这样保证采集的样本可以包含各种情况下的人物特征，提高训练后分类算法的准确率。

两种环境下共1200帧数据，经过点集分割后得到15437个点集片段。对所有这些点集片段做特征提取处理后构成了样本库，其中正样本874个。将样本库划分为训练样本和测试样本两个部分，样本数量分别为10034个和5400个，训练样本中的正样本数为435个。

上文中特征提取部分对每个点集片段提取8个特征，所以Adaboost分类器的训练迭代次数设置为16次，保证每一维特征都可以得到训练。训练结束后，使用测试样本对检测算法进行了测试，得到的结果如表2所示。

表1 基于Adaboost的检测算法对测试样本的计算结果

从表1中数据可以看出，基于机器学习的人物检测算法可以达到97%的正确识别率，误识别率也可以控制在1%左右。接下来将比较基于机器学习的人物检测算法和传统的固定特征的线性分类算法。两种不同思路算法的比较将采用相同的样本集，相同的8种点集特征，训练和测试样本，对比分类效果如下表。

表2 固定特征线性分类算法对测试样本的计算结果

从表1与表2的比较中可以看出，无论正样本的识别率还是负样本的误识别率，基于机器学习的人物检测算法都比传统的固定特征线性分类算法表现出更好的性能。

4.2 主动感知系统实验

实验的环境设置为在全局坐标系下，人的初始位置为(0.2, 5.1)，单位m，朝向-101.7°。机器人的初始位置为(3.1, 0.3)，单位m，朝向89.3°。主要进行三个实验，第一个实验测试了装有主动感知系统的机器人的运动轨迹，然后分别进行了基于传统人物检测算法的机器人运动规划和缺少运动成本优化的机器人运动规划两组对比实验。

实验结果如图6，图中的实曲线代表机器人在主动感知系统规划下的运动轨迹。轨迹上的数字标号代表路径上的一些观测点。

图6 主动感知过程中的位置估计误差

图6记录了主动感知过程中人物位置估计的误差变化，0到15s间的误差变化对应于图6中观测点0至观测点3间的运动，这期间位置误差收敛速率较平缓。之后由于机器人主动较大幅度改变运动方向和观测角度，人物位置估计准确性快速提升。

5 结论

服务机器人根据环境中目标信息的不确定性主动进行运动决策来降低目标位置不确定性，同时以较少的成本到达目标位置是本文的研究重点。本文从两方面设计主动感知系统，一是机器人静态的感知能力即人物位姿估计，一是机器人的运动规划能力即运动决策优化系统。以位姿估计和不确定性推理来预测环境中目标信息的不确定性，以机器人运动距离作为成本。通过实验，证明了基于机器学习的人物位姿估计算法比传统的固定特征线性分类算法更鲁棒，功能更全面。验证了主动感知系统能够赋予机器人在运动中自主搜集环境信息，降低目标信息不确定性和规划处较短的运动轨迹的能力。

[1] R. MacLachlan. Tracking Moving Objects from a Moving Vehicle Using a Laser Scanner. Technical report, the Robotics Institute Carnegie Mellon University, 2005.

[2] E,Floreano, D.Mondada.Active perception, navigation, homing, and grasping: an autonomous perspective[C]. Proceedings of PerAc, 1994, 122-130.

[3] A. Saffiotti, K. LeBlanc. Active Perceptual Anchoring of Robot Behavior in a Dynamic Environment[C], International Conference on Robotics & Automation, April 2000, 4:3796-3806.

[4] A. Makarenko, T. Kaupp, B. Grocholsky, H. Durrant-Whyte. Human-Robot Interactions in Active Sensor Networks[C], Computational Intelligence in Robotics and Automation Proceedings.July 2003, 1:247-252.

[5] D. Paul Benjamin, D. Lonsdale, D. Lyons. Designing a Robot Cognitive Architecture with Concurrency and Active Perception[C], AAAI Fall Symposium - Technical Report, October, 2004, Vol. FS-04-05, 8-19.

[6] P. N′u～nez, R. V′azquez-Mart′ln, J. C. del Toro, A. Bandera, F. Sandoval. Feature Extraction from Laser Scan Data based on Curvature Estimation for Mobile Robotics[C], IEEE International Conference on Robotics and Automation, May 2006, 1167-1175.

[7] K. O. Arras, S. Grzonka, M. Luber, W. Burgard. Efficient People Tracking in Laser Range Data using a Multi-Hypothesis Leg-Tracker with Adaptive Occlusion Probabilities[C], IEEE International Conference on Robotics and Automation, May 19-23, 2008, 1710-1718.

[8] C. Premebida. Segmentation and Geometric Primitives Extraction from 2D Laser Range Data for Mobile Robot Applications. technical report, Departamento de Engenharia Electrotécnica e de Computadores, Pólo II - Universidade de Coimbra, June, 2005.

[9] J. Lee, T. TsubouchiKenjiro, D. Yamamoto, S. Egawa. People Tracking Using a Robot in Motion with Laser Range Finder [C]. International Conference on Intelligent Robots and Systems, October 9-15, 2006, Beijing, 2936-2942.

[10] K. O. Arras, O′ scar, M.. Mozos, W. Burgard. Using Boosted Features for the Detection of People in 2D Range Data [C].IEEE International Conference on Robotics and Automation, Roma, Italy, April 2007, 3402-3411.

[11] G. Monteiro, C. Premebida, P. Peixoto, U. Nunes. Tracking and Classification of Dynamic Obstacles Using Laser Range Finder and Vision [C]. IROS 2006, 137-146.

(编辑 李秀敏)

Research on Active Sensing System of Service Robot Based on State Estimation

YUAN Liang

(School of Mechanical Engineering, Xinjiang University, Urumqi 830037, China)

In the interaction process between service robot and people, it is a significant problem for the robot to quickly and actively localize the position of people and find an optimal trajectory to get close to people. For this problem, this paper proposes the active sensing of service robot based on the laser sensor, which can endow the robot the ability of real time detecting, calculating people’s pose and computing the optimal path to the people. The people pose calculating module adopts the Adaboost based pattern learning algorithm and the action decision making module utilizes the uncertainty reasoning based multi-goal optimization algorithm. The whole system is implemented on robotic wheelchair as well as evaluations of the performance are achieved, too.

service robot; active sensing; people pose calculation; action decision optimization; uncertainty reasoning

1001-2265(2014)05-0073-05

10.13462/j.cnki.mmtamt.2014.05.019

2013-12-15；

2014-01-20

国家自然科学基金(61262059)；新疆优秀青年科技创新人才培养项目；新疆大学博士启动基金

袁亮(1972—)，男，山东郓城人，新疆大学副教授，博士，研究领域为机器人控制、计算机视觉与图像处理，(E-mail)ylhap@163.com。

TH16;TG65

A