代码收藏家 STM32F103智能循迹小车方案：舵机、双电机和灰度传感器
2020年大三上的时候和同学们一起组队参加了学校举办的机器人大赛，走的是循迹竞速赛道，规则很简单，就是看谁可以以最快的速度跑完全程，经过一个多月的学习与调试，最终我们的小车“德芙”（因为全程跑的十分丝滑）以26s的成绩（领先第二名7s）获得了第一，在此就写一篇博客记录记录自己调车的经历吧。
我们队在比赛中使用的是舵机加双编码电机的机械结构，以芯片主频为72MHz的STM32F103ZET6为核心控制器，赛道的主要元素包括直道，环岛，S弯和连续直角弯； 我们通过一字排列的光电传感器阵列对黑线进行识别，进而检测车身在赛道上的位置；通过编码电机检测智能车的实时速度；使用结合BANGBANG算法的积分分离PI控制算法调节电机的转速，通过基于分段控制思想的舵机转向PD控制舵机的打角，通过基础速度和偏离速度相结合的思想实现针对不同赛道元素的速度调节，实现了智能车在运动过程中速度和方向的闭环控制。
一、路径识别模块的设计
在选择传感器的时候，我们最初选择的是TCRT5000红外反射式开关传感器，但是发现因为赛道和车子结构的原因，其实际的循迹效果不是特别好，经常会误判，故而我们便改用了数字灰度传感器，其灵敏度更高，抗干扰能力强；普通照明灯基本对其无影响，其发光源常用高度良白色聚光LED,接收管对不同的发射光的强弱进行对比处理，只要对白光发射强弱不同即可，差值越大，分辨越好，比普通的红外传感器抗干扰能力要强的多。
其输出为数字输出，即1或0输出，需要根据场地，光线等基本情况来调节基准电压，电压比较器有着两个电压输入，一个为接受管的电压，另一个是电位器输入的基准电压，需要根据接受管在2种色的电压值来调节基准电压，一般将电位器电压调到2种电压的中间值。
我们将七个传感器按一字型非等距排列在超前于车身主体的横杆上，正中间的传感器在正常情况下正对于赛道的黑线，两边的传感器则按间隔距离则由小到大对称分布排列，用以确定小车与中心线的偏差大小，我们根据各个传感器返回的01值来确定小车与中心线偏差的大小，同时我们将偏差的大小分为9级，并由参数error来表征，0代表小车循迹无偏差，不需要进行转弯，正数代表左转，负数代表右转，而数字越大代表小车偏离中心的程度越大。对于十字圆环而言，所有的传感器均会检测到黑线，此时应题目要求是向前走直道，故而将error设定为0。当小车超过终止区域黑线时，所有的传感器都检测不到黑线，此时比赛完成，故而小车要停止，此时令两个电机反转，消除其惯性，使其迅速停下来，当检测到实际速度为 0 时则断开电源，实现制动的同时避免反接制动运行情况发生。
二、舵机
该循迹系统选用的是HS-425BB型舵机，HS-425BB型舵机是一款高性能舵机，扭力大，稳定性好，角度控制准确。由于舵机所需要的电压较大，运行过程中电流也会发生很大变化，还会带来不同程度的干扰，因此，舵机往往采用单独供电，舵机的输入线有三根，其中红色的线用于接电源，黑色的线用于接地，白色的线为信号线，对舵机的控制过程比较简单，提供主控制器产生的PWM控制信号输出至舵机的信号线接口，在输出PWM波频率不变的情况下，通过改变PWM波的占空比，就能相应地改变机器人的转角，从而实现舵机的转角控制。
#include "Servo.h"
#include "sys.h"
//舵机初始化
void TIM3_PWM_Servo_Init(u16 arr,u16 psc)
{
GPIO_InitTypeDef GPIO_InitStructure;
TIM_TimeBaseInitTypeDef TIM_TimeBaseStructure;
TIM_OCInitTypeDef TIM_OCInitStructure;
RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM3, ENABLE);
RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOB , ENABLE);
RCC_APB2PeriphClockCmd(RCC_APB2Periph_AFIO, ENABLE);
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_1; //TIM5_CH2
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF_PP;
GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
GPIO_Init(GPIOB, &GPIO_InitStructure);
//TIM2
TIM_TimeBaseStructure.TIM_Period = arr;
TIM_TimeBaseStructure.TIM_Prescaler =psc;
TIM_TimeBaseStructure.TIM_ClockDivision = 0;
TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up;
TIM_TimeBaseInit(TIM3, &TIM_TimeBaseStructure);
TIM_ARRPreloadConfig(TIM3, ENABLE);
//TIM3 Channel PWM
TIM_OCInitStructure.TIM_OCMode = TIM_OCMode_PWM1;
TIM_OCInitStructure.TIM_OutputState = TIM_OutputState_Enable;
TIM_OCInitStructure.TIM_Pulse = 0;
TIM_OCInitStructure.TIM_OCPolarity = TIM_OCPolarity_High;
TIM_OC4Init(TIM3, &TIM_OCInitStructure);
TIM_OC4PreloadConfig(TIM3, TIM_OCPreload_Enable);
TIM_Cmd(TIM3, ENABLE);
}
void Servo_angle(int pwm)
{
TIM_SetCompare4(TIM3,pwm);
}三、电机
在该系统的设计中，我们选择了GB37-520编码电机。电机编码器由光电模块和光栅组成，光电模块输出的信号有AB两相。电机的主轴连接着编码器并带动编码器的光栅盘转动，光电模块检测其输出的脉冲数。由于AB两相相差90°，可通过比较A相在前还是B相在前，以判别编码器的正转与反转，通过零位脉冲，可获得编码器的零位参考位；绝大部分的直流电机采用的是开关驱动方式，采用这种方式，半导体功率器件有开启和关闭两种状态，然后用输出可调的PWM电平实现对电机电压的控制，实现调速功能。
extern int Motor_1,Motor_2;
//GPIO 初始化
void MOTOR_GPIO_Init(void)
{
GPIO_InitTypeDef GPIO_InitStructure;
RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOB , ENABLE);
RCC_APB2PeriphClockCmd(RCC_APB2Periph_AFIO, ENABLE);
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_6|GPIO_Pin_7 |GPIO_Pin_8|GPIO_Pin_9;
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF_PP;
GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
GPIO_Init(GPIOB, &GPIO_InitStructure);
}
//电机PWM定时器输出初始化
//PWM频率= 定时器模块频率/ (Period+1) / (Prescaler+1)
void TIM4_PWM_Motor_Init(u16 arr,u16 psc)
{
TIM_TimeBaseInitTypeDef TIM_TimeBaseStructure;
TIM_OCInitTypeDef TIM_OCInitStructure;
RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM4 , ENABLE);
TIM_TimeBaseStructure.TIM_Period = arr;
TIM_TimeBaseStructure.TIM_Prescaler =psc;
TIM_TimeBaseStructure.TIM_ClockDivision = 0;
TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up;
TIM_TimeBaseInit(TIM4, &TIM_TimeBaseStructure);
TIM_ARRPreloadConfig(TIM4, ENABLE);
TIM_OCInitStructure.TIM_OCMode = TIM_OCMode_PWM1;
TIM_OCInitStructure.TIM_OutputState = TIM_OutputState_Enable;
TIM_OCInitStructure.TIM_Pulse = 0;
TIM_OCInitStructure.TIM_OCPolarity = TIM_OCPolarity_High;
TIM_OC1Init(TIM4, &TIM_OCInitStructure);
TIM_OC1PreloadConfig(TIM4, TIM_OCPreload_Enable);
TIM_OC2Init(TIM4, &TIM_OCInitStructure);
TIM_OC2PreloadConfig(TIM4, TIM_OCPreload_Enable);
TIM_OC3Init(TIM4, &TIM_OCInitStructure);
TIM_OC3PreloadConfig(TIM4, TIM_OCPreload_Enable);
TIM_OC4Init(TIM4, &TIM_OCInitStructure);
TIM_OC4PreloadConfig(TIM4, TIM_OCPreload_Enable);
TIM_Cmd(TIM4, ENABLE);
}
//幅值给PWM寄存器，左轮PWM
void Set_Pwm_Motor1(int motor_1)
{
if(motor_1<0)
{
TIM_SetCompare1(TIM4,7200+motor_1);
TIM_SetCompare2(TIM4,7200);
}
else
{
TIM_SetCompare1(TIM4,7200);
TIM_SetCompare2(TIM4,7200-motor_1);
}
}
//轮子反转
void Motor_back(void)
{
Set_Pwm_Motor2(-2000);
Set_Pwm_Motor1(-2000);
}
//测试电机
void MOTOR_TEST(void)
{
Set_Pwm_Motor2(7000);
Set_Pwm_Motor1(-7000);
}
四，PID算法控制
PID控制是工程实际中应用最为广泛的调节器控制规律，单位反馈的PID控制原理框图如图：
e(t)代表理想输入与实际输入的误差，这个误差信号被送到控制器，控制器算出误差信号的积分值和微分值，并将它们与原误差信号进行线性组合，得到输出量。
其中，kp、kd、ki分别为比例系数、积分系数、微分系数。
PID各个参数作用基本介绍：
（1）比例调节(p)：是按比例反应系统的偏差，系统一旦出现了偏差，比例调节立即产生调节作用，以减少偏差。比例作用大，可以加快调节，减少误差，但是过大的比例，会使系统稳定性下降，甚至造成系统的不稳定。
（2）积分调节(I)：使系统消除稳态误差，提高无差度，因为有误差，积分调节就进行，直至无差，积分调节停止，积分调节输出一个常值。
（3）微分调节(D)：微分作用反映系统偏差信号的变化率，具有预见性，能预见偏差变化的趋势，因此能产生超前的控制作用，在偏差还没有形成之前，已被微分调节作用消除。因此，可以改善系统的动态性能。在微分时间选择合适情况下，可以减少超调，减少调节时间。微分作用对噪声干扰有放大作用，因此过强的微分调节，对系统抗干扰不利。此外，微分反应的是变化率，而当输入没有变化时，微分作用输出为零。微分作用不能单独使用，需要与另外两种调节规律相结合，组成 PD 或 PID 控制器。
以 T 作为采样周期，则离散采样时间对应着连续时间，用矩形法数值积分近似代替积分，用一阶向后差分近似代替微分，可得到离散 PID公式为：
位置式 PID 算法具有以下优点：比例部分只与当前的偏差有关，而积分部分则是系统过去所有偏差的累积。故而位置式 PI 调节器的结构清晰，P 和 I 两部分作用分明，参数调整简单明了，编写代码更为简单。
速度策略是影响智能车速度快慢的一个重要因素。首先速度控制必须配合方向控制，即在直道或类似直道时，应该将PWM波提到最高，让通过主电机的电流达到最大，从而让智能车以最快的速度行驶；当进入弯道时，应该根据弯道的曲率大小，适当的较低小车的基础速度，使得小车不容易冲出赛道，对于该基础速度v_set的设定，我们采用PD控制，以小车与中线的偏差大小为输入，以速度的设定量为输出，实现不同赛道类型下的自主速度调整。
我们的小车转弯方式不同于其他万向轮转弯的智能小车，我们采用差速转向和舵机转向方式相配合，相比之下其灵活性提高了很多。小车利用差速电机实现转弯，其原理是利用两个电机的转速不同构成转速差；舵机控制前轮是从动轮，不需要有转速。舵机的位置是在两个前轮的中间，控制两个前轮的转向。
void TIM6_IRQHandler(void) //TIM6中断服务函数
{
if(TIM6->SR&0X0001)//定时器每10ms定时中断一次
{
TIM6->SR&=~(1<<0);
if(backconfig==0)
{
servo_pid_adjust=Servo_Position_PID (Position_error,0); //将PID的输出值赋给舵机
motor_basic_pid=Motor_basic_Position_PD(Basic_Speed_error,0); //将PID控制器的输出赋给基础速度
motor_different_pid=Motor_different_Position_PID(Different_Speed_error,0);//将PID控制器的的输出值赋给偏差速度
Pwm_limit(); //将PWM进行限幅，避免角度或速度过大，超过范围
Poistion_adjust(); //输出PWM给相应的管脚
}
else if(backconfig==1)
{
backconfig=0;
}
}
}
//舵机PID控制器Encoder=Position_error£¬Target=0;
int Servo_Position_PID (int Encoder,int Target)
{
float Position_KP=45,Position_KI=0,Position_KD=35;
//angle=KP*Position_error*0.135
static float Bias,Pwm,Integral_bias,Last_Bias;
Bias=Encoder-Target;
Integral_bias+=Bias;
Pwm=Position_KP*Bias+Position_KI*Integral_bias+Position_KD*(Bias-Last_Bias);
Last_Bias=Bias;
return Pwm;
}
//基础速度PID控制器Encoder=Basic_Speed_error£¬Target=0;
int Motor_basic_Position_PD (int Encoder,int Target)
{
float Position_KP=300 ,Position_KD=0;
static float Bias,Pwm,Last_Bias;
Bias=Encoder-Target; //Æ«²î=ʵ¼ÊÖµ-Ä¿±êÖµ
Pwm=7200-Position_KP*Bias+Position_KD*(Bias-Last_Bias);
Last_Bias=Bias;
return Pwm;
}
//差速PID控制器Encoder=Different_Speed_error£¬Target=0;
int Motor_different_Position_PID (int Encoder,int Target)
{
float Position_KP=650,Position_KI=0,Position_KD=15000;
static float Bias,Pwm,Integral_bias,Last_Bias;
Bias=Encoder-Target;
Integral_bias+=Bias;
Pwm=Position_KP*Bias+Position_KI*Integral_bias+Position_KD*(Bias-Last_Bias);
Last_Bias=Bias;
return Pwm;
}
//输出舵机和电机的PWM
void Poistion_adjust(void)
{
Servo_angle(servo_output);
Set_Pwm_Motor1(motor1_output);
Set_Pwm_Motor2(motor2_output);
}
//限制舵机和电机的PWM
void Pwm_limit(void)
{
servo_output=1500-servo_pid_adjust;
motor2_output=5300-motor_different_pid;
motor1_output=5300+motor_different_pid;
if(servo_output>2000) servo_output=2000;
if(servo_output<1000) servo_output=1000;
if( motor1_output>7200) motor1_output=7200;
if( motor1_output<-7200) motor1_output=-7200;
if( motor2_output>7200) motor2_output=7200;
if( motor2_output<-7200) motor2_output=-7200;
}整个比赛过程，从最开始的选择车型和主控芯片到后来的编程和现场实际的调车，可以说那段时间一有空，我们就呆在调试赛道旁边，不断改善程序，同时因为我们是舵机和差速控制的方案，故而pid的参数较多，我们需要一个一个的慢慢整定，一遍遍的跑去确定一个合适的参数；当时的我们参数整定全靠自己的判断和车子跑的情况，故而效率其实还是很低的，如果可以的话，其实可以运用无线模块将每一次车子的PWM数据实时的传送到电脑上，画出相应的曲线，这样的话调试会快一点，还有就是最后因为时间等缘故，我们的速度到最后其实没有闭环，即没有用编码电机将实际的速度值传送回来，这样输出的PWM和实际的还是会有一定的区别的，这个以后也可以加上，如果有机会的话，其实我还是想去参加一下飞思卡尔智能车比赛的，再享受一次调车的乐趣，享受看着小车一次又一次以更快的速度跑完全程的那种喜悦！
