信息雙向融合的機器人協同導航方法*

任仁凱, 錢偉行， 彭　晨， 祝燕華， 謝　非

(1.南京師范大學 電氣與自動化工程學院，江蘇 南京 210042;2.東南大學 儀器科學與工程學院，江蘇 南京 210096)

?

信息雙向融合的機器人協同導航方法*

任仁凱1, 錢偉行1， 彭晨1， 祝燕華2， 謝非1

(1.南京師范大學 電氣與自動化工程學院，江蘇 南京 210042;2.東南大學 儀器科學與工程學院，江蘇 南京 210096)

根據雙足機器人足部慣性導航系統與輪式機器人航位遞推/同步定位與建圖(SLAM)導航系統不同的誤差發散特性構建信息雙向融合濾波器，即利用雙足機器人導航系統中的誤差零速修正(ZUPT)方法減少輪式機器人導航系統誤差，并利用輪式機器人導航系統中的SLAM算法在一定環境條件下修正雙足機器人的定位誤差，從而同步提高兩類機器人導航系統的定位與航向精度。實驗結果表明:導航系統定位誤差約為行進距離的2.3%，在室內等衛星導航系統失效的環境中可有效提高機器人群體導航系統的綜合導航性能。

雙足機器人； 輪式機器人； 信息雙向融合； 同步定位與建圖； 零速修正

0　引　言

移動機器人是一個集環境感知、動態決策與規劃、行為控制與執行等多功能于一體的綜合系統，適用于復雜的工作任務及多變的工作環境，在軍事行動、工程作業等領域具有很高的應用價值[1,2]。其中，雙足機器人和輪式機器人是兩類較典型的移動機器人。

應用于導航領域時，移動機器人多采用全球導航衛星系統(global navigation satellite system，GNSS)。但在室內等GNSS失效環境中，慣性導航以其自主性強、更新率高、導航信息完整、系統便于安裝等特點，成為移動機器人導航定位的主要手段。目前，雙足機器人采用足部慣性導航方式[3,4]，輪式機器人采用車輛航位遞推(vehicle dead recko-ning，VDR)與同步定位與建圖(simultaneous localization and mapping,SLAM)相結合的導航方式[5,6]，均能取得較好的導航效果。然而，雙足機器人導航系統誤差隨路程發散，長距離行進時精度較低；輪式機器人導航系統采用的VDR算法誤差隨時間發散較快，影響SLAM算法發揮作用。

本文研究了一種基于信息雙向融合的機器人協同導航方法。該方法利用兩種導航系統不同的誤差發散特性，通過無線通信方式進行信息交互，構建信息雙向融合濾波器，從而同步修正其導航信息誤差，同時提高兩者的定位與航向精度。

1　機器人協同導航機理分析

1.1基于零速修正的雙足機器人導航機理

雙足機器人導航系統中，將慣性/地磁測量單元(inertial magnetic measurement unit，IMMU)安裝在機器人足部，與其內部微型導航計算機通過有線或無線通信方式完成數據傳輸，其工作流程如圖1所示。導航開始前要進行磁傳感器誤差標定與補償[3]，以及系統初始對準[4]。足部慣性導航方法通過對IMMU組件上的慣性測量單元(inertial measurement unit，IMU)信息進行捷聯慣導解算獲得機器人足部的姿態、速度和位置信息，利用磁傳感器確定運動的航向信息，并采用基于步態相位檢測的零速修正(ZUPT)方法，實時修正導航系統級誤差和慣性傳感器的隨機誤差。

圖1　雙足機器人導航系統工作流程Fig 1　Working process of navigation system of biped robot

首先定義加速度計三個正交軸的輸出為fx,fy,fz，再定義如下變量

(1)

式中各變量定義詳見文獻[4]。通過上述的檢測方式，可在復雜的足部運動中有效提高步態相位檢測的準確性。由于雙足機器人足部運動的特點，IMMU組件在某些時間間隔里是保持靜止的，從而可采用零速修正的方式估計導航系統級誤差以及陀螺儀與加速度計的部分慣性器件漂移誤差。

1.2基于VDR與SLAM的輪式機器人導航機理

輪式機器人導航系工作流程如圖2所示。

圖2　輪式機器人導航系統工作流程Fig 2　Working process of navigation system of wheeled robot

根據VDR算法過程中系統測量輸出和導航坐標系之間的關系，在不考慮導航系統水平姿態變化的情況下，只要獲得了各參量初始值，即可實現后續的VDR推算過程。然而，單獨的VDR定位誤差隨時間迅速累積，導致系統性能下降，可用SLAM算法進行定位誤差補償。

SLAM算法是機器人在自身位置不確定的未知環境中，通過對外部環境進行識別并加以分析，然后創建地圖，并利用地圖進行定位和導航[7]。首先，對視覺傳感器采集的圖像進行處理與分析，識別特征點并定位，將輪式機器人的視覺投影與空間物體的幾何關系轉換為機器人相對特征點的距離和角度。然后，利用加權最小二乘擬合方法和尺度不變特征變換算法分別從激光信息與視覺信息中提取直線和點特征，進行特征級的信息融合[5]。最后，運用SLAM算法，整體步驟按照預測、觀測、數據關聯、更新、地圖構建的遞推算法進行同步定位與建圖，從而識別環境目標，并進行平滑運動[6]。

輪式機器人采用視覺和激光傳感器信息融合的SLAM算法減少定位誤差，當重復經過某處路標時，即可進行誤差校正，提高導航系統的定位精度。

1.3機器人的協同導航機理

基于以上兩節分析可知，雙足機器人采用IMMU足部安裝的導航定位方案，通過步態相位檢測對導航系統進行間斷性的誤差零速修正，其誤差隨路程發散，在一定路程內可保持較高的定位精度；輪式機器人導航系統采用VDR算法導航，運用視覺和激光傳感器信息融合的SLAM算法進行誤差校正，其誤差隨時間發散，在一定時間內可保持較高的定位精度。

由于采用步態相位檢測和零速修正后導航系統定位精度高于VDR算法，可將雙足機器人導航系統零速修正后的定位信息實時修正輪式機器人的導航系統誤差。此外，雙足機器人行走時晃動較大，而負載小，輪式機器人行駛穩定，負載較大，滿足SLAM算法對載體穩定性較高的要求。因此，將視覺、激光等傳感器安裝在輪式機器人上，經SLAM輔助后的定位信息也可對雙足機器人導航系統進行誤差校正。

雙足機器人與輪式機器人的信息雙向融合協同導航原理如圖3所示。雙足機器人與輪式機器人同步行進，兩者導航系統通過無線通信方式進行數據交換。本研究將無線通訊視為理想狀態，即暫不將數據傳輸導致的導航定位誤差作為本文的研究內容。

圖3　信息雙向融合協同導航原理Fig 3　Principle of information bidirectional fusion cooperative navigation

2　基于最優估計理論的信息雙向融合方法

2.1雙足機器人導航系統誤差建模與修正

雙足機器人導航系統零速修正的誤差模型由平臺誤差角模型、速度誤差模型、定位誤差模型以及慣性傳感器誤差模型組成[3]。

雙足機器人行進過程中，采用SLAM提供的定位信息完成誤差估計與補償。SLAM算法中，系統的狀態向量可表示為

(2)

式中xv(k)=[xvx(k),xvy(k),xvθ(k)]T表示機器人在k時刻的位姿，M=[m1,m2,…,ml]，mi=[xi,yi]T，i=1,2,…,l，表示路標集合，其中,l為路標的標記序號[8]。

當檢測到重復經過某一路標時，其定位誤差即校正為上一時刻經過該路標時的數據。檢測到同一路標的次數越多，定位就越精確。通過SLAM算法對雙足機器人的誤差修正，提高定位精度，有效抑制其誤差隨路程的發散。

2.2輪式機器人導航系統誤差建模與修正

輪式機器人導航系統誤差模型共有9維，由VDR算法的基本導航參數誤差模型和慣性傳感器誤差模型組成。

基本導航參數誤差包括航向誤差、東向速度誤差、北向速度誤差、經度誤差和緯度誤差，狀態建模方程為

(3)

(4)

根據以上誤差微分方程，可獲得系統誤差狀態方程如下

(5)

在行進過程中，輪式機器人VDR算法輸出的定位信息和雙足機器人導航系統提供的定位信息分別可表示為

(6)

式中λWR,LWR分別為輪式機器人提供的經度和緯度，λBR,LBR分別為雙足機器人提供的經度和緯度。

定義導航系統的觀測矢量為

=H(t)X(t)+NBR(t)

(7)

則有觀測方程

Z=HX+V

(8)

式中由于一定行進距離內雙足機器人定位誤差NeBR和NnBR遠小于輪式機器人定位誤差δλ和δL，可忽略，并在觀測方程中加入小量白噪聲作為量測噪聲。

3　機器人協同導航系統定位性能實驗與分析

3.1信息雙向融合的導航定位實驗

對雙足機器人和輪式機器人上安裝的IMMU組件分別進行靜態測試，基本性能如表1所示。

表1　IMMU組件基本性能參數

實驗前，在不存在電磁干擾的環境中完成導航系統的初始對準與磁傳感器標定，標定后水平姿態精度在0.5°左右，磁航向精度在1°左右。實驗中，雙足機器人與輪式機器人同步行進，兩者導航系統通過藍牙無線通信方式進行數據交換。圖4所示為機器人進行實驗時的行進路線地圖，總行進路程約280m，歷時約370s，沿途設有路標1和路標2，其中，起始位置到路標1約100m，路標1到路標2約40m，路標2到返回位置約20m，路標1到終止位置約60m。

圖4　行進路線地圖Fig 4　Map of walking route

采用三種不同方案驗證協同導航方法的有效性,實驗結果如圖5所示。

圖5　不同工作方案下移動機器人導航定位誤差Fig 5　Navigation and positioning error of mobile robot under different working schemes

由圖5可知：以實驗地點地理測繪信息為定位基準，雙足機器人單獨工作方案在實驗結束時刻定位誤差約為9.6m，約為行進路程的3.4 %；輪式機器人單獨工作方案在實驗結束時刻定位誤差約為16.8m，約為行進路程的6.0 %；移動機器人信息雙向融合的協同導航工作方案在實驗結束時刻定位誤差約為6.4m，約為行進路程的2.3 %。

3.2實驗結果分析

分析上述實驗結果，采用信息雙向融合的協同導航方法，可有效提高移動機器人群體導航系統的定位精度。這種導航方式下的導航定位誤差主要來源于以下幾個方面：

1)姿態與航向誤差，主要由IMMU組件的器件誤差、硬軟磁誤差經標定補償后的殘差等方面造成；

2)零速修正誤差，主要由慣性導航系統模型誤差、慣性器件模型誤差以及步態相位的誤檢造成；

3)SLAM與VDR導航定位誤差，主要由視覺與激光傳感器的檢測誤差、信息融合的延遲以及VDR算法誤差等造成。

4　結　論

本文以在室內等GNSS失效環境中移動機器人導航定位方法為研究背景，研究了一種基于信息雙向融合的機器人群體協同導航方案。該方案以同步行進的雙足機器人和輪式機器人為研究對象，采用基于雙足機器人步態相位檢測的誤差零速修正方法為輪式機器人提供較精確的定位信息；并采用輪式機器人導航系統中SLAM算法提供的定位信息實時修正雙足機器人定位誤差，抑制其隨路程的發散。由于可實時通過雙向信息融合實現位置匹配來進行系統誤差的修正，在GNSS失效環境中，移動機器人群體的定位精度相對于單獨工作模式有明顯的提高，具有一定的理論研究與工程應用價值。

[1]Str?mbackP,RantakokkoJ,WirkanderSL,etal.Foot-mountedinertialnavigationandcooperativesensorfusionforindoorpositioning[C]∥Proceedingsofthe2010InternationalTechnicalMee-tingofTheInstituteofNavigation，2010:89-98.

[2]CalusdianJ.Apersonalnavigationsystembasedoninertialandmagneticfieldmeasurements[D].Monterey：NavalPostgraduateSchool，2010.

[3]張金亮,秦永元,梅春波.基于MEMS慣性技術的鞋式個人導航系統[J].中國慣性技術學報,2011,19(3):253-256.

[4]錢偉行,朱欣華,蘇巖.基于足部微慣性/地磁測量組件的個人導航方法[J].中國慣性技術學報,2012,20(5):567-572.

[5]杜釗君,吳懷宇.基于激光測距與雙目視覺信息融合的移動機器人SLAM研究[J].計算機測量與控制,2013,21(1):180-183.

[6]弋英民,劉丁.基于單目視覺的輪式機器人同步定位與地圖構建[J].儀器儀表學報,2010,31(1):117-121.

[7]SmithR,SelfM,CheesemanP.Autonomousrobotvehicles[M].NewYork：Springer，1990:167-193.

[8]張文玲,朱明清,陳宗海.基于強跟蹤UKF的自適應SLAM算法[J].機器人,2010,32(2):190-195.

錢偉行，通訊作者，E—mail：61192@njnu.edu.cn。

Robot cooperative navigation method based on information bidirectional fusion*

REN Ren-kai1, QIAN Wei-xing1, PENG Chen1, ZHU Yan-hua2, XIE Fei1

(1.School of Electrical and Automation Engineering,Nanjing Normal University,Nanjing 210042,China; 2.School of Instrument Science & Engineering,Southeast University,Nanjing 210096,China)

The method builds information bidirectional fusion filters on the basis of different error divergence of the foot-mounted inertial navigation system and dead reckoning/simultaneous localization and mapping(SLAM)navigation system.The zero-velocity update(ZUPT) of biped robot is used to reduce the error of navigation system,while the SLAM algorithm of wheeled robot is used to correct the positioning error,which both improve the positioning and heading precision of two navigation systems synchronously.Experimental results show that,the positioning error of navigation system is about 2.3 % of the walking distance, which can effectively improve the navigation performance of the robot group in the environment without GNSS.

biped robot; wheeled robot; information bidirectional fusion; simultaneous localization and mapping(SLAM); zero-velocity update(ZUPT)

2015—10—23

國家自然科學基金資助項目(61304227)；江蘇省自然科學基金資助項目(BK20141453，BK20130628)；江蘇省高校自然科學基金資助項目(13KJB590001)

U 666.1

A

1000—9787(2016)08—0040—04

任仁凱(1991-)，男，江蘇南京人，碩士研究生，主要研究方向為慣性導航和智能家居設備。

DOI:10.13873/J.1000—9787(2016)08—0040—04