基于空間陣列式慣性單元的防沖鉆孔機(jī)器人位姿解算方法

王忠賓，司 壘，王 浩，張修峰，趙世豪，魏 東，譚 超，閆海峰

(1. 中國礦業(yè)大學(xué) 機(jī)電工程學(xué)院，江蘇 徐州 221116； 2. 山東能源集團(tuán)有限公司，山東 濟(jì)南 250014)

隨著我國煤礦開采深度和強(qiáng)度的不斷加大，沖擊地壓已成為我國深部煤礦開采中最嚴(yán)重的動(dòng)力災(zāi)害之一，其發(fā)生頻次和破壞強(qiáng)度不斷增大，對(duì)深部煤層的安全開采造成了極大的影響。目前，我國煤礦企業(yè)進(jìn)行沖擊地壓鉆孔卸壓作業(yè)時(shí)，卸壓作業(yè)需要人員進(jìn)入危險(xiǎn)區(qū)域，卸壓過程中容易誘發(fā)沖擊地壓災(zāi)害，進(jìn)而造成較大的人員傷害事故。因此，迫切需要實(shí)現(xiàn)鉆孔卸壓作業(yè)的機(jī)器人化，而鉆孔卸壓區(qū)域的機(jī)器人化作業(yè)首要解決的關(guān)鍵技術(shù)便是鉆孔機(jī)器人的精確定位和定姿問題。

現(xiàn)有的移動(dòng)機(jī)器人定位技術(shù)主要包括紅外定位、超寬帶定位、超聲波定位、里程計(jì)定位、視覺定位、激光雷達(dá)定位及慣性導(dǎo)航定位等。JOBES等提出了一種采煤機(jī)的遠(yuǎn)程定位系統(tǒng)MPHS，并給出了計(jì)算位置和航向的方法。MARREF和BARRET基于超聲波技術(shù)實(shí)現(xiàn)了移動(dòng)機(jī)器人在自主導(dǎo)航過程中的路徑規(guī)劃及合理避障。LEDERGERBER A等利用單向信號(hào)傳遞的超寬帶雷達(dá)開發(fā)了一種定位系統(tǒng)，通過在固定位置設(shè)置收發(fā)器，獲取目標(biāo)位置信息，以達(dá)到定位目的。馮佳萌等提出了一種增加掃描匹配(SM)和離散傅里葉變換(DFT)的優(yōu)化AMCL算法，實(shí)驗(yàn)結(jié)果表明機(jī)器人運(yùn)動(dòng)過程中軸絕對(duì)定位精度誤差減小為0.020 m，軸絕對(duì)定位精度誤差減小為0.019 m。徐慶坤等提出了一種基于UWB與里程計(jì)融合的移動(dòng)機(jī)器人定位方法，其平均定位誤差為0.2 m。朱道俊等設(shè)計(jì)了基于視覺和輪速計(jì)緊耦合的輪式機(jī)器人定位算法，算法軌跡估計(jì)誤差在0.5%以內(nèi)。尚磊等基于開源ORB-SLAM2算法實(shí)現(xiàn)了煤礦井下搜救機(jī)器人的精確定位。蔡李花等基于航跡推算的自主定位方法提出了一種有限記憶卡爾曼濾波方法，并采用逐級(jí)濾波的方式融合多傳感器信息，經(jīng)過仿真分析，該方法可以穩(wěn)定實(shí)現(xiàn)0.5 m的跟蹤精度，提高了礦井救災(zāi)機(jī)器人的自主定位精度。

煤礦井下巷道空間狹小、環(huán)境復(fù)雜、工況惡劣，地面上常用的衛(wèi)星導(dǎo)航定位、無線通訊定位等常規(guī)定位技術(shù)無法實(shí)現(xiàn)在狹長巷道中對(duì)鉆孔機(jī)器人進(jìn)行精確定位。由于慣性測量單元(IMU)不需要借助外界的信息，用于解算的初始數(shù)據(jù)來自元件自身，對(duì)外界環(huán)境依賴小，且不易受到外界環(huán)境干擾，也不需要與外接設(shè)備交換信息，可以較好的適應(yīng)井下復(fù)雜環(huán)境。因此，慣性導(dǎo)航逐漸成為井下移動(dòng)裝備自主導(dǎo)航的重要技術(shù)，國內(nèi)外學(xué)者針對(duì)基于慣性導(dǎo)航系統(tǒng)的井下移動(dòng)裝備定位方法進(jìn)行了大量的研究。澳大利亞聯(lián)邦科學(xué)與工業(yè)研究組織(CSIRO)推出了基于陀螺儀導(dǎo)向定位的自動(dòng)化采煤方法(簡稱LASC)，解決了慣性導(dǎo)航系統(tǒng)與采煤機(jī)高度通信、采煤機(jī)初始校準(zhǔn)、截割曲線生成和支架推移精確推溜控制等難題。趙靖、郭曉晶等提出了基于卡爾曼濾波算法的采煤機(jī)慣性導(dǎo)航定位方法，經(jīng)過卡爾曼濾波不斷修正系統(tǒng)設(shè)定狀態(tài)參數(shù)與實(shí)際狀態(tài)參數(shù)之間的誤差，實(shí)現(xiàn)對(duì)采煤機(jī)的精確定位。樊啟高、楊海等設(shè)計(jì)了基于捷聯(lián)慣導(dǎo)的采煤機(jī)定位系統(tǒng)，通過解算采煤機(jī)的三軸加速度和三軸角速度來獲得采煤機(jī)的位姿信息。張羽飛等通過將慣性導(dǎo)航數(shù)據(jù)與視覺數(shù)據(jù)進(jìn)行融合優(yōu)化求解機(jī)器人自身位姿信息，絕對(duì)誤差由0.6 m降低到0.4 m以下，平均誤差由0.20 m減小到0.15 m，均方根誤差由0.24 m減小到0.18 m。溫良等設(shè)計(jì)了基于慣性傳感器和和超聲波融合的導(dǎo)航方法，經(jīng)過仿真分析后算法輸出軌跡的誤差在-3～4 cm，滿足了煤礦救災(zāi)機(jī)器人的定位需求。張璞針對(duì)煤礦井下巷道道路彎曲、能見度低等問題，提出了一種基于捷聯(lián)慣導(dǎo)和里程計(jì)組合定位的井下移動(dòng)機(jī)器人的定位方法，并利用卡爾曼濾波算法完成對(duì)機(jī)器人的位姿估計(jì)，實(shí)驗(yàn)結(jié)果表明機(jī)器人整體定位準(zhǔn)確度達(dá)到95%左右，組合定位方法具有可行性。李猛鋼針對(duì)井下特殊的工作環(huán)境，提出了一種基于雷達(dá)、IMU和UWB融合的LIU-SLAM方法，大場景下靜態(tài)絕對(duì)定位精度均值10 cm以內(nèi)，動(dòng)態(tài)絕對(duì)定位精度均值25 cm以內(nèi)，提高了鉆孔機(jī)器人的定位精度。

盡管國內(nèi)外學(xué)者對(duì)移動(dòng)機(jī)器人定位進(jìn)行了大量的研究工作，但仍然存在一些問題：① 傳統(tǒng)的紅外定位、超聲波定位等移動(dòng)機(jī)器人定位技術(shù)，在煤礦井下應(yīng)用時(shí)對(duì)實(shí)際工況與環(huán)境因素的依賴度較高，無法較好地適應(yīng)實(shí)際工作環(huán)境；② 慣性導(dǎo)航系統(tǒng)容易隨時(shí)間產(chǎn)生累積誤差，目前針對(duì)慣性導(dǎo)航系統(tǒng)累積誤差修正的方法主要集中在改進(jìn)慣性導(dǎo)航算法和利用輔助定位手段校正慣性導(dǎo)航誤差方面，缺少從慣性系統(tǒng)結(jié)構(gòu)上抑制累積誤差的方法。

為了降低防沖鉆孔機(jī)器人慣性導(dǎo)航定位帶來的累計(jì)誤差，筆者分析了慣性導(dǎo)航定位過程中產(chǎn)生的確定性漂移與非確定性漂移，建立了空間陣列式慣性單元組件，實(shí)現(xiàn)了鉆孔機(jī)器人位置和姿態(tài)的精確解算，并通過仿真和實(shí)驗(yàn)驗(yàn)證了所提位姿解算方法的可行性和實(shí)用性。

1 空間陣列式慣性單元數(shù)據(jù)融合模型建立

1.1 坐標(biāo)系建立及轉(zhuǎn)換過程

基于空間陣列式慣性單元的防沖鉆孔機(jī)器人位姿解算思想是將慣性單元組件固連在防沖鉆孔機(jī)器人上，分別測量出其三軸加速度和三軸角速度信息，通過工控機(jī)將采集到的傳感信息進(jìn)行解算，獲取防沖鉆孔機(jī)器人的位姿信息。坐標(biāo)系建立方式如圖1所示。

圖1 坐標(biāo)系建立Fig.1 Establishment of the coordinate system

慣性坐標(biāo)系用表示，原點(diǎn)位于地球中心，軸位于赤道平面內(nèi)，軸沿著地球的極軸，軸與軸和軸構(gòu)成右手螺旋。地球坐標(biāo)系與地球固連，隨地球的轉(zhuǎn)動(dòng)而轉(zhuǎn)動(dòng)，并用表示，原點(diǎn)位于地球中心，軸指向赤道與格林尼子午線的交點(diǎn)，軸與慣性坐標(biāo)系重合，軸由右手螺旋定則確定。導(dǎo)航坐標(biāo)系用表示，用來進(jìn)行防沖鉆孔機(jī)器人的定位誤差分析。載體坐標(biāo)系固連在防沖鉆孔機(jī)器人機(jī)身，原點(diǎn)位于防沖鉆孔機(jī)器人中心處，軸指向前進(jìn)的方向，軸垂直于機(jī)身指向右，軸垂直機(jī)身平面指向上。

防沖鉆孔機(jī)器人的位姿參數(shù)在導(dǎo)航坐標(biāo)系下獲得，而慣性單元組件感知的運(yùn)動(dòng)特征是相對(duì)于載體坐標(biāo)系的。因此需要建立旋轉(zhuǎn)矩陣實(shí)現(xiàn)坐標(biāo)系的轉(zhuǎn)換。各個(gè)坐標(biāo)系的轉(zhuǎn)換是通過坐標(biāo)軸的旋轉(zhuǎn)來完成的，步驟如下：導(dǎo)航坐標(biāo)繞軸轉(zhuǎn)動(dòng)角度，獲得坐標(biāo)系；之后將獲得的坐標(biāo)系繞其軸轉(zhuǎn)動(dòng)角度，最后將新坐標(biāo)系繞其軸轉(zhuǎn)動(dòng)角度，將導(dǎo)航坐標(biāo)系轉(zhuǎn)換成載體坐標(biāo)系，其中為航偏角，為橫滾角，為俯仰角。

1.2 空間陣列式慣性單元的布局方法

采用慣性單元對(duì)防沖鉆孔機(jī)器人進(jìn)行定位、定姿時(shí)，由于溫度、安裝位置和振動(dòng)等外界因素的干擾，導(dǎo)致慣性單元測量的運(yùn)動(dòng)參數(shù)與真實(shí)運(yùn)動(dòng)參數(shù)之間存在偏差，使得經(jīng)過解算之后獲得的位姿信息有較大誤差。筆者將慣性單元產(chǎn)生的誤差分為2類：確定性誤差和非確定性誤差。確定性誤差指的是方向和大小確定的誤差；非確定性誤差指的是方向或大小不確定的誤差。為了盡量消除確定性誤差和減少非確定性誤差，筆者設(shè)計(jì)了一種基于空間陣列式慣性單元組件的誤差補(bǔ)償方法，該補(bǔ)償方法將5個(gè)慣性傳感單元進(jìn)行空間陣列式分布，利用空間陣列式慣性單元組件進(jìn)行數(shù)據(jù)融合，融合后的傳感數(shù)據(jù)再經(jīng)過解算，得到防沖鉆孔機(jī)器人準(zhǔn)確的位置和姿態(tài)信息?？臻g陣列式慣性單元的布局方式如圖2所示。

圖2 空間陣列式慣性單元布局方式Fig.2 Space array inertial sensing component layout

由圖2可知，5個(gè)慣性單元分別位于正四面體的4個(gè)頂點(diǎn)和中心處，構(gòu)成了空間布局方式。通過坐標(biāo)軸的反向布局及坐標(biāo)軸之間成一定角度的空間結(jié)構(gòu)，對(duì)輸出參數(shù)進(jìn)行分解和多個(gè)傳感軸取平均的方法融合5個(gè)慣性傳感單元數(shù)據(jù)。理論上，空間陣列式慣性單元能夠基本消除確定性誤差，降低非確定性誤差。

1.3 數(shù)據(jù)融合方程

..角速度融合方程

當(dāng)單個(gè)慣性單元繞軸轉(zhuǎn)動(dòng)時(shí)，慣性單元輸出的角速度包括真實(shí)角速度、角速度確定性誤差、角速度非確定性誤差的矢量和，計(jì)算公式為

=++

(1)

由于角速度確定性誤差的大小和方向一致，因此在各慣性單元的融合過程中可以將其抵消。對(duì)于角速度非確定性誤差，如圖2所示，3號(hào)和4號(hào)慣性單元的軸和軸與載體坐標(biāo)系存在一定角度，融合前需要將角速度換算到載體坐標(biāo)系坐標(biāo)軸上。將3號(hào)慣性單元測得角速度轉(zhuǎn)換到載體坐標(biāo)系中需要將坐標(biāo)系繞軸順時(shí)針旋轉(zhuǎn)150°，由此得到3號(hào)慣性單元測得的載體坐標(biāo)系下三軸角速度為

(2)

式中，3，3,3分別為3號(hào)慣性單元坐標(biāo)系變換到載體坐標(biāo)系繞，,軸旋轉(zhuǎn)的角速度;3,3,3分別為3號(hào)慣性單元測量的,,軸旋轉(zhuǎn)角速度；為正三角形的內(nèi)角，空間陣列式布局(如圖2所示)中2號(hào)慣性單元、3號(hào)慣性單元、4號(hào)慣性單元在同一平面上且距離相同構(gòu)成正三角形。同理，可以得到4號(hào)慣性單元測得載體坐標(biāo)系下的三軸角速度為

(3)

由此，可以得到的空間陣列式慣性單元組件的角速度融合方程為

(4)

式中，為空間陣列式慣性單元輸出的角速度矢量；s,s,s為繞,,軸旋轉(zhuǎn)的角速度。

通過對(duì)式(4)分析可知，慣性單元的確定性誤差大小和方向均已知。當(dāng)陣列式慣性單元中每個(gè)慣性單元的確定性誤差大小和方向全都一致時(shí)，融合后的確定性誤差在軸和軸的角速度和比力加速度確定性誤差為原來的1/5，軸的角速度和比力加速度確定性誤差與單個(gè)慣性單元保持一致。雖然實(shí)際情況中很難保證多個(gè)慣性單元的確定性誤差大小和方向都一致，這種情況下融合后的確定性誤差仍保持在一個(gè)較低的水平，在位姿解算時(shí)可以通過誤差校準(zhǔn)將該確定性誤差徹底消除。因此，確定性誤差可以被基本抵消，而非確定性誤差大小和方向均未知，融合后的非確定性誤差可以使每一時(shí)刻非確定性誤差都保持在較低的水平，并且一些特殊時(shí)間點(diǎn)非確定性誤差為0，從而抑制誤差的快速積累。因此，非確定性誤差僅能得到一定的抑制，無法徹底消除。

..比力加速度融合方程

當(dāng)單個(gè)慣性單元出現(xiàn)位移時(shí)，其測出的比力加速度包括真實(shí)比力加速度、比力加速度確定性漂移和比力加速度非確定性漂移，計(jì)算公式為

=++

(5)

由于比力加速度為矢量，可以在空間內(nèi)進(jìn)行合成和分解，所以空間陣列式慣性單元的比力加速度融合方程為

(6)

式中，為空間陣列式慣性單元輸出的比力加速度矢量；s,s,s分別為為沿著,,軸的比力加速度；0,0,0分別為第0號(hào)慣性傳感單元輸出的，,軸比力加速度。

分析比力加速度的融合方程可知，該融合方式可以較大程度消除比力加速度的確定性誤差；另外，由于軸線角度的偏差可以降低振動(dòng)等非確定性的干擾，所以該融合方式可以大大降低非確定性誤差，提高載體運(yùn)動(dòng)參數(shù)輸出的精度。

2 空間陣列式慣性單元位姿解算

2.1 防沖鉆孔機(jī)器人姿態(tài)解算

筆者采用四元數(shù)法來求解防沖鉆孔機(jī)器人的姿態(tài)變換矩陣，進(jìn)行姿態(tài)更新。四元數(shù)由1個(gè)實(shí)數(shù)單位和3個(gè)虛數(shù)單位構(gòu)成，防沖鉆孔機(jī)器人載體坐標(biāo)系相對(duì)于導(dǎo)航坐標(biāo)系的轉(zhuǎn)動(dòng)可以用四元數(shù)惟一表示:

=+++

(7)

(8)

其中，，，，為實(shí)數(shù)。

四元數(shù)運(yùn)動(dòng)學(xué)微分方程為

(9)

(10)

則由式(8)，(9)可以求得防沖鉆孔機(jī)器人的姿態(tài)角：

(11)

由于航偏角為0°～360°，解算時(shí)出現(xiàn)多值對(duì)應(yīng)，因此需要確定航向角的真值，有

(12)

2.2 防沖鉆孔機(jī)器人速度解算

防沖鉆孔機(jī)器速度解算方法是將加速度進(jìn)行積分得到其速度，更新方程為

(13)

(14)

將式(14)中的變量簡記為

(15)

可以得到

(16)

2.3 防沖鉆孔機(jī)器人位置解算

防沖鉆孔機(jī)器人在煤礦井下巷道中進(jìn)行作業(yè)，其運(yùn)動(dòng)距離遠(yuǎn)小于地球的曲率半徑，因此在其運(yùn)動(dòng)過程中可以認(rèn)為地球的曲率半徑不變。定義防沖鉆孔機(jī)器人運(yùn)動(dòng)起始中心處為原點(diǎn)，軸指向東方向，軸指向北方向，軸垂直于當(dāng)?shù)厮矫嬷赶蛏?。在此坐?biāo)系下描述防沖鉆孔機(jī)器人的空間位置，如下：

(17)

3 仿真分析

3.1 防沖鉆孔機(jī)器人運(yùn)行工況分析

為了驗(yàn)證筆者提出的防沖鉆孔機(jī)器人位姿解算方法在不同運(yùn)動(dòng)工況下的位姿解算精度，需要獲取不同運(yùn)動(dòng)工況下慣性傳感單元的原始輸出數(shù)據(jù)，作為空間陣列式慣性單元解算方法的輸入，并利用解算后的位姿信息和真實(shí)的位姿信息進(jìn)行對(duì)比。由于防沖鉆孔機(jī)器人在井下巷道中作業(yè)工況和環(huán)境比較復(fù)雜，無法獲取完善的鉆孔機(jī)器人實(shí)際運(yùn)行參數(shù)。因此，筆者采用數(shù)值仿真來模擬防沖鉆孔機(jī)器人的不同運(yùn)動(dòng)工況，并給出不同運(yùn)動(dòng)工況下的慣性單元原始輸出數(shù)據(jù)和參考速度、參考位置等防沖鉆孔機(jī)器人的運(yùn)動(dòng)微分方程。

防沖鉆孔機(jī)器人的運(yùn)動(dòng)工況主要有直線運(yùn)動(dòng)、曲線運(yùn)動(dòng)兩種，是鉆孔卸壓作業(yè)時(shí)最常見的運(yùn)動(dòng)。為了方便運(yùn)動(dòng)工況的模擬，需要定義一個(gè)防沖鉆孔機(jī)器人運(yùn)動(dòng)軌跡坐標(biāo)系，軸沿運(yùn)動(dòng)軌跡水平向前，軸沿運(yùn)動(dòng)軌跡切線指向右，軸符合右手螺旋原則垂直于面指向上，筆者模擬運(yùn)動(dòng)工況時(shí)加速度()和角速度()的定義均在此坐標(biāo)系內(nèi)。防沖鉆孔機(jī)器人工作過程中的運(yùn)動(dòng)狀態(tài)變化均是由角速度()和加速度()變化引起的。

3.2 防沖鉆孔機(jī)器人運(yùn)動(dòng)微分方程

防沖鉆孔機(jī)器人運(yùn)動(dòng)微分方程用來求解空間陣列式慣性單元的原始輸出數(shù)據(jù)和參考運(yùn)動(dòng)數(shù)據(jù)，包括姿態(tài)角，，，速度，位置經(jīng)度、緯度、東向位移、北向位移、天向位移等。防沖鉆孔機(jī)器人運(yùn)動(dòng)微分方程以防沖鉆孔機(jī)器人運(yùn)動(dòng)工況分析中所述的角速度()和加速度()作為輸入，直接積分求解便可獲取各種運(yùn)動(dòng)參數(shù)。

(1)姿態(tài)角微分方程：

(18)

(19)

(20)

(2)速度微分方程：

(21)

(3)位置微分方程：

(22)

(4)慣性傳感組件輸出方程：

(23)

為了驗(yàn)證筆者提出的空間陣列式慣性單元位姿解算方法的性能，分別利用文獻(xiàn)[20-21]中的算法(分別記為單IMU、差分式IMU)獲取的防沖鉆孔機(jī)器人的位姿信息，并與模擬出的防沖鉆孔機(jī)器人參考位姿信息進(jìn)行對(duì)比，得到位姿信息的誤差曲線。由于對(duì)曲線運(yùn)動(dòng)模擬時(shí)已經(jīng)包含直線運(yùn)動(dòng)，所以筆者就不再對(duì)直線運(yùn)動(dòng)進(jìn)行解算分析。

在曲線運(yùn)動(dòng)仿真中，防沖鉆孔機(jī)器人初始姿態(tài)角=[0 0 0]，初始速度=[0 0 0]，初始位置=[0 0 0]，初始位移=[0 0 0]，采樣頻率=10 Hz。設(shè)置防沖鉆孔機(jī)器人沿軸方向運(yùn)動(dòng)28 m，沿軸方向運(yùn)動(dòng)0.8 m。根據(jù)以上參數(shù)，結(jié)合防沖鉆孔機(jī)器人運(yùn)動(dòng)工況模擬方法，通過計(jì)算便可獲得曲線運(yùn)動(dòng)的仿真參數(shù)：仿真時(shí)間500 s，0～60 s靜止，60～70 s加速，加速度為0.01 m/s，70～200 s勻速，200～230 s進(jìn)入曲線，航向角角速度為0.003 8 rad/s，230～270 s勻速行走，270～300 s退出曲線，航向角角速度為-0.003 8 rad/s，300～430 s勻速，430～440 s減速，加速度為0.01 m/s，440～500 s靜止。最終，上述曲線運(yùn)動(dòng)的3種軌跡解算結(jié)果如圖3所示。

圖3 曲線運(yùn)動(dòng)的解算結(jié)果Fig.3 Contrast result of curve motion

由圖3可以看出，單個(gè)慣性單元解算方法偏差最大，特別是在運(yùn)動(dòng)后期，累積誤差使輸出軌跡明顯偏離真實(shí)軌跡，差分式解算方法可以抑制一定的漂移，使得輸出軌跡基本可以跟隨真實(shí)軌跡形態(tài)，但是仍然存在很大的誤差。筆者提出的空間陣列式解算方法能夠較好地跟蹤防沖鉆孔機(jī)器人的基準(zhǔn)軌跡，具有更好地解算精度。

圖4為防沖鉆孔機(jī)器人作模擬曲線運(yùn)動(dòng)時(shí)，姿態(tài)角和位移的解算誤差對(duì)比結(jié)果，曲線運(yùn)動(dòng)是通過航偏角的多次變化而獲得，因此航偏角的模擬曲線發(fā)生了多次突變。可以看出，筆者提出的位姿解算方法在三向位移和三向姿態(tài)角的解算結(jié)果均明顯優(yōu)于單IMU和差分式IMU解算方法?？臻g陣列式慣性單元解算方法在東向位移的最大誤差為0.676 0 m，平均絕對(duì)誤差為0.344 9 m，北向位移的的最大誤差為0.016 1 m，平均誤差為7.039 8×10m，天向位移最大誤差為0.001 0 m，平均誤差為3.086×10m；在航偏角的最大誤差0.147 4°，平均誤差0.030 9°，俯仰角的最大誤差0.072 8°，平均誤差為0.025 8°，橫滾角的最大誤差0.044 0°，平均誤差為0.012 7°。詳細(xì)對(duì)比結(jié)果見表1，可以看出，筆者提出的解算方法具有較小的位姿解算誤差，說明在航偏角頻繁改變的情況下，提出的解算方法能夠更好地抑制慣性單元的測量誤差和漂移誤差，可以在不同工況下均具有更優(yōu)的跟隨能力和解算精度。

圖4 模擬曲線運(yùn)動(dòng)的位姿解算對(duì)比結(jié)果Fig.4 Comparison of position and attitude calculation results of simulated curve motion

表1 模擬曲線運(yùn)動(dòng)的位姿解算誤差對(duì)比

4 實(shí)驗(yàn)驗(yàn)證

4.1 運(yùn)動(dòng)工況模擬實(shí)驗(yàn)

為了進(jìn)一步驗(yàn)證筆者設(shè)計(jì)的位姿解算方法在真實(shí)工況下的可行性，根據(jù)實(shí)際工況搭建了移動(dòng)載體運(yùn)動(dòng)工況模擬實(shí)驗(yàn)臺(tái)，進(jìn)行曲線運(yùn)行工況的模擬實(shí)驗(yàn)，如圖5所示。

圖5 移動(dòng)載體運(yùn)動(dòng)工況模擬實(shí)驗(yàn)臺(tái)Fig.5 Motion simulation test bench of mobile carrier

移動(dòng)載體運(yùn)動(dòng)工況模擬實(shí)驗(yàn)臺(tái)運(yùn)動(dòng)軌跡設(shè)置如圖6所示，為曲線運(yùn)動(dòng)的起始點(diǎn)，到為直線運(yùn)動(dòng)階段，與之間的距離為240 cm，在點(diǎn)移動(dòng)平臺(tái)順時(shí)針旋轉(zhuǎn)135°，與之間的距離為339.41 cm，在點(diǎn)，移動(dòng)平臺(tái)逆時(shí)針旋轉(zhuǎn)135°，與之間的距離為240 cm，在順時(shí)針旋轉(zhuǎn)150°，移動(dòng)平臺(tái)從點(diǎn)直線行走277.13 cm到達(dá)終點(diǎn)。在運(yùn)動(dòng)過程中，運(yùn)動(dòng)平臺(tái)的加速度設(shè)定為50 cm/s，速度為10 cm/s，轉(zhuǎn)動(dòng)角速度設(shè)定為π/20 rad/s。

圖6 模擬運(yùn)動(dòng)軌跡Fig.6 Simulate a motion trajectory

根據(jù)以上設(shè)置的節(jié)點(diǎn)以及運(yùn)行軌跡的長度和姿態(tài)，設(shè)定了移動(dòng)平臺(tái)的加速度和最大速度等參數(shù)，擬合出移動(dòng)平臺(tái)真實(shí)的位姿曲線，并利用筆者提出的解算方法對(duì)機(jī)器人位置和姿態(tài)進(jìn)行解算，解算對(duì)比結(jié)果如圖7所示。

由圖7(a)～(c)可以看出，在導(dǎo)航坐標(biāo)系中三軸方向的位移都發(fā)生了一定漂移，但筆者提出的解算方法位移平均誤差明顯小于單IMU和差分式IMU解算方法，在軸方向的位移最大誤差為17.69 cm，平均誤差為5.83 cm；在軸方向的位移最大誤差為19.43 cm，平均誤差8.76 cm；在軸方向的位移最大誤差為2.07 cm，平均誤差為0.84 cm，詳細(xì)對(duì)比結(jié)果見表2。

由于實(shí)驗(yàn)室地面為水平平面，干擾較小，所以移動(dòng)平臺(tái)在軸方向的位移誤差小于另外兩軸方向。從軸和軸方向位移誤差可以看出，在平面運(yùn)動(dòng)中，筆者提出的解算方法能夠較好的抑制誤差的漂移，減少誤差的積累，從而實(shí)現(xiàn)對(duì)載體運(yùn)動(dòng)軌跡較為準(zhǔn)確地跟蹤。

由圖7(d)～(f)可以看出，筆者提出的解算方法在俯仰角、航偏角和橫滾角的解算精度均明顯高于單IMU和差分式IMU，3個(gè)姿態(tài)角的平均誤差分別為0.07°，0.15°和0.06°，詳細(xì)對(duì)比結(jié)果見表3。由于曲線運(yùn)動(dòng)中航偏角發(fā)生多次變化，導(dǎo)致航偏角誤差較大，符合運(yùn)動(dòng)特征。在航偏角多次變化的情況下，筆者提出的解算方法依然能夠較為準(zhǔn)確地解算出載體的運(yùn)動(dòng)姿態(tài)。

4.2 地面實(shí)驗(yàn)

為了進(jìn)一步對(duì)防沖鉆孔機(jī)器人位姿解算方法的可行性進(jìn)行驗(yàn)證，筆者搭建了防沖鉆孔機(jī)器人慣性單元位姿監(jiān)測實(shí)驗(yàn)平臺(tái)，并進(jìn)行了直線行走地面實(shí)驗(yàn)，如圖8所示。空間陣列式慣性單元被固定在空間架上，空間架被固定在防沖鉆孔機(jī)器人本體上?？臻g陣列式慣性單元組件通過信號(hào)線與轉(zhuǎn)換模塊連接，轉(zhuǎn)換模塊通過USB接頭與上位機(jī)通信。

..位置監(jiān)測實(shí)驗(yàn)

由于車間環(huán)境復(fù)雜、空間狹小等條件的限制，無法設(shè)置防沖鉆孔機(jī)器人行走過于復(fù)雜的軌跡，因此將防沖鉆孔機(jī)器人加速到最大速度，然后以最大速度運(yùn)行至設(shè)定位置，設(shè)定防沖鉆孔機(jī)器人沿鉆進(jìn)方向直線行走10 m，并將前進(jìn)方向設(shè)定為到導(dǎo)航坐標(biāo)系軸正方向，垂直于機(jī)身指向右為軸正方向，軸垂直于軸和軸平面指向上。將在防沖鉆孔機(jī)器人運(yùn)動(dòng)軌跡上設(shè)定標(biāo)記點(diǎn)，通過記錄防沖鉆孔機(jī)器人到達(dá)標(biāo)記的時(shí)間計(jì)算出其真實(shí)速度、加速度和位置，如圖8所示。

圖7 位姿解算結(jié)果對(duì)比Fig.7 Comparison of position and attitude calculation results

表2 三軸方向的位移解算誤差對(duì)比

表3 姿態(tài)角的解算誤差對(duì)比

圖8 現(xiàn)場實(shí)驗(yàn)平臺(tái)Fig.8 Field experimental platform

在上位機(jī)上利用不同方法解算出防沖鉆孔機(jī)器人的位置參數(shù)，將解算出的位置參數(shù)與真實(shí)軌跡進(jìn)行對(duì)比，得出位置監(jiān)測誤差，如圖9所示。

由圖9可知，單IMU解算方法在軸、軸和軸方向上位移的最大誤差分別為116.00，24.76和4.41 cm，平均誤差分別為42.13，9.73和9.84 cm；差分式IMU解算方法的位移最大誤差分別為19.03，10.91 和17.09 cm，平均誤差為4.82，3.86和3.35 cm；筆者提出解算方法的位移最大誤差分別為6.04，3.41和5.12 cm，平均誤差1.98，1.72和1.57 cm。對(duì)比結(jié)果表明鉆孔機(jī)器人的位置解算結(jié)果沿各軸線均存在一定誤差，這是由于防沖鉆孔機(jī)器人的位移參數(shù)是加速度經(jīng)過多次積分而得到的，進(jìn)而導(dǎo)致位置誤差的漂移，但本文方法解算的位置誤差最小，能夠滿足使用要求。

..姿態(tài)監(jiān)測實(shí)驗(yàn)

設(shè)定與上述相同的防沖鉆孔機(jī)器人運(yùn)動(dòng)軌跡，并對(duì)其姿態(tài)進(jìn)行監(jiān)測。由于防沖鉆孔機(jī)器人沿直線運(yùn)動(dòng)，所以俯仰角、航偏角和橫滾角理論上均未發(fā)生變化。由此得到姿態(tài)監(jiān)測誤差，如圖10所示。

圖9 位移監(jiān)測結(jié)果Fig.9 Position monitoring results

從圖10可以得出，單IMU解算方法在航偏角、俯仰角和橫滾角方面的最大誤差分別為3.57°，3.77°和4.10°，平均誤差分別為0.97°，1.47°和1.52°；差分式IMU解算方法的姿態(tài)角最大誤差分別為1.93°，1.31° 和1.93°，平均誤差分別為0.72°，0.54°和0.71°；筆者提出方法的姿態(tài)角最大誤差0.72°，0.62°和0.67°，平均誤差分別為0.38°，0.25°和0.30°。由此可知，筆者所提方法的姿態(tài)角解算平均誤差均小于0.5°，姿態(tài)漂移誤差較小，滿足防沖鉆孔機(jī)器人姿態(tài)監(jiān)測精度要求。

5 結(jié) 論

(1)為了精確測量防沖鉆孔機(jī)器人的加速度和角速度參數(shù)，減少防沖鉆孔機(jī)器人定位、定姿誤差的累積，筆者提出了一種基于空間陣列式慣性單元的防沖鉆孔機(jī)器人位姿解算方法，并進(jìn)行了曲線運(yùn)動(dòng)工況下的仿真分析。結(jié)果表明：筆者提出的空間陣列式位姿解算方法在位移累積誤差和姿態(tài)角平均絕對(duì)誤差方面均優(yōu)于原始位姿解算方法和差分位姿解算方法。

圖10 姿態(tài)監(jiān)測結(jié)果Fig.10 Attitude monitoring results

(2)搭建了移動(dòng)載體運(yùn)動(dòng)工況模擬實(shí)驗(yàn)臺(tái)，進(jìn)行了曲線運(yùn)動(dòng)工況下防沖鉆孔機(jī)器人模擬運(yùn)動(dòng)實(shí)驗(yàn)。實(shí)驗(yàn)結(jié)果表明：空間陣列式慣性單元解算的俯仰角、航偏角和橫滾角的平均誤差分別為0.07°，0.15°和0.06°；在軸、軸和軸方向的位移平均誤差分別為5.83，8.76和0.84 cm，驗(yàn)證了筆者所提方法的適用性和可行性。

(3)搭建了防沖鉆孔機(jī)器人慣性單元位姿監(jiān)測實(shí)驗(yàn)平臺(tái)，進(jìn)行了防沖鉆孔機(jī)器人直線行走實(shí)驗(yàn)，實(shí)驗(yàn)結(jié)果表明：所提方法解算的防沖鉆孔機(jī)器人俯仰角、航偏角和橫滾角的平均誤差分別為0.25°，0.38°和0.30°；在軸、軸和軸方向的位移平均積差分別為1.98，1.72和1.57 cm。上述位姿解算誤差均能滿足防沖鉆孔機(jī)器人在巷道內(nèi)運(yùn)動(dòng)時(shí)定位和定姿精度的實(shí)際要求，驗(yàn)證了防沖鉆孔機(jī)器人位姿解算方法的可行性和有效性。

(4)通過運(yùn)動(dòng)工況仿真和實(shí)驗(yàn)數(shù)據(jù)分析可以看出，由于非確定性誤差的方向和大小均未知，本文提出的空間陣列式位姿解算方法可以有效抑制累積誤差的增長速度，但難以將其完全消除。因此位姿解算誤差始終在積累，無法收斂，但本文提出算法的誤差累積速度遠(yuǎn)遠(yuǎn)小于其他算法。在后續(xù)研究中，筆者將通過機(jī)器視覺定時(shí)對(duì)位姿解算誤差進(jìn)行補(bǔ)償，并利用機(jī)器學(xué)習(xí)算法對(duì)誤差進(jìn)行學(xué)習(xí)，進(jìn)一步減小累積誤差的產(chǎn)生。

[1] 齊慶新,李一哲,趙善坤,等. 我國煤礦沖擊地壓發(fā)展70年:理論與技術(shù)體系的建立與思考[J]. 煤炭科學(xué)技術(shù),2019,47(9):1-40.

QI Qingxin,LI Yizhe,ZHAO Shankun,et al. Seventy years development of coal mine rockburst in China:Establishment and consideration of theory and technology system[J]. Coal Science and Technology,2019,47(9):1-40.

[2] 陳巖峰,李學(xué)軍,吳宏斌,等. 沖擊地壓機(jī)理及防控現(xiàn)狀與發(fā)展態(tài)勢分析[J]. 煤炭科技,2021,42(5):70-75.

CHEN Yanfeng,LI Xuejun,WU Hongbin,et al. Control status and development trend of rockburst mechanism and prevention in China[J]. Coal Science and Technology Magazine,2021,42(5):70-75.

[3] 王國法,龐義輝,任懷偉. 煤礦智能化開采模式與技術(shù)路徑[J]. 采礦與巖層控制工程學(xué)報(bào),2020,2(1):013501.

WANG Guofa,PANG Yihui,REN Huaiwei. Intelligent coal mining pattern and technological path[J]. Journal of Mining and Strata Control Engineering,2020,2(1):013501.

[4] JOBES Christopher C. Mechanical sensor guidance of a mining machine[J]. IEEE Transactions on Industry Applications,1993,29(4):755-761.

[5] MAAREF H,BARRET C. Sensor-based navigation of a mobile robot in an indoor environment[J]. Robotics and Autonomous Systems,2002,38:11-18.

[6] LEDERGERBER Anton,HAMER Michael,D′ANDREA Raffaello. A robot self-localization system using one-wayultra-wideband communication[A]. 2015 IEEE/RSJ International Conference on Intelligent Robots and Systems [C]. Piscataway:IEEE,2015:3131-3137.

[7] 馮佳萌,裴東,鄒勇,等. 基于機(jī)器人激光定位的一種改進(jìn)AMCL算法[J]. 激光與光電子學(xué)進(jìn)展,2021,58(20):479-487.

FENG Jiameng,PEI Dong,ZOU Yong,et al. An improved AMCL algorithm based on robot laser localization[J]. Laser & Optoelectronics Progress,2021,58(20):479-487.

[8] 徐慶坤,王天皓,宋中越. UWB與里程計(jì)融合的室內(nèi)移動(dòng)機(jī)器人定位設(shè)計(jì)[J]. 機(jī)械設(shè)計(jì)與制造,2021(8):295-299,304.

XU Qingkun,WANG Tianhao,SONG Zhongyue. Design of indoor mobile robot location based on uwb and odometer data fusion[J]. Machinery Design & Manufacture,2021(8):295-299,304.

[9] 朱道俊,徐湛楠,馬婷婷,等. 基于視覺和輪速計(jì)緊耦合的輪式機(jī)器人定位算法研究[J]. 傳感技術(shù)學(xué)報(bào),2021,34(7):896-903.

ZHU Daojun,XU Zhannan,MA Tingting,et al. Tightly-coupled vision and wheel odometer localization for ground vehicle[J]. Chinese Journal of Sensors and Actuators,2021,34(7):896-903.

[10] 尚磊,王杰,宋尊師,等. 基于單目ORB-SLAM2算法的煤礦搜救機(jī)器人定位研究[J]. 機(jī)床與液壓,2020,48(11):49-52.

SHANG Lei,WANG Jie,SONG Shizun,et al. Research on the localization of the coal mine search and rescue robot based on the monocular ORB-SLAM2 algorithm[J]. Machine Tool & Hydraulics,2020,48(11):49-52.

[11] 蔡李花,方海峰,李允旺,等. 礦井救災(zāi)機(jī)器人自主定位方法研究[J]. 工礦自動(dòng)化,2015,41(7):62-67.

CAI Lihua,FANG Haifeng,LI Yunwang,et al. Research of self-locallization method of mine rescue robot[J]. Industry and Mine Automation,2015,41(7):62-67.

[12] 趙靖. 基于卡爾曼濾波算法的采煤機(jī)慣導(dǎo)定位方法[J]. 工礦自動(dòng)化,2014,40(10):29-32.

ZHAO Jing. Inertial navigation positioning method of shearer based on Kalman filtering algorithm[J]. Industry and Mine Automation,2014,40(10):29-32.

[13] 郭曉晶,田華,李明君. 基于卡爾曼濾波算法的采煤機(jī)慣導(dǎo)定位方法[J]. 煤炭技術(shù),2015,34(7):278-280.

GUO Xiaojing,TIAN Hua,LI Mingjun. INS positioning method of shearer based on kalman filter[J]. Coal Technology,2015,34(7):278-280.

[14] 樊啟高,李威,王禹橋,等. 一種采用捷聯(lián)慣導(dǎo)的采煤機(jī)動(dòng)態(tài)定位方法[J]. 煤炭學(xué)報(bào),2011,36(10):1758-1761.

FAN Qigao,LI Wei,WANG Yuqiao. A shearer dynamic positioning method using strap down inertial navigation[J]. Journal of China Coal Society,2011,36(10):1758-1761.

[15] 楊海,李威,羅成名,等. 基于捷聯(lián)慣導(dǎo)的采煤機(jī)定位定姿技術(shù)實(shí)驗(yàn)研究[J]. 煤炭學(xué)報(bào),2014,39(12):2550-2556.

YANG Hai,LI Wei,LUO Chengming. Experimental study on position and attitude technique for shearer using SINS measurement[J]. Journal of China Coal Society,2014,39(12):2550-2556.

[16] 張羽飛,馬宏偉,毛清華,等. 視覺與慣導(dǎo)融合的煤礦移動(dòng)機(jī)器人定位方法[J]. 工礦自動(dòng)化,2021,47(3):46-52.

ZHANG Yufei,MA Hongwei,MAO Qinghua,et al. Coal mine mobile robot positioning method based on fusion of vision and inertial navigation[J]. Industry and Mine Automation,2021,47(3):46-52.

[17] 溫良,葉錦嬌,張立亞. 煤礦救災(zāi)機(jī)器人超聲波和慣性導(dǎo)航技術(shù)[J]. 煤礦安全,2017,515(6):127-130.

WEN Liang,YE Jinjiao,ZHANG Liya. Ultrasonic and inertial navigation technology of coal mine rescue robot[J]. Safety in Coal Mines,2017,515(6):127-130.

[18] 張璞. 煤礦井下移動(dòng)機(jī)器人自主定位方法研究[D]. 西安:西安科技大學(xué),2019:35-44.

ZHANG Pu. Research on autonmomous positioning method of coal mine mobile robot[D]. Xi’an:Xi’an University of Science and Technology,2019:35-44.

[19] 李猛鋼. 面向井下鉆孔機(jī)器人應(yīng)用的精確定位與地圖構(gòu)建技術(shù)研究[D]. 徐州:中國礦業(yè)大學(xué),2020:148-167.

LI Menggang. Research on Technologies of accurate localization and mapping for underground drilling robot application[D]. Xuzhou:China University of Mining and Technology,2020:148-167.

[20] 郭衛(wèi),張露,趙栓峰. 基于捷聯(lián)慣導(dǎo)的采煤機(jī)姿態(tài)解算算法研究[J]. 礦山機(jī)械,2014(6):15-20.

GUO Wei,ZHANG Lu,ZHAO Shuanfeng. Study on shearer posture algorithm based on strapdown inertial navigation[J]. Mining & Processing Equipment,2014(6):15-20.

[21] 司壘,王忠賓,譚超,等. 基于差分式慣性傳感組件的采煤機(jī)位姿解算法[J]. 振動(dòng). 測試與診斷,2021,41(2):220-227,406.

SI Lei,WANG Zhongbin,TAN Chao,et al. Research on differential calculation method of shearer position and attitude[J]. Journal of Vibration,Measurement & Diagnosis,2021,41(2):220-227,406.