参考文献
在电赛的培训中，第一个独立完成的项目是搭建的平衡小车，能实现在平面上的自平衡，并且被干扰时能恢复。
1、数学模型
两轮自平衡小车的结构主要由车体和双轮两部分组成，可以看成一个移动的倒立摆。下面分别对两轮自平衡小车的车轮和车体进行力学分析，建立动力学模型，最后，通过对两者的分析给出系统的状态空间表达式。
1.1车轮模型
以右轮为例进行受力分析
1.2车体模型
车轮的运动类似，车体的运动也可以分解为正向运动（前向、俯仰）和侧向运动（转向、偏航）。其中，偏航运动可以看成是转向运动的特殊情况
2、LQR控制器
LQR(Linear Quadratic Regulator)，即线性二次型调节器。LQR 可得到状态线性反馈的最优控制规律，易于构成闭环最优控制。
在现代控制理论中，最基本的控制系统就是全状态反馈控制系统，其目标是设计一个状态反馈控制器 U=−Kx 来控制系统的表现。但是K并不是唯一的，为此我们引入一个代价函数
式中，Q 为 n × n 维半正定的状态加权矩阵，R 为 n × n 维正定的控制加权矩阵。在工程实际中，Q 和 R 是对称矩阵且常取对角阵。Q 中对角线上的元素qi表示对相应误差分量xi（最终的目标是让每一个状态变量的值都变为 0，所以每一个状态变量的值又称为误差值）的重视程度，越被重视的误差分量，希望它越快的变小，相应地，其权重系数就取得越大。类似的，R 中对角线上的元素ri表示对相应输入分量ui的限制，让其不要太大。LQR 控制器的设计思路就是设计一个状态反馈控制器 u =− Kx，使得代价函数最小min J，从而达到利用廉价成本使原系统达到较好的性能指标的目的。
3、PID算法
3.1PID闭环控制
PID 控制即比例-积分-微分控制，即通过对偏差进行比例-积分-微分控制，使得当前值趋于目标值的过程。一般来说，比例 P 控制是必须的，所以衍生出很多组合的 PID 控制器，如 PD、PI、PID 等
离散PID公式为：
其中，e(k)为 k 时刻的偏差，u(k)为输出的控制量，对于电机来说即是 PWM。
根据位置闭环控制公式
位置信息通过编码器读数获取，初始编码器读数为10000，偏差为目标位置与当前位置之差。以下是使用C语言实现该控制的代码示例：
int Position_PID(int Position, int Target) {
static float Bias; // 当前偏差
static float Pwm; // 输出PWM值
static float Integral_bias; // 偏差的积分
static float Last_Bias; // 上一次偏差
// 计算当前偏差
Bias = Target - Position;
// 求出偏差的积分
Integral_bias += Bias;
// 限制积分值的范围
if (Integral_bias > 100000)
Integral_bias = 100000;
if (Integral_bias < -100000)
Integral_bias = -100000;
// 计算PID输出
Pwm = Position_KP * Bias
+ Position_KI * Integral_bias / 10
+ Position_KD * (Bias - Last_Bias);
// 保存当前偏差，以便下次使用
Last_Bias = Bias;
return Pwm; // 返回增量输出
}
入口参数为当前测量值与目标值，返回值为电机控制的 PWM。为防止超出执行器输出范围，在程序中作了积分限幅处理。
速度闭环控制。
速度信息 M 法测速得出，即单位时间内读取编码器脉冲数得到速度。速度闭环控制就是根据单位时间获取的脉冲数与目标值进行比较，得到偏差，使偏差趋于 0 的过程。
PID 控制要求当误差为 0 时，控制器的输出也应该为 0，但是对于速度反馈，当误差为 0 时，即达到了我们目标速度，若输出 PWM 也为 0，那么必定不能维持目标速度，故对于速度控制，我们使用增量式 PID，令所有 PWM 输出叠加作用于电机，使得速度保持。增量式 PID 控制公式为：
在速度闭环控制系统里只使用 PI 控制，代码可以简化
int Incremental_PI(int Encoder, int Target) {
static float Bias; // 当前偏差
static float Pwm; // 输出 PWM 值
static float Last_bias; // 上一次偏差
// 计算当前偏差
Bias = Target - Encoder;
// 增量式 PI 控制器计算
Pwm += Velocity_KP * (Bias - Last_bias) + Velocity_KI * Bias;
// 保存当前偏差，以便下次使用
Last_bias = Bias;
return Pwm; // 返回增量输出
}
小车开发
MPU6050 是一款常用的六轴传感器，集成了三轴加速度计和三轴陀螺仪。它可以用来测量小车的角度、角速度、线性加速度等信息。具体来说：
加速度计：测量小车在三个轴上的线性加速度，从而可以推算出小车的倾斜角度。
陀螺仪：测量小车在三个轴上的角速度，帮助我们判断小车的旋转状态。
通过这两种传感器的数据，我们可以获取小车的完整姿态信息。
卡尔曼滤波算法
我们使用卡尔曼滤波算法对传感器测量的数据进行最优估计。卡尔曼滤波的思想就是使用系统的状态方程预测当前的值，使用传感器测出来的观测值来修正这个预测值。与互补滤波一样，可以选择不同的权重来实现，但是这个权重是动态变化的。
例如一个无人驾驶汽车想要在城市中自由行驶，它必须知道自己的确切位置才能进行导航。关于如何导航，我们一般有以下方式：
GPS装置，但缺点是精度可能太低；
里程表、惯性测量等传感器得出的信息；
汽车发动机温度、邮箱液体量信息；
汽车运动程序本身发出的指令等。
以上信息存在精度误差，并且实际行驶情况可能存在自然与非自然因素影响，所以得出的导航结果不是非常准确。卡尔曼滤波的作用就是综合利用以上信息，根据其本身噪声，分配一定权重，去估计获得一个导航信息的最优估计
卡尔曼滤波主要分为预测和更新两部分，需要构建了系统的状态方程和观测方程，且已知系统的初始状态。在预测阶段，滤波器使用上一状态的估计，做出对当前状态的估计。在更新阶段，滤波器利用对当前状态的观测值优化在预测阶段获得的预测值，以获得一个更精确的新估计值。
#include <stdio.h>
#include <stdlib.h>
#include <time.h>
#define Kk_calc(x,y) (x)/(x+y)
struct KalmanFilter {
float x_mea; // measure value, instead of random number
float x_est; // estimate value
float e_mea; // measure offset, can not be removed
float e_est; // estimate offset
float Kk; // Karlman Gain
};
float RandomNumGenerator(int base, int range)
{
float k = 0.0;
float randomNum = 0.0;
k = 2 * range * 10;
randomNum = rand() % (int)k;
k = base - range + (randomNum / 10);
return k;
}
void BoostRandomNumGenerator() {
srand((unsigned)time(NULL));
}
void Kalman_Init(KalmanFilter* kalmanFilter, float FirstMeaValue, float E_mea, float FirstEstValue, float E_est) {
kalmanFilter->x_est = FirstEstValue;
kalmanFilter->x_mea = FirstMeaValue;
kalmanFilter->e_est = E_est;
kalmanFilter->e_mea = E_mea;
kalmanFilter->Kk = Kk_calc(kalmanFilter->e_est, kalmanFilter->e_mea);
}
void Kalman_Update(KalmanFilter* kalmanFilter, float newMeaValue) {
float temp = kalmanFilter->e_est;
kalmanFilter->x_est = kalmanFilter->x_est + kalmanFilter->Kk * (newMeaValue - kalmanFilter->x_est);
kalmanFilter->x_mea = newMeaValue;
kalmanFilter->Kk = Kk_calc(kalmanFilter->e_est, kalmanFilter->e_mea);
kalmanFilter->e_est = (1 - kalmanFilter->Kk) * temp;
}
int main()
{
KalmanFilter k;
BoostRandomNumGenerator();
Kalman_Init(&k, 51.0, 3.0, 40, 5);
for (int i = 0; i < 10; i++)
{
// Ten iterations
Kalman_Update(&k, RandomNumGenerator(50, 3));
printf("%.3f | %.3f\n",k.x_mea,k.x_est);
}
return 0;
}电路设计
整体电路分为几个模块：
最小单片机系统：整合处理所有模块的数据，并进行控制，实现所需功能。
串口电路：与上位机进行通信。
电源系统：提供所有模块的供电电压。
电机驱动电路：输出电压驱动电机。
.姿态检测电路：获取小车的角度与角速度信息。
主控板
小车使用的主控芯片是STM32F103VET6,高性能低功耗。
串口电路
我们选用的 USB 转串口的芯片是CH340G。
姿态传感器
使用MPU6050姿态传感器，集成了三轴加速度计和三轴陀螺仪，以及一个可扩展的数字运动处理器 DMP，可用 IIC 接口连接一个第三方的数字传感器，如磁力计等。此集成模块使得角速度和加速度传感器处于同样的环境中，对消除零点漂移有着积极的作用。MPU6050 内部自带 ADC，在获取姿态模拟信息后，可以把它直接转化为数字量。
电机驱动
选用 TB6612FNG电机驱动芯片，此芯片为双通道电路输出，可以驱动两个电机，其中的A和 B分别为一组电机的输入端和输出端。
软件开发
参数定义
#define STBY PEout(10)
// 获取角度的算法，1：四元数 2：卡尔曼 3：互补滤波
u8 Way_Angle = 2;
// 蓝牙遥控相关的变量
u8 Flag_front, Flag_back, Flag_Left, Flag_Right, Flag_velocity = 2;
// 电机停止标志位 默认停止
u8 Flag_Stop = 1;
// 电机PWM变量
int Motor_Left, Motor_Right;
// 温度变量
int Temperature;
// 左右编码器的脉冲计数
int Encoder_Left, Encoder_Right;
// 电池电压采样相关的变量
int Voltage;
// 平衡倾角 平衡陀螺仪 转向陀螺仪
float Angle_Balance, Gyro_Balance, Gyro_Turn;
// 延时和调参相关变量
int delay_50, delay_flag;
// 蓝牙模式、普通模式标志位
u8 Flag_Blooth = 0, Flag_Normol = 1;
// 获取时钟
RCC_ClocksTypeDef RCC_CLK;
// Z轴加速度计
float Acceleration_Z;
// PID参数（放大100倍）
float Balance_Kp = 20500, Balance_Kd = 110, Velocity_Kp = 16000, Velocity_Ki = 80, Turn_Kp = 4200, Turn_Kd = 0;
// 车轮速度(mm/s)
float Velocity_Left, Velocity_Right;
// 平衡环PWM变量，速度环PWM变量，转向环PWM变量
int Balance_Pwm, Velocity_Pwm, Turn_Pwm;
// 位置PID控制变量，速度PID控制变量
int Pos_Pwm, Vel_Pwm;
// 位置PID系数
//float Position_KP = 20, Position_KI = 0.0785, Position_KD = 182.7;
float Position_KP = 20, Position_KI = 0.07, Position_KD = 180;
// 速度PID系数
float Velocity_KP = 30, Velocity_KI = 6, Velocity_KD = 0;
// 位置PID设定目标数
int Target_Position_L = 800, Target_Position_R = 800;
// 速度PID设定目标数
int Target_Velocity_L = 10, Target_Velocity_R = 20;主程序设计
int main(void)
{
init_Basic();
// 初始化设备驱动
init_drv_Main();
// 初始化开始声音
sendLedSignal(LEDSignal_Start1);
// 初始化控制系统
init_drv_ControlSystem();
// 设置初始化已完成
setInitializationCompleted();
// 获取时钟
RCC_GetClocksFreq(&RCC_CLK);
STBY = 1;
// 进入主循环
while (1)
{
// 调试模式的选择
#if Debug
DataScope();
delay_ms(50);
#else
Commulink_Server();
// 显示
GUI_Server();
#endif
}
}
直立控制程序设计
#define APB2TIMERCLK 72000000
#define AIN1 PDout(4)
#define AIN2 PCout(12)
#define BIN1 PDout(11)
#define BIN2 PDout(10)
/*
* 函数功能：直立PD控制
* 入口参数：Angle:角度；Gyro：角速度
* 返回 值：balance：直立控制PWM
*/
int Balance(float Angle, float Gyro);
/*
* 函数功能：速度控制PWM
* 入口参数：encoder_left：左轮编码器读数；encoder_right：右轮编码器读数
* 返回 值：速度控制PWM
*/
int Velocity(int encoder_left, int encoder_right);
/*
* 函数功能：转向控制
* 入口参数：Z轴陀螺仪
* 返回 值：转向控制PWM
*/
int Turn(float gyro);
/*
* 函数功能：赋值给PWM寄存器
* 入口参数：左轮PWM、右轮PWM
* 返回 值：无
*/
void Set_Pwm(int motor_left, int motor_right)
{
int pwmb, pwma;
if (motor_left > 0)
{
BIN1 = 0;
BIN2 = 1;
}
else
{
BIN1 = 1;
BIN2 = 0;
}
PWMB = myabs(motor_left);
//TIM_SetCompare3(TIM1, pwmb);
if (motor_right > 0)
{
AIN1 = 0;
AIN2 = 1;
}
else
{
AIN1 = 1;
AIN2 = 0;
}
PWMA = myabs(motor_right);
//TIM_SetCompare4(TIM1, pwma);
}
/*
* 函数功能：限制PWM赋值
* 入口参数：IN：输入参数 max：限幅最大值 min：限幅最小值
* 返回 值：限幅后的值
*/
int PWM_Limit(int IN, int max, int min)
{
int OUT;
if (IN > max)
{
OUT = max;
}
else if (IN < min)
{
OUT = min;
}
else
{
OUT = IN;
}
return OUT;
}
/*
* 函数功能：按键修改小车运行状态
* 入口参数：无
* 返回 值：无
*/
void Key(void);
/*
* 函数功能：异常关闭电机
* 入口参数：angle：小车倾角；voltage：电压
* 返回 值：1：异常 0：正常
*/
u8 Turn_Off(float angle, int voltage);
/*
* 函数功能：绝对值函数
* 入口参数：a：需要计算绝对值的数
* 返回 值：无符号整型
*/
int myabs(int a)
{
int temp;
if (a < 0)
{
temp = -a;
}
else
{
temp = a;
}
return temp;
}
/*
* 函数功能：检测小车是否被拿起
* 入口参数：Acceleration：z轴加速度；Angle：平衡的角度；encoder_left：左编码器计数；encoder_right：右编码器计数
* 返回 值：1:小车被拿起 0：小车未被拿起
*/
int Pick_Up(float Acceleration, float Angle, int encoder_left, int encoder_right);
/*
* 函数功能：检测小车是否被放下
* 入口参数：平衡角度；左编码器读数；右编码器读数
* 返回 值：1：小车放下 0：小车未放下
*/
int Put_Down(float Angle, int encoder_left, int encoder_right);
/*
* 函数功能：编码器读数转换为速度（mm/s）
* 入口参数：无
* 返回 值：无
*/
void Get_Velocity_Form_Encoder(int encoder_left, int encoder_right);
/*
* 函数功能：选择小车运行模式
* 入口参数：encoder_left：左编码器读数 encoder_right：右编码器读数
* 返回 值：无
*/
void Choose(int encoder_left, int encoder_right);
void init_drv_ControlSystem()
{
RCC->APB2ENR |= (1 << 13);
// os_delay(1e-2);
TIM8->PSC = (APB2TIMERCLK / 1000000) - 1;
TIM8->ARR = 1e6 / 200 - 1;
TIM8->DIER = 1 << 0;
TIM8->CR1 = 1 << 0;
NVIC_SetPriority(TIM8_UP_IRQn, 3);
NVIC_EnableIRQ(TIM8_UP_IRQn);
}
int Set_Postion_PID_L(int position, int target)
{
static float bias, pwm, integral_bias, last_bias;
bias = target - position;
integral_bias += bias;
if (integral_bias > 3000)
{
integral_bias = 3000;
}
else if (integral_bias < -3000)
{
integral_bias = -3000;
}
pwm = Position_KP * bias + Position_KI * integral_bias + Position_KD * (bias - last_bias);
last_bias = bias;
return pwm;
}
int Set_Postion_PID_R(int position, int target)
{
static float bias, pwm, integral_bias, last_bias;
bias = target - position;
integral_bias += bias;
if (integral_bias > 3000)
{
integral_bias = 3000;
}
else if (integral_bias < -3000)
{
integral_bias = -3000;
}
pwm = Position_KP * bias + Position_KI * integral_bias + Position_KD * (bias - last_bias);
last_bias = bias;
return pwm;
}
// 增量式PID：P是D，I是P；
int Set_Incremental_PI_L(int incremental, int target)
{
static float bias, pwm, last_bias;
bias = target - incremental;
pwm += Velocity_KP * (bias - last_bias) + Velocity_KI * bias;
last_bias = bias;
return pwm;
}
int Set_Incremental_PI_R(int incremental, int target)
{
static float bias, pwm, last_bias;
bias = target - incremental;
pwm += Velocity_KP * (bias - last_bias) + Velocity_KI * bias;
last_bias = bias;
return pwm;
}
int Balance(float Angle,float Gyro)
{
float Angle_bias,Gyro_bias;
int balance;
Angle_bias=Middle_angle-Angle;
Gyro_bias=0-Gyro;
balance=-Balance_Kp/100*Angle_bias-Gyro_bias*Balance_Kd/100;
return balance;
}
int Velocity(int encoder_left,int encoder_right)
{
static float velocity,Encoder_Least,Encoder_bias,Movement;
static float Encoder_Integral,Target_Integral;
Encoder_Least = 0 - (encoder_left+encoder_right);
Encoder_bias *= 0.84;
Encoder_bias += Encoder_Least*0.16;
Encoder_Integral += Encoder_bias;
if(Encoder_Integral>10000)
{
Encoder_Integral=10000;
}
if(Encoder_Integral<-10000)
{
Encoder_Integral=-10000;
}
velocity=-Encoder_bias*Velocity_Kp/100-Encoder_Integral*Velocity_Ki/100;
return velocity;
}
int Turn(float gyro)
{
static float Turn_Target,turn;
float Kp = Turn_Kp, Kd=Turn_Kd;
Turn_Target=-30;
turn = Turn_Target*Kp/100+gyro*Kd/100;
return turn;
}
void Get_Velocity_Form_Encoder(int encoder_left,int encoder_right)
{
float Rotation_Speed_L,Rotation_Speed_R;
Rotation_Speed_L = encoder_left * Control_Frequency/EncoderMultiples/Reduction_Ratio/Encoder_precision;
Velocity_Left = Rotation_Speed_L*PI*Diameter_67;
Rotation_Speed_R = encoder_right * Control_Frequency/EncoderMultiples/Reduction_Ratio/Encoder_precision;
Velocity_Right = Rotation_Speed_R*PI*Diameter_67;
}
extern "C" void TIM8_UP_IRQHandler(void)
{
if (TIM_GetFlagStatus(TIM8, TIM_FLAG_Update) == SET)
{
TIM_ClearITPendingBit(TIM8, TIM_IT_Update);
if (getInitializationCompleted() == false)
{
return;
}
Get_Angle(2);
Encoder_Left = -Read_Encoder(4);
Encoder_Right = -Read_Encoder(3);
Voltage = Get_battery_volt();
Get_Velocity_Form_Encoder(Encoder_Left,Encoder_Right);
Balance_Pwm=Balance(Angle_Balance,Gyro_Balance);
Velocity_Pwm=Velocity(Encoder_Left,Encoder_Right);
Turn_Pwm=Turn(Gyro_Turn);
Motor_Left=Balance_Pwm+Velocity_Pwm+Turn_Pwm;
Motor_Right=Balance_Pwm+Velocity_Pwm-Turn_Pwm;
Motor_Left=PWM_Limit(Motor_Left,6900,-6900);
Motor_Right=PWM_Limit(Motor_Right,6900,-6900);
//Set_Pwm(Motor_Left,Motor_Right);
Set_Pwm(4000,4000);
}
}编码器程序设计
#include "drv_Encoder.hpp"
#include "stm32f10x_gpio.h"
#include "Control.hpp"
/*
* 函数功能：把TIM3初始化为编码器接口模式
* 入口参数：无
* 返回 值：无
*/
void drv_Encoder_Init_TIM3(void)
{
// PC6 ---> E1A T3C1
// PC7 ---> E1B T3C2
TIM_TimeBaseInitTypeDef TIM_TimeBaseStructure;
TIM_ICInitTypeDef TIM_ICInitStructure;
GPIO_InitTypeDef GPIO_InitStructure;
RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM3, ENABLE);
RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOC, ENABLE);
//重映射使能
GPIO_PinRemapConfig(GPIO_FullRemap_TIM3,ENABLE);
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_6 | GPIO_Pin_7;
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IN_FLOATING;
GPIO_Init(GPIOC, &GPIO_InitStructure);
TIM_TimeBaseStructInit(&TIM_TimeBaseStructure);
TIM_TimeBaseStructure.TIM_Prescaler = 0x0;
//设定计数器自动重装值
TIM_TimeBaseStructure.TIM_Period = ENCODER_TIM_PERIOD;
//选择时钟分频：不分频
TIM_TimeBaseStructure.TIM_ClockDivision = TIM_CKD_DIV1;
//TIM向上计数
TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up;
TIM_TimeBaseInit(TIM3, &TIM_TimeBaseStructure);
//使用编码器模式3
TIM_EncoderInterfaceConfig(TIM3, TIM_EncoderMode_TI12, TIM_ICPolarity_Rising, TIM_ICPolarity_Rising);
TIM_ICStructInit(&TIM_ICInitStructure);
//滤波10
TIM_ICInitStructure.TIM_ICFilter = 10;
TIM_ICInit(TIM3, &TIM_ICInitStructure);
//清除TIM的更新标志位
TIM_ClearFlag(TIM3, TIM_FLAG_Update);
TIM_ITConfig(TIM3, TIM_IT_Update, ENABLE);
//Reset counter
TIM_SetCounter(TIM3,0);
TIM_Cmd(TIM3, ENABLE);
}
/*
* 函数功能：把TIM4初始化为编码器接口模式
* 入口参数：无
* 返回 值：无
*/
void drv_Encoder_Init_TIM4(void)
{
// PD12 ---> E2A T4C1
// PD13 ---> E2B T4C2
TIM_TimeBaseInitTypeDef TIM_TimeBaseStructure;
TIM_ICInitTypeDef TIM_ICInitStructure;
GPIO_InitTypeDef GPIO_InitStructure;
//使能定时器4的时钟
RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM4, ENABLE);
//使能PB端口时钟
RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOD, ENABLE);
//重映射使能
GPIO_PinRemapConfig(GPIO_Remap_TIM4,ENABLE);
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_12|GPIO_Pin_13;
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IN_FLOATING;
GPIO_Init(GPIOD, &GPIO_InitStructure);
TIM_TimeBaseStructInit(&TIM_TimeBaseStructure);
TIM_TimeBaseStructure.TIM_Prescaler = 0x0;
TIM_TimeBaseStructure.TIM_Period = ENCODER_TIM_PERIOD;
TIM_TimeBaseStructure.TIM_ClockDivision = TIM_CKD_DIV1;
TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up;
TIM_TimeBaseInit(TIM4, &TIM_TimeBaseStructure);
TIM_EncoderInterfaceConfig(TIM4, TIM_EncoderMode_TI12, TIM_ICPolarity_Rising, TIM_ICPolarity_Rising);
TIM_ICStructInit(&TIM_ICInitStructure);
TIM_ICInitStructure.TIM_ICFilter = 10;
TIM_ICInit(TIM4, &TIM_ICInitStructure);
TIM_ClearFlag(TIM4, TIM_FLAG_Update);
TIM_ITConfig(TIM4, TIM_IT_Update, ENABLE);
//Reset counter
TIM_SetCounter(TIM4,0);
TIM_Cmd(TIM4, ENABLE);
}
/*
* 函数功能：单位时间读取编码器计数
* 入口参数：TIMX：定时器
* 返回 值：速度值
*/
int Read_Encoder(u8 TIMX)
{
int Encoder_TIM;
switch(TIMX)
{
#if Debug
#if Debug_pos
case 2: Encoder_TIM= (short)TIM2 -> CNT;break;
case 3: Encoder_TIM= (short)TIM3 -> CNT;break;
case 4: Encoder_TIM= (short)TIM4 -> CNT;break;
default: Encoder_TIM=0;
#elif Debug_vel
case 2: Encoder_TIM= (short)TIM2 -> CNT; TIM2 -> CNT=0;break;
case 3: Encoder_TIM= (short)TIM3 -> CNT; TIM3 -> CNT=0;break;
case 4: Encoder_TIM= (short)TIM4 -> CNT; TIM4 -> CNT=0;break;
default: Encoder_TIM=0;
#endif
#else
case 2: Encoder_TIM= (short)TIM2 -> CNT; TIM2 -> CNT=0;break;
case 3: Encoder_TIM= (short)TIM3 -> CNT; TIM3 -> CNT=0;break;
case 4: Encoder_TIM= (short)TIM4 -> CNT; TIM4 -> CNT=0;break;
default: Encoder_TIM=0;
#endif
}
return Encoder_TIM;
}
/*
* 函数功能：TIM4中断服务函数
* 入口参数：无
* 返回 值：无
*/
extern "C" void TIM4_IRQHandler(void)
{
//溢出中断
if(TIM4->SR&0X0001)
{
}
//清除中断标志位
TIM4->SR&=~(1<<0);
}
/*
函数功能：TIM3中断服务函数
入口参数：无
返回 值：无
**************************************************************************/
extern "C" void TIM3_IRQHandler(void)
{
if(TIM3->SR&0X0001)
{
}
TIM3->SR&=~(1<<0);
}角度滤波算法设计
#include "Filter.hpp"
#include <math.h>
#include <stdio.h>
/**************************************************************************
函数功能：获取x轴角度简易卡尔曼滤波
入口参数：加速度获取的角度、角速度
返回 值：x轴角速度
**************************************************************************/
float dt=0.005; //每5ms进行一次滤波
float Kalman_Filter_x(float Accel,float Gyro)
{
//static float angle_dot;
static float angle;
float Q_angle=0.001; // 过程噪声的协方差
float Q_gyro=0.003; // 过程噪声的协方差 过程噪声的协方差为一个一行两列矩阵
float R_angle=0.5; // 测量噪声的协方差 既测量偏差
char C_0 = 1;
static float Q_bias, Angle_err;
static float PCt_0, PCt_1, E;
static float K_0, K_1, t_0, t_1;
static float Pdot[4] ={0,0,0,0};
//首次协方差矩阵
static float PP[2][2] = { { 1, 0 },{ 0, 1 } };
angle+=(Gyro - Q_bias) * dt; // 角度先验估计积分
Pdot[0]=Q_angle - PP[0][1] - PP[1][0]; // Pk-先验估计误差协方差的微分
Pdot[1]=-PP[1][1];
Pdot[2]=-PP[1][1];
Pdot[3]=Q_gyro;
PP[0][0] += Pdot[0] * dt; // Pk-先验估计误差协方差微分的积分
PP[0][1] += Pdot[1] * dt; // 先验估计误差协方差
PP[1][0] += Pdot[2] * dt;
PP[1][1] += Pdot[3] * dt;
Angle_err = Accel - angle; // zk-先验估计，角度偏差
//求卡尔曼增益
PCt_0 = C_0 * PP[0][0];
PCt_1 = C_0 * PP[1][0];
E = R_angle + C_0 * PCt_0; // 求卡尔曼增益分母
K_0 = PCt_0 / E;
K_1 = PCt_1 / E; // 计算卡尔曼增益
//更新协方差矩阵
t_0 = PCt_0;
t_1 = C_0 * PP[0][1];
PP[0][0] -= K_0 * t_0; //后验估计误差协方差
PP[0][1] -= K_0 * t_1;
PP[1][0] -= K_1 * t_0;
PP[1][1] -= K_1 * t_1;
//利用卡尔曼进行最优估计
angle += K_0 * Angle_err; //后验估计
Q_bias += K_1 * Angle_err; //后验估计
//angle_dot = Gyro - Q_bias; //输出值(后验估计)的微分=角速度
return angle;
}
/**************************************************************************
函数功能：一阶互补滤波
入口参数：加速度获取的角度、角速度
返回 值：x轴角速度
**************************************************************************/
float Complementary_Filter_x(float angle_m, float gyro_m)
{
static float angle;
float K1 =0.02;
angle = K1 * angle_m+ (1-K1) * (angle + gyro_m * dt);
return angle;
}
/**************************************************************************
函数功能：获取y轴角度简易卡尔曼滤波
入口参数：加速度获取的角度、角速度
返回 值：y轴角速度
**************************************************************************/
float Kalman_Filter_y(float Accel,float Gyro)
{
//static float angle_dot;
static float angle;
float Q_angle=0.001; // 过程噪声的协方差
float Q_gyro=0.003; // 0.003 过程噪声的协方差 过程噪声的协方差为一个一行两列矩阵
float R_angle=0.5; // 测量噪声的协方差 既测量偏差
char C_0 = 1;
static float Q_bias, Angle_err;
static float PCt_0, PCt_1, E;
static float K_0, K_1, t_0, t_1;
static float Pdot[4] ={0,0,0,0};
static float PP[2][2] = { { 1, 0 },{ 0, 1 } };
angle+=(Gyro - Q_bias) * dt; //先验估计
Pdot[0]=Q_angle - PP[0][1] - PP[1][0]; // Pk-先验估计误差协方差的微分
Pdot[1]=-PP[1][1];
Pdot[2]=-PP[1][1];
Pdot[3]=Q_gyro;
PP[0][0] += Pdot[0] * dt; // Pk-先验估计误差协方差微分的积分
PP[0][1] += Pdot[1] * dt; // 先验估计误差协方差
PP[1][0] += Pdot[2] * dt;
PP[1][1] += Pdot[3] * dt;
Angle_err = Accel - angle; //zk-先验估计
PCt_0 = C_0 * PP[0][0];
PCt_1 = C_0 * PP[1][0];
E = R_angle + C_0 * PCt_0;
K_0 = PCt_0 / E;
K_1 = PCt_1 / E;
t_0 = PCt_0;
t_1 = C_0 * PP[0][1];
PP[0][0] -= K_0 * t_0; //后验估计误差协方差
PP[0][1] -= K_0 * t_1;
PP[1][0] -= K_1 * t_0;
PP[1][1] -= K_1 * t_1;
angle += K_0 * Angle_err; //后验估计
Q_bias += K_1 * Angle_err; //后验估计
//angle_dot = Gyro - Q_bias; //输出值(后验估计)的微分=角速度
return angle;
}
/**************************************************************************
函数功能：一阶互补滤波
入口参数：加速度获取的角度、角速度
返回 值：y轴角速度
**************************************************************************/
float Complementary_Filter_y(float angle_m, float gyro_m)
{
static float angle;
float K1 =0.02;
angle = K1 * angle_m+ (1-K1) * (angle + gyro_m * dt);
return angle;
}
#define M_LN2_FLOAT 0.69314718055994530942f
#define M_PI_FLOAT 3.14159265358979323846f
//二阶巴特沃斯滤波器的带宽
#define BIQUAD_BANDWIDTH 1.9f
//二阶巴特沃斯滤波器的品质因数Q
#define BIQUAD_Q (1.0f / sqrtf(2.0f))
#define UNUSED(x) (void)(x)
/*
* 函数功能：释放滤波器
*/
float nullFilterApply(void *filter, float input)
{
UNUSED(filter);
return input;
}
/*
* 函数功能：低通滤波器的设置
* filter：滤波器的相关参数
* f_cut：截止频率
* dT：数据更新时间隔（秒）
*/
void pt1FilterInit(pt1Filter_t *filter, uint8_t f_cut, float dT)
{
//参考模电RC截止频率公式
filter->RC = 1.0f / ( 2.0f * M_PI_FLOAT * f_cut );
//数据更新时间隔（秒）
filter->dT = dT;
}
/*
* 函数功能：应用低通滤波
* filter：滤波器的相关参数
* input：新输入的数据
*/
float pt1FilterApply(pt1Filter_t *filter, float input)
{
//dt固定时，截止频率越低，k越小，数据最终输出的变化越慢（高频去掉了）
//截止频率固定时，dT越大，k越小，数据最终输出的变化越慢（高频去掉了）
filter->state = filter->state + filter->dT / (filter->RC + filter->dT) * (input - filter->state);
return filter->state;
}
/*
* 函数功能：应用低通滤波
* filter：滤波器的相关参数
* input：新输入的数据
* f_cut：截止频率
*/
float pt1FilterApply4(pt1Filter_t *filter, float input, uint8_t f_cut, float dT)
{
//先前计算并储存参数
if (!filter->RC)
{
pt1FilterInit(filter, f_cut, dT);
}
filter->state = filter->state + filter->dT / (filter->RC + filter->dT) * (input - filter->state);
return filter->state;
}
/*
* 函数功能：双二阶滤波器的设置
* filter：滤波器的相关参数
* filterFreq：滤波器的中心频率
* sampleDeltaUs：滤波器的采样系数
* Q：滤波器的品质因数
* filterType：滤波器的种类
*/
static void biquadFilterInit(biquadFilter_t *filter, float filterFreq, uint32_t sampleDeltaUs, float Q, biquadFilterType_e filterType)
{
//设置采样频率
const float sampleHz = 1 / ((float)sampleDeltaUs * 0.000001f);
const float omega = 2 * M_PI_FLOAT * filterFreq / sampleHz;
const float sn = sinf(omega);
const float cs = cosf(omega);
const float alpha = sn / (2 * Q);
//滤波器的系数
float b0, b1, b2, a0, a1, a2;
//滤波器的种类
switch (filterType)
{
default:
//低通滤波器
case FILTER_LPF:
b0 = (1 - cs) / 2;
b1 = 1 - cs;
b2 = (1 - cs) / 2;
a0 = 1 + alpha;
a1 = -2 * cs;
a2 = 1 - alpha;
break;
//带通滤波器
case FILTER_NOTCH:
b0 = 1;
b1 = -2 * cs;
b2 = 1;
a0 = 1 + alpha;
a1 = -2 * cs;
a2 = 1 - alpha;
break;
}
//预先计算系数
filter->b0 = b0 / a0;
filter->b1 = b1 / a0;
filter->b2 = b2 / a0;
filter->a1 = a1 / a0;
filter->a2 = a2 / a0;
//零初始样本
filter->d1 = filter->d2 = 0;
}
//双二阶滤波器的设置
void biquadFilterInitLPF(biquadFilter_t *filter, float filterFreq, uint32_t sampleDeltaUs)
{
biquadFilterInit(filter, filterFreq, sampleDeltaUs, BIQUAD_Q, FILTER_LPF);
}
/*
* 函数功能：应用双二阶滤波器
* filter：滤波器的相关参数
* input：新输入的数据
*/
float biquadFilterApply(biquadFilter_t *filter, float input)
{
const float result = filter->b0 * input + filter->d1;
filter->d1 = filter->b1 * input - filter->a1 * result + filter->d2;
filter->d2 = filter->b2 * input - filter->a2 * result;
return result;
}
