[發(fā)明專利]一種基于軌跡對(duì)齊的語(yǔ)義建圖方法

有效

• 申請(qǐng)?zhí)枺?/td>: 202011644778.8
• 申請(qǐng)日: 2020-12-31
• 公開（公告）號(hào): CN112833892B
• 公開（公告）日: 2022-12-16
• 發(fā)明（設(shè)計(jì)）人: 張亮;朱光明;汪火良;蔣得志
• 申請(qǐng)（專利權(quán)）人: 杭州自適應(yīng)科技有限公司
• 主分類號(hào): G01C21/32
• 分類號(hào): G01C21/32
• 代理公司: 杭州知見專利代理有限公司 33295
• 代理人: 盧金元

1.一種基于軌跡對(duì)齊的語(yǔ)義建圖方法，其特征在于，包括以下步驟：

S1、沿同一條運(yùn)行軌跡同步采集RGB數(shù)據(jù)、激光數(shù)據(jù)、IMU數(shù)據(jù)和里程計(jì)數(shù)據(jù)；

S2、通過語(yǔ)義ORB-SLAM，在利用RGB數(shù)據(jù)進(jìn)行ORB追蹤的同時(shí)，對(duì)每一幀關(guān)鍵幀圖像進(jìn)行目標(biāo)檢測(cè)，再根據(jù)相鄰關(guān)鍵幀之間的目標(biāo)檢測(cè)結(jié)果構(gòu)建三角化關(guān)系，提取3D語(yǔ)義地標(biāo)；

S3、利用降采樣操作對(duì)激光數(shù)據(jù)進(jìn)行過濾，降低計(jì)算量，并對(duì)IMU數(shù)據(jù)和里程計(jì)數(shù)據(jù)進(jìn)行計(jì)算處理；

S4、通過Cartographer算法處理激光數(shù)據(jù)、IMU數(shù)據(jù)和里程計(jì)數(shù)據(jù)，構(gòu)建2D柵格地圖；

S5、利用ORB-SLAM和Cartographer算法生成的軌跡序列求解兩者之間的相似變換關(guān)系，將3D語(yǔ)義地標(biāo)和2D柵格地圖進(jìn)行融合，生成語(yǔ)義地圖；

所述步驟S2，具體按照以下步驟進(jìn)行：

S21、完成ORB-SLAM算法的地圖初始化，使ORB-SLAM算法可以對(duì)每一幀RGB圖像進(jìn)行ORB追蹤，并生成關(guān)鍵幀；

S22、在獲得關(guān)鍵幀后，迭代更新物體地標(biāo)的3D位姿：首先將物體地標(biāo)作為邊界框投影到每個(gè)關(guān)鍵幀圖像中，然后利用KM算法將這些投影和目標(biāo)檢測(cè)的結(jié)果兩兩進(jìn)行匹配，匹配一個(gè)投影邊界框p和一個(gè)檢測(cè)到的邊界框d的代價(jià)c(p,d)如式(1)所示：

其中，p表示投影邊界框，d表示檢測(cè)到的邊界框，l、t、r、b分別表示在像素坐標(biāo)系中邊界框的左側(cè)、頂部、右側(cè)和底部；

S23、通過從相機(jī)中心延伸到檢測(cè)到的邊界框的光線相交點(diǎn)來(lái)對(duì)地標(biāo)的3D 點(diǎn)位置進(jìn)行三角測(cè)量；在將一組物體地標(biāo)假設(shè)采樣為3D點(diǎn)之后，再為每個(gè)點(diǎn)實(shí)例化3D邊界立方體，將邊界立方體頂面的中心錨定在3D點(diǎn)上；

生成新的物體的立方體地標(biāo)假設(shè)后對(duì)這個(gè)新的假設(shè)進(jìn)行評(píng)分，物體假設(shè)h的得分S(h)如式(2)所示：

其中，L是地標(biāo)集合，H_o是o∈L的假設(shè)，K_o包含了所有檢測(cè)到o的關(guān)聯(lián)目標(biāo)的關(guān)鍵幀，δ_k是在k中的關(guān)聯(lián)檢測(cè)，而f_k將h投影到關(guān)鍵幀k中，α是用來(lái)調(diào)整第一個(gè)和的比例因子，Γ根據(jù)對(duì)象立方體h和h'的典型上下文關(guān)系來(lái)衡量它們的相關(guān)性；在對(duì)所有地標(biāo)的每一個(gè)假設(shè)進(jìn)行評(píng)分之后，若新的假設(shè)的最佳得分要比現(xiàn)有估計(jì)的地標(biāo)的得分更低的話，就使用得分最低的那個(gè)假設(shè)來(lái)替換掉原來(lái)的地標(biāo)；

S24、在每次迭代中，如果在數(shù)據(jù)關(guān)聯(lián)期間檢測(cè)到的邊界框未與任何現(xiàn)有地標(biāo)匹配，那么就實(shí)例化一個(gè)新的物體地標(biāo)以進(jìn)行此檢測(cè)；

所述步驟S5，具體按照以下步驟進(jìn)行：

S51、獲取語(yǔ)義ORB-SLAM算法生成的軌跡序列S_o和Cartographer算法生成的軌跡序列S_c，利用時(shí)間戳將兩個(gè)序列對(duì)齊，逐漸把S_o中沒有匹配點(diǎn)的位姿節(jié)點(diǎn)過濾掉，最終會(huì)生成兩個(gè)位姿點(diǎn)數(shù)目相同且按順序一一對(duì)應(yīng)的新序列S_c′和S_o′；

S52、在得到匹配后的序列S_c′和S_o′后，對(duì)這兩個(gè)序列進(jìn)行相似變換求解，要使變換后的誤差方差值ε²最小，誤差值ε²如式(4)所示：

其中，s是比例系數(shù)，R是旋轉(zhuǎn)系數(shù)，t是平移系數(shù)；這里使用umeyama方法求解這個(gè)方程，可以獲得最終的解：

R＝USV^T (5)

S53、獲得相似變換參數(shù)s,R,t后，求出ORB-SLAM生成的環(huán)境點(diǎn)云內(nèi)任意一個(gè)點(diǎn)在2D占據(jù)柵格坐標(biāo)系中的對(duì)應(yīng)位置，具體的轉(zhuǎn)換公式為：

p′＝s(Rp+t) (8)

其中，p′是柵格地圖坐標(biāo)系中的某個(gè)點(diǎn)，p是語(yǔ)義地標(biāo)坐標(biāo)系中的某個(gè)點(diǎn)；

S54、通過變換關(guān)系得到3D語(yǔ)義地表和2D柵格地圖的對(duì)應(yīng)關(guān)系，將3D語(yǔ)義地表和2D柵格地圖合并為一個(gè)語(yǔ)義地圖。

2.根據(jù)權(quán)利要求1所述的一種基于軌跡對(duì)齊的語(yǔ)義建圖方法，其特征在于，所述步驟S1中，RGB數(shù)據(jù)、激光數(shù)據(jù)、IMU數(shù)據(jù)和里程計(jì)數(shù)據(jù)同步開始采集，采集時(shí)各個(gè)傳感器以一個(gè)整體的狀態(tài)進(jìn)行移動(dòng)。