[發明專利]一種確定車體位姿的方法、裝置及環視視覺里程計系統

在審

• 申請號: 201910575005.X
• 申請日: 2019-06-28
• 公開（公告）號: CN112150547A
• 公開（公告）日: 2020-12-29
• 發明（設計）人: 徐抗
• 申請（專利權）人: 北京初速度科技有限公司
• 主分類號: G06T7/73
• 分類號: G06T7/73;G06T7/246;G06T7/80;G06K9/00
• 代理公司: 北京科領智誠知識產權代理事務所(普通合伙) 11782
• 代理人: 陳士騫

1.一種確定車體位姿的方法，其特征在于，所述方法包括：

接收各初始圖像；其中，任一所述初始圖像為安裝于車體的相機采集的所述車體周圍環境的圖像；

根據對所述各初始圖像中的特征點進行跟蹤的結果，確定重新獲取所述車體的位姿初始值或繼續對所述車體的位姿進行跟蹤；

當確定重新獲取所述車體的位姿初始值時，針對每個所述初始圖像，選取該初始圖像中的特征點，對各所述特征點進行三角化后得到該初始圖像對應的車體位姿估計值；對各初始圖像對應的車體位姿估計值求平均得到車體位姿初值，并通過最小化重投影誤差進行相機之間聯合優化，得到優化后的地圖點坐標和車體位姿信息；其中，所述地圖點為所述初始圖像中的特征點對應的三維點；

當確定繼續對所述車體的位姿進行跟蹤時，根據所述車體的當前位姿和勻速模型得到車體位姿初值，將所述車體位姿初值和預先標定得到的相機外參求乘積得到每個所述相機的相機位姿初值；將各所述相機的地圖點通過所述相機位姿初值投影到對應的初始圖像上，以投影位置為初值進行光流法匹配，得到對應的匹配點，并通過最小化所述重投影誤差優化車體位姿，得到優化后的車體位姿信息。

2.根據權利要求1所述的方法，其特征在于，所述根據對所述各初始圖像中的特征點進行跟蹤的結果，確定重新獲取所述車體的位姿初始值或繼續對所述車體的位姿進行跟蹤之前，所述方法還包括：

當所述初始圖像中任一點坐標為(x,y)時，其中，

x＝r^*cos(ψ)+x_cc,y＝r^*sin(ψ)+y_cc,ψ＝arctan2(y_w,x_w)

將上述表達式中的相機內參值，替換為對應去畸變后的圖像相機內參值，得到所述初始圖像去畸變后圖像中對應點的坐標；

其中，相機坐標系下的三維點P(x_w,y_w,z_w)投影到圖像的入射光角度為r為對應的圖像像素到圖像中心的像素半徑r＝f^*(1+k₀^*θ³+k₁^*θ⁵+k₂^*θ⁷+k₃^*θ⁹)，[k0,k1,k2,k3,f,x_cc,y_cc]為離線標定的相機內參；[kk0,kk1,kk2,kk3,ff,xx_cc,yy_cc]為去畸變后的圖像相機內參。

3.根據權利要求1所述的方法，其特征在于，所述根據對所述各初始圖像中的特征點進行跟蹤的結果，確定重新獲取所述車體的位姿初始值或繼續對所述車體的位姿進行跟蹤包括：

當對所述各初始圖像中的特征點進行跟蹤，確定任一所述特征點運動距離大于預設閾值時，確定重新獲取所述車體的位姿初始值，否則，確定繼續對所述車體的位姿進行跟蹤。

4.根據權利要求1所述的方法，其特征在于，所述方法還包括：

當對所述各初始圖像中的特征點進行跟蹤，確定當前的特征點的數量小于預設數量閾值時，添加關鍵幀，并通過最小化所述重投影誤差對當前所有的關鍵幀及地圖點進行聯合優化，得到優化后的地圖點坐標和車體位姿信息。

5.根據權利要求1-4任一項所述的方法，其特征在于，

所述重投影誤差為：

其中，是第c個相機所觀測到的第i個三維點的坐標信息，為對應的特征點在圖像上的坐標信息，為所述相機外參，為所述車體的第j個時刻的車體位姿信息。