目录

写在前面

运动学逆解原理

运动学逆解PA_IK代码实现

VMC算法原理

VMC代码实现

引用文献：

---

写在前面

本篇文章介绍的是，运动控制线程padog.py的逻辑顺序这篇文章中如红框所示的部分

运动学逆解原理

推荐预习视频：运动学逆解，观看2:00~9:13即可。

运动学正解，运动学逆解这些名词想必大家都不会太陌生，

运动学正解：已经知道关节角度计算出足端坐标

运动学逆解：已经知道足端坐标计算出关节角度

看完视频想必大家对这个的推导已经非常清晰，如果去网上搜这部分的知识，会有很多教程，你可能会觉得这和我们看的不太一样，但我想说的是，由于我们使用的机器人只是一个 8 自由度的机器人，因此还不需要用到网上 12 自由度的推导方法(旋转矩阵)。

对于推到过程，我只是觉得有几点需要提醒的，计算时，需要注意象限问题。

将单腿模型简化成坐标形式，则任何时刻都存在下式

即可得，当足端坐标处于右半平面（即小于90°）时，由三角形的关系可得；当足端坐标处于左半平面（即大于90°）时，由三角形的关系可得。

运动学逆解PA_IK代码实现

明白了原理之后，我们就可以明确输入的物理量需要什么？首先是 4 条腿的足端坐标，和大小腿的长度，这份程序是可以兼容串联腿和并联腿的，因此有个选择case的变量。

按照我们的物理公式转换成代码，先计算关节，接着计算关节，计算时，需要注意我上面说的，判断象限的问题。算出一条腿后，以此类推算出其余四条即可。

#函数功能：运动学逆解运算库，运算完成后返回每个舵机的转动角度
#变量作用：
# case：0代表串联腿，1代表并连腿。
# l1,l2：机器人大小腿长度，用于运动学计算，
# x1-x4：逆解的足端x坐标
# y1-y4：逆解的足端y坐标
#返回值：8个舵机所需要到达的角度
def ik(case,l1,l2,x1,x2,x3,x4,y1,y2,y3,y4):
    if case==0:
      #腿2 
      x2=-x2
      shank2=pi-acos((x2*x2+y2*y2-l1*l1-l2*l2)/(-2*l1*l2))

      fai2=acos((l1*l1+x2*x2+y2*y2-l2*l2)/(2*l1*sqrt(x2*x2+y2*y2)))
      if x2>0:
          ham2=abs(atan(y2/x2))-fai2
      elif x2<0:
          ham2=pi-abs(atan(y2/x2))-fai2
      else:
          ham2=pi-1.5707-fai2
      shank2=180*shank2/pi
      ham2=180*ham2/pi
    
    #并联模式和其余腿省略

VMC算法原理

VMC代码实现

（鸽了太久，实在是对不住灯哥了）

在主循环文件中，可以通过对模式的选择进入到VMC代码模块，并将由步态计算出的四个腿的x，y坐标传入到此代码模块中，即P_[0]+ges_x_1等。

 VMC代码模块中主要可以分为三部分，即变量的声明，传参进入VMC计算器计算和对VMC计算器计算出的数值进行处理，这里重点要介绍的是传参进入VMC计算器计算这一个部分，重点需要搞明白在这一个部分中，VMC计算器如何将期望足端坐标转化为舵机的输入角度。

 VMC计算器计算是对单腿起作用，即输入单腿的坐标得到该腿的角度，故我们对其中一个腿进行讲解即可。VMC计算器的输入量主要有：

def cal_vmc_single_leg(l1,l2,ham1_origin,shank1_origin,Xe,Ze,X,Z,Xl,Zl):

分别代表的意义为，大小腿的长度，长小腿的关节角度，Xe,Ze是期望角度，X,Z是当前角度，Xl,Zl是上次角度。值得注意的是，这与逆运动学相比多了上次角度，这个参数的引入是为了达到计算PID中的D项而做的铺垫。对VMC计算器模块进行分解，其又可以分为6大部分，分别为

1.计算x，y上的力，此项在上文VMC原理中有详细阐述。

2.对变量进行弧度的转换，储存当前位置值为下次循环做准备。

3.利用雅可比矩阵求力矩，此项在上文VMC原理中有详细阐述。

4.建立伪反馈，通过力矩求出角度，此项在上文VMC原理中有详细阐述。此步中的参数整定，由于本人才疏学浅没整明白，懂得大神可以在评论区讲一下，我对于此参数的整定倾向于是通过实际调试所获得的。

5.由正解求足端坐标，属于常规步骤。

6.输出。

 至此，VMC计算器模块已经完成任务，返回大小腿关节角度，需要输出的足端坐标和上次的足端坐标。这些值返回到VMC代码模块中，代码模块对所有值进行累加或者更新，得到最新状态下的数据值，并为下次循环做准备！

 最后，主循环对VMC计算的值进行输入，就完成了一次完美的VMC循环。

引用文献

李田法. 电机驱动四足机器人能耗优化与高速运动步态规划研究 [D]; 山东大学, 2020.

谢惠祥. 四足机器人对角小跑步态虚拟模型直觉控制方法研究 [D]; 国防科学技术大学, 2015.