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