第十届“飞思卡尔”杯全国大学生

Transcription

1 第十届 飞思卡尔 杯全国大学生 智能汽车竞赛 光电组智能车技术报告 学校 : 中国石油大学 ( 华东 ) 队伍名称 : 旋风冲锋参赛队员 : 刘希岩李佩佩孙小雷带队教师 : 鄢志丹王宇红 I

2 关于技术报告和研究论文使用授权的说明 本人完全了解第十届 飞思卡尔 杯全国大学生智能汽车竞赛关保留 使用技术报告和研究论文的规定, 即 : 参赛作品著作权归参赛者本人, 比赛组委会和飞思卡尔半导体公司可以在相关主页上收录并公开参赛作品的设计方案 技术报告以及参赛模型车的视频 图像资料, 并将相关内容编纂收录在组委会出版论文集中 参赛队员签名 : 带队教师签名 : 日期 : II

3 目录 第一章引言... 1 第二章智能车总体设计方案 系统总体方案的选定 系统总体方案的设计 小结... 3 第三章智能车机械结构设计 机械部分总体概述 线性 CCD 布局与安装 电池 电路板等部分的固定 D 模型设计 角度传感器安装... 7 第四章智能车硬件电路设计 系统主板电路设计 系统电源电路设计 电机驱动电路设计 线性 CCD 原理介绍 蜂鸣器电路设计 硬件电路部分总结 第五章智能车软件及控制算法设计 开发工具 软件控制的总体思路 主程序设计 主程序结构 上位机设计 串口调试工具 上位机 角度控制 互补滤波 角度闭环控制 速度控制 方向控制 图像二值化 图像滤波 赛道元素识别 电机差动控制 软件部分总结 第六章智能车技术参数 第七章结语 总结 致谢 参考文献 附录 A 程序核心源代码 III

4 第一章引言 全国大学生 飞思卡尔杯 智能车竞赛以飞思卡尔半导体公司为协办方, 在规定的模型汽车平台上, 使用该公司的 8 位 16 位微控制器作为核心控制模块, 通过增加道路传感器, 电机驱动电路以及编写相应软件, 制作一个能够自主识别道路的模型汽车, 按照规定路线行进, 以完成时间最短者优胜 该竞赛融科学性 趣味性和观赏性为一体, 是以迅猛发展 前景广阔的汽车电子为背景, 涵盖自动控制 模式识别 传感技术 电子 电气 计算机 机械与汽车等多学科专业的创意性比赛 本文中, 我们通过介绍车体机械结构 硬件电路 软件算法等内容, 详细阐述了我们组在此次比赛中对于光电组智能车的研究与制作, 同时也在车体机械结构以及控制策略上有一定的创新点 1

5 第二章智能车总体设计方案 2.1 系统总体方案的选定 此次比赛, 我们选用官方提供的 E 车模 ( 包括其提供的电机 电池 轮胎等器件 ); 赛道检测选择 TSL1401 线性 CCD, 其具有体积小 重量轻 使用简单 易于固定 接口简单等优点, 同时其前瞻较远, 能够有效提取赛道相关信息, 满足此次比赛的要求 图 2-1 车体总体方案 2.2 系统总体方案的设计 智能车以 MKL26Z256VLL4 单片机为控制核心, 采用 TSL1401 线性 CCD 采集图像, 并对二值化图像进行了低通滤波, 通过边沿检测算法识别赛道的黑白边沿, 求出中心偏差后作为转角的控制量 我们通过结合陀螺仪与加速度计的采集值来使得小车直立, 同时用编码器测出轮胎实际转速, 通过 PID 控制, 形成闭环控制 为方便调试, 在智能车上还设计了 OLED 液晶显示和蓝牙通讯模块, 通过无线通信实现上位机实时图像显示 参数曲线显示和在线调整功能 实验证明小车可以准确识别赛道, 并能保持平稳快速运行 2

6 第二章智能车总体设计方案 图 2-2 系统结构图 2.3 小结 本章主要阐述了总体方案的选择, 以及从大方面对各个模块的作用 智能车基本行 驶有一个系统把握 3

7 第三章智能车机械结构设计 第三章智能车机械结构设计 3.1 机械部分总体概述 平衡组智能车要求重心尽量放低, 行驶过程中转动惯量要小, 车模的平衡角度取决于各个部件的安装位置和重量, 必须保证结构稳定牢固 好的机械结构是车模进一步提速的基础, 对于平衡组小车来说尤其重要 我们对车模机械的调整都是为了使车模重量减轻, 重心降低, 齿轮咬合既不宽松也不紧密, 小车行驶顺滑流畅 3.2 线性 CCD 布局与安装 图 3-1 线性 CCD 支架立杆位置示意图图 3-1 是车模 CCD 支架立杆与地面的数学几何图 讨论立杆与地面夹角 的变化对车模前瞻 b 的影响, 对 两个变量进行定量分析, 得出 b 关于 的函数 由正弦定理式 (3-1) 和式 (3-2) b sin sin (3-1) (3-2) 联立 (3-1) 和 (3-2) 得式 (3-3) sin b a (3-3) sin( ) 对式 (3-3) 对 b 关于 求偏导得式 (3-4) b a 1 b 2 a 2 b 2 2 (sin ) sin 2 (3-4) 由式 (3-4) 可得 : 当时, 最小, 此时小于 90 度 4

8 第三章智能车机械结构设计由此可知, 线性 CCD 采集行与 CCD 立杆间的夹角为 90 度, 且立杆前倾时, 平衡车行进过程中车身的晃动对图像稳定性的影响最小 根据今年的新规则, 平衡组小车需要爬过坡度不超过 15 度, 高度不限的的对称坡道和不对称坡道, 由于线性 CCD 采集信息局限, 这时可能会造成小车看得很远致使视野变花, 造成小车冲出跑道, 所以我们采取两个线性 CCD 竖直摆放, 用下面的 CCD 进行坡道辅助识别, 效果明显 采用 PVC 支架将线性 CCD 紧紧固定在直径 5mm 长约 35mm 的碳素杆上 为保证碳素杆不会在小车运行过程中摇晃, 我们使用三个短碳素杆和热熔胶, 采用三脚架结构将竖直碳素杆牢牢固定在车身上 最终确定 CCD 安装位置如图 3-2 所示 图 3-2 CCD 安装位置 3.3 电池 电路板等部分的固定 电池和电路板是车模上重量较大的物体, 它的安装位置决定了小车的平衡角度, 转动惯量大小, 进而影响小车在运行过程中的流畅程度和极限速度 我们将电池和电路板安装在小车前方, 两者紧紧贴合, 保证重心尽可能的低 电池使用厚度 3mm 的 PVC 支架托在车模前方, 安装稳固 mini 编码器安装在车模车身上方, 轮轴上方, 使用塑料支架和螺丝固定在车身上, 并保证齿轮咬合恰到好处 5

9 第十届全国大学生智能汽车邀请赛技术报告 图 3-3 电路板编码器电池安装位置 3.4 3D 模型设计 小车的直立要求机械结构绝对合理稳定, 结合我们小车的特点, 我们用 Solidworks 为小车量身设计了电池固定带 编码器支架 CCD 固定支架, 用学院 3D 打印机制作出来 模型设计如图 3-4 所示 (a) (b) 6

10 第三章智能车机械结构设计 (c) 图 3-4 3D 模型 (a) 电池固定带 ;(b) 编码器支架 ;(c) CCD 固定支架 ; 3.5 角度传感器安装 数字加速度计 MMA8451 安装在车模上, 车模运行震动对它的影响比较大, 所以我们 将加速度计安装在车模底部中间, 尽量减少车模震动对它的影响 图 3-5 加速度计安装位置 为保证数字陀螺仪 L3G4200 的水平安装以及测量精度, 我们沿着车模固有的水平轮 廓线将陀螺仪传感器安装在车模上方中间 7

11 第十届全国大学生智能汽车邀请赛技术报告 图 3-6 陀螺仪安装位置 8

12 第四章智能车硬件电路设计 4.1 系统主板电路设计 主板电路主要有电源接口 单片机最小系统板插座, 线性 CCD 插口,AD 端口, 陀螺 仪 加速度计 液晶蓝牙插口, 电机驱动接口, 编码器接口等 ( 如图 4-1 所示 ) 图 4-1 系统主板电路设计 4.2 系统电源电路设计 稳定的系统首先需要电源稳定, 系统中有三路电源,8.0V,5V,3.3V 8.0V 电池电压直接接到电机驱动模块,5V 为测速编码器 线性 CCD 供电,3.3V 给单片机 陀螺仪 加速度计 液晶 蓝牙 蜂鸣器供电 采用了一片 TPS7350[1] 为两个线性 CCD 供电,TPS7350 为微功耗低压差线性稳压芯片 使用一片低压降串联稳压芯片 LM2940 将电源电压 8.0V 稳到 5.0V, 作为两片电源芯片 TPS7333 的输入电压, 避免 TPS7333 的前后压差太大 同时 LM2940 还要为两个编码器供电 用一片 TPS7333 单独为单片机最小系统供电, 另一片 TPS7333 为陀螺仪 加速度计 液晶 蓝牙 蜂鸣器供电 系统电源原理图如图 4-2 所示 9

13 第十届全国大学生智能汽车邀请赛技术报告 (a) (b) (c) (d) 图 4-2 系统电源原理图 (a)tps7350 5V 稳压 ;(b)lm2940 5V 稳压 ;(c)tps V 稳压 ;(d)tps V 稳压 4.3 电机驱动电路设计 我们使用驱动芯片 BTN7971 搭建 H 桥来控制电机正反转, 硬件电路简单, 控制调试方便 光电平衡组智能车有两个直流电机, 所以我们采用 4 片 BTN7971, 每两片控制一个电机 ( 如图 4-3 所示 ), 用四路 PWM 控制电机正反转 BTN7971 的内阻比较小, 驱动电流可达七十安, 运行过程中发热量较少, 符合智能车比赛要求 10

14 第四章智能车硬件电路设计 图 4-3 电机驱动模块原理图 4.4 线性 CCD 原理介绍 TSL1401 线性 CCD 是本次比赛官方指定传感器, 它由 128 个光电二极管的线性阵列组成 照射在光电二极管上的光的能量产生光电流, 相关像素点上的有源积分电路对这些光电流进行积分 在积分期间, 积分器的输出通过一个模拟开关连接到采样电容 在每个像素中积累的电荷量与光强度和积分时间成正比 积分器的输出和复位由一个 128 位的移位寄存器和复位逻辑控制 11

15 第四章智能车硬件电路设计 图 4-4 线性 CCD 驱动时序图 对于线性 CCD 模拟信号的采集, 我们采取的方法是用单片机两个普通 IO 端口 PTE20 PTE21 驱动两个 CCD 的两个 SI 和两个 CLK 引脚, 采用两路 AD 端口 PTE16 PTE17 分别连接线性 CCD 的模拟输出 AO 口, 既节省了单片机端口资源, 在进行模拟信号采集时也会节约时间 4.5 蜂鸣器电路设计 蜂鸣器的作用在于验证单片机程序是否运行到指定的位置, 对于软件调试起到非常重要的作用 采用贴片式 SS8050 三极管作为驱动三极管, 具有体积小 重量轻 电流大等优点 电路图如图 4-5 所示 图 4-5 蜂鸣器电路原理图 4.6 硬件电路部分总结 硬件电路部分, 我们在前人的基础上做了一些尝试, 也出现了若干问题 尝试在通往驱动的电源上串联一个 470uf 的电感, 因为电感阻碍电流的变化使得平 12

16 第四章智能车硬件电路设计衡车在扰动下站不起来站得很软, 最后把电感剪掉, 用焊锡将焊盘连接到一起解决了问题 为了防止焊接过程中发生短路, 将 PCB 敷铜密度规则设置为线宽的两倍, 没有注意到铜被分割了, 导致 LM2940 的地没有和其它地连接在一起 调试过程中因为粗心大意导致 PCB 短路, 造成一个编码器的地与 PCB 断开并烧坏了一个编码器, 最后只能通过铁丝将地线连接在一起 各个电子元器件的布局, 杜邦线插针和插排的位置也非常重要, 关系到板子的简洁和机械结构重心的合理性, 插线横跨位置尽量少 13

17 第五章 智能车软件及控制算法设计 5.1 开发工具 我们使用的开发环境是 IAR Embedded Workbench,IAR Systems 是全球领先的嵌入式系统开发工具和服务的供应商 IAR Embedded Workbench 是一套完整的集成开发工具集合, 包括从代码编辑器 工程建立到 C/C++ 编译器 连接器和调试器的各类开发工具 功能强大, 支持在线调试, 可以应用到多种处理器 图 5-1 IAR 软件界面 5.2 软件控制的总体思路 平衡车的软件控制首先要求保持车模直立不倒, 然后加入速度控制和方向控制, 它们对角度控制的影响要要尽可能的小 下面首先介绍单片机的中断时序设计和信号采集设计, 进而分别介绍控制所用算法 14

18 第五章智能车软件及控制算法设计 5.3 主程序设计 开始 参数定义 系统初始化 开中断 直立控制 电机输出 速度控制 路径分析 方向控制 (a) 15

19 第十届全国大学生智能汽车邀请赛技术报告 1.5ms 中断进入 时间片段 1: 读入加速度计数值 时间片段 2: 读入上下陀螺仪数值角度互补滤波角度控制电机输出 时间片段 3: 进行 10 次计数每次平滑输出控制量 第 10 次计数 : 读入编码器脉冲数值速度控制 时间片段 4: 进行 2 次计数每次平滑输出控制量第 1 次计数 : 线性 CCD 图像处理第 2 次计数 : 读入转向陀螺仪数值方向控制 1.5ms 中断退出 (b) 双边沿赛道 双边沿赛道对应参 数及算法 直角 直角对应 路径分析 方向控制 单引导线 参数及算法单引导线对应 返回偏差值 参数及算法 楔形障碍 楔形障碍对应 参数及算法 (c) 16

20 第十届全国大学生智能汽车邀请赛技术报告 200ns 中断进入 时间片段 1: SI 置 1 时间片段 2: CLK 置 1 时间片段 3: SI 置 0 时间片段 4-259: 偶数次 : AD1 采集 AD2 采集 CLK 反转奇数次 : 不做动作 时间片段 260: CLK 置 0 计数清零 200ns 中断退出 (d) 图 5-2 主程序结构示意图 (a) 主程序流程图 ;(b)pit0 中断处理 ;(c) 路径分析方向控制 ;(d)pit1 中断处理 ; 主程序结构平衡车的平稳直立和恒速运行需要严格的控制周期, 将所有程序都放在了定时器中断里面, 保证直立, 速度, 方向之间的相互影响尽可能的小 陀螺仪 L3G4200 和加速度 MMA8451 计数据的读入均通过单片机 I2C 总线进行, 使用示波器实际测量陀螺仪和加速度计采集时间, 测得分别耗费时间 1.2ms 左右, 为了保证采样的快速性和在中断时间内采样完成, 将定时器 PIT0 中断配置为 1.5ms, 在定时器 PIT0 里面完成的任务有 : 17

21 第十届全国大学生智能汽车邀请赛技术报告第一个 1.5ms 采集加速度计加速度信号 ; 第二个 1.5ms 采集陀螺仪角速度信号, 进行角度融合, 角度控制, 电机输出 ; 第三个 1.5ms 进行方向控制, 方向控制量 2 次平滑输出 ; 第四个 1.5ms 进行速度控制, 速度控制量 10 次平滑输出 在调试过程中发现, 线性 CCD 采集的图像存在不稳定跳动 经过分析, 原因是线性 CCD 采样放在主循环里面, 一次时序完成需要耗费 4.5ms 左右, 在这期间会被定时器 0 中断打断 2-3 次, 影响 CCD 若干点采样的曝光时间 所以配置了第二路定时器 PIT1, 由于 MKL26 只有 PIT0 和 PIT1 两个定时器通道, 两个定时器共用一个中断号, 两个中断任务写在一个函数里面, 并且写在前面的标志位对应的任务优先级要高 在 PIT0 中每 6ms 开一次 PIT1 定时器中断, 当再次进入 PIT 服务函数时会进入 PIT1 中断, 定时器 1 中断时间为 200ns, 模拟线性 CCD 输入端 SI CLK 采样时序电平, 采样电平同时对两路 CCD 提供驱动信号, 在 260 次 PIT1 中断完成后, 实现了对两路 CCD 数据的采集, 此时关闭 PIT1 中断 [2] 具体中断服务程序见附录 5.4 上位机设计 串口调试工具 利用蓝牙模块可以将车模的角度 车速和偏差等数据无线发送到上位机, 供在线调 试和数据离线分析, 加快调试进程 图 5-3 蓝牙模块 上位机 18

22 第五章智能车软件及控制算法设计直立车有较多的参数需要调试整定, 设计了一个基于 C# winform[3] 环境下的上位机, 对小车参数进行在线传送和参数观测, 下位机中则根据既定的协议配置串口接收中断 图 5-4 上位机面板 利用上位机软件的数据保存功能对智能车的速度, 角度, 图像等进行保存, 采用 matlab 对数据进行绘图显示, 方便离线分析 5.5 角度控制 互补滤波平衡车需要通过车身角度来维持车身平衡, 角度的测量对平衡车极其关键, 常用的用来测量角度的传感器有加速度计和陀螺仪 加速度计的特点是动态响应较慢, 由于机械振动原因, 加速度计测量信号会在实际值附近出现大波动, 在高频段信号不可用 陀螺仪的特点是响应快, 积分后可测倾角, 不过由于零漂和温漂, 会出现积分漂移 [4], 在低频段信号不好 通过低通滤波抑制加速度计高频噪声, 通过高通滤波抑制陀螺仪低频噪声, 这就是互补滤波 图 5-5 是互补滤波器的图形表达 Y X 低通滤波器 角度 加速度计 高通滤波器 数值积分 19 角速度 陀螺仪

23 第十届全国大学生智能汽车邀请赛技术报告 图 5-5 互补滤波器互补滤波将陀螺仪和加速度计的优点融合起来, 得到在高频和低频都比较好的信号 互补滤波相比较卡尔曼滤波 [5] 的优点是不需要对噪声精确建模, 实现简单 互补滤波的程序实现如式 (5-1) angle = Q*(angle + gyro * dt) + R*(x_acc) (5-1) 其中 angle 是滤波后的角度,Q 可以理解为陀螺仪占的比重,gyro 是陀螺仪测得的角速度,dt 是采样时间,R 为加速度计占得比重,R 与 Q 的和为 1,R>0,Q>0,x_acc 是加速度计测得的倾角 本系统采样时间为 dt=0.006s 在小车开机后还没有开始运动时, 因为此时加速度计测得的角度扰动比运动过程中因为电机 车模震动带来的扰动要小得多, 所以在电机还没有开始转动之前让加速度计占得比重大一些, 让滤波后角度快速跟踪加速度计的输出 经过调试取 Q=0.5,R=0.5, 如图 5-6 所示, 滤波后角度能够及时跟踪加速度计输出 互补滤波 加速度计 Z 轴角度 滤波后角度 图 5-6 平衡车开机静止初期加速度计和滤波后角度对比图 在小车运动过程中, 合适的加速度计比例可以滤除加速度计的波动影响, 同时也能 有效抑制陀螺仪的漂移, 这个比例值比小车静止时的比例值要小 经过调试取 Q=0.995, R=0.005 滤波效果不错, 如图 5-7 所示 20

24 第五章智能车软件及控制算法设计 互补滤波 加速度计 Z 轴角度滤波后角度 图 5-7 平衡车运动过程中加速度计和滤波后角度对比图 角度闭环控制 角度采取 PD 控制, 保持小车在静止和运行过程中的角度恒定,PD 参数调整好之后, 小车的角度控制平滑, 整体上小车站得比较硬, 没有大的突变 5.6 速度控制 通过控制电机转速可以控制车轮速度, 实现车模运行速度控制 在平衡组的速度控制中, 最终是通过角度控制改变车模速度, 简单形容则为前倾加速 后倾减速 程序中的速度环实际上是对小车角度控制的扰动, 我们进行 PI 控制的目的是使该扰动在小车运行过程中保持稳定快速, 这和普通速度控制有些类似 但所不同的是, 由于小车速度控制是对角度控制的扰动, 为使此扰动不至于变化太大影响车模稳定直立, 需要设置速度控制周期比角度控制周期长, 但是速度周期过长又会使速度调节缓慢 参考官方方案的 100ms, 调试初期我们采用了 120ms, 后来缩短到 96ms, 再后来缩短到 60ms, 通过不断调节角度 PD 参数使小车在运行过程中保持稳定 综合考虑扰动和响应快慢, 速度控制周期设为角度控制周期的 10 倍, 即 60ms 小车在运行过程中要使速度尽快到达设定值并保持速度稳定, 以减少对小车角度的干扰 对于小车速度, 速度调节采用 PI 控制, 对速度积分项进行限幅, 最终速度控制量输出也要进行限幅 21

25 第十届全国大学生智能汽车邀请赛技术报告 5.7 方向控制 通过线性 CCD 返回的图像信息, 运用路径识别算法得到车模的中心偏离赛道中心的 偏差, 通过控制算法与电机差动控制结合实现方向控制 线性 CCD 路径识别算法主要涉 及两部分, 一是图像的二值化和滤波处理, 二是各种赛道的识别, 最后得出小车转向的 偏差 图像二值化 由于线性 CCD 是模拟传感器, 其传感器信号为电压值, 电压值的大小反映的是传感 器所看到环境的亮暗程度 一方面, 传感器本身的信号稳定性极其重要, 它是识别的基 础 ; 另一方面, 对信号在各种环境下进行处理, 通过软件程序使其能够分辨白色和黑色, 增强其环境适应性极其重要 采取固定阈值二值化, 仅限于在光线特别理想均匀的地方 对于智能车比赛场地, 光线往往不是最理想状态, 有亮的地方也有比较暗的地方, 在这 种情况下采取固定阈值二值化很容易发生误判 经过不断实验和数据观察, 我们最后采取动态阈值二值化 具体步骤是对每行的最 大值和最小值计算比值, 如果最大值比最小值的 2.2 倍要小, 则认为是灰度值差别不大 的颜色, 采取上一次的动态阈值对此行进行二值化 用阈值对每行数据进行二值化, 如 果数据大于阈值, 则将数组 Pixel[i]=255, 否则 Pixel[i]= 图像滤波 车模在运行过程中 CCD 得到的数据具有噪声, 本文主要考虑了两种滤波方法, 高斯 滤波和低通滤波 高斯滤波也是低通滤波, 用在图像二值化以前 ; 低通滤波也就是去除 高频噪声, 如两个黑点中间夹一个白点, 用在二值化之后 高斯滤波是一种线性平滑滤波, 用于消除高斯噪声, 广泛应用于图像处理的减噪过 程 高斯滤波就是对整幅图像进行加权平均的过程, 每一个像素点的值, 都由其本身和 邻域内的其他像素值经过加权平均后得到 高斯滤波的具体操作是 : 用一个模板扫描图像中的每一个像素, 用模板确定的邻域 内像素的加权平均灰度值去替代模板中心像素点的值 由于线性 CCD 为一维数据, 我们实验采取大小为 3 1 的模板, 采取中间点权重 0.8, 两边点权重 0.1, 计算公式如式 (5-2) g 其中, f x 0.1 f x f x 0.1 f x 1 x 为图像中 x 点的灰度值, g x 22 (5-2) 为该点经过高斯滤波后的值

26 灰度值 第五章智能车软件及控制算法设计 采取 matlab 对原图像数据进行仿真, 效果图如图 5-9 所示 250 CCD 图像 原始图像灰度值 高斯滤波后图像灰度值 CCD 像素点位置 图 5-9 原始图像灰度值与高斯滤波后图像灰度值 低通滤波器, 在灰度连续变化的图像中是比较有效的, 二值化后的图像将是成片的白色或者成片的黑色 如果两个相同颜色像素点之间出现一个不一样颜色的像素点, 比如说一片暗区中突然出现了一个白点, 这种情况被认为是一种噪声 利用低通滤波器可以减少图像噪声 最终我们只采用了低通滤波来滤掉小车运行过程中的图像干扰, 效果满足智能车运行要求 具体代码如下 if((max/min)<=ratio_max_min_white)// 比值太小上次阈值二值化 for(i=0;i<=127;i++)// 二值化 if(ad_data[i]>=mid)pixel[i] = 255; else Pixel[i] = 0; 23

27 第五章智能车软件及控制算法设计 else mid= (max+min)*0.5; // 求平均自适应取阈值 for(i=0;i<=127;i++)// 二值化 if(ad_data[i] >= mid)pixel[i] = 255; else Pixel[i]= 0; for(i=1;i<=126;i++)// 图像低通滤波 if(pixel[i-1]!=pixel[i]&&pixel[i]!=pixel[i+1])pixel[i]=pixel[i+1]; 赛道元素识别赛道元素的识别是建立在赛道具有连续性的基础上实现的 图 5-10 是一些典型 24

28 灰度值 灰度值 灰度值 灰度值 第十届全国大学生智能汽车邀请赛技术报告 赛道的原始图像和二值化后图像 300 直道 CCD 像素点位置全白 原始图像 二值化图像 CCD 像素点位置 (a) 300 右转弯 CCD 像素点位置左转弯 原始图像 二值化图像 CCD 像素点位置 (b) 图 5-10 双线元素 (a) 直道和全白 ;(b) 右转弯和左转弯 ; 25

29 灰度值 第十届全国大学生智能汽车邀请赛技术报告 第十届智能车比赛加入了直角元素, 在数学上它属于连续但不可导的, 不同于之前的连续又可导的双边线赛道, 因此直角元素需要特殊识别一下 我们的识别方法首先判别直角元素前面是直道, 之后如果是左边 ( 右边 ) 全白, 触发可能会是左 ( 右 ) 拐直角条件, 然后如果连续 6-7 次左 ( 右 ) 全白即认为是左 ( 右 ) 拐直角, 左 ( 右 ) 拐直角标志置位, 左 ( 右 ) 转向保持, 直到双边沿寻到为止才清左 ( 右 ) 拐直角标志 因为平衡车在运行过程中角度会发生变化, 导致视角的不同, 看相同宽度的东西像素值所对应的宽度差别比较大 因此根据绝对宽度判别楔形障碍容易发生漏判或者误判 我们根据相对宽度为主, 绝对宽度为辅的方法判别楔形障碍 : 观察当小车看到楔形障碍时的图像特征, 如果障碍在赛道左侧, 障碍侧有一段小白, 右边相邻的是障碍的黑色, 右边相邻的是赛道的白色 它们的宽度满足 W1>20,W1<60,W2>10,W1>2*W2,W3>3, W3<W2, 其中 W1 是白块的长度,W2 是障碍的宽度,W3 是小白块的宽度 300 楔形障碍 CCD 像素点位置 图 5-11 楔形障碍 第十届新加入的单引导线背景有多种方式, 其中最典型的是单引导线铺在普通双线赛道中间和铺在白色白板上 对于铺在普通双线赛道中间的单引导线, 识别之后需要往左往右扫一下左右黑边沿是否同时存在, 然后沿着最后的左右边沿求出的赛道偏差运行 对于铺在白色白板上的单引导线, 因为这个白色背景是在两个双边沿赛道的衔接处, 所以当小车在白色背景上运行时, 需要保证小车做好衔接工作, 不能跑出去 因此标志 26

30 灰度值 灰度值 位置 1 之后清零的标志是双边沿已经寻到, 并且宽度合适 第五章智能车软件及控制算法设计 300 白色背景单引导线 CCD 像素点位置背景为双黑线单引导线 原始图像 二值化图像 CCD 像素点位置 图 5-12 单引导线对于坡道的识别, 首先利用历史数据判断直道, 如果最新几行赛道宽度逐渐变宽, 则认为是坡道 由于线性 CCD 采集图像信息少, 容易丢线, 模仿摄像头图像处理的方法, 当车丢线时, 补最近时刻双边沿的宽度 对于弯道入十字我们也进行了特殊处理, 效果显著 方法是看到全白, 则对历史边沿进行分析, 寻找十字与弯道衔接处拐点, 以此时的左右边沿作为此时的修正值进行车体姿态修正, 一定次数后再回到普通寻线 具体图像识别处理算法见附录 电机差动控制利用计算方向所得偏差经过比例微分算法分别与车模速度控制信号进行加和减, 形成左右轮差动控制电压, 使得车模左右轮运行角速度不一致进而控制车模方向 通过简单的比例控制可以初步实现方向控制 通过左右电机速度差驱动车模转向消除车模偏差是一个积分过程, 因此车模差动控制不需要进行积分控制 由于车模本身安装有电池等比较重的物体, 具有很大的转动惯量, 为了抑制车模在拐弯过程中出现过冲摆动现象, 引入陀螺仪的水平转向角速度加入微分作用, 使车模不会发生摆动, 转弯流畅 27

31 第五章智能车软件及控制算法设计 为了减少小车方向控制对角度控制的影响, 方向控制周期设为角度控制周期的 2 倍, 即 12ms 5.8 软件部分总结 软件的调试就是一次一次想算法, 不断实验 改进和排除错误的过程 调试过程中需要根据经验来判断哪里存在问题 CCD 信号采集需要稳定的周期, 不能被其它中断打断 ; 同时调试过程中需要串口发送 CCD 数据, 如果发送与 CCD 采集是顺序关系, 会改变 CCD 曝光时间, 就是此时所得到的数据不是小车真正运行时看到的图像 控制参数的调整也非常重要, 它们是平衡车直立稳定的基础 小车角度设定值要是小车保持静止时的角度, 大了会造成小车超速, 小了会造成小车加速缓慢 小车运行过程中不能站得太软否则角度会波动较大, 速度积分限幅非常重要, 太小小车不能达到设定速度, 太大则容易造成波动 小车转向需要用到水平陀螺仪角速度, 需要确定小车的水平轴具体是什么 调试初期我们用的是小车的陀螺仪 Y 轴作为转向轴, 经过实测才发现水平转向轴是 Z 轴 28

32 第六章智能车技术参数 车模重量 (g) 500 车模长 宽 高 (mm) 200*210*320 电路功耗 (w) 8w 电容总容量 (uf) 1017 传感器种类 线性 CCD TSL1401; 加速度计 MMA8451; 陀 传感器个数 ( 个 ) 6 螺仪 L3G4200; 编码器龙邱 附加电机 赛道检测精度 赛道检测频率 无 5mm 12ms/ 次 29

33 第七章结语 7.1 总结 在这期间, 我们完成了一些基础性工作, 并有一些不错的想法 : (1) 图像处理采用动态阈值二值化并进行低通滤波 能有效适应各种光强, 准确识别赛道信息 (2) 缩短速度控制周期 改进官方方案 100ms 的速度控制周期, 改为 60ms, 保证控制及时有效 (3) 使用上位机与 matlab 结合进行系统软件调试, 包括在线调试和离线分析, 大大加快调试进程 7.2 致谢 感谢实验室智能车小伙伴的辛勤付出, 感谢队友 学长 老师 朋友在艰难岁月里的陪伴和鼓励, 感谢母校中国石油大学 ( 华东 ) 对我们的培养 第十届比赛有幸见到了卓晴老师, 非常佩服老师孜孜不倦, 乐于付出的品德, 向您致敬! 30

34 参考文献 [1] 王明顺, 沈谋全. Power Design of Intelligent Vehicle Based on TPS7350[J]. 东北大 学信息科学与工程学院.2006,(24):1-5. [2] 姚成文, 苏晨光, 顾竟潇. 天津大学光电平衡组 SpeedTime 技术报告 [R].2013: [3] Karli Watson, Jacob Vibe Hammer, Jon D.Reid, Morgan Skinner, Daniel Kemper, Christian Nagel, 齐立波, 黄俊伟, 黄静.C# 入门经典 ( 第 6 版 )[M]. 北京 : 清华大学出版社,2012: [4] Nagel D J. MEMS:Micro technology[j].ieee Circuits & Device, 2001:17(2): [5] Kalman, R.E. A New Approach to Linear Filtering and Prediction Problems. Transaction of the ASME-Journal of Basic Engineering[D]. 1960, 20(10):4-8. [6] 第七届全国大学生飞思卡尔杯智能汽车竞赛电磁组直立行车参考设计方案 ( 版本 2.0)[R]. 竞赛秘书处,2012:

35 附录 A 程序核心源代码 void PIT_IRQHandler(void) if(pit_tflg(pit1) == 1) // 判断是否 PIT1 进入中断 PIT_Flag_Clear(PIT1); // 清中断标志位 static uint16 cnt=0; if(cnt==0) TSL_SI=1; else if(cnt==1)tsl_clk=1;// 第一个 CLK(1) else if(cnt==2)tsl_si=0; else if(cnt==3) gpixel[0]=adc_once(adc0_se5a,adc_8bit); //cnt 奇数时采集, 即在 CLK 置 1 的 200ns 之后 gpixel2[0]=adc_once(adc0_se1,adc_8bit); //cnt 奇数时采集, 即在 CLK 置 1 的 200ns 之后 TSL_CLK=0; else if((cnt%2==1)&&(cnt<=257)) gpixel[((cnt-3)>>1)]=adc_once(adc0_se5a,adc_8bit); //cnt 奇数时采集, 即在 CLK 置 1 的 200ns 之后 gpixel2[((cnt-3)>>1)]=adc_once(adc0_se1,adc_8bit); //cnt 奇数时采集, 即在 CLK 置 1 的 200ns 之后 TSL_CLK=~TSL_CLK; cnt++; if(cnt>=259) // 在这里完成的 129 个 CLK(0) 结束一次采集 TSL_CLK=0; // 第一个 CLK(0) cnt=0; disable_pit(pit1); // gpio_turn (PTD6); if(pit_tflg(pit0) == 1) // 判断是否 PIT0 进入中断 PIT_Flag_Clear(PIT0); // 清中断标志位 static uint8 g_time_counter=0; 32

36 static uint32 Stand_Count=0,START_RUN_Count=0;; static uint8 g_ccd_integ_counter=0; B21_Last=B21; B21=((GPIOB_PDIR>>21)&0x01); if(car_start_run==1) START_RUN_Count++; if(start_run_count>2000) START_RUN_Count=2000; if(b21==1)noreset=1; if(b21_last==1&&b21==0&&e4==1) motor_shut=1; // Beep(1); if(key==0) CCD_DEBUG(gPixel,g_SEND_CNT); g_send_cnt++; if(g_send_cnt==129)g_send_cnt=0; if(key==1) CCD_DEBUG(Pixel,g_SEND_CNT); g_send_cnt++; if(g_send_cnt==129)g_send_cnt=0; /**/ if(car_start_run==0)// 启动控制阶段 if(stand_ready==1) Stand_Count++; if(stand_count==200) car_run_parameter(); // 检测灯塔发车 // if(b21==0&&b21_last==1&&car_start_run==0) 33

37 car_run_start(); CAR_START_RUN=1; if(e5==1&&stand_count>=1000) Stand_Count=1000; car_run_start(); CAR_START_RUN=1; g_time_counter++; g_ccd_integ_counter++; g_battery_cnt++; g_reset_counter++; switch(g_time_counter) case 1: GetGravityAngle();//MMA8451 读取加速度计角度值 break; case 2: GetGyroscopeAngleSpeed(); Angle_Handle();// 互补滤波 // AngleCalculate();// 清华方案 /* if(gpio_get(pte4)&&!(gpio_get(pte5))) // AngleDebug(); AngleDebugNew(); */ // AngleDebug_1(); // 采集角度调试函数 AngleControl(); // 平衡控制函数 motor_speed_output(); break; case 3: if(g_ndirectioncontrolperiod==1) if(!stand_ready)return; Get_Deviation(); //if(gpio_get(pte4)&&gpio_get(pte5)) // DirectionDebug() ; if(g_ndirectioncontrolperiod>=direction_control_period) 34

38 DirectionControl(); g_ndirectioncontrolperiod=0; DirectionControlOutput(); g_ndirectioncontrolperiod++; break; case 4: g_time_counter=0; GetPulse(); if(g_nspeedcontrolperiod>=speed_control_period) SpeedSet();// 速度设定函数 SpeedControl(); // 速度控制函数 /*if(gpio_get(pte5)&&!(gpio_get(pte4)))speeddebug(); if(gpio_get(pte4)&&!(gpio_get(pte5))) AngleDebug(); // AngleDebugNew(); */ g_nspeedcontrolperiod=0; SpeedControlOutput(); g_nspeedcontrolperiod++; break; if(g_ccd_integ_counter>=g_ccd_integ_time) g_ccd_integ_counter=0; enable_pit(pit1); void DirectionControl(void) float fvalue=0; float Error=0,Error_D_Temp=0; static float Error_D=0,Last_Error=0; static float err=4; if(!g_dir_ctrl_on)return; g_fgyroscopeanglespeeddir=(float)get_angle_speed(axis_z)*z_axis_angle_ratio_const;//-gyr 35

39 OSCOPE_OFFSET_DIR;// 转向陀螺仪读数, 防止过冲 Z 轴角速度 // CCD 求出的偏差 // Error=Deviation; /*if(brick_flag==1 LEFT_VERTICAL_START==1 RIGHT_VERTICAL_START==1)Error=Error;// 不连续偏差 else // 连续偏差 if((error-last_error)>err)error=last_error+err;// 防止偏差跳变 else if((error-last_error)<-err)error=last_error-err; */ Error_D_Temp=Error-Last_Error; if(error_d_temp>err)error_d_temp=err; if(error_d_temp<-err)error_d_temp=-err; Last_Error=Error; // 不完全微分 // Error_D=0.5*Error_D+0.5*Error_D_Temp; fvalue=error*dir_control_p+dir_control_d*error_d+g_fgyroscopeanglespeeddir*dir_gy RO; if(fvalue>directioncontroloutmax) // Beep_New2=1; fvalue=directioncontroloutmax; if(fvalue<directioncontroloutmin) fvalue=directioncontroloutmin; // Beep_New2=1; 36

40 DirCtrlOutOld=DirCtrlOutNew; DirCtrlOutNew=fValue; void DirectionControlOutput(void)// 将方向控制输出量的变化平均分配在 2 个周期内 12ms float fvalue=0; if(!g_dir_ctrl_on)return; fvalue=dirctrloutnew-dirctrloutold; //--- 相邻输出变化幅度控制 ---- // /* if(fvalue>dir_change_max) fvalue=dir_change_max; Servo=Servo_last+DIR_CHANGE_MAX; Beep_New=1; else if(fvalue<-dir_change_max) fvalue=-dir_change_max; Servo=Servo_last-DIR_CHANGE_MAX; Beep_New=1; */ g_fdirectioncontrolout=fvalue*(g_ndirectioncontrolperiod+1)/direction_control_period+dir CtrlOutOld; void SpeedControl(void) 37

41 static uint8 Ramp_End_Cnt=15; float fdelta=0; float fp=0,fi=0,fd=0; if(!g_speed_ctrl_on)return; g_fcarspeed_last=g_fcarspeed; g_fcarspeed=(count_left+count_right)*0.05; // 左右编码器脉冲取平均值 g_fcarspeed=g_fcarspeed*0.4+g_fcarspeed_last*0.6; Count_Left_1=Count_Left*0.1; Count_Right_1=Count_Right*0.1; Count_Left=Count_Right=0; /* for(i=2;i>=1;i--) CarSpeed[i]=CarSpeed[i-1]; CarSpeed[0]=g_fCarSpeed;*/ if(e3==1&&ramp_like_flag==1) // g_speed_set=35; //----- 坡道控速 // Ramp_Count++; if(ramp_count==1) CAR_ANGLE_SET=g_CAR_ANGLE_SET-5; // g_speed_set=0; if(g_speed_tar<=65) 38

42 Ramp_End_Cnt=15; // g_speed_set=45; else if(g_speed_tar<=75) Ramp_End_Cnt=15; // g_speed_set=45; else if(g_speed_tar<=85) Ramp_End_Cnt=12; // g_speed_set=45; if(ramp_count>=ramp_end_cnt) //g_speed_set=g_fcarspeed; // Beep_New2=1; Ramp_Count=0; Ramp_Like_Flag=0; // car_run_parameter(); // car_run_start(); CAR_ANGLE_SET=g_CAR_ANGLE_SET; Beep(Ramp_Like_Flag); fdelta=g_fcarspeed-g_speed_set; 39

43 fp=fdelta*g_speed_kp; // 开始把积分项减小 // if(g_speed_set>0&&g_speed_set<g_speed_set_max&&ramp_like_flag==0) SpeedIntegralMax=400; else SpeedIntegralMax=1500; // 去除无用积分 // /*if((g_fspeedcontrolout>=speedcontroloutmax&&fdelta>0) (g_fspeedcontrolout<=speedcontrolout Min&&fDelta<0)) Beep_New=1; else */ fi=fdelta*g_speed_ki; g_fspeedcontrolintegral+=fi; SpeedIntegralMin=-SpeedIntegralMax; if(g_fspeedcontrolintegral>speedintegralmax)g_fspeedcontrolintegral=speedintegralmax;// 积分限幅 if(g_fspeedcontrolintegral<speedintegralmin)g_fspeedcontrolintegral=speedintegralmin; g_fspeedcontroloutold=g_fspeedcontroloutnew; 40

44 // g_fspeedcontroloutnew=fp; g_fspeedcontroloutnew=fp+g_fspeedcontrolintegral; if(g_fspeedcontroloutnew>speedcontroloutmax)g_fspeedcontroloutnew=speedcontroloutmax; if(g_fspeedcontroloutnew<speedcontroloutmin)g_fspeedcontroloutnew=speedcontroloutmin; // Send_integral=0.1*(g_fSpeedControlIntegral+SpeedIntegralMax); void SpeedControlOutput(void) float fvalue=0; if(!g_speed_ctrl_on)return; fvalue=g_fspeedcontroloutnew-g_fspeedcontroloutold; //--- 相邻输出变化幅度控制 ---- // // 防止速度大跳变影响角度控制包括开机即加到全速使得速度控制量瞬间增大拐急弯等 // 此值太小也会导致速度调节不及时 // 过起伏路时特别有用 if(fvalue>speed_change_max) fvalue=speed_change_max; g_fspeedcontroloutnew=g_fspeedcontroloutold+speed_change_max; else if(fvalue<-speed_change_max) fvalue=-speed_change_max; g_fspeedcontroloutnew=g_fspeedcontroloutold-speed_change_max; g_fspeedcontrolout=fvalue*(g_nspeedcontrolperiod+1)/speed_control_period+g_fspeedcontro loutold; 41

45 void Get_Final_Edge() if(left_found==1&&right_found==0) if(left_standard>ccd_end_point-width) Left_edge=Left_standard; Right_edge=Left_edge+width; else Left_edge=Left_standard; Right_edge=Right_standard; else if(left_found==0&&right_found==1) if(right_standard<ccd_start_point+width) Right_edge=Right_standard; Left_edge=Right_edge-width; else Left_edge=Left_standard; Right_edge=Right_standard; 42

46 else if(left_found==0&&right_found==0) Left_edge=Left_standard; Right_edge=Right_standard; else if(left_found==1&&right_found==1) Left_edge=Left_standard; Right_edge=Right_standard; if((right_standard-left_standard>70))//&&(right_standard-left_standard<90) width=right_standard-left_standard; void Find_Ramp() // 判断坡道 // 首先是直道, 最新几行越来越宽 ---- // for(uint8 i=0;i<=19;i++) if((right_line[i]+left_line[i])*0.5>74 (Right_Line[i]+Left_Line[i])*0.5<54 Right_Line[i]==CCD_END_POINT Left_Line[i]==CCD_START_POINT) break; if(i<5&&((right_line[i]-left_line[i])<100 Right_Line[i]<Right_Line[i+1] Left_Line[i]>Left_Line[i+1]) )// ((Right_Line[i]==Right_Line[i+1]&&Left_Line[i]==Left_Line[i+1])) 43

47 break; if(i==19) // Ramp_Like_Flag=1; motor_shut=1; //----- 直角元素识别 // void Find_Vertical_New() static uint8 StraightFlag=0; static uint8 LEFT_TURNCnt=0,RIGHT_TURNCnt=0; static uint8 RightVERTICALcnt=0,LeftVERTICALcnt=0; uint8 Clear_Cnt=20; // 判断直道 // int cnt=9.0*65.0/g_fcarspeed; if(cnt>9)cnt=9; if(cnt<4)cnt=4; if(g_speed_tar<=65) cnt=8; LeftVERTICALcnt=cnt; RightVERTICALcnt=cnt; 44

48 else if(g_speed_tar<=70) cnt=6; LeftVERTICALcnt=cnt; RightVERTICALcnt=cnt; else if(g_speed_tar<=75) cnt=5; LeftVERTICALcnt=cnt; RightVERTICALcnt=cnt; else if(g_speed_tar<=80) cnt=5; LeftVERTICALcnt=cnt; RightVERTICALcnt=cnt; else if(g_speed_tar<=85) cnt=4; LeftVERTICALcnt=cnt; RightVERTICALcnt=cnt; int Len=10; int str_len=len*60.0/g_fcarspeed; if (str_len<6)str_len=6; //str_len=5; 45

49 for(uint8 i=0;i<str_len;i++) StraightFlag=1; if((right_line[i]+left_line[i])*0.5>74 (Right_Line[i]+Left_Line[i])*0.5<54 Right_Line[i]==CCD_END_POINT Left_Line[i]==CCD_START_POINT (Right_Line[i]-Left_Line[i])< 30) StraightFlag=0; break; if(straightflag==1&&left_standard==ccd_start_point&&left_vertical_likely==0)left_ VERTICAL_LIKELY=1; if(straightflag==1&&right_standard==ccd_end_point&&right_vertical_likely==0)righ T_VERTICAL_LIKELY=1; if(left_vertical_likely==1) if(left_vertical_start==0) if(right_standard!=ccd_end_point&&(right_standard-left_standard>80)&&left_standard==ccd_s TART_POINT) LEFT_VERTICAL++; if(left_vertical>=leftverticalcnt) 46

50 LEFT_VERTICAL=0; LEFT_VERTICAL_START=1; LEFT_TURNCnt=0; else LEFT_VERTICAL=0; LEFT_VERTICAL_LIKELY=0; // 清左直角标志 --- // if(left_vertical_start==1) LEFT_TURNCnt++; if(left_turncnt>=clear_cnt (left_found==1&&right_found==1&&(right_standard-left_standard>50 )) (Right_standard==CCD_END_POINT&&Left_standard==CCD_START_POINT)) LEFT_TURNCnt=0; LEFT_VERTICAL_START=0; LEFT_VERTICAL_LIKELY=0; if(right_vertical_likely==1) 47

51 if(right_vertical_start==0) if(left_standard!=ccd_start_point&&(right_standard-left_standard>80)&&right_standard==ccd _END_POINT) RIGHT_VERTICAL++; if(right_vertical>=rightverticalcnt) RIGHT_VERTICAL=0; RIGHT_VERTICAL_START=1; RIGHT_TURNCnt=0; else RIGHT_VERTICAL=0; RIGHT_VERTICAL_LIKELY=0; // 清右直角标志 --- // if(right_vertical_start==1) RIGHT_TURNCnt++; if(right_turncnt>=clear_cnt (left_found==1&&right_found==1&&(right_standard-left_standard> 50)) (Right_standard==CCD_END_POINT&&Left_standard==CCD_START_POINT)) RIGHT_TURNCnt=0; 48

52 RIGHT_VERTICAL_START=0; RIGHT_VERTICAL_LIKELY=0; //--- 寻找楔形路障 ---- // void Find_Brick() uint8 i=0; static int Brick_Left_standard=64; static int Brick_Right_standard=64; static int Brick_elements[2]=0; static uint8 B1_found=0,B2_found=0; static uint8 W1=0,W2=0,W3=0; if(left_brick_flag==0) //----- 路障摆在赛道左边 --- // flag_left=0,flag_right=0; B1_found=0,B2_found=0; for(i=ccd_end_point-20; i>=ccd_start_point;i--) if(pixel[i+1]==255&&pixel[i]==0&&pixel[i-1]==0) // 检测到左边沿 Brick_Left_standard=i; flag_left=1; break; 49

53 if(flag_left==1) for(i=brick_left_standard; i<=ccd_end_point;i++) if(pixel[i-1]==255&&pixel[i]==0&&pixel[i+1]==0) Brick_Right_standard=i; W1=Brick_Right_standard-Brick_Left_standard; flag_right=1; break; if(flag_right==1&&w1>20&&w1<60)//&&(brick_right_standard-brick_left_standard>30)&&(brick_r ight_standard-brick_left_standard<50) for(i=brick_left_standard; i>=ccd_start_point;i--) if(pixel[i]==0&&pixel[i-1]==255) Brick_elements[0]=i; W2=Brick_Left_standard-Brick_elements[0]; B1_found=1; break; if(b1_found==1&&w2>10&&w1>2*w2)// for(i=brick_elements[0]; i>=ccd_start_point;i--) 50

54 if(pixel[i+1]==255&&pixel[i]==0) Brick_elements[1]=i; W3=Brick_elements[0]-Brick_elements[1]; B2_found=1; break; if(b2_found==1&&w3>1&&w3<w2)// Left_Brick_flag=1; Brick_flag=1; Left_standard=Brick_Left_standard; Right_standard=Brick_Right_standard; Brick_width=Right_standard-Left_standard; return; if(right_brick_flag==0) //----- 路障摆在赛道右边 --- // flag_left=0,flag_right=0; B1_found=0,B2_found=0; for(i=ccd_start_point+20; i<=ccd_end_point;i++) 51

55 if(pixel[i-1]==255&&pixel[i]==0&&pixel[i+1]==0) // 检测到右边沿 Brick_Right_standard=i; flag_right=1; break; if(flag_right==1) for(i=brick_right_standard; i>=ccd_start_point;i--) if(pixel[i+1]==255&&pixel[i]==0&&pixel[i-1]==0) Brick_Left_standard=i; W1=Brick_Right_standard-Brick_Left_standard; flag_left=1; break; if(flag_left==1&&w1>20&&w1<60)//&&((brick_right_standard-brick_left_standard>30)&&(brick_ri ght_standard-brick_left_standard<50)) for(i=brick_right_standard; i<=ccd_end_point;i++) if(pixel[i]==0&&pixel[i+1]==255) Brick_elements[0]=i; W2=Brick_elements[0]-Brick_Right_standard; B1_found=1; 52

56 break; if(b1_found==1&&w2>10&&w1>2*w2)//&&(brick_elements[0]-brick_right_standard>15&&brick_el ements[0]-brick_right_standard<25) for(i=brick_elements[0]; i<=ccd_end_point;i++) if(pixel[i-1]==255&&pixel[i]==0) Brick_elements[1]=i; W3=Brick_elements[1]-Brick_elements[0]; B2_found=1; break; if(b2_found==1&&w3>1&&w3<w2)// Right_Brick_flag=1; Brick_flag=1; Left_standard=Brick_Left_standard; Right_standard=Brick_Right_standard; Brick_width=Right_standard-Left_standard; return; 53

57 // 十字修正, 防止弯道入十字串道 // if(whitefound==1&&(right_standard!=ccd_end_point Left_standard!=CCD_START_POINT)) WhiteFound=0; WhiteCount=0; if(whitefound==1&&right_standard==ccd_end_point&&left_standard==ccd_start_point) WhiteCount++; //----- 修正 if(whitecount<=2) // Beep_flag=1; Left_standard=Left_Line[0]; Right_standard=Right_Line[0]; return ierror; else ierror=(left_standard+right_standard)/ ; return ierror; if(right_standard==ccd_end_point&&left_standard==ccd_start_point&&whitefound==0) 54

58 WhiteFound=1; if(ierror>=0) for(i=1;i<=len-2;i++)// 寻找拐点 Left_standard=Left_Line[0]; Right_standard=Right_Line[0]; if(left_line[i]>left_line[i-1]&&left_line[i]>=left_line[i+1]&&right_line[i]==ccd_end_point) Left_standard=Left_Line[i]; Right_standard=Right_Line[i]; break; else if(ierror<0) for(i=1;i<=len-2;i++)// 寻找拐点 Left_standard=Left_Line[0]; Right_standard=Right_Line[0]; if(right_line[i]<right_line[i-1]&&right_line[i]<=right_line[i+1]&&left_line[i]==ccd_start_po INT) Left_standard=Left_Line[i]; Right_standard=Right_Line[i]; break; 55

59 ierror=(left_standard+right_standard)/ ; return ierror; void main(void) float Battery_Voltage=0; DisableInterrupts; cpu_device_init(); LCD_CLS();// 清屏 LCD_Init();//LCD 初始化 Beep_warning(); parameter_init_default(); Key_Switch(); //filter GRAVITY_ADJUST_TIME_CONSTANT=0.04; // 此值越大融合角度过程中加速度计占的比例越小陀螺仪占的比例越大 Q=0.5,R=1.0-Q;// 互补滤波权值 EnableInterrupts; DELAY_MS(500);// 等待滤波器稳定 while(abs_f(g_fcarangle-g_car_angle_set)>blance_range abs_f(g_fgravityangle-g_car_angle_set)>blance_range); //filter GRAVITY_ADJUST_TIME_CONSTANT=1.0; // 此值越大融合角度过程中加速度计占的比例越小陀螺仪占的比例越大 Q=0.995,R=1.0-Q;// 互补滤波权值 56

60 car_ctrl_start(); motor_shut=0; Stand_Ready=1; Beep_New2=1; while(1) if(beep_new2==1)// 蜂鸣器中断 Beep_warning(); Beep_New2=0; Key_Switch(); // Display_int(B21,6,60); // Deviation=dir_error_get_new(ImageData); // Display_int(GYROSCOPE_OFFSET_DIR,6,60); //--- 检测电池电压, 每隔 1.5S 检测一次 --- // if(g_battery_cnt>1000) g_battery_cnt=0; /* Battery_Voltage=hw_ad_ave(ADC0_SE8,ADC_8bit,100)/255.0*3.3*3.0; if(battery_voltage<7.7) Beep_warning_Low_Battery(); */ Display_float(Battery_Voltage,0,0); if(car_start_run==1) 57

61 if(g_reset_counter>1000&&noreset==0) g_reset_counter=0; gpio_set (PTD3,0); else gpio_set (PTD3,1); // GetGravityAngle();//MMA8451 读取加速度计角度值 // g_fgyroscopeanglespeed= (float)(get_angle_speed(axis_x));//i2c 采集 ms display_car_state(); //display_para(); // display_ccd();//4ms // display_ccd2(); 58