[發明專利]一種基于軌跡對齊的語義建圖方法有效
| 申請號: | 202011644778.8 | 申請日: | 2020-12-31 |
| 公開(公告)號: | CN112833892B | 公開(公告)日: | 2022-12-16 |
| 發明(設計)人: | 張亮;朱光明;汪火良;蔣得志 | 申請(專利權)人: | 杭州自適應科技有限公司 |
| 主分類號: | G01C21/32 | 分類號: | G01C21/32 |
| 代理公司: | 杭州知見專利代理有限公司 33295 | 代理人: | 盧金元 |
| 地址: | 310000 浙江省杭*** | 國省代碼: | 浙江;33 |
| 權利要求書: | 查看更多 | 說明書: | 查看更多 |
| 摘要: | |||
| 搜索關鍵詞: | 一種 基于 軌跡 對齊 語義 方法 | ||
1.一種基于軌跡對齊的語義建圖方法,其特征在于,包括以下步驟:
S1、沿同一條運行軌跡同步采集RGB數據、激光數據、IMU數據和里程計數據;
S2、通過語義ORB-SLAM,在利用RGB數據進行ORB追蹤的同時,對每一幀關鍵幀圖像進行目標檢測,再根據相鄰關鍵幀之間的目標檢測結果構建三角化關系,提取3D語義地標;
S3、利用降采樣操作對激光數據進行過濾,降低計算量,并對IMU數據和里程計數據進行計算處理;
S4、通過Cartographer算法處理激光數據、IMU數據和里程計數據,構建2D柵格地圖;
S5、利用ORB-SLAM和Cartographer算法生成的軌跡序列求解兩者之間的相似變換關系,將3D語義地標和2D柵格地圖進行融合,生成語義地圖;
所述步驟S2,具體按照以下步驟進行:
S21、完成ORB-SLAM算法的地圖初始化,使ORB-SLAM算法可以對每一幀RGB圖像進行ORB追蹤,并生成關鍵幀;
S22、在獲得關鍵幀后,迭代更新物體地標的3D位姿:首先將物體地標作為邊界框投影到每個關鍵幀圖像中,然后利用KM算法將這些投影和目標檢測的結果兩兩進行匹配,匹配一個投影邊界框p和一個檢測到的邊界框d的代價c(p,d)如式(1)所示:
其中,p表示投影邊界框,d表示檢測到的邊界框,l、t、r、b分別表示在像素坐標系中邊界框的左側、頂部、右側和底部;
S23、通過從相機中心延伸到檢測到的邊界框的光線相交點來對地標的3D 點位置進行三角測量;在將一組物體地標假設采樣為3D點之后,再為每個點實例化3D邊界立方體,將邊界立方體頂面的中心錨定在3D點上;
生成新的物體的立方體地標假設后對這個新的假設進行評分,物體假設h的得分S(h)如式(2)所示:
其中,L是地標集合,Ho是o∈L的假設,Ko包含了所有檢測到o的關聯目標的關鍵幀,δk是在k中的關聯檢測,而fk將h投影到關鍵幀k中,α是用來調整第一個和的比例因子,Γ根據對象立方體h和h'的典型上下文關系來衡量它們的相關性;在對所有地標的每一個假設進行評分之后,若新的假設的最佳得分要比現有估計的地標的得分更低的話,就使用得分最低的那個假設來替換掉原來的地標;
S24、在每次迭代中,如果在數據關聯期間檢測到的邊界框未與任何現有地標匹配,那么就實例化一個新的物體地標以進行此檢測;
所述步驟S5,具體按照以下步驟進行:
S51、獲取語義ORB-SLAM算法生成的軌跡序列So和Cartographer算法生成的軌跡序列Sc,利用時間戳將兩個序列對齊,逐漸把So中沒有匹配點的位姿節點過濾掉,最終會生成兩個位姿點數目相同且按順序一一對應的新序列Sc′和So′;
S52、在得到匹配后的序列Sc′和So′后,對這兩個序列進行相似變換求解,要使變換后的誤差方差值ε2最小,誤差值ε2如式(4)所示:
其中,s是比例系數,R是旋轉系數,t是平移系數;這里使用umeyama方法求解這個方程,可以獲得最終的解:
R=USVT (5)
S53、獲得相似變換參數s,R,t后,求出ORB-SLAM生成的環境點云內任意一個點在2D占據柵格坐標系中的對應位置,具體的轉換公式為:
p′=s(Rp+t) (8)
其中,p′是柵格地圖坐標系中的某個點,p是語義地標坐標系中的某個點;
S54、通過變換關系得到3D語義地表和2D柵格地圖的對應關系,將3D語義地表和2D柵格地圖合并為一個語義地圖。
2.根據權利要求1所述的一種基于軌跡對齊的語義建圖方法,其特征在于,所述步驟S1中,RGB數據、激光數據、IMU數據和里程計數據同步開始采集,采集時各個傳感器以一個整體的狀態進行移動。
該專利技術資料僅供研究查看技術是否侵權等信息,商用須獲得專利權人授權。該專利全部權利屬于杭州自適應科技有限公司,未經杭州自適應科技有限公司許可,擅自商用是侵權行為。如果您想購買此專利、獲得商業授權和技術合作,請聯系【客服】
本文鏈接:http://www.17sss.com.cn/pat/books/202011644778.8/1.html,轉載請聲明來源鉆瓜專利網。
- 上一篇:錐套及包括其的輥箱
- 下一篇:一種具有伸縮特性的氣動變剛度柔性操作臂





