二连杆机械臂阻抗控制模拟（一）

XXX已失联

发布时间 2021.11.30阅读数 5612 评论数 0

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

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

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

　　连杆1与连杆2在坐标系中的方向向量与角度 $θ_{1}$ 、 $θ_{2}$ 相关，其中连杆1的方向向量为，连杆2的方向向量为 $\mathbf{er_2}=\begin{bmatrix}-sin(\theta_1+\theta_2)\\ \cos(\theta_1+\theta_2)\\0\end{bmatrix}$

　　 $r_{a c_{1}} = d_{1} \cdot e r_{1} r_{b c_{2}} = d_{2} \cdot e r_{2} r_{b e} = l_{2} \cdot e r_{2} r_{a b} = l_{1} \cdot e r_{1} r_{a c_{2}} = r_{a b} + r_{b c_{2}} r_{a e} = r_{a b} + r_{b e}$

　　然后计算各点速度：连杆1上线速度的方向向量为，连杆2上线速度的方向向量为 $\mathbf{ev_2}=\begin{bmatrix}-cos(\theta_1+\theta_2)\\ -\sin(\theta_1+\theta_2)\\0\end{bmatrix}$

　　 $\mathbf{v_{c_1}}=d_1 \cdot \dot{\theta_1} \cdot\mathbf{ev_1}\\ \mathbf{v_b}=l_1 \cdot \dot{\theta_1}\cdot \mathbf{ev_1}\\ \mathbf{v_{c_2}}=\mathbf{v_b} + d_2 \cdot (\dot{\theta_1}+\dot{\theta_2})\cdot \mathbf{ev_2}\\ \boxed{\mathbf{v_e}=\mathbf{v_b} + l_2 \cdot (\dot{\theta_1}+\dot{\theta_2})\cdot \mathbf{ev_2}}$

　　接着对速度求导计算各点加速度：

　　 $\begin{align*}&\mathbf{a_{c_1}}=\dot{\mathbf{v_{c_1}}} = d_1 \cdot \ddot{\theta_1} \cdot\mathbf{ev_1} - d_1 \cdot \dot{\theta_1}^2 \cdot \mathbf{er_1}\\&\mathbf{a_b}=\dot{\mathbf{v_b}} = l_1 \cdot \ddot{\theta_1} \cdot\mathbf{ev_1} - l_1 \cdot \dot{\theta_1}^2 \cdot \mathbf{er_1}\\&\mathbf{a_{c_2}}=\dot{\mathbf{v_{c_2}}} = d_2 \cdot (\ddot{\theta_1}+\ddot{\theta_2}) \cdot\mathbf{ev_2} - d_2 \cdot (\dot{\theta_1}+\dot{\theta_2})^2 \cdot \mathbf{er_2} + \mathbf{a_b}\end{align*}$

　　根据受力情况可计算出关节处的力矩，对于关节 $b$ 来说出除了电机力矩外，杆件2自身重量以及末端上的作用力 $\mathbf{F}$ 都会对 $b$ 点产生力矩，因此 $b$ 点的合力矩为：

　　 $\mathbf{M_b}=(\mathbf{r_{bc_2}}\times m_2 \mathbf{g}) + (\mathbf{r_{be}}\times \mathbf{F}) +\mathbf{\tau_2}$

　　根据动量矩定理，杆2对 $b$ 点的动量矩变化率 $\frac{d L_{2}}{d t} = M_{b}$ ， $\frac{d L_{2}}{d t} = I_{2} (\ddot{θ_{1}} + \ddot{θ_{2}}) + r_{b c_{2}} \times m_{2} a_{c_{2}}$ ，其中 $I_{2}$ 为杆2绕其质心 $c_{2}$ 的转动惯量， $I_{2} = \frac{1}{12} m_{2} {l_{2}}^{2}$ 。根据动量矩定理可得到角加速度 $\ddot{θ_{2}}$ 与力矩 $M_{b}$ 的关系式。

　　对于关节 $a$ ，外力的合力矩为：

　　 $\begin{align*}\mathbf{M_a}&=(\mathbf{r_{ac_2}}\times m_2 \mathbf{g}) +(\mathbf{r_{ac_1}}\times m_1 \mathbf{g}) + (\mathbf{r_{ae}}\times \mathbf{F}) +\mathbf{\tau_1}\\ \frac{d\mathbf{L_1}}{dt}&=I_2(\mathbf{\ddot{\theta_1}}+\mathbf{\ddot{\theta_2}})+I_1\mathbf{\ddot{\theta_1}}+ (\mathbf{r_{ac_2}} \times m_2 \mathbf{a_{c_2}}) + (\mathbf{r_{ac_1}} \times m_1 \mathbf{a_{c_1}})\end{align*}$

　　其中 $I_{1}$ 为杆1绕其质心 $c_{1}$ 的转动惯量， $I_{1} = \frac{1}{12} m_{1} {l_{1}}^{2}$ 。根据动量矩定理 $\frac{d L_{1}}{d t} = M_{a}$ 可得到角加速度 $\ddot{θ_{1}}$ 与力矩 $M_{a}$ 的关系式。　　

　　通常可将根据动量矩定理或牛顿-欧拉法推导出的等式写为如下形式：

τ = M (θ) θ ¨ + C (θ, θ ˙) + G (θ)

$\boxed{\tau = M(\theta)\ddot{\theta}+C(\theta,\dot{\theta})+G(\theta)}$

　　如果机械臂自由度为 $n$ ， $M (θ)$ 为 $n \times n$ 阶正定对称矩阵， $M (θ) \ddot{θ}$ 代表惯性力项。 $M (θ)$ 中的主对角线元素表示各连杆本身的有效惯量，代表给定关节上的力矩与产生的角加速度之间的关系，非对角线元素表示连杆之间的耦合惯量，即是某连杆的加速运动对另一关节产生的耦合作用力矩的度量； $C (θ, \dot{θ})$ 为 $n \times 1$ 阶向心力和科氏力项； $G (θ)$ 为 $n \times 1$ 阶的重力项，与机器人的形位 $θ$ 有关。

　　在纯位置控制下施加在机械臂末端的外力并不会影响末端的运动，因为这种情况可以认为机械臂是完全刚性的。而如果要实现主动柔顺控制，即使机械臂表现出一定的柔性就需要考虑其与环境之间的相互作用。这时关节驱动力矩可写为：

τ = M (θ) θ ¨ + C (θ, θ ˙) + G (θ) + J T (θ) F t i p

$\boxed{\tau = M(\theta)\ddot{\theta}+C(\theta,\dot{\theta})+G(\theta)+J^T(\theta)F_{tip}}$

　　上面等式中，为机械臂末端与外界环境之间的交互力， $J$ 为机械臂的雅可比矩阵，用于将关节空间速度映射到操作空间： $v = J \dot{θ}$ ，雅可比矩阵的转置也可将操作空间中的力映射到关节空间中： $τ = J^{T} F$ 。对于本例，机器人雅可比矩阵可用MATLAB中的函数jacobian来计算， $J = j a c o b i a n (r_{a e}, [θ_{1}, θ_{2}])$ 。代表作用于机器人关节上的外界环境力矩，等式左边的 $\tau$ 为机器人关节驱动力矩，计算出该力矩后就可以输入给机器人驱动系统，实现期望的运动。

　　机械臂与环境交互产生的力矩可写为 $\tau_{ext}=J^T(\theta)F_{tip}=\underline{J^T(\theta)[M(\ddot{x_d}-\ddot{x})+D(\dot{x_d}-\dot{x})+K(x_d-x)]}$ ，和 $x$ 分别代表目标位置和实际位置， $\dot{x_d}$ 和分别代表目标速度和实际速度。矩阵 $K$ 和 $D$ 代表与环境交互的刚度和阻尼。机械臂末端的加速度 $\ddot{x}$ 一般难以测量，如果直接对速度进行微分又会产生大量噪声。通常对于协作型机械臂，自身重量设计的较轻，因此可以忽略其惯性，即令 $M = 0$ 。