SimMechanics/Second Generation倒立摆模型建立及初步仿真学习

笔者最近捣鼓Simulink，发现MATLAB的仿真模块真的十分强大，以前只是在命令窗口敲点代码，直到不小心敲入simulink，就一发不可收拾。话说simulink的模块化建模确实方便，只要拖拽框框然后双击设置属性就可以慢慢堆建自己的模型，这一点我很中意(*^__^*) 。

最近在学习一些控制理论，发现倒立摆是个不错的学习平台，开始学习肯定需要机构上面的运动学及动力学建模，一直习惯Adams进行运动仿真，但Adams安装比较繁琐。。。便想利用MATLAB代替仿真，顺便学习了解下传说的simulink，直接搜查阅资料。发现Simscape里面的simmechanics就是运动仿真模块。于是赶紧打开俺的MATLAB2015b，敲入simulink再用力敲打回车，就出现如下窗口。。。

一.新建一个模型New Model

新建模型将出现空白界面，所有建模要用的block都必须拖到该界面进行设置和连接，因为模块库里Block特别的多，一个一个找真的特别麻烦，强烈建议记下常用block的名字，比如示波器scope，时钟clock，阶跃信号step等等等等，今后可以直接在模型空白处敲入字符，自动检索，真的是方便感人，如图。

二.SimMechanics/First Generation模型

需要注意的是，SimMechanics库分为First Generation和Second Generation，即第一代和第二代。

其实MATLAB帮助文档里面很多demo值得学习理解，我简单了解了第一代模块，跟着说明建立了一个平面四连杆机构模型，模块框图和仿真界面如下：

难点就是body block的属性设置，必须清楚了解坐标系的建立，新建的CS（coordinate system）相对哪个坐标平移，又相对哪个坐标旋转。其中World坐标系是世界坐标系，也就是绝对坐标系，所有的坐标系都是相对它而言的，所以说这个模块必不可少。

总之，模型的建立顺序一般如图，除了机械环境Env，其它的block连接都是坐标系与坐标系之间的连接，但body与body之间不能直接连接，必须通过joints确定两者之间的连接副。最后的body与地面连接就形成闭环机构，也可以只出现一个Ground，即悬空机构，像单摆运动，机械臂等只有基座与地面连接。当然也可以body与body之间形成闭环。Joint/Body Actuator 是连接副和机构驱动器，发送信号可以向Joint或者body添加动力或运动；同理有Joint和Body的Sensor，即传感器，可以利用scope示波器观察Joint和body的受力情况和运动规律。

三.SimMechanics/Second Generation模型

第二代模型比较直观，可以直接建立几何特征，solid代替了一代的body，可以直接从中设置很多属性。参考MATLAB的demo模型，在使用 second generation建立机构模型时，必须添加的模块有

1) solver configuration求解器配置块，是进行求解的必须模块，双击可以配置具体求解方式。

2) world frame 世界坐标系，其它一切坐标系都要直接或间接与其连接。

3）mechanism configuration 机构配置块，双击打开可以修改重力的大小和方向等。

为了建立倒立摆，我打算添加两个构件，一个是小车构件，记为cart，质量设置在重心处，与地面（世界坐标系）为滑块连接（移动副），几何外形简化为为长方体；另一个是倒立杆构件，记为pole，质量在最高处，与小车进行旋转副连接，外形简化为圆柱体。

几何体的建立模块为solid，如下图，参数编辑窗口分为Description和Properties两栏，描述的内容十分有阅读价值，了解其属性才能设置好具体参数，以下是小车机构的属性设置：

小车构件新建了一个Frames，在第二代SIMmechanics里面坐标系用Frames表示，一个零件可以添加多个Frame，并且两个Frame之间可以直接连线进行刚性连接，中间可以不用joint进行连接，如下图，我把倒立杆的外形润色了一下：

现在的模块窗口有以下Block，直接连接的话就是和World Frame作刚性连接，那么整个机构将是静止不动的，所以接下来是Joint block的添加。

接下来是比较重要的连接模块，即连接副的选择，如下是库里面提供的连接副：

单级直线倒立摆的机构只需两个连接副，即revolute joint（平面旋转副）和prismatic joint（柱形滑块副），将其拖入窗口进行属性设置和连接。属性设置窗口如下：

如上图所示，两个副的设置窗口比较复杂，必须仔细阅读Description里面的内容，本模型设计了阻尼系数，并且打开了Sensing接口，这样可以通过采集两个副的位置，速度，加速度等运动参数。最后方框图如下：

连接模块直接进行仿真的话会弹出如下窗口，重力的默认方向是world frame的-z方向，所以模拟会发生异常，图示整个机构会自由落体向下。。。。

所以必须引入参考系模块，如下图。这个才是设置难点，因为每个坐标系的连接和参考都不同，为了找到合适的方向和位移，必须记牢每个坐标系关于世界坐标系的方位，也可能是英语看的人头疼，我在这里设置了好久。第一个rigid transform是让移动副相对world frame的+y方向旋转90度，第二个rigid transform的引入是可以改变摆杆的初始位置。

连接后的模型框图为

仿真界面如下

为了显示运动参数，可以添加示波器，不过示波器显示的是信号量，必须将运动副的sensor接口连接到PS-Simulink Converter转为信号量，如下图：

仿真结果为

因为设置的初始角度为竖直向上，所以装置静止，信号稳定输出为水平线，所以接下来添加外力模块如图，同理需要将方波信号转为物理信号，模拟在一秒时给摆杆质心一个推力，推力设置为x方向。

最后模型框图为：

仿真结果如下：

由图中可以得知，在1s时摆杆顶部球心处受到100N沿世界坐标系+x持续0.5S的推力，产生的各种运动参数曲线可以直接在示波器中看到，因为没有控制力的引入，摆杆会直接倒下。此时为了实现倒立摆的平衡，会在小车x方向添加一个控制力来保持平衡，接下来便是控制器的设计，倒立摆的控制算法有很多，从经典控制理论（传递函数），现代控制理论（状态空间），一直到智能控制理论。每一个理论都可以用来控制倒立摆，可见倒立摆的控制实验是学习控制理论的最有效平台。。。

为了初步设计控制器输出控制力的信号，我了解了自控理论最经典的PID控制算法，得知其控制理念是直接采集被控变量进行对比然后处理输出（如下图），只是参数的整定比较繁琐，不过却可以进行初步的实验仿真，激发自己的学习兴趣。PID控制器参考https://www.cnblogs.com/shangdawei/p/4825259.html。文中说的很详细。里面有程序流程图教你写最简单的控制程序，笔者手头刚好有12xs128的MCU、普通有刷直流电机和光电编码器。根据流程，设计了电机的稳定转速控制器，不过难度依旧是参数的整定。好在MATLAB里关于PID的工具十分强大，里面的可视化工具包pidTuner，命令工具TuningGoal,systune等对于控制器的设计十分有帮助，最近一直在理解这几个函数指令，自己也从中受益颇深，尤其考验对自控原理的理解掌握。。。理解后PID算法后，就可以在simulink下开始控制倒立摆的平衡了，由于笔者整定参数还不太利索，经历多次尝试后，摆杆还是会倒下。。。。惭愧啊。

以下是运动规律：

不过之后我发现MATLAB里面自带了一个demo模型，在MATLAB命令窗口键入 InvertedPendulumExample即可打开如下界面。可见，此模型的控制方法是PID Controller和传递函数控制器综合输出为控制力F（其实角度控制也能用PID调节，只要响应够快就能实现摆杆的平衡）。不过想要更好的控制倒立摆还是需要动力分析，将传递函数写出，进行根轨迹找出极点，确定好平衡位置。