matlab遞推牛頓-歐拉法解機械臂動力學方程

遞推牛頓歐拉動力學演算法形式如下（貟超譯：機器人學導論）：

重點理解：

1、外推過程是從基座開始一級一級往外計算連桿的速度和加速度；內推過程是從連桿n到連桿1向內計算連桿間的相互作用力。

2、上下角標關係的含義

值得注意的是，i是從0開始取值，但是matlab對於數組的取值不像C語言一樣從0取值，這個現象使得我在代碼實現過程中，順著往上加了一個數。即

外推 i：1->6

內推 i：7->2

這裡以二關節機械臂來舉例：

我們要充分利用好matlab的符號計算優勢，首先定義如下符號：

syms l1 l2 m g I w0 dw0 dv0 c1 s1 c2 s2 dq dq1 dq2 ddq2 dw Pc F N f n m1 m2 ddq1 ddq2

接著，因為我們要使用數組矩陣，所以在這裡利用matlab的cell函數定義好我們要用的量：

R=cell(1,6);w=cell(1,6);dw=cell(1,6);dq=cell(1,6);ddq=cell(1,6);dv=cell(1,6);f=cell(1,6);n=cell(1,6);P=cell(1,6);Pc=cell(1,6);F=cell(1,6);m=cell(1,6);N=cell(1,6);I=cell(1,6);

將已知的初值賦值：

%初值賦值R{1}=[c1 -s1 0;s1 c1 0;0 0 1];R{2}=[c2 -s2 0;s2 c2 0;0 0 1];R{3}=[1 0 0;0 1 0;0 0 1];dv{1}=[0;g;0];%坐標系原點位移，用P{1}表示坐標系1與坐標系0原點位置關係，用P{2}表示坐標系2與坐標系1原點位置關係。P{1}=[0;0;0];P{2}=[l1;0;0];P{3}=[l2;0;0];%每個連桿質心的位置矢量Pc{2}=[l1;0;0];Pc{3}=[l2;0;0];%連桿質量m{2}=m1;m{3}=m2;I{2}=[0;0;0];%機器人底座不旋轉w{1}=[0;0;0];dw{1}=[0;0;0];%旋轉關節dq{2}=[0;0;dq1];dq{3}=[0;0;dq2];ddq{2}=[0;0;ddq1];ddq{3}=[0;0;ddq2];%末端執行器沒有力f{4}=[0;0;0];n{4}=[0;0;0];

牛頓歐拉法遞推過程：

%外推for i=1:2w{i+1}=R{i}.*w{i}+dq{i+1};dw{i+1}=R{i}.*dw{i}+cross(R{i}.*w{i},dq{i+1})+ddq{i+1};dv{i+1}=R{i}.*(cross(dw{i},P{i})+cross(w{i},cross(w{i},P{i}))+dv{i});dvc{i+1}=cross(dw{i+1},Pc{i+1})+cross(w{i+1},cross(w{i+1},Pc{i+1}))+dv{i+1};F{i+1}=m{i+1}*dvc{i+1};%假設質量集中，每個連桿慣性張量為0N{i+1}=[0;0;0];end%內推for i=3:-1:2 f{i}=R{i}*f{i+1}+F{i}; n{i}=N{i}+R{i}*n{i+1}+cross(Pc{i},F{i})+cross(P{i},R{i}*f{i+1});end

這就是二關節機械臂動力學方程的matlab實現過程。

輸出結果：

%輸出力矩tol2=l2*m2*(c2*(c1*g + ddq1*l1) + l2*(ddq1 + ddq2) - s2*(- l1*dq1^2 + g*s1))tol1=l1*(m2*s2*(s2*(c1*g + ddq1*l1) - l2*(dq1 + dq2)^2 + c2*(- l1*dq1^2 + g*s1)) + c2*m2*(c2*(c1*g + ddq1*l1) + l2*(ddq1 + ddq2) - s2*(- l1*dq1^2 + g*s1))) + l2*m2*(c2*(c1*g + ddq1*l1) + l2*(ddq1 + ddq2) - s2*(- l1*dq1^2 + g*s1)) + l1*m1*(c1*g + ddq1*l1)

力矩輸出結果與書上的結果在形式上並不完全一樣，但是你將括弧打開後，結果是一樣的。

感興趣的同學可以將代碼擴展到六自由度，輸出結果太長了貼不上來~~