两连杆机械臂动力学建模

不到150不改名698

1589人浏览 · 2025-05-05 20:14:00

不到150不改名698 · 2025-05-05 20:14:00 发布

一、两连杆演示图与假设

假设机械臂模型如上图所示，首先，我们做如下假设：

1.假设质点集中在连杆末端

2.刚体机械臂，不发生形变

3.关节运动时无摩擦

以底座为原点，机械臂双杆的活动平面为坐标轴平面，水平线为x轴，垂直于x轴为y轴，x轴与y轴相交于原点（底座）。

l1,l2,m1,m2分别是各个杆件的长度、质量，q1为第一个杆件与x轴的夹角，q2为第二个杆件与第一个的夹角。(x1,y1)为连杆1末端的位置，(x2,y2)为连杆2末端的位置。

二、动能与势能的计算方式

构造拉格朗日函数： $L(q, \dot{q}) = E(q, \dot{q}) - P(q, \dot{q})$

机械臂的动力学方程： $\frac{d}{dt} \frac{\partial L(q, \dot{q})}{\partial \dot{q}} - \frac{\partial L(q, \dot{q})}{\partial q} = \tau$

由上述函数，我们首先要得到 $E(q, \dot{q})$ , $P(q, \dot{q})$ ，分别是连杆的动能和势能。因此，我们需要一个一个来进行运算。

由上图的机械臂模型，通过简单的数学运算，可以得到 $P_1 = m_1 g l_1 \sin(q_1)$ 。 $P_2 = m_2 g l_2 ( l_1 \sin (q_1) + l_2 \sin (q_1 + q_2))$ ，那么 $P=P_1+P_2$ ，这样的我们就得到了势能之和。

现在我们开始求解动能之内，高中时，大家想必都学会动能的运算公式 $E = \frac{1}{2} m v^2$ ，因此，我们需要算出速度的平方。

先拿第一个连杆为例：我们对位置求时间求导就得到了速度。

$\begin{cases} x_1 = l_p \cos(q_1) \\ y_1 = l_p \sin(q_1) \end{cases}\rightarrow$ $\begin{bmatrix} \dot{x}_1 \\ \dot{y}_1 \end{bmatrix} = \begin{bmatrix} -l_1 \sin(q_1) \\ l_1 \cos(q_1) \end{bmatrix} \dot{q}_1$

所以，我们可以得到

$E_1 = \frac{1}{2} m_{1} ( \dot{x}_1^2 + \dot{y}_1^2) = \frac{1}{2} m_{1} l_1^2 \dot{q}_1^2$

同理可得第二个连杆：

$\begin{cases} x_2 = l_1 \cos(q_1) + l_2 \cos(q_1 + q_2) \\ y_2 = l_1 \sin(q_1) + l_2 \sin(q_1 + q_2) \end{cases}\rightarrow$ $\begin{bmatrix} \dot{x}_2 \\ \dot{y}_2 \end{bmatrix} = \begin{bmatrix} -l_1 \sin(q_1) - l_2 \sin(q_1 + q_2) & -l_2 \sin(q_1 + q_2) \\ l_1 \cos(q_1) + l_2 \cos(q_1 + q_2) & l_2 \cos(q_1 + q_2) \end{bmatrix} \begin{bmatrix} \dot{q}_1 \\ \dot{q}_2 \end{bmatrix}$

所以，同理，我们可以得到

$E_2 = \frac{1}{2} m_2 (\dot{x}_2^2 + \dot{y}_2^2) = \frac{1}{2} m_2 \left[ \dot{q}_1^2 (l_1^2 + l_2^2 + 2 l_1 l_2 \cos(q_2)) + l_2^2 \dot{q}_2^2 + 2 \dot{q}_1 \dot{q}_2 l_2 (l_1 \cos(q_2) + l_2) \right]$

这样，我们的 $E=E_1+E_2$

三、拉格朗日法推导动力学方程

对上述方程简化，计 $\cos(q_1) = c_1 , \cos(q_2) = c_2 , \sin(q_1) = s_1, \sin(q_2) = s_2, \cos(q_1 + q_2) = c_{12} , \sin(q_1 + q_2) = s_{12}$

利用上述已经计算量，套用拉格朗日方程函数可以得到：

$\frac{\partial L}{\partial \dot{q}_1} = (m_1 + m_2) l_1^2 \dot{q}_1 + m_2 l_2^2 (\dot{q}_1 + \dot{q}_2) + m_2 l_1 l_2 (2 \dot{q}_1 + \dot{q}_2) c_2 \\ \frac{d}{dt} \frac{\partial L}{\partial \dot{q}_1} = (m_1 + m_2) l_1^2 \ddot{q}_1 + m_2 l_2^2 (\ddot{q}_1 + \ddot{q}_2) + m_2 l_1 l_2 (2 \ddot{q}_1 + \ddot{q}_2) c_2 - m_2 l_1 l_2 (2 \dot{q}_1 + \dot{q}_2) \dot{q}_2 s_2 \\ \frac{\partial L}{\partial q_1} = -(m_1 + m_2) g l_1 c_1 - m_2 g l_2 c_{12} \\ \frac{\partial L}{\partial \dot{q}_2} = m_2 l_2^2 (\dot{q}_1 + \dot{q}_2) + m_2 l_1 l_2 \dot{q}_1 c_2 \\\frac{d}{dt} \frac{\partial L}{\partial \dot{q}_2} = m_2 l_2^2 (\ddot{q}_1 + \ddot{q}_2) + m_2 l_1 l_2 \ddot{q}_1 c_2 - m_2 l_1 l_2 \dot{q}_1 \dot{q}_2 s_2 \\ \frac{\partial L}{\partial q_2} = -m_2 l_1 l_2 (\dot{q}_1^2 + \dot{q}_1 \dot{q}_2) s_2 - m_2 g l_2 c_{12}$

这样，我们就把所以需要的量都运算出来了，代入拉格朗日函数方程运算得到：

$\tau_1 = \left( (m_1 + m_2) l_1^2 + m_2 l_2^2 + 2 m_2 l_1 l_2 c_2 \right) \ddot{q}_1 + \left( m_2 l_2^2 + m_2 l_1 l_2 c_2 \right) \ddot{q}_2 - m_2 l_1 l_2 (2 \dot{q}_1 + \dot{q}_2) \dot{q}_2 s_2 + (m_1 + m_2) g l_1 c_1 + m_2 g l_2 c_{12}$

$\tau_2 = (m_2 l_2^2 + m_2 l_1 l_2 c_2) \ddot{q}_1 + m_2 l_2^2 \ddot{q}_2 + m_2 l_1 l_2 \dot{q}_1^2 s_2 + m_2 g l_2 c_{12}$

将上述方程整理可得：

$M(q) \begin{bmatrix} \ddot{q}_1 \\ \ddot{q}_2 \end{bmatrix} + C(q, \dot{q}) \begin{bmatrix} \dot{q}_1 \\ \dot{q}_2 \end{bmatrix} + G(q) = \begin{bmatrix} \tau_1 \\ \tau_2 \end{bmatrix}$

其中

$M(q) = \begin{bmatrix} (m_1 + m_2) l_1^2 + m_2 l_2^2 + 2 m_2 l_1 l_2 c_2 & m_2 l_2^2 + m_2 l_1 l_2 c_2 \\ m_2 l_2^2 + m_2 l_1 l_2 c_2 & m_2 l_2^2 \end{bmatrix} \\ C(q, \dot{q}) = \begin{bmatrix} -2 m_2 l_1 l_2 \dot{q}_2 s_2 & -m_2 l_1 l_2 \dot{q}_2 s_2 \\ m_2 l_1 l_2 \dot{q}_1 s_2 & 0 \end{bmatrix} \\ G(q) = \begin{bmatrix} (m_1 + m_2) g l_1 c_1 + m_2 g l_2 c_{12} \\ m_2 g l_2 c_{12} \end{bmatrix}$