[發(fā)明專利]一種機器人航向角的測量方法、裝置及存儲介質

有效

• 申請?zhí)枺?/td>: 201911254139.8
• 申請日: 2019-12-06
• 公開（公告）號: CN111007863B
• 公開（公告）日: 2023-05-02
• 發(fā)明（設計）人: 何漢烜;胡顯達
• 申請（專利權）人: 廣州市申迪計算機系統有限公司
• 主分類號: G05D1/08
• 分類號: G05D1/08
• 代理公司: 廣州三環(huán)專利商標代理有限公司 44202
• 代理人: 郭浩輝;麥小嬋

本發(fā)明公開了一種機器人航向角的測量方法、裝置及存儲介質，所述方法包括：在每個采集間隔獲取各個陀螺儀當前時刻的原始航向角、上一時刻的最終航向角以及機器人轉向速度；根據轉向速度指令采用融合算法計算機器人下一時刻的預測航向角；計算當前時刻的一步協方差；分別計算各個陀螺儀的原始航向角相互之間的最短距離、所有陀螺儀的距離差的總和以及各個陀螺儀的偏差懲罰因子；根據上述各個陀螺儀的測量數據依序對所述預測航向角和當前時刻的一步協方差進行迭代校正，得到機器人在當前時刻進行數據融合后的最終航向角。本發(fā)明能夠有效解決基于單一陀螺儀的測量方式帶來航向角不穩(wěn)定的問題，提高了航向角測量方法的可靠性以及數據的可恢復性。

技術領域

本發(fā)明涉及機器人導航控制技術領域，尤其涉及一種機器人航向角的測量方法、裝置及存儲介質。

背景技術

當前巡檢機器人在進行自主室內導航時，需要測定自身的航向角以確定自身在環(huán)境中的位姿，航向角測定的準確與否直接決定機器人的定位準確性，因此需要設計一個高度可靠的航向角測量方法來確定機器人的航向角。

現有技術中機器人航向角的測定方法主要包括兩種，第一種方法是基于單個陀螺儀計算航向角，第二種方法是基于單個陀螺儀和電子羅盤的數據融合計算航向角。基于單個陀螺儀計算航向角在開機的時候，靜止一段時間進行均值測量，隨后把測得的陀螺儀角速度數據減去這個均值，同時對測量的角速度信號進行高通濾波，過濾低頻的溫漂干擾，對濾波后的角速度數據進行積分從而得出機器人的航向角。第二種方法的開始過程和第一種的原理相似，在過濾低頻的溫漂干擾后，對角速度數據進行積分，但并不立即將該積分結果作為航向角的最終值，而是把該結果和電子羅盤獲得的絕對地磁角度進行互補濾波，通過地磁角來校正航向角。

但是，在對現有技術的研究與實踐的過程中，本發(fā)明的發(fā)明人發(fā)現，現有技術存在以下缺陷：由于現有方法均采用單一的航向角測量設備輸出唯一的結果，一旦測量設備出現異常，將不可避免導致航向角的測量錯誤，數據容錯性較低；同時，由于僅僅通過單一設備來測量航向角，當該測量設備的測量結果錯誤時，則會永久丟失當前機器人的航向角，導致由于缺乏航向角的真值無法對航向角進行校正恢復，無法為機器人恢復為正確的航向角。

發(fā)明內容

本發(fā)明實施例所要解決的技術問題在于，提供一種機器人航向角的測量方法、裝置及存儲介質，通過采用多個陀螺儀得出多個原始航向角，并提出基于懲罰因子的數據融合算法，對多個原始航向角進行數據融合濾波來獲取最終的航向角。

為解決上述問題，本發(fā)明的一個實施例提供一種機器人航向角的測量方法，至少包括如下步驟：

在每一個采集間隔內獲取各個陀螺儀當前時刻所測量的原始航向角、機器人上一時刻的最終航向角以及機器人當前的轉向速度；

根據機器人所接收的轉向速度指令，采用融合算法每隔一個預設時間段計算機器人下一時刻的預測航向角；

根據一步協方差公式計算機器人當前時刻的一步協方差；

分別計算各個陀螺儀的原始航向角相互之間的最短距離、所有陀螺儀的距離差的總和以及各個陀螺儀的偏差懲罰因子；

根據各個陀螺儀的測量數據和引入偏差懲罰因子的卡爾曼濾波算法，依序對所述預測航向角和當前時刻的一步協方差進行迭代校正，完成迭代校正后得到機器人在當前時刻進行數據融合后的最終航向角。

進一步的，所述機器人航向角的測量方法，還包括：

根據所述各個陀螺儀的原始航向角相互之間的最短距離判斷是否需要陀螺儀自恢復處理。

進一步的，所述機器人航向角的測量方法，還包括：

在機器人底盤平面上豎直安裝所述各個陀螺儀，以使各個陀螺儀的重心均在同一條鉛垂線上。

進一步的，所述根據所述各個陀螺儀的原始航向角相互之間的最短距離判斷是否需要陀螺儀自恢復處理，具體為：