資料：https://blog.csdn.net/huangjunsheng123/article/details/110630665
用vscode直接看工具箱api代碼比較方便，代碼說明很多
一、模型設置
1、基礎效果
%采用機器人工具箱進行正逆運動學驗證
a=[0,-0.3,-0.3,0,0,0];%DH參數
d=[0.05,0,0,0.06,0.05,0.04];
alp=[pi/2,0,0,pi/2,-pi/2,0];
%機器人工具箱模型，每一個L建立一個連桿，輸入DH參數即可，offset是連桿的初始偏移量，此處為0
L(1) = Link('d', d(1), 'a', a(1), 'alpha', alp(1));
L(2) = Link('d', d(2), 'a', a(2), 'alpha', alp(2), 'offset', 0 );
L(3) = Link('d', d(3), 'a', a(3), 'alpha', alp(3), 'offset', 0 );
L(4) = Link('d', d(4), 'a', a(4), 'alpha', alp(4), 'offset',0);
L(5) = Link('d', d(5), 'a', a(5), 'alpha', alp(5));
L(6) = Link('d', d(6), 'a', a(6), 'alpha', alp(6));
bot = SerialLink(L,'name','rbt');%將六個連桿組合起來
figure(1)
bot.display();%顯示機器人模型
view(3)
bot.teach;
2、link的常用屬性
運動學參數
% theta kinematic: joint angle 關節角度
% d kinematic: link offset
% a kinematic: link length
% alpha kinematic: link twist % jointtype kinematic: 'R' if revolute, 'P' if prismatic 關節類型
% mdh kinematic: 0 if standard D&H, else 1 參數類型（標準/改進）
% offset kinematic: joint variable offset 關節變量偏移量（設置新零點）
% qlim kinematic: joint variable limits [min max] 關節變量限制范圍
動力學參數
% m dynamic: link mass 連桿質量
% r dynamic: link COG wrt link coordinate frame 3x1 質心
% I dynamic: link inertia matrix, symmetric 3x3, about link COG. 慣性張量
% B dynamic: link viscous friction (motor referred) 連桿粘性摩擦
% Tc dynamic: link Coulomb friction 連桿庫侖摩擦
%-
% G actuator: gear ratio 減速比
% Jm actuator: motor inertia (motor referred) 電機慣量二、運動學
1、正逆運動學
q=[1,-1,1,-1,1,1];%給定初始角度
T_rbt = bot.fkine(q);%采用機器人工具箱函數求正運動學
q_rbt = bot.ikine(T_rbt,'q0',q)%采用RTB的逆運動學計算上邊得到的齊次矩陣對應的關節角度，給定初始值可以更快的收斂到給定的角度2、雅各比矩陣
bot.jacob0() 關于絕對坐標系的雅各比矩陣
bot.jacobn() 關于工作坐標系的雅各比矩陣三、動力學
1、查看動力學參數
bot.dyn()
2、正逆動力學
bot.nofriction() //無摩擦力動力學模型
bot.accel(角度，角速度，外力矩陣) //計算關節加速度bot.fdyn(時間，外力矩陣) //正動力學
bot.rne(角度，角速度，角加速度，重力加速度，輸出動力要求) //逆動力學
3、計算動力學方程
bot.gravload() //G
bot.inertia() //M
bot.coriolis() //V
bot.payload() //有效載荷
四、例子：二自由度平面機械臂
syms Ix1 Iy1 Iz1 Ix2 Iy2 Iz2
syms q1 q2 m1 m2 L1 L2
syms qd1 qd2 g
%%%%%%%%%%%%%%%%%%%1、設置運動學參數%%%%%%%%%%%%%%%%%%%
a=[0,L1,L2];%DH參數
d=[0,0,0];
alp=[0,0,0];q = [q1,q2,0];
qd = [qd1,qd2,0];L(1) = Link('d', d(1), 'a', a(1), 'alpha', alp(1),'modified'); %'modified'代表使用改進dh法
L(2) = Link('d', d(2), 'a', a(2), 'alpha', alp(2),'modified');
L(3) = Link('d', d(3), 'a', a(3), 'alpha', alp(3),'modified');
bot = SerialLink(L,'name','rbt'); %將三個連桿組合起來，形成機器人
%%%%%%%%%%%%%%%%%%%2、設置動力學參數%%%%%%%%%%%%%%%%%%%
%質量
bot.links(1).m=m1;
bot.links(2).m=m2;
bot.links(3).m=0;
%質心
bot.links(1).r=[L1/2,0,0];
bot.links(2).r=[L2/2,0,0];
bot.links(3).r=[0,0,0];
%轉動慣量
bot.links(1).I = [Ix1, Iy1, Iz1, 0, 0,0];
bot.links(2).I= [Ix2, Iy2, Iz2,0, 0,0];
bot.links(3).I = [0, 0, 0, 0, 0,0];
%重力
bot.gravity=[0;0;-g];bot.dyn(); %顯示所有桿的動力學參數
%%%%%%%%%%%%%%%%%%%3、計算運動學參數%%%%%%%%%%%%%%%%%%%
T = bot.fkine(q); %機器人正運動學，輸入關節角度，輸出T_e^0
J = bot.jacob0(q); %計算雅各比矩陣，基于世界坐標的
%%%%%%%%%%%%%%%%%%%4、計算動力學參數%%%%%%%%%%%%%%%%%%%
M = bot.inertia(q); %關節空間質量矩陣
V = bot.coriolis(q,qd); %計算離心力和科氏力矢量，注意這里給出的是矩陣V，V*qd才等于計算出的向量V（qd）
G = bot.gravload(q); %重力矢量
%%%%%%%%%%%%%%%%%%%5、正逆運動學%%%%%%%%%%%%%%%%%%%
%逆運動學，角度（不是微分運動學）
theta1 = bot.ikine(T,'mask',[1,1,1,0,0,0],'q0',[0,0,0]); %T代表的是工作點的位姿，輸出各個關節角度，q0是基座坐標，mask是關節小于6個時選擇開啟的
theta2 = bot.ikunc(T);
%%正逆微分運動學
%%%%%%%%%%%%%%%%%%%6、正逆動力學%%%%%%%%%%%%%%%%%%%
bot_nf=bot.nofriction(); %設置沒有摩擦力 %其實就是根據關節力矩和作用力矩+初始q和qd算qdd，再根據qdd更新q和qd
%句柄那里不可以用[1,2,3,4,5,6]這樣賦值了，可以直接給[]就是不給力
%0.4是時間間隔，@函數句柄（函數需要特別設置，因為要解算方程），第一個qz是q0,第二個qz是qd0,5和1就是函數句柄的參數
%輸出就是一系列t、q、qd[t q qd] = p560.nofriction().fdyn(0.4, @test1, qz,qz,5,1);torqfun = [0,30,6]; %設定一組關節力
qdd = bot_nf.accel(q(i,:),qd(i,:),torqfun); %獲得中間量的辦法，fdyn應該調用了accelfprintf("-----------------------")
%逆動力學
tau = bot.rne(q, qd, qdd, 'gravity',[0,9.8,0],'fext',[0,0,0,0,0,0]);%fext是外力test1.m
function tau = test1(t,q,qd,qstar,P,D)
tau=P*(qstar-q) + D*qd;
tau =[0,0,0,0,0,0];
end
)
)
