拉格朗日法实现SCARA 机械臂的动力学

REF：SCARA 机器人的拉格朗日动力学建模

1. 拉格朗日公式

可以理解为一种 “能量记账法” ，核心思想是：一个力学系统的真实运动轨迹，会使一个叫做“拉格朗日函数”的量在时间上的平均积累（即作用量）取极值。

核心公式是 拉格朗日方程：

与牛顿-欧拉法的对比：

• 拉格朗日法：基于能量（标量），通过微分得到方程，步骤程式化，适合推导封闭形式的动力学方程。
• 牛顿-欧拉法：基于力与力矩（矢量） 的平衡，物理直观，更适合计算机进行递归数值计算。

使用拉格朗日公式推导机械臂动力学方程的一般步骤如下：

（1）选定机器人的广义坐标 q。虽然广义坐标的选定并不唯一，但是一般优先选择使用 D-H 方法所确定的运动学参数；

（2）确定作用在机器人系统上，并且对系统可以做功的非保守力；

（3）计算机器人系统的总动能 T、总势能 V 以及拉格朗日量L=T-V；

（4）计算拉格朗日公式所需的偏导数；

（5）计算机器人系统的广义力 F；

（6）代入使用拉格朗日公式求得系统动力学方程。

2. 对SCARA机械臂建模

一个典型的4轴SCARA机械臂为例，其关节变量为 q=[θ1,θ2,d3,θ4]，确定作用在机器人系统上，并且对系统可以做功的非保守力。

假设机器人末端与环境的作用力 f 为 0，那么对于 SCARA 机器人系统而言，“不做功的非保守力”是各关节电机的驱动力，准确地说，是各关节的输入力矩（力），分别记为 τ1、τ2、τ3 以及 τ4。

为了计算机器人各连杆的动能和势能，定义机器人各连杆的质心分别为 C1、C2、C3 和 C4，各连杆质心位置矢量分别为 p_C1、p_C2、p_C3 和 p_C4，如图所示。设各连杆的质量分别为 m1、m2、m3、m4，连杆 1 关于其关节轴的惯性矩为 I_1，连杆 i 关于其质心坐标系 Z轴的惯量矩为 I_ZZi。

第一步：建立运动学与坐标系

使用D-H参数法建立连杆坐标系，推导出正运动学，得到每个连杆坐标系相对于基座标系的变换矩阵 T0_i，以及各连杆质心的位置和速度。

第二步：计算系统的动能（T）和势能（V）

这是最关键的一步，需要逐连杆计算。

1. 计算动能：每个连杆的动能包括平移动能和旋转动能。

2. 计算势能：通常只考虑重力势能。设重力加速度矢量为 g，第 i个连杆质心在基座标系下的位置为 p_ci

第三步：构造拉格朗日函数并代入方程

将 L 代入拉格朗日方程，并进行繁琐但规范的求导运算，可以推导出标准形式的机器人动力学方程，机械臂动力学方程的一般形式为：

对于4轴SCARA，其完整的拉格朗日动力学模型如下：

# 动力学方程标准形式
M(q) * q_ddot + C(q, q_dot) * q_dot + G(q) + F(q_dot) = tau

# 1. 惯性矩阵 M(q) - 一个4x4的对称正定矩阵
# 它描述了系统加速度与所需惯性力之间的映射关系。
# 对于SCARA，其结构有大量零元素，体现了解耦特性。
M = [[M11, M12,   0,   0],  # M11, M12是θ1, θ2的耦合惯量
     [M12, M22,   0,   0],  # M22是关节2的等效惯量
     [  0,   0,  M33,  0],  # M33是Z轴平移质量，与旋转关节解耦
     [  0,   0,    0, M44]] # M44是末端旋转惯量

# 2. 科里奥利和向心力矩阵 C(q, q_dot) - 一个4x4矩阵
# 它包含了由速度引起的耦合项（科里奥利力）和向心力。
# 对于SCARA，科氏力主要存在于旋转关节θ1和θ2之间。
C = [[C11, C12, 0, 0],  # C11, C12是θ2速度和θ1, θ2速度乘积的函数
     [C21,   0, 0, 0],  # 体现了“转动关节1的运动会影响关节2的受力”
     [  0,   0, 0, 0],
     [  0,   0, 0, 0]]

# 3. 重力向量 G(q) - 一个4x1向量
# 它是势能V(q)对广义坐标的负梯度：G(q) = ∂V/∂q。
# SCARA的特点是重力主要作用于Z轴。
G = [0, 0, (m3 + m_tool)*g, 0]^T  # 只有第三关节（Z轴平移）受重力直接影响

# 4. 摩擦项 F(q_dot) - 实际系统中不可忽略
# 通常建模为：F(q_dot) = Fv * q_dot + Fc * sign(q_dot)

核心含义解读：

1. 惯性矩阵 M(q) 是对称正定的：这意味着无论机械臂处于什么姿势，从一个方向加速它所需的能量总是正的，这符合物理实际。
2. C(q,q˙) 的物理现象：当快速旋转SCARA的大臂（关节1）时，即使没有主动驱动小臂（关节2），小臂也会因为科里奥利力而感受到一个力矩，这个矩阵就是用来描述这种复杂的、由速度耦合产生的“感觉”。
3. 重力项 G(q)的独立性：在SCARA中，只有垂直方向的运动需要对抗重力，这是一个非常好的特性，使得平面内的运动控制可以得到简化。