基于stm32的openmv爬坡PID小车

一、前言

小车采用hal库进行编程

在写爬坡小车之前我们进行很多论证

对于转向方法

方案一：采用四电机进行驱动，在转向的时候使用差速进行转向。

方案二：采用双电机进行后轮驱动，并使用舵机进行转向。

方案一在平地可以进行有效转向，可是在爬坡过程无法进行转向误差很大

对于循迹模块

方案一：采用红外寻迹模块，检测到黑线固定返回高低电平

方案二：采用openmv摄像头模块，采用色块识别与巡线算法，返回偏差值。

方案一在黑线较粗的时候精度尚可，在黑线较细的时候无法精准识别黑线，方案二在补光灯打光后，非常精准。

    经综上所述，我们选择方法二。

二、小车pid部分

PID算法调速，可以在不同摩擦，不同坡度的路面上（首先得保证车轮与路面不打滑）按照设定的期望速度行驶。

该设计的原理是，通过闭环控制系统控制小车的速度，再通过编码器A、B相测量小车行驶过程中的脉冲数，在固定时间内检测到的脉冲数的值经过计算得到电机的实际速度，然后将计算的速度与上一次测量的速度进行比较，采用PID算法控制单片机输出PWM脉冲到电机驱动模块PWM端，以此控制电机转速。

在本次中，我使用的是增量式pid，响应更加精准，可是会有偶尔爆冲现象

pid增量式代码

#include "Encoder.h"

int16_t ENcoder_cnt_A=0;
int16_t ENcoder_cnt_B=0;
uchar mess[50];
extern uint duty;
int speed_A;
int speed_B;
int PID_duty_B;
int PID_duty_A;
extern uint Encoder_Count;

struct
{
    float target_val;               //目标值
    float actual_val;        		//实际值
    float err;             			//定义偏差值
    float err_last;          		//定义上一个偏差值
    float err_prev;					//上上次误差
	float actual_val_last;
	float Kp,Ki,Kd;          		//定义比例、积分、微分系数
    float integral;          		//定义积分值
}pid_A;

struct
{
    float target_val;               //目标值
    float actual_val;        		//实际值
    float err;             			//定义偏差值
    float err_last;          		//定义上一个偏差值
    float err_prev;
	float actual_val_last;
	float Kp,Ki,Kd;          		//定义比例、积分、微分系数
    float integral;          		//定义积分值
}pid_B;

void PID_A_init()
{
	pid_A.Kp=20;
	pid_A.Ki=12.6;
	pid_A.Kd=6;
}

void PID_B_init()
{
	pid_B.Kp=20;
	pid_B.Ki=12.6;
	pid_B.Kd=5;
}

/*
增量式
*/
float PID_A_value(float target_val_A,float actual_val_A)//预估值，实际值
{
	pid_A.err=target_val_A-actual_val_A;//目标值和实际值的误差
	
//	if(abs(pid_A.err)<=3)
//	{
//		pid_A.actual_val=pid_A.actual_val_last;
//	}//死区控制
//	else
//	{
	pid_A.actual_val+=pid_A.Kp*(pid_A.err-pid_A.err_last)+pid_A.Ki*pid_A.err+pid_A.Kd*(pid_A.err-2*pid_A.err_last+pid_A.err_prev);//进行算法计算
//	}
	pid_A.err_prev=pid_A.err_last;
	pid_A.err_last=pid_A.err;//将误差进行传递,有两次误差

	return pid_A.actual_val;
}

float PID_B_value(float target_val_B,float actual_val_B)//预估值，实际值
{
	pid_B.err=target_val_B-actual_val_B;//目标值和实际值的误差
	
//	if(abs(pid_B.err) <= 3)
//	{
//		pid_B.actual_val=pid_B.actual_val_last;
//	}//死区控制
//	else
//	{
	pid_B.actual_val+=pid_B.Kp*(pid_B.err-pid_B.err_last)+pid_B.Ki*pid_B.err+pid_B.Kd*(pid_B.err-2*pid_B.err_last+pid_B.err_prev);//进行算法计算
//	}
	pid_B.err_prev=pid_B.err_last;
	pid_B.err_last=pid_B.err;//将误差进行传递,有两次误差
//	pid_B.actual_val_last=pid_B.actual_val;
	
	return pid_B.actual_val;
}

pid小车调速控制

 通过pid进行调速，旋转编码器控制速度的具体大小，并在转向的时候减少内轮的速度，增大外轮的速度（注意不要变化过大，可能会出现爆冲），50ms进入一次中断。

void Encoder_speed_get()
{//速度环,可以直接使用脉冲进行计算
	ENcoder_cnt_A=__HAL_TIM_GetCounter(&htim2);//获取采集了几次脉冲,A
	ENcoder_cnt_B=__HAL_TIM_GetCounter(&htim3);//B
	
	int speed_A=ENcoder_cnt_A*20*3.1415*65/330/4;
	int speed_B=ENcoder_cnt_B*20*3.1415*65/330/4;
	
	if(duty==0)
	{
		PID_duty_A=PID_A_value(1000/Encoder_Count,speed_A);
		PID_duty_B=PID_B_value(1000/Encoder_Count,speed_B);
	}
	else
	{
		PID_duty_A=PID_A_value(1000/Encoder_Count*0.70,speed_A);
		PID_duty_B=PID_B_value(1000/Encoder_Count*1.35,speed_B);
	}
	
	__HAL_TIM_SetCompare(&htim1,TIM_CHANNEL_2,PID_duty_A);//控制转速,左A
	__HAL_TIM_SetCompare(&htim1,TIM_CHANNEL_1,PID_duty_B);//控制转速,右B
	
	
	sprintf((char *)mess, "%d,%d,%d,%d,%d,%d,%d,%d\n", speed_A,speed_B,0,0,0,0,0,0);
	HAL_UART_Transmit(&huart2,(uchar *)mess,30,50);
	
	__HAL_TIM_SET_COUNTER(&htim2,0);
	__HAL_TIM_SET_COUNTER(&htim3,0);
}

三、小车循迹部分

小车采用openmv进行寻迹，并返回偏移值，由偏移值控制舵机进行转向，最后检测到黑线进行停止，由小车底下的两个红外寻迹模块进行控制。

#include "follow_motor.h"

uint duty;
int numbers[5];
int average_num;
uint view_duty;
extern int angle;
extern unsigned char buff[30];
extern int num;
extern uint view;

/*
摄像头寻迹
转弯模式两种小幅度修改 大幅度转弯
对num数据进行处理
5次进行一次取平均
*/
void follow_turn()
{
//	for(int i=0;i<5;i++)
//	{
//		num += numbers[i];
//	}
	average_num = num;
	
	if(-5 <= average_num && average_num <= 40) {
		duty=0;
    angle = 90;
	} else if(-10 < average_num && average_num < -5) {
		duty=0;
    angle = 0;
	} else if(40 < average_num && average_num < 50) {
		duty=0;
    angle = 120;
	}  
	else if(average_num < -10) {//当偏离角度大于40
//		HAL_TIM_PWM_Stop(&htim1,TIM_CHANNEL_2);
		duty=1;
    angle = 0;
	} else if(average_num > 50) {
		duty=0;
    angle = 120;
	}
}

/*
红外寻迹进行寻迹,检测到黑线返回1
直角弯进行大幅度修正
PB5右寻迹 PB10左寻迹
*/
void follow_o()
{
	if(HAL_GPIO_ReadPin(GPIOB,GPIO_PIN_5)==1&&HAL_GPIO_ReadPin(GPIOB,GPIO_PIN_10)==1)
	{
		HAL_TIM_PWM_Stop(&htim1,TIM_CHANNEL_2);
		HAL_TIM_PWM_Stop(&htim1,TIM_CHANNEL_1);
	}
}

四、主函数（部分）

中断回调函数

void HAL_TIM_PeriodElapsedCallback(TIM_HandleTypeDef *htim)
{
	if(htim->Instance==TIM4)//50毫秒进入一次中断
	{
		if(view==1)
		{
			Encoder_speed_get();
		}

	}
	
}

外部中断

采用外部中断进行控制，旋转编码器控制速度，并由按键启动

void HAL_GPIO_EXTI_Callback(uint16_t GPIO_Pin)
{
	if(GPIO_Pin == GPIO_PIN_0)
	{
		if(HAL_GPIO_ReadPin(GPIOB,GPIO_PIN_1) == GPIO_PIN_RESET)
		{
			--Encoder_Count;
			if(Encoder_Count<10)
			{Encoder_Count=20;}
		}
	}
	if(GPIO_Pin == GPIO_PIN_1)
	{
		if(HAL_GPIO_ReadPin(GPIOB,GPIO_PIN_0) == GPIO_PIN_RESET)
		{
			++Encoder_Count;
			if(Encoder_Count>20)
			{Encoder_Count=10;}
		}
	}
	if(GPIO_Pin == GPIO_PIN_3)
	{
		if(HAL_GPIO_ReadPin(GPIOB,GPIO_PIN_3)==0)
		{
			view=1;
		}
	}
}	

五、openmv模块

openmv采用Python进行编写，返回黑块的偏移量，从而确定车的位置，相对比红外循迹更加精准。

通过串口与单片机进行通信

import sensor, image, time
from pyb import UART

sensor.reset() # 初始化摄像头传感器。
sensor.set_pixformat(sensor.RGB565) # 使用RGB565格式。
sensor.set_framesize(sensor.QQVGA) # 使用QQVGA尺寸以提高速度。
sensor.skip_frames(10) # 等待新设置生效。
sensor.set_auto_whitebal(False) # 关闭自动白平衡。

clock = time.clock() # 用于跟踪FPS。
uart = UART(3, 115200)

green_threshold = (36, 0, -128, 127, -128, 127)

roil = [(0, 30, 160, 60)]

def find_max(blobs):
    max_size = 0
    max_blob = None
    for blob in blobs:
        if blob[2] * blob[3] > max_size and 70 < blob[2] * blob[3] < 150:
            max_blob = blob
            max_size = blob[2] * blob[3]
    return max_blob

while True:
    clock.tick() # 跟踪快照之间经过的毫秒数。
    img = sensor.snapshot() # 拍摄一张照片并返回图像。

    blobs = img.find_blobs([green_threshold], roi=roil[0])
    if blobs:
        max_blob = find_max(blobs)
        if max_blob is not None:
            x_error = max_blob[5] - img.width() / 2
            print("x error: ", x_error)
            uart.write(str(x_error).encode())
            uart.write("\r\n")
            img.draw_rectangle(max_blob[0:4]) # 绘制矩形
            img.draw_cross(max_blob[5], max_blob[6]) # 绘制交叉点
        else:
            uart.write("-40\r\n") # 向 UART 写入字符串 "-40"
    else:
        uart.write("No blobs found\r\n")
      
    img.draw_rectangle(roil[0], color=(255, 0, 0))
    
    time.sleep_ms(50)

结论

小车在0-30度可以平稳运行，在坡度过大可能会打滑现象，可以通过优化小车的配重进行调控

实物展示