基于克里金預測的機器人源定位方法

曹 凱，陳陽泉+，高 嵩，曹牧寒，王 坤

(1.西安工業大學 機電工程學院，陜西 西安 710021；2.西安工業大學 電子信息工程學院，陜西 西安 710021)

0 引 言

隨著現代化工業的發展，各種各樣的危險固體源、液體源和氣體源在工農業生產、原料運輸等環節發揮著至關重要的作用。在工業事故或是野外探索導致有害、有毒或無味氣體釋放等事件中，危險源的快速定位可以有效保護人身財產，避免大面積災難發生。

傳統固定傳感器網絡和移動機器人定位是目前源定位的兩種主要方式。基于傳感器網絡的源定位方法的主要思想是預先模擬源的擴散模型，結合模型對源的參數進行迭代求解[1-4]。但該方法有兩個較明顯的短板，一是進行源參數求解時，對預先建立的源模型有較強的依賴。二是傳感器網絡的拓撲結構不能無限制擴大，無法保證傳感器分布在具有代表性的區域內。移動機器人具有搜索效率高、部署成本低、安全性好等優點，更適合在復雜的環境中執行具體任務，不僅加快了搜索速度，而且避免了過于復雜的理論推導。在工廠或是野外環境中，移動機器人可以發揮其機動性實現更有針對性的搜索定位，提高了場景適應性。Rozas等作為源搜索定位領域的先行者，將基于梯度的算法在單機器人上實現，也是在源定位的第一項工作[5]。Moth算法是通過觀察雄性蠶蛾跟蹤雌性蠶蛾釋放的信息素的行為得到的一種仿生算法[6，7]。基于編隊[8，9]的源定位算法是指在不破壞隊形的情況下，通過機器人之間的信息共享來尋找源的位置。概率與基于地圖的算法作為源定位方法被應用，該算法稱為信息趨向性算法[9]，是一種典型的貝葉斯推理方法，這類算法的特點是在不計算源強度梯度的情況下進行定位。

在實際應用場景中，基于梯度的源定位方法在實現上有很多局限性，比如算法性能低、數據傳輸慢等導致定位任務不準確或是定位失敗。基于此本文采用一種有效的源定位方法，稱為場探索，簡單來說，就是在目標區域進行預先設定的路線運動，采集未知場中的場強數據，最后預測出場的分布，可直觀地生成源強度分布圖，該方法是一種非常有效的源定位的方法。

1 場探索理論方法

1.1 問題描述

場探索方法在很大程度上依賴于空間統計、地質統計學和地理學領域的研究成果[10-13]，該方法是基于從場中采集的數據樣本進行預測，根據采集樣本的協方差矩陣p生成權重，進而預測場中其它點的狀態，預測過程中每一個計算的方差Ni也可以作為克里金的結果。

(1)

其中，C為在該位置處的強度值，ρ為概率密度，μ和σ2分別為均值和方差，其實高斯模型就是用高斯概率密度函數量化事物。

1.2 克里金法

克里金法假設平均值是已知的，并且對于非常大的目標區域是恒定的，但是實際情況中針對不同的源，強度的均值可能發生變化。克里金法可以從一個較小的觀測子集推斷出一個鄰域的局部平均值，其優點在于進行預測之前不需要求出平均值，通過設置一個表示平均趨勢模型的多項式，而不是從表示該鄰域的恒定平均值[14-16]開始計算。

變異函數是一個連續函數，如圖1所示，在空間中Z(si)，Z(sj) 任意兩點之間產生協方差，已知這兩點之間的歐氏距離，用hi,j∈R來表示，見式(2)

圖1 變異函數模型

(2)

則在場中該點的值，即場強表示為式(3)

Z(si)=μ(si)+θ(si)

(3)

其中，θ為零均值平穩隨機維納過程，μ為場中感興趣狀態的平均值，通過場強可構建出半變異函數，由于實驗過程中在垂直方向上幾乎沒有變化，故建立二維平面下的半變異函數，見式(4)

(4)

式中：A為目標場，即實驗過程中的區域，Z(s) 表示區域中位置為s的場強值，Z(s+h) 為距離s位置的點h處的值，經驗半變異函數可以表示這兩個采樣點位置觀測值的協方差，可定義為式(5)

(5)

其中，N(h) 為觀測點的個數，在場中取N個點作為樣本，經驗半變異函數作為場的離散模型。在目標場中采集一組樣本，該組樣本可以代表整個場的特性，通過計算出樣本合適的方差得到個觀測值的協方差矩陣，且P∈RN×N。 見式(6)，s表示為觀測的點，元素Pi,j代表場中第i次和第j次觀測值之間協方差，如果i=j， 則Pi,j代表第i個觀測值的方差

(6)

(7)

隨著在目標場中的觀測值N數量增加，式(7)的協方差矩陣P與N2成正比。對于大小為180×180的場，且掃描區域占整個目標區域的30%，則P的大小為540×540的矩陣。一般情況下，維度這么高的矩陣很可能是病態矩陣，矩陣中的某一列是其它列的線性組合。高條件數意味著PP-1偏離大小相同的單位矩陣，這種逆矩陣使用LU分解便于計算。本文中使用偽逆函數計算矩陣的奇異值分解(SVD)，雖然計算較為復雜，但是在計算逆病態矩陣時誤差更穩定。

對于場中的任何給定點，即觀測點，可以構造一個鄰近向量d0∈RN， 見式(8)和式(9)，其中包含給定點的協方差，因此dN的第k個元素將包含由點s0和第k個觀測值產生的sk之間間隔的協方差

(8)

(9)

對于給定的預測位置的s0和普通克里金權重向量λ0將作為協方差矩陣的逆和鄰近向量，見式(10)

(10)

(11)

(12)

1.3 場探索

利用克里金法對場進行預測是在目標區域內每個未觀測的網格單元上進行，為了從一組有限的N個觀測值及其各自的位置來預測整個目標場。以下為利用克里金法預測目標場的具體過程：

(1)建立離散模型，構造一個離散的變異函數模型；

(4)構造協方差矩陣Pi,j=γ(hi,j)；

2 遍歷路徑

2.1 傳感器數據匹配

在危險源的擴散過程中，源模型有兩種形性質，即模型不隨時間變化的時不變性和模型隨時間變化的時變性。本文僅研究源的時不變特性，以光源為研究對象，所以設計機器人遍歷方式之前，需將光照傳感器信息與機器人的傳感器信息匹配，應用到源定位方法中。為此，在ROS中采用rosserial協議對光照強度信息及機器人odom信息進行匹配，作為后續機器人搜索遍歷時的“嗅覺”系統，實現stm32與ROS的框架。

因為本文中所使用的機器人是基于ROS的平臺，ROS中利用消息、話題等進行通信，基本通信方式為TCP/IP，而通常微控制器中使用的通信方式為串行通信，如UART等，所以需要將串行通信方式通過某種方法轉換為ROS中的通信方式。rosserial協議可以解決這個問題，起到了中介的作用，如圖2所示，可實現ROS的微控制器和計算機之間的通信。

圖2 rosserial協議

圖2中rosserial服務端指的是運行ROS的PC，連接到PC的微控制器則為rosserial客戶端。具體來說，將連接到微控制器的傳感器的值進行AD轉換，通過UART傳輸，那么計算機的rosserial服務端節點將串口發送過來的數據轉換為ROS中話題。rosserial服務端作為ROS的PC上的一個節點，通過rosserial協議作為PC和嵌入式設備之間的中介。其實rosserial的服務端和客戶端是以基于串行通信的數據包的形式發送和接收數據，定義時以字節為單位，包含了數據包同步和數據驗證所需要的信息。

在此基礎上，本文設計一個串口節點my_serial_node，如圖3(a)所示，這個節點訂閱talker控制節點發來的命令，將命令通過串口設備發送至移動機器人底盤，同時my_serial_node實時接收機器人底盤發送過來的傳感器數據，將傳感器數據封裝后以sensor話題進行發布，listener節點可以訂閱sensor主題。

圖3 ROS與機器人串口通信過程

由于源定位過程中需要拿到機器人的實時位置，這中間存在一個問題，在采集到光照強度信息后，繼續訂閱機器人的位置信息會存在有延時的問題，導致光照強度信息與機器人位置信息不能一一對應。所以在獲取光強傳感器信息的同時，創建一個多線程節點mulpthread_node，以保證能夠同時接收和發送兩種信息，具體過程如圖3(b)所示。該節點同時訂閱由my_serial_node發布的sensor話題和odom發布的里程計信息，在里程計信息中提取出所需要的位置信息即可，最后由listener訂閱mulpthread_node實時發布的移動機器人位置信息及光照傳感器。

2.2 Zigzag形遍歷和圓形遍歷

本文從統計學角度出發，采用場探索的方法，在執行探索和巡邏等任務時常見的方法是Zigzag遍歷(Z字形)和圓形遍歷模式，即引導機器人通過預先定義的路徑。場探索方法通過設計遍歷模式，減少了由機器人自身探索帶來的數據偏差，降低了機器人的能耗，使機器人可以在同等電量的情況下遍歷更多的路徑。

實驗在指定大小為1.8 m×1.8 m的區域中進行，分為兩種情形，情形一光源在目標區域一角，坐標可為(0,0)、(0,180)、(180,180)或(180,0)，本實驗中將光源位置定為(0,0)。情形二光源在目標區域的正中間，坐標為(90,90)。在兩種情形的基礎上，為研究機器人不同數據點對后續繪制的場強分布的影響，分3種采集方式：按網格劃分采集、按遍歷路徑采集、按區域采集。其中，機器人的采樣頻率為10 Hz。在Matlab中建立可視化窗口實時監測移動機器人的運動軌跡并將光照強度的大小反應在軌跡中。如圖4所示，均采用Zigzag形和圓形的方式遍歷區域，五角星代表光源。

圖4 遍歷方式

情形一：光源坐標為(0,0)

(1)網格劃分

按網格劃分即在實驗區域內部分別形成3×3、4×4、5×5的正方形網格，五角星為光源所在的位置，這些采樣點的位置在網格線的交匯處，由于實驗區域面積和機器人體積限制，故在區域內有25個采樣點時即可預測出較為精確的場分布，不必繼續進行網格細分。實驗中，根據網格點的坐標，在Rviz可視化界面中標定網格點的坐標，將對應的Rviz中相對應的坐標點作為目標點，按順序預先設定在機器人導航包中，使機器人按照Z字形依次遍歷指定區域。在3×3、4×4、5×5網格的實驗中，實時觀察濃度大小變化，強度越大，顏色越亮，相應的點也越大，不符合濃度變化規律則重新進行實驗。

(2)遍歷路徑

實驗中分兩種路徑遍歷，分別是Zigzag形遍歷和圓形遍歷，Zigzag形遍歷是指機器人保持一定夾角或幅度做Z字形或類Z字形運動，本實驗中機器人在遍歷時方向夾角為90°，即每次轉向保持90°不變在目標區域中遍歷。垂直方向上每隔18 cm設置一個觀測點，水平方向上每隔10 cm設置一個觀測點，機器人整條路徑中共設置39個觀測點，并且起始位置坐標為(45,0)，終點坐標為(135,180)，機器人在場中做勻速運動。圓形遍歷中分別設置3個半徑，分別為30 cm、60 cm和90 cm，機器人的線速度v=0.2 m/s， 根據角速度與線速度的關系v=ωr可計算得出角速度大小，分別設置為ω1=0.6 rad/s，ω2=0.3 rad/s，ω3=0.2 rad/s， 機器人在目標區域中做圓周運動采集光強數據。由于機器人做圓周運動時，使用多點導航功能包需要重新設置坐標點，所以在圓形遍歷時分為3次進行，在每個圓中設置均勻分布的10個觀測點。

(3)區域采集

將實驗區域劃分為4塊面積形狀相同的區域，按照坐標系中的象限規則，右上角區域為第一象限，逆時針旋轉分別為第二、三、四象限。實驗中為了觀察在不同區域中采集的數據對場分布的預測效果，在第一二象限、第一三象限、第一四象限、第二三象限和第二四象限分別采集數據，由于在第三四象限采集數據與在第二三象限采集數據后的效果相同，故只考慮其中一種情況。每個象限的大小為0.9 m×0.9 m，所以在每一個象限中分別取4個位置處的數據，實驗中機器人在每個象限中做Z字形運動。當遍歷區域屬于同一側時，使得機器人做U字形運動，提高整個實驗的效率。當機器人在對角兩個區域采集數據時，需要重新設置坐標值。

情形二：光源坐標為(90,90)

(1)網格劃分

在情形二的實驗中光源處于區域中央，這導致在一些情況光源可能位于機器人的遍歷路徑當中，故實驗中手持光源至機器人上方，保證機器人能夠平穩的運行，其它條件均與情形一相同。

(2)遍歷路徑

在Zigzag遍歷算法與圓形遍歷算法中，分別設置相與情形一相同數量的坐標點。為了避免機器人的位置與光源的位置沖突，分為兩部分遍歷，即預先設置坐標點時機器人不經過(90,90)點，在(90,108)采集結束后，手動將機器人搬至(90,72)的位置。

(3)區域采集

當光源坐標為(90,90)時，與情形一中相同，在第一二象限(同側)采集數據與在第一四、二三、三四象限采集數據的效果相同，在第一三象限(對角)采集數據與在二四象限采集數據的效果相同，故只考慮兩種情況。

3 實驗及仿真結果

為模擬無障礙物條件下時不變型的危險源泄露環境，本文搭建出用于光源定位的平臺，圖5展示了由長方體塊、移動機器人和光源組成的真實實驗場景，實驗在3 m×3 m的指定方形場地中進行，由于本實驗中研究對象為光源，所以為了盡量排除人為因素等外界光源因素的干擾，利用簡易制作的長方體塊圍繞在實驗場地四周，所以可供機器人活動的范圍為1.8 m×1.8 m，目的是在二維平面上營造一個無干擾的目標區域。機器人平臺為差速式Turtlebot2機器人，配備的電腦參數為2.5 Hz主頻、Intel Core i5處理器和Ubuntu16.04操作系統，裝有Kinetic視覺傳感器和L3G4200D陀螺儀，并外接了STM32開發板，用來光照傳感器的配置。

圖5 真實實驗場景及Turtlebot2機器人

將機器人假設為一個點，近似認為機器人坐標和傳感器坐標是相同的，并且所用的光源是均勻擴散的。實驗中機器人的運動速度以0.2 m/s勻速遍歷，經多次測量實驗中光源的峰值約為2283 lx。實驗中存在有以下3個影響因素：一是傳感器的精度；二是機器人的定位不準，遍歷時機器人可能會偏離預先設定的路徑和坐標點；三是實驗平臺中的障礙物是手工制作，可能存在實驗中整個目標場不是方形區域的情況。為避免以上3個因素對實驗產生的偶然結果，每組分別進行20次實驗，將偏差較大的實驗舍棄。通過對數據的統計后利用克里金法預測場的分布，及繪制出等值線圖，可以直觀地顯示出不同情形下光強的分布結果和源的大概位置，以下為兩種情形中的實驗結果。在20次實驗后，光強數據被存放在csv文件中，對每個點的數據求均值后作為繪制等值線圖時的數據。

情形一：光源坐標為(0,0)時的仿真結果：

(1)網格劃分

如圖6及表1所示，對目標區域網格化后，在場中的9個觀測點采集光強信息，預測得到源的位置大致在(41.86,42.47)處，在該區域附近形成局部最優，定位效果較差。采集16個觀測點時，在(20.44,21.25)也形成局部最優，整個場的分布更接近于光源擴散模型。在此基礎上，增加為25個觀測點時場強分布可基本預測出光源所在的位置，坐標為(13.68,14.52)，相比于其它兩組實驗誤差最小，主要原因是實驗中機器人的尺寸較大，在場中采集更多觀測點的值會導致數據重復以及在某些位置難以采集到數據。圖7記錄了20次實驗中預測出源的位置與實

圖6 情形一網格劃分等值線

圖7 預測源位置與實際源位置的誤差

表1 實驗數據統計

際值的差值，實驗中誤差變化范圍都穩定在10 cm內。仿真結果顯示，在場中的觀測點數量越多，預測的結果也越準確，誤差也越小。

(2)遍歷路徑

圖8為兩種遍歷方式預測的場分布。

圖8 情形一遍歷路徑等值線

如圖8及表2所示，靠近光源位置處的圓形遍歷的預測效果更符合光源的分布模型，但是在遠離光源的一側，雖然有著偏向光源的大致趨勢，但這時Zigzag形遍歷方法更準確，繪制出的等值線圖更平滑。從整體場分布來看，采用圓形遍歷預測源的位置的誤差較Zigzag形遍歷更小，效果更為明顯。圖9記錄了20次實驗中的Zigzag形遍歷和圓形遍歷的源位置誤差，顯示誤差均在20 cm之內，所以在場中的遍歷路徑對于預測的結果影響不大，但是預先設置的路徑必須合理有效，避免出現在某一位置重復采集的問題，最大化機器人的效率。

圖9 預測源位置與實際源位置的誤差

表2 實驗數據統計

(3)區域采集

圖10及表3為5種采集方式預測的結果。

表3 實驗數據統計

圖10 情形一區域采集等值線

圖11記錄了20次實驗的情況，采集位置在一三、二三和二四象限時，等值線圖符合光源分布模型,誤差小。其它兩種情況下，存在5處局部最優值，可能是采集區域與存在極值處的區域相距較遠，故最好采用對角區域遍歷的方式采集信息。

圖11 預測源位置與實際源位置的誤差

情形二：光源坐標為(90,90)時的實驗結果：

(1)網格劃分

考慮到光源與機器人尺寸的問題，預先設置的目標點均增加和減少5 cm，即以(90，90)為原點，5 cm為半徑的圓周上，均勻取4個點的數據求均值作為光源處的光強值，如圖12及表4所示。

表4 光照強度數據統計

如圖12(a)，場中設置9個觀測點時，光源附近半徑為30 cm的區域中場的分布符合光源的高斯分布模型，但是在如(30,30)位置處出現局部最優，主要原因可能是預先設置的坐標點較分散，導致在該點附近有數據缺失，使預測時場的誤差較大。相比之下，圖12(c)中網格點的數量增加可以減少預測時的誤差，定位效果較好，在光源附近預測場分布的范圍擴大。在此情況中預測光源位置就是實際源的位置，并且在場中網格點的數量越多，則預測的場的效果越接近真實場的分布。

圖12 情形二網格劃分遍歷等值線

(2)遍歷路徑

如圖13及表5所示，利用Zigzag方法遍歷比圓形遍歷的效果差，可能是機器人運動時的定位不準，偏離了預先設定的坐標位置。利用場探索的方法預測場分布時，即采用圓形遍歷的定位精度更高，等值線圖更符合高斯模型，達到了預期的效果。

表5 實驗數據統計

圖13 情形二遍歷路徑等值線

(3)區域采集

如圖14及表6所示，該種情形只能預測出光源位置的一個大致范圍，并且該區域較大，定位精度低。在真實危險源定位的環境下，區域采集的情形可能需要利用其它方法，比如深度學習的方法，通過訓練數據集得到更準確的數據，本文采用的方法也只是在無障礙物情況下的場景，在有建筑物等的隔檔時在全局場景中遍歷可以得到更為有效的數據，得到的結果也更理想。圖15記錄了20次實驗中兩種情況時的源位置誤差，雖然預測場的分布不能直觀地表示出真實光源的分布，但預測后源的位置與真實源位置誤差范圍也在10 cm內。

圖15 預測源位置與實際源位置的誤差

表6 實驗數據統計

圖14 情形二區域采集等值線

綜上所述，在情形一中，采用區域采集的方式定位誤差小，同時用時和路徑長度都較短。具體表現為在一三象限和二四象限時信息的獲取，分析原因可能是在這兩個象限的跨度較其它幾種方法更大，所以機器人在獲取環境信息時遍歷區域的面積占比更大，更形象地表現出該區域場的特征。在情形二中，采用遍歷路徑的方式定位誤差極小，具體表現為圓形遍歷方式。雖然在遍歷路徑長度和用時上都較多，但是預測出場的分布更符合高斯分布，符合真實的光源分布。故在實際場景中，選擇圓形遍歷的方式進行源定位任務會得到更好的效果，主要是由于圓形遍歷可以掃描占比更大的區域，獲取的信息也更能代表出場的特征。

4 結束語

本文提出了一種用于源定位的場探索方法，與傳統的基于梯度的算法相比更為突出。傳統的源定位方法雖然在仿

真容易建立，但是在機器人上實現時基于梯度的算法性能低下或是數據傳輸慢導致機器人延時相應，發送給機器人錯誤的場信息。與基于梯度的算法相比，場探索方法結合了統計學中的數據處理方法和場的分布特點，優點在于不需要建立復雜的動力學模型，避免了復雜的數學建模過程，利用克里金法對場中的每個單元網格進行預測，提高了數據的穩定性。同時減少了機器人隨機搜索帶來的能耗問題，簡化了傳統的基于梯度的源定位方法對于算法的有效性和機器人的性能的高要求。通過設計機器人的遍歷方式并搭建出真實實驗場景，驗證了該方法可以對目標場的分布預測的可行性，成功預測出場的分布，提高了源定位方法的靈活性與實用性。

在未來的工作中，需要解決的是機器人動態繪制場分布的等值線，也可以稱為實時繪制問題。另一方面，在機器人的底層控制上需要更精準的遍歷搜索，實現更準確的數據獲取，從而得到更真實的場分布。進一步地，在研究達到很好的效果時，將擴展至三維空間進行研究。