四足机器人——运动学逆解

目录

单腿运动学逆解

小腿和大腿的坐标转角

肩部的坐标转角 

---

单腿运动学逆解

运动学正解：知道舵机转角，求足端坐标

运动学逆解：知道足端坐标，求舵机转角

小腿和大腿的坐标转角

 此时是单腿二自由度时的角度

将此公式转化为代码

  # 求小腿转角beta
  s=math.sqrt(l*l+x*x) 
  n=(s*s-hl*hl-hu*hu)/(2*hu)
  beta=180/math.pi*(-math.acos(n/hl))

 # 求大腿转角alpha
  alpha1=-math.atan(x/l)
  alpha2=math.acos((hu+n)/s)
  alpha=180/math.pi*(alpha2+alpha1)

肩部的坐标转角 

将此公式转化为代码

  # 求肩部转角gama
  d=math.sqrt(y*y+z*z)
  l=math.sqrt(d*d-h*h)
  gama2=-math.atan(y/z)
  gama1=-math.atan(-h/l)
  gama=180/math.pi*(gama2-gama1)  #肩部转角