全局变量宏定义
globalm1大臂质量
globalm2小臂的质量
globalL1大臂长度
globalL2小臂长度
globalLc1大臂质心到臂起点距离
globalLc2小臂臂质心到臂起点距离
globalI1
大臂转动惯量
globalI2
小臂转动惯量
globalg
重力加速度
globaltau转矩
m11
m202
L11
L206
Lc105
Lc203
I101
I2002
g98
clearclcparameters参数传递xx关节角度和角速度存储矩阵tt时间点矩阵th存取关节角度矩阵dth存取关节角速度矩阵r存取机械臂末端位置坐标矩阵kp100闭环系统的刚度kd14阻尼比
x068572655900关节角度和角速度初始值1212
计算期望关节角度和角速度fort0001100时间间隔001，取1000个时间片段
ttttt存储当前时间点
thkth_patht当前时刻机械臂末端位置所对应的关节角度12
thththk存储这一时间点的关节角度dthdthi
vJaccbithktdr_patht根据雅克比矩阵和机械臂末端速度求解关节角速度
12，保存
rrr_patht存储这一时间点机械臂末端位置坐标e
d

tle
gthtt
t所有的时间点即1000
fork1
ttaukpthkx12kd00x34根据闭环系统刚度和阻尼比计算伺服控制规律补偿
转矩
fMcHcestimat_M_HxtauMctauHc根据动力学方程是求解系统转矩tspa
k110001k10求解区间
tx1ode45d_ftspa
x求解动力学方程式得到1212
xx1e
d取求解区间中最后一个解xxxxx保存当前时刻点关节角度和角速度e
dholdo
plotL1cosxx1L2cosxx1xx2L1si
xx1L2si
xx1xx2plotr1r2
动力学方程式fu
ctio
vd_ftxparametersth_1x1关节1角度th_2x2关节2角度thd_1x3关节1角速度thd_2x4关节2角速度Mm1Lc12I1I2m2L12Lc222L1Lc2costh_2I2m2Lc22L1Lc2costh_2I2m2Lc22L1Lc2costh_2m2Lc22I2vecm2L1Lc2si
th_2thd_222thd_1thd_2m2L1Lc2si
th_2thd_12gravgm1Lc1costh_1m2L1costh_1Lc2costh_1th_2m2Lc2costh_1th_2resultMvecgravtauthdd_1result1thdd_2result2
vthd_1thd_2thdd_1thdd_21212
e
d
根据控制要求计算机械臂末端位置坐标
fu
ctio
rr_patht
parameters
r00
机械臂末端位置坐标
r101cos02pit05计算x轴坐标
r201si
02pit计算y轴坐标
e
d
根据控制要求计算机械臂末端速度
fu
ctio
drdr_patht
parameters
dr00机械臂末端速度大小（x轴、y轴方向）
dr10102pisi
02pit计算x轴方向速度
dr20102picos02pit计算y轴方向速度
e
d
根据逆运动学公式上面两个函r