CUBLI 简化 一维模型（有bug）

CUBLI 简化 一维模型（有bug）

---

前言

简化CUBLI为一维，制作了一个平衡三角（暂且这么叫），淘宝有成品卖。本着学习的态度于寒假期间（21年一月至二月）制作。
现情况如下：
1.MPU6050 获取平衡三角姿态
2.直流电机 以及 光电编码器
3.控制器 STM32 F411RE NUCLEO板
4.PID控制器
5.HAL库 开发
6.3D打印 && SW制图
7.工程代码、文件以及3D模型已打包值BaiDu Pan（按需索取）链接: link 提取码：aw5i
8.存在一些问题于文中述

---

一、CUBLI是什么？

「Cubli」出身于瑞士苏黎世联邦理工学院动力系统和控制实验室的项目，直译就是「小方块」。它是个15 × 15 × 15 厘米的金属方块，能借助3个转轮来实现多种技巧。最令人印象深刻的是 Cubli 超乎寻常的平衡能力。（出自知乎链接: link.）
将其简化为一维模型，原理类似于平衡小车。
先上靓照（哈哈哈啊哈）

二、制作步骤

1.MPU6050&&编码器操作

Cube MX配置以及代码等已置于下面两篇文章中
HAL库 MPU6050的使用 链接: link
HAL库 AB相编码器链接: link

2.什么是PID

PID即：Proportional（比例）、Integral（积分）、Differential（微分）的缩写。顾名思义，PID控制算法是结合比例、积分和微分三种环节于一体的控制算法，它是连续系统中技术最为成熟、应用最为广泛的一种控制算法，该控制算法出现于20世纪30至40年代，适用于对被控对象模型了解不清楚的场合。实际运行的经验和理论的分析都表明，运用这种控制规律对许多工业过程进行控制时，都能得到比较满意的效果。PID控制的实质就是根据输入的偏差值，按照比例、积分、微分的函数关系进行运算，运算结果用以控制输出。

在工业过程中，连续控制系统的理想PID控制规律为：

式中，Kp——比例增益，Kp与比例度成倒数关系； [2]
Tt——积分时间常数； [2]
TD——微分时间常数； [2]
u（t）——PID控制器的输出信号； [2]
e（t）——给定值r（t）与测量值之差。 [2]
以上关于PID内容摘自百度百科

3.我的控制策略

（1）控制策略如上图，目标是实现平衡三角直立，其角度是固定值；角度控制的输出为速度控制的输入，速度控制的输出为加载至电机的PWM高电平值；系统可反馈实时角度、转速等数据实现反馈。
实际代码中涉及积分限幅、输出限幅。个人理解：积分过大不利于系统稳定；由于输出为PWM高电平值，周期具有固定性，因此输出PWM高电平值应具有是上下限。

（2）系统具有方向性，因此应规定正方向。飞轮起到角动量平衡的作用，加速旋转方向如下图（减速相反）。

（3）保护措施。电机旋转速度快，飞轮具有一定的伤害性，当倾角偏离一定范围（直立目标挽救不回来）时控制电机停车。另外，3D模型设计将飞轮包裹于材料中，可实现一定的保护作用。

4.代码实现

此处仅展示PID控制策略

/**********************************
直立环PD控制器：Kp * Ek + Ki * Ek_S(积分) + Kd * Ek_D

入口：Med：机械中值（期望角度），Angle：真实角度
出口：直立环输出
**********************************/
int Vertical(float Med ,float Angle )
{
	int PWM_OUT ;
	
	Vertical_Err_Last  = Vertical_Err ;		//保存上次偏差
	Vertical_Err       = Angle - Med  ;		//计算偏差
	Vertical_Integral  = Vertical_Integral + Vertical_Err ;		//积分 

	//积分限幅
	if(Vertical_Integral > Vertical_Integral_Max) Vertical_Integral = Vertical_Integral_Max ;
	if(Vertical_Integral <-Vertical_Integral_Max) Vertical_Integral =-Vertical_Integral_Max ;
	
	//pid运算
	PWM_OUT = Vertical_Kp * Vertical_Err + Vertical_Ki * Vertical_Integral + Vertical_Kd * ( Vertical_Err - Vertical_Err_Last ) ;
	
	//PWM_OUT = Vertical_Kp * (Angle - Med ) + Vertical_Kd * (gyro - 0) ;
	
	//输出限幅
	if(PWM_OUT > Vertical_Out_Max ) PWM_OUT = Vertical_Out_Max ;
	if(PWM_OUT <-Vertical_Out_Max ) PWM_OUT =-Vertical_Out_Max ;
	
	return PWM_OUT ;
}

/**********************************
速度环PI控制器：Kp * Ek + Ki * Ek_S(积分) + Kd * Ek_D

**********************************/
int Velocity(int Target,int encoder)
{
	
	int PWM_OUT ;
	
	Velocity_Err_Last  = Velocity_Err ;		//保存上次偏差
	Velocity_Err       = encoder - Target ;		//计算偏差
	Velocity_Integral  = Velocity_Integral + Velocity_Err ;		//积分 
	
	//积分限幅
	if(Velocity_Integral > Velocity_Integral_Max) Velocity_Integral = Velocity_Integral_Max ;
	if(Velocity_Integral <-Velocity_Integral_Max) Velocity_Integral =-Velocity_Integral_Max ;
	
	//pid运算
	PWM_OUT = Velocity_Kp * Velocity_Err + Velocity_Ki * Velocity_Integral + Velocity_Kd * ( Velocity_Err - Velocity_Err_Last ) ;
	
	//PWM_OUT = Vertical_Kp * (Angle - Med ) + Vertical_Kd * (gyro - 0) ;
	
	//输出限幅
	if(PWM_OUT > Velocity_Out_Max ) PWM_OUT = Velocity_Out_Max ;
	if(PWM_OUT <-Velocity_Out_Max ) PWM_OUT =-Velocity_Out_Max ;
	
	return PWM_OUT ;
}

---

三、存在的问题及可能的解决方法

问题一：电机方向

当平衡三角情况如上图所示时实现平衡可逆时针加速或顺时针减速。顺时针减速不可实现，仅可逆时针加速

当平衡三角情况如上图所示时，实现平衡可逆时针加速或顺时针减速。若当前转速很高，逆时针加速会导致平衡三角瞬间失去平衡。此情况未考虑到。

问题二：平衡时间短（2~5S）

现阶段分析原因如下：
1.没有采用DMP库（小垃圾现在还搞不懂）角度计算方法为加速度简单的积分，系统启动时角度为0，而平衡三角启动时需手扶启动，存在机械中值（即Angle_Target）不准确问题。
2.如问题一中所述，电机方向控制好似存在不合理处，可能在第二种情况时瞬间反向，系统瞬间失衡。
3.调参问题，调参方法不正确，或者参数未找到合适值（可能性较小）。

解决方法：

1. 完善电机方向控制函数
2. 不采用角度控制，反而直接利用角速度控制。原因：平衡三角平衡时角速度变化范围小，应在0附近摆动。
3. 采用MPU9250，可以直接获取角度，避开DMP库（价格昂贵）

---

总结

平衡效果

VID

B站链接: link.

个人总结

此篇文章记述了作者（单片机业余爱好者）2021新年期间的劳动（哈哈），不完善处较多，时间原因暂且搁置。考研结束后没有进行复习，初试成绩将出，复试势在必行，明天就要进行复习了，祝自己好运，冲冲冲！！！