家庭服務(wù)機器人自主定位控制

王剛 周軍 蘇曉明

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

關(guān)鍵詞：服務(wù)機器人;定位導航;數(shù)據(jù)融合;激光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

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

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

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

1 機器人系統(tǒng)和定位方案的設(shè)計

5對家庭服務(wù)機器人總體的系統(tǒng)架構(gòu)進行設(shè)計和介紹，針對其定位技術(shù)硬件部分的核心傳感模塊進行構(gòu)架出合適搭配，并對其編譯環(huán)境ROS操作系統(tǒng)進行介紹。在對機器人定位技術(shù)的研究現(xiàn)狀進行分析，對其室內(nèi)定位方案進行設(shè)計。

1.1 機器人總體架構(gòu)

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

傳感器模塊內(nèi)部選擇以下4種傳感器：

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

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

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

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

1.2 家庭服務(wù)機器人定位方案

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

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

2 定位算法研究

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

2.1 Hector SLAM算法

2.2 無跡卡爾曼算法

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

2.3 自適應(yīng)無跡卡爾曼算法

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

（1）自適應(yīng)調(diào)整系統(tǒng)噪聲的統(tǒng)計特性。

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

（3）在UKF框架下，加入多維加權(quán)系數(shù)。

完成對無跡卡爾曼的改進，增加其算法的環(huán)境適應(yīng)能力。其流程圖如圖4所示。

2.4 實驗仿真

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

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

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

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

3 結(jié) 論

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

參考文獻

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

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

[3] 徐景碩，周勝明，蔣華君.慣性導航系統(tǒng)的發(fā)展及其關(guān)鍵技術(shù)綜述[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] 吳乃亮，閆飛，卜春光.基于視覺里程計的移動機器人三維場景重構(gòu)[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] 楊俊駒.動態(tài)環(huán)境下基于 ROS的移動機器人路徑規(guī)劃技術(shù)研究[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]邊浩然， 資新運， 王洪濤， 等. 基于變形履帶的搜救偵察機器人設(shè)計與試驗[J]. 兵器裝備工程學報， 2017（3）：143-146.

[12]馬閃閃， 呂昭陽， 王迪， 等. 仿人足球機器人目標定位與追蹤算法改進[J]. 電子技術(shù)， 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 匹配算法的室內(nèi)移動機器人定位[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]陳杰，楊朝，賀子琦，等.電纜隧道中巡檢機器人實現(xiàn)及應(yīng)用[J].電子技術(shù)與軟件工程，2019（17）：73-74.

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

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

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

[21]陳延國，于澎，高振東，等.自主移動機器人定位方法的研究現(xiàn)狀[J].應(yīng)用科技，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.