[發(fā)明專利]里程計(jì)累計(jì)推算誤差的數(shù)據(jù)源糾正方法及裝置

有效

• 申請(qǐng)?zhí)枺?/td>: 202110282290.3
• 申請(qǐng)日: 2021-03-16
• 公開（公告）號(hào): CN113063441B
• 公開（公告）日: 2022-11-18
• 發(fā)明（設(shè)計(jì)）人: 李金波;周啟龍
• 申請(qǐng)（專利權(quán)）人: 李金波
• 主分類號(hào): G01C25/00
• 分類號(hào): G01C25/00;G01C22/00
• 代理公司: 長(zhǎng)沙永星專利商標(biāo)事務(wù)所(普通合伙) 43001
• 代理人: 周詠;米中業(yè)

本發(fā)明公開了一種里程計(jì)累計(jì)推算誤差的數(shù)據(jù)源糾正方法，包括如下步驟：獲取里程計(jì)輸出的第一位姿信息；采用第二定位系統(tǒng)獲取第二位姿信息；進(jìn)行第二位姿信息的置信度判定；在第二位姿信息的置信度滿足設(shè)定的置信度的判定準(zhǔn)則時(shí)進(jìn)行第一位姿信息的誤差等級(jí)判定；當(dāng)?shù)谝晃蛔诵畔⒋嬖谡`差等級(jí)時(shí)，將第二位姿信息作為數(shù)據(jù)源，進(jìn)行糾正。本發(fā)明還公開了一種基于里程計(jì)累計(jì)推算誤差的數(shù)據(jù)源糾正方法的裝置，包括里程調(diào)節(jié)單元和第二定位系統(tǒng)。本發(fā)明利用第二定位系統(tǒng)進(jìn)行輔助定位，對(duì)里程計(jì)的累計(jì)推算誤差的數(shù)據(jù)源進(jìn)行了糾正，高效快捷地提供了準(zhǔn)確的數(shù)據(jù)源，讓里程計(jì)更加準(zhǔn)確測(cè)量了移動(dòng)載體所經(jīng)過的里程。

技術(shù)領(lǐng)域

本發(fā)明具體涉及一種里程計(jì)累計(jì)推算誤差的數(shù)據(jù)源糾正方法及裝置。

背景技術(shù)

現(xiàn)在輪式機(jī)器人的發(fā)展越來越趨向于自動(dòng)化，對(duì)于位置測(cè)量和里程測(cè)量的精確度要求越來越高。如今輪式移動(dòng)機(jī)器人和自主車輛等移動(dòng)載體通常配備有多個(gè)輪速傳感器，其中，每個(gè)輪速傳感器能夠進(jìn)行輪式里程推算，得到輪式里程信息。輪式里程信息包括移動(dòng)載體從出發(fā)位置開始的累積位姿和當(dāng)前載體速度信息。但是，由于輪子打滑及輪徑隨著輪子磨損而變化等原因，里程計(jì)存在里程誤差累計(jì)的現(xiàn)象。若發(fā)生一次里程計(jì)角度誤差，隨著移動(dòng)載體的運(yùn)動(dòng)，則這個(gè)角度誤差數(shù)值會(huì)導(dǎo)致里程計(jì)的位置偏差隨著移動(dòng)距離增加而增大，即在該角度誤差的方向上出現(xiàn)位置誤差遞增的現(xiàn)象。現(xiàn)有技術(shù)一般采用擴(kuò)展卡爾曼濾波及無跡卡爾曼濾波等算法將輪式里程計(jì)與若干慣性元件進(jìn)行融合的相關(guān)工作，得到融合后的里程，但是所使用的里程數(shù)值仍是輪式里程計(jì)含有誤差累計(jì)之后的不準(zhǔn)確數(shù)值。傳統(tǒng)方法的相關(guān)算法采用因子圖優(yōu)化框架的多傳感融合里程算法，使用閉環(huán)檢測(cè)和后端優(yōu)化的方式來減小移動(dòng)載體位姿的累計(jì)誤差。

因此，現(xiàn)有技術(shù)在使用濾波方法和基于因子圖優(yōu)化等多傳感數(shù)據(jù)融合算法將輪式里程計(jì)與慣性元件進(jìn)行融合時(shí)，缺少對(duì)輪式里程計(jì)累計(jì)誤差在數(shù)據(jù)源上的糾正。操作復(fù)雜，無法提供準(zhǔn)確的數(shù)據(jù)源。

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

本發(fā)明的目的之一在于提供一種里程計(jì)累計(jì)推算誤差的數(shù)據(jù)源糾正方法，該方法通過對(duì)里程計(jì)累計(jì)誤差的數(shù)據(jù)源進(jìn)行糾正，準(zhǔn)確地計(jì)算出待測(cè)機(jī)器人的原始運(yùn)動(dòng)里程。

本發(fā)明的目的之二在于提供一種基于里程計(jì)累計(jì)推算誤差的數(shù)據(jù)源糾正方法的裝置。

本發(fā)明提供的這種里程計(jì)累計(jì)推算誤差的數(shù)據(jù)源糾正方法，包括如下步驟：

S1.獲取里程計(jì)輸出的第一位姿信息；

S2.采用第二定位系統(tǒng)獲取第二位姿信息；

S3.進(jìn)行第二位姿信息的置信度判定；

S4.在第二位姿信息的置信度滿足設(shè)定的置信度的判定準(zhǔn)則時(shí)進(jìn)行第一位姿信息的誤差等級(jí)判定；

S5.當(dāng)?shù)谝晃蛔诵畔⒎险`差等級(jí)判斷時(shí)，將第二位姿信息作為數(shù)據(jù)源，對(duì)數(shù)據(jù)源進(jìn)行糾正。

步驟S1所述的第一位姿信息包括第一位置信息、第一角度信息和第一速度信息。

步驟S2(中)，具體為第二定位系統(tǒng)提供位置輸出，通過與里程坐標(biāo)系進(jìn)行角度換算，得到第二位姿信息；所述的第二位姿信息包括第二位置信息，或者第二位姿信息包括第二角度信息和第二速度信息；當(dāng)?shù)诙蛔诵畔ǖ诙嵌刃畔⒑偷诙俣刃畔r(shí)，直接輸出第二位姿信息；當(dāng)?shù)诙蛔诵畔⒉话ǖ诙嵌刃畔⒒虻诙嵌刃畔⒂捎诘诙恢眯畔⒌闹眯哦鹊陀谠O(shè)定值時(shí)，通過當(dāng)前的第二位姿信息與在設(shè)定時(shí)間窗口內(nèi)的若干組第二位姿信息歷史值及間隔時(shí)間，計(jì)算當(dāng)前移動(dòng)載體的第二角度信息。