[發明專利]一種基于方位的移動機器人協同校正算法

在審

• 申請號: 202111167717.1
• 申請日: 2021-09-29
• 公開（公告）號: CN113984073A
• 公開（公告）日: 2022-01-28
• 發明（設計）人: 田玉平;劉超群
• 申請（專利權）人: 杭州電子科技大學
• 主分類號: G01C21/34
• 分類號: G01C21/34;G01C21/36;G01C21/16
• 代理公司: 暫無信息
• 代理人: 暫無信息

1.一種基于方位的移動機器人協同校正算法，其特征在于，包括以下步驟：

A、定位體記錄觀測信息，所述觀測信息包括目標體發送的播報信息以及定位體的自身坐標信息；

B、定位體接收觀測信息并對觀測信息進行預處理；

C、根據預處理后得到的觀測值通過迭代計算不斷校正更新，從而實現移動機器人的精確定位，

所述步驟A中限定目標體和定位體在二維平面內任意運行，且所述目標體間歇性發送坐標信息，

所述目標體發送的播報信息包括在全局坐標系Σ_t的坐標p_t，所述定位體記錄的坐標信息包括自身在局部慣性坐標系Σ_a中的位置p_a、根據目標體發送的坐標信息測量得到目標體在其局部坐標系Σ_a中的位置p_a中的方位角α，所述方位角α為觀測到目標體的方向與局部坐標系x軸的夾角。

2.根據權利要求1所述的基于方位的移動機器人協同校正算法，其特征在于，所述步驟B將步驟A中徑n次觀測的p_t、p_a、α形成幾何關系，并求解坐標系Σ_a的原點到坐標系Σ_t的原點的平移向量F和兩個坐標系x軸之間的旋轉角θ。

3.根據權利要求2所述的基于方位的移動機器人協同校正算法，其特征在于，所述觀測次數不少于4次。

4.根據權利要求2或3所述的基于方位的移動機器人協同校正算法，其特征在于，所述步驟B中包括如下步驟：

B1)將p_t通過平移向量F和旋轉角θ轉換到Σ_a內的坐標

B2)經過n次觀測后形成了一個多邊形，要調整參數F和θ使得多邊形的各個頂點依次落在對應的由P_a＝[p_a1,…,p_an]^T和ρ＝[α₁,…,α_n]^T形成的射線上。

B3)由和P_a＝[p_a1,…,p_an]^T對應的各個點形成的向量的角度與ρ＝[α₁,…,α_n]^T定義最小二乘誤差；

B4)最小二乘誤差在n≥4時采用梯度下降法得到接近真實值的平移向量和旋轉角

5.根據權利要求4所述的基于方位的移動機器人協同校正算法，其特征在于，所述步驟C中將步驟B4中得到的和進行擴展卡爾曼濾波從而得到定位體在全局坐標系中的坐標的估計值。

6.根據權利要求5所述的基于方位的移動機器人協同校正算法，其特征在于，所述步驟C中通過更多的觀測信息消除噪聲帶來的誤差。

7.根據權利要求5所述的基于方位的移動機器人協同校正算法，其特征在于，所述步驟C中還包括通過獲得的平移向量F和旋轉角θ的估計值更新定位體在全局坐標系中的估計值。