C算法
轮腿机器人代码调试与运动控制补充
轮腿机器人的硬件选型、软件架构设计及代码调试流程。硬件采用 RoboMaster C 型开发板配合关节与轮毂电机。软件基于任务封装、模块封装及算法封装，涉及五连杆运动学解算与 VMC 算法。重点阐述了 LQR 控制器在倒立摆模型中的调试方法，包括状态变量极性确定、左右腿坐标系差异处理及转向控制逻辑。提供了关键代码片段与 MATLAB 计算辅助工具的使用说明，旨在帮助开发者完成轮腿系统的平衡与运动控制实现。
轮腿机器人的硬件选型、软件架构设计及代码调试流程。硬件采用 RoboMaster C 型开发板配合关节与轮毂电机。软件基于任务封装、模块封装及算法封装，涉及五连杆运动学解算与 VMC 算法。重点阐述了 LQR 控制器在倒立摆模型中的调试方法，包括状态变量极性确定、左右腿坐标系差异处理及转向控制逻辑。提供了关键代码片段与 MATLAB 计算辅助工具的使用说明，旨在帮助开发者完成轮腿系统的平衡与运动控制实现。
| 材料选型 | 型号 | 数量 | 单价 |
|---|---|---|---|
| 关节电机 | DM-J8009P-2EC | 4 | 1369.00（教育优惠后） |
| 轮毂电机 | 瓴控 MF9025V2(16T) | 2 | 990.00 |
| 腿及电机固定板 | 碳板加工 | 900.00 | |
| 车架 | 铝管加工 | 500.00 | |
| 橡胶充气轮胎 | 伊诺华 8x1 1/4 内外胎 | 2 | 50.00 |
| 开发板 | RoboMaster C 型开发板 | 1 |
轮腿建模参考哈工程建模方法，双腿分开建模。五连杆解算参考相关技术文档。MATLAB 脚本讲解视频供参考。
| 任务 | 作用 |
|---|---|
| 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 清除函数 |
| 名称 | 作用 | 用法 |
|---|---|---|
| get_k_length.m | 根据输入的腿长（单位 m）和输入的 QR 矩阵来计算 k 矩阵 | 在命令行窗口输入：K = get_k_length(腿长值) |
| get_k.m | 计算多项式拟合的系数 | 点击'运行' |
本车所使用的轮毂电机协议（官方参考资料：瓴控电机 CAN 协议说明 V2.36.pdf）
轮毂电机驱动回复协议：
轮毂电机为转矩闭环模式：
笔者轮腿所使用的关节电机模式为MIT 模式。（官方参考资料：达妙文档: 包含通讯协议说明、帮助文档）
笔者轮腿关节电机零点设置在下限位处，当各电机处于零点处时，车子姿态如图所示：
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]);
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]);
mp 摆杆质量（腿的质量）在实物中为： m p = （m l1 +ml 2 +ml 3 +ml 4）/2
在电控方面，由于时间问题，本车只完成了轮腿的基础功能：平衡、前后平移、左右转向。未来还可以一步添加跳跃、不同腿长下的拟合、roll 轴补偿、打滑检测、离地检测、左右摇摆、太空漫步等动能的算法。
在机械方面，轮腿关节轴系需要进一步优化，笔者所使用的关节处的轴系为'塞打螺丝 - 垫片 - 法兰轴承 - 大臂 - 法兰轴承 - 四氟垫片（或推力球轴承）- 小臂 - 垫片 - 螺母'的传统形式，好处就是结构简单，方便设计，零件均为标件价格便宜。但是弊端也非常严重：（1）高强度跳跃下自锁螺母容易松开，时不时需要用开口扳手与内六角扳手进行拧紧、（2）拧的太死又会出现两个连杆无法相对旋转、（3）在轮电机输出面与关节电机输出面距离较远且轮组重的情况下容易出现外八的情况。所以在未来可参考 RoboMaster 论坛上的一篇文章 [轮腿关节轴系设计——自行车碗组的应用]。使用自行车碗组的结构，能有效减少车轮外八的现象。
微信公众号「极客日志」，在微信中扫描左侧二维码关注。展示文案：极客日志 zeeklog
使用加密算法（如AES、TripleDES、Rabbit或RC4）加密和解密文本明文。 在线工具，加密/解密文本在线工具，online
将字符串编码和解码为其 Base64 格式表示形式即可。 在线工具，Base64 字符串编码/解码在线工具，online
将字符串、文件或图像转换为其 Base64 表示形式。 在线工具，Base64 文件转换器在线工具，online
将 Markdown（GFM）转为 HTML 片段，浏览器内 marked 解析；与 HTML转Markdown 互为补充。 在线工具，Markdown转HTML在线工具，online
将 HTML 片段转为 GitHub Flavored Markdown，支持标题、列表、链接、代码块与表格等；浏览器内处理，可链接预填。 在线工具，HTML转Markdown在线工具，online
通过删除不必要的空白来缩小和压缩JSON。 在线工具，JSON 压缩在线工具，online