[發(fā)明專利]一種基于語(yǔ)義邊緣對(duì)齊的由粗到精的多傳感器融合定位方法

有效

• 申請(qǐng)?zhí)枺?/td>: 202111518610.7
• 申請(qǐng)日: 2021-12-14
• 公開（公告）號(hào): CN113920198B
• 公開（公告）日: 2022-02-15
• 發(fā)明（設(shè)計(jì)）人: 郭成成;成二康;白昕暉;曹旭東
• 申請(qǐng)（專利權(quán)）人: 紐勱科技（上海）有限公司
• 主分類號(hào): G06T7/73
• 分類號(hào): G06T7/73;G06T5/30;G06T7/12;G06T7/13;G01C21/30
• 代理公司: 上海三方專利事務(wù)所(普通合伙) 31127
• 代理人: 吳瑋

1.一種基于語(yǔ)義邊緣對(duì)齊的由粗到精的多傳感器融合定位方法，包括獲取高精地圖、原始圖像、消費(fèi)級(jí)的車載GPS以及輪子里程計(jì)的步驟，其特征在于方法具體如下：

A.輸入的原始圖像首先會(huì)經(jīng)過語(yǔ)義分割網(wǎng)絡(luò)進(jìn)行處理得到語(yǔ)義分割圖;

B.系統(tǒng)初始化，獲取當(dāng)前車輛的初始化位姿；

C.計(jì)算當(dāng)前幀的位姿初值；

D.基于當(dāng)前幀的位姿初值從高精度地圖中搜索語(yǔ)義元素，形成對(duì)應(yīng)的地圖元素集合，對(duì)三維地圖點(diǎn)按照一定的距離進(jìn)行插值采樣形成散點(diǎn)；

E.將采樣后的三維地圖點(diǎn)根據(jù)當(dāng)前幀的位姿初值投影到圖像上，并在圖像上做均勻采樣；

F.將地圖點(diǎn)在代價(jià)圖像上的投影點(diǎn)光度殘差作為非線性優(yōu)化問題的優(yōu)化目標(biāo)，優(yōu)化車輛本體的位姿使得投影點(diǎn)整體光度殘差最小；

位姿優(yōu)化的殘差表達(dá)式如下：

其中I代表語(yǔ)義代價(jià)圖像，π為相機(jī)成像模型，T_wb代表車輛相對(duì)于高精地圖的位姿，也就是優(yōu)化估計(jì)的狀態(tài)量，T_bc表示相機(jī)相對(duì)于車輛的外參，P_w為三維地圖點(diǎn)，1.0為代價(jià)圖像中的灰度的最大值，即圖像中亮度最大值；

G.在系統(tǒng)運(yùn)行的過程中，會(huì)實(shí)時(shí)構(gòu)建一個(gè)基于固定長(zhǎng)度的滑動(dòng)窗口的位姿圖；

H.當(dāng)前幀的位姿跟蹤結(jié)束后，系統(tǒng)輸出當(dāng)前幀的車輛定位結(jié)果，即相對(duì)于高精地圖的位置和姿態(tài)。

2.如權(quán)利要求1所述的一種基于語(yǔ)義邊緣對(duì)齊的由粗到精的多傳感器融合定位方法，其特征在于輸入的原始圖像首先會(huì)經(jīng)過語(yǔ)義分割網(wǎng)絡(luò)進(jìn)行處理分別得到車道線的語(yǔ)義分割圖、桿子的語(yǔ)義分割圖和路牌的語(yǔ)義分割圖，為了能夠在圖像上進(jìn)行代價(jià)構(gòu)建，會(huì)對(duì)語(yǔ)義分割圖像進(jìn)行交替的圖像腐蝕膨脹處理得到代價(jià)圖像，對(duì)于路牌的分割圖像，會(huì)首先使用拉普拉斯變換方法從二值分割圖像中進(jìn)行邊緣提取，然后再進(jìn)行腐蝕膨脹處理得到路牌對(duì)應(yīng)的非線性優(yōu)化的代價(jià)圖像。

3.如權(quán)利要求1所述的一種基于語(yǔ)義邊緣對(duì)齊的由粗到精的多傳感器融合定位方法，其特征在于系統(tǒng)初始化具體方法如下：

步驟一.獲取第一幀的輸入，判斷第一幀輸入的GPS信號(hào)的有效性， 滿足要求則將其記錄為第一個(gè)有效二維軌跡點(diǎn)，否則退出重新進(jìn)入步驟A；

步驟二.獲取第二幀的輸入，判斷第二幀輸入的GPS信號(hào)的有效性，如果有效且與第一個(gè)軌跡點(diǎn)的距離適中，則將其記錄為第二個(gè)有效二維軌跡點(diǎn)，若不滿足條件，則重新進(jìn)步驟一；

步驟三.完成步驟二后，將車輛平面坐標(biāo)設(shè)置為第二個(gè)有效軌跡點(diǎn)，車輛高度通過在高精地圖中搜索得到，滾動(dòng)角和俯仰角置為0,航向角根據(jù)第一個(gè)有效點(diǎn)和第二個(gè)有效點(diǎn)的矢量方向確定；

步驟四.在步驟三的初始位置和姿態(tài)的基礎(chǔ)上，進(jìn)行位姿搜索，設(shè)定搜索間隔和搜索數(shù)量，計(jì)算每種位姿下的語(yǔ)義地圖點(diǎn)在代價(jià)圖像上的投影得分，將投影得分最高的位姿作為當(dāng)前車輛的初始化位姿。

4.如權(quán)利要求1所述的一種基于語(yǔ)義邊緣對(duì)齊的由粗到精的多傳感器融合定位方法，其特征在于所述步驟C中根據(jù)前后兩幀的里程計(jì)數(shù)據(jù)計(jì)算兩幀間的位姿增量，利用位姿增量和前一幀的位姿計(jì)算當(dāng)前幀的位姿初值。

5.如權(quán)利要求4所述的一種基于語(yǔ)義邊緣對(duì)齊的由粗到精的多傳感器融合定位方法，其特征在于所述步驟C中如果當(dāng)前幀的GPS觀測(cè)為有效狀態(tài)且定位系統(tǒng)在較長(zhǎng)時(shí)間內(nèi)未經(jīng)過縱向位置的更新，則利用當(dāng)前幀的GPS測(cè)量更新當(dāng)前幀的車輛縱向位置。

6.如權(quán)利要求1所述的一種基于語(yǔ)義邊緣對(duì)齊的由粗到精的多傳感器融合定位方法，其特征在于所述步驟F中優(yōu)化的時(shí)候會(huì)采用兩階段優(yōu)化，第一階段對(duì)所有投影點(diǎn)形成的殘差施加魯棒核，以對(duì)離群點(diǎn)觀測(cè)對(duì)最小二乘優(yōu)化問題造成的影響進(jìn)行約束，第二階段將一階段優(yōu)化后殘差超過一定閾值的觀測(cè)進(jìn)行去除，對(duì)剩余觀測(cè)不施加魯棒核，重新進(jìn)行優(yōu)化求解。

7.如權(quán)利要求1所述的一種基于語(yǔ)義邊緣對(duì)齊的由粗到精的多傳感器融合定位方法，其特征在于所述步驟G中新的幀被加入，最老的幀被移除，當(dāng)車輛處于靜止?fàn)顟B(tài)的時(shí)候，次新幀會(huì)被移除，位姿圖中的優(yōu)化變量包括當(dāng)前幀以及歷史幀的位姿，觀測(cè)約束包括窗口中每幀的視覺邊緣對(duì)齊的位姿以及兩幀之間的里程計(jì)的約束。