基于STM32和PID算法实现小车循迹

通过这次功能的实现，我清楚的了解循迹传感器、驱动电机、PID算法控制的原理和使用，还有实现一个功能需要不断的调试，测试，同时也了解自己的不足，还是有很大的提升空间，加油，共勉。

文章共2,142字 · 阅读需要大约8分钟

一键AI生成摘要，助你高效阅读

问答

2301_79565212

6423人浏览 · 2023-12-14 23:02:59

2301_79565212 · 2023-12-14 23:02:59 发布

本文用的单片机是STM32F103C8T6，循迹模块是五路循迹传感器，电机是直流电机，电机驱动模块是L293D

效果展示

这里是没优化完成的效果，后续优化完成会呈上

视频演示：https://www.bilibili.com/video/BV1ka4y197B8/

基于STM32和PID控制算法实现循迹功能

五路循迹传感器

工作原理

五路循迹传感器的核心是五个感光二极管，通过感知地上的黑线，从而实现对小车运动轨迹的控制和调整。就是这么简单！

传感器的工作原理是通过五个感光电二极管来感知地面上的黑线。当小车运动到黑线上方时，这些感光电二极管会接收到反射回来的光线，从而变化电子状态并产生电压信号。这些信号经过放大和滤波处理后，成为高低电平信号，被输入到控制器中，控制器再通过算法来分析这些数据，从而判断小车的位置和方向，并调整运动轨迹，使小车按照预设路径行驶。

硬件

五路循迹模块

引脚

让我们看一下它的引脚排列。

GND 是连接到STM32的地。

VCC 是传感器的电源，我们连接了5V的供电。

OUT1~OUT5 是5个传感器的引脚，用于发送电平信号。

接线

将五路循迹模快连接到STM32。

五路循迹传感器	STM32
VCC	5V
GND	GND
OUT1	GPIO PA4
OUT2	GPIO PA5
OUT3	GPIO PA6
OUT4	GPIO PA7
OUT5	GPIO PB0

L293D电机驱动模块

L293D是一种基本的电机驱动器集成芯片（IC），它能够在任意方向驱动直流电机并控制惦记的速度。L293D是一个16引脚IC，每侧有8个引脚，可以很好的控制电机。

电路图

工作原理

L293D有4个方向控制输入引脚，IC左侧的引脚2、7（A2和A3）和右侧的引脚15、10（A0和A1）。左侧输入引脚调节连接在左端的电机旋转，右侧输入引脚调节右侧的电机。电机根据输入引脚上提供的高电平或低电平信号进行旋转。

例如：

引脚2 = 高，引脚7 = 低顺时针旋转
引脚2 = 低，引脚7= 高逆时针旋转
引脚2 = 低，引脚7 = 低无旋转
引脚2 = 高，引脚7= 高无旋转

右侧同理。

接线

本次接线因为有底板设计了电机驱动模快，我们只需将4个输入引脚接在核心板上。

PID控制算法

PID是比例（proportion）、积分（integral）、微分（derivative）是一种闭环控制算法，通过反馈值进行误差计算，然后通过PID算法控制负载设备。

公式

原理

在PID控制器中，比例环节的作用是对偏差瞬间做出反应。偏差一旦产生控制器立即产生控制作用，使控制量向减少偏差方向变化。控制作用的强弱取决于比例系数，比例系数越大，控制作用越强，则过渡越快，控制过程的静态偏差也就越小；但是越大，也越容易产生振荡，破坏系统的稳定性。故而，比例系数选择必须恰当，才能过渡时间短，稳定的效果。

积分，顾名思义就是一个累加的过程，积分调节的输出是与输入、输出反馈的误差的积分成正比关系，所以积分调节器的作用就是用来消除稳态误差。

为了加快调节过程，在偏差出现的瞬间，或在偏差变化的瞬间，不但要对偏差量做出立即响应，而且要根据偏差的变化趋势预先给出适当的纠正。

自动循迹思路

误差分析

约定左右轮速度调整：
left_motor_pwm_speed  =  left_motor_pwm_speed + pid_output
right_motor_pwm_speed =  right_motor_pwm_speed - pid_output

1.异常情况，小车向需要右偏（左轮速度增加，右轮减少）
case 0x19: cur_error = 2; break;   //B11001
case 0x1d: cur_error = 4; break;   //B11101
case 0x1c: cur_error = 6; break;   //B11100
case 0x18: cur_error = 7; break;   //B11000
case 0x1e: cur_error = 8; break;   //B11110

2.正常情况
case 0x0 :                          //B00000
case 0x1b: cur_error = 0;  break;   //B11011

3.异常情况，小车向需要左偏（右轮速度增加，左轮减少）
case 0x13: cur_error = -2;  break;   //B10011
case 0x17: cur_error = -3;  break;   //B10111
case 0x3 : cur_error = -4;  break;   //B00111
case 0x7 :                           //B00111
case 0x1 : cur_error = -7;  break;   //B00001
case 0xf : cur_error = -8;  break;   //B01111

4.极端情况，小车已经冲出赛道，根据冲出之前的误差，将误差调整到最大
case 0x1f: cur_error = pid.error > 0 ? 9 : -9; break;//B11111

五路循迹管电平读取状态代码

 

#define MOTOR_PWM_SPEED 500
#define MOTOR_PWM_MAX_SPEED 700
#define MOTOR_PWM_MIN_SPEED 0

#define PID_KP 25
#define PID_KI 0.8
#define PID_KD 15


typedef struct {
	int8_t error;
	int8_t last_error;
	int8_t kp;
	float ki;
	int8_t kd;
} pid_t;

bool pid_contorl = true;
pid_t pid;

int32_t left_motor_pwm_speed = MOTOR_PWM_SPEED;
int32_t right_motor_pwm_speed = MOTOR_PWM_SPEED;

//读取状态
uint8_t read_irs_state( void )
{
	int i;
	uint8_t status  = 0;
	uint8_t state[5];

	state[0]  = HAL_GPIO_ReadPin( GPIOA,GPIO_PIN_4 );
	state[1]  = HAL_GPIO_ReadPin( GPIOA,GPIO_PIN_5 );
	state[2]  = HAL_GPIO_ReadPin( GPIOA,GPIO_PIN_6 );
	state[3]  = HAL_GPIO_ReadPin( GPIOA,GPIO_PIN_7 );
	state[4]  = HAL_GPIO_ReadPin( GPIOB,GPIO_PIN_0 );
	

	for( i = 0; i < 5; i++ ) {
		status |=( state[i] << i );
	}

	return status;
}

采用中值滤波算法获取传感器状态

uint8_t get_middle_filter_irs_state( void )
{
	int i;
	uint8_t state[9];

	for( i =0; i<9; i++ ) {
		state[i] = read_irs_state();
	}

	printf( "irs:" );

	for( i =4; i>=0; i-- ) {
		if( state[4] & ( 1 << i ) ) {
			printf( "1" );

		} else {
			printf( "0" );
		}
	}

	printf( "\r\n" );

	return state[4];

}

根据传感器状态获取误差值

uint8_t calc_error_by_irs( uint8_t state )
{ 
  int8_t cur_error = pid.error;
	
	  switch( state ) {
	    
	    case 0x19: cur_error = 2; break;   //B11001
			case 0x1d: cur_error = 4; break;   //B11101
			case 0x1c: cur_error = 6; break;   //B11100
			//case 0x18: cur_error = 6; break;   //B11000
			case 0x18: cur_error = 7; break;   //B11000
	    case 0x1e: cur_error = 8; break;   //B11110
			case 0x0 :                          //B00000
			case 0x1b: cur_error = 0;  break;   //B11011
			case 0x13: cur_error = -2;  break;   //B10011
			case 0x17: cur_error = -4;  break;   //B10111
			//case 0x7 : cur_error = -5;  break;   //B00111
			case 0x3 : cur_error = -6;  break;   //B00111
			case 0x7 :                           //B00111
			case 0x1 : cur_error = -7;  break;   //B00001
			case 0xf : cur_error = -8;  break;   //B01111
	    case 0x1f: cur_error = pid.error > 0 ? 9 : -9; break;//B11111
	  }
  return cur_error;
}

采用平均数求和滤波算法获取当前的误差值

int8_t get_current_irs_error( void )
{
	int i;
	int sum =0;

	for( i = 0; i < 10; i++ ) {
		int8_t state = get_middle_filter_irs_state();
		int8_t error = calc_error_by_irs( state );
		sum += error;
	}

	return sum/10;
}

根据误差值用PID算法算出输出的控制量调整左右轮速度

int32_t pid_calc_output( void )
{
	int32_t P;
	static int32_t I;
	int32_t D;
	int32_t pid_output;

	pid.error = get_current_irs_error();
	P = pid.kp * pid.error;
	I +=pid.ki * pid.error;
	D = pid.kd * ( pid.error -pid.last_error );
	pid.last_error = pid.error;

	pid_output = P + I + D;
	return pid_output;

}

void update_motor_speed_by_pid( int32_t pid_output )
{
  if(!pid.error){
    left_motor_pwm_speed  = MOTOR_PWM_SPEED;
    right_motor_pwm_speed = MOTOR_PWM_SPEED;
    return;
  }
  
	left_motor_pwm_speed =  left_motor_pwm_speed + pid_output;
	if( left_motor_pwm_speed <=MOTOR_PWM_MIN_SPEED ) {
		left_motor_pwm_speed = MOTOR_PWM_MIN_SPEED;

	} else if( left_motor_pwm_speed >= MOTOR_PWM_MAX_SPEED ) {
		left_motor_pwm_speed = MOTOR_PWM_MAX_SPEED;
	}

	right_motor_pwm_speed =  right_motor_pwm_speed - pid_output;

	if( right_motor_pwm_speed <=MOTOR_PWM_MIN_SPEED ) {
		right_motor_pwm_speed = MOTOR_PWM_MIN_SPEED;

	} else if( right_motor_pwm_speed >= MOTOR_PWM_MAX_SPEED ) {
		right_motor_pwm_speed = MOTOR_PWM_MAX_SPEED;
	}

	return;
}

PID参数初始化

#define PID_KP 5
#define PID_KI 0.01
#define PID_KD 10

void pid_init( void )
{
	pid.error = 0;
	pid.last_error = 0;
	pid.kp = PID_KP;
	pid.ki = PID_KI;
	pid.kd = PID_KD;

	return;
}

根据PID算法控制小车

#define MOTOR_PWM_SPEED 500
#define MOTOR_PWM_MAX_SPEED 700
#define MOTOR_PWM_MIN_SPEED 0

void pid_control_motor( void )
{
  if(pid.error >= 7 && pid.error <= 9){
    car_turn_right(MOTOR_PWM_MAX_SPEED);
    return;
  }else if(pid.error >= -9 && pid.error <= -7){
    car_turn_left(MOTOR_PWM_MAX_SPEED);
    return;
  }
	left_motor_forward( left_motor_pwm_speed );
	right_motor_forward( right_motor_pwm_speed );
}

void pid_control_car( void )
{
	int32_t pid_output;
	pid_init();

	while( pid_contorl ) {
		pid_output = pid_calc_output();
		update_motor_speed_by_pid( pid_output );
		pid_control_motor();
		HAL_Delay( 10 );
	}

	return;
}