[發明專利]一種面向動態環境的地面輔助的視覺里程計方法

有效

• 申請號: 202010043799.8
• 申請日: 2020-01-15
• 公開（公告）號: CN111260709B
• 公開（公告）日: 2022-04-19
• 發明（設計）人: 何再興;楊勤峰;趙昕玥
• 申請（專利權）人: 浙江大學
• 主分類號: G06T7/521
• 分類號: G06T7/521
• 代理公司: 杭州求是專利事務所有限公司 33200
• 代理人: 林超

1.一種面向動態環境的地面輔助的視覺里程計方法，其特征在于，包括以下步驟：

步驟一，獲取深度相機拍攝的相鄰時刻的圖像

將深度相機固定安裝在地面移動機器人上，深度相機的光軸指向機器人正前方，通過深度相機采集到前一時刻和當前時刻的灰度圖像和深度圖像；

步驟二，檢測完整地面

在前一時刻和當前時刻的深度圖像上提取所有主平面，將屬于地面部分的主平面合并成完整的地面，作為地面區域；

步驟三，估計初始位姿

在前一時刻的灰度圖像的地面區域提取角點，對角點采用LK光流法進行跟蹤得到關聯的點對，作為地面點對并歸于靜態角點集合，然后使用n點透視方法進行計算得到當前時刻坐標系在前一時刻坐標系中的初始相對位姿；

步驟四，篩選角點

根據所有地面點對的重投影誤差進行擬合設定動態閾值，然后在前一時刻的灰度圖像的非地面區域提取角點，對角點采用LK光流法進行跟蹤得到關聯的點對作為非地面點對并歸于靜態角點集合，然后采用步驟三得到的初始相對位姿將當前時刻的角點重投影至前一時刻的圖像上求得重投影誤差后根據動態閾值進行篩選；

步驟五，估計優化位姿

聯合地面點對和非地面點對的靜態點對，使用n點透視方法進行計算得到優化后的當前時刻相機坐標系在前一時刻相機坐標系中的最終相對位姿；

步驟六，對于相鄰兩幀圖像重復上述步驟二～步驟五，計算獲得各個時刻的最終相對位姿，由各個時刻的最終相對位姿銜接作為結果，實現了動態環境下的視覺里程計。

2.根據權利要求1所述的一種面向動態環境的地面輔助的視覺里程計方法，其特征在于：所述的步驟二中，檢測完整地面采用以下方式進行處理：

1)提取主平面并計算地面似然參數：

根據深度圖像生成點云，采用凝聚層次聚類平面檢測算法提取點云中可能存在的多個主平面，得到主平面在灰度圖像中對應的區域，然后根據每個主平面的質心和法線方程計算以下公式的地面似然參數err，表示為：

其中，θ_a表示主平面的法線和理想地面法線的銳夾角；(c_u,c_v)^T表示主平面對應點云的質心在圖像上的二維像素坐標；cols和rows分別表示灰度圖像的寬度和高度；a為權重；

2)合并主平面：

選取地面似然參數最小的主平面作為種子面，遍歷其余主平面中的每個點，計算每個點到種子面的垂直距離，若每個點的垂直距離均小于預設距離閾值則將該主平面合并到種子面中，直至合并完全獲得的平面作為地面區域，表示為：

d_p＝Π_s·pT_dis

其中，p表示當前判斷的點，Π_s表示種子面的法線，T_dis表示距離閾值，d_p表示點p到種子面的垂直距離。

3.根據權利要求1所述的一種面向動態環境的地面輔助的視覺里程計方法，其特征在于：所述步驟四中，篩選角點具體為：

1)設定動態閾值：

計算所有地面點對的重投影誤差，采用正態分布對所有地面點對的重投影誤差進行擬合，取2倍的標準差作為動態閾值T_adp，即T_adp＝2σ，σ表示正態分布擬合后獲得的標準差；

2)篩選：在前一時刻的灰度圖像的非地面區域提取角點，并對角點采用LK光流法進行跟蹤得到關聯的點對，作為非地面點對；

然后采用步驟三得到的初始相對位姿，將后一時刻的角點重投影至前一時刻的圖像上求得重投影誤差，然后根據動態閾值進行篩選：

若后一時刻的角點p重投影至前一時刻的圖像求得的重投影誤差e(p)小于等于動態閾值T_adp，則將該角點p歸為靜態角點集合；

若后一時刻的角點p重投影至前一時刻的圖像求得的重投影誤差e(p)大于動態閾值T_adp，則不將該角點p歸為靜態角點集合。

4.根據權利要求1所述的一種面向動態環境的地面輔助的視覺里程計方法，其特征在于：所述的步驟三中，同一角點在前一時刻和當前時刻的灰度圖像中的坐標構成一對關聯的點對。

5.根據權利要求1所述的一種面向動態環境的地面輔助的視覺里程計方法，其特征在于：所述步驟四中，計算所有地面點對的重投影誤差，具體是將后一時刻的角點重投影至前一時刻的灰度圖像上的投影點，以投影后的投影點和角點之間的像素距離作為重投影誤差。