聯(lián)合D*Lite算法與視覺的AGV路徑規(guī)劃與定位

周祥明，陳員義，徐華銀

(1.江西師范高等專科學(xué)校 航空旅游學(xué)院，江西 鷹潭 335000；2.江西師范高等專科學(xué)校 物聯(lián)網(wǎng)學(xué)院，江西 鷹潭 335000；3.江西師范大學(xué) 教務(wù)處，南昌 330000)

隨著現(xiàn)代工業(yè)生產(chǎn)自動化進(jìn)程的快速發(fā)展，智能物流運(yùn)輸、包裝系統(tǒng)已經(jīng)逐漸成為自動化進(jìn)程的一個重要支撐點(diǎn)。自動導(dǎo)航車(Automated Guided Vehicle,AGV)已經(jīng)廣泛應(yīng)用于各種柔性制造系統(tǒng)、智能倉儲物流系統(tǒng)、智能碼垛搬運(yùn)系統(tǒng)等。路徑的自主規(guī)劃與后期定位精度是AGV領(lǐng)域的研究熱點(diǎn)。

文獻(xiàn)[1]提出一種動態(tài)時間窗的最優(yōu)路徑規(guī)劃方法，以分時利用策率結(jié)合迪杰斯特拉(Dijkstra)算法和時間窗法，首先根據(jù)Dijkstra算法按優(yōu)先級高低對AGV運(yùn)行任務(wù)規(guī)劃最短路徑，然后以時間窗法解決運(yùn)行過程中的路徑?jīng)_突、碰撞等問題，整個系統(tǒng)的魯棒性和柔性都比較好；但是，并沒有考慮Dijkstra算法效率低、運(yùn)算過程中占用空間大等缺陷，所系統(tǒng)的實(shí)時性較差。文獻(xiàn)[2]提出一種F-D*Lite算法，相對于傳統(tǒng)D*Lite算法路徑重規(guī)劃效率更高，但是對于多障礙情況路徑規(guī)劃效果依然不理想。文獻(xiàn)[3]提出一種融合粒子群和遺傳算法的混合自適應(yīng)路徑規(guī)劃算法，該算法具有較強(qiáng)的全局搜索能力和較高的收斂精度，但是計算量比較大。以上3種方法都只考慮AGV的路徑規(guī)劃問題，并沒有涉及到后期的AGV路徑糾偏問題。

針對以上問題，本文提出一種基于改進(jìn)的D*Lite量子風(fēng)驅(qū)動路徑規(guī)劃算法的AGV控制系統(tǒng)。在原有D*Lite遺傳算法[4]的基礎(chǔ)上融入量子風(fēng)驅(qū)動理論，在適應(yīng)度函數(shù)中加入碰撞系數(shù)，使用對角線函數(shù)取代傳統(tǒng)的歐幾里得距離，在風(fēng)驅(qū)動(WDO)算法的基礎(chǔ)上使用量子行為來解決優(yōu)化問題。通過仿真實(shí)驗結(jié)果可以看出：改進(jìn)的D*Lite量子風(fēng)驅(qū)動優(yōu)化算法在路徑安全性和路徑長度上有了明顯的改善，算法不會存在局部最優(yōu)的問題，提高了算法路徑搜索效率，并且能規(guī)劃出一條流暢的最優(yōu)路徑。最后，引入一種自定義標(biāo)識符定位法實(shí)現(xiàn)AGV精確的位姿糾偏。

1 D*Lite路徑規(guī)劃

機(jī)器人在保證路徑最優(yōu)的同時也要保證其運(yùn)動過程的安全性，因此本文在適應(yīng)度函數(shù)中加入了碰撞檢測來降低障礙物周圍元胞被選中的概率，碰撞系數(shù)設(shè)置如圖1所示。

圖1 碰撞系數(shù)設(shè)置

用o(x,y)表示障礙物，p(x,y)和q(x,y)代表元胞[4-5]。則碰撞系數(shù)公式如下：

(1)

g′(k)=φ(o1,k)+φ(o2,k)+…+φ(on,k)

(2)

其中，φ(o,k)表示碰撞系數(shù)，k表示通路中的元胞，Sk=+∞表示元胞k是自由通路。o1,o2,…,on表示k周圍的障礙物，g′(k)為k的碰撞系數(shù)[5-6]。

2 融合風(fēng)驅(qū)動優(yōu)化算法

2.1 量子比特

在量子計算中，最小的量子信息單元是量子比特，量子比特與傳統(tǒng)的比特相比不僅可以處于“0”或“1”狀態(tài)，而且還能處于“0”和“1”之間，一個量子比特的狀態(tài)可以描述為[7-8]：

|Ψ〉=α|0〉+β|1〉

(3)

式中：α和β是一對復(fù)數(shù)，表示量子比特的概率振幅，α和β分別表示趨向于“0”和“1”的概率，且滿足|α|+|β|=1，|Ψ〉表示一個量子狀態(tài)。一個n元量子位定義如下：

(4)

2.2 量子風(fēng)驅(qū)動優(yōu)化算法

本文提出了一種新的量子啟發(fā)式算法，即量子風(fēng)驅(qū)動優(yōu)化(QWDO)[9-10]。QWDO使用量子比特的概率振幅來表示粒子的位置。位置的運(yùn)動可以通過量子旋轉(zhuǎn)門策略實(shí)現(xiàn)。位置實(shí)現(xiàn)了量子非門策略的變異[11]。這種操作可以改善種群的多樣性，避免早熟收斂。由于每個量子位都有兩個概率幅度，每個粒子也可以表示優(yōu)化空間的兩個位置。在相同數(shù)量的粒子的情況下，可以加速搜索過程[12-13]。

1)生成初始種群。由于概率振幅滿足方程α2+β2=1，設(shè)α=cosθ,β=sinθ，θ為旋轉(zhuǎn)角，編碼方式如下：

(5)

式中，θij=2π×rand，rand是一個介于0和1之間的隨機(jī)數(shù)，i=1,2,…,m，j=1,2,…,n，m是種群大小，n是空間維度。每個個體對應(yīng)于問題空間的兩個位置，量子態(tài)|0〉和|1〉的概率幅公式如下：

(6)

式中，Pic是一個余弦位置，Pis是一個正弦位置。

2)解空間轉(zhuǎn)換。為了計算種群個體的當(dāng)前位置，需要進(jìn)行解空間的變換。需要將個體的兩個位置從單元空間I=[-1,1]n映射到優(yōu)化問題的解空間。解空間的變量如下：

(7)

3)更新過程。為了避免算法陷入局部最優(yōu)，本文應(yīng)用了兩個量子門策略。利用量子旋轉(zhuǎn)門策略實(shí)現(xiàn)了位置的運(yùn)動，并利用量子非門策略實(shí)現(xiàn)個體的突變。在量子風(fēng)力驅(qū)動優(yōu)化算法中，相位角增量和相位角的更新公式如下：

(8)

式中: Δθij、θij分別是第i個相位角增量和相位角的第j維。采用量子旋轉(zhuǎn)門策略更新概率幅度，公式如下：

(9)

更新后的兩個位置如下：

(10)

采用量子非門策略使種群的位置突變。這種操作可以增加種群的多樣性，避免種群早熟收斂。randj是一個在 0 和 1 之間的隨機(jī)數(shù)。如果randj

(11)

3 自定義定位標(biāo)識符

針對某些特殊應(yīng)用場合，關(guān)鍵工位點(diǎn)停靠精度高的要求，采用視覺二次精定位的方法，提高AGV定位精度和拓寬應(yīng)用領(lǐng)域。

3.1 攝像頭選擇

本文采用凱視佳Falcon2 FA-81-12M1H工業(yè)相機(jī)作為單目精定位采樣模塊。分辨率為4 096×3 072，幀速度為58 fps，彩色成像。其中，幀速度大小對于后期圓形標(biāo)識符采樣效果影響最大，圓形標(biāo)識符采樣時長為0.2 s，所以幀速度不得小于12 fps。

3.2 自定義圖標(biāo)定位原理

基于CANNY邊緣檢測識別圓形標(biāo)識符作為AGV小車的后期定位標(biāo)識符[14-15]。首先對標(biāo)識符信息進(jìn)行高斯濾波處理，去除一些干擾信息，然后Hough圓檢測提取標(biāo)識符輪廓。定位原理如圖2所示。

圖2中，P1(x1,y1)、P2(x2,y2)、P3(x3,y3)分別表示這一幀圖片的中心點(diǎn)坐標(biāo)、自定義標(biāo)識符的中心點(diǎn)坐標(biāo)和糾偏參照點(diǎn)，簡化后分別記為A、B、C三點(diǎn)。這三點(diǎn)構(gòu)成的直角三角形的三條邊的邊長分別記為a、b、c，b和c兩條邊的夾角記為θ，Wx、Wy分別表示當(dāng)前幀圖像的長和寬。

假設(shè)src為當(dāng)前幀圖像，則Wx=src·cols、Wy=src·rows。利用函數(shù)HoughCircles()提取圓形輪廓，進(jìn)而圓心坐標(biāo)P2(x2,y2)=Point(circles[i][0]), cvRound(circles[i][1]))，此時P3(x3,y3)=Point(center·x,srcImage·rows/2)。因為A、B、C三點(diǎn)所構(gòu)成三角形為直角三角形所以可以直接獲得a、b的邊長。根據(jù)余弦定理可以求得偏轉(zhuǎn)角θ，如式(12)所示：

(12)

得到AGV小車具體糾偏數(shù)據(jù)后根據(jù)圖3進(jìn)行位姿精確校正。從圖3中可以看出：根據(jù)a、b值的大小，通過實(shí)驗分析獲得像素值與實(shí)際距離的轉(zhuǎn)換系數(shù)，然后執(zhí)行水平位姿糾偏；根據(jù)偏轉(zhuǎn)角θ的大小執(zhí)行旋轉(zhuǎn)角度的糾偏。

4 實(shí)驗結(jié)果與分析

首先，在MatlabR2014a環(huán)境下，首先針對路徑規(guī)劃的長度、安全距離、轉(zhuǎn)角數(shù)分別對文獻(xiàn)[1-3]及本文方法進(jìn)行實(shí)驗對比；其次，通過柵格地圖模仿現(xiàn)實(shí)障礙物場景測試路徑規(guī)劃效果；最后，通過固定工位點(diǎn)重復(fù)停車測試位置和角度糾偏效果。

4種方法的路徑規(guī)劃結(jié)果如圖4所示，從圖4中可以看出文獻(xiàn)[1]所規(guī)劃出來的路徑長度最長，效果最差，在迭代88次后趨于穩(wěn)定；文獻(xiàn)[2]和文獻(xiàn)[3]規(guī)劃出來的路徑效果基本一致，但是文獻(xiàn)[2]在迭代70次后就趨于穩(wěn)定，而文獻(xiàn)[3]在迭代80時候才逐漸趨于穩(wěn)定；本文方法規(guī)劃出的路徑較其他3種方法路徑都要短，迭代62次后就基本趨于穩(wěn)定，并且最終的穩(wěn)定值也是最低的。圖5為4種算法路徑規(guī)劃的安全距離，從圖5中可以看出本文算法的安全距離最大，所以路徑規(guī)劃的安全性最高。圖6為4種算法規(guī)劃路徑中的轉(zhuǎn)角個數(shù)，從圖6中可以看出本文方法的轉(zhuǎn)角個數(shù)最少，最大化縮短了路徑長度和不必要的轉(zhuǎn)角操作。

其次，通過柵格地圖驗證文獻(xiàn)[1-3]及本文方法的路徑規(guī)劃質(zhì)量。如圖7所示，其中文獻(xiàn)[1]和本文方法規(guī)劃出的路徑均為最短路徑，但是文獻(xiàn)[1]規(guī)劃出的路徑的平滑度相對于本文方法很差，非最優(yōu)路徑。文獻(xiàn)[2]和文獻(xiàn)[3]規(guī)劃的出的路徑都不是最短路徑，雖然文獻(xiàn)[3]的路徑相對平滑，但是仍然不是最優(yōu)路徑。本文方法規(guī)劃出的路徑為最短路徑，并且平滑性也很高。

圖5 安全距離結(jié)果

圖6 路徑轉(zhuǎn)角個數(shù)

圖7 路徑規(guī)劃結(jié)果

最后，通過在某一固定工位點(diǎn)重復(fù)停車100次測試，自定義標(biāo)識符定位精度，位置糾偏測試結(jié)果如圖8所示。角度糾偏測試效果如圖9所示。從圖9中可以看出：位置糾偏精度理論上可以維持在±1 mm范圍內(nèi)，角度偏差可以控制在±1.5°，滿足一般精度要求。

圖8 位置糾偏測試效果

圖9 角度糾偏測試效果

5 結(jié)論

1)提出一種基于改進(jìn)的D*Lite量子風(fēng)驅(qū)動路徑規(guī)劃算法的AGV控制系統(tǒng)。整合D*Lite遺傳算法和量子風(fēng)驅(qū)動理論，在適應(yīng)度函數(shù)中加入碰撞系數(shù)，使用對角線函數(shù)取代傳統(tǒng)的歐幾里得距離，在WDO算法的基礎(chǔ)上使用量子行為解決優(yōu)化問題。

2)通過實(shí)驗仿真結(jié)果可以看出改進(jìn)的D*Lite量子風(fēng)驅(qū)動優(yōu)化算法在路徑安全性和路徑長度上有了明顯的改善，算法不會存在局部最優(yōu)，提高了算法路徑搜索效率，并且能規(guī)劃出一條最優(yōu)無碰撞的平滑路徑。

3)引入一種自定義標(biāo)識符定位法實(shí)現(xiàn)AGV精確的位姿糾偏，實(shí)現(xiàn)點(diǎn)位實(shí)時精確糾偏位置誤差在±1 mm，角度偏差在±1.5°，滿足一般應(yīng)用場合需求。