图4.5.2 主板电路设计
第五章 智能车控制算法设计
5.1 控制流程图
本队所用的软件调试工具为CodeWarrior软件和BDM仿真器，这个软件支持C语言和汇编语言的混合编程，由于C语言操作简单，可修改和移植性强，所以大部分程序都使用C语言编写，只有在某些地方加入了汇编语句。编程基本上采用模块化的思想，使得整个设计清晰明了。下面是系统总体的软程图。
软件的主要功能包括有：
（1）各传感器信号的采集、处理； （2）电机 PWM输出；
（3）车模运行控制：直立控制、速度控制、方向控制；
（4）车模运行流程控制：程序初始化、车模启动与结束、车模状态监控； 上述功能可以分成两大类：
第一类包括 1-3功能，它们属于需要精确时间周期执行，因此可以在一个周期定时断里完成。第二类包括 4功能。它的执行不需要精确的时间周期。可以放在程序的主程序中完成。
31
第七届飞思卡尔智能汽车邀请赛技术报告
图5.1 系统软件流程图
中断函数实现
#pragma CODE_SEG __NEAR_SEG NON_BANKED
void interrupt VectorNumber_Vpit0 VPIT0(void){ //时间1ms
/* Write your code here ... */ int i;
asm(MOVB #$01,PITTF);
//-------------------------------------------------------------------------- EventCount_1MS ++;
//-------------------------------------------------------------------------- if(KeyDelayCounter > 0){ //按键延时 KeyDelayCounter--;
if(KeyDelayCounter == 0){ TFLG1_C4F = 1; TIE_C4I = 1; TFLG1_C5F = 1; TIE_C5I = 1; } }
32
//-------------------------------------------------------------------------- SpeedControlOutput(); SpeedControlPeriod ++; DirectionControlOutput(); DirectionControlPeriod ++;
//--------------------------------------------------------------------------
if(EventCount_1MS >= CONTROL_PERIOD) { // Motor CarSpeedSet adjust
EventCount_1MS = 0; // Clear the event counter; GetMotorPulse(); }
else if(EventCount_1MS == 1) { if(ADFlag) // Start ADC convert and Car erect adjust { for(i = 0; i < INPUT_VOLTAGE_AVERAGE; i ++) SampleCarVoltage(); } }
else if(EventCount_1MS == 2) // Get the voltage and start calculation {
GetCarVoltageAverage(); //---------------------------------------------------------------------- AngleCalculate(); AngleControl();
//---------------------------------------------------------------------- // Output motor control voltage; }
else if(EventCount_1MS == 3) { // Car CarSpeedSet adjust SpeedControlCount ++; if(SpeedControlCount >= SPEED_CONTROL_COUNT) { if(StartFlag) SpeedControl(); else { LeftMotorPulseSigma = RightMotorPulseSigma = 0; } SpeedControlCount = 0; SpeedControlPeriod = 0; } }
else if(EventCount_1MS == 4) { // Car direction control DirectionControlCount ++; DirectionVoltageSigma(); if(DirectionControlCount >= DIRECTION_CONTROL_COUNT) {
33
第七届飞思卡尔智能汽车邀请赛技术报告
if(Bit4) DirectionControl(); DirectionControlCount = 0; DirectionControlPeriod = 0; } }
if(ErectFlag) { MotorOutput(); } }
5.2 卡尔曼滤波角度算法
鉴于陀螺仪始终存在一定的温漂，导致中值需要频繁修改。倘若可以通过软件算法消除温漂，则可以为控制带来极大的便利之处，同时使得系统稳定性增加。因此需要对陀螺仪测得的角速度进行卡尔曼估计，算法如下：
void KalmanUpdate(float angle_m,float gyro_m) //gyro_m:gyro_measure {
static const float Q_angle=0.001, Q_gyro=0.003, R_angle=300, dt=0.005; //注意：dt的取值为kalman滤波器采样时间; static float P[2][2] = {{ 1, 0 },{ 0, 1 }}; static float Pdot[4] ={0,0,0,0}; static const char C_0 = 1;
static float q_bias, angle_err, PCt_0, PCt_1, E, K_0, K_1, t_0, t_1; //------------------------------------------------------- CarAngle+=(gyro_m-q_bias) * dt; 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; angle_err = angle_m - CarAngle; 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;
34
}
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;
CarAngle += K_0 * angle_err; q_bias += K_1 * angle_err; CarGyro = gyro_m-q_bias;
5.3直立控制算法
void AngleControl(void) {
float fValue;
/*-------------------------------------------------------------------------- if(CarAngle > MAX_ANGLE || CarAngle < -MAX_ANGLE) { LeftMotorOut = 0; RightMotorOut = 0; MotorSpeedOut(); CarStopFlag = 1; return; }
*///--------------------------------------------------------------------------
fValue = (CarAngleSet - CarAngle) * CAR_CONTROL_P - CarGyro * CAR_CONTROL_D;
if(fValue > ANGLE_CONTROL_OUT_MAX) fValue = ANGLE_CONTROL_OUT_MAX;
else if(fValue < ANGLE_CONTROL_OUT_MIN) fValue = ANGLE_CONTROL_OUT_MIN;
AngleControlOut = fValue; /*
OutData[0] = CarAngle * CAR_CONTROL_P; OutData[1] = CarGyro * CAR_CONTROL_D; OutData[2] = AngleControlOut; OutData[3] = CarSpeed; Process_Flag = 1;
35