家庭服務機器人自主定位控制

王剛 周軍 蘇曉明

摘 要：家庭服務機器人是目前機器人領域的一個研究熱點，家庭服務機器人的研發可以減輕社會和家庭負擔，緩解人口老齡化等問題。自主定位技術是家庭服務機器人研發的核心技術之一。本文針對家庭服務機器人定位技術研究，進行了其機械結構的設計以及傳感器的選型提供方案，也對ROS編譯環境進行介紹。最重要的是設計出一種基于激光SLAM技術為核心的傳感器數據融合方法適用于家庭的微動態環境下的定位方法。在對系統進行運動學模型和坐標系模型建模的基礎上，提出一種傳統的Hector SLAM算法并對其進行改進成自適應無跡卡爾曼（AUKF）融合算法，提高了定位的穩定性和準確性。

關鍵詞：服務機器人;定位導航;數據融合;激光SLAM

Abstract：Home service robot is a research hotspot in the field of robotics. The research and development of family service robot can reduce the burden of the family and the aging of the population. Autonomous positioning technology is one of the core technologies in the research and development of home service robots. This paper focuses on the research of home service robot positioning technology， the design of its mechanical structure and the selection of sensors， and also introduces the ROS compilation environment. The most important thing is to design a sensor data fusion method based on laser SLAM technology， which is suitable for home positioning in micro-dynamic environment. Based on the kinematics model and coordinate system model of the system， a traditional Hector SLAM algorithm is proposed and improved into an adaptive unscented Kalman （AUKF） fusion algorithm to improve the stability of the positioning and accuracy.

Key words：service robot;positioning;data fusion;laser SLAM

機器人技術是一個集機械，信息，計算機科學和自動控制理論于一體的多學科系統[1-2]。 它不僅具有自身技術的高附加值，而且還具有廣泛的產品。這已說明機器人已是成為重要的技術輻射平臺，增強了軍事防御力量，提高了操控性，提高事態發展水平，推動經濟整體發展，很大程度上改善人民生活水平具有重要意義[3-5]。

我國2015年提出《中國制造2025》中更是把機器人作為未來發展的十大重點領域之一[6-8]。其中服務機器人更是本世紀最有前途的應用之一。有人預測，未來對服務機器人的需求務機器人的未來發展去制定國家短期和長期的研究計劃[9-11]。在2006年我國也提出了863計劃，把服務機器人的“智能機器人技術”視為研究方向[12-14]。對移動機器人的基本智能問題，Durrant-Whyte在1992年得出結論，機器人移動主要反映在三個問題上：

（1）“我現在在哪里？”（2）“我要去哪里？”（3）“如何到達那里”，對應著移動機器人的定位，路徑規劃和運動控制問題[15-19]。研究了家庭定位機器人的自主定位技術。自主定位問題是移動機器人實現特定功能的基礎和前提，貫穿于自主導航之中，是眾多問題中最先需要解決的一個問題，因為在一定程度上是反應了機器人的智能情況，如果其問題沒有解決好，就可能導致無法實現機器人本身需要執行的功能[20-21]。

1 機器人系統和定位方案的設計

5對家庭服務機器人總體的系統架構進行設計和介紹，針對其定位技術硬件部分的核心傳感模塊進行構架出合適搭配，并對其編譯環境ROS操作系統進行介紹。在對機器人定位技術的研究現狀進行分析，對其室內定位方案進行設計。

1.1 機器人總體架構

家庭服務機器人總體框架主要為機械模塊、傳感模塊、控制模塊、驅動模塊、軟件模塊五大模塊。機器模塊是機器人的外部機械結構。傳感模塊是檢查周圍環境和機器人本身位姿狀態的傳感器集合，也是定位技術的核心所在。控制模塊則是機器人的核心，包含著許多硬件和控制器，負責實現機器人正常運行所有功能。驅動模塊保證機器人運動的位置、方向、速度。軟件模塊則是保證系統控制模塊的有效運行。其結構示意圖如圖1所示。

傳感器模塊內部選擇以下4種傳感器：

（1）激光雷達：作為獲取外部環境信息的主要傳感器，其數據的精確度就是構建地圖和導航精度的基礎。激光雷達需要對返回的數據進行預處理并作為繪制柵格地圖的數據重要來源。

（2）慣性測量單元（IMU）：內部封裝三個方向的陀螺儀和加速度計裝置，來表示機器人的位姿，這也是在現代SLAM技術中機器人解決自主定位技術的關鍵信息。

（3）超聲波測距：用于增加機器人定位的穩定性，通過檢測機器人與物體之間的間距以防止雷達模塊數據出現錯誤。

（4） 紅外測距：應用于機器人在進行自身定位時發生物體間的碰撞。

1.2 家庭服務機器人定位方案

早期室內機器人定位主要為傳感器定位，如慣性導航法和航跡推算法等[22-24]。然而，這些方法存在缺點，難以單個直接應用于家庭微動態環境進行自主定位，并且一些傳感器可能產生一些累積誤差。特別是家庭環境這種動態環境，由于無法使用GPS，定位精度要求高等特點和限制，更是增加了定位的難度，使得單一的定位方法難以適用。

針對單個傳感器所需精度不滿足實際情況的問題許多學者近年來進行融合多個傳感器的數據問題研究，通過傳感器間相互進行數據補償調整精度，從而實現更好的機器人定位算法。當前來講，多個傳感器融合進行相互補償定位數據方法主要以卡爾曼濾波，粒子濾波和貝葉斯理論三種方法為主。其中卡爾曼濾波與貝葉斯理論相比具有更好的動態性能，與粒子濾波相比又進行了簡化，不需要龐大的計算量和考慮粒子的有效性和多樣性問題。因此卡爾曼濾波算法下的多個傳感器數據補償已經在多種場合作為高效實用的最優估計算法。卡爾曼濾波算法發展過程示意圖如圖2所示。

2 定位算法研究

基于卡爾曼濾波的EKF-SLAM方法提出的Hector SLAM算法進行逐步優化成自適應性無跡卡爾曼算法，并進行仿真對比，查看優化后對定位精確度糾正的分析。

2.1 Hector SLAM算法

2.2 無跡卡爾曼算法

雖然Hector SLAM利用多分辨率地圖等方法使得避免陷入局部極值，但是在客廳較大的環境中依然無法準確定位，只依靠激光雷達數據計算位置容易產生較大偏差。所以要使用多傳感器的融合，如利用卡爾曼濾波算法，融合激光數據與里程計數據來減小誤差。又由于真實環境下是處于非線性狀態，所以無跡卡爾曼濾波器（UKF）來執行傳感器數據融合估計得到時刻的在線位姿估計。

2.3 自適應無跡卡爾曼算法

無跡卡爾曼濾波方法在一般簡單的環境下可以很準確估計出機器人當前時刻的狀態。但是，在現實生活中，根據環境不同會導致模型的不準確性使得系統噪聲發生變化，導致濾波效果會變差，此時預測的狀態結果會變得不準確。使用Sage-Husa自適應濾波算法，在EKF的原有基礎上對機器人的位姿狀態估計的精度和魯棒性進行改善。主要步驟如下：

（1）自適應調整系統噪聲的統計特性。

（2）然后當除去量測噪聲協方差矩陣估計的無偏估計。

（3）在UKF框架下，加入多維加權系數。

完成對無跡卡爾曼的改進，增加其算法的環境適應能力。其流程圖如圖4所示。

2.4 實驗仿真

根據上文分析，我們認為激光SLAM的定位相對穩定，特別是角度的估計在室內環境認為相對準確。而里程計的實際量測特性容易發生變化。故取自適應向量，不同算法的定位軌跡如圖5所示。

由圖2.3可見，AUKF對定位位置精確度和運行軌跡平滑性上而言對UKF都是有所改進的，UKF和AUKF都可以通過抑制累積誤差，能與真實值的軌跡近似相符。

進一步的，對單純使用航跡推算、Hector SLAM和引入UKF、AUKF后的位姿誤差進行統計分析，其位置誤差和角度誤差的概率密度如圖6和7，均值如表1。

分析圖6、圖7和表1，使用AUKF比單純的激光定位，位置精度提高了56%，角度精度提高了40%;與使用傳統UKF融合算法相比，位置精度提高約14%，角度精度提高68%。從仿真結果來看，通過AUKF融合Hector SLAM的定結果和里程計數據，可以有效提高定位的結果。

3 結 論

機器人作為《中國制造2025》中未來發展的十大重點領域之一。機器人深度學習等相關性的理論知識和技術逐漸變得成熟，使得機器人技術得到迅速的發展。尤其是服務機器人，因其能直接造福人類的日常生活，正成為機器人領域新的藍海和重點發展方向。而其家庭服務機器人真正實現自主定位所涉及問題有很多，而針對每個問題都會有多樣的解決辦法，來滿足不同情況下的需求問題。且隨著自主定位技術的逐漸成熟，機器人可以具有更好的環境適應能力，魯棒性和效率也會有很大提升，從而實現服務人們的最終目的。

參考文獻

[1] 王田苗，陶永，陳陽.服務機器人技術研究現狀與發展趨勢[J].中國科學（信息科學），2012，42（9）：1049-1066.

[2] 與人共融的家庭服務機器人定位與導航[D].濟南：山東大學，2015.

[3] 徐景碩，周勝明，蔣華君.慣性導航系統的發展及其關鍵技術綜述[J].科技信息，2009，（35）：865-866.

[4] LUCAS B D， KANADE T. An iterative image registration technique with an application to stereo vision （DARPA）[J]. Nutrient Cycling in Agroecosystems， 1981， 83（1）：13-26.

[5] ZHANG Z. Iterative point matching for registration of free form curves and surfaces[J]. International Journal of Computer Vision，1994， 13（2）：119-152.

[6] 吳乃亮，閆飛，卜春光.基于視覺里程計的移動機器人三維場景重構[J].華中科技大學學報（自然科學版），2015，43（S1）：337-340.

[7] GUO B，DAI H，LI Z.A visual-attention-based 3D mapping method for mobile robots[J].Acta Automatica Sinica，2017，43（7）：1248-1256.

[8] ZHOU R，YUAN Q，GU X，et al.Spatial pyramid VLAD[C]// Proc. of Visual Communications and Image Processing Conference.Valletta：IEEE，2015：342-345.

[9] 楊俊駒.動態環境下基于 ROS的移動機器人路徑規劃技術研究[D].蘇州：蘇州大學，2016.

[10]ZHOU H， ZOU D， PEI L， et al. StructSLAM： visual SLAM with building structure lines[J]. IEEE Transactions on Vehicular Technology， 2015， 64（4）： 1364-1375.

[11]邊浩然， 資新運， 王洪濤， 等. 基于變形履帶的搜救偵察機器人設計與試驗[J]. 兵器裝備工程學報， 2017（3）：143-146.

[12]馬閃閃， 呂昭陽， 王迪， 等. 仿人足球機器人目標定位與追蹤算法改進[J]. 電子技術， 2014 （11）： 9-13.

[13]VAN DER MERWE R， DOUCET A， DE FREITAS N， et al.The unscented particle filter[C]// Denver： NIPS， 2010：584-590.

[14]顧文華， 周波， 戴先中. 基于 ICP 匹配算法的室內移動機器人定位[J]. 華中科技大學學報（自然科學版），2013， 41（s1）： 262-266.

[15]SHEN G W， ZETIK R， THOMA R S. Performance comparison of TOA and TDOA based locatin estimation algorithms in LOS environment[C]// 5th Workshop on Positioning， Navigation and Communication， Hannover， Germany. USA： IEEE Computer Society， 2014： 71-78.

[16]ALQUDAH R， BASAQNI S. On the effects of multiple beacon on localization for wireless sensor networks[C]// IEEE International Conference on Wireless Telecommunications Symposium， Pomona， CA， USA， April 2013. USA： IEEE， 2013： 304-309.

[17]陳杰，楊朝，賀子琦，等.電纜隧道中巡檢機器人實現及應用[J].電子技術與軟件工程，2019（17）：73-74.

[18]劉送永，崔玉明.煤礦井下定位導航技術研究進展[J].礦業研究與開發，2019，39（7）：114-120.

[19]廖自威.激光雷達/微慣性室內自主建圖與導航技術研究[D].南京：南京航空航天大學，2016.

[20]王志文，郭戈.移動機器人導航技術現狀與展望[J].機器人，2003，25（5）：470-474.

[21]陳延國，于澎，高振東，等.自主移動機器人定位方法的研究現狀[J].應用科技，2002（11）：41-43.

[22]MURARTAL R，TARDS J D. ORB-SLAM2：an open-source SLAM system for monocular，stereo，and RGB-D cameras[J]. IEEE transactions on robotics，2017，33（5）：1255-1262.

[23]PENG Yan-fei，PENG Jian-jun，LI Ji-ping，et al. Design and development of the fall detection system based on point cloud[J]. Procedia Computer Science，2019，147：271-275.

[24]ENGEL J，KOLTUN V，CREMERS D. Direct sparse odometry[J]. IEEE Transactions on Pattern Analysis and Machine Intelligence，2018，40（3）：611-625.