哈尔滨工业大学紫丁香五队技术报告

1、 第十三届“恩智浦”杯全国大学生智能汽车竞赛技术报告学校：哈尔滨工业大学队伍名称：紫丁香五队参赛队员：李琳昊 田竺轩 王博鑫带队教师：霍矩 李岩松第十三届全国大学生智能汽车技术报告关于技术报告和研究论文使用授权的说明本人完全了解第 13 届“恩智浦”杯全国大学生智能汽车竞赛关保留、使用技术报告和研究论文的规定，即：参赛作品著作权归参赛者本人，比赛组委会和恩智浦半导体公司可以在相关主页上收录并公开参赛作品的设计方案、技术报参赛队员签名： 带队教师签名： 日期： 摘要本文详细介绍了我们为了十三届全国大学生智能车大赛信标对抗组准备的车模。车模采用恩智浦公司的 MK66FX1M0VLQ18 芯片作为控

2、制器；通过 NXP官方 OPENMV 摄像头采集信标和障碍信息；利用编码器测量车模速度，构成速度闭环控制；利用左右的差速，实现车模转向；利用 PID 控制算法控制电机和舵机；通过减轻车模质量，来获得更好的加速和转弯性能；利用蓝牙、上位机、OLED 屏幕等对程序进行调试，获得更好的运行效果。关键词：智能车单片机PIDMK66 麦克纳姆轮I第十三届全国大学生智能汽车技术报告目录摘要I关键词I第一章 引言1第二章 车模整体设计22.1 系统概述22.2 整车布局及参数2第三章 系统机械系统设计及实现33.1 轮胎的处理33.2 传感器的选择与安装33.2.1 摄像头的安装33.2.2 编码器的安装4

3、3.2.3 MPU6050 陀螺仪和加速度计43.3 电池和电路板的安装5第四章 系统硬件设计及实现64.1 主控板64.1.1 主控芯片64.1.2 电源管理模块74.1.4 调试模块84.1.5 图像采集模块84.2 驱动板94.2.1 转 12V 电路94.2.3 全桥驱动94.2.4 测速模块104.2.5 交互模块11第五章 系统软件设计及算法实现12II5.1 控制流程图135.2 摄像头图像采集135.3 信标数据处理145.4 避障策略155.4.1 对熄灭的信标灯的处理155.5 双单片机通信165.6 撞击策略165.7 防翻车策略165.8 控制算法175.8.1 PID

4、 算法介绍175.8.2 方向控制175.8.3 PI 速度控制18第六章 系统开发和调试工具196.1 开发环境：IAR EWARM196.2 上位机调试206.3 图像处理软件20第七章 总结22参考文献23附录 A 核心控制程序代码I附录 B 图像处理程序代码XXXIIII第一章 引言智能车是以汽车电子为背景，涵盖控制、模式识别、传感技术、电子、电气、计算机、机械等多学科的科技创意性设计，由主要路径识别、速度采集、角度控制及车速控制等模块组成。其设计与开发涉及控制、模式识别、传感技术、汽车电子、电气、计算机、机械等多个学科，可以分为三大部分：传感器检测部分，执行部分，CPU。全国大学生“

5、恩智浦”杯智能汽车竞赛以“立足培养，重在参与，鼓励探索， 追求卓越”为指导思想，旨在促进高等学校素质教育，培养大学生的综合知识运用能力、基本工程实践能力和创新意识，激发大学生从事科学研究与探索的兴趣和潜能,倡导理论联系实际、求真务实的学风和团队协作的人文精神，为优秀人才的脱颖而出创造条件，是一项鼓励创新的科技竞赛活动。信标对抗组要求在规定的汽车模型平台上，使用恩智浦半导体公司的微控制器作为核心控制模块，通过增加道路传感器、电机驱动模块以及编写相应控制程序，制作完成一个能够自主识别信标的模型汽车。针对信标组的任务要求，我们采用具有控速特性和转弯性能较好的 H 车模，使用摄像头采集赛道信息，控制芯

6、片选择性能更好的 MK66 单片机。本篇技术报告主要从机械、硬件、软件、调试工具等方面，详细的介绍了车模的设计和制作方案，以及我们做车过程积累的经验，希望对以后的参赛选手能够带来启发和帮助。1第二章 车模整体设计2.1 系统概述车模主要分三个部分：控制器、传感器和执行器。控制器采用 MK66 单片机，负责处理由各传感器采集到的赛道信息，并且根据赛道信息输出控制信号； 传感器包括摄像头、光电管、编码器等，负责采集赛道信息和车模运行状态信息； 执行器有电机及相应的驱动电路，接收单片机控制信号，对车模运行状态进行控制。车模运行的整体流程为：摄像头和光电管检测赛场信标位置信息，并传给单片机；编码器采集

7、电机转速并传给单片机，形成速度闭环控制，电机采用增量式PID 控制，用 PWM 波控制电机输出功率；差速采用 PD 控制；单片机通过摄像头和光电管的信息，计算出电机的期望速度值，通过速度环控制车模速度。2.2 整车布局及参数我们组采用 H 型车模，尺寸为 250*200*69mm，轮胎尺寸为 61*24mm,极。驱动电机为 RS-380，7.2vRS-380 马达，工作电压范围是 5.4V-9V,空载转速 15000r左右，空载电流 0.55A,最大力矩 91.32g.cm，最大输出功率 12.6W，车身总体重量 1.6kg。车模三视图如图 2.1 所示：正视图左视图2第三章 系统机械系统设计

8、及实现图 2.1 车模三视图 第三章 系统机械系统设计及实现我们选用的车模是 H 车模。H 车模使用 4 个 380 电机，由于 H 车没有舵机， 所以过弯采用电机主动差速的方式。同时，为了获得更好的加减速效果，我们尽量减轻车模质量、尽量将重心降低，在电机输出力矩一定时，轻便的车模将获得更高的加速度，这是我们机械设计的基本原则。3.1 轮胎的处理在调试过程中，我们发现轮胎磨损之后摩擦力会大大下降，所以我们采用频繁更换小上的胶皮的方法来缓解这一问题，同时在安装小的时候应当注意固定的松紧程度应适中，太紧了会影响车模的横移以及转弯的灵活程度，太松的话螺母容易脱落，造成严重的后果。3.2 传感器的选择

9、与安装3.2.1 摄像头的安装摄像头我们选择用基于 NXP i.MX.RT 的可编程图像识别模块，相比于灰度摄像头可以通过颜色识别来寻找信标灯。我们一共安装了两个 OPENMV 便于实现双向控制。并且我们在镜头上加上了偏振片用于滤除反光。关于摄像头高度，规则规定，车模最高点不能超过 20 厘米，摄像头太低3第十三届全国大学生智能汽车技术报告会出现障碍灯挡住信标灯的情况，所以我们决定将摄像头安装在尽可能高的地方，我们将摄像头安装在距离地面 20 厘米的位置。3.2.2 编码器的安装如果采用普通低转速编码器，在小车高速运转时很容易出现丢步的情况， 进而有可能导致跑飞，我们选用龙邱生产的 512 线

10、 mini 增量式编码器，该编码器体积小、质量轻、稳定性好、精度高，输出型号类型为 CMOS 信号，抗干扰能力强。安装时要仔细调节编码器齿轮与电机齿轮的啮合，保证阻尼适中，啮合太紧会增加电机负载，太松又会影响编码器测量精度或者打齿。实际安装如图 3.1图 3.1 编码器的安装 所示。3.2.3 MPU6050 陀螺仪和加速度计4第三章 系统机械系统设计及实现为了检测车模倾角，防止侧翻和仰翻，我们用了陀螺仪和加速度计。为了使测量尽量准确，我们选择将陀螺仪放在车体中间，主控板的下面。陀螺仪安装如图 3.2 所示。图 3.2 陀螺仪的安装 3.3 电池和电路板的安装我们车上有 4 块电路板，分别是主

11、控板和两块驱动板和一块电池板，将两块 18650 锂电池固定在电池板上，为了使车体尽量前后对称我们选择将电池放在车体中间位置，将主控板放在电池板上；驱动板放在电机上方；电池和电路板图 3.9.1 电池的安装 安装如图 3.9。5第十三届全国大学生智能汽车技术报告第四章 系统硬件设计及实现本车硬件设计主要分为以下几个模块：电源管理模块，图像采集模块， 速度采集模块，单片机处理核心模块，电机驱动模块，交互模块。稳定性是硬件的基本要求，我们选取线性电源芯片给 MCU 供电，减少主控的附件，按照车模外形设计电路板的外形，将驱动电路与 MCU 分开，分成了主控和驱动部分电路。4.1 主控板图 4.1.1

12、 主控板 主控板对称布置，将单片机直接布置在主控板上减少了多余的连接件，每个电源网络均设置 LED 显示各个供电芯片是否正常工作，减少故障排查时间，根据各个传感器的车布位置合理安排接口位置，为减轻质量同时使电路板简洁，电路板多采用 FPC 排线和防反接接口，主控板实物如图 4.1.1。4.1.1 主控芯片6第四章 系统硬件设计及实现本车模采用 NXP 公司的 MK66 芯片作为主控芯片，该芯片采用封装，方便焊接，144 个引脚，16 位 ADC 两路，5 路 UART,4 路 IIC，180M 主频。单片机及外围电路。图 4.1.2 单片机及外围电路原理图4.1.2 电源管理模块电源模块方面采

13、用线性转压芯片，线性转压相比于开关转压更加稳定，更加适用于用多级转压电路中，但是缺点是效率较低，容易发热，最后我们采用TPS76850 和 TPS76833 作为转压芯片为单片机以及 OPENMV 供电，效果较为理想。转压电路原理图如图 4.1.3 所示。图 4.1.3 转压电路7第十三届全国大学生智能汽车技术报告4.1.4 调试模块在调试时必要的会需要对行进时实时数据进行监测，运用蓝牙可以很方便的实现，OLED 对菜单显示可以方便调节参数。蓝牙和 OLED 接口原理图如图4.1.4。图 4.1.4 蓝牙及 oled4.1.5 图像采集模块图像处理方面我们用了 OPENMV，进行识别场地信息，

14、摄像头与单片机通过串口连接，摄像头接口电路图如下所示。图 4.1.6 光电管接口图 4.1.5 摄像头接口由于摄像头的角度有限，无法对出现在车体左右的信标灯做出及时有效的8第四章 系统硬件设计及实现判断，所以采用光电管辅助识别红外信号，将光电管至于自己绘制的 PCB 上并将 PCB 架在碳素杆上。4.2 驱动板图 4.2 驱动板4.2.1 转 12V 电路转 12V 采用 MIC34063转压芯片，在实际效果比较理想。4.2.3 全桥驱动图 4.2.3 全桥驱动9第十三届全国大学生智能汽车技术报告驱动为分立元件制作的直流电机可逆双极型桥式驱动器，其功率元件由 N沟道 MOSFET 管组成，峰值

15、电流可以达到 100A 以上，大大提高了电动机的工作转矩和转速，电路图如 4.9。图 4.9 全桥驱动4.2.4 测速模块测速模块为了减少车辆负载，采用体积较小的可以兼容正交解码的 512 线增量式龙邱 MINI 编码器。图 4.2.4 编码器10第四章 系统硬件设计及实现4.2.5 交互模块为了方便调节参数，在驱动板上有拨码开关和五向按键。图 4.11 五向按键及拨码开关11第五章 系统软件设计及算法实现高效稳定的控制程序和高适应性的算法是智能车快速平稳运行的基础。我们设计的智能车采用恩智浦公司生产的 MK66FX1M0VLQ18 芯片作为控制器， 通过 OPENMV 采集图像信息，为使小车

16、快速平稳运行，我们使用了编码器测速对小车进行速度闭环。速度和方向控制算法方面，我们采用工业控制广泛采用的经典 PID 算法，配合理论计算与实际参数调试过程使程序和小车充分契合，使智能车达到快速平稳运行的目的。12第五章系统软件设计及算法实现5.1 控制流程图初始化发车状态 撞灯状态 追灯状态 避障状态 无灯状态 是否切换摄像头图 5.1 程序状态转换图5.2 摄像头图像采集我们采用 NXP 官方OPENMV 进行图像处理，完成信标和障碍的采集工作。OPENMV 采用 NXP1062 芯片，处理速度更快，且为彩色摄像头，因而可以适应一定的场地光线干扰。但是 OPENMV 也有一些致命弱点，也是我

17、们一直未能妥善解13第十三届全国大学生智能汽车技术报告决的问题。 我们采用 120160 的图像大小，这种大小分辨率相对较高，处理速度也在可以接受的范围内。 5.3 信标数据处理信标灯发出红色光为 10HZ，识别信标便采用 LAB 色彩空间图像，选择合适的阈值，即可在图像中识别出较好的图像坐标，求出信标灯的重心坐标，即作为追灯的目标位置。又因为红色光为 10HZ 闪烁，而 OPENMV 帧率也不是特别高，大概 33 帧左右，所以需要一定时间的延时，来判断信标灯是否被灭掉，具体延时多久需要根据现场帧率判断。为适应双摄方案，需要记录信标灯从无到有的过程，这样能够做到快速找到信标灯，这样就分两种情况

18、：一种是单个摄像头看到新的信标灯，此时通过判断信标灯坐标变化可以锁定新的信标灯；另一种是后摄出现信标灯，直接判断信标从无到有这个“上升沿”即可实现摄像头切换。当视野因为对方车辆干扰出现多个连通域时，采用帧差分算法，对比多张图像，尽可能锁定真实信标灯，但这样会极大降低有效帧率，从而失去宝贵的处理时间，同时，对于闪烁的抓取并不能做到百分百的精准，有可能会出现漏灯的情况，故对于干扰元素的处理并没有一个完美的解决办法。如果对方车辆出现闪烁的红色 led，则这种方失去作用，但对于这种对手，我们车模的保护措施也不至于让我们的车被撞坏，所以也无需多言。车模中间安装了光电管，即红外接收头，用于接收侧向的信标灯

19、信号，在灭灯状态后，开启红外接收头接收中断，对比两侧数据变化情况，即可完成对侧向信号的检测，决策绕灯的方向。14第五章 系统软件设计及算法实现图 5.3.1 Openmv 采集的信标灯图像图 5.3.2 处理后的图像5.4 避障策略针对场地环境，需要处理的障碍主要为熄灭的信标灯，以下简称障碍灯。利用 H 车独特的横移性能，避障采用的策略主要为遇到障碍减速，横移进入无障碍区域，继续完成追灯，因为 H 车本身惯性较大，前进速度往往无法瞬间消失，所以利用前进速度的惯性以及横向速度，车体将以切向运行轨迹完成避障；如果车模本身速度较低，则车模将优先完成避障动作。考虑到摄像头视野较大，较远的障碍，比较偏的

20、障碍都不用避障，因此需要提前划定避障区域，障碍物重心坐标落到避障区域进入避障状态，不在则无需考虑避障，因为 H 车不如 C 车响应那么快，所以很难做到路径规划，反而对最近一个障碍避障效率较高。具体障碍物的距离可以根据坐标，面积等进行判断，但这些和现场调试有关，所以也不能对这些信息过于依赖。5.4.1 对熄灭的信标灯的处理障碍灯为白色，在场地环境较好的情况下，识别视野中的白色连通域，可以识别出长方形的障碍灯连通域。但是场地内的黄色胶带，顶灯反光等因素会极大影响障碍物的识别，因为需要利用障碍物的联通域特征进行筛选，我们主要采用图形周长面积比例，长宽比例来进行筛选，基本能处理掉形状怪异的连通域，但对

21、于有些形状与障碍灯相似的反光实在是难以解决，不过考虑到这种情况较少，15第十三届全国大学生智能汽车技术报告且角度变化对这种反光影响较大，所以即使触发避障也很短暂，对车模运行影响较小。为了防止避障误判，将亮着的信标灯作为障碍躲避，导致无法灭灯的情况， 我们采用以下两种限制：首先，因为是用同一个摄像头同时完成信标和障碍的识别，所以如果识别信标灯为障碍物，即可通过坐标对比消除这个“障碍”连通域； 其次，规则限制两个信标灯的距离在一米以上，可以根据红外摄像头的信标信息大致判断车模距离信标灯的距离，在一米以内关闭避障功能。经过实验，通过上面两种限制，可以很好地区分亮着的信标灯和障碍。图 2.4.2 处理

22、后的障碍图像图 5.4.1 openmv 采集的障碍图像5.5 双单片机通信为了满足对四个电机的控制，利用两块单片机完成控制，一主一从同时采集编码器读数，利用单片机 UART 通信实现数据的实时共享，经过实际测试，UART通信效率较高，可以通过自己的算法完成数据校验，从而保证数据的准确性，能够支持车模的控制。5.6 撞击策略车模实际运行中难免遇到撞灯，撞挡板等情况，因此需要针对这些情况做特殊处理，同时利用两个车头的优势以及麦克纳姆轮独特的性能，主要分为两种情况，一种是有灯撞击和无灯撞击，在有灯状态下，直接利用横移速度即可避障，在无灯状态下直接切换另外一个摄像头完成躲避动作。5.7 防翻车策略1

23、6第五章 系统软件设计及算法实现翻车情况分为两种，一种是左右方向的侧翻，另一种是前后方向的仰翻：仰翻情况较少，车头做成弧形基本可以让车模由仰翻状态进入侧翻状态。侧翻时，利用车模两边的防翻车保险杠可以完成车模的回归正常状态，原理较简单，在此不再赘述。5.8 控制算法5.8.1 PID 算法介绍在过程控制中，PID 控制器是一直应用最为广泛的一种控制器，也是众多控制算法中应用最为普遍的控制算法当被控对象的结构和参数不能完全掌握，或得不到精确的数学模型时，控制理论的其它技术难以采用时，系统控制器的结构和参数必须依靠经验和现场调试来确 定，这时应用 PID 控制技术最为方便。即当我们不完全了解一个系统

24、和被控对象，或不能通过有效的测量手段来获得系统参数时，最适合用 PID 控制技术。 PID 控制，实际中也有 PI 和 PD 控制。PID 控制器是一种线性控制器，它根据给定值与实际输出值构成控制偏差。 将偏差的比例(P)、积分(I)和微分(D)通过线性组合构成控制量，对被控对象进行控制，故称 PID 控制器。由于单片机控制是采样控制，只能根据采样时刻的偏差控制输出量，不能计算连续输出量，所以要采用离散化的 PID 算法。离散 PID 分为位置式和增量式两种。位置式的输出量直接就是控制量，由于全量输出，所以每次输出均与过去的状态有关，计算时要对过去偏差进行累加，计算机工作量大；而且因为计算机输

25、出量对应的是执行机构的实际输出，如计算机出现故障，输出大幅度变化会引起执行机构位置的大幅度变化，这种情况往往是生产实践中不允许的，在某些场合，还可能造成严重的生产事故。因而产生了增量式 PID 控制的控制算法，所谓增量式 PID 是指数字控制器的输出只是控制量的增量。由于计算机输出增量， 所以误动作时影响小算式中不需要累加。控制增量的确定仅与最近 k 次的采样值有关，所以较容易通过加权处理而获得比较好的控制效果。5.8.2 方向控制追灯过程中，利用左右差速实现车模的方向控制，近灯状态下，则利用恒定17第十三届全国大学生智能汽车技术报告差速实现小车的绕灯动作，绕灯可以最大限度保持车模运行速度，但

26、是由于车模惯性较大，加之图像帧率有限，无法做到立刻判断信标灯熄灭，所以仍需要降低速度。5.8.3 PI 速度控制由于增量式 PID 相对于位置式PID 有明显的优势，所以我们选择增量式 PID作为速度控制算法，用编码器获取速度作为反馈量，实现闭环控制。18第六章 系统开发和调试工具6.1 开发环境：IAR EWARMIAR Embedded Workbench for ARM（下面简称 IAR EWARM）是一个针对ARM 处理器的集成开发环境，它包含项目管理器、编辑器、C/C+编译器和 ARM 汇编器、连接器 XLINK 和支持 RTOS 的调试工具 C-SPY。在 EWARM 环境下可以使

27、用 C/C+和汇编语言方便地开发嵌入式应用程序。比较其他的 ARM 开发环境，IAR EWARM 具有入门容易、使用方便和代码紧凑等特点。利用 IAR EWARM，可以通过设置断点、单步执行等操作对程序进行调试，还可以用 Livewatch 查看变量数值，大大节省了软件调试时间。 图 6.1IAR 调试界面 19第十三届全国大学生智能汽车技术报告6.2 上位机调试为了获取车模在行驶时的运行数据，我们在车上装了蓝牙，可以将关键参数实时发送到上位机查看，上位机我们使用山外的多功能调试助手，它支持查看图像、虚拟示波器、串口助手等功能，使用方便。图 6.2 山外多功能调试助手另外，我们社团还自己编写了

28、可以在安卓手机上使用的 APP 紫丁香调试助手，同样支持虚拟示波器、图像、调试遥控等智能车调试常用的功能。图 6.4 紫丁香调试助手的虚拟示波器功能图 6.3 紫丁香调试助手的主界面 20第六章 系统开发和调试工具6.3 图像处理软件为了更直观的看到图像处理算法的效果，一方面，我们将摄像头采集到的图像存到 TF 卡中，然后在电脑中导出，再利用 visual studio 结合 Opencv，将处理过后的效果图显示出来，方便调试。Opencv 的配置方法网上很多，在这不做介绍。另一方面，我们还有利用星瞳科技提供的 Openmv IDE 处理助手，在 IDE 界面中能直观的看到摄像头传回的图像，并

29、得到当前图像在 LAB 色彩空间中三个色彩通道的直方图。在 IDE 中，我们通过对图像的实时观察，对直方图的实时分析，并利用下方的输出界面，来实现对图像的实时调控。图 3.3OpenMv IDE 21第十三届全国大学生智能汽车技术报告 第七章 总结本文从机械、硬件、软件等方面详细介绍了车模的设计和制作方案。从机械和硬件方面，我们力求电路的稳定可靠，尽量将车模做的轻快灵活，尽可能降低重心；在软件方面，我们努力追求最有效的行驶路径，不断优化各方面策略，软件主动防翻、有效的避障策略等是我们的创新点。同时，考虑到对抗过程中的各种不确定因素，我们还做了车模翻车后的应急策略，尽可能提高车模的稳定性。在车模

30、制作过程中，我们学到很多知识，逐渐对单片机控制、电路设计等有了比较好的认识，很感谢社团能够给我们这样的平台，感谢各位指导老师和学长的指导。经过近一年的辛苦准备，我们也积累了很多经验，深刻认识到坚持不懈的重要性，经过一次次的校内选拔，从开始的近到最后的十几人，考验的不是谁更聪明，而是谁可以坚持努力，不断地发现问题解决问题，坚持为比赛付出时间和精力。 最后，再次感谢哈工大智能车创新俱乐部这个大家庭，感谢一直支持和关注智能车比赛的学校和学院领导以及各位老师，也感谢比赛组委会能组织这样一项很有意义的比赛。 22参考文献 卓晴，黄开胜，邵贝贝学做智能车北京：北京航空航天大学122007.蔡述庭“飞思卡尔

31、”杯智能汽车竞赛设计与实践北京：北京航空航天大学. 2012.34王淑娟，蔡惟铮，齐明模拟电子技术基础北京：高等教育2009朱政合飞思卡尔智能赛车及调试平台开发研究.大连理工大学硕士学位论文201056789高隽，谢昭图像理解理论与方法北京：科学2009王映辉人脸识别原理、方法与技术北京：科学2010余志生主编汽车理论北王望予主编汽车设计北械工业械工业20092004白志刚自动调节系统解析与 PID 整定化学工业，2012.23附录 A核心控制程序代码#include include.h #include math.h #define obstacle_min 20信标障碍对比距离用最小值#d

32、efine obstacle_max 60信标灯列数差值上限，越大越避障#define Long_A 260#define Long_B 218/以下为速度宏定义#define V_rollover 40 预备翻回速度#define V_rollright 30 翻回瞬间速度#define SPEED_X_MAX 20#define speedf_hit 25 撞灯前轮速度#define speedb_hit 20装订后轮速度，也为原地转速度#define speed_redy 20 光电管导航 y 速度#define speed_redz 8光电管导航 z 速度/int LOW = 0;/具

33、体使用的摄像头行数#define ERROR_MAX 60/后灯偏差极限值，最大为 160 float l_error_adjust = 0;次坐标偏差float p_error_adjust = 0;次坐标偏差int hit_time = 0;撞灯时间记数变量/上一/上上/I第十三届全国大学生智能汽车技术报告int hit_flag = 0; int red_flag = 0;光电管状态标志位int add_flag = 0;陀螺仪补偿标志位int mode_time = 0;记录各状态持续次数int avoid_time = 0;避障计步int obstacle_flag_last = 0

34、;一次障碍标志位int light_last = 0; 的信标灯上次的值float speed_high = 0;加速程序int hit_count = 0;记录触发撞灯检测次数float l_error_simu = 0; 一次坐标偏差float p_error_simu = 0; int mode_last = 2;上一次状态标志位void CONTROL_A(float percent); void CONTROL_B(float percent); void CONTROL_C(float percent); void CONTROL_D(float percent);void PID

35、_A(int speed_true,float speed_expect); void PID_B(int speed_true,float speed_expect); void PID_C(int speed_true,float speed_expect); void PID_D(int speed_true,float speed_expect); void PID_E();void SIDE_JUDGE();void OBSTACLE_JUDGE();void HIT_JUDGE(); void STOP_JUDGE();void FAR_JUDGE();void SELECT_MO

36、DE();/上/亮/上/II附录 A 部分程序代码void FIND_LIGHT();void GYRO_ADD();int LIMIT_INT(int date,int limit);float LIMIT_FLOAT(float date,float limit); void ERROR();void CHANGE_SPEED(float VA,float VB,float VC,float VD); void CHANGE_FREE(float Vx,float Vy,float Vraw);void CAMERA_JUDGE(); void DEBUG_SPEED();/调试用期望速度

37、，占空比void MODE_JUDGE();void ROLLOVER_JUDGE();int my_count = 0; int correct_time = 0; 正时间int camera_last = 0;记录上一次用的哪个摄像头/float COUNT = 0; int rollover_time = 0; 车时间检测int rollright_time = 0;车矫正检测int shift_time = 0;换车头死区时间/顺/翻/翻/切/*/ void CONTROL_PWM()HIT_JUDGE();/优先判断是否撞灯STOP_JUDGE();if(mode_flag = 0)

38、追灯状态加速判断FAR_JUDGE();elsefar_flag = 0;/以下为避障模式选择控制电机控制程序/III第十三届全国大学生智能汽车技术报告if(avoid_time = 0)只有在非延时避障下进行避障判断OBSTACLE_JUDGE();/障碍物判断处理if(obstacle_flag != 0)obstacle_flag_last = obstacle_flag;if(obstacle_flag = 0 & obstacle_flag_last != 0 & avoid_time = 0)avoid_time = 1;if(avoid_time 0)单稳态触发器避障avoid_

39、time +;if(avoid_time = *avoid_delay)avoid_time = 0;obstacle_flag_last = 0;if(abs(error) 5)障模式 2 重新选择切灯方向if(error 0)light_flag = 1; elselight_flag = -1; SELECT_MODE();/调速前，强制模式选择GYRO_ADD();/陀螺仪速度补偿if(camera_flag = 0)如果 A 摄工作！if(obstacle_flag_last = 1)碍物较近/避/障IV附录 A 部分程序代码speed_A_expect = (float)-*spe

40、edb_avoid + add_flag * percent_add; speed_B_expect = (float)*speedb_avoid - add_flag * percent_add; speed_C_expect = (float)-*speedf_avoid - add_flag * percent_add;speed_D_expect = (float)*speedf_avoid + add_flag * percent_add;else if(obstacle_flag_last = -1)speed_A_expect = (float)*speedb_avoid + a

41、dd_flag * percent_add; speed_B_expect = (float)-*speedb_avoid - add_flag * percent_add; speed_C_expect = (float)*speedf_avoid - add_flag * percent_add; speed_D_expect = (float)-*speedf_avoid + add_flag * percent_add;else if(abs(obstacle_flag_last) = 2)障区域if(abs(speed_x) SPEED_X_MAX)if(speed_x 0)obst

42、acle_flag_last = -2;elseobstacle_flag_last = 2;if(obstacle_flag_last = 2)speed_A_expect = (float)-*speedb_avoid + add_flag * percent_add; speed_B_expect = (float)*speedb_avoid - add_flag * percent_add; speed_C_expect = (float)-*speedf_avoid - add_flag * percent_add; speed_D_expect = (float)*speedf_a

43、void + add_flag * percent_add;else if(obstacle_flag_last = -2)speed_A_expect = (float)*speedb_avoid + add_flag * percent_add;/较远避V第十三届全国大学生智能汽车技术报告speed_B_expect = (float)-*speedb_avoid - add_flag * percent_add;speed_C_expect = (float)*speedf_avoid - add_flag * percent_add; speed_D_expect = (float)-

44、*speedf_avoid + add_flag * percent_add;else if(mode_flag != 2)/非正常顺正补偿PID_E();speed_A_expect = (float)(*speed_expect + far_flag * speed_high - diff_percent + add_flag * percent_add);speed_B_expect = (float)(*speed_expect + far_flag * speed_high + diff_percent - add_flag* percent_add);speed_C_expect

45、= (float)(*speed_expect + far_flag * speed_high + diff_percent); speed_D_expect = (float)(*speed_expect + far_flag * speed_high - diff_percent);/else if(mode_flag != 2)/正常顺正不补偿/PID_E();speed_A_expect = (float)(*speed_expect + far_flag * speed_high - diff_percent); speed_B_expect = (float)(*speed_exp

46、ect + far_flag * speed_high + diff_percent); speed_C_expect = (float)(*speed_expect + far_flag * speed_high + diff_percent);speed_D_expect = (float)(*speed_expect + far_flag * speed_high - diff_percent);else if(camera_flag = 1)果 B 摄工作！if(obstacle_flag_last = 1)speed_A_expect = (float)*speedf_avoid -

47、 add_flag * percent_add; speed_B_expect = (float)-*speedf_avoid + add_flag * percent_add; speed_C_expect = (float)*speedb_avoid + add_flag * percent_add; speed_D_expect = (float)-*speedb_avoid - add_flag * percent_add;else if(obstacle_flag_last = -1)/如VI附录 A 部分程序代码speed_A_expect = (float)-*speedf_av

48、oid - add_flag * percent_add; speed_B_expect = (float)*speedf_avoid + add_flag * percent_add; speed_C_expect = (float)-*speedb_avoid + add_flag * percent_add;speed_D_expect = (float)*speedb_avoid - add_flag * percent_add;else if(abs(obstacle_flag_last) = 2)障区域if(abs(speed_x) SPEED_X_MAX)if(speed_x 0

49、)obstacle_flag_last = 2;elseobstacle_flag_last = -2;if(obstacle_flag_last = 2)speed_A_expect = (float)*speedf_avoid - add_flag * percent_add; speed_B_expect = (float)-*speedf_avoid + add_flag * percent_add; speed_C_expect = (float)*speedb_avoid + add_flag * percent_add; speed_D_expect = (float)-*spe

50、edb_avoid - add_flag * percent_add;else if(obstacle_flag_last = -2)speed_A_expect = (float)-*speedf_avoid - add_flag * percent_add; speed_B_expect = (float)*speedf_avoid + add_flag * percent_add; speed_C_expect = (float)-*speedb_avoid + add_flag * percent_add; speed_D_expect = (float)*speedb_avoid -

51、 add_flag * percent_add;else if(mode_flag != 2)PID_E();/较远避VII第十三届全国大学生智能汽车技术报告speed_A_expect = -(float)(*speed_expect + far_flag * speed_high + diff_percent); speed_B_expect = -(float)(*speed_expect + far_flag * speed_high - diff_percent); speed_C_expect = -(float)(*speed_expect + far_flag * speed_

52、high - diff_percent +add_flag * percent_add);speed_D_expect = -(float)(*speed_expect + far_flag * speed_high + diff_percent - add_flag * percent_add);/else if(mode_flag != 2)PID_E();speed_A_expect = -(float)(*speed_expect + far_flag * speed_high + diff_percent); speed_B_expect = -(float)(*speed_expect + far_flag * speed_high - diff_percent); speed_C_expect = -(float)(*speed_expect + far_flag * speed_high - diff_percent); speed_D_expect = -(float)(*speed_expect + far_flag * speed_high + diff_percent);if(back_flag != 0) side_flag = back_flag;/以下为一系列强制执行速