毕业论文-仿生六足机器人行走策略研究

仿生六足机器人行走策略研究 摘要 移动机器人中轮式和履带式移动方式已获 得了广泛应用，但是足式移动方式具有轮式和履带式移动方式所没有的优点：足式移动机器人的立足点是离散的，通过合理选择支撑点，能灵活调整行走姿态，具有良好的避障和越障能力。步态是足式移动机器人的迈步方式，是足式移动机器人各条腿协调运动的规律，是其它控制算法实现的基础。本文设计的六足机器人主要仿生“六足纲”昆虫的行走步态， 采用多舵机分时控制，实现三脚和四脚的六足步态，并具有避开障碍物的功能。 针对仿生六足机器人的作业任务和工作环境对 其探测系统的要求,提出了基于红外线传感器的复合探测装置的设计方案,扩大 了机器人探测的范围;克服了传统的避障控制策略中环境模型难以建立的缺点. 在设计中首先进行了对六脚步行机器人脚 部以及各部件的装配、而后主要通过Basic Stamp编辑器，利用PBASIC语言，设计程序，并完成机器人运动控制程序的设计，在实现数据的正常通信后，利用计算机传输给 Basic Stamp 控制器，写入程序。最后通过实验来实现预先设计的几个运动控制形态。并且验证它机械性能的良好，与程序设计的正确性。从而完善并完成对六脚步行机器人的控制设计任务。 关键词 ：六足机器人，Basic Stamp，仿生，避障 - I - 仿生六足机器人行走策略研究 ABSTRACT Wheeled and tracked mobile robots in the mobile mode has been widely used, but foot mobile way has the advantage which not found in wheeled and tracked mobile robots. The foot mobile way robots standpoint is discrete. Support point by a reasonable choice. It can adjust its walking posture flexibility and it has good ability of obstacle avoidance. Gait is the stride foot mobile robot mode, is sufficient mobile robot coordinated movement of all legs of the law is the basis of other control algorithm . This design of the main bionic 2hexapod robot six foot program, Insect walking gait, Time-sharing multi-actuator control, to achieve six-foot three-legged and four-footed gait, and has the function to avoid the obstacle. In order to meet the demand of the hexapod walking bio-robots task and working environment, this paper puts forward the arrangement of the complex sensing system based on multiple infrared sensors, which enlarges the robot s detecting range. We use the control obstacles avoiding strategy, which overcomes the disadvantage that the environment model is difficult to set. In the design, it completed the assemble of robots leg and other parts at first. Then it uses the Basic Stamp editor, Pbasic language, design program and complete the design of robots motion control program. After normal communicate of data, translate program to Basic Stamp control by computer. At last, to realize several motion control form which are designed beforehand. Whats more, ensuring mechanical behavior work right and the accurate of program design. To complete the control design duty of hexapod walking bio-robot. Key words: hexapod walking bio-robot， Basic Stamp，avoiding strategy - II - 仿生六足机器人行走策略研究 目 录 第一章 绪论 .1 1.1 仿生六足机器人发展的背景及研究意义.1 1.2 仿生多足机器人研究现状及发展趋势.2 第二章 仿生六足机器人的硬件组装 .4 2.1 仿生六足机器人的硬件结构.4 2.2 BASIC Stamp微控制器.6 2.3 机器人伺服马达.7 第三章 仿生六足机器人步态规划 .10 3.1仿生机器人步态研究的几个基本概念 .10 3.2三角步态 .11 3.2.1 三角步态原理 . 11 3.2.2 腿部动作与占空系数 . 13 3.2.3 稳定性分析 . 14 3.3 四足步态、波动步态与三角步态的区别.17 3.4各种步态的分析 .19 第四章 仿生六足机器人运动控制程序设计 .21 4.1 六足步行机器人编程初始值的设定.21 4.2 指针指令的使用.23 4.3 三足步态、四足步态的正向直行程序设计.24 第五章 基于红外传感的实时避障技术 .30 5.1红外传感器的硬件安装 .30 5.1.1 用 FREQOUT命令测试红外发射探测器 . 33 5.2 基于红外传感的软件编程.40 第七章 结论与展望 .43 参考文献 .44 致 谢 .45 - III - 仿生六足机器人行走策略研究 - IV - 附录 1 .46 附录 2 .50 附录 3 .55 仿生六足机器人行走策略研究 第一章 绪论 1.1 仿生六足机器人发展的背景及研究意义 众所周知，自然界中的各种生物通过物竞天择和长期进化，已对外界环境产生了极强的适应性，在能量转换、运动控制、姿态调节、信息处理和方位辨别等方面还表现出了高度的合理性，已日益成为人类开发先进技术装备的参照物，仿生机器人便是仿生学与机器人学相结合的产物。相比而言，仿生多足机器人具有其它类型机器人所不具备的一些优点，如其具有较强的运动灵活性和较高的功能适用性、较好的越障和避障能力，以及其运动系统可以实现自主隔振等，因此仿生多足机器人及其相关技术的研究与开发已经成为当前机器人技术领域应人关注的热点。 自20世纪80年代美国著名机器人学专家R.B.McGhee等人开始研究四足步行机器人以来，多组步行机器人的研究一直是世界范围内众多学者关注的热点和难点问题，其研究状况也如加拿大著名机器人学专家J.Angeles教授指出的“步行机器人的基础理论研究步伐要远滞后于其他技术开发的步伐”。之所以会存在这样的状况，主要是因为多足步行机器人是一种具有冗余驱动、多支链、事变拓扑运动机构的特种机器人，它集构造设计技术、电子技术、控制技术、传感器技术、信息处理技术于一体，代表着当代科技发展的前沿水平，是以国家科技综合是实力的集中体现，必须投入巨大的人力、物力、和财力，且必须在基础研究合适应用等方面取得大量突破和丰富积累，才能取得相应的发展与进步。因此，若想在步行机器人技术领域有所作为和有所建树，我们必须瞄准步行机器人的世界先进水平奋起直追，争取在尽可能短的时间内赶上和超过世界先进水平3。 与其他多足机器人相比六足机器人具有以下几个特点： （1）控制结构简单。这是因为昆虫体型和行走步态比较简单，在机器人的设计过程中可以对其控制系统和行走装置进行简化。 （2）行走平稳。昆虫行走时为多足支撑，可以始终保持稳定的地面支撑，所以系统比较容易实现平稳行走。其三角步态行走法是基于三角型稳定性原理的一种步态，可确保昆虫稳定地行走。 （3）作为行走机构的腿部的数目属于冗余设计。即使有的腿损坏无法工作， 其他腿仍可以完成一定的行走。具有的大量的自由度可以使机器人的运动更加灵活，对凹凸不平的地面的适应能力更强9。 因此研究仿生六足机器人可以适应复杂地形的工况要求。 - 1 - 仿生六足机器人行走策略研究 1.2 仿生多足机器人研究现状及发展趋势 早期的六足机器人 随着美国宇航总署对外太空探测计划的不断深入，迫切需要一种可以在未知复杂星球表面执行勘探任务的机器人。由于六足机器人的所具有的这方面优点，使其早在上世纪八十年代就已被列入资助研究计划。其研究成果包括八十年代末的Genghis2和九十年代初的Attila和Hannibal。Genghis是由irobot公司研制于80年代，每条腿装有两个电机，使得它可以自由行动，但是因为每腿只有两个自由度，行动有些笨拙。采用递归控制结构，可以使Genghis在复杂路面上行走，包括横越陡峭的地势，爬过高大的障碍，避免掉下悬崖。 九十年代中期的六足机器人 对于跨海登陆作战的部队来说，浅滩地雷是无疑是最危险也最头疼的登陆障碍，出于这点考虑，美国麻省理工大学和其下的is-robot公司得到国防部高级研究计划局的资助，研制了两代浅滩探雷机器人Ariel。 近年完成的典型六足机器人Scorpion2是由美国波士顿东北大学海洋科学中心自主水下机器人研究小组和德国Fraunhofer 自主智能系统研究所（AIS ）共同完成于2001 年。这项工程的目标是运用集成来自行为学实验和无脊椎动物的神经生物学数据的低级行为指令，通过高级的控制模式来组成行为序列，实现复杂的行为。机器人的设计是根据来自多足节支动物的解剖学数据。其采用机器人的行走控制基于两个仿生控制元：中央模式生成元和基本运动的高级行为元。 通过对仿生多足机器人研究现状的分析，可以看到仿生多足机器人呈现出新的特征和发展趋势。其主要表现在： （1）自然界中生物的结构和技能远比现在所涉及的机器人更为合理，因此应该进一步开展生物观测分析实验，在充分研究生物机体结构和运动特征的基础上，完善仿生多足机器人的设计。研发人员要在“仿生”二字上下功夫，不仅要“形似”，而且要“神似”。 （2）与单个腿式机器人相比，多个足式步行机器人的总负荷更大，可以携带的仪器和工具更多，功能性更强。他们之间通过通信惊醒协调，也可以按照某种规则制定主机器人和从机器人，从而按照一定的队形和顺序对目标进行不同的测量和操作。而且当其中某一退市机器人出现故障时，其他机器人还可以照常工作，大大提高了工作效率和可靠性。因此，仿生多足机器人的研究影响着群体化发展。 （3）自重构式仿生多足机器人比起固定结构的多租机器人对地形的适应性更强，可应用的场合更多。可以根据作业人物和环境的不同，变换体态和形状。如当- 2 - 仿生六足机器人行走策略研究 穿越管道时，它可以变成蛇形；当穿越崎岖的地形时，它可以变成腿式机器人。因此，自重构机器人是仿生多足机器人发展的重要方向之一。 1.3 课题的研究内容与过程 对仿生六足机器人的研究主要包括以下几个方面的内容： （1）对机器人的机构进行运动学分析 （2）认识机器人控制系统硬件。包括主控电路的认识，舵机控制器的认识。 （3）研究了仿生学的基本知识和应用于昆虫行走的三角步态，通过对其步态分析，占空系数分析、稳定性分析、转弯步态和转弯半径分析设计出六足机器人三角步态行走以及转弯的程序。提出四足步态和波动步态，并编写四足步态程序和三足步态比较。 （4）将所设计的控制系统软硬件在机器人本体上进行实验，对其三角步态、四足步态进行验证。 （5）在机器人身上安装红外传感，使之实现避障功能。 （6）编写相应程序使机器人实现避障该功能。 - 3 - 仿生六足机器人行走策略研究 第二章 仿生六足机器人的硬件组装 2.1 仿生六足机器人的硬件结构 材质： 采用国家标准铝材。表面特别处理，防腐蚀防刮痕。 尺寸（长X 宽）：49.68厘米X40.00厘米 腿到腿距离 外设可扩展空间：本六脚爬虫机器人提供达1819.35平方厘米的可扩展空间。 高度： 站立高度15.24厘米 蹲下高度12.34厘米 离地净高： 8.89厘米 腿部运动方式： 2自由度（垂直及水平） 负荷能力： 3.4千克 自重： 1.81千克（带电机） 电子部件： 帕拉斯教学板，微处理器，帕拉斯伺服电机控制器 伺服电机规格： 铁芯有刷电机，自润滑防护轴承 4.8伏扭矩：3.0千克/厘米，6.0伏扭矩：3.7千克/厘米 4.8伏速率：0.19秒/60度 无负载，6.0伏速率：0.15秒/60度 无负载 尺寸：40毫米X20毫米X36.5毫米 自重：43克 可配附件： 云台系统、传感器安装板、红外测距套件、5伏适配器套件、镍锰电池、充电器、教学板电源、电机电源、其他部件等。 图2-1 仿生六足机器人的外观 - 4 - 仿生六足机器人行走策略研究 机构设计是仿生六足机器人系统设计的基础，整机机械结构、自由度数、驱动方程式和传动机构等都会直接影响机器人的运动和动力性能。同时，仿生六足机器人机构设计出了需要满足系统的技术性能外，还需要满足经济性要求，即必须在满足机器人的预期技术指标的同时，考虑用才合理、制造安装便捷、价格低廉以及可靠性高等问题。 为了行走稳定起见，仿生六足机器人在行走过程中，机体中心的投影必须落在三条支撑足的支撑点所构成的三角形区域内，其情况如图2.2所示。其中箭头表示重心的移动。因为当重心靠近边界时会使机器人的稳定性急剧降低。 图2-2 支撑点构成的三角形 六脚步行机器人的是由三大部分构成，机器人主体、六只带动机器人运动的腿、机器人的伺服控制器芯片与Basic Stamp控制芯片。如图2-31 - 5 - 仿生六足机器人行走策略研究 图 2-3 机器人的三大构成部分 机器人的运动是靠 12 个伺服电机作为动力，每只腿分配 2 个电机（2*6）,分别控制机器人的两个自由度，即水平方向和竖直方向（纵向），腿部是由伺服控制器控制完成运动过程，水平推动机器人的前进或后退，竖直方向则是利用机器人的关节拉高或降低。机器人的主体部分，是由两块铝制板材和两块侧板构成，安装比较简单，但它具有一定的物理意义：固定机器人的六只脚，换言之不使六只脚有任何非移动范畴的松动，并使 6 只脚在机器人平放地面时保持同一水平面，每只脚必须与地面完全接触。因为不稳固的安装会使每只腿的移动距离存在误差，从而使机器人行走时出现摔落情况，十分危险。另外两个控制器也安装其上（如图 2-3） ，因此要求比较高的稳定性。 2.2 BASIC Stamp微控制器 BASIC Stamp微控制器是以PBASIC为编程语言，通过解释器对PBASIC应用程序进行解释执行的微型计算机，具有8路或16路I/O通道，每个I/O通道接脚可直接连接到按钮开关、发光二极管、扬声器、电压计及其它TTL装置等。通过增加一到两个额外元器件，I/O接脚就可以连接到非TTL装置，如线圈、继电器等2。 BASIC Stamp由一个9伏特电压调节器、晶振器、Serial EEPROM、及一个PBASIC- 6 - 仿生六足机器人行走策略研究 解释器组成。PBASIC程序储存在非挥发性(断电后程序仍可保留)的Serial EEPROM内，解释器从EEPROM读取已储存的程序代码或将数据写进去。解释器每次读取一条指令，经过解释后对输入/输出接脚、或本身内部结构进行适当的操作。PBASIC 程序储存在EEPROM中，程序可无限地被重复修改及写入。 Basic Stamp 控制器的主要功能是完成计算机与机器人的数据传输（Basic Stamp 指令的传输） ，计算机用 15 针 COM 口数据线与控制器相连，传输各种 Basic Stamp 程序，在控制器收到数据后再次传输给伺服电机控制器。伺服控制器与机器人的六只脚通过电路相连，对它们进行直接控制，而后伺服电机带动机器人运动。 图2-4 basic stamp 板 2.3 机器人伺服马达 伺服马达主要有三根线，分别为：黑、红、白，其中红色的为电源线（最大为6V 直流电压）；黑色为地线；白色的为控制信号线（一般输入周期为 23ms 左右的脉冲序列），通过对这信号线输入脉冲序列来控制电机的运动，可以控制电机的运动速度，运动方向）。所有马达使用前必须进行零位校正。 - 7 - 仿生六足机器人行走策略研究 图2-5 伺服马达 通过利用一种信号来控制这些伺服电机的位置，称这种信号为脉冲序列。伺服电机内安装有一个机械限位器，防止电机转动超出其设定的运动范围。伺服电机内还装有一个位置反馈装置，这样一来伺服电机内的控制电路才能知道在响应脉冲序列时转到哪。本次使用六脚步行机器人的电机能转过最大角度约270度。 从 Basic Stamp 控制 器中发送出的一组控制伺服电机的控制信号被称为“脉冲序列” ，控制器能通过编程产生这样的信号波型，而且还能用它任意的一个I/O口进行信号输出。 伺服电机控制器和控制器用一根信号传输线相连，接到控制器的P15串口，在伺服电机控制器上有16个通道用于和伺服电机相连接。本文所提到的六足机器人由于只有12个伺服电机，所以只用其中的12个通道用于和伺服电机的连接。ch0、ch2、ch4、ch6、ch8、ch10 六个通道用于连接水平方向的电机，ch1、ch3、ch5、ch7、ch9、ch11六个通道用于连接垂直方向的电机。 图2-6显示的信号是发送到与P12连接的伺服电机校准信号，又称零点标定信号。伺服电机调好之后，这个信号就可以指示 电机保持静止。这个指令是由间隔20ms,脉冲带宽1.5ms的一组系列脉冲组成。 把PULSOUT命令和PAUSE命令放在DO.LOOP循环语句之间就可以产生该信 - 8 - 仿生六足机器人行走策略研究 图 2-6 1.5ms 脉冲宽度的时间矢量图 号，从图中很容易可以看出两个脉冲之的PAUSE时间为20ms,即PAUSE 20。下面来计算下带宽为1.5ms时PULSOUT命令参数Duration的值。1.5ms=0.0015s 前面阐述过，无论 PULSOUT 命令的参数 Duration 的值是多少，都要乘以 2 微秒，这样就可以计算出脉冲持续的时间。计算方式：0.0015s/0.000002s=750.因此我们得出要向PIN12发送1.5ms脉冲信号的命令是：PULSOUT 12，750 对于六脚步行机器人的12个伺服电机，由于伺服电机数量比较多，可以进行轮流调零，也可以进行2-4个同时调零。具体程序如下： $STAMP BS2 $PBASIC 2.5 DEBUG text! DO PULSOUT 12, 750 PAUSE 20 LOOP 同时编辑两个伺服电机的调零工作： DEBUG text! DO PULSOUT 12, 750 PAUSE 20 PULSOUT 13, 750 PAUSE 20 LOOP - 9 - 仿生六足机器人行走策略研究 第三章 仿生六足机器人步态规划 3.1 仿生机器人步态研究的几个基本概念 步态是步行类机器人的迈步方式，即机器人行走系统抬腿和放腿的顺序、由于开发步行机器人的需要，McGhee在总结前人对动物步态研究成果的基础上，比较系统的给出了一系列描述和分析步态的严格的数需定义。目前多足机器人技术领域常用到的基本术语如下： 步态： 即步行机器人有关腿部摆动顺序极其时间相序等的步行模式。 支撑相、摆动相：步行机器人腿部着地的状态叫做支撑相或站立相。 支撑多边形：即步行机器人支撑足着地点用凸形轮廓线所构成的凸形多边形在水平面上的投影。 稳定裕度：多足机器人重心在水平面上的垂直投影点处于支撑多边形各边的最短距离叫做该时刻的稳定裕度。对于机器人的某一种周期步态，在其步行周期中稳定裕度的最小值叫做步态的稳定裕度。 静态步态： 步行机器人合成重心在水平面上的垂直投影点处于支撑多边形内的步行叫做静态稳定步行，始终保持静态稳定性的步行叫做静态步行。 动态性步行、准动态步行、动态步行：机器人在步行过程中某段时间不能保持静态稳定性的步行叫做动态性步行或准动态性步行；步行中完全不能满足静态稳定条件的储蓄步行叫做动态步行。 跨步： 步行机器人腿部的周期运动叫做跨步；单位周期中机器人重心移动的距离叫做步长。 占空系数：设在一个跨步周期 T 内，步行机器人第 i 足处于支撑相的时间 ti与其步行周期T之比称为占空系数（用i表示 ），也叫做该足的有荷因数。所有足的占空系数都相等的步态称为规则步态，这时用表示。几乎所有的动物都采用规则步态，步行机器人一般也以此为基准。 一般在0和1之间取值。对于六足机器人而言，其步态随 变化而变化。当00.5,机器人移动较慢时,摆动相与支撑相有一短暂的重叠过程,即机器人有六条腿同时着地的状态(见图 2(b);(3) $FF IF (servoAddr / 2) = 1 THEN ramp = LiftRamp ELSE IF (servoAddr = $00) OR (servoAddr = $02) OR (servoAddr = $04) THEN ramp = rightRamp ENDIF IF(servoAddr = $06) OR (servoAddr = $08) OR (servoAddr = $0A) THEN Ramp = leftRamp ENDIF ENDIF GOSUB Write_PSC LOOP GOTO Walking_Engine 程序写入 Basic Stamp 控制器 Write_PSC: READ ptrEEPROM+1, servoPosition.LOWBYTE, servoPosition.HIGHBYTE 读取三个地址：伺服电机地址位 、伺服电机高位、伺服电机低位 SEROUT PSC,Baud,!SC,ServoAddr, 设置传输的参数以及具体位置 Ramp,servoPosition.LOWBYTE, servoPosition.HIGHBYTE, CR ptrEEPROM = ptrEEPROM + 3 每次读取三组程序 READ ptrEEPROM,servoAddr PAUSE delay RETURN - 49 - 仿生六足机器人行走策略研究 附录 2 四脚步态程序 $STAMP BS2 $PBASIC 2.5 constants definitions Psc CON 15 baud CON 396 ramp CON 1 variables definitions ch0 VAR Byte ch1 VAR Byte ch2 VAR Byte ch3 VAR Byte ch4 VAR Byte ch5 VAR Byte ch6 VAR Byte ch7 VAR Byte ch8 VAR Byte ch9 VAR Byte ch10 VAR Byte ch11 VAR Byte n VAR Byte ho1 VAR Word ho2 VAR Word ho3 VAR Word ho4 VAR Word ho5 VAR Word ho6 VAR Word ch0 = 0 ch1 = 1 ch2 = 2 ch3 = 3 ch4 = 4 ch5 = 5 - 50 - 仿生六足机器人行走策略研究 ch6 = 6 ch7 = 7 ch8 = 8 ch9 = 9 ch10 = 10 ch11 = 11 main : GOSUB initialization FOR n = 0 TO 50 GOSUB movestraight n = n+1 NEXT initialization: ho1= 750 SEROUT Psc , Baud+$8000,!SC, ch0, ramp, ho1.LOWBYTE, ho1.HIGHBYTE, CR SEROUT Psc , Baud+$8000,!SC, ch2, ramp, ho1.LOWBYTE, ho1.HIGHBYTE, CR SEROUT Psc , Baud+$8000,!SC, ch4, ramp, ho1.LOWBYTE, ho1.HIGHBYTE, CR SEROUT Psc , Baud+$8000,!SC, ch6, ramp, ho1.LOWBYTE, ho1.HIGHBYTE, CR SEROUT Psc , Baud+$8000,!SC, ch8, ramp, ho1.LOWBYTE, ho1.HIGHBYTE, CR SEROUT Psc , Baud+$8000,!SC, ch10, ramp, ho1.LOWBYTE, ho1.HIGHBYTE, CR SEROUT Psc , Baud+$8000,!SC, ch1, ramp, ho1.LOWBYTE, ho1.HIGHBYTE, CR SEROUT Psc , Baud+$8000,!SC, ch3, ramp, ho1.LOWBYTE, ho1.HIGHBYTE, CR SEROUT Psc , Baud+$8000,!SC, ch5, ramp, ho1.LOWBYTE, ho1.HIGHBYTE, CR - 51 - 仿生六足机器人行走策略研究 SEROUT Psc , Baud+$8000,!SC, ch7, ramp, ho1.LOWBYTE, ho1.HIGHBYTE, CR SEROUT Psc , Baud+$8000,!SC, ch9, ramp, ho1.LOWBYTE, ho1.HIGHBYTE, CR SEROUT Psc , Baud+$8000,!SC, ch11, ramp, ho1.LOWBYTE, ho1.HIGHBYTE, CR PAUSE 200 RETURN movestraight : ho1 = 750 ho2 = ho1-300 ho3 = ho1+300 SEROUT Psc , Baud+$8000,!SC, ch1, ramp, ho1.LOWBYTE, ho1.HIGHBYTE, CR SEROUT Psc , Baud+$8000,!SC, ch3, ramp, ho1.LOWBYTE, ho1.HIGHBYTE, CR SEROUT Psc , Baud+$8000,!SC, ch5, ramp, ho2.LOWBYTE, ho2.HIGHBYTE, CR SEROUT Psc , Baud+$8000,!SC, ch7, ramp, ho1.LOWBYTE, ho1.HIGHBYTE, CR SEROUT Psc , Baud+$8000,!SC, ch9, ramp, ho3.LOWBYTE, ho3.HIGHBYTE, CR SEROUT Psc , Baud+$8000,!SC, ch11, ramp, ho1.LOWBYTE, ho1.HIGHBYTE, CR PAUSE 200 ho4 = ho1+100 ho5 = ho1-100 ho6 = ho1-150 SEROUT Psc , Baud+$8000,!SC, ch0, ramp, ho6.LOWBYTE, ho6.HIGHBYTE, CR SEROUT Psc , Baud+$8000,!SC, ch4, ramp, ho4.LOWBYTE, ho4.HIGHBYTE, CR SEROUT Psc , Baud+$8000,!SC, ch8, ramp, ho5.LOWBYTE, ho5.HIGHBYTE, CR SEROUT Psc , Baud+$8000,!SC, ch2, ramp, ho6.LOWBYTE, ho6.HIGHBYTE, CR - 52 - 仿生六足机器人行走策略研究 SEROUT Psc , Baud+$8000,!SC, ch6, ramp, ho4.LOWBYTE, ho4.HIGHBYTE, CR SEROUT Psc , Baud+$8000,!SC, ch10, ramp, ho4.LOWBYTE, ho4.HIGHBYTE, CR PAUSE 200 SEROUT Psc , Baud+$8000,!SC, ch1, ramp, ho1.LOWBYTE, ho1.HIGHBYTE, CR SEROUT Psc , Baud+$8000,!SC, ch3, ramp, ho2.LOWBYTE, ho2.HIGHBYTE, CR SEROUT Psc , Baud+$8000,!SC, ch5, ramp, ho1.LOWBYTE, ho1.HIGHBYTE, CR SEROUT Psc , Baud+$8000,!SC, ch7, ramp, ho3.LOWBYTE, ho3.HIGHBYTE, CR SEROUT Psc , Baud+$8000,!SC, ch9, ramp, ho1.LOWBYTE, ho1.HIGHBYTE, CR SEROUT Psc , Baud+$8000,!SC, ch11, ramp, ho1.LOWBYTE, ho1.HIGHBYTE, CR PAUSE 200 SEROUT Psc , Baud+$8000,!SC, ch0, ramp, ho6.LOWBYTE, ho6.HIGHBYTE, CR SEROUT Psc , Baud+$8000,!SC, ch4, ramp, ho6.LOWBYTE, ho6.HIGHBYTE, CR SEROUT Psc , Baud+$8000,!SC, ch8, ramp, ho4.LOWBYTE, ho4.HIGHBYTE, CR SEROUT Psc , Baud+$8000,!SC, ch2, ramp, ho4.LOWBYTE, ho4.HIGHBYTE, CR SEROUT Psc , Baud+$8000,!SC, ch6, ramp, ho5.LOWBYTE, ho5.HIGHBYTE, CR SEROUT Psc , Baud+$8000,!SC, ch10, ramp, ho4.LOWBYTE, ho4.HIGHBYTE, CR PAUSE 200 SEROUT Psc , Baud+$8000,!SC, ch1, ramp, ho2.LOWBYTE, ho2.HIGHBYTE, CR SEROUT Psc , Baud+$8000,!SC, ch3, ramp, ho1.LOWBYTE, ho1.HIGHBYTE, CR - 53 - 仿生六足机器人行走策略研究 SEROUT Psc , Baud+$8000,!SC, ch5, ramp, ho1.LOWBYTE, ho1.HIGHBYTE, CR SEROUT Psc , Baud+$8000,!SC, ch7, ramp, ho1.LOWBYTE, ho1.HIGHBYTE, CR SEROUT Psc , Baud+$8000,!SC, ch9, ramp, ho1.LOWBYTE, ho1.HIGHBYTE, CR SEROUT Psc , Baud+$8000,!SC, ch11, ramp, ho3.LOWBYTE, ho3.HIGHBYTE, CR PAUSE 200 SEROUT Psc , Baud+$8000,!SC, ch0, ramp, ho4.LOWBYTE, ho4.HIGHBYTE, CR SEROUT Psc , Baud+$8000,!SC, ch4, ramp, ho6.LOWBYTE, ho6.HIGHBYTE, CR SEROUT Psc , Baud+$8000,!SC, ch8, ramp, ho4.LOWBYTE, ho4.HIGHBYTE, CR SEROUT Psc , Baud+$8000,!SC, ch2, ramp, ho6.LOWBYTE, ho6.HIGHBYTE, CR SEROUT Psc , Baud+$8000,!SC, ch6, ramp, ho4.LOWBYTE, ho4.HIGHBYTE, CR SEROUT Psc , Baud+$8000,!SC, ch10, ramp, ho5.LOWBYTE, ho5.HIGHBYTE, CR PAUSE 200 RETURN - 54 - 仿生六足机器人行走策略研究 附录 3 实现避障的完整程序： $PORT COM1 $PBASIC 2.5 irDetectLeft VAR Bit irDetectRight VAR Bit pulseCount VAR Byte FREQOUT 4,2000,3000 DO FREQOUT 8,1,38500 irDetectLeft=IN9 FREQOUT 2,1,38500 irDetectRight=IN0 IF(irDetectLeft=0)AND(irDetectRight=0)THEN HIGH 10 HIGH 1 GOSUB Back_Up GOSUB Turn_Left GOSUB Turn_Left ELSEIF (irDetectLeft=0)THEN HIGH 10 LOW 1 GOSUB Back_Up GOSUB Turn_Right ELSEIF(irDetectRight=0)THEN LOW 10 HIGH 1 GOSUB Back_Up GOSUB Turn_Left ELSE LOW 10 LOW 1 GOSUB Forward_Pulse ENDIF LOOP Forward_Pulse: - 55 - 仿生六足机器人行走策略研究 PSC PIN 15 PSC module #IF ($stamp = BS2SX) OR ($stamp = BS2P) #THEN Baud CON 1021 2400 baud #ELSE baud CON 33164 2400 baud #ENDIF Walking Variables servoAddr VAR Byte Servo addresses ptrEEPROM VAR Word Gait select servoPosition VAR Word Servo Position ramp VAR Byte Ramp used in SEROUT rightRamp VAR Byte Right side ramp values leftRamp VAR Byte Left side ramp values Horizontal Leg Constants Center1 CON 750 Leg1 horizontal servo Center2 CON 750 Leg2 horizontal servo Center3 CON 750 Leg3 horizontal servo Center4 CON 750 Leg4 horizontal servo Center5 CON 750 Leg5 horizontal servo Center6 CON 750 Leg6 horizontal servo Stride Calculations Stride CON 100 Stride units delay CON Stride/5 Delay as a ratio of Stride Leg1Center CON Center1 Leg1Forward CON Center1+Stride Stride + Leg1 Center Leg1Back CON Center1-Stride Leg2Center CON Center2 Leg2Forward CON Center2+Stride Leg2Back CON Center2-Stride Leg3Center CON Center3 Leg3Forward CON Center3+Stride - 56 - 仿生六足机器人行走策略研究 Leg3Back CON Center3-Stride Leg4Center CON Center4