机械臂控制仿真

引言

机械臂是一种基于机电一体化技术的自动化设备，通过多关节结构和智能控制系统模拟人类手臂的运动功能，广泛应用于工业中的生产环节、物流运输服务及特种作业等领域。其核心价值在于替代人工完成高危、高精度或重复性任务，同时提升生产效率和操作安全性。

在工业制造场景中，机械臂是智能制造的核心装备。汽车生产线上的焊接机械臂能够长时间连续运行，完成高精度的车身焊接任务，显著提升了生产效率；电子装配线上的多轴机械臂可以快速完成精密元件的安装，具备极高的重复定位精度。这些设备通过可编程控制器和视觉识别系统实现流水线生产，能够灵活适应多品种、小批量的现代制造需求。

同样的，机械臂的发展离不开仿真技术的支持。Sysblock 仿真可以让工程师可以在设计阶段预测机械臂的性能，并测试算法的有效性，缩短开发周期，最终将模块生成为代码，部署至实际的机械臂中进行直接测试，有效提高效率。因而 Sysblock 作为一种基于模型的设计工具，将使用于本项目案例的仿真阶段，并在未来机械臂的研发和优化中发挥重要作用。

模型数据准备

单击右侧打开示例按钮，在 Sysplorer 中打开文件。

提示

正常情况下，单击打开示例按钮，软件会自动加载模型所需的全部模型库，涵盖 Modelica 模型库、Sysblock 模型库以及 TYMultibody 模型库。若这些模型库未正常开启，需手动添加，具体操作步骤为：先单击库浏览器，接着单击 ，最后单击缺失的模型库名称，就能手动加载该缺失模型库。

导入模型所需数据

在该模型中，假如直接运行案例模型，会看到以下错误提示：

ID2_Bias 以及 ID3_Bias 是这个模型两个重要的参数，使用时需要将其具体数据使用 json 格式提前定义于一个数据文件中，而不是直接存储在模型中，因此我们需要向模型导入这些数据。

在开始模型实现以及仿真之前，模型需要定义多个将被使用的参数值，假如使用时逐一更新这些参数值，不仅效率低下且很可能出现错误。因而仿真前可以通过一定的格式预先储存好需要定义的模型参数的数据，在需要使用时，直接通过数据管理器导入至模型中。

导入基础工作区变量

例如，我们将在本项目中使用一组控制机械臂动画角度偏移的数据，因为在本案例中，仿真依赖舵机标定参数数据，因而基于模型数据需求，可以在建模选项卡中，选择数据管理器->基础工作区，右击基础工作区，选择导入文件，可将数据以文件方式导入工作区：

接下来，在Default5文件夹中找到基础工作区_机械臂案例数据文件. json（该文件的具体路径为：%Sysplorer安装目录%\Docs\Sysblock\Samples\Sysblock\ApplicationCase\Demo_RobotArm\Default5），该文件被写入了各个舵机执行动作的一些数据参数，在导入文件的打开的路径中，选中该文件并打开，便可将该文件导入至基础工作区：

导入模型工作区变量

另外的，在 NewRobot 文件夹下找到 Control 文件夹，在 Control 中打开 robot_Ctrl，重新打开 数据管理器，在模型层次下，选择 robot_Ctrl 打开其包含的模型工作区，右击模型工作区导入模型工作区_机械臂案例控制数据文件. json（该文件的具体路径为：%Sysplorer安装目录%\Docs\Sysblock\Samples\Sysblock\ApplicationCase\Demo_RobotArm\Default5），这个文件包含舵机的控制编号等数据，过程如下：

注意

该案例的模型工作区已保存有一组与数据文件相同的舵机的控制编号等数据，重新导入会对该数据进行覆盖。 因而除非有修改模型工作区参数的需要，可以不需要向模型工作区导入数据文件。

模型说明

创建子系统

在具体地实现该项目之前，为了便于在功能上实现该机械臂模型，我们便需要基于这个项目中的需求，在 Sysblock 中创建一些模型组件或者子系统来独立的承担这个项目的一些功能。

1. 抓取动作编组模块

   对于模型而言，最重要的输入数据是每个动作机械臂运行时间以及每个动作的目标角度，因而我们下图中的向量即为需要进入机械臂的数据，向量内第一个数据是以毫秒为单位的时间数据，第 2~7 个数据是 6 个舵机依次需要在本次动作中所规定的最终到达的角度（范围从 0~1000 折算为 0~240°，300 即为 72°，500 为 120°）。即为下图是我们创建的抓取动作子系统：

   

   首先，模型设定一个输出为仿真时间的时钟信号，如下图蓝色曲线所示；对该信号进行模 10 运算操作，相当于将该时钟信号转化为一个周期为 10，且上升至 10 时 y 轴即降回 0 的三角波，如下图红色曲线所示；对该三角波做向上取整，可转变为下限为 1，上限为 10，y 轴每秒增加 1 的阶梯信号，如下图绿色曲线所示并使用 saturation 将输出的阶梯信号限制在 1~12 之间，作用在于避免信号超出预设范围，使后续系统正常运行。而产生这个阶梯信号的目的就在于使接下来 switch 模块的 10 个分支中，每个向量分支都能够轮流得到执行。

   

   接下来将输出信号进入包含 10 个分支的 switch 分支选择结构，由于输入为 1~10 的阶梯信号，这意味着该 switch 分支结构内，每个分支都能得到执行。在该 switch 结构中，每个分支内存在一个包含 7 个数据的向量，向量在 deMux 部分中将被拆解为 7 部分，便于对多个分支的某个数据进行统一修改。例如我们需要修改分支执行时间，可以通过在 deMux 中将进入第一个分支的时间数据乘上一个大小为（目标时间 / 原有时间）的增益。 最终被拆分的向量将进入在同一个总线中，到达输出端口进行输出。

2. 动态调整舵机模块

   下图是用于调整舵机的模块，其中 time_out 是接收来自抓取动作编组的 timeout 参数，作为控制舵机从当前角度到目标角度的运动时长，而 zero_ID1_Angle_input 作为输入是设定舵机的初始角度。uniDelay 模拟实际系统的响应延迟，以在状态机中前后两个采样时刻存在角度差值，作为下一个时刻舵机移动的角度。最后 zero_ID1_Angle_output 将状态机输出的角度输入至舵机中。

   

   该子系统的功能是根据输入的目标舵机角度和转动时间（timeout）进行动态调整。子系统将按照输入角度与目标角度推断出步进角度，并使输入角度与步进角度相加输出每个仿真时刻的舵机角度，从而满足通过转动舵机移动机械臂的需求。具体分析该子系统，该子系统状态机分为 5 个状态：

   (1) 初始状态 state 将目前的角度直接赋值给 end_data 作为目标角度，将目前的角度经过延时模块赋值给 start_data 作为起始角度，因而假如舵机存在运动则目标角度与起始角度始终存在差异。此外我们将状态机输出结果初始化为经过延时模块的目前的角度。完成后进行判断，假如目标角度 > 起始角度则进入 state1，假如目标角度 < 起始角度则进入 state3。

   (2) State1 中我们将（目标角度 - 起始角度）的值作为舵机在接下来的转动时间所要转动的角度，将该差值除以转动时间，我们可以得到每个仿真时刻下舵机应该转动的角度 step_data. 此时我们规定该状态下输出结果为起始角度（和初始状态输出相同），执行结束后直接进入 state2。

   (3) State2 中我们将目前的输出结果加上转动角度使之成为下一仿真时刻的输出结果，观察结果是否满足转移线上规定的‘输出结果大于目标角度’，满足则回到 state 状态，不满足则在下一仿真时刻继续执行 State2。

   (4) State3 与 State1 相似，只不过前提是目标角度 < 起始角度，因此，step_data 步进角度 =（起始角度 - 目标角度）/ 转动时间，同样的，我们规定该状态下输出结果为起始角度，并直接进入 state4。

   (5) State4 中我们将目前的输出结果减去转动角度使之成为下一仿真时刻的输出结果，观察结果是否满足转移线上规定的“输出结果小于目标角度”，满足则回到 state 状态，不满足则在下一仿真时刻继续执行 State4。

   

   最终的输出如下图，蓝色线为输入状态机的目标角度，红色线为实时输出至舵机的本时刻所要运行到的角度。我们可以发现输出曲线变化如上文所述，是先接收到一个最终的角度，然后再输出再按照抓取动作编组模块中给定的的转动时间（案例的转动时间为 800ms）匀速逐步转动至预设角度。由状态机的输出同样可以发现每个匀速动作时长也如设定的转动时间一样为 800ms（受到单位延迟模块影响，以下图为例子，理论上舵机启动时间为 9s，事实上要延迟一个单位时间，等待输入状态机的目标角度改变后，舵机才开始运动，例子中舵机开始移动的时间为 9.002s，舵机动作结束时间同样存在延迟，实际应为 9.802s）。

   理论上：

   

   实际上：

   

3. 转换舵机移动角度模块

   

   将每个时刻舵机需要移动范围输入至该模块，将其转化为舵机实际转动的弧度。各个计算出的逐时刻的角度进入不同的舵机，以 servo_LX_225_3 为例，此时角度的范围仍然是 0-1000（0~240 度），对进入的角度减去 500，等于将零点视作脉宽中心位置，便于表示出舵机的偏转角度，小于 0 时为反向转动，大于 0 时为正向转动，0 为 120 度。将脉宽乘以 240/1000 换算成角度值（-120°~120°），再乘以 pi/180 可将角度值换算成对应的弧度值，将该弧度值减去舵机动画角度偏移标定值输出值为连接至该舵机的各部件的运动弧度。

模型连接

使用信号线连接各组件

在完成组件创建之后，我们需要将这些子系统以及组件的接口进行相连，以从功能上组织模型中的模块。

我们将从我们创建的抓取动作编组开始，如下图所示：

为减少信号线的数量，简化结构，我们在抓取动作编组中使用总线（BusCreator）输出了一组包含每个动作运行时间以及完成这个动作所需要数个舵机移动的角度的向量。因而为了进行接下来的实际执行每个舵机的移动，我们需要使用 BusSelector 模块将信号从总线中分组出来，并将这组包括目标舵机角度输入与 timeout（转动时间）输入到一个包含状态机的子系统中。

总线使用说明的链接: BusCreator 。

仿真验证

构建完模型的框图并导入模型所需要的数据之后，通过工具栏的仿真选项进行仿真，使用窗口观察仿真机械臂末端各个维度相对于原点的距离。若需更改仿真运行时长，可在仿真选项处的停止栏输入目标运行时间进行修改，如下图左上角红框所示：

此外，我们同样可以将模型中任何一处输入端口或者输出端口添加至波形窗口中，通过观察波形或者具体某个时刻的值以验证模型是否存在错误。例如，我们规定了机械臂每个动作时长为 800ms，我们可以观察每个动作变化的时长来确认是否符合我们的预期目标，如上图左下角的两个游标读数的差值，可以发现一个动作时长确实为 800ms，没有出现错误。另外的在 20s 时，恰好位于机械臂第二个周期的结束，机械臂应为直立状态并在 z 轴上达到最高，此时由图可见 y[2] 读数同样也位于周期中最高的时刻，同样可以验证仿真的正确性。

仿真 3D 视图

打开 3D 视图方式

如下图，在第一次仿真绘出机器臂各个终端的曲线之后，假如需要观看 3D 仿真结果，可以通过单击仿真选项重新运作仿真程序，再打开动画窗口，来播放机械臂的 3D 动画。

播放时可以通过滚动滑轮以改变视角大小，也可以通过按下滑轮左右移动鼠标以改变观察机械臂的视角。

播放结束后，重新观看机械臂的 3D 视图可以通过单击播放以重播而不必再次仿真，假如需要观看机械臂特定时间点的动作，可以选择特定时间之后再进行播放，或者播放到对应的时间点时单击暂停：

3D 仿真视图展示

从仿真输出的 3D 图形，我们观察到随时间的变化，机械臂运动角度以及机械臂末端位置会按照预设的程序移动。这些 3D 图形可以让我们分析机械臂的运动是否符合我们的需求或者是否稳定，我们也同样可以使用这张 3D 视图来验证我们的曲线图是否正确。

以上这些图形化输出为机械臂的设计、控制和提供了全面的可视化支持，显著提高了案例开发效率和系统可靠性。