[發明專利]一種用于雙移動機器人剛體協作搬運的協同定位方法

有效

• 申請號: 201911154494.8
• 申請日: 2019-11-22
• 公開（公告）號: CN110967017B
• 公開（公告）日: 2023-03-31
• 發明（設計）人: 劉劍鋒;普杰信;孫力帆;張松燦;司彥娜;王燕玲
• 申請（專利權）人: 河南科技大學
• 主分類號: G01C21/20
• 分類號: G01C21/20;G01C1/00
• 代理公司: 洛陽公信知識產權事務所(普通合伙) 41120
• 代理人: 王學鵬

本發明提供一種用于雙移動機器人剛體協作搬運的協同定位方法，包括以下步驟：S1、構建基于剛性約束的雙機器人系統的運動學模型和量測模型；S2、初始化高斯?厄米特求積分卡爾曼濾波器；S3、基于運動學模型對雙機器人系統進行狀態推演，得到剛體狀態預測值；S4、獲取雙機器人系統的狀態量測值，并且將狀態量測值與量測模型相結合計算卡爾曼增益；S5、利用卡爾曼增益對剛體狀態預測值進行更新后得到剛體狀態估計值；S6、將更新后的剛體狀態估計值轉換為雙機器人系統中的兩個移動機器人的位姿狀態，其能夠融合由不變剛性約束產生的相對距離與方位約束條件限制誤差上界，使得估計結果更加接近真實運動軌跡。

技術領域

本發明涉及多機器人協同定位領域，具體涉及一種用于雙移動機器人剛體協作搬運的協同定位方法。

背景技術

近年來，多機器人系統受到了廣泛關注，其不僅能夠克服單機器人系統在信息獲取、處理和控制等方面的局限，而且運行效率高、靈活性好，可用于執行各種復雜任務。多機器人協作搬運是其主要應用之一，并在生產制造、物流倉儲等工業自動化應用中扮演著重要角色，除此之外，多機器人系統還能夠在一些高危場景中代替人執行搬運任務，如太空與深海探索、核泄漏廢棄物清理、戰場排雷等。因此，多機器人協作搬運具有重要的研究價值。

在多機器人剛體協作搬運中，如何對移動機器人團隊進行精確定位是其順利完成搬運任務的基礎和關鍵。本領域有的技術人員利用放置于環境上方的相機采集機器人圖像，并結合計算機視覺以實現搬運機器人定位。然而，該方法中有限的相機視野極大地限制了目標定位范圍，且使用較為繁瑣，需要對其進行標定。與借助外部設備不同，本領域有的技術人員利用機器人自身配備的傳感器進行多傳感器數據融合，以實現精確狀態估計。然而，這些方法并沒有考慮協作機器人之間的相關性，有可能導致根據定位結果計算出的相對信息具有較大方差，不利于協同控制，從而致使搬運任務失敗。

為了充分利用機器人之間的相對信息來改善定位性能，多機器人協同定位技術應運而生，其能夠通過機器人之間相互通信實現導航資源共享，進而減少對外部環境的依賴性，增強單個機器人的感知能力。此外，協同定位將個體定位誤差分散至協同網絡，提高整體定位精度，同時利用分布式架構提高計算效率和魯棒性。本領域有的技術人員利用擴展卡爾曼濾波器(EKF)對主從自主水下航行器(AUV)之間的相對距離信息進行融合，有效提高了僅配備低精度導航設備的從AUV定位精度，但由于EKF使用一階泰勒展開對非線性系統進行近似表示，會產生較大的截斷誤差，濾波精度仍有待于提高。本領域有的技術人員將能夠對任意非線性系統進行逼近的高斯-厄密特粒子濾波器(GHPF)應用于協同定位中，可獲得比EKF更高的濾波精度，但由于濾波器在每次遞歸中需要對多個粒子求高斯-厄米特積分，顯著增加了計算成本，難以實現對機器人團隊位置的實時估計。綜上所述，以上協同定位方法沒有對定位精度和效率進行較好地權衡，且沒有考慮剛體協作搬運任務的特殊性，不適合直接應用于協作搬運任務中。

發明內容

本發明的目的是提供一種用于雙移動機器人剛體協作搬運的協同定位方法，其能夠融合由不變剛性約束產生的相對距離與方位約束條件限制誤差上界，使得估計結果更加接近真實運動軌跡；同時，其能夠利用約束建立的耦合關系實現狀態降維，可顯著提高計算效率；而且，機器人之間的相對信息無需測量，有助于降低應用成本，便于實際應用。

為了達到上述目的，本發明所采用的技術方案是：一種用于雙移動機器人剛體協作搬運的協同定位方法，包括以下步驟：

S1、構建基于剛性約束的雙機器人系統的運動學模型和量測模型；

S2、初始化高斯-厄米特求積分卡爾曼濾波器；

S3、基于運動學模型對雙機器人系統進行狀態推演，得到剛體狀態預測值；

S4、獲取雙機器人系統的狀態量測值，并且將狀態量測值與量測模型相結合計算卡爾曼增益；

S5、利用卡爾曼增益對剛體狀態預測值進行更新后得到剛體狀態估計值；