六足爬行机器人毕业论文

CDIO实验报告实验项目名称：六足爬行机器人所属课程名称：工程实践实验日期2012年9月1日—12月28日班级姓名（学号）成 绩摘要随着计算机、网络、机械电子、信息、自动化以及人工智能等技术的飞速发展，移动机器人的研究进入了一个崭新的阶段。在腿型行走机器人的研究中，双足机器人是研究的比较多的项目，同时，多足机器人的研究也占了很大的比例。多足机器人能够比较好的适应周围的地形环境，但是相对于双足机器人来说，由于要兼顾多条行走机构之间的相互配合，在编程过程中就需要考虑多方面的因素，在本项目中，我们使用创新套件的部件组装了一个六足爬行机器人（新型移动智能机器人），这就涉及到了六条行走走机构的协调问题。多足机器人能够直走前方有一个传感器被遮挡则往反方向运动当两个传感器被遮挡是则向后运动，实现简单的避障行走，具有运动稳定性好，适应能力强，控制方便的优点，它可以轻易地跨过比较大的障碍并且机器人所具有的大量的自由度可以是机器人的运动更加灵活，对凹凸不平的地形的适应能力更强，复杂的肢体结构和简易灵巧的运动控制策略，因此多足机器人在未来有广阔的前景。关键词：移动机器人；六足爬行机器人；协调；避障；稳定性；新型移动智能机器人HexapodcrawlingrobotAbstractWiththerapiddevelopmentofcomputer,network,mechanics,electronics,informatics,automationandartificialintelligenttechnologyetc,roboticshasenteredanewstage.Legtypewalkingrobot,abipedrobotismoreresearchprojects,atthesametime,themulti-leggedrobotalsoaccountedforalargeproportion.Multi-leggedrobotisabletobetteradapttotheenvironmentofthesurroundingterrain,bipedrobottotakeintoaccounttheinteractionbetweenthewalkingmechanismintheprogrammingprocess,youneedtoconsidermanyfactorsinthisproject,weuseinnovativesuiteofcomponentsassembledasix-leggedcrawlingrobot,whichinvolvessixwalkingawayagencycoordinationproblem.Multi-leggedrobotabletogostraightinfrontofasensorisblockedthemovementintheoppositedirectionwhenthetwosensorsareobstructedbackwardmovement,simpleobstacleavoidancewalking,movement,goodstability,adaptability,andcontroloftheadvantagesofeasyitcaneasilycrosstheobstaclesandtherobothasalargenumberofdegreesoffreedomcanbethemovementoftherobotismoreflexible,strongeradaptabilityruggedterrain,thecomplexstructureofthelimbsandsimpleandsmartmotioncontrolstrategy,somulti-leggedrobothasbroadprospectsinthefuture.Keywords:Mobilerobot;Hexapodcrawlingrobot;Coordination;obstacleavoidance;stability;MT-UROBOT目录目录....................................................4一、引言..............................................61.1课题背景..................................................................61.2本项目研究的意义...........................................................61.3国内外发展状况.............................................................71.4本文的研究方法.............................................................8二、设计原理及方法比较.................................92.1多足机器人相关简介.........................................................92.1.1原理及其实物图...........................................................92.1.2多足机器人的相关应用....................................................102.2MT-U控制器模块...........................................................102.2.1自由度..................................................................112.2.2控制按键部分............................................................112.3舵机及舵机控制卡模块......................................................122.3.1舵机原理介绍...........................................................122.3.2舵机控制卡.............................................................132.4微控制器模块..............................................................142.4.1微控制器................................................................142.4.2扩展控制主芯片..........................................................152.5传感器模块................................................................162.5.1红外传感器..............................................................16三、方案设计.........................................163.1步态分析..................................................................163.2步态设计..................................................................173.2.1初始化..................................................................173.2.2直行行走（前方无障碍）..................................................173.2.3转向（前方有障碍）.....................................................右转（左上方检测到障碍，或者左边有过近的障碍）........................左转：（右上方检测到障碍，或右边有过近的障碍）.........................后退（正前方发现障碍，且无法转弯）....................................183.3程序设计..................................................................183.3.1主程序设计..............................................................183.3.2运动程序设计............................................................19四、 作品调试......................................... 22五、 调试结果 235.1 设计成果： 235.2后续研究工作展望： 23六、 结论 24参考文献 24致 谢 25附 录 26一、引言1.1 课题背景本次项目设计是响应我们学校的 《工程实践》这课程首次提出的。众所周知自1999年高校开始扩大招生规模以来，高校毕业生就业率日趋下降，而全球性的金融危机更是让就业形势变的更加不容乐观。其次，当今社会所需要的人才是全面发展的综合性人才，而我国高校所培养的人才多在综合能力上有所欠缺，专业技能也不够强、动手能力差、不善于去探讨研究、合作意思更是差，很难把所学的专业知识熟练地应用到实际工作中在社会上也很难找到适合自己的工作。所以我们学校针对就业问题对我们工科学院的同学提出了cdio的项目设计。项目任务是以CDID理念为指导，在机器人创新实验室的平台上设立多个子项目，融合构思、设计与实现，完成机器人设计与实现的全过程。通过本项目的实施，来锻炼我们的动手能力，合作意识，养成研究精神，提高我们的综合能力。本项目设计涉及到先修课程为《工程导论》、《C语言程序设计》。在《工程导论》中培养学生的工程意识，明确工程师担当的角色，学习项目设计的相关知识，并通过《C语言程序设计》，使学生具备一定的软件编程能力。在2-5学期的《工程实践》课程的学习中，相继学习《电路分析基础》、《模拟电子技术》、《数字电路与逻辑设计》等课程，系统地学习电路、模拟电路、数字电路的基本知识。然后学习本课程，将所学电路、模拟电路、数字电路、软件编程等知识运用到《工程实践》的项目设计中，使学生初步具备工业控制系统的开发能力。后修课程为《智能小区》、《智能控制》、《机电控制系统设计》等，可进一步在工业自动化控制领域完成控制系统的集成设计与实现。1.2本项目研究的意义人类在认识自然、改造自然、推动社会进步的过程中，不断地创造出各种各样为人类服务的工具，其中机器人技术的开发和研究更具有划时代的意义。“机器人产业在二十一世纪将成为和汽车、电脑并驾齐驱的主干产业。”从庞大的工业机器人到微观的纳米机器人，从代表尖端技术的仿人型机器人到孩子们喜爱的宠物机器人，从民用的机器人到军用的机器人，机器人正在日益走近人们的生活，成为人类最亲密的伙伴。总体来说机器人技术综合了机械和精密机械、微电子和计算机、自动控制和驱动、传感和信息处理以及人工智能等多种学科的最新研究成果，是典型的机电一体化技术。它包括传感器与感知系统、驱动、建模与控制、自动规划与调度和计算机系统等。机器人的研究与应用水平，是一个国家经济实力和科技发展水平的反映，一个国家如果不拥有一定数量和质量的机器人，就不具备产品国际竞争的工业基础。因此，世界上许多国家，包括中国在内，都对机器人的发展予以高度的重视。本文六足机器人是一种基于仿生学原理研制开发的新型足式机器人。新型机器人比传统的轮式机器人有更好的移动性，它采用类拟生物的爬行机构进行运动,自动化程度高，具有丰富的动力学特性。此外，足式机器人相比其它机器人具有更多的优点：它可以较易地跨过比较大的障碍（如沟、坎等），并且机器人足所具有的大量的自由度可以使机器人的运动更加灵活，对凹凸不平的地形的适应能力更强；足式机器人的立足点是离散的，跟地面的接触面积较小，因而可以在可达到的地面上选择最优支撑点，即使在表面极度不规则的情况下，通过严格选择足的支撑点，也能够行走自如。因此，足式步行机器人的研究已成为机器人学中一个引人注目的研究领域，由于六足机器人强大的运动能力，可以提供给运动学、仿生学和机械构造原理研究有力的工具。在研究昆虫运动方式、关节承力、稳定姿态调整的过程中，可以运用本机器人对设想的虫体姿态、运动过程进行模拟，最大程度地接近真实，将理论和实践联系起来，从而更好地观察昆虫运动模式的优点，以及探究哪些现象能够运用到机械设计的实践中去。这对于以上学科的研究和探索都是十分有意义的。当然，我们还可以作为教学器械，通过研究昆虫爬行时各脚的运动情况，用机械形式表达出来，也可以作为仿生玩具及探险、搜救设备，还可以进入细小管道、地洞中勘察。在研究了昆虫步态的基础上，运用仿生原理，本文借鉴相关资料，使用一种六足机器人机械结构设计的新思路，制作了基于这种设计思路的机器人。相信该机器人可以实现直线运动与转向运动的合理、有效结合，转向角度、角速度可控，直线行进步距、速度可调，行动灵活可靠。就我们学校而言Cdio工程实践为我们提供了一个大家合作动手参与制作机器人的平台；为我们开创了一个了解、认识、熟悉机器人技术的机会；提高了我们的综合能力；也为我们工科学生大学毕业能够找到合适的工作，减轻我们的就业压力。1.3国内外发展状况机器人技术的发展从无到有，从低级到高级，随着科学技术的进步而不断深入发展。谈到足式机器人，当然目前主流大多是联想到和人相似、有亲切感的双足机器“人”，从某一层面来看，以双足步行为演化上的一个极为小众的特例，本身对达到稳定运作控制的困难度很高，从了解「生物出生到可以开始自行运动所需的时间」便可以窥知一二。从另一个角度来看，人类所能自在运动的地表也局限在某一些型态之中，若要探讨如何在各式自然地形上运动的法则，势必得回过头来探讨多足动物的运动机制。而从物理直觉来评析，单就在崎岖路面上运动的稳定性来探讨，采用多足机器人会比较简单且实际。基于这一些原因，仿生多足机器人的研发便有了背后的动机，模仿经过长时间演化后动物的构造，藉由观察牠们的运动，了解为什么有如此的动作，再利用机构或是控制去完成。在自然界中，我们看到体型较大、有优秀运动能力的动物像马、猎豹、羚羊等等都是四只脚的哺乳类动物，但考虑到稳定性却是六足比较占优势， 只要用简单的三脚步态（tripod gait)即可让重心轻易落在支撑的三角形中。四足动物的脚可能需要比较大的力量才能表现出他的特性， 但人类尚无法仿造出重要的肌肉和控制系统，以现有机构和马达组成的系统， 重量太重而无法有效运动。 这时，自由度的选择以及机构设计便成了一个很重要的课题。 近年来为实现生产过程自动化， 已有不少操作机器人广泛应用于生产过程， 尤其是那些人力所限和人所不及的环境或危险场所，将是机器 人进一步发展的应用领域。日 前，美军的蜂鸣机 器人(Mini-Drohne) 在巴基斯坦击毙了一名恐怖分子嫌疑人。在未来，这种昆虫型机器人有望成为战场上的主角。在美国，军事科技研究一般拥有数亿美元的巨资作为后盾。美国国防部高级研究计划局(DARPA)常为各个大学和自由经济体的科研项目慷慨解囊。军方亦拥有独立的大型研究实验室，然而其大部分研究成果从未公之于众。尽管如此，目前披露的成果足以令人惊叹不已，智能型战斗机器人、自动汽车、植入电脑芯片的动物等等令人联想起扣人心弦的科幻电影——这一切都有可能在未来的高科技战场上大显身手。目前，多足仿生机器人的研究基本上是基于模仿自然界中昆虫的运动步态（如蚂蚁）来设计的，通常都会选择周期规则步态作为仿生多足机器人的步态规划依据。虽然该类多足仿生机器人的脚具有较大的自由度，但是其控制起来较为烦琐，并且不能精确的定位。中国与国外相比，目前还存在一定的差距，虽然掌握了机器人操作机的设计制造技术、控制系统硬件和软件设计技术、运动学和轨迹规划技术，生产了部分机器人关键元器件，但可靠性低于国外产品；机器人应用工程起步较晚，应用领域窄，生产线系统技术与国外比有差距。中国的智能机器人和特种机器人也取得了不少成果。但是在多传感器信息融合控制技术、遥控加局部自主系统遥控机器人、智能装配机器人、机器人化机械等的开发应用方面则刚刚起步。随着社会文明程度的提高，对机器人的要求也会越来越高。中国要做好充分的准备迎接新的技术挑战。1.4本文的研究方法本文六足机器人是一种基于仿生学原理研制开发的新型足式机器人。机器人行走：步态：六足机器人采用经典三角步态， 左1，3，右2组成第一组，右1，3，左2组成第二组。分别对两组足操作，分别完成抬腿与跨步的动作。一、直走：（测距传感器收到前方无障碍信号）抬腿：第一组足抬腿，第二组足下撑完成准备工作；前行：第一组前推，第二组向后推，带动机器人前行；复位，第二次抬腿：第二组足抬腿，第一组足下撑完成准备工作；前行(2)：第二组前推，第一组向后推，带动机器人前行；以上完成第一步行走，循环执行完成连贯行走动作；二、右转：（左上方检测到障碍，或者左边有过近的障碍）抬腿：第一组足抬腿，第二组足下撑完成准备工作；转弯：左边脚前推，右边脚略微后推，通过左右脚相互间的扭力完成右转动作；复位，第二次抬腿：第二组足抬腿，第一组足下撑完成准备工作；重复完成转弯动作三、左转：（右上方检测到障碍，或右边有过近的障碍）抬腿：第一组足抬腿，第二组足下撑完成准备工作；转弯：右边脚前推，左边脚略微后推，通过左右脚相互间的扭力完成右转动作；复位，第二次抬腿：第二组足抬腿，第一组足下撑完成准备工作；重复完成转弯动作二、设计原理及方法比较2.1多足机器人相关简介机器人技术集机械、电子、计算机、材料、传感器、智能控制等多种技术于一体，代表了机电一体化技术的核心成就。目前许多国家都投入大量的人力物力对它的基础理论和应用技术进行了广泛的研究，机器人技术水平的高低和应用成就，在一定程度上体现了一个国家科技发展水平的高低，它的应用在很大程度上可以促进工业基础，特别是装备制造业技术水平和能力的提高。近年来，随着人类对在复杂环境中既具备高移动能力，又具高可靠性，且易于扩展的移动平台日益迫切的需求，有相当多的研究探讨两足至多足机器人的应用，过去两足机器人多为轮型机构系统，其运动局限于二维平面，无法克服许多困难山区崎岖的地形。因此，人类开始思考创造类似人类、昆虫、动物等运动模式的仿生爬行机器人。仿生六足爬行机器人是一种基于仿生学原理研制开发的新型足式机器人。与传统的轮式或履带式机器人相比，足式机器人自由度多、可变性大、结构复杂、控制繁琐，但其在运动特性方面具有独特的优点：首先是足式机器人具有较好的机动性，对不平地面的适应能力十分突出，由于其立足点是离散的，与地面的接触面积较小，因而可以在可能达到的地面上选择最优支撑点，从而能够相对容易地通过松软地面(如沼泽和沙漠)以及跨越比较大的障碍(如沟、坎、台阶等)；其次是足式机器人的运动系统可以实现主动隔振，允许机身运动轨迹与足运动轨迹解藕。尽管地面高低不平，机身的运动仍可达到相当平稳；再次是在不平地面和松软地面上的行进速度较高，而能耗较少。正是由于上述特点，足式机器人正日益成为机器人技术领域的研究热点。2.1.1原理及其实物图从下方的图2.1中可以看到我们的六足爬行机器人的大致组装图，需要的组装配件包括：MT-U控制器，MT-U控制器扩展板，两个舵机控制器，另外还需要两根四针的串口线将两个舵机控制器连接到MT-U控制器扩展板上的串口1和2；六足爬行机器人需要12个舵机组成机器人的六条腿型行走机构，每个腿型机构需要两个舵机支撑，每个舵机控制板控制六个舵机；在六足爬行机器人前方还需两个红外传感器， 通过传感器检测以避开机器人前方的障碍物。图 多足机器人的相关应用在自然界和人类社会中存在一些人类无法到达的地方和可能危及人类生命的特殊场合。如行星表面、灾难发生矿井、防灾救援和反恐斗争等，对这些危险环境进行不断地探索和研究，寻求一条解决问题的可行途径成为科学技术发展和人类社会进步的需要。地形不规则和崎岖不平是这些环境的共同特点。从而使轮式机器人和履带式机器人的应用受到限制。以往的研究表明轮式移动方式在相对平坦的地形上行驶时，具有相当的优势运动速度迅速、平稳，结构和控制也较简单，但在不平地面上行驶时，能耗将大大增加，而在松软地面或严重崎岖不平的地形上，车轮的作用也将严重丧失移动效率大大降低。为了改善轮子对松软地面和不平地面的适应能力，履带式移动方式应运而生但履带式机器人在不平地面上的机动性仍然很差行驶时机身晃动严重。与轮式、履带式移动机器人相比在崎岖不平的路面步行机器人具有独特优越性能在这种背景下多足步行机器人的研究蓬勃发展起来。而仿生步行机器人的出现更加显示出步行机器人的优势。多足步行机器人的运动轨迹是一系列离散的足印运动时只需要离散的点接触地面对环境的破坏程度也较小可以在可能到达的地面上选择最优的支撑点对崎岖地形的适应性强。正因为如此多足步行机器人对环境的破坏程度也较小。轮式和履带式机器人的则是一条条连续的辙迹。崎岖地形中往往含有岩石、泥土、沙子甚至峭壁和陡坡等障碍物可以稳定支撑机器人的连续路径十分有限,这意味着轮式和履带式机器人在这种地形中已经不适用。多足步行机器人的腿部具有多个自由度使运动的灵活性大大增强。它可以通过调节腿的长度保持身体水平也可以通过调节腿的伸展程度调整重心的位置因此不易翻倒稳定性更高。当然多足步行机器人也存在一些不足之处。比如为使腿部协调稳定运动从机械结构设计到控制系统算法都比较复杂相比自然界的节肢动物仿生多足步行机器人的机动性还有很大差距。2.2MT-U控制器模块如图2.2所示2.2.1自由度独立驱动电源/电池供电：6V，2000mAH，专用充电器，自动检测充电状态，过流过热保护控制方式：独立控制芯片，最多可达12自由度控制通讯方式：串口控制（UART），可直接与MT-U机器人通讯；有2个小灯，它们指示 MT-UROBOT所处的状态。图控制按键部分开关按钮：控制MT-UROBOT电源开关的按钮，按此按钮可以打开或关闭机器人电源；电源指示灯：按下MT-UROBOT的开关后，这个灯会发绿光，这时可以与机器人进行交流了；充电指示灯：当你给机器人充电时，充电指示灯发红光；充电口：将充电器的相应端插入此口，再将另一端插到电源上即可对机器人充电；下载口：充电口旁边的下载口用于下载程序到机器人主板上，使用时只需将串口连接线的相应端插入下载口，另一端与计算机连接好，这样机器人与计算机就连接起来了；复位/MTOS按钮：这是个复合按钮，用于下载操作系统和复位。当串口通信线接插在下载口上时，按击此按钮，机器人系统默认为此操作为下载操作系统；如果你想使用其复位功能则需要将通信线拔下，按击此按钮，机器人系统认为此操作为系统复位；运行键：打开电源后，按击运行键，机器人就可以运行内部已存储的程序，按照你的指令行动；扩展电源：在主控盒得前侧有专门的备用电源接口，用户可以直接MT-UROBOT的连接和检测MT-UROBOT的连接；除了运行MT-UROBOT的情况外，很多情况下MT-UROBOT是要和计算机连接以后使用的。连接MT-UROBOT是一项基本操作，下面是连接的标准步骤：取出串口连接线。一头接MT-UROBOT的“下载口”插口，另一头接PC机箱后的9针串口。如果你的电脑后面没有空余9针串口，请咨询电脑维护人员。（可以把暂时不用的设备移开，腾出一个串口。）打开MT-UROBOT，按击控制按键中的“开关”键，见到“电源”指示灯发光即可。开机后液晶显示屏LCD显示正常。有两个选择：“运行”和“下载”用户可以通过左侧的上下按钮进行选择，运行 OK后进入运行或者下载。4.如果机器内有多个其他程序，在运行后会有 1、2、、、一共8 个程序，通过上下选择按钮可以选择其中某一个程序，按运行按钮就可以运行。如果液晶屏是空白的，检查电池是否有电，接触是否良好，请充电或更换电池。如果没有小太极图或者小太极图不跳动，说明操作系统没有正常运行，按复位键重启系统（注意此时拔掉通信线）。5.MT-UROBOT的检测：双击程序运行图标，会要求你新建（或打开）“流程图”或“C语言程序”，选择进入“流程图”界面，这样就进入了图形化编程界面。在此界面中你可以简单操作一个流程图标，然后编译、下载，就可以等待出现“下载成功！”，说明自检程序已经下载到机器人的操作系统中。或者选择“C语言程序”，进入程序编写界面，输入编写的程序，然后编译、下载，就可以等待出现“下载成功！”，说明自检程序已经下载到机器人的操作系统中。自检程序下载完毕后，我们可以开始检测MT-UROBOT了。拔下串口连接线，并将机器人带到安全的地方（空旷，无障碍平地），进行自检。2.3舵机及舵机控制卡模块2.3.1 舵机原理介绍型号与相关参数：MG995金属齿轮结构 双滚珠轴承连接线长度30厘米尺寸：40mmX20mmX36.5mm重量：62g技术参数：无负载速度0.13秒/60

0.17秒/60度(6.0V)

度(4.8V)

图

2.3

舵机扭矩：11KG使用温度:-30~~+60摄氏度死区设定:4微秒工作电压:3.0V-7.2V舵机是一个通过输出轴的转动来控制物体转动的装置。它在特定编码的驱使下可以转动到工作范围内的任意给定位置。因为它的轻便，位置控制简易且精确，在机器人中使用尤多。多舵机的协调工作可满足机器人的多自由度要求。舵机可以实现0~180°的旋转，当轴处于正确位置时，舵机将停止运动；当所在位置不对时，电路则驱动电机运转到需求位置。舵机是采用比例调节方式控制速度。当舵机转动的距离大时，则加载在舵机上的能量就大，反之亦然。而舵机的转动角度控制是由加载在控制线上的脉冲周期完成的， 即所谓的脉冲编码调制。舵机设定为每隔 20ms接受一个脉冲，这样的话，实际脉冲长度就可用来控制舵机转动的角度。如图 2.4，例如1.5ms的脉冲可使舵机转动 90°。图2.4 舵机转角与脉冲周期关系图当舵机安装好后，需要获取其初始位置值，为编程做准备；通常情况下，将舵机电源线直接接到舵机控制卡上，开启控制卡电源，舵机自动复位时的位置作为初始位置，并参照此位置组装电路以及编写驱动程序。2.3.2 舵机控制卡控制卡最多可以同时控制八个舵机的运动，在上电状态下（舵机控制器不和其他的控制部件进行通讯），8个舵机控制端口分别输出周期为20ms,宽度为1.5ms的脉宽调制波。因此，按照舵机的控制原理，如果各个控制端口上接有舵机，则各个舵机输出角为90度。控制卡需要接收从串口发来的控制信息，控制信息中包括舵机要转动的角度以及其他的通讯协议。此时舵机控制卡就是一个外接控制器控制信号与舵机动作之间的转换板。（1）端口定义舵机控制卡上有八个舵机接口，分别定义为（Servo1～Servo8）；每个舵机输出口可以控制舵机转动（-90°～+90°）；外接控制器上的串口（UART）直接和AT89C2051的串口相连，采用9600的波特率，此接口可以和其他采用TTL电平的设备进行通讯。（2）通讯协议a 接收数据：每一帧控制指令：4个字节；第一个字节：0xAF---- 数据帧起始字节；第二个字节：0x00～0x08---- 舵机序号（Servo1～Servo8）；第三个字节：0x00～0xB4---- 旋转角度设定（-90°～+90°）；第四个字节：0xFA----数据帧结束字节。返回数据：当正确接收一帧数据之后，伺服舵机控制器会返回一个字节数据做为接收确认信息。返回数据为：大写字母‘ R’的ASCII码。说明：舵机控制板通过串口和外部控制器进行通讯，外部控制器要让某个舵机转动一定的角度，则需要按照一定的通讯协议向向舵机控制板发送控制信号。例如：要使用外接控制器的串口2，令第4个舵机转动40度，就可以使用下面的控制方法。voidsend(intcom,intmotoID,intangle){serial_send(com,0x00AF);serial_send(com,motoID);serial_send(com,(char)angle);serial_send(com,0x00FA);}send(2,4,40);图2.5 舵机控制卡2.4微控制器模块大学版智能机器人计算机硬件的设计策略是尽量选择高速、功能齐全、可靠、周边设备集成度高的微控制器。大学版智能机器人微控制器采用TI公司生产的高速数字信号处理器TMS320LF2407A。同时，充分考虑到软件开发工具问题。因为没有优秀方便的软件开发工具，硬件只能成为专有系统，而无法成为开发平台。公司创新性的在DSP2407A上实现的自下载功能，使我们拥有了纯软件开发调试的优秀工具C语言。C语言即可用于开发高层应用软件，又便于开发低层驱动，还能交互调试。同时还兼容了汇编语言的编程功能。2.4.1微控制器TI公司生产的TMS320系列DSP的体系结构专为实时信号处理而设计，该系列DSP将实时处理能力和控制器外设功能集于一身,适合于应用在控制系统中。TMS320系列中TMS320LF2407A是TI公司最新推出的高性能16位DSP，是240X家族中的新成员，是定点DSPC2000平台系列中的一员，专门为电机控制与运动控制数字化优化实现而设计。它集C2XX内核增强型TMS320设计结构及适用于控制的低功耗、高性能、优化外围电路于一体，CPU内部采用增强型哈佛结构，四级流水线作业，相对于过去的16的微处理器和微控制器，具有更高的性能和可靠性。主控制CPU是TQFP封装，具有144个引脚。如图3.23CPUTMS320LF2407A的CPU是基于TMS320C2XX的16位定点低功耗内核。体系结构采用四级流水线技术加快程序的执行，可在一个处理周期内完成乘法、加法和移位运算。其中央算术逻辑单元（CALU）是一个独立的算术单元，它包括一个32位算术逻辑单元（ALU）、一个32位累加器、一个16×16位乘法器（MUL）和一个16位桶形移位器，同时乘法器和累加器内部各包含一个输出移位器。完全独立于CALU的辅助寄存器单元（ARAU）包含八个16位辅助寄存器，其主要功能是在CALU操作的同时执行八个辅助寄存器（AR7至AR0）上的算术运算。两个状态寄存器ST0和ST1用于实现CPU各种状态的保存。TMS320LF2407A采用增强的哈佛结构，芯片内部具有六条 16 位总线，即程序地址总 线（PAB）、数据读地址总线（DRAB）、数据写地址总线（DWAB）、 图2.6主控制DSP芯片程序读总线（PRDB）、数据读总线（DRDB）、数据写总线（DWEB），其程序存储器总线和数据存储器总线相互独立， 支持并行的程序和操作数寻址，因此CPU的读/写可在同一周期内进行，这种高速运算能力使自适应控制、卡尔曼滤波、神经网络、遗传算法等复杂控制算法得以实现。存储器配置TMS320LF2407A地址映象被组织为三个可独立选择的空间：程序存储器（64K）、数据存储器（64K）、输入/输出（I/O）空间（64K）。这些空间提供了共192K字的地址范围。其片内存储器资源包括：544字×16位的双端口数据/程序DARAM、2K字×16位的单端口数据/程序SARAM、片内32K×16位的Flash程序存储器、256字×16位片上BootROM、片上Flash/ROM具有可编程加密特性。TMS320LF2407A的指令集有三种基本的存储器寻址方式：立即寻址方式、直接寻址方式、间接寻址方式。2.4.2 扩展控制主芯片扩展控制主芯片采用复杂可编程逻辑芯片（ CPLD）EPM7064STC100，EPM7064S是Altera 公司推出的速度非常快的高性能、高集成度可编程逻辑器件，属于MAX7000系列，是特殊的可编程ASIC芯片。在第二代MAX结构基础上，采用先进的CMOSEEPROM技术制造的，是100引脚的TQFP封装，芯片内部是一个包含有大量逻辑单元的阵列，采用了连续式的布线结构，因而可以通过设计模型精确地计算信号在器件内部的时延。具有集成度高、工作速度快和在线编程方便的特性，适合于时序、组合逻辑电路以及输入/输出口扩展的应用；8通道10位模数转换（A/D），转换速度500ns；6通道数字量输入（DI），每个通道具有光电隔离功能；5通道数字量输出（DO），每个通道电流可达500mA；2路异步通信接口（串口SCI）；CAN总线通信接口；3路MTBUS总线接口（包括并行接口、IIC接口、同步通信接口（SPI））；扩展卡电源级联接口；扩展卡信号级联接口；5V电源扩展接口；CPLD可编程JTAG口。图2.7扩展版功能图2.5传感器模块2.5.1红外传感器测量射程范围：10-80cm最大允许角度：>40°电源电压：4.5-5.5V平均功耗：33-40mA5.峰值功耗：约200mA图2.8更新频率/周期：25Hz/40ms模拟输出噪声：<200mV精度和采集的AD位数以及转化计算公式相关，10AD一般能达到0.1CM.测量距离与输出模拟电压关系：2.4V~0.4VMT-UROBOT机器人的红外传感器共包含两种器件：红外发射管和红外接收管可以安装于MT-UROBOT机器人的正前方，两只红外发射管安装于红外接收管的两侧；同时红外发射管也可以安装于MT-UROBOT机器人的正前方，两只红外接受管安装于红外发射管两侧。而且他们也可以安装到灭火风扇支架上面,因而可以说MT-UROBOT机器人提供给了用户更多的发挥自主创新的空间。红外发射管可以发出红外线，红外线在遇到障碍后被反射回来，红外接收管接收到被反射回来的红外线以后，通过 A/D转换送入CPU进行处理。MT-UROBOT机器人的红外传感器能够看到前方 10cm～80cm，90°范围内的比210mmx150mm面积大的障碍物，如果障碍物太小太细、或者在它的可视范围图2.9以外，它可就没法看到了。在MT-UROBOT机器人的可视范围内，它的可视距离是可以调整的，具体参见后面传感器部分。三、方案设计3.1步态分析六足步行机器人的步态是多样的，其中三角步态是六足步行机器人实现步行的典型步态。三角步态走法是六足昆虫的基本走法，其规则是每次落脚和提脚前进时相应的一组足都是正三角型，这可以确保步态的稳定。六足昆虫在地面上行进时，多以交替的三角步态运动，即在步行时将六只足分为两组，以身体一侧的前足、后足与另一侧的中足作为一组，其他三只足作为另一组。在同一时间内只有一组的三条足起行走作用：前足用爪固定物体后拉动虫体前进，中足用以支撑并举起所属一侧的身体，后足则推动虫体前进，同时使虫体转向，行走时虫体向前并稍向外转，三条足同时行动，形成一个稳定的三角形，然后再与另一组的三条足交替进行，通过两组足交替地摆动和支撑，从而实现昆虫的快速运动。其步态如图3.1所示。图3.1 三角步态运动示意图3.2步态设计3.2.1初始化项目设计共使用12个舵机用于步态实现。每条腿上有两个舵机，分别控制髋关节和膝关节的运动，舵机安装呈正交，构成垂直和水平方向的自由度。由于腿只有水平和垂直平面的运动自由度，所以只考虑利用三角步态实现直线行走。分别给12个舵机编号（left1~left6 ，right1~right6 ），从头至尾部分别有奇数号舵机控制垂直方向，偶数号舵机控制水平方向运动。分组：左1，3，右2组成第一组，右1，3，左2组成第二组。分别对两组足操作，分别完成抬腿与跨步的动作。编程控制伺服舵机旋转角度（0°~180°），初始化设置所有舵机处于90°状态，此时机器人处于待机状态。3.2.2 直行行走（前方无障碍）抬腿：第一组足抬腿，第二组足下撑完成准备工作；前行：第一组前推，第二组向后推，带动机器人前行；复位，第二次抬腿：第二组足抬腿，第一组足下撑完成准备工作；前行(2)：第二组前推，第一组向后推，带动机器人前行；以上完成第一步行走，循环执行完成连贯行走动作，如图 3.1所示；3.2.3 转向（前方有障碍） 右转（左上方检测到障碍，或者左边有过近的障碍）抬腿：第一组足抬腿，第二组足下撑完成准备工作；转弯：左边脚前推，右边脚略微后推，通过左右脚相互间的扭力完成右转动作；复位，第二次抬腿：第二组足抬腿，第一组足下撑完成准备工作；重复完成右转动作 左转：（右上方检测到障碍，或右边有过近的障碍）抬腿：第一组足抬腿，第二组足下撑完成准备工作；转弯：右边脚前推，左边脚略微后推，通过左右脚相互间的扭力完成右转动作；复位：第二次抬腿：第二组足抬腿，第一组足下撑完成准备工作；重复完成左转动作 后退（正前方发现障碍，且无法转弯）抬腿：第一组足抬腿，第二组足下撑完成准备工作；前行：第一组后推，第二组向前推，带动机器人后退；复位，第二次抬腿：第二组足抬腿，第一组足下撑完成准备工作；前行(2)：第二组后推，第一组向前推，带动机器人后退；以上完成第一步行走，循环执行完成连贯行走动作；3.3程序设计3.3.1 主程序设计软件的主要功能是使机器人在向前行进的过程中能够避开障碍物，在本项目中一共采用两个远红外避障传感器实现（具体实现过程参见步态设计）。本项目程序采用模块化设计，通过程序调用实现功能，具体调用函数如下：#include<stdio.h>110;//舵机控制卡控制协议#includeintangle_right5=70;{"ingenious.h"intangle_right6=70;serial_send(com,0intangle_left1=90;x00AF);intcount=0;intangle_left2=80;serial_send(com,JintAD1=0;intangle_left3=120;ointID);intAD2=0;intangle_left4=90;serial_send(com,(intAD7=0;intangle_left5=90;char)angle);intAD8=0;intangle_left6=100;serial_send(com,0x00FA);voidzhizou();}intangle_right1=20;voidyouzou();//六条腿型结构的初始voidzuozou();voidmain()化角度voidhoutui();{intangle_right2=90;serial_init(9600)intangle_right3=90;voidsend(intcom,int;intangle_right4=JointID,intangle)send(2,1,angle_right1);",AD2);",AD2);//初始化六条腿型结构send(2,2,angle_riMprintf(7,"AD6=1"Mprintf(5,"ght2);,AD1);AD7=%d",AD7);send(2,3,angle_ri}ght3);elseifMprintf(5,"AD8=%dsend(2,4,angle_ri(AD2>200&&AD1<",AD8);ght4);180)send(2,5,angle_ri{Mprintf(7,"AD6=1"ght5);,AD1);send(2,6,angle_riyouzou();}ght6);elsesend(1,1,angle_leMprintf(3,"if(AD1>250&&AD2>ft1);AD1=%d",AD1);250)send(1,2,angle_le{ft2);Mprintf(3,"AD2=%dsend(1,3,angle_le",AD2);sleep(10);ft3);send(1,4,angle_leMprintf(5,"houtui();ft4);AD7=%d",AD7);send(1,5,angle_leMprintf(3,"ft5);Mprintf(5,"AD8=%dAD1=%d",AD1);send(1,6,angle_le",AD8);ft6);Mprintf(3,"AD2=%dsleep(10);Mprintf(7,"AD6=1"",AD2);while(1),AD1);{AD1=AD(1);}Mprintf(5,"AD2=AD(2);elseifAD7=%d",AD7);if(AD1<(AD1>200&&AD2<180&&AD2<180)180)Mprintf(5,"AD8=%d{{",AD8);zhizou();zuozou();Mprintf(7,"AD6=1",AD1);Mprintf(3,"Mprintf(3,"}AD1=%d",AD1);AD1=%d",AD1);}}Mprintf(3,"AD2=%dMprintf(3,"AD2=%d3.3.2 运动程序设计六足步行机器人采用对称结构设计，其程序也具有相当强的相似性，其完成各个动作均能通过改变相应舵机的转动角度实现，所以在此只列出直行程序示例，完整程序请参阅附录；直行程序例程：voidzhizou()

if(angle_left1<12

}{

0)

}while(count

!=do

{

1);{/*垂直方向*/if(count==0){ // 右1、3

angle_left1=angle_left1+2;

/*/*

另外三条腿的动作水平方向*/do

*/左2着地向下伸脚

60°

{send(1,1,angle_le

if(count==1)20)

if(angle_right1<1

ft1);

}

右边

{//支脚前推1、3左边2

60°{if(angle_left5<12

if(angle_right2<1angle_right1=angl

0)

20)e_right1+2;

{

{send(2,1,angle_right1);

angle_left5=angle_left5+2;

angle_right2=angle_right2+2;}if(angle_left3>60

ft5);

send(1,5,angle_le

send(2,2,angle_right2);)

}

}{if(angle_right3>2

if(angle_left4>60angle_left3=angle

0)

)_left3-2;

{

{ft3);

send(1,3,angle_le

angle_right3=angle_right3-2;

angle_left4=angle_left4-2;}if(angle_right5<1

send(2,3,angle_right3);

ft4);

send(1,4,angle_le20)

}

}{angle_right5=angle_right5+2;send(2,5,angle_right5);}

if(angle_left1>=120&&angle_left5>=120&&angle_right3<=60&&angle_right1>=120&&angle_left3<=90&&angle_right5>=120){

if(angle_right6<100){angle_right6=angle_right6+2;//左边右2着地向上抬脚

1、360°

1;

count

=

send(2,6,angle_right6);}

}60°左边

//向后移脚1、3右边2

2;

}

左脚

//右脚1、3向上抬脚

2，90°}if(angle_left2<11

}while(count

!=

if(angle_left1>700)

2);

){

/*

垂直方向*/

{doangle_left2=angle_left2+2;

{

if(count==2)

angle_left1=angle_left1-2;{//左脚

2、右send(1,2,angle_le

脚1、3收回脚

send(1,1,angle_left2);

ft1);}

if(angle_right1>1

})if(angle_left6<13

{

if(angle_left5>700)

){

angle_right1=angle_right1-2;

{angle_left6=angle_left6+2;

send(2,1,angle_right1);

angle_left5=angle_left5-2;send(1,6,angle_le

}

send(1,5,angle_left6);

ft5);}

if(angle_left3<17

}0)if(angle_right4>8

{

if(angle_right3<10)

50){

angle_left3=angle_left3+2;

{angle_right4=angle_right4-2;

send(1,3,angle_le

angle_right3=angle_right3+2;ft3);send(2,4,angle_right4);

}

send(2,3,angle_right3);}

if(angle_right5>4

}0)if(angle_left2>=110&&angle_left6>=130&&angle_right4<=80&&angle_right2>=120&&angle_left4<=60&&angle_right6>=100){

{angle_right5=angle_right5-2;send(2,5,angle_right5);

if(angle_left1<=70&&angle_left5<=70&&angle_right3>=150&&angle_right1<=0&&angle_left3>=170&&angle_right5<=40){count=

}

count

=3;

}}

send(1,6,angle_le}

if(angle_right6>4

ft6);}while(count

!=

0)

}3);

{/*

移动水平方向

*/

if(angle_right4<1do{

angle_right6=angle_right6-2;

40)

{if(count==3)脚左边

{//向前前移右2右边1、3

send(2,6,angle_right6);

angle_right4=angle_right4+2;}0)

if(angle_right2>6

左脚左边

//向后移动1、3右边2

send(2,4,angle_right4);{

}if(angle_left2>50angle_right2=angle_right2-2;send(2,2,angle_right2);}

){angle_left2=angle_left2-2;

if(angle_left2<=50&&angle_left6<=70&&angle_right4>=140&&angle_right2<=60&&angle_left4>=120&&angle_right6<=40)send(1,2,angle_le

{if(angle_left4<12

ft2);

count

=0)

}

0;//重复开始动作{

}if(angle_left6>70

}angle_left4=angle

)

}while(count

!=_left4+2;

{

0);}send(1,4,angle_le

angle_left6=angleft4);

_left6-2;四、作品调试本研究在比较了基于行为的体系结构、 基于知识的体系结构和混合式控制结构之后,选择混合式控制结构,将计划作为一种行为 ,与实时避障行为间采取并行处理方式。相对应地,传感器检测区域为避障区如果在避障区有障碍物 ,则由应激避障模块采取相应的避障行为 ;该模块具有最低优先级 ,在无任务情况下 ,机器人根据传感器选择无障碍物方向直线运行 。目的是在全局环境下 ,暂时无任务的机器人同样保持清醒的意识和一定的机动性 ,避免冲突 。趋向目标模块 。完成对机器人各模块进行任务分配和协调。 当完成特定任务时，该模块选择规划模块，此时机器人采用的是基于知识的体系结构， 当完成一般任务时，该模块选择实时避障模块，此时机器人采用的是基于行为的体系结构。针对不同的任务，机器人可以采用不同的处理方式， 提高了系统的可靠性和反应能力。 实时避障模块。机器人在运动过程中 ,如果红外线传感器测得近障区有障碍物存在， 则机器人暂时停止趋向子目标的运动 ,实施应激避障行为，之后重新规划目标。经多次试验证实：机器人步态平稳、行走良好，能自动识别障碍物和躲避障碍物，在出现突发障碍物时，机器人有一段思考时间，会出现一个小小的停顿，作出应急反应。机器人避障行走时智能控制，适应性强、抗干扰能力好。五、调试结果5.1 设计成果：1.制作了一台小型六足机器人，该机器人长 45cm，高15cm，宽25cm，每只腿上有两个自由度，通过串口通讯发送控制字符，实现机器人的运动试验。通过物理样机的调试，验证了所规划运动姿态的可行性，获得了设计、控制和调试六足机器人的直接经验，为以后的研究工作积累了必要的技术储备。5.2后续研究工作展望：本次课题的完成构建了六足机器人的物理样机， 虽然取得了一定的成果，但是，还属于初步研究，今后要在以下方面取得改进和进一步的研究。结构的进一步优化和选材的改进改进制作机器人的材料，选用更轻巧坚固的材料，使机器人的重量减轻；选择更加紧凑的结构配置，使得机器人的物理样机性能更加坚固、稳定、美观。驱动方式的改进本次课题完成的机器人是外接电源供电的。 下步的研究应该使机器人的电源内置与机器人本体当中。 只有机器人本体脱离了电源线的限制，才能实现机器人的大范围自主活动，但是自带电源的容量有限制了机器人的活动时间。增加机器人的功能如运用六维力传感器、摄像头、红外传感器等，增加机器人对外部信息的搜集和处理能力，使得机器人具有自主识别环境的能力， 在识别的基础上，根据客观环境的变化自主地选择合适的步态和行进的路径。随着传感器系统的建立，使机器人具有语音识别能力和图像的识别能力。不断完善程序，使其具有绘图、写字等简单的功能。六、 结论我们组的同学完成了机器人的组装， 并且完成了复杂的编程，使智能机器人能够避开障碍物行走。但由于我们的机器人是六足的，所以比较耗电。简单的说就是能够直走前方有一个传感器被遮挡则往反方向运动当两个传感器被遮挡是则向后运动，具有运动稳定性好，适应能力强，控制方便的优点，它可以轻易地跨过比较大的障碍并且机器人所具有的大量的自由度可以是机器人的运动更加灵活，对凹凸不平的地形的适应能力更强， 复杂的肢体结构和简易灵巧的运动控制策略，因此多足机器人在未来有广阔的前景。参考文献郭鸿勋、陈学东.六足步行机器人机械系统，机械工程学报，2008.雷静桃、高峰、崔莹.多足步行机器人的研究现状及展望.北京，机械设计学报，2006.罗孝龙、罗庆生等.仿生六足机器人多电机控制系统的研究与设计.计算机测量与控[期刊].2008.蒋新松，机器人学导论[M]，辽宁，辽宁科学技术出版社，1993．徐小云，颜国正，丁国清．微型六足仿生机器人及其三角步态的研究【J]．光学精密工程，2002，8(10)：392．393漆向军，陈霖，刘明丹．控制六足仿生机器人三角步态的研究[J】四川农业大学信息与工程技术学院：2007(4)六足爬虫机器人说明，教学型六足爬行机器人参考手册致 谢在论文即将完成之际，我想向曾经给我帮助和支持的人表示衷心的感谢。首先感谢控制工程学院，给我提供这么好的学习生活环境，在校学习和生活的日子是我一生中一段难忘的经历。本文是在老师的热情关心和指导下完成的，他渊博的知识和严谨的治学态度使我受益匪浅。对顺利完成本课题起到了极大的作用。叶老师积极乐观的生活态度，以诚相待的处事原则，给我留下了深深的印象，在此向他表示我最衷心的感谢！感谢生我养我的父母，他们给了我无私的爱，我深知他们为我求学所付出的巨大牺牲和努力。祝福他们，以及那些给予我关爱的长辈，祝他们幸福、安康！最后向在百忙之中评审本文的各位专家、老师表示衷心的感谢！附录全套完整程序#include<stdio.h>voidhoutui();ght5);#includesend(2,6,angle_ri"ingenious.h"voidsend(intcom,intght6);JointID,intangle)send(1,1,angle_leintcount=0;//舵机控制卡控制协议ft1);intAD1=0;{send(1,2,angle_leintAD2=0;serial_send(com,0ft2);intAD7=0;x00AF);send(1,3,angle_leintAD8=0;serial_send(com,Jft3);ointID);send(1,4,angle_leserial_send(com,(ft4);intangle_right1=20;char)angle);send(1,5,angle_le//六条腿型结构的初始serial_send(com,0ft5);化角度x00FA);send(1,6,angle_leintangle_right2=90;}ft6);intangle_right3=90;sleep(10);intangle_right4=voidmain()while(1)110;{{AD1=AD(1);intangle_right5=70;serial_init(9600)AD2=AD(2);intangle_right6=70;;AD7=AD(7);intangle_left1=90;send(2,1,angle_riAD8=AD(8);intangle_left2=80;ght1);intangle_left3=120;//初始化六条腿型结构if(AD1<intangle_left4=90;send(2,2,angle_ri180&&AD2<180)intangle_left5=90;ght2);{intangle_left6=100;send(2,3,angle_right3);zhizou();voidzhizou();send(2,4,angle_rivoidyouzou();ght4);Mprintf(3,"voidzuozou();send(2,5,angle_riAD1=%d",AD1);Mprintf(3,"AD2=%d",AD2);Mprintf(5,"AD7=%d",AD7);

Mprintf(3,"AD1=%d",AD1);Mprintf(3,"AD2=%d",AD2);

/*六足行走的第一步，下面六条if 语句控制三条腿的动作，根据三点确定一个面的原理，三条腿能支撑六足平台的稳定*/Mprintf(5,"AD8=%d",AD8);

Mprintf(5,"AD7=%d",AD7);

voidzhizou(){Mprintf(7,"AD6=1",AD1);

Mprintf(5,"AD8=%d",AD8);

do{}else if(AD2>200&&AD1<

Mprintf(7,"AD6=1",AD1);

if(count==0){180)

}

if(angle_right1<1{

else

20)if(AD1>250&&AD2>

{250)youzou();

{

angle_right1=angle_right1+2;sleep(10);Mprintf(3,"AD1=%d",AD1);

houtui();

send(2,1,angle_right1);}Mprintf(3,"AD2=%d",AD2);

Mprintf(3,"AD1=%d",AD1);

if(angle_left3>60)Mprintf(5,"AD7=%d",AD7);

Mprintf(3,"AD2=%d",AD2);

{Mprintf(5,"AD8=%d",AD8);

Mprintf(5,"AD7=%d",AD7);

angle_left3=angle_left3-2;send(1,3,angle_leMprintf(7,"AD6=1"

Mprintf(5,"AD8=%d

ft3);,AD1);

",AD8);

}}else if

Mprintf(7,"AD6=1"

if(angle_right5<1(AD1>200&&AD2<

,AD1);

20)180)

}

{{angle_right5=angl}

e_right5+2;zuozou();

}send(2,5,angle_ri

count=

}ght5);

1;}

}

if(angle_left2<11}

0)if(angle_left1<12

}while(count

!=

{0)

1);{

/*的动作*/

另外三条腿

angle_left2=angle_left2+2;angle_left1=angle

do_left1+2;

{

send(1,2,angle_lesend(1,1,angle_le

if(count==1){

ft2);

}ft1);}

if(angle_right2<1

if(angle_left6<1320)

0)if(angle_left5<12

{

{0){

angle_right2=angle_right2+2;

angle_left6=angle_left6+2;angle_left5=angle_left5+2;

send(2,2,angle_ri

send(1,6,angle_leght2);

ft6);send(1,5,angle_le

}

}ft5);}

if(angle_left4>60

if(angle_right4>8)

0)if(angle_right3>2

{

{0){

angle_left4=angle_left4-2;

angle_right4=angle_right4-2;angle_right3=angle_right3-2;

ft4);

send(1,4,angle_le

send(2,4,angle_right4);send(2,3,angle_ri

}

}ght3);}if(angle_left1>=120&&angle_left5>=120&&angle_right3<=60&&angle_right1>=120&&a

if(angle_right6<100){angle_right6=angle_right6+2;

if(angle_left2>=110&&angle_left6>=130&&angle_right4<=80&&angle_right2>=120&&angle_left4<=60&&angle_right6>=100)ngle_left3<=90&&angl

{e_right5>=120){

send(2,6,angle_right6);

2;

count

=})3);}{}while(count!=do2);angle_left1=angle{_left1-2;if(count==3)do{{send(1,1,angle_leif(count==2)ft1);if(angle_right2>6{}0){if(angle_right1>1if(angle_left5>70))angle_right2=angl{{e_right2-2;angle_right1=anglangle_left5=anglesend(2,2,angle_rie_right1-2;_left5-2;ght2);send(2,1,angle_risend(1,5,angle_le}ght1);ft5);if(angle_left4<12}}0){if(angle_left3<17if(angle_right3<10)50)angle_left4=angle{{_left4+2;angle_left3=angleangle_right3=anglsend(1,4,angle_le_left3+2;e_right3+2;ft4);send(1,3,angle_lesend(2,3,angle_ri}ft3);ght3);if(angle_right6>4}}0){if(angle_right5>4if(angle_left1<=70)0&&angle_left5<=70&&angle_right6=angl{angle_right3>=150&&ae_right6-2;ngle_right1<=0&&anglangle_right5=angle_left3>=170&&angle_send(2,6,angle_rie_right5-2;right5<=40)ght6);{}send(2,5,angle_ricount=ght5);3;if(angle_left2>50}})}{if(angle_left1>70}while(count!=angle_left2=angle_left2-2;

{send(1,2,angle_le

voidzuozou(){

angle_left1=angle_left1+2;ft2);

}

do{

send(1,1,angle_leif(angle_left6>70

if(count==0){

ft1);

}){

if(angle_right1<1

if(angle_left5<1220)

0)