二关节机械臂matlab控制,二连杆机械臂阻抗控制模拟（一）

在学习机器人动力学相关内容时看到MATLAB论坛上一个有意思的仿真项目Impedance Control for a 2-Link Robot Arm - User-interactive，一个用MATLAB实现的平面二连杆机械臂阻抗控制仿真。用户可以点击并拖拽鼠标来实时改变机械臂的目标位置，在控制力矩作用下机械臂会跟随目标点运动。按空格键可以切换控制模式，此时拖拽鼠标用来给末端施加一个扰动力，由于阻抗控制的作用末端表现得像弹簧阻尼器一样，扰动力消失后末端回复到目标位置。

让我们来关注一下实现的细节。如下图所示，连杆1和连杆2在XY平面上通过旋转关节串联构成一个二自由度的机械臂(忽略关节摩擦等因素)，重力加速度$g$沿着Y轴负方向(向量形式可写为$\mathbf{g}=[0,-g,0]^T$)。机械臂关节$a$固定在坐标原点，末端为$e$，F为作用在末端的力。$c_1$与$c_2$分别为两个连杆的质心，杆长分别为$l_1$、$l_2$，杆的质量分别为$m_1$、$m_2$，质心到关节的距离分别为$d_1$、$d_2$。$\theta_1$为与Y轴正方向的夹角($\theta_1=\theta_2=0$时机器人保持竖直状态)，$\theta_2$为连杆2与连杆1之间的夹角，是一个相对角度。

定义上述变量后根据理论力学等知识开始一系列计算。首先计算连杆上各点的位置坐标：

连杆1与连杆2在坐标系中的方向向量与角度$\theta_1$、$\theta_2$相关，其中连杆1的方向向量为$\mathbf