联合D*Lite算法与视觉的AGV路径规划与定位

周祥明，陈员义，徐华银

(1.江西师范高等专科学校 航空旅游学院，江西 鹰潭 335000；2.江西师范高等专科学校 物联网学院，江西 鹰潭 335000；3.江西师范大学 教务处，南昌 330000)

随着现代工业生产自动化进程的快速发展，智能物流运输、包装系统已经逐渐成为自动化进程的一个重要支撑点。自动导航车(Automated Guided Vehicle,AGV)已经广泛应用于各种柔性制造系统、智能仓储物流系统、智能码垛搬运系统等。路径的自主规划与后期定位精度是AGV领域的研究热点。

文献[1]提出一种动态时间窗的最优路径规划方法，以分时利用策率结合迪杰斯特拉(Dijkstra)算法和时间窗法，首先根据Dijkstra算法按优先级高低对AGV运行任务规划最短路径，然后以时间窗法解决运行过程中的路径冲突、碰撞等问题，整个系统的鲁棒性和柔性都比较好；但是，并没有考虑Dijkstra算法效率低、运算过程中占用空间大等缺陷，所系统的实时性较差。文献[2]提出一种F-D*Lite算法，相对于传统D*Lite算法路径重规划效率更高，但是对于多障碍情况路径规划效果依然不理想。文献[3]提出一种融合粒子群和遗传算法的混合自适应路径规划算法，该算法具有较强的全局搜索能力和较高的收敛精度，但是计算量比较大。以上3种方法都只考虑AGV的路径规划问题，并没有涉及到后期的AGV路径纠偏问题。

针对以上问题，本文提出一种基于改进的D*Lite量子风驱动路径规划算法的AGV控制系统。在原有D*Lite遗传算法[4]的基础上融入量子风驱动理论，在适应度函数中加入碰撞系数，使用对角线函数取代传统的欧几里得距离，在风驱动(WDO)算法的基础上使用量子行为来解决优化问题。通过仿真实验结果可以看出：改进的D*Lite量子风驱动优化算法在路径安全性和路径长度上有了明显的改善，算法不会存在局部最优的问题，提高了算法路径搜索效率，并且能规划出一条流畅的最优路径。最后，引入一种自定义标识符定位法实现AGV精确的位姿纠偏。

1 D*Lite路径规划

机器人在保证路径最优的同时也要保证其运动过程的安全性，因此本文在适应度函数中加入了碰撞检测来降低障碍物周围元胞被选中的概率，碰撞系数设置如图1所示。

图1 碰撞系数设置

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

(1)

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

(2)

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

2 融合风驱动优化算法

2.1 量子比特

在量子计算中，最小的量子信息单元是量子比特，量子比特与传统的比特相比不仅可以处于“0”或“1”状态，而且还能处于“0”和“1”之间，一个量子比特的状态可以描述为[7-8]：

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

(3)

式中：α和β是一对复数，表示量子比特的概率振幅，α和β分别表示趋向于“0”和“1”的概率，且满足|α|+|β|=1，|Ψ〉表示一个量子状态。一个n元量子位定义如下：

(4)

2.2 量子风驱动优化算法

本文提出了一种新的量子启发式算法，即量子风驱动优化(QWDO)[9-10]。QWDO使用量子比特的概率振幅来表示粒子的位置。位置的运动可以通过量子旋转门策略实现。位置实现了量子非门策略的变异[11]。这种操作可以改善种群的多样性，避免早熟收敛。由于每个量子位都有两个概率幅度，每个粒子也可以表示优化空间的两个位置。在相同数量的粒子的情况下，可以加速搜索过程[12-13]。

1)生成初始种群。由于概率振幅满足方程α2+β2=1，设α=cosθ,β=sinθ，θ为旋转角，编码方式如下：

(5)

式中，θij=2π×rand，rand是一个介于0和1之间的随机数，i=1,2,…,m，j=1,2,…,n，m是种群大小，n是空间维度。每个个体对应于问题空间的两个位置，量子态|0〉和|1〉的概率幅公式如下：

(6)

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

2)解空间转换。为了计算种群个体的当前位置，需要进行解空间的变换。需要将个体的两个位置从单元空间I=[-1,1]n映射到优化问题的解空间。解空间的变量如下：

(7)

3)更新过程。为了避免算法陷入局部最优，本文应用了两个量子门策略。利用量子旋转门策略实现了位置的运动，并利用量子非门策略实现个体的突变。在量子风力驱动优化算法中，相位角增量和相位角的更新公式如下：

(8)

式中: Δθij、θij分别是第i个相位角增量和相位角的第j维。采用量子旋转门策略更新概率幅度，公式如下：

(9)

更新后的两个位置如下：

(10)

采用量子非门策略使种群的位置突变。这种操作可以增加种群的多样性，避免种群早熟收敛。randj是一个在 0 和 1 之间的随机数。如果randj

(11)

3 自定义定位标识符

针对某些特殊应用场合，关键工位点停靠精度高的要求，采用视觉二次精定位的方法，提高AGV定位精度和拓宽应用领域。

3.1 摄像头选择

本文采用凯视佳Falcon2 FA-81-12M1H工业相机作为单目精定位采样模块。分辨率为4 096×3 072，帧速度为58 fps，彩色成像。其中，帧速度大小对于后期圆形标识符采样效果影响最大，圆形标识符采样时长为0.2 s，所以帧速度不得小于12 fps。

3.2 自定义图标定位原理

基于CANNY边缘检测识别圆形标识符作为AGV小车的后期定位标识符[14-15]。首先对标识符信息进行高斯滤波处理，去除一些干扰信息，然后Hough圆检测提取标识符轮廓。定位原理如图2所示。

图2中，P1(x1,y1)、P2(x2,y2)、P3(x3,y3)分别表示这一帧图片的中心点坐标、自定义标识符的中心点坐标和纠偏参照点，简化后分别记为A、B、C三点。这三点构成的直角三角形的三条边的边长分别记为a、b、c，b和c两条边的夹角记为θ，Wx、Wy分别表示当前帧图像的长和宽。

假设src为当前帧图像，则Wx=src·cols、Wy=src·rows。利用函数HoughCircles()提取圆形轮廓，进而圆心坐标P2(x2,y2)=Point(circles[i][0]), cvRound(circles[i][1]))，此时P3(x3,y3)=Point(center·x,srcImage·rows/2)。因为A、B、C三点所构成三角形为直角三角形所以可以直接获得a、b的边长。根据余弦定理可以求得偏转角θ，如式(12)所示：

(12)

得到AGV小车具体纠偏数据后根据图3进行位姿精确校正。从图3中可以看出：根据a、b值的大小，通过实验分析获得像素值与实际距离的转换系数，然后执行水平位姿纠偏；根据偏转角θ的大小执行旋转角度的纠偏。

4 实验结果与分析

首先，在MatlabR2014a环境下，首先针对路径规划的长度、安全距离、转角数分别对文献[1-3]及本文方法进行实验对比；其次，通过栅格地图模仿现实障碍物场景测试路径规划效果；最后，通过固定工位点重复停车测试位置和角度纠偏效果。

4种方法的路径规划结果如图4所示，从图4中可以看出文献[1]所规划出来的路径长度最长，效果最差，在迭代88次后趋于稳定；文献[2]和文献[3]规划出来的路径效果基本一致，但是文献[2]在迭代70次后就趋于稳定，而文献[3]在迭代80时候才逐渐趋于稳定；本文方法规划出的路径较其他3种方法路径都要短，迭代62次后就基本趋于稳定，并且最终的稳定值也是最低的。图5为4种算法路径规划的安全距离，从图5中可以看出本文算法的安全距离最大，所以路径规划的安全性最高。图6为4种算法规划路径中的转角个数，从图6中可以看出本文方法的转角个数最少，最大化缩短了路径长度和不必要的转角操作。

其次，通过栅格地图验证文献[1-3]及本文方法的路径规划质量。如图7所示，其中文献[1]和本文方法规划出的路径均为最短路径，但是文献[1]规划出的路径的平滑度相对于本文方法很差，非最优路径。文献[2]和文献[3]规划的出的路径都不是最短路径，虽然文献[3]的路径相对平滑，但是仍然不是最优路径。本文方法规划出的路径为最短路径，并且平滑性也很高。

图5 安全距离结果

图6 路径转角个数

图7 路径规划结果

最后，通过在某一固定工位点重复停车100次测试，自定义标识符定位精度，位置纠偏测试结果如图8所示。角度纠偏测试效果如图9所示。从图9中可以看出：位置纠偏精度理论上可以维持在±1 mm范围内，角度偏差可以控制在±1.5°，满足一般精度要求。

图8 位置纠偏测试效果

图9 角度纠偏测试效果

5 结论

1)提出一种基于改进的D*Lite量子风驱动路径规划算法的AGV控制系统。整合D*Lite遗传算法和量子风驱动理论，在适应度函数中加入碰撞系数，使用对角线函数取代传统的欧几里得距离，在WDO算法的基础上使用量子行为解决优化问题。

2)通过实验仿真结果可以看出改进的D*Lite量子风驱动优化算法在路径安全性和路径长度上有了明显的改善，算法不会存在局部最优，提高了算法路径搜索效率，并且能规划出一条最优无碰撞的平滑路径。

3)引入一种自定义标识符定位法实现AGV精确的位姿纠偏，实现点位实时精确纠偏位置误差在±1 mm，角度偏差在±1.5°，满足一般应用场合需求。