[發(fā)明專利]一種基于里程計(jì)的機(jī)器人定位恢復(fù)方法

有效

• 申請(qǐng)?zhí)枺?/td>: 201711457137.X
• 申請(qǐng)日: 2017-12-28
• 公開（公告）號(hào): CN108151760B
• 公開（公告）日: 2020-08-11
• 發(fā)明（設(shè)計(jì)）人: 林歡;李彬;魏軍;程敏;趙偉;邱顯東;張國良;王鋒;項(xiàng)導(dǎo);毛成林
• 申請(qǐng)（專利權(quán)）人: 億嘉和科技股份有限公司
• 主分類號(hào): G01C22/00
• 分類號(hào): G01C22/00
• 代理公司: 南京知識(shí)律師事務(wù)所 32207
• 代理人: 陳靜

1.一種基于里程計(jì)的機(jī)器人定位恢復(fù)方法，其特征在于，包括以下步驟：

正常導(dǎo)航過程中維護(hù)里程計(jì)信息和關(guān)鍵點(diǎn)信息；所述關(guān)鍵點(diǎn)信息包括里程計(jì)信息中第一幀數(shù)據(jù)、機(jī)器人行走固定距離或轉(zhuǎn)向固定角度后的里程計(jì)信息；

判斷機(jī)器人發(fā)生定位丟失后，進(jìn)行定位恢復(fù)；所述定位恢復(fù)，包括以下步驟：

判斷里程計(jì)隊(duì)列和關(guān)鍵點(diǎn)隊(duì)列中有存儲(chǔ)數(shù)據(jù)時(shí)，控制機(jī)器人行走到上一關(guān)鍵點(diǎn)位置，根據(jù)當(dāng)前位置重新設(shè)置機(jī)器人定位；直至定位恢復(fù)；

所述正常導(dǎo)航過程中維護(hù)里程計(jì)信息和關(guān)鍵點(diǎn)信息，包括以下步驟：

機(jī)器人行走時(shí)，將第一次收到的里程計(jì)信息作為關(guān)鍵點(diǎn)第一幀數(shù)據(jù)存入關(guān)鍵點(diǎn)隊(duì)列中；

機(jī)器人每行走固定時(shí)間間隔，記錄下機(jī)器人當(dāng)前里程計(jì)信息，存入里程計(jì)隊(duì)列中；

機(jī)器人在行走固定時(shí)間間隔內(nèi)，將當(dāng)前里程計(jì)信息與關(guān)鍵點(diǎn)隊(duì)列最后一幀數(shù)據(jù)進(jìn)行幾何求值，將每行走固定距離后的里程計(jì)信息同時(shí)存入關(guān)鍵點(diǎn)隊(duì)列中，并記錄新關(guān)鍵點(diǎn)；或者機(jī)器人在轉(zhuǎn)向流程中有轉(zhuǎn)向動(dòng)作達(dá)到固定角度時(shí)，則在轉(zhuǎn)向結(jié)束時(shí)記錄當(dāng)前里程計(jì)信息，存入關(guān)鍵點(diǎn)隊(duì)列中，并記錄新關(guān)鍵點(diǎn)；

機(jī)器人每行走固定時(shí)間間隔內(nèi)，將關(guān)鍵點(diǎn)隊(duì)列中與當(dāng)前里程信息距離大于最大距離或者轉(zhuǎn)向角度大于最大轉(zhuǎn)向角度的數(shù)據(jù)，清除出關(guān)鍵點(diǎn)隊(duì)列。

2.根據(jù)權(quán)利要求1所述的基于里程計(jì)的機(jī)器人定位恢復(fù)方法，其特征在于，控制機(jī)器人行走到上一關(guān)鍵點(diǎn)位置時(shí)，使用周期糾正流程，通過對(duì)機(jī)器人運(yùn)動(dòng)過程中每一幀數(shù)據(jù)進(jìn)行目標(biāo)點(diǎn)與當(dāng)前位置的對(duì)比，重新進(jìn)行速度規(guī)劃，對(duì)行走過程中位置偏差進(jìn)行糾正。

3.根據(jù)權(quán)利要求2所述的基于里程計(jì)的機(jī)器人定位恢復(fù)方法，其特征在于，所述速度規(guī)劃使用速度平滑流程，根據(jù)機(jī)器人離目標(biāo)點(diǎn)的距離來規(guī)劃速度和加速度。

4.根據(jù)權(quán)利要求3所述的基于里程計(jì)的機(jī)器人定位恢復(fù)方法，其特征在于，所述速度平滑流程，遵循以下原則：

(1)機(jī)器人離目標(biāo)點(diǎn)越近時(shí)，速度越低；反之，速度越高；

(2)機(jī)器人離目標(biāo)點(diǎn)越近時(shí)，加速度越低；反之，加速度越高。

5.根據(jù)權(quán)利要求4所述的基于里程計(jì)的機(jī)器人定位恢復(fù)方法，其特征在于，所述速度平滑流程中速度v的函數(shù)為：

v＝a*d^b

其中a、b是函數(shù)待確定的參數(shù)，根據(jù)每個(gè)機(jī)器人所能運(yùn)動(dòng)的速度上下限和通常的運(yùn)動(dòng)距離來定義函數(shù)中的v和d，將v和d代入公式獲取參數(shù)a、b。

6.根據(jù)權(quán)利要求1～5任一所述的基于里程計(jì)的機(jī)器人定位恢復(fù)方法，其特征在于，所述定位丟失是指定位置信度小于閾值。

7.根據(jù)權(quán)利要求1所述的基于里程計(jì)的機(jī)器人定位恢復(fù)方法，其特征在于，所述定位恢復(fù)的具體步驟如下：

1)判斷記錄機(jī)器人行走數(shù)據(jù)的里程計(jì)隊(duì)列中是否有存儲(chǔ)數(shù)據(jù)，如果有存儲(chǔ)數(shù)據(jù)，則進(jìn)入步驟2)；否則，定位恢復(fù)失敗；

2)判斷關(guān)鍵點(diǎn)隊(duì)列是否有歷史關(guān)鍵點(diǎn)，如果有歷史關(guān)鍵點(diǎn)，進(jìn)入步驟3)；否則，定位恢復(fù)失敗；

3)開啟回退定時(shí)器；

4)根據(jù)里程計(jì)數(shù)據(jù)控制機(jī)器人行走到上一關(guān)鍵點(diǎn)位置：對(duì)于運(yùn)行中的每一幀，獲取機(jī)器人當(dāng)前位置，同時(shí)讀取關(guān)鍵點(diǎn)隊(duì)列中隊(duì)尾的關(guān)鍵點(diǎn)數(shù)據(jù)，利用周期糾正流程對(duì)機(jī)器人進(jìn)行速度控制，直到到達(dá)關(guān)鍵點(diǎn)隊(duì)列中隊(duì)尾關(guān)鍵點(diǎn)對(duì)應(yīng)的位置，隨后關(guān)鍵點(diǎn)隊(duì)列將隊(duì)尾的關(guān)鍵點(diǎn)數(shù)據(jù)移出隊(duì)列，進(jìn)入步驟5)；

5)若回退定時(shí)器的計(jì)時(shí)到期，則進(jìn)入步驟6)；否則，進(jìn)入步驟4)；

6)根據(jù)當(dāng)前位置重新設(shè)置機(jī)器人定位：根據(jù)里程計(jì)累積的運(yùn)動(dòng)距離估計(jì)倒退時(shí)移動(dòng)的距離，以此為移動(dòng)量預(yù)測(cè)當(dāng)前位置，重新設(shè)置機(jī)器人定位；

7)判斷定位是否恢復(fù)，如未恢復(fù)，則進(jìn)入步驟2)；如已恢復(fù)，則定位恢復(fù)結(jié)束。