前几天自己写完了FOC程序并深度掌握以后，我就想找一个无刷电机的项目练练手，想到自己寒假已经做过有刷平衡车的项目以后，并且对轮腿的实现原理也挺感兴趣的，就决定深度学一学轮足机器人。

我目前学的轮足机器人是桌面级别的，也就是它不是所有的地方都用的是无刷电机，所以实现难度没有那么高，主要难在轮腿如何连续地变腿高，如何在变腿高的同时还能保持机器人稳定，以及复杂地形自适应上。而在有了平衡车以及FOC的学习经验后，其实要实现桌面级轮足机器人难度也不大，本篇文章就大概介绍一下轮足机器人的实现思路。

这是我自己绘制的一个思维导图，有助于理解整个轮足机器人实现的原理。这次代码的实现需要用到两颗MCU，一颗专门负责两路无刷电机的控制，一颗负责除了FOC控制之外的其他任务。这么做的目的，是因为我的FOC电流环频率是10khz，也就是100us内要执行一轮FOC所需执行的任务，而如果把后续变腿高里的运动学逆解公式以及控制舵机的任务都交给F407的话，会导致407CPU超载，并且实时性也很难有所保证，所以其他的任务交给第二颗芯片，同时利用板间通信，将数据及时传给407以进行无刷电机的控制。

接下来介绍一下各个功能是怎么实现的：

1，轮足机器人的自稳定以及转向的控制

  float speedAvg = (M0Dir*BLDCData.M0_Vel + M1Dir*BLDCData.M1_Vel)/2;
  float targetAngle = PID_VEL(targetSpeed - speedAvg);
  float turnTorque = turnKp * (robotMotion.turn-MPU6050.GyroZ);
  float torque1 = kp1*(targetAngle - robotPose.pitch) + turnTorque;
  float torque2 = kp1*(targetAngle - robotPose.pitch) - turnTorque;

  motors.setTargets(M0Dir*torque1, M1Dir*torque2);

其实这部分代码的实现思路与平衡车是一样的，在腿高没有改变的情况下，轮足机器人就是一台平衡车。这里还是讲讲代码，首先，先利用编码器测算出两个无刷电机的平均轮速，然后将这个平均轮速放入速度PID环进行计算，targetSpeed初始值为0，就表示是希望轮足机器人平衡时速度为0，而这个值也是可以改变的，需要机器人向前走，就给这个值赋值为正数，机器人就会像前走了，反向也是同理。将这个经速度PID环输出的结果作为最终角度环（也就是最后要使轮足机器人倾斜角为0）的目标值，可以使角度环迅速调整机器人俯仰角为0，同时还可以使机器人速度变为0，就不会出现机器人因电机超出物理极限而使机器人倒下的情况了。接下来是转向环，robotMotion.turn与上面的targetSpeed也是同理的，初始值都是0，想要机器人转向，就改变这个目标值，然后再将计算出来的turnTorque放入torque1和torque2，使轮子产生差速，机器人就可以实现灵活的前进与转向了。最后在将目标值放入双路FOC的电流环进行力矩控制，机器人很容易就可以实现平衡。

如果我上面说的你理解不了的话，那去看看网上平衡车的教程吧，网上都说的很清楚了。

2，已知轮足底部两个无刷电机的坐标，求解控制腿高的舵机所需角度的运动学逆解算

这部分代码体现的就是轮足机器人与平衡车最大的不同，也就是腿高可连续变化。由于我这个只是个桌面级轮腿机器人，所以控制腿高的四个关节用的都是舵机。关于运动学逆解算公式怎么推导的，可以看看B站灯哥开源的轮足机器人讲解，里面讲的很清楚了。这里直接给出运动学逆解算的代码：

void inverseKinematics(){
  float alpha1,alpha2,beta1,beta2;
  uint16_t servoLeftFront,servoLeftRear,servoRightFront,servoRightRear;

  float aLeft = 2 * IKParam.XLeft * L1;
  float bLeft = 2 * IKParam.YLeft * L1;
  float cLeft = IKParam.XLeft * IKParam.XLeft + IKParam.YLeft * IKParam.YLeft + L1 * L1 - L2 * L2;
  float dLeft = 2 * L4 * (IKParam.XLeft - L5);
  float eLeft = 2 * L4 * IKParam.YLeft;
  float fLeft = ((IKParam.XLeft - L5) * (IKParam.XLeft - L5) + L4 * L4 + IKParam.YLeft * IKParam.YLeft - L3 * L3);

  alpha1 = 2 * atan((bLeft + sqrt((aLeft * aLeft) + (bLeft * bLeft) - (cLeft * cLeft))) / (aLeft + cLeft));
  alpha2 = 2 * atan((bLeft - sqrt((aLeft * aLeft) + (bLeft * bLeft) - (cLeft * cLeft))) / (aLeft + cLeft));
  beta1 = 2 * atan((eLeft + sqrt((dLeft * dLeft) + eLeft * eLeft - (fLeft * fLeft))) / (dLeft + fLeft));
  beta2 = 2 * atan((eLeft - sqrt((dLeft * dLeft) + eLeft * eLeft - (fLeft * fLeft))) / (dLeft + fLeft));

  alpha1 = (alpha1 >= 0)?alpha1:(alpha1 + 2 * PI);
  alpha2 = (alpha2 >= 0)?alpha2:(alpha2 + 2 * PI);

  if(alpha1 >= PI/4) IKParam.alphaLeft = alpha1;
  else IKParam.alphaLeft = alpha2;
  if(beta1 >= 0 && beta1 <= PI/4) IKParam.betaLeft = beta1;
  else IKParam.betaLeft = beta2;
  
  float aRight = 2 * IKParam.XRight * L1;
  float bRight = 2 * IKParam.YRight * L1;
  float cRight = IKParam.XRight * IKParam.XRight + IKParam.YRight * IKParam.YRight + L1 * L1 - L2 * L2;
  float dRight = 2 * L4 * (IKParam.XRight - L5);
  float eRight = 2 * L4 * IKParam.YRight;
  float fRight = ((IKParam.XRight - L5) * (IKParam.XRight - L5) + L4 * L4 + IKParam.YRight * IKParam.YRight - L3 * L3);

  IKParam.alphaRight = 2 * atan((bRight + sqrt((aRight * aRight) + (bRight * bRight) - (cRight * cRight))) / (aRight + cRight));
  IKParam.betaRight = 2 * atan((eRight - sqrt((dRight * dRight) + eRight * eRight - (fRight * fRight))) / (dRight + fRight));

  alpha1 = 2 * atan((bRight + sqrt((aRight * aRight) + (bRight * bRight) - (cRight * cRight))) / (aRight + cRight));
  alpha2 = 2 * atan((bRight - sqrt((aRight * aRight) + (bRight * bRight) - (cRight * cRight))) / (aRight + cRight));
  beta1 = 2 * atan((eRight + sqrt((dRight * dRight) + eRight * eRight - (fRight * fRight))) / (dRight + fRight));
  beta2 = 2 * atan((eRight - sqrt((dRight * dRight) + eRight * eRight - (fRight * fRight))) / (dRight + fRight));

  alpha1 = (alpha1 >= 0)?alpha1:(alpha1 + 2 * PI);
  alpha2 = (alpha2 >= 0)?alpha2:(alpha2 + 2 * PI);

  if(alpha1 >= PI/4) IKParam.alphaRight = alpha1;
  else IKParam.alphaRight = alpha2;
  if(beta1 >= 0 && beta1 <= PI/4) IKParam.betaRight = beta1;
  else IKParam.betaRight = beta2;

  alphaLeftToAngle = (int)((IKParam.alphaLeft / 6.28) * 360);//弧度转角度
  betaLeftToAngle = (int)((IKParam.betaLeft / 6.28) * 360);

  alphaRightToAngle = (int)((IKParam.alphaRight / 6.28) * 360);
  betaRightToAngle = (int)((IKParam.betaRight / 6.28) * 360);

  servoLeftFront = 90 + betaLeftToAngle;
  servoLeftRear = 90 + alphaLeftToAngle;
  servoRightFront = 270 - betaRightToAngle;
  servoRightRear = 270 - alphaRightToAngle;

  setServoAngle(servoLeftFront,servoLeftRear,servoRightFront,servoRightRear);
}

通过与遥控器通信，就可以改变腿高Y的值，经过运动学逆解，就可以平滑地改变舵机的角度。实现机器人丝滑变腿高，同时还能保持平衡。

3，如何实现机器人左右摆动（不是左右行驶），也就是机器人滚转角的改变？

刚刚我们知道，通过输入想要的腿部高度，经过运动学逆解算后，舵机会自动转到合适的角度。我们只需要在这基础上，通过控制两边的期望腿高，就可以实现机器人左右摆动。

  getRCValue();  //获取遥控器的摇杆值
  Y_demand = ((int)map(RCValue[2], RCCHANNEL3_MIN, RCCHANNEL3_MAX, lowest, highest));//腿高期望
  Phi = map(RCValue[3], RCCHANNEL_MIN, RCCHANNEL_MAX, -1*rollLimit, rollLimit);//滚转角期望

  X = 0;
  Y = Y + Kp_Y * (Y_demand - Y);

  uint16_t Remoter_Input = Y;
  float E_H = (L/2) * sin(Phi*(PI/180));
  float L_Height = Remoter_Input + E_H;
  float R_Height = Remoter_Input - E_H;

  IKParam.XLeft = X;
  IKParam.XRight = X;
  IKParam.YLeft = L_Height;
  IKParam.YRight = R_Height;
  // Serial.printf("%f,%f,%f,%f,%f\n",X,Phi,E_H,L_Height,R_Height);

  inverseKinematics();

 Y_demand值的是摇杆的竖向值，竖向控制腿的高度；Phi指的是横向的腿高差，通过横向摇杆的偏移值就可转换为轮足机器人左右两边的腿高差。最后只要将更改后的左右两边的腿高分别放入运动学逆解算函数中，即可实现腿高上下左右灵活改变了。

4，复杂地形的自适应

这个可以说是轮足机器人与平衡车性能之间最大的差异了，面对崎岖不平的路面，平衡车由于不能灵活的调整腿高，就很有可能失衡。通过上面的分析之后，相信你一定也有思路了，只要在路过崎岖路面时，自动的调节左右两边的腿高，机器人不就能平稳的通过了吗？

那么怎么做到让机器人做到自动调整左右两边的腿高？首先就要想到能读取到MPU6050的滚转角，只有滚转角改变了，机器人才知道这个时候地面不平了。那么就要通过滚转角的改变反过来控制左右的腿高差，以实现自动调整腿高。代码如下：

  getMPUValue();
  
  Y_demand = ((int)map(RCValue[2], RCCHANNEL3_MIN, RCCHANNEL3_MAX, lowest, highest));//腿高期望

  X = 0;
  Y = Y + Kp_Y * (Y_demand - Y); // 横滚PID

  uint16_t Remoter_Input = Y;
  stab_roll = stab_roll + Kp_roll * (0 - robotPose.roll); // 复杂地形适应控制PID
  float L_Height = Remoter_Input + stab_roll;
  float R_Height = Remoter_Input - stab_roll;

  IKParam.XLeft = X;
  IKParam.XRight = X;
  IKParam.YLeft = L_Height;
  IKParam.YRight = R_Height;

  inverseKinematics();

需要注意的就是 stab_roll = stab_roll + Kp_roll * (0 - robotPose.roll); // 复杂地形适应控制PID，这一行代码。有人可能会觉得只要把MPU6050得到的滚转角乘个负数，然后将这个值传给左右腿高控制函数不就行了吗？关键是这样子的话，当滚转角又变回0的时候，传给左右两个腿高函数的左右两个腿高值不就一样了吗？这样轮足机器人就又变回去了。所以要使用这种类似累加的式子去自动抵消因地形不平而导致的左右腿高差值。到此为止，轮足机器人自适应地形代码就结束了。

5，轮足机器人的刹车以及加速功能

到此，你会发现运动学逆解中的足部X坐标一直都没有使用，其实把这个X坐标利用起来，可以使轮足机器人有更好的运动性能。在轮足机器人需要刹车时，可以给X坐标一个正的值，使轮腿向前移，再配合之前嵌入在角度换的速度环，可以使轮足机器人实现更快的刹车，启动的时候也是同理，轮足机器人起步也会更快。

想实现这个功能其实也很简单，只需要在X输入运动学逆解算函数之前经过PID运算后就可以实现与速度环共同控制，代码如下：

X = -Kp_x * (targetSpeed - speedAvg); // 根据速度调整X坐标