智能车内部资料-直立控制篇

智能车内部资料—直立控制篇注：不要将思维局限于以下的资料，任何方案都要进行辨证，以下资料仅限于参考。关于智能车的直立控制，最早源于第七届电磁车，在看本篇之前，请参看《电磁组直立行车参考设计方案2.0》和《电磁组直立车模调试指南》，有不懂之处，需要多看几次技术文档，并且利用好网络的资源，百度、智能车论坛、CSDN（需要淘宝购买积分）、PUDN这几个网站是制作智能车的好助手，可以搜集的几乎所有做车过程中遇到的问题及资料，值得一提的是，要时刻与智能车论坛保持联系，不懂的问题可以在上面发问，只要不是太幼稚的问题，都会得到很多人的解答，这样可以认识很多人，要和论坛的一些大神保持联系，定期询问进度（加QQ，要电话号码…）,在做车过程中，发现东北赛区的同学比较热情，乐于分享。好了，废话不多说了。一：AD采集 关于直立控制，需要用到陀螺仪和加速度计，通过与网友的交流，发现使用模拟模块比数字模块的要多，很多网友反映IIC通信占用时间比较长，而直立控制对时间要求比较苛刻，因此在本届比赛中我们使用的是模拟模块（学弟学妹们可以尝试比较一下优缺点，毕竟那个enc-03的陀螺仪确实很渣），事实上AD读取花费的时间也不少，AD_ACC_Z=ad_ave(ADC1,AD13,ADC_12bit,6);//加速度AD_GYRO=ad_ave(ADC1,AD15,ADC_12bit,4);//陀螺仪以分别读取6次和4次来说，大约44us，若分别读取10和10次，大约80us，若分别读取3和3次，大约28us（关于时间测量，在后面的文档中将会提及）。二：滤波融合 AD读取完成后，需要对两者（即加速度计和陀螺仪）的信号进行滤波、融合，关于滤波方法大致分为清华方案、互补滤波、卡尔曼滤波，以上3种方案共4种方法经过筛选，最终选择了清华方案，以下将介绍这几种方法。清华方案，具体原理看《电磁组直立行车参考设计方案2.0》。voidAngleCalculate(void){g_fGravityAngle=((AD_ACC_Z-GRAVITY_OFFSET)*GRAVITY_ANGLE_RATIO);g_fGyroscopeAngleSpeed=(AD_GYRO-GYROSCOPE_OFFSET)*GYROSCOPE_ANGLE_RATIO;g_fCarAngle=fGyroscopeAngleIntegral;fDeltaValue=(g_fGravityAngle-g_fCarAngle)/GRAVITY_ADJUST_TIME_CONSTANT;fGyroscopeAngleIntegral+=(g_fGyroscopeAngleSpeed+fDeltaValue)/GYROSCOPE_ANGLE_SIGMA_FREQUENCY;}其中，g_fGravityAngle为加速度计倾角；AD_ACC_Z为加速计的Z轴的AD值；GRAVITY_OFFSET为加速度计Z轴的偏移量，为定值，通过改变该值的大小调整直立时的平衡位置；GRAVITY_ANGLE_RATIO为加速度计Z轴归一化比例系数，具体测量方法是将加速计模块平放于桌面上，读取AD值，记为A值，然后将模块反过来再读取一次AD值，记为B值，然后用180/(B-A),结果就是GRAVITY_ANGLE_RATIO（为正值），由于测量时会存在误差，后期可以稍作调整，也可不用调整；AD_GYRO为陀螺仪的AD值；GYROSCOPE_OFFSET为陀螺仪的零偏，由于陀螺仪存在温飘，因此这个值每次开机需要自动采集零偏，GYROSCOPE_OFFSET=ad_ave(ADC1,AD15,ADC_12bit,200)；GYROSCOPE_ANGLE_RATIO为陀螺仪归一化系数，需要根据滤波图像进行整定；GRAVITY_ADJUST_TIME_CONSTANT为加速度计时间补偿系数，一般在1~4之间，增大可使融合后的曲线减小加速度计的比重；GYROSCOPE_ANGLE_SIGMA_FREQUENCY为陀螺仪积分时间，数值为1000/（控制周期，一般为5ms），因此该值为1000/5=200；其余补充内容可以参看程序以及清华方案技术文档。卡尔曼滤波（矩阵），跟随性强，车子调的比较硬，但不懂原理难以整定参数，而且运行比较占用时间，以下几个参数都需要整定。floatQ_angle=0.001;//增大跟随好，噪点多floatQ_gyro=0.003;//增大噪点多，跟随不及时floatR_angle=0.5;//增大可去除噪点，但会跟随不及时floatdt=0.005;//增大消除相位差floatP[2][2]={{1,0},{0,1}}; floatPdot[4]={0,0,0,0};floatC_0=1;floatq_bias,angle_err,PCt_0,PCt_1,E,t_0,t_1;floatK_0;//含有卡尔曼增益的另外一个函数，用于计算最优估计值floatK_1;//含有卡尔曼增益的函数，用于计算最优估计值的偏差floatangle_m,gyro_m;voidKalman_Filter() {g_fGravityAngle=((AD_ACC_Z-GRAVITY_OFFSET)*GRAVITY_ANGLE_RATIO);//g_fGyroscopeAngleSpeed=(AD_GYRO+GYROSCOPE_OFFSET)*GYROSCOPE_kalman_RATIO6050;g_fGyroscopeAngleSpeed=(AD_GYRO-GYROSCOPE_OFFSET)*GYROSCOPE_kalman_RATIO;angle_m=g_fGravityAngle;gyro_m=g_fGyroscopeAngleSpeed;//角度更新angle+=(gyro_m-q_bias)*dt;//先验估计//派生协方差矩阵计算Pdot[0]=Q_angle-P[0][1]-P[1][0];//Pk-'0,0先验估计误差协方差的微分Pdot[1]=-P[1][1];//0,1Pdot[2]=-P[1][1];//1,0Pdot[3]=Q_gyro;//1,1Pdot=A*P+P*A导+Q//协方差矩阵更新P[0][0]+=Pdot[0]*dt;//Pk-先验估计误差协方差微分的积分=先验估计误差协方差P[0][1]+=Pdot[1]*dt;P[1][0]+=Pdot[2]*dt;P[1][1]+=Pdot[3]*dt; //计算测量角度和估计角度的偏差angle_err=angle_m-angle;//zk-先验估计PCt_0=C_0*P[0][0];PCt_1=C_0*P[1][0];//误差估计计算E=R_angle+C_0*PCt_0;//估计偏差//卡尔曼增益，E越大，K越小K_0=PCt_0/E;//KkK_1=PCt_1/E;//后验估计，更新协方差阵t_0=PCt_0;t_1=C_0*P[0][1];s32AAangPWMOut(s32NewAangPWM,s32LastAangPWM,s32PeriodCount){s32AangPWMfav,AangOUT;AangPWMfav=NewAangPWM-LastAangPWM;AangOUT=AangPWMfav*(PeriodCount+1)/AANGPERIODFAV+LastAangPWM;returnAangOUT;}参数整定：先增大ANGLE_CONTROL_P，不要一点一点的加，看不出效果，等到小车有立起来的效果时，再逐渐增大，慢慢的会来回摇晃，这时整定ANGLE_CONTROL_D，增大D可以使小车平稳很多，过大会造成小车震动，就是打齿声比较严重，通过几次来回整定P和D，得到一组参数，可以使小车立的比较硬，如果小车总是朝某一方向跑，需要整定GRAVITY_OFFSET加速度计的零偏，直到小车在平衡位置来回摆动。注意：若摆动车子，虽有正反转，但发现轮子方向旋转与实际相反，需要将电机线对调一下。若整定好加速度计的零偏后，重新上电仍然不确定性的朝某个方向跑，要检查加速度计的安装位置，要确保小车处于平衡状态下，传感器要竖直安装！四：直立控制时序设定一个1ms的定时器，每一ms执行一次run函数。voidrun(){TimeCount++;SpeedPeriodCount++;//计算电机PWMTurnPeriodCount++;AAngPeriodCount++;MotorTurnPWM=TurnPWMOut(TurnPWMOUT,LastTurnPWMOUT,TurnPeriodCount);MotorSpeedPWM=SpeedPWMOut(g_SpeedControlOutNew,g_SpeedControlOutOld,SpeedPeriodCount);MotorAAngPWM=AAangPWMOut(AAngPWM,LastAAngPWM,AAngPeriodCount);Checkcarstate();if(TimeCount>=5)//读取速度脉冲5us{TimeCount=0;GetMotorPulse();}elseif(TimeCount==1)//方向控制5us{LastTurnPWMOUT=TurnPWMOUT;TurnPeriodCount=0;}elseif(TimeCount==2)//进行融合20us{AAngPeriodCount=0;AngleCalculate();//计算加速值和角度值LastAAngPWM=AAngPWM;AAngPWM=AngleControl();//计算平衡电机速度PWMout=MotorSpeedOut(AAngPWM,MotorSpeedPWM,MotorTurnPWM);//电机输出MotorTurnPWM}elseif(TimeCount==3)//速度pid控制5us{ATimeCount++;if(ATimeCount>=20){ATimeCount=0;SpeedPID();SpeedPeriodCount=0;}}elseif(TimeCount==4)//3,328us10,1080us6,444usAD值读取{AD_ACC_Z=ad_ave(ADC1,AD13,ADC_12bit,6);//加速度计AD_GYRO=ad_ave(ADC1,AD15,ADC_12bit,4);//陀螺仪15}}五：速度控制要进行速度控制，首先需要读取编码器的脉冲，由于K60自带正交解码的功能，因此输入编码器的AB相，就可以直接读取速度脉冲以及正反转。//*****************************************************************************//FTM1编码器1引脚PTA8-9//*****************************************************************************voidFTM1_QUAD_Iint(void){PORTA_PCR8=PORT_PCR_MUX(6);//设置引脚A12引脚为FTM1_PHA功能PORTA_PCR9=PORT_PCR_MUX(6);//设置引脚A13引脚为FTM1_PHB功能PORT_PCR_REG(PORTA_BASE_PTR,8)|=PORT_PCR_PE_MASK|PORT_PCR_PS_MASK;//开弱上拉PORT_PCR_REG(PORTA_BASE_PTR,9)|=PORT_PCR_PE_MASK|PORT_PCR_PS_MASK;//开弱上拉SIM_SCGC6|=SIM_SCGC6_FTM1_MASK;//使能FTM1时钟FTM1_MODE|=FTM_MODE_WPDIS_MASK;//写保护禁止FTM1_QDCTRL|=FTM_QDCTRL_QUADMODE_MASK;//AB相同时确定方向和计数值FTM1_CNTIN=0;//FTM0计数器初始值为0FTM1_MOD=65535;//结束值FTM1_QDCTRL|=FTM_QDCTRL_QUADEN_MASK;//启用FTM1正交解码模式FTM1_MODE|=FTM_MODE_FTMEN_MASK;//FTM1EN=1FTM1_CNT=0;}//*****************************************************************************//FTM2编码器2引脚PTA10-11//*****************************************************************************voidFTM2_QUAD_Iint(void){PORTA_PCR10=PORT_PCR_MUX(6);//设置引脚A10引脚为FTM2_PHA功能PORTA_PCR11=PORT_PCR_MUX(6);//设置引脚A11引脚为FTM2_PHB功能PORT_PCR_REG(PORTA_BASE_PTR,10)|=PORT_PCR_PE_MASK|PORT_PCR_PS_MASK;//开弱上拉PORT_PCR_REG(PORTA_BASE_PTR,11)|=PORT_PCR_PE_MASK|PORT_PCR_PS_MASK;//开弱上拉SIM_SCGC3|=SIM_SCGC3_FTM2_MASK;//使能FTM2时钟FTM2_MODE|=FTM_MODE_WPDIS_MASK;//写保护禁止FTM2_QDCTRL|=FTM_QDCTRL_QUADMODE_MASK;//AB相同时确定方向和计数值FTM2_CNTIN=0;//FTM0计数器初始值为0FTM2_MOD=65535;//结束值FTM2_QDCTRL|=FTM_QDCTRL_QUADEN_MASK;//启用FTM2正交解码模式FTM2_MODE|=FTM_MODE_FTMEN_MASK;//FTM2EN=1FTM2_CNT=0;}//注：g_nRightMotorPulse，g_nLeftMotorPulse，g_nRightMotorPulseSigma，g_nLeftMotorPulseSigma要定义为s16的类型，即shortint类型。若用C、D车模，电机全速运行，GetMotorPulse（）5ms读取一次，g_nRightMotorPulseSigma100ms读取一次，g_nRightMotorPulseSigma的值大约在4000-6000之间，若读取结果不对，检查下编码器的AB相的线是否接对。voidGetMotorPulse(void)//正交解码，可以读出正负{g_nRightMotorPulse=FTM2_CNT;FTM2_CNT=0;//及时清零g_nLeftMotorPulse=FTM1_CNT;FTM1_CNT=0;//及时清零if(g_nLeftMotorPulse>0) g_nLeftMotorPulse=-g_nLeftMotorPulse;elseg_nLeftMotorPulse=-g_nLeftMotorPulse;g_nRightMotorPulseSigma+=g_nRightMotorPulse;g_nLeftMotorPulseSigma+=g_nLeftMotorPulse;}速度PID：voidSpeedPID()//100ms控制一次{LastSpeedCut0=(g_nLeftMotorPulseSigma+g_nRightMotorPulseSigma)/2;//实际采集的脉冲数g_nLeftMotorPulseSigma=g_nRightMotorPulseSigma=0;if(g_nSpeedControlFlag==0){SpeedPWMKI=0;SpeedPWMKP=0;LastSpeedCut0=DSpeed;g_SpeedControlIntegral=0;}DSpeed=g_Speedgoal;//设定速度SpeedDifference0=LastSpeedCut0-DSpeed;SpeedPWMKP=SpeedKP*SpeedDifference0;SpeedPWMKI=SpeedKI*SpeedDifference0;g_SpeedControlIntegral+=SpeedPWMKI;if(g_SpeedControlIntegral>=Pre_SPEED_OUT_MAX_I)//需要对积分相进行限幅，若限制太大，小车难以加速{g_SpeedControlIntegral=Pre_SPEED_OUT_MAX_I;}if(g_SpeedControlIntegral<=Pre_SPEED_OUT_MIN_I){g_SpeedControlIntegral=Pre_SPEED_OUT_MIN_I;}g_SpeedControlOutOld=g_SpeedControlOutNew;g_SpeedControlOutNew=SpeedPWMKP+g_SpeedControlIntegral;}速度平滑输出：#defineSPEEDPERIODFAV100//速度控制周期s32SpeedPWMOut(s32NewspeedPWM,s32LastspeedPWM,s32PeriodCount){s32speedPWMfav,SpeedOUT;speedPWMfav=NewspeedPWM-LastspeedPWM;SpeedOUT=speedPWMfav*(PeriodCount+1)/SPEEDPERIODFAV+LastspeedPWM;returnSpeedOUT;}关于参数整定：直立控制的速度PID只需要使用PI控制即可，在整定参数时根据出现的现象调整参数，P过大，小车过弯道时出现点头现象，当然速度要在1m/s以上，I过大小车一走一停，速度不均匀，直立车速度在2m/s以下不要考虑直道加速，弯道减速，这样会造成小车的不稳定，反而丧失了速度，如果你能以2m/s的速度匀速跑完，事实上参数已经整定的不错了。一般来说，一组速度PI参数可以适应高速和低速，因此，这套参数不需要根据速度不同而多设几组。参数整定的方法有很多，如果仅仅是利用肉眼观察，是不容易知道小车是否匀速，这时有几种方法可以借鉴。第一种，由于速度PI是100ms控制一次，因此每100ms把编码器测的速度存到SD卡中，这里需要注意的是，对SD卡不要写文件（如.txt），而是要写扇区，写文件太费时间了，如果是摄像头组的同学，建议好好研究下SD卡如何写扇区，如何读取（用winhex），自己制作一套上位机（VB就足够），用来读取。此种方法的缺点是需要插拔SD卡，不方便。第二种方法，就是把编码器的脉冲存入单片机的一个数组，等到跑完一圈后，让小车发送到虚拟示波器上，这种方法比较可靠、方便。注意：不要期望小车边跑边发脉冲