基於IMU的抗暴力幹擾裝置及方法與流程
2023-12-09 16:50:11 2
本發明涉及導航定位領域，更具體而言，涉及一種基於IMU的抗暴力裝置及方法。
背景技術：
在導航定位領域中，裡程計單元的誤差隨路程而增大，在輪子打滑、離開地面時失效，可靠性低。雷射傳感器在良好的室內環境下雷射精度高，其誤差不累積，但在惡劣環境下如環境單一的長走廊精度嚴重下降，因此需要外部提供載體的局部位姿信息，再由雷射進行匹配。另外室內的動態障礙物也有可能對雷射造成影響。
慣性導航單元IMU能跟蹤和反映載體的任何機動運動，輸出平穩，輸出頻率高，全天候工作，不受環境幹擾，可靠性高，能獨立進行自主導航。但IMU的位姿精度隨時間發散，不能直接用於機器人的導航。
因此有必要提出一種基於IMU的抗暴力幹擾裝置及方法，在不改變現有硬體和軟體配置的情況下，根據IMU提供的姿態信息對異常情況的裡程計單元和雷射傳感器進行校正。
公開於本發明背景技術部分的信息僅僅旨在加深對本發明的一般背景技術的理解，而不應當被視為承認或以任何形式暗示該信息構成已為本領域技術人員所公知的現有技術。
技術實現要素：
本發明提出了一種基於IMU的抗暴力幹擾裝置及方法。本發明的目的是在不改變現有硬體和軟體配置的情況下，根據IMU提供的姿態信息對異常情況的裡程計單元和雷射傳感器進行校正。
根據本發明的一方面，提出了一種基於IMU的抗暴力幹擾裝置。該裝置可以包括：裡程計單元、雷射傳感單元以及慣性導航單元IMU。其中，所述裡程計單元用於推算機器人或智能車輛在預先繪製的地圖中的第一位姿信息；所述雷射傳感單元基於所述裡程計單元輸出的第一位姿信息將所述雷射傳感單元探測的障礙物與所述預先繪製的地圖中障礙物進行匹配，以得出機器人或智能車輛在所述地圖中的精確位姿信息；所述慣性導航單元IMU用於通過慣導算法計算出機器人或智能車輛的第二位姿信息。
根據本發明的基於IMU的抗暴力幹擾裝置通過以下步驟對異常情況下的裡程計單元和雷射傳感單元進行校正：
1)初始化機器人或智能車輛的IMU、裡程計單元、雷射傳感單元以及定時器；
2)每隔1秒檢測一次機器人或智能車輛的IMU、裡程計單元、雷射傳感單元的狀態；
3)定義機器人或智能車輛運動時的正常情況；
4)定義機器人或智能車輛運動時的異常情況；
5)當檢測到機器人或智能車輛從正常情況變成異常情況時，將前1秒裡程計單元輸出的第一位姿信息、雷射傳感單元輸出的精確位姿信息以及IMU輸出的第二位姿信息保存為基準裡程計單元數據、基準雷射傳感單元數據以及IMU基準數據；
6)在檢測到機器人或智能車輛從異常情況變成正常情況之後，將所述基準雷射傳感單元數據與異常情況時間段IMU輸出的第二位姿信息變化量相加的結果作為校正後的雷射傳感單元數據，並重複執行步驟5)和6)。
可選地，機器人或智能車輛運動時的正常情況為：IMU通信正常、裡程計單元通信正常以及雷射傳感單元通信正常，並且IMU與裡程計單元每1秒變化的角度一致。
可選地，IMU與裡程計單元每1秒變化的角度一致為每1秒二者偏差不超過10°
可選地，機器人或智能車輛運動時的異常情況為：連續2秒裡程計單元輸出的數據中時間變量未更新，或者連續2秒雷射傳感單元輸出的數據中時間變量未更新，或者IMU與裡程計單元每1秒的角度變化不一致。
根據本發明的另一方面，提出一種基於IMU的抗暴力幹擾方法。該方法可以包括以下步驟：
1)初始化機器人或智能車輛的IMU、裡程計單元、雷射傳感單元以及定時器；
2)每隔1秒檢測一次機器人或智能車輛的IMU、裡程計單元、雷射傳感單元的狀態；
3)定義機器人或智能車輛運動時的正常情況；
4)定義機器人或智能車輛運動時的異常情況；
5)當檢測到機器人或智能車輛從正常情況變成異常情況時，將前1秒裡程計單元輸出的第一位姿信息、雷射傳感單元輸出的精確位姿信息以及IMU輸出的第二位姿信息保存為基準裡程計單元數據、基準雷射傳感單元數據以及IMU基準數據；
6)在檢測到機器人或智能車輛從異常情況變成正常情況之後，將所述基準雷射傳感單元數據與異常情況時間段IMU輸出的第二位姿信息變化量相加的結果作為校正後的雷射傳感單元數據，並重複執行步驟5)和6)。
可選地，機器人或智能車輛運動時的正常情況為：IMU通信正常、裡程計單元通信正常以及雷射傳感單元通信正常，並且IMU與裡程計單元每1秒變化的角度一致。
可選地，IMU與裡程計單元每1秒變化的角度一致為每1秒二者偏差不超過10°
可選地，機器人或智能車輛運動時的異常情況為：連續2秒裡程計單元輸出的數據中時間變量未更新，或者連續2秒雷射傳感單元輸出的數據中時間變量未更新，或者IMU與裡程計單元每1秒的角度變化不一致。
本發明通過根據IMU提供的姿態信息對異常情況的裡程計單元或雷射傳感器進行校正而實現了基於IMU的抗暴力幹擾。
本發明的方法和裝置具有其它的特性和優點，這些特性和優點從併入本文中的附圖和隨後的具體實施例中將是顯而易見的，或者將在併入本文中的附圖和隨後的具體實施例中進行詳細陳述，這些附圖和具體實施例共同用於解釋本發明的特定原理。
附圖說明
通過結合附圖對本發明示例性實施例進行更詳細的描述，本發明的上述以及其它目的、特徵和優勢將變得更加明顯，其中，在本發明示例性實施例中，相同的參考標號通常代表相同部件。
圖1示出了根據本發明的一個實施例的基於IMU的抗暴力幹擾裝置的示意圖。
圖2示出了根據本發明的一個實施例的基於IMU的抗暴力幹擾方法的流程圖。
具體實施方式
下面將參照附圖更詳細地描述本發明。雖然附圖中顯示了本發明的優選實施例，然而應該理解，可以以各種形式實現本發明而不應被這裡闡述的實施例所限制。相反，提供這些實施例是為了使本發明更加透徹和完整，並且能夠將本發明的範圍完整地傳達給本領域的技術人員。
實施例1
圖1示出了根據本發明的一個實施例的基於IMU的抗暴力幹擾裝置的示意圖。
在該實施例中，基於IMU的抗暴力幹擾裝置可以包括：裡程計單元101、雷射傳感器102和慣性導航單元IMU103。
裡程計單元101用於推算機器人或智能車輛在預先繪製的地圖中的第一位姿信息。在本發明中，該位姿信息可以為二維位姿信息，即x、y坐標和航向角。裡程計單元的誤差隨著其所裝載的機器人或智能車輛行走的路程而增大。
雷射傳感器102基於所述裡程計單元輸出的第一位姿信息將探測的障礙物與所述預先繪製的地圖中障礙物進行匹配，以得出機器人或智能車輛在所述地圖中的精確位姿信息。
裡程計單元所輸出的位姿信息是比較粗糙的，雷射傳感器結合該粗糙的位姿信息而得到更加精確的位姿信息。
然而雷射傳感器所探測的障礙物與預先繪製的地圖中的障礙物精確匹配的前提是裡程計單元的誤差，尤其是航向誤差在一定的範圍內，否則雷射匹配失敗，機器人不能繼續前行。而機器人行走過程中，遇到如下暴力幹擾的情況很容易導致機器人導航失敗：
1)裡程計單元打滑、裡程計單元空轉、裡程計單元壓到障礙物(過坎)、裡程計單元通信中斷；
2)機器人被人為搬動；
3)雷射通信中斷(切換地圖時)。
慣性導航單元IMU103用於通過慣導算法計算出機器人或智能車輛的第二位姿信息，並且IMU103也可以輸出機器人或智能車輛的速度信息。
IMU基於牛頓經典力學測量載體的角速度和線加速度，通過慣導算法計算出載體姿態信息和速度位置信息。其工作不受環境幹擾，輸出平穩，輸出頻率高，可以全天候工作。但IMU的位姿精度隨時間發散，不能直接用於機器人的導航，需要與裡程計單元和雷射傳感器相配合來進行導航。
在該實施例中，基於IMU的抗暴力幹擾裝置通過以下步驟對異常情況下的裡程計單元和雷射傳感器進行校正。
1)初始化機器人或智能車輛的IMU、裡程計單元、雷射傳感器以及定時器。
2)每隔1秒檢測一次機器人或智能車輛的IMU、裡程計單元、雷射傳感器的狀態。
3)定義機器人或智能車輛運動時的正常情況。
可選地，將IMU通信正常、裡程計單元通信正常以及雷射傳感器通信正常，並且IMU與裡程計單元每1秒變化的角度一致定義為正常情況。
可選地，IMU與裡程計單元每1秒變化的角度一致為每1秒二者偏差不超過10°。
4)定義機器人或智能車輛運動時的異常情況。
可選地，將連續2秒裡程計單元數據中時間變量未更新，或者連續2秒雷射傳感器數據中時間變量未更新，或者裡程計單元與IMU每1秒的角度變化不一致定義為智慧機器人或智能車輛運動的異常情況。
5)當檢測到機器人或智能車輛從正常情況變成異常情況時，將前1秒裡程計單元輸出的第一位姿信息、雷射傳感單元輸出的精確位姿信息以及IMU輸出的第二位姿信息保存為基準裡程計單元數據、基準雷射傳感單元數據以及IMU基準數據。
即，當檢測到裡程計單元通信中斷，或者雷射傳感器通信中斷，或者裡程計單元與IMU每1秒的角度變化不一致時，則保存機器人或智能車輛處於正常狀態的最後1秒裡程計單元輸出的第一位姿信息、雷射傳感單元輸出的精確位姿信息以及IMU輸出的第二位姿信息以用於後續的校正步驟。
6)在檢測到機器人或智能車輛從異常情況變成正常情況之後，將所述基準雷射傳感單元數據與異常情況時間段IMU輸出的第二位姿信息變化量相加的結果作為校正後的雷射傳感單元數據，並重複執行步驟5)和6)。
即，當檢測到IMU、裡程計單元和雷射傳感器正常通信，並且裡程計單元與IMU每1秒變化的角度一致之後，將當前的雷射傳感器數據更新為所述基準雷射傳感器數據與異常時間段IMU輸出的第二位姿信息變化量之和。其中，異常時間段IMU輸出的第二位姿信息變化量為在檢測到機器人或智能車輛從異常情況變成正常情況時IMU所輸出的第二位姿信息與所述IMU基準數據的差值。
由於雷射傳感器是基於裡程計單元輸出的位置坐標變量和航向角變量來得到機器人或智能車輛的精確位姿的，因此在裡程計單元恢復正常通信之後即可以與雷射傳感器相配合繼續進行導航。
本實施例通過根據IMU提供的姿態信息對異常情況的裡程計單元或雷射傳感器進行校正而實現了基於IMU的抗暴力幹擾。
實施例2
根據本發明的另一個實施例，提出了一種基於IMU的抗暴力幹擾方法。該方法可以包括以下步驟：
1)初始化機器人或智能車輛的IMU、裡程計單元、雷射傳感單元以及定時器；
2)每隔1秒檢測一次機器人或智能車輛的IMU、裡程計單元、雷射傳感單元的狀態；
3)定義機器人或智能車輛運動時的正常情況；
4)定義機器人或智能車輛運動時的異常情況；
5)當檢測到機器人或智能車輛從正常情況變成異常情況時，將前1秒裡程計單元輸出的第一位姿信息、雷射傳感單元輸出的精確位姿信息以及IMU輸出的第二位姿信息保存為基準裡程計單元數據、基準雷射傳感單元數據以及IMU基準數據；
6)在檢測到機器人或智能車輛從異常情況變成正常情況之後，將所述基準雷射傳感單元數據與異常情況時間段IMU輸出的第二位姿信息變化量相加的結果作為校正後的雷射傳感單元數據，並重複執行步驟5)和6)。
在一個示例中，機器人或智能車輛運動時的正常情況可以為：IMU通信正常、裡程計單元通信正常以及雷射傳感單元通信正常，並且IMU與裡程計單元每1秒變化的角度一致。
在一個示例中，IMU與裡程計單元每1秒變化的角度一致可以為每1秒二者偏差不超過10°。
在一個示例中，機器人或智能車輛運動時的異常情況可以為：連續2秒裡程計單元輸出的數據中時間變量未更新，或者連續2秒雷射傳感單元輸出的數據中時間變量未更新，或者IMU與裡程計單元每1秒的角度變化不一致。
本領域技術人員應理解，上面對本發明的實施例的描述的目的僅為了示例性地說明本發明的實施例的有益效果，並不意在將本發明的實施例限制於所給出的任何示例。
以上已經描述了本發明的各實施例，上述說明是示例性的，並非窮盡性的，並且也不限於所披露的各實施例。在不偏離所說明的各實施例的範圍和精神的情況下，對於本技術領域的普通技術人員來說許多修改和變更都是顯而易見的。本文中所用術語的選擇，旨在最好地解釋各實施例的原理、實際應用或對市場中的技術的改進，或者使本技術領域的其它普通技術人員能理解本文披露的各實施例。