双轮载人平衡车设计完整教程之软件篇

系统软件设计
5.1 系统初始化
为了防止程序运行过程中跑飞而造成的人身安全的伤害，系统主控制循环程序中添加独立看门狗程序，在程序死锁或跑飞状态下可自动复位，确保行车安全。系统初始化流程图如图5.1所示：

图5.1 系统初始化

5.2 数字滤波器设计
5.2.1卡尔曼滤波
卡尔曼滤波算法广泛地应用于智能机器人控制，航模传感器数据融合，卫星导航领域以及计算机图像处理当中。姿态传感器MPU-6050陀螺仪测量的角速度通过数学积分运算得到角度的信息，总是会存在微小的偏差和漂移变化所形成的累积误差，并且误差会随时间的增加逐步增大，到最后无法输出正确可控的角度信号。先对其偏差验证估计一个大概值，在用倾角减去刚才估计的八个大概值误差协方差的微分，后期验证再估计一个大概值，最后输出稳定的角度信息。经过多次试验，使用卡尔曼滤波算法融合得到的角度值非常稳定。部分程序代码如下所示。

void Kalman_Filter(float Accel,float Gyro)
{
angle+=(Gyro - Q_bias) * dt; //ÏÈÑé¹À¼Æ
Pdot[0]=Q_angle - PP[0][1] - PP[1][0];

Pdot[1]=-PP[1][1];
Pdot[2]=-PP[1][1];
Pdot[3]=Q_gyro;
PP[0][0] += Pdot[0] * dt;   
PP[0][1] += Pdot[1] * dt;  
PP[1][0] += Pdot[2] * dt;
PP[1][1] += Pdot[3] * dt;
	
Angle_err = Accel - angle;	

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;	

}

void Yijielvbo(float angle_m, float gyro_m)
{
angle = K1 * angle_m+ (1-K1) * (angle + gyro_m * 0.005);
}

5.2.2一介低通滤波
机械齿轮的减速电机是存在一定的控制死区的，我们没有必要把它控制在一个精度非常高的范围里面，这会让电机产生强烈的震动、更严重的会烧毁电机等不可逆转的损害。恰恰相反，我们可以利用减速电机这存在的死区，适当的减缓速度的变化，可以有效的降低速度控制对直立控制的干扰。在自平衡系统里，直立平衡才是最主要的，其他一切的控制相对平衡来说都算是一种干扰，干扰越大，稳定性越差[6]。所以我们对编码器所测得的电机转速信息乘以0.8后再赋值给编码器输出，对速度偏差乘以0.2后赋值给新的速度偏差变量。相关代码如下所示：

Encode = 0.8; //对Encode进行低通滤波
Encode += Encode_Least0.2; //对Encode偏差进行低通滤波

5.3 数字PID控制器
5.3.1直立环PD控制
相对于平衡车而言，所有的功能都是建立在车身平衡的基础上的。平衡是主要，其他所有的附加功能相较于直立来说都是干扰[7]。首先定义程序入口参数Angle(角度), Gyro(角速度)，Bias(偏差)为浮点型变量，比例值，微分值可以先行估计一个大概值，方便后期调试。定义使得系统平衡的平衡(Balance)变量为整形数据。其次估计车身的机械安装中值为0(姿态传感器安装在绝对零点位置的时候)，用陀螺仪所测得的实时变化的角度减去机械安装中值并赋值给偏差变量；再对偏差乘以比例值并加上角速度乘以微分值赋值给平衡PWM波，最后返回的是整形的平衡PWM波值。程序设计如图5.3.1所示。

图5.3.1 直立环PD控制代码
以上为角度闭环控制也就是直立PD控制的关键部分程序，一般的控制系统而言，只需