[發(fā)明專(zhuān)利]一種基于地標(biāo)識(shí)別的AGV導(dǎo)航定位方法

有效

• 申請(qǐng)?zhí)枺?/td>: 202110283867.2
• 申請(qǐng)日: 2021-03-17
• 公開(kāi)（公告）號(hào): CN113074729B
• 公開(kāi)（公告）日: 2022-09-13
• 發(fā)明（設(shè)計(jì)）人: 李瀧杲;黃翔;黃智超;李根;曾琪;齊凱;陶克梅
• 申請(qǐng)（專(zhuān)利權(quán)）人: 南京航空航天大學(xué)
• 主分類(lèi)號(hào): G01C21/20
• 分類(lèi)號(hào): G01C21/20;G01C21/34
• 代理公司: 南京天華專(zhuān)利代理有限責(zé)任公司 32218
• 代理人: 瞿網(wǎng)蘭

1.一種基于地標(biāo)識(shí)別的AGV導(dǎo)航定位方法，其特征在于，包括粗定位和精定位，粗定位實(shí)現(xiàn)基于視覺(jué)地標(biāo)解算的路徑規(guī)劃，精定位實(shí)現(xiàn)基于視覺(jué)地標(biāo)的精定位調(diào)姿量解算；所述基于視覺(jué)地標(biāo)解算的路徑規(guī)劃包括：

(1)基于地標(biāo)解算的精定位與粗定位分界方式；

將路徑規(guī)劃過(guò)程分為精定位與粗定位階段：粗定位導(dǎo)航過(guò)程為初始站位導(dǎo)航至分界地標(biāo)的過(guò)程；精定位導(dǎo)航過(guò)程為，粗定位真實(shí)駛達(dá)分解地標(biāo)站位導(dǎo)航至目標(biāo)站位的過(guò)程；距離目標(biāo)站位最近的視覺(jué)地標(biāo)為分界地標(biāo)；記建立的視覺(jué)地標(biāo)位姿索引庫(kù)中，地標(biāo)位姿為N_tag表示地標(biāo)的數(shù)量，M表示地標(biāo)位姿的集合，地標(biāo)標(biāo)號(hào)i的位姿為M_i；記目標(biāo)站位的位姿為T(mén)_Goal，D_i為標(biāo)號(hào)i的地標(biāo)與目標(biāo)站位的位姿關(guān)系，則地標(biāo)與站位的位姿關(guān)系表示為：

D_i＝M_i-T_Goal,i∈{1，2,3…，N_tag} (1)

記g為分界地標(biāo)序號(hào)，||D_g||為分界地標(biāo)與站位的最小二范數(shù)，則可得：

||D_g||＝||D_i||_min,i∈{1，2,3…，N_tag} (2)；

(2)基于AGV相關(guān)不確定度與地標(biāo)解算的導(dǎo)航分段方法：

路徑規(guī)劃其實(shí)是一個(gè)動(dòng)態(tài)的過(guò)程，在循跡過(guò)程中常需重新規(guī)劃路線(xiàn)，重新規(guī)劃路線(xiàn)時(shí)以當(dāng)前位姿為初始站位，故當(dāng)時(shí)位姿精度水平，影響AGV路徑規(guī)劃算法的魯棒性；為提高循跡過(guò)程中AGV運(yùn)動(dòng)精度，提高循跡可靠性，結(jié)合解算地標(biāo)校準(zhǔn)AGV位姿方法，在粗定位導(dǎo)航階段的路徑規(guī)劃中，設(shè)計(jì)基于AGV相關(guān)不確定度與地標(biāo)解算的分段導(dǎo)航方法；

粗定位導(dǎo)航路徑中，因地標(biāo)測(cè)量不確定度影響AGV運(yùn)動(dòng)控制的精度，為此，分別測(cè)算如下不確定度：分界地標(biāo)位姿測(cè)量不確定度σ_Mg、路徑規(guī)劃坐標(biāo)系與運(yùn)行環(huán)境坐標(biāo)系耦合不確定度σ_T、AGV所載視覺(jué)傳感器解算視覺(jué)地標(biāo)不確定度σ_C以及運(yùn)動(dòng)控制不確定度σ_m，因此，初始點(diǎn)至地標(biāo)g不確定度匯總表述為∑σ_g：

式中，σ_m與AGV實(shí)際運(yùn)動(dòng)距離密切相關(guān)，也是AGV運(yùn)動(dòng)誤差的主要來(lái)源，考慮不確定度極值，運(yùn)動(dòng)距離d_m、運(yùn)動(dòng)單位距離的不確定度定義為σ_u，運(yùn)動(dòng)不確定度可表述為σ_m＝σ_u·d_m；f表示函數(shù)關(guān)系，為保證不確定度計(jì)算的魯棒性，在無(wú)法獲得不確定度具體函數(shù)關(guān)系前提下，考慮不確定度單向極限偏差；

相機(jī)視距d_F與地標(biāo)尺寸d_M同樣影響分段規(guī)劃次數(shù)N，結(jié)合不確定度分析可得如下關(guān)系：

從而得到分段規(guī)劃次數(shù)N：

(3)分段導(dǎo)航規(guī)劃中分段地標(biāo)的選取方法：

分段的參考距離d_Seg為：

式中，d(T_Start,M_g)表示T_Start與M_g之間的曼哈頓距離；

記選取的N個(gè)地標(biāo)標(biāo)號(hào)分別為i₁,i₂,…,i_N-1,g，則i_j地標(biāo)的選取方法為：

(4)提升AGV運(yùn)行穩(wěn)定性的基于地標(biāo)的路徑規(guī)劃算法：

各分段中，設(shè)計(jì)有初始站位至目標(biāo)站位的規(guī)劃；路徑規(guī)劃算法中，為提高AGV轉(zhuǎn)站運(yùn)動(dòng)平穩(wěn)性減少AGV運(yùn)動(dòng)方向突變，引入拓展方向概念；柵格地圖的路徑搜索中，節(jié)點(diǎn)n-1拓展方向表示為D_n-1,n，則路徑搜索過(guò)程中的代價(jià)函數(shù)計(jì)算公式為：

f(n)是節(jié)點(diǎn)n的綜合優(yōu)先級(jí)，h(n)是節(jié)點(diǎn)n距離終點(diǎn)的預(yù)計(jì)代價(jià)；g(n)是節(jié)點(diǎn)距離起點(diǎn)n的代價(jià)，當(dāng)拓展節(jié)點(diǎn)的拓展方向與上一結(jié)點(diǎn)相同時(shí)，該拓展節(jié)點(diǎn)曼哈頓距離值減小為實(shí)際的k_same倍，當(dāng)拓展節(jié)點(diǎn)與上一拓展節(jié)點(diǎn)不同時(shí)，該拓展節(jié)點(diǎn)曼哈頓距離值增大為實(shí)際距離的k_diff倍；當(dāng)選擇下一個(gè)遍歷節(jié)點(diǎn)時(shí)，總會(huì)選取f(n)最小的節(jié)點(diǎn)，距離采用曼哈頓距離；依次計(jì)算搜索節(jié)點(diǎn)n,即可規(guī)劃處初始站位至目標(biāo)站位的路徑；

所述基于視覺(jué)地標(biāo)的精定位調(diào)姿量解算包括如下內(nèi)容：

粗定位導(dǎo)航過(guò)程完成后，即為精定位導(dǎo)航過(guò)程，精定位導(dǎo)航的核心為解算精定位調(diào)姿量；

分界地標(biāo)的位姿為M_g＝(x_g,y_g,θ_g)，記其對(duì)應(yīng)的位姿矩陣為

則AGV識(shí)別地標(biāo)解算的自身位姿為坐標(biāo)為式中下標(biāo)(0,2)表示第0行第2列：

目標(biāo)站位位姿為T(mén)_Goal，識(shí)別地標(biāo)時(shí)刻的AGV與理論目標(biāo)站位的偏差為Δp：

精定位調(diào)姿過(guò)程，調(diào)姿量為偏差ΔT與里程計(jì)位姿的差值，其中里程計(jì)位姿記錄為：

調(diào)姿量為：

x方向、y方向以及θ角度方向的調(diào)姿裕度為：

精定位調(diào)姿結(jié)束判定條件為：