智能機(jī)器人原理與應(yīng)用 課件 第11章 家庭智能空間服務(wù)機(jī)器人系統(tǒng)

智能機(jī)器人原理與應(yīng)用“智能檢測技術(shù)與模式識別”研究所11家庭智能空間服務(wù)機(jī)器人系統(tǒng)智能空間是一種新的人機(jī)交互協(xié)作的復(fù)雜系統(tǒng)，它通過普適傳感器網(wǎng)絡(luò)，把大量的多模態(tài)傳感器和帶有嵌入式處理器的計(jì)算、信息設(shè)備相互連接起來，從而將計(jì)算智能分布和嵌入到環(huán)境與日常工具中，以滿足各種特定需要[7]。與服務(wù)機(jī)器人研究的最終目的一致，智能空間研究也是為了提高人的生活質(zhì)量；從技術(shù)層面上看，將智能空間技術(shù)與服務(wù)機(jī)器人技術(shù)結(jié)合起來，可以實(shí)現(xiàn)兩者優(yōu)勢的互補(bǔ)[8]：一方面，服務(wù)機(jī)器人只需與智能空間建立通信即可隨時(shí)隨地感知整個(gè)家庭環(huán)境，擺脫了必須集各種傳感器于一身的束縛；另一方面，服務(wù)機(jī)器人的引入大大提高了智能空間感知與執(zhí)行的主動(dòng)性?？梢?，智能空間服務(wù)機(jī)器人的研究，是服務(wù)機(jī)器人技術(shù)與智能空間技術(shù)的完美結(jié)合，是不同于兩者的各自獨(dú)立發(fā)展，卻可以同時(shí)進(jìn)行的另一條道路，其研究也必將豐富機(jī)器人和智能空間領(lǐng)域的相關(guān)理論。11.1家庭智能空間服務(wù)機(jī)器人系統(tǒng)介紹11.1.1家庭服務(wù)機(jī)器人1.家庭服務(wù)機(jī)器人研究尚不成熟當(dāng)前的家庭服務(wù)機(jī)器人研究和應(yīng)用遠(yuǎn)遠(yuǎn)沒有達(dá)到我們的期望，這其中一部分原因是當(dāng)前對服務(wù)機(jī)器人本身，包括智能、自主等方面的研究還不成熟：首先，機(jī)器人的智能程度較為低下，目前機(jī)器人的服務(wù)功能往往是通過使用者預(yù)先對機(jī)器人設(shè)置定時(shí)啟動(dòng)，或通過遙控等設(shè)備發(fā)出指令為絕對前提，機(jī)器人缺乏主動(dòng)智能；其次，機(jī)器人感知和自主運(yùn)動(dòng)能力尚待提高，為機(jī)器人本體配備更多的感知設(shè)備，難以從根本上解決機(jī)器人全局范圍內(nèi)有效信息的感知問題，反而導(dǎo)致了成本和計(jì)算開銷的增加。因此，機(jī)器人進(jìn)入家庭作業(yè)仍受到很大的限制，其基本功能如定位、導(dǎo)航和物體識別等仍是最具有挑戰(zhàn)性的課題。11.1家庭智能空間服務(wù)機(jī)器人系統(tǒng)介紹11.1.1家庭服務(wù)機(jī)器人2.家庭服務(wù)任務(wù)的難度眾所周知，家庭服務(wù)機(jī)器人面對的是整個(gè)家庭環(huán)境及其中的各種目標(biāo)，不同于工業(yè)現(xiàn)場固定和可編程的環(huán)境，其復(fù)雜性具體表現(xiàn)為以下幾個(gè)方面。(1)機(jī)器人工作環(huán)境為較為擁擠、混雜的家庭環(huán)境，且動(dòng)態(tài)性較高；(2)家庭目標(biāo)種類繁多特征各異，部分目標(biāo)具有較強(qiáng)的移動(dòng)性并在家庭環(huán)境進(jìn)出；(3)家庭環(huán)境和目標(biāo)相輔相承，融為一體，并無明確界限可分。由于上述因素的影響，造成了家庭環(huán)境及其中目標(biāo)的多而混雜、異質(zhì)性和動(dòng)態(tài)性，在此情況下，傳統(tǒng)的只適合在特定條件、特定場合下完成簡單任務(wù)的工作模式和處理方法將不能滿足家庭服務(wù)機(jī)器人任務(wù)的需要。11.1家庭智能空間服務(wù)機(jī)器人系統(tǒng)介紹11.1.2智能空間1.智能空間的特點(diǎn)近年來，伴隨計(jì)算機(jī)、信息和網(wǎng)絡(luò)技術(shù)的飛速發(fā)展，出現(xiàn)了基于普適計(jì)算理論的智能空間技術(shù)，它是一種新的復(fù)雜系統(tǒng)，通過普適傳感器網(wǎng)絡(luò)，把大量的多模態(tài)傳感器安裝在室內(nèi)的多個(gè)位置，并將其與帶有嵌入式處理器的計(jì)算設(shè)備進(jìn)行互連，從而將計(jì)算智能分布和嵌入到附有唯一識別標(biāo)簽的環(huán)境與日常目標(biāo)中，以實(shí)現(xiàn)對環(huán)境及其中目標(biāo)的隨時(shí)、隨地感知，滿足各種特定任務(wù)的需要?？梢?，智能空間既可以提供對其所在環(huán)境的精確觀測信息，同時(shí)又具備高效快速的信息處理能力。11.1家庭智能空間服務(wù)機(jī)器人系統(tǒng)介紹11.1.2智能空間2.智能空間的應(yīng)用前景智能空間技術(shù)旨在建立一個(gè)以人為中心的充滿計(jì)算和通信能力的空間，讓計(jì)算機(jī)參與到日常活動(dòng)中，使用戶能像與其他人一樣與計(jì)算機(jī)系統(tǒng)發(fā)生交互，從而隨時(shí)隨地、透明地獲得人性化的服務(wù)。從普適計(jì)算的角度來看，智能空間是普適計(jì)算理論研究的理想實(shí)驗(yàn)平臺(tái)，同時(shí)作為一種集成化系統(tǒng)，智能空間也具有十分重要的應(yīng)用價(jià)值，在智能交通、應(yīng)急反應(yīng)、機(jī)器人等領(lǐng)域顯示出廣泛的應(yīng)用前景。11.1家庭智能空間服務(wù)機(jī)器人系統(tǒng)介紹11.1.3家庭智能空間服務(wù)機(jī)器人系統(tǒng)構(gòu)建背景1.家庭智能空間服務(wù)機(jī)器人系統(tǒng)的理念家庭智能空間服務(wù)機(jī)器人系統(tǒng)的建立主要是通過在室內(nèi)環(huán)境布撒各種信息檢測傳感器、用于數(shù)據(jù)處理和信息服務(wù)的計(jì)算主機(jī)、人-機(jī)交互的觸摸顯示器界面、網(wǎng)絡(luò)系統(tǒng)以及數(shù)據(jù)庫等軟硬件設(shè)施，從而將室內(nèi)家庭環(huán)境改造為遍布觀測和處理能力的信息空間，為服務(wù)機(jī)器人提供海量信息的環(huán)境信息，并通過無線傳輸方式與機(jī)器人交互，輔助機(jī)器人實(shí)現(xiàn)準(zhǔn)確導(dǎo)航、精確定位等工作，進(jìn)而完成抓取、運(yùn)送、整理家具和物品等各種家庭服務(wù)任務(wù)。11.1家庭智能空間服務(wù)機(jī)器人系統(tǒng)介紹11.1.3家庭智能空間服務(wù)機(jī)器人系統(tǒng)構(gòu)建背景2.家庭智能空間服務(wù)機(jī)器人系統(tǒng)的系統(tǒng)構(gòu)成圖11.1給出了一種以攝像機(jī)為節(jié)點(diǎn)構(gòu)建智能空間傳感器網(wǎng)絡(luò)的家庭智能空間服務(wù)機(jī)器人系統(tǒng)示意圖。本章的研究同樣是基于攝像機(jī)節(jié)點(diǎn)來構(gòu)建智能空間傳感器網(wǎng)絡(luò)。攝像機(jī)1攝像機(jī)N智能空間攝像機(jī)網(wǎng)絡(luò)智能空間主機(jī)ZigBeeZigBee操作目標(biāo)服務(wù)對象攝像頭圖11.1家庭智能空間服務(wù)機(jī)器人系統(tǒng)構(gòu)成11.2機(jī)器人同步定位、傳感器網(wǎng)絡(luò)標(biāo)定與環(huán)境建圖眾所周知，機(jī)器人定位、傳感器網(wǎng)絡(luò)標(biāo)定與環(huán)境建圖是物聯(lián)網(wǎng)機(jī)器人系統(tǒng)中三個(gè)相互耦合的基本問題，其有效解決是物聯(lián)網(wǎng)機(jī)器人系統(tǒng)提供高效智能服務(wù)的前提。本節(jié)提出了物聯(lián)網(wǎng)機(jī)器人系統(tǒng)同時(shí)機(jī)器人定位、傳感器網(wǎng)絡(luò)標(biāo)定與環(huán)境建圖的概念，通過分析三者之間的耦合關(guān)系，給出同時(shí)定位、標(biāo)定與建圖問題的聯(lián)合條件概率表示，基于貝葉斯公式和馬爾科夫特性將其分解為若干可解項(xiàng)，并借鑒Rao-Blackwellized粒子濾波的思想分別求解。首先，聯(lián)合傳感器網(wǎng)絡(luò)對機(jī)器人的觀測、機(jī)器人對已定位環(huán)境特征的觀測以及機(jī)器人自身控制量設(shè)計(jì)了位姿粒子的采樣提議分布和權(quán)值更新公式；其次，聯(lián)合傳感器網(wǎng)絡(luò)對機(jī)器人運(yùn)動(dòng)軌跡及已定位環(huán)境特征的觀測設(shè)計(jì)了傳感器網(wǎng)絡(luò)標(biāo)定的遞推公式；然后，聯(lián)合傳感器網(wǎng)絡(luò)和機(jī)器人對(已定位或新發(fā)現(xiàn))環(huán)境特征的觀測設(shè)計(jì)環(huán)境建圖的遞推公式。給出了完整的同時(shí)定位、標(biāo)定與建圖算法，并通過仿真實(shí)驗(yàn)驗(yàn)證了該算法的有效性。11.2機(jī)器人同步定位、傳感器網(wǎng)絡(luò)標(biāo)定與環(huán)境建圖11.2.1問題簡化作為人們?nèi)粘Ｉ詈凸ぷ鞯闹匾獔鏊彝セ蜣k公室環(huán)境的布局結(jié)構(gòu)往往較為復(fù)雜，且其中目標(biāo)種類繁多、特征各異并具有不同程度的動(dòng)態(tài)特性。為方便分析，不妨將物聯(lián)網(wǎng)機(jī)器人系統(tǒng)進(jìn)行合理簡化。首先建立機(jī)器人坐標(biāo)系：以標(biāo)識色塊的中心為坐標(biāo)系原點(diǎn)，z軸方向垂直標(biāo)識色塊向上，x軸方向?yàn)闄C(jī)器人前進(jìn)方向，y軸方向由右手法則確定。假定機(jī)械手基座坐標(biāo)系、PTZ云臺(tái)坐標(biāo)系在機(jī)器人坐標(biāo)系下的位姿均已離線標(biāo)定。以初始建圖時(shí)刻的機(jī)器人坐標(biāo)系作為世界坐標(biāo)系，由于機(jī)器人運(yùn)行在平行于地面的二維平面，不妨假設(shè)任意時(shí)刻機(jī)器人在世界坐標(biāo)系z軸的投影始終為零。此外，環(huán)境目標(biāo)往往分布在三維空間中，故本文將整個(gè)環(huán)境描述為世界坐標(biāo)系下包含環(huán)境布局及其中目標(biāo)的三維特征地圖，并通過機(jī)器人和傳感器網(wǎng)絡(luò)的實(shí)時(shí)交互來聯(lián)合構(gòu)建并共同維護(hù)該地圖。11.2機(jī)器人同步定位、傳感器網(wǎng)絡(luò)標(biāo)定與環(huán)境建圖11.2.2模型求解從概率的觀點(diǎn)看，物聯(lián)網(wǎng)機(jī)器人系統(tǒng)同時(shí)定位、標(biāo)定與建圖問題可以用概率密度來表示，即在已知機(jī)器人控制輸入序列、機(jī)器人對N個(gè)環(huán)境目標(biāo)的觀測序列、傳感器網(wǎng)絡(luò)M個(gè)節(jié)點(diǎn)對機(jī)器人的觀測序列，以及傳感器網(wǎng)絡(luò)M個(gè)節(jié)點(diǎn)對N個(gè)環(huán)境特征的觀測序列的條件下，求解傳感器網(wǎng)絡(luò)M個(gè)節(jié)點(diǎn)的參數(shù)、機(jī)器人位姿、以及N個(gè)目標(biāo)所構(gòu)成地圖的聯(lián)合后驗(yàn)概率估計(jì)問題?；赗ao-Blackwellized粒子濾波的思想，機(jī)器人運(yùn)動(dòng)軌跡的遞歸估計(jì)由粒子濾波器完成，各粒子對應(yīng)傳感器網(wǎng)絡(luò)參數(shù)估計(jì)和特征地圖估計(jì)均由擴(kuò)展Kalman濾波器完成。11.2機(jī)器人同步定位、傳感器網(wǎng)絡(luò)標(biāo)定與環(huán)境建圖11.2.3算法描述通過前面的分解，并采用Rao-Blackwellized粒子濾波思想，在t時(shí)刻，物聯(lián)網(wǎng)機(jī)器人系統(tǒng)同時(shí)定位、標(biāo)定與建圖問題的求解算法如下：Step1.基于粒子濾波的機(jī)器人定位(1)機(jī)器人位姿估計(jì)：采樣機(jī)器人位姿粒子；(2)位姿粒子權(quán)值計(jì)算：計(jì)算各位姿粒子的權(quán)值，歸一化；(3)計(jì)算有效粒子數(shù)，進(jìn)行粒子重采樣；Step2.傳感器節(jié)點(diǎn)標(biāo)定環(huán)節(jié)：基于EKF估計(jì)傳感器節(jié)點(diǎn)的位姿參數(shù)(1)預(yù)測更新；(2)根據(jù)傳感器節(jié)點(diǎn)對機(jī)器人觀測的觀測更新；(3)根據(jù)傳感器節(jié)點(diǎn)對已定位環(huán)境特征觀測的觀測更新；Step3.環(huán)境特征建圖環(huán)節(jié)：基于EKF估計(jì)環(huán)境特征的位置：(1)預(yù)測更新：(2)根據(jù)機(jī)器人對環(huán)境特征觀測的觀測更新；(3)根據(jù)傳感器節(jié)點(diǎn)對環(huán)境特征觀測的觀測更新。11.2機(jī)器人同步定位、傳感器網(wǎng)絡(luò)標(biāo)定與環(huán)境建圖11.2.3算法復(fù)雜度分析同時(shí)定位、標(biāo)定與建圖問題的本質(zhì)是多傳感器信息融合意義下的狀態(tài)估計(jì)問題。本文方法采用序貫方式融合兩類觀測信息進(jìn)行狀態(tài)的觀測更新，針對每一類觀測，當(dāng)同時(shí)存在多個(gè)該類觀測時(shí)，仍然采用序貫方式加以融合，如對于步驟Step2.(2)的觀測更新，當(dāng)存在多個(gè)傳感器節(jié)點(diǎn)對機(jī)器人的觀測時(shí)，采用序貫方式融合多傳感器節(jié)點(diǎn)的觀測信息，同樣策略應(yīng)用于步驟Step2.(3)、Step3.(2)和Step3.(3)中存在多個(gè)同類觀測的情形。假定物聯(lián)網(wǎng)機(jī)器人系統(tǒng)中包含一個(gè)移動(dòng)機(jī)器人和一個(gè)由M個(gè)節(jié)點(diǎn)構(gòu)成的傳感器網(wǎng)絡(luò)，且環(huán)境中包含N個(gè)特征點(diǎn)，本文算法中粒子個(gè)數(shù)選為K，在極端情形下，即各傳感器節(jié)點(diǎn)始終可以觀測到機(jī)器人和所有環(huán)境特征點(diǎn)，且機(jī)器人在任意時(shí)刻也都可以觀測到所有環(huán)境特征點(diǎn)時(shí)，本文算法所要融合的數(shù)據(jù)量最大，通過算法分析可知此情況下算法循環(huán)次數(shù)為K(M(N+1)+N(M+1))，或者說，本算法在最壞情況下的時(shí)間復(fù)雜度為O(KMN)。11.2機(jī)器人同步定位、傳感器網(wǎng)絡(luò)標(biāo)定與環(huán)境建圖11.2.4實(shí)驗(yàn)測試TimBailey提供了SLAM的matlab仿真程序和一個(gè)200m×200m的數(shù)據(jù)集，在此基礎(chǔ)上作如下改動(dòng)：在地圖中隨機(jī)添加landmark和waypoint數(shù)據(jù)，其位置如圖11.2所示；機(jī)器人初始位置在原點(diǎn)處、方向朝左；控制周期為0.025s；機(jī)器人能夠得到距離和方位的觀測信息，觀測范圍為其前方半徑為30m的半球區(qū)域，觀測采樣周期0.2s，觀測噪聲的協(xié)方差矩陣為diag{0.01.0.01}，運(yùn)動(dòng)速率3m/s，運(yùn)動(dòng)噪聲的協(xié)方差diag{0.09,(3o)2}，實(shí)驗(yàn)中所用到粒子濾波的采樣粒子數(shù)均取為100。圖11.2環(huán)境特征、傳感器節(jié)點(diǎn)和機(jī)器人運(yùn)行路徑示意圖11.2機(jī)器人同步定位、傳感器網(wǎng)絡(luò)標(biāo)定與環(huán)境建圖11.2.4實(shí)驗(yàn)結(jié)果分析第一組仿真實(shí)驗(yàn)進(jìn)行機(jī)器人定位精度對比，分別采用EKF-SLAM、FastSLAM2.0與本文的物聯(lián)網(wǎng)機(jī)器人系統(tǒng)U-SLAM(ubiquitousrobotsystemSLAM,U-SLAM)方法進(jìn)行對比，三類方法各運(yùn)行50次，得到的機(jī)器人位置誤差的數(shù)學(xué)期望和方差如圖11.3所示，角度誤差的數(shù)學(xué)期望和方差如圖11.4所示?？梢钥闯?，傳統(tǒng)的SLAM方法中，EKF-SLAM算法僅利用機(jī)器人運(yùn)動(dòng)模型進(jìn)行位姿估計(jì)而沒有考慮對環(huán)境的觀測信息，得到的定位誤差較大；FastSLAM2.0算法由于充分考慮機(jī)器人觀測信息，定位精度較高，但由于傳感器隨機(jī)器人運(yùn)動(dòng)，導(dǎo)致這兩種傳統(tǒng)SLAM方法都存在定位誤差隨運(yùn)動(dòng)時(shí)間明顯增大的缺陷。而對于本文U-SLAM，由于傳感器網(wǎng)絡(luò)各節(jié)點(diǎn)獨(dú)立于機(jī)器人，解除了觀測與機(jī)器人運(yùn)動(dòng)之間的數(shù)據(jù)耦合，在明顯提高機(jī)器人位姿估計(jì)精度的同時(shí)，估計(jì)的穩(wěn)定性也有大幅改善。圖11.4機(jī)器人角度誤差對比圖11.3機(jī)器人位置誤差對比11.2機(jī)器人同步定位、傳感器網(wǎng)絡(luò)標(biāo)定與環(huán)境建圖11.2.4實(shí)驗(yàn)結(jié)果分析第二組仿真實(shí)驗(yàn)進(jìn)行傳感器網(wǎng)絡(luò)標(biāo)定精度對比，分別對僅依據(jù)運(yùn)動(dòng)方程的機(jī)器人位姿、依據(jù)FastSLAM2.0的機(jī)器人位姿，以及本文的物聯(lián)網(wǎng)機(jī)器人系統(tǒng)U-SLAM中聯(lián)合機(jī)器人位姿與環(huán)境特征這三類傳感器網(wǎng)絡(luò)標(biāo)定方法進(jìn)行對比，三類方法各運(yùn)行50次，得到的傳感器網(wǎng)絡(luò)標(biāo)定誤差的數(shù)學(xué)期望和方差如圖11.5所示?？梢钥闯觯瑑H依據(jù)運(yùn)動(dòng)方程的方法，機(jī)器人位姿估計(jì)誤差隨運(yùn)動(dòng)距離增加迅速，得到的標(biāo)定結(jié)果誤差很大，難以滿足要求；依據(jù)FastSLAM2.0算法能夠有效減少機(jī)器人位姿估計(jì)誤差，從而提高了節(jié)點(diǎn)標(biāo)定的精度，但由于僅考慮了節(jié)點(diǎn)對機(jī)器人的觀測，產(chǎn)生的標(biāo)定誤差仍然較高。而對于本文U-SLAM，傳感器網(wǎng)絡(luò)各節(jié)點(diǎn)標(biāo)定、機(jī)器人位姿估計(jì)相對獨(dú)立，解除了觀測與機(jī)器人運(yùn)動(dòng)之間的數(shù)據(jù)耦合，在明顯提高機(jī)器人位姿估計(jì)精度的同時(shí)，傳感器節(jié)點(diǎn)標(biāo)定的精度和穩(wěn)定性也得到了大幅改善。圖11.5傳感器網(wǎng)絡(luò)標(biāo)定誤差對比11.2機(jī)器人同步定位、傳感器網(wǎng)絡(luò)標(biāo)定與環(huán)境建圖11.2.4實(shí)驗(yàn)結(jié)果分析第三組仿真實(shí)驗(yàn)進(jìn)行環(huán)境建圖精度對比，分別對僅依據(jù)運(yùn)動(dòng)方程的機(jī)器人位姿、FastSLAM2.0的機(jī)器人位姿，以及本文的物聯(lián)網(wǎng)機(jī)器人系統(tǒng)U-SLAM中聯(lián)合機(jī)器人位姿與環(huán)境特征這三類環(huán)境建圖方法進(jìn)行對比，三類方法各運(yùn)行50次，得到的傳感器網(wǎng)絡(luò)標(biāo)定誤差的數(shù)學(xué)期望和方差如圖11.6所示?？梢钥闯?，僅依據(jù)運(yùn)動(dòng)方程的方法，機(jī)器人位姿估計(jì)誤差隨運(yùn)動(dòng)距離增加迅速，導(dǎo)致建圖誤差很大，難以滿足要求；依據(jù)FastSLAM2.0算法能夠有效減少機(jī)器人位姿估計(jì)誤差，從而提高了建圖的精度，但由于僅考慮了節(jié)點(diǎn)對機(jī)器人的觀測，產(chǎn)生的標(biāo)定誤差仍然較高。而對于本文U