本發(fā)明涉及導(dǎo)航定位領(lǐng)域，更具體而言，涉及一種基于IMU的抗暴力裝置及方法。

背景技術(shù)：

在導(dǎo)航定位領(lǐng)域中，里程計(jì)單元的誤差隨路程而增大，在輪子打滑、離開地面時(shí)失效，可靠性低。激光傳感器在良好的室內(nèi)環(huán)境下激光精度高，其誤差不累積，但在惡劣環(huán)境下如環(huán)境單一的長走廊精度嚴(yán)重下降，因此需要外部提供載體的局部位姿信息，再由激光進(jìn)行匹配。另外室內(nèi)的動態(tài)障礙物也有可能對激光造成影響。

慣性導(dǎo)航單元IMU能跟蹤和反映載體的任何機(jī)動運(yùn)動，輸出平穩(wěn)，輸出頻率高，全天候工作，不受環(huán)境干擾，可靠性高，能獨(dú)立進(jìn)行自主導(dǎo)航。但I(xiàn)MU的位姿精度隨時(shí)間發(fā)散，不能直接用于機(jī)器人的導(dǎo)航。

因此有必要提出一種基于IMU的抗暴力干擾裝置及方法，在不改變現(xiàn)有硬件和軟件配置的情況下，根據(jù)IMU提供的姿態(tài)信息對異常情況的里程計(jì)單元和激光傳感器進(jìn)行校正。

公開于本發(fā)明背景技術(shù)部分的信息僅僅旨在加深對本發(fā)明的一般背景技術(shù)的理解，而不應(yīng)當(dāng)被視為承認(rèn)或以任何形式暗示該信息構(gòu)成已為本領(lǐng)域技術(shù)人員所公知的現(xiàn)有技術(shù)。

技術(shù)實(shí)現(xiàn)要素：

本發(fā)明提出了一種基于IMU的抗暴力干擾裝置及方法。本發(fā)明的目的是在不改變現(xiàn)有硬件和軟件配置的情況下，根據(jù)IMU提供的姿態(tài)信息對異常情況的里程計(jì)單元和激光傳感器進(jìn)行校正。

根據(jù)本發(fā)明的一方面，提出了一種基于IMU的抗暴力干擾裝置。該裝置可以包括：里程計(jì)單元、激光傳感單元以及慣性導(dǎo)航單元IMU。其中，所述里程計(jì)單元用于推算機(jī)器人或智能車輛在預(yù)先繪制的地圖中的第一位姿信息；所述激光傳感單元基于所述里程計(jì)單元輸出的第一位姿信息將所述激光傳感單元探測的障礙物與所述預(yù)先繪制的地圖中障礙物進(jìn)行匹配，以得出機(jī)器人或智能車輛在所述地圖中的精確位姿信息；所述慣性導(dǎo)航單元IMU用于通過慣導(dǎo)算法計(jì)算出機(jī)器人或智能車輛的第二位姿信息。

根據(jù)本發(fā)明的基于IMU的抗暴力干擾裝置通過以下步驟對異常情況下的里程計(jì)單元和激光傳感單元進(jìn)行校正：

1)初始化機(jī)器人或智能車輛的IMU、里程計(jì)單元、激光傳感單元以及定時(shí)器；

2)每隔1秒檢測一次機(jī)器人或智能車輛的IMU、里程計(jì)單元、激光傳感單元的狀態(tài)；

3)定義機(jī)器人或智能車輛運(yùn)動時(shí)的正常情況；

4)定義機(jī)器人或智能車輛運(yùn)動時(shí)的異常情況；

5)當(dāng)檢測到機(jī)器人或智能車輛從正常情況變成異常情況時(shí)，將前1秒里程計(jì)單元輸出的第一位姿信息、激光傳感單元輸出的精確位姿信息以及IMU輸出的第二位姿信息保存為基準(zhǔn)里程計(jì)單元數(shù)據(jù)、基準(zhǔn)激光傳感單元數(shù)據(jù)以及IMU基準(zhǔn)數(shù)據(jù)；

6)在檢測到機(jī)器人或智能車輛從異常情況變成正常情況之后，將所述基準(zhǔn)激光傳感單元數(shù)據(jù)與異常情況時(shí)間段IMU輸出的第二位姿信息變化量相加的結(jié)果作為校正后的激光傳感單元數(shù)據(jù)，并重復(fù)執(zhí)行步驟5)和6)。

可選地，機(jī)器人或智能車輛運(yùn)動時(shí)的正常情況為：IMU通信正常、里程計(jì)單元通信正常以及激光傳感單元通信正常，并且IMU與里程計(jì)單元每1秒變化的角度一致。

可選地，IMU與里程計(jì)單元每1秒變化的角度一致為每1秒二者偏差不超過10°

可選地，機(jī)器人或智能車輛運(yùn)動時(shí)的異常情況為：連續(xù)2秒里程計(jì)單元輸出的數(shù)據(jù)中時(shí)間變量未更新，或者連續(xù)2秒激光傳感單元輸出的數(shù)據(jù)中時(shí)間變量未更新，或者IMU與里程計(jì)單元每1秒的角度變化不一致。

根據(jù)本發(fā)明的另一方面，提出一種基于IMU的抗暴力干擾方法。該方法可以包括以下步驟：

1)初始化機(jī)器人或智能車輛的IMU、里程計(jì)單元、激光傳感單元以及定時(shí)器；

2)每隔1秒檢測一次機(jī)器人或智能車輛的IMU、里程計(jì)單元、激光傳感單元的狀態(tài)；

3)定義機(jī)器人或智能車輛運(yùn)動時(shí)的正常情況；

4)定義機(jī)器人或智能車輛運(yùn)動時(shí)的異常情況；

5)當(dāng)檢測到機(jī)器人或智能車輛從正常情況變成異常情況時(shí)，將前1秒里程計(jì)單元輸出的第一位姿信息、激光傳感單元輸出的精確位姿信息以及IMU輸出的第二位姿信息保存為基準(zhǔn)里程計(jì)單元數(shù)據(jù)、基準(zhǔn)激光傳感單元數(shù)據(jù)以及IMU基準(zhǔn)數(shù)據(jù)；

6)在檢測到機(jī)器人或智能車輛從異常情況變成正常情況之后，將所述基準(zhǔn)激光傳感單元數(shù)據(jù)與異常情況時(shí)間段IMU輸出的第二位姿信息變化量相加的結(jié)果作為校正后的激光傳感單元數(shù)據(jù)，并重復(fù)執(zhí)行步驟5)和6)。

可選地，機(jī)器人或智能車輛運(yùn)動時(shí)的正常情況為：IMU通信正常、里程計(jì)單元通信正常以及激光傳感單元通信正常，并且IMU與里程計(jì)單元每1秒變化的角度一致。

可選地，IMU與里程計(jì)單元每1秒變化的角度一致為每1秒二者偏差不超過10°

可選地，機(jī)器人或智能車輛運(yùn)動時(shí)的異常情況為：連續(xù)2秒里程計(jì)單元輸出的數(shù)據(jù)中時(shí)間變量未更新，或者連續(xù)2秒激光傳感單元輸出的數(shù)據(jù)中時(shí)間變量未更新，或者IMU與里程計(jì)單元每1秒的角度變化不一致。

本發(fā)明通過根據(jù)IMU提供的姿態(tài)信息對異常情況的里程計(jì)單元或激光傳感器進(jìn)行校正而實(shí)現(xiàn)了基于IMU的抗暴力干擾。

本發(fā)明的方法和裝置具有其它的特性和優(yōu)點(diǎn)，這些特性和優(yōu)點(diǎn)從并入本文中的附圖和隨后的具體實(shí)施例中將是顯而易見的，或者將在并入本文中的附圖和隨后的具體實(shí)施例中進(jìn)行詳細(xì)陳述，這些附圖和具體實(shí)施例共同用于解釋本發(fā)明的特定原理。

附圖說明

通過結(jié)合附圖對本發(fā)明示例性實(shí)施例進(jìn)行更詳細(xì)的描述，本發(fā)明的上述以及其它目的、特征和優(yōu)勢將變得更加明顯，其中，在本發(fā)明示例性實(shí)施例中，相同的參考標(biāo)號通常代表相同部件。

圖1示出了根據(jù)本發(fā)明的一個(gè)實(shí)施例的基于IMU的抗暴力干擾裝置的示意圖。

圖2示出了根據(jù)本發(fā)明的一個(gè)實(shí)施例的基于IMU的抗暴力干擾方法的流程圖。

具體實(shí)施方式

下面將參照附圖更詳細(xì)地描述本發(fā)明。雖然附圖中顯示了本發(fā)明的優(yōu)選實(shí)施例，然而應(yīng)該理解，可以以各種形式實(shí)現(xiàn)本發(fā)明而不應(yīng)被這里闡述的實(shí)施例所限制。相反，提供這些實(shí)施例是為了使本發(fā)明更加透徹和完整，并且能夠?qū)⒈景l(fā)明的范圍完整地傳達(dá)給本領(lǐng)域的技術(shù)人員。

實(shí)施例1

圖1示出了根據(jù)本發(fā)明的一個(gè)實(shí)施例的基于IMU的抗暴力干擾裝置的示意圖。

在該實(shí)施例中，基于IMU的抗暴力干擾裝置可以包括：里程計(jì)單元101、激光傳感器102和慣性導(dǎo)航單元IMU103。

里程計(jì)單元101用于推算機(jī)器人或智能車輛在預(yù)先繪制的地圖中的第一位姿信息。在本發(fā)明中，該位姿信息可以為二維位姿信息，即x、y坐標(biāo)和航向角。里程計(jì)單元的誤差隨著其所裝載的機(jī)器人或智能車輛行走的路程而增大。

激光傳感器102基于所述里程計(jì)單元輸出的第一位姿信息將探測的障礙物與所述預(yù)先繪制的地圖中障礙物進(jìn)行匹配，以得出機(jī)器人或智能車輛在所述地圖中的精確位姿信息。

里程計(jì)單元所輸出的位姿信息是比較粗糙的，激光傳感器結(jié)合該粗糙的位姿信息而得到更加精確的位姿信息。

然而激光傳感器所探測的障礙物與預(yù)先繪制的地圖中的障礙物精確匹配的前提是里程計(jì)單元的誤差，尤其是航向誤差在一定的范圍內(nèi)，否則激光匹配失敗，機(jī)器人不能繼續(xù)前行。而機(jī)器人行走過程中，遇到如下暴力干擾的情況很容易導(dǎo)致機(jī)器人導(dǎo)航失?。?/p>

1)里程計(jì)單元打滑、里程計(jì)單元空轉(zhuǎn)、里程計(jì)單元壓到障礙物(過坎)、里程計(jì)單元通信中斷；

2)機(jī)器人被人為搬動；

3)激光通信中斷(切換地圖時(shí))。

慣性導(dǎo)航單元IMU103用于通過慣導(dǎo)算法計(jì)算出機(jī)器人或智能車輛的第二位姿信息，并且IMU103也可以輸出機(jī)器人或智能車輛的速度信息。

IMU基于牛頓經(jīng)典力學(xué)測量載體的角速度和線加速度，通過慣導(dǎo)算法計(jì)算出載體姿態(tài)信息和速度位置信息。其工作不受環(huán)境干擾，輸出平穩(wěn)，輸出頻率高，可以全天候工作。但I(xiàn)MU的位姿精度隨時(shí)間發(fā)散，不能直接用于機(jī)器人的導(dǎo)航，需要與里程計(jì)單元和激光傳感器相配合來進(jìn)行導(dǎo)航。

在該實(shí)施例中，基于IMU的抗暴力干擾裝置通過以下步驟對異常情況下的里程計(jì)單元和激光傳感器進(jìn)行校正。

1)初始化機(jī)器人或智能車輛的IMU、里程計(jì)單元、激光傳感器以及定時(shí)器。

2)每隔1秒檢測一次機(jī)器人或智能車輛的IMU、里程計(jì)單元、激光傳感器的狀態(tài)。

3)定義機(jī)器人或智能車輛運(yùn)動時(shí)的正常情況。

可選地，將IMU通信正常、里程計(jì)單元通信正常以及激光傳感器通信正常，并且IMU與里程計(jì)單元每1秒變化的角度一致定義為正常情況。

可選地，IMU與里程計(jì)單元每1秒變化的角度一致為每1秒二者偏差不超過10°。

4)定義機(jī)器人或智能車輛運(yùn)動時(shí)的異常情況。

可選地，將連續(xù)2秒里程計(jì)單元數(shù)據(jù)中時(shí)間變量未更新，或者連續(xù)2秒激光傳感器數(shù)據(jù)中時(shí)間變量未更新，或者里程計(jì)單元與IMU每1秒的角度變化不一致定義為智能機(jī)器人或智能車輛運(yùn)動的異常情況。

5)當(dāng)檢測到機(jī)器人或智能車輛從正常情況變成異常情況時(shí)，將前1秒里程計(jì)單元輸出的第一位姿信息、激光傳感單元輸出的精確位姿信息以及IMU輸出的第二位姿信息保存為基準(zhǔn)里程計(jì)單元數(shù)據(jù)、基準(zhǔn)激光傳感單元數(shù)據(jù)以及IMU基準(zhǔn)數(shù)據(jù)。

即，當(dāng)檢測到里程計(jì)單元通信中斷，或者激光傳感器通信中斷，或者里程計(jì)單元與IMU每1秒的角度變化不一致時(shí)，則保存機(jī)器人或智能車輛處于正常狀態(tài)的最后1秒里程計(jì)單元輸出的第一位姿信息、激光傳感單元輸出的精確位姿信息以及IMU輸出的第二位姿信息以用于后續(xù)的校正步驟。

6)在檢測到機(jī)器人或智能車輛從異常情況變成正常情況之后，將所述基準(zhǔn)激光傳感單元數(shù)據(jù)與異常情況時(shí)間段IMU輸出的第二位姿信息變化量相加的結(jié)果作為校正后的激光傳感單元數(shù)據(jù)，并重復(fù)執(zhí)行步驟5)和6)。

即，當(dāng)檢測到IMU、里程計(jì)單元和激光傳感器正常通信，并且里程計(jì)單元與IMU每1秒變化的角度一致之后，將當(dāng)前的激光傳感器數(shù)據(jù)更新為所述基準(zhǔn)激光傳感器數(shù)據(jù)與異常時(shí)間段IMU輸出的第二位姿信息變化量之和。其中，異常時(shí)間段IMU輸出的第二位姿信息變化量為在檢測到機(jī)器人或智能車輛從異常情況變成正常情況時(shí)IMU所輸出的第二位姿信息與所述IMU基準(zhǔn)數(shù)據(jù)的差值。

由于激光傳感器是基于里程計(jì)單元輸出的位置坐標(biāo)變量和航向角變量來得到機(jī)器人或智能車輛的精確位姿的，因此在里程計(jì)單元恢復(fù)正常通信之后即可以與激光傳感器相配合繼續(xù)進(jìn)行導(dǎo)航。

本實(shí)施例通過根據(jù)IMU提供的姿態(tài)信息對異常情況的里程計(jì)單元或激光傳感器進(jìn)行校正而實(shí)現(xiàn)了基于IMU的抗暴力干擾。

實(shí)施例2

根據(jù)本發(fā)明的另一個(gè)實(shí)施例，提出了一種基于IMU的抗暴力干擾方法。該方法可以包括以下步驟：

1)初始化機(jī)器人或智能車輛的IMU、里程計(jì)單元、激光傳感單元以及定時(shí)器；

2)每隔1秒檢測一次機(jī)器人或智能車輛的IMU、里程計(jì)單元、激光傳感單元的狀態(tài)；

3)定義機(jī)器人或智能車輛運(yùn)動時(shí)的正常情況；

4)定義機(jī)器人或智能車輛運(yùn)動時(shí)的異常情況；

5)當(dāng)檢測到機(jī)器人或智能車輛從正常情況變成異常情況時(shí)，將前1秒里程計(jì)單元輸出的第一位姿信息、激光傳感單元輸出的精確位姿信息以及IMU輸出的第二位姿信息保存為基準(zhǔn)里程計(jì)單元數(shù)據(jù)、基準(zhǔn)激光傳感單元數(shù)據(jù)以及IMU基準(zhǔn)數(shù)據(jù)；

6)在檢測到機(jī)器人或智能車輛從異常情況變成正常情況之后，將所述基準(zhǔn)激光傳感單元數(shù)據(jù)與異常情況時(shí)間段IMU輸出的第二位姿信息變化量相加的結(jié)果作為校正后的激光傳感單元數(shù)據(jù)，并重復(fù)執(zhí)行步驟5)和6)。

在一個(gè)示例中，機(jī)器人或智能車輛運(yùn)動時(shí)的正常情況可以為：IMU通信正常、里程計(jì)單元通信正常以及激光傳感單元通信正常，并且IMU與里程計(jì)單元每1秒變化的角度一致。

在一個(gè)示例中，IMU與里程計(jì)單元每1秒變化的角度一致可以為每1秒二者偏差不超過10°。

在一個(gè)示例中，機(jī)器人或智能車輛運(yùn)動時(shí)的異常情況可以為：連續(xù)2秒里程計(jì)單元輸出的數(shù)據(jù)中時(shí)間變量未更新，或者連續(xù)2秒激光傳感單元輸出的數(shù)據(jù)中時(shí)間變量未更新，或者IMU與里程計(jì)單元每1秒的角度變化不一致。

本領(lǐng)域技術(shù)人員應(yīng)理解，上面對本發(fā)明的實(shí)施例的描述的目的僅為了示例性地說明本發(fā)明的實(shí)施例的有益效果，并不意在將本發(fā)明的實(shí)施例限制于所給出的任何示例。

以上已經(jīng)描述了本發(fā)明的各實(shí)施例，上述說明是示例性的，并非窮盡性的，并且也不限于所披露的各實(shí)施例。在不偏離所說明的各實(shí)施例的范圍和精神的情況下，對于本技術(shù)領(lǐng)域的普通技術(shù)人員來說許多修改和變更都是顯而易見的。本文中所用術(shù)語的選擇，旨在最好地解釋各實(shí)施例的原理、實(shí)際應(yīng)用或?qū)κ袌鲋械募夹g(shù)的改進(jìn)，或者使本技術(shù)領(lǐng)域的其它普通技術(shù)人員能理解本文披露的各實(shí)施例。