筆者最近搗鼓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調節，只要響應夠快就能實現擺桿的平衡）。不過想要更好的控制倒立擺還是需要動力分析，將傳遞函數寫出，進行根軌跡找出極點，確定好平衡位置。

總之，Simulink對倒立擺的控制學習也提供了可視化的模擬。之後我打算以此模型為基礎，添加不同控制器，學習基本控制理念。今後的大致框圖將擴展成如下幾個區域（當然可以整成一個subsystem）：

(1)直線倒立擺模型

(2)幹擾信號發生器

(3)參數采集顯示器

(4)不同控制調節器

SimMechanics/Second Generation倒立擺模型建立及初步仿真學習