[發(fā)明專(zhuān)利]基于視覺(jué)與IMU或里程計(jì)的AGV組合導(dǎo)航定位方法

有效

• 申請(qǐng)?zhí)枺?/td>: 201910648140.2
• 申請(qǐng)日: 2019-07-18
• 公開(kāi)（公告）號(hào): CN110308729B
• 公開(kāi)（公告）日: 2022-05-10
• 發(fā)明（設(shè)計(jì)）人: 杜惠斌;劉靜怡;李中勝;張榮山;李財(cái)盛;李江楠
• 申請(qǐng)（專(zhuān)利權(quán)）人: 石家莊辰宙智能裝備有限公司
• 主分類(lèi)號(hào): G05D1/02
• 分類(lèi)號(hào): G05D1/02;G01C21/00;G01C21/20
• 代理公司: 石家莊眾志華清知識(shí)產(chǎn)權(quán)事務(wù)所(特殊普通合伙) 13123
• 代理人: 高寧寧

1.一種基于視覺(jué)與IMU或里程計(jì)的AGV組合導(dǎo)航定位方法，基于具有規(guī)則參照物的地面，其特征在于，包括以下步驟：

步驟A、建立坐標(biāo)系：以地面任意一點(diǎn)為原點(diǎn)建立地面平面直角坐標(biāo)系，并在地面上粘貼具有位姿信息的二維碼標(biāo)識(shí)；

步驟B、設(shè)置配置文件：將規(guī)則參照物的尺寸信息及二維碼位姿信息輸入至AGV的控制器配置文件，AGV控制器根據(jù)該配置文件自動(dòng)生成網(wǎng)格地圖；

步驟C、AGV初始化：AGV上電后手動(dòng)操控其經(jīng)過(guò)二維碼標(biāo)識(shí)，視覺(jué)相機(jī)掃描二維碼標(biāo)識(shí)并通過(guò)控制器計(jì)算得到AGV在地面平面直角坐標(biāo)系中的絕對(duì)位置完成AGV初始化；

步驟D、組合導(dǎo)航定位：AGV在初始化完成后的運(yùn)動(dòng)過(guò)程中，視覺(jué)相機(jī)采集參照物信息，控制器將采集到的參照物信息利用平面幾何關(guān)系求解此時(shí)AGV位姿數(shù)據(jù)、并將得到的AGV位姿數(shù)據(jù)與IMU或里程計(jì)數(shù)據(jù)通過(guò)卡爾曼濾波算法進(jìn)行融合，完成組合導(dǎo)航定位；

在步驟D中控制器將采集到的參照物信息利用平面幾何關(guān)系求解此時(shí)AGV位姿數(shù)據(jù)包括以下步驟：

S1：讀取視覺(jué)相機(jī)采集到的地面圖像；

S2：經(jīng)過(guò)灰度化、去噪、增強(qiáng)等處理后檢測(cè)圖像中的直線，如果檢測(cè)到直線存在，則通過(guò)相機(jī)內(nèi)參單應(yīng)矩陣H求出圖像中所有直線在視覺(jué)相機(jī)坐標(biāo)系{C}下的方程{L_i}；

S3：由IMU或里程計(jì)數(shù)據(jù)確定網(wǎng)格地圖中網(wǎng)格線的直線族{M}＝{LX}∪{LY}中位于視覺(jué)相機(jī)視野內(nèi)且距離其鏡頭原點(diǎn)最近的兩條直線{Lx}和{Ly}；

S4：求解所有直線方程{L_i}在視覺(jué)相機(jī)鏡頭坐標(biāo)系{C}下的斜率θ_i與截距d_i以及直線{Lx}和{Ly}在視覺(jué)相機(jī)鏡頭坐標(biāo)系{C}下的斜率θ_x、θ_y與截距d_x、d_y；

S5：按照設(shè)定的閾值θ_t和d_t將所有直線分成兩組并排除錯(cuò)誤直線，得到兩組分別對(duì)應(yīng)于{Lx}和{Ly}的檢測(cè)直線{Lx_i|i＝1,2,…,m}和{Ly_i|i＝1,2,…,n}，其中，L_i∈{Lx_i}，if|θ_i-θ_x|≤θ_t∩|d_i-d_x|≤d_t

L_i∈{Ly_i}，if|θ_i-θ_y|≤θ_t∩|d_i-d_y|≤d_t；

S6：對(duì)每個(gè)方向上檢測(cè)出的直線以斜率差和截距差的加權(quán)和s_i分配每條直線的權(quán)重q_i：

s_i＝p×|θ_i-θ_x|+(1-p)×|d_i-d_x|

S7：對(duì)每個(gè)方向上檢測(cè)出的直線以斜率的加權(quán)均值和截距的加權(quán)均值作為該方向上最終的檢測(cè)直線{Lx_d}和{Ly_d}；

S8：由齊次變換關(guān)系計(jì)算出由視覺(jué)相機(jī)確定的AGV當(dāng)前位姿觀測(cè)值其中，^AT_C為相機(jī){C}在車(chē)體{A}上的安裝位姿。