二连杆机器人的动力学
参考资料
二连杆机器人
DH参数
对连杆附加坐标系的规定
在连杆坐标系中对连杆参数的归纳
运动学
速度雅可比
Matlab验证代码
拉格朗日法动力学建模
理论
分析
实现重力配平
牛顿欧拉法动力学建模
参数辨识
线性化
辨识轨迹的设计
simscape仿真
参考
参考资料
「ロボット工学入門」高田洋吾著
Matlab 2018b
二连杆机器人
模型如下：

忽略摩擦力，空气阻力和轴承的粘滞摩擦力。

DH参数

对连杆附加坐标系的规定

在连杆坐标系中对连杆参数的归纳

运动学
见Matlab验证代码

速度雅可比
设末端坐标为( x p , y p )

Matlab验证代码

%数值计算
clear
clc
syms g;
syms L1 L2;
syms Lg1 Lg2;
syms I1 I2;
syms m1 m2 mL;
syms cta1 cta2;
syms d_cta1 d_cta2;
syms x1 y1;
syms x2 y2;
syms xL yL;
syms d_x1 d_y1;
syms d_x2 d_y2;
syms d_xL d_yL;
syms Fx Fy

%cta1=deg2rad(90); cta2=deg2rad(0); 
%L1=1; L2=1; 

dh_table=...
    [0,      0,   0,   cta1
     0,      L1,  0,   cta2
     0,      L2,  0,     0];
 
 alpha=dh_table(:,1);
 a=dh_table(:,2);
 d=dh_table(:,3);
 theta=dh_table(:,4);
 
T01=T_para(theta(1),d(1),a(1),alpha(1));
T12=T_para(theta(2),d(2),a(2),alpha(2));
T23=T_para(theta(3),d(3),a(3),alpha(3));

T02=T01*T12;
T03=T02*T23;
%正运动学
T_1=T01*[Lg1;0;0;1];
T_2=T02*[Lg2;0;0;1];
T_3=T03*[0;0;0;1];

x1=simplify(T_1(1));
y1=simplify(T_1(2));
%复合函数如何求导数(matlab复合函数求导无力)
d_x1=-Lg1*d_cta1*sin(cta1);
d_y1=Lg1*d_cta1*cos(cta1);

x2=simplify(T_2(1));
y2=simplify(T_2(2));
d_x2=-L1*d_cta1*sin(cta1)-Lg2*(d_cta1+d_cta2)*sin(cta1+cta2);
d_y2=L1*d_cta1*cos(cta1)+Lg2*(d_cta1+d_cta2)*cos(cta1+cta2);

xL=simplify(T_3(1));
yL=simplify(T_3(2));
d_xL=-L1*d_cta1*sin(cta1)-L2*(d_cta1+d_cta2)*sin(cta1+cta2);
d_yL=L1*d_cta1*cos(cta1)+L2*(d_cta1+d_cta2)*cos(cta1+cta2);

%速度雅可比
J11=-L1*sin(cta1)-L2*sin(cta1+cta2);
J12=-L2*sin(cta1+cta2);
J21=L1*cos(cta1)+L2*cos(cta1+cta2);
J22=L2*cos(cta1+cta2);
Jacobin=[J11 J12;J21 J22;0 0];
%动能
k1=0.5*m1*[d_x1;d_y1;0].'*[d_x1;d_y1;0]+0.5*[0;0;d_cta1].'*I1*[0;0;d_cta1];
k2=0.5*m2*[d_x2;d_y2;0].'*[d_x2;d_y2;0]+0.5*[0;0;d_cta1+d_cta2].'*I1*[0;0;d_cta1+d_cta2];%注意角速度的叠加
kL=0.5*mL*[d_xL;d_yL;0].'*[d_xL;d_yL;0];
u1=-m1*[0;-g;0].'*[x1;y1;0]+m1*[0;g;0].'*[0;Lg1;0];
u2=-m2*[0;-g;0].'*[x2;y2;0]+m2*[0;g;0].'*[0;Lg2+L1;0];
uL=-mL*[0;-g;0].'*[xL;yL;0]+mL*[0;g;0].'*[0;L2+L1;0];
k=k1+k2+kL;
u=u1+u2+uL;
L=simplify(k-u);
%虚功原理
FL=[Fx;Fy;0];%相对于基座标系
tau=Jacobin.'*FL;
tau_L1=tau(1);
tau_l2=tau(2);

%只考虑重力进行重力配平
tau_1=simplify(diff(u,cta1));
tau_1=collect(tau_1,cos(cta1));
tau_1=collect(tau_1,cos(cta1+cta2))
tau_2=simplify(diff(u,cta2))

%齐次变换矩阵
function T = T_para(theta,d,a,alpha)
	T=[ cos(theta),-sin(theta),0,a
	    sin(theta)*cos(alpha),cos(theta)*cos(alpha),-sin(alpha),-sin(alpha)*d
	    sin(theta)*sin(alpha),cos(theta)*sin(alpha),cos(alpha),cos(alpha)*d
	    0,0,0,1];
end

拉格朗日法动力学建模

理论

动能

分析

设如下图所示的坐标系统

图中的x n 
 和y n 
 均是相对于{ 0 }坐标系而言的。
在实际操作过程中，matlab在对符号方程存在较弱的求导能力，因此考虑之后使用Mathematica，
在这里仅对势能进行考虑。 或者在考虑动力学问题时使用牛顿欧拉法进行建模。

实现重力配平
不考虑动能及摩擦力，只考虑物体的势能。
根据仿真代码可得

牛顿欧拉法动力学建模
参数辨识
线性化
辨识轨迹的设计
simscape仿真
参考
URDF Import
How do I import XACRO files as Rigid Body Trees in Robotics System Toolbox? 这种方法对于ROS-pkg里的无效，简单点：

rosrun xacro xacro --inorder -o "$MYROBOT_NAME".urdf "$MYROBOT_NAME".urdf.xacro

然后check一下，是正确的: