轮腿机器人代码调试与软硬件配置说明

一、硬件说明

材料选型

| 材料选型 | 型号 | 数量 | 单价 | | --- | --- | --- | | 关节电机 | DM-J8009P-2EC | 4 | 1369.00（教育优惠后） | | 轮毂电机 | 瓴控 MF9025V2(16T) | 2 | 990.00 | | 腿及电机固定板 | 碳板加工 | | 900.00 | | 车架 | 铝管加工 | | 500.00 | | 橡胶充气轮胎 | 伊诺华 8x1 1/4 内外胎 | 2 | 50.00 | | 开发板 | RoboMaster C 型开发板 | 1 | |

关节电机 ID 分配

轮毂电机 ID 分配

C 型开发板放置实物图

c 板放置示意图

二、软件说明

轮腿建模参考哈工程建模方法，双腿分开建模。五连杆解算参考相关运动学理论。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 清除函数

需要用到的 MATLAB 文件

名称	作用	用法
get_k_length.m	根据输入的腿长（单位 m）和输入的 QR 矩阵来计算 k 矩阵	在命令行窗口输入：`K = get_k_length(腿长值)`
get_k.m	计算多项式拟合的系数	点击'运行'

三、电机相关补充说明

轮毂电机

本车所使用的轮毂电机协议为官方标准协议。

轮毂电机驱动回复协议：

轮毂电机驱动回复协议

轮毂电机为转矩闭环模式：

轮毂电机转矩闭环模式

关节电机

笔者轮腿所使用的关节电机模式为MIT 模式。

笔者轮腿关节电机零点设置在下限位处，当各电机处于零点处时，车子姿态如图所示：

零点示意图

四、代码调试及编写顺序

通过在 MATLAB 文件：get_k_length.m 中的命令行窗口输入（以腿长 0.24m 为例，默认 QR 矩阵的各参数权重已经调好）：K = get_k_length(0.24)，复制运行结果；
接着进入 while 循环，更新数据。

获取底盘 pitch 角度，此时对于右腿，车子前倾，陀螺仪 pitch 轴反馈弧度为正数。由于左右腿坐标系方向相反，所以左腿模型获取到的 pitch 前要加上负号；

update_pitch

再获取底盘 yaw 角度当前值同时转化为角度制，同时将陀螺仪反馈的数据（0~180 及 -180~0）映射到 0~360 度；

update_yaw

先获取 phi1 和 phi4 的角度，根据关节电机反馈的弧度值加上零点时腿所在的弧度值（零点时，phi1 弧度值为π，phi4 为 0rad）;

update_phis1

进入到 void ChassisR_task(void const *pvParameters){...} 中，首先进行初始化，获取到遥控器的数据指针、获取电机反馈数据、获取陀螺仪角速度指针、获取陀螺仪角度指针、给右杆长赋值、给左杆长赋值、初始化左右腿长的 pid、初始化 yaw 转向 pid：

chassisR_task 初始化

在 chassisR_task.c 文件中，将结果粘贴到 float LQR_KK[12]={...}； 中，如图所示：

LQR_KK

左右腿坐标系相反详解图解

左右腿坐标系相反详解

接下来进行右腿的循环计算环节，通过 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 数据

在初始化之后，获取底盘 yaw 角度当前值；

yaw 目标值初始化

转向控制

仿照右腿模型，不作过多讲解

左腿解算

左腿模型部分

在得到 Tp 和 F0 之后，通过 VMC_calc_2(&vmc_leg_right); 函数计算出前后两关节电机输出力矩，最后转换为转矩电流；

右腿 VMC

转矩电流

接下来加入腿长控制器：vmc_leg_right.F0=10.0f/arm_cos_f32(vmc_leg_right.theta)+PID_calc(&LegR_Pid,vmc_leg_right.L0,0.24f); 得到沿腿的推力 F0，方向为指向地面；

腿长控制器及 VMC

接着再计算右边中心轴髋关节输出力矩，即车体绕两关节电机连线中心点的力矩

右腿 LQR

得到 theta 和 d_theta 之后，进行右轮毂电机的 lqr 计算，得到轮毂的输出力矩；

右轮毂 LQR

右腿模型部分

根据轮毂电机反馈的速度（注意，MF9025V2 电机反馈的速度单位为 1dps，需要转换为 rpm）计算左右轮的位移速度，其中 CHASSIS_MOTOR_RPM_TO_VECTOR_SEN 为 LK9025 转子转速 (rpm) 转化成底盘速度 (m/s) 的比例即 k= 2πr/60。最后将得到的左右轮位移速度相加（右轮速度方向应取反以保证两轮反馈的速度方向最后一致）再除 2 取平均得到整车位移速度。通过对整车位移速度积分，求得整车位移；

update_整车位移速度

获取遥控器对于整车的速度目标值，其中 CHASSIS_VX_RC_SEN 为遥控器前进摇杆（max 660）转化成车体前进速度（m/s）的比例；

update_remotecontrol

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 图解补充

VMC 图解

mp 摆杆质量（腿的质量）在实物中为： m p = （m l1 + m l 2 + m l 3 + m l 4）/2

未来展望

在电控方面，由于时间问题，本车只完成了轮腿的基础功能：平衡、前后平移、左右转向。未来还可以一步添加跳跃、不同腿长下的拟合、roll 轴补偿、打滑检测、离地检测、左右摇摆、太空漫步等动能的算法。

在机械方面，轮腿关节轴系需要进一步优化，笔者所使用的关节处的轴系为'塞打螺丝 - 垫片 - 法兰轴承 - 大臂 - 法兰轴承 - 四氟垫片（或推力球轴承）- 小臂 - 垫片 - 螺母'的传统形式，好处就是结构简单，方便设计，零件均为标件价格便宜。但是弊端也非常严重：（1）高强度跳跃下自锁螺母容易松开，时不时需要用开口扳手与内六角扳手进行拧紧、（2）拧的太死又会出现两个连杆无法相对旋转、（3）在轮电机输出面与关节电机输出面距离较远且轮组重的情况下容易出现外八的情况。所以在未来可参考 RoboMaster 论坛上的一篇文章关于轮腿关节轴系设计的文章。使用自行车碗组的结构，能有效减少车轮外八的现象。