基于狀態(tài)估計的服務機器人主動感知系統(tǒng)的研究*

袁 亮

(新疆大學 機械工程學院，烏魯木齊市 830047)

基于狀態(tài)估計的服務機器人主動感知系統(tǒng)的研究*

袁 亮

(新疆大學 機械工程學院，烏魯木齊市 830047)

服務機器人在人機交互過程中，一個重要問題是如何在未知環(huán)境下確定人的位姿并能夠快速準確的運動到人的面前。針對這一問題，文章設計了基于激光傳感器的服務機器人主動式感知系統(tǒng)。在本系統(tǒng)中，服務機器人在沒有先驗信息的條件下，可以通過運動過程中檢測、更新環(huán)境中的人物位姿來降低目標位置的不確定性，并且實時規(guī)劃可以到達該人物正前方的最優(yōu)路徑。系統(tǒng)采用基于Adaboost的模板估計算法來估計人物位姿，采用基于不確定性預測的多目標優(yōu)化算法來實現(xiàn)運動決策優(yōu)化。在機器人輪椅平臺上實現(xiàn)了該系統(tǒng)，并對系統(tǒng)的性能進行了評估。

服務機器人；主動感知；人物位姿估計；運動決策優(yōu)化；不確定性推理

0 引言

人物感知和定位是服務型移動機器人研究領域的一項主要內(nèi)容，是聯(lián)系機器人和人的關鍵技術。早期的人物感知定位技術被較多地應用于服務機器人目標跟蹤中。機器人往往是被動的接受環(huán)境信息，然后應用基于動態(tài)信息檢測和跟蹤濾波的方法對人物進行定位和跟蹤[1]。基于動態(tài)信息的人物感知定位技術缺乏廣泛適用性和穩(wěn)定性，一旦環(huán)境信息變化或人物目標特征不完整，感知定位算法將丟失目標，機器人不能主動地尋找人物目標。這樣的服務機器人是缺乏智能和自主性的。

目前，主動感知算法主要是解決機器人在非結構化的環(huán)境下，如何在主動獲取環(huán)境信息來降低目標信息的不確定性的同時，盡快的完成任務。文獻[2]較早提出了機器人主動感知問題，并且從概念上區(qū)分了主動感知和主動傳感器的區(qū)別。主動傳感器是一種傳感器的工作原理，是傳感器主動發(fā)送和采集環(huán)境數(shù)據(jù)。而機器人主動感知是一個系統(tǒng)問題，其中涉及傳感器，目標優(yōu)化，控制等問題。文獻[3]總結了主動感知系統(tǒng)的系統(tǒng)框架和應用方法。主動感知系統(tǒng)主要包括三個部分，狀態(tài)觀測，狀態(tài)濾波和決策優(yōu)化。同時提出了將主動感知運用于機器人軌跡優(yōu)化的問題中，通過對機器人運動進行建模，運用多目標規(guī)劃和分析運動模型的協(xié)方差矩陣來達到使機器人快速準確到達目標位置。

文獻[4]進一步擴展了主動感知問題的應用范圍，將其應用到群機器人和傳感器網(wǎng)絡中去，使機器人在人機交互的過程中具備更高的智能性。當前，主動感知的算法和系統(tǒng)設計思想除了用于控制和規(guī)劃外，更多地被應用于智能服務機器人領域，因為在這個領域，機器人對環(huán)境的智能感知能力往往要求更高。文獻[5]介紹了綜合多種傳感器構建移動機器人的主動感知系統(tǒng)，使移動機器人可以綜合多種感知手段觀察和預測人的意圖和行為。還介紹了如何運用主動感知來實時降低環(huán)境信息的不確定性和規(guī)劃機器人的運動決策來達到最終的運動目標。

服務機器人能夠順利完成服務的前提就是如何準確的定位人物位置和姿態(tài)，并且規(guī)劃出最優(yōu)的運動軌跡。之前的研究主要將這兩個問題分開處理[6-7]，即先利用傳感器數(shù)據(jù)計算出人的位姿信息，然后離線規(guī)劃計算最優(yōu)軌跡。但由于忽略了觀測位置和數(shù)據(jù)處理的不確定性，運動規(guī)劃的結果存在較大偏差。基于狀態(tài)估計的人物主動感知系統(tǒng)將傳感器的靜態(tài)感知能力和移動機器人的運動能力結合起來，實時同步地更新校準人物位姿信息來降低目標信息的不確定性和優(yōu)化運動軌跡，使機器人可以在無任何人物位置先驗信息的情況下，快速準確的到達人物的面前，進而完成后續(xù)的服務工作。

本文主要解決的問題是服務機器人如何在人機交互過程中主動快速的運動到人物面前。通過這項研究使服務機器人具備在復雜環(huán)境下能夠自主地降低目標不確定性，檢測和定位人的能力。本文提出了基于狀態(tài)估計的人物主動感知系統(tǒng)。基于主動感知的思想，設計了新的人物位姿狀態(tài)估計算法和運動決策優(yōu)化算法。

1 人物主動感知系統(tǒng)整體設計

服務機器人對人的主動感知系統(tǒng)主要包含基于機器學習的人物位姿估計和運動決策優(yōu)化等。主動感知系統(tǒng)的流程圖如下圖：

圖1 主動感知系統(tǒng)流程圖

基于機器學習的人物位姿估計部分首先檢測并計算出人物的位置和姿態(tài)。運動決策部分利用人物位姿信息來預測機器人下一時刻不同的運動位置對降低人物姿態(tài)信息不確定性的影響，同時兼顧優(yōu)化運動成本。人物位姿估計采用基于Adaboost的人物檢測和基于模板匹配的位置姿態(tài)估計方法。運動決策優(yōu)化中的人物姿態(tài)不確定性預測采用基于主觀Bayes的不確定性估計，然后通過啟發(fā)式和空間壓縮算法來求解多目標優(yōu)化函數(shù)。

2 人物位姿估計

基于激光測距儀的人物位姿估計算法主要包含人物檢測和位姿計算兩個部分。傳統(tǒng)的人物檢測方法主要基于固定特征的閾值判定方法[7]，不足是缺乏魯棒性，參數(shù)需要經(jīng)常調整。針對這些缺點，本文提出來基于機器學習的人物檢測算法。該檢測算法通過大量樣本的學習可以對不同環(huán)境具有較好的適應性。對于符合人物特征的數(shù)據(jù)集，進一步運用基于模板的位姿計算方法。人物位姿估計的算法流程圖如圖2。

圖2 人物位姿估計算法流程圖

2.1 模板匹配

姿態(tài)估計算法首先將激光測距儀原始數(shù)據(jù)轉化為距離直方圖，然后從中提取出三種模板[11]，根據(jù)實驗觀察，這三種模板涵蓋了人在激光測距儀前所有可能的成像。距離直方圖是將點集中每個點的徑向距離做成一張直方圖。選擇從距離直方圖中提取人物姿態(tài)模板的原因是：

(1) 人相對于其背景環(huán)境是處于較突出的位置，人物位置更容易從背景環(huán)境中分離出來。

(2) 人的姿態(tài)在距離直方圖中呈現(xiàn)出有限種模板，便于提取和分析。

根據(jù)實驗觀測，人物在激光測距儀前不同的站立姿態(tài)在距離直方圖的中的成像可以分為三種模板：m1: 人側身站立在機器人前;m2: 人雙腿合并面對機器人站立;m3: 人雙腿分開自然站立在機器人面前。

2.2 點集特征

人物檢測的基本思想是從激光測距儀的原始數(shù)據(jù)中提取幾何特征，然后依次做線性分類，這符合Adaboost 分類器的特點，所以利用Adaboost算法來選擇最具區(qū)分度的特征和閾值。所有特征提取后，每個點集將擁有一個特征向量，該向量包含了一個點集的以上所有特征，所有點集的特征向量構成了Adaboost分類器的樣本集。

通常，Adaboost中的弱分類器算法采用的是啟發(fā)式優(yōu)化算法來確定線性分類的閾值[10]，這類算法不足在于其解并非最優(yōu)解，而且時間復雜度往往較高。為了克服以上缺點，提高人物檢測算法的分類準確率和算法效率，本文設計了基于動態(tài)規(guī)劃的弱分類器訓練算法，可以得到理論上的最優(yōu)閾值，而且有確定的時間復雜度下界。

變量說明：

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是所有樣本的個數(shù)。同時記錄當前的閾值劃分位置循環(huán)結束，得到最優(yōu)的線性分類閾值，使正確劃分的樣本權重最大。該算法可以將弱分類器的訓練時間降低到O(nlogn)的復雜度，而且得到的是該維特征上最優(yōu)的分類閾值。這樣訓練出的若干個弱分類器級聯(lián)起來就組成了Adaboost強分類器。每一組由距離直方圖中提取出的模板點集可以用該分類器判斷其是否符合人物特征。

2.3 位姿估計

對于符合人物特征的點集，位姿估計的方法是利用模板數(shù)據(jù)的特征來估算人物的位姿。三種不同模板的位姿計算方法如下：

(1)m1：當人側身站立在激光傳感器前，該區(qū)間的距離直方圖可以較明顯的分為兩個子集subS1和subS2，分別代表人物的雙腿，各自包含的激光點的個數(shù)為numS1，numS2。提取兩個子集的中心點坐標(x1,y1)和(x2,y2)，人物的位置和姿態(tài)計算方法如下，

(1)

(2)m2：當人雙腿合并面對機器人站立，此時人的位置為該點集setM2的中心坐標，朝向和激光測距儀的徑向平行，位置和姿態(tài)的計算方法為：

(2)

(3)m3：當人雙腿分開自然站立在機器人面前，模板區(qū)間兩條腿的特征明顯，可以劃分為兩個子集，位姿計算方法同模板m1。

至此已完成了人物位姿估計系統(tǒng)的全部設計，該模塊返回的結果是激光測距儀中提取出的人物位置，姿態(tài)和模板匹配類型。

3 運動決策優(yōu)化

這里主要介紹主動感知系統(tǒng)得到人物的位置和姿態(tài)信息后，如何規(guī)劃機器人的運動決策。主動感知利用觀測對象即人的位姿估計算法為運動決策優(yōu)化提供實時人物位姿信息，運動決策優(yōu)化部分的算法流程圖如圖3，本章首先介紹多目標優(yōu)化函數(shù)，然后分析優(yōu)化函數(shù)中的目標信息不確定性和運動成本函數(shù)，最后介紹解空間搜索算法。

圖3 運動決策優(yōu)化算法流程圖

3.1 多目標優(yōu)化函數(shù)

主動感知的主要任務是在運動目標位置信息不完整的情況下，運動過程中如何兼顧快速到達目標和收集更多狀態(tài)觀測數(shù)據(jù)來降低目標信息不確定性，實時修正運動決策兩個目標。

運動決策優(yōu)化的目標函數(shù)為：

(3)

3.2 目標不確定性估計和運動成本分析

圖4 機器人在不同位置的人物感知示意圖

3.2.1 人物位姿不確定性表示

人物位姿不確定性包括位置不確定性upos和姿態(tài)不確定性uori。人物位置不確定性的表示如圖4人物兩腿間的陰影區(qū)域，在i觀測時刻，人物位置估計存在一個疑似區(qū)域OSi，真實環(huán)境中人物的準確位置一定落在OSi區(qū)域內(nèi)。機器人在不同時刻的OSi互相重合和疊加，可以不斷縮小疑似區(qū)域，提高人物位置估計的精度。位置不確定性upos的表示：

(4)

upos表示為疑似區(qū)域的長寬比。雙腿間直線距離在機器人不同觀測位置局部坐標系內(nèi)保持不變，所以upos值越趨近于零，人物疑似位置區(qū)域OSi在激光測距儀徑向上的分量越小，水平分量計算出的人物位置的結果越精確。

人物姿態(tài)不確定性的表示首先利用實驗統(tǒng)計數(shù)據(jù)來構造了人物姿態(tài)估計數(shù)據(jù)中的兩個概率分布模型。

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

圖5a表示人物檢測算法計算出的姿態(tài)數(shù)據(jù)||θ||在(-90°～+90°)的概率密度函數(shù)p(θ)，可以看出該位姿估計算法在θ成(-40°～+40°)時性能較好。角度越大，在距離直方圖中提取三種估計模板的難度越大，進而造成估算得到的姿態(tài)角的精度也越小。

圖5b說明了姿態(tài)估計的誤差Error的概率密度p(Error)，累計概率85%以上的誤差分布在(0°～40°)內(nèi)。姿態(tài)角的計算和對應的模板是有關聯(lián)的，每種模板在距離直方圖中的幾何形狀不同，不同特征提取的精度不同，所以其計算出的姿態(tài)角存在相應的概率分布 。姿態(tài)估計模塊計算出的結果存在不確定性，表現(xiàn)在姿態(tài)角的誤差和概率分布兩方面。

(5)

其中Maxerr為歸一化處理。P(μi|err)很難從大量實驗數(shù)據(jù)中構造出模型，μi和當前人物姿態(tài)的估計值有關，當前的估計值θ也是存在不確定性的。所以目前的問題模型是在當前時刻存在不確定性的姿態(tài)估計值θ的基礎上預測執(zhí)行運動決策ai后人物姿態(tài)的不確性。

3.2.2 基于主觀Bayes決策的人物姿態(tài)不確定性預測

本節(jié)將介紹應用主觀Bayes決策方法來計算uori函數(shù)，應用主觀Bayes方法的原因是因為其能夠解決上文的兩個問題：

(1)姿態(tài)估計中的條件概率p(mi|θ)，p(μi|err)等模型很難從實驗和數(shù)據(jù)中建立出來。

(2)觀測值，即人物姿態(tài)估計值θ存在不確定性，觀測數(shù)據(jù)的不確定性會傳遞給μi和P(err|μi)的計算。

主觀Bayes方法主要解決Bayes估計中難以取得的條件概率和證據(jù)不確定性的傳遞問題。

對于每一個人物姿態(tài)估計值θ，其對應的模板mi是確定的，利用主觀Bayes方法，

(6)

LS1：為經(jīng)驗參數(shù)，代表mi對θ的支持程度。

(7)

由于θ存在不確定性，作為p(θ|mi)的觀測值mi是確定的，則p(err|μi)可以利用p(θ|mi)=1 和p(θ|mi)=p(θ)兩個特殊點的做線性插值，再結合計算得出。至此，姿態(tài)估計不確定性指標可以通過p(θ|mi)，p(err|f(μi))和主動Bayes不確定性推理方法計算得出。

3.2.3 運動成本分析

(8)

3.3 解空間搜索

離散運動決策優(yōu)化目標函數(shù)的解空間為二維平面。首先利用離散化的思想，考慮交龍移動機器人在運動控制上的特性，將機器人當前位置的臨域范圍離散化為若干柵格，每個柵格為機器人運動可控的最小單位面積，同時將主動感知的每一步運動決策優(yōu)化限定在一定范圍內(nèi)。但樸素的搜索算法在這樣的解空間內(nèi)，時間和空間復雜度上仍較大，所以需要設計一個高效的最優(yōu)化方法。

目標函數(shù)的優(yōu)化算法將利用分支限界和啟發(fā)式搜索的思想。分支限界的節(jié)點擴展方程如式：

x(k)=rsin(k*β)

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

(9)

式中k代表啟發(fā)式搜索的方向，r代表每次運動決策中搜索算法的步長，這樣將決策變量降維到目標位置的方向。maxr為一個規(guī)劃周期內(nèi)解空間的界限。

啟發(fā)式搜索的兩個啟發(fā)函數(shù)定義為：

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

(10)

fdown(k)=step*r/maxr

(11)

其中step：當前搜索的層數(shù)。每一步運動優(yōu)化決策，可以通過以上方法計算出最優(yōu)決策。而且運動決策優(yōu)化的目標函數(shù)是離散解空間上的目標函數(shù)，符合擬陣的傳遞性和可擴展性，整個目標函數(shù)適用于Greedy規(guī)則，可以證明每一步的最優(yōu)決策可以保證運動決策優(yōu)化目標函數(shù)的整體最優(yōu)。

4 實驗和分析

本文以機器人輪椅為平臺，主動感知系統(tǒng)的評估實驗主要包括兩部分，首先是評估基于機器學習的人物檢測算法，其次是評估整個主動感知系統(tǒng)在運動決策優(yōu)化和降低目標信息不確定性方面的優(yōu)越性。

4.1 人物檢測算法評估實驗

在辦公室和家居環(huán)境下，各采集600幀激光測距儀數(shù)據(jù)。兩種不同背景環(huán)境下的樣本集可以提高訓練后分類算法的適用范圍，降低其誤識別率。在數(shù)據(jù)采集過程中，環(huán)境中有靜止站立的人，也有走動的人，這樣保證采集的樣本可以包含各種情況下的人物特征，提高訓練后分類算法的準確率。

兩種環(huán)境下共1200幀數(shù)據(jù)，經(jīng)過點集分割后得到15437個點集片段。對所有這些點集片段做特征提取處理后構成了樣本庫，其中正樣本874個。將樣本庫劃分為訓練樣本和測試樣本兩個部分，樣本數(shù)量分別為10034個和5400個，訓練樣本中的正樣本數(shù)為435個。

上文中特征提取部分對每個點集片段提取8個特征，所以Adaboost分類器的訓練迭代次數(shù)設置為16次，保證每一維特征都可以得到訓練。訓練結束后，使用測試樣本對檢測算法進行了測試，得到的結果如表2所示。

表1 基于Adaboost的檢測算法對測試樣本的計算結果

從表1中數(shù)據(jù)可以看出，基于機器學習的人物檢測算法可以達到97%的正確識別率，誤識別率也可以控制在1%左右。接下來將比較基于機器學習的人物檢測算法和傳統(tǒng)的固定特征的線性分類算法。兩種不同思路算法的比較將采用相同的樣本集，相同的8種點集特征，訓練和測試樣本，對比分類效果如下表。

表2 固定特征線性分類算法對測試樣本的計算結果

從表1與表2的比較中可以看出，無論正樣本的識別率還是負樣本的誤識別率，基于機器學習的人物檢測算法都比傳統(tǒng)的固定特征線性分類算法表現(xiàn)出更好的性能。

4.2 主動感知系統(tǒng)實驗

實驗的環(huán)境設置為在全局坐標系下，人的初始位置為(0.2, 5.1)，單位m，朝向-101.7°。機器人的初始位置為(3.1, 0.3)，單位m，朝向89.3°。主要進行三個實驗，第一個實驗測試了裝有主動感知系統(tǒng)的機器人的運動軌跡，然后分別進行了基于傳統(tǒng)人物檢測算法的機器人運動規(guī)劃和缺少運動成本優(yōu)化的機器人運動規(guī)劃兩組對比實驗。

實驗結果如圖6，圖中的實曲線代表機器人在主動感知系統(tǒng)規(guī)劃下的運動軌跡。軌跡上的數(shù)字標號代表路徑上的一些觀測點。

圖6 主動感知過程中的位置估計誤差

圖6記錄了主動感知過程中人物位置估計的誤差變化，0到15s間的誤差變化對應于圖6中觀測點0至觀測點3間的運動，這期間位置誤差收斂速率較平緩。之后由于機器人主動較大幅度改變運動方向和觀測角度，人物位置估計準確性快速提升。

5 結論

服務機器人根據(jù)環(huán)境中目標信息的不確定性主動進行運動決策來降低目標位置不確定性，同時以較少的成本到達目標位置是本文的研究重點。本文從兩方面設計主動感知系統(tǒng)，一是機器人靜態(tài)的感知能力即人物位姿估計，一是機器人的運動規(guī)劃能力即運動決策優(yōu)化系統(tǒng)。以位姿估計和不確定性推理來預測環(huán)境中目標信息的不確定性，以機器人運動距離作為成本。通過實驗，證明了基于機器學習的人物位姿估計算法比傳統(tǒng)的固定特征線性分類算法更魯棒，功能更全面。驗證了主動感知系統(tǒng)能夠賦予機器人在運動中自主搜集環(huán)境信息，降低目標信息不確定性和規(guī)劃處較短的運動軌跡的能力。

[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)；新疆優(yōu)秀青年科技創(chuàng)新人才培養(yǎng)項目；新疆大學博士啟動基金

袁亮(1972—)，男，山東鄆城人，新疆大學副教授，博士，研究領域為機器人控制、計算機視覺與圖像處理，(E-mail)ylhap@163.com。

TH16;TG65

A