基于無線定位與激光傳感器信息融合的叉車機器人智能跟隨算法研究

寧波如意股份有限公司 寧波 315600

0 引言

自動跟隨是高端移動式產(chǎn)品的功能之一，目前部分技術(shù)已成功應(yīng)用到行李箱、平衡車[1]、機器人[2]以及跟隨叉車機器人[3]等產(chǎn)品上?，F(xiàn)有的智能跟隨方案[4]大多數(shù)針對機器人開展的，主要包括了基于視覺的跟隨方案[5-7]，基于激光傳感器數(shù)據(jù)[8]的方法以及基于超聲波[9,10]、紅外[11]等信號實現(xiàn)的自主跟隨方案。而在研究過程中發(fā)現(xiàn)，基于視覺的跟隨方案跟現(xiàn)場的光線有很大關(guān)系，很難適用于工廠環(huán)境；基于激光、紅外、超聲波等通過測量距離來實現(xiàn)的自動跟隨方案，其主要原理均利用測距和定位的方式來實現(xiàn)。而上述測量距離的方法均是基于二維平面，只能檢測到某一平面的信息，通常很難找到被跟蹤人員的位置。特別是當(dāng)環(huán)境中存在多人的情況下，這個問題變得更為復(fù)雜，甚至現(xiàn)有較好的商業(yè)機器人產(chǎn)品（如Adept）也無法實現(xiàn)準確跟蹤，此外，采用紅外及超聲波的方法還存在較大的盲區(qū)，容易導(dǎo)致誤動作。采用無線定位方法，可以不受環(huán)境的干擾，隨著UWB/RFID等采用檢測無線信號強度來實現(xiàn)測距的方法的發(fā)展[12,13]，目前測量精度可以達到10～20 cm左右，但如果僅采用無線信號，仍會受到測距精度不夠的影響，導(dǎo)致跟隨不準。特別是通常無線信號均采用相位檢測的方式來測量角度，其角度誤差比距離誤差更大，通常誤差可達10°以上，且容易受到環(huán)境、振動等因素的影響，導(dǎo)致機器人很難實現(xiàn)穩(wěn)定的人員跟蹤。

因此，本文提出了采用激光傳感器信息與無線UWB信號方法相結(jié)合的方法來實現(xiàn)叉車機器人的人體跟隨。該方案采用無線UWB信號的定位裝置，通常分為兩部分，一是信號發(fā)射裝置，安裝于移動設(shè)備的固定位置，二是信號接收裝置，通過檢測人員所在位置信號的強度計算距離和角度信號，計算完成后通過無線傳輸回信號發(fā)射裝置完成測距。信號接收裝置由被跟蹤人員攜帶，具有唯一的ID，故能保證信號的唯一性。而激光傳感器具有較高的精度，通過激光傳感器所獲得的距離和角度信息與無線信息相融合，便可獲得被跟蹤人員的高精度位置信息。由于只需實現(xiàn)人體跟隨，故可采用近距離激光傳感器以降低成本，此外激光傳感器還可用于機器人的實時避障。

1 軟件算法

1.1 無線（UWB）傳感器數(shù)據(jù)

采用的無線測距傳感器模塊如圖1所示，包含1塊無線信號發(fā)射器和1塊信號接收器，信號接收器將發(fā)射器發(fā)送來的數(shù)據(jù)通過RS232串口傳輸數(shù)據(jù)傳輸給工控機，傳輸數(shù)據(jù)的波特率為115 200 bps，數(shù)據(jù)協(xié)議基于自定義的串口協(xié)議，采用CRC校驗方式進行數(shù)據(jù)校驗，協(xié)議格式如表1所示。

表1 無線測距傳感器通信協(xié)議

圖1 無線信號接收裝置

1.2 激光傳感器數(shù)據(jù)

激光傳感器掃描的是某單一平面，為了更好地檢測周圍物體，激光傳感器安裝在離地面20～30 cm，通常掃描到的是人體雙腿的位置。激光傳感器可掃描0～4 m范圍內(nèi)的所有障礙物，需讀取環(huán)境的障礙物信息。激光傳感器數(shù)據(jù)分為512個點，每個點代表障礙物到當(dāng)前機器人的位置，平均分布于0°～270°的位置。

1.3 基于無線和激光傳感器數(shù)據(jù)融合的自動跟蹤算法

由于無線UWB傳感器的測距精度較差，故首先需對無線傳感器的數(shù)據(jù)進行滑動中值濾波[14,15]。

式中：n為需要濾波的點的個數(shù)，j=1，2，…，n；i為無線傳感器讀到的數(shù)據(jù)點。

1.4 基于動態(tài)K-means聚類的激光無線數(shù)據(jù)融合方法

在K-means[16,17]聚類分析的基礎(chǔ)上，提出了一種可實施增減聚類個數(shù)的動態(tài)聚類分析方法。聚類數(shù)目實時可變，從聚類中找出與無線測距傳感器相匹配的結(jié)果用于行人跟隨。

傳統(tǒng)的K-means聚類是隨機選取個對象作為初始的聚類中心，然后計算每個對象與各個種子聚類中心之間的距離，把每個對象分配給距離它最近的聚類中心。

基于動態(tài)K-means聚類的激光無線數(shù)據(jù)融合方法思想為：先選取聚類K值，然后利用無線傳感器計算行人離移動機器人的距離，如果傳感器的接收器存在行人，在其附近一定會掃描到行人的雙腿，相關(guān)數(shù)據(jù)會以障礙物的形式反映到激光數(shù)據(jù)中，如圖2所示。由于激光的定位精度通常小于2 cm，故系統(tǒng)找到的被跟隨行人的精度大大提高。該方法的主要步驟：

圖2 激光數(shù)據(jù)點示意圖

1）以車體為中心，建立極坐標系，分別利用激光傳感器和無線UWB傳感器進行數(shù)據(jù)測量，其中無線傳感器獲得的機器人離行人的距離與角度數(shù)據(jù)為

式中：rw為機器人中心到行人的距離值，ρw為行人偏離機器人中心的角度值。

而激光讀取的數(shù)據(jù)為環(huán)境中0°～270°的所有障礙物的數(shù)據(jù)，可表示為

式中：R為數(shù)組，保存著0°～270°范圍內(nèi)障礙物到機器人中心的距離值；ρ為數(shù)組，保存著0°～270°范圍內(nèi)障礙物到機器人中心的距離值。

2）將無線傳感器找到距離值的一定范圍內(nèi)的激光傳感器數(shù)據(jù)進行聚類分析，并找到離無線傳感器數(shù)據(jù)最小的行人聚類特征，定義最小尋找半徑為h，例如h=20 cm。

每個聚類的總長度LK和聚類首尾連線的直線長度DK

若滿足Sc＞δc，且Lmin＜Lk＜Lmax，則把CK中數(shù)據(jù)存入Ccen［tn］中，其中Lmax需要根據(jù)人腿部的數(shù)據(jù)的特點設(shè)計，盡量減少漏檢測，降低誤檢測。

計算Ccent［n］中所有腿部數(shù)據(jù)的中心點Pcent(xc，yc)

圖3 人體腿部激光點數(shù)據(jù)

3）選擇Ccent［n］中離無線傳感器所測得的距離最近的點作為需要跟隨的行人的位置。具體的流程如圖4所示。

圖4 無線與激光數(shù)據(jù)融合算法流程圖

2 實驗結(jié)果分析

2.1 無線測距傳感器數(shù)據(jù)特征分析

實驗用的軟件在機器人操作系統(tǒng)（ROS）平臺上進行分析。首先對無線測距傳感器分析，通過232協(xié)議的解碼，工控機直接接收到的數(shù)據(jù)中，包含了采用極坐標形式的距離和角度數(shù)據(jù)。實驗過程中，分別將發(fā)射器在不同位置進行靜止放置和人為晃動，并將原始數(shù)據(jù)與濾波后的數(shù)據(jù)進行比較，得到實驗結(jié)果如圖5所示。發(fā)射器處于靜止狀態(tài)時，角度和距離信號質(zhì)量均比較穩(wěn)定，當(dāng)出現(xiàn)人為晃動時，其角度和距離誤差信息均波動較大。經(jīng)過濾波后，信號質(zhì)量均得到了改善，當(dāng)出現(xiàn)晃動時，無論距離誤差還是角度誤差均較大，距離誤差可到20～30 cm，角度誤差可到20°～30°，故如果單獨采用此種傳感器，將很難實現(xiàn)穩(wěn)定和精確的人體跟蹤。

圖5 無線測距傳感器濾波前后結(jié)果對比

2.2 激光傳感器數(shù)據(jù)特征分析

激光傳感器數(shù)據(jù)通過USB傳送數(shù)據(jù)，通過ROS的hokuyo node便可以直接讀取到傳感器的數(shù)據(jù)，采用ROS的Rviz可實時顯示激光傳感器的數(shù)據(jù)，顯示情況如圖6所示，從圖6a中可知，激光傳感器讀取的點云數(shù)據(jù)中兩個連續(xù)的弧形數(shù)據(jù)即是人腿的位置，圖6b中可以看出，當(dāng)雙人存在于環(huán)境中時，兩個人的腿部特征基本上一致，很難利用激光數(shù)據(jù)特征來辨別需要跟隨的行人，當(dāng)環(huán)境中存在多人時，情況更為復(fù)雜，導(dǎo)致機器人無法實現(xiàn)正常的跟隨，故需要進行數(shù)據(jù)融合，利用基于無線信號定位的有源特征，利用激光傳感器來提高跟隨精度。

圖6 激光傳感器顯示人腿的位置點云數(shù)據(jù)

2.3 人體跟隨算法與實驗驗證

利用無線傳感器獲得的距離和角度值，在激光傳感器數(shù)據(jù)中尋找聚類的中心離無線距離傳感器最近的聚類，作為需要跟隨的目標。在實驗過程中假設(shè)人體是動態(tài)的，且離人腿較近的地方不存在與人腿特征相似的障礙物，去除不符合人腿特征的聚類前后的激光點云數(shù)據(jù)如圖7所示。

圖7 數(shù)據(jù)聚類并處理前后對比圖

為了驗證該方法的可行性，搭建了移動機器人實驗裝置，將無線傳感器及激光傳感器分別安裝于機器人上，如圖8所示，通過坐標轉(zhuǎn)換將二者所得到的距離和角度坐標轉(zhuǎn)換至同一坐標系。先利用無線傳感器獲得行人的大致位置信息，后利用激光傳感器與無線距離傳感器獲得的信息作為比較，提取來自激光傳感器的數(shù)據(jù)作為智能跟隨使用。

圖8 叉車機器人傳感器安裝位置圖

經(jīng)過處理后的激光數(shù)據(jù)和無線距離與角度信息如圖9所示。由圖可知，利用此方法找到的激光傳感器信息是較為穩(wěn)定的，它不受其他行人的影響，且當(dāng)無線傳感器輸出誤差較大信息時，仍能利用激光傳感器實現(xiàn)準確跟蹤，實驗顯示當(dāng)算法切換到激光跟蹤時，跟蹤精度在±5 cm以內(nèi)。

圖9 輸出的激光傳感器及無線傳感器信號對比

3 結(jié)語

提出了一種新型的基于動態(tài)K-means聚類的激光無線數(shù)據(jù)融合方法，實現(xiàn)了叉車機器人的智能跟蹤，該方法避免了傳統(tǒng)的采用單一傳感器實現(xiàn)跟蹤的不足，能夠使叉車機器人在復(fù)雜環(huán)境下較好地跟蹤唯一的行人。本文通過實驗驗證了其可行性，實驗結(jié)果顯示與單獨采用無線（UWB）傳感器相比，采用無線和激光傳感器融合方法可提升跟蹤精度至±5 cm以內(nèi)，精度提高了一倍左右。下一步需研究的主要問題是如何改進聚類算法，使得系統(tǒng)能夠更加準確地區(qū)別與人腿特征類似的環(huán)境，更加準確地從環(huán)境中提取人腿的信息。