本文学习自电科中山柳同学的方案分享

遇到的问题：

1、轮毂输出力矩不足以配合腿部收敛机体姿态（即腿部关节输出和轮毂输出都被LQR拉大了，但是轮毂最大力矩不够用了）

可以引入MPC对LQR输出的反馈增益矩阵K 进行反向增益（只削弱腿部的输出），可以加快机体姿态收敛速度，但这种方式会牺牲一部分加速度。

2、容易打滑。

引入卡尔曼滤波对加速度和轮毂速度进行融合，因为相对惯性空间完全不存在打滑的概念，不会使得机体因为速度状态变量的突变而发散

3、轮腿离地

引入KNN对离地检测问题进行二分类，省去VMC逆解，直接用正解的数据进行离地检测判断，当进入离地状态则关闭腿部推力前馈，相比直接用腿部当前向下推力和髋关节推力求得的结果更加稳定，调试更为简单。

嵌入式系统采用了FreeRTOS和C++STD标准库函数作为基底进行框架搭建。

硬件上，采用了STM32F405RGT6作为底盘控制的主控。其中CAN1中断用于接收关节电机数据，CAN2中断负责接收板间通讯、轮毂电机和裁判系统的数据。

其中裁判系统数据是经过一块单独搭载STM32F405RGT6的“裁判系统处理&转发板”进行解包，并发送部分有效的数据作为底盘控制，自定义UI的发送也是通过“裁判系统处理&转发板”发送给电管。

考虑到关节电机数量为4个，CAN占用率会比较高，就不在这条CAN上加入更多的通讯了。由于作为底盘控制的单片机算力“几乎用完”，故将IMU放置到外部去做处理，使用ICM20602和BMI088数据融合加速度和角速度使用（其中BMI088的权重更大），使用卡尔曼滤波和互补滤波进行姿态解算得到欧拉角，并通过串口方式回传给底盘控制的主控，主控采用DMA进行数据读取。

系统开启时间片轮转的任务包括了：IMU_Task、RC_Task、Joint_Task、Power_Task、RunningTime_Task、Cal_Task、Chassis_Task。

其中，高优先级的任务为：RunningTime_Task 、IMU_Task、Cal_Task。

中优先级的任务为：Chassis_Task、Joint_Task。

低优先级的任务为：RC_Task、Power_Task。

下图为底盘控制嵌入式系统流程图。

软件实现逻辑

MPC+LQR

        在平衡步兵的运动中，LQR不能做到非常高的鲁棒性，增加Q矩阵可能会导致某些量权重太大，造成机体震荡甚至发散，故引入MPC进行姿态修正。将MPC的输出反向叠加到LQR状态反馈矩阵K中对应腿部输出的部分，削弱腿部的作用，这样能够快速让机体姿态收敛。但在机体姿态发生剧烈变化的时候，机体加速度会减小（速度和位移状态变量对应的输出会变小），此时可以将LQR中的Q矩阵的相关状态变量的权重给大，以增加鲁棒性。

下图为MPC+LQR的控制框图：