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

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

?

信息雙向融合的機(jī)器人協(xié)同導(dǎo)航方法*

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

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

根據(jù)雙足機(jī)器人足部慣性導(dǎo)航系統(tǒng)與輪式機(jī)器人航位遞推/同步定位與建圖(SLAM)導(dǎo)航系統(tǒng)不同的誤差發(fā)散特性構(gòu)建信息雙向融合濾波器，即利用雙足機(jī)器人導(dǎo)航系統(tǒng)中的誤差零速修正(ZUPT)方法減少輪式機(jī)器人導(dǎo)航系統(tǒng)誤差，并利用輪式機(jī)器人導(dǎo)航系統(tǒng)中的SLAM算法在一定環(huán)境條件下修正雙足機(jī)器人的定位誤差，從而同步提高兩類機(jī)器人導(dǎo)航系統(tǒng)的定位與航向精度。實(shí)驗(yàn)結(jié)果表明:導(dǎo)航系統(tǒng)定位誤差約為行進(jìn)距離的2.3%，在室內(nèi)等衛(wèi)星導(dǎo)航系統(tǒng)失效的環(huán)境中可有效提高機(jī)器人群體導(dǎo)航系統(tǒng)的綜合導(dǎo)航性能。

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

0　引　言

移動(dòng)機(jī)器人是一個(gè)集環(huán)境感知、動(dòng)態(tài)決策與規(guī)劃、行為控制與執(zhí)行等多功能于一體的綜合系統(tǒng)，適用于復(fù)雜的工作任務(wù)及多變的工作環(huán)境，在軍事行動(dòng)、工程作業(yè)等領(lǐng)域具有很高的應(yīng)用價(jià)值[1,2]。其中，雙足機(jī)器人和輪式機(jī)器人是兩類較典型的移動(dòng)機(jī)器人。

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

本文研究了一種基于信息雙向融合的機(jī)器人協(xié)同導(dǎo)航方法。該方法利用兩種導(dǎo)航系統(tǒng)不同的誤差發(fā)散特性，通過無線通信方式進(jìn)行信息交互，構(gòu)建信息雙向融合濾波器，從而同步修正其導(dǎo)航信息誤差，同時(shí)提高兩者的定位與航向精度。

1　機(jī)器人協(xié)同導(dǎo)航機(jī)理分析

1.1基于零速修正的雙足機(jī)器人導(dǎo)航機(jī)理

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

圖1　雙足機(jī)器人導(dǎo)航系統(tǒng)工作流程Fig 1　Working process of navigation system of biped robot

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

(1)

式中各變量定義詳見文獻(xiàn)[4]。通過上述的檢測(cè)方式，可在復(fù)雜的足部運(yùn)動(dòng)中有效提高步態(tài)相位檢測(cè)的準(zhǔn)確性。由于雙足機(jī)器人足部運(yùn)動(dòng)的特點(diǎn)，IMMU組件在某些時(shí)間間隔里是保持靜止的，從而可采用零速修正的方式估計(jì)導(dǎo)航系統(tǒng)級(jí)誤差以及陀螺儀與加速度計(jì)的部分慣性器件漂移誤差。

1.2基于VDR與SLAM的輪式機(jī)器人導(dǎo)航機(jī)理

輪式機(jī)器人導(dǎo)航系工作流程如圖2所示。

圖2　輪式機(jī)器人導(dǎo)航系統(tǒng)工作流程Fig 2　Working process of navigation system of wheeled robot

根據(jù)VDR算法過程中系統(tǒng)測(cè)量輸出和導(dǎo)航坐標(biāo)系之間的關(guān)系，在不考慮導(dǎo)航系統(tǒng)水平姿態(tài)變化的情況下，只要獲得了各參量初始值，即可實(shí)現(xiàn)后續(xù)的VDR推算過程。然而，單獨(dú)的VDR定位誤差隨時(shí)間迅速累積，導(dǎo)致系統(tǒng)性能下降，可用SLAM算法進(jìn)行定位誤差補(bǔ)償。

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

輪式機(jī)器人采用視覺和激光傳感器信息融合的SLAM算法減少定位誤差，當(dāng)重復(fù)經(jīng)過某處路標(biāo)時(shí)，即可進(jìn)行誤差校正，提高導(dǎo)航系統(tǒng)的定位精度。

1.3機(jī)器人的協(xié)同導(dǎo)航機(jī)理

基于以上兩節(jié)分析可知，雙足機(jī)器人采用IMMU足部安裝的導(dǎo)航定位方案，通過步態(tài)相位檢測(cè)對(duì)導(dǎo)航系統(tǒng)進(jìn)行間斷性的誤差零速修正，其誤差隨路程發(fā)散，在一定路程內(nèi)可保持較高的定位精度；輪式機(jī)器人導(dǎo)航系統(tǒng)采用VDR算法導(dǎo)航，運(yùn)用視覺和激光傳感器信息融合的SLAM算法進(jìn)行誤差校正，其誤差隨時(shí)間發(fā)散，在一定時(shí)間內(nèi)可保持較高的定位精度。

由于采用步態(tài)相位檢測(cè)和零速修正后導(dǎo)航系統(tǒng)定位精度高于VDR算法，可將雙足機(jī)器人導(dǎo)航系統(tǒng)零速修正后的定位信息實(shí)時(shí)修正輪式機(jī)器人的導(dǎo)航系統(tǒng)誤差。此外，雙足機(jī)器人行走時(shí)晃動(dòng)較大，而負(fù)載小，輪式機(jī)器人行駛穩(wěn)定，負(fù)載較大，滿足SLAM算法對(duì)載體穩(wěn)定性較高的要求。因此，將視覺、激光等傳感器安裝在輪式機(jī)器人上，經(jīng)SLAM輔助后的定位信息也可對(duì)雙足機(jī)器人導(dǎo)航系統(tǒng)進(jìn)行誤差校正。

雙足機(jī)器人與輪式機(jī)器人的信息雙向融合協(xié)同導(dǎo)航原理如圖3所示。雙足機(jī)器人與輪式機(jī)器人同步行進(jìn)，兩者導(dǎo)航系統(tǒng)通過無線通信方式進(jìn)行數(shù)據(jù)交換。本研究將無線通訊視為理想狀態(tài)，即暫不將數(shù)據(jù)傳輸導(dǎo)致的導(dǎo)航定位誤差作為本文的研究內(nèi)容。

圖3　信息雙向融合協(xié)同導(dǎo)航原理Fig 3　Principle of information bidirectional fusion cooperative navigation

2　基于最優(yōu)估計(jì)理論的信息雙向融合方法

2.1雙足機(jī)器人導(dǎo)航系統(tǒng)誤差建模與修正

雙足機(jī)器人導(dǎo)航系統(tǒng)零速修正的誤差模型由平臺(tái)誤差角模型、速度誤差模型、定位誤差模型以及慣性傳感器誤差模型組成[3]。

雙足機(jī)器人行進(jìn)過程中，采用SLAM提供的定位信息完成誤差估計(jì)與補(bǔ)償。SLAM算法中，系統(tǒng)的狀態(tài)向量可表示為

(2)

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

當(dāng)檢測(cè)到重復(fù)經(jīng)過某一路標(biāo)時(shí)，其定位誤差即校正為上一時(shí)刻經(jīng)過該路標(biāo)時(shí)的數(shù)據(jù)。檢測(cè)到同一路標(biāo)的次數(shù)越多，定位就越精確。通過SLAM算法對(duì)雙足機(jī)器人的誤差修正，提高定位精度，有效抑制其誤差隨路程的發(fā)散。

2.2輪式機(jī)器人導(dǎo)航系統(tǒng)誤差建模與修正

輪式機(jī)器人導(dǎo)航系統(tǒng)誤差模型共有9維，由VDR算法的基本導(dǎo)航參數(shù)誤差模型和慣性傳感器誤差模型組成。

基本導(dǎo)航參數(shù)誤差包括航向誤差、東向速度誤差、北向速度誤差、經(jīng)度誤差和緯度誤差，狀態(tài)建模方程為

(3)

(4)

根據(jù)以上誤差微分方程，可獲得系統(tǒng)誤差狀態(tài)方程如下

(5)

在行進(jìn)過程中，輪式機(jī)器人VDR算法輸出的定位信息和雙足機(jī)器人導(dǎo)航系統(tǒng)提供的定位信息分別可表示為

(6)

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

定義導(dǎo)航系統(tǒng)的觀測(cè)矢量為

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

(7)

則有觀測(cè)方程

Z=HX+V

(8)

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

3　機(jī)器人協(xié)同導(dǎo)航系統(tǒng)定位性能實(shí)驗(yàn)與分析

3.1信息雙向融合的導(dǎo)航定位實(shí)驗(yàn)

對(duì)雙足機(jī)器人和輪式機(jī)器人上安裝的IMMU組件分別進(jìn)行靜態(tài)測(cè)試，基本性能如表1所示。

表1　IMMU組件基本性能參數(shù)

實(shí)驗(yàn)前，在不存在電磁干擾的環(huán)境中完成導(dǎo)航系統(tǒng)的初始對(duì)準(zhǔn)與磁傳感器標(biāo)定，標(biāo)定后水平姿態(tài)精度在0.5°左右，磁航向精度在1°左右。實(shí)驗(yàn)中，雙足機(jī)器人與輪式機(jī)器人同步行進(jìn)，兩者導(dǎo)航系統(tǒng)通過藍(lán)牙無線通信方式進(jìn)行數(shù)據(jù)交換。圖4所示為機(jī)器人進(jìn)行實(shí)驗(yàn)時(shí)的行進(jìn)路線地圖，總行進(jìn)路程約280m，歷時(shí)約370s，沿途設(shè)有路標(biāo)1和路標(biāo)2，其中，起始位置到路標(biāo)1約100m，路標(biāo)1到路標(biāo)2約40m，路標(biāo)2到返回位置約20m，路標(biāo)1到終止位置約60m。

圖4　行進(jìn)路線地圖Fig 4　Map of walking route

采用三種不同方案驗(yàn)證協(xié)同導(dǎo)航方法的有效性,實(shí)驗(yàn)結(jié)果如圖5所示。

圖5　不同工作方案下移動(dòng)機(jī)器人導(dǎo)航定位誤差Fig 5　Navigation and positioning error of mobile robot under different working schemes

由圖5可知：以實(shí)驗(yàn)地點(diǎn)地理測(cè)繪信息為定位基準(zhǔn)，雙足機(jī)器人單獨(dú)工作方案在實(shí)驗(yàn)結(jié)束時(shí)刻定位誤差約為9.6m，約為行進(jìn)路程的3.4 %；輪式機(jī)器人單獨(dú)工作方案在實(shí)驗(yàn)結(jié)束時(shí)刻定位誤差約為16.8m，約為行進(jìn)路程的6.0 %；移動(dòng)機(jī)器人信息雙向融合的協(xié)同導(dǎo)航工作方案在實(shí)驗(yàn)結(jié)束時(shí)刻定位誤差約為6.4m，約為行進(jìn)路程的2.3 %。

3.2實(shí)驗(yàn)結(jié)果分析

分析上述實(shí)驗(yàn)結(jié)果，采用信息雙向融合的協(xié)同導(dǎo)航方法，可有效提高移動(dòng)機(jī)器人群體導(dǎo)航系統(tǒng)的定位精度。這種導(dǎo)航方式下的導(dǎo)航定位誤差主要來源于以下幾個(gè)方面：

1)姿態(tài)與航向誤差，主要由IMMU組件的器件誤差、硬軟磁誤差經(jīng)標(biāo)定補(bǔ)償后的殘差等方面造成；

2)零速修正誤差，主要由慣性導(dǎo)航系統(tǒng)模型誤差、慣性器件模型誤差以及步態(tài)相位的誤檢造成；

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

4　結(jié)　論

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

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

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

[3]張金亮,秦永元,梅春波.基于MEMS慣性技術(shù)的鞋式個(gè)人導(dǎo)航系統(tǒng)[J].中國慣性技術(shù)學(xué)報(bào),2011,19(3):253-256.

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

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

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

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

[8]張文玲,朱明清,陳宗海.基于強(qiáng)跟蹤UKF的自適應(yīng)SLAM算法[J].機(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

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

U 666.1

A

1000—9787(2016)08—0040—04

任仁凱(1991-)，男，江蘇南京人，碩士研究生，主要研究方向?yàn)閼T性導(dǎo)航和智能家居設(shè)備。

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