[發明專利]一種基于物體語義路標的移動機器人視覺定位方法

有效

• 申請號: 202110411557.4
• 申請日: 2021-04-16
• 公開（公告）號: CN113034584B
• 公開（公告）日: 2022-08-30
• 發明（設計）人: 林旭濱;何力;楊益枘;管貽生;張宏
• 申請（專利權）人: 廣東工業大學
• 主分類號: G06T7/70
• 分類號: G06T7/70;G06T7/30;G06N3/08;G06N3/04
• 代理公司: 廣東廣信君達律師事務所 44329
• 代理人: 張生梅

1.一種基于物體語義路標的移動機器人視覺定位方法，其特征在于，包括如下步驟：

S1、構建離線的物體路標全局地圖；

S2、構建在線的物體路標局部地圖；

S3、以圖結構為數學模型對物體路標局部地圖和物體路標全局地圖進行數學表達，并以子圖匹配問題作為地圖匹配數學方程，以子地圖匹配求解結果實現物體路標局部地圖在物體路標全局地圖中的配準估計，最終實現移動機器人的視覺定位；

所述步驟S1構建離線的物體路標全局地圖的具體過程如下：

S1-1、獲取輸入圖像，并對輸入圖像進行預處理；

S1-2、利用訓練好的卷積神經網絡實現多目標物體檢測，并最終以2D矩形包絡框的形式輸出，輸出圖像中的所有目標物體的類別向量c_i及其包絡框在像素坐標系中的位置x_i，y_i及尺寸信息w_i，h_i，即{c_i,x_i,y_i,w_i,h_i,i∈N}，w_i為高度，h_i為高度；

S1-3、利用光流法對當前圖像與上一幀圖像進行像素級別配準，通過最小化兩幀間配準的像素的灰度值代價函數，估計相機運動位姿：

式(1)中，(·)^符號表示將李代數se(3)中的6維向量變換成4×4的矩陣形式，即而exp(·)是對李代數se(3)的指數映射，即exp(ξ^)∈SE(3)為相鄰幀的剛體運動位姿，SE(3)是特殊歐幾里得群；P₁和P₂為對應相機的投影矩陣，X_i,1和X_i,2為空間中對應的三維點在對應相機坐標系下的表達，I₁(·)和I₂(·)為對應像素點的像素灰度值；

S1-4、利用對偶橢球曲面對物體進行包絡作為物體路標地圖表達，先對圖像物體檢測框進行內切橢圓擬合并得到橢圓的對偶矩陣形式，對于視圖i而言，在其相機姿態(R_i，t_i)和相機內參矩陣K已經求得的情況下，三維空間中的對偶橢球曲面Q^*在該視圖中的投影輪廓通過投影矩陣P_i＝K·[R_i t_i]具有如下關系：

式(2)中，標量表明方程在相差一個尺度下具有等價性；

S1-5、在齊次坐標系中，橢球的對偶形式Q^*為一個4X4的對稱矩陣，而橢圓的對偶形式C^*為一個3X3的對稱矩陣，即Q^*和C^*分別有10個和6個獨立元素；方程(2)是一個二次型方程，將P_i進行二次型向量表達，記為B_i，原方程線性化表達成：

S1-6、對于多個視圖，聯立多個方程(3)構建線性方程組，對于過約束方程的求解問題是線性最小二乘問題，通過SVD進行求解出變量將其恢復成對稱矩陣可得到橢球原對偶形式：

完成物體包絡橢球的重建，形成物體路標全局地圖。

2.根據權利要求1所述的一種基于物體語義路標的移動機器人視覺定位方法，其特征在于，所述步驟S1-1中，預處理過程包括：濾波去噪、灰度圖轉化以及尺寸縮放，以適應神經網絡輸入維度。

3.根據權利要求1所述的一種基于物體語義路標的移動機器人視覺定位方法，其特征在于，所述步驟S2構建在線的物體路標局部地圖的過程如下：

對視覺圖像進行預處理后，進行物體檢測和數據關聯；接著利用光流法進行位姿跟蹤，即實現視覺里程計功能，該視覺里程計融合上一時刻優化位姿，以最大程度消除累計誤差；然后利用多視圖構建局部物體路標地圖，并采用滑動窗口式的局部地圖規模控制策略，并對物體路標包絡面坐標系進行剛體變換，在相機坐標系下進行表達，最終形成以相機坐標系相關聯的規模可控的物體路標局部地圖。

4.根據權利要求1所述的一種基于物體語義路標的移動機器人視覺定位方法，其特征在于，所述步驟S3的具體過程如下：

對物體路標局部地圖與物體路標全局地圖進行匹配，匹配問題從數學上看為一個子圖匹配問題，將物體路標全局地圖表示為圖結構G＝(V,E)，其中V為節點，包含每個路標i的三維坐標和標簽，E為邊，表示兩個路標之間的相互三維關系；同理將局部地圖表示為圖結構H＝(V',E')，V'為節點，E'為邊；

則地圖匹配問題具體定義為：給定具有n個節點的圖G＝(V,E)和具有m個節點的圖H＝(V',E')，在圖G和圖H尋求節點對應關系：

X∈{0,1}^n×m

以最大化圖屬性和結構的一致性：

其中v表示圖G的i₁節點和圖H的i₂節點的一致性度量，該度量來自路標的三維尺度一致性以及語義標簽的一致性；d表示圖G的i₁-j₁邊和圖H的i₂-j₂邊的一致性度量，該度量來自地圖中的不同路標之間三維位置距離一致性；利用語義信息對兩個圖中的相同標簽的節點進行匹配，減少優化迭代次數，最后獲得最優匹配解，確定兩個地圖之間路標的一一對應關系；

完成物體路標局部地圖對物體路標全局地圖的匹配之后，以物體路標全局地圖中的路標三維信息作為基準進行位姿傳播，即匹配上的全局地圖路標三維信息-局部地圖路標三維信息-局部地圖的參考坐標系——相機坐標系在世界坐標系中的變換T_wc∈SE(3)，最后獲得當前時刻機器人相對于地圖世界坐標系的位姿：

T_WR＝T_WCT_CR∈SE(3)

式中，T_CR∈SE(3)是機器人相對相機坐標系的變換矩陣,T_WC∈SE(3)是相機坐標系相對世界坐標系的變換矩陣，根據變換矩陣鏈式法則，二者相乘做矩陣運算可以得到機器人相對世界坐標系的變換矩陣T_WR∈SE(3)，即機器人的定位姿態信息。