“ 本节以一个二连杆系统为例，使用simscape建立一个简单的机器人系统，并使用Robotics Toolbox进行正运动学建模。”

本节以一个二连杆系统为例，使用simscape建立一个简单的机器人系统，并使用Robotics Toolbox进行正运动学建模。模型如下：

01

—

二连杆模型建立

首先新建一个模型。

打开simulink，通过Simscape -> Utiliters -> Solver Configuration,添加一个求解器，用于配置ODE求解器。

在Simscape->MultiBody->Frames and Transforms->World Frames中添加一个参考世界坐标系，用来定义所有机械系统的坐标系。

随后添加一个Mechanism Configuration，用于定义重力和作用在当前系统上的其他外力。

以上三个模块是所有MultiBody系统的基础模块。

接下来我们来创建一个简单的机器人系统，首先是base，我们使用实心圆柱来表示机器人底座Base，双击可以打开设置，设置必要参数（圆柱体半径和长度等）后保存关闭。

创建机器人底座后需要定义底座的坐标系，如果直接将底座与世界坐标系连接，则世界坐标系将会在圆柱体中心，但此处想要将世界坐标系建立在圆柱体的base处，因此需要坐标转换。添加一个Rigid Transform来实现坐标系转换并移动z方向。

接下来添加连杆1和关节1，连杆1和关节1应该设置在base上方。先来添加关节1，在添加关节之前，需要先定义其坐标系，同样使用Rigid Transform，此处关节1建立在base正上方0.01m处，相对于base中心则是[0,0,0.06]；关节1添加之后则为连杆1，同样先定义坐标系，连杆1中心相对于关节1中心关系为[0.1,0,0]，随后添加一个矩形并定义尺寸。此时模型如下图所示。

此时若运行当前模块，可以看到当前建模预览：

同样也可以看到建立的各个坐标系

接下来我们按照上述同样的方法添加关节2和连杆2。

接下来是法兰，若想要添加法兰，需要先添加一个固定关节，此处添加的是Weld Joint，最后再添加末端执行器并定义其尺寸和坐标，便得到最终的模型了。

运行模型，便可以看到最终建立的模型：

将Base、连杆部分组成子系统，使模型看起来更加简洁。

02

—

模型导入

模型建立后，可以通过如下代码导入上述建立的模型。

函数返回的DOF2_Arm为RigidBodyTree，这一返回值可以被Robotic Toolbox直接使用。

03

—

模型输入输出

到目前为止，我们已经建立了一个二关节系统的机械模型，但是如何给关节增加输入呢？双击模型中的关节可以修改设置。

首先添加关节限制，包含关节限位、刚度、阻尼、背隙等；其次在Actuation选项，可以选择Torque或Motion，此处我们选择Motion为外部输入，而力矩则是自动计算；Sensing选项为关节输出，此处我们选择位置为反馈输出。 

修改完成后，可以看到在关节处分别多了位置输入和输出的选项

现在需要给关节提供输入，在simscape模型中，其模型连接与基础simulink模型中的连接不同，所以需要将simscape模型中的信号转换为simulink信号，此处需要用到PS-Simulink/Simulink-PS converter。新增一个转换模块后，双击设置。

将上述系统创建为一个子系统，并命名为Robot，然后我们来给机器人定义一个简单的输入。我们使用正弦曲线来简单模拟机器人各个关节的位置。此外，我们可以借助机器人工具箱根据建立的simscape模型来建立机器人的正运动学。使用机器人工具箱中的Get Transform模块来实现这一目标。

最后再添加模块将齐次变换矩阵转换为位移向量，并添加scope等对数据进行检测。最终模型如图所示。

运行模型，得到如下轨迹，至此，我们的仿真就结束啦。

最终仿真视频：

相关源码和模型可通过如下方式获取，欢迎技术交流。