拉格朗日法动力学建模：平面2R机器人3项关键矩阵推导与仿真

拉格朗日法动力学建模：平面2R机器人3项关键矩阵推导与仿真

在机器人控制领域，动力学建模是实现精确运动控制的基础。平面2R机器人作为最简单的串联机械臂结构之一，其动力学特性却蕴含着丰富的物理内涵。本文将采用拉格朗日法系统推导动力学方程，并分解出惯性矩阵、科氏力/向心力矩阵和重力矩阵这三个关键组成部分，为后续计算力矩控制等高级控制算法奠定理论基础。

1. 平面2R机器人系统描述

1.1 物理参数定义

考虑如图所示的平面2R机器人系统，其物理参数定义如下：

• 连杆参数：

  • 连杆1长度：$l_1 = 1m$
  • 连杆2长度：$l_2 = 1m$
  • 连杆质量：$m_1 = m_2 = 5kg$
  • 质量分布：均匀分布

• 坐标系设定：

  • 基坐标系$O_0$固定在关节1旋转中心
  • 关节坐标系$O_1$与$O_0$重合
  • 关节坐标系$O_2$位于关节2中心

• 运动变量：

  • 关节角：$\theta_1$, $\theta_2$
  • 关节角速度：$\dot{\theta}_1$, $\dot{\theta}_2$
  • 关节力矩：$\tau_1$, $\tau_2$

1.2 运动学基础

通过DH参数法可建立正运动学方程：

% MATLAB Robotics Toolbox示例 L1 = Link('d', 0, 'a', 1, 'alpha', 0); L2 = Link('d', 0, 'a', 1, 'alpha', 0); robot = SerialLink([L1 L2], 'name', '2R Planar'); T = robot.fkine([theta1 theta2]); % 正运动学求解

提示：在实际工程中，通常先验证运动学模型的正确性，再开展动力学分析。

2. 拉格朗日动力学推导

2.1 系统动能计算

系统总动能由两部分组成：

1. 连杆1动能： $$ T_1 = \frac{1}{2}I_1\dot{\theta}_1^2 $$ 其中$I_1 = \frac{1}{3}m_1l_1^2$为绕质心的转动惯量

2. 连杆2动能： $$ T_2 = \frac{1}{2}m_2v_2^2 + \frac{1}{2}I_2(\dot{\theta}_1+\dot{\theta}_2)^2 $$ 末端速度$v_2$可通过雅可比矩阵求得

动能矩阵形式： $$ T = \frac{1}{2}\dot{\theta}^T D(\theta)\dot{\theta} $$

2.2 系统势能计算

以基座x轴为零势能面，系统势能为：

$$ V = -m_1g\frac{l_1}{2}\cos\theta_1 - m_2g\left(l_1\cos\theta_1 + \frac{l_2}{2}\cos(\theta_1+\theta_2)\right) $$

2.3 拉格朗日方程

构建拉格朗日函数： $$ L = T - V $$

动力学方程标准形式： $$ \frac{d}{dt}\left(\frac{\partial L}{\partial \dot{\theta}}\right) - \frac{\partial L}{\partial \theta} = \tau $$

3. 关键矩阵分解

3.1 惯性矩阵D(θ)

惯性矩阵表征了系统动能与关节速度的二次型关系：

$$ D(\theta) = \begin{bmatrix} D_{11} & D_{12} \ D_{21} & D_{22} \end{bmatrix} $$

具体元素推导结果：

$$ \begin{aligned} D_{11} &= \frac{1}{3}m_1l_1^2 + m_2\left(l_1^2 + \frac{1}{3}l_2^2 + l_1l_2\cos\theta_2\right) \ D_{12} &= D_{21} = m_2\left(\frac{1}{3}l_2^2 + \frac{1}{2}l_1l_2\cos\theta_2\right) \ D_{22} &= \frac{1}{3}m_2l_2^2 \end{aligned} $$

3.2 科氏力/向心力矩阵C(θ,θ̇)

该矩阵反映了哥氏力和向心力的影响：

$$ C(\theta,\dot{\theta}) = \begin{bmatrix} -h\dot{\theta}_2 & -h(\dot{\theta}_1+\dot{\theta}_2) \ h\dot{\theta}_1 & 0 \end{bmatrix} $$

其中$h = -\frac{1}{2}m_2l_1l_2\sin\theta_2$

3.3 重力矩阵G(θ)

重力项由势能梯度决定：

$$ G(\theta) = \begin{bmatrix} \frac{1}{2}m_1gl_1\sin\theta_1 + m_2g\left(l_1\sin\theta_1 + \frac{1}{2}l_2\sin(\theta_1+\theta_2)\right) \ \frac{1}{2}m_2gl_2\sin(\theta_1+\theta_2) \end{bmatrix} $$

4. 动力学仿真实现

4.1 数值求解方法

采用欧拉积分法进行动力学仿真：

# Python仿真示例 def dynamics_sim(theta, theta_dot, tau, dt): # 计算各矩阵元素 D11 = (4/3)*m1*l1**2 + m2*(l1**2 + l2**2/3 + l1*l2*np.cos(theta[1])) D12 = m2*(l2**2/3 + 0.5*l1*l2*np.cos(theta[1])) D = np.array([[D11, D12], [D12, m2*l2**2/3]]) h = -0.5*m2*l1*l2*np.sin(theta[1]) C = np.array([[-h*theta_dot[1], -h*(theta_dot[0]+theta_dot[1])], [h*theta_dot[0], 0]]) G1 = 0.5*m1*g*l1*np.sin(theta[0]) + m2*g*(l1*np.sin(theta[0]) + 0.5*l2*np.sin(sum(theta))) G = np.array([G1, 0.5*m2*g*l2*np.sin(sum(theta))]) # 计算加速度 theta_ddot = np.linalg.inv(D) @ (tau - C @ theta_dot - G) # 状态更新 theta_new = theta + theta_dot*dt theta_dot_new = theta_dot + theta_ddot*dt return theta_new, theta_dot_new

4.2 典型工况仿真

设置初始条件$\theta_1=\pi/4$, $\theta_2=\pi/2$，零初始速度，观察自由运动：

4.3 控制输入响应

施加阶跃力矩$\tau=[1.0, 0.5]^T Nm$的响应特性：

% MATLAB控制响应仿真 tspan = [0 5]; init_cond = [0; 0; 0; 0]; % [θ1; θ2; θ1_dot; θ2_dot] [t,y] = ode45(@(t,y) robot_dynamics(t,y,[1.0; 0.5]), tspan, init_cond);

注意：实际控制中需要考虑力矩饱和、关节限位等工程约束条件。

5. 工程应用指导

5.1 计算力矩控制实现

基于动力学模型的计算力矩控制律：

$$ \tau = D(\theta)(\ddot{\theta}_d + K_v\dot{e} + K_pe) + C(\theta,\dot{\theta})\dot{\theta} + G(\theta) $$

其中：

• $\theta_d$为期望轨迹
• $e = \theta_d - \theta$为跟踪误差
• $K_p$, $K_v$为增益矩阵

5.2 模型简化策略

针对实时控制需求，可考虑以下简化：

1. 忽略科氏力项：在低速运动时可简化
2. 离线计算重力补偿：固定工作点时预计算
3. 惯性矩阵对角化：牺牲精度换取计算效率

5.3 参数敏感性分析

关键参数对动态性能的影响：

在机械臂抓取不同负载时，需要重新辨识质量参数$m_2$以获得最佳控制性能。