硕士学位论文-番茄采摘机械手运动学仿真与避障算法研究.pdf

江苏大学硕士学位论文番茄采摘机械手运动学仿真与避障算法研究姓名：戈志勇申请学位级别：硕士专业：农业机械化工程指导教师：陈树人20070615江苏大学硕士学位论文摘要水果和蔬菜是人类生活中必不可少的食物，种植面积和产量逐年提高。研究开发适合目前农业生产实际的果蔬采摘机器人，不仅可以很大程度减轻劳动强度、提高生产效率，而且具有广阔的应用前景。在国外，这项工作已经进行了很多年，并取得了一定的研究成果；而在国内，这个领域的应用研究还相对滞后，离实际应用还有很大距离。本文以番茄采摘机械手从初始位置移动到目标番茄位置为考察对象，充分考虑采摘作业空间中障碍物的种类及属性，结合MamMAN_sV3x采摘机器人机械手本体特点，实现了番茄采摘机械手的基于局部采摘空间模型的避障路径规划。本文主要完成的工作：机械手的运动学是避障路径规划研究的基础，本文首先建立了MOTOMAN-SV3X的运动学模型，并在MATLABroboticstoolbox平台下，对MOTOMAN-SV3X的运动学进行了仿真研究。针对工作空间中可能存在的障碍物及形状进行了分类及抽象，确定了关节空间(C-空间)为规划空间，并利用边界特征对机械手各关节和连杆与工作空间(IN-空间)中的障碍物可能的碰撞方向和位置作了几何分析。结合采摘机器人机械手本体特点和几何分析的结果，提出了一种番茄生长环境中的障碍物均可由一个点或两个，最多三个点的I临界碰撞角表示的W-空间到D空间转换的计算方法。通过这种计算方法建立的G空间障碍，可以把复杂的空间环境信息快速转换为简单的二维平面信息。最后利用人工智能中重要搜索算法A算法，在D空间(-维平面信息)中实现了基于局部环境模型的番茄采摘机械手的避障路径规划。计算机仿真实验和实物实验均表明，采用避障路径规划算法最终生成的运动轨迹去实现机械手的采摘动作，机械手关节连杆在采摘过程中能够顺利避开障碍物。本文的研究是假定果实和障碍物的识别和定位等信息均以确定的情况下，将机器人技术与人工智能技术应用于农业工程领域，目的在于提高农业机器人采摘番茄的效率和成功率，为以后在硬件上实现番茄采摘机器人机械手的实时路径规划系统打下基础。关键词：番茄采摘，机械手，避障，C空间，A算法江苏大学硕士学位论文FruitandvegetablesareabsolutelynecessarilyfoOdinourlivestheplantedareaandoutputofthemisboostingyearbyyearResearchanddevelopmentaboutrobotforpickingfi-uitandvegetablesthatadapttogrowthpracticalatpresentnotonlycanalleviateintensityoflabourandadvanceproductivityeffectalsohaveexpansiveappliedprospectInabroadtheresearchinthisfieldhasbeendoneanyyearsandhasgotacertainachievementButinourcountrythedevelopmentofresearchaboutitisslow；thereisalongwaytoapplytherobottopracticeTilLspaperhascaniedoutasystemofpathplanningaimtoavoidobstaclefortomatopickmanipulatoraccordingtocharacteristicofmanipulatorofmotomansv3xandvarietyandattributeofbarrierinworkspaceBasedonthegoalsabove-mentioRedthepapermainlyhasbeenaccomplishedworkasfollowed：kinematicsofmanipulatoristhefoundationofpathplanningstudiesaimingtoavoidobstaclekinematicsmodelofmotomansv3xmanipualtorwasbuildandkinematicsWassimulatedalso，thenobstaclesinworkspaceworeassortedandabstractConfigurationspace(Cspace)asplanningspacewagascertainBydintofcomputingthecriticalangleofcollisionbetweenjointofmanipulatorandobstacle，Asortofwhichcomputingthecriticalangleofcollisionofasortofobstaclecantransintocomputingthecriticalangleofofiepoimortwopoin_tandthreepointispresentAfterC-spacewasbuild，complicatedspaceenvironmentinationwasconvertedintotwodimensionalgridinationIntheC-spaceweuseAalgorithmwhichisanimportantsearchalgorithmcarryingoutpathplanningsystembasedonmodeloflocalWOllf出paceaimtoavoidobstaclefortomatopickmanipulatofComputersimulationexperimentandobjectexperimentshowthattheend-effectandlinkofmanipulatorcanavoidobstaclefreelywhentheend-effectofmanipulatornaminginitialpositiontotargetpositionOurresearchassumethatrecognitionandlocationofthefruitandtheobstacleareknownandroboticsandartificialintelligencehaveapplicationinagriculture，goalsofthestudyisimprovedtheSUCOBSSratioofautomatedharvestingtomatOandlaythebaseonimplementingpathplanningsystembyhardwareKeywords：Pick-Tomato，ManipualtorAvoidobstacleC-spaceAalgorithm学位论文版权使用授权书本学位论文作者完全了解学校有关保留、使用学位论文的规定，同意学校保留并向国家有关部门或机构送交论文的复印件和电子版，允许论文被查阅和借阅。本人授权江苏大学可以将本学位论文的全部内容或部分内容编入有关数据库进行检索，可以采用影印i缩印或扫描等复制手段保存和汇编本学位论文。学位论文作者签名：垆一年多月7日指导教师签名：(氯川人加7年占月r日岛权授本用适后密解年在口密密保保不于属文沦位学本独创性声明本人郑重声明：所呈交的学位论文，是本人在导师的指导下，独立进行研究工作所取得的成果。除文中已注明引用的内容以外，本论文不包含任何其他个人或集体已经发表或撰写过的作品成果。对本文的研究做出重要贡献的个人和集体，均已在文中以明确方式标明。本人完全意识到本声明的法律结果由本人承担。学位论文作者签名：戈毛勇日期：沙刁年2月f7日江苏大学硕士学位论文11引言第一章绪论我国是一个农业大国。农业是国家政治稳定和人民安居乐业的基础。随着我国加入WTO以后承诺的各项有关农业方面的条款的相继落实，农业将面临前所未有的巨大挑战和机遇。目前，国际化、机械化、自动化已成为我国农业发展的一个基本趋势。加入世贸组织将是我国农业向国际化、机械化、自动化迈进的极为关键的一步，它有利于我国农业更好地进入世界市场，同时也将给农业机械的发展带来严峻挑战。当今科技的发展和农业作业模式的变革促进了农业机械的升级和进步，对农业机械的智能化、自动化的要求也越来越高；而农业机器人作为高科技时代的产物，是农业机械智能化的中坚力量，在农业工程中有着广阔的应用前景。可以预见，21世纪将是农业机器人在实际中广泛应用的世纪。在日本、美国等发达国家，农业人口较少，随着农业生产的规模化、多样化、精确化，劳动力不足的现象越来越明显。许多作业项目如蔬菜、水果的挑选与采摘，蔬菜的嫁接等都是劳动力密集型的工作，劳动力不仅成本高而且还不容易得到，而人工收获的成本在果蔬的整个生产成本中所占的比例高达33一55，正是基于这种情况，农林业机器人应运而生【11121。使用机器人有很多好处，可以提高劳动生产率，解决劳动力的不足；改善农业的生产环境，防止农药、化肥等对人体的伤害；提高作业质量等。随着信息化时代的到来和设旌农业、精确农业的出现，一向被视为落后的农业生产方式也必将乘上现代化的快车，而农业的新发展尤其离不开生物工程与信息化，在这方面，机器人具有得天独厚的能力。在农业机器人的研究方面，目前日本居于世界各国之首。但是由于农业机器人所具有的在技术和经济方面的特殊性，还没有普及。农业机器人的优点是显著的31：提高农业劳动生产率，解决劳动力不足的问题；改善农林业人员的生产环境，防止农药、化肥等对人体的伤害；提高作业质量和作业成功率等。在我国，由于机械化、自动化程度比较低，80的人口是农民，人均土地面积非常少，所以农业机械化、自动化的需求似乎不像发达国家那么迫切。但是随着科技的发展，信息化的要求，同时为了适应精确农业的需要，不管是从技术上江苏大学硕士学位论文紧跟国际发展趋势，处于竞争的有利地位，还是在实现农业的新发展，促进国民经济发展等方面，农业自动化方面的研究都有着相当重要的影响，是实现农业现代化必不可少的一环。研究采摘机器人不仅具有巨大的应用价值，而且具有深远的理论意义【4】。12农业机器人的特点与组成部分121农业机器人的特点工业领域是机器人技术的传统应用领域。由于在工业生产中，机器人的工作位置和障碍往往都能事先预知，因此机器人的性能得到很好的体现。和工业机器人相比，农业机器人有以下特剧5】【6】：(1)农业机器人作业对象的娇嫩性。细心轻柔地对待和处理。且其种类繁多，度不一，相互差异很大。生物具有柔软很容易损伤的特性，必须形状复杂，在三维空问里的生长发育程(2)农业机器人的作业环境的非结构性。由于农作物随着时间和空间的不同而变化，机器人的工作环境是变化的、未知的，是开放性的环境。作物生长环境除受园地、倾斜度等地形条件的约束外，还直接受季节、天气和时间等自然条件的影响。这就要求生物农业机器人不仅要具有与生物体柔性相对应的处理功能，而且还要能够顺应变化无常的自然环境。要求农业机器人在视觉、知识推理和判断等方面具有相当的智能。(3)农业机器人作业动作的复杂性。农业机器人一般是作业、移动同时进行，农业领域的行走不是连接出发点和终点的最短距离，而是具有狭窄的范围，较长的距离及遍及整个田问表面等特点。(4)农业机器人操作者是农民，不具有机械与电子知识的工程师，因此要求农业机器人必须具有高可靠性和操作简单的特点。(5)农业机器人的价格特性。工业机器人所需大量投资由工厂或工业集团支付，而农业机器人以个体经营为主，如果不是低价格，就很难普及。122农业机器人的组成部分目前的果蔬收获机器人一般可分为行走部分、图像识别和定位系统、控制系统、末端执行器、机械手等5大部分。机械手又称操作机或操作臂，是指具有和人手臂相似的动作功能，并使工作2江苏大学硕士学位论文对象能在空间内自由移动的机械装置，是机器人赖以完成工作任务的实体。与工业机器人机械手不同，果蔬收获机器人的机械手，所处的外部环境是复杂的、多变的、非结构的，并且与果实的栽培方式有很大关系。在收获机器人中，机械手的主要任务就是将末端执行器移动到可以采摘的目标果实所处的位置r7】。收获机械手机构的运动性能指标主要有奇异性、可操作度、避关节极限、避障能力、冗余空间与姿态多样性等。日本冈山大学研制出一种具有7个自由度的番茄收获机械手，如图11所采川。其中包括2个移动关节和5个旋转关节。工图11番茄采摘机械手结构图Fi911Stru删diagramofmanipulatorofpick-tomato13国内外农业机器人研究现状农业领域，相对其它作业而言。果实采摘比较容易实现自动化和智能化，1968年美国学者Schertz和Brown首次提出应用机器人技术进行果蔬的收获【9】。目前，日本、荷兰、英国、法国、意大利、美国、以色列等国都在开展果蔬采摘机器人方面的研究，涉及到的研究对象有：苹果、西红柿、芦笋、黄瓜、葡萄、甘蓝、菊花、草莓、蘑菇等。目前己研制出西红柿、黄瓜、葡萄、柑桔等水果和蔬菜收获机器人f埘。现介绍几种比较典型的果蔬采摘机器人：日本岗山大学NKondo等人研制的番茄采摘机器人【Il】如图12所示。由机械手、末端执行器、视觉传感器、移动机构和控制部分组成。用彩色摄像机作为视觉传感器寻找和识别成熟果实，果实有时可能被叶茎遮住，这要求机械手活动范围大，并能自主避开障碍物，所以采用的是七自由度的SCORBOT-ER工业机器人机械手。为了不损伤果实，机械手的末端执行器是带有软衬挚的吸引器，中间有压力传感器，把果实吸住后，利用机械手臂上的腕关节把果实拧下。行走机构有4个车轮，能在田间自动行走，利用机器人上的光电传感器和设置在地头土埂3江苏大学硕士学位论文的反射板，可检测是否到达土埂，到达后自动停止，转动后再继续前进。从识别到采摘完成约需要15s个，成功率在70左右。图12番茄采摘机器人Fi912Pick-tamtorobot英国Silsoe研究院研制的蘑菇采摘机器人1121，它可以自动测量蘑菇的位置、大小，并选择性的采摘和修剪。它的机械手包括2个气动移动关节和一个步进电机驱动的旋转关节组成；末端执行器是带有软衬媲的吸引器；视觉传感器采用T、，摄像头，安装在顶部用来确定蘑菇的位置和大小。采摘成功率在75左右，采摘速度为67个s，采摘成功率不高的主要原因之一是有些蘑菇倾斜生长，非常不利于机械手的采摘。图13黄瓜采摘机器人Fi913Cucumber-pickrobot4江苏大学硕士学位论文图13所示为日本的Kondo等人研制的黄瓜采摘机器人雕。该机器人采用三菱MITSUBISHIRV-E2六自由度工业机器人，利用CCD摄像机，根据黄瓜比其叶茎对红外光的反射率高的原理来识别黄瓜和叶茎。黄瓜和果梗的连接与番茄不同，因此采用拧摘方法较难，而是用剪断方法，先把黄瓜抓住，用接近觉传感器找出柄，然后剪断，由于黄瓜是长条形，受到叶茎的影响更大，所以采摘的成功率较低，大约在60左右。同样，此机器人需要改进其机器手的结构、采摘工作方式和避障规划功能，以提高采摘成功率，提高采摘速度。以上只介绍几种水果的采摘机器人。目前还开发出的农业机器人有：喷药机器人、蔬菜嫁接机器人、收割机器人、蔬菜水果采摘机器人、林木修剪机器人、果实分拣机器人、耕耘机器人、施肥机器人、除草机器人等。国内在农业机器人方面的研究开始于20世纪90年代中期，相对于发达国家起步较晚，但是发展很快，很多院校，研究所都在进行农业机器人和智能农业机械相关的研究。中国农业大学张铁中教授率先在我国开展了自动化嫁接技术的研究工作，先后研制成功了自动插接法、自动旋切贴合法嫁接技术，填补了我国自动化嫁接技术的空白，形成了具有我国自主知识产权的自动化嫁接技术【141。如利用传感器和计算机图像处理技术，实现了嫁接苗子叶方向的自动识别、判断。嫁接机器人能完成砧木、穗木的取苗、切茁、接合、固定、排苗等嫁接过程的自动化作业。操作者只需把砧木和穗木放到相应的供苗台上，其余嫁接作业均由机器自动完成，从而大大提高了作业效率和质量，减轻了劳动强度。嫁接机器人可以进行黄瓜、西瓜、甜瓜苗的自动嫁接，为蔬菜、瓜果自动嫁接技术的产业化提供了可靠条件。上海交通大学机器人研究所的曹其新、刘成良、吕恬生、殷跃红等人进行了用于精确农业的智能农业机械的研究工作钏1161鲫I，已经完成了智能化联合收割机、蔬菜工厂化育苗播种流水线样机的研制，正在进行草莓挑选机器人、黄瓜采摘机器人、嫁接机器人的研究。浙江大学应义斌教授研究的水果自动分级机器人系统得到国家“863”计划的支持IlS|191，吉林大学的王荣本、于海业从90年代中期开始进行农用自动引导行走车的研究嗍。南京农业大学的沈明霞等人口，浙江工业大学的张立斌等人221，在进行农业机器人的视觉研究。江苏大学纪良文、吴春笃进行了喷药机器人的研究1231，他们采用超声测距作为喷药机器人的辅助视觉系统。江苏大学赵杰文5江苏大学硕士学位论文进行了田间成熟番茄的识别技术研究剀。梁喜凤等为分析并改善番茄收获机械手运动学特性进行了番茄收获机械手运动学与仿真试验【2s11261，取得了较好的效果。14农业机器人存在的问题及解决思路机器人在农业中的应用远远低于工业中的应用，更多的是处于实验室的试验阶段，没有得到实际应用。究其原因：一方面因为农业机器人的成本过高；另一方面是农业机器人没有达到农业生产的智能需要。如果不解决这两个问题，农业机器人很难得到广泛的应用。目前农业机器人的智能化程度，没有达到农业生产的要求，农业工程领域的专家纷纷把研究重心从机械部分转向机器视觉，人工智能方面，力图解决农业机器人的智能问题。从目前的技术水平来看，在自动导航、果实的识别和定位以及障碍物的识别和定位方面已经有了成熟的解决方案，但从总的方面来说，目前的智能系统的发展还不够完善，特别是基于避障路径规划算法研究并没有得到应有的重视125】，仅有的针对采摘机械手的避障研究大部分也集中在末端执行器方面，并没有考虑到机械手关节和连杆在运动过程中的自主避开障碍物，采摘的成功率以及采摘速率难以达到要求，也阻碍了采摘机器人在实际中的应用。15本课题研究的内容本文的主要任务是跟踪当前世界在此领域的先进技术，特别是机器人技术和人工智能，充分利用国内外在这方面或者相关方面已取得的研究成果，提出了一种适合在温室环境中的番茄采摘机器人机械手避障路径规划的简单、快速、有效算法。为以后在硬件上实现番茄采摘机器人路径规划系统打下基础。151技术路线图14番茄采摘机械手路径规划系统示意图Fig14Skotchmapofpathplanningsystemofmanipulatorforpick-tamto6江苏大学硕士学位论文在本课题中，假定目标番茄的位置信息和障碍物的属性和位置信息均已经给定。因此本论文的主要任务是使机器人能根据上述信息，自主的决定机械手各关节的运动路径，避开障碍，使末端执行器到达目标番茄的位冕附近。采摘机械手的避障路径规划算法可以用如图14所示。根据图14所示的流程，本文的研究内容主要如下：1机械手的运动学是避障路径规划研究的基础，本文首先建立了MOTOMANSV3X的运动学模型，并在MATLABroboticstoolbox平台下，对MOTOMANSV3X的运动学进行了仿真。2针对工作空间中可能的障碍物及形状进行了分类及抽象，确定了关节空间(C-空间)为规划空间，利用边界特征对机械手各关节及连杆和末端执行器与工作空间(W-空间)中的障碍物可能的碰撞方向和位置作了几何分析。3结合采摘机器人机械手本体特点和几何分析的结果，提出了一种番茄生长环境中的障碍物均可由一个点或两个，最多三个点的临界碰撞角表示的W-空间到D空间转换的计算方法。通过建立G空间障碍，把复杂的空间环境转换为平面信息，提高了计算机处理的速度。4利用人工智能中重要搜索算法-A算法，在D空间(二维化平面栅格信息)中实现了基于局部采摘环境模型的番茄采摘机械手的避障路径规划，计算机仿真和实物实验均验证了算法的有效性和正确性。152关键问题1、环境空间、障碍物空间抽象化模型的建立。由于采摘番茄工作环境的复杂性和非结构性，给环境的计算机建模带来很大的困难，考虑到采摘机器人的实时性要求，必须对环境空间、障碍物空间模型进行合理的抽象化。2、搜索路径算法的最终确定。由于采摘过程中，计算量很大，如何让搜索算法更简单、有效，满足采摘实时作业要求是本文的另一个关键问题。153预期效果本文的预期效果是研究出快速而有效的番茄采摘机械手避障路径规划算法，期望能够满足实时作业的要求，且具有较高的避障成功率，能为后期在硬件上实现避障路径规划系统打下基础。7江苏大学硕士学位论文16本章小结本章主要阐述了农业机器人的特点及国内外的研究现状，对农业机器人存在的问题及解决思路作了简单的介绍和分析。最后简要介绍了本文的研究内容、关键问题及预期效果。8江苏大学硕士学位论文第二章番茄采摘机械手运动学分析与仿真研究机械手运动学是避障路径规划研究的基础，主要研究机械手臂各杆件之间的位移、速度和加速度关系，运动学问题包括正运动学问题和逆运动学问题。正运动学(Direct呦ali岱、问题是指给出机械手各关节的位置、速度、加速度，求解各杆件的位置、姿态、速度、加速度等运动参数的问题，特别是求解机械手手臂端点(或末端杆件)的位置、姿态、速度、加速度。逆运动学(InverseKinematics)I司题是己知工作所要求的末端执行器的位置、姿态、速度、加速度时，求解能实现这些要求的机械手各关节的位置、速度、加速度等运动参数。21运动学数学基础1271研究机械手的运动，不仅涉及机械手本身，而且涉及各物体间以及物体与机械手的关系。为了描述机器人本身的各个连杆之问、机器人和操作对象以及障碍物之间的运动关系，通常将它们都当成剐体，研究各刚体之间的运动关系。刚体参考点的位置和刚体的姿态统称为刚体的位姿，其描述方法较多，如齐次变换法、矢量法、旋量法和四元数法等。本文采用齐次变换法，其优点在于它将运动、变换和映射与矩阵运算联系起来，便于在MTALAB环境下编程。此外，齐次变换法不仅能够表示动力学问题，而且能够表达机器人控制算法、计算机图形学和视觉信息处理等问题。21I位置和姿态描述在描述物体(如零件、工具或机械手)问的关系时，要用到位置矢量、平面和坐标系等概念。1、位置描述一旦建立了一个坐标系，我们就能够用某个3x1位置矢量来确定该空间内的任一点的位置。对于直角坐标系似，空间任意一点的P位置可用的31列矢量P表示。如公式21所示。其中，Px，Py，Pz是点P在坐标系脚中的三个坐标分量。P的上标A代表参考坐标系tA。我们称4p为位置矢量，如图21所示。除了采用直角坐标系以外，也可采用圆柱坐标系、球(极)坐标系来描述点的位置。9江苏大学硕士学位论文-以AP=I以I也1l见j图21位置爱不不葸图Fig21ketchmapofpositiondescribe2、方位描述研究机器人的运动与操作，往往不仅要表示空间某个点的位置，而且需要表示物体的方位。物体的方位可由某个固接于此物体的坐标系描述。为了固定空间某刚体B的位置，设置一个直角坐标系伊与此刚体固接。用坐标系p的三个单位主矢量、Ys、相对于参考坐标系似方向余弦组成的3x3矩阵rlI吒2，i31BAR=。Y。4z。】=r2。嘞r2，I(22)r3l嘞r33j来表示刚体B对于坐标系似的方位，和称为旋转矩阵。式中，上标A表示参考坐标系A，下标代表被描述的坐标系伊。客共有9个元素，但只有3个是独立的。由于尔的三个列矢量。，。和。都是单位向量，且正好垂直，因而它的9个元素满足6个约束条件(正交条件)。粕。=。yB。YB2。ZB。ZB-1(23)。Y日=。=。Z口4=O(24)对应于轴X，Y或者z作转角为秒的旋转变换，其旋转矩阵为：rl001置力=10cos8一sin8I(25)10sin0一cosOJrcosO0sinelR(_，国=l010l(26)L_sinO0s目Jrcos8一sinOolR亿力=lsin口cosO0l(27)100110江苏大学硕士学位论文图22表示一物体的方位，此物体与坐标系p固接，并相对于坐标系A运动。总之，采用位置矢量描述点的位置，而用旋转矩阵描述物体的方位。幽22坐标方位描述Fig22Sketchmapofgridbc啦describe3、位姿描述为了完全描述刚体B在空间的位姿，通常将物体B与某一坐标系伊)相固接。坐标系伊的原点一般选在物体B的特征点上，如质心、或对称中心等。相对参考坐标系A，坐标系但的原点位置和坐标轴的方位分别由位置矢量。和旋转矩阵扣描述。这样，刚体B的位姿可由坐标系B来描述，即有口)=。最)(28)当表示位置时，式(28)中的旋转矩阵。=I(单位矩阵)；当表示方位时，式(28)中的位置矢量。B=o。212坐标变换空间中的任意点P在不同的坐标系中的描述式不同的。为了阐明从一个坐标系的描述到另一个坐标系的描述关系，需要讨论这种变换的数学问题。坐标变换一般有两种形式：平移坐标变换和旋转坐标变换。1、坐标平移设坐标系伊与A具有相同的方位，但是p的坐标系的原点与A的原点不重合。用位簧矢量。仇描述相对于A的位置，如图23所示。称4办为p相对于A的平移矢量。如果点P在坐标系p中的位置为。P，那么它相对于坐标11江苏大学硕士学位论文系A的位置矢量。P可由矢量相加得出，即：。P=Bp+tpB称式(29)为坐标平移方程。(29)图23坐标平移变换Fig23Sketchmapofcoordinatetranslationtransation2、坐标旋转设坐标系御与A有共同的坐标原点，但是两者的方位不同，如图24所示。翻24坐标旋转变换Fig24Sketchmapofcoordinaterotationtransation用旋转坐标矩阵扭描述伊对于A的方位。同一点P在两个坐标系A和p中的描述。p和8p具有如下变换关系：4P=扣8P(210)称(210)式为坐标旋转矩阵。同样，我们可以用招描述坐标系似相对于p的方位。扭和：R都是正交矩阵，两者互逆。江苏大学硕士学位论文3、复合变抉考虑最一般的情形：坐标系p)的原点与似的原点既不重合，p的方位与A的方位也不相同。我们用位置矢量。如描述伊)的坐标原点相对于A的位置，用旋转矩阵扭描述伊)相对于A的方位，如目f1：25所示。对于任意一点P在两坐标系舛和伊中的描述。p和8P具有以下变换关系：。P=扭。p+。n(211)式(211)可以看成是坐标旋转和坐标平移的复合变换。实际上，规定一个过渡坐标c。c的坐标原点与伊的重要，而c的方位与A的相同。根据式(210)，得到向过渡坐标系的变换：。P=缸8P=口AR。P(212)再由式(29)，得到复合变换：。p=cp+。Pc=客8p+4PB(213)X图25坐标复合变换Fi925Sketchmapofcoordinatecompoundtransation213齐次坐标和齐次变换已知一直角坐标系中的某一点坐标，那么该点在另一直角坐标系中的坐标可通过齐次坐标变换求得。复合变换式(211)对于点8P而言是非齐次的，但是可以将其表示为等价的齐次变换形式：川牟将料13(214)江苏大学硕士学位论文其中，4x1的列矢量表示三维空间的点，称为点的齐次坐标，仍然记为4P或8P。上式可以写成矩阵的形式：。p=P。式中，齐次坐标式。P和8p是4xl的列矢量。与式(211)中的维数不同，加入了第四个元素1。齐次变换矩阵是4x4的方阵，具有以下形式岔_爿斟多综合地表示了平移变换和旋转变换。22D-H描述I篮11291(215)机械手运动学研究的是手臂各连杆间的位移关系、速度关系和加速度关系。机器人操作臂可以看作为一个开式链，它是由一系列连杆通过转动或移动关节串联而成，每个关节有一个自由度。基座称为连杆0，不包含在这6个连杆之内。连杆1与基座由关节1相连；连杆2与连杆1通过关节2相连接，依次类推。开链的一端固定在基座上，另一端是自由的，安装着工具(或称末端执行器)，用于操作物体，完成各种作业。在果蔬采摘中用于直接接触果实并分离果实。关节由驱动器驱动，关节的相对运动导致连杆的运动，使各连杆到达所需的位姿。机械手末端执行器相对于固定参考系的空间描述是路径规划和轨迹规划首先必须要考虑的问题。为了研究机械手各连杆之间的位移关系，可在每个连杆上固接一个坐标系，然后描述这些坐标系之间的关系。1955年，Denavit和Hartenberg提出一种通用的方法，用一4x4的齐次变换矩阵描述相邻两连杆的空间关系【刈，D-H模型表示了对机器人连杆和关节进行建模的一种非常简单的方法。221连杆描述连杆的功能在于保持其两端的关节轴线具有固定的几何关系，连杆的特征也是出由这两条轴线规定的。如图26所示连杆f-1是由关节轴线i-1和f的公法线长度4J一。和夹角q一。所规定的，q一1和q一分别称为连杆f1的长度和扭角，q一的方向规定为从轴线i-1公垂线转至轴线i，q。和噶一完全地定义了连杆i_l的特征。江苏大擘硕士学位论文222连杆参数与关节变量每个连杆由四个参数描述可以用q-l呸，4，6：来描述，a。和嘶一。描述了连杆i1本身的特征：d。和谚描述连杆i-1与连杆i之间的联系。对于旋转关节f，仅6I是关节变量，其他三个参数给定不变；对于移动f关节，仅正是关节变量，其他三个参数给定不变。这种描述机构运动的方法称为D-H方法，坐标系称为D-H坐标系。一个6关节的机器人，用18个参数可以完全表示它的运动学固定部分，而用6个关节变量描述运动学中的变动部分。为了确定机器人各连杆之间的相对运动关系，在各连杆上分别固接一个坐标系。与基座固接的坐标记为田，与连杆i固接的坐标系记为j。下面讨论确定连杆坐标系的方法。基坐标系f讲与基座固接，固定不动，常用它来描述操作臂其他连杆的运动。基坐标系田原则上可以任意规定，但为了简单起见，总是规定，当第一个关节变量为零时，田与m重合。这种规定隐含：a0-0，和=0，当第一个关节是旋转关节时，正=o；第一关节是移动关节时，岛=o。末端连杆坐标系伽的规定与基坐标D相似。值得注意的是，连杆坐标系的设定不是唯一的，选择不同的连杆坐标系，相应的连杆参数将会改变。图26连杆描述不葸图Fig26Sketchmapoflinkdescribe根据所设定的连杆坐标系，相应的连杆参数可以定义如下；4一。=从毛一l到：，沿五一l测量的距离；q一。=从五一l到z，绕五一l旋转的角度；di=从而。到五沿z，测量的距离；q=从而。到墨绕z。旋转的角度。江苏大学硕士学位论文aj一，为连杆i1的长度，因此规定4J。0，而q。，巧和谚的值可正可负。223连杆变换与运动学方程我们为机械手的每个连杆建立一个坐标系，并用齐次变换来描述这些坐标系的相应的位置姿态。通常把描述一个连杆与下一个连杆问相对关系的齐次变换叫做A矩阵，也叫连杆变换矩阵。一个A矩阵就是一个描述连杆坐标系之问相对平移和旋转的齐次变换。连杆坐标系f相对于j-l的连杆变换，显然，与4。与_l，d，和6I这四个连杆参数有关。因此，可以把连杆变换分解为四个基本的子变换：其中每个变换只依赖于一个连杆参数，以便直接写出来。(1)绕互一。旋转，6：使得t-1和相互平行；(2)沿刁一。轴平移码距离，使得再一。和薯共线；(3)沿再。轴平移a，的距离，使得五一。和毛的原点重合；(4)将五一l轴绕薯轴旋转q，使得一。轴与弓轴对准。因为这些予变换都是相对于动坐标描述的，按照“从左到右”的原则，得到As=Rot(_l只)Trans(0，0，d)Trans(a，0，o)Rot(x，啦)(216)由式(216)右边的四个子变换，通过矩阵相乘，可以得到连杆变换的通式：4=cose一shoec稍啦smOcos6cos啦0sinal00smasinalcos毋sinaICOS呸Oqcosaqsine吐l(217)将各个连杆变换正(i=1，2，n)相乘，得到=兀4(218)1=1，称为手臂变换矩阵。它是n个关节变量ql，q2，吼(对于转动关节i，吼=包，对于移动关节，伍=t)的函数，表示末端连杆坐标系n相对于基坐标系O的描述，(儡，仍，吼)=A(q1)A2他)A)(219)根据各关节位置传感器的输出，得到各关节变量吼(i=1，2，n)的值，即可求出，。江苏大学硕士学位论文知0和0知0：1p=一0一墨00：1旦=A妞，如c如，Ac吼，G2IJI：l1、1。72、127“、1”7、7式(220)称为运动方程，它表示末端连杆的位姿(nO，a，P)与关节变量吼，鼋2，吼之间的关系。23MOTOMANSV3X机械手运动学数学模型目前采摘机器人机械手部分大多数仍采用工业机器人本体加上适合农业作业的专用末端执行器对果实和蔬菜进行采摘或收获D”。在本课题中，采用的机械手本体是由日本YASKAWA(安川)抹社会社制造的MOTOMAN-SV3X机械手，它是一种精密轻型垂直关节型机械手系列的机器人，在科研和教学以及工程实践中得到广泛应用。如图27所示：图27MOTOMANSV3X机械手实物图Fig27ManipulatorofMotomansv3x231数学模型的建立MOTOMANSV-3X示意图如图27所示，其中1腰部、2-下臂、3上臂、4手腕回转、5手腕摆动、6-末端执行器，共有六个关节，因此具有六个自由度，且每个关节均为旋转关节。前三个关节确定末端执行器的位置，后三个关节确定末端执行器的姿态，且后三个关节轴线相交于一点，该点作为末端执行器的参考点。为了适应不同的采摘环境，并对同一型号或者相同自由度的机械手具有通用性，本文作以下附加规定：1、(xo，Yo，Z。)为基坐标系，(，儿，Z。)为末端执行器参考点坐标系：17江苏大学硕士学位论文2、互轴与关节f+1的轴线重合，而轴沿连杆i两关节轴线之公垂线，并指向i+1关节，yj轴按右手法则确定：3、6：与其转轴zj的关系符合右手定则，互的正方向规定为垂直向上、水平向右或者垂直于纸面指向观察者。根据上文所述的D-H法则和附加规定，建立D_H标架及系统坐标系如图28所示。图28中，弘，弓)为坐标系、q为连杆长度、q为连杆扭角、西为沿着关节，的两个公垂线的距离、谚为关节变量，f=126。由此确定机器人杆件参数见表21所示。由于MOTOMANSV3X型机器人均是旋转关节，故q、啦、吐都是常数，只有秒是关节变量。图28MOTOMANSV3X机械手示意图Fi鲒8Sketchmapofmanipulator4图29MOTOMANSV3X机械手连杆坐标系Fj929Coordinatesystemoflinkofraanipulator表21gOTOgANsv3x连杆参数Tab21LinkparameterofMOTOMANsv3x根据式(217)和表21所示的连杆参数，可求得各连杆变换矩阵如下：江苏大学硕士学位论文A=q0墨alq焉014l而010dlO00lA=A=c|0一s00c400-10d40OO1A=A=A=其中：岛=sin0，cj=cos瞑，f=L2，。6。由式(217)、式(218)可知，只要给定关节变量吼(谚)，即可求出手臂变换矩阵，从而得到末端执行器的位置和姿态。反之，已知末端执行器的位置和姿态，同样可以得出各关节变量值。=”；D，厅yDy，l；D：00a，P，口yp。口zpz01=(6ln(02地(岛地(幺)A(岛地(嚷)(221)232机械手逆运动学方程番茄采摘机器人系统中，视觉采集及处理系统已获得了机器人机械手末端执行器的采摘工作空间位姿(T矩阵)，但是如何获得六个关节的关节量，这就是采摘机器人的逆运动学求解问题。运动学正解可看作从机器人关节空间(c一空间)至工作空间的非线性一一映射，相应地，运动学逆解则可看作从工作空间到关节空间的非线性映射。从本质上讲，运动学正解和逆解都是机器人关节空间与工作空间之间的非线性映射关系，二者可以相互转换De。机器人运动学运动逆解在机器人学中占有非常重要的地位，它直接关系到运动分析、离线编程、路径规划、轨迹规划等，是将工作空间内机器人末端件的位置、姿势转化成关节量的方法13311341。本文逆运动学求解目的是根据采摘机器人机械手的实际要求，如机械手的初始状态和目标状态，为最终实现的番茄采摘机械手路径规划系统提供输入信息。由式(221)可得p5】：2A=“z茂“A-1(222)令M；AA3A4=蚺o。黾乜OOOOlO白黾OO啦o，OOlO吗岛OO乞岛OOOO以lOOlO吨龟0OOOO0O1鸭0OOOlO岛岛OOc23c4-S23嘞3瓯以+c23a3+42岛膏锄-$。23s40一埚0001时峨j旺lc4IIOIl岛G+c2s5岛屯一岛c5岛一岛以+q只+而弓一qIlHc5+4：us58乒5吃s6+o：c64：以+一盔l(224)frhCstl2GthS5一thC5th一珑唬+最只一q弓Il0001l，幺+43+如c2262d6+c1只+鱼弓一4l(227)蚪arcst呱毫搿卜arctan(二)：泣捌江苏大学硕士学位论文岛=arcsin(蠢专：争卜螂啪(毒卜岛心2”求幺，皖：由MG3)=NG3)，肘(2，3)=(2，3)则：，嘞毛2觚+stn)s6+(ClO，+墨。，概(230)L一3s2他+o。c6由式(230)可解得：壤：删剐塑出型掣(231)。、J艺一tan23幢，k+丑，)只=a“，s(岛，kq一，溅+Goiqo)c6)(232)其中tan23=tan(02+岛)求B：取Ap，2)=口(32)，可得毛岛一仍岛=0(233)由式(234)可解出髓：a“沏nr苎生二鱼生(234)岛=删雨i苏五蓑鬲画2233数学模型的验证为了验证连杆坐标系和D-H参数的正确性，可计算MOTOMANSV3X在初始位置时末端执行器相对于基坐标系的空间位置，由图2-7可知，末端执行器参考点在基坐标系中的初始位置坐标为(150+260+90，0，300+260+60)，即(5，0，620)。另一方面把机械手初始位置时的关节变量岛_-00、岛=900、03=00、B=00、B=oo、纯=00代入连杆变换矩阵及式(221)中得牙=A。如如A4AA=因此末端执行器在初始状态时的位置为(见，所，见)=(500，0，620)，与图28所示的情况完全一致。姗。锄。1OOOOdOOOO1O江苏大学硕士学位论文24基于MATLAB的MOTOMANSV3X逆运动学仿真由于人脑具有处理视觉信息所需要的高度能力，而图像又是沟通信息最自然的手段，因此仿真信息的可视化处理成为仿真技术的重要内容。机器人图形仿真是将机器人仿真的结果以图形的形式显示出来，从而直观地显示出机器人的运行情况，得到从数据曲线或数据本身难以分析出来地许多重要信息361，同时能直观的显示机器人在运动过程中能否顺利的避开障碍物。以往由于机器人图形和动画仿真的计算量很大，在微机上实现效果不佳和根本无法实现，因此大多数的仿真都是基于图形工作站的，由于造价昂贵，从而限制了这项技术的发展。随着计算机硬件以及CAD、CAld技术的飞速发展、加上性能卓越的开放式三维图形标准接口lVlaflab的出现，使得在PC机上实现高品质的机器人三维图形仿真成为现实。MATLAB是由美国lVIathWorks公司开发的目前应用最为广泛的数学软件。提供强大的科学计算、灵活的程序设计流程、高质量的图形可视化与界面设计、以及与其他程序和语言的接口的便捷功制371。目前，MATLAB已成为国际、国内控制领域最流行的设计仿真软件。MATLAB不仅提供了诸如坐标变换、投影变换等基本功能，而且还实现了繁琐的图形消隐。更为有利的是，图形库中不仅提供了点、线、面的绘制函数，还提供了高效的绘制复杂三维物体以及曲线和曲面的函数。借助双缓存在技术增强了机器人三维物体动画实时显示的连续性和动感性。特别是与机器人控制箱的集合，利用工具箱库作为MATLAB与WINDOWS的接I=1，开发出具有立体感的三维效果图形软州381。随着M_ATLAB70的出现和性能的提高，MATLAB在WINDOWS平台上广泛应用更促进快速开发高效、低成本的数学计算和工程仿真软件。所以在机器人控制运动仿真中，利用此强大的图形界面进行开发，实现模拟机器人的仿真运动。目前国内也有一些学者从事这方面的研究f394011411421。241仿真模型的建立本文的采摘机器人机械手本体采用的MOTOMANSV3X型机械手，如图27和图28可知，它是一个六自由度系统，由底座、底盘、大臂、小臂、手腕、末端关节等几个部分组成。从几何造型来看，底坐、底盘和末端关节是圆柱体，其余部分是多面体。本文中，把多面体用圆柱体进行包洛，易于避障分析理论模型的建立。因此，机械手的造型过程按照结构化立体造型法的方式来进行，整台机江苏大学硕士学位论文械手可分解为底坐、底盘、大臂、小臂、手腕、末端关节六个部分，每个部件按照与前一关节的相对位置进行描述。机械手的具体几何模型是自底向上的过程，如图210所示：图210机械手几何模型构造示意图Fig210Sketchmapofconstructionofgeometricmodel仿真是在MATI_AB70、：Roboticstoolboxl431平台上进行的，根据图29和表2-1以及MOTOMANSV3X机器人的各关节的属性、仿真模型的参数选取如下所不21、基本参数表2-2MOTOMANSV3X机器人仿真基本参数Tab22SimulationbasicparameterofMotaman表22中。Alpha、A、Theta、D的意义及单位分别和D-H描述中的口、a、0、d的相同。2、结构参数表23MOTOMANSV3X机器人仿真结构参数Tab23SimulationconfigurationparameterofMotoman表23中sigma(cr)机械手关节的外形，0为圆柱型，非0为棱型。根据上述参数，建立MOTOMANSV3X的仿真模型如下图2-9所示：江苏大学硕士学位论文图211MOTOMANSV3X机械手仿真模型图Fi9211SimulationmodelmapofmanipulatorofMotomansv3x242运动学仿真说明(1)仿真环境：MicrosoftgrmdowsXP(Professional、ServicePack21操作系统、CPU为170Gk(In