仿斯坦福四足机器人的运动学逆解

1 引言

  仿斯坦福四足机器人的软件流程如下图所示。其中运动学逆解直接输出给舵机，控制机器人的运动，因此运动学逆解很重要。

2 基本概念

2.1机械结构模型

  对于8自由度机器人，其机械结构模型如下图所示。图中两个并联大腿$L_1$杆分别由独立的舵机带动，假设这两个独立舵机位于同一点O(0,0)，两个并联小腿$L_2$杆相交于足端Z(x,y)。两个舵机的转角即两个$L_1$与过O点的铅垂线的夹角分别为${\theta }_1$和${\theta }_2$。

2.2运动学正解

  已知舵机转动角度求足端坐标，称为运动学正解。即已知${\theta }_1$和${\theta }_2$，求Z点的坐标(x,y)。

2.3运动学逆解

  已知足端坐标求舵机转动角度，称为运动学逆解。即已知Z点坐标(x,y)，求解${\theta }_1$和${\theta }_2$。

3数学建模

3.1规划足端轨迹

  机器人运动时，首先规划足端Z的轨迹。在该四足机器人中，足端采用摆线方程。
$$\{\begin{array}{l}x=S[\frac{t}{T_m}-\frac{1}{2\pi }sin(\frac{2\pi t}{T_m})]\\ y=H[\frac{1}{2}-\frac{1}{2}cos(\frac{2\pi t}{T_m})]\end{array}$$  其轨迹为下图中的黄色虚线。

  通过逆解得到${\theta }_1$和${\theta }_2$，再命令舵机根据${\theta }_1$和${\theta }_2$转动，使机械腿转到指定的位置，完成指定的动作。

3.2运动学逆解数学建模

3.2.1简化结构

  仿斯坦福机器狗的并联腿，机械是一个闭链的5连杆：2个大腿$L_1$、2个小腿$L_2$以及两舵机连线。

  为了简化计算，将两舵机简化为重合得一个点。

3.2.2运动学逆解数学建模

  建立数学模型，实际上是建立(x,y)和(${\theta }_1$, ${\theta }_2$)的关系，即已知(x,y)，通过建模求出(${\theta }_1$, ${\theta }_2$)。再将${\theta }_1$和${\theta }_2$传递给舵机，使机器人实现：随着规划的足端坐标(x,y)运动。
  机械结构简化为下图。

  由OCZ构成的直角三角形可以求出虚拟腿长L和腿角$\psi$：
  虚拟腿长度：$L=\sqrt{x^2+y^2}$
  腿角：$\psi =arcsin(x/L)$
  由三角形OBZ根据余弦定理可求得连杆分离角$\varphi$：
$$\varphi =arccos(\frac{L^2+L_1^2-L_2^2}{2L_{1}L})$$  利用腿角$\psi$和分离角$\varphi$求出${\theta }_1$和${\theta }_2$：
$$\{\begin{array}{l}{\theta }_1=\varphi -\psi \\ {\theta }_2=\varphi +\psi \end{array}$$

3.2.3程序实现

利用Python编写程序及运行结果如下。

4 几点疑惑和建议

（1）简化模型的误差

  模型简化将两个舵机看成一个点来处理，如果机器狗的腿不够长，即两舵机间距不能忽略的情况下仍做此简化，会带来较大误差，从而影响机器狗运动的准确性。

（2）足端轨迹误差

  足端轨迹按摆线来处理是基于腿长不变这一前提的。然而随着机器狗运动姿态的变化，并联腿的长度会随之而变化。事实上，腿长会随着舵机转角$\theta$的增大而减小。在改进的建模算法中应考虑这个问题。

（3）姿态平衡的控制

  运动学逆解求解的是机器狗在平面上运动步态的问题，当运动表面不平整，或某种原因导致机器狗姿态改变时如何恢复平衡，是后面需要解决的问题。

参考资料

[1] http://github.com/ToanTech/py-apple-quadruped-robot