基于STM32開(kāi)源項(xiàng)目：球上自平衡機(jī)器人
[導(dǎo)讀]關(guān)注星標(biāo)公眾號(hào)，不錯(cuò)過(guò)精彩內(nèi)容來(lái)源|?達(dá)爾聞?wù)f作者|?Jumping潤(rùn)本次畢業(yè)設(shè)計(jì)過(guò)程中，Jumping潤(rùn)受到過(guò)許多人的幫助，收獲良多，在此將機(jī)器人整體開(kāi)源，同時(shí)總結(jié)一下機(jī)器人搭建過(guò)程中遇到的坑和未來(lái)的改進(jìn)方向。在分享的文件里包含了結(jié)構(gòu)設(shè)計(jì)、程序控制、電路設(shè)計(jì)以及其他模塊相關(guān)資料...
關(guān)注 星標(biāo)公眾號(hào)，不錯(cuò)過(guò)精彩內(nèi)容來(lái)源 |?達(dá)爾聞?wù)f作者 |?Jumping潤(rùn)本次畢業(yè)設(shè)計(jì)過(guò)程中，Jumping潤(rùn)受到過(guò)許多人的幫助，收獲良多，在此將機(jī)器人整體開(kāi)源，同時(shí)總結(jié)一下機(jī)器人搭建過(guò)程中遇到的坑和未來(lái)的改進(jìn)方向。在分享的文件里包含了結(jié)構(gòu)設(shè)計(jì)、程序控制、電路設(shè)計(jì)以及其他模塊相關(guān)資料，對(duì)球上自平衡機(jī)器人感興趣的小伙伴，可以在“達(dá)爾聞?wù)f”微信回復(fù)：球上機(jī)器人，資料供大家參考。如果想和他交流，可以下拉至文末去他的博客。
# 機(jī)器人原理分析 #首先來(lái)看成品圖：如圖所示，該機(jī)器人根據(jù)陀螺儀的位姿數(shù)據(jù)，通過(guò)三個(gè)全向輪驅(qū)動(dòng)底部球體調(diào)整自己在球上的位置，保持動(dòng)態(tài)平衡的同時(shí)實(shí)現(xiàn)全向移動(dòng)。
保持動(dòng)態(tài)平衡過(guò)程需要對(duì)機(jī)器人進(jìn)行運(yùn)動(dòng)學(xué)分析，這里參考了平衡小車(chē)之家的運(yùn)動(dòng)學(xué)方程：自平衡控制問(wèn)題轉(zhuǎn)化為三步：輸入X、Y角度—控制器計(jì)算—輸出A、B、C電機(jī)轉(zhuǎn)速的控制模型。
# 控制器設(shè)計(jì)?#首先考慮參考平衡車(chē)控制，球上自平衡機(jī)器人本質(zhì)上依然是一個(gè)一階倒立擺問(wèn)題。這里參考了飛思卡爾直立車(chē)的控制方法，采用串級(jí)PID控制器，外環(huán)PD角度環(huán)，內(nèi)環(huán)速度PI環(huán)。
由于我的驅(qū)動(dòng)方案選擇的是42步進(jìn)電機(jī)，在速度閉環(huán)的時(shí)候有些問(wèn)題。正常的直流電機(jī) 編碼器的控制方案可以通過(guò)編碼器將輪子的真實(shí)速度計(jì)算出來(lái)，從而和控制器的理想轉(zhuǎn)速作差，實(shí)現(xiàn)速度控制。
而我這里的速度閉環(huán)是通過(guò)計(jì)算上一個(gè)時(shí)鐘周期時(shí)給步進(jìn)電機(jī)的控制量，通過(guò)運(yùn)動(dòng)學(xué)方程分解，得到機(jī)器人的虛擬速度，與理想轉(zhuǎn)速作差控制。我認(rèn)為這種速度閉環(huán)方式還是存在一定缺陷的，但是在網(wǎng)上查看論文的時(shí)候我發(fā)現(xiàn)有很多自平衡機(jī)器人都是用42步進(jìn)電機(jī)來(lái)實(shí)現(xiàn)速度閉環(huán)的，不知道是什么方法。
這里還可以好好思考一下為什么角度環(huán)要用PD控制，速度環(huán)要PI控制，角度環(huán)的P部分和D部分對(duì)機(jī)器人控制有什么影響？在很多CSDN調(diào)試平衡車(chē)的博客中都有解釋，這里就留給大家思考了。
# 硬件及結(jié)構(gòu)設(shè)計(jì)?#自平衡機(jī)器人的硬件清單有：56mm全向輪 45元/個(gè)42步進(jìn)電機(jī) 25/個(gè)?42步進(jìn)閉環(huán)模塊 59.8元/個(gè)?LM2596S降壓模塊 20元?STM32F103C8T6-4飛控板 59.8元?GY-521六軸陀螺儀 25元
用到的模塊大致如上所示，C8T6的價(jià)格隨著最近芯片漲價(jià)直線上升，我白嫖了實(shí)驗(yàn)室的兩塊板子，現(xiàn)在買(mǎi)一塊實(shí)在太貴，可以等芯片價(jià)格穩(wěn)定一些再買(mǎi)。其余開(kāi)關(guān)排針等常見(jiàn)元件不再贅述。
使用立創(chuàng)EDA軟件進(jìn)行設(shè)計(jì)，原理圖如下所示：PCB走線與原理圖設(shè)計(jì)打包放在GitHub中了，鏈接如下：https://github.com/1812598631/graduate-design以上資料也可以在“達(dá)爾聞?wù)f”微信回復(fù)：球上機(jī)器人，獲取。
機(jī)器人使用solidworks設(shè)計(jì)整體結(jié)構(gòu)，底板可在淘寶定制6050太空鋁切割，藍(lán)色件為正常3D打印件。
# 程序部分 #在keil 5中開(kāi)發(fā)STM32。
控制程序采用定時(shí)器0.5ms定時(shí)中斷的方式進(jìn)行計(jì)算，每觸發(fā)兩次中斷計(jì)算對(duì)電機(jī)控制一次，這里還是推薦大家采用外部中斷讀取GY-521上的INT引腳的方式，控制計(jì)算周期。GY-521上的INT引腳每5ms觸發(fā)一次跳變，采用外部中斷的方式可以嚴(yán)格保證讀取位姿數(shù)據(jù)與計(jì)算處理同步。
對(duì)于電機(jī)控制，由于采用的驅(qū)動(dòng)方案是步進(jìn)電機(jī)，調(diào)速的方式是改變驅(qū)動(dòng)步進(jìn)電機(jī)的脈沖頻率。我這里選擇了三個(gè)定時(shí)器，動(dòng)態(tài)調(diào)節(jié)定時(shí)器的頻率，具體方式是在初始化時(shí)設(shè)定好定時(shí)器的預(yù)分頻系數(shù)psc的值，然后在程序里動(dòng)態(tài)更改ARR寄存器的值，從而改變定時(shí)器的定時(shí)頻率。
小車(chē)的運(yùn)動(dòng)學(xué)分解代碼實(shí)現(xiàn)如下，參考了平衡小車(chē)之家的代碼：
# 總結(jié)與展望?#球上自平衡機(jī)器人可以作為算法試驗(yàn)平臺(tái)， 輸入輸出固定，更換不同控制器，將數(shù)據(jù)導(dǎo)入MATLAB進(jìn)行分析即可比較控制器性能。
個(gè)人認(rèn)為結(jié)構(gòu)有兩個(gè)改進(jìn)方向，一方面參考以下論文：余義. 單球驅(qū)動(dòng)自平衡機(jī)器人位姿解算與控制系統(tǒng)研究[D].武漢科技大學(xué),2019。論文中采用的四足式驅(qū)動(dòng)結(jié)構(gòu)更有利于機(jī)器人自平衡控制。
另一方面可以增加球體和機(jī)器人固定裝置，利用機(jī)械結(jié)構(gòu)將機(jī)器人與底部驅(qū)動(dòng)球結(jié)合成一個(gè)整體防止機(jī)器人跳輪等問(wèn)題。同時(shí)驅(qū)動(dòng)球?qū)τ跈C(jī)器人平衡的影響較大，最好還是定制空心鋼球，然后噴漆增大摩擦力，最有利于機(jī)器人自平衡控制。
控制部分的改進(jìn)，首先是控制原理，本文是針對(duì)建立好的運(yùn)動(dòng)學(xué)方程進(jìn)行分析，通過(guò)串級(jí)PID算法來(lái)實(shí)現(xiàn)自平衡運(yùn)動(dòng)。該機(jī)器人的控制問(wèn)題本質(zhì)上是一階倒立擺問(wèn)題，可以采用動(dòng)力學(xué)建模的方式，通過(guò)動(dòng)力學(xué)分析算出平衡需要的虛擬力矩，再對(duì)電機(jī)進(jìn)行力矩控制。
其次是控制器，PID控制算法應(yīng)用廣泛但也有一定的缺點(diǎn)，可以考慮采用模糊PID，ADRC自抗擾控制器，強(qiáng)化學(xué)習(xí)等智能控制算法對(duì)機(jī)器人自平衡進(jìn)行控制。
主要參考論文[1]余義. 單球驅(qū)動(dòng)自平衡機(jī)器人位姿解算與控制系統(tǒng)研究[D].武漢科技大學(xué),2019.[2]劉桐. 基于三全向輪驅(qū)動(dòng)的球輪自平衡機(jī)器人的研究與實(shí)現(xiàn)[D].北京工業(yè)大學(xué),2018.[3]韓京清.從PID技術(shù)到“自抗擾控制”技術(shù)[J].控制工程,2002(03):13-18.
本文為CSDN博主Jumping潤(rùn)的原創(chuàng)文章，原文出處鏈接：https://blog.csdn.net/qq_42823167/article/details/118085368
免責(zé)聲明：本文來(lái)源網(wǎng)絡(luò)，版權(quán)歸原作者所有。如涉及作品版權(quán)問(wèn)題，請(qǐng)與我聯(lián)系刪除。
------------?END?------------
●嵌入式專欄精選教程●精選匯總 | ST工具、下載編程工具●精選匯總 | 嵌入式軟件設(shè)計(jì)與開(kāi)發(fā)●精選匯總 | STM32、MCU、單片機(jī)
歡迎關(guān)注我的公眾號(hào)，回復(fù)“加群”按規(guī)則加入技術(shù)交流群，回復(fù)“1024”查看更多內(nèi)容。
歡迎關(guān)注我的視頻號(hào)：
點(diǎn)擊“閱讀原文”查看更多分享，歡迎點(diǎn)分享、收藏、點(diǎn)贊、在看。
# 機(jī)器人原理分析 #首先來(lái)看成品圖：如圖所示，該機(jī)器人根據(jù)陀螺儀的位姿數(shù)據(jù)，通過(guò)三個(gè)全向輪驅(qū)動(dòng)底部球體調(diào)整自己在球上的位置，保持動(dòng)態(tài)平衡的同時(shí)實(shí)現(xiàn)全向移動(dòng)。
保持動(dòng)態(tài)平衡過(guò)程需要對(duì)機(jī)器人進(jìn)行運(yùn)動(dòng)學(xué)分析，這里參考了平衡小車(chē)之家的運(yùn)動(dòng)學(xué)方程：自平衡控制問(wèn)題轉(zhuǎn)化為三步：輸入X、Y角度—控制器計(jì)算—輸出A、B、C電機(jī)轉(zhuǎn)速的控制模型。
# 控制器設(shè)計(jì)?#首先考慮參考平衡車(chē)控制，球上自平衡機(jī)器人本質(zhì)上依然是一個(gè)一階倒立擺問(wèn)題。這里參考了飛思卡爾直立車(chē)的控制方法，采用串級(jí)PID控制器，外環(huán)PD角度環(huán)，內(nèi)環(huán)速度PI環(huán)。
由于我的驅(qū)動(dòng)方案選擇的是42步進(jìn)電機(jī)，在速度閉環(huán)的時(shí)候有些問(wèn)題。正常的直流電機(jī) 編碼器的控制方案可以通過(guò)編碼器將輪子的真實(shí)速度計(jì)算出來(lái)，從而和控制器的理想轉(zhuǎn)速作差，實(shí)現(xiàn)速度控制。
而我這里的速度閉環(huán)是通過(guò)計(jì)算上一個(gè)時(shí)鐘周期時(shí)給步進(jìn)電機(jī)的控制量，通過(guò)運(yùn)動(dòng)學(xué)方程分解，得到機(jī)器人的虛擬速度，與理想轉(zhuǎn)速作差控制。我認(rèn)為這種速度閉環(huán)方式還是存在一定缺陷的，但是在網(wǎng)上查看論文的時(shí)候我發(fā)現(xiàn)有很多自平衡機(jī)器人都是用42步進(jìn)電機(jī)來(lái)實(shí)現(xiàn)速度閉環(huán)的，不知道是什么方法。
這里還可以好好思考一下為什么角度環(huán)要用PD控制，速度環(huán)要PI控制，角度環(huán)的P部分和D部分對(duì)機(jī)器人控制有什么影響？在很多CSDN調(diào)試平衡車(chē)的博客中都有解釋，這里就留給大家思考了。
# 硬件及結(jié)構(gòu)設(shè)計(jì)?#自平衡機(jī)器人的硬件清單有：56mm全向輪 45元/個(gè)42步進(jìn)電機(jī) 25/個(gè)?42步進(jìn)閉環(huán)模塊 59.8元/個(gè)?LM2596S降壓模塊 20元?STM32F103C8T6-4飛控板 59.8元?GY-521六軸陀螺儀 25元
用到的模塊大致如上所示，C8T6的價(jià)格隨著最近芯片漲價(jià)直線上升，我白嫖了實(shí)驗(yàn)室的兩塊板子，現(xiàn)在買(mǎi)一塊實(shí)在太貴，可以等芯片價(jià)格穩(wěn)定一些再買(mǎi)。其余開(kāi)關(guān)排針等常見(jiàn)元件不再贅述。
使用立創(chuàng)EDA軟件進(jìn)行設(shè)計(jì)，原理圖如下所示：PCB走線與原理圖設(shè)計(jì)打包放在GitHub中了，鏈接如下：https://github.com/1812598631/graduate-design以上資料也可以在“達(dá)爾聞?wù)f”微信回復(fù)：球上機(jī)器人，獲取。
機(jī)器人使用solidworks設(shè)計(jì)整體結(jié)構(gòu)，底板可在淘寶定制6050太空鋁切割，藍(lán)色件為正常3D打印件。
# 程序部分 #在keil 5中開(kāi)發(fā)STM32。
控制程序采用定時(shí)器0.5ms定時(shí)中斷的方式進(jìn)行計(jì)算，每觸發(fā)兩次中斷計(jì)算對(duì)電機(jī)控制一次，這里還是推薦大家采用外部中斷讀取GY-521上的INT引腳的方式，控制計(jì)算周期。GY-521上的INT引腳每5ms觸發(fā)一次跳變，采用外部中斷的方式可以嚴(yán)格保證讀取位姿數(shù)據(jù)與計(jì)算處理同步。
int TIM1_UP_IRQHandler (void) { u8 key_cal; if(TIM_GetITStatus(TIM1,TIM_IT_Update) != RESET) { TIM_ClearITPendingBit(TIM1,TIM_IT_Update); flag_target=!flag_target; key_cal=KEY_Scan(0); if(state_flag==1)//矯正結(jié)束 { if(flag_target==1)//每讀取兩次陀螺儀控制一次 { Read_DMP(); //===讀取傾角 scope(); return 0; } } if(key_cal==1)//矯正按鍵 { Angle_Zero_X=Angle_Balance_X; Angle_Zero_Y=Angle_Balance_Y; key_cal=0; Flag_Stop=0; } if(key_cal==2||key_cal==3)//矯正按鍵 { Flag_Stop=1;//關(guān)閉速度環(huán)I積分 key_cal=0; } Angle_Bias_X =Angle_Balance_X-Angle_Zero_X; //獲取Y方向的偏差 Angle_Bias_Y =Angle_Balance_Y-Angle_Zero_Y; //獲取Y方向的偏差 if(control_mode==0)//PID控制模式 { Encoder_Analysis(Motor_A,Motor_B,Motor_C); //正運(yùn)動(dòng)學(xué)分析，得到X Y方向的速度 Balance_Pwm_X= balance_X(Angle_Bias_X,Gyro_Balance_X);//X方向的傾角控制 Balance_Pwm_Y=-balance_Y(Angle_Bias_Y,Gyro_Balance_Y); //Y方向的傾角控制// if( flag_target_2==4)//速度環(huán)頻率慢于加速度環(huán) 但是還沒(méi)加速度環(huán) // { Velocity_Pwm_X=velocity_X(compute_X); //X方向的速度控制 Velocity_Pwm_Y=velocity_Y(compute_Y); //Y方向的速度控制 // flag_target_2=0;// } Move_X =Balance_Pwm_X Velocity_Pwm_X; //===X方向控制量累加 Move_Y =Balance_Pwm_Y Velocity_Pwm_Y; //===Y方向控制量累加 Move_Z=0; Kinematic_Analysis(Move_X,Move_Y,Move_Z);//逆運(yùn)動(dòng)學(xué)分析得到ABC電機(jī)控制量 } Motor_A=Target_A;//直接調(diào)節(jié)PWM頻率 Motor_B=Target_B;//直接調(diào)節(jié)PWM Motor_C=Target_C;//直接調(diào)節(jié)PWM//以下都是為了速度連續(xù)化處理防止突變 if(Motor_A==0) Motor_A=motor_a_last; if(Motor_B==0) Motor_B=motor_b_last; if(Motor_C==0) Motor_C=motor_c_last; Xianfu_Pwm(2000); Set_Pwm(Motor_A,Motor_B,Motor_C); Gyro_Balance_X_last=Gyro_Balance_X; Gyro_Balance_Y_last=Gyro_Balance_Y; Gyro_Balance_Z_last=Gyro_Balance_Z; Angle_Balance_X_last=Angle_Balance_X; Angle_Balance_Y_last=Angle_Balance_Y; Angle_Balance_Z_last=Angle_Balance_Z; motor_a_last=Motor_A; motor_b_last=Motor_B; motor_c_last=Motor_C; } return 0;}對(duì)于電機(jī)控制，由于采用的驅(qū)動(dòng)方案是步進(jìn)電機(jī)，調(diào)速的方式是改變驅(qū)動(dòng)步進(jìn)電機(jī)的脈沖頻率。我這里選擇了三個(gè)定時(shí)器，動(dòng)態(tài)調(diào)節(jié)定時(shí)器的頻率，具體方式是在初始化時(shí)設(shè)定好定時(shí)器的預(yù)分頻系數(shù)psc的值，然后在程序里動(dòng)態(tài)更改ARR寄存器的值，從而改變定時(shí)器的定時(shí)頻率。
//這里以A電機(jī)的速度控制為例 輸入為 電機(jī)方向和電機(jī)速度void set_motorA_speed(u8 dir,u16 speed){ u32 arr; arr=speed; TIM_ARRPreloadConfig(TIM3,DISABLE); TIM3->ARR=arr;//計(jì)數(shù)到10000在歸零重新計(jì)數(shù) TIM3->CCR4=arr/2;//保持占空比為50% TIM_ARRPreloadConfig(TIM3,ENABLE); TIM_Cmd(TIM3,ENABLE); if(dir==0) { GPIO_SetBits(GPIOA,GPIO_Pin_1); } else { GPIO_ResetBits(GPIOA,GPIO_Pin_1); }}小車(chē)的運(yùn)動(dòng)學(xué)分解代碼實(shí)現(xiàn)如下，參考了平衡小車(chē)之家的代碼：
/**************************************************************************函數(shù)功能：小車(chē)運(yùn)動(dòng)數(shù)學(xué)模型入口參數(shù)：X Y Z 三軸速度或者位置返回 值：無(wú)**************************************************************************/void Kinematic_Analysis(float Vx,float Vy,float Vz){ Target_A = Vx L_PARAMETER*Vz; Target_B = -X_PARAMETER*Vx Y_PARAMETER*Vy L_PARAMETER*Vz; Target_C = -X_PARAMETER*Vx - Y_PARAMETER*Vy L_PARAMETER*Vz;}/**************************************************************************函數(shù)功能：小車(chē)運(yùn)動(dòng) 正運(yùn)動(dòng)學(xué)分析 入口參數(shù)：A B C三個(gè)電機(jī)的速度返回 值：無(wú)**************************************************************************/void Encoder_Analysis(float Va,float Vb,float Vc){ compute_X=(Va*2-Vb-Vc); compute_Y=((Vb-Vc)*sqrt(3)); compute_Z=(Va Vb Vc); }其余代碼不全放出，具體可轉(zhuǎn)到：https://github.com/1812598631/graduate-design# 總結(jié)與展望?#球上自平衡機(jī)器人可以作為算法試驗(yàn)平臺(tái)， 輸入輸出固定，更換不同控制器，將數(shù)據(jù)導(dǎo)入MATLAB進(jìn)行分析即可比較控制器性能。
個(gè)人認(rèn)為結(jié)構(gòu)有兩個(gè)改進(jìn)方向，一方面參考以下論文：余義. 單球驅(qū)動(dòng)自平衡機(jī)器人位姿解算與控制系統(tǒng)研究[D].武漢科技大學(xué),2019。論文中采用的四足式驅(qū)動(dòng)結(jié)構(gòu)更有利于機(jī)器人自平衡控制。
另一方面可以增加球體和機(jī)器人固定裝置，利用機(jī)械結(jié)構(gòu)將機(jī)器人與底部驅(qū)動(dòng)球結(jié)合成一個(gè)整體防止機(jī)器人跳輪等問(wèn)題。同時(shí)驅(qū)動(dòng)球?qū)τ跈C(jī)器人平衡的影響較大，最好還是定制空心鋼球，然后噴漆增大摩擦力，最有利于機(jī)器人自平衡控制。
控制部分的改進(jìn)，首先是控制原理，本文是針對(duì)建立好的運(yùn)動(dòng)學(xué)方程進(jìn)行分析，通過(guò)串級(jí)PID算法來(lái)實(shí)現(xiàn)自平衡運(yùn)動(dòng)。該機(jī)器人的控制問(wèn)題本質(zhì)上是一階倒立擺問(wèn)題，可以采用動(dòng)力學(xué)建模的方式，通過(guò)動(dòng)力學(xué)分析算出平衡需要的虛擬力矩，再對(duì)電機(jī)進(jìn)行力矩控制。
其次是控制器，PID控制算法應(yīng)用廣泛但也有一定的缺點(diǎn)，可以考慮采用模糊PID，ADRC自抗擾控制器，強(qiáng)化學(xué)習(xí)等智能控制算法對(duì)機(jī)器人自平衡進(jìn)行控制。
主要參考論文[1]余義. 單球驅(qū)動(dòng)自平衡機(jī)器人位姿解算與控制系統(tǒng)研究[D].武漢科技大學(xué),2019.[2]劉桐. 基于三全向輪驅(qū)動(dòng)的球輪自平衡機(jī)器人的研究與實(shí)現(xiàn)[D].北京工業(yè)大學(xué),2018.[3]韓京清.從PID技術(shù)到“自抗擾控制”技術(shù)[J].控制工程,2002(03):13-18.
本文為CSDN博主Jumping潤(rùn)的原創(chuàng)文章，原文出處鏈接：https://blog.csdn.net/qq_42823167/article/details/118085368
免責(zé)聲明：本文來(lái)源網(wǎng)絡(luò)，版權(quán)歸原作者所有。如涉及作品版權(quán)問(wèn)題，請(qǐng)與我聯(lián)系刪除。
------------?END?------------
●嵌入式專欄精選教程●精選匯總 | ST工具、下載編程工具●精選匯總 | 嵌入式軟件設(shè)計(jì)與開(kāi)發(fā)●精選匯總 | STM32、MCU、單片機(jī)
歡迎關(guān)注我的公眾號(hào)，回復(fù)“加群”按規(guī)則加入技術(shù)交流群，回復(fù)“1024”查看更多內(nèi)容。
歡迎關(guān)注我的視頻號(hào)：
點(diǎn)擊“閱讀原文”查看更多分享，歡迎點(diǎn)分享、收藏、點(diǎn)贊、在看。
下載文檔
