王剛 周軍 蘇曉明

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

關(guān)鍵詞：服務(wù)機(jī)器人;定位導(dǎo)航;數(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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

2 定位算法研究

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

2.1 Hector SLAM算法

2.2 無跡卡爾曼算法

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

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

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

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

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

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

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

2.4 實(shí)驗(yàn)仿真

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

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

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

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

3 結(jié) 論

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

參考文獻(xiàn)

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

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

[3] 徐景碩，周勝明，蔣華君.慣性導(dǎo)航系統(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] 吳乃亮，閆飛，卜春光.基于視覺里程計(jì)的移動機(jī)器人三維場景重構(gòu)[J].華中科技大學(xué)學(xué)報(bào)（自然科學(xué)版），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的移動機(jī)器人路徑規(guī)劃技術(shù)研究[D].蘇州：蘇州大學(xué)，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]邊浩然， 資新運(yùn)， 王洪濤， 等. 基于變形履帶的搜救偵察機(jī)器人設(shè)計(jì)與試驗(yàn)[J]. 兵器裝備工程學(xué)報(bào)， 2017（3）：143-146.

[12]馬閃閃， 呂昭陽， 王迪， 等. 仿人足球機(jī)器人目標(biāo)定位與追蹤算法改進(jìn)[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ī)器人定位[J]. 華中科技大學(xué)學(xué)報(bào)（自然科學(xué)版），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ī)器人實(shí)現(xiàn)及應(yīng)用[J].電子技術(shù)與軟件工程，2019（17）：73-74.

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

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

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

[21]陳延國，于澎，高振東，等.自主移動機(jī)器人定位方法的研究現(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.