《室内自主机器人的实时避障与导航系统的设计与实现.docx》由会员分享，可在线阅读，更多相关《室内自主机器人的实时避障与导航系统的设计与实现.docx（25页珍藏版）》请在taowenge.com淘文阁网|工程机械CAD图纸|机械工程制图|CAD装配图下载|SolidWorks_CaTia_CAD_UG_PROE_设计图分享下载上搜索。
1、室内自主机器人的实时避障与导航系统的设计与实现摘 要本文根据ABU2007机器人大赛的规则要求，对机器人的控制系统和调试方法做了比较深入的探索，并实际完成了所有机器人的软件控制系统。机器人采用了TI公司的TMS320LF2407A作为核心控制器，使用EP1C3T144C8作为外设控制器，很好地解放了核心对外设的接口控制，使得DSP能够更加高效地用于定位计算。本文成功解决了机器人导航过程中的定位、累计误差修正、路径规划以及避障等问题，使其运动多线多样化，提高了机器人的灵活性和可靠性。另外，本文在机器系统中引入了无线调试系统，使得机器人在运行过程中的各种信息变的可控可观测，极大地加快了系统的调试进
2、度。同时，调试系统的PC端程序可以生成机器人的路线代码，使得机器人策略路线的制定更加方便可靠。整个机器人系统在经过长时间的实地测试与改进后，基本达到了设计要求，为本次成功参赛垫定了坚实基础，也为以后机器人控制系统的设计积累了十分宝贵的经验。关键词：机器人，定位，路径规划，避障，无线调试，DSPDesign and Implementation of Real-Time Navigation and Obstruction Avoidance for Indoor Autonomic Mobile RobotAbstractIn this page, we have made a deep re
3、search about the robot control system and the debug method according to the subject and rule of the ABU2007 contest, then we finished the software control system of all the robots. The system use TIs product TMS320LF2407A to be the core controller, and the FPAG EP1C3T144C8 as the assistant controlle
4、r, which works very well to make the DSP computes more effectively. This page resolves the localization, correcting of accumulative total error, path plan and avoiding the obstruction successfully, which makes the robots have more path to move, and improves the moving agility and reliability of the
5、robots. Additionally, we developed a wireless debug system, so that we have a real-time program to watch the trace and parameters that we are interested in. We can change the important parameters of the robots and set the path by the real-time control program. The hole system has been tested and imp
6、roved for a long time. It reach the design goal basically, so that we can join the contest successfully, and also gives much important experience to the following design.Keywords: localization, path plan, obstruction avoidance, wireless debug, DSP目录1绪论11.1室内自主机器人导航与壁障系统的发展现状11.2ABU大学生机器人电视大赛背景资料11.3
7、赛事现状和技术概况22课题背景32.1比赛场地介绍32.2机器人42.3积分规则42.4方案总体42.5具体方案53机器人控制电路分析与设计73.1硬件选型73.2地址映射73.3FPGA外设控制器83.4电机驱动83.5印刷电路板104机器人控制软件分析与设计134.1定位方式介绍134.2机器人定位系统154.3机器人导航算法194.4机器人避障策略204.5路径规划算法214.6超时处理244.7软件系统255无线调试系统275.1无线串口收发器275.2串口通讯协议275.3PC端控制程序286总结30致谢32参考文献33附录34附录一：机器人实物图34附录二：定位算法源代码35附录三
8、：路径规划核心源代码361 绪论机器人是一类能够自动完成某项功能的机械系统，机器人通过传感器和执行机构与外界进行信息物理和交互，处理器负责处理传感器采集来的信息并将相应的控制命令送给执行机构执行。机器人因其对环境的强适应性，使得他在很多领域取代了人的劳动，将人从繁重、危险的环境中解放出来。机器人广泛应用于工业生产、科学研究、危险品处理乃至国防领域1。机器人也是各国科学研究的一个重要方向，它综合了精密机械制造、微电子电路、自动控制理论、传感器技术、仿生学、人工智能等多门基础及新兴学科。从某种意义上讲，机器人技术水平的高低，可以体现一个国家的综合科学技术水平。1.1 室内自主机器人导航与壁障系统的
9、发展现状在机器人技术中的一个关键的目标就是让自动机器人能够在室内环境下完成一系列的工作。为了达到这个目标，机器人必须能够感知周围的环境以及获得它自己的定位信息2。为了让机器人能够得到比较准确到达目标点，起到一定的导航效果，一般的方法有寻线，视觉，惯性导航，编码器等3。这些方各有优劣，本文在第4章中对其进行的详细的分析，并深入地研究了使用编码器的定位方式，在原有基础上做出了很大的改进。同时，为了使机器人具有感知环境的能力，一般在机器人身上安装了许多传感器，如摄像头，编码器，光学测距仪，超声速信标，超声波传感器等4。而如何将这些传感器信息很好地融合在一起，并处理好传感器所表现出来的不准确信息，是研
10、究的热点5。1.2 ABU大学生机器人电视大赛背景资料全国大学生机器人电视大赛从2002年第一次举办至今，已经有6个年头了。我们学校从2004年开始，已经成功参加了三届比赛。全国大学生机器人电视大赛由中央电视台举办，该大赛也被称为“亚广联亚太地区机器人大赛”（ABU ROBOCON）中国赛区的预选赛。“亚广联亚太地区机器人大赛”是由亚洲广播联盟（Asia Broadcast Union）节目部发起倡导的，并于1999年在亚广联年会上正式通过了该项目的提案。ABU ROBOCON机器人大赛是一种借助得分机制而分出胜负的机器人间接对抗型比赛，需要双方的机器人有智能较高的自动控制性能和可靠的机械控制
11、性能同时作为保证，才能在短短3分钟的比赛时间之内成功完成比赛任务。接近1年的准备时间对每个学校的机器人设计者是一次严峻的考验，也是工作能力提升的大好机会。该项目的宗旨是致力于培养各国青少年对于学习和掌握高科技的兴趣，提高各参与国的科技水平，为机器人工业的发展发掘培养后备人才。今年的亚太地区比赛由越南主办。全国大赛的冠军将代表中国参加亚太地区比赛。1.3 赛事现状和技术概况国内此项赛事的参与者为全国各理工科院校的本科在读学生，代表队中不乏哈尔滨工程大学，中国科学技术大学，西安交通大学，国防科技大学等这些在机器人领域有所专长的学校，也有象北京科技大学，西南科技大学等在ABU比赛中大量投入，高度重视
12、并取得多次国内冠军的学校。他们大都已经有了四，五年的参赛经验，也积累了一定的成熟技术，从往届比赛来看，他们的机器人设计已经有了一定的水准，有着较强的实力。从技术上来讲，该赛事的参赛队使用最多的定位和导航方法是寻线，但也有使用视觉的，编码盘+陀螺仪的。从比赛成绩上看，虽然不乏低技术打败高技术的战事，但高技术水平的队伍在比赛时总能占据先机，拥有更多制胜的机会。2 课题背景本次毕设以2007年ABU机器人大赛为背景。本届比赛的主题是“华夏之光”6，旨在通过自动和手动机器人的合作，把“木块”放到“岛”上。把“木块”放到“岛”顶部的队占领了这个“岛”，并获得得分。最先用占领比赛场地中央的“岛”建成V字形
13、“指南车”的队就是获胜队。在两队均未建成“指南车”的参赛队的情况下，得分多的队获胜。比赛时间为三分钟。2.1 比赛场地介绍比赛在方形场地（14000mm14000mm）上进行，场地四周有木质围栏（高100mm、厚30mm）。 场地地面用20mm厚的多层板制成，刷乳胶漆。比赛场地由自动区和手动区组成。(1) 手动区：手动区在自动区四周，刷淡蓝色乳胶漆。在手动区有两个手动机器人启动区和两个珍珠存放区。(2) 手动机器人启动区：手动机器人启动区位于手动区，是1000mm1000mm的方形区域。有两个启动区。刷红色乳胶漆的供红队使用，刷绿色乳胶漆的供绿队使用。(3) 木块存放区：每个参赛队的木块存放区
14、位于比赛场地的角上。每场比赛开始前，每个存放区排列了15个木块（详见图1-1）。(4) 自动区：自动区位于比赛场地中央。形状为正十边形，刷蓝色乳胶漆，四周有高100mm、厚30mm的木质围栏（详见附图）。在自动区内，有6条间距为660mm、与正十边形的边平行的引导线及20条呈辐射状的引导线。引导线用30 mm宽的白色无光不干胶纸制成。（详见附图）。自动区由3个区域组成，分别是自动机器人启动区、外自动区和内自动区。图 2-1 ABU2007场地示意图2.2 机器人(1) 每个参赛队必须自己设计和制作手动机器人和自动机器人参赛。每场比赛中，每队只允许有一台手动机器人，自动机器人不能超过3台。比赛开
15、始时，手动机器人的尺寸限制是1000mm长、1000mm宽、1500mm高。(2) 启动前，所有自动机器人在它们的启动区内必须纳入1000mm长、1000mm宽、1500mm高的空间。自动机器人的尺寸和形状可以改变，但高度限制在2000mm以内，宽度不能超过1500mm（按水平投影计）。(3) 所以机器人重量总和不得超过50Kg。2.3 积分规则(1) 外区第一条（有外向内）引导线上的10个岛，每个可得1分(2) 内区第三条（有外向内）引导线上的5个岛，每个可得2分(3) 内区第六条（有外向内）引导线上的5个岛，每个可得3分最先用占领比赛场地中央的岛建成V字形“指南车”的队就是获胜队。2.4
16、方案总体为了能够完成今年主题的得分任务，并确保和对方机器人对抗下的优势，决定采用2个得分机器人，1个干扰机器人，1个手动机器人的组合方式。各个机器人的任务框图如图2-2：图 2-3 机器人任务图其中：1号自动为大得分机器人，拥有与手动交接5块积木的能力，并做为场上得分的主力。2号自动为小得分机器人，拥有储存2块积木的能力，主要处理场上我方预置的积木并得分。3号自动为干扰机器人，拥有展开裙板封堵对方路线的能力。三个机器人都拥有破坏对方岛上得分的能力。手动机器人有夹取5块积木，并逐一投放的能力，并可以利用弹射机构适时在中圈岛得分。各个机器人的重量，尺寸分配如表2-1：表2-1 机器人重量尺寸分配表
17、类别重量尺寸数量备注手动机器人17kg1000100015001可夹取5块积木并逐一投放得分机器人112kg50010008001可存放5块积木得分机器人210kg40050010001可存放2块积木干扰机器人310kg4004006001可展开阻挡对方机器人2.5 具体方案考虑到机器人的各项任务不同，机械和电路部分的侧重也不尽一样。所有机器人的总体结构框图见下图：图 2-4 机器人结构框图手动机器人着重于机械机构的可靠准确，对于精确控制的要求不如自动机器人那么高，传感器的安装都只需要少许行程开关作为辅助，主要发挥操作手的作用即可很好的完成任务。自动机器人着重于电路控制，机械机构上不如手动机器
18、人那么复杂，但是安装了大量的传感器，线路和控制算法都相当繁琐和复杂，需要反复的调试，在赛场上也始终是得分的主力，需要投入绝大部分精力。机器人的设计工程细化为：(1) 机械机构的加工装配，安装，调试。(2) 电路板原理图PCB图的绘制，电路的焊接等。(3) 软件控制算法的调试，策略路线的调试。本人负责的主要任务是软件控制算法的设计与实现。3 机器人控制电路分析与设计3.1 硬件选型由于本届我们使用的定位方式计算量很大，因此以前使用的51控制器已经无法满足要求，故我们使用了TI公司在控制领域专用的DSP，型号为TMS320LF2407A。该DSP的最高主频为40MHz，总线为哈佛结构，地址和数据位
19、宽均为16位，在各个存储空间的最大寻址范围为64K。其用于控制的最大优势是，其中集成了8路PWM波产生模块，2路正交编码器模块，32K的FLASH，2.5K的SRAM，十分适合应用于我们的系统中7。同时，由于程序运行需要大量的存储空间，DSP内部的2.5K的SRAM已经不够使用，因此必须外扩一个SRAM作为外部存储器。根据需要，系统外扩了一块64K的SRAM8。由于DSP的通用IO有限，无法满足要求。因此，为了让DSP能够更好地发挥计算上的优势，同时降低程序复杂度，我们在其IO空间的总线上挂上了一个FPGA(型号为EP1C3T144C8) 9作为专门的外设控制器，用来做电机，编码器，舵机，颜色
20、传感器等外设的控制。3.2 地址映射图3-1 DSP 存储器映射图图3-1为TMSLF2407A的存储器映射图10。从图中可以看出该DSP采用的是哈佛总线结构，其将整个存储空间分成了程序空间、数据空间和IO空间。DSP内部的流水线保证了这三个空间可以并行访问。由于其地址总线为16位宽，故其各个空间的寻址能力为64KB。该DSP的程序空间和地址空间的高32K均被映射成了外部空间。因此，系统中外扩的SRAM为64KB，其被分成了两个32KB，分别被映射到了程序空间和数据空间的高位地址上。系统中的FPGA被映射到了IO空间上。从图3-1中可以看出，IO空间的64K地址基本上都被映射到外部，而本系统中
21、外设所需要的地址远远少于64K，因此为了降低PCB布线难度，减少接线数量，节省FPGA的IO资源，我们只将地址总线的低7位和FPGA相连，即实际分配给FPGA的地址数为128个。3.3 FPGA外设控制器图3-2表示了系统中FPGA和存储器与DSP的连接方式。图3-2 FPGA模块图从图中可以看出FPGA中集成了8个模块，其中总线接口模块模拟了DSP外部总线的读写时序，并将地址与数据信息发送到各个功能模块。由于FPGA内部是使用“硬件”来完成功能，因此，其相对于CPU而言，有许多先天的优势。如测量频率，输出PWM波等，都可以轻而易举的做到，并且十分稳定，不过出现程序跑飞的现象。同时，各个模块都
22、是并行执行的。FPGA的引入，方便了上层DSP的控制，简化了程序结构，提高了整个系统的效率。3.4 电机驱动根据机器人的运动要求，底盘电机需要做正反转调速控制。传统的方式是采用MOSFET搭成“H”型全桥式驱动电路11,12，可以方便的控制电机在四个象限内工作，电路原理图如图3-3所示。图中场效应管采用国际整流器公司（International Rectifier）生产的N沟道场效应管IRF3205，其漏极和源极极限耐压VDSS为55伏、持续漏极电流为110安培、通态电阻仅为80毫欧，漏极最大耗散功率为200瓦。是低压电机调速的理想选择。图 3-3 场效应管“H”全桥式电机驱动电路D1D4为快
23、恢复二极管FR307，为电机电流提供泄放回路。四只场效应管的驱动信号可以根据不同的控制方式来给定，但必须要保证同一侧桥臂的两路信号之间的死区时间大于IRF3205的恢复时间，否则会造成该侧桥臂两管同时导通，造成场效应管发热，甚至烧毁。考虑到采用继电器可以方便地实现电机的正反转控制，而场效应管可以实现优良的PWM调速，我们将继电器与场效应管结合起来使用，成功地实现了底盘电机的正反转和连续的PWM调速。调速电路原理图如图3-4所示。这种电路结构避免了场效应管死区影响，继电器的使用大大提高了驱动电路的可靠性。图中通过INPUT1和INPUT2两端控制继电器将电机以不同的方式接到电源VCC与场效应管的
24、漏极之间，以实现电机方向的控制。通过调节PWM IN端控制信号的占空比来实现对电机的调速。图 3-4 双向可调速电机驱动电路电机驱动电路是整个系统中的“强电”部分，其电流大，干扰强，对核心控制器有很大的影响。为了降低干扰，系统配备了两块电池，一块给控制器供电，另一快给电机供电，两个电路系统使用光电耦合器完全隔离，这样提高的系统的抗干扰能力。3.5 印刷电路板由于项目初期，各个模块的稳定性均不高，为了更换方便，隔离查错，我们将整个电路系统分成了三个电路板，即DSP控制板，FPGA接口板和电机控制板。这三个电路板都有独立的电源借口，均可以单独工作。后来事实证明，这种模块化的方式，给电路查错，和有问
25、题板子的更换带来了很大的方便。DSP控制核心如图3-5：图3-5 DSP核心FPGA接口板如图3-6：图3-6 FPGA接口板自动机器人用驱动板如图3-7：图 3-7 自动机器人用驱动板手动机器人用驱动板如图3-8：图 3-8 手动机器人用驱动板图3-9为整个控制电路组装后的实物图：图3-9 控制电路全图从图中可以看出，整个硬件系统是由三块电路板拼接而成，最上面的是DSP核心板，中间为FPGA接口板，最下面为电机控制板。4 机器人控制软件分析与设计4.1 定位方式介绍定位方式的选择决定了机器人的行走方式和巡航效率，是机器人完成各项任务的基础。在ABU比赛中使用的最多的就是寻线定位，另外也有不寻
26、线定位的，如惯性导航、机器视觉、编码器等。下面将对各种定位方式进行分析。4.1.1 寻线定位由于ABU比赛中的场地上有白色的引倒线，机器人可以利用这些白色的引导线来进行定位。寻线传感器一般使用光电式传感器，通过多个光电发射和接收管，可以获得机器人相对于低下的白色引导线的位置，从而进行闭环控制。寻线定位方式要解决的问题主要是传感器的稳定性和抗干扰能力以及比较完善的寻线算法。相对于其他的定位方式而言，寻线比较容易实现，而且成本比较低。在传感器稳定的情况下，如果寻线算法比较完备的话，机器人将是非常稳定的，因此，绝大多数队伍都是使用了这种点位方式。但寻线的定位方法仍然有其弊端。首先，很难获得比较稳定的
27、寻线传感器；其次，这种行走方式决定了机器人只能“横平竖直”地运动，限制了机器人的灵活性；最后，由于今年比赛的场地上有很多折线，在高速情况下，要能够顺利地通过这些折线是比较困难的。为了测试该定位方式的效率与稳定性，我们做了一些实验。图4-1是我们所使用的寻线传感器方阵。图4-1 寻线传感器方阵为了寻线的可靠性，我们在机器人底盘的四周各安装了一条寻线传感器，每条有16个光电接收管。实验证明，即使是使用了如此多的传感器的情况下，机器人在高速过折线的时候仍然会出现错误。为了提高稳定性，必须降低机器人的巡航速度。另外，由于传感器众多，整个系统的功耗也很大。4.1.2 惯性导航惯性导航的历史已经很久了，最
28、早的应用是飞机上的陀螺仪13。而其在ABU机器人上使用也是近几年的事情，而且使用的学校很少。惯性导航是使用一些惯性制导传感器，如加速度计、陀螺仪等，来记录当前机器人的位置和角度信息，从而进行定位。对陀螺仪输出的角加速度进行积分，即可以得到当前的角度，对加速度进行二次积分，也可以得到自己的位置，这使得机器人完全摆脱了场地上引导线的缚束，实现全场定位。但是，真正能够提供高精度，低漂移的陀螺仪和加速度计非常昂贵，不是一般的学校能够承受的起的。我们也对陀螺仪进行过测试。我们购买的是ADI公司生产的微机电陀螺仪ADXRS300。其接口是模拟输出，需要AD采样。当时我们使用FPGA对其进行了测试，发现低速
29、平稳旋转90度后，误差有12度。而且漂移很严重，因此其不适合用于机器人上。4.1.3 机器视觉机器视觉14是一种比较高的技术，迄今为止，在ABU中只有四个学校使用过，而且其中只有中国科技大学和国防科技大学的机器人可以通过视觉来定位。该技术的门槛很高，不稳定因素也很多，没有一定的理论基础，想要在几个月的时间内做到稳定可靠是很困难的。另外，进行图像处理所需的计算量和存储器都非常大，一般的控制器是不能胜任的，需要使用更加强大的处理器。而这些，对于我们而言，都是不实际的。因此，该方案也被淘汰了。4.1.4 使用编码器定位使用增量式编码盘15来获得路程和角度信息在机器人上使用的也比较普遍。图3-1是编码
30、器的原理图。编码器输出的分辨率的高低取决于其内部栅格的多少。一般的编码器输出两路相位相差90度的方波16，这两相方波可以用来区分编码盘的旋转方向。利用一个编码其可以获得机器人前进的路程信息，利用两个平行编码器的路程差可以获得机器人旋转的角度信息，也可以用作反馈来保证机器人走直线。为了防止编码盘打滑，编码器一般是以从动轮的形式安装在机器人上。这种定位方式致命的弱点是：其无法检测横向的偏移，当机器人受到侧面方向的撞击而发生侧移时，编码器无法检测出这种情况下的偏移量，从而给定位带来问题。因此，很少有队伍单独使用编码其进行定位，编码器很多情况下只是用来辅助定位，或者用来计算旋转的角度。在我们使用的控制
31、核心TMS320LF2407A中集成了编码器控制模块，其分别在两路方波的上跳沿和下跳沿进行采样，因此该控制模块相当于将输入的方波进行了4倍频，极大地提高了编码器的精度。图4-2 编码器原理图4.1.5 方案总结根据以上几个小节的分析，我们可以看出，如果不在现有的基础上做出改进的话，只有寻线定位方式比较适合做该比赛。但我们仔细研究了一下编码的定位方式，发现可以换一种思路来利用编码器所获得的路程值，并且其精度和抗干扰能力上有了很大的提升。从而使得机器人使用了以码盘作为基础的定位方式。4.2 机器人定位系统以前利用编码器的时候，只是对其当前值进行处理，并没有实时地对历史值进行积分，因此，机器人在每个
32、时刻并不知道自己当前角度和位置信息，只知道当前所处的状态。如当机器人正在走直线的时候，它只知道自己在走直线，并不知到自己在行走过程中的位置和角度信息，旋转时也是如此。如果对从初始状态之后所有的值进行积分的话机器人就可以知道自己当前的位置和角度信息了。4.2.1 定位算法模型机器人的定位信息使用来描述。其中分别表示机器人在全局坐标系下的坐标，表示机器人的方位角。图4-3是定位算法模型示意图，其中表示全局坐标系，表示机器人的局部坐标系。图4-3 定位算法模型当机器人从第n个状态(蓝色)，经过很小的运动到达第n+1个状态(红色)时，左边和右边的从动轮各自行走了一段很小的圆弧。设机器人左边编码器的变化
33、量为，右边的变化量为。机器人在这个过程中旋转的角度可以表示为：其中与分别是两边编码器的矫正系数，其与编码器的分辨率N和从动轮半径r有关；为两边编码器从动轮的中心轮距。同时根据值可以得出在机器人坐标系中x, y方向的变化量：为了将机器人坐标系中的,转换为全局坐标系中的位置和角度信息，需要进行如下变换：其中表示前一时刻的定位信息，表示当前时刻的位置信息。这样累加出来的结果就可以实时地得出机器人的定位信息了。从整个算法上来看，如果机器人在横向出现便偏移的话，位置信息会出现比较大的误差，而角度值由于是使用差值，故不会出现较大的误差。因此，整个运动过程中，角度值受到影响的程度很小。同时需要指出的是，在轮
34、式机器人运动过程中，其旋转中心可以认为在两个主动轮的连线中心上，但整个定位系统的反馈信息全部是从从动轮上的编码器获得的，如果两者的中心不在同一个垂线上，则会使得机器人在旋转的时候所计算得到的信息是实际值的一个分量，并且这个分量不是线性的，无法对其进行矫正。因此，必须在安装上尽量保证从动轮连线的中心点和主动轮连线的中心点在同一垂线上。理想的安装方式的示意图如图4-4所示：图4-4 编码器的安装要求另外，该定位算法对编码器的采样频率也有要求。理论上来说，采样频率越高，积分越准确。但是，由于整个计算过程中，不管是使用定点数还是浮点数运算，都会产生计算精度的损失，因此采样频率不能太高，当然也不能太低。
35、一般保证每次采样的时候，编码器的变化量为10左右。4.2.2 编码器参数矫正由于安装、工艺、测量等原因，导致编码器上的一些参数存在误差。主要表现在r和Len的值不准。为了得到精确的定位信息，必须对这两个参数进行矫正。矫正的过程中应该先矫正r再矫正Len。先手推机器人，保证其行走的路线是一条直线，再通过无线调试系统记录其轨迹，通过其轨迹的形状，来调整两边的r值。如果发现轨迹是向左的弧线，则说明左边的从动轮的半径比右边小，需要增大左边的值；如果是向右的弧线，处理方式类似。直到调试系统记录的轨迹为直线为止。接下来等比例放大(缩小)两个r值，直到其调试系统上反映的机器人所走的长度和实际长度相同位置。完
36、成对从动轮半径的矫正之后，需要对其中心距进行矫正。用手推机器人，保证机器人作旋转运动，旋转足够大的角度之后(一般是10圈)，对比调试系统上记录的值和实际机器人所旋转的角度值，通过差量的正负和机器人的旋转方向，对中心距Len进行矫正。实验证明，经过参数矫正后的机器人，在没有受到严重撞击的情况下，可以正常行走10个岛。4.2.3 累计误差的消除由于系统的所有定位信息都是由编码器的值来确定的，虽然对编码器的参数进行了矫正，但是误差仍然是存在的，在长时间的工作之后，累计误差会导致机器人无法正常定位。为了保证机器人能够长时间的工作，需要想办法消除或者减少累计误差。要修正累计误差，必须依靠一些事先知道的“
37、绝对”信息17。在本次比赛中，可以事先知道的是每个岛的位置和自动区外围木栏的角度。可以利用这些信息对机器人的定位信息进行矫正。如图4-5所示，由于场地上的岛均为圆形，而机器人前端的形状保证其接近岛的时候岛的中心和从动轮中心连线的中心的距离为一个可以测量的常数。正如前面所提及的那样，机器人在运动过程中，角度的误差是很小的，而坐标信息却很容易受到干扰。因此，在这里，我们可以通过比较精确的值来矫正不精确的值，即，使用角度来矫正坐标。图4-5 累计误差修正设岛的中心和从动轮中心连线的中心的距离为l，岛的坐标为，则通过以下公式可以得到机器人的当前位置：实验证明，经过累计误差修正之后的机器人，在没有受到严
38、重撞击的情况下可以连续运行10分钟而不丢失自己的位置。这种性能用在3分钟的比赛中已是绰绰有余。4.3 机器人导航算法在解决了机器人定位的问题之后，接来需要解决的问题就是如何让机器人到达给定的目标点。由于系统在每个时刻知道自己的位置和角度，因此可以利用这些信息来对机器人做反馈。具体的，本系统使用目标点和编码器中心点的连线与当前机器人行走方向的夹角作为反馈参数，使用机器人与目标点的距离作为判断是否到达的条件。其可用如下模型来描述。图4-6 导航算法示意图如图4-6所示，设机器人车头方向和目标点的夹角为，目标点的坐标为，通过如下公式可将目标点的坐标转化为机器人的局部坐标值：然后通过来求出值（当3号2
39、号。通过实验证明，这些避障方案，可以让机器人顺利通过一般的障碍物。4.5 路径规划算法4.5.1 问题提出在完成了底层的定位、导航、避障之后，接下来要解决的问题是如何让机器人采用正确的路径到达指定的目标点。现有了路径规划算法有很多18，如A*算法（图4-8）等，但这些路径规划算法都是基于网格的，即其假设所有的路径都是沿着一定的网格进行规划的，其行走方式为“横平竖直”型。而我们对机器人的控制方式决定了这种基于网格的规划方式不适合我们使用。因为导航算法所产生的控制效果是机器人走的所有路线都是曲线，很难保证其走很短的横线或竖线。比较适合我们的定位方式的路径应该是尽量少的路径点和比较平滑的折线。图4-
40、8 A*算法演示图图4-9 是本次比赛自动区的场地图，以绿方出发区中网格交叉点为坐标圆点。如图，整个场地可以被分成6块区域，每个区域有5个出口点(入口点)和相邻的区域连通，每个出口点都有一定的权值。机器人从一个区域运动到另外的一个区域，必须从其中的一个出口出发，而从目标区域的一个入口进入。图4-9 路径规划原理图可以看出，机器人从图中任意点出发，到任意目标点，最多经历3个区域(包括出发区域和目标区域)，因此，规划路径的时候可以分成3种情况：A) 目标点和出发点属于同一区域。这种情况下，不需要在目标点和出发点之间插入路径规划点。B) 目标点和出发点属于相邻的区域。在这种情况下，机器人需要找到一个
41、最优的出口点，到达目标区域。C) 目标点和出发点之间相隔一个中间区域。这时，对与出发区域而言，需要找一个最优的出口点，对于目标点而言，需要找一个最优的入口点。根据上面的分析，可以看出，需要解决的问题就是，如何找出各种最优的出口(入口)。4.5.2 算法模型设目标点为坐标为,出发点的坐标为，考虑到目标点有可能是区域边界上的岛，这种情况下，很难判断出目标点属于哪个区域，为了解决这个问题，我们使用了以下方法：如图4-10所示，在目标点和出发点的连线上添加一个辅助点，该辅助点和目标点很近，因此，可以将辅助点当成目标点，而不影响具体的导航结果。这样便解决了区域边界点的问题，而且这种方法适用于所有的目标点
42、。图4-10 辅助点模型图4-11为路径规划算法流程图：图4-11 路径规划程序流程图需要指出的是，上图中所说的最优出口(入口)点是通过点的权值与距离平方的倒数之积取最大而产生的。最后所有的存储点即为路径规划点。下图是自编的路径规划测试程序的界面：图4-12 路径规划算法测试效果4.6 超时处理由于比赛时机器人不可避免的会出现碰撞，死锁，卡死等让机器人不能完成当前给定任务的情况。为了提高机器人在这些情况下的生存能力，必须对机器人的动作做超时处理18。在本次比赛中，应用超时比较多的是底盘超时候和执行机构超时。底盘超时是指通过内部的计时器对底盘的运动进行检测，如果机器人当前正处于运动状态，而定位系
43、统所算出的坐标和角度值在一段时间内又变化很小，说明机器人当前已经被卡死，这时需要进行超时处理。对底盘的超时处理主要分为两种情况： 在路径规划点超时。这时说明原来规划的路径上已经有让机器人卡死的障碍物，必须让机器人后退一段时间，脱离卡死状态之后，避开这条路径，而重新对路径进行规划。 在任务点超时。这说明机器人已经到达了目标点附近，而无法完成任务。这种情况一般发生在岛的附近已经有一个机器人，并妨碍了我方机器人执行任务。这种情况下，需要让机器人后退一段时间，脱离卡死状态，再放弃当前任务，执行下一任务。执行机构超时是指，执行机构在一定的时间内没有完成给定的任务，并触发了执行机构超时。这种情况下，需要立
44、即放弃当前任务，保证执行机构顺利归位后再执行下一个任务。4.7 软件系统由于定位和导航系统的计算量很大，而且执行机构控制复杂，因此，一个好的软件结构至关重要19, 20。4.7.1 软件系统结构图4-13为软件系统的结构图：图4-13 软件系统结构图模块调度器负责各个模块的调度，如创建，挂起，恢复等。各个没有被挂起的模块将顺序执行。找岛和导航模块是不能并行执行的，只有在导航挂起之后，才有可能开启找岛模块。定位模块永远不能被挂起。这种程序结构可以保证各个功能的正常执行，并实现了各种功能的并行执行。4.7.2 机器人任务的生成，分析与执行为了方便策略的生成与执行，系统用来表示机器人的任务。其中，x
45、, y表示任务点的位置，type表示执行的任务种类。机器人需要执行的所有任务被存储在一个被称为目标池的结构体数组中。机器人从目标池中顺序地取出任务来执行，当出现超时的时候，将放弃当前的任务，再从目标池中取出下一个任务来执行。一般而言机器人该到哪里，执行什么任务是由人决定的。当人为给定的任务执行完毕之后，需要机器人自己生成目标点。这时需要使用任务生成器来自动生成目标点。目标生成器的工作原理是，先找出岛的分值与机器人离岛距离的倒数之积最小的岛，并过滤掉已经去过的岛；再根据机器人身上珍珠的情况来决定是生成破坏任务，还是得分任务。4.7.3 计算方式的选择定位和导航系统计算量很大，而且对精度要求很高。我们使用的DSP是定点DSP，数据宽度为16位，比较适合做定点数运算，而进行浮点数运算则相对较慢。处理用定点数来处理浮点数运算一般使用的方法是Q值，在Q15格式下，最高精度为3.0517578125e-5。对于主得分机器人而言，其左边从动轮的矫正系数k=0.02258306