基于傳感器信息融合的AGV路徑跟蹤技術(shù)設(shè)計(jì)與應(yīng)用

引言

自1972年NDC公司為瑞典沃爾沃公司創(chuàng)建了全球第一家使用AGV的汽車(chē)生產(chǎn)廠以來(lái),AGV技術(shù)已經(jīng)歷了幾十年的發(fā)展。AGV是一種涉及了多門(mén)學(xué)科技術(shù)的智能輪式移動(dòng)機(jī)器人[1],隨著計(jì)算機(jī)及相關(guān)工業(yè)技術(shù)的發(fā)展,很多低成本、高可靠性的技術(shù)不斷應(yīng)用于AGV,比如單片機(jī)控制、圖像識(shí)別、磁條引導(dǎo)、激光雷達(dá)等,使得AGV在柔性制造業(yè)、物流業(yè)和倉(cāng)儲(chǔ)物流領(lǐng)域具備了廣泛的應(yīng)用前景和價(jià)值[2]。應(yīng)用于眾多領(lǐng)域,AGV的行駛路徑更加多樣化,運(yùn)行工況也更加復(fù)雜,傳統(tǒng)的單一固定路徑行駛已經(jīng)無(wú)法滿足要求。因此,需要應(yīng)用多傳感器信息融合技術(shù),利用AGV上的多種傳感器進(jìn)行數(shù)據(jù)采集,并融合得到任意區(qū)域的實(shí)時(shí)位置和姿態(tài)數(shù)據(jù),針對(duì)不同的路徑采取不同的控制策略進(jìn)行路徑跟蹤,以最大限度實(shí)現(xiàn)AGV的柔性運(yùn)行。多傳感器信息融合可以消除噪聲干擾,同時(shí)對(duì)AGV自身的位置和姿態(tài)進(jìn)行描述,減少了由于某一個(gè)傳感器測(cè)量數(shù)據(jù)不穩(wěn)定給位姿估算帶來(lái)的影響。通過(guò)多傳感器的應(yīng)用,可降低對(duì)單個(gè)傳感器的性能要求,AGV的應(yīng)用成本也大大降低。

本文主要從基于圖像識(shí)別的多傳感器融合實(shí)現(xiàn)AGV的位置和姿態(tài)估算出發(fā),提出AGV在各種行駛路徑下的軌跡跟蹤及控制技術(shù),并給出其在實(shí)際項(xiàng)目中的運(yùn)行效果。

1多傳感器信息融合的位姿估算

1.1AGV控制系統(tǒng)硬件構(gòu)成

本文中的AGV是基于地面二維碼圖像識(shí)別的兩輪差速搬運(yùn)機(jī)器人,核心控制器是基于TMS320F28335的DSP控制電路板。車(chē)輛行駛方向的面板內(nèi)嵌SICK迷你型激光雷達(dá),用于安全避障及特殊工況的機(jī)機(jī)協(xié)同控制。整體硬件分布及設(shè)備外形如圖1所示,圖像識(shí)別單元如圖2所示。系統(tǒng)主要涉及CAN、UART、SPI、TCP/UDP通信,實(shí)現(xiàn)和設(shè)備之間的連接。

1.2多傳感器信息融合方法

多傳感器信息融合也稱(chēng)為信息融合,多傳感器的信息一般來(lái)自不同的層次,如時(shí)間、工況等,按照一定的規(guī)則和要求進(jìn)行信息處理,最終實(shí)現(xiàn)對(duì)所測(cè)信息統(tǒng)一的定義和說(shuō)明,然后進(jìn)行評(píng)估和決定,就可以得到更準(zhǔn)確的信息。通過(guò)融合算法對(duì)被觀測(cè)量的各個(gè)傳感器所測(cè)數(shù)據(jù)進(jìn)行融合,就可以對(duì)觀測(cè)量得到一個(gè)總結(jié)性的狀態(tài)描述,所以融合算法具備較強(qiáng)的魯棒性。按信息的抽象程度,信息融合可以劃分為三個(gè)級(jí)別:低層次數(shù)據(jù)層融合、中層次特征層融合和高層次決策層融合。AGV的實(shí)際作業(yè)環(huán)境如圖3所示,由于路面凹凸不平、異物碾壓、加減速打滑等工況,加上運(yùn)行速度快,控制精度要求高,因此本文選用了決策層融合方法。

本文主要是對(duì)電機(jī)編碼器、MEMS陀螺儀、圖像二維碼讀數(shù)進(jìn)行數(shù)據(jù)融合,以提高AGV在路徑跟蹤中的位姿估算及控制精度,如圖4所示。

1.3多信息融合的位姿估算

本文所討論的坐標(biāo)主要有機(jī)器視覺(jué)坐標(biāo)系(C)、AGV坐標(biāo)系(R)、全域坐標(biāo)系(倉(cāng)庫(kù)地面坐標(biāo)系w),如圖5所示。

通過(guò)左右驅(qū)動(dòng)輪的電機(jī)編碼器測(cè)得驅(qū)動(dòng)輪的速度,求解對(duì)應(yīng)的車(chē)輛底盤(pán)的運(yùn)動(dòng)學(xué)狀態(tài)微分方程組,即可得到AGV的位置、角速度及姿態(tài)角度估算值。

式中,小為AGV的姿態(tài)角度:Rl,Rr分別為左右輪半徑:ol,or分別為左右輪轉(zhuǎn)速。

利用單軸陀螺儀直接測(cè)量的AGV旋轉(zhuǎn)角速度與編碼器估算值幾個(gè)不同的運(yùn)行工況加權(quán)濾波,得到更加準(zhǔn)確可靠的旋轉(zhuǎn)角速度。通過(guò)式(1)推算得到位置和角度數(shù)據(jù)后,當(dāng)AGV經(jīng)過(guò)并讀取到地面二維碼時(shí),將二維碼定位數(shù)據(jù)利用公式(2)進(jìn)行坐標(biāo)變換,得到全域坐標(biāo)系下AGV絕對(duì)校正的位姿數(shù)據(jù)。

式中,TEQ \* jc3 \* hps10 \o\al(\s\up 3(為圖像到車(chē)輛的坐標(biāo)系轉(zhuǎn)換矩陣:TEQ \* jc3 \* hps10 \o\al(\s\up 3(為車(chē)輛到全域的坐標(biāo)系轉(zhuǎn)換矩陣。

將式(2)得出的位姿數(shù)據(jù)與估算值按照一定規(guī)則進(jìn)行推導(dǎo)計(jì)算出最優(yōu)的位姿數(shù)據(jù)。這里的規(guī)則是指結(jié)合AGV先驗(yàn)位姿數(shù)據(jù)對(duì)當(dāng)前地面二維碼數(shù)據(jù)進(jìn)行識(shí)別和判斷,并給予對(duì)應(yīng)的權(quán)重進(jìn)行加權(quán)平均。本文以直線移動(dòng)工況為例,如圖6所示。

2路徑跟蹤技術(shù)的實(shí)現(xiàn)

2.1路徑類(lèi)型

本文中的AGV屬于高度柔性的物流搬運(yùn)機(jī)器人,無(wú)固定的行駛路徑,根據(jù)調(diào)度系統(tǒng)的作業(yè)任務(wù)及實(shí)際路況,實(shí)時(shí)動(dòng)態(tài)分配對(duì)應(yīng)的行車(chē)道和軌跡類(lèi)型,完成貨架的搬運(yùn)。行駛路徑類(lèi)型主要包含:原地旋轉(zhuǎn)、直線前進(jìn)、直線后退、90О直角轉(zhuǎn)彎(順時(shí)針、逆時(shí)針)、180ОU型轉(zhuǎn)彎(順時(shí)針、逆時(shí)針),如圖7所示。

2.2軌跡規(guī)劃及跟蹤

直線和圓弧組合是目前AGV真實(shí)作業(yè)場(chǎng)景下比較常用的軌跡規(guī)劃方式,用于實(shí)現(xiàn)車(chē)輛從一條車(chē)道高效轉(zhuǎn)換到另一條車(chē)道。

這種組合方式主要有三方面的優(yōu)勢(shì):(1)無(wú)需減速停車(chē)后轉(zhuǎn)向,提高車(chē)輛通行效率:(2)直線和圓弧相切,行駛軌跡平滑,避免車(chē)輛側(cè)滑:(3)減少車(chē)輛的啟停動(dòng)作,降低能耗。但由于本文的AGV是兩輪差速底盤(pán),不具備舵輪轉(zhuǎn)向的特性,因此弧線軌跡必須要考慮差速輪的轉(zhuǎn)彎特性進(jìn)行平滑處理。這里主要通過(guò)預(yù)先規(guī)劃出AGV的姿態(tài)角度一旋轉(zhuǎn)速度曲線,結(jié)合式(1)推算出對(duì)應(yīng)的位置移動(dòng)軌跡。根據(jù)既定的軌跡類(lèi)型,在行駛過(guò)程中AGV主要完成移動(dòng)和轉(zhuǎn)向兩個(gè)基本動(dòng)作。為了減少力矩劇烈變化對(duì)電機(jī)的沖擊,使得驅(qū)動(dòng)系統(tǒng)更加平穩(wěn),需要對(duì)電機(jī)的加速度進(jìn)行平滑處理。

本文采用了3階速度曲線規(guī)劃,以直線移動(dòng)工況為例,在執(zhí)行移動(dòng)和轉(zhuǎn)向動(dòng)作的過(guò)程中按照6個(gè)階段一加加速、減加速、勻速、加減速、減減速及滑行停車(chē)進(jìn)行速度和位置規(guī)劃,如圖8所示。

AGV在移動(dòng)過(guò)程中,根據(jù)規(guī)劃好的上述軌跡,以速度曲線作為路徑跟蹤中的前饋控制,位置曲線進(jìn)行閉環(huán)控制,實(shí)現(xiàn)行駛路徑縱向位置的跟蹤。利用經(jīng)過(guò)信息融合的位置和姿態(tài)信息,分別計(jì)算得到角度和橫向位置控制誤差A(yù)9和Ad,如圖9所示,實(shí)現(xiàn)路徑橫向位置及姿態(tài)角度的跟蹤控制。將計(jì)算得到的位置和姿態(tài)角度控制誤差相互補(bǔ)償,利用經(jīng)典PID控制算法輸出對(duì)應(yīng)的最終旋轉(zhuǎn)角速度控制量,并將其轉(zhuǎn)化為車(chē)輛旋轉(zhuǎn)角速度控制誤差值,利用式(1)計(jì)算得到左右輪對(duì)應(yīng)的控制速度,從而實(shí)現(xiàn)車(chē)輛的路徑跟蹤控制。

2.3軌跡跟蹤實(shí)際效果分析

收集AGV在某個(gè)商用項(xiàng)目(圖10、圖11)中的運(yùn)行數(shù)據(jù),并對(duì)其中的車(chē)輛實(shí)際位置曲線和路徑跟蹤偏差數(shù)據(jù)進(jìn)行分析,如圖12所示。

圖12(a)(b)分別為左右輪的速度控制曲線,結(jié)果顯示在車(chē)輛加減速過(guò)程中速度變化趨勢(shì)平緩,且現(xiàn)場(chǎng)貨架搬運(yùn)過(guò)程中視覺(jué)上也沒(méi)有明顯的前后晃動(dòng):

圖12(c)為車(chē)輛在直線移動(dòng)過(guò)程中的路徑跟蹤橫向位置誤差變化曲線,結(jié)果顯示誤差均值<1cm,控制精度較高,且穩(wěn)定性強(qiáng):

圖12(d)為車(chē)輛在轉(zhuǎn)向及移動(dòng)過(guò)程中的姿態(tài)角度變化曲線,結(jié)果顯示在直線行駛過(guò)程中姿態(tài)角度始終穩(wěn)定在89.5o~90.5o之間。

3結(jié)語(yǔ)

綜上,本文的多傳感器信息融合和路徑跟蹤控制技術(shù)在項(xiàng)目中取得了良好的應(yīng)用效果,為提高AGV的路徑跟蹤控制精度和運(yùn)行穩(wěn)定性提供了技術(shù)支撐,確保了設(shè)備順利交付,用于客戶倉(cāng)庫(kù)自動(dòng)化日常分揀作業(yè)。