已知機械臂動力模型（很複雜時），如何進行模擬？

01-27

當用拉格朗日方法導出動力方程時（用拉格朗日因為方便設計控制器），得到如下形式的表達式（式子1）：
這樣的表示如何matlab里進行模擬呢？s－function的話要求如下形式的表達（式子2）,這要對矩陣A求逆。對於簡單系統還好，但如果是複雜的多關節機械臂（特別是基座浮動的空間機械臂），矩陣A會非常複雜，我在matlab里用syms定義各關節變數和基座移動量後求出的A矩陣含有上十萬個字母和符號，然後matlab跑了20多個小時也沒算出逆來。
請問如何用第一張圖的式子進行模擬呢？或者還有別的方法模擬嗎？

怎麼解逆不懂……只能給題主提一個建議

如果機械臂的各個關節參數都定下來之後，可以用動力學模擬軟體做，像桂凱提到的Adams具有很好的matlab介面，至於Matlab的simmechanics好像內部沒有Ground的模型，如果做float base的話還是不太建議。

當然最建議的還是Vrep啦，方便好用，操作比Adams簡單多了，和各種語言都有介面，跨平台，專門做機器人模擬。內部有很多demo，有機械臂的還有機器人的，題主可以看看。

圖裡都是軟體自帶的一些機械臂

歡迎關注專欄：Vrep機器人動力學建模模擬

年前有一段空閑時間做了一些Vrep的教程，歡迎大家關注哈~

[連載 0]Vrep入門介紹
[連載 1]Vrep小車建模——前進和轉向
[連載 2]Vrep小車建模——內嵌腳本
[連載 3]Vrep小車建模——matlab控制
[番外1]彈跳平衡車

如題主描述，機械臂的動力學方程十分複雜，求解精確解也很困難。動力學方程： $A(q)q+B(q,q)q= au$

中如果已知 $au$ 求 $q$ 就是模擬問題，工程上常用數值積分的方法來求解對應的 $q$ 值，以在滿足精度的情況下大大減小計算量。

事實上，Matlab中已經有解決機器臂動力學問題的工具包--Matlab Robotics Toolbox。對於正向動力學問題（即已知 $au$ 求 $q$ ）定義機械臂SerialLink後可以使用SerialLink.fdyn求解。同樣對於逆動力學問題（即已知 $q ,q ,q$ 求 $au$ ，常用於控制），可以使用工具包的SerialLink.rne求解。具體用法見官方文檔。

Matlab Robotics Toolbox中的斯坦福機械臂

此外，正如 @劉宏偉所提，此問題對於機器人模擬工具如V-REP, Gazebo就更簡單了，這兩個模擬工具自身集成了ODE, Bullet等物理引擎，實時計算動力，做出響應，以達到模擬的效果。而事實上物理引擎本身也用的是數值積分的方法來求解力矩。

V-REP中模擬機械臂碰撞（實時計算動力）

對於該問題，Matlab的工具包就足夠用了。

@黃豆豆說得很對。我之前試過六自由度機械臂，Matlab計算動力學方程一次，耗時數秒。你可以通過這兩種方式提高計算效率：

1.Matlab里的simmechanics工具箱。你建立機器人模型後，可以快速用matlab的物理引擎進行計算，通過程序可獲取機器人運動狀態。

2.Adams。它做動力學分析也很好用，並易於與matlab通信。

不要用符號計算，用數值。

如何對機器人動力學矩陣M、C和G進行實時計算？可以關注該問題

對於6自由度機器人，整個方程大約有兩千項（具體多少和構型及關節類型有關），質量陣大約八、九百項，這種方程即使是用符號推導也不會用MATLAB，MATLAB的符號推導功能基本就是個笑話。

不論質量陣有多少項，只要不是在奇異構型，質量陣一般都是良態的，有了數值結果就可以求逆，沒什麼特別的地方。

機械臂這種高度耦合導致的非線性ODE一般也不會用MATLAB進行模擬，否則這個計算時間會讓人無法接受。

先把方程里的每一項做一下simplify，你會發現項數少了非常多，因為有很多很蠢的比如sin2+cos2原本沒有化為1的都化簡了。

然後把符號表達式列印到文件，略做修改改寫成函數文件，再調用，而不是直接用subs的符號替換計算，會快很多。

另外一個優化方法是預處理cos(q_i), sin(q_i), cos2(q_i), sin2(q_i), dq_i2等量，它們在方程中出現的次數是非常高的。dq_i*dq_j其實也能做預處理，但是數組到二維之後查表的消耗已經接近直接計算的消耗了。另外正餘弦查表需要把方程simplify之後再expand才能使用，否則會有很多類似於sin(a*q_i+b*q_j)的形式。

matlab下用拉格朗日方程求解動力學，的確是很困難。因為對時間再求導是很困難的一件事情，要增加很多預處理。而且matlab在符號計算方面並不是長項。

建議用牛頓歐拉迭代（RNE），matlab實現起來很方便，前段時間我也著手推導了一遍7自由度的正逆動力學。《機器人學導論》里也給出了很詳細的推到，搞清楚運動和力的關係之後，實現起來不難。推導的結果和KDL庫做了一下對比，也沒有出入。

關於機器人動力學，也有很多現成的庫，題主不妨借鑒一下。如Peter Corke的Robotics toolbox。如果C++能過關的話倒可以看看MIT的drake庫以及開源的KDL庫。我看到的大部分關於動力學的都是用牛頓歐拉迭代方法實現的。

如果純做模擬，ADAMS, Gazebo，Simmechanics都能完成。

這個case完美表現了拉格朗日運動方程在處理機器人問題時候的特點：方程非常簡潔，也很漂亮，但是要做這個方程的解，運算量會非常非常大。記得在上機器人動力學的時候書上寫過，到6軸的程度運算量可以達到10的8次方左右。所以一般拉格朗日運動方程只用於，2關節或者至多3關節的機器人，多關節的話，建議樓主還是定個參考系，一個關節一個關節的列方程算變換矩陣，完了再進行模擬，那樣的話今後要是出了什麼問題也比較好解決。

非要求解析解的話就沒必要用matlab了，用maplemathmaticapython都比他強。然後，求逆為何也要求解析解，就不能先求出A的數值，在求逆嗎

你可以試試matlab function模塊，我記得跟s-function模塊一個文件夾內。這兩個模塊用起來差不多，不過matlab function模塊內可以是任何函數，不需要狀態方程形式，最後也都能得到期望的結果，不過網上論壇有人做過測試，s-function速度明顯更快。對計算時間沒什麼要求的話你可以試試這個模塊。

外行問一句：為啥一定要做符號運算呢？這不就是一個數值求解常方程組活么？

這階數才多少？幾百上千頂天了吧。FEA裡面幾千萬的階數呢

你要是想搞出一個傳遞函數之類的，數值積分不可以直接給出。但是你可以用系統辨識啊。

這些玩意在matlab裡面都是現成的。

採用遞推拉格朗日演算法(Recursive Lagrangian Formulation)