權(quán)利要求書： 1.一種帶式輸送機(jī)巡檢機(jī)器人系統(tǒng)高精度定位方法，其特征在于，所述定位方法包括以下步驟：

S1，對(duì)編碼器增量進(jìn)行累加計(jì)算，得到機(jī)器人沿軌道運(yùn)動(dòng)距離；

S2，結(jié)合步驟S1獲得機(jī)器人沿軌道運(yùn)動(dòng)距離，采用NFC讀到的IC卡位置信息對(duì)機(jī)器人連續(xù)位置進(jìn)行融合修正，得到機(jī)器人連續(xù)位置的融合修正公式為：



IC為NFC讀到的IC卡位置信息；c表示NFC讀到對(duì)應(yīng)IC卡時(shí)，編碼器開(kāi)始計(jì)數(shù)的采樣點(diǎn)對(duì)應(yīng)的位置；編碼器累加系數(shù)k是機(jī)器人運(yùn)行實(shí)際距離與對(duì)應(yīng)編碼器累加距離之比，與摩擦輪直徑成正比；i為機(jī)器人從軌道位置a到軌道位置b對(duì)應(yīng)的編碼器的第i個(gè)采樣周期，取值由編碼器采樣頻率決定；Δli表示編碼器在第i個(gè)采樣周期中得到增量；

S3，利用NFC讀到的IC卡間數(shù)據(jù)，和編碼器累加得到的對(duì)應(yīng)的IC卡間數(shù)據(jù)，對(duì)步驟S2中編碼器累加系數(shù)k的取值進(jìn)行分段、分方向的預(yù)測(cè)修正，獲得表示機(jī)器人從ICj卡正向運(yùn)動(dòng)到ICj+1卡測(cè)得的k值



和表示機(jī)器人從ICj卡反向運(yùn)動(dòng)到ICj-1卡測(cè)得的k值/>



j表示IC卡在軌道上安裝的序列號(hào)，ICj是NFC讀到第j個(gè)IC卡的編碼器的采樣點(diǎn)位置；判斷修正后的編碼器累加系數(shù)k的取值有效性；

S4，對(duì)步驟S3修正后的編碼器累加系數(shù)k的取值進(jìn)行卡爾曼濾波，減小修正后的編碼器累加系數(shù)k的隨機(jī)誤差，獲得最優(yōu)估計(jì)修正k值；

S5，依據(jù)步驟S4獲得最優(yōu)估計(jì)修正k值和步驟S2獲得的融合修正位置公式，根據(jù)軌道與定位模塊相對(duì)位置模型計(jì)算機(jī)器人所在軌道的高精度位置；

步驟S3中，利用NFC讀到的IC卡間數(shù)據(jù)，和編碼器累加得到的對(duì)應(yīng)的IC卡間數(shù)據(jù)，對(duì)步驟S2中編碼器累加系數(shù)k的取值進(jìn)行分段、分方向的預(yù)測(cè)修正的過(guò)程包括以下步驟：

利用NFC讀到的IC卡間數(shù)據(jù)，和編碼器累加得到的對(duì)應(yīng)的IC卡間數(shù)據(jù)，對(duì)k值進(jìn)行分段、分方向的預(yù)測(cè)修正，則修正k值為：



其中，j表示IC卡在軌道上安裝的序列號(hào)，



表示機(jī)器人從ICj卡正向運(yùn)動(dòng)到ICj+1卡測(cè)得的k值，/>



表示機(jī)器人從ICj卡反向運(yùn)動(dòng)到ICj-1卡測(cè)得的k值；

步驟S4中，對(duì)步驟S3修正后的編碼器累加系數(shù)k的取值進(jìn)行卡爾曼濾波，減小修正后的編碼器累加系數(shù)k的隨機(jī)誤差，獲得最優(yōu)估計(jì)修正k值的過(guò)程包括以下步驟：

S41，令zjt＝kjt，kjt表示在第t次修正的kj值，則系統(tǒng)模型為：



其中，xjt，表示狀態(tài)真實(shí)值，zjt表示觀測(cè)值，vjt表示測(cè)量噪聲，且滿足p(vj)∈N(0,R)；

S42，預(yù)測(cè)方程為：



其中，



為狀態(tài)預(yù)測(cè)值，/>



為最優(yōu)估計(jì)值，/>



狀態(tài)預(yù)測(cè)協(xié)方差，pjt為狀態(tài)估計(jì)協(xié)方差；

S43，更新方程為：



其中，Kjt為卡爾曼增益，且Kjt∈[0,1]，第t次修正的最優(yōu)估計(jì)值為



依據(jù)下述高精度融合修正位置公式計(jì)算機(jī)器人所在軌道的高精度位置：



式中，



表示第t次修正的/>



值，/>



表示第t次修正的/>



值。

2.根據(jù)權(quán)利要求1所述的帶式輸送機(jī)巡檢機(jī)器人系統(tǒng)高精度定位方法，其特征在于，步驟S1中，對(duì)編碼器增量進(jìn)行累加計(jì)算，得到機(jī)器人沿軌道運(yùn)動(dòng)距離的過(guò)程包括以下步驟：

S11，假設(shè)編碼器采樣周期內(nèi)最大旋轉(zhuǎn)角度小于半圈，編碼器在每個(gè)采樣周期中的旋轉(zhuǎn)增量為：



其中，n表示編碼器脈沖，y表示編碼器原始數(shù)據(jù)，i表示編碼器的采樣序號(hào)，由編碼器采樣頻率決定；

S12，機(jī)器人沿軌道運(yùn)動(dòng)的距離表示為：



3.根據(jù)權(quán)利要求1所述的帶式輸送機(jī)巡檢機(jī)器人系統(tǒng)高精度定位方法，其特征在于，步驟S3中，判斷有效性的過(guò)程包括以下步驟：

采用e表示有效性誤差，當(dāng)



時(shí)，判斷修正kj值不存在粗大誤差，認(rèn)為kj有效，更新kj值；當(dāng)/>



時(shí)，判斷修正kj值存在粗大誤差，認(rèn)為kj無(wú)效，不更新kj值。

說(shuō)明書： 一種帶式輸送機(jī)巡檢機(jī)器人系統(tǒng)高精度定位方法技術(shù)領(lǐng)域

本發(fā)明涉及煤礦安全生產(chǎn)巡檢技術(shù)領(lǐng)域以及機(jī)器人融合定位技術(shù)領(lǐng)域，具體而言涉及一種帶式輸送機(jī)巡檢機(jī)器人系統(tǒng)高精度定位方法。

背景技術(shù)

機(jī)器人一般選用里程計(jì)進(jìn)行軌跡位置解算，實(shí)現(xiàn)機(jī)器人軌跡位置反饋。而牽引式軌道機(jī)器人將機(jī)器人本體與驅(qū)動(dòng)機(jī)構(gòu)進(jìn)行了分離，之間通過(guò)繩索牽引的方式實(shí)現(xiàn)間接驅(qū)動(dòng)，這種驅(qū)動(dòng)方式的機(jī)器人位置信息采集方案有間接測(cè)量和直接測(cè)量?jī)煞N。

間接測(cè)量是通過(guò)采集驅(qū)動(dòng)電機(jī)旋轉(zhuǎn)的圈數(shù)，間接計(jì)算機(jī)器人位置，該種方案的優(yōu)點(diǎn)是簡(jiǎn)單易部署，但缺點(diǎn)是牽引繩索與電機(jī)的驅(qū)動(dòng)輪之間存在打滑現(xiàn)象，更重要的是牽引繩索存在彈性，應(yīng)用于煤礦中帶式輸送機(jī)巡檢機(jī)器人牽引繩索往往很長(zhǎng)，其軸向彈性形變不可忽略，因此，間接測(cè)量誤差很大。

直接測(cè)量是通過(guò)在機(jī)器人本體上安裝的編碼器與軌道相對(duì)運(yùn)動(dòng)，實(shí)現(xiàn)直接測(cè)量，需要機(jī)器人本體與運(yùn)動(dòng)單元間進(jìn)行無(wú)線通信，傳遞傳感器測(cè)量數(shù)據(jù)，部署難度大。但這種編碼器測(cè)量數(shù)據(jù)的方式與牽引繩索的打滑和彈性形變無(wú)關(guān)，可以比較準(zhǔn)確測(cè)量出機(jī)器人位置信息，適用于較高精度定位的閉環(huán)控制。

帶式輸送機(jī)巡檢機(jī)器人主要部署在煤礦上，用于帶式輸送機(jī)的安全生產(chǎn)巡檢。煤礦環(huán)境惡劣，空氣中攜帶的煤粉比較多，容易落在軌道上，從而導(dǎo)致與編碼器連接的摩擦輪打滑，以及摩擦輪外圈包膠長(zhǎng)時(shí)間運(yùn)行磨損會(huì)導(dǎo)致編碼器的定位不準(zhǔn)確。因此，現(xiàn)有的軌道機(jī)器人的位置測(cè)量存在不準(zhǔn)確、不穩(wěn)定等問(wèn)題，使得軌道式器機(jī)人的高精度定位控制比較困難。

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

本發(fā)明針對(duì)現(xiàn)有技術(shù)中的不足，提供一種帶式輸送機(jī)巡檢機(jī)器人系統(tǒng)高精度定位方法，為軌道機(jī)器人故障定位、自動(dòng)充電等需要高精度定位功能的實(shí)現(xiàn)，提供了高精度位置反饋條件。

為實(shí)現(xiàn)上述目的，本發(fā)明采用以下技術(shù)方案：

本發(fā)明實(shí)施例提出了一種帶式輸送機(jī)巡檢機(jī)器人系統(tǒng)高精度定位方法，所述定位方法包括以下步驟：

S1，對(duì)編碼器增量進(jìn)行累加計(jì)算，得到機(jī)器人沿軌道運(yùn)動(dòng)距離；

S2，結(jié)合步驟S1獲得機(jī)器人沿軌道運(yùn)動(dòng)距離，采用NFC讀到的IC卡位置信息對(duì)機(jī)器人連續(xù)位置進(jìn)行融合修正，得到機(jī)器人連續(xù)位置的融合修正公式為：



IC為NFC讀到的IC卡位置信息；c表示NFC讀到對(duì)應(yīng)IC卡時(shí)，編碼器開(kāi)始計(jì)數(shù)的采樣點(diǎn)對(duì)應(yīng)的位置；編碼器累加系數(shù)k是機(jī)器人運(yùn)行實(shí)際距離與對(duì)應(yīng)編碼器累加距離之比，與摩擦輪直徑成正比；i為機(jī)器人從軌道位置a到軌道位置b對(duì)應(yīng)的編碼器的第i個(gè)采樣周期，取值由編碼器采樣頻率決定；Δli表示編碼器在第i個(gè)采樣周期中得到增量；

S3，利用NFC讀到的IC卡間數(shù)據(jù)，和編碼器累加得到的對(duì)應(yīng)的IC卡間數(shù)據(jù)，對(duì)步驟S2中編碼器累加系數(shù)k的取值進(jìn)行分段、分方向的預(yù)測(cè)修正，獲得表示機(jī)器人從ICj卡正向運(yùn)動(dòng)到ICj+1卡測(cè)得的k值kj+和表示機(jī)器人從ICj卡反向運(yùn)動(dòng)到ICj-1卡測(cè)得的k值kj-，j表示IC卡在軌道上安裝的序列號(hào)，ICj是NFC讀到第j個(gè)IC卡的編碼器的采樣點(diǎn)位置；判斷修正后的編碼器累加系數(shù)k的取值有效性；

S4，對(duì)步驟S3修正后的編碼器累加系數(shù)k的取值進(jìn)行卡爾曼濾波，減小修正后的編碼器累加系數(shù)k的隨機(jī)誤差，獲得最優(yōu)估計(jì)修正k值；

S5，依據(jù)步驟S4獲得最優(yōu)估計(jì)修正k值和步驟S2獲得的融合修正位置公式，根據(jù)軌道與定位模塊相對(duì)位置模型計(jì)算機(jī)器人所在軌道的高精度位置。

進(jìn)一步地，步驟S1中，對(duì)編碼器增量進(jìn)行累加計(jì)算，得到機(jī)器人沿軌道運(yùn)動(dòng)距離的過(guò)程包括以下步驟：

S11，假設(shè)編碼器采樣周期內(nèi)最大旋轉(zhuǎn)角度小于半圈，編碼器在每個(gè)采樣周期中的旋轉(zhuǎn)增量為：



其中，n表示編碼器脈沖，y表示編碼器原始數(shù)據(jù)，i表示編碼器的采樣序號(hào)，由編碼器采樣頻率決定；

S12，機(jī)器人沿軌道運(yùn)動(dòng)的距離表示為：



進(jìn)一步地，步驟S3中，利用NFC讀到的IC卡間數(shù)據(jù)，和編碼器累加得到的對(duì)應(yīng)的IC卡間數(shù)據(jù)，對(duì)步驟S2中編碼器累加系數(shù)k的取值進(jìn)行分段、分方向的預(yù)測(cè)修正的過(guò)程包括以下步驟：

利用NFC讀到的IC卡間數(shù)據(jù)，和編碼器累加得到的對(duì)應(yīng)的IC卡間數(shù)據(jù)，對(duì)k值進(jìn)行分段、分方向的預(yù)測(cè)修正，則修正k值為：



其中，j表示IC卡在軌道上安裝的序列號(hào)，



表示機(jī)器人從ICj卡正向運(yùn)動(dòng)到ICj+1卡測(cè)得的k值，/>



表示機(jī)器人從ICj卡反向運(yùn)動(dòng)到ICj-1卡測(cè)得的k值。

進(jìn)一步地，步驟S3中，判斷有效性的過(guò)程包括以下步驟：

采用e表示有效性誤差，當(dāng)



時(shí)，判斷修正kj值不存在粗大誤差，認(rèn)為kj有效，更新kj值；當(dāng)/>



時(shí)，判斷修正kj值存在粗大誤差，認(rèn)為kj無(wú)效，不更新kj值。

進(jìn)一步地，步驟S4中，對(duì)步驟S3修正后的編碼器累加系數(shù)k的取值進(jìn)行卡爾曼濾波，減小修正后的編碼器累加系數(shù)k的隨機(jī)誤差，獲得最優(yōu)估計(jì)修正k值的過(guò)程包括以下步驟：

S41，令zjt＝kjt，kjt表示在第t次修正的kj值，則系統(tǒng)模型為：



其中，xjt，表示狀態(tài)真實(shí)值，zjt表示觀測(cè)值，vjt表示測(cè)量噪聲，且滿足p(vj)∈N(0,R)；

S42，預(yù)測(cè)方程為：



其中，



為狀態(tài)預(yù)測(cè)值，/>



為最優(yōu)估計(jì)值，/>



狀態(tài)預(yù)測(cè)協(xié)方差，pjt為狀態(tài)估計(jì)協(xié)方差；

S43，更新方程為：



其中，Kjt為卡爾曼增益，且Kjt∈[0,1]，第t次修正的最優(yōu)估計(jì)值為



進(jìn)一步地，依據(jù)下述高精度融合修正位置公式計(jì)算機(jī)器人所在軌道的高精度位置：



式中，



表示第t次修正的/>



值，/>



表示第t次修正的/>



值。

本發(fā)明的有益效果是：

本發(fā)明提出的帶式輸送機(jī)巡檢機(jī)器人系統(tǒng)高精度定位方法，在軌道機(jī)器人運(yùn)行過(guò)程中不需要工作人員測(cè)量修正參數(shù)，利用兩個(gè)自身定位傳感器測(cè)得的數(shù)據(jù)，通過(guò)傳感器融合相互修正的思想，實(shí)現(xiàn)帶式輸送機(jī)巡檢機(jī)器人系統(tǒng)的高精度定位。

附圖說(shuō)明

圖1為本發(fā)明實(shí)施例的帶式輸送機(jī)巡檢機(jī)器人系統(tǒng)結(jié)構(gòu)示意圖。

圖2為本發(fā)明實(shí)施例的兩個(gè)定位傳感器安裝示意圖。

圖3為本發(fā)明實(shí)施例的軌道與定位模塊相對(duì)位置模型示意圖。

圖4為本發(fā)明實(shí)施例的修正k值與最優(yōu)估計(jì)值曲線圖。

圖5為本發(fā)明實(shí)施例的帶式輸送機(jī)巡檢機(jī)器人系統(tǒng)高精度定位方法流程圖。

圖中序號(hào)為：1、工字鋼軌道；2、NFC讀卡器；3、編碼器；4、機(jī)器人本體；5、IC卡片。

具體實(shí)施方式

現(xiàn)在結(jié)合附圖對(duì)本發(fā)明作進(jìn)一步詳細(xì)的說(shuō)明。

需要注意的是，發(fā)明中所引用的如“上”、“下”、“左”、“右”、“前”、“后”等的用語(yǔ)，亦僅為便于敘述的明了，而非用以限定本發(fā)明可實(shí)施的范圍，其相對(duì)關(guān)系的改變或調(diào)整，在無(wú)實(shí)質(zhì)變更技術(shù)內(nèi)容下，當(dāng)亦視為本發(fā)明可實(shí)施的范疇。

圖5為本發(fā)明實(shí)施例的帶式輸送機(jī)巡檢機(jī)器人系統(tǒng)高精度定位方法流程圖。參見(jiàn)圖5，該高精度定位方法包括以下步驟：

S1，對(duì)編碼器增量進(jìn)行累加計(jì)算，得到機(jī)器人沿軌道運(yùn)動(dòng)距離。

S2，結(jié)合步驟S1獲得機(jī)器人沿軌道運(yùn)動(dòng)距離，采用NFC讀到的IC卡位置信息對(duì)機(jī)器人連續(xù)位置進(jìn)行融合修正，得到機(jī)器人連續(xù)位置的融合修正公式為：



IC為NFC讀到的IC卡位置信息；c表示NFC讀到對(duì)應(yīng)IC卡時(shí)，編碼器開(kāi)始計(jì)數(shù)的采樣點(diǎn)對(duì)應(yīng)的位置；編碼器累加系數(shù)k是機(jī)器人運(yùn)行實(shí)際距離與對(duì)應(yīng)編碼器累加距離之比，與摩擦輪直徑成正比；i為機(jī)器人從軌道位置a到軌道位置b對(duì)應(yīng)的編碼器的第i個(gè)采樣周期，取值由編碼器采樣頻率決定；Δli表示編碼器在第i個(gè)采樣周期中得到增量。

S3，利用NFC讀到的IC卡間數(shù)據(jù)，和編碼器累加得到的對(duì)應(yīng)的IC卡間數(shù)據(jù)，對(duì)步驟S2中編碼器累加系數(shù)k的取值進(jìn)行分段、分方向的預(yù)測(cè)修正，獲得表示機(jī)器人從ICj卡正向運(yùn)動(dòng)到ICj+1卡測(cè)得的k值



和表示機(jī)器人從ICj卡反向運(yùn)動(dòng)到ICj-1卡測(cè)得的k值/>



j表示IC卡在軌道上安裝的序列號(hào)，ICj是NFC讀到第j個(gè)IC卡的編碼器的采樣點(diǎn)位置；判斷修正后的編碼器累加系數(shù)k的取值有效性。

S4，對(duì)步驟S3修正后的編碼器累加系數(shù)k的取值進(jìn)行卡爾曼濾波，減小修正后的編碼器累加系數(shù)k的隨機(jī)誤差，獲得最優(yōu)估計(jì)修正k值。

S5，依據(jù)步驟S4獲得最優(yōu)估計(jì)修正k值和步驟S2獲得的融合修正位置公式，根據(jù)軌道與定位模塊相對(duì)位置模型計(jì)算機(jī)器人所在軌道的高精度位置。

圖1為本發(fā)明帶式輸送機(jī)巡檢機(jī)器人系統(tǒng)結(jié)構(gòu)圖，由懸掛于膠帶正上方的軌道機(jī)器人本體、運(yùn)動(dòng)單元和監(jiān)控室中的客戶端組成，巡檢機(jī)器人可以實(shí)現(xiàn)數(shù)據(jù)采集、客戶端監(jiān)控和運(yùn)動(dòng)控制。圖2為本發(fā)明兩個(gè)定位傳感器安裝示意圖，IC卡中保存有較為精確的軌道點(diǎn)位置信息，通過(guò)NFC(Near Field Communication)讀取，編碼器可以獲得連續(xù)位置信息，通過(guò)感器融合相互修正的思想，實(shí)現(xiàn)帶式輸送機(jī)巡檢機(jī)器人系統(tǒng)高精度定位。圖3為本發(fā)明實(shí)施例的軌道與定位模塊相對(duì)位置模型示意圖。本實(shí)施例的帶式輸送機(jī)巡檢機(jī)器人系統(tǒng)高精度定位方法具體包括以下步驟：

一、得到機(jī)器人沿軌道運(yùn)動(dòng)距離

步驟S1中，對(duì)編碼器增量進(jìn)行累加計(jì)算，得到機(jī)器人沿軌道運(yùn)動(dòng)距離的過(guò)程包括以下步驟：

S11，假設(shè)編碼器采樣周期內(nèi)最大旋轉(zhuǎn)角度小于半圈，編碼器在每個(gè)采樣周期中的旋轉(zhuǎn)增量為：



其中，n表示編碼器脈沖，y表示編碼器原始數(shù)據(jù)，i表示編碼器的采樣序號(hào)，由編碼器采樣頻率決定；

S12，機(jī)器人沿軌道運(yùn)動(dòng)的距離表示為：



二、得到機(jī)器人連續(xù)位置的融合修正公式

IC卡中保存的位置信息是工作人員通過(guò)測(cè)量后存入的，可以認(rèn)為IC卡中的位置信息是相對(duì)準(zhǔn)確的。當(dāng)NFC讀到一個(gè)IC卡中數(shù)據(jù)時(shí)，將剔除之前編碼器的累加值，在此位置基礎(chǔ)上重新累加，則有機(jī)器人連續(xù)位置的融合修正公式為：



其中，c表示NFC讀到對(duì)應(yīng)IC卡時(shí)，編碼器開(kāi)始計(jì)數(shù)的采樣點(diǎn)。比例系數(shù)k是機(jī)器人運(yùn)行實(shí)際距離與對(duì)應(yīng)編碼器累加距離之比，也稱編碼器累加系數(shù)，與摩擦輪直徑成正比。i為機(jī)器人從a到b，編碼器的采樣過(guò)程，由編碼器采樣頻率決定。Δl表示編碼器在每個(gè)采樣周期中得到增量。

當(dāng)然，在軌道上存在著布置一個(gè)IC卡，此時(shí)只需要將IC卡作為節(jié)點(diǎn)，但同樣適用前述連續(xù)位置的融合修正公式。

三、對(duì)編碼器累加系數(shù)k的取值預(yù)測(cè)修正

步驟S3中，利用NFC讀到的IC卡間數(shù)據(jù)，和編碼器累加得到的對(duì)應(yīng)的IC卡間數(shù)據(jù)，對(duì)步驟S2中編碼器累加系數(shù)k的取值進(jìn)行分段、分方向的預(yù)測(cè)修正的過(guò)程包括以下步驟：

利用NFC讀到的IC卡間數(shù)據(jù)，和編碼器累加得到的對(duì)應(yīng)的IC卡間數(shù)據(jù)，對(duì)k值進(jìn)行分段、分方向的預(yù)測(cè)修正，則修正k值為：



其中，j表示IC卡在軌道上安裝的序列號(hào)，



表示機(jī)器人從ICj卡正向運(yùn)動(dòng)到ICj+1卡測(cè)得的k值，/>



表示機(jī)器人從ICj卡反向運(yùn)動(dòng)到ICj-1卡測(cè)得的k值。

步驟S3中，判斷有效性的過(guò)程包括以下步驟：

采用e表示有效性誤差，當(dāng)



時(shí)，判斷修正kj值不存在粗大誤差，認(rèn)為kj有效，更新kj值；當(dāng)/>



時(shí)，判斷修正kj值存在粗大誤差，認(rèn)為kj無(wú)效，不更新kj值。

四、獲得最優(yōu)估計(jì)修正k值

步驟S4中，對(duì)步驟S3修正后的編碼器累加系數(shù)k的取值進(jìn)行卡爾曼濾波，減小修正后的編碼器累加系數(shù)k的隨機(jī)誤差，獲得最優(yōu)估計(jì)修正k值的過(guò)程包括以下步驟：

S41，令zjt＝kjt，kjt表示在第t次修正的kj值，則系統(tǒng)模型為：



其中，xjt，表示狀態(tài)真實(shí)值，zjt表示觀測(cè)值，vjt表示測(cè)量噪聲，且滿足p(vj)∈N(0,R)；

S42，預(yù)測(cè)方程為：



其中，



為狀態(tài)預(yù)測(cè)值，/>



為最優(yōu)估計(jì)值，/>



狀態(tài)預(yù)測(cè)協(xié)方差，pjt為狀態(tài)估計(jì)協(xié)方差；

S43，更新方程為：



其中，Kjt為卡爾曼增益，且Kjt∈[0,1]，第t次修正的最優(yōu)估計(jì)值為



在實(shí)例中，摩擦輪直徑取60mm，編碼器采樣頻率1000Hz，則編碼器累加系數(shù)k為0.184，最優(yōu)估計(jì)修正k值將趨近于0.183附近。

五、計(jì)算機(jī)器人所在軌道的高精度位置

依據(jù)下述高精度融合修正位置公式計(jì)算機(jī)器人所在軌道的高精度位置：



式中，



表示第t次修正的/>



值，/>



表示第t次修正的/>



值。

綜上所述，本實(shí)施例的一種帶式輸送機(jī)巡檢機(jī)器人系統(tǒng)高精度定位方法，其通過(guò)對(duì)編碼器累加的機(jī)器人行走距離，與NFC讀取IC卡中軌道位置信息相融合修正，得到機(jī)器人在軌道上連續(xù)的融合修正位置公式，根據(jù)歷史數(shù)據(jù)，再對(duì)編碼器累加系數(shù)進(jìn)行修正、有效性判斷和卡爾曼濾波，最后得到帶式輸送機(jī)巡檢機(jī)器人在軌道上的高精度位置信息。

實(shí)例應(yīng)用

應(yīng)用時(shí)，參考圖1，帶式輸送機(jī)巡檢機(jī)器人系統(tǒng)，通過(guò)機(jī)器人本體獲取帶式輸送機(jī)工作環(huán)境數(shù)據(jù)和運(yùn)行圖像，上傳到客戶端，進(jìn)行監(jiān)控，通過(guò)客戶端選擇機(jī)器人運(yùn)行模式，運(yùn)動(dòng)單元結(jié)合定位信息實(shí)現(xiàn)機(jī)器人本體的運(yùn)動(dòng)控制。

定位傳感器相對(duì)安裝位置，參考圖2，定位傳感器包括安裝于機(jī)器人本體4上側(cè)的NFC模塊2，用于讀取軌道1下側(cè)保存有軌道位置信息的IC卡5的點(diǎn)位置數(shù)據(jù)，以及通過(guò)與軌道摩擦驅(qū)動(dòng)的編碼器組成，用于讀取連續(xù)位置信息。通過(guò)定位融合與參數(shù)修正，實(shí)現(xiàn)帶式輸送機(jī)巡檢機(jī)器人的高精度定位。

機(jī)器人第t次從a運(yùn)動(dòng)到b，以4m/s的勻速前進(jìn)，如圖3所示軌道與定位模塊相對(duì)位置模型示意圖。

根據(jù)定位傳感器參數(shù)，可得編碼器采樣頻率為1000Hz，對(duì)應(yīng)i的更新周期1ms，Δli＝22；編碼器脈沖為1024，使用60mm直徑的摩擦輪安裝編碼器，則



IC0＝0，IC1＝10m，IC2＝20m，a對(duì)應(yīng)-5m，b對(duì)應(yīng)15m。計(jì)算過(guò)程和計(jì)算結(jié)果如下：

(1)由以上參數(shù)可得，初始值：取z10＝k10＝0.1841，e1＝5％，



p10＝1，R＝0.5，則：/>



(2)令編碼器的累加值



則一組隨機(jī)數(shù)數(shù)據(jù)A1～A9分別是：54325，55906，52706，55813，55482，54243，55661，53358，56161，54363。

(3)第一次k值計(jì)算與有效性判斷。



及為有效。

(4)對(duì)第一次狀態(tài)與協(xié)方差進(jìn)行預(yù)測(cè)，由預(yù)測(cè)方程得：



(5)對(duì)第一次最優(yōu)狀態(tài)值進(jìn)行更新，由更新方程得：



(6)因此，可以得到第一次機(jī)器人從a到達(dá)b點(diǎn)時(shí)，對(duì)應(yīng)軌道b點(diǎn)的位置為



(7)同理迭代計(jì)算，可以得到k2～k9，分別為：0.1897，0.1792，0.1802，0.1844，0.1797，0.1874，0.1781，0.1839?？梢缘玫?br />


分別為：0.1842，0.1828，0.1822，0.1826，0.1822，0.1829，0.1823，0.1825?？梢缘玫?>



分別為：15.07，15.03，15.01，15.02，15.01，15.03，15.01，15.02。

(8)由圖4可以看出，最優(yōu)估計(jì)修正k值波動(dòng)小于修正k值，趨近于0.183附近，可以為高精度的位置控制提供位置反饋信息。

步驟(3)～(8)為從IC1卡到IC2卡軌道間的機(jī)器人位置融合修正計(jì)算，其他軌道區(qū)間位置融合修正計(jì)算方式同上步驟，最終獲得帶式輸送機(jī)巡檢機(jī)器人在整個(gè)軌道上的高精度位置信息，可用于帶式輸送機(jī)巡檢機(jī)器人高精度的位置控制。

以上僅是本發(fā)明的優(yōu)選實(shí)施方式，本發(fā)明的保護(hù)范圍并不僅局限于上述實(shí)施例，凡屬于本發(fā)明思路下的技術(shù)方案均屬于本發(fā)明的保護(hù)范圍。應(yīng)當(dāng)指出，對(duì)于本技術(shù)領(lǐng)域的普通技術(shù)人員來(lái)說(shuō)，在不脫離本發(fā)明原理前提下的若干改進(jìn)和潤(rùn)飾，應(yīng)視為本發(fā)明的保護(hù)范圍。