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

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

0 引言

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

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

1 軟件算法

1.1 無線（UWB）傳感器數據

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

表1 無線測距傳感器通信協議

圖1 無線信號接收裝置

1.2 激光傳感器數據

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

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

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

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

1.4 基于動態K-means聚類的激光無線數據融合方法

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

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

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

圖2 激光數據點示意圖

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

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

而激光讀取的數據為環境中0°～270°的所有障礙物的數據，可表示為

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

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

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

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

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

圖3 人體腿部激光點數據

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

圖4 無線與激光數據融合算法流程圖

2 實驗結果分析

2.1 無線測距傳感器數據特征分析

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

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

2.2 激光傳感器數據特征分析

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

圖6 激光傳感器顯示人腿的位置點云數據

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

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

圖7 數據聚類并處理前后對比圖

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

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

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

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

3 結語

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