轮腿机器人代码调试与运动学解算补充

模块封装

算法封装

需要用到的 MATLAB 文件

三、电机相关补充说明

轮毂电机

本车所使用的轮毂电机协议为转矩闭环模式。

关节电机

笔者轮腿所使用的关节电机模式为 MIT 模式。零点设置在下限位处，当各电机处于零点处时，车子姿态如图所示。

四、代码调试及编写顺序

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

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

再获取底盘 yaw 角度当前值同时转化为角度制，同时将陀螺仪反馈的数据（0180 及 -1800）映射到 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 轴补偿值加到左轮力矩上

五、调试补充

一阶倒立摆（板凳模型）

1. 调试时，应先调板凳模型（即先用位置速度模式，将腿的位置固定，然后整车当做一阶倒立摆模型来调试。）当板凳模型调好之后，轮毂的 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]);

二阶倒立摆模型

3. 接下来在板凳模型的基础上调试关节电机，将关节电机设置为 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 是来使车子保持平稳的。

4. 然后只保留 ob_leg_r.lqr[4]~ob_leg_r.lqr[5] 行，此时车子往前倾，腿会往后蹬，车子往后倾，腿会往前蹬。

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_l1 + m_l2 + m_l3 + m_l4) / 2

未来展望

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

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