2015年9月农机化研究第9期

农业激光自动采摘定位机器人控制系统设计

—基于PID控制

123

王立谦，汪小志，林卫国

（1．华中科技大学武昌分校机电与自动化学院，武汉430070；3．华中农业大学工学院，武汉摘

330108）

430064；2．武汉理工大学物流工程学院，武汉

要：结合STM32和MSP430单片机设计了一种新的农业果实采摘机器人激光自动瞄准系统，并在系统设计

过程中引入了PID算法，大大提高了果实采摘机器人的定位精度和自动化程度。该系统对果实目标区域采用两STM32单片机控制图像采集设备，组控制系统进行图像采集和运动控制。其中，并对图像信息进行分析处理，数据结果经无线通信送MSP430单片机，控制电机带动激光笔移动瞄准目标位置。系统采用OpenCV处理图像，实现了人机交互功能，利用PID控制算法调整瞄准误差，提高了瞄准精度，通过对电机的闭环控制，实现了激光自动瞄准功能。实验结果表明：此系统可以成功锁定目标，达到了较高的精度，为激光瞄准系统在农业自动化和现代化中的应用研究提供了理论依据。

关键词：果实采摘；激光瞄准；PID控制；STM32；MSP430；人机交互中图分类号：TP242．3；S123

文献标识码：A

文章编号：1003－188X（2015）09－0211－06

DOI:10.13427/http://ki.njyi.2015.09.048

0引言

农作物果实机器人采摘过程中，机器人采摘系统

所示

。

受到多方面的影响，如可见光、温度、湿度等，其采摘的位置往往和实际果实位置具有一定的偏差；而激光瞄准系统能以较高频率提供大量准确的距离信息，可靠地提供果实的方位和尺寸信息，为机器人采摘果实提高可靠的数据支持。激光瞄准器能够方便快速准确地瞄准目标，且激光瞄准器辐射强度高、单色性好、相干性好、方向性强，在目标定位方面具有很多优势。为了研究高效的机器人采摘控制系统，提高机器人的作业效率，设计了以STM32单片机和MSP430单片机为控制核心、以摄像头进行图像采集、以直流减速电机为激光笔瞄准带动装置的激光采摘定位装置的系统。

Fig．1

图1

自动瞄准装置系统示意图

Theschematicdiagramautomaticaimingdevicesystem

系统需对摄像头数据接收及处理来寻根据要求，

找激光斑，此外根据寻找结果控制电机带动激光枪改变瞄准位置。因此，系统采用了两套单片机控制系统：一套用于图像信息采集与分析处理，一套用于控制激光枪二维平面移动瞄准。这两套控制系统通过无线通信模块进行数据交换，配合实现系统要求。

根据图1的系统功能需求，系统采用STM32和MSP430单片机，结合摄像机、无线通信模块、电源驱动模块及语言模块进行结构设计，如图2所示。

STM32根据采集到的图像信息判断激光斑其中，

是否击中目标位置，并将判断结果通过无线通信的方式送给MSP430；MSP430根据此信息发出控制信息驱

1系统总体设计

摄像头作为图像系统以农作物果实为瞄准目标，

采集设备，通过对图像信息的分析控制激光枪的激光斑置于农作物果实的指定位置，其结构示意图如图1

收稿日期：2014－11－05

基金项目：国家自然科学基金青年基金项目（51305152）

（E－mail）作者简介：王立谦（1975－），女，四川资阳人，讲师，

724104439@qq．com。

（E－mail）fiber-通讯作者：汪小志（1981－），女，武汉人，讲师，博士，

home@126．com。

2015年9月农机化研究

闭环控制，直至激光斑击中目标位置。

第9期

动电机转动，从而带动激光枪重新瞄准，此过程形成

图2Fig．2

系统设计结构图

Thesystemdesignstructure

2

2．1

硬件系统设计

图像信息采集硬件电路

系统采用常见的OV7670模块进行数据采集，在

分离的方式，从图像数据中舍弃色度UV值而直接提取亮度信息Y，可以简化数据的处理过程。无损的存U、V储方式下一个像素由3个字节构成，分别为Y、值。由于只需Y值，为降低数据处理量，配置OV76701个像素由2个字节构工作于YUV4∶2∶2的模式，

成。从其采集的数据中提取Y值从而定位激光斑，此方式对于强光照射下激光斑的定位也极为有效。图3为STM32工作电路及信息采集硬件电路图。

控制上，首先对无激光照射的农作物果实图像通过ＲGB灰度计算标注出靶心。当激光照射到胸靶环重新采集图像后，对激光斑定位并与靶心位置比较，输出控制信号。对于高亮度激光斑的信号辨识，常见的ＲGB方式辨识会产生较大误差，因此采用YUV亮度

图3Fig．3

摄像头控制电路

Thecameracontrolcircuit

通过PD2和PD3系统中STM32遵照SCCB协议，

口对OV7670的内部寄存器进行配置，设置其工作方式为YUV4∶2∶2模式，然后STM32即可在控制信号下进行数据接收。数据读取的时序是：帧同步信号VSYNC在给出一个高电平脉冲同步信号后回到低电

平（低有效），此时行同步信号HＲEF出现有效信号高电平，每STM32在像素同步信号PCLK指挥下进行8位像素数据读取。取到640个数据时完成一行数据的读取，此时行同步信号变为低电平，下一行数据又从其转换为高电平开始。STM32工作于72MHz的超高

频率下，通过捕捉帧同步信号VSYNC及行同步信号HＲEF可以做到时序匹配，保证系统在高频工作条件下不会出现数据丢失。2．2

无线通信硬件电路

系统的两个主控芯片之间采用无线方式进行通信。无线收发模块采用高度集成的SI4432芯片，具有

极低的接收灵敏度（－118dBm）及较大的输出功率，有效通信距离可达1km，无需外加功放电路即可保证传输范围和穿透能力。无线接收模块接收到遥控信息后通过SPI模式与主控芯片进行数据传递。STM32及MSP430均具备多个SPI硬件接口，使用非常方便。其硬件电路如图4所示。

图4

Fig．4

无线通信硬件电路

Thewirelesscommunicationhardwarecircuit

设定其无线通信主控芯片先对SI4430进行配置，

频率为433MHz，为了使程序调试更为简单，配置其工作于FIFO模式；设定波特率为9600。当芯片工作模式被设定后，即可按照设定的数据结构通过SPI向SI4430写数据进行数据发送。当接收端SI4430接收nIＲQ会出现一个低电平触发中断，到有效数据时，主控芯片即可通过SPI方式接收无线数据。系统的两个主控芯片均通过此无线通信模块进行数据交换。2．3

电机驱动硬件电路

系统采用两个直流减速电机带动激光笔进行二维平面的扫描，结构如图5所示。其中，电机1带动电机2横向旋转，电机2的轴旋转则带动激光笔纵向扫描，对两个电机的控制可以控制激光笔对二维平面的扫描。

电机驱动采用飞思卡尔公司的驱动芯片MC33886，它可连续提供5A电流，驱动负载能力强。STM32采用16位定时器产生PWM波对电机进行速度控制。由于减速直流电机性能平稳，运行中速度较慢，且系统中激光斑的位置信息为反馈信号控制电机，已经形成闭环，因此未设测速单元。具体的硬件

IN1和IN2与STM32的I/O口设计电路如图6所示，直连

。

图5

Fig．5

电机驱动硬件电路

Thedrivinghardwarecircuitofmotor

3程序设计

农业采摘机器人激光自动瞄准系统的数学模型设

计主要是误差识别模型的设计，本文采用PID闭环控制的方式，有效地缩小了控制误差，并采用了OpenCV设计了人机交互界面。

图6Fig．6

电机控制电路Motorcontrolcircuit

q

假设系统瞄准转角和位移的误差分别为Ｒ，在发射望远镜直径和波长一定的情况下，本文利用模糊PID控制的方式提高系统的稳定性和精度。首先，定义瞄准转角和位移误差分别为

Ｒ（Q）=kbr（Q）+kj∑r（i）T+kw

i=0

r（Q）－r（Q－1）

T

（4）

kw表示微分系数，T表示kb表示积分系数，其中，

（1）

e表示计算误差。通过迭代计算和逐次逼采样周期，

近的方法，可以降低瞄准误差，提供瞄准精度。

本次设计利用OpenCV计算机视觉函数库，设计了人机交互界面，通过控制用户界面上的图形控件来执行上位机程序，其主要程序如下：

#include"cv．hpp"#include"cxcore．hpp"

#include"opencv2\highgui\highgui．hpp"usingnamespacecv；

（3）

｛

intmain（）

voidonTrackbar（int，void*）blur（gray，edge，Size（3，3））；

Canny（gray，edge，edgeThresh，edgeThresh*3，3）；cedge=Scalar：：all（0）；voidonTrackbar（int，void*）｛

blur（gray，edge，Size（3，3））；

Canny（gray，edge，edgeThresh，edgeThresh*3，3）；

{－10μm≤Ｒ（k）≤－10μm

y=

－10μrad≤Ｒ（k）≤－10μrad

10］，误差基本论域是［－10，利用代数式将智能控制系统的模糊控制算法进行离散化，有

[

2na+b

（x－）b－a2

]

（2）

x∈［a，b］；n为离散度，其中，通过离散化共得到n个子矩阵关系，利用模糊控制算法对机器人智能模糊控制算法设计为

2，…，n）Ｒ=Ｒ1∪Ｒ2∪…∪Ｒi（i=1，

PID控制器是一种结构简单的线性控制器，本文利用PID算法对误差进行控制，其结构如图7所示

。

图7Fig．7

PID误差控制算法

cedge=Scalar：：all（0）；．．．．．．

通过图像分析处理之后利用无线通信技术将图像使电机发出动作，初步锁定目数据传递给控制模块，

PIDerrorcontrolalgorithm

图7所示表示运用PID控制器对误差进行控制，其控制方程为

2015年9月农机化研究

表1Table1

测试次数

123456

指定点坐标果实中心果实中心果实中心果实中心果实中心果实中心

测试数据Testdata

瞄准误差/mm352528332027

第9期

标，并利用逐次逼近的方式逼近目标，完成目标的激光自动化锁定。

4测试结果分析

测试工具选用精度0．1cm的卷尺和精度0．01s秒

所用时间

/s＜2．0＜1．2＜1．5＜1．8＜1．6＜1．3

表，测试过程将激光枪固定于指定范围内的任一点，并将摄像头安装于农作物果实附近，测试内容主要是果实中心瞄准。目标锁定示意图如图8所示。

如图10所示，在进行多次误差统计和分析对比发现，使用PID算法可以有效的降低误差，增加瞄准的精度，提高机器人的自动化作业水平。

图8Fig．8

激光锁定目标结果图Thelasertargetresult

由实验结果可以看出：激光可以成功有效地锁定目标，并且锁定时间在1～2s内就能完成；系统根据逐次逼近的方法可以降低锁定的误差精度。

对激光逐次逼近进行多次实验，得到激光逐次逼近瞄准后的激光点中心与目标中心位置对比结果，如图9所示。激光对目标点进行瞄准定位最后的位置偏差在30×30像素内，瞄准精度较高。在靶环的测试过程中虑了环境光强对光斑信号的干扰的情况，对激光枪瞄准控制上也进行了多次测量。通过对激光枪瞄准点果实中心的跟踪测试，速度及精度结果测试数据如表1所示。

Fig．10

图10

瞄准定位误差结果对比图

Thecontrastfigureoftargetpositioningerrorresults

5结论

1）设计了一种基于图像处理技术的激光自动瞄

准的农业果实采摘机器人控制系统。该系统采用STM32单片机和MSP430单片机作为核心控制，采用OV7670作为图像信息提取设备，以直流电机带动激光笔进行二维平面扫描，提高了果实采摘机器人的定位精度和自动化程度。

2）建立了激光自动瞄准的PID，提高了激光瞄准的精度和自动化程度，并利用OpenCV实现了人机交互功能，在硬件系统上设计了图像信息采集硬件电路、无线通信硬件电路、电机驱动硬件、电路彩屏显示及语音播报硬件电路。

3）通过激光果实中心瞄准实验研究发现：该系统具有定位速度快等优势，可以快速锁定目标，对于强光干扰有一定抗干扰能力，能够达到较高的瞄准精度。参考文献：

［1］吴越，王新伦．一种激光瞄准具瞄准偏差的判读系统

［J］．机械研究与应用，2014（4）：128－130．［2］

王姣，谭立龙．基于C8051F330的光电瞄准系统设计

图9

Fig．9

目标逐次逼近示意图

Targetsuccessiveapproximationschemes

激光枪能够在2s内从胸靶环的任根据测试数据，意位置瞄准果实中心，平均误差为0．5%左右，达到了较高的瞄准精度。

2015年9月

2013（19）：123－125．［J］．电子设计工程，

农机化研究第9期

［9］魏振忠，张广军，等．激光跟踪视觉导引测量系统孙文，

．仪器仪表学报，2009，30（11）：的全局校准方法［J］2262－2266．

［10］王大鹏，侯满义，等．基于AＲM的轰炸光电瞄范惠林，

J］．测控技术，2013，32（10）：62－65．准控制与实现［

［11］宋晓茹，薛永刚．基于ＲS－Chaos－LSSVM的光电稳瞄

．西安工业大学学报，2013，系统稳定精度检测方法［J］33（10）：845－850．

［12］胥青青，郭新胜，等．基于dSPACE的光电稳瞄半纪明，

J］．兵工自动化，2013，32（11）：67实物仿真系统设计［－70．

［13］刘洋，安志勇，等．白光瞄准镜多参数数字化综王劲松，

．科学技术与工程，2012，12（10）：2442－合检测［J］2445．

［3］高伟伟，高凤岐，等．稳瞄稳向系统FOG随机信王广龙，

J］．中国测试，2014（2）：98－104．号处理方法研究［

．科技信息，2013［4］汤咏梅．瞄准器数控车削加工工艺［J］

（19）：99－109．

［5］吕溥，张艾莉，等．高精度瞄准镜零位走动量检韩国华，

J］．激光技术，2013，37（3）：404－408．测研究［

［6］贾彦斌，李荧兴．光电观瞄仪轴系垂直误差分析及修正

［J］．火力与指挥控制，2013，38（9）：157－159．

［7］吴永浩，尚亮．基于OpenCV的运动目标跟踪系华云松，

J］．电子测试，2010（8）：21－25．统研究［

［8］刘万里，王占奎，曲兴华，等．激光跟踪测量系统跟踪转

J］．光学精密工程，2008，16（4）：585－镜的误差分析［590．

DesignofAgriculturalLaserAutomaticPickingPositioningＲobotControl

SystemBasedonPIDControl

WangLiqian1，WangXiaozhi2，LinWeiguo3

（1．SchoolofMechanical＆ElectricalEngineeringandAutomation，HuazhongUniversityofScienceandTechnologyWuchangBranch，Wuhan430064，China；2．SchoolofLogisticsEngineering，WuhanUniversityofTechnology，Wuhan430063，China；3．CollegeofEngineeringTechnology，HuazhongAgriculturalUniversity，Wuhan430070，China）Abstract：Thispaperdescribesthedesignofanewautomaticaimingsystemofagriculturalfruitpickingrobotlasercom-binedwithSTM32andMSP430singlechipmicrocomputer，andPIDalgorithmisintroducedintheprocessofsystemde-sign，whichgreatlyimprovesthepositioningaccuracyandautomationdegreeoffruitpickingrobot．Thesystemadoptstwosetsofcontrolsystemtoacquireimageandcontrolmotioninthefruitobjectarea，theSTM32singlechipisusedtocon-troltheimageacquisitionequipment，andanalyzeimageinformationprocessing．ThedataresultsissenttoMSP430throughwirelesscommunication，andcontrolmotordrivesthelaserpenmovingatthetargetlocation．ThesystemadoptsOpenCVtoprocessimage，whichrealizethefunctionofhuman－computerinteraction，usingPIDcontrolalgorithmtoad-justthepointingerror，andimprovestheprecisionofaimcontrol．Itrealizestheautomaticlaseraimingfunctionthroughtheclosedloopofthemotor．Finally，itshowsthatthissystemcantargetsuccessfullylocked，andachieveshigheraccu-racythroughtheexperimentalresults，whichprovidesatheoreticalbasisforlaseraimingsystemapplicationstudyintheagricultureautomationandmodernization．

Keywords：fruitpicking；lasercollimation；PIDcontrol；STM32；MSP430；human－computerinteraction