將機器人整體開源,同時總結(jié)一下機器人搭建過程中遇到的坑和未來的改進方向。在分享的文件里包含了結(jié)構(gòu)設(shè)計、程序控制、電路設(shè)計以及其他模塊相關(guān)資料供大家參考。
第一:機器人原理分析
首先來看成品圖:
如圖所示,該機器人根據(jù)陀螺儀的位姿數(shù)據(jù),通過三個全向輪驅(qū)動底部球體調(diào)整自己在球上的位置,保持動態(tài)平衡的同時實現(xiàn)全向移動。
保持動態(tài)平衡過程需要對機器人進行運動學(xué)分析,這里參考了平衡小車之家的運動學(xué)方程:
自平衡控制問題轉(zhuǎn)化為三步:輸入X、Y角度—控制器計算—輸出A、B、C電機轉(zhuǎn)速的控制模型。
第二:控制器設(shè)計
首先考慮參考平衡車控制,球上自平衡機器人本質(zhì)上依然是一個一階倒立擺問題。
這里參考了飛思卡爾直立車的控制方法,采用串級PID控制器,外環(huán)PD角度環(huán),內(nèi)環(huán)速度PI環(huán)。
由于我的驅(qū)動方案選擇的是42步進電機,在速度閉環(huán)的時候有些問題。正常的直流電機+編碼器的控制方案可以通過編碼器將輪子的真實速度計算出來,從而和控制器的理想轉(zhuǎn)速作差,實現(xiàn)速度控制。
而我這里的速度閉環(huán)是通過計算上一個時鐘周期時給步進電機的控制量,通過運動學(xué)方程分解,得到機器人的虛擬速度,與理想轉(zhuǎn)速作差控制。我認(rèn)為這種速度閉環(huán)方式還是存在一定缺陷的,但是在網(wǎng)上查看論文的時候我發(fā)現(xiàn)有很多自平衡機器人都是用42步進電機來實現(xiàn)速度閉環(huán)的,不知道是什么方法。
這里還可以好好思考一下為什么角度環(huán)要用PD控制,速度環(huán)要PI控制,角度環(huán)的P部分和D部分對機器人控制有什么影響?在很多CSDN調(diào)試平衡車的博客中都有解釋,這里就留給大家思考了。
第三:硬件及結(jié)構(gòu)設(shè)計
自平衡機器人的硬件清單有:
56mm全向輪 45元/個
42步進電機 25/個
42步進閉環(huán)模塊 59.8元/個
LM2596S降壓模塊 20元
STM32F103C8T6-4飛控板 59.8元
GY-521六軸陀螺儀 25元
用到的模塊大致如上所示,C8T6的價格隨著最近芯片漲價直線上升,我白嫖了實驗室的兩塊板子,現(xiàn)在買一塊實在太貴,可以等芯片價格穩(wěn)定一些再買。其余開關(guān)排針等常見元件不再贅述。
電路原理圖如下所示:
機器人使用solidworks設(shè)計整體結(jié)構(gòu),底板可在某寶定制6050太空鋁切割,藍色件為正常3D打印件。
第四:程序部分
在keil 5中開發(fā)STM32。
控制程序采用定時器0.5ms定時中斷的方式進行計算,每觸發(fā)兩次中斷計算對電機控制一次,這里還是推薦大家采用外部中斷讀取GY-521上的INT引腳的方式,控制計算周期。GY-521上的INT引腳每5ms觸發(fā)一次跳變,采用外部中斷的方式可以嚴(yán)格保證讀取位姿數(shù)據(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); //正運動學(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) 但是還沒加速度環(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);//逆運動學(xué)分析得到ABC電機控制量 } 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; }對于電機控制,由于采用的驅(qū)動方案是步進電機,調(diào)速的方式是改變驅(qū)動步進電機的脈沖頻率。我這里選擇了三個定時器,動態(tài)調(diào)節(jié)定時器的頻率,具體方式是在初始化時設(shè)定好定時器的預(yù)分頻系數(shù)psc的值,然后在程序里動態(tài)更改ARR寄存器的值,從而改變定時器的定時頻率。
//這里以A電機的速度控制為例 輸入為 電機方向和電機速度 void set_motorA_speed(u8 dir,u16 speed) { u32 arr; arr=speed; TIM_ARRPreloadConfig(TIM3,DISABLE); TIM3->ARR=arr;//計數(shù)到10000在歸零重新計數(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); } }
小車的運動學(xué)分解代碼實現(xiàn)如下,參考了平衡小車之家的代碼:
/********************************************************** 函數(shù)功能:小車運動數(shù)學(xué)模型 入口參數(shù):X Y Z 三軸速度或者位置 返回 值:無 ***********************************************************/ 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ù)功能:小車運動 正運動學(xué)分析 入口參數(shù):A B C三個電機的速度 返回 值:無 ******************************************************************/ 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); }其余代碼不全放出,可在文末點擊“閱讀原文”下載查看。
第五:總結(jié)與展望
球上自平衡機器人可以作為算法試驗平臺, 輸入輸出固定,更換不同控制器,將數(shù)據(jù)導(dǎo)入MATLAB進行分析即可比較控制器性能。
個人認(rèn)為結(jié)構(gòu)有兩個改進方向,一方面參考以下論文:余義. 單球驅(qū)動自平衡機器人位姿解算與控制系統(tǒng)研究[D].武漢科技大學(xué),2019。論文中采用的四足式驅(qū)動結(jié)構(gòu)更有利于機器人自平衡控制。
另一方面可以增加球體和機器人固定裝置,利用機械結(jié)構(gòu)將機器人與底部驅(qū)動球結(jié)合成一個整體防止機器人跳輪等問題。同時驅(qū)動球?qū)τ跈C器人平衡的影響較大,最好還是定制空心鋼球,然后噴漆增大摩擦力,最有利于機器人自平衡控制。
控制部分的改進,首先是控制原理,本文是針對建立好的運動學(xué)方程進行分析,通過串級PID算法來實現(xiàn)自平衡運動。該機器人的控制問題本質(zhì)上是一階倒立擺問題,可以采用動力學(xué)建模的方式,通過動力學(xué)分析算出平衡需要的虛擬力矩,再對電機進行力矩控制。
其次是控制器,PID控制算法應(yīng)用廣泛但也有一定的缺點,可以考慮采用模糊PID,ADRC自抗擾控制器,強化學(xué)習(xí)等智能控制算法對機器人自平衡進行控制。
審核編輯:湯梓紅
-
控制器
+關(guān)注
關(guān)注
112文章
15885瀏覽量
175360 -
機器人
+關(guān)注
關(guān)注
210文章
27838瀏覽量
204592 -
電路設(shè)計
+關(guān)注
關(guān)注
6636文章
2398瀏覽量
201126 -
STM32
+關(guān)注
關(guān)注
2258文章
10828瀏覽量
352479 -
平衡小車
+關(guān)注
關(guān)注
1文章
39瀏覽量
12473
原文標(biāo)題:利用STM32實現(xiàn)自平衡機器人功能項目
文章出處:【微信號:嵌入式開發(fā)愛好者,微信公眾號:嵌入式開發(fā)愛好者】歡迎添加關(guān)注!文章轉(zhuǎn)載請注明出處。
發(fā)布評論請先 登錄
相關(guān)推薦
評論