無路標環境下遙操作機器人SLAM系統

曹衛華，吳凈斌，吳 敏，陳 鑫

自K.Goldbergz在Mercury Project中提出基于Internet的網絡機器人后［1］，隨著網絡和無線通信技術的發展，結合無線網絡通信設計具有本地自主的探索移動機器人具有非常重要的意義，其在軍事、安防、家政等領域具有很好的應用前景.

對于探索機器人而言，SLAM(simultaneous localization and mapping)是一項非常重要的功能［2-5］.目前多位學者提出一些SLAM算法，將其應用于探索機器人中，其中基于EKF(extended Kalman filter)的 SLAM 算法［4，6-8］、基于EM(expectation maximization)的SLAM算法［9-14］、基于粒子濾波器的FastSLAM算法［15-16］成為SLAM研究的主流方向.這些算法都有較大的計算量且復雜的優化過程，對機器人的控制器要求高，這對于目前以嵌入式系統為控制器的機器人而言就顯得過于復雜.

實時定位是SLAM的基礎，室內環境下可使用里程計結合電子羅盤或人為設定路標的方式.在有路標條件下的定位研究已經取得了很多成功實例［17-18］，但在未知環境中設置路標不具有實用性.一些學者提出，在未知環境下，從機器人攜帶的激光測距數據中提取直線段信息或直線段之間的交點，將其作為路標［19-20］;但其尋找路標過程卻增加了SLAM算法的復雜度，同時也增加了控制器的負擔，不適宜應用于嵌入式系統.

針對SLAM算法的復雜性和實時性問題，設計了多控制器協作及多傳感器信息融合的移動機器人硬件結構，開發了快速地圖構建及探索方法.該方法包括:多傳感器信息融合的實時定位及路徑規劃和基于網絡地圖構建方法.基于無線網絡的未知環境下探索機器人系統可分為移動機器人子系統(以下簡稱機器人)和機器人遠程狀態監視子系統(以下簡稱監視系統)2個部分.遠程訪問者通過Web登錄機器人系統，并下載機器人上的Java Applet監視系統的操作界面程序，對機器人進行訪問.

1 機器人SLAM和路徑規劃

未知環境下探索機器人在實現SLAM過程中，需要經過實時定位、局部路徑規劃、地圖繪制3個階段.機器人通過基于里程計和電子羅盤的實時定位方式，獲得當前機器人的實時坐標.機器人基于當前坐標與預設定的目標點，通過多傳感器信息融合技術完成實時路徑規劃，并引導機器人向目標點移動.同時將所獲得的環境信息，通過無線網絡通信的方式傳輸給監視系統完成地圖繪制.

1.1 實時定位

在常規的機器人控制中，機器人往往需要以機器人的朝向角為X軸建立局部坐標系，機器人在進行局部到全局坐標系變換時需要建立位姿轉換矩陣.本文機器人通過電子羅盤，可以自身維持一個不變方向的局部坐標系，實現局部坐標系與全局坐標系朝向保持一致.這樣簡化了坐標變換過程，計算量也相對減少，有利于將有限的控制器資源合理利用.

機器人采用里程計結合電子羅盤信息的定位方式.其定位過程如下:機器人得到目標點后，以自身為原點，東西方向為X軸，南北方向為Y軸，建立坐標系AOB，計算出目標點坐標.然后再以目標點為原點建立全局坐標系XOY，經過坐標變換得出機器人在全局坐標系中的坐標.當機器人移動時，電機反饋脈沖并計數，可實時計算出機器人走過的里程.電子羅盤以30 Hz的頻率輸出角度信息，可實時獲知機器人移動朝向的角度.由此，便可實時計算出機器人在全局坐標系中的坐標，如圖1所示.

為便于控制及簡化計算，機器人通過原地轉向運動和直線運動的組合實現向任意目標點移動.設初始時刻t0機器人的坐標為(xt0，yt0)，直線運動狀態下每經過Δt時段通過電機的光電編碼盤脈沖反饋可得機器人移動的距離 ΔL，則 ΔL1，ΔL2，…，ΔLn分別表示機器人在 Δt1，Δt2，…，Δtn時段走過的距離.通過電子羅盤角度信息反饋可實時獲得機器人朝向角θbot，則經過T時段，機器人走過距離為在 tn時刻機器人到達(xtn，ytn)，假設任意一個T時段，機器人都處于直線運動狀態下，則

式中:0≤θbot≤2π.

圖1 系統坐標系示意圖Fig.1 Coordinate systems referred in the calculation

1.2 局部路徑規劃

機器人根據設定，每遇到障礙物時就重新作路徑規劃，因而需要利用激光測距重新掃描周圍障礙物.本文提出一種改進的 VFH(vector field histogram)局部路徑規劃算法［21］，該算法是基于多傳感器信息融合的路徑尋優方法.在規劃路徑之前需要定義一個與機器人朝向角相關的通過代價值，表示機器人沿該方向移動所付出的代價.在每個運動周期前，機器人自身旋轉，掃描其周圍360°的范圍，并每隔5°計算出一個代價值.機器人根據代價最小原則選擇運動方向.設待定運動方向為θ，則方向選擇算法表示為

式中:C(α)為機器人在α方向上的代價函數，其計算方式為

式中:wj為影響機器人路徑規劃的優化分量權值，且滿足為機器人相關信息，下標j代表不同的優化分量，包括:F1(q，α)為紅外傳感器測距值;F2(q，α)為激光傳感器反饋測距值;F3(q，α)為機器人朝向與機器人和目標點連線的夾角值，其經過實時計算得出.

基于紅外傳感器測距值F1(q，α)可以確定機器人何時完成避障的動作.機器人在移動過程中，F1(q，α)反映其側身的紅外傳感器實時檢測右側障礙物延伸狀況.當檢測到側身障礙物消失，F1(q，α)值減小，機器人趨于目標點方向直接轉向.基于激光測距值F2(q，α)可以測量障礙物距離信息指引機器人選擇路徑.基于機器人朝向與機器人和目標點連線的夾角值F3(q，α)可以確定機器人朝目標點移動的方向.因此，通過權值wj將各優化分量映射為惟一的代價值，也就綜合考慮了以上信息對機器人的影響，實現實時路徑規劃.

圖2(a)展示了該方法的一個實例.在該環境中，機器人對周圍障礙物進行掃描并計算代價值.圖2(b)所示為機器人掃描0°～180°時綜合代價的直方圖.根據綜合代價，機器人運動角度在40°～55°和80°～95°的代價最小.機器人便可根據式(2)得到下一步運動的方向角.

圖2 局部路徑規劃實例說明圖Fig.2 Example of local path planning

1.3 實時地圖構建

監視系統接收并顯示機器人在定位與路徑規劃的相關信息，且同時重構機器人所經過環境的障礙物模型.實時繪圖與定位都需要機器人利用激光測量周圍障礙物的距離信息.距離信息的測量使用激光和攝像頭相結合的技術.測量時，激光直射到障礙物上，攝像頭截取當前圖像，并通過對圖像進行二值化，定位在圖像上由激光直射到障礙物上產生的亮斑，之后根據相似三角原理計算出亮斑離攝像頭的距離，即機器人離障礙物的距離.如圖3所示，可知D為激光攝像頭與障礙物之間的距離.并且D可由

計算得出.式中:h為激光發生器到攝像頭的距離，θls為攝像頭和亮斑連線與激光發生器的夾角，pto為亮斑在縱軸方向上離圖像中心的像素值距離，e為角度對應像素變換率.

圖3 基于圖像處理的激光測距示意圖Fig.3 Laser ranging with image processing

與TOF(time of flight)激光測距法相比，該方法的優點為短距離測距效果好.測距時激光是靜態照射，只需處理獲得的圖像，此過程不存在激光反射速度過快、感受器來不及感受的問題.所以此方法能測出離機器人距離較小的障礙物.與超聲波測距法相比，該方法的優點為單向性好，很長距離內光束發散很小，具有小角度分辨率.

整個繪圖過程可劃分為多個繪圖周期.一個周期的基本步驟是:1)當機器人探測到前方一定距離內的一個障礙物時，以自身為中心建立局部坐標系AOB，通過激光測距和電子羅盤測量出相對于自身在半徑為R范圍內的障礙物信息，包括障礙物離機器人的距離及方向;2)根據障礙物的距離和方向信息進行局部路徑規劃，同時將該掃描周期得到的障礙物距離和方向信息上傳至監視系統;3)監視系統將每個周期所上傳的信息還原成直觀的特征圖.機器人到達目標點的過程中，已經歷了多個繪圖周期，將這些繪圖周期按順序整合在一張地圖中，便漸進地將機器人所遍歷的環境繪制成一張環境模型地圖.

在第3)步繪圖過程中，以機器人出發點為參考原點，可利用機器人反饋信息，提取障礙物的直線特征與點特征，并顯示在繪圖窗口.

2 實驗結果與分析

為驗證以上提出的未知環境探索技術，使用基于嵌入式系統的自主式移動機器人完成對預設目標的探索及實時路徑規劃，通過實驗測試實時定位、路徑規劃及遠程地圖構建方法的有效性.

2.1 實驗平臺

本文所述的機器人采用ARM9體系的嵌入式處理器作為主控制器，并移植了具有較高實時性的linux(2.6.14內核)作為其操作系統，配備攝像頭、無線網卡、激光發生器、紅外測距傳感器和電子羅盤.機器人平面長寬分別為0.2 m、0.16 m，其整體硬件結構如圖4所示.監視系統通過無線網絡與機器人建立連接，并負責向機器人傳送控制者設定的目標點，顯示機器人傳送上來的視頻信息和機器人的運行參數，且可利用相關傳感器采集的信息繪制機器人運行軌跡和環境地圖.機器人負責在未知環境中移動到目標點過程的探索，并將傳感器采集的相關信息發送給監視系統.機器人在探索過程中需要具備:無線網卡實現網絡通信，攝像頭實現視頻監視，攝像頭、激光、電子羅盤實現掃描測距和路徑規劃，里程計和電子羅盤實現自身定位.

圖4 機器人硬件結構Fig.4 Robot hardware

機器人需要一個多控制器協作與多傳感器信息融合的結構實現所需的功能.可將其為3個模塊:嵌入式系統為上位機的控制與決策模塊;單片機為控制傳感器信息融合傳遞的傳感器模塊;單片機為控制電機轉動并對電機做速度反饋檢測的驅動模塊.控制與決策模塊通過串口分別與傳感器模塊和驅動模塊進行交互.整個硬件控制系統結構如圖5所示.

圖5 機器人總體結構圖Fig.5 Configuration of the robot system

2.2 激光測距實驗

激光測距效果直接影響實時地圖繪制的精確度，首先對激光測距效果進行測試.結合激光的圖像處理測距法，對圖像進行二值化處理后得到只有亮斑的圖片，將亮斑的位置放大處理，可清晰看到亮點，如圖6所示.由此，便可得出亮點在圖片的像素點坐標，經過比例運算，可得亮斑與發生器的距離，即機器人與障礙物的距離.

圖6 圖像處理效果圖Fig.6 Results of image processing

表1中顯示所測距離與實際距離的比較.可以發現，在機器人與障礙物相距0.74 m之內時，該測距方法具有較高精度，但個別測量數據精度略有跳動;當實際距離超過0.9 m的時候，誤差增大.究其原因，在0.74 m之內時，機器人運動造成激光與攝像頭輕微晃動，而使測量精度產生跳動.而當實際距離超過0.9 m時，由于圖像上的激光亮斑移動變緩，圖像處理精度下降，使其測距誤差增大.

表1 測距數據對比Table 1 Data comparison

在實際應用中，現有激光測距精度不影響機器人的路徑規劃及監視系統的地圖繪制.因為，當機器人進行測距掃描時，其移動方向上的障礙物已經處在0.74 m之內，即使某障礙物位于機器人前進方向的0.74 m之外，隨著機器人移動，障礙物距離縮短，掃描的精度也隨之提高，從而保證了地圖繪制的準確度.

2.3 機器人運行實驗

監視系統通過無線網絡與機器人建立連接，其操作界面包括繪圖區、視頻區、數據參數區.如圖7所示.將機器人置于如圖8所示的一個人工實驗環境中，該環境為2 m×3.2 m的矩形場地，場地中間有2塊突起的隔板，將場地劃分為3個較小空間，3個小空間由一直道連通.

將機器人置于場地西側空間內，在繪圖區通過鼠標點擊空白區域，給機器人設定一個目標點，經過計算，機器人將目標點映射到實際場地東側空間內，這樣機器人便獲知其與目標點之間的距離.機器人在起始點掃描并感知周圍環境后，開始朝目標點移動.當機器人到達第1道隔板前，如圖9(a)所示，掃描周期被激活，機器人進行掃描并感知周圍環境.監視系統的繪圖區實時將機器人獲取的環境信息繪制成抽象圖形.機器人在第1道隔板前完成周圍環境掃描后，決策并向南移動.當機器人左側紅外傳感器感知到隔板消失，機器人啟動掃描并轉向目標點移動.當機器人越過第1道隔板，機器人再一次啟動掃描，并向目標點移動，如圖9(b)所示.繪圖區的地圖根據機器人逐步反饋的信息生長.最終，機器人到達目標點，繪圖區繪制出完整的地圖及軌跡，如圖9(d)所示.

圖7 監視系統界面Fig.7 Interface of the monitoring system

圖8 實驗環境示意圖Fig.8 Experimental environment

圖9 機器人軌跡圖及環境地圖Fig.9 Traces of the robot and map building

實驗完成時，機器人認為已準確到達目標點.而經過實際測量發現，機器人到達的目標點與實際設定的目標點之間存在一定的偏差，這是由光電編碼盤和電子羅盤的精度誤差造成的.經過多次實驗測量表明，機器人在實驗場地環境內，每米大約產生1.5 cm的誤差，在無路標且環境未知的條件下，該誤差范圍是可接受的.實驗表明，該機器人系統設計開發是有效的.

3 結束語

本文重點為設計一套具有高自主性的網絡遙操作機器人系統，并根據機器人硬件結構設計一套在未知環境下、無路標實現SLAM的算法.算法從設計上很好滿足了ARM9體系處理器的特點，結合機器人所攜帶的傳感器，使定位與地圖繪制過程都具有較好的實時性.

實驗表明，機器人激光測距效果滿足路徑規劃及實時地圖繪制的要求，電子羅盤與里程計定位法的誤差處于可接受范圍內.機器人的局部路徑規劃算法能有效避開障礙物，并選擇可行的路徑.監視系統根據機器人傳回的掃描信息，很好地完成繪圖任務.

下一步的研究將著手提高機器人在SLAM過程中的精度，以及研究網絡遙操作機器人系統實現多機器人協作繪圖的可能性，并進一步將二維靜態環境擴展為三維動態環境建圖.

［1］GOLDBERG K，MASCHA M，GENTNER S，et al.Desktop teleoperation via the world wide web［C］//Proceedings of the 1995 IEEE International Conference on Robotics and Automation.Nagoya，Japan:1995:654-659.

［2］LEONARD J，DURRANT-WHYTE H.Simultaneous map building and localization for an autonomous mobile robot［C］//Proceedings of the IEEE International Workshop on Intelligent Robots and Systems. Osaka， Japan:1991:1442-1447.

［3］DURRANT-WHYTE H，BAILEY T.Simultaneous localization and mapping(SLAM):part I the essential algorithms［J］.Robotics and Automation Magazine，2006，13(2):99-110.

［4］SMITH R，SELF M，CHESSEMAN P.Estimating uncertain spatial relationships in robotics［C］//Proceedings of the IEEE International Conference on Robotics and Automation.Piscataway，USA:IEEE，1987:850.

［5］SMITH R，CHEESEMAN P.On the representation and estimation of spatial uncertainty［J］.The International Journal of Robotics Research，1986，5(4):56-68.

［6］CSORBA M.Simultaneous localization and map building［D］.Oxford:University of Oxford，1997.

［7］DISSANAYAKE M W M G，NEWMAN P，CLARK S，et al.A solution to the simultaneous localization and map building(SLAM)problem［J］.IEEE Transactions on Robotics and Automation，2001，17(3):229-241.

［8］CASTELLANOS J A，TARDOS J D.Mobile robot localization and map building:a multisensor fusion approach［J］.Automatica，2003，39(6):1120-1121.

［9］THRUN S，BURGARD W，FOX D.A probabilistic approach to concurrent mapping and localization for mobile robots［J］.Machine Learning，1998，31(1/3):29-53.

［10］DEMPSTER A，LAIRD A，RUBIN D.Maximum likelihood from incomplete data via the EM algorithm［J］.Journal of the Royal Statistical Society:Series B(Methodological)，1997，39(1):1-38.

［11］BURGARD W，FOX D，JANS H，et al.Sonar-based mapping of large-scale mobile robot environments using EM［C］//Proceedings of the 16th International Conference on Machine Learning.Bled，Slovenia:Morgan Kaufmann，1999:67-76.

［12］THRUN S，BURGARD W，FOX D.A real-time algorithm for mobile robot mapping with application to multi-robot and 3D mapping［C］//Proceedings of IEEE InternationalConference on Robotics and Automation.San Francisco，USA:IEEE，2000:321-328.

［13］THRUN S，BEETZ M，BURGARD W，et al.Probabilistic algorithms and the interactive museum tour-guided robot Minerva［J］.Journal of Robotics Research，2000，19(11):972-1000.

［14］HANHEL D，SCHULZ D，BURGARD W.Mapping building with mobile robots in populated environments［C］//Proceedings of the 2002 IEEE/RSJ International Conference on Intelligent Robots and Systems.Lausanne，Switzerland:IEEE，2002:496-501.

［15］MONTEMERLO M，THRUN S，KOLLER D，et al.FastSLAM:a factored solution to the simultaneous location and mapping problem［C］//Proceedings of the AAAI National Conference on Artificial Intelligence.Edmonton，Canada:AAAI，2002:593-598.

［16］MONTEMERLO M，THRUN S，KOLLER D，et al.FastSLAM 2.0:an improved particle filtering algorithm for simultaneous localization and mapping that provably converges［C］//Proceedings of the International Conferenceon ArtificialIntelligence. Acapulco， Mexico:AAAI，2003:1151-1156.

［17］ARRAS K O，TOMATIS N，JENSEN B T，et al.Multisensor on-the-fly localization:precision and reliability for applications［J］.Robotics and Autonomous System，2001，34(2/3):131-143.

［18］JENSFELT P，KRISTENSEN S.Active global localization for a mobile robot using multiple hypothesis tracking［J］.IEEE Transactions on Robotics and Automation，2001，17(5):748-760.

［19］BORGES G A，ALDON M J.A split-and-merge segmentation algorithm for line extraction in 2-D range images［C］//Proceedings of International Conference on Pattern Recognition.Barcelona，Spain:IEEE，2000:441-444.

［20］JENSFELT P，CHRISTENSEN H.Laser based position acquisition and tracking in an indoor environment［C］//Proceedings of the International Symposium on Robotics and Automation. Saltillo， Coahuila， Mexico:IEEE，1998:331-338.

［21］ULRICH I，BORENSTEIN J.VFH+:reliable obstacle avoidance for fast mobile robots［C］//Proceedings of the 1998 IEEE International Conference on Robotics and Automation.Leuven，Belgium:IEEE，1998:1572-1577.