电机速度与舵机转向的控制算法

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

智能汽车竞赛

技术报告

学校：大连民族学院

队伍名称：民院二队

参赛队员：聂志戎易弓张新秀

带队教师：陈兴文

关于技术报告和研究论文使用授权的说明

本人完全了解第二届全国大学生“飞思卡尔”杯智能汽车竞赛关保留、使用技术报告和研究论文的规定，即：参赛作品著作权归参赛者本人，比赛组委会和飞思卡尔半导体公司可以在相关主页上收录并公开参赛作品的设计方案、技术报告以及参赛模型车的视频、图像资料，并将相关内容编纂收录在组委会出版论文集中。

参赛队员签名：聂志

戎易弓

张新秀

带队教师签名：陈新文

日期：2007-8-20

第一章引言 (1)

第二章智能车寻迹的硬件系统设计 (2)

2.1.电源管理电路 (2)

2.2.电机驱动电路 (4)

第三章系统软件设计 (7)

3.1.软件主流程 (7)

https://www.360docs.net/doc/6c5890553.html,D图像信息的采集与特征的提取 (8)

3.3.电机速度与舵机转向的控制算法 (9)

3.3.1.当前赛道信息的识别与控制 (9)

3.3.2.对未来赛道信息的预测与控制 (10)

第四章模型车机械安装及主要技术参数说明 (11)

4.1.智能车机械部分的安装及改造 (11)

4.2.智能车的主要技术参数说明 (11)

第五章开发工具与调试 (12)

5.1.开发工具、安装 (12)

5.2.调试过程 (12)

参考文献 (13)

附录A 程序源代码 (14)

附录B基于CCD图像的分析方法及预测算法 (22)

第一章引言

全国大学生“飞思卡尔”智能汽车竞赛的比赛规则是：使用大赛组委会统一提供的竞赛车模，采用飞思卡尔的16位单片机MC9S12DG128 作为核心控制单元，自主构思控制方案及系统设计，包括路径信息的采集与处理、控制算法及执行、电机驱动、舵机转向控制等，以比赛完成的时间短者为优胜者。其中赛道为在白色底板上铺设黑色引导线。制作智能车，需要参赛队员学习并应用嵌入式开发工具CodeWarrior和在线开发与在线调试。自行设计并制作硬件电路板，设计自动识别路径的方案，学习微控制器的MC9S12DG128的软件编程。其专业知识涉及控制、模式识别、检测技术、汽车电子、计算机和机械等多个学科，对学生的知识的融合和实践动手能力的培养，有很大的推动作用。

本队以单片机MC9S12DG128 作为核心，使用CodeWarrior 软件开发工具，充分利用单片机各功能模块设计制作了本智能车控制系统。根据赛道特点，主要有三种方案：光电传感器方案，摄像头方案，光电传感器与摄像头融合控制方案。

本方案采用CCD摄像头对道路信息进行检测，可以从图像中提取较多的有用信息，其中包括小车前视方向与道路方向的斜率和道路的曲率等。提前判断道路弯道、直道的路况信息，从而实现安全过弯，快速通过直道，在保证稳定的条件下追求行使时间最短。

第二届全国大学生智能汽车竞赛技术报告

第二章智能车寻迹的硬件系统设计

本方案采用组委会提供的核心9S12DG128电路板，自行设计了核心板以外的驱动系统。利用CCD采集路径信息，传给单片机，对图像信号进行处理，从而控制舵机转向和直流电机调速。整个系统的硬件结构框图如图 2.1所示。

图2.1 系统硬件结构框图

整个系统的主要单元电路包括电源管理电路、电机驱动电路、视频同步分离电路、转速测量电路。下面将各电路简要介绍如下：

2.1.电源管理电路

电源的管理在整个电路中起着很重要的作用，工作电压的稳定性直接关系着系统的稳定性，关系着系统能否正常工作。它不仅为单片机提供工作电压，而且为各个控制芯片提供电压，为CCD提供工作电压。

本智能车系统中，我们选择了一款低差压芯片TPS7350，该芯片的优点是当输出电流为100mA时，最大差压只有35mV。为了减小系统运行过程中电机波纹对电源的干扰，设计了大电容与大电感组成的LC滤波电路，对电源管理芯片的电源进行滤波，保证电源芯片的正常工作电压。

第二章智能车寻迹的硬件系统设计

整个智能车系统所需的+5V的工作电压是使用一片TPS7350提供的。原理图如图2.2所示。

图2.2 TPS7350工作原理图

为了提高舵机的响应时间，我们将舵机的工作电压提高到+6V。仍然使用低差压电源芯片TPS7350。由于TPS7350的输出电压是+5V，为了得到+6V的工作电压，我们通过提高芯片地电位的方式，即，将芯片的地通过两个二极管接至供电电源的地，这样就可以为舵机提供+6V的工作电压。原理图如图2.3所示。

图2.3 舵机工作电压原理图

第二届全国大学生智能汽车竞赛技术报告

CCD摄像头的工作电压要求比较高，我们使用升压模块将电压提高到+9V，从而保证CCD能够正常工作。原理图如图2.4 所示。

图2.4 CCD工作电压原理图

2.2.电机驱动电路

直流电机的转速控制采用PWM控制。由于单片机输出的脉宽无法驱动竞赛用的直流电机，因此需要通过驱动电路才能驱动电机，设计中采用组委会提供的MC33886 芯片。为增强其驱动能力，可以将芯片两个输入引脚并联使用，接到单片机PWM引脚，并将两个输出并联接电机，增强电机带负载能力。此芯片具有短路保护、欠压保护与过温保护功能，将芯片的FS引脚接一信号灯，对异常情况进行报警。原理图如图2.5所示。

第二章智能车寻迹的硬件系统设计

图2.5 电机驱动原理图

2.3.视频同步分离电路

视频同步分离电路主要采用视频同步分离芯片LM1881，直接对视频信号进行同步分离，准确地获得所需的视频图像信号，将视频信号通过一个电容接到LM1881的2脚，即可得到控制单片机进行A/D采集的控制信号：一行同步CSO，一场同步VS与奇偶场同步O/E。原理图如图 2.6所示。

图2.6 视频同步分离原理图

2.4.转速测量电路

小车转速的检测是对车速进行闭环控制的基础，直接影响到将来控制效果的好坏。本设计采用光电码盘测速法。把分度盘安装在小车的后轴，随电机驱

第二届全国大学生智能汽车竞赛技术报告

动后轴转动使分度盘随之等速转动。利用红外透射式传感器检测到的通断个数就能检测到小车后轮转速。速度传感器采用红外投射式传感器，利用发射管发出红外光线，透过分度盘的缺口照射光敏三极管，使其迅速由截至状态变为导通。随着分度盘转动，反复形成脉冲信号，利用脉冲累加对脉冲信号进行计数，在一选定的时间内读出脉冲总数，从而计算出小车的当前速度。传感器结构原理图如图2.7 所示。

图2.7 转速测量原理图

第三章系统软件设计

本设计系统中采用的是CCD摄像头对道路信息进行采集，得到实时图像数据后，对图像进行处理，提取赛道中的引导黑线的位置，从而以此作为改变舵机转向和控制电机转速的理论依据。

本方案中，以CodeWarrior4.1为开发工具，系统的软件设计包括：系统的初始化，视频图像信号采集及处理程序，以及对小车电机、舵机的控制程序。具体实现以下的功能：系统初始化、视频图像信号的采集、视频图像的分析及处理、电机控制、舵机的控制。

3.1.软件主流程

小车系统软件的运行主流程是：采集程序在PJ口中断服务程序中对CCD 摄像头的场视频信号进行控制，在外中断服务程序中采集每行信息，每13行读取一行信息。主程序交替对数据进行处理和计算并给出控制量，控制周期为20ms。主程序检测到完成一场采集数据结束的标志后，对相应的数据进行处理并计算控制量，控制舵机转向以及电机的转速，清除数据结束标志位，同时等待下一场数据的到来。软件的主流程如图 3.1 所示。程序见附录A。

第二届全国大学生智能汽车竞赛技术报告

3.2.C CD图像信息的采集与特征的提取

CCD摄像头输出的是标准的视频复合信号，利用同步信号分离芯片LM1881 和S12单片机的A/D转换器可以对视频信号进行采集，从而得到CCD的灰度图像数据，经二值化处理就可以得到跑道黑线轨迹在图像上的点阵。单片机总线

第三章系统软件设计

频率超频到32MHz ，每行最多能够采集42 个点，前10个数据为行消隐，共采集32个有效数据点，采集20行信息。

CCD黑线中心坐标的提取大致的思想如下。首先判断图像采集是否完成，完成了才开始下面的处理。从最近一行图像开始检测，设定检测的左右边界位置。根据本行获得的黑线左右边界来确定下一行的左右边界值。如此循环直到处理完整幅图像。另外，可能会有异常信息，全黑或者检测不到黑线，此时作为异常情况，置一个异常标志位，对其进行单独处理。

3.3.电机速度与舵机转向的控制算法

3.3.1.当前赛道信息的识别与控制

一场图像数据处理结束后，赛道的路径信息会清晰的再现出来。根据整幅图像黑线的有效起点和有效终点的斜率和偏离图像中心位置的偏移量，就可以判断当前赛道是直道还是弯道。

根据这些信息给定舵机的控制量。此方案获的主要思想如下：首先找到图像数据最近位置的第一个有效行，同时记录黑线的中心位置信息，定义为（X0，Y0 ）。然后再找最远位置的第一个有效行，以及其黑线中心位置，定义为（X1，Y1 ）。据此算出黑线的斜率（Y1 - Y0 ）/ （X1 - X0 ），和黑线偏离图像中心位置的偏移量| X0 |。其示意图如图3.2所示。

第二届全国大学生智能汽车竞赛技术报告

其计算公式为：

舵机控制量 = K0*X0 + K1* （X1 - X0 ）/（Y1 -Y0 ）（4）

根据不断的调试和经验数据得到比例系数 K0，K1 的确切的值。

3.3.2.对未来赛道信息的预测与控制

单纯的判断当前赛道的信息，不能十分准确地给出舵机转角的控制量。小车在高速运行时，遇到比较急的弯道，容易出现差错。经常出现转角控制量滞后而冲出跑道。鉴于此，我们在原方案的基础上添加了预测算法。

根据CCD视频图像的扫描特点，前视距离较远处扫描到的图像含有信息较多，所以根据一幅图像的前视距离最远处1/4段曲线的切线可以预测到未来赛道的信息。在曲线的前面1/4处取一点（X2，Y2），其示意图如图3.1所示。舵机转向的计算公式为：

舵机控制量 = K0*X0 + K1*（X1 - X0）/（Y1 -Y0）+K2 *（X1-X2）/(Y1-Y2) (5)

根据点（X2，Y2）和点（X1，Y1）段曲线的弯曲程度，可以预测出下一段道路的走向。调解系数K2，控制舵机转向的角度，降低舵机转向滞后的机率。可以使小车不完全按照黑线走，走内道，灵活，连续地行使。

电机的控制速度采用闭环控制，根据速度反馈和弯道信息给定当前电机的速度，直道上加速，进弯道时减速，由于CCD摄像头看的比较远，可以提前看到弯道，减速，进弯道后看到另一场图像，再进行下一次处理，根据控制策略，进行加减速。同时结合速度反馈，当速度超过某标定值时，减速处理，低于标定值时，加速处理。

第四章模型车机械安装及主要技术参数说明整个智能车主要由车模、舵机、直流电机、 S12核心电路板、驱动电路板、CCD摄像头、电池等组成。智能车的安装包括电池部分的安装、最小系统电路板的安装、舵机的安装、驱动电路板的安装及摄像头的安装。

4.1.智能车机械部分的安装及改造

电池固定在智能车自带的电池槽上，为了加大力臂，增强舵机转向的灵活性，缩短其响应时间，我们将舵机安装在车身前部，并将高度提高。电机安装在车身后部，用来驱动后轮控制小车的前进、后退、加速或减速及刹车。S12 核心板与驱动板安装在车身中间。

CCD的位置、高度、俯仰角度很重要，直接影响CCD的视场。CCD的安装采用门形，架在小车的前身。通过校准板，利用小电视观察，对CCD的位置、高度和角度进行调解，以得到最好的效果，找到CCD的最佳位置、高度及角度并将其固定。

4.2.智能车的主要技术参数说明

1)改造后的车模总体重量，长28 cm 、宽17 cm 、高35 cm尺寸等基本参数。

2)平均电路功耗，电容总容量1990uF；

3)传感器共有2个：CCD视频传感器1个，光电传感器1个。

4)除了车模原有的驱动电机、舵机之外无伺服电机。

5)赛道信息检测精度18.75mm ，频率为50Hz

第二届全国大学生智能汽车竞赛技术报告

第五章开发工具与调试

5.1.开发工具、安装

开发工具使用的是CodeWarrior4.1的开发环境，该开发环境包括集成开发环境IDE、处理器专家库、全芯片仿真、可视化参数显示工具、项目工程管理器、C交叉编译器、汇编器、链接器以及调试器。它能够为单片机MC9S12DG128提供与之配套的应用程序开发模块。下载程序时，可以利用串口, 也可以利用调试器BDM与单片机之间的连接下载程序。

5.2.调试过程

下面具体介绍智能车的制作与调试过程。

（1）熟悉S12 单片机与开发环境。为了能够迅速的掌握MC9S12DG128型单片机各个模块及CodeWarrior4.1开发环境的应用，队员们各自查找资料，翻阅书籍，上网查询等，并进行交流，分别对各个模块进行调试。（2）完成小车的各个基本模块，画版图。包括电源模块、电机驱动模块、视频采集、舵机控制等。

（3）完成硬件安装。PCB板完成后，根据模型车说明，将小车装好，将各部分硬件安装好，包括核心板、驱动板的安装、CCD的安装。

（4）CCD摄像头的标定，小车整体调试。调试过程记录经验数据，据此优化速度，优化资源。

第二届全国大学生智能汽车竞赛技术报告

参考文献

[1] Motorola. “MC9SDG128B Device User Guide V1.09 ”. 2005.11

[2] Motorola. “S12 VREG Block User Guide V01.01 ”. 2005.5

[3] Motorola. “S12 ATD_10B8C Block User Guide V02.12 ”. 2005.11

[4] Motorola. “S12 PWM_8B8C Block User Guide V01.17 ”. 2005.5

[5] Motorola. “S12 ECT_16B8C Block User Guide V01.06 ”. 2005.5

[6] Motorola. “S12 INT Block User Guide V01.06 ”. 2005.5

[7] Motorola. “S12 MEBI Block User Guide V01.06 ”. 2005.5

[8] Motorola. “Codewarrior 使用指南” . 2005. 10

[9] Freescale. “MC33886 User Guide”. 2005.7

[10]National. “LM1881 User Guide”. 2006.3

[11] TI. “TPS7350 User Guide”. 2005.7

[12] 清华大学. “BDM_forS12_TBDML_用户手册V31 ”. 2005.12

[13] 卓晴黄开胜邵贝贝主编.学做智能车[M]，北京航空航天大学出版社

2007.3

[14] 邵贝贝.单片机嵌入式应用的在线开发方法[M].北京：清华大学出版社，

2004.

[15] 谭浩强.C程序设计[M].北京：清华大学出版社，1999.

第二届全国大学生智能汽车竞赛技术报告

附录A 程序源代码

#include /* common defines and macros */ #include /* derivative information */ #pragma LINK_INFO DERIVATIVE "mc9s12dg128b"

/********** Variables Definition *********************/ uint uiVideo_line_count;

uint uiVideo_line_flag;

uint uiVideo_Data[20][32];

uint uiVideo_over_flag;

uint uiVideo_line;

uint uiVideo_AD_Data[20][32];

uint uiVideo_Limen[20][6];

uint uiVideo_Ava_Limen[20];

uint uiVideo_Field_uicount;

uint uiVideo_init_flag;

uint uiVideo_Center[20][6];

uint uiVideo_Center_Count;

uint uiVideo_Continuous_Line[20];

uint uiVideo_Continuous_Count;

uint uiLine_First_Place;

uint uiLine_Last_Place;

uint uiRow_First_Place;

uint uiRow_Last_Place;

int iRow_First;

int iRow_Last;

int iLine_First;

int iLine_Last;

int iPlace_first;

int iPlace_last;

uint Video_Place_flag;

第二届全国大学生智能汽车竞赛技术报告

uint uiSpeed;

uint uiflag;

uint uiOdd;

uint uiSpeed_Flag;

uint uiSpeed_Tbl[900];

ST_PID pst_pid;

/********PWM Initial***********/

void PWM_init(void)

{ PWMCTL = 0X80;

PWMPOL = 0X02;

PWMCAE = 0x00;

PWMCLK = 0X00;

PWMPRCLK = 0x36;

PWMSCLA = 0X00;

PWMSCLB = 0X00;

PWMPER67 = 30000;

PWMDTY67 = 6600;

PWMPER0 = 200;

PWMPER1 = 200;

PWMDTY0 = 0;

PWMDTY1 = 0;

PWME = 0X83;

}

/**************AD Initial ****************/

void AD_init(void)

{

ATD0CTL2=0xC0;

ATD0CTL3=0x08;

ATD0CTL4=0x81;

ATD0CTL5=0xA0;

ATD0DIEN=0x00;

第二届全国大学生智能汽车竞赛技术报告

}

/**************CCD Initial ****************/ void Video_Init(void)

{ uint i, j;

HPRIO = (unsigned char)(Virq & 0xff);

INTCR |= INTCR_IRQE_MASK;

PIEJ = 0x02;

PPSJ = 0X02;

INTCR_IRQEN = 0;

uiVideo_line_count = 0;

DDRA = 0X00;

DDRB = 0XFF;

for(i = 0; i < 20; i++)

{ for(j = 0; j < 6; j++)

{ uiVideo_Center[i][j] = 255;

}

uiVideo_Continuous_Line[i] = 255;

}

/**********Get Image Information **********/ void Video_Get_Image(void)

{if(1 == uiVideo_line_flag)

{ Video_transform_Line();

uiVideo_line_flag = 0;

}}

void Video_transform_Line(void)

{ uint i, j;

uint uiVideo_flag;

uint uiVideo_count;

uint uiVideo_count1;

uint uiVideo_count2;

uiVideo_flag = 0;