单片机控制的自平衡小车设计探讨

Gyro1 - Gyro1_zero; //陀螺仪AD采集值减去陀螺仪零偏值

Gyro_Data = Gyro1;

accelerometer_angle= Acc_z*180/(47915.71-12843.7); //加速度计计算出的角度 归一化到-90 到+90

gyroscope_rate = Gyro1*0.0235*0.005; //0.0235 是转换角度的比例值 0.005是控制周期

gyroscope_rat =gyroscope_rat -Gyro1*0.0235*0.005; //积分角速度得到角度

//卡尔曼五个公式的算法实现

NowData = RealData -gyroscope_rate;

NowData_P = Q+RealData_P;

Kg = NowData_P/(NowData_P+R);

RealData = NowData + Kg*(accelerometer_angle - NowData);

RealData_P = (1-Kg)*NowData_P;

QingJiao = RealData; //将准确角度结果给QingJiao

}

}

整个调试过程有三个参数需要调整，Q R 及那个0.0235 。具体的调试这个真是说不清楚，往往算法的调试都是经验，尝试多了就有规律了。个人感觉做到这，就得用去三分之二时间。

好了，假如你已经得到准确角度，自然是开始以此作为控制量，那我们要控制成啥样？想一想也知道是要把这个角度值控制成0度（我自己将直立时定义为0度），那么自然使用常用的PID算法，偏差自然就是QingJiao-0=QingJiao,当然你可以反过来，这其实根据你自己对方向的定义。我们来个最简单的位置式PD算法：

fValue = (float) P *QingJiao -(float) D*Gyro_Data;

P就是PID的P参数 D就是PID的D参数，QingJiao反映幅度，Gyro_Data反映快慢。这也是需要不断调试出来的。再把fvalue值给PWM输出就可以了。实际在做的时候，往往没那么简单，所以一定要一步一步做好之后再做后面的，假如你第二部没做好，在第三部时你怎么也直立不起来，你不知道到底是PD参数不对，还是卡尔曼出来的角度本身不准。所以个人感觉，得到准确角度是整个过程至关重要的一步。小车的直立是一直是一个动态过程，即使最好的状态一动不动，也是在动态控制中，只是看不出而已。

以上只针对直立控制，即最基本的自平衡。