一、平衡小车原理
平衡小车是通过两个电机运动下实现小车不倒下直立行走的多功能智能小
车,在外力的推拉下,小车依然保持不倒下。这么一说可能还没有很直观的了解
究竟什么是平衡小车,不过这个平衡小车实现的原理其实是在人们生活中的经验
得来的。如果通过简单的练习,一般人可以通过自己的手指把木棒直立而不倒的
放在指尖上,所以练习的时候,需要学会的两个条件:一是放在指尖上可以移动,
二是通过眼睛观察木棒的倾斜角度和倾斜趋势(角速度)。通过手指的移动去抵
消木棒倾斜的角度和趋势,使得木棒能直立不倒。这样的条件是不可以缺一的,
实际上加入这两个条件,控制过程中就是负反馈机制。

(1) 时序总算法
void inter()
{
sei();
countpluse(); //脉冲叠加子函数
mpu.getMotion6(&ax, &ay, &az, &gx, &gy, &gz); //IIC 获取
MPU6050 六轴数据 ax ay az gx gy gz
kalmanfilter.Angletest(ax, ay, az, gx, gy, gz, dt, Q_angle, Q_gyro,R_
angle,C_0,K1); //获取 angle 角度和卡曼滤波
angleout(); //角度环 PD 控制

speedcc++;
if (speedcc >= 8) //50ms 进入速度环控制
{
Outputs = balancecar.speedpiout(kp_speed,ki_speed,kd_speed,fron
t,back,setp0);
speedcc = 0;
}
turncount++;
if (turncount > 2) //10ms 进入旋转控制
{
turnoutput = balancecar.turnspin(turnl,turnr,spinl,spinr,kp_turn,k
d_turn,kalmanfilter.Gyro_z); //旋转子函数
turncount = 0;
}
balancecar.posture++;
balancecar.pwma(Outputs,turnoutput,kalmanfilter.angle,kalmanfilt
er.angle6,turnl,turnr,spinl,spinr,front,back,kalmanfilter.accelz,IN1M,
IN2M,IN3M,IN4M,PWMA,PWMB); //小车
总 PWM 输出
}

(2) 平衡程序
/////////////卡曼滤波计算角度,一阶滤波换算姿体///////////////
void Angletest()
{
//平衡参数
Angle = atan2(ay , az) * 57.3; //角度计算公式
Gyro_x = (gx - 128.1) / 131; //角度转换
Kalman_Filter(Angle, Gyro_x); //卡曼滤波
//旋转角度 Z 轴参数
if (gz > 32768) gz -= 65536; //强制转换 2g 1g
Gyro_z = -gz / 131; //Z 轴参数转换
//姿态识别
angleAx=atan2(ax,az)*180/PI;//计算与 x 轴夹角
Gyro_y=-gy/131.00;//计算角速度
Yijielvbo(angleAx,Gyro_y);//一阶滤波
}
///////////////////////////卡曼滤波计算角度////////////////////////////////////

////////////////////////kalman/////////////////////////

void Kalman_Filter(double angle_m, double gyro_m)
{
angle += (gyro_m - q_bias) * dt;
angle_err = angle_m - angle;
Pdot[0] = Q_angle - P[0][1] - P[1][0];
Pdot[1] = - P[1][1];
Pdot[2] = - P[1][1];
Pdot[3] = Q_gyro;
P[0][0] += Pdot[0] * dt;
P[0][1] += Pdot[1] * dt;
P[1][0] += Pdot[2] * dt;
P[1][1] += Pdot[3] * dt;
PCt_0 = C_0 * P[0][0];
PCt_1 = C_0 * P[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 * P[0][1];
P[0][0] -= K_0 * t_0;
P[0][1] -= K_0 * t_1;
P[1][0] -= K_1 * t_0;
P[1][1] -= K_1 * t_1;
angle += K_0 * angle_err; //优角度
q_bias += K_1 * angle_err;
angle_dot = gyro_m - q_bias; //优角速度
}

////////////////////////kalman/////////////////////////

//////////////////////////一阶滤波////////////////////
void Yijielvbo(float angle_m, float gyro_m)
{
angle6 = K1 * angle_m+ (1-K1) * (angle6 + gyro_m * dt);
}

//////////////////速度 PI////////////////////
void speedpiout()
{
speeds = (smal + smar) * 1.0;
//车速 脉冲值
smar = smal = 0;
speeds_filterold *= 0.7;
//一阶互补滤波
speeds_filter = speeds_filterold + speeds * 0.3;
speeds_filterold = speeds_filter;
positions += speeds_filter;
positions += front;
//全进控制量融合
positions += back;
//全进控制量融合
positions = constrain(positions, -16550, 16550); //抗积分饱和
Outputs = ksi * (setp0 - positions) + ksp * (setp0 - speeds_filter);
//速度环控制 PI
speedcc = 0;
}

////////////////////////////////////////pwm///////////////////////////////////
void pwma()
{

pwm1 = -Output - Outputs - turnoutput; //Left 电机 PWM 输出值
pwm2 = -Output - Outputs + turnoutput;//Right电机PWM输出值

//幅度值限制
if (pwm1 > 255) pwm1 = 255;
if (pwm1 < -255) pwm1 = -255; if (pwm2 > 255) pwm2 = 255;
if (pwm2 < -255) pwm2 = -255;

//角度过大,停止电机
if (angle > 30 || angle < -30)
{
pwm1 = 0;
pwm2 = 0;
}

//电机的正负输出判断 左电机判断
if (pwm1 >= 0) {
digitalWrite(IN2M, 0);
digitalWrite(IN1M, 1);
analogWrite(PWMA, pwm1);
} else {
digitalWrite(IN2M, 1);
digitalWrite(IN1M, 0);
analogWrite(PWMA, -pwm1);
}
//电机的正负输出判断 右电机判断
if (pwm2 >= 0) {
digitalWrite(IN4M, 0);
digitalWrite(IN3M, 1);
analogWrite(PWMB, pwm2);
} else {
digitalWrite(IN4M, 1);
digitalWrite(IN3M, 0);
analogWrite(PWMB, -pwm2);
}

}

////////////////////////////////////////turn//////////////////////////////////

int turnmax = 10; //旋转输
出幅值
int turnmin = -10; //旋转输出
幅值

void turnspin()
{
float turnspp;
turnvert=0.9;
if (turnl == 1 || spinl == 1)//根据方向参数叠加
{
turnout += turnvert;
}
else if (turnr == 1 || spinr == 1)//根据方向参数叠加
{
turnout -= turnvert;
}
else turnout = 0;
if (turnout > turnmax) turnout = turnmax;//幅值大值设置
if (turnout < turnmin) turnout = turnmin;//幅值小值设置

turnoutput = -turnout * ktp - Gyro_z * ktd;//旋转 PD 算法控制 融
合速度和 Z 轴旋转定位。

}

(3) 蓝牙
////////////////////////bluetooth//////////////////////
void kongzhi()
{
while (Serial.available())
//等待蓝牙数据
switch (Serial.read())
//读取蓝牙数据
{
case 0x01: front=600; break;
//前进
case 0x02: back=-600; break;
//后退
case 0x03: turnl=1; break;
//左转
case 0x04: turnr=1; break;
//右转
case 0x05: spinl=1; break; //左
旋转
case 0x06: spinr=1; break; //
右旋转
case 0x07: turnl = 0; turnr = 0; front = 0; back = 0; spinl = 0;
spinr = 0; break; //确保按键松开后为停车操

case 0x08: spinl = 0; spinr = 0; front = 0; back = 0; turnl =
0; turnr = 0; break; //确保按键松开后为停车
操作
case 0x09: front = 0; back = 0; turnl = 0; turnr = 0; spinl = 0;
spinr = 0; turnoutput = 0; break; // 确保按键松开后为停车操

default: front = 0; back = 0; turnl = 0; turnr = 0; spinl = 0;
spinr = 0; turnoutput = 0; break;
}
}


on the way to be a Electrical Engineer & Designer......