[發明專利]一種回環閉合方法、裝置及機器人

有效

• 申請號: 201811602933.2
• 申請日: 2018-12-26
• 公開（公告）號: CN111435244B
• 公開（公告）日: 2023-05-30
• 發明（設計）人: 孫銘澤;鄒風山;梁亮;李燊;范煜東生;趙梁玉
• 申請（專利權）人: 沈陽新松機器人自動化股份有限公司
• 主分類號: G05D1/02
• 分類號: G05D1/02
• 代理公司: 沈陽科苑專利商標代理有限公司 21002
• 代理人: 許宗富

1.一種回環閉合方法，其特征在于，所述回環閉合方法包括：

在所創建地圖需要進行回環閉合時，重新計算所述地圖中最終節點的位姿和所述地圖創建過程中產生的累積誤差，所述累積誤差包括距離誤差和角度誤差；

獲取所述地圖創建過程中需要進行角度糾正的節點的糾正點集合；

根據所述糾正點集合以及所述最終節點的位姿重新計算預設節點的第一位姿，所述預設節點包括多個節點；

平均所述距離誤差到所述預設節點得到所述預設節點的第二位姿以完成所述地圖的閉合；

所述獲取所述地圖創建過程中需要進行角度糾正的節點的糾正點集合，包括：

選定目標節點；

將所述目標節點與其分別相鄰的兩個節點相連接；

若三個節點之間的連線所形成的角度不在預設角度范圍內，則判定所述目標節點為需要進行角度糾正的節點；

標記所述地圖創建過程中所有需要進行角度糾正的節點形成所述糾正點集合。

2.如權利要求1所述的回環閉合方法，其特征在于，所述將所述目標節點與其分別相鄰的兩個節點相連接，包括：

將所述目標節點、所述目標節點順時針方形的前一個節點和所述目標節點順時針方向的后一個節點按照節點形成的順序直線連接；

相應地，所述若三個節點之間的連線所形成的角度不在預設角度范圍內，則判定所述目標節點為需要進行角度糾正的節點，包括：

若所述前一個節點和所述目標節點的連線的延長線與所述目標節點和所述后一個節點的連線所形成的角度大于預設角度，則判定所述目標節點為需要進行角度糾正的節點。

3.如權利要求1所述的回環閉合方法，其特征在于，所述根據所述糾正點集合以及所述最終節點的位姿重新計算預設節點的第一位姿，包括：

計算所述糾正點集合中每個節點的糾正角度；

根據所述每個節點的糾正角度和所述最終節點的位姿重新計算所述預設節點的第一位姿。

4.如權利要求3所述的回環閉合方法，其特征在于，所述計算所述糾正點集合中每個節點的糾正角度，包括：

計算所述糾正點集合中每個節點的旋轉角度以及所有所述糾正點集合中所有節點的旋轉角度之和；

根據所述每個節點的旋轉角度占所述糾正點集合中所有節點的旋轉角度之和的比例，確定所述每個節點的修正權值；

根據所述修正權值和所述角度誤差確定所述糾正點集合中每個節點的糾正角度。

5.一種回環閉合裝置，其特征在于，所述回環閉合裝置包括：

第一計算模塊，用于在所創建地圖需要進行回環閉合時，重新計算所述地圖中最終節點的位姿和所述地圖創建過程中產生的累積誤差，所述累積誤差包括距離誤差和角度誤差；

獲取模塊，用于獲取所述地圖創建過程中需要進行角度糾正的節點的糾正點集合；

第二計算模塊，用于根據所述糾正點集合以及所述最終節點的位姿重新計算預設節點的第一位姿，所述預設節點包括多個節點；

平均模塊，用于平均所述距離誤差到所述預設節點得到所述預設節點的第二位姿以完成所述地圖的閉合；

所述獲取模塊具體用于：

選定目標節點；

將所述目標節點與其分別相鄰的兩個節點相連接；

若三個節點之間的連線所形成的角度不在預設角度范圍內，則判定所述目標節點為需要進行角度糾正的節點；

標記所述地圖創建過程中所有需要進行角度糾正的節點形成所述糾正點集合。

6.如權利要求5所述的回環閉合裝置，其特征在于，所述獲取模塊在用于將所述目標節點與其分別相鄰的兩個節點相連接時，具體包括：

將所述目標節點、所述目標節點順時針方形的前一個節點和所述目標節點順時針方向的后一個節點按照節點形成的順序直線連接；

相應地，所述若三個節點之間的連線所形成的角度不在預設角度范圍內，則判定所述目標節點為需要進行角度糾正的節點，包括：

若所述前一個節點和所述目標節點的連線的延長線與所述目標節點和所述后一個節點的連線所形成的角度大于預設角度，則判定所述目標節點為需要進行角度糾正的節點。