莱罗三角形平衡系统

---

前言

莱罗三角形也叫鲁洛克斯三角形，它是这样得到的：先画正三角ABC，然后以正三角形ABC的三个顶点为圆心，边长长为半径画弧得到的图形，如下图

我用cad画出两个中间留出电机位的莱罗三角形利用激光器切割机切割亚克力板，两个板用铜柱连接，电机装上动量轮，mpu6050水平放置在两铜柱之间得到初始机械结构

他的平衡原理就是角动量守恒，当整体有向左边倒的时候，动量轮逆时针加速，根据角动量守恒，电机应该连着整个系统顺时针加速从而抵消向左边倒的趋势，向右边倒时原理相同，然后先看看我做出来的效果，我把代码上传csdn，想要做这个项目的朋友可以下载参考

一、mpu6050使用dmp得到姿态角

dmp是mpu6050内部一个可扩展的数字运动处理器可以将欧拉角以四元数的形式输出这样就不需要我们对陀螺仪和加速度传感器采集的原始数据进行融合和滤波，这一步我使用了正点原子的代码，下面说说怎么移植过来自己的板子使用。

首先把这几个文件添加到工程

修改inv_mpu.c文件，把i2c的读写函数改成自己的，delay_ms函数也改为自己编写的（要注意不能使用hal库给的延时函数因为它使用的是系统定时器优先级过低会导致程序卡死）

修改inv_mpu.h里面的数据输出频率，和系统控制周期对应就好，但是不能超过200hz这是硬件限制的。

二、使用pid算法控制

直接上代码
角度的控制要把动态误差控制到很小（不然倾斜角度过大回调所需动量超过了电机极限系统就会崩溃）所以PD的控制作用很大，后面的调参也主要是对这两个参数，I可有可无

为什么需要速度控制呢？回到前面分析的原理，我们系统的平衡利用电机加速提供角动量，那如果电机速度到了极限没办法加速了那系统不是要崩溃了？所以这是一个对静态误差的控制（要求电机速度保持为0），使用PI控制器控制即可。

最后我把两个pid输出做差后的值作为pwm波占空比用来驱动电机，这里我们不需要纠结其中数学原理，只需要明白测量角度和目标角度的差值作为角度控制输入，电机编码器10ms读到的值（这个相当于电机转速）作为速度控制输入，这样我们得到的两个输出，一个和系统倾斜角度有关一个和电机速度有关，他们共同决定了电机转速，后面我们只需要把pid参数调好就能让角度和电机速度对pwm占空比的作用得到适合的权重使系统稳定。

三、pid调参

调参之前测一下系统平衡时候mpu6050的角度输出，把这个作为角度目标值写进代码

1、首先是确定角度环P，这时先把速度环去掉

确定极性：向左边倒时动量轮应该逆时针加速，向右边倒时动量轮应该顺时针加速，否则极性反了
确定大小：把系统在平衡位置立起来逐步加大P值直到有明显的回复现象，可以来回震荡几次

2、然后是确定角度环D

确定极性：与P相同
确定大小：在原本P大小下逐步加大D，直到震荡幅度是可以收敛的

3、最后加入速度环

确定极性：如果动量轮转速越来越快就是反了，PI极性一样
确定大小：逐步加大P、I的值，两个值差不多，得到很好的控制效果后再尝试分别微调两个值