[發明專利]一種基于譜圖方法的多機器人互定位可觀性判別方法及系統

有效

• 申請號: 202111671574.8
• 申請日: 2021-12-31
• 公開（公告）號: CN114353800B
• 公開（公告）日: 2023-10-24
• 發明（設計）人: 郝寧;賀風華;田春耕;姚昊迪
• 申請（專利權）人: 哈爾濱工業大學
• 主分類號: G01C21/20
• 分類號: G01C21/20
• 代理公司: 黑龍江立超同創知識產權代理有限責任公司 23217
• 代理人: 楊立超

一種基于譜圖方法的多機器人互定位可觀性判別方法及系統，涉及多機器人互定位技術領域，用以解決任意相對位置測量構型和運動約束條件下多機器人互定位系統的可觀性判別問題。本發明的技術要點包括：首先為多機器人互定位系統定義了拓展相對位置測量圖，包括相對位置測量配置和運動約束條件，進而給出了快速判別系統可觀或不可觀的充分條件；然后構造了基于拓展相對位置測量圖的可觀性判別矩陣，并基于該判別矩陣給出可觀性判別的充要條件。本發明簡化了判別基于相對位置測量的多機器人互定位可觀性的邏輯與計算量。本發明可應用于判別相對位置測量的多機器人互定位可觀性和多機器人互定位系統相對位置量測拓撲結構設計中。

技術領域

本發明涉及多機器人互定位技術領域，具體涉及一種基于譜圖方法的多機器人互定位可觀性判別方法及系統。

背景技術

多機器人進行互定位的目的是獲得一組機器人在某一共同參考系下的位姿。對于許多多機器人協同任務，如協同監視、偵察、搜索、救援和編隊控制等，多機器人互定位具有重要的作用。傳統的定位方法，如基于GNSS和動作捕捉定位系統，能夠獲得各機器人在全局坐標系下的絕對位姿，進而求出機器人之間的相對位姿，但是這些定位方法的使用受到成本和應用環境的制約?；诘貓D匹配的方法需要應用環境的先驗信息，在無紋理環境中性能表現較差。近年來，基于相機、激光雷達、UWB等相對量測傳感器和編碼器、羅盤、IMU等自運動量測傳感器的互定位方法得到了廣泛關注。

本質上，互定位問題是非線性狀態估計問題。首先需要解決的是可觀性判別問題，它保證了狀態估計誤差的有界性。然而，對于由n個機器人組成的多機器人系統，進行互定位的可觀性分析是一個非常困難的問題。首先，n個機器人之間可能存在的n(n-1)個相對測量，使得多機器人系統存在2^n(n-1)種可能的相對測量構型，增加了可觀性分析的復雜性；其次，機器人的運動也會影響多機器人系統的可觀性，現有的方法很難分析不同的相對測量構型和運動條件下的多機器人系統的可觀性。

目前，多機器人互定位系統的可觀性判別方法主要是基于Pairwise的方法，該方法將多機器人互定位的可觀性問題視為一系列解耦的成對可觀性問題。首先推導出兩個機器人的可觀性判別條件，然后通過假設系統中的相鄰機器人是可觀的，進而推出系統中的n個機器人都是可觀的。然而，該方法只給出了可觀性的充分條件，許多相對測量構型和運動約束條件下的多機器人系統的可觀性無法通過Pairwise的方法判別。

發明內容

鑒于以上問題，本發明提出一種基于譜圖方法的多機器人互定位可觀性判別方法及系統，用以解決任意相對位置測量構型和運動約束條件下多機器人互定位系統的可觀性判別問題。

根據本發明的一方面，提供一種基于譜圖方法的多機器人互定位可觀性判別方法，該方法包括以下步驟：

步驟一、每個機器人R_i的狀態為：

其中，和分別表示機器人R_i的機體坐標系相對于自運動坐標系f_i的位置和偏航角；和分別表示機器人R_i的自運動坐標系f_i相對于公共參考坐標系f₀的位置和偏航角；建立多機器人互定位系統的狀態方程和量測方程；其中，機器人R_i的狀態方程為：

其中，表示機器人R_i的機體坐標系相對機器人R_i的自運動坐標系f_i的線速度；表示機器人R_i的機體坐標系相對機器人R_i的自運動坐標系f_i的角速度；

相對位置的量測方程為：