基于STM32F103的智能平衡车

主要实现基于STM32F103C8T6的HAL库编程PID平衡车项目。

实物

硬件部分

Layout布局

STM32 CubeMX配置

使用外部高速时钟8MHz，通过PLL倍频到72MHz

RCC---->HSE、LSE = Crystal/Ceramic Resonator（晶振）---->HCLK = 72MHz

OLED

屏的大小为0.96寸，像素点为128*64。

4PIN分别为GND、VCC(3.3V/5V)、SCL(IIC的时钟信号)、SDA(IIC的数据总线)。

名称	IO	HAL配置
I2C1_SDA	PB9	I2C1_SCL,OD模式
I2C1_SCL	PB8	I2C1_SDA,OD模式

IIC

硬件IIC：上拉输入，开漏输出。(直接用STM32的真实外设)

软件IIC：上拉输入，推挽/开漏输出。（GPIO实现时序）

IIC支持一主多从，同步，半双工，每个从机都有其设备地址。

起始信号：SCL高电平，SDA从高电平跳到低电平

void IIC_Start(void)
{
    /* 当SCL高电平时，SDA出现一个下跳沿表示IIC总线启动信号 */
    IIC_SDA_1();
    IIC_SCL_1();
    IIC_Delay();
    IIC_SDA_0();
    IIC_Delay();
    IIC_SCL_0();
    IIC_Delay();
}

停止信号：SCL高电平，SDA从低电平跳到高电平

void IIC_Stop(void)
{
    /* 当SCL高电平时，SDA出现一个上跳沿表示IIC总线停止信号 */
    IIC_SCL_0();
    IIC_SDA_0();
    IIC_SCL_1();
    IIC_Delay();
    IIC_SDA_1();
}

ACK信号：SCL高电平，SDA为低（ACK），SDA为高（NACK）

void IIC_Ack(void)
{
    IIC_SDA_0();	/* CPU驱动SDA = 0 */
    IIC_Delay();
    IIC_SCL_1();	/* CPU产生1个时钟 */
    IIC_Delay();
    IIC_SCL_0();
    IIC_Delay();
    IIC_SDA_1();	/* CPU释放SDA总线 */
}
void IIC_NAck(void)
{
    IIC_SDA_1();	/* CPU驱动SDA = 1 */
    IIC_Delay();
    IIC_SCL_1();	/* CPU产生1个时钟 */
    IIC_Delay();
    IIC_SCL_0();
    IIC_Delay();
}

发送一个Byte：

void IIC_Send_Byte(uint8_t Byte)
{
    uint8_t i;

    for (i = 0; i < 8; i++)
    {
        if (Byte & 0x80)
        {
            IIC_SDA_1();
        }
        else
        {
            IIC_SDA_0();
        }
        IIC_Delay();
        IIC_SCL_1();
        IIC_Delay();
        IIC_SCL_0();
        if (i == 7)
        {
            IIC_SDA_1();
        }
        Byte <<= 1;
        IIC_Delay();
    }
}

读取一个Byte：

uint8_t IIC_Read_Byte(uint8_t ack)
{
    uint8_t i, value;

    value = 0;
    for (i = 0; i < 8; i++)
    {
        value <<= 1;
        IIC_SCL_1();
        IIC_Delay();
        if (IIC_SDA_READ())
        {
            value++;
        }
        IIC_SCL_0();
        IIC_Delay();
    }
    if(ack==0)
        IIC_NAck();
    else
        IIC_Ack();
    return value;
}

取模软件（PCtoLCD）

设置：“宋体”， 16 * 16，阴码，列行式，逆向，再修改前缀。

MPU6050

MPU6050传感器可以同时检测出三轴加速度、三轴角速度以及温度数据，内部集成DMP（Digital Motion Processor数字运动处理器）模块，可以实现滤波、融合处理。

绕IMU的Z轴旋转：偏航角yaw

绕IMU的Y轴旋转：俯仰角pitch

绕IMU的X轴旋转：横滚角roll

通过IIC通信

名称	IO	HAL配置
SDA	PB3	GPIO_OUTPUT
SCL	PB4	GPIO_OUTPUT
INT	PB5	GPIO_EXIT5

float pitch, roll, yaw;
uint8_t display_buf[20];
mpu_dmp_get_data(&pitch, &roll, &yaw);
sprintf((char*)display_buf, "row:%.2f",row);

TIP:MPU6050内部有上拉电阻。

MPU6050模块如果在最开始没有平稳放置，自检测后会进入return语句，导致初始化失效；注释掉其自检后的return，可临时解决问题

超声波

HC-SR04 超声波测距模块可提供2cm-400cm的非接触式距离感测功能，精度可达到3mm，模块包括超声波发射器、接收器和控制电路。

工作原理

名称	IO	HAL配置
TRIG（输入触发，测距）	PA3	GPIO_OUTPUT
ECHO（传回信号、计算时间差）	PA2	GPIO_EXTI2(外部中断2)，TIM3计数

TIP：为什么上升沿和下降沿都中断（上升时开始计时，下降时结束计时）。

$$Internal\_Clock=\frac{HCLK}{PSC + 1}$$

NVIC---->EXTI line2 interrupt = ENABLE

TIM3---->PSC = 71，1MHz = 1us

使用的自动重载器ARR是16位，因此65535us=0.065535s，乘上340m/s / 2可得最远距离超过4m。

//1、IO口TRIG触发测距，给最少10us的高电平信呈
//2、模块自动发送8个40KHz的方波，自动检测信号是否返回
//3、有信号返回，通过IO口ECHO输出一个高电平，高电平持续的时间是超声波从发送到返回的时间
//测试距离 = (高电平时间 * 声速 (340m/s) / 2
//测量周期 > 60ms，防止上一个超声波与现在的有干扰

uint16_t count;
float distance;
extern TIM_HandleTypeDef htim3;

//触发信号，获取距离
void Get_Dist(void)
{
	HAL_GPIO_WritePin(GPIOA, GPIO_PIN_3, GPIO_PIN_SET);			//设置Trig开启
	RCCdelay_us(12);
	HAL_GPIO_WritePin(GPIOA, GPIO_PIN_3, GPIO_PIN_RESET);		//设置Trig关闭
}
//外部中断2和中断5函数重写，通过PIN口判断
void HAL_GPIO_EXTI_Callback(uint16_t GPIO_Pin)
{
	if(GPIO_Pin == GPIO_PIN_5)
	{
		Control();
	}
	if(GPIO_Pin == GPIO_PIN_2)									//此为超声波接收到信号
	{
		if(HAL_GPIO_ReadPin(GPIOA, GPIO_PIN_2)==GPIO_PIN_SET)	//Trig开启
		{
			__HAL_TIM_SetCounter(&htim3, 0);					//设置htim3定时器值为0
			HAL_TIM_Base_Start(&htim3);							//开启htim3定时器
		}
		else													//Trig关闭
		{
			HAL_TIM_Base_Stop(&htim3);							//停止htim3定时器
			count = __HAL_TIM_GetCounter(&htim3);				//获取其值
			distance = count * 0.017;							//计算距离
		}
	}
}

//计时函数
void RCCdelay_us(uint32_t udelay)
{
	__IO uint32_t Delay = udelay * 72 / 8;
	do
	{
		__NOP();
	}
	while(Delay--);
}

电机驱动

TB6612电机

IN1	IN2	直流电机的状态
0	0	制动
0	1	正转
1	0	反转
1	1	制动

PWM

全称Pulse Width Modulation(脉宽调制)；实质是在一个方波中，高电平的占比。

名称	IO	HAL配置
PWMA	PA11	TIM1_CH4，Pulse = 7200
AIN2	PB12	GPIO_OUTPUT
AIN1	PB13	GPIO_OUTPUT
BIN1	PB14	GPIO_OUTPUT
BIN2	PB15	GPIO_OUTPUT
PWMB	PA8	TIM1_CH1，Pulse = 7200

$$\\ PWM的频率f = \frac{HCLK}{(PSC + 1)(ARR + 1)}\\ \\ 占空比Duty=\frac{Pulse}{ARR + 1}\\ \\ f=5KHz，HCLK=72MHz，则PSC=1,ARR=7199 \\ \\$$

实现函数

// 占空比设置
#define PWM_MAX 7200
#define PWM_MIN -7200

void Load(int motorL, int motorR)			//-7200 ---- 7200
{
	if(motorL < 0)
	{
		HAL_GPIO_WritePin(GPIOB, GPIO_PIN_13, GPIO_PIN_SET);
		HAL_GPIO_WritePin(GPIOB, GPIO_PIN_12, GPIO_PIN_RESET);
	}
	else
	{
		HAL_GPIO_WritePin(GPIOB, GPIO_PIN_13, GPIO_PIN_RESET);
		HAL_GPIO_WritePin(GPIOB, GPIO_PIN_12, GPIO_PIN_SET);
	}
	__HAL_TIM_SetCompare(&htim1, TIM_CHANNEL_4, abs(motorL));		//加载占空比
	if(motorR < 0)
	{
		HAL_GPIO_WritePin(GPIOB, GPIO_PIN_14, GPIO_PIN_SET);
		HAL_GPIO_WritePin(GPIOB, GPIO_PIN_15, GPIO_PIN_RESET);
	}
	else
	{
		HAL_GPIO_WritePin(GPIOB, GPIO_PIN_14, GPIO_PIN_RESET);
		HAL_GPIO_WritePin(GPIOB, GPIO_PIN_15, GPIO_PIN_SET);
	}
	__HAL_TIM_SetCompare(&htim1, TIM_CHANNEL_1, abs(motorL));		//加载占空比
}
// 需要限幅，防止跑飞
void Limit(int *Motor1, int* Motor2)
{
	if(*Motor1 > PWM_MAX) *Motor1 = PWM_MAX;
	if(*Motor1 < PWM_MIN) *Motor1 = PWM_MIN;
	if(*Motor2 > PWM_MAX) *Motor2 = PWM_MAX;
	if(*Motor2 < PWM_MIN) *Motor2 = PWM_MIN;
}

编码器

速度通过脉冲波的方式测量，旋转一圈11个脉冲；两个霍尔传感器A相、B相，可以判断旋转方向。A相领先B相，即顺时针

名称	IO	HAL配置
编码器1	PA0、PA1	TIM2—Encoder Mode TI1 and TI2
编码器2	PB6、PB7	TIM4—Encoder Mode TI1 and TI2

Encoder Mode TI1只计算A相上升沿

Encoder Mode TI2只计算B相上升沿

Encoder Mode TI1 and TI2上升沿就计算，计数精度会更加准确

int Read_Speed(TIM_HandleTypeDef* htim)
{
	unsigned int tmp;
	// 
	tmp = (short)__HAL_TIM_GetCounter(htim);
	__HAL_TIM_SetCounter(htim, 0);
	return tmp;
}
// 用系统自带uwTick（1ms自加）每10ms读取数值
void Read(void)
{
	if(uwTick-sys_tic<10)			
		return;
	sys_tic=uwTick;
	Encoder_Left = Read_Speed(&htim2);					//读取htim2的值
	Encoder_Right = -Read_Speed(&htim4);				//读取htim4的值
}

TIP：编码器输出的脉冲计数本身是无符号的，此时正转时数值递增，反转时数值递减。

蓝牙

使用JDY-31蓝牙模块（从机），通过蓝牙转串口通信。

名称	IO	HAL配置
RXD	PB10	USART3_TX
TXD	PB11	USART3_RX

USART3设置MODE = Asynchronous(异步)，波特率为9600Bits/s，8位，无校验位，停止位1位

USART3 global interrupt = ENABLE

extern uint8_t rx_buf[2];
HAL_UART_Receive_IT(&huart3, rx_buf, 1);
// USART3中断函数，小端存储
void USART3_IRQHandler(void)
{
	HAL_UART_IRQHandler(&huart3);
	Bluetooth_data = rx_buf[0];
	if(Bluetooth_data == 0x00) 		Fore=0,Back=0,Left=0,Right=0;
	else if(Bluetooth_data == 0x01) Fore=1,Back=0,Left=0,Right=0;
	else if(Bluetooth_data == 0x05) Fore=0,Back=1,Left=0,Right=0;
	else if(Bluetooth_data == 0x03) Fore=0,Back=0,Left=0,Right=1;
	else if(Bluetooth_data == 0x07) Fore=0,Back=0,Left=1,Right=0;
	else														Fore=0,Back=0,Left=0,Right=0;
	
	//HAL_UART_Transmit(&huart3, rx_buf, 1, 1000);
	HAL_UART_Receive_IT(&huart3, rx_buf, 1);
  /* USER CODE END USART3_IRQn 1 */
}

PID

原理

本项目使用串级PID，速度环PI+直立环PD。

$$\theta为当前小车的倾角roll+机械中值 \\ \theta^{'}为倾角微分，即角速度gyro\_x \\ 假如速度环目标角度Med为\theta_1，作为参数输入到直立环，\\ 则直立环的输出a将直接作用于电机，有如下关系式。\\ 直立环输出 a= kp * (\theta - \theta_1) + kd *\theta^{'} \\ \\ e(k)是速度环中目标速度与当前速度的偏差 \\ \sum{e(k)}为偏差的积分项。\\ 速度环输出\theta_1 = kp_1 * e(k) + ki_1 * \sum{e(k)}\\ \\ 因此a= kp * \theta + kd *\theta^{'} - kp * [kp_1 * e(k) + ki_1 * \sum{e(k)}] \\$$

调整P、I、D的效果

比例P需要看什么时候跌倒，主要依靠倾斜角度。

微分D需要看什么时候振荡，若KD过大，则会振荡地严重。其效果就是阻尼，越大越慢。

积分I需要看什么情况下平衡地快，其主要用于消除稳态误差，提高控制精度。

调整过程

1、先确定机械中值，通过它来中和计算出的theta，这个需要自己手动测量。

2、调参P、I、D

确定极性：设置kp为正，向前倾斜时，平衡车轮子速度加快，则极性正确。

直立环PD

直立环直接调用公式。（输入期望角度、真实角度和角速度）

当然还需要看下我们希望平衡车在什么角度下能够返回，以此保持平衡，KP计算(7200 / 30)，这里的30是角度；通过将KP置0后，得到此时的gyro_x（数值为2000左右），再进行KD计算（7200/2000）

double Vertical_Kp = -240, Vertical_Kd = -3.6;
//不滤波的原因是MPU6050滤过了。
int Vertical_PD(float Med, float Angle, float gyro_x)
{
	int tmp = Vertical_Kp * (Angle - Med) + Vertical_Kd * gyro_x;
	return tmp;
}

速度环PI

（输入期望速度、左编码器、右编码器）

1、计算偏差值：误差值 = （左+右）- 期望速度

2、低通滤波：误差A = (1-a)×偏差值 + a×上一次的偏差值，再更新上一次的偏差值

3、积分：Encoder_S += 误差A （STM32是离散的数字信号，求积分就是求和）

4、限幅Encoder_S

5、速度环套用公式

//速度环PI控制器
//输入：期望速度、真实速度（左编码、右编码）
int Velocity_PI(int Target, int Left_Encoder, int Right_Encoder)
{
	//Encoder_S是偏差累加
	static int Err_Lowout_Last, Encoder_S;
	static float a = 0.7;
	//1、计算偏差值
	int Err, Err_Lowout;
	Err = (Left_Encoder + Right_Encoder) - Target;
	//2、低通滤波
	Err_Lowout = (1 - a) * Err + a * Err_Lowout_Last;
	Err_Lowout_Last = Err_Lowout;
	//3、积分
	Encoder_S += Err_Lowout;
	//4、限幅
	Encoder_S = Encoder_S > 10000 ? 10000 : (Encoder_S < (-10000) ? (-10000) : Encoder_S);
	if(stop == 1)
	{
		Encoder_S=0;
		stop=0;
	}
	//5、速度环计算
	Velocity_Ki = Velocity_Kp / 200;
	int tmp = Velocity_Kp * Err_Lowout + Velocity_Ki * Encoder_S;
	return tmp;
}

KP通过公式换算，KI一般为KP/200；

转向环PD

（用于转向操作，输入角速度、角度值）

int Turn_PD(int gyro_Z, int Target_Turn)
{
	int tmp = Turn_Kp * Target_Turn + Turn_Kd * gyro_Z;
	return tmp;
}

蓝牙控制

通过蓝牙APP发送0x01（上）、0x05（左）、0x03（右）、0x07（下）等数据分别控制方向。

跌倒保护

通过MPU6050，监听roll值，若它低于/高于设定的界限，则将PWM设为0。

void Stop(float *Med_Jiaodu,float *Jiaodu)
{
	if(abs((int)(*Jiaodu-*Med_Jiaodu))>60)
	{
		Load(0,0);
		stop=1;
	}
}

物体跟随

仅实现直线跟随，方案是借助超声波传回的距离，设置在10-20mm时可以跟随，若超过，则小车保持平衡不动。

TIP1:TIM1-TIM4用完了，如何10ms调用一次呢？答：MPU6050中的INT引脚，修改其采样率为100HZ即可

TIP2:MPU6050会进行自检，若陀螺仪不在水平状态最初状态不为平衡，则不通过，导致其计算结果一直为0，注释掉

其他

PID各种算法

1、位置式PID（本方式实际应用）

优点：静态误差小，溢出的影响小。

缺点：计算量很大，累积误差相对大，在系统出现错误的情况下，容易使系统失控，积分饱和。

使用：一般需要结合输出限幅和积分限幅使用。

2、增量式PID

优点：溢出的影响小，在系统出现错误的情况下,影响相对较小（因为只与过去的三次误差有关）,运算量也相对较小。

缺点：有静态误差（因为没有累积误差）。

3、积分分离式PID

积分分离式PID主要是针对位置式PID的积分，引入判断误差大小条件，是否使用积分项。

4、变速积分PID

积分分离式PID 积分的的权重是1或者0，而变积分PID积分的权重会动态变化。取决于偏差，偏差越大，积分越慢。

5、不完全微分PID

微分通过低通滤波。

6、微分先行

微分的作用是预测未来，能够预知变化，做出调整。其实就是先操作微分。

7、死区

输出了量，但是不执行任何动作，也就是输出的量不起作用。

8、梯形积分

积分有余差，消除不了，为了减少余差，提高运算的精度，便有了梯形积分PID。