|
控制器:ATmega16;8MHz;
加速度傳感器:MMA2260;陀螺儀:EWTS82;
傳感器的融合:卡爾曼濾波;
馬達(dá):EN_2342CR(速比64)+雙路12脈沖編碼器+CD40106對(duì)信號(hào)整形;
驅(qū)動(dòng)板芯片:CD4001+IR2111+IRF1404(驅(qū)動(dòng)電流可以很大);
制作資料在附件壓縮包里面,另外這篇文章http://www.zg4o1577.cn/bbs/dpj-47882-1.html 供參考
自己做的自平衡小車,基本達(dá)到預(yù)期效果,圖片(拍攝效果不佳)及文件如下,視頻請(qǐng)見(jiàn):
http://v.youku.com/v_show/id_XMTc3ODg0Mjky.html
http://v.youku.com/v_show/id_XMTgxNTAxOTA4.html
http://v.youku.com/v_show/id_XMTgxNTAzMTI4.html
資料供大家參考;
ourdev_562071.jpg (79.8 KB, 下載次數(shù): 227)
下載附件
2016-4-10 03:04 上傳
照片 (原文件名:RII_2.jpg)
ourdev_562072.jpg (67.65 KB, 下載次數(shù): 217)
下載附件
2016-4-10 03:04 上傳
照片 (原文件名:RII_1.jpg)
ourdev_562073.jpg (58.48 KB, 下載次數(shù): 206)
下載附件
2016-4-10 03:04 上傳
照片 (原文件名:RII_3.jpg)
今天做了取消馬達(dá)轉(zhuǎn)速傳感器的測(cè)試,利用PWM的值低通濾波后代替轉(zhuǎn)速信號(hào),也可以穩(wěn)定控制,但在移動(dòng)距離偏差較大時(shí)不能修正,小范圍內(nèi)和利用傳感器閉環(huán)控制差不多;源文件如下
- //MCU:Mega16;晶振:8MHz;
- //PWM:4KHz;濾波器頻率:100Hz;系統(tǒng)頻率:100Hz;10ms;
- //趙國(guó)霖:10.05.10;
- #include
- #include
- #include
- //#define checkbit(var,bit) (var&(0x01<<(bit))) /*定義查詢位函數(shù)*/
- //#define setbit(var,bit) (var|=(0x01<<(bit))) /*定義置位函數(shù)*/
- //#define clrbit(var,bit) (var&=(~(0x01<<(bit)))) /*定義清零位函數(shù)*/
- //-------------------------------------------------------
- //輸出端口初始化
- void PORT_initial(void)
- {
- DDRA=0B00000000;
- PINA=0X00;
- PORTA=0X00;
-
- DDRB=0B00000000;
- PINB=0X00;
- PORTB=0X00;
-
- DDRC=0B00010000;
- PINC=0X00;
- PORTC=0X00;
-
- DDRD=0B11110010;
- PIND=0X00;
- PORTD=0X00;
- }
- //-------------------------------------------------------
- //定時(shí)器1初始化
- void T1_initial(void)
- {
- TCCR1A|=(1<<com1a1)|(1<<wgm10)|(1<<com1b1); t1:8位快速pwm模式、匹配時(shí)清0,top時(shí)置位
- TCCR1B|=(1<<wgm12)|(1<<cs11); pwm:8分頻:8m="" 8="" 256="4KHz;
- }
- //-------------------------------------------------------
- //定時(shí)器2初始化
- void T2_initial(void) //T2:計(jì)數(shù)至OCR2時(shí)產(chǎn)生中斷
- {
- OCR2=0X4E; //T2:計(jì)數(shù)20ms(0X9C)10ms(0X4E)時(shí)產(chǎn)生中斷;
- TIMSK|=(1<<ocie2);
- TCCR2|=(1<<wgm21)|(1<<cs22)|(1<<cs21)|(1<<cs20); ctc模式,1024分頻
- }
- //-------------------------------------------------------
- //外部中斷初始化
- void INT_initial(void)
- {
- MCUCR|=(1<<isc01)|(1<<isc00)|(1<<isc11)|(1<<isc10); int0、int1上升沿有效
- GICR|=(1<<int0)|(1<<int1); 外部中斷使能
- }
- //-------------------------------------------------------
- //串口初始化;
- void USART_initial( void )
- {
- UBRRH = 0X00;
- UBRRL = 51; //f=8MHz;設(shè)置波特率:9600:51;19200:25;
- UCSRB = (1<<rxen)|(1<<txen); 接收器與發(fā)送器使能;
- UCSRC = (1<<ursel)|(1<<ucsz0)|(1<<ucsz1); 設(shè)置幀格式:="" 8="" 個(gè)數(shù)據(jù)位,="" 1="" 個(gè)停止位;
-
- UCSRB|=(1<<rxcie); usart接收中斷使能
- }
- //-------------------------------------------------------
- //串口發(fā)送數(shù)據(jù);
- void USART_Transmit( unsigned char data )
- {
- while ( !( UCSRA & (1<<udre))); 等待發(fā)送緩沖器為空;
- UDR = data; //將數(shù)據(jù)放入緩沖器,發(fā)送數(shù)據(jù);
- }
- //-------------------------------------------------------
- //串口接收數(shù)據(jù)中斷,確定數(shù)據(jù)輸出的狀態(tài);
- #pragma interrupt_handler USART_Receive_Int:12
- static char USART_State;
- void USART_Receive_Int(void)
- {
- USART_State=UDR;//USART_Receive();
- }
- //-------------------------------------------------------
- //計(jì)算LH側(cè)輪速:INT0中斷;
- //-------------------------------------------------------
- static int speed_real_LH;
- //-------------------------------------------------------
- #pragma interrupt_handler SPEEDLHINT_fun:2
- void SPEEDLHINT_fun(void)
- {
- if (0==(PINB&BIT(0)))
- {
- speed_real_LH-=1;
- }
- else
- {
- speed_real_LH+=1;
- }
- }
- //-------------------------------------------------------
- //計(jì)算RH側(cè)輪速,:INT1中斷;
- //同時(shí)將輪速信號(hào)統(tǒng)一成前進(jìn)方向了;
- //-------------------------------------------------------
- static int speed_real_RH;
- //-------------------------------------------------------
- #pragma interrupt_handler SPEEDRHINT_fun:3
- void SPEEDRHINT_fun(void)
- {
- if (0==(PINB&BIT(1)))
- {
- speed_real_RH+=1;
- }
- else
- {
- speed_real_RH-=1;
- }
- }
- //-------------------------------------------------------
- //ADport采樣:10位,采樣基準(zhǔn)電壓Aref
- //-------------------------------------------------------
- static int AD_data;
- //-------------------------------------------------------
- int ADport(unsigned char port)
- {
- ADMUX=port;
- ADCSRA|=(1<<aden)|(1<<adsc)|(1<<adps1)|(1<<adps0); 采樣頻率為8分頻;
- while(!(ADCSRA&(BIT(ADIF))));
- AD_data=ADCL;
- AD_data+=ADCH*256;
- AD_data-=512;
- return (AD_data);
- }
- //*
- //-------------------------------------------------------
- //Kalman濾波,8MHz的處理時(shí)間約1.8ms;
- //-------------------------------------------------------
- static float angle, angle_dot; //外部需要引用的變量
- //-------------------------------------------------------
- static const float Q_angle=0.001, Q_gyro=0.003, R_angle=0.5, dt=0.01;
- //注意:dt的取值為kalman濾波器采樣時(shí)間;
- 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;
- //-------------------------------------------------------
- void Kalman_Filter(float angle_m,float gyro_m) //gyro_m:gyro_measure
- {
- angle+=(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 - angle;
-
-
- 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;
-
- 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;
-
-
- angle += K_0 * angle_err;
- q_bias += K_1 * angle_err;
- angle_dot = gyro_m-q_bias;
- }
- //*/
- /*
- //-------------------------------------------------------
- //互補(bǔ)濾波
- //-------------------------------------------------------
- static float angle,angle_dot; //外部需要引用的變量
- //-------------------------------------------------------
- static float bias_cf;
- static const float dt=0.01;
- //-------------------------------------------------------
- void complement_filter(float angle_m_cf,float gyro_m_cf)
- {
- bias_cf*=0.998; //陀螺儀零飄低通濾波;500次均值;
- bias_cf+=gyro_m_cf*0.002;
- angle_dot=gyro_m_cf-bias_cf;
- angle=(angle+angle_dot*dt)*0.90+angle_m_cf*0.05;
- //加速度低通濾波;20次均值;按100次每秒計(jì)算,低通5Hz;
- }
- */
- //-------------------------------------------------------
- //AD采樣;
- //以角度表示;
- //加速度計(jì):1.2V=1g=90°;滿量程:1.3V~3.7V;
- //陀螺儀:0.5V~4.5V=-80°~+80°;滿量程5V=200°=256=200°;
- //-------------------------------------------------------
- static float gyro,acceler;
- //-------------------------------------------------------
- void AD_calculate(void)
- {
-
- acceler=ADport(2)+28; //角度校正
- gyro=ADport(3);
-
- acceler*=0.004069; //系數(shù)換算:2.5/(1.2*512);
- acceler=asin(acceler);
- gyro*=0.00341; //角速度系數(shù):(3.14/180)* 100/512=0.01364;
-
- Kalman_Filter(acceler,gyro);
- //complement_filter(acceler,gyro);
- }
-
- //-------------------------------------------------------
- //PWM輸出
- //-------------------------------------------------------
- void PWM_output (int PWM_LH,int PWM_RH)
- {
- if (PWM_LH<0)
- {
- PORTD|=BIT(6);
- PWM_LH*=-1;
- }
- else
- {
- PORTD&=~BIT(6);
- }
-
- if (PWM_LH>252)
- {
- PWM_LH=252;
- }
-
-
- if (PWM_RH<0)
- {
- PORTD|=BIT(7);
- PWM_RH*=-1;
- }
- else
- {
- PORTD&=~BIT(7);
- }
-
- if (PWM_RH>252)
- {
- PWM_RH=252;
- }
-
-
- OCR1AH=0;
- OCR1AL=PWM_LH; //OC1A輸出;
-
- OCR1BH=0;
- OCR1BL=PWM_RH; //OC1B輸出;
-
- }
- //-------------------------------------------------------
- //計(jì)算PWM輸出值
- //車輛直徑:76mm; 12*64pulse/rev; 1m=3216pulses;
- //-------------------------------------------------------
- //static int speed_diff,speed_diff_all,speed_diff_adjust;
- //static float K_speed_P,K_speed_I;
- static float K_voltage,K_angle,K_angle_dot,K_position,K_position_dot;
- static float K_angle_AD,K_angle_dot_AD,K_position_AD,K_position_dot_AD;
- static float position,position_dot;
- static float position_dot_filter;
- static float PWM;
- static int speed_output_LH,speed_output_RH;
- static int Turn_Need,Speed_Need;
- //-------------------------------------------------------
- void PWM_calculate(void)
- {
- if ( 0==(~PINA&BIT(1)) ) //左轉(zhuǎn)
- {
- Turn_Need=-40;
- }
- else if ( 0==(~PINB&BIT(2)) ) //右轉(zhuǎn)
- {
- Turn_Need=40;
- }
- else //不轉(zhuǎn)
- {
- Turn_Need=0;
- }
-
- if ( 0==(~PINC&BIT(0)) ) //前進(jìn)
- {
- Speed_Need=-2;
- }
- else if ( 0==(~PINC&BIT(1)) ) //后退
- {
- Speed_Need=2;
- }
- else //不動(dòng)
- {
- Speed_Need=0;
- }
-
- K_angle_AD=ADport(4)*0.007;
- K_angle_dot_AD=ADport(5)*0.007;
- K_position_AD=ADport(6)*0.007;
- K_position_dot_AD=ADport(7)*0.007;
- position_dot=speed_output_RH*0.04;
-
- position_dot_filter*=0.9; //車輪速度濾波
- position_dot_filter+=position_dot*0.1;
-
- position+=position_dot_filter;
- //position+=position_dot;
- position+=Speed_Need;
-
- if (position<-768) //防止位置誤差過(guò)大導(dǎo)致的不穩(wěn)定
- {
- position=-768;
- }
- else if (position>768)
- {
- position=768;
- }
-
- PWM = K_angle*angle *K_angle_AD + K_angle_dot*angle_dot *K_angle_dot_AD +
- K_position*position *K_position_AD + K_position_dot*position_dot_filter *K_position_dot_AD;
-
-
- speed_output_RH = PWM - Turn_Need;
- speed_output_LH = - PWM - Turn_Need ;
-
- /*
- speed_diff=speed_real_RH-speed_real_LH; //左右輪速差PI控制;
- speed_diff_all+=speed_diff;
- speed_diff_adjust=(K_speed_P*speed_diff+K_speed_I*speed_diff_all)/2;
- */
-
- PWM_output (speed_output_LH,speed_output_RH);
- }
- //-------------------------------------------------------
- //定時(shí)器2中斷處理
- //-------------------------------------------------------
- static unsigned char temp;
- //-------------------------------------------------------
- #pragma interrupt_handler T2INT_fun:4
- void T2INT_fun(void)
- {
- AD_calculate();
-
- PWM_calculate();
-
- if(temp>=4) //10ms即中斷;每秒計(jì)算:100/4=25次;
- {
- if (USART_State==0X30) //ASCII碼:0X30代表字符'0'
- {
- USART_Transmit(angle*57.3+128);
- USART_Transmit(angle_dot*57.3+128);
- USART_Transmit(128);
- }
- else if(USART_State==0X31) //ASCII碼:0X30代表字符'1'
- {
- USART_Transmit(speed_output_LH+128);
- USART_Transmit(speed_output_RH+128);
- USART_Transmit(128);
- }
- else if(USART_State==0X32) //ASCII碼:0X30代表字符'2'
- {
- USART_Transmit(speed_real_LH+128);
- USART_Transmit(speed_real_RH+128);
- USART_Transmit(128);
- }
- else if(USART_State==0X33) //ASCII碼:0X30代表字符'3'
- {
- USART_Transmit(K_angle+128);
- USART_Transmit(K_angle_dot+128);
- USART_Transmit(K_position_dot+128);
- }
- temp=0;
- }
- speed_real_LH=0;
- speed_real_RH=0;
- temp+=1;
- }
- //-------------------------------------------------------
- int i,j;
- //-------------------------------------------------------
- void main(void)
- {
- PORT_initial();
- T2_initial();
- INT_initial();
- USART_initial ();
-
- SEI();
-
- K_position=0.8 * 0.209; //換算系數(shù):(256/10) * (2*pi/(64*12))=0.20944;//256/10:電壓換算至PWM,256對(duì)應(yīng)10V;
- K_angle=34 * 25.6; //換算系數(shù):256/10 =25.6;
- K_position_dot=1.09 * 20.9; //換算系數(shù):(256/10) * (25*2*pi/(64*12))=20.944;
- K_angle_dot=2 * 25.6; //換算系數(shù):256/10 =25.6;
-
- for (i=1;i<=500;i++) //延時(shí)啟動(dòng)PWM,等待卡爾曼濾波器穩(wěn)定
- {
- for (j=1;j<=300;j++);;
-
- }
- T1_initial();
-
- while(1)
- {
- ;
- }
- }
復(fù)制代碼
OK:其實(shí)控制很簡(jiǎn)單,利用定時(shí)器產(chǎn)生10ms中斷,每次中斷進(jìn)行如下工作:
1:AD采樣加速度傳感器和陀螺儀,然后卡爾曼濾波得出角度與角速度;(濾波模塊借鑒老外的)(互補(bǔ)濾波效果感覺(jué)不是很好);
2:計(jì)算車輪的速度,積分得出位置;
3:利用PD算法得出PWM值=K1*angle + K2*angle_dot + K3*speed + K4 * position;
4:前進(jìn)后退給定參考速度計(jì)算即可;
注:
1:增量式輪速傳感器A相中斷,讀取B相電位判斷前進(jìn)還是后退,在10ms的時(shí)間內(nèi)累加,計(jì)算車輪的速度;
2:由于10ms累加的輪速信號(hào)不多,直接計(jì)算車體會(huì)發(fā)抖,所以增加了低通濾波,解決問(wèn)題;
3:我在外面增加4個(gè)電位器可以手動(dòng)調(diào)節(jié)4個(gè)K值,方便調(diào)試;
以上;
附件的文件夾里面有matlab建模的資料,可以求出所需的4個(gè)K的值,(參考NXT的),大家可自行調(diào)試K值看看對(duì)車輛的影響;
回復(fù)hahahagg
環(huán)路控制怎么這么慢啊?
振蕩很久才穩(wěn)定...
-----------------------------------------------------------------------
其實(shí)我也在思考,可能原因:
1:對(duì)速度做了濾波導(dǎo)致相位延遲;
2:傾斜角速度和車輪速度的K值不夠大;
經(jīng)實(shí)踐:K值增大后,振蕩減小;:)
回復(fù)format
敢問(wèn)樓主的車輪和電機(jī)哪里買的?
價(jià)格?
-----------------------------------------------------------------------
電機(jī)在淘寶上買二手的,大幾十元一個(gè),因?yàn)榭紤]到減速器,而且?guī)鞲衅鳎?br />
其實(shí)后面實(shí)踐得知可以不要位置傳感器,這樣選擇電機(jī)的空間就很大了,買便宜的;
建議:
1:電機(jī)扭矩要大;說(shuō)白了,功率要大;轉(zhuǎn)動(dòng)慣量大似乎的更穩(wěn)定,所以不用空心杯的也許更好;
2:減速比可以大一點(diǎn),用較大的輪子;
3:重心越靠下,抗擾性越強(qiáng);但是可能會(huì)影響穩(wěn)定性;
只是刪除了RS232通訊和多余的注釋(包括互補(bǔ)濾波),效果和原來(lái)的一樣,這個(gè)是沒(méi)有速度傳感器的版本;片內(nèi)8M振蕩器:
無(wú)速度傳感器的精簡(jiǎn)版程序.rar
(2.34 KB, 下載次數(shù): 19)
2016-4-10 02:41 上傳
點(diǎn)擊文件名下載附件
下載積分: 黑幣 -5
電路圖:
protel99格式的電路圖_563431QMA9X5.rar
(74.88 KB, 下載次數(shù): 16)
2016-4-10 02:41 上傳
點(diǎn)擊文件名下載附件
下載積分: 黑幣 -5
全部制作資料:
平衡小車制作資料ProjectRII.rar
(1.44 MB, 下載次數(shù): 24)
2016-4-10 02:41 上傳
點(diǎn)擊文件名下載附件
下載積分: 黑幣 -5
</aden)|(1<<adsc)|(1<<adps1)|(1<</ursel)|(1<<ucsz0)|(1<</rxen)|(1<</int0)|(1<</isc01)|(1<<isc00)|(1<<isc11)|(1<</wgm21)|(1<<cs22)|(1<<cs21)|(1<</ocie2);
</wgm12)|(1<</com1a1)|(1<<wgm10)|(1< |
評(píng)分
-
查看全部評(píng)分
|