[發(fā)明專利]基于視覺和慣性里程定位的方法及裝置

在審

• 申請?zhí)枺?/td>: 202010543589.5
• 申請日: 2020-06-15
• 公開（公告）號: CN111797906A
• 公開（公告）日: 2020-10-20
• 發(fā)明（設(shè)計）人: 韓天思;智向陽;聶瓊;申浩
• 申請（專利權(quán)）人: 北京三快在線科技有限公司
• 主分類號: G06K9/62
• 分類號: G06K9/62;G06F17/16
• 代理公司: 北京曼威知識產(chǎn)權(quán)代理有限公司 11709
• 代理人: 方志煒

1.一種基于視覺以及慣性里程定位的方法，其特征在于，包括：

獲取當前幀的圖像以及慣性測量數(shù)據(jù)；

確定所述當前幀的圖像與上一幀的圖像的圖像特征點的匹配結(jié)果；

根據(jù)所述匹配結(jié)果以及所述慣性測量數(shù)據(jù)，確定無人車在當前幀與上一幀之間的預(yù)估位姿變化；

當所述預(yù)估位姿變化小于預(yù)設(shè)值時，確定當前幀的待解的狀態(tài)向量，根據(jù)基于先驗殘差項、視覺殘差項以及慣性傳感器殘差項確定的目標函數(shù)以及所述待解的狀態(tài)向量，確定所述待解的狀態(tài)向量的變化矩陣、待解系數(shù)矩陣以及待解殘差矩陣的矩陣關(guān)系式，其中，狀態(tài)向量包含環(huán)境點向量以及位姿向量，環(huán)境點向量包含各圖像特征點對應(yīng)的環(huán)境點的待解坐標；

根據(jù)所述矩陣關(guān)系式，確定所述狀態(tài)向量的變化矩陣中的環(huán)境點向量的變化量求解式，對所述求解式進行展開，得到展開后的低階項和高階項，并根據(jù)將所述求解式展開后的低階項解算所述環(huán)境點向量的變化量；

根據(jù)所述矩陣關(guān)系式和解算得到的環(huán)境點向量的變化量，解算所述位姿向量的變化量；

根據(jù)解算得到所述環(huán)境點向量的變化量、所述位姿向量的變化量以及上一幀的狀態(tài)向量，確定當前幀的位姿，以供所述無人車確定控制策略。

2.如權(quán)利要求1所述的方法，其特征在于，確定當前幀的待解的狀態(tài)向量，具體包括：

根據(jù)當前行駛過程中每幀采集的圖像，確定各圖像的圖像特征點對應(yīng)的環(huán)境點的四維齊次坐標；

根據(jù)確定出的各環(huán)境點的四維齊次坐標，確定待解環(huán)境點向量；

確定所述當前幀相對于上一幀的待解位姿向量；

根據(jù)所述待解位姿向量以及所述待解環(huán)境點向量，確定當前幀的待解的狀態(tài)向量。

3.如權(quán)利要求2所述的方法，其特征在于，確定所述當前幀相對于上一幀的待解位姿向量，具體包括：

根據(jù)所述當前幀相對于上一幀的三維位移矢量、三維速度矢量、四維旋轉(zhuǎn)四元數(shù)、慣性測量數(shù)據(jù)中加速度三維零偏矢量以及慣性測量數(shù)據(jù)中角速度三維零偏矢量，確定待解位姿向量。

4.如權(quán)利要求1所述的方法，其特征在于，根據(jù)所述匹配結(jié)果以及所述慣性測量數(shù)據(jù)，確定所述前端在當前幀與上一幀之間的預(yù)估位姿變化，具體包括：

根據(jù)所述匹配結(jié)果，確定當前幀的圖像與上一幀圖像中匹配的各圖像特征點；

確定各圖像特征點在所述兩幀圖像中的位置變化；

確定所述慣性測量數(shù)據(jù)在各坐標上的積分結(jié)果；

根據(jù)確定出的各圖像特征點的位置變化、各圖像特征點的匹配結(jié)果以及所述積分結(jié)果，確定預(yù)估位姿變化。

5.如權(quán)利要求3所述的方法，其特征在于，基于先驗殘差項、視覺殘差項以及服從慣性傳感器殘差項確定的目標函數(shù)，具體包括：

根據(jù)當前行駛過程中歷史上各幀已經(jīng)解算出坐標的各環(huán)境點，以及所述待解的狀態(tài)向量中的環(huán)境點向量，確定先驗殘差項；

針對所述當前幀的圖像的每個圖像特征點，根據(jù)采集圖像設(shè)備的內(nèi)參矩陣、所述待解的狀態(tài)向量中的位姿向量的旋轉(zhuǎn)四元數(shù)以及待解的狀態(tài)向量中的該圖像特征點對應(yīng)環(huán)境點的坐標，確定該圖像特征點在圖像中待解的三維齊次坐標，并根據(jù)該圖像特征點在圖像中實際的三維其次坐標與所述待解的三維齊次坐標，確定該圖像特征點的視覺殘差項；

針對所述待解位姿向量中的三維位移矢量、所述三維速度矢量、所述四維旋轉(zhuǎn)四元數(shù)、所述加速度三維零偏矢量以及所述角速度三維零偏矢量，根據(jù)上一幀解算得到的結(jié)果，確定慣性傳感器殘差項；

基于先驗殘差項、視覺殘差項以及慣性傳感器殘差項，確定使各殘差項之和最小的待解的狀態(tài)向量，作為目標函數(shù)。

6.如權(quán)利要求1所述的方法，其特征在于，根據(jù)所述矩陣關(guān)系式，確定所述狀態(tài)向量的變化矩陣中的環(huán)境點向量的變化量求解式，具體包括：

將所述矩陣關(guān)系式中的所述待解系數(shù)矩陣展開為上一幀的系數(shù)矩陣與系數(shù)矩陣變化量，所述待解殘差矩陣展開為上一幀的殘差矩陣與殘差矩陣變化量；

對展開后的矩陣關(guān)系式進行舒爾補消元，確定消元后所述狀態(tài)向量的變化矩陣中的環(huán)境點向量的變化量求解式。