[發(fā)明專利]一種基于視覺地圖的視覺定位方法、裝置

有效

• 申請?zhí)枺?/td>: 202010620223.3
• 申請日: 2020-06-30
• 公開（公告）號: CN111780764B
• 公開（公告）日: 2022-09-02
• 發(fā)明（設(shè)計）人: 龍學雄;易雨亭;李建禹
• 申請（專利權(quán)）人: 杭州?？禉C器人技術(shù)有限公司
• 主分類號: G01C21/20
• 分類號: G01C21/20
• 代理公司: 北京德琦知識產(chǎn)權(quán)代理有限公司 11018
• 代理人: 謝安昆;宋志強

1.一種基于視覺地圖的視覺定位方法，其特征在于，該方法包括，

采集當前圖像，得到當前幀；

提取當前幀的特征點，

將當前幀的特征點與地圖中的地圖點進行匹配，得到匹配成功的第一匹配特征點，

將當前幀增加至用于位姿圖優(yōu)化的約束幀集合中，基于第一匹配特征點與該第一匹配特征點匹配的第一匹配地圖點所構(gòu)成的地圖匹配約束，利用當前約束幀集合，對第一目標函數(shù)進行當前幀位姿優(yōu)化，得到優(yōu)化后的位姿，作為定位結(jié)果，并優(yōu)化當前約束幀集合，以作為下一幀位姿優(yōu)化的約束幀集合；

其中，

地圖匹配約束為：第一匹配地圖點反投影到當前幀上的像素位置與當前幀中與該第一匹配地圖點匹配的第一匹配特征點的像素位置之間的誤差，或者，當前幀中的第一匹配特征點投影至世界坐標系中的空間位置與該第一匹配特征點匹配的第一匹配地圖點在世界坐標系中的空間位置之間的誤差；

第一目標函數(shù)為：當前約束幀集合中所有幀的所有第一匹配特征點的地圖匹配誤差之和；

所述優(yōu)化當前約束幀集合包括，

判斷當前幀中第一匹配特征點的數(shù)量是否小于設(shè)定的第一閾值，如果是，則判定當前幀為關(guān)鍵幀，否則，判定為非關(guān)鍵幀，

若當前幀為非關(guān)鍵幀時，則在約束幀集合中刪除該當前幀，

若當前幀為關(guān)鍵幀，則判斷當前約束幀集合中幀數(shù)量是否達到設(shè)定的幀閾值，如果是，則刪除約束幀集合中最早添加的關(guān)鍵幀，否則，則不刪除約束幀集合中最早添加的關(guān)鍵幀。

2.如權(quán)利要求1所述的方法，其特征在于，所述基于第一匹配特征點與該第一匹配特征點匹配的第一匹配地圖點所構(gòu)成的約束，對當前幀位姿進行優(yōu)化，得到優(yōu)化后的位姿，作為定位結(jié)果，進一步包括，

基于地圖匹配約束、以及至少幀間匹配約束、慣性約束之一，對當前幀位姿進行優(yōu)化，得到優(yōu)化后的位姿，作為定位結(jié)果，

其中，

幀間匹配約束為：當前幀中的第一匹配特征點投影至世界坐標系中的空間位置與當前幀的上一關(guān)鍵幀中與該第一匹配特征點匹配的第二匹配特征點投影至世界坐標系中的空間位置之間的誤差，或者，與第二匹配特征點匹配的第二匹配地圖點反投影到當前幀的像素位置與該第二匹配地圖點反投影到上一關(guān)鍵幀的像素位置之間的誤差；

慣性約束為：根據(jù)當前里程計數(shù)據(jù)和上一關(guān)鍵幀的里程計數(shù)據(jù)所得到的幀間里程計相對位姿與根據(jù)當前幀位姿和上一關(guān)鍵幀位姿所得到的幀間相對位姿之間的誤差。