采用混合方案的移動運輸機器人的定位與跟蹤

史宜巧，周湘貞

（1.江蘇電子信息職業學院智能制造學院，江蘇 淮安 223003；2.北京航空航天大學計算機學院，北京 100191；3.鄭州升達經貿管理學院信息工程系，河南 鄭州 451191）

1 引言

自動引導車輛（Automated Guided Vehicles，AGVs）主要用于倉庫、配送中心和制造工廠，實現物流的自動化。而實時庫存管理和精細化作業需要小的運輸單位，同時短生產周期需要靈活的物料流動。滿足這些要求的有效解決方案就是通過采用小型移動機器人來實現[1-2]，如許多國內物流企業從國外引進的“Kiva移動實現系統（Mobile Fulfillment System，MFS）”。隨著自動化和人力節省的發展趨勢，小型AGVs的低成本是許多物流應用企業的首選，同時也是當前該領域的一個研究熱點。文獻[3]針對目前所使用的激光掃描儀不適合小型移動機器人裝配的不足，提出了一種由智能小車和低成本的360°激光雷達組成的二維環境地圖構建系統，通過提取周圍環境特征信息和軟件系統完成二維環境地圖的實時創建，以獲得更為精確的環境地圖，并成功應用于室內小型移動機器人的視覺導航中；文獻[4]提出了超寬帶（UltraWide-Band，UWB）距離測量作為跟蹤倉庫中的車輛；文獻[5]采用先進的激光導引作為AGV的導引方式，能夠實現平面內任意方向上的移動；文獻[6]采用基于雙目視覺的視覺里程計實現移動機器人在未知環境中的自主定位；文獻[7]概述了無線傳感器網絡（Wireless Sensor Networks，WSNs）在物流中的應用，但僅采用WSNs定位移動機器人的缺點是精度相對較低，且難以進行機動對接。為此，提出了采用基于IEEE 802.15.4a WSNs標準的nanoLOCWSNs作為通信和全局實時定位系統的WSNs，并采用激光測距儀作為安全和檢測地標及局部定位。實驗結果表明，提出的混合方案能夠很好地實現對移動機器人的定位與跟蹤，具有較小的跟蹤誤差。

2 機械輪式全向移動機器人

某國有物流企業從國外引進的Kiva運輸機器人，如圖1所示。它可以在制造工廠、配送中心或倉庫中運送（600×400）mm的貨箱。該機器人配備4個機械輪，以提供全向移動；機器人還配備了2個激光測距儀（Sick S300），以提供安全操作和局部定位。

移動機器人裝備了機械輪和電子驅動，能夠提供3個自由度，移動機器人的單個機械輪，如圖2所示。它由一個中心輪轂和自由移動的滾輪構成，滾輪安裝在中心輪轂周邊的45°上，機器人有4個電機驅動機械輪。移動系統的車輪配置，如圖3所示。機器人總體呈長方體，如圖1所示。

圖1 某國有物流企業進口的全向移動運輸機器人Fig.1 Omnidirectional Mobile Transport Robot Imported by a State-Owned Logistics Enterprise

圖2 機械輪Fig.2 Mechanical Wheels

圖3 帶機械輪的全向移動機器人移動系統Fig.3 Mobile System of Omnidirectional Mobile Robot with Mechanical Wheels

2.1 移動機器人的運動方程

移動機器人能夠在x和y方向以及圍繞z軸旋轉θ提供任何所期望的運動。機器人框架的速度是四輪速度的函數［8］：

式中：r—車輪的半徑；ɑ、b—由機器人的尺寸給出，如圖3所示。式（1）的逆即將機器人框架的速度變換為車輪的速度：

為了獲得移動機器人在世界坐標系中的位置，如圖4所示。必須將機器人框架的速度變換到世界坐標系：

圖4 世界坐標系和機器人框架Fig.4 World Coordinate System and Robot Frame

世界坐標系中給定的運動也可以借助其逆將其變換為機器人框架的速度：

2.2 控制結構

移動機器人的驅動單元由4個帶有傳動齒輪裝置和編碼器的直流伺服電機構成。機器人的車載計算機為世界坐標系中的位置提供級聯控制器，控制器結構，如圖5所示。矩陣V包含世界坐標中的速度與機器人框架中對應速度之間的變換，雅可比矩陣J及其偽逆J*提供機器人框架中的速度與相關輪速之間的變換。由于有內部傳感器，故一些內部狀態在外部是可見的：如通過內部轉速表測得的單個車輪的轉動速度，通過MEMS陀螺儀測得的機器人的方向θ和方向變化

圖5 移動機器人的控制器結構Fig.5 Controller Structure of Mobile Robot

3 移動運輸機器人的位置跟蹤算法

3.1 NanoLOC無線傳感器網絡

IEEE 802.15.4a指定了2種基于UWB和信道信令系統（Channel Signaling System，CSS）的可選信令格式，具有精確的測距能力。Nanotron技術公司開發了一種可作為實時定位系統的WSNs。在該定位系統中，兩個無線節點之間的距離由對稱雙面雙向測距（Symmetrical Double-Sided Two Way Ranging，SDSTWR）確定。SDS-TWR通過信號傳播延遲進行距離測量［9］，它通過對稱測量兩側的RToF來估計兩個節點之間的距離；SDS-TWR采用信號傳輸中的2個延遲來確定兩個節點之間的距離。這種技術測量往返時間，避免了同步時鐘的需要。時間測量從節點A發送一個數據包開始，當節點B接收到來自于節點A的數據包時，節點B將開始其測量，當它將其發送回前一個發射器時就停止操作，當節點A接收到來自于節點B的確認時，將接收到的數據包中的累積時間值用于計算兩個站點之間的距離。由節點A測得的時間減去由節點B測得的時間的差值是信號傳播時間的2倍。為避免時鐘漂移，對距離測量進行2次對稱測量［9］。信號傳播時間td計算為：

式中：T1、T4—分別在第一次和第二次往返中在節點A中測得的延遲時間；T2、T3—分別在第一次和第二次往返中在節點B中測得的延遲時間。這種雙面測量消除了由于時鐘漂移引起的一階誤差。

3.2 基于粒子濾波的樣本處理算法

由NanoLOC測距獲得的距離測量值存在噪聲。為了用這種帶有噪聲的測量值準確估計移動機器人的姿態，采用基于貝葉斯濾波［10］的粒子濾波（Particle Filter，PF）方法。在PF中，一組N個樣本的集合S分布在環境中或已知地點，一個樣本s是通過笛卡爾坐標和方位來定義的，移動機器人的姿態估計及其關于正確性的不確定性是用樣本來表示的。算法實現由2個階段構成：預測階段和更新階段。

在預測階段，把運動信息ut應用于每個樣本（1≤i≤N）。預測階段也稱為運動模型，運動模型的結果是一組新的表示移動機器人執行動作ut后的位置的樣本集；在更新階段，把距離測量值的集合Dt用來為每個樣本分配一個重要性權值w，該重要性權值遵從概率，也就是說，在環境中一個點上的距離測量值Dt的概率是由樣本和通過采用來自于地圖m的信息確定的。錨和地標存儲在m個位置中。更新階段的結果（也稱為測量更新）是預測階段的樣本集，具有相應的重要性權值集。這兩組集合共同表示移動機器人的當前位置的可能性；在更新階段之后，執行重采樣步驟。在重采樣步驟中，去除低重要性權值的樣本，并復制高重要性權值的樣本。重采樣的結果是表示移動機器人的當前位置的N個樣本集St；在下一個時間步，集合St被用作為St-1；從樣本集St中提取移動機器人的姿態有2種方法：一種方法是采用全部樣本的平均值，第二種方法是采用有最高重要性權值的樣本。算法實現的原理圖，如圖6所示。算法融合了來自于nanoLOCWSN和激光測距儀的距離測量值。

圖6 基于粒子濾波的樣本處理算法實現原理Fig.6 Principle of Sample Processing Algorithm Based on Particle Filtering

3.3 NanoLOC測量模型

在更新階段，采用nanoLOC測量模型來計算每個樣本s的重要性因子w，測量模型就是概率密度函數，它描述了測量的性質和誤差。測量集dnanLOC，t包含到錨的距離測量值。為了估計nanoLOC距離測量值的密度函數，采用4個錨的視距（Line-of-Sight，LoS）測量值，同時機器人在它們之間的一條直線路徑上移動。當機器人移動時，地面真實值通過到2個壁的激光測量值來估計。到4個錨的測量值的誤差是測量距離和機器人位置到錨ɑ的歐氏距離的差值。誤差依賴于錨的位置和環境，誤差分布的中值和標準偏差是不同的，但均為高斯結構，因此，可以采用高斯分布作為nanoLOC的概率密度函數：

式中：（xɑ，yɑ）—錨ɑ的位置；（xi，y）i—樣本的笛卡爾坐標。歐氏距離和實測距離與固定高斯分布中的錨特定常數d［ɑ］const一起使用得到：

式中：d［ɑ］const—距離誤差的中值；這種固定高斯分布的優勢在于，為了保證Σp=1，不需要定位過程中的歸一化；如果多個樣本相差太大，可以在環境中抽取新樣本，重新定位移動機器人。樣本i的重要性權值計算如下：

可見，重要性權值w是到A個錨的測量值和到2個地標（l=1，2）測量值的概率的乘積。下面給出如何檢測地標。顯然，如果沒有地標檢測，則重要性權值等于到A個錨的距離測量值的概率。

3.4 地標檢測

地標檢測是對傳感器數據進行檢查。在實驗環境中，兩個圓柱體位于對接臺前面，這兩個圓柱體用作地標，所使用的傳感器是激光測距儀，因此必須對激光測距數據B中的圓進行檢測。B是L束光束b的集合，它們是通過已知角度ɑ上的距離測量值bˉ來描述的：

采用圓柱體是因為圓柱體是對稱的，它們從每個角度看都具有相同的特性。采用2個地標是因為到2個地標的距離測量值可以保證精確定位。

為了對激光測距數據中的圓進行檢測，采用計算每一光束的平方殘差的平均值來進行，如果平方殘差因子的平均值低于給定的閾值，就檢測圓。為了計算光束bl的平方殘差的平均值，假設這束光束影響地標中心，然后計算出光束的數目。這個數目依賴于對光束bl測得的距離、地標半徑r和激光測距儀的分辨率Δα。這些光束用于計算當前光束bl的平方殘差因子。計算光束數目的公式如下：

用于計算光束bl的平方殘差平均值的一組光束為：

在這個集合中，元素個數總是（2n+1）。由于n指定一側的光束數目，故在當前光束bl的左邊的光束和邊右的光束被選擇。

對于集合Bl中的每一光束i，用余弦定理計算到假定中心的距離為：

式中：ɑl、—光束的角度和距離；光束bl的平方殘差的平均值計算為：

4 實驗結果

為了測試提出的位置跟蹤算法性能，讓移動機器人在某物流公司倉庫的地面上沿一條路徑移動。在機器人移動過程中，把這里的算法所需要的全部信息儲存起來，這些信息包括：光學測量數據、到4個nanoLOC錨的LOS距離測量值和激光測距數據。為了驗證算法的準確性，從激光測距數據中提取地面真實數據。激光測距儀的分辨率Δα為0.5°，地標半徑為5.2cm，且位于起點后18m處；數據處理環境基于Matlab7.0并結合C++來進行。

算法從1000個具有正確方向的已知位置的樣本開始。得到的位置跟蹤結果，如圖7所示。在這3個圖中，圖中右邊的實線是地面的真實數據（路徑），地面真實路徑是從到兩面壁的激光距離測量值估計得到的，圖中左邊的密集點（圖7（a）和圖7（b））和線段（圖7（c））是采用基于粒子濾波的樣本處理算法得到的結果。

具體來說，圖7（a）為從到兩壁的激光距離測量值得到的地面真實路徑以及基于粒子濾波的最佳加權樣本，圖7（b）為地面真實路徑以及基于粒子濾波的樣本云，圖7（c）為地面真實路徑以及基于粒子濾波的樣本云中心；可見在每個圖中，得到的位置都能跟蹤真實路徑，盡管存在誤差，但都比較很小；圖7（a）表明，采用最佳加權樣本作為移動機器人的姿態估計是比較困難的，因為有時會有大的位置跳躍，克服這個缺點是采用樣本云中心點，如圖7（c）所示，這些位置能更好地跟蹤真實的路徑。

圖7 在LoS環境中對移動機器人進行定位的結果Fig.7 The Results of Positioning a Mobile Robot in a LoS Environment

從圖7（b）還可以看出，由于在定位開始時的樣本云較寬，所以x分量存在的信息較少，地標檢測由在y=13500mm時的一個非常密集的圓形樣本云表示，而在圖7（a）和圖7（c）中，檢測的定位結果要好得多。如果僅使用nanoLOC測量值，則定位的不確定性就會大得多。為了驗證算法的準確性，我們將圖7（c）的云中心與從激光測距數據得到的實際位置進行比較，所得誤差分布圖，如圖8所示。可見，最大誤差為480mm是僅使用nanoLOC距離測量值，如果檢測到地標，誤差在（20～50）mm之間。地標檢測是非常準確的，而且第一次在距地標5.20m處檢測地標，最后一次在距離地標40cm處檢測地標。

圖8 圖7（c）所示的云中心到實際路徑的誤差Fig.8 Error of the Cloud Centers Shown in Figure 7（c）to the Real Path

5 結論

提出了一種基于nanoLOC無線傳感器網絡和激光測距儀獲得距離測量值的混合式傳感器融合全向移動運輸機器人的位置跟蹤算法，將IEEE 802.15.4a無線傳感器網絡用于通信和全局定位，激光測距儀用于安全和地標檢測以及局部定位，實現并測試了算法；還描述了測量模型的結構和地標檢測的過程；實驗結果表明，距離測量值的定位精度是適合于在環境運動過程中的位置跟蹤。