? ? 將機(jī)器人整體開源，同時總結(jié)一下機(jī)器人搭建過程中遇到的坑和未來的改進(jìn)方向。在分享的文件里包含了結(jié)構(gòu)設(shè)計、程序控制、電路設(shè)計以及其他模塊相關(guān)資料供大家參考。

機(jī)器人原理分析

? ? 首先來看成品圖：

? ? 如圖所示，該機(jī)器人根據(jù)陀螺儀的位姿數(shù)據(jù)，通過三個全向輪驅(qū)動底部球體調(diào)整自己在球上的位置，保持動態(tài)平衡的同時實現(xiàn)全向移動。

? ? 保持動態(tài)平衡過程需要對機(jī)器人進(jìn)行運動學(xué)分析，這里參考了平衡小車之家的運動學(xué)方程：

? ? 自平衡控制問題轉(zhuǎn)化為三步：輸入X、Y角度—控制器計算—輸出A、B、C電機(jī)轉(zhuǎn)速的控制模型。

控制器設(shè)計

? ? 首先考慮參考平衡車控制，球上自平衡機(jī)器人本質(zhì)上依然是一個一階倒立擺問題。

? ? 這里參考了飛思卡爾直立車的控制方法，采用串級PID控制器，外環(huán)PD角度環(huán)，內(nèi)環(huán)速度PI環(huán)。

? ? 由于我的驅(qū)動方案選擇的是42步進(jìn)電機(jī)，在速度閉環(huán)的時候有些問題。正常的直流電機(jī)+編碼器的控制方案可以通過編碼器將輪子的真實速度計算出來，從而和控制器的理想轉(zhuǎn)速作差，實現(xiàn)速度控制。

? ? 而我這里的速度閉環(huán)是通過計算上一個時鐘周期時給步進(jìn)電機(jī)的控制量，通過運動學(xué)方程分解，得到機(jī)器人的虛擬速度，與理想轉(zhuǎn)速作差控制。我認(rèn)為這種速度閉環(huán)方式還是存在一定缺陷的，但是在網(wǎng)上查看論文的時候我發(fā)現(xiàn)有很多自平衡機(jī)器人都是用42步進(jìn)電機(jī)來實現(xiàn)速度閉環(huán)的，不知道是什么方法。

? ? 這里還可以好好思考一下為什么角度環(huán)要用PD控制，速度環(huán)要PI控制，角度環(huán)的P部分和D部分對機(jī)器人控制有什么影響？在很多CSDN調(diào)試平衡車的博客中都有解釋，這里就留給大家思考了。

硬件及結(jié)構(gòu)設(shè)計

? ? 自平衡機(jī)器人的硬件清單有：

56mm全向輪 45元/個

42步進(jìn)電機(jī) 25/個?

42步進(jìn)閉環(huán)模塊 59.8元/個?

LM2596S降壓模塊 20元?

STM32F103C8T6-4飛控板 59.8元?

GY-521六軸陀螺儀 25元

? ? 用到的模塊大致如上所示，C8T6的價格隨著最近芯片漲價直線上升，我白嫖了實驗室的兩塊板子，現(xiàn)在買一塊實在太貴，可以等芯片價格穩(wěn)定一些再買。其余開關(guān)排針等常見元件不再贅述。

? ? 電路原理圖如下所示：

? ??機(jī)器人使用solidworks設(shè)計整體結(jié)構(gòu)，底板可在某寶定制6050太空鋁切割，藍(lán)色件為正常3D打印件。

程序部分

? ??在keil 5中開發(fā)STM32。

? ? 控制程序采用定時器0.5ms定時中斷的方式進(jìn)行計算，每觸發(fā)兩次中斷計算對電機(jī)控制一次，這里還是推薦大家采用外部中斷讀取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電機(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;}

//這里以A電機(jī)的速度控制為例 輸入為 電機(jī)方向和電機(jī)速度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三個電機(jī)的速度返回 ?值：無******************************************************************/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é)與展望

? ? 球上自平衡機(jī)器人可以作為算法試驗平臺， 輸入輸出固定，更換不同控制器，將數(shù)據(jù)導(dǎo)入MATLAB進(jìn)行分析即可比較控制器性能。

? ? 個人認(rèn)為結(jié)構(gòu)有兩個改進(jìn)方向，一方面參考以下論文：余義. 單球驅(qū)動自平衡機(jī)器人位姿解算與控制系統(tǒng)研究[D].武漢科技大學(xué),2019。論文中采用的四足式驅(qū)動結(jié)構(gòu)更有利于機(jī)器人自平衡控制。

? ? 另一方面可以增加球體和機(jī)器人固定裝置，利用機(jī)械結(jié)構(gòu)將機(jī)器人與底部驅(qū)動球結(jié)合成一個整體防止機(jī)器人跳輪等問題。同時驅(qū)動球?qū)τ跈C(jī)器人平衡的影響較大，最好還是定制空心鋼球，然后噴漆增大摩擦力，最有利于機(jī)器人自平衡控制。

? ? 控制部分的改進(jìn)，首先是控制原理，本文是針對建立好的運動學(xué)方程進(jìn)行分析，通過串級PID算法來實現(xiàn)自平衡運動。該機(jī)器人的控制問題本質(zhì)上是一階倒立擺問題，可以采用動力學(xué)建模的方式，通過動力學(xué)分析算出平衡需要的虛擬力矩，再對電機(jī)進(jìn)行力矩控制。

? ? 其次是控制器，PID控制算法應(yīng)用廣泛但也有一定的缺點，可以考慮采用模糊PID，ADRC自抗擾控制器，強(qiáng)化學(xué)習(xí)等智能控制算法對機(jī)器人自平衡進(jìn)行控制。

原文鏈接：https://blog.csdn.net/qq_42823167/article/details/118085368