轮腿机器人代码调试补充
* @Author: 星夜雨夜
* @brief: 轮腿基础代码编写调试补充，移植自达妙开源代码
* @attention：笔者默认读者已经熟练掌握机甲大师RoboMaster c型开发板例程代码的底盘代码和INS_task.c陀螺仪代码、熟练掌握各电机can协议和遥控器dbus协议。默认读者已能看懂轮腿圣经和玺佬的五连杆运动学解算与VMC。建议读者仔细研读轮腿圣经3~5遍，边看MATLAB文件和达妙开源代码，掌握轮腿调试和编写大致思路。一定要注意各状态变量的单位和正负号是否正确，轮腿调试过程中，最难之处在于极性是否正确。本车所有电机均为逆时针旋转为正方向。
！！！强烈建议读者在开发轮腿之前，先运用LQR算法完成一阶倒立摆的平衡小车（即板凳模型）的实现
！！！如果时间紧，其实完全可以不搞仿真，直接实机开调。仿真不疯，实物不一定不疯；但实物疯，仿真必疯。
调试成果展示视频链接（抖音）：轮腿机器人
一阶倒立摆平衡小车参考资料：
1.本科毕设 轮腿式双足机器人 开源文件演示_哔哩哔哩_bilibili(资料在视频评论区)
2.达妙平衡小车开源：[达妙科技开源系列-平衡小车] 第一弹_哔哩哔哩_bilibili（资料下载链接：平衡小车 · kit/balance_robot - 码云 - 开源中国）
——————————————————
本文代码及机械结构下载链接：https://pan.baidu.com/s/1zV66FamRp5gAD4H8K5IDGA?pwd=85zx 提取码: 85zx
文章目录
轮腿模型图
一、 硬件说明
| 材料选型 | 型号 | 数量 | 单价 |
|---|---|---|---|
| 关节电机 | DM-J8009P-2EC | 4 | 1369.00（教育优惠后） |
| 轮毂电机 | 瓴控MF9025V2(16T) | 2 | 990.00 |
| 腿及电机固定板 | 碳板加工 | 900.00 | |
| 车架 | 铝管加工 | 500.00 | |
| 橡胶充气轮胎 | 伊诺华8x1 1/4内外胎 | 2 | 50.00 |
| 开发板 | RoboMaster C型开发板 | 1 |
关于电机选型参考资料：轮腿平衡步兵总结 - 电机篇,目前为止，笔者所选用的关机电机和轮毂电机暂未出现缺陷。
关于轮胎选型，3D模型开源链接：【RM2023-平衡步兵充气轮开源】武汉工程大学-RoboMaster 社区
淘宝购买链接：儿童车8寸轮胎200x45充气胎自行车8x1 1/4寸内胎外胎-淘宝网
关节电机ID分配
轮毂电机ID分配
C型开发板放置实物图
二、软件说明
轮腿建模完全参考哈工程的建模方法，双腿分开建模
五连杆解算完全参考玺佬的五连杆运动学解算与VMC
五连杆视频讲解参考链接：【手把手教做平衡步兵轮足】5_运动学正逆解-从理论到实操_哔哩哔哩_bilibili
上交模型讲解参考链接：上交平衡步兵模型。理论，仿真，源码详细讲解。助您快速上手。（一）_哔哩哔哩_bilibili
MATLAB脚本讲解视频参考链接：首发—手把手介绍如何将理论应用到matlab代码_哔哩哔哩_bilibili
以上视频笔者全部都看过，供参考。
任务封装
| 任务 | 作用 |
|---|---|
| INS_task.c | 解算陀螺仪数据，得到三轴加速度计、三轴弧度 |
| Uart1_task.c | 向上位机发送数据，方便配合“vofa+”调试pid |
| chassisR_task.c | 通过调用“VMC_calc.c”的函数得到数据，并结合MATLAB计算得到k[12]矩阵计算左右轮毂电机的输出力矩和车架中心轴的输出力矩，最后调用VMC_calc.c的函数得到单腿的前后两个关节电机的输出力矩 |
模块封装
| 模块 | 作用 |
|---|---|
| CAN_receive.c | 所有电机协议及电机功能的函数封装 |
| remote_control.c | 遥控器dbus协议函数封装 |
算法封装
| 算法 | 作用 |
|---|---|
| VMC_calc.c | 1.五连杆解算得到单腿的phi0、腿长L0、theta、dot_theta 2.通过雅可比矩阵得到单腿的前后关节电机的输出力矩 3.建议参考MATLAB文件中的 d_phi0.m和VMC_calc.m |
| arm_math.h | |
| arm_cortexM4lf_math.lib | 结合arm_math.h提供必要的sin、cos、tan三角函数运算 |
| pid.c | pid初始化函数、pid计算函数、pid清除函数 |
需要用到的MATLAB文件
| 名称 | 作用 | 用法 |
|---|---|---|
| get_k_length.m | 根据输入的腿长（单位m）和输入的QR矩阵来计算k矩阵 | 在命令行窗口输入：K = get_k_length(腿长值) |
| get_k.m | 计算多项式拟合的系数 | 点击“运行” |
三、电机相关补充说明
轮毂电机
本车所使用的轮毂电机协议（官方参考资料：瓴控电机CAN协议说明 V2.36.pdf）
轮毂电机驱动回复协议：
轮毂电机为转矩闭环模式：
关节电机
笔者轮腿所使用的关节电机模式为MIT模式。（官方参考资料：达妙文档: 包含通讯协议说明、帮助文档）
笔者轮腿关节电机零点设置在下限位处，当各电机处于零点处时，车子姿态如图所示：
四、代码调试及编写顺序
- 通过在MATLAB文件：
get_k_length.m中的命令行窗口输入（以腿长0.24m为例，默认QR矩阵的各参数权重已经调好）：K = get_k_length(0.24)，复制运行结果； - 接着进入
while循环,更新数据。
获取底盘pitch角度，此时对于右腿，车子前倾，陀螺仪pitch轴反馈弧度为正数。由于左右腿坐标系方向相反，所以左腿模型获取到的pitch前要加上负号；
再获取底盘yaw角度当前值同时转化为角度制，同时将陀螺仪反馈的数据（0~180及-180~0）映射到0~360度；
先获取phi1和phi4的角度，根据关节电机反馈的弧度值加上零点时腿所在的弧度值（零点时，phi1弧度值为π，phi4为0rad）;
进入到void ChassisR_task(void const *pvParameters){...}中，首先进行初始化，获取到遥控器的数据指针、获取电机反馈数据、获取陀螺仪角速度指针、获取陀螺仪角度指针、给右杆长赋值、给左杆长赋值、初始化左右腿长的pid、初始化yaw转向pid：
在chassisR_task.c文件中，将结果粘贴到float LQR_KK[12]={...}；中，如图所示：
左右腿坐标系相反详解图解
- 接下来进行右腿的循环计算环节，通过
VMC_calc_1_right(&vmc_leg_right, &INS_data,0.003f);计算theta和d_theta给lqr用，同时也通过计算实时得到右腿长L0，该任务控制周期是0.003秒。
在底盘循环计算环节，首先通过循环限幅函数，将yaw目标值限制在0~360°内，再通过过零处理函数，处理陀螺仪反馈的yaw值在0与360之间的突变问题。最后通过PD控制器计算出转向力矩赋值给wz_set。通过左右轮的轮毂力矩加上转向力矩wz_set,使得两轮能实现差速，最终车子能够实现转向。
在void chassis_feedback_update(void){...}中实时获取底盘yaw角度当前值和计算底盘yaw轴的目标值；
在初始化之后，获取底盘yaw角度当前值；
转向控制
仿照右腿模型，不作过多讲解
左腿模型部分
在得到Tp和F0之后，通过VMC_calc_2(&vmc_leg_right);函数计算出前后两关节电机输出力矩，最后转换为转矩电流；
接下来加入腿长控制器：vmc_leg_right.F0=10.0f/arm_cos_f32(vmc_leg_right.theta)+PID_calc(&LegR_Pid,vmc_leg_right.L0,0.24f);得到沿腿的推力F0,方向为指向地面；
接着再计算右边中心轴髋关节输出力矩，即车体绕两关节电机连线中心点的力矩
得到theta和d_theta之后，进行右轮毂电机的lqr计算，得到轮毂的输出力矩；
右腿模型部分
根据轮毂电机反馈的速度（注意，MF9025V2电机反馈的速度单位为1dps，需要转换为rpm）计算左右轮的位移速度，其中CHASSIS_MOTOR_RPM_TO_VECTOR_SEN为LK9025转子转速(rpm)转化成底盘速度(m/s)的比例即k= 2πr/60。最后将得到的左右轮位移速度相加（右轮速度方向应取反以保证两轮反馈的速度方向最后一致）再除2取平均得到整车位移速度。通过对整车位移速度积分，求得整车位移；
获取遥控器对于整车的速度目标值，其中CHASSIS_VX_RC_SEN为遥控器前进摇杆（max 660）转化成车体前进速度（m/s）的比例；
chassis_move.wheel_motor[1].wheel_T+=wz_set; chassis_move.wheel_motor[0].wheel_T+=wz_set;//将yaw轴补偿值加到左轮力矩上五、调试补充
一阶倒立摆（板凳模型）
- 调试时，应先调板凳模型（即先用位置速度模式，将腿的位置固定，然后整车当做一阶倒立摆模型来调试。）当板凳模型调好之后，轮毂的
ob_lqr.lqr[0]~ob_lqr.lqr[5]的极性也就正确了。
ob_lqr.lqr[0]~ob_lqr.lqr[5]的极性应该保持一致，要么全为正数要么全为负数。
//observe结构体，方便调试极性时debug观测 ob_lqr.lqr[0]=LQR_KK[0]*(vmc_leg_right.theta-0.0f); ob_lqr.lqr[1]=LQR_KK[1]*(vmc_leg_right.d_theta-0.0f); ob_lqr.lqr[2]=LQR_KK[2]*(chassis_move.x_filter-0); ob_lqr.lqr[3]=LQR_KK[3]*(chassis_move.v_filter-0); ob_lqr.lqr[4]=-LQR_KK[4]*(chassis_move.myPithR-0.0f); ob_lqr.lqr[5]=-LQR_KK[5]*(chassis_move.myPithGyroR-0.0f); chassis_move.wheel_motor[1].wheel_T=(ob_lqr.lqr[0]+ob_lqr.lqr[1]+ob_lqr.lqr[2]+ob_lqr.lqr[3]+ob_lqr.lqr[4]+ob_lqr.lqr[5]);二阶倒立摆模型
- 接下来在板凳模型的基础上调试关节电机，将关节电机设置为MIT模式，先把
ob_leg_r.lqr[2]~ob_leg_r.lqr[5]行注释掉，只保留ob_leg_r.lqr[0]~ob_leg_r.lqr[1]行。此时车子前倾，腿往前蹬，车子往后倾，腿往后蹬。vmc_leg_right.theta是来使车子保持平稳的。如下段代码： - 然后只保留
ob_leg_r.lqr[4]~ob_leg_r.lqr[5]行，此时车子往前倾，腿会往后蹬，车子往后倾，腿会往前蹬。如下段代码： - 当只保留
ob_leg_r.lqr[0]~ob_leg_r.lqr[1]和ob_leg_r.lqr[4]~ob_leg_r.lqr[5]行时，即去掉位移项和速度项时，结合轮毂部分的lqr，此时车子可以保持平稳。
//ob_leg_r.lqr[0]=LQR_KK[6]*(vmc_leg_right.theta-0.0f);//ob_leg_r.lqr[1]=LQR_KK[7]*(vmc_leg_right.d_theta-0.0f);//ob_leg_r.lqr[2]=LQR_KK[8]*(-chassis_move.x_filter-0);//ob_leg_r.lqr[3]=LQR_KK[9]*(-chassis_move.v_filter-0); ob_leg_r.lqr[4]=-LQR_KK[10]*(chassis_move.myPithR-0.0f); ob_leg_r.lqr[5]=-LQR_KK[11]*(chassis_move.myPithGyroR-0.0f); 右边中心轴髋关节输出力矩，车体绕两关节电机连线中心点的力矩 vmc_leg_right.Tp=(ob_leg_r.lqr[0]+ob_leg_r.lqr[1]+ob_leg_r.lqr[2]+ob_leg_r.lqr[3]+ob_leg_r.lqr[4]+ob_leg_r.lqr[5]);//只有vmc_leg_right.theta和vmc_leg_right.d_theta时，腿往前蹬；只有chassis_move.myPithR和chassis_move.myPithGyroR时，腿往后蹬。 ob_leg_r.lqr[0]=LQR_KK[6]*(vmc_leg_right.theta-0.0f); ob_leg_r.lqr[1]=LQR_KK[7]*(vmc_leg_right.d_theta-0.0f);//ob_leg_r.lqr[2]=LQR_KK[8]*(-chassis_move.x_filter-0);//ob_leg_r.lqr[3]=LQR_KK[9]*(-chassis_move.v_filter-0);//ob_leg_r.lqr[4]=-LQR_KK[10]*(chassis_move.myPithR-0.0f);//ob_leg_r.lqr[5]=-LQR_KK[11]*(chassis_move.myPithGyroR-0.0f);//右边中心轴髋关节输出力矩，车体绕两关节电机连线中心点的力矩 vmc_leg_right.Tp=(ob_leg_r.lqr[0]+ob_leg_r.lqr[1]+ob_leg_r.lqr[2]+ob_leg_r.lqr[3]+ob_leg_r.lqr[4]+ob_leg_r.lqr[5]);[注意！]vmc_leg_right.theta与陀螺仪的放置方向有关，建议使用keil的debug调试工具调试一下，观测腿长L0、phi0、phi1、phi2、phi3、phi4和vmc_leg_right.theta是否正确，观测vmc_leg_right.theta时，不能只看其是否增加减小，建议代入两三组数据手动解算一下数值。
关节电机的速度项和位移项的极性一开始较为难确定，总的来说，关节电机的位移项的极性与速度项一致。关节电机的速度项的极性与chassis_move.myPithR的极性一致。
各状态变量的极性说明（极性的确定非常重要！！！）
轮毂极性部分
//observe结构体，方便调试极性时debug观测 ob_lqr.lqr[0]=LQR_KK[0]*(vmc_leg_right.theta-0.0f); ob_lqr.lqr[1]=LQR_KK[1]*(vmc_leg_right.d_theta-0.0f); ob_lqr.lqr[2]=LQR_KK[2]*(chassis_move.x_filter-0); ob_lqr.lqr[3]=LQR_KK[3]*(chassis_move.v_filter-0); ob_lqr.lqr[4]=-LQR_KK[4]*(chassis_move.myPithR-0.0f); ob_lqr.lqr[5]=-LQR_KK[5]*(chassis_move.myPithGyroR-0.0f); chassis_move.wheel_motor[1].wheel_T=(ob_lqr.lqr[0]+ob_lqr.lqr[1]+ob_lqr.lqr[2]+ob_lqr.lqr[3]+ob_lqr.lqr[4]+ob_lqr.lqr[5]);ob_lqr.lqr[0]~ob_lqr.lqr[5]的极性应该保持一致，要么全为正数要么全为负数。
vmc_leg_right.d_theta的极性应与vmc_leg_right.theta保持一致。
chassis_move.myPithGyroR的极性应与chassis_move.myPithR保持一致。
关节极性部分
//只有vmc_leg_right.theta和vmc_leg_right.d_theta时，腿往前蹬；只有chassis_move.myPithR和chassis_move.myPithGyroR时，腿往后蹬。 ob_leg_r.lqr[0]=LQR_KK[6]*(vmc_leg_right.theta-0.0f); ob_leg_r.lqr[1]=LQR_KK[7]*(vmc_leg_right.d_theta-0.0f); ob_leg_r.lqr[2]=LQR_KK[8]*(-chassis_move.x_filter-0); ob_leg_r.lqr[3]=LQR_KK[9]*(-chassis_move.v_filter-0); ob_leg_r.lqr[4]=-LQR_KK[10]*(chassis_move.myPithR-0.0f); ob_leg_r.lqr[5]=-LQR_KK[11]*(chassis_move.myPithGyroR-0.0f);//右边中心轴髋关节输出力矩，车体绕两关节电机连线中心点的力矩 vmc_leg_right.Tp=(ob_leg_r.lqr[0]+ob_leg_r.lqr[1]+ob_leg_r.lqr[2]+ob_leg_r.lqr[3]+ob_leg_r.lqr[4]+ob_leg_r.lqr[5]);六、轮腿圣经图解补充
轮腿模型图解补充
五连杆及VMC图解补充
mp摆杆质量（腿的质量）在实物中为：
m p = （ m l 1 + m l 2 + m l 3 + m l 4 ） / 2 m~p~=（m~l1~+ml~2~+ml~3~+ml~4~）/2 m p =（m l1 +ml 2 +ml 3 +ml 4 ）/2
未来展望
在电控方面，由于时间问题，本车只完成了轮腿的基础功能：平衡、前后平移、左右转向。未来还可以一步添加跳跃、不同腿长下的拟合、roll轴补偿、打滑检测、离地检测、左右摇摆、太空漫步等动能的算法。
在机械方面，轮腿关节轴系需要进一步优化，笔者所使用的关节处的轴系为”塞打螺丝-垫片-法兰轴承-大臂-法兰轴承-四氟垫片（或推力球轴承）-小臂-垫片-螺母”的传统形式，好处就是结构简单，方便设计，零件均为标件价格便宜。但是弊端也非常严重：（1）高强度跳跃下自锁螺母容易松开，时不时需要用开口扳手与内六角扳手进行拧紧、（2）拧的太死又会出现两个连杆无法相对旋转、（3）在轮电机输出面与关节电机输出面距离较远且轮组重的情况下容易出现外八的情况。所以在未来可参考RoboMaster论坛上的一篇文章[轮腿关节轴系设计——自行车碗组的应用](轮腿关节轴系设计——自行车碗组的应用-RoboMaster 社区)。使用自行车碗组的结构，能有效减少车轮外八的现象。
