智能车内部资料直立控制篇.docx
- 文档编号:24467007
- 上传时间:2023-05-27
- 格式:DOCX
- 页数:18
- 大小:57.77KB
智能车内部资料直立控制篇.docx
《智能车内部资料直立控制篇.docx》由会员分享,可在线阅读,更多相关《智能车内部资料直立控制篇.docx(18页珍藏版)》请在冰豆网上搜索。
智能车内部资料直立控制篇
智能车内部资料—直立控制篇
注:
不要将思维局限于以下的资料,任何方案都要进行辨证,以下资料仅限于参考。
关于智能车的直立控制,最早源于第七届电磁车,在看本篇之前,请参看《电磁组直立行车参考设计方案2.0》和《电磁组直立车模调试指南》,有不懂之处,需要多看几次技术文档,并且利用好网络的资源,XX、智能车论坛、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,1
Pdot[2]=-P[1][1];//1,0
Pdot[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;//Kk
K_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.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.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.167
Gyr_jiao=Gyr*0.18;//陀螺仪的角度0.052zenmtiaode
real_angle=Q*(real_angle+Gyr_jiao*dtc)+(1-Q)*(Acc_jiao);//0.01采样周期0.00956
GyrAccCra=real_angle;
}
滤波图像,黄色为融合后的曲线,蓝色为陀螺仪的曲线,红色为加速度计的曲线,上位机为虚拟示波器,需要配置协议,具体协议参看程序。
下图为官方的滤波方案,也就是本套程序所采用的方案。
三:
输出调用
s32AngleControl(void)
{
fValue=g_fCarAngle*ANGLE_CONTROL_P+g_fGyroscopeAngleSpeed*ANGLE_CONTROL_D;//角度控制系数
if(fValue>ANGLE_CONTROL_OUT_MAX)fValue=ANGLE_CONTROL_OUT_MAX;//4000
elseif(fValue g_fAngleControlOut=fValue; returng_fAngleControlOut; } #defineAANGPERIODFAV(5) //平滑输出 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++;//计算电机PWM TurnPeriodCount++; 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计数器初始值为0 FTM1_MOD=65535;//结束值 FTM1_QDCTRL|=FTM_QDCTRL_QUADEN_MASK;//启用FTM1正交解码模式 FTM1_MODE|=FTM_MODE_FTMEN_MASK;//FTM1EN=1 FTM1_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计数器初始值为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的类型,即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; else g_nLeftMotorPulse=-g_nLeftMotorPulse; g_nRightMotorPulseSigma+=g_nRightMotorPulse; g_nLeftMotorPulseSigma+=g_nLeftMotorPulse; } 速度PID: voidSpeedPID()//100ms控制一次 { LastSpeedCut0=(g_nLef
- 配套讲稿:
如PPT文件的首页显示word图标,表示该PPT已包含配套word讲稿。双击word图标可打开word文档。
- 特殊限制:
部分文档作品中含有的国旗、国徽等图片,仅作为作品整体效果示例展示,禁止商用。设计者仅对作品中独创性部分享有著作权。
- 关 键 词:
- 智能 内部 资料 直立 控制