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

智能车内部资料直立控制篇注：不要将思维局限于以下的资料，任何方案都要进行辨证，以下资料仅限于参考。关于智能车的直立控制，最早源于第七届电磁车，在看本篇之前，请参看电磁组直立行车参考设计方案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 。void AngleCalculate(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为加速度计时间补偿系数，一般在14之间，增大可使融合后的曲线减小加速度计的比重；GYROSCOPE_ANGLE_SIGMA_FREQUENCY为陀螺仪积分时间，数值为1000/（控制周期，一般为5ms），因此该值为1000/5=200；其余补充内容可以参看程序以及清华方案技术文档。2、 卡尔曼滤波（矩阵），跟随性强，车子调的比较硬，但不懂原理难以整定参数，而且运行比较占用时间，以下几个参数都需要整定。float Q_angle=0.001; /增大跟随好，噪点多float Q_gyro=0.003;/增大噪点多，跟随不及时float R_angle=0.5; /增大可去除噪点，但会跟随不及时float dt=0.005; /增大消除相位差float P22 = 1, 0 , 0, 1 ;float Pdot4 =0,0,0,0;float C_0 = 1;float q_bias, angle_err,PCt_0,PCt_1,E,t_0,t_1;float K_0;/含有卡尔曼增益的另外一个函数，用于计算最优估计值float K_1;/含有卡尔曼增益的函数，用于计算最优估计值的偏差float angle_m,gyro_m;void Kalman_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;/先验估计 /派生协方差矩阵计算 Pdot0=Q_angle - P01 - P10;/ Pk- 0,0 先验估计误差协方差的微分 Pdot1=- P11;/0,1 Pdot2=- P11;/1,0 Pdot3=Q_gyro;/1,1Pdot=A*P+P*A导+Q /协方差矩阵更新 P00 += Pdot0 * dt;/ Pk- 先验估计误差协方差微分的积分 = 先验估计误差协方差 P01 += Pdot1 * dt; P10 += Pdot2 * dt; P11 += Pdot3 * dt; /计算测量角度和估计角度的偏差 angle_err = angle_m - angle;/zk-先验估计 PCt_0 = C_0 * P00; PCt_1 = C_0 * P10; /误差估计计算 E = R_angle + C_0 * PCt_0;/估计偏差 /卡尔曼增益，E越大，K越小 K_0 = PCt_0 / E;/Kk K_1 = PCt_1 / E; /后验估计，更新协方差阵 t_0 = PCt_0; t_1 = C_0 * P01; P00 -= K_0 * t_0;/后验估计误差协方差 P01 -= K_0 * t_1; P10 -= K_1 * t_0; P11 -= 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、 卡尔曼滤波（非矩阵），此种方案很少使用。float Kg = 0,gyroscope_rate = 0,accelerometer_angle=0;void kalman_update(void) float Q =1,R = 300; static float RealData = 0,RealData_P = 0; float NowData = 0,NowData_P = 0; float Acc_x = 0,Gyro=0,Acc_z=0; /需要修改测试下面的三个值 1400 1360 1390- Acc_x = Acc_X_7260-2000;/x轴水平的输出值 此时车状态是水平放在桌子上 Acc_z = AAngleAccele3 - 2057.0;/加速度传感器的z轴 此时车状态是竖直放在桌子上 Gyro = (AAngleAccele2- GYROSCOPE_OFFSET); /陀螺仪直立的输出中立值 此时车状态是竖直放在桌子上 /-如果是硬件积分出角度的板子，那么就可以这样用完全可以acc_x这个引脚 /也就是ad1，把他接到传感器的J引脚，直接当做角度使用。中间完全跳过卡尔曼滤波 accelerometer_angle = atan2f(-Acc_x,Acc_z); gyroscope_rate = Gyro*0.00028;/0.0023 0.0070 /参考电压3.3v 12位ADC 放大9.1倍 enc-03 0.67mv/deg./sec. /RealData：上次得到的角度+陀螺仪得到的角速度*陀螺仪积分时间 /(3300/4096)/(0.67*9.1)*(3.14/180) = 0.0023 NowData = 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.044、 互补滤波，效果不太好用。void Datehandle() float Q =0.99,R = 0.01; /加速度计陀螺仪权重 float tau=0.005,dtc=0.005; Q=tau/(tau+dtc);/0.9375 /定义 前进为有单片机的方向 往前倾数减小 Acc=AAngleAccele3-GRAVITY_OFFSET ; /-90z轴1为垂直修正量 纠正垂直时的不确定 Gyr=(-xout+GYROSCOPE_OFFSET) *0.0625 ; / 陀螺仪的值 Acc_jiao=Acc*0.123; /z轴转过的角度 0.134 b-a=c 180/c=0.167 Gyr_jiao=Gyr*0.18; /陀螺仪的角度 0.052 zenm tiaode real_angle = Q*(real_angle + Gyr_jiao*dtc) + (1-Q)*(Acc_jiao); / 0.01 采样周期0.00956 GyrAccCra=real_angle;滤波图像，黄色为融合后的曲线，蓝色为陀螺仪的曲线，红色为加速度计的曲线，上位机为虚拟示波器，需要配置协议，具体协议参看程序。下图为官方的滤波方案，也就是本套程序所采用的方案。三：输出调用s32 AngleControl(void) fValue = g_fCarAngle* ANGLE_CONTROL_P +g_fGyroscopeAngleSpeed * ANGLE_CONTROL_D; /角度控制系数 if(fValue ANGLE_CONTROL_OUT_MAX) fValue = ANGLE_CONTROL_OUT_MAX;/4000 else if(fValue =5)/读取速度脉冲 5us TimeCount=0; GetMotorPulse(); else if(TimeCount = 1)/方向控制5us LastTurnPWMOUT = TurnPWMOUT ; TurnPeriodCount = 0 ; else if(TimeCount = 2)/进行融合20us AAngPeriodCount = 0; AngleCalculate();/计算加速值和角度值 LastAAngPWM = AAngPWM ; AAngPWM = AngleControl() ; /计算平衡电机速度 PWMout=MotorSpeedOut(AAngPWM,MotorSpeedPWM,MotorTurnPWM);/电机输出 MotorTurnPWM else if(TimeCount = 3)/速度pid控制5us ATimeCount + ; if(ATimeCount = 20) ATimeCount=0; SpeedPID() ; SpeedPeriodCount = 0 ; else if(TimeCount = 4)/3,3 28us 10,10 80us 6,4 44us AD值读取 AD_ACC_Z=ad_ave(ADC1,AD13,ADC_12bit,6); /加速度计 AD_GYRO =ad_ave(ADC1,AD15,ADC_12bit,4); /陀螺仪15 五：速度控制要进行速度控制，首先需要读取编码器的脉冲，由于K60自带正交解码的功能，因此输入编码器的AB相，就可以直接读取速度脉冲以及正反转。/*/ FTM1 编码器1 引脚 PTA 8-9/*void FTM1_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计数器初始值为0 FTM1_MOD=65535;/结束值 FTM1_QDCTRL|=FTM_QDCTRL_QUADEN_MASK;/启用FTM1正交解码模式 FTM1_MODE |= FTM_MODE_FTMEN_MASK;/FTM1EN=1 FTM1_CNT=0; /*/ FTM2 编码器2 引脚 PTA 10-11/*void FTM2_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计数器初始值为0 FTM2_MOD = 65535; / 结束值 FTM2_QDCTRL |= FTM_QDCTRL_QUADEN_MASK; / 启用FTM2正交解码模式 FTM2_MODE |= FTM_MODE_FTMEN_MASK; / FTM2EN=1 FTM2_CNT = 0; /注：g_nRightMotorPulse，g_nLeftMotorPulse，g_nRightMotorPulseSigma，g_nLeftMotorPulseSigma要定义为s16的类型，即short int类型。若用C、D车模，电机全速运行，GetMotorPulse（） 5ms读取一次，g_nRightMotorPulseSigma 100ms读取一次，g_nRightMotorPulseSigma的值大约在4000-6000之间，若读取结果不对，检查下编码器的AB相的线是否接对。void GetMotorPulse(void) /正交解码，可以读出正负 g_nRightMotorPulse =FTM2_CNT ; FTM2_CNT = 0; /及时清零 g_nLeftMotorPulse =FTM1_CNT ; FTM1_CNT = 0; /及时清零 if(g_nLeftMotorPulse0) g_nLeftMotorPulse = -g_nLeftMotorPulse; else g_nLeftMotorPulse = -g_nLeftMotorPulse; g_nRightMotorPulseSigma+=g_nRightMotorPulse; g_nLeftMotorPulseSigma +=g_nLeftMotorPulse;速度PID：void SpeedPID()/100ms控制一次 LastSpeedCut0 = ( g_nLeftMotorPulseSigma+g_nRightMotorPulseSigma)/2 ;/实际采集的脉冲数 g_nLeftMotorPulseSigma=g_nRi