基于蚁群算法的六自由度采摘机器人轨迹规划研究

黄轶文，张 梅

( 1.广东工程职业技术学院 信息工程学院，广州 510520；2.华南理工大学 自动化科学与工程学院，广州 510640)



基于蚁群算法的六自由度采摘机器人轨迹规划研究

黄轶文1，张 梅2

( 1.广东工程职业技术学院 信息工程学院，广州 510520；2.华南理工大学 自动化科学与工程学院，广州 510640)

在现代农业生产中，果蔬采摘作业复杂而繁重，采摘机器人在作业过程中常常需要经历成千上万个果蔬采摘点，面对这样巨大的工作量，采摘机器人移动路径规划显得非常重要。为此，以采摘机器人运动轨迹为研究对象，以其运动轨迹总长最短为研究目标，针对机器人各关节机构运动速度变化情况及机器人运动特性，利用基本蚁群原理对六自由度采摘机器人的路径进行规划。实验结果表明：所设计的采摘机器人轨迹优化技术不但路径优化能力强、运动轨迹平滑，还具有可靠性强及稳定性好的优点。

蚁群算法；六自由度；采摘机器人；路径优化

0 引言

近年来，随着新农业生产模式和计算机技术的快速发展，智能机器人的研究有了很大的突破，且在农业生产中的应用越来越普及，已成为农业生产不可缺少的部分。在机器人的研究中，设计规划一条运动轨迹最优、躲避障碍次数最少、运行代价最低及轨迹最平滑的路线是重中之重。目前，机器人路径优化的研究方法主要有蚁群、Hopfield、遗传、网络神经及人工势场等算法。本文将根据果蔬采摘作业环境的特点，结合机器人整体结构运动特性，采用改进蚁群算法，对果蔬采摘机器人运动轨迹计算优化，实现果蔬采摘机器人运动路径最优化。

1 轨迹规划的蚁群算法

1.1 蚁群算法概述

20世纪90年代，意大利著名研究学家Dorigo提出了一种仿生的蚂蚁算法。蚂蚁群觅食过程如图1所示。其中，图1(a)表示刚开始时蚂蚁群寻找食物的运动路线；图1(b)表示在两条路线长短不一样的情况下，蚂蚁基本上会选择路径短的线路。因为路径短，蚂蚁走完全程需要的时间相对较少，因此在相同时间的情况下，走过的蚂蚁会多一些，路上的信息素也会越多，后面蚂蚁选择短路程的机率就大一些。

(a) (b)图1 蚂蚁群觅食示意图Fig.1 Schematic diagram of ant colony foraging

开始时刻，将蚂蚁置于相同的地点，每条路径都有一定初始的信息素，假设距离近的路径为S，距离远的路径为L，M1和M2为经过S和L的蚂蚁数量之和，则

M=M1+M2

(1)

当M只蚂蚁从S和L经过以后，第M+1只蚂蚁选择S路线的机率为

(2)

其中，a和h为参数值。第M+1只蚂蚁选择时，计算PS(M)，得到[0,1]之间的概率φ，若φ≤PS(K)，则选择S，不然选择L。

1.2 路径规划建模

机器人在采摘作业中，工作环境比较复杂，首先需要确定自己的任务，感知所处环境，然后对所走路线进行规划。假设采摘机器人要成功从果树A移动到果树B，在避开其他障碍时，整个运动过程应该满足：

1)用时最少，则

f1=min(maxt)， 1≤t≤6

(3)

其中，f1为从A到B所需最短时间；t为某一段的运动时间。

2)移动距离最短，则

(4)

其中，f2为从A到B所走最短路程；t为某一段的运动路程；(xi，yi)为路径点坐标。

3)路径最平滑，则

(5)

其中，f2为最小冲力；t为走完该路径时间。

路径优化是在同一时刻处理多个目标，对于优化总目标则需要综合多个目标来规划。采摘机器人的路径优化问题需要同时考虑用时最少、移动距离最短和路径最平滑3个因素，因此建立机器人轨迹规划函数为

F=a1f1+a2f2+a3f3

(6)

其中，a1、a2、a3为函数的权。

1.3 实时更新信息素

蚂蚁行走以后，若路径上的信息素太多，会导致启发信息的消失。为了解决这个问题，一般需要在蚂蚁完成一个节点路径时，实时更新信息素。更新信息素包括全局更新和局部更新两部分。由于采摘机器人在作业中可能会因地理环境进入陷阱而导致算法终止。因此，本文采用全局更新的方法，则在t+n时刻某一节点路径信息素更新准则为

(7)

(8)

其中，ρ⊂[0,1)为路径上信息素消失系数。

在更新信息素时，对于蚂蚁到达没有后继路径且没有到达终点时，自动判断该蚂蚁死亡，信息素更新退回至前一路径节点。

2 路径点坐标识别

机器人在采摘作业中，最重要的是对目标的识别和定位。实现机器人准确采摘首先需要感知目标果实的具体三维信息，现阶段获取目标空间信息一般采用双目视觉技术。本文采用双目视觉模型，坐标转化关系如图2所示

图2 三维空间坐标转化关系Fig.2 three dimensional space coordinate transformation relation

由三维空间坐标转化关系可得

(9)

(10)

(11)

(12)

T=R(α,β,γ)

(13)

其中，R为标准正交矩阵；T为平移值；f为双目摄像机焦距。

图像定义的直角坐标系“u-O-v”中，像素坐标(u,v)为该像素在数组中的行数和列数，即(u,v)是以像素为单元的图像坐标。假设包括N个坐标点，采用奇异值分析算法可以得到图像中的内部、外部参数，由此可进行运动路线的优化。双目视觉模型示意图如图3所示。

3 最优路径识别规划

3.1 路径编码

首先给所有的路径点编上序号，即在坐标系XOY中，将直角坐标系等分为N个栅格，并依次编号，则每个序号表示一个唯一对应的栅格。令C={1,2,3,…,N}，gi∈(xi,yi),序号与栅格坐标对应关系如图4所示。

图3 双目视觉模型示意图Fig.3 Schematic diagram of binocular vision model

图4 序号与栅格对应关系图Fig.4 The relationship between serial number and grid

将两者转化为公式，有

(14)

其中，mod是求余运算；int为求整运算。

3.2 求出映射关系

为了使计算机在处理图像的更加方便快捷，采用序号与栅格坐标相对应标识，即可以根据序号求目标的直角坐标。首先设定像素(u,v)与序号的映射关系为

(u,v)=m×(j-1)+i

(15)

求出像素(u,v)与目标坐标的映射关系为

(16)

最后，得到目标坐标值与序号的关系为

(u,v)=m×(y+0.5δ-1)+x+0.5δ

(17)

3.3 求解路径节点距离与合适度函数

在软件仿真之前先确定路径节点的具体位置，本文采用计算机编码生成直角坐标参数。路径节点距离采取N×N矩阵D进行保存，D(i-1,i)表示第(i-1)个路径节点和第i个路径节点间的实际距离。两点之间的距离公式为

(18)

在求解最优函数时，所有节点总距离与合适度函数成反比，即总距离越大，该路径合适度越低。根据所有节点总距离，合适度函数为

(19)

在进行最优路径选择时，一般去除节点总距离大的，保留节点总距离小的。

3.4 最有路径求解的算法流程

蚁群算法解决路径优化问题流程如图5所示。

图5 蚁群算法解决路径优化问题流程图

Fig.5 Flow chart of ant colony algorithm to solve path optimization problem

具体路径求解的算法流程：

1)建立环境模型，初始化蚂蚁算法，格式化信息素，清空迭代次数。

2)设定起点和终点，将蚂蚁群置于起始点S。

3)蚂蚁按概率函数在各路径间运动，并对移动次数自动计数。

4)当移动总次数小于软件设置值，且蚂蚁没有到达终点时，回到步骤3)重新开始；反之，大于或者等于软件设置值，且没有到达终点，将蚂蚁重新初始化， 清空该蚂蚁所有数据，并将蚂蚁重置数加1；如果重新初始化小于设定值则返回至步骤3),否则系统自动判断该蚂蚁死亡。

5)统计成功到达终点蚂蚁需要的路径代价。

6)所有蚂蚁重复执行执行步骤3)到5)。

7)计算路径代价最小值。

8)判断蚂蚁到达各路径节点时运动方向，实时更新信息素值，并将迭代次数自动加1后保存。

9)输出最优路径。

4 实验仿真与结果分析

为验证该蚁群算法的六自由度采摘机器人轨迹规划可靠性，利用MatLab仿真软件对其进行有效性仿真实验，并在该实验中特别加入各种障碍物和难度系数大的工作环境。为了更好地验证蚂蚁算法，首先按需要给路径节点编号，根据所走路径占据的栅格数量来判断路径的长度，在求解最优路径时，引入平滑度参数，以路径中拐点数判断该路径的平滑度。在实试验中，设定障碍系数为16%，蚂蚁总数为1 000，移动次数NC=200，起始点为A，终点为B，仿真结果如图6所示。

(a) (b)图6 蚂蚁算法仿真结果图Fig.6 Simulation results of ant algorithm

图6(a)中是没有考虑平滑度因素的轨迹图；图6(b)中是加入平滑度因素的轨迹图。轨迹优化性能主要拐点数、路径长度和移动次数。两种不同算法反复试验50次后，性能状况如表1所示。

表1 轨迹优化性能状况表Table 1 Performance status of trajectory optimization

结合图6和表1可以看出：在路径长度相同的情况下，轨迹平滑度对轨迹优化影响很大；在拐点数减少的同时，可以减少机器人移动次数，降低机器人采摘作业中的机械损伤。因此，对于精确度高的轨迹优化需要加入轨迹平滑度参数。采用蚂蚁算法的轨迹优化如图7所示。

5 结论

针对六自由度采摘机器人在采摘作业过程中行走路径过长及效率低的问题，以采摘机器人运动轨迹为研究对象，以其运动轨迹总长最短为研究目标，加入轨迹平滑度约束条件，采用基本蚁群算法原理对六自由度采摘机器人的路径进行规划，并运用MatLab对蚂蚁算法进行仿真实验，对机器人运动轨迹优化结果进行验证。结果表明：所设计研究的采摘机器人轨迹优化算法是可行的，其路径优化能力强，运动轨迹平滑，具有可靠性强及稳定性好的特点。

图7 轨迹规划图Fig.7 Trajectory planning

[1] 苏玉.蚁群算法的研究及其在路由选择方面的应用[D].无锡：江南大学,2009.

[2] 陈天宏.双目采摘机器人路径优化设计的研究[D].哈尔滨：东北农业大学,2010.

[3] 王鑫.基于蚁群算法的6自由度工业点焊机器人的轨迹优化及仿真[D].上海：华东理工大学,2011.

[4] 宋世杰,刘高峰,周忠友,等.基于改进蚁群算法求解最短路径和TSP问题[J].计算机技术与发展,2010(4):144-147.

[5] 陈天宏,崔天时,李广军.基于遗传算法的采摘机器人轨迹规划[J].农机化研究,2010,32(8):31-34.

[6] 蔡文彬,朱庆保.具有感觉适应功能蚁群算法的机器人路径规划[J].计算机工程与应用,2010(31): 215-218.

[7] 杜占玮,杨永健,孙永雄,等.基于互信息的混合蚁群算法及其在旅行商问题上的应用[J].东南大学学报：自然科学版,2011(3):478-481.

[8] 张凌.蚁群算法的研究及其在网络路由优化上的应用[D].无锡：江南大学,2008.

[9] 刘国才.无人机单目机器视觉着降定位的算法研究及DSP实现[D].成都：电子科技大学,2008.

[10] 李庆瀛.基于视觉注意的移动机器人目标跟踪技术研究[D].大连：大连理工大学,2008.

[11] 王倩.基于视觉导航的模型车自主行驶研究[D].南京：南京航空航天大学,2007.

[12] 雷春英.基于改进蚁群算法的火灾疏散路径优化研究[D].武汉：武汉理工大学,2014.

[13] 贾磊,程乃伟.基于蚁群算法的动态疏散路径改进[J]. 科技传播,2013(20):85-86.

[14] 王海员.无线传感器网络能量均衡数据汇集算法研究[D].重庆：重庆大学,2011.

[15] 李志鹏.蓝莓采摘机采摘策略及轨迹规划研究[D].哈尔滨：东北林业大学,2011.

[16] 吕继东.苹果采摘机器人视觉测量与避障控制研究[D].镇江：江苏大学,2012.

[17] 黄铝文.苹果采摘机器人视觉识别与路径规划方法研究[D].杨凌：西北农林科技大学,2013.

[18] 马强.苹果采摘机器人关键技术研究[D].北京：中国农业机械化科学研究院,2012.

[19] 胡荟.基于改进蚁群算法的机器人三维路径规划技术的研究[D].杭州：浙江师范大学,2012.

[20] 王辉.机器视觉技术在果园自动化中的应用研究[D].北京：中国农业机械化科学研究院,2011.

[21] 严卉.基于视觉的轮式移动机器人目标跟踪技术研究[D].南京：南京理工大学,2005.

[22] 胡龙.基于双目立体视觉的旋转体三维重建技术研究[D].大连：大连理工大学,2007.

[23] 张昌远.基于蚁群的P2P网络副本一致性维护策略[D].大连：大连理工大学,2013.

[24] 刘瑛.基于蚁群优化算法的车辆出行问题研究与应用[D].重庆：重庆理工大学,2013.

[25] 李鹤喜.基于视觉反馈的焊接机器人自主示教关键技术研究[D].广州：华南理工大学,2010.

[26] 王燕,张果,葛运旺.四自由度关节型采摘机械手轨迹规划与实验研究[J].现代制造工程,2013(7): 27-31.

[27] 吴靓,何清华,黄志雄,等.基于蚁群算法的多机器人集中协调式路径规划[J].机器人技术与应用,2006(3):32-37.

[28] 尹向东. 基于蚂蚁优化的QoS约束分布式多播路由算法研究与实现[D].长沙：中南大学,2008.

[29] 崔玉洁,张祖立,白晓虎.采摘机器人的研究进展与现状分析[J].农机化研究,2007(2):4-7.

Research on Trajectory Planning of Six-freedom-degree Picking Robot Based on Ant Colony Algorithm

Huang Yiwen1, Zhang Mei2

(1.Department of Information Engineering, Guangdong Engineering Polytechnic, Guangzhou 510520, China; 2.School of Automation Science and Control Engineering, South China University of Technology, Guangzhou 510640, China)

In modern agriculture planting picking, picking fruits and vegetables operating in a complex and arduous, picking robot in the process often need to experience thousands of fruit and vegetable picking point, the face of such a huge workload, picking mobile robot path planning is very important. The to picking robot trajectory as the research object, takes the shortest length of the motion trajectory as the research target, for each robot joint mechanism motion speed changes, combined with robot motion characteristics, the basic ant colony algorithm of six degree of freedom picking robot path planning. The experimental results show that: the design of the picking robot trajectory optimization technology not only has strong ability of path optimization, smooth motion trajectory, but also has the advantages of strong reliability and good stability.

ant colony algorithm; six degree of freedom; picking robot; path optimization

2015-01-25

广东省工业攻关科技计划项目(2013B010401002)

黄轶文(1974-)，女，广州人，高级工程师，硕士，(E-mail)yw_huang1974@sina.com。

S225.92;TP242.6

A

1003-188X(2017)03-0242-05