资料：https://blog.csdn.net/huangjunsheng123/article/details/110630665
用vscode直接看工具箱api代码比较方便，代码说明很多
一、模型设置
1、基础效果
matlab
%采用机器人工具箱进行正逆运动学验证
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的常用属性
matlab
运动学参数
% 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、正逆运动学
matlab
q=[1,-1,1,-1,1,1];%给定初始角度
T_rbt = bot.fkine(q);%采用机器人工具箱函数求正运动学
q_rbt = bot.ikine(T_rbt,'q0',q)%采用RTB的逆运动学计算上边得到的齐次矩阵对应的关节角度，给定初始值可以更快的收敛到给定的角度2、雅各比矩阵
matlab
bot.jacob0() 关于绝对坐标系的雅各比矩阵
bot.jacobn() 关于工作坐标系的雅各比矩阵三、动力学
1、查看动力学参数
matlab
bot.dyn()2、正逆动力学
matlab
bot.nofriction() //无摩擦力动力学模型
bot.accel(角度，角速度，外力矩阵) //计算关节加速度
bot.fdyn(时间，外力矩阵) //正动力学
bot.rne(角度，角速度，角加速度，重力加速度，输出动力要求) //逆动力学3、计算动力学方程
matlab
bot.gravload() //G
bot.inertia() //M
bot.coriolis() //V
bot.payload() //有效载荷四、例子：二自由度平面机械臂
matlab
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应该调用了accel
fprintf("-----------------------")
%逆动力学
tau = bot.rne(q, qd, qdd, 'gravity',[0,9.8,0],'fext',[0,0,0,0,0,0]);%fext是外力test1.m
matlab
function tau = test1(t,q,qd,qstar,P,D)
tau=P*(qstar-q) + D*qd;
tau =[0,0,0,0,0,0];
end