电磁循迹小车

本文为第一次智能车校赛的回忆整理，主要是从软件部分说明如何从零开始实现一辆电磁循迹小车，不涉及复杂的算法以及特殊赛道元素识别。

一、系统整体架构

1. 硬件架构

2. 软件架构

二、基础知识准备

1. PWM（脉冲宽度调制）

pwm中比较重要的两个参数为频率和占空比

频率：是指1秒钟内信号从高电平到低电平再回到高电平的次数(一个周期)

占空比：一个脉冲周期内，高电平的时间与整个周期时间的比例

简单来说，PWM是由单片机IO口输出的一系列高低电平信号。当固定频率的时候，通过调节占空比可以输出不同的模拟电压，占空比越大，输出电压越大，当占空比为100%时，输出电压为一般为单片机供电电压（3.3V或5V）。

2. 舵机控制原理

舵机是一种位置伺服电机，带有一个输出轴。当我们向其发送一个控制信号时，输出轴就可以转到特定的位置。只要控制信号持续不变，伺服机构就会保持相对的角度位置不变。如果控制信号发生变化，输出轴的位置也会相应发生变化。大部分舵机的控制都是通过PWM信号控制的。

PWM信号控制舵机的原理主要是通过控制脉宽时间来决定。对应关系如下：

占空比 = t / T 相关参数如下：
t = 0.5ms —————— 舵机会转到 -90°
t = 1.0ms —————— 舵机会转到 -45°
t = 1.5ms —————— 舵机会转到 0°
t = 2.0ms —————— 舵机会转到 45°
t = 2.5ms —————— 舵机会转到 90°

若设置PWM信号的频率为50HZ，则一个脉冲周期为2 ms，调节占空比在2.5%-12.5%即可控制舵机从-90°—90°。

PS：不是所有的舵机都是180度舵机，但脉冲宽度为1.5ms所对应的位置一般都为舵机的中间位置（即全范围的一半）

3. 电机控制原理

智能车所用到的电机主要分为有刷电机和无刷电机两种。一般来说有刷电机更为常用，但近两年来在越野组和单车组也用到了无刷电机，由于作者本身未接触过无刷电机，故本文主要以有刷电机为主。

对于有刷电机，在电机两端施加电压，电机就能转动，电压越大，电机转速越快。电机正反转取决于通电方向，故电机没有严格意义上的正负极。

由于电机正反转由通电方向决定，但实际电路中电源接口一般来说是固定的（即电源接入电路的正负极是固定的），故需要H桥驱动电路实现电机通电方向的切换。H桥驱动电路如下所示，四个开关由MOS管构成，当对角的开关闭合时，电机转动，如图S1和S4闭合时电机正转，S2和S3闭合时反转。

通过上述分析，电机控制主要是控制电机转速以及电机方向。前文提到的PWM信号刚好可以满足电压大小的变换，所以很自然的可以想到用PWM信号去控制电机，但问题在于是否可以直接用单片机IO口输出的PWM信号直接供电给电机，答案是不行的。原因在于单片机IO口的带载能力有限（通俗的说就是电流太小，只有几十mA），而驱动电机需要较大的电流。故需要所谓的电机驱动（这也是很多小白不理解的点），作者本人在做智能车使用过的驱动有DRV8701和HIP4082，本文主要是针对算法分析，不涉及具体的硬件电路分析，故不过多介绍。

4. 电磁循迹原理

赛道的中心会铺设一条电磁引导线，其中会通有20khz、100mA的交变电流。故电磁引导线附近会产生按电流规律变化的磁场，当导体置于该磁场中就会产生感应电动势。一般采用工字电感作为导体，并将产生的感应电压通过运放（运算放大器）放大后输入给单片机处理。

显然，单片机获取到的电压值越大，电感离赛道中心越近，通常使用对称的两个电感进行巡线，当两个电感的差值为0时，小车位于赛道中心。

5. PID算法

PID算法是闭环控制系统的常用算法，简单来说它就是控制输出的物理量达到我们的期望值，比如控制电机转速到某一固定的速度。PID算法又分为位置式PID和增量式PID，算法具体内容网上已有很多介绍，本文不在赘述。
由于本文针对从零开始的小白，尽量不涉及更多的传感器，故电机控制采用开环控制，舵机控制采用位置式PD算法实现。
三、模块调试

1. 舵机调中

当从商家购买到车模时，四轮C车模的舵机是未安装的，需要我们自己进行组装。组装分为以下两步：

1. 机械调中：上文提到过当给舵机1.5ms的高电平pwm信号时，舵机会转到其中间位置，此时将连接车轮的转轴以平行于舵机机身的位置安装上去，就完成了机械调中。

2. 软件调中：需要软件调中的主要原因在于将机械调中后的舵机安装至车模上后，由于一些人为原因和车身机械结构的原因，会产生一些偏差，这时候就需要进行软件调中，大致方法就是利用代码去测试中值位置（一般在机械调中的中值附近），直到轻推小车，小车能直线前行，则软件调中完成。

2. 运放调试

前文提到过需要将电磁传感器获得的电磁值通过运算放大器放大后再输入给单片机处理，原因在于一般情况下电磁传感器获得的值较小，需要将其放大以便后续的处理。调试运放的步骤如下：

• 将安装好电磁传感器的车模放在赛道中间（确保赛道的电磁线已经通电）

• 通过屏幕打印出获得的电感值

• 调节运放上的电位器使对称位置电感所读取的电感值一致，且处在一个合适的数值上（这个数值根据赛道而调整，一般来说设置为所能读取到最大值的一半以上即可）

四、数据处理

1. 归一化

归一化是指将数据映射到0~1的范围内，以便后续的处理。电磁数据需要进行归一化的原因在于由于电感的性能差异以及不同赛道的电磁线的差异会导致电感数据出现较大的波动，导致小车在不同情况下的稳定性下降。

通过归一化就能较好的消除这些影响，在不同赛道调试的时候，只需要测出电感所能读到的最大值和最小值，就能很好的矫正误差。

归一化的公式如下：

value = (value-value_Min)/（value_Max-value_Min）

对于电磁数据，我们会在归一化的同时将其扩大100倍，以便后续进行元素识别和舵机控制。

value = 100*(value-value_Min)/（value_Max-value_Min）

可能会有小白问为什么归一化后还要放大100倍，这样子不是失去了归一化的作用吗？其实这个问题很好解释，理解为我们将数据归一化到了0~100的范围内即可，这样在消除影响的同时便于了后续元素识别（本文不会涉及此部分，可以参考网上大佬们的博客）。

2. 差比和

差比和算法的公式如下：

Err = (A(L−R) + B(LM−RM)) / (A(L+R) + C∣LM−RM∣)

上述公式为卓晴老师推文中所提到的公式（应该是某个学校改进后的差比和算法），但本文是为了入门电磁巡线小车，故简化该公式为：

Err = (L - R) / (L + R)

该公式也是最简单的差比和公式，能够实现正常的巡线，但由于舍弃了中间两个八字电感的数据，会对弯道没有那么敏感。

公式中L代表左侧电感的值，R代表右侧电感的值，可以看出当小车偏向赛道左边时，L变小、R变大，Err为负数，反之Err为正数，即可判定小车在赛道上的位置，正常来说，只使用L - R即可实现此功能，为什么要采用差比和算法。原因在于差比和能够使得获得的Err数据更加的平滑，也就是说不容易出现在某一位置Err发生突变的情况。

3. 数据滤波

数据滤波的主要目的是滤除采集到一些不合理的数据，这些数据可能是因为电感元件本身或者赛道本身的问题所出现的一些异常波动数据。

常用的滤波方法有中值滤波，均值滤波、中值平均滤波、卡尔曼滤波等等，本文给出中值平均滤波的代码：

for(j=0;j<9;j++)  // 冒泡排序
{            
    for(i=0;i<9-j;i++) 
    {
        if(ad_value[i]>ad_value[i+1])          
        {
            temp = ad_value[i]; 
            ad_value[i] = ad_value[i+1]; 
            ad_value[i+1] = temp; 
        } 
    }
}

// 去掉最大值和最小值
for(i=1;i<9;i++)
{
    sum_value += ad_value[i];
}
// 求平均值
temp = sum_value/8;

其思路就是将连续获取到的10次数据从小到大排列，去掉最大值和最小值，然后将剩下的数据求平均值。

五、整车控制实现

整车流程图如下：

前文中已经提及过数据采集和处理的部分，故本部分主要讲述如何实现舵机和电机控制。

1. 舵机pd控制

舵机的控制原理前文已经解释过，对于智能车巡线，舵机起到的作用是控制车身位置始终处于赛道中心，那么如何拟合电感所采集到的误差值与舵机转角之间的关系就是我们所需要考虑的。PID算法就是一个能很好实现闭环控制的算法，它能够在误差和舵机转角之间建立一种联系，使得舵机能够迅速且准确的转向，由于此部分需要进行长时间的调参，需要读者自行摸索（建议读者对PID算法有一定了解后，再进行调参，盲目调参不可取）。

代码实现如下：

error_last = error;
error = get_adc();                                     //获取处理后的电感值

error_angle = kp*error + kd*(error-error_last);        //舵机pd算法确定转角偏值

2. 电机开环控制

由于作者第一次校赛未能实现速度闭环控制，故本文采用的是电机开环控制，原理其实很简单，给定控制电机pwm的占空比，即可让电机保持一个速度转动。

当然，开环控制有一个很大的问题，那就是速度不稳定，当pwm的占空比固定的时候，由于电池电压的下降，以及其他因素的影响，会导致电机的转速产生波动。在低速情况下该问题不明显，但在高速情况下（如小车速度达到2m/s）会出现严重波动，从而导致小车失去控制，故电机速度闭环是十分重要的，该闭环控制采用的是增量式PID算法，本文也不再赘述。

3. 其他细节

在舵机控制时，需要对舵机所能转动的最大角度进行限制，由于舵机安装到车模上后，所能转动的角度范围并不大，如果不加以限制，可能会造成舵机产生堵转最终烧毁舵机。

当电感感应不到电磁信号时，让小车的速度为0，避免小车冲出赛道后导致车模损毁。

六、基于逐飞底层的代码实现

1. 引脚定义

//引脚定义区
#define POWER_ADC1_MOD  ADC_1           //定义通道一 ADC模块号
#define POWER_ADC1_PIN  ADC1_CH3_B14    //定义通道一 ADC引脚

#define POWER_ADC2_MOD  ADC_1           //定义通道二 ADC模块号
#define POWER_ADC2_PIN  ADC1_CH4_B15    //定义通道二 ADC引脚

#define POWER_ADC3_MOD  ADC_1           //定义通道三 ADC模块号
#define POWER_ADC3_PIN  ADC1_CH10_B21   //定义通道三 ADC引脚

#define POWER_ADC4_MOD  ADC_1           //定义通道四 ADC模块号
#define POWER_ADC4_PIN  ADC1_CH12_B23   //定义通道四 ADC引脚

#define S_MOTOR1_PIN PWM4_MODULE2_CHA_C30    //定义舵机引脚

#define PWM_0 PWM1_MODULE3_CHA_D0            //定义电机pwm信号引脚
#define PWM_1 PWM1_MODULE3_CHB_D1
#define PWM_2 PWM2_MODULE3_CHA_D2
#define PWM_3 PWM2_MODULE3_CHB_D3

2. 外设初始化

void Init_Proc(void)
{
    pit_init();                     //初始化pit外设
    pit_interrupt_ms(PIT_CH0,5);    //初始化pit通道0,中断时间为5ms

    pwm_init(S_MOTOR1_PIN,50,3600);    //舵机pwm引脚初始化

    //电机pwm引脚初始化
    pwm_init(PWM_0, 17000, 0);    //单片机端口D0 初始化PWM_1周期10K 占空比0 
    pwm_init(PWM_1, 17000, 0);    //单片机端口D1 初始化PWM_2周期10K 占空比0
    pwm_init(PWM_2, 17000, 0);    //单片机端口D2 初始化PWM_1周期10K 占空比0
    pwm_init(PWM_3, 17000, 0);    //单片机端口D3 初始化PWM_2周期10K 占空比0

    ips200_init();                  //初始化IPS屏幕

//同一个ADC模块分辨率应该设置为一样的，如果设置不一样，则最后一个初始化时设置的分辨率生效
    adc_init(POWER_ADC1_MOD,POWER_ADC1_PIN,ADC_8BIT);  
    adc_init(POWER_ADC2_MOD,POWER_ADC2_PIN,ADC_8BIT);
    adc_init(POWER_ADC3_MOD,POWER_ADC3_PIN,ADC_8BIT);
    adc_init(POWER_ADC4_MOD,POWER_ADC4_PIN,ADC_8BIT);
}

3. 电感数据采集与处理

//电感数据采集处理函数
int get_adc(void)
{
		int i,j;
		int ad_value[10], sum_value = 0;
		int temp;
		for(i=0;i<10;i++)
		{		
			//采集电感值，巡线只使用了两侧电感，若需要进行元素识别则需要使用中间电感
			ad_value1 = adc_mean_filter(POWER_ADC1_MOD,POWER_ADC1_PIN,10);
			ad_value2 = adc_mean_filter(POWER_ADC2_MOD,POWER_ADC2_PIN,10);
			ad_value3 = adc_mean_filter(POWER_ADC3_MOD,POWER_ADC3_PIN,10);
			ad_value4 = adc_mean_filter(POWER_ADC4_MOD,POWER_ADC4_PIN,10);

			//差比和算法
			ad_value[i] = 100*(ad_value4 - ad_value1)/(ad_value4 + ad_value1);
		}
		
		for(j=0;j<9;j++)  														 //冒泡排序
		{
			for(i=0;i<9-j;i++)  
			{
				if(ad_value[i]>ad_value[i+1])  		
				{
					temp = ad_value[i]; 
					ad_value[i] = ad_value[i+1]; 
					ad_value[i+1] = temp; 
				} 
			}
		}
		
		for(i=1;i<9;i++)
		{
			sum_value += ad_value[i];
		}
		temp = sum_value/8;
		return temp;
}

4. HIP电机驱动函数

//HIP电机驱动函数
void motor_ctr(int32 motor1, int32 motor2)
{
		if(motor1 > 0)
		{
			pwm_duty(PWM_0, motor1);
			pwm_duty(PWM_2, 0);
		}
		else
		{
			pwm_duty(PWM_0, 0);
			pwm_duty(PWM_2, -motor1);
		}
				
		if(motor2 > 0)
		{
			pwm_duty(PWM_1, motor2);
			pwm_duty(PWM_3, 0);
		}
		else
		{
		  pwm_duty(PWM_1, 0);
		  pwm_duty(PWM_3, -motor2);
		}
}	

5. main函数

int main(void)
{
    DisableGlobalIRQ();
    board_init();   				//务必保留，本函数用于初始化MPU 时钟 调试串口
    systick_delay_ms(300);	//延时300ms，等待主板其他外设上电成功
	
		Init_Proc();						//外设初始化
		
    EnableGlobalIRQ(0); 	 //使能
		
	  ips200_clear(WHITE);	 //显示屏清屏
		
		//变量初始化
		error = 0;
		error_last = 0;
		kp = 5;								 //舵机pd算法的pd值
    kd = 30;
		
    while(1)
    {
			ips_show();
			
			duty = 3600;    //中值3600，3180-4020（车轮从右到左） //3180，4020都没转到极限，不过3170 4030就会转到极限了 
			
			error_last = error;
			error = get_adc();																//获取处理后的电感值

			error_angle = kp*error + kd*(error-error_last);		//舵机pd算法确定转角偏值
			
			if(abs(error)<20)
			{
				error_angle=0;
			}

			duty = 3600 + error_angle;													
			if(duty > 4000)																		//舵机限幅，防止舵机打死
				duty = 4000;				
			else if(duty < 3200)
				duty = 3200; 
			
			pwm_duty(S_MOTOR1_PIN, duty);											//舵机占空比设置

			motor1 = 10000;																		//前两行电机配速，最后一行调用电机函数，发动电机
			motor2 = 10000;
			if((abs(ad_value1)<2) && (abs(ad_value4)<2))			//感应不到电磁后电机停止转动
			{
				motor1 = 0;
				motor2 = 0;
			}
			motor_ctr(motor1, motor2);	
		}
}

附录：程序源码

#include "SEEKFREE_FONT.h"
#include "headfile.h"

//引脚定义区
#define POWER_ADC1_MOD  ADC_1       	//定义通道一 ADC模块号
#define POWER_ADC1_PIN  ADC1_CH3_B14	//定义通道一 ADC引脚
																											
#define POWER_ADC2_MOD  ADC_1       	//定义通道二 ADC模块号
#define POWER_ADC2_PIN  ADC1_CH4_B15	//定义通道二 ADC引脚
	
#define POWER_ADC3_MOD  ADC_1       	//定义通道三 ADC模块号
#define POWER_ADC3_PIN  ADC1_CH10_B21 //定义通道三 ADC引脚

#define POWER_ADC4_MOD  ADC_1       	//定义通道四 ADC模块号
#define POWER_ADC4_PIN  ADC1_CH12_B23 //定义通道四 ADC引脚

#define S_MOTOR1_PIN PWM4_MODULE2_CHA_C30	//定义舵机引脚

#define PWM_0 PWM1_MODULE3_CHA_D0			//定义电机pwm信号引脚
#define PWM_1 PWM1_MODULE3_CHB_D1
#define PWM_2 PWM2_MODULE3_CHA_D2
#define PWM_3 PWM2_MODULE3_CHB_D3

//函数声明区
void Init_Proc(void);   //初始化
int get_adc(void);			//电感采集
void ips_show(void);		//屏幕显示
void motor_ctr(int32 motor1, int32 motor2);		//电机控制

//变量定义区
int32 duty; 						//舵机占空比
int32 motor1, motor2;		//设置电机转速变量

float kp, kd;						//舵机pid的p d参数
int level;

// 误差值
int16 error;
int16 error_last;
int16 error_angle;	  	//舵机转角pd算法偏值

//四个电感值
int16 ad_value1;
int16 ad_value2;
int16 ad_value3;
int16 ad_value4;


int main(void)
{
    DisableGlobalIRQ();
    board_init();   				//务必保留，本函数用于初始化MPU 时钟 调试串口
    systick_delay_ms(300);	//延时300ms，等待主板其他外设上电成功
	
		Init_Proc();						//外设初始化
		
    EnableGlobalIRQ(0); 		//使能
		
	  ips200_clear(WHITE);		//显示屏清屏
		
		//变量初始化
		error = 0;
		error_last = 0;
		kp = 5;									//舵机pd算法的pd值
    kd = 30;
		
    while(1)
    {
			ips_show();
			
			duty = 3600;				 //中值3600，3180-4020（车轮从右到左） //3180，4020都没转到极限，不过3170 4030就会转到极限了 
			
			error_last = error;
			error = get_adc();	//获取处理后的电感值

			error_angle = kp*error + kd*(error-error_last);		//舵机pd算法确定转角偏值
			
			if(abs(error)<20)
			{
				error_angle=0;
			}

			duty = 3600 + error_angle;													
			if(duty > 4000)																		//舵机限幅，防止舵机打死
				duty = 4000;				
			else if(duty < 3200)
				duty = 3200; 
			
			pwm_duty(S_MOTOR1_PIN, duty);											//舵机占空比设置

			motor1 = 10000;																		//前两行电机配速，最后一行调用电机函数，发动电机
			motor2 = 10000;
			if((abs(ad_value1)<2) && (abs(ad_value4)<2))			//感应不到电磁后电机停止转动
			{
				motor1 = 0;
				motor2 = 0;
			}
			motor_ctr(motor1, motor2);	
		}
}

//初始化函数
void Init_Proc(void)
{
	pit_init();                     //初始化pit外设
	pit_interrupt_ms(PIT_CH0,5);  	//初始化pit通道0,中断时间为5ms

	pwm_init(S_MOTOR1_PIN,50,3600);	//舵机pwm引脚初始化

	pwm_init(PWM_0, 17000, 0);      				 					 //单片机端口D0 初始化PWM_1周期10K 占空比0 //电机pwm引脚初始化
	pwm_init(PWM_1, 17000, 0);     					  				 //单片机端口D1 初始化PWM_2周期10K 占空比0
	pwm_init(PWM_2, 17000, 0);      				 					 //单片机端口D2 初始化PWM_1周期10K 占空比0
	pwm_init(PWM_3, 17000, 0);     					 					 //单片机端口D3 初始化PWM_2周期10K 占空比0

	ips200_init();              											 //初始化IPS屏幕

	adc_init(POWER_ADC1_MOD,POWER_ADC1_PIN,ADC_8BIT);  //同一个ADC模块分辨率应该设置为一样的，如果设置不一样，则最后一个初始化时设置的分辨率生效
	adc_init(POWER_ADC2_MOD,POWER_ADC2_PIN,ADC_8BIT);
	adc_init(POWER_ADC3_MOD,POWER_ADC3_PIN,ADC_8BIT);
	adc_init(POWER_ADC4_MOD,POWER_ADC4_PIN,ADC_8BIT);
}

//电感数据采集处理函数
int get_adc(void)
{
		int i,j;
		int ad_value[10], sum_value = 0;
		int temp;
		for(i=0;i<10;i++)
		{		
			//采集电感值，巡线只使用了两侧电感，若需要进行元素识别则需要使用中间电感
			ad_value1 = adc_mean_filter(POWER_ADC1_MOD,POWER_ADC1_PIN,10);
			ad_value2 = adc_mean_filter(POWER_ADC2_MOD,POWER_ADC2_PIN,10);
			ad_value3 = adc_mean_filter(POWER_ADC3_MOD,POWER_ADC3_PIN,10);
			ad_value4 = adc_mean_filter(POWER_ADC4_MOD,POWER_ADC4_PIN,10);

			//差比和算法
			ad_value[i] = 100*(ad_value4 - ad_value1)/(ad_value4 + ad_value1);
		}
		
		for(j=0;j<9;j++)  														 //冒泡排序
		{
			for(i=0;i<9-j;i++)  
			{
				if(ad_value[i]>ad_value[i+1])  		
				{
					temp = ad_value[i]; 
					ad_value[i] = ad_value[i+1]; 
					ad_value[i+1] = temp; 
				} 
			}
		}
		
		for(i=1;i<9;i++)
		{
			sum_value += ad_value[i];
		}
		temp = sum_value/8;
		return temp;
}

//ips屏幕显示函数
void ips_show(void)
{
		ips200_showstr(0,1,"ad_value1=");           		//显示屏显示电感值
		ips200_showint16(80,1,ad_value1);
 
		ips200_showstr(0,2,"ad_value2=");
		ips200_showint16(80,2,ad_value2);

		ips200_showstr(0,3,"ad_value3=");
		ips200_showint16(80,3,ad_value3);

		ips200_showstr(0,4,"ad_value4=");
		ips200_showint16(80,4,ad_value4);

		ips200_showstr(0,5,"error=");
		ips200_showint16(80,5,error);

		ips200_showstr(0,6,"error_angle=");
		ips200_showint16(100,6,error_angle);
	
		ips200_showstr(0,7,"duty=");										//显示舵机转角
		ips200_showint16(80,7,duty);
	
		ips200_showstr(0,8,"motor1");
		ips200_showint16(80,8,motor1);
		
		ips200_showstr(0,9,"motor2");
		ips200_showint16(80,9,motor2);
}

//HIP电机驱动函数
void motor_ctr(int32 motor1, int32 motor2)
{
		if(motor1 > 0)
		{
			pwm_duty(PWM_0, motor1);
			pwm_duty(PWM_2, 0);
		}
		else
		{
			pwm_duty(PWM_0, 0);
			pwm_duty(PWM_2, -motor1);
		}
				
				
		if(motor2 > 0)
		{
			pwm_duty(PWM_1, motor2);
			pwm_duty(PWM_3, 0);
		}
		else
		{
		  pwm_duty(PWM_1, 0);
		  pwm_duty(PWM_3, -motor2);
		}
}

写在最后：本文为作者本人对第一次做智能车的回顾，也是自己第一次做车碰到的一些比较难理解的问题的整理，希望能帮助第一次做车的同学入门，只要大家实现了从0到1的跨越，那么从1到100就是水到渠成的。

如有错误和疑问，欢迎与作者私信交流指正

参考博客：
【嵌入式 · 单片机】一文带你搞懂电机驱动模块
【电磁循迹】从0到1
PWM原理 PWM频率与占空比详解
智能车电感差比和差加权算法研究