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

智能车内部资料—直立控制篇注：不要将思维局限于以下的资料，任何方案都要进行辨证，以下资料仅限于参考。关于智能车的直立控制，最早源于第七届电磁车，在看本篇之前，请参看《电磁组直立行车参考设计方案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种方法经过筛选，最终选择了清华方案，以下将介绍这几种方法。1、清华方案，具体原理看《电磁组直立行车参考设计方案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；其余补充内容可以参看程序以及清华方案技术文档。2、卡尔曼滤波（矩阵），跟随性强，车子调的比较硬，但不懂原理难以整定参数，而且运行比较占用时间，以下几个参数都需要整定。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];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;//P（K|K）E越大，P越大//更新状态估计angle+=K_0*angle_err;//后验估计q_bias+=K_1*angle_err;//后验估计angle_dot=gyro_m-q_bias;//输出值（后验估计）的微分=角速度g_fCarAngle=angle;g_fGyroscopeAngleSpeed=angle_dot;}3、卡尔曼滤波（非矩阵），此种方案很少使用。floatKg=0,gyroscope_rate=0,accelerometer_angle=0;voidkalman_update(void){floatQ=1,R=300;staticfloatRealData=0,RealData_P=0;floatNowData=0,NowData_P=0;floatAcc_x=0,Gyro=0,Acc_z=0;//需要修改测试下面的三个值140013601390-----Acc_x=Acc_X_7260-2000;//x轴水平的输出值此时车状态是水平放在桌子上Acc_z=AAngleAccele[3]-2057.0;//加速度传感器的z轴此时车状态是竖直放在桌子上Gyro=(AAngleAccele[2]-GYROSCOPE_OFFSET);//陀螺仪直立的输出中立值此时车状态是竖直放在桌子上//--如果是硬件积分出角度的板子，那么就可以这样用完全可以acc_x这个引脚//也就是ad1，把他接到传感器的J引脚，直接当做角度使用。中间完全跳过卡尔曼滤波accelerometer_angle=atan2f(-Acc_x,Acc_z);gyroscope_rate=Gyro*0.00028;//0.00230.0070//参考电压3.3v12位ADC放大9.1倍enc-030.67mv/deg./sec.//RealData：上次得到的角度+陀螺仪得到的角速度*陀螺仪积分时间//(3300/4096)/(0.67*9.1)*(3.14/180)=0.0023NowData=RealData+gyroscope_rate*0.0001;//0.0001//1.预估计X(k|k-1)=A(k,k-1)*X(k-1|k-1)+B(k)*u(k)NowData_P=sqrt(Q*Q+RealData_P*RealData_P);//2.计算预估计协方差矩阵P(k|k-1)=A(k,k-1)*P(k-1|k-1)*A(k,k-1)'+Q(k)Kg=sqrt(NowData_P*NowData_P/(NowData_P*NowData_P+R*R));//3.计算卡尔曼增益矩阵K(k)=P(k|k-1)*H(k)'/(H(k)*P(k|k-1)*H(k)'+R(k))RealData=NowData+Kg*(accelerometer_angle-NowData);//4.更新估计X(k|k)=X(k|k-1)+K(k)*(Z(k)-H(k)*X(k|k-1))RealData_P=sqrt((1-Kg)*NowData_P*NowData_P);//5.计算更新后估计协防差矩阵P(k|k)=（I-K(k)*H(k)）*P(k|k-1)QingJiao=RealData-0.04;//1.57是90度的值0.04}4、互补滤波，效果不太好用。voidDatehandle(){floatQ=0.99,R=0.01;//加速度计陀螺仪权重floattau=0.005,dtc=0.005;Q=tau/(tau+dtc);//0.9375//定义前进为有单片机的方向往前倾数减小Acc=AAngleAccele[3]-GRAVITY_OFFSET;//-90z轴1为垂直修正量纠正垂直时的不确定Gyr=(-xout+GYROSCOPE_OFFSET)*0.0625;//陀螺仪的值Acc_jiao=Acc*0.123;//z轴转过的角度0.134b-a=c180/c=0.167Gyr_jiao=Gyr*0.18;//陀螺仪的角度0.052zenmtiaodereal_angle=Q*(real_angle+Gyr_jiao*dtc)+(1-Q)*(Acc_jiao);//0.01采样周期0.00956GyrAccCra=real_angle;}滤波图像，黄色为融合后的曲线，蓝色为陀螺仪的曲线，红色为加速度计的曲线，上位机为虚拟示波器，需要配置协议，具体协议参看程序。下图为官方的滤波方案，也就是本套程序所采用的方案。三：输出调用s32AngleControl(void){fValue=g_fCarAngle*ANGLE_CONTROL_P+g_fGyroscopeAngleSpeed*ANGLE_CONTROL_D;//角度控制系数if(fValueANGL