《第二届全国大学生飞思卡尔杯智能汽车竞赛技术报告》由会员分享,可在线阅读,更多相关《第二届全国大学生飞思卡尔杯智能汽车竞赛技术报告(97页珍藏版)》请在金锄头文库上搜索。
1、第 二 届 全 国 大 学 生 “飞 思 卡 尔 ” 杯智 能 汽 车 竞 赛技 术 报 告附件B关于理论模型与智能车实际运动轨迹的研究学 校: 西北工业大学队伍名称: 翱翔队参赛队员: 能 波带队教师:常会贤曹 阳曲仕茹时间就是金钱,效率就是生命!关于技术报告和研究论文使用授权的说明本人完全了解第二届全国大学生“ 飞思卡尔”杯智能汽车竞赛关保留、使用技术报告和研究论文的规定,即:参赛作品著作权归参赛者本人,比赛组委会和飞思卡尔半导体公司可以在相关主页上收录并公开参赛作品的设计方案、技术报告以及参赛模型车的视频、图像资料,并将相关内容编纂收录在组委会出版论文集中。参赛队员签名:带队教师签名:日
2、 期 :唯有惜时才能成功,唯有努力方可成就!目录摘 要 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . IAbstract.II第 一 章 引 言 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
3、. . . . . . . . . . . . . . . . . . .11 .1 比赛背景介绍. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 11 .2 本文章节安排及文献综述. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 2第二章 方案选择. . . .
4、 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .32 1 测量模块方案选择. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 32.1.1 路径检测模块. . . . . . . . . . . . . . . . . . . .
5、. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 32.1.2 速度检测模块. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 52 .2 控制模块方案选择. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
6、 . . . . . . 62.2.1 路径控制模块. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 62.2.2 方向控制模块. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 92.2.3 速度控制模块. . . . . . . . . . . . . . . .
7、. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 102 .3 执行模块方案选择. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .102.3.1 路径执行模块. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
8、. . . . . . . . 102.3.2 方向执行模块. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 132.3.3 速度执行模块. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 132.4 本章小结. . . . . . . . . . . . . . . . .
9、. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 13第 三 章 机械结构设计. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 153 .1 车模组装与改造. . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
10、. . . . . . . . . . . . . . . . . . . . . . . . . . . 153.1.1 车模组装. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .153.1.2 前轮定位的调整. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 153.
11、1.3 差速的调整. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 163.1.4 舵机力臂的调整. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 163 .2 摄像头的安装. . . . . . . . . . . . . . . . . . . . . . . . . .
12、 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 163 .3 光栅编码器的安装. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .183 .4 电路板的固定与安装. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
13、 . . . .19第四章 硬件系统设计与实现. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 214 .1 电源模块. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 214.1.1 降压稳压电路设计. . . . . . . .
14、. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 224.1.2 升压稳压电路设计. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .224.1.3 电源模块小结. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
15、 . . . . . . 234 .2 路径识别模块. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .244 .3 电机模块. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 264 .4 舵机模块. . . . . .
16、 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 264 .5 测速传感器模块. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .264.6 本章小结. . . . . . . . . . . . . . . . . . . . . . . . .
17、. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 28第五章 理论分析与算法实现. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 29唯有惜时才能成功,唯有努力方可成就!时间就是金钱,效率就是生命!5 .1 模型建立. . . . . . . . . . . . . . . . . . . . . . . . . . . . .
18、. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 295.1.1 基于后轮差速的运动模型. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 295 .1 .2 基于速度和前轮转角的运动模型. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 325 .2 运动模型仿真. . . . . . . . .
19、. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .375 .3 控制算法. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .405.3.1 控制算法的简单介绍. . . . . . . . . . . . . . . . . . . . . . . . . .
20、. . . . . . . . . . . . . . . . . . . . . 405.3.2 方向控制. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 435.3.3 速度控制. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 485.3.
21、4 控制算法的优化部分. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 505.3.5 算法的程序实现. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .51第六章软件系统设计与实现. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
22、 . . . . . . . . . . . . . . . . . . .536 .1 系统初始化. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 536 .2 视频图像信号采集. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 546 .
23、3 图像处理和黑线提取. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .556.3.1 图像处理. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 556.3.2 黑线的提取. . . . . . . . . . . . . . . . . . . . . . . .
24、 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 56第七章 开发与调试. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 597 .1 软件开发环境介绍. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
25、 . . . . . . . . . .597 .2 智能车整体调试. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .61第八章结论. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 638 .1 总结. .
26、 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .638 .2 展望. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 63参考文献. . . . . . . . . . .
27、 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . I附录:控制程序. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .I唯有惜时才能成功,唯有努力方可成就!摘 要本文在第二届“ 飞思卡尔”杯
28、全国大学生智能汽车比赛的背景下,制作一个在专门设计的跑道上自动识别道路行驶的智能车,能够在最短时间内跑完全程,不脱离跑道并遵守大赛的一系列规则的队伍即可胜出。本系统主要由MC9s12DG128控制核心、电源管理单元、路径识别电路、车速检测模块、舵机控制单元和直流电机驱动单元组成,以飞思卡尔公司的16位单片机S12为控制核心,路径识别和车速的检测相结合,通过控制转向舵机和驱动电机,使智能车系统达到所需的稳定性及快速性要求。本文详细的介绍了智能汽车的机械结构设计,硬件电路设计,系统软件设计合理论分析以及模型车的控制算法设计。本智能车采用了适合智能控制的模糊控制算法,首先对系统的模型进行了分析,从而
29、选择合适的算法,其次,对图像采集所用到的基本模块进行了简要介绍,然后介绍了所使用的图像采集和路径识别的算法。关键词:智能汽车,模糊控制,PID控制,CCD传感器,直流电机唯有惜时才能成功,唯有努力方可成就!时间就是金钱,效率就是生命!AbstractThis text is under the background of the intelligence automobile invitationaltournament of the national university student of the second freescare cup,manufactures to be able
30、independently to distinguish the way the intelligent vehicleon the runway which specially designs the automatic diagnosis path travel, can runthe entire journey in the shortest time, is not separated from the heavy line andobserves the big game a series of rules troop then to win.This system is main
31、ly composed with the MC9S12DG128 control core, thepower source management unit, the way identification circuit, the vehicle speedexamination module, the servo control unit and the direct current machine actuatesthe unit, take 16 monolithic integrated circuits S12 as the control core, the wayrecognit
32、ion and the vehicle speed examination unifies, changes the servo and theactuation electrical machinery through the control, enables the intelligent vehiclesystem to achieve needs stable and rapid request.This paper introduced the Intelligent Vehicle mechanical structure design,hardware circuit desig
33、n, system software design and analysis of a theoretical modelof vehicle control algorithm design. Control of fuzzy control algorithms was used inour Intelligent Vehicle, first, in order to choose a suitable algorithm, we analysis themodel of the system,; secondly, the basic module which used to imag
34、e gatheringhas carried on the brief introduction; then, introduced the used image gatheringmethod and the road recognition algorithm.Keyword: intelligence car,fruzzy control,PID control, CCD sensor, DC-motor唯有惜时才能成功,唯有努力方可成就!第 一 章 引 言1 .1比赛背景介绍教育部为了加强大学生实践、创新能力和团队精神的培养,在已举办的全国大学生数学建模、电子设计、机械设计、结构设计等
35、4大竞赛的基础上,经研究决定,委托教育部高等学校自动化专业教学指导分委员会主办每年一度的全国大学生智能汽车竞赛,并成立了由教育部、 自动化分教委、清华大学、飞思卡尔半导体公司等单位领导及专家组成了组委会。该竞赛与教育部已举办的4大专业竞赛一样,都是为了提高大学生的动手能力和创新能力而举办的,具有重大的现实意义。与其他大赛不同的是,这个大赛的综合性很强,是以迅猛发展的汽车电子为背景,涵盖了控制、模式识别、传感、电子、电气、计算机和机械等多个学科交叉的科技创意性比赛,这对进一步深化高等工程教育改革,培养本科生获取知识、应用知识的能力及创新意识,培养硕士生从事科学、技术研究能力,培养博士生知识、技术
36、创新能力具有重要意义。根据自动化专业教学指导分委员会与飞思卡尔公司签署的有效期为5年的飞思卡尔公司协办全国大学生智能竞赛的合作协议书,竞赛由飞思卡尔公司提供统的标准硬软件技术平台。各参赛队以飞思卡尔HC12单片机为核心控制模块,以引导改装后的模型汽车按照规定路线行进,以完成时间最短者为优胜。组委会办公室技术组专家赴韩国汉阳大学交流访问,认真考察了其举办的多届智能汽车竞速比赛,在学习与总结其宝贵经验的基础上,为了保证竞赛的普及性,规定每支参赛队伍三名成员中最多只能有名研究生参加;为了保证竞赛的公平性,制定了多种赛道方案以及体现公平、透明的比赛规则;为了进一步训练大学生的科学技术研究素质,参赛队伍
37、除了进行现场比赛之外,还须提交技术报告,并计入竞赛总分。经各参赛队与组委会充分准备,于2006年8月2 0 -2 1日在清华大学成功举办了由清华大学承办、飞思卡尔公司协办的第届“ 飞思卡尔”杯全国大学生智能汽车邀请赛。赛后队员们反映热烈,众多媒体竞相报导。为了使该竞赛向普及、健康的方向发展,成为在全国范围内大学生科技创唯有惜时才能成功,唯有努力方可成就!时间就是金钱,效率就是生命!意性的重要赛事,经自动化分教指委、飞思卡尔公司及组委会协商决定,在清华大学自动化系设立竞赛秘书处;为了使更多的高校、更多的大学生参与到这一活动中来,从第二届开始,采用分赛区比赛形式,将大赛参赛范围扩大为全国3 0 0
38、多所学校( 包括港、澳等地区的高校)t l o1.2本文章节安排及文献综述本文系统的介绍了制作本智能模型车的各项技术。具体章节安排如下:第一章引言介绍了本次比赛的背景,引出下文。第二章方案选择将智能车控制系统分解为模块,分别从各个模块讨论本智能车系统将要采用的控制方案。在这部分中,为了确定方案我们查找了很多文献,有关于传感器和传感器技术的,如参考文献 3 , 4 ;还有关于机器人技术的,如参考文献 5 ;以及关于去年比赛的论文,如参考文献 6 。第三章机械结构设计介绍了智能车的搭建与调整,以及摄像头、光栅编码器与电路板的安装。应用了一些相关的汽车理论知识,如参考文献 8 。第四章硬件系统设计及
39、实现分析智能车系统各组成部分为实现特定功能应采用什么样的电路,能达到最好效果同时产生的噪声和对其他电路的干扰最小。 参考的主要资料是芯片的d a t a s h e e t,如参考文献 7 , 9 , 1 0 , 的 , 。第五章理论分析与算法实现从建模的角度分析车的运动形式,最后得出控制算法。其中,数学基础理论参考了文献 1 3 。在控制算法上,我们对比了模糊控制和P I D控制,最终采用模糊控制。主要参考文献有 1 4 , 1 5 , 1 6 。第六章软件系统设计与实现介绍了本智能车系统的初始化,图像采集、图像处理和黑线提取的方法。在视频信号的采集上,我们主要参考了 视频技术与应用( 参考
40、文献 1 8 )。 在图像的处理上用到了M A TLA B 7. 0控制系统应用与实例( 参考文献口9 )。第七章开发与调试介绍了软件开发的环境,以及对各部分的调试方法。我们软件开发环境为M e trowe rk s公司开发的软件集成开发环境C od e wa rri or,因此,我们仔细研究了 C od e wa rri or使用指南( 参考文献”7 )。第八章总结与展望总结了几个月来的工作,对未来进行了展望。唯有惜时才能成功,唯有努力方可成就!时间就是金钱,效率就是生命!第二章方案选择本智能车系统采用模块化设计,分为测量模块、控制模块和执行模块。2 .1 测量模块方案选择2.1.1 路径检
41、测模块路径识别模块是智能车系统的关键模块之一,路径识别方案的好坏,直接关系到最终性能的优劣。在以往比赛中光电传感器寻迹方案应用最多,单独采用C C D摄像头寻迹方案或者C C D摄像头寻迹与光电传感器寻迹结合在一起的方案也都有应用。所谓光电传感器寻迹方案,即路径识别电路由一系列发光二极管、接收二极管组成,由于赛道中存在轨迹指示黑线,落在黑线区域内的光电二极管接收到的反射光线强度与白色的赛道不同,由此判断行车的方向。光电传感器寻迹方案的优点是电路简单、信号处理速度快。光电传感器的排列方法、个数、彼此之间的间隔都与控制方法密切相关,但- 一 般的认识是,在不受到外部因素影响的前提下,能够感知前方的
42、距离越远,行驶效率越高。由于光电传感器电路板不可能伸出车体太远,因此不少参赛队伍调整了光电传感器电路板与地面的夹角,使光电传感器可以感知更远一点的赛道情况。图2.1、图2.2是两种典型的光电寻迹方案。图2 .1中模型车采用了 8对光电传感器且分布得较宽,图2 .2中模型车只采用了 3对光电传感器,放置在向外伸出的小电路板上,探测的范围较小。具体何种方案合适,与光电传感器扫描前方的距离和宽度以及控制策略密切相关。但是,基于光电管阵列检测赛道参数的方法主要具有如下的缺点:1)赛道空间分辨率低:一方面受到大赛规则关于传感器个数的限制;另一方面, 过多的光电传感器在固定安装、 占用CPU输入端口资源等
43、方面也有限制;2)识别道路信息少,一般只能检测路径中心位置;3)由于固定位置的限制,光电管只能安装在车模前面不远的位置,观测信息前瞻性差;唯有惜时才能成功,唯有努力方可成就!时间就是金钱,效率就是生命!4)容易受到环境光线的干扰;图 2. 1 8 个光电传感器寻迹的电路图 2.2仅 3 个光电传感器寻迹的电路以上问题会影响车控制精度以及运行速度。通过一定的硬软件设计,虽然可以部分解决上述的问题, 但相比之下, 使用面阵CCD器件则可以更有效的解决这些问题。CCD器件在比赛规则中算作一个传感器。普通CCD传感器图像分辨率都在300线之上,远大于光电管阵列。通过镜头,可以将车前方很远的道路图像映射
44、到CCD器件中, 从而得到车前方很大范围内的道路信息。 对图像中的道路参数进行检测,不仅可以识别道路的中心位置,同时还可以获得道路的方向、曲率等信息。 利用CCD器件, 通过图像信息处理的方式得到道路信息,可以有效进行车运动控制,提高路径跟踪精度和车运行速度。因此,我们选择使用面阵CCD ,它能够有效利用S12单片机内部硬件资源唯有惜时才能成功,唯有努力方可成就!时间就是金钱,效率就是生命!的路径参数检测方法。 普通CCD图像传感器的工作电压一般为12V,输出NTSC或 者PAL制式的模拟视频信号。利 用S12内部的AD转换器,配合从视频信号中分离出的同步信号,可直接将图像信号采集到单片机内部
45、RAM中,然后通过软件对图像信息进行处理,得到路径各种参数。2 .1 .2速度检测模块随着电池电压的逐渐下降,电机供给电压也会随之降低,导致电机的转速不断下降。若采用闭环控制,将车轮转速信息反馈给微控制器,从而使电机的实际转速值等于指令转速值,就能提高速度的准确性。常用的有以下两种测速传感器:(1 )轴编码器 它常被用来测量旋转轴的位置和转速。“ 绝对式位置编码器”被用来测量轴的实际位置,这种编码器长被用于伺服系统中来获得一定的转轴位置。“ 增量式轴编码器”常被用来测量转轴的转速( 速率和方向) 。增量式轴编码器可以产生直接对应于轴转速的脉冲序列,如果采用有两相信号输出的增量式轴编码器实际只能
46、称之为转速计。工程中应用最多的是光学编码器,但也有一些利用电磁原理制成的霍尔效应编码器。(2 )模拟转速计它也被安装到电机的输出轴上,但输出的是正比于电机转速的电压。比较两种方式,从输出信号的格式看,轴编码器输出的是方波信号,很容易输入到控制器中,而模拟转速器输出的是模拟信号,还 需AD转换,另外,模拟转速器对动力大小也有影响,而轴编码器对它儿乎没有影响,要对智能车进行速度的测量,故比较之下选用增量式轴编码器。增量式轴编码器又可分为反射式光学编码器、光栅式编码器和基于霍尔效应的编码器。如 图2.3所示。光栅式编码器的可靠性高于反射式编码器,而基于霍尔效应的编码器采用纯机械的方法无法承受长时间的
47、使用,且精度不高。因此,速度的测量采用光栅式编码器。唯有惜时才能成功,唯有努力方可成就!时间就是金钱,效率就是生命!反射式 光栅式 霍尔式图2.3简易增量式轴编码器2 .2 控制模块方案选择2 .2 .1 路径控制模块智能车竞赛的要求是制作的智能车在专门设计的跑道上自动识别道路行驶,谁最快跑完全程而没有冲出跑道并且技术报告评分较高,谁就是获胜者。所以对于模型车的路径规划是关系到能否在短时间内跑完全程取得优异成绩的关键因素。结合以往的比赛情况,现主要有两种控制方法:(1 )跟踪黑线,以黑线为基准,将小车儿何中心控制在黑线上,主要是经典PID控制方法;(2 )适合跑道,以黑线算出跑道范围,将小车看
48、作刚体,控制使其保持在跑道范围内,可以算作一种智能的模糊控制方法。无论那种方法,为了达到时间最短,必须对小车的行使路线进行优化。在不考虑交叉的情况下( 以行使的角度考虑,交叉属于一种直线形式) ,一般的,根据赛道的形状主要有3种组成:( 如图2.5所示) 直 线 转 弯 波 浪唯有惜时才能成功,唯有努力方可成就!时间就是金钱,效率就是生命!直线波浪图2 .5 赛道的三种基本形状对于弯道,应尽量沿着内圈行驶。( 如图2.6所示)对于波浪道,最优路线应为直线穿过。( 如图2.7所示)图2.6 过弯时小车行驶路线图2 .7 波浪道小车行驶路线对两种控制方法的优化:( 1)跟踪黑线方法对路线的优化:唯
49、有惜时才能成功,唯有努力方可成就!时间就是金钱,效率就是生命!跟踪黑线对路线的优化要区分开三种基本类型的跑道,再用不同的动态性能对路线优化。直道时响应应快速、平缓,需要综合调节各参数。弯道时可以将超调量适当放大、调节时间减小,或者进行超前控制。波浪时应将超调量减小、调节时间加大,或者进行滞后控制。对于波浪, 跟踪黑线方法很难做到最优的直线穿越, 但可以小车摆幅减小。( 如图2.8所示)优化后图 2 . 8 波浪道小车的优化(2)适合跑道的方法对路线的优化适合跑道的方法对路线的优化主要思想为:A .得到较长一段赛道信息与小车在赛道上的姿态、位置。B .根据以上信息算出小车在下一段时间内的行驶路线
50、。这种方法的重点主要是行驶路线的计算方法。比较两种方法,如表2所示:为了达到更好的成绩,要选择适合跑道的方法,并且要综合考虑各种情况,努力提高稳定性。唯有惜时才能成功,唯有努力方可成就!时间就是金钱,效率就是生命!表 2 .1两种方法的比较跟踪黑线适合跑道复杂程度一 般较复杂稳定性较好未知优化性能一 般较好2 .2 .2 方向控制模块本智能车方向的控制是通过PWM波对舵机进行控制来实现的。舵机的控制是通过周期固定的脉冲信号控制的,舵机的转位正比于脉冲的宽度,这个连续的脉冲信号可以由PWM实现。 舵机内部会产生一个频率为50Hz的基准信号,通过基准信号与外部所给PWM波的正脉冲持续时间进行比较,
51、从而确定转向和转角的大小。当所加PWM波的频率为50Hz时,脉宽与转角之间满足下图2.9所示的线性关系。对舵机的控制采用离散PID控制,即根据CCD图像传感器采集的图像计算出当前时刻车与黑线的精确夹角,然后对方向控制量进行校正。缺点是计算脉宽/usee唯有惜时才能成功,唯有努力方可成就!时间就是金钱,效率就是生命!2.2.3速度控制模块本届大赛组委会规定使用的后轮驱动电机型号为RS-380SH,工 作 在7.2V电压下,空载电流为0.5A ,转 速 为16200 r/mino在工作电流为3.3A,转速达到14060 r/m in时,工作效率最大。由于单片机输出的脉宽无法驱动大赛提供的直流电机,
52、因此需要通过电机驱动芯片MC33886驱动电机正转、 反转。 由于单片机带有PWM输出端口,PWM波获取方便,为了加强灵活性,能实时改变控制量,所以我们利用PWM脉宽与速度的对应关系对电机进行控制。2 .3执行模块方案选择2.3.1路径执行模块直接利用S12单片机中的AD采集视频图像,存在着采集速度、存储数据空间、处理速度、工作电压以及同步信号分离等方面的技术难点。下面就这些问题及其解决方法进行讨论。1)采集速度:普 通CCD图像传感器通过行扫描方式, 将图像信息转换为一维的视频模拟信号输出。CCD输出的信号变化很快,比如PAL制式的视频信号,每秒钟输出5 0帧 图 像 信 息 ( 分为奇、偶
53、场) ,每帧图像有312.5行,每行图像信号时间为64微秒,其中有效的图像信号约为56微妙左右。相比之下,S12的AD转换器采集速度较低: 根 据S12器件手册, 进 行10位AD转换所需要的时间为7微秒。这样,采集的图像每行只能有8个像素,水平分辨率很低。另一方面,每场图像可以采集300行左右的图像信息,所以图像垂直分辨率相对较高。从这种水平分辨率低、垂直分辨率高的图像中, 我们无法获取具有足够精度的路径信息。为此,我们分析一下大赛采用跑道的形状特点:跑道都是由直线和圆弧组成,检测车模前方一段路线参数,只需要得到中心线上35个点的位置信息就可以估算出路径参数( 位置、方向、曲率等) 。这些点
54、的位置,通过图像中若干行信息就可以检测出来( 如 图2.10所示) 。因此,所需检测图像应该是水平分辨率高、垂直分辨率低。唯有惜时才能成功,唯有努力方可成就!时间就是金钱,效率就是生命!图2 .1 0部分赛道形状,赛道中心线检测位置将 S12单片机采集的图像分辨率特点,与赛道检测对图像分辨率的要求进行对比,可以发现,在安装CCD摄像头的时候,只要将它旋转90度,输出的图像信息也相应旋转90度 ( 如图2.11所示) 。这样一来,S12中的AD转换器采集的图像信息,水平分辨率与垂直分辨率就会互换,原来水平分辨率低、垂直分辨率高的图像,就会变成水平分辨率高、垂直分辨率低的实际图像,正好满足道路参数
55、检测的要求。图2.11 赛道图像以及旋转90度后的图像止 匕 外 , 我们还可以通过让S12适当超频运行、 降低AD转换器精度等方式,提高AD转换器的速度。 实验中我们发现, 通过设定S12中的时钟PLL寄存器,可以将S12内部总线频率提高到40-48MHZ,而此时CPU仍然可以正常工作。唯有惜时才能成功,唯有努力方可成就!时间就是金钱,效率就是生命!同时, 可以将AD转换器时钟频率提高到12-24MHZ,在损失一定转换精度的代价下,将转换时间缩短为L 5 微秒左右。这样,我们就可以在一行图像信号中采集48个有效的图像信息。将上述方法结合在一起,可以采集到300 * 48分辨率的帧图像数据。在
56、此基础上,可以有效的检测出路径参数。通过实际测试,基于75 *24分辨率的图像,检测出的路径参数仍能满足控制需要。也就是说,我们每隔4 行采集一行数据、 每行采集24个点就可以了。这样一来,所需要的图像存储空间以及图像采集的时间就大为降低了。2)图像存储空间:由于将图像水平旋转了 90度, 需要将图像数据进行存储, 在整幅图像的基础上计算出路径水平信息。 S12内部有8K字节的RAM空间。 如果存储300*48分辨率图像则不够,但可以存储若干幅75*24的低分辨率图像数据。从低分辨率图像所得到的路径参数,其精度仍可以满足车模控制的需要。一般情况下,只需要两块图像存储空间即可,一块作为采集图像的
57、存储空间,另外一块作为处理缓冲区。3)图像信息处理速度:CPU的主要工作包括图像采集、图像信息处理以及运动控制等。图像采集采用中断的方式进行,如果采用75*24分辨率的图像,每隔4 行采集一行图像信息,图像采集所占用的CPU时间不会超过1/4。因此,大部分的CPU工作时间可以用于图像处理以及运动控制。由于采集到的图像由白色背景和黑色中心线组成,所以检测每一行路径中心线位置可以通过简单的阈值比较的方式计算出来。在此基础上,还可以通过参数拟合获取道路位置、方向以及曲率等参数。另外,通过适当的动态阈值的方法,可以提高算法的稳定性。核心算法如果处理相对简单,可通过适当的优化方法,在图像采集周期20毫秒
58、内计算出结果,达到实时图像处理的要求。如果算法比较复杂,可以将核心算法采用汇编语言完成,以提高效率,配合CPU超频运行方法,保证算法需要时间小于20毫秒。4) CCD器件工作电压:CCD器件工作电压需要在12V左右。为此,需要在系统电路设计中,增加唯有惜时才能成功,唯有努力方可成就!时间就是金钱,效率就是生命!独立12V电源模块。具体电路后面给出。5)视频同步信号分离:为了采集图像信息,CPU需要根据行、场同步信号启动AD转换器,采集稳定的图像。由于视频信号的变化很快,所以需要另外设计同步分离电路。在本方案中,使用了 LM1881视频同步分离集成块,获取视频同步信号,将此同步信号连到单片机的中
59、断输入端口。除此之外,一般的CCD输出的视频信号的峰峰值在IV左右,可以不经过放大直接连接到单片机的AD输入端口进行采集,也可以进行适当视频信号放大后将信号的峰峰值提高到3-4V左右输入到单片机。2 .3 .2 方向执行模块本智能车的方向执行机构是舵机,舵机控制采用PWM技术,不同占空比对应不同的转角。由于舵机内部含有自带的比较电平,有利于精确控制。舵机的额定电压一般是4.8V ,本模型车舵机额定电压为6V o当额定电压为 6 V 时,功率通常更强劲,速度也更快。这意味着只要提高舵机的电压,就可以获得更大的功率输出和更快的速度。对于提高电压这种未经认可的做法,每一家厂家的舵机反应也不尽相同。经
60、实践认证,本模型车的舵机完全可以工作在7.2V电压下。因此,提高了功率并加快了速度。另外,舵机的响应时间对于控制非常重要,一方面可以通过修改PWM周期获得。另一方面也可以通过机械方式,利用舵机的输出转角余量,将角度进行放大,加快舵机响应速度。本文在后面的模型车改装中将详细介绍。2 .3 .3 速度执行模块本智能车的速度执行机构是电机,采用PWM控制,利用脉宽占空比与速度的对应关系进行调速。采用电机驱动芯片MC33886,内部自带“H” 桥,可以快速实现电机的正转反转,从而对速度进行实时调整,精确控制。2.4本章小结根据本章以上的模块方案比较与论证,得出本智能车控制系统模型框图如图2.12所 不
61、 :唯有惜时才能成功,唯有努力方可成就!时间就是金钱,效率就是生命!速度信号速度检测包含赛道电机转速仑连杆机构转角I模型车模型车运动车 迹.图2.12系统模型框图唯有惜时才能成功,唯有努力方可成就!时间就是金钱,效率就是生命!第三章机械结构设计在智能车比赛中,最主要的比赛内容是速度,而模型车的机械结构无疑是影响速度的关键因素之一。鉴于这个原因,我们对模型车的机械结构做了很多的工作,进行了大量的调整,达到了比较满意的效果。3 .1 车模组装与改造3.1.1 车模组装模型车的组装工作看似简单,却需要很多的耐心和经验。首先,仔细阅读说明书。通过阅读模型车的装配图,可以了解各个不同零件的用途和安装顺序
62、。然后,根据模型车的装配图组装智能车模型。由经验得到,在组装过程中,不但要注意模型车的组装顺序,而且由于模型车零部件较小,组装过程中要防止零部件滑落和丢失。特别是,由于模型车上的大部分零部件材质均为塑料,在拧螺丝以及对零件进行加工时要格外的小心,以免损坏。3.1.2 前轮定位的调整在调试中我们发现,模型车过弯时,转向舵机的负载会因为车轮转向角度增大而增大。为了尽可能降低转向舵机负载,我们对前轮定位进行了调整。前轮定位的作用是保障汽车直线行驶的稳定性,转向轻便和减少轮胎的磨损。前轮定位参数主要包括:主销后倾角、主销内倾角、前轮外倾角和前轮前束。主销后倾角是主销轴线与地面垂直线在汽车纵向平面内的夹
63、角。主销内倾角是主销轴线与地面垂直线在汽车横向断面内的夹角。前轮外倾角是汽车横向平面与车轮平面的郊县与地面垂线之间的夹角。在一般情况下,主销后倾角为0-3度,主销内倾角为0-10度 ,前轮外倾角为 0 度 或 者 1度。在本模型中,后倾角过大会使得模型车转向沉重,从而使舵机转向存在严重的滞后,故在模型车中将主销后倾角调整为0 度;主销内倾角过大不仅会使得转向变得沉重,还将加速轮胎的磨损,因此将主销内倾角控制唯有惜时才能成功,唯有努力方可成就!时间就是金钱,效率就是生命!在5度以内;前轮外倾角和前轮前束分别设为0度、Ommo3.1.3 差速的调整模型车的差速对转弯时的影响很大,差速不好会导致后轮
64、空转,发生侧滑现像。我们通过添加推力轴承和润滑油的手段,改进差速装置,使得模型车在转向时,右轮与后轴之间的摩擦大大降低,从而提高了差速的效果,从而提高了小车的转向性能。3.1.4 舵机力臂的调整相 对 于S12单片机的处理速度,舵机的响应存在着较大的延时,对舵机的改造着实需要。在相同的舵机转速条件下,转向连杆在舵机一端的连接点离舵机轴心距离越远,转向轮转向变化越快,本模型车中通过用转向盘代替舵机上的曲柄来增大舵机的上连接点到舵机中心的距离,增加了输出转动力矩,使得前轮在转向时更加灵敏。图 3.1舵机的改造3 .2 摄像头的安装考虑到单片机的处理速度和电路限制问题, 把CCD图像传感器的数量定为
65、1个。如何安装CCD传感器将会影响到两个方面的性能, 一是图像处理的复杂程度和精度,一是小车的机械性能。由常识可以知道,如果小车的中心靠近后轴,对模型车动力性能是由益的;唯有惜时才能成功,唯有努力方可成就!时间就是金钱,效率就是生命!重心靠近前轴,对模型车的制动性和操纵性有益。考虑到智能车大赛对模型车的动力性要求并不像真正的赛车比赛要求那么高,而模型车所能达到的最快速度并不高, 所在安装CCD摄像头、电池和电路板位置时, 应适当使模型车的重心位置前移,从而提高转弯性能。因此我们将摄像头架设在舵机的上方,这样不但能够明显的将模型车的整个重心前移,而且能够使得摄像头能够更好的获取前方路况信息。模型
66、车摄像头的架设主要要考虑一下几个因素:1、确保摄像头位置的居中,因为当摄像头不居中,其采集进来的图像也不是居中的,而处理程序对舵机输出量是居中的,这样就会导致模型车在直道上也会存在左右摆动的问题。2、摄像头的高度要足够高,这样可以使得模型车在摄像头的角度不是很高的情况下就能够前瞻到前方足够远处的路况信息,因为当摄像头的角度过大时候,采集进来的图像形变过大,且图像中的干扰信息增多,对模型车的处理算法十分的不利。3、摄像头的架设一定要是可调整的,这样以便于摄像头居中的校正,以及在实际调试中选择最佳摄像头角度,以及对摄像头视野范围进行标定。图 3.2摄像头远度的标定经过多次的实验和总结,我们对摄像头
67、的远度进行了标定,对摄像头的采集的图象信息进行了中心位置的校正。 如图3.2所示,当摄像头架设为40cm高时候, 倾角为4 2度时候能够采集到前方8cm到58cm的图象信息, 对于识别直唯有惜时才能成功,唯有努力方可成就!时间就是金钱,效率就是生命!道、弯 道 和“S”道信息量已经足够。如 图3.3所示,将小车位置放置于所作的方格纸中心位置,将摄像头的视频信息接入到电视机中,通过调节摄像头各个旋转变量使得摄像头的图象位置居中。校正后的摄像头能够采集到小车前方上底 为20cm ,下底为60cm ,高 为50cm的等腰梯形图象。边长为4cm的正方形图3 .3图象中心位置校正3 .3光栅编码器的安装
68、对光栅编码器的安装,可以将光栅盘安装在电机轴上,通过先计算电机转速再来计算模型车后驱动轴得知车速。但是,这种方法太麻烦,并且在电机轴上装光栅盘会影响电机的性能。 所以,我们将光栅盘安装在模型车后驱动轴上,根据光电传感器的输出脉冲计算不同时刻模型车的后轮转速。图3 .4光栅编码器的安装唯有惜时才能成功,唯有努力方可成就!时间就是金钱,效率就是生命!3 .4电路板的固定与安装在电路板的安装这部分,我们考虑到结构的稳定性,以及规则中对车辆尺寸的限制,最终决定采用高架、立体的搭建方法,即用支架把主控板较高地固定在底盘上方。这种布局可以保证车辆行驶稳定。唯有惜时才能成功,唯有努力方可成就!时间就是金钱,
69、效率就是生命!唯有惜时才能成功,唯有努力方可成就!时间就是金钱,效率就是生命!第四章硬件系统设计与实现硬件电路的设计是自动控制器的基础。 智能汽车竞赛指定飞思卡尔公司S12系列的16位单片机MC9s 12DG128作为核心控制处理器。 本智能车采用了组委会提供的开发板MC9S12EVKC作为单片机最小系统,并在此基础上增加了各种接口电路板组成整个硬件系统。下面将对硬件设计中除了单片机最小系统之外的其他几个主要的模块设计进行讨论。4 .1电 源模块电源模块为系统其他各个模块提供所需要的电源。设计中,除了需要考虑电压范围和电流容量等基本参数之外,还要在电源转换效率、降低噪声、防止干扰和电路简单等方
70、面进行优化。可靠的电源方案是整个硬件电路稳定可靠运行的基础。全部硬件电路的电源由配发的标准车模用7.2V 2000mAh Ni-cd蓄电池提供。由于电路中的不同电路模块所需要的工作电压和电流容量各不相同,因此电源模块应该包含多个稳压电路,将充电电池电压转换成各个模块所需要的电压。主要包括以下不同的电压。 5V电压。主要为单片机系统、信号调理电路以及部分接口电路提供电源,电压要求稳定、噪声小,电流容量大于500mA。 12V电压。主要为CCD图像传感器提供12V的工作电源。 7.2V电压。这部分直接取自蓄电池两端电压,主要为舵机、后轮电机驱动模块和部分接口电路提供电源。除了 7.2V电压可以直接
71、由蓄电池获得,5V 电压需要通过降压稳压电路获得,12V电压通过升压稳压电路获得。电机驱动电路的电源可以直接使用蓄电池两端电压。模型车在启动过程中往往会产生很大的冲击电流,一方面会对其他电路造成电磁干扰;另一方面由于电池内阻造成电池两端的电压下降,甚至会低于稳压电路所需要的最低电压值,产生单片机复位现像。为了克服启动冲击电流的影响,可以在电源中增加容值较大的电解电容,也可以采用缓启动的方式控制电机。在启动时,驱动电唯有惜时才能成功,唯有努力方可成就!时间就是金钱,效率就是生命!路输出电压有一个渐变过程,使得电机启动速度略为降低从而减小启动冲击电流的幅度。4.1.1 降压稳压电路设计我们采用的降
72、压稳压芯片是LM1117-5.019l。LM1117 - 5.0是一种低压差的线性稳压器件, 最大输出电流为1A,足够提供系统中5V器件所需功率。另外,其输出电压波动范围仅为 (M V ,精度较高,经实验证明,能够满足本智能车系统中各项要求。典型电路如图3.1所示。我们最终应用的降压稳压电路就是在图3.1的基础上在输出端加一个1000 n F 的滤波电容即可。4.1.2 升压稳压电路设计主要的升压芯片有 MC34063A/E, MAX734, MAX632。在去年的比赛中,使用MAX734, MAX632芯片作为升压器件的参赛队伍很少。而MC34063A/E芯片的使用却很多。并且我在很多参考数
73、目上都看到了使用MC34063A/E芯片作为升压电路的实例, 所以我决定采用它作为升压器件。MC34063是开关稳压芯片,可构成升压、降压斩波电路。输出开关电流大于 1.5A。2.5mA的低静态电流。典型电路如图3 .2 所示 也若将MC34063应用到本升压电路,使输入V IN 为7.2V ,输出为12V ,则应将电路更改为图3 .3 所示。 实验表明, 这个电路能很好的实现12V升压的功能,满足本智能车硬件电路的需要。虽然有一定的发热,但是完全不影响各部分工作。唯有惜时才能成功,唯有努力方可成就!时间就是金钱,效率就是生命!图 3.2 MC34063典型电路图 3.3 MC34063得 到
74、 12V升压4 . 1 . 3电源模块小结综上,可以得到电源模块星型电路结构图3 . 4 :唯有惜时才能成功,唯有努力方可成就!时间就是金钱,效率就是生命!7.2V蓄电池图 3 .4 电源模块框图4 .2路径识别模块一般来说, 面阵CCD已经将芯片的驱动电路集成在一起了, 它的输出信号为标准的模拟复合视频信号。该信号中主要包括了同步信号和图像信号,它的幅值为IV 左右。对于该信号可以不经过放大直接由单片机的A/D端口采集到视频图像数据。此外,还可以直接通过外部的电压比较器,将模拟视频信号变成高低电平信号, 通过单片机的I/O 口输入到计算机, 这样可以避免由于单片机A/D转换速度而带来的采集图
75、像分辨率低的问题。无论采用哪种方法,都需要专门的视频同步分离电路提供行、场同步信号,这些同步信号一般送到单片机的外部中断端口。本智能车采用LM1881作为视频信号同步分离芯片。基于S12单片机采集视频图像电路系统框图如图3 .5 所示。其中包括有S12单片机最小系统、同步分离电路、5 V 稳压电路、12VMC34063斩波升压电路等。视频信号同步分离电路如图3.6所示口汽其中S12单片机端口资源配置如下:1) AD输入端口 PADOO, PAD08:输入视频模拟信号;2 ) 外部中断口 IRQ:输入视频同步信号;3) CS接单片机的PE1 口:输出复合同步信号;唯有惜时才能成功,唯有努力方可成
76、就!时间就是金钱,效率就是生命!4) VS接单片机的PH1 口:输出场同步信号;PV +12V图 3.5 视频信号同步分离电路电晚图 3.6 单片机采集图像系统框图唯有惜时才能成功,唯有努力方可成就!时间就是金钱,效率就是生命!4 .3 电机模块直流电机的控制由单片机的PW M 信号来完成,驱动芯片采用飞思卡尔半导体公司的半桥式驱动器MC33886。其工作电压为5-40V ,导通电阻为120mQ ,输入信号为TTL/CMOS, PWM频率小于lOKHz。电 路 如 图 3 .7 所示。图 3.7 MC33886电路示意图其 中 D I、D 2是 MC33886的使能端,INK IN 2为输入端
77、,0UT1、0UT2为其输出端。单片机通过PW M 通道的占空比控制电机速度,IN 2 和 IN 1分别接到PP1和 PPO上控制电机的正转和反转速度( 因为电机工作频率小于lOKHz,所以一个通道控制电机速度就够了) ,正转为智能车加速,当转弯时利用了反转PW M波来控制电机的减速; D 1和 D 2接到单片机的10 口上控制电机转动方向( 正转或反转) 。4 .4 舵机模块由于舵机工作频率为50H Z,需采用双通道PW M 控制舵机转向( 合 并 PP4、PP5)0将舵机的信号线接到PP5 口上。4 .5 测速传感器模块本智能车的测速采用光栅编码器, 它每转动一圈都会输出一定个数的脉冲,通
78、过在单位时间内测量得到的脉冲数,就可以得出电机的转速。唯有惜时才能成功,唯有努力方可成就!时间就是金钱,效率就是生命!光栅编码器由光栅盘和光栅式光电开关组成。光栅盘可以自己制作,光栅式光电开关为配对并集成的LED管和光敏三极管。LED ( 发射端)的出射光照射到一小段距离之外的光敏三极管( 接收端)上,传感器的状态随出射光是否被遮挡而改变。光电开关外围电路如图3 .8所示。图 3 . 8 光电开关外围电路电路A为传感器的发射端提供了限流电阻RI。R1可以选220Q1KQ。与反射式光电开关不同的是,在这里LED管的亮度不需要很高。事实上,如果LED管过亮,不仅增加功耗,而且红外光容易穿过阻光区域
79、,造成误信号。因此,可以从高阻值的电阻开始选用( 越1KQ) ,当发生误差信号问题时,只需将电阻值下调。电路B为传感器接收器( 光敏三极管)的外围电路。如果接收端使用的是PNP型光敏三极管,则采用电路C。对B与C的两个电路来说,R 3决定着灵敏 度 ,其取值范围由具体的接收端光敏三极管所决定,一般 为10KQ20MQo但光电开关直接输出的信号并不理想,为了使光电开关的输出信号数字化,提高抗噪声能力,应用施密特触发器。外围电路如图3.9所示:输出送单片机的增强型定时捕捉模块的PT7 口进行脉冲记数。唯有惜时才能成功,唯有努力方可成就!时间就是金钱,效率就是生命!4.6本章小结根 据 硬 件 系
80、统 的 设 计 ,得 出 系 统 硬 件 参 数 如 下 表3.1:表 3 .1系统硬件参数项目参数车 模 儿 何 尺 寸 ( 长 、宽 、高 )( 毫米 )280,162,368车 模 轴 距 / 轮 距 ( 毫 米 )139车 模 平 均 电 流 ( 匀 速 行 驶 ) ( 毫安)2000电 路 电 容 总 量 ( 微 法 )1817.6传感器种类及个数CCD图 像 传 感 器 ,1个新增加伺服电机个数无赛 道 信 息 检 测 空 间 精 度 ( 毫 米 )3.5赛 道 信 息 检 测 频 率 ( 次/ 秒)50除MC9S12DG128之外其它主要芯片MC33886,LM 1881,LM1
81、117,MC34063,74HC14车 模 重 量 ( 带 有 电 池 )( 千 克 )1.13唯有惜时才能成功,唯有努力方可成就!时间就是金钱,效率就是生命!第五章理论分析与算法实现5 .1 模型建立5.1.1 基于后轮差速的运动模型在车辆运动模型中,当车速不是很高的情况下,车辆转向中一般可以参考下面的一个模型当小车转角为0时候可以根据车长来求出小车当前的转弯半径/ ? = 公 式1tana式中L是小车的轴长,a为小车的转角,R为后轮转弯半径。然而这个模型只是适合在低速的情况下才能够得出较为精确的结果,鉴于本智能车的速度不高,在后轮差速很好的情况下,侧滑的因素将得到有效的抑制,可以考虑用这个
82、模型来处理。考虑图5. 2所示的以小车驱动轴中点为参考的运动模型,为参考点运动的两个连续位置,XK、X.I为后轴中点的速度方向, 即小车的纵向,eK唯有惜时才能成功,唯有努力方可成就!时间就是金钱,效率就是生命!为小车初始的方向角,。为小车的转向中心,那么可以得到1 :图 5 .2 智能车后轴中点运动模型 = /?1 /公式2其中是人到3 I时间内小车所走过的的距离,0是小车的横向角速度,&是小车的后轮的转弯半径。由图中的儿何关系可以得到:Xk+1 = Xk +ACOS( 4 + ) % +i=H+A sin( a + )4 + i= a +啰公式3上式的结果即为小车后轴中点在运动过程中的轨迹
83、方程,在加足够小的情况下,随着以 和X 以的不断变化可以画出其运动轨迹。当给出小车的初始方向角和初始坐标时,就可以推导出任何时刻小车的坐标值。在下面考虑小车整个模型的时候,用后轮差速的已知量,或者前轮转角和后轮速度值来替换图中的变量就可以得到,基于后轮差速和基于前轮转角和后轮速度的运动方程。接下来我们考虑小车的整个模型,如 图5. 3所示。其中L表示小车的轴间的距离,e表示轮距的一半,表示小车的转角,? ? ,表示小车后轮的转弯半径,中间的方向轮为虚轮。由图中所示的几何关系可以唯有惜时才能成功,唯有努力方可成就!时间就是金钱,效率就是生命!得 至IJ:& = 4tan将公式2带入公式4可以得到
84、:公式4公式5图 5.3智能车整车转向模型将 图5 .2的中反映的情况运用到整车的模型中去,当已知转角的情况下,用虚轮的转角近似等于前轮的转角,当后轮中点运动经过时,小车各个轮经过的距离和转角情况如图5.4, 所示:唯有惜时才能成功,唯有努力方可成就!时间就是金钱,效率就是生命!从上图的小车模型的几何关系中,我们很容易就能得到以下的结果:AR/ = R -e) 公式 6RR = G( R + e) 公式 7其中AR,.表示小车左后轮行驶的距离,A/?R表示小车右后轮行驶的距离, A,表示小车虚轮行驶的距离。由式6 和 7 可以得到:A/? + A/?,A =旦 -l2co = - -!2e公式
85、8那么只要能测量出小车两个后轮的行驶距离,就可以得出和。的值,小车两个后轮上分别安装上两个测速反馈,就能够得到想要的结果。因此在结合式% = K 4 + /才与+ ) ( 4 -六 。 1+ %和公式8 可以得到:RR + A/?z九+i = % +2RR+ N RI2 RR - AR/. C0S(4 + sin(4 +RR A/?,4e_ _A_/?,1 .4e公式94i +)2e公式9 所表示的模型就是基于后轮差速的运动模型, 只要给出一开始小车的位置和方位角,就能够根据已知的儿何关系递推的算出小车的后轴中点任何时刻的位置。5 .1 .2 基于速度和前轮转角的运动模型当小车在运动中,如果在
86、后轮安装上测速反馈,再根据当前小车转角的控制量的大小,就能够及时的得到小车的速度v和转角材的反馈值,那么由A = y加 ,。= 丝 里 叱 ,结合公式3 可以得出后轮中点的坐标为:唯有惜时才能成功,唯有努力方可成就!时间就是金钱,效率就是生命!+ 1 =为 + y cos(q +v Ar tan州+ i = 耳 + vAfsin(4 +2Lv Ar tan公 式 10v Ar tan8 + -也就是后轮中点连续两个位置的坐标变化为:Ax = v Ar cosAy = v Ar sin 0k+ u tan 根 据 图 5 .2 中的几何关系可以由后轮中点位置得出前轮中点位置的坐标,那么在连续两个
87、状态的情况下前轮中点的坐标如下:4 x = x,k + Lcos0,k 八公4式 12J = % + Lsin%x = xk+ l+Lcos(0k +co) , , 八 公 式 13y = yk+l +Lsm (4 +a)那么由公式12和公式13可以得到前轮中点在连续两个位置上的坐标变化为:A,AAx = v Ar cos (%n + -t-a-n- 、 + L ( coss v e t a n x 八、(4 H -) 一 cos 0ky = v Ar sin|A . g v / fta n y / . 口+ L sm(4 + -) - sin 0k4 +1 = 4 +v Ar tan公 式
88、14由上面的分析可以得出:只要知道当前的转角和速度值乙以及图像处理时间间隔加,结合小车一开始时候给出的位置和方位角,就能够逐步递推出小车前后轴中点在任何时候的位置,进而可以得出小车的运动轨迹。唯有惜时才能成功,唯有努力方可成就!时间就是金钱,效率就是生命!在小车的运动过程中,小车根据当前采集到的图像,由控制算法可以得出小车在下一个运动区间内的该给出多大的速度和转角。那么如果在小车上建立一个动坐标系,只根据当前的图像计算出小车的速度和转角的控制量,就能够满足小车运动需求。如下图,在已经知道小车速度和转角的情况下,以小车的后轴中点为坐标原点,小车的后轴为 轴 ,以小车的纵向为y轴 ,由上面得出的结
89、果可以很容易的得出下一个小车后轴中点和前轴中点位置的坐标。从图中很容易得到后轮中点的坐标变化为:Ax = cos(90 + )2L公 式15Ay = sin(90 + 血 )2L其中为小车在处理周期内经过的距离,即 = 4。因此由采集来的图像中知道坐标 , 每时候, 也可以由该公式反推出小车应该转过的角度,同时根据小车的机械性能和跑道的材质可以得出在该角度下不发生侧滑的最大速度,从而给出速度的参考量,详细的算法设计将在下面的内容中涉及。为了能够更好的实现控制算法及在实际情况下运用,可以将tanx, sinx,cosx在无o=O的情况下用泰勒公式展开可以作如下近似: 1、tan x x + r3
90、V3 1 P W M 波速度量v e l o c i t y 模 糊 控 制 器 - 电机驱动模块- - - - - - - 编科器 PWM模块0通道独立使用,4、5通道合并为一 16位 的PWM通道使用;PWM时钟选择为总线频率8分频即3MHz。定时/ 计数器模块全部通道设置为定时模式;时钟选择为总线频率8唯有惜时才能成功,唯有努力方可成就!时间就是金钱,效率就是生命!分频即3 M H z o此外,初始化时也对下面需用到的自变量进行了定义与赋初值,在此不作赘述。6 .2 视频图像信号采集本智能车图像采集的原理 网是根据视频同步信号对单个奇场或偶场图像信号进行隔行、隔点采集,一方面为了保证时序
91、上的准确性,必须准确的判断同步信号和严格的定时,另一方面定时的时间相对于C P U周期是比较长的,在这些时间内可以让C P U去做其它工作,故在此部分中的程序多采用了中断。具体的程序流程如图6 . 1所示。对视频信号的采集过程如下:等待场信号;延时,跳过场回程的2 2 . 5个行周期即1 . 4 4 m s ;等待行同步信号;判断采集行数是否满足要求,满足则采集完成,否则延时,跳过行同步信号和消隐信号,约4 . 7 +5 . 7 = 1 0 . 4 u s ;对一行视频信号进行连续采集;延时,跳 过6 . 5行视频信号即延时4 1 6 u s ,跳回到等待行同步信号,直至完成。(b)唯有惜时才
92、能成功,唯有努力方可成就!时间就是金钱,效率就是生命!( 中断进入7( d ) ( c)图6 . 1 ( a )主程序图像采集部分( b ) T i m e r。中断程序( c) T i m e r2中断程序( d ) T i m e rl中断程序( e )行同步6 .3图像处理和黑线提取6.3.1 图像处理由摄像头直接采集进来的图像的干扰信息比较多,并且在特殊的位置采集进来的图像有很多的信息是无用的信息,如十字交叉,甚至会严重的影响模型车的控制。针对上面提出的问题,我们先对采集进来的图像作一次差分和取阈值二值化。一次差分:假设采集进来的图像信息有N列信息,则用n+1列的值减去n列的值,作为n
93、列的值,那么可以得到n-1列值。取阈值二值化是根据当前的图像信息设定一个阈值,当图像上某点的值大于该阈值的时候设为1 ,当图像唯有惜时才能成功,唯有努力方可成就!时间就是金钱,效率就是生命!上某点的值小于该阈值时设为0。用上面提出的两种方法来处理采集进来的图像I,可以得到:原始图像原始图像(b)图 6 .2 图像处理结果取阈值二值化取阈值二值化由图6.2可以明显的看出, 在(a)中通过一次差分法处理后, 可以有效的将跑道外面的干扰信息去掉,以使得模型车能够在各种环境下适应跑道。在(b)中当模型车正对着十字交叉时,采集进来的图像是一个正的十字交叉通过一次差分的方法能够将错误的信息排除掉。采集进来
94、的图像经过两次上述一次差分处理后再用取阈值二值化法可以进步降低干扰因素,使得图像上的有效信息得到强化,提高了模型车的抗干扰能力,并且能够更好的识别出当前的路况信息,为下面的黑线提出做好了准备。6 .3 .2黑线的提取图像检测范围与图像复杂程度是一组矛盾。若图像检测范围大,在转弯等一些情况下在图像的边界或角落里就会出现干扰,不利于在软件上对图像的处唯有惜时才能成功,唯有努力方可成就!时间就是金钱,效率就是生命!理。若令图像检测的范围减小以使在所有情况下图像中都不会出现干扰,结果会出现如下情况:有些转弯采集不到黑线。检测到的直线赛道与弯道的差别很小,不利于算法上区分。因此在系统设计过程中, 先 把
95、 C C D 信号接入电视, 在多个不同位置多次试验得出一个C C D 摄像头安装最佳位置,使得检测范围足够大且干扰尽量小。在有干扰的情况下,经过测试发现,一般在图像中最后一行像素点数据即离小车最近的行出现干扰的情况极小,故可以在此行内搜索第一个黑点作为黑线左边界位置,搜索结束,以这个位置为基准左右各偏移一定距离作为下一行搜索的范围,下一行搜到边界后,再依次类推计算出以后各行的搜索范围。在有灯光反射干扰的情况下,有些行的图像中无黑线,若在此行搜索范围内搜索不到黑线,则记录一个错误位置标志,下一行搜索范围在此行基础上再增加一些。当一幅图像中搜索的结果中错误标志过多时,就认为此幅图像出错,控制方案
96、按照上一幅图像的处理结果决定。试验结果表明,此种方法能够有效地排除干扰,找到黑线的左边界,且效率高,处理时间短。同时加入右边界检测,可以减少干扰。视频去噪处理在再现真实的路径方面有着不可忽视的作用,正是由于C C D视觉传感器易受环境影响,才需要去噪算法进行去噪处理,保证视频数据的准确性。在本系统中,我们采取的视频去噪策略是取距离车体最近的数据,也就是第一行的数据作为参考数据, 根据数据的连续性, 进行整场数据的噪声处理。本模型车检测黑线中心位置算法的大体思想如下:首先判断图像采集是否完成,完成了才开始下面的计算。从最近一行图像开始检测,设定检测的左、右边界值。 在边界值限定的范围内检测各个点
97、是否是黑点,并记录下黑点的左、右边界位置。根据该行获得的黑线左、右边界位置来确定下一行检测的左、右边界值。如此循环直到处理完整幅图像。此外,考虑到在某些行上可能没有测到黑点,这种情况的发生有可能是图像本身就没有黑点,也有可能是错误的信息。因此,对于没有黑点的行将置一个无效标志位,为之后能成功地对电机及舵机进行控制做准备。黑线中心的检测算法流程图如图6.3所示。唯有惜时才能成功,唯有努力方可成就!时间就是金钱,效率就是生命!图 6.3 黑线提取流程图唯有惜时才能成功,唯有努力方可成就!时间就是金钱,效率就是生命!第七章开发与调试7 .1软件开发环境介绍Codewarrior是Metrowerks
98、公司开发的软件集成开发环境1, 飞思卡尔所有系列的微控制器都可以在codewarrior IDE下进行软件开发。 本模型车所用的处理器是Mc9sDG128B,程序调试是在codewarrior IDE 3.1环境下实现的,所用语言为汇编。首先要新建一个基于Mc9sDG128B的HCS12的工程,选用语言为绝对汇编,具体的过程如图7-1到7-3所示。建立好新的工程后,就可以在编译器里进行程序的编写。图7-1 Codewarrioer新建工程界面(a)唯有惜时才能成功,唯有努力方可成就!时间就是金钱,效率就是生命!图7-2 Codewarrioer新建工程界面(b)的 列 出/ffi ttTM n
99、t 卬;irnTi iFite Edt View Search Project Debug Processor Expert Wndow Hdt占届 0。电 省 圈守力、曲涵J x inewproject mep.b - . M, 母Path: C:DocsontM and Sottin$zAdnituxtratoiA桌面naYprojctSod . nin. sr Simulator 二j / 1 “ G 斗 0F iles | Link Ordr Tar gats | Filem radn txt - pSources, Rain, a&rj/ CjPrr-r:Q L ibr4ris ic
100、9sl2dgl28 inc ( 1 PobiKgr Project F ile n Debugger Cnd F iles用ewez lz l:40000000Sa/i ; * * * *,* *;* This stationery serves as the framevork for a *;* user application (single ilez absolute assenbly application) ,* For a nore comprehensive progran that ,* denonstrates the more advanced functionality
101、 o this ,* processorz please see the demonstratxon applications ; located in the exaAples subdirectory of the , Metrowerks CodeVaxrior for the HC12 Program directory , export symbolsXDEF Entry export Entry symbolABSENTRY Entry . for absolute assembly: nark this as applicatic; include derivative spec
102、ific MacrosINCLUDE nc9sl2dgl28 incROMStart EQU $4000 . absolute address to place code/constant data. variable/data sectionORG RAMStartInsert here your data def inition For demonstration. temp_byte is usedtenp_byte ds b 1o2; code sectionORG ROMStartEntryCXI , enable interruptsMOVE #l,tenp_byte just s
103、ome demonstration codeNOP Insert here you own codeBRA Entry : endless loop; * * * * * * * * * * * *, Interrupt Vectors * * * * * * * * * * * * ORG SFFFEfdb Entry ; ResetJ1。 皿工 L in J Col 1 1 |图7-3 Codewarrioer新建工程界面(c)唯有惜时才能成功,唯有努力方可成就!时间就是金钱,效率就是生命!7 .2智能车整体调试舵机调试首先在程序里不断的修改舵机的控制量,确定舵机左转和右转极限的PWM值,
104、记下该值留着在程序里设定左转和右转的极限值。然后用一小段跑道来测试舵机的转向,用不同的跑道段来测试模型车的转向是否符合要求,当用的是直道时候要保证模型车的舵机位置绝对的居中,小“S”道时候,模型车给出的转角应该足够的小,大“S”时候转角应该近似为圆弧弦的角度,弯道时候要给足转角。电机调试像测试舵机一样用不同的赛道来测试电机给出的转速是否满足实际控制的需要,当用的是小“S”道时候,模型车应该保持直道上的速度,大“S”时候,应该适当的减速,以使得通过时候不会产生过大的超调,弯道时候要能够很快的将速度降下来,如果是急转弯时可以不给控制量或者让电机反转以达到刹车的效果。整体调试首先以一个较低的速度跑完
105、整个赛道,然后再慢慢的提高速度,直到模型车在某一个地方出错,然后调整控制算法,如此反复,直到模型车能够以理想的速度,在理想的路线上运行完为止唯有惜时才能成功,唯有努力方可成就!时间就是金钱,效率就是生命!唯有惜时才能成功,唯有努力方可成就!时间就是金钱,效率就是生命!第八章结论8 .1总 结经过队员们几个月的努力,我们完成了智能车系统的制作。整个车模硬件电路简单,在CCD传感器的控制下,通过判断,使模型车转向准确稳定,能够完全通过各种弯道和十字交叉路口。该模型车在满足大赛要求的前提下,具有良好的自主道路识别能力和稳定性,并能够以较快的速度行驶。在最初的方案选择阶段,通过比较不同方法可能得到的结
106、果明确了我们研究的方向。在机械设计方面,我们对模型车的性能不断的探索改进了前轮定位、差速、舵机力矩等,使得舵机的转向更加灵活、更容易控制,打滑现象基本解决。在控制算法方面,我们通过建模和仿真,最终确定使用模糊控制算法对模型车的方向和速度进行控制。在图像采集和图像处理方面,我 们 以S12单片机为核心,结合摄像头的安装设计了合理的采集方法,并对采集的图像用阈值二值化方法进行处理,降低干扰。8 .2 展望同时由于时间的不足,在模型车的研究中还有许多需要改进的地方:1、本文所建立的智能汽车的运动模型是在理想情况下建立的,其实际情况要复杂的多,所建立的模型过于简单。另一方面,智能汽车是一个高度非线性化
107、、时变的系统,如何对模型中的参数进行估计以及使模型更能反映实际车辆运动是一个需要进一步解决的问题。2、模糊控制对数学模型很难建立的系统具有先天性的优势,而在本文中只是用一种很简单的模糊控制方法来控制模型车,考虑的模型车的其他干扰因素还不够多,因此如何进一步改进控制算法是十分有必要的。3、在本模型车在处理算法中,处理的频率为50H z,模型车的处理算法的频率,极大地限制着模型车整体速度的提高,在以后的研究中,我们将考虑使唯有惜时才能成功,唯有努力方可成就!时间就是金钱,效率就是生命!用非标准的频率高的特殊摄像头或者在算法中实现处理频率的提高。4、模型车的测速装置为自制的光栅式编码器,模型车后轮每
108、转动一圈输出16个脉冲,这样使得测得的速度只有参考的价值,而不能定量的去使用,限制模型车速度的提高,我们以后将考虑使用更高精度的轴编码器或者测速电机,使得反馈的速度值更加精确,以便提高控制算法的可靠性。唯有惜时才能成功,唯有努力方可成就!时间就是金钱,效率就是生命!参考文献1 卓晴, 黄开胜, 邵贝贝. 学做智能车挑 战 “ 飞思卡尔”杯M.北京:北京航空航天大学出版社,2007.2 邵贝贝. 单片机嵌入式应用的在线开发方法M.北京:清华大学出版社,2004.3 周继明,江世明,彭解华. 传感器技术与应用M.长沙:中南大学出版社,2005.4 Ramon Pallas-Areny, John
109、G. Webster.传感器和信号调节( 第二版) M.北京:清华大学出版社,2003.5 宗光华. 机器人的创意与实践M.北京:北京航空航天大学出版社,2004.6 卓晴,王硼,王磊. 基于面阵CCD的赛道参数检测算法.7 MC33886 Technical Data, Freescale Semiconductor July 2005.8 余志生. 汽车理论( 第 3 版)M.机械工业出版社,2004.9 LM1117 800mA Low-Dropout Linear Regulator, National Semiconductor February1999.10 MC34063A/E
110、DC-DC Converter Control Circuits, Motorola.11 Motorola MC9S12DG128 Device User Guide Motorola, Inc.12 LM 1881 Video Sync Separator, National Semiconductor June 2003.13 熊万龙. 基于Kalman滤波器的自主车辆定位方法研究J.湖南:国防科技大学学位论文,2005.14 殷人昆,陶永雷,谢若阳,盛绚华. 数据结构( 用面向对象方法和C+描述) . 北京:清华大学出版社. 2005.15 关柏青. 基于彩色视觉和模糊控制的移动机器人
111、路径跟踪J.河北工业大学学位论文. 2002.16 谢剑英,贾青. 微型计算机控制技术M.北京:国防工业出版社,2004.17 飞思卡尔HC08/HCS12系列微控制器开发环境-Codewarrior使用指南.18 孙景琪等,视频技术与应用. 北京工业大学出版社. 1 9 刘叔军,盖晓华,樊京,崔世林等.MATLAB 7 .0控制系统应用与实例M.北京. 机械工业出版社.2006.唯有惜时才能成功,唯有努力方可成就!时间就是金钱,效率就是生命!唯有惜时才能成功,唯有努力方可成就!时间就是金钱,效率就是生命!附录:控制程序XDEF EntryABSENTRY EntryINCLUDE PWM.i
112、nc1INCLUDE ECT.inc1INCLUDE PORT.incINCLUDE ATD.inc1ROMStarlEQU$4000INTCREQU$00 IEF_FINISHEQU$2000TCNTEQU$0044C_LINEEQU$2002C_POINTEQU$2003P_SAVEEQU$2004P_LINEEQU$2006P_LINE2EQU$2008P_ROWEQU$200AP_POSITIONEQU$200CP_YOUEQU$200EP_YOU1EQU$2010P_COMPARE0EQU$2012COUNT 10EQU$2014TEMEQU$2016P_LINE1EQU$2018L
113、EFT_SEQU$201ARIGHT_SEQUf $20 IBADD_NUM0EQU$20 ICADD_SUM0EQU $20 IECOUNT_LAVERAGETEMOERRORTCNTOTCNT1COMPAREOCOMPARERIGHT 1UUUNUUW犯uE QE QE QE QE QE QE QE QE Q$2020$2021$2023$2025$2027$2029$202B$202C$202D唯有惜时才能成功,唯有努力方可成就!时间就是金钱,效率就是生命!LEFTIMM0LINE_CHAPOINT_CHASUDU01SUDU02SUDUO3EFO152156$202$202$203$2
114、03$20z3$203E Q UE Q UE QXUE Q UE Q UE Q ULIANXU_ERROR EQU $2037MIN$2038MAX$2039E_HIGH$203AP_POINTL$203CP_POINTR$203DADD_SUM$203EADD_NUM$2040C_MIN$2042C_MAX$2043TEM10$2044FIRST$2046LAST$2047FIRST_LINE$2048LAST_LINE$2049RESUETOEQU $204ARESULT 1EQU$204CRESULT2EQU$204ERESULT3EQU$2050RESULT4EQU$2052RESUL
115、TSEQU$2054DDEQU$2056DDIEQU$2057LINE_LINE_MAX EQU $2058LIANXUEQU$2059GAOSUEQU$205ADDDEQU $205BRESULT6EQU $205DRESULT7EQIJ $205FSTIME.COUNT EQU $2061STIME_COUNTO EQU $2063FLOWEC)U $2065唯有惜时才能成功,唯有努力方可成就!时间就是金钱,效率就是生命!FIRST0EQU $2066PERIOD 1EQU $2068PERIOD2EQU $206APERIODEQU $206CZHUANJIAOEQU $206ESUDU
116、EQU $2070ZHI_XIANEQU $2072WAN.XIANEQU $2074ORG $2080CRRNT_INSRMB 2FUZJNSRMB $0AFUZ.OUTSRMB 7COG_OUTRMB 1CRRNTJNS1RMB 2FUZJNS 1RMB $0AFUZ.OUTSlRMB 7COG_OUT1RMB 1ORG $B600INPUT_MFS EQU *FCB $0001,$001A,$00ff,$000EFCB $000E,$002B,$000E,$000EFCB $001 F,$003C,$000E,$000EFCB $0030,$004D,5000E,$000EFCB $00
117、41 ,$005C,$000E,$00ffFCB $0001,$0032,$00ff,$0006FCB $0019,$0055,$0006,$0006FCB $003C,$0078,$0006,$0006FCB $005F,$009B,$0006,$0006FCB $0082,$00b4,$0006,$00ffSGLTN_POS EQU *FCB 60,90,120,130,140,165,210INPUT_MFS 1 FCB $0001,$0032,$00ff,$0006FCB $0019,$0055,$0006,$0006FCB $003C,$0078,$0006,$0006FCB $(X
118、)5F,$009B,$0006,$0006FCB $0082,$00b4,$0006,$00ffFCB $0001,$0008,$00ff,$003fFCB $0004,$000e,$003f,$003f唯有惜时才能成功,唯有努力方可成就!时间就是金钱,效率就是生命!FCB $000a,$0014,$003f,$003fFCB $0010,$001a,$003f,$003fFCB $0016,$001F,$003f,$00ffSGETN_POS1 EQU *FCB 01,40,70,90,110,140,178RULE EQU *FDB FUZJNS+$0003,FUZJNS+$0005,$F
119、FFE,FUZ_INS4-$000C,$FFFFFDB FUZ_INS+$0003,FUZNS+$0006,$FFFE,FUZNS+$000d,$FFFFFDB FUZJNS+$0003,FUZJNS+$0007,$FFFE,FUZ_INS+$000e,$FFFFFDB FUZNS+$0003,FUZNS+$0008,$FFFE,FUZ_INS+$000f,$FFFFFDB FUZ_INS+$0003,FUZ_INS+$0009,$FFFE,FUZNS+$0010,$FFFFFDB FUZ_INS+$0004,FUZ_INS+$0005,$FFFE,FUZ_INS+$000d,$FFFFFDB
120、 FUZ_INS+$0004,FUZJNS+$0006,$FFFE,FUZ_INS+$000e,$FFFFFDB FUZJNS+$0004,FUZJNS+$0007,$FFFE,FUZ_INS+$000f,$FFFFFDBFUZ_INS+$0004,FUZJNS+$0008,$FFFE,FUZ_INS+$0010,$FFFFFDB FUZ_INS+$0004,FUZ_INS+$0009,$FFFE,FUZNS+$0010,$FFFFFDBFUZJNS+$0002,FUZJNS+$0005,$FFFE,FUZ_INS+$000b,$FFFFFDBFUZ_INS+$0002,FUZJNS+$000
121、6,$FFFE,FUZ_INS+$000c,$FFFFFDB FUZ_INS+$0002,FUZ_INS+$0007,$FFFE,FUZ_INS+$000d,$FFFFFDB FUZJNS+$0002,FUZJNS+$0008,$FFFE,FUZ_INS+$000e,$FFFFFDB FUZ_INS+$0002,FUZ_INS+$0009,$FFFE,FUZ_INS+$000f,$FFFFFDB FUZNS+$0001 ,FUZNS+$0005,$FFFE,FUZNS+$000a,$FFFFFDB FUZ_INS+$0001 ,FUZJNS+$0006,$FFFE,FUZ_INS+$000b,
122、$FFFFFDB FUZNS+$0001 ,FUZNS+$0007,$FFFE,FUZNS+$000c,$FFFFFDB FUZ_INS+$0001 ,FUZ_INS+$0008,$FFFE,FUZ_INS+$000d,$FFFFFDB FUZ_INS+$0001 ,FUZ_INS+$0009,$FFFE,FUZ_INS+$000e,$FFFFFDB FUZJNS+$0000,FUZJNS+$0005,$FFFE,FUZ_INS+$000a,$FFFFFDB FUZ_INS+$0000,FUZ_INS+$0006,$FFFE,FUZ_INS+$000a,$FFFFFDB FUZ_INS+$00
123、00,FUZJNS+$0007,$FFFE,FUZ_INS+$000b,$FFFFFDB FUZJNS+$0000,FUZJNS+$0008,$FFFE,FUZ_INS+$000C,$FFFFFDBFUZ_INS+$0000,FUZJNS+$0009,$FFFE,FUZ_INS+$000d,$FFFFRULE1 EQU *FDB FUZJNS1 +$0003,FUZ_INS l+$0005,$FFFE,FUZ_INS l+$0010,$FFFFFDB FUZJNS I+$0003,FUZJNS 1 +$0006,$FFFE,FUZ_INS 1 +$000F,$FFFFFDB FUZJNS 1+
124、$0003,FUZJNS 1+$0007,$FFFE,FUZ_INS l+$000E,$FFFFFDB FUZJNS 1 +$0003,FUZ_INS 1 +$0008,$FFFE,FUZJNS 1 +$0(X)E,$FFFFFDB FUZJNS 1 +$0003,FUZJNS 1 +$0009,$FFFE,FUZJNS 1 +$000E,$FFFFFDB FUZJNS 1 +$0004,FUZ_INS 1 +$0005,$FFFE,FUZNS 1 +$0010,$FFFF唯有惜时才能成功,唯有努力方可成就!时间就是金钱,效率就是生命!FDB FUZNS 1 +$0004,FUZ_INS 1
125、+$0006,$FFFE,FUZNS 1 +$0010,$FFFFFDB FUZJNS1 +$0004,FUZ_INS 1 +$0007,$FFFE,FUZ_INS 1 +$000F,$FFFFFDB FUZJNS 1 +$0004,FUZ_INS I +$0008,$FFFE,FUZ_INS 1 +$000E,$FFFFFDB FUZJNS 1 +$0004,FUZ_INS 1 +$0009,$FFFE,FUZ_INS 1 +$000E,$FFFFFDB FUZJNSl+$0002,FUZJNSl+$0005,$FFFE,FUZJNSl+$000D,$FFFFFDB FUZJNS 1+$00
126、02,FUZJNS 1 +$0006,$FFFE,FUZ_INS 1 +$000D,$FFFFFDB FUZJNS 1 +$0002,FUZ_INS 1 +$0007,$FFFE,FUZ_INS 1 +$000D,$FFFFFDB FUZJNS 1 +$0002,FUZ_INS 1 +$0008,$FFFE,FUZ_INS 1+S000D,$FFFFFDB FUZJNS 1 +$0002,FUZ_INS 1 +$0009,$FFFE,FUZ_INS 1 +$000D,$FFFFFDB FUZJNS 1 +$0001,FUZJNS 1 +$0005,$FFFE,FUZ_INS 1 +$000A,
127、$FFFFFDB FUZJNS 1 +$0001,FUZJNS 1 +$0006,$FFFE,FUZ_INS 1 +$000B,$FFFFFDB FUZJNS 1 +$0001,FUZJNS 1 +$0007,$FFFE,FUZ_INS 1 +$000C,$FFFFFDB FUZJNS 1 +$0001,FUZJNS 1 +$0008,$FFFE,FUZ_INS 1 +$000C,$FFFFFDB FUZJNS 1 +$0001,FUZJNS 1 +$0009,$FFFE,FUZ_INS 1 +$000C,$FFFFFDB FUZJNS l+$0000,FUZ_INS 1 +$0005,$FF
128、FE,FUZ_INS I +$000A,$FFFFFDB FUZJNS 1 +$0000,FUZ_INS 1 +$0006,$FFFE,FUZ_INS 1+$000A,$FFFFFDB FUZJNS 1 +$0000,FUZ_INS 1 +$0007,$FFFE,FUZ_INS I +$000B,$FFFFFDB FUZJNS 1 +$0000,FUZ_INS 1 +$0008,$FFFE,FUZ_INS 1 +$000C,$FFFFFDB FUZJNS 1 +$0000,FUZJNS 1 +$0009,$FFFE,FUZ_INS 1 +$000C,$FFFFWEIGHTEQU *FCB $F
129、F,$FF,$FF,$FF,$FFORG ROMStartEntryLDAA #$40STAA PWMCTLLDAA #$33STAAPWMPRCLKLDAA #$00STAA PWMCLKLDAA #$FFSTAA PWMPOLLDAA #$64STAA PWMPEROLDAA #$64STAAPWMPER1LDAA #$00STAA PWMDTYOLDAA #$00唯有惜时才能成功,唯有努力方可成就!时间就是金钱,效率就是生命!STAAPWMDTY1LDAA#$EASTAA PWMPER4LDAA #$60STAAPWMPER5LDAA #$11STAA PWMDTY4LDAA #$94S
130、TAA PWMDTY5LDAA #$33STAA PWMELDAA #$0FSTAATIOSLDAA #$00STAA TIELDAA #$03STAATSCR2LDAA #$40STAATCTL3LDAA #$81STAATSCR1LDAA #$00STAAATD0CTL2LDAA #$08STAAATD0CTL3LDAA #$80STAAATD0CTL4LDAA #$00STAAATDODIENLDAA #$00STAAATD1CTL2LDAA #$08STAAATD1CTL3LDAA #$80STAAATD1CTL4LDAA #$00STAA ATD1 DIENLDAA #$00STAA
131、 DDRHLDAA #$00唯有惜时才能成功,唯有努力方可成就!时间就是金钱,效率就是生命!SUDUOSUDU1STAA DDRBLDAA#$FFSTAA DDRKLDAA #$08STAA PORTKLDAA #$00STAA INTCRLDAA #$00STAA MOLDAA#$2DSTAA DDLDD #$1173STD RESULTOLDD #$10D0STD RESULT6LDD #$1200STD RESULT7LDAA PORTBSTAALIANXUCMPA#$FEBEQ SUDU1CMPA#$F7BEQSUDU2CMPA#$F6BEQSUDU3BRASUDUOLDAA #$40
132、STAA GAOSULDD #$14 AOSTD ZHI_XIANLDD #$16A0STD WAN.XIANLDAA #$42STAA SUDUO 1LDAA #$41STAA SUDU02LDAA #$41STAASUDU03LB RA AGAINLDAA #$41唯有惜时才能成功,唯有努力方可成就!时间就是金钱,效率就是生命!STAA GAOSULDD #$1400STD ZHI_XIANLDD #$1630STD WAN_XIANLDAA #$42STAASUDU01LDAA #$41STAA SUDU02LDAA #$41STAA SUDU03BRA AGAINSUDU2LDAA #
133、$42STAA GAOSULDD #$1350STD ZHI_XIANLDD #$16A0STD WAN_XIANLDAA #$42STAASUDU01LDAA #$42STAASUDU02LDAA #$41STAA SUDU03BRA AGAINSUDU3LDAA #$42STAA GAOSULDD #$12A0STD ZHI.XIANLDD #$16A0STD WAN_XIANLDAA #$42STAA SUDU01LDAA #$42STAA SUDU02LDAA #$41STAA SUDU03BRA AGAINAGAINWF_FLDAA PORTH唯有惜时才能成功,唯有努力方可成就!时间
134、就是金钱,效率就是生命!ANDA #$02CMPA#$02BEQ WF_FCLILDAA #$00STAAC_LINESTAA F.FINISHLDD #$2100STD P_SAVELDD TCNTADDD #$0EE0STD TCOLDAA #$FFSTAATFLG1LDAA #$01STAA TIEWF_FINISH LDAA F_FINISHCMPA#$01BNE WF_FINISHSEILDD #$2C3FSTD P_LINESTD P_ROWLDD #$3159STDP_YOU1LDD #$2E00STD P_POSITIONLDD #$3100STD P_LINE1LDX P_L
135、INE1NEXT_POINTO LDY P_LINELDAAYSTAAXINXLDD P_LINESUBD #$0020STD P_LINECPD #$2100BHI NEXT_POINTONEXT.LINEO LDD P_ROW唯有惜时才能成功,唯有努力方可成就!时间就是金钱,效率就是生命!SUBD#$0001STD P_ROWSTD P_LINECPD #$2C21BHI NEXT_POINTOLDX #$3268LDAAXSTAA C_MINSTAA C_MAXJUXINXLDAAXCMPA C_MINBHI MAXISTAA C_MINMAX1CMPA C_MAXBLO MINISTA
136、A C_MAXMINICPX #$3B32BGT FINISH2BRA JUXFINISH2LDAA #$00LDAB C_MINSTD TEM10LDAA #$00LDAB C_MAXADDDTEM10LDX #$0002IDIVTFR X,DSUBD #$0010STAB COMPARELDD #$3100STD P_LINE2LDAA #$05STAA P_POINTLLDAA #$55STAAP_POINTRNEXT_LINE LDX P_LINE2LDAB P_POINTRABX唯有惜时才能成功,唯有努力方可成就!时间就是金钱,效率就是生命!STX E_HIGHLDX P.LINE2
137、LDAB P_POINTLABXNEXT_POINT LDAA XCMPA COMPARELBHI N_BLACK1TFR X,DSUBD P_LINE2STAB LEFT_SN_WHITE INXCPX E_HIGHBGT LINE_FULLLDAAXCMPA COMPAREBLO N.WHITEN_WHITE1 TFR X,DSUBD P_LINE2STAB RIGHT_SLDX E_HIGHRIGHT LDAA XCMPA COMPAREBHI N_BLACK2TFR X,DSUBD P_LINE2CMPB RIGHT.SLBLO HUANFINISH_0 SUBB RIGHT_SCMP
138、B #$0ALBGT LINE_ERRORLINE_FULL TFR X,DSUBD P_LINE2STAB RIGHT_SADDB LEFT_SLSRBLDY P_POSITIONSTAB YCMPB #$10BLS L_OVERFLOWCMPB #$49唯有惜时才能成功,唯有努力方可成就!时间就是金钱,效率就是生命!BGE R_OVERFLOWSUBB #$10STAB P_POINTLADDB #$20STAB P_POINTRBRA INC_LINEL_OVERFLOW TBAADDA #$10STAA P_POINTRLDAA#$01STAA P_POINTLBRA INC.LINE
139、R_OVERFLOW TBASUBA #$10STAA P_POINTLLDAA#$58STAA P_POINTRBRA INC_LINEINC.LINE LDD P_LINE2ADDD #$005ASTD P_LINE2CPD #$3B32BHI FIND_FINISHLDX P_POSITIONINXSTX P_POSITIONLBRA NEXT_LINEN_BLACK1 INXCPX E_HIGHBGT LINE.ERRORLBRA NEXT_POINTN_BLACK2 DEXCPX P_LINE2BET LINE_ERRORLBRA RIGHTLINE_ERROR LDAA #$FFL
140、DY P_POSITIONSTAAYBRA INC_LINEHUAN STAB TEM唯有惜时才能成功,唯有努力方可成就!时间就是金钱,效率就是生命!LDAB RIGHT.SLDAATEMSTAA RIGHT.SLBRA FINISH_OFIND_FINISHSSLDX #$2E00LDAA #$OOSTAA ERRORAGAINOOLDAB XCPX #$2E1ELBEQHINXCMPB #$FFBNE AGAINOOINC ERRORBRA AGAINOOHLDAAERRORCMPA#$18LBHI FIGURE_ERRORLDAA #$1ESTAA LAST.LINELDX #$2E1D
141、CHAOLDAAXCMPA#$FFBEQ AGAIN002STAA LASTLDAA #$01STAA FIRST_LINELDX #$2DFFCHAIINXLDAAXCMPA#$FFBEQ AGAIN000STAA MINSTAA MAXSTAA FIRSTCHA2INXLDAAXCMPA#$FFBEQ AGAINOO 1唯有惜时才能成功,唯有努力方可成就!时间就是金钱,效率就是生命!CM PA MAXBUT MINOSTAA MAXMINOCMPAMINBGT MAXOSTAA MINMAXOCPX #$2E1DBGT FINISH 1BRACHA2AGAINOOOINC FIRST.LI
142、NECPX #$2E1DLBEQ AGAINBRA CHAIAGAINOO1CPX #$2E1DBEQ FINISH 1BRA CHA2AGAIN002DEC LAST_LINEDEXCPX #$2E00LBLT AGAINBRA CHAOFINISH 1LDAA LASTLDAB FIRSTSBASTAA POINT_CHALDAA POINT_CHAADDA#$5ASTAA POINT_CHALDAA LAST_LINELDAB FIRST_LINESBASTAA LINE_CHALDAAPOINT_CHAADDA #$O1STAA CRRNT_INS1LDAA LINE.CHAADDA
143、#$01STAA CRRNTJNS1 + 1FUZZIFY 1:LDX #INPUT_MFS1LDY #FUZNS1唯有惜时才能成功,唯有努力方可成就!时间就是金钱,效率就是生命!LDAA CRRNTJNS1LDAB #5GRAD_LOOP1: MEMDBNE B,GRAD_LOOP1LDAA CRRNT_INS1+1LDAB #5GRAD_LP1: MEMDBNE B,GRAD_LP1LDAB #07RULE_EVAL1: CLR 1,Y+DBNEB,RULE_EVAL1LDX #RULE1LDY#WEIGHTLDAB #25EVALUE 1: LDAA #$FFREVWDBNEB,EVAL
144、UE1DEFUZ1: LDX #SGLTN_POS 1LDY #FUZ_OUTS1LDAB #07WAVEDIVTFR Y,DSTAB COG_OUT1LDAA LASTADDA #$01STAA CRRNTJNSLDAA COG_OUT1ADDA #$01STAA CRRNTJNS+1FUZZIFY: LDX #INPUT_MFSLDY#FUZJNSLDAA CRRNTJNSLDAB #5GRAD_LOOP: MEMDBNE B,GRAD_LOOPLDAA CRRNT_INS+1LDAB #5GRAD_LP: MEMDBNE B,GRAD_LP唯有惜时才能成功,唯有努力方可成就!时间就是金钱
145、,效率就是生命!LDAB #07RULE.EVAL: CLR 1,Y+DBNE B,RULE_EVALLDX #RULELDY#WEIGHTLDAB #25EVALUE: LDAA #$FFREVWDBNE B,EVALUEDEFUZ: LDX #SGLTN_POSLDY #FUZ_OUTSLDAB #07WAVEDIVTFR Y,DSTAB COG.OUTLDAA #$0ALDAB COG_OUTMULADDD #$0C26STD RESULT 1CPD RESULT6BLT XIAOCPD RESULT7BGT DAZHUAN LDD RESULT 1CPD #$14AOBGT GUOCP
146、D #$0E00BLT GAOSTAA PWMDTY4STAB PWMDTY5STD RESULTOLBRA MOTORGUOLDD #$14A0STAA PWMDTY4STAB PWMDTY5STD RESULTOLBRA MOTORGAOLDD #$0E03唯有惜时才能成功,唯有努力方可成就!时间就是金钱,效率就是生命!STAA PWMDTY4STAB PWMDTY5STD RESULTOLBRA MOTORXIAOLDD RESULTOCPD RESULT7BGTSBRAZHUANDALDD RESULTOCPD RESUET6BLTSBRAZHUANSLDD RESULTOADDD R
147、ESULT 1LDX #$0002IDIVTFR X,DCPD #$14AFBGT GUOOCPD #$0E00BLO GAIOSTAA PWMDTY4STAB PWMDTY5STD RESULTOSTD RESULT4JMP MOTORGUOOLDD #$14AFSTD RESULTOSTD RESULT4STAA PWMDTY4STAB PWMDTY5BRA MOTORGAIOLDD #$0E00STAA PWMDTY4STAB PWMDTY5STD RESULTOSTD RESULT4STAA PWMDTY4STAB PWMDTY5BRA MOTOR唯有惜时才能成功,唯有努力方可成就!时
148、间就是金钱,效率就是生命!FIGURE_ERROR LDAA #$AASTAA PORTBLDAA #$41STAA PWMDTY0LDAA #$00STAA PWMDTY1LDD RESULTOCPD #$1194BHI FIGURE_ERROR1LDD RESULTOSU BD #$0064CPD #$0E00BLO GAI1STAA PWMDTY4STAB PWMDTY5STD RESULTOLDAA#$2DSTAA DDJMP AGAINGAI1LDD #$0E00STAA PWMDTY4STAB PWMDTY5STD RESULTOLDAA#$2DSTAA DDJMP AGAINFI
149、GURE_ERROR 1 LDD RESULTOADDD #$0064CPD #$14AFBHI GUO1STAA PWMDTY4STAB PWMDTY5STD RESULTOLDAA#$2DSTAA DDJMP AGAINGUO1LDD #$14AFSTAA PWMDTY4STAB PWMDTY5STD RESULTO唯有惜时才能成功,唯有努力方可成就!时间就是金钱,效率就是生命!LDAA #$2DSTAADDJMP AGAINMOTORLDAA COG_OUT1CMPA#$51LBLT BlCMPA #$64LBGTB1BRA BOB0INC MOLDD #$0000STD STIME_C
150、OUNTSTD STIME_COUNTOLDAA #$80STAATFLGICEOLDAATFLG1ANDA #$80CMPA #$80BEQ CAMPUREOLDD STIME_COUNTOADDD #$0001STD STIME_COUNTOLDD STIME_COUNTOCPD ZHI_XIANBNE CEOBRA MANCAMPUREOLDD TC7STD FIRSTOLDAA #$80STAA TFLG1CELDAA TFLG1ANDA #$80CMPA #$80BEQ KUAILDD STIME_COUNTADDD #$0001STD STIME.COUNTLDD STIME_CO
151、UNTCPD ZHI_XIANBNE CE唯有惜时才能成功,唯有努力方可成就!时间就是金钱,效率就是生命!BRA MANMAN LDAA#$OOSTAA SUDUBRAXELVKUAI LDAA#$01STAA SUDUBRAXIELVXIELV LDAA SUDUCMPA#$01BEQ CHULILDAA GAOSUSTAA PWMDTYOLDAA #$00STAA PWMDTY1LBRAANGLEOOCHULI LDAA #$00STAA PWMDTYOLDAA #$00STAA PWMDTY1CALL STIMELDAA #$00STAA MOLBRAANGLEOOBl LDAA MOC
152、MPA #$03BETBUBRA XIANBU LDAA SUDU02STAA PWMDTYOLDAA #$00STAA PWMDTY1LDAA #$00STAA MOLBRAANGLEOOXIAN LDD #$0000STD STIME_COUNTSTD STIME_COUNTOLDAA #$80STAATFLG1CE01 LDAATFLG1唯有惜时才能成功,唯有努力方可成就!时间就是金钱,效率就是生命!ANDA #$8OCMPA#$80BEQ CAMPUREOOLDD STIME_COUNTOADDD #$0001STD STIME_COUNTOLDD STIME_COUNTOCPD WA
153、N.XIANBNECE01LBRA MANCAMPUREOOLDAA #$80STAA TFLG1CEOOLDAATFLG1ANDA #$80CMPA #$80BEQ KUAIOLDD STIME_COUNTADDD #$0001STD STIME_COUNTLDD STIME_COUNTCPD WAN_XIANBNE CEOOBRA MANOMANOLDAA #$00STAA SUDUBRA XIELVOKUAIOLDAA #$01STAASUDUBRA XIELVOXIELVOLDAA #$00STAA MOLDAA SUDU03STAA PWMDTYOLDAA #$00STAA PWMD
154、TY1LDAA #$F0STAA PORTBLDAASUDUCMPA #$01BNE ANGLEOO唯有惜时才能成功,唯有努力方可成就!时间就是金钱,效率就是生命!BEQ CHULIOBRA ANGLEOOCHULIOLDAA #$00STAA PWMDTYOLDAA #$64STAA PWMDTY1CALL STIMEOLDAASUDU01STAA PWMDTYOLDAA #$00STAA PWMDTY1LDAA#$FOSTAA PORTBLDAA #$00STAA MOBRA ANGLEOOANGLEOOSTIMEAGAIN5JMP AGAINLDX #$12FFLDY #$008FDBN
155、E Y,*DBNE X,AGAIN5RTCSTIMEOAGAIN7LDX #$12FFLDY #$008FDBNE Y,*DBNE X,AGAIN7RTCSTIME1AGAIN6LDX #$12FFLDY #$00 IFDBNE Y,*DBNE X,AGAIN6RTCTCOINTLDAA #$00STAA TIELDAA #$40STAA INTCRRTITC1INTLDAA #$00STAA TIELDX P_SAVE唯有惜时才能成功,唯有努力方可成就!时间就是金钱,效率就是生命!TC1INT_B1 LDA A #$C0STAAATD0CTL2LDAA #$80STAAATD0CTL5NOP
156、NOPNOPNOPNOPNOPNOPNOPNOPNOPNOPNOPLDAA #$C0STAAATD1CTL2LDAA #$80STAAATD1CTL5TC 1INT.B2 LDAA ATDOSTAT1ANDA #$01CMPA#$01BNE TC1INT.B2LDAA ATD0DR0LSTAAXINXTC1INT_B3 LDAA ATD1 STAT 1ANDA #$01CMPA#$01BNETC1INT_B3LDAAATD1DR0LSTAAXINXINC C_POINTLDAA C_POINTCMPA#$10BNE TC1INT_B1LDD P_SAVEADDD #$0020唯有惜时才能成功,
157、唯有努力方可成就!时间就是金钱,效率就是生命!STD P_SAVELDD TCNTADDD #$0178STD TC2LDAA #$FFSTAATFLG1LDAA #$04STAATIERTITC2INTLDAA #$00STAATIELDAA #$40STAAINTCRRTIINTVLDAA #$00STAA INTCRINC C_LINELDAA C_LINECMPA#$5BBEQ INTV_B1LDAA #$00STAAC_POINTLDD TCNTADDD #$00 IDSTD TCILDAA #$FFSTAATFLG1LDAA #$02STAATIEJMP INTV_B2INTV_B1LDAA #$0ISTAAF_FINISHINTV_B2RTIORG $EFEAFDB TC2INTORG $EFECFDB TC1INTORG $EFEEFDB TCOINTORG $EFF2唯有惜时才能成功,唯有努力方可成就!时间就是金钱,效率就是生命!FDB INTVORG SEFFEFDB Entry唯有惜时才能成功,唯有努力方可成就!