一種煤礦巷道機(jī)器人定位與環(huán)境建模方法
【技術(shù)領(lǐng)域】
[0001] 本發(fā)明屬于一種智能移動機(jī)器人導(dǎo)航控制方法，具體涉及一種適用于地面移動機(jī) 器人在煤礦地下巷道環(huán)境中自主定位和環(huán)境建模的方法。
【背景技術(shù)】
[0002] 智能移動機(jī)器人是一類能夠通過傳感器感知環(huán)境和自身狀態(tài)，實現(xiàn)在有障礙物的 環(huán)境中面向目標(biāo)的自主導(dǎo)航運(yùn)動，從而完成預(yù)定任務(wù)的機(jī)器人系統(tǒng)。要實現(xiàn)機(jī)器人自主導(dǎo) 航運(yùn)動，必須要解決軌跡規(guī)劃、運(yùn)動控制、環(huán)境建模、實時定位等一系列問題。
[0003] 煤礦巷道的三維模型，可以立體、準(zhǔn)確地反映井下巷道及其空間關(guān)系，對于指導(dǎo)現(xiàn) 場生產(chǎn)、培訓(xùn)礦工安全生產(chǎn)、實施井下救援有著積極意義，是礦山數(shù)字化建設(shè)的重要內(nèi)容之 一，為實現(xiàn)礦山井下環(huán)境的可視化虛擬再現(xiàn)奠定了基礎(chǔ)。
[0004] 煤礦地下巷道環(huán)境復(fù)雜，尤其在當(dāng)煤礦發(fā)生坍塌、瓦斯爆炸等事故時，井下條件十 分惡劣，給測量工作帶來很大困難，甚至危及測量人員的安全。將智能機(jī)器人技術(shù)應(yīng)用于煤 礦巷道探測，將為煤礦井下測繪提供一種安全、快速、有效的解決途徑。

【發(fā)明內(nèi)容】

[0005] 本發(fā)明的目的是提供一種煤礦巷道機(jī)器人定位與環(huán)境建模方法，它能夠用于煤礦 地下巷道探測的機(jī)器人定位、環(huán)境建模算法。同步定位與建圖算法模塊，通過融合多種傳感 器信息，在精確定位的同時，建立井下巷道的3D環(huán)境模型。
[0006] 本發(fā)明是這樣實現(xiàn)的，一種煤礦巷道機(jī)器人定位與環(huán)境建模方法，它包括如下步 驟，
[0007] 1)機(jī)器人組合定位算法；
[0008] 2)基于環(huán)境信息的定位修正；
[0009] 3)機(jī)器人3D環(huán)境建模方法。
[0010] 所述的步驟1)為在煤礦井下環(huán)境中，選用基于Kalman濾波的中精度慣導(dǎo)系統(tǒng)+ 里程儀的組合導(dǎo)航定位方式，中等精度慣導(dǎo)系統(tǒng)具有較強(qiáng)的姿態(tài)保持能力，但定位誤差會 隨時間不斷累積；而里程儀的測量誤差一般隨里程增加，兩者具有很強(qiáng)的互補(bǔ)性，通過組合 可以構(gòu)成高精度的自主導(dǎo)航系統(tǒng)，
[0011] 慣導(dǎo)/里程儀組合導(dǎo)航系統(tǒng)的狀態(tài)方程為：xk = Akxk i+wk ρ
[0012] 其中：
[0013] 狀態(tài)變量~=[?α], +'.為加速度計隨機(jī)常值零偏誤差，△ vk、△ Sk分 別為慣性導(dǎo)航解算的速度誤差和位置誤差，為里程儀刻度系數(shù)誤差；
[0015] 系統(tǒng)噪聲Wk為零均值白噪聲，噪聲方差陣為Qk，即E[w k] = 0、
[0016] 慣導(dǎo)/里程儀組合導(dǎo)航系統(tǒng)的測量方程為：zk = Hkxk+ ξ k， _7] 其中：觀測量Zk為從tk i時刻到tk時刻，慣導(dǎo)解算的位移增量％ -%.:?與里程儀測 量的位移增量$之差，
在剔除打滑和滑行 等隨機(jī)誤差后，里程儀測量值&與真實值dk之間的關(guān)系為元=
[0019] 測量噪聲ξ k為零均值白噪聲，測量噪聲方差陣為Rk，即Ε[ ξ k] = 0、
[0020] 所述的步驟2)里程儀的誤差主要包括刻度系數(shù)誤差，以及由于打滑或滑行造成 的隨機(jī)誤差，設(shè)機(jī)器人導(dǎo)航的全局坐標(biāo)系為0ΧΥΖ為東北天坐標(biāo)系，坐標(biāo)原點(diǎn)為機(jī)器人導(dǎo)航 的起始點(diǎn)，機(jī)器人相對坐標(biāo)系OrXrYr定義為以機(jī)器人為原點(diǎn)，以機(jī)器人前進(jìn)方向為Xr軸， 垂直于Xr軸逆時針90°方向為Yr軸。在全局坐標(biāo)系內(nèi)，機(jī)器人航向角Ψ定義為機(jī)器人 Xr軸相對于X軸的夾角，偏北為正；俯仰角Θ定義為Xr軸在0YZ平面內(nèi)的投影與Y軸夾 角，向上為正，在機(jī)器人相對坐標(biāo)系內(nèi)，目標(biāo)航向角ct定義為，目標(biāo)與機(jī)器人連線相對于Xr 軸的夾角，逆時針方向為正，
[0021] 選用激光雷達(dá)探測的環(huán)境信息對里程儀誤差進(jìn)行修正，規(guī)定定位修正時激光雷達(dá) 的俯仰角度為〇,算法步驟如下：
[0022] (1)根據(jù)里程儀提供的位移增量i和慣導(dǎo)系統(tǒng)提供的航向角Vk、俯仰角0 k， 從k-1時刻機(jī)器人在全局地圖中的位置（xk d yk u zk ^，推算機(jī)器人在k時刻的預(yù)測位置 (之 ?.);
[0023]
[0024] (2)根據(jù)已知的環(huán)境特征在全局地圖中的位置（Xc]，yc]，zj，以及機(jī)器人k時刻的預(yù) 測位置（X' k，/ k，W k)，計算環(huán)境特征在k時刻相對機(jī)器人的距離I:
[0026] (3)k時刻激光雷達(dá)對周圍環(huán)境進(jìn)行掃描，從掃描點(diǎn)中提取環(huán)境特征，得到當(dāng)前關(guān) 注的環(huán)境特征在機(jī)器人坐標(biāo)系中的實測距離P。和角度ct。；
[0027] (4)剔除異常數(shù)據(jù)：對比I與P ^，當(dāng)VP ^大于設(shè)定的打滑閾值Μ時，認(rèn)為機(jī)器 人車輪處于打滑狀態(tài)；當(dāng)l-Ρ。小于設(shè)定的滑行閾值Ν時，認(rèn)為機(jī)器人車輪處于滑行狀態(tài)； 上述異常數(shù)據(jù)即不參與導(dǎo)航解算也不參與誤差修正；
[0028] (5)根據(jù)由激光雷達(dá)獲得的特征點(diǎn)在機(jī)器人坐標(biāo)系的坐標(biāo)（Ρ。，α。)，以及環(huán)境特 征在全局地圖中的位置（X。，y。，zj，計算k時刻機(jī)器人的位置（x k，yk，zk)，及機(jī)器人在k-Ι時 刻到k時刻的實際里程d k :
[0031] (6)計算里程儀刻度系數(shù)誤差Λ Kk，在組合導(dǎo)航算法中用Λ Kk對里程儀測量值進(jìn) 行修正：
[0033] 所述的步驟3)為實現(xiàn)對煤礦巷道的3D環(huán)境建模，選用二維激光雷達(dá)與高精度電 控轉(zhuǎn)臺組成環(huán)境探測系統(tǒng)，二維激光雷達(dá)在電控轉(zhuǎn)臺的驅(qū)動下可以繞Yr軸轉(zhuǎn)動，激光雷達(dá) 的俯仰角β定義為向上為正，向下為負(fù)，
[0034] 采用二維笛卡爾矩形柵格及高度圖的方式來表示雷達(dá)探測到的環(huán)境，用一個二維 數(shù)組錄該環(huán)境地圖：
[0035]
[0036] 根據(jù)k時刻機(jī)器人的位置（xk, yk, zk)、航向角￥k、俯仰角Θ k，以及激光雷達(dá)俯仰 角和激光雷達(dá)探測到的障礙物信息（P。，aj，可以計算障礙物在全局坐標(biāo)系中的坐標(biāo) (χ〇, y〇, z〇)：
[0038] 設(shè)柵格的大小為wXw，則障礙物占據(jù)的柵格的二維坐標(biāo)（xgi。，ygi。）為：

[0041] 本發(fā)明的優(yōu)點(diǎn)是，通過煤礦巷道測量移動機(jī)器人能夠自動生成巷道的三維模型， 可以立體、直觀、準(zhǔn)確地表現(xiàn)并反映井下巷道及其空間關(guān)系，對于指導(dǎo)現(xiàn)場生產(chǎn)、培訓(xùn)礦工 安全生產(chǎn)有著積極意義。
【附圖說明】
[0042] 圖1為控制系統(tǒng)原理圖；
[0043] 圖2為信息處理算法原理圖。
【具體實施方式】
[0044] 下面結(jié)合附圖和實例對本發(fā)明進(jìn)行詳細(xì)介紹：
[0045] 煤礦巷道探測機(jī)器人控制原理如圖1所示。內(nèi)部傳感器包括里程儀和慣性導(dǎo)航系 統(tǒng)，用于測量機(jī)器人的位移和姿態(tài)。外部傳感器包括激光雷達(dá)、攝像機(jī)、超聲測距傳感器、紅 外測距傳感器，其中激光雷達(dá)、攝像機(jī)用于直接感知環(huán)境信息，超聲、紅外測距傳感器用于 緊急避障。車載計算機(jī)用于采集各傳感器信息，并對信息進(jìn)行處理，進(jìn)行決策，并向驅(qū)動單 元發(fā)送控制指令。
[0046] 信息采集處理模塊的算法原理如圖2所示。內(nèi)部傳感器里程儀和慣導(dǎo)系統(tǒng)通過組 合導(dǎo)航算法得到機(jī)器人當(dāng)前位置和航向；激光雷達(dá)數(shù)據(jù)經(jīng)濾波處理，攝像機(jī)信息經(jīng)圖像處 理后分別提取環(huán)境特征，兩類傳感器數(shù)據(jù)的融合采取特征級數(shù)據(jù)融合算法；機(jī)器人位姿和 環(huán)境特征經(jīng)SLAM算法，更新全局地圖；并根據(jù)全局地圖、當(dāng)前環(huán)境特征對里程儀模型參數(shù) 進(jìn)行修正，提高定位精度。
[0047] -種煤礦巷道機(jī)器人定位與環(huán)境建模方法，它包括如下步驟：
[0048] 1.機(jī)器人組合定位算法
[0049] 在煤礦井下環(huán)境中，機(jī)器人無法接收GPS信息，因此機(jī)器人必須具備自主定位功 能。而且為了確保環(huán)境建圖的準(zhǔn)確性，對機(jī)器人長航時的定位精度提出了很高的要求。為 此選用基于Kalman濾波的中精度慣導(dǎo)系統(tǒng)+里程儀的組合導(dǎo)航定位方式。中等精度慣導(dǎo) 系統(tǒng)具有較強(qiáng)的姿態(tài)保持能力，但定位誤差會隨時間不斷累積；而里程儀的測量誤差一般 隨里程增加。兩者具有很強(qiáng)的互補(bǔ)性，通過組合可以構(gòu)成高精度的自主導(dǎo)航系統(tǒng)。
[0050] 慣導(dǎo)/里程儀組合導(dǎo)航系統(tǒng)的狀態(tài)方程為：xk = Akxk JWk ρ
[0051] 其中：
[0052]
為加速度計隨機(jī)常值零偏誤差，Δ Vk、Δ Sk分 別為慣性導(dǎo)航解算的速度誤差和位置誤差，為里程儀刻度系數(shù)誤差；
[0054] 系統(tǒng)噪聲Tk為零均值白噪聲，噪聲方差陣為Qk，即E[wk] = 0、
[0055] 慣導(dǎo)/里程儀組合導(dǎo)航系統(tǒng)的測量方程為：zk = Hkxk+ ξ k。
[0056] 其中：
[0057] 觀測量zk為從tk