[發(fā)明專利]一種基于激光和二維碼融合的即時(shí)定位與地圖構(gòu)建方法

有效

• 申請(qǐng)?zhí)枺?/td>: 201811110672.2
• 申請(qǐng)日: 2018-09-21
• 公開（公告）號(hào): CN109211251B
• 公開（公告）日: 2022-01-11
• 發(fā)明（設(shè)計(jì)）人: 肖烜;覃梓雨;王晨;劉康妮;劉俐伶
• 申請(qǐng)（專利權(quán)）人: 北京理工大學(xué)
• 主分類號(hào): G01C21/32
• 分類號(hào): G01C21/32;G01C21/34
• 代理公司: 北京理工大學(xué)專利中心 11120
• 代理人: 郭德忠;李愛(ài)英

本發(fā)明提供一種基于激光和二維碼融合的即時(shí)定位與地圖構(gòu)建方法，將里程計(jì)的行程與速度信息、單目相機(jī)采集的二維碼、慣性測(cè)量單元的信息融合為狀態(tài)向量，根據(jù)里程計(jì)的觀測(cè)模型、慣性測(cè)量單元的觀測(cè)模型以及單目相機(jī)的觀測(cè)模型構(gòu)建聯(lián)邦卡爾曼濾波器的觀測(cè)方程，在傳統(tǒng)的里程計(jì)與慣性測(cè)量單元的基礎(chǔ)上加入二維碼定位信息，從而得到無(wú)人移動(dòng)平臺(tái)的狀態(tài)向量的最優(yōu)解；再利用二維碼信息處理的快速性和精確性的特點(diǎn)，形成精確度較高的位姿預(yù)測(cè)信息，從而降低粒子濾波即時(shí)定位與地圖構(gòu)建方法中的粒子數(shù)量，由此能夠提高低功率處理器，如無(wú)人移動(dòng)平臺(tái)的定位精度，保證無(wú)人移動(dòng)平臺(tái)在復(fù)雜環(huán)境的定位效果，完成地圖構(gòu)建，滿足復(fù)雜環(huán)境的應(yīng)用需求。

技術(shù)領(lǐng)域

本發(fā)明屬于即時(shí)定位與地圖構(gòu)建技術(shù)領(lǐng)域，尤其涉及一種基于激光和二維碼融合的即時(shí)定位與地圖構(gòu)建方法。

背景技術(shù)

無(wú)人移動(dòng)平臺(tái)，例如自動(dòng)導(dǎo)引運(yùn)輸車(AGV，Automated Guided Vehicle)已經(jīng)普遍的運(yùn)用于多個(gè)行業(yè)，其應(yīng)用環(huán)境也逐漸趨于復(fù)雜化，表現(xiàn)在同一導(dǎo)航范圍內(nèi)可能出現(xiàn)各種不同的場(chǎng)景，如：較大范圍的空曠路面、復(fù)雜排列的貨物架等。同時(shí)，在實(shí)際運(yùn)用中，高速運(yùn)行的工業(yè)自動(dòng)導(dǎo)引運(yùn)輸車常常搭載著低功耗處理器。因此，根據(jù)目前自動(dòng)導(dǎo)引運(yùn)輸車的實(shí)際應(yīng)用需求提出合適的導(dǎo)航技術(shù)，保證其導(dǎo)航效果，是自動(dòng)導(dǎo)引運(yùn)輸車能夠廣泛運(yùn)用的關(guān)鍵。

現(xiàn)有技術(shù)中主有激光即時(shí)定位與地圖構(gòu)建和慣性器件相結(jié)合的大型叉車搬運(yùn)AGV，視覺(jué)傳感器和慣性器件融合的物流機(jī)器人；其中，大型叉車搬運(yùn)AGV利用激光即時(shí)定位與地圖構(gòu)建(SLAM，Simultaneous Localization And Mapping)進(jìn)行地圖構(gòu)建、慣性器件輔助定位完成導(dǎo)航任務(wù)，雖然有效的保證了導(dǎo)航的精度，但是該導(dǎo)航技術(shù)應(yīng)用于空曠或轉(zhuǎn)彎的場(chǎng)合容易導(dǎo)致定位失敗；物流機(jī)器人僅僅通過(guò)識(shí)別二維碼標(biāo)簽完成定位導(dǎo)航，簡(jiǎn)潔的算法雖然保證了物流機(jī)器人高速運(yùn)行時(shí)的定位效果，但是應(yīng)用場(chǎng)合單一，無(wú)法滿足復(fù)雜環(huán)境的應(yīng)用需求。

發(fā)明內(nèi)容

為解決上述問(wèn)題，本發(fā)明提供一種基于激光和二維碼融合的即時(shí)定位與地圖構(gòu)建方法，能夠完成效果較好且精度較高的輕量級(jí)定位與地圖構(gòu)建，滿足復(fù)雜環(huán)境的應(yīng)用需求。

一種基于激光和二維碼融合的即時(shí)定位與地圖構(gòu)建方法，應(yīng)用于無(wú)人移動(dòng)平臺(tái)，所述無(wú)人移動(dòng)平臺(tái)包括處理模塊、單目相機(jī)、激光雷達(dá)、里程計(jì)以及慣性測(cè)量單元；

所述方法包括以下步驟：

S1：所述單目相機(jī)采集地圖待構(gòu)建區(qū)域上預(yù)先鋪設(shè)的二維碼，其中，所述二維碼包含表征二維碼在地圖待構(gòu)建區(qū)域上的坐標(biāo)的ID信息；

S2：所述處理模塊對(duì)二維碼進(jìn)行解碼，得到二維碼在地圖待構(gòu)建區(qū)域上的坐標(biāo)，再根據(jù)二維碼的坐標(biāo)得到無(wú)人移動(dòng)平臺(tái)在世界坐標(biāo)系下的初始全局坐標(biāo)；

S3：將無(wú)人移動(dòng)平臺(tái)在世界坐標(biāo)系下的全局坐標(biāo)、無(wú)人移動(dòng)平臺(tái)的航向角、無(wú)人移動(dòng)平臺(tái)的速度以及無(wú)人移動(dòng)平臺(tái)的角速度作為聯(lián)邦卡爾曼濾波器的狀態(tài)向量；其中，所述全局坐標(biāo)由步驟S2中的初始全局坐標(biāo)以及里程計(jì)測(cè)量的無(wú)人移動(dòng)平臺(tái)的行程與速度確定，無(wú)人移動(dòng)平臺(tái)的航向角與角速度通過(guò)慣性測(cè)量單元獲取，無(wú)人移動(dòng)平臺(tái)的速度由里程計(jì)獲取；

S4：根據(jù)所述狀態(tài)向量構(gòu)建聯(lián)邦卡爾曼濾波器的狀態(tài)方程，根據(jù)里程計(jì)的觀測(cè)模型、慣性測(cè)量單元的觀測(cè)模型以及單目相機(jī)的觀測(cè)模型構(gòu)建聯(lián)邦卡爾曼濾波器的觀測(cè)方程；

S5：根據(jù)聯(lián)邦卡爾曼濾波算法對(duì)所述狀態(tài)方程與觀測(cè)方程進(jìn)行求解，得到所述狀態(tài)向量的最優(yōu)解；

S6：構(gòu)建粒子濾波即時(shí)定位與地圖構(gòu)建算法的運(yùn)動(dòng)狀態(tài)轉(zhuǎn)移方程與運(yùn)動(dòng)觀測(cè)方程，其中，當(dāng)無(wú)人移動(dòng)平臺(tái)的速度小于設(shè)定閾值、無(wú)轉(zhuǎn)彎或者運(yùn)動(dòng)路徑上存在障礙物時(shí)，所述運(yùn)動(dòng)狀態(tài)轉(zhuǎn)移方程的控制向量為激光雷達(dá)對(duì)地圖待構(gòu)建區(qū)域進(jìn)行特征匹配得到的位姿，當(dāng)無(wú)人移動(dòng)平臺(tái)的速度不小于設(shè)定閾值、有轉(zhuǎn)彎或者運(yùn)動(dòng)路徑上不存在障礙物時(shí)，所述運(yùn)動(dòng)狀態(tài)轉(zhuǎn)移方程的控制向量為所述狀態(tài)向量的最優(yōu)解中包含的位姿；其中，所述位姿為所述全局坐標(biāo)與無(wú)人移動(dòng)平臺(tái)的航向角；