基于STM32的麦克纳姆轮小车的寻迹实现
本项目用STM32F407,麦克纳姆轮小车,八路红外寻迹模块,实现小车的寻迹功能.本文着重解释了PID控制器,并给出仿真结果,意在理论中结合实践中更加深刻理解过程控制的原理.
麦克纳姆轮
麦克纳姆轮，是瑞典麦克纳姆公司设计。在中心轮圆周方向又布置了一圈独立的、倾斜角度（45°）的行星轮，这些成角度的行星轮把中心轮的前进速度分解成X和Y两个方向，实现前进及横行。麦克纳姆轮结构紧凑，运动灵活，是很成功的一种全方位轮。有4个这种新型轮子进行组合，可以更灵活方便的实现全方位移动功能。
麦克纳姆轮受力转动分析:
各个轮子的转动方向对小车运行方向的影响:
寻迹传感器
灰度传感器是模拟传感器。灰度传感器利用不同颜色的检测面对光的反射程度不同，光敏电阻对不同检测面返回的光其阻值也不同的原理进行颜色深浅检测。灰度传感器有一只发光二极管和一只光敏电阻，安装在同一面上。在有效的检测距离内，发光二极管发出白光，照射在检测面上，检测面反射部分光线，光敏电阻检测此光线的强度并将其转换为机器人可以识别的信号。
PID
过程控制:
对生产过程某一或某些物理参数进行自动控制。下图为基本模拟反馈控制回路:
被控量的值由传感器或变送器来检测，这个值与给定值进行比较，得到偏差，模拟调节器按照一定控制规律使操作变量变化，以使偏差趋近于零，其输出通过执行器作用于过程。
模拟PID调节器
PID调节器是一种线性调节器，它将给定的r(t)的值与实际输出的c(t)的偏差的比例（P）、积分（I）、微分（D）通过线性组合构成控制量，对控制对象进行控制。
PID的微分方程如下:
PID的传递函数如下:
PID调节各个单元的作用：
（1）、比例单元P:即时成比例地反应控制系统的偏差信号e(t)，偏差一旦产生，调节器立即产生控制作用以减小偏差。
（2）、积分单元I：主要用于消除静差，提高系统的无差度。积分作用的强弱取决于积分时间常数TI，TI越大，积分作用越弱，反之则越强。
（3）、微分单元D：能反应偏差信号的变化趋势（变化速率），并能在偏差信号的值变得太大之前，在系统中引入一个有效的早期修正信号，从而加快系统的动作速度，减小调节时间。
数字PID控制器
PID的差分方程:
PID的控制方式:
PID算法的两种形式
1. 位置式控制
2. 增量式控制
运动学分析
小车在运行过程中,可能会偏离中心线,此时需要对小车进行修正.这种修正有平动修正和旋转修正.一般情况下,我们小车的修正应该是平动修正和选装修正的叠加.
实例:小车向前运动,当小车偏离到中心线的左边,此时小车的速度变化应该往右平动同时顺时针旋转:
往右平动修正的运动分析:
往顺时针旋转修正的运动分析:
当往右修正和顺时针修正的幅度一样时,叠加得:
往左修正同理.
系统框图
程序实现
电机驱动程序
请参考上一篇文章<基于HAL库的STM32F4的直流电机控制>
红外寻迹传感器
STM32CubeMX配置引脚:
上图中,TS意为Track Sensor,有A和B两个八路寻迹传感器,KEY和LED引脚与本程序无关
Tracksensor.h头文件:
#ifndef _TRACKSENSOR_H
#define _TRACKSENSOR_H
#include
#include "main.h"
#include "myuart.h"
//定义引脚
#define TSA1 HAL_GPIO_ReadPin(TSA1_GPIO_Port,TSA1_Pin)
#define TSA2 HAL_GPIO_ReadPin(TSA2_GPIO_Port,TSA2_Pin)
#define TSA3 HAL_GPIO_ReadPin(TSA3_GPIO_Port,TSA3_Pin)
#define TSA4 HAL_GPIO_ReadPin(TSA4_GPIO_Port,TSA4_Pin)
#define TSA5 HAL_GPIO_ReadPin(TSA5_GPIO_Port,TSA5_Pin)
#define TSA6 HAL_GPIO_ReadPin(TSA6_GPIO_Port,TSA6_Pin)
#define TSA7 HAL_GPIO_ReadPin(TSA7_GPIO_Port,TSA7_Pin)
#define TSA8 HAL_GPIO_ReadPin(TSA8_GPIO_Port,TSA8_Pin)
//用于串口发送
extern u8 newLineArray[2];
//返回传感器误差
float trackErrForward(void);
//测试函数
void trackSensorTest(void);
#endif
Tracksensor.c源文件
#include "tracksensor.h"
//中间的红外传感器优先被响应,返回的偏差值根据寻迹传感器距离中线的位置决定,这里是//两边的寻迹传感器距离中线的距离,单位是mm
float trackErrForward(void)
{
float f;
if(TSA4)
f=-7;
else if(TSA5)
f=7;
else if(TSA3)
f=-25;
else if(TSA6)
f=25;
else if(TSA2)
f=-45;
else if(TSA7)
f=45;
else if(TSA1)
f=-68;
else if(TSA8)
f=68;
else
f=0;
return f;
}
PID实现
头文件pid.c
#ifndef _PID_H
#define _PID_H
//定义pid计算的结构体
typedef struct {
float target_value;
float current_value;
float err;
float last_err;
float last_last_err;
float Ki,Kp,Kd;
float output;
float integral;
} PID_Typedef;
//电机基本速度系数
#define SPEED_BASE 400
//不受PID控制的电机速度系数
#define SPEED_EX_SLOW 100
//受PID控制的电机速度系数
#define SPEED_EX_FAST 200
//PID初始化函数
void PID_Init(PID_Typedef *PID);
//PID计算
float getPID(PID_Typedef *PID,float value);
#endif
源文件pid.c
float GetPID(float value,PID_Typedef *PID)
{
PID->setValue = value;
PID->err = PID->setValue - PID->current_value;
PID->integral += PID->err;
PID->current_value = PID->Kp*PID->err + PID->Ki*PID->integral/2+ PID->Kd*(PID->err-PID->last_err);
PID->last_err = PID->err;
return PID->current_value;
}
主程序:
说明:此时平动分量和是旋转分量的三倍:
//
void CarForwardPID(void)
{
#define ERROR_COE_FORWARD 3.0f
#define TS_LIMITE_1_F 200
#define TS_LIMITE_2_F 300
setValue=TrackErrForward();
if(setValue<0.1f && setValue>-0.1f)
setValue=lastSetValue;
else
lastSetValue=setValue;
pidValue=GetPID(setValue,&PID_Car)/ERROR_COE_FORWARD;
if(pidValue<-TS_LIMITE_2)pidValue=-TS_LIMITE_2;
else if(pidValue>TS_LIMITE_2)pidValue=TS_LIMITE_2;
pidValue/=2.0f;
float t=pidValue/4;
//电机实际转速修正
ProMotorSpeed(car.speedBase+4*t,car.speedBase-4*t,car.speedBase+2*t,car.speedBase-2*t);
CarForwardSetFixedSpeed();
}
测试结果
小车:
运行结果: