基于微控制器k60的光电智能车控制软件设计论文毕设论文.doc

西安文理学院物理与机械电子工程学院本科毕业论文（设计）题 目 基于微控制器k60的光电智能车 控制软件设计 专业班级 09自动化1班 学 号 08103090102 学生姓名 赵昆鹏 指导教师 雷俊红 设计所在单位 西安文理学院 2013 年 5 月西安文理学院本科毕业设计（论文）指导教师评分表学生姓名赵昆鹏学 号08103090102专 业自动化班 级1班毕业设计（论文）题目基于微控制器k60的光电智能车控制软件设计设计（论文）起止时间 2013年 1 月 10 日至 2013 年 5 月 24 日指导教师评语：建议成绩：指导教师签名： 年 月 日西安文理学院本科毕业设计（论文）评阅教师评分表学生姓名赵昆鹏学 号08103090102专 业自动化班 级1班毕业设计（论文）题目基于微控制器k60的光电智能车控制软件设计设计（论文）起止时间 2013 年 1 月 10 日至 2013 年 5 月 24 日评阅教师评语：建议成绩：评阅教师签名： 年 月 日西安文理学院本科毕业设计（论文）答辩记录学生姓名赵昆鹏学 号08103090102专业名称自动化答辩时间2013年5月24 日答辩地点C0311指导教师雷俊红题 目基于微控制器k60的光电智能车控制软件设计答辩小组成 员姓 名职 称姓 名职 称提问及回答情况记录： 记录人签字： 年 月 日答辩成绩：答辩小组组长签名： 年 月 日毕业论文成绩答辩委员会认定成绩毕业论文等级学院答辩委员会意见：负责人签名： 年 月 日注：1、毕业论文成绩=指导教师成绩×40%评阅教师成绩×20%答辩成绩×40%；2、答辩委员会认定成绩是根据该生毕业设计期间的表现及该专业整体论文情况的综合评定成绩。3、论文等级分优秀（90分）、良好（8089分）、中等（7079分）、及格（6069分）、不及格（<60分）。基于微控制器k60的光电智能车控制软件设计摘要：全国大学生“飞思卡尔”杯智能汽车竞赛要求在规定的汽车模型平台上，使用飞思卡尔半导体公司的微控制器作为核心控制模块，通过增加道路传感器、电机驱动模块以及编写相应控制程序，制作完成一个能够自主识别道路的模型汽车。在本次控制软件设计中，为了实现车模直立行走，将车模直立行走主要的控制算法集中起来，形成控制算法总框图。根据选用的传感器等硬件方面的条件，需要采集的信号有：陀螺仪信号；加速度计信号；电机转速脉冲信号；传感器CCD信号。控制车模电机转动需要进行的控制环节有：车模直立控制，使用车模倾角的PD（比例 微分）控制；车模速度控制，使用PI（比例 积分）控制；车模方向控制，使用P（比例）控制。通过控制软件的设计使车模达到了预期中直立行走的目的。关键词：Freescale 智能车 CCD 陀螺仪 加速度器The design of control software of photoelectric intelligent vehicle controller based on K60Abstract：The National College " Freescale " cup competition for intelligent vehicle demands that we should made complete an independent identification of road model cars in the platform of vehicle model on the regulations micro controller using Freescale Semiconductor microinstruction as the core control module, by increasing the road sensors, motor drive module and the preparation of the corresponding control procedures.In the control software design, in order to achieve the walking model, we.concentrate the main control algorithm for the car walk upright to,form the control algorithm block diagram. According to the sensor hardware conditions, we need acquire the signal that: the gyro signal; accelerometer signal; the motor speed sensor pulse signal; CCD signal. What we need the motor rotation control links to control the rotation of models are: by using the model angle erect, PD (proportional differential) control to realize the upright or model control;by using the PI (proportional integral) control to realize the car speed control; by using the P (proportional) control to realize the vehicle direction control. Through the design of control software of the model to achieve the desired purpose of walking upright. Key words：Freescale smart car CCD gyro accelerometer 目录目录第1章 绪论11.1 智能车发展历史11.2 智能车研究前景11.3 本课题主要研究问题1第2章 车模直立行走的原理32.1车模直立控制32.2 车模速度控制62.3 车模方向控制82.4 车模倾角测量9第3章 车模控制算法设计143.1控制算法143.1.1PID控制143.1.2 模糊控制153.2 总结163.2.1车模直立PD控制163.2.2 车模速度PI 控制173.2.3 车模方向P控制18第4章 软件系统设计及实现.20结束语24致谢25附录A:部分程序源代码27附录B: Single-chip Microcomputer40第1页西安文理学院本科毕业设计（论文）第1章 绪论1.1 智能车发展历史智能小车系统是迷你版的智能汽车，二者在信息提取，信息处理，控制策略及系统搭建上有很多相似之处，可以说智能小车系统将为智能汽车提供很好的试验和技术平台，推动智能汽车的发展。CCD作为很多智能汽车的信息输入的传感器具有其他很多传感器所不具有的优势，如对物体的特征提取，视野宽广等，因此基于CCD的智能小车系统的研究将推动智能汽车的发展。智能汽车是未来汽车的发展方向，将在减少交通事故、发展自动化技术、提高舒适性等许多方面发挥很重要的作用；同时智能汽车是一个集通信技术，计算机技术，自动控制，信息融合技术，传感器技术等于一身的行业，它的发展势必促进其他行业的发展，在一定程度上代表了一个国家在自动化智能方面的水平1。汽车在走过的100多年的历史中，从没停止过智能化的步伐，进入20世纪90年代以来，随着汽车市场竞争激烈程度的日益加剧和智能运输系统（ITS）地兴起，国际上对于智能汽车及其相关技术的研究成为热门，一大批有实力有远见的大公司、大学和研究机构开展了这方面的研究。很多美国、日本和欧洲等国家都十分重视并积极发展智能车系统，并进行了相关实验，取得了很多成就。我国的相关研究也已经开展，清华大学成立了国内最早的研究智能汽车和智能交通的汽车研究所，在汽车导航、主动避撞、车载微机等方面进行了广泛而深入的研究，2000年上海智能交通系统进入实质性实施阶段，国防科大研制出第四代无人驾驶汽车，西北工业大学、吉林交通大学、重庆大学等也展开了相关研究2。这一新兴学科正在吸引越来越多的研究机构和学者投入其中。1.2 智能车研究前景智能车系统有着极为广泛的应用前景。结合传感器技术和自动驾驶技术可以实现汽车的自适应巡航并把车开得又快又稳、安全可靠；汽车夜间行驶时，如果装上红外摄像头，就能实现夜晚汽车的安全辅助驾驶；他也可以工作在仓库、码头、工厂或危险、有毒、有害的工作环境里，此外他还能担当起无人值守的巡逻监视、物料的运输、消防灭火等任务。在普通家庭轿车消费中，智能车的研发也是很有价值的，比如雾天能见度差，人工驾驶经常发生碰撞，如果用上这种设备，激光雷达会自动探测前方的障碍物，电脑会控制车辆自动停下来，撞车就不会发生了。1.3 本课题主要研究问题系统是以检测图像信号为基础，通过单片机处理信号实现对车体控制，实现车体能够直立的准确沿着预设路径寻迹。本课题主要是通过对线性CCD、陀螺仪和加速度计的认识和掌握通过PID算法基于微处理器K60的运用实现车模的直立行走功能。第2章 车模直立行走的原理设计课题要求车模在直立的状态下以两个轮子着地沿着赛道进行比赛， 为了分析方便，根据比赛规则，假设维持车模直立、运行的动力都来自于车模的两个后车轮，后轮转动由两个直流电机驱动。因此从控制角度来看，由控制车模两个电机旋转方向及速度实现对车模的控制。车模运动控制任务可以分解成以下三个基本任务： （1） 控制车模直立：通过控制两个电机正反向运动保持车模直立状态； （2） 控制车模速度：通过控制两个电机转速速度实现车模行进控制； （3） 控制车模转向：通过控制两个电机之间的转动差速实现车模转向控制。 以上三个任务都是通过控制车模两个后轮驱动电机完成的。可以假设车模的电机可以虚拟地被拆解成三个不同功能的驱动电机，它们同轴相连，分别控制车模的直立平衡、前进行走、左右转向。2.1车模直立控制 车模直立是通过负反馈实现的。因为车模有两个轮子着地，因此车体只会在轮子滚动的方向上发生倾斜。控制轮子转动，抵消倾斜的趋势便可以保持车体直立了。如图2.1： 图2.1通过车轮运动控制保持车体直立 那么车轮如何运行，才能够最终保持车体垂直稳定？为了回答这个问题，一般的做法需要建立车模的运动学和动力学数学模型，通过设计最优控制来保证车模的稳定。下面通过对比单摆模型来说明保持车模稳定的控制规律。 重力场中使用细线悬挂着重物经过简化便形成理想化的单摆模型。直立着的车模可以看成放置在可以左右移动平台上的倒立着的单摆。如图2.2所示。 图2.2 车模可以简化成倒立的单摆普通的单摆受力分析如图2.3所示。图2. 3 普通的单摆受力分析当物体离开垂直的平衡位置之后，便会受到重力与悬线的作用合力，驱动重物回复平衡位置。这个力称之为回复力，其大小为F=-mgsin -mg在此回复力作用下，单摆便进行周期运动。在空气中运动的单摆，由于受到空气的阻尼力，单摆最终会停止在垂直平衡位置。空气的阻尼力与单摆运行速度成正比，方向相反。阻尼力越大，单摆越会尽快在垂直位置稳定下来。图2.7显示出不同阻尼系数下，单摆的运动曲线。如图2.4所示图2.4 单摆在不同阻尼下的运动情况总结单摆能够稳定在垂直位置的条件有两个： （1） 受到与位移（角度）相反的恢复力； （2） 受到与运动速度相反的阻尼力。 如果没有阻尼力，单摆会在垂直位置左右摆动。阻尼力会使得单摆最终停止在垂直位置。阻尼力过小（欠阻尼）会使得单摆产生震荡，阻尼力过大（过阻尼）会使得单摆到达平衡位置时间拉长。存在一个阻尼临界阻尼系数，使得单摆最快稳定在平衡位置。 为什么倒立摆在垂直位置时，在受到外部扰动的情况下，无法保持稳定呢？分析倒立摆的受力，如图2.5所示。图2.5 在车轮上参照系中车体受力分析 倒立摆之所以不能象单摆一样可以稳定在垂直位置，就是因为在它偏离平衡位置的时候，所受到的回复力与位移方向相同，而不是相反！因此，倒立摆便会加速偏离垂直位置，直到倒下。 如何通过控制使得倒立摆能够像单摆一样，稳定在垂直位置呢？要达到这一目的，只有两个办法：一个是改变重力的方向；另一个是增加额外的受力，使得恢复力与位移方向相反才行。由此，能够做的显然只有第二种方式。 10 控制倒立摆底部车轮，使得它作加速运动。这样站在小车上（非惯性系）看倒立摆，它就会受到额外的力( 惯性力)，该力与车轮的加速度方向相反，大小成正比。这样倒立摆所受到的回复力为 (2-1)式中，假设控制车轮加速度与偏角 成正比，比例为 。显然，如果>g（ g 是重力加速度）那么回复力的方向便于位移方向相反了。 此外，为了使得倒立摆能够尽快地在垂直位置稳定下来，还需要增加阻尼力，与偏角的速度成正比，方向相反。因此式（2-1）可变为 (2-2)按照上面的控制方法，可把倒立摆模型变为单摆模型，能够稳定在垂直位置。因此，可得控制车轮加速度的控制算法 (2-3)式中， 为车模倾角； 为角速度； 、 均为比例系数；两项相加后作为车轮加速度的控制量。只要保证在 >g >0、 条件下，可以维持车模直立状态。其中， 决定了车模是否能够稳定到垂直位置，它必须大于重力加速度； 决定了车模回到垂直位置的阻尼系数，选取合适的阻尼系数可以保证车模尽快稳定在垂直位置。 因此控制车模稳定，需要下列两个条件：（1）能够精确测量车模倾角 的大小和角速度 的大小； （2）可以控制车轮的加速度2.2 车模速度控制车模运行速度是通过控制车轮速度实现的，车轮通过车模两个后轮电机经由减速齿轮箱驱动，因此通过控制电机转速可以实现对车轮的运动控制。 电机的运动控制有三个作用： （1）通过电机加速度控制实现车模直立稳定。其中控制规律由上一节给出； （2）通过电机速度控制，实现车模恒速运行和静止。比赛规则中要求车模速度恒定，要求车模在比赛之前和冲过终点之后保持静止状态。通过速度控制，就可以提高车模稳定性，从而达到静止的要求。比赛中规则要求车模静止，或者需要通过路桥等障碍，速度控制将会发挥作用。 （3）通过电机差速控制，可以实现车模方向控制。差速的控制方法参见下一小节“车模方向控制”。电机运动控制是通过改变施加在其上的驱动电压实现的。对于电机的电磁模型、动力学模型以及车模的动力学模型进行分析和简化，可以将电机运动模型简化成如下的一阶惯性环节模型。施加在电机上一个阶跃电压 Eu (t)，电机的速度变化曲线为 (2-4)式中，E 为电压；u (t)为单位阶跃函数；T1 为惯性环节时间常数；km 为电机转速常数。 对应不同的电压，电机的速度变化曲线如图2.6所示。图2. 6 电机在不同电压下的速度由图2.6 可以看出，电机运动明显分为两个阶段：第一个阶段是加速阶段；第二个阶段为恒速阶段。其中，在加速阶段，电机带动车模后轮进行加速运动，加速度近似和施加在电机上的电压成正比，加速阶段的时间长度取决于时间常数 ，该常数由电机转动惯量、减速齿轮箱、车模的转动惯量决定；在恒速阶段，电机带动车模后轮进行恒速运行，运行速度与施加在电机上的电压成正比。 调整车模直立时间常数很小，此时电机基本上运行在加速阶段。由上一节式（2-3）计算所得到的加速度控制量a 再乘以一个比例系数，即为施加在电机上的控制电压，这样便可以控制车模保持直立状态。 车模运行速度调整时间相对很长，此时，电机速度与施加在其上的电压成正比。通过传统的PID 反馈控制，便可以精确控制电机的运行速度，从而控制车模的运行速度。 电机速度控制需要测量电机的转速，电机旋转速度可以通过安装在电机输出轴上的光电编码盘方便获得。如图2.7 所示 图2. 7 电机速度检测对于电机速度PID 控制方法如图2.8 所示。图2.8 电机 PI 反馈控制电机速度控制采用了PI 调节器，具体实现可以通过单片机软件编程实现。2.3 车模方向控制 实现车模方向控制是保证车模沿着竞赛道路比赛的关键。通过道路中心反射光强偏差检测与电机差动控制实现方向控制。（1） 道路反射光强度的偏差检测道路中心反射光强度偏差可以通过线性CCD来反馈实现。CCD是一种半导体器件，能够把光影信号转化为数字信号。（2） 电机差动控制 利用光强偏差检测信号分别与车模速度控制信号进行加和减，形成左右轮差动控制电压，使得车模左右轮运行角速度不一致进而控制车模方向。如图2.9 所示。图2. 9 通过差动控制左右电机驱动电压控制车模方向通过左右电机速度差驱动车模转向消除车模偏差，这个过程是一个积分过程。因此车模差动控制一般只需要进行简单的比例控制就可以完成车模方向控制。2.4 车模倾角测量在上节车模直立控制中介绍了控制车模直立的算法，通过测量车模的倾角和倾角加速度控制车模车轮的加速度来消除车模的倾角。因此车模倾角以及倾角加速度的测量成为控制车模直立的关键。测量车模倾角和倾角加速度可以通过加速度传感器和陀螺仪实现。加速度传感器可以测量由地球引力作用或者物体运动所产生的加速度。竞赛规则规定如果车模使用加速度传感器必须使用飞思卡尔公司产生的加速度传感器。该系列的传感器采用了半导体表面微机械加工和集成电路技术，传感器体积小，重量轻。它的基本原理如图2.10 所示。图2.10 加速度传感器通过微机械加工技术在硅片上加工形成了一个机械悬臂。它与相邻的电极形成了两个电容。由于加速度使得机械悬臂与两个电极之间的距离发生变化，从而改变了两个电容的参数。通过集成的开关电容放大电路量测电容参数的变化，形成了与加速度成正比的电压输出。MMA7260是一款三轴低g 半导体加速度计，可以同时输出三个方向上的加速度模拟信号，如图2.11 所示。图2.11 三轴加速度传感器通过设置可以使得MMA7260最大输出灵敏度为800mV/g 。 只需要测量其中一个方向上的加速度值，就可以计算出车模倾角，比如使用Z 轴方向上的加速度信号。车模直立时，固定加速度器在 Z 轴水平方向，此时输出信号为零偏电压信号。当车模发生倾斜时，重力加速度 g 便会在Z 轴方向形成加速度分量，从而引起该轴输出电压变化。变化的规律为q D = 式中，g 为重力加速度；q 为车模倾角；k 为比例系数。当倾角q 比较小的时候，输出电压的变化可以近似与倾角成正比。似乎只需要加速度就可以获得车模的倾角，再对此信号进行微分便可以获得倾角加速度。但在实际车模运行过程中，由于车模本身的运动所产生的加速度会产生很大的干扰信号叠加在上述测量信号上，使得输出信号无法准确反映车模的倾角，如图2.12 所示。图2.12 车模运动引起加速度计信号波动车模运动产生的振动加速度使得输出电压在实际倾角电压附近波动，可以通过数据平滑滤波将其滤除。但是平滑滤波也会使得信号无法实时反映车模倾角的变化，从而减缓对于车模车轮控制，使得车模无法保持平衡。因此对于车模直立控制所需要的倾角信息需要通过另外一种器件获得，那就是角速度传感器- 陀螺仪，如图2.13 所示。（2）角速度传感器-陀螺仪 陀螺仪可以用来测量物体的旋转角速度。竞赛允许选用村田公司出品的EN-03 系列的加速度传感器。它利用了旋转坐标系中的物体会受到克里利奥力的原理，在器件中利用压电陶瓷做成振动单元。当旋转器件时会改变振动频率从而反映出物体旋转的角速度。图2.13 角速度传感器在车模上安装陀螺仪，可以测量车模倾斜的角速度，将角速度信号进行积分便可以得到车模的倾角。如图2.14 所示。图2.14 测量车模角速度和角度由于陀螺仪输出的是车模的角速度，不会受到车体振动影响。因此该信号中噪声很小。车模的角度又是通过对角速度积分而得，这可进一步平滑信号，从而使得角度信号更加稳定。因此车模控制所需要的角度和角速度可以使用陀螺仪所得到的信号。 由于从陀螺仪的角速度获得角度信息，需要经过积分运算。如果角速度信号存在微小的偏差，经过积分运算之后，变化形成积累误差。这个误差会随着时间延长逐步增加，最终导致电路饱和，无法形成正确的角度信号，如图2.15 所示。图2.15 角度积分漂移如何消除这个累积误差呢？ 可以通过上面的加速度传感器获得的角度信息对此进行校正，如图2.16 所示。图2.16 角度积分漂移校正利用加速度计所获得的角度信息与陀螺仪积分后的角度进行比较，将比较的误差信号经过比例放大之后与陀螺仪输出的角速度信号叠加之后再进行积分。从图2.16中的框图可以看出，对于加速度计给定的角度，经过比例、积分环节之后产生的角度必然最终等于。由于加速度计获得的角度信息不会存在积累误差，所以最终将输出角度中的积累误差消除了。 加速度计所产生的角度信息中会叠加很强的有车模运动加速度噪声信号。为了避免该信号对于角度的影响，因此比例系数应该非常小。这样，加速度的噪声信号经过比例、积分后，在输出角度信息中就会非常小了。由于存在积分环节，所以无论比例多么小，最终输出角度必然与加速度计测量的角度相等，只是这个调节过程会随着的减小而延长。 为了避免输出角度跟着过长，可以采取以下两个方面的措施：（1）仔细调整陀螺仪的放大电路，使得它的零点偏置尽量接近于设定值，并且稳定。 （2）在控制电路和程序运行的开始，尽量保持车模处于直立状态，这样一开始就使得输出角度与相等。此后，加速度计的输出只是消除积分的偏移，输出角度不会出现很大的偏差。第3章 车模控制算法设计3.1控制算法智能车的控制包主要是电机控制。具体的控制算法有PID控制和模糊控制等控制算法。3.1.1PID控制 PID 控制是工业过程控制中历史最悠久，生命力最强的控制方式。这主要是因为这种控制方式具有直观、实现简单和鲁棒性能好等一系列的优点。PID控制主要有三部分组成，比例、积分、微分。 比例控制是一种最简单的控制方式。其控制器的输出与输入误差信号成比例关系。偏差一旦产生，调节器立即产生控制作用使被控量朝着减小偏差的方向变化，控制作用的强弱取决于KP。当仅有比例控制时系统输出存在稳态误差（Steady-state error）。为了消除稳态误差，引入积分控制。积分项对误差取决于时间的积分，随着时间的增加，积分项会增大。这样，即便误差很小，积分项也会随着时间的增加而加大，它推动控制器的输出增大使稳态误差进一步减小，直到等于零。为了预测预测误差变化的趋势，引入微分的控制器，这样就能够提前使抑制误差的控制作用等于零，甚至为负值，从而避免了被控量的严重超调。PID控制框图如下图所示对应的误差传递函数为： U（s）/E(s)=Kp(1+1/Ti+Td)式中，Kp为比例增益；Ti为积分时间常数；Td为微分时间常数；U(s)为控制量；E(s) 为被控量与设定值R(s)的偏差。时域表达式为 （式一）在单片机中，我们仅能对数字信号处理，即数字PID控制。将上式离散化，得错误！未找到引用源。（式二）A.位置式PID算法直接利用上述离散化公式计算，框图如右图所示。由于积分项（Pi）是将所有采集值偏差相加，在一段时间后会很浪费单片机资源。对其稍加改进，得到增量型PID算法。B.增量式PID算法根据式二得第k-1个采样周期的控制量为错误！未找到引用源。 （式三）式二减式三得 错误！未找到引用源。 （式四) 由此，第k个采样时刻实际控制量为错误！未找到引用源。,为方便书写，写为错误！未找到引用源。 （式五）其中，错误！未找到引用源。由上可知，利用三个历史数据，递推使用，即可完成PID控制量。框图如右图所示:3.1.2 模糊控制一般控制系统包含了五个主要部分，即:定义变量、模糊化、知识库、逻辑判断及反模糊化，底下将就每一部分做简单的说明：(1) 定义变量：也就是决定程序被观察的状况及考虑控制的动作，例如在一般控制问题上，输入变量有输出误差E与输出误差之变化率CE，而控制变量则为下一个状态之输入U。其中E、CE、U统称为模糊变量。(2) 模糊化（fuzzify）：将输入值以适当的比例转换到论域的数值，利用口语化变量来描述测量物理量的过程，依适合的语言值（linguisitc value）求该值相对之隶属度，此口语化变量我们称之为模糊子集合（fuzzy subsets）。(3) 知识库：包括数据库（data base）与规则库（rule base）两部分，其中数据库是提供处理模糊数据之相关定义；而规则库则藉由一群语言控制规则描述控制目标和策略。(4) 逻辑判断：模仿人类下判断时的模糊概念，运用模糊逻辑和模糊推论法进行推论，而得到模糊控制讯号。此部分是模糊控制器的精髓所在。(5) 解模糊化（defuzzify）：将推论所得到的模糊值转换为明确的控制讯号，做为系统的输入值。模糊算法可以解决一些非线性问题，将赛道分为直线、入大小弯、出大小弯、蛇形弯道，对应的直线加速、入大弯减速转方向、入小弯制动转方向、出弯加速、蛇形弯道直接通过（若可以达到这种前瞻性）。要达到这种控制要通过实际检测，分析大量赛道磁场信息，找出它们的特征。3.2 总结虽然模糊控制可以较好解决一些非线性问题，但控制复杂，实际调试中较PID控制无明显优势，所以我们采用PID控制。电机控制为增量式PID，由于给定速度频繁变化，采用微分先行PID，使电机能够快速响应。3.2.1车模直立PD控制 车模直立控制是关键子程序。其中涉及到两个关键控制系数： :CAR_AA_P_INT ：倾角比例，（0.75*0x7fff ） :CAR_AA_D_INT ：角速度比例，（0.125*0x7fff）上面的乘以0x7fff 是为了将小数化成定点小数表示形式。 这两个系数是通过实验调试逐步确定的。具体确定的方式如下： 首先将CAR_AA_D_INT 置为0 。逐步加大CAR_AA_P_INT数值，直到车模开始前后震荡。此时再逐步增大CAR_AA_D_INT，消除震荡。然后逐步增加这两个数值，直到车模开始抖动，然后略减少这两个数值即可。 这个过程需要反复调整，直到车模可以比较稳定的达到直立平衡。由于现在还没有加入速度闭环，所以由于加速度传感器零偏的误差，会导致有时车模在直立的时候会往一个方向加速行驶。 如下是车模直立控制子程序代码： void CarAngleAdjust(void) int nLeft, nRight; int nSpeed; int nP, nD; nP = g_nCarAngle; nP = (int)mult_r(nP, CAR_AA_P_INT); nD = g_nCarGyroVal >> 5; nD = (int)mult_r(nD, CAR_AA_D_INT); nSpeed = nD + nP; if(nSpeed > MOTOR_SPEED_SET_MAX) nSpeed = MOTOR_SPEED_SET_MAX; else if(nSpeed < MOTOR_SPEED_SET_MIN) nSpeed = MOTOR_SPEED_SET_MIN; nLeft = nSpeed + g_nLeftMotorOutSpeed - g_nMotorLeftRightDiff; nRight = nSpeed + g_nRightMotorOutSpeed + g_nMotorLeftRightDiff; g_nLeftMotorOut = nLeft << 6; g_nRightMotorOut = nRight << 6; if(g_nLeftMotorOut > MOTOR_OUT_MAX) g_nLeftMotorOut = MOTOR_OUT_MAX; if(g_nLeftMotorOut < MOTOR_OUT_MIN) g_nLeftMotorOut = MOTOR_OUT_MIN; if(g_nRightMotorOut > MOTOR_OUT_MAX) g_nRightMotorOut = MOTOR_OUT_MAX; if(g_nRightMotorOut < MOTOR_OUT_MIN) g_nRightMotorOut = MOTOR_OUT_MIN; MotorSpeedOut();3.2.2 车模速度PI 控制车模速度控制采用了典型的PI 控制算法。其中速度采用了左右电机速度的平均值进行计算。对于PI 调节算法中的参数整定可以参照一般PI 参数整定的方法进行。 该函数的输入为g_nLeftMotorSpeedCount ，g_nRightMotorSpeedCount。 计算得到g_nLeftMotorSpeedOut ，g_nRightMotorSpeedOut。 由于速度控制子程序是每100 毫秒才调用一次。所以程序并没有直接更新g_nLeftMotorSpeedOut ，g_nRightMotorSpeedOut 的数值，而是通过调用函数CalculateMotorSpeedOut() 更新上面的数值。函数 CalculateMotorSpeedOut() 是每 5 毫秒被调用一次，所以程序是将速度控制的变化量平均到20次进行更新。这样可以降低速度控制对于车模直立控制的影响。如下是车模速度控制子程序：void MotorSpeedAdjustCal(void) int nLeftSpeed, nRightSpeed; int nDeltaValue, nP, nI; int nSpeed; nLeftSpeed = (int)g_nLeftMotorSpeedCount; nRightSpeed = (int)g_nRightMotorSpeedCount; nSpeed = (nLeftSpeed + nRightSpeed) / 2; nDeltaValue = g_nMotorSpeedSet - nSpeed; nP = mult_r(nDeltaValue, MOTOR_SPEED_P_INT); nI = mult_r(nDeltaValue, MOTOR_SPEED_I_INT); g_nMotorOutSpeedOld = g_nMotorOutSpeedNew; g_nMotorOutSpeedKeep -= nI; g_nMotorOutSpeedNew = (g_nMotorOutSpeedKeep >> 3) - nP; if(g_nMotorOutSpeedKeep > MOTOR_OUT_MAX) g_nMotorOutSpeedKeep = MOTOR_OUT_MAX; if(g_nMotorOutSpeedKeep < MOTOR_OUT_MIN) g_nMotorOutSpeedKeep = MOTOR_OUT_MIN; void CalculateMotorOutSpeed(void) int nValue; nValue = g_nMotorOutSpeedNew - g_nMotorOutSpeedOld; nValue = nValue * (g_nCarMotionCount + 1) / (CAR_MOTION_PERIOD - 1) + g_nMotor