兩輪平衡車自算法和大家的一樣一個傾角環(huán)車速取得角度、角速度

算法：和大家的一樣，一個傾角環(huán)，一個車速環(huán)。取得角度、角速度、車速、車位移四個量后經(jīng)過運(yùn)算送給驅(qū)動電機(jī)主控角度傳感器：角速度傳感器(陀螺儀)ENC-03MB(ADMMA7260，kalman角度、角速度。PS：zlstone電機(jī)速度傳感器：每個電機(jī)兩個霍爾傳感器（AB）電機(jī)：型號不清楚，很常見的電機(jī)。額定電壓6V，功率3W。電源：變壓器整流橋那種普通電源，幾塊錢一個。兩個，電機(jī)、MCU15V5V(原文件名 (原文件名 源代碼 +AVRStudio4.18ourdev_610434C8FD1C.rar(文件大小:104K)(原文件名atmega16ourdev_610214M89OEI.pdf(文件大小:30K)(原文件名:M16ourdev_610318TY8G24.rar(文件大小:48K)(原文件名:串口調(diào)試車速未濾波之前波形(原文件名:車速未濾波之前波形10Hz10Hz在這 現(xiàn)在還不是很穩(wěn)，有兩個原因，一個是參數(shù)沒調(diào)到最佳，調(diào)了好久，先這樣吧。再有就是電源太爛了，電機(jī)是額定6V的可實際5ENC-03ADdatasheet高通濾波脈沖響應(yīng)(原文件名:QQover了STCSTC89C51T1：；T2T法測速（M法測速，不需要用INT0、INT1：接霍爾傳感器，速INT2：紅外接收頭輸入信號（要用無線的話，也省了驅(qū)動我買的模塊20塊，賣家沒提供電路圖。應(yīng)該是和這個一樣的 L298偏偏是沒有，PCB8個高速二極管是很讓人吐血的一件事情……L293是有二極管的，用起來方便多了。問題找到了，本來程序是Debug版本的，要改成正式版的，VC我只用了不久，是低級錯誤啦，呵呵。LZ位軟VC(寫的有點亂，10.1假期寫的，后邊就沒怎么更新了。兩種顯示，字符顯示和調(diào)試助手相同，要用示波器的話，協(xié)議{}10ms自己頂一個，沒人關(guān)注啦，大家對這個沒什么？所有資料都有??！~AVRMMA7260模塊：48（5V/3.3V供電；：34；45+35+48+96+34+20=278300所有代碼，電路開源（PCB在做此車過程中，我先后用它看了角速度、加速度、陀螺儀積分的角度、KALMAN濾波的角度、車運(yùn)行過程中的角度、電機(jī)空載速度、PI閉環(huán)的空載調(diào)速曲線（此車沒用速度閉環(huán)、車位移等曲線。很直觀！發(fā)送數(shù)據(jù)的協(xié)議很簡知道可不可以貼淘寶地址，有做的嫌疑啊。。10MMA726048做此車不是非要用AVR的，只要速度、外設(shè)夠用就可以！需要的外設(shè)上邊都有說。關(guān)鍵要懂算法！(文章從頭開始看，看完離散濾波那一節(jié)，就會理解很多。后面擴(kuò)展我看不懂，最后面舉了個例子，好好看下Courdev_611839GC1Z3U.txt(文件大小:7K)原文件名:kalman.txt（zlstone的，他貌似也是的，呵呵，最難的就是。下面是我的voidKalman_Filter(floatangle_m,floatgyro_m){angle+=(gyro_m-q_bias*dt;//Pdot[0]=Q_angleP[0][1]P[1][0]PkPdot[1]=-Pdot[2]=-P[0][0]Pdot[0*dt;/PkP[0][1]+=Pdot[1]*P[1][0]+=Pdot[2]*P[1][1]+=Pdot[3]*angle_errangle_mangle;//zk-PCt_0=C_0*P[0][0];PCt_1=C_0*E=R_angle+C_0*K_0=PCt_0/E;//KkK_1=PCt_1/E;t_0=t_1=C_0*P[0][0]K_0*t_0;//后驗估計誤差協(xié)方差P[0][1]-=K_0*t_1;P[1][0]-=K_1*P[1][1]-=K_1*angle+=K_0*angle_err;//后驗估計q_biasK_1*angle_err;//后驗估計angle_dotgyro_m-q_bias;//輸出值（后驗估計）}我只能幫這些了，我的方向是運(yùn)動控制方面，KALMAN貌似在信號分析相關(guān)專業(yè)會學(xué)的，所以我也沒細(xì)看了。上述程序我是直接過來的，只改了采樣周期，剩下一點沒動（還加了點注釋，呵呵。有關(guān)KALMAN的詳細(xì)解釋一般都是外站的，中國的都是扯淡。我做這個車的目的就是想搞下PID，怎么獲取角度是次要的東西。我用了角速度傳感器和加速度傳感器得到傾角是沒問題的但對車子計算偏航的角度就出現(xiàn)問題了因為加速度傳感器沒法對角速度傳感器進(jìn)行誤差補(bǔ)償,這方面lz是怎么解決呢??期修改dt6msdt=0.006。用融合角速度傳感器和加速度傳感器的數(shù)據(jù),這里明白了,也可以得到傾角,因為有對地面的加速度是不變的,也是到了計算偏航的時候沒有了地心引力偏航的也是角速度和加速度嗎gyro_mangle_m呢?不好意思，錯了。送給KALMAN的數(shù)據(jù)應(yīng)該是角速度、加速度計求得的角度而不是加速度if(acc>1)//1或小于-1elseif(acc<-1)acc=180/3.1415*asin(acc);//asin求反正弦，得出的單位是弧度，*180/pi準(zhǔn)。角速度積分求出的角度，因為是積分，所以在累加過程中又慢慢的累計誤差，慢慢得出的角度也了，角速度傳感器本身也有acc是加速度計是輸出這是一軸加速度計嗎就是說加速度計的方向軸是與車子的方向承直角這理解對嗎?acc=acc-1650;//加速度測量數(shù)據(jù)-靜態(tài)值<---0g輸出的值acc=acc/800;//這里得到角度的正弦值<---800這值...不理解之後的都理解了asin後換成角度w還有的是lz主控的AD是多少位的?0-5V0-1024的其實這也不是問題800是怎么計算的呢剛想到了一個問題車子向前左或前右轉(zhuǎn)的時間加速度計不是應(yīng)該有兩個輸出嗎?acc一個呢想了想acc是與地平面承水平的話asin的時候1g(地心引力)吧?那不是也在做傾角嗎800mv見：點擊此處ourdev_612056WR1X9S.pdf(文件大小:197K)(原文件名(原文件名:QQ(原文件名:QQ800mvg，0.2g，0.2g/g=0.2加速度：（3360-1650）/800=2.1375，2g5VMMA72603.3V供電的吧，直接把電源線接到AD參考電壓就行了。還有要考慮陀螺儀的量程，靜態(tài)輸出1.35V，由Outputvoltage=V0+SvxwV0:Staticoutput[V](atangularvelocityis0[deg/s])Sv:Scalefactor[mV/deg/s]w:Angularvelocity[angularvelocityrange:-90to+90最大能測的w為-90to +0.67*90=1410.3,1350-0.67*90=1289.7，落在參考電壓3.36V內(nèi)，沒有在昨晚想到的車子向前左或前右轉(zhuǎn)的時間加速度計不是應(yīng)該有兩個輸出嗎acc一個呢?想了想,acc是與地平面承水平的話,在正弦的時候,那不是也在做傾角嗎??這已猜到了是正弦了但是800mv之前不是要把AD得到的數(shù)據(jù)換成電壓值嗎例如我的主控AD0-5V0-1023的先把AD值*5/1024800m才對嗎傾角的計算(800mv後得到的是多少g1g的正弦)回到一直的問題是水平面的偏角z軸向天水平面就是xy面的角度變化1g的參與,但是xy面(z軸向天)的偏角還困擾著....lz,水平面的偏角有辦法解決嗎感謝得到角度就行了唄，還算什么“水平面的偏角”O(jiān)UT1、OUT2OUT1、OUT2間電壓就是電機(jī)電源經(jīng)過 源電壓回復(fù)【74樓】ZRYXY外加速度，這就給獲得真實角度帶來干擾了，可能可以通過補(bǔ)償獲得角度吧，但是應(yīng)該很了，呵呵，但是只用陀螺儀貌似可以我用到了PID， =ut=Kp1*角度+Kd1*角速度+Kp2*車速+Ki2*車位移。但是和傳統(tǒng)的PID有點不太一樣，一般PID就是單輸入單輸你這個程序有些難啊，能不能簡化下，程序主要的只留AD、濾波和PID計算輸出就能跑的，不要的車速檢測紅外、液晶顯示、和鍵盤什么的，只留加速度傳感器和角速度傳感器以及輸出還有電機(jī)正反轉(zhuǎn)換，底子薄的人看的好辛苦就將其放大后才送入濾波的回復(fù)【102樓】ucharuintio.h中了，這樣以后就直接用這兩個宏#ifndef#defineucharunsignedchar#ifndef#defineuintunsignedint此段程序分別加在Balance_1602.c和Balance_2motors.cOK首先要確保到的角度足夠穩(wěn)定，要沒有毛刺。有微小偏差沒關(guān)系，這是因為車平衡時的角度為0度，如果有點偏差，車頂多就調(diào)節(jié)過程中，要時刻記住量程是多少，假設(shè)為0~255，那么在調(diào)節(jié)過程中就要根據(jù)這個參數(shù)來設(shè)定各個K值大概的值。你想想太精確地0度，那么電機(jī)就會全速運(yùn)行，以最大力量調(diào)整車體，這時你想想能穩(wěn)定么，車體傾斜