馬宏偉，楊金科，毛清華，汪強

（1.西安科技大學(xué) 機械工程學(xué)院,陜西 西安 710054；2.陜西省礦山機電裝備智能監(jiān)測重點實驗室,陜西 西安 710054）

0 引言

煤礦護盾式掘進機器人適用于巷道斷面大、夾矸與片幫共存的復(fù)雜地質(zhì)條件掘進作業(yè)。煤礦護盾式掘進機器人在掘進過程中大多采用激光指向儀指示巷道的設(shè)計中線，通過人工目測激光光斑的方式控制截割頭截割，極易因工作面環(huán)境惡劣、作業(yè)現(xiàn)場光線差等原因造成超挖或欠挖等，無法滿足掘進工作面智能化、無人化的要求，而掘進機的井下精確定位是煤礦掘進智能化、無人化的核心技術(shù)之一[1-3]。因此，深入研究煤礦護盾式掘進機器人精確定位方法十分必要。

目前，煤礦護盾式掘進機器人一般采用單一傳感器定位方法，主要采用全站儀、機器視覺、超寬帶等技術(shù)進行定位。全站儀雖然定位精度高，但煤礦井下巷道環(huán)境惡劣，光路可能被遮擋，使測量結(jié)果不可靠[4]。機器視覺測量精度容易受井下粉塵影響[5]。超寬帶雖能夠解決傳統(tǒng)無線技術(shù)傳播問題，但其節(jié)點安裝和維護是個難題，不適用于長距離井下掘進定位[6-7]。因此，在井下環(huán)境復(fù)雜多變的情況下，單一傳感器定位方法常常會導(dǎo)致定位精度和可靠性降低，針對上述問題，有學(xué)者提出了組合慣導(dǎo)定位方法。慣導(dǎo)系統(tǒng)是一種自主式定位定向系統(tǒng)，適用范圍廣，通過加速度計及陀螺儀采集掘進機的加速度及角速度[8]，通過數(shù)值積分運算可獲得掘進機器人的姿態(tài)、速度及位置信息，對掘進機器人定位具有較好的效果。當前，掘進機組合慣導(dǎo)定位方法有慣導(dǎo)/視覺、慣導(dǎo)/里程計、慣導(dǎo)/全站儀等。文獻[9-10]通過融合慣導(dǎo)和視覺信息，可在粉塵濃度較小的情況下實現(xiàn)掘進機器人位姿精準測量，但在粉塵濃度較高的情況下工業(yè)相機會失效，從而導(dǎo)致慣導(dǎo)/視覺組合定位精度降低。文獻[11-12]通過融合里程計和慣導(dǎo)信息，可在地質(zhì)條件較好的巷道中有效測量掘進機的位姿信息，但是隨著時間的增長，掘進機易受浮煤影響造成履帶滑移及打滑等，此時里程計會產(chǎn)生較大誤差，從而導(dǎo)致慣導(dǎo)/里程計組合定位精度降低。文獻[13-14]提出了利用慣導(dǎo)與全站儀組合對掘進機進行自主定位定向的方法，實現(xiàn)了在粉塵濃度較小情況下掘進機的精確定位，但是井下環(huán)境復(fù)雜、粉塵較多時，遠距離全站儀激光透視性差，從而導(dǎo)致慣導(dǎo)/全站儀組合定位精度降低。

針對現(xiàn)有煤礦掘進機器人定位方法存在定位精度低的問題，本文在慣導(dǎo)/全站儀組合定位方法的基礎(chǔ)上，提出了捷聯(lián)慣導(dǎo)+數(shù)字全站儀+位移傳感器的組合定位方法，利用基于聯(lián)邦濾波器的多信息融合算法有效融合三者的信息，實現(xiàn)煤礦護盾式掘進機器人系統(tǒng)的精確定位。

1 煤礦護盾式掘進機器人系統(tǒng)

煤礦護盾式掘進機器人系統(tǒng)構(gòu)成如圖1 所示，主要由截割機器人、臨時支護機器人Ⅰ和Ⅱ、鉆錨機器人、錨網(wǎng)運輸機器人、電液控平臺、運輸與通風(fēng)除塵系統(tǒng)組成。臨時支護機器人Ⅰ與臨時支護機器人Ⅱ之間通過推移油缸的推拉作用，實現(xiàn)整個煤礦護盾式掘進機器人系統(tǒng)的自主行駛。煤礦護盾式掘進機器人系統(tǒng)集探、掘、支、錨、運、通風(fēng)與除塵等于一體，能夠?qū)崿F(xiàn)掘進與鉆錨協(xié)同作業(yè)，對提高掘進效率、保證安全生產(chǎn)、實現(xiàn)快速掘進具有重要意義。

圖1 煤礦護盾式掘進機器人系統(tǒng)構(gòu)成Fig.1 Structure of coal mine shield roadheader robot system

2 組合定位方法原理

煤礦護盾式掘進機器人系統(tǒng)組合定位過程包括基于捷聯(lián)慣導(dǎo)的機身位姿參數(shù)解算、基于數(shù)字全站儀的機身位置參數(shù)測量、基于位移傳感器的機身位置參數(shù)推算、基于聯(lián)邦濾波器的機身位置參數(shù)與位姿參數(shù)融合。

組合定位方法組成如圖2 所示（x軸指向設(shè)計巷道中線方向；y軸垂直于設(shè)計巷道中線；z軸垂直于oxy平面）。將捷聯(lián)慣導(dǎo)和360 棱鏡安裝在掘進機機身前段臨時支護機器人Ⅰ上，將位移傳感器安裝在臨時支護機器人Ⅰ和臨時支護機器人Ⅱ之間的推移油缸中，使其隨掘進機機體移動，將數(shù)字全站儀固定在掘進機機身后端頂錨桿處并設(shè)置為跟蹤模式。開始定位前，地測人員測量掘進機器人機體所在位置的經(jīng)緯度，并依據(jù)設(shè)計巷道確定任意2 個坐標點，分別為數(shù)字全站儀后視點和測站點。根據(jù)2 個坐標點構(gòu)建數(shù)字全站儀坐標系，數(shù)字全站儀通過測量臨時支護機器人Ⅰ上的360 棱鏡，解算出掘進機器人機身在當?shù)氐乩碜鴺讼迪碌淖鴺恕＠梦灰苽鞲衅鬏敵龅奈蛔诵畔⑴c捷聯(lián)慣導(dǎo)輸出的實時姿態(tài)轉(zhuǎn)換矩陣進行航位推算，得到掘進機器人機身在當?shù)氐乩碜鴺讼碌奈恢眯畔?。捷?lián)慣導(dǎo)根據(jù)初始經(jīng)緯度及初始姿態(tài)角可實時測量出掘進機器人的位姿和速度。將捷聯(lián)慣導(dǎo)測量的經(jīng)緯度轉(zhuǎn)換為當?shù)氐乩碜鴺讼迪碌淖鴺酥?，利用基于?lián)邦濾波器的多信息融合算法將捷聯(lián)慣導(dǎo)解算出的掘進機器人位置及姿態(tài)角信息與全站儀測量的掘進機器人位置信息及位移傳感器推算的掘進機器人位置信息進行融合，減少捷聯(lián)慣導(dǎo)隨時間產(chǎn)生的位置累計誤差，得到掘進機器人的位姿信息，最終實現(xiàn)其精確定位，定位原理如圖3所示。

圖2 組合定位方法組成Fig.2 Composition of combination positioning method

圖3 組合定位方法原理Fig.3 Principle of combined positioning method

3 基于聯(lián)邦濾波器的多信息融合算法

3.1 基于聯(lián)邦濾波器的多信息融合原理

本文提出的捷聯(lián)慣導(dǎo)+數(shù)字全站儀+位移傳感器的組合定位方法有捷聯(lián)慣導(dǎo)、數(shù)字全站儀、位移傳感器3 個信息源，利用聯(lián)邦濾波器對這些信息進行融合。依據(jù)聯(lián)邦濾波器結(jié)構(gòu)，設(shè)計1 個主濾波器和2 個子濾波器。子濾波器1 處理捷聯(lián)慣導(dǎo)與數(shù)字全站儀的融合信息，輸出局部狀態(tài)估計值及誤差協(xié)方差矩陣P1。若子濾波器1 輸出值有效，則代表數(shù)字全站儀光線不被遮擋，其值進入主濾波器參與融合，若無效則代表數(shù)字全站儀光線被遮擋，不參與此次主濾波器信息融合。子濾波器2 處理捷聯(lián)慣導(dǎo)與位移傳感器的融合信息，輸出局部狀態(tài)估計值及誤差協(xié)方差矩陣P2，最后進入主濾波器。主濾波器參考各子濾波器狀態(tài)估計值及對應(yīng)的誤差協(xié)方差矩陣Pi進行信息融合，得到全局狀態(tài)估計值和誤差協(xié)方差矩陣Pg。主濾波器按照信息分配原則向各子濾波器反饋更新后的全局狀態(tài)估計值和被放大的誤差協(xié)方差矩陣為子濾波器i的放大倍數(shù)），用來重置子濾波器的估計值。信息融合原理如圖4 所示。

圖4 基于聯(lián)邦濾波器的多信息融合原理Fig.4 Multi-information fusion principle based on federated filter

3.2 聯(lián)邦濾波器狀態(tài)方程

本文選取捷聯(lián)慣導(dǎo)與數(shù)字全站儀系統(tǒng)誤差作為子濾波器1 的狀態(tài)量，位置差值作為量測量；選取捷聯(lián)慣導(dǎo)與位移傳感器系統(tǒng)誤差作為子濾波器2 的狀態(tài)量，位置差值作為量測量；采用基于聯(lián)邦濾波器的多信息融合算法對三者信息進行融合，獲取掘進機器人的位姿。因此，需要對捷聯(lián)慣導(dǎo)、數(shù)字全站儀、位移傳感器的誤差進行分析和建模。

捷聯(lián)慣導(dǎo)為公共信息參考系統(tǒng)，其誤差主要考慮隨機漂移和噪聲，主要包括加速度計的零偏誤差和高斯白噪聲、陀螺儀的漂移誤差和高斯白噪聲。關(guān)于捷聯(lián)慣導(dǎo)更新算法中的姿態(tài)、速度及位置誤差模型在文獻[15-16]中有詳細介紹，在此不再贅述。

數(shù)字全站儀測量誤差主要考慮斜距測量誤差和方位角誤差，公式為

式中：e為數(shù)字全站儀測量誤差；m為兩點斜距測量誤差；θ為方位角；S為斜距；ρ為弧度化秒因子，ρ=206 265；Δθ為方位角誤差。

位移傳感器輸出的是推移掘進機器人前進的位移信息，無法單獨將其用于對掘進機器人的位姿測量，因此需要將油缸推移掘進機器人前進的位移數(shù)據(jù)與開始推移時掘進機器人的姿態(tài)信息結(jié)合起來進行航位推算，得到其相對于每次推移起始點的相對位置。位移傳感器對推移油缸行程的測量結(jié)果含有測量誤差，本方法選用的位移傳感器測量誤差為0.1%，即在推移1 m 的行程中，誤差控制在1 mm 以內(nèi)。

聯(lián)邦子濾波器1 的狀態(tài)變量為

式中：φE，φN，φU分 別為東、北、天向姿態(tài)角誤差；υE，υN，υU分別為東、北、天向速度誤差；σλ，σL分別為經(jīng)度、緯度位置誤差；分別為三向加速度計零偏誤差；?E，?N，?U分別為三向陀螺儀漂移誤差。

依據(jù)上述系統(tǒng)狀態(tài)變量可以得到聯(lián)邦子濾波器1 的狀態(tài)方程：

式中：X1(t)為t時刻聯(lián)邦子濾波器1 的狀態(tài)變量；ω1(t)為十五維狀態(tài)轉(zhuǎn)移矩陣；G1(t)為聯(lián)邦 子濾波器1 的噪聲狀態(tài)矩陣；W1(t)為期望為0、方差為Q的聯(lián)邦子濾波器1 高斯白噪聲矩陣。

聯(lián)邦子濾波器2 的狀態(tài)變量為

依據(jù)上述系統(tǒng)狀態(tài)變量可以得到聯(lián)邦子濾波器2 的狀態(tài)方程：

式中：X2(t)為t時刻聯(lián)邦子濾波器2 的狀態(tài)變量；ω2(t)為十四維狀態(tài)轉(zhuǎn)移矩陣；G2(t)為 聯(lián)邦 子濾 波器2 的噪聲狀態(tài)矩陣；W2(t)為期望為0、方差為Q的聯(lián)邦子濾波器2 高斯白噪聲矩陣。

3.3 聯(lián)邦子濾波器量測方程

本文設(shè)計的聯(lián)邦濾波器主濾波器不進行濾波，只對每個子濾波器的局部估計值進行融合得到最優(yōu)估計，下面分別給出子濾波器的量測方程。

3.3.1 子濾波器1 的量測方程

選取捷聯(lián)慣導(dǎo)輸出的位置xSINS，ySINS與數(shù)字全站儀解算的位置xDTS，yDTS差值為子濾波器1 的量測值，則子濾波器1 的量測方程為

式 中：H1(t)為子濾波器1 的量測矩陣，H1(t)=[02×6I2×2I2×302×4]，I為單位矩 陣；V1(t)為期 望為0、方差為R1的高斯白噪聲矩陣。

3.3.2 子濾波器2 的量測方程

選取捷聯(lián)慣導(dǎo)輸出的位置xSINS，ySINS與位移傳感器推算的位置xDS，yDS差值為子濾波器2 的量測值，則子濾波器2 的量測方程為

式 中：H2(t)為子濾波器2 的量測矩陣，H2(t)=[02×6I2×2I2×302×3]；V2(t)為期望為0、方差為R2的高斯白噪聲矩陣。

3.4 聯(lián)邦濾波器設(shè)計

將子濾波器i的狀態(tài)方程與量測方程離散化，獲得子濾波器i的離散型狀態(tài)方程和量測方程：

式中：Xi,k為k時刻的狀態(tài)變量；ωi(k,k-1)為k-1到k時刻的狀態(tài)轉(zhuǎn)移矩陣；Gi,k-1為k-1時刻噪聲狀態(tài)矩陣；Wi,k-1為k-1時 刻期望為0、方差為Q的高斯白噪聲矩陣；Zi,k為k時刻觀測量；Hi,k為k時刻量測矩陣；Vi,k為k時刻期望為0、方差為Ri的高斯白噪聲矩陣。

子濾波器與主濾波器之間遵守信息分配守恒原則，即

式中 βm為主濾波器的放大倍數(shù)。

本文設(shè)計的主濾波器不參與信息分配，即

主濾波器信息在子濾波器中的分配方式為

式中Ni，Ng分別為子濾波器i的噪聲協(xié)方差矩陣、主濾波器噪聲協(xié)方差矩陣。

由式（8）可知，子濾波器依據(jù)系統(tǒng)上一時刻的狀態(tài)變量預(yù)測下一時刻的狀態(tài)變量，即子濾波器協(xié)方差矩陣的時間更新過程為

由式（14）可知，子濾波器的狀態(tài)誤差協(xié)方差矩陣Pi,(k-1)和噪聲協(xié)方差矩陣Ni,(k-1)都被放大了倍。

子濾波器的預(yù)測方程為

子濾波器的估計方程為

式中Ki,k為子濾波器濾波增益，。

子濾波器的估計均方差更新為

子濾波器局部狀態(tài)估計完成后，主濾波器依據(jù)局部估計值進行全局最優(yōu)估計，其公式為

4 仿真與分析

利用基于聯(lián)邦濾波器的多信息融合算法對組合定位方法進行信息融合仿真驗證。假設(shè)陀螺儀零偏誤差為0.01 d eg/h，角度隨機游走誤差為加速度計零偏誤差為 0.5×10-6g（g為重力加速度），速度隨機游走誤差為，采樣周期為0.01 s，采樣時間為7 500 s，初始姿態(tài)角均為0，掘進機器人前進位移為50 m。將初始加速度及速度均設(shè)為0，仿真得到組合定位下掘進機器人靜態(tài)姿態(tài)角誤差，如圖5 所示?？煽闯鼍蜻M機器人靜態(tài)姿態(tài)角誤差穩(wěn)定控制在0.06′（0.001°）內(nèi)。

圖5 組合定位方法下掘進機器人靜態(tài)姿態(tài)角誤差Fig.5 Static attitude angle error of roadheader robot under combined positioning method

仿真組合定位下掘進機器人掘進過程，由于掘進機器人借助油缸按1 m 截距通過反復(fù)推拉來實現(xiàn)行走，可定義掘進機器人直線行走1 m 為1 個行走周期，1 個行走周期軌跡包括勻加速運動、勻速運動、勻減速運動、截割狀態(tài)。勻加速運動過程加速度設(shè)為0.000 5 m/s2，速度設(shè)為0，持續(xù)時間為20 s；勻速運動過程加速度設(shè)為0，速度設(shè)為0.01 m/s，持續(xù)時間為80 s；勻減速運動過程加速度設(shè)為-0.000 5 m/s2，速度設(shè)為0，持續(xù)時間為20 s；截割狀態(tài)加速度及速度均為0，持續(xù)時間為30 s。采集掘進機器人動態(tài)姿態(tài)角誤差及位置解算誤差，如圖6、圖7 所示。從圖6可看出，隨著時間的增加，掘進機器人動態(tài)姿態(tài)角誤差由1.5′（0.025°）收斂趨于0.5′（0.008 3°）內(nèi)。從圖7可看出，位置解算誤差隨著時間的增加慢慢趨于穩(wěn)定，位置解算誤差最大為0.005 m。

圖6 組合定位方法下掘進機器人動態(tài)姿態(tài)角誤差Fig.6 Dynamic attitude angle error of roadheader robot under combined positioning method

圖7 組合定位方法下掘進機器人位置解算誤差Fig.7 Position calculation error of roadheader robot under combined positioning method

在相同條件下做純慣導(dǎo)定位位置解算仿真，位置解算誤差如圖8 所示?？煽闯黾儜T導(dǎo)位置解算誤差隨時間增加而增大，最大誤差為0.3 m。

圖8 純慣導(dǎo)定位位置解算誤差Fig.8 Position calculation error of pure inertial navigation positioning

綜上可看出：組合定位方法定位精度較高，且能夠很好地抑制純慣導(dǎo)位置解算誤差累計。

5 工業(yè)性試驗與分析

在陜煤榆北煤業(yè)小保當煤礦對捷聯(lián)慣導(dǎo)+數(shù)字全站儀+位移傳感器組合定位方法在煤礦護盾式掘進機器人系統(tǒng)中的應(yīng)用進行了工業(yè)性試驗，試驗采用MG045 捷聯(lián)慣導(dǎo)、XI -1001Q 數(shù)字全站儀、GUC1200A 位移傳感器、360 棱鏡、工控機等設(shè)備。

5.1 姿態(tài)角靜態(tài)漂移試驗

在井下掘進工作面采集煤礦護盾式掘進機器人的靜止姿態(tài)角數(shù)據(jù)，時長為3 h，結(jié)果如圖9 所示?？煽闯龊较蚪恰⒏┭鼋?、橫滾角的變化趨勢均穩(wěn)定保持在0.005°以內(nèi)，說明組合定位方法可以精確測量煤礦護盾式掘進機器人靜止姿態(tài)信息，并且在較長時間內(nèi)保持較高的姿態(tài)解算精度。

圖9 靜止狀態(tài)時姿態(tài)角變化趨勢Fig.9 Change trend of attitude angle in static state

5.2 姿態(tài)角動態(tài)穩(wěn)定性試驗

為了驗證本文提出的組合定位方法在煤礦護盾式掘進機器人系統(tǒng)掘進過程中姿態(tài)測量的有效性，選取掘進機器人靜態(tài)姿態(tài)角信息作為其掘進時的姿態(tài)角參考值，與組合定位方法測量得到的姿態(tài)信息進行對比，對比結(jié)果如圖10 所示。從圖10 可看出：當煤礦護盾式掘進機器人系統(tǒng)處于掘進狀態(tài)時，整體姿態(tài)角變化趨勢呈現(xiàn)動態(tài)穩(wěn)定狀態(tài)，航向角、俯仰角、橫滾角最大誤差分別為0.031 1、0.048 4、0.039 7°，測量精度較高，可以準確反映出掘進機器人系統(tǒng)在掘進過程中的姿態(tài)信息。

圖10 掘進狀態(tài)時姿態(tài)角變化趨勢Fig.10 Change trend of attitude angle in driving state

5.3 組合定位與分析

井下跟班9 h，煤礦護盾式掘進機器人按照初始方向行進22 m，油缸每次推移量為1 m。在試驗中，煤礦護盾式掘進機器人實際位置無法得知，以數(shù)字全站儀測量的位置數(shù)據(jù)為掘進機器人實際位置。捷聯(lián)慣導(dǎo)、數(shù)字全站儀和位移傳感器三者組合定位數(shù)據(jù)曲線如圖11 所示，可看出煤礦護盾式掘進機器人融合定位曲線與實際位置曲線基本重合。組合定位數(shù)據(jù)與實際值之間的誤差曲線如圖12 所示，可看出組合定位方法下煤礦護盾式掘進機器人在x軸和y軸上的位置誤差分別控制在±0.03 m 和±0.02 m，有效消除了純慣導(dǎo)解算所造成的累計誤差，保證了系統(tǒng)的精確性、穩(wěn)定性和可靠性。

圖11 組合定位方法下煤礦護盾式掘進機器人位置曲線Fig.11 Position curves of coal mine shield roadheader robot under combined positioning method

圖12 組合定位方法下煤礦護盾式掘進機器人位置誤差曲線Fig.12 Position error curve of coal mine shield roadheader robot under combined positioning method

6 結(jié)論

（1）基于捷聯(lián)慣導(dǎo)+數(shù)字全站儀+位移傳感器的組合定位方法通過數(shù)字全站儀和位移傳感器共同測量的位置信息輔助修正捷聯(lián)慣導(dǎo)系統(tǒng)解算得到的煤礦護盾式掘進機器人的位置及姿態(tài)角信息，能夠有效抑制慣導(dǎo)系統(tǒng)隨時間所產(chǎn)生的累計位置誤差。利用基于聯(lián)邦濾波器的多信息融合算法將捷聯(lián)慣導(dǎo)解算得到的位置及姿態(tài)角信息、全站儀測量得到的位置信息及位移傳感器推算得到的位置信息融合，從而得到煤礦護盾式掘進機器人的精確位姿信息，實現(xiàn)較高精度定位。

（2）仿真及工業(yè)性試驗結(jié)果表明：基于捷聯(lián)慣導(dǎo)+數(shù)字全站儀+位移傳感器的組合定位方法有效克服了單一輔助測量方式與慣導(dǎo)組合測量方法的不穩(wěn)定、不可靠問題，能夠很好地抑制純慣導(dǎo)位置解算誤差累計，實現(xiàn)了煤礦護盾式掘進機器人的精確定位，x軸和y軸上的位置誤差分別控制在±0.03 m 和±0.02 m，滿足井下掘進工作面要求。