【经验分享】STM32自平衡机器人项目

[复制链接]

STMCU小助手发布时间：2022-6-1 18:57

文章
文章封面:
文章简介:	STM32自平衡机器人项目

将机器人整体开源，同时总结一下机器人搭建过程中遇到的坑和未来的改进方向。在分享的文件里包含了结构设计、程序控制、电路设计以及其他模块相关资料供大家参考。

文末点击阅读原文可以获取相关资料。

机器人原理分析

首先来看成品图：

如图所示，该机器人根据陀螺仪的位姿数据，通过三个全向轮驱动底部球体调整自己在球上的位置，保持动态平衡的同时实现全向移动。

保持动态平衡过程需要对机器人进行运动学分析，这里参考了平衡小车之家的运动学方程：

自平衡控制问题转化为三步：输入X、Y角度—控制器计算—输出A、B、C电机转速的控制模型。

控制器设计

首先考虑参考平衡车控制，球上自平衡机器人本质上依然是一个一阶倒立摆问题。

这里参考了飞思卡尔直立车的控制方法，采用串级PID控制器，外环PD角度环，内环速度PI环。相关文章：PID算法原理介绍。

由于我的驱动方案选择的是42步进电机，在速度闭环的时候有些问题。正常的直流电机+编码器的控制方案可以通过编码器将轮子的真实速度计算出来，从而和控制器的理想转速作差，实现速度控制。

而我这里的速度闭环是通过计算上一个时钟周期时给步进电机的控制量，通过运动学方程分解，得到机器人的虚拟速度，与理想转速作差控制。我认为这种速度闭环方式还是存在一定缺陷的，但是在网上查看论文的时候我发现有很多自平衡机器人都是用42步进电机来实现速度闭环的，不知道是什么方法。

这里还可以好好思考一下为什么角度环要用PD控制，速度环要PI控制，角度环的P部分和D部分对机器人控制有什么影响？在很多CSDN调试平衡车的博客中都有解释，这里就留给大家思考了。

硬件及结构设计

自平衡机器人的硬件清单有：

56mm全向轮 45元/个

42步进电机 25/个

42步进闭环模块 59.8元/个

LM2596S降压模块 20元

STM32F103C8T6-4飞控板 59.8元

GY-521六轴陀螺仪 25元

用到的模块大致如上所示，C8T6的价格随着最近芯片涨价直线上升，我白嫖了实验室的两块板子，现在买一块实在太贵，可以等芯片价格稳定一些再买。其余开关排针等常见元件不再赘述。

电路原理图如下所示：

机器人使用solidworks设计整体结构，底板可在某宝定制6050太空铝切割，蓝色件为正常3D打印件。

程序部分

在keil 5中开发STM32。

控制程序采用定时器0.5ms定时中断的方式进行计算，每触发两次中断计算对电机控制一次，这里还是推荐大家采用外部中断读取GY-521上的INT引脚的方式，控制计算周期。GY-521上的INT引脚每5ms触发一次跳变，采用外部中断的方式可以严格保证读取位姿数据与计算处理同步。

int TIM1_UP_IRQHandler (void) $ @/ {* k- u& W, @0 o
{
( W$ G* f) E4 ~, N; [1 z2 s+ l
u8 key_cal;& p0 r% m" u' u) t1 C6 H# o' z! }( P
if(TIM_GetITStatus(TIM1,TIM_IT_Update) != RESET): K2 [# T# T0 O
{ }& ~! u* X+ G$ Y1 j0 y0 k
TIM_ClearITPendingBit(TIM1,TIM_IT_Update);
0 i0 m6 o% N' d; L2 D
flag_target=!flag_target;9 s% G' t# {* B+ F/ v7 R, E) E1 Y
key_cal=KEY_Scan(0);
; Z, w, c& @3 u3 }+ x. l
if(state_flag==1)//矫正结束3 N% L$ u, J* B. g6 j5 O
{; O% D3 ?0 q# O
if(flag_target==1)//每读取两次陀螺仪控制一次 " Q1 ~0 x7 l+ O- c) s" v9 d
{
2 K% K! i, K( U2 r" t
Read_DMP(); //===读取倾角+ x# I' h- S$ I2 O& s1 L6 Z
scope();7 H R8 y! v, m# ]& j5 {
return 0;, ]# t; |1 d9 N
}
# v! B& i9 h# c5 k$ @/ s) b: E
}$ [- N/ n' n6 j' a4 u4 f
if(key_cal==1)//矫正按键
) y* k. q, s3 f5 i6 x) {$ u
{+ d9 m- B0 L- L/ ^' F* I
Angle_Zero_X=Angle_Balance_X;
. L* n! s3 k$ i$ D* Q
Angle_Zero_Y=Angle_Balance_Y;& f. X/ A! h0 F! z+ D
key_cal=0;
+ R+ `' \9 N' {$ o& y
Flag_Stop=0;
0 x5 W. V# I( q8 q
}5 Y7 |6 O7 S$ C& e1 y
if(key_cal==2||key_cal==3)//矫正按键
$ d4 W3 a) x; c( n6 N
{" H" o; u( w; J# Q6 I
Flag_Stop=1;//关闭速度环I积分
: N& u; V/ x6 p+ A& X0 @' y; ]0 O+ u; [
key_cal=0;
& {1 f+ `4 g4 D/ U+ N7 }
}9 t9 H% u6 d- y8 P
Angle_Bias_X =Angle_Balance_X-Angle_Zero_X; //获取Y方向的偏差
% Q% P2 ]! x% V0 s( i
Angle_Bias_Y =Angle_Balance_Y-Angle_Zero_Y; //获取Y方向的偏差" U/ i! o4 M* M) H% h# f1 H8 o
if(control_mode==0)//PID控制模式
8 h0 R/ R; y& A. ~6 }
{
" U o, Q2 m* M8 u& Q5 Y
Encoder_Analysis(Motor_A,Motor_B,Motor_C); //正运动学分析，得到X Y方向的速度7 x( J2 U/ b. T5 T) o4 T! ^
Balance_Pwm_X= balance_X(Angle_Bias_X,Gyro_Balance_X);//X方向的倾角控制
, s( U, }8 q: X% N5 {
Balance_Pwm_Y=-balance_Y(Angle_Bias_Y,Gyro_Balance_Y); //Y方向的倾角控制
; h' |5 Y" [3 H
// if(++flag_target_2==4)//速度环频率慢于加速度环但是还没加速度环 . U" m4 L: q. R5 u2 F4 w
// {6 H6 J7 h: w3 E+ u# E
Velocity_Pwm_X=velocity_X(compute_X); //X方向的速度控制 x) @" D% |1 o" d( M! F* R5 }& a. k
Velocity_Pwm_Y=velocity_Y(compute_Y); //Y方向的速度控制 # U9 e6 \$ q8 ^$ O& `. k2 p
// flag_target_2=0;! o: ~7 w! R* m+ A
// } [4 D% G# d' A3 t
Move_X =Balance_Pwm_X+Velocity_Pwm_X; //===X方向控制量累加 ! i9 ]' C8 ^% g
Move_Y =Balance_Pwm_Y+Velocity_Pwm_Y; //===Y方向控制量累加
# h, y* x4 r ]7 O4 u8 e, u" `- _
Move_Z=0;
* U( y- E( T6 h$ v8 x6 y
Kinematic_Analysis(Move_X,Move_Y,Move_Z);//逆运动学分析得到ABC电机控制量
( Y6 q) }! h2 S' { E8 m2 B" X
}
0 Z( I- t( C4 y* ~ D; V* x0 f' e
Motor_A=Target_A;//直接调节PWM频率
; j/ H p) y! j# l# Q% `9 t9 G
Motor_B=Target_B;//直接调节PWM# h6 S7 t9 u" v
Motor_C=Target_C;//直接调节PWM2 M! ?$ y/ E" S! t. E2 X
//以下都是为了速度连续化处理防止突变+ T2 X4 O4 i+ ~3 G, V7 U( C& g: b
if(Motor_A==0) Motor_A=motor_a_last;
- ^; O+ o. z$ L3 w& m% x. W, `
if(Motor_B==0) Motor_B=motor_b_last;
7 L& S( l, n; n: ~( b
if(Motor_C==0) Motor_C=motor_c_last;
6 I( t3 ]: r. B5 w6 o! L# d
Xianfu_Pwm(2000);2 f" e, w% D' d( a* `9 A
Set_Pwm(Motor_A,Motor_B,Motor_C);/ ^7 \# w, Z) `) B7 U z( z% W
Gyro_Balance_X_last=Gyro_Balance_X;
! l# p1 ]. m# c& ?/ C! @
Gyro_Balance_Y_last=Gyro_Balance_Y;
; S$ e" a$ A2 ^6 Y" ~3 h
Gyro_Balance_Z_last=Gyro_Balance_Z;
8 y) n$ w% @( T- R3 D8 H
Angle_Balance_X_last=Angle_Balance_X;; K: l8 E1 l+ V' I; j# t
Angle_Balance_Y_last=Angle_Balance_Y;
( ]5 U- D. I. C
Angle_Balance_Z_last=Angle_Balance_Z;, @' E8 f& y$ P: a; s g1 V
motor_a_last=Motor_A;
0 R: ^8 Y% t' ]) I! x: a0 i
motor_b_last=Motor_B;( t& y( S% ^0 t5 ?
motor_c_last=Motor_C;% V/ s! C4 g( O4 a, y. F/ q
}' q" ~3 V C: R/ d, s+ I. U4 z# `
return 0;! o1 _% }4 e# z4 G$ l5 A4 g( l
}

复制代码

对于电机控制，由于采用的驱动方案是步进电机，调速的方式是改变驱动步进电机的脉冲频率。我这里选择了三个定时器，动态调节定时器的频率，具体方式是在初始化时设定好定时器的预分频系数psc的值，然后在程序里动态更改ARR寄存器的值，从而改变定时器的定时频率。

小车的运动学分解代码实现如下，参考了平衡小车之家的代码：

; ]: a. P8 {+ F' p* s
/**********************************************************, p+ n2 r+ f" U8 y' p
函数功能：小车运动数学模型: s2 d+ L" W! f5 k4 H2 t
入口参数：X Y Z 三轴速度或者位置
7 \. [! ~% m& l3 h+ [0 d
返回值：无
7 p" r0 y# S, K/ h$ `
***********************************************************/" n, D) W2 S" W1 b2 ^/ H
void Kinematic_Analysis(float Vx,float Vy,float Vz)) e# r4 p6 ^1 q
{ t4 p' E3 _+ u" V3 `" Z
Target_A = Vx + L_PARAMETER*Vz;
2 o8 m6 r! s( Q! [% V* T* x
Target_B = -X_PARAMETER*Vx + Y_PARAMETER*Vy + L_PARAMETER*Vz;
, E6 M) Q6 d. @ |) q7 V% @! I
Target_C = -X_PARAMETER*Vx - Y_PARAMETER*Vy + L_PARAMETER*Vz;2 O+ _: ]: n n( Q$ d
}
8 c$ t3 k! d; Z, h B# C
/*****************************************************************$ C) P' R: z. V k% f/ Y& E0 E3 N4 ^
函数功能：小车运动正运动学分析 / J6 s1 I6 V# v G6 U
入口参数：A B C三个电机的速度8 X' Q' X# J+ j1 ]+ T; D0 T1 S3 C+ u
返回值：无: Q6 @2 }" A) z+ w
******************************************************************/
|' h0 x# ~3 H' E% @! p
void Encoder_Analysis(float Va,float Vb,float Vc)
8 j9 p7 ~" p f) p
{
: T# v( d/ Y) Q
compute_X=(Va*2-Vb-Vc);* X1 X" x6 L- p+ Y8 I/ L
compute_Y=((Vb-Vc)*sqrt(3)); r# ~& Z" P* L# q# `: l
compute_Z=(Va+Vb+Vc);
5 g4 h y: U( Z& R' F2 w
}