SimMechanics机械臂建模与仿真：利用Matlab的SimMechanics进行机械臂的模型搭建及动力学仿真（二）

二、正逆运动学模块的搭建

提示：关于逆运动学的求解方法，本章节不做重点介绍，可以参考一下文章学习求逆解的方法，本章主要讲解如何利用SimMechanics搭建正逆运动学模型，并仿真、验证！

https://www.cnblogs.com/21207-iHome/p/9452896.html

1.正、逆运动学的表达

2.Simulink中，相关的数学计算模块。
复杂的表达式是通过简单的四则运算（加减乘除）表达出来的，同样，在Matlab的Simulink中，定义好输出输出端口，通过若干个加减乘除模块组合，来表达出复杂的表达式。 我们主要用到的模块有：

模块的作用，就如同他的名字所表达的意思，不多做介绍。（可以自定义模块的输入端口数量，比如Add，可以做到同时多个输入相加！），在Trigonometric Function中，可以自定义你所需要的三角函数，包括反三角函数。
3.封装子系统
正逆运动学本身就可作为一个子系统存在，但在搭建过程中，例如cos(θ1+θ2)，往往需要展开计算，我们可以将这样子函数再次封装为一个子系统，降低整个系统的复杂度，提高整洁度。

搭建的cos(θ1+θ2)和sin(θ1+θ2)如图所示：

4.搭建正运动学系统，根据之前求出的表达式，一步一步分解搭建即可：

---

注意：根据DH参数表，下图中的L4应为8.55。

---

---

注意：根据DH参数表，下图中的L4应为8.55。

---

5.系统的仿真并验证
即输入一组[x,y,z]坐标，通过逆运动学求解得到[theta1,theta2,d3]，再将逆运动学得到的结果作为正运动学的输入量，查看正运动学的输出量是否等于一开始自定义的坐标。

输入量，我们可以用constant模块，可自定义一个常量值作为输出，输出的表示我们用Display模块，可以将输入量直接显示出来。

如图所示，结果一致。（由于反三角函数的输出值为弧度制，我们在采用逆运动学的输出结果时，忽略了部分小数，导致结果有很小的误差，但是基本一致，说明系统搭建没有问题。）