機器人搭建過程中的坑和未來的改進方向

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

機器人原理分析

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

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

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

控制器設計

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

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

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

而我這里的速度閉環(huá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)試平衡車的博客中都有解釋，這里就留給大家思考了。

硬件及結構設計

自平衡機器人的硬件清單有：

56mm全向輪 45元/個

42步進電機 25/個

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

LM2596S降壓模塊 20元

STM32F103C8T6-4飛控板 59.8元

GY-521六軸陀螺儀 25元

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

電路原理圖如下所示：

機器人使用solidworks設計整體結構，底板可在某寶定制6050太空鋁切割，藍色件為正常3D打印件。

程序部分

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

控制程序采用定時器0.5ms定時中斷的方式進行計算，每觸發(fā)兩次中斷計算對電機控制一次，這里還是推薦大家采用外部中斷讀取GY-521上的INT引腳的方式，控制計算周期。GY-521上的INT引腳每5ms觸發(fā)一次跳變，采用外部中斷的方式可以嚴格保證讀取位姿數(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）//矯正結束 { 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;//關閉速度環(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）; //正運動學分析，得到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）;//逆運動學分析得到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ù)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）; }}

小車的運動學分解代碼實現(xiàn)如下，參考了平衡小車之家的代碼：

/**********************************************************函數(shù)功能：小車運動數(shù)學模型入口參數(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ù)功能：小車運動 正運動學分析 入口參數(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）; }

其余代碼不全放出，可在文末點擊“閱讀原文”下載查看。

總結與展望

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

個人認為結構有兩個改進方向，一方面參考以下論文：余義。 單球驅(qū)動自平衡機器人位姿解算與控制系統(tǒng)研究［D］。武漢科技大學，2019。論文中采用的四足式驅(qū)動結構更有利于機器人自平衡控制。

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

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

其次是控制器，PID控制算法應用廣泛但也有一定的缺點，可以考慮采用模糊PID，ADRC自抗擾控制器，強化學習等智能控制算法對機器人自平衡進行控制。

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

責任編輯：haq