[發(fā)明專利]一種確定車體位姿的方法、裝置及環(huán)視視覺里程計(jì)系統(tǒng)

在審

• 申請?zhí)枺?/td>: 201910575005.X
• 申請日: 2019-06-28
• 公開（公告）號: CN112150547A
• 公開（公告）日: 2020-12-29
• 發(fā)明（設(shè)計(jì)）人: 徐抗
• 申請（專利權(quán)）人: 北京初速度科技有限公司
• 主分類號: G06T7/73
• 分類號: G06T7/73;G06T7/246;G06T7/80;G06K9/00
• 代理公司: 北京科領(lǐng)智誠知識產(chǎn)權(quán)代理事務(wù)所(普通合伙) 11782
• 代理人: 陳士騫

本發(fā)明實(shí)施例公開一種確定車體位姿的方法、裝置及環(huán)視視覺里程計(jì)系統(tǒng)。該方法包括：接收各初始圖像；當(dāng)確定重新獲取車體的位姿初始值時(shí)，針對每個(gè)初始圖像，選取該初始圖像中的特征點(diǎn)，對各特征點(diǎn)進(jìn)行三角化后得到該初始圖像對應(yīng)的車體位姿估計(jì)值；計(jì)算車體位姿初值，并通過最小化重投影誤差得到優(yōu)化后的地圖點(diǎn)坐標(biāo)和車體位姿信息；當(dāng)確定繼續(xù)對車體的位姿進(jìn)行跟蹤時(shí)，計(jì)算每個(gè)相機(jī)的相機(jī)位姿初值；將各相機(jī)的地圖點(diǎn)通過相機(jī)位姿初值投影到對應(yīng)的初始圖像上，以投影位置為初值進(jìn)行光流法匹配，得到對應(yīng)的匹配點(diǎn)，并通過最小化重投影誤差優(yōu)化車體位姿，得到優(yōu)化后的車體位姿信息。應(yīng)用本發(fā)明實(shí)施例提供的方案，能夠提高車輛定位的適用性。

技術(shù)領(lǐng)域

本發(fā)明涉及智能駕駛技術(shù)領(lǐng)域，具體而言，涉及一種確定車體位姿的方法、裝置及環(huán)視視覺里程計(jì)系統(tǒng)。

背景技術(shù)

自動(dòng)泊車中，需要獲得精確的車輛位姿信息。目前獲取位姿信息常用的傳感器包括相機(jī)，具體的，為了充分利用環(huán)境信息，可以在車身的前后左右各安裝一個(gè)相機(jī)，車上4個(gè)相機(jī)組成的系統(tǒng)稱為車載環(huán)視系統(tǒng)。

基于車載環(huán)視系統(tǒng)可以實(shí)現(xiàn)車輛的視覺里程計(jì)，已知的多目視覺里程計(jì)算法，均是利用相機(jī)之間的重疊區(qū)域進(jìn)行圖像數(shù)據(jù)的關(guān)聯(lián)。然而，此種方法要求相機(jī)安裝有大部分視角重疊，在實(shí)際應(yīng)用中，由于車輛的車身限制，使得這一安裝要求很難達(dá)到，導(dǎo)致上述方法適用性較低。因此，為了解決車輛定位中的適用性問題，亟需一種提高車輛定位適用性的方法。

發(fā)明內(nèi)容

本發(fā)明提供了一種確定車體位姿的方法、裝置及環(huán)視視覺里程計(jì)系統(tǒng)，以提高車輛定位的適用性。具體的技術(shù)方案如下。

第一方面，本發(fā)明實(shí)施例提供一種確定車體位姿的方法，所述方法包括：

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

根據(jù)對所述各初始圖像中的特征點(diǎn)進(jìn)行跟蹤的結(jié)果，確定重新獲取所述車體的位姿初始值或繼續(xù)對所述車體的位姿進(jìn)行跟蹤；

當(dāng)確定重新獲取所述車體的位姿初始值時(shí)，針對每個(gè)所述初始圖像，選取該初始圖像中的特征點(diǎn)，對各所述特征點(diǎn)進(jìn)行三角化后得到該初始圖像對應(yīng)的車體位姿估計(jì)值；對各初始圖像對應(yīng)的車體位姿估計(jì)值求平均得到車體位姿初值，并通過最小化重投影誤差進(jìn)行相機(jī)之間聯(lián)合優(yōu)化，得到優(yōu)化后的地圖點(diǎn)坐標(biāo)和車體位姿信息；其中，所述地圖點(diǎn)為所述初始圖像中的特征點(diǎn)對應(yīng)的三維點(diǎn)；

當(dāng)確定繼續(xù)對所述車體的位姿進(jìn)行跟蹤時(shí)，根據(jù)所述車體的當(dāng)前位姿和勻速模型得到車體位姿初值，將所述車體位姿初值和預(yù)先標(biāo)定得到的相機(jī)外參求乘積得到每個(gè)所述相機(jī)的相機(jī)位姿初值；將各所述相機(jī)的地圖點(diǎn)通過所述相機(jī)位姿初值投影到對應(yīng)的初始圖像上，以投影位置為初值進(jìn)行光流法匹配，得到對應(yīng)的匹配點(diǎn)，并通過最小化所述重投影誤差優(yōu)化車體位姿，得到優(yōu)化后的車體位姿信息。

可選的，所述根據(jù)對所述各初始圖像中的特征點(diǎn)進(jìn)行跟蹤的結(jié)果，確定重新獲取所述車體的位姿初始值或繼續(xù)對所述車體的位姿進(jìn)行跟蹤之前，所述方法還包括：

當(dāng)所述初始圖像中任一點(diǎn)坐標(biāo)為(x,y)時(shí)，其中，

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

將上述表達(dá)式中的相機(jī)內(nèi)參值，替換為對應(yīng)去畸變后的圖像相機(jī)內(nèi)參值，得到所述初始圖像去畸變后圖像中對應(yīng)點(diǎn)的坐標(biāo)；