1 引言

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

2 基本概念

2.1机械结构模型

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

2.2运动学正解

  已知舵机转动角度求足端坐标，称为运动学正解。即已知θ1\theta_1θ1​和θ2\theta_2θ2​，求Z点的坐标(x,y)。

2.3运动学逆解

  已知足端坐标求舵机转动角度，称为运动学逆解。即已知Z点坐标(x,y)，求解θ1\theta_1θ1​和θ2\theta_2θ2​。

3数学建模

3.1规划足端轨迹

  机器人运动时，首先规划足端Z的轨迹。在该四足机器人中，足端采用摆线方程。
{x=S[tTm−12πsin(2πtTm)]y=H[12−12cos(2πtTm)] \begin{cases} 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{cases} {x=S[Tm​t​−2π1​sin(Tm​2πt​)]y=H[21​−21​cos(Tm​2πt​)]​  其轨迹为下图中的黄色虚线。

  通过逆解得到θ1\theta_1θ1​和θ2\theta_2θ2​，再命令舵机根据θ1\theta_1θ1​和θ2\theta_2θ2​转动，使机械腿转到指定的位置，完成指定的动作。

3.2运动学逆解数学建模

3.2.1简化结构

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

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

3.2.2运动学逆解数学建模

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

  由OCZ构成的直角三角形可以求出虚拟腿长L和腿角ψ\psiψ：
  虚拟腿长度：L=x2+y2L=\sqrt{x^{2}+y^{2}}L=x2+y2​
  腿角：ψ=arcsin(x/L)\psi=arcsin(x/L)ψ=arcsin(x/L)
  由三角形OBZ根据余弦定理可求得连杆分离角ϕ\phiϕ：
ϕ=arccos(L2+L12−L222L1L) \phi=arccos(\frac{L^{2}+L^{2}_{1}-L^{2}_{2}}{2L_{1}L}) ϕ=arccos(2L1​LL2+L12​−L22​​)  利用腿角ψ\psiψ和分离角ϕ\phiϕ求出θ1\theta_1θ1​和θ2\theta_2θ2​：
{θ1=ϕ−ψθ2=ϕ+ψ \begin{cases} \theta_{1}=\phi-\psi\\ \theta_{2}=\phi+\psi \end{cases} {θ1​=ϕ−ψθ2​=ϕ+ψ​

3.2.3程序实现

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

4 几点疑惑和建议

（1）简化模型的误差

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

（2）足端轨迹误差

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

（3）姿态平衡的控制

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

参考资料

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