实验一 2dof（五连杆）建模¶
一、实验名称¶
2-DOF并联机械手运动学建模
二、实验目的¶
通过实际操作Matlab软件及应用多体动力学，通过构建系统运动副约束方程，掌握构建2-DOF并联机械手系统运动学模型的基本方法与步骤。
加深对机器人独立变量与非独立变量的物理意义，以及相应的剥离方法。
三、实验设备¶
装有Python (SymPy, NumPy, Matplotlib) 运行环境的计算机。 (注：实验要求中提及RobotStudio，本报告基于提供的Python代码进行建模原理阐述，该建模方法和原理同样适用于其他多体动力学仿真平台。)
四、实验原理（或实验方案）¶
本实验研究的2-DOF并联机械手是一个五杆闭环机构。我们将采用多体系统动力学中的方法，通过定义广义坐标和建立约束方程来描述系统的运动学特性。
1. 机构简图与坐标系¶
所研究的2-DOF并联机械手简化为一个平面五杆机构，如下图所示。
符号定义:
\(L_1, L_2, L_3, L_4\): 分别为连杆1、2、3、4的长度。
\(L_5\): 地面连杆 (机架) 的长度，即固定点 \(P_0\) 和 \(P_5\) 之间的距离。
\(P_0\): 固定转动副，坐标为 \((0, 0)\)。
\(P_5\): 固定转动副，坐标为 \((L_5, 0)\)。 (在代码中为
P3_p)\(P_1, P_2, P_3j\): 活动转动副。
\(a_1, a_2, a_3, a_4\): 分别为连杆1、2、3、4相对于全局X轴正方向的绝对转角（逆时针为正）。
\(R_i = \begin{pmatrix} r_{i1} \\ r_{i2} \end{pmatrix}\): 连杆 \(i\) (i=1,2,3,4) 的质心在全局坐标系下的坐标。
\(A_i = \begin{pmatrix} \cos(a_i) & -\sin(a_i) \\ \sin(a_i) & \cos(a_i) \end{pmatrix}\): 连杆 \(i\) 从局部坐标系到全局坐标系的旋转矩阵。
\(u_{ia}, u_{ib}, \dots\): 在连杆 \(i\) 局部坐标系中定义的从质心指向其端点的矢量。根据代码，连杆质心位于其几何中心：
\(u_{1a} = \begin{pmatrix} -L_1/2 \\ 0 \end{pmatrix}\) (从\(R_1\)指向\(P_0\)的局部矢量)
\(u_{1b} = \begin{pmatrix} L_1/2 \\ 0 \end{pmatrix}\) (从\(R_1\)指向\(P_1\)的局部矢量)
\(u_{2b} = \begin{pmatrix} -L_2/2 \\ 0 \end{pmatrix}\) (从\(R_2\)指向\(P_1\)的局部矢量)
\(u_{2c} = \begin{pmatrix} L_2/2 \\ 0 \end{pmatrix}\) (从\(R_2\)指向\(P_2\)的局部矢量)
\(u_{3c} = \begin{pmatrix} -L_3/2 \\ 0 \end{pmatrix}\) (从\(R_3\)指向\(P_2\)的局部矢量)
\(u_{3d} = \begin{pmatrix} L_3/2 \\ 0 \end{pmatrix}\) (从\(R_3\)指向\(P_{3j}\)的局部矢量)
\(u_{4c} = \begin{pmatrix} -L_4/2 \\ 0 \end{pmatrix}\) (从\(R_4\)指向\(P_{3j}\)的局部矢量)
\(u_{4d} = \begin{pmatrix} L_4/2 \\ 0 \end{pmatrix}\) (从\(R_4\)指向\(P_5\)的局部矢量)
2. 广义坐标的选取¶
为了完全描述系统的位形，我们选取以下12个变量作为广义坐标 \(q\): \(q = [a_1, a_2, a_3, a_4, r_{11}, r_{12}, r_{21}, r_{22}, r_{31}, r_{32}, r_{41}, r_{42}]^T\) 这是一个包含4个角度和4个质心（每个质心2个坐标）的向量。
3. 自由度分析¶
对于平面机构，其自由度 \(F\) 可由 Grübler 公式计算： \(F = 3(n-1) - 2j_1 - j_2\) 其中：
\(n\): 构件数量 (包括地面)。对于五杆机构， \(n=5\) (L1, L2, L3, L4, L5/地面)。
\(j_1\): 低副 (转动副或移动副) 的数量。机构中有5个转动副 (\(P_0, P_1, P_2, P_{3j}, P_5\))，所以 \(j_1=5\)。
\(j_2\): 高副数量。机构中无高副，所以 \(j_2=0\)。
因此，自由度为： \(F = 3(5-1) - 2(5) - 0 = 3 \times 4 - 10 = 12 - 10 = 2\) 该机构具有2个自由度，这与实验名称“2-DOF并联机械手”相符。
4. 约束方程的建立 \(C(q)=0\)¶
由于选取的广义坐标数量 (12个) 大于系统的自由度 (2个)，这些广义坐标之间并非完全独立，而是通过几何约束方程联系起来。对于该五杆机构，共有 \(12 - 2 = 10\) 个独立的标量约束方程。这些约束方程由各连杆的连接关系确定：
连杆1的A端 (与\(P_0\)连接的一端) 固定在原点 \(P_0 = (0,0)^T\): 定义 \(P_{1A}\) 为连杆1与 \(P_0\) 的连接点在全局坐标系中的位置。 \(P_{1A} = R_1 + A_1 u_{1a}\) 约束方程 \(C_1(q)\):
\[\begin{split} C_1(q) = R_1 + A_1 u_{1a} - P_0 = \begin{pmatrix} r_{11} \\ r_{12} \end{pmatrix} + \begin{pmatrix} \cos(a_1) & -\sin(a_1) \\ \sin(a_1) & \cos(a_1) \end{pmatrix} \begin{pmatrix} -L_1/2 \\ 0 \end{pmatrix} - \begin{pmatrix} 0 \\ 0 \end{pmatrix} = \begin{pmatrix} 0 \\ 0 \end{pmatrix} \end{split}\]展开得到两个标量方程： \(c_{1x}: r_{11} - \frac{L_1}{2} \cos(a_1) = 0\) \(c_{1y}: r_{12} - \frac{L_1}{2} \sin(a_1) = 0\)
连杆1的B端与连杆2的B端在点 \(P_1\) 重合: 定义 \(P_{1B}\) 为连杆1的B端位置，\(P_{2B}\) 为连杆2的B端位置。 \(P_{1B} = R_1 + A_1 u_{1b}\) \(P_{2B} = R_2 + A_2 u_{2b}\) 约束方程 \(C_2(q)\):
\[\begin{split} C_2(q) = P_{2B} - P_{1B} = (R_2 + A_2 u_{2b}) - (R_1 + A_1 u_{1b}) = \begin{pmatrix} 0 \\ 0 \end{pmatrix} \end{split}\]展开得到两个标量方程： \(c_{2x}: (r_{21} - \frac{L_2}{2} \cos(a_2)) - (r_{11} + \frac{L_1}{2} \cos(a_1)) = 0\) \(c_{2y}: (r_{22} - \frac{L_2}{2} \sin(a_2)) - (r_{12} + \frac{L_1}{2} \sin(a_1)) = 0\)
连杆2的C端与连杆3的C端在点 \(P_2\) 重合: 定义 \(P_{2C}\) 为连杆2的C端位置，\(P_{3C}\) 为连杆3的C端位置。 \(P_{2C} = R_2 + A_2 u_{2c}\) \(P_{3C} = R_3 + A_3 u_{3c}\) 约束方程 \(C_3(q)\):
\[\begin{split} C_3(q) = P_{3C} - P_{2C} = (R_3 + A_3 u_{3c}) - (R_2 + A_2 u_{2c}) = \begin{pmatrix} 0 \\ 0 \end{pmatrix} \end{split}\]展开得到两个标量方程： \(c_{3x}: (r_{31} - \frac{L_3}{2} \cos(a_3)) - (r_{21} + \frac{L_2}{2} \cos(a_2)) = 0\) \(c_{3y}: (r_{32} - \frac{L_3}{2} \sin(a_3)) - (r_{22} + \frac{L_2}{2} \sin(a_2)) = 0\)
连杆3的D端与连杆4的C’端 (代码中为C端) 在点 \(P_{3j}\) 重合: 定义 \(P_{3D}\) 为连杆3的D端位置，\(P_{4C'}\) 为连杆4的C’端位置。 \(P_{3D} = R_3 + A_3 u_{3d}\) \(P_{4C'} = R_4 + A_4 u_{4c}\) 约束方程 \(C_4(q)\):
\[\begin{split} C_4(q) = P_{4C'} - P_{3D} = (R_4 + A_4 u_{4c}) - (R_3 + A_3 u_{3d}) = \begin{pmatrix} 0 \\ 0 \end{pmatrix} \end{split}\]展开得到两个标量方程： \(c_{4x}: (r_{41} - \frac{L_4}{2} \cos(a_4)) - (r_{31} + \frac{L_3}{2} \cos(a_3)) = 0\) \(c_{4y}: (r_{42} - \frac{L_4}{2} \sin(a_4)) - (r_{32} + \frac{L_3}{2} \sin(a_3)) = 0\)
连杆4的D端固定在点 \(P_5 = (L_5, 0)^T\): 定义 \(P_{4D}\) 为连杆4的D端位置。 \(P_{4D} = R_4 + A_4 u_{4d}\) 约束方程 \(C_5(q)\):
\[\begin{split} C_5(q) = P_{4D} - P_5 = (R_4 + A_4 u_{4d}) - \begin{pmatrix} L_5 \\ 0 \end{pmatrix} = \begin{pmatrix} 0 \\ 0 \end{pmatrix} \end{split}\]展开得到两个标量方程： \(c_{5x}: (r_{41} + \frac{L_4}{2} \cos(a_4)) - L_5 = 0\) \(c_{5y}: (r_{42} + \frac{L_4}{2} \sin(a_4)) - 0 = 0\)
这5组矢量约束方程共同构成了10个标量约束方程 \(C(q) = [C_1(q)^T, C_2(q)^T, C_3(q)^T, C_4(q)^T, C_5(q)^T]^T = 0_{10 \times 1}\)。
5. 独立变量与非独立变量的剥离¶
由于系统有2个自由度，我们可以从12个广义坐标中选取2个作为独立变量 (或驱动变量)，记为 \(q_i\)。其余的 \(12-2=10\) 个广义坐标则为非独立变量 (或从动变量)，记为 \(q_d\)。独立变量的值可以自由指定（例如，通过电机驱动），而非独立变量的值则由约束方程 \(C(q_i, q_d)=0\) 唯一确定。
在提供的Python代码中，DRIVEN_JOINTS 参数允许用户从 \(\{a_1, a_2, a_3, a_4\}\) 中选择任意两个角度作为独立变量。例如，若选择 \(a_1\) 和 \(a_2\) 作为独立变量，则：
\(q_i = [a_1, a_2]^T\)
\(q_d = [a_3, a_4, r_{11}, r_{12}, r_{21}, r_{22}, r_{31}, r_{32}, r_{41}, r_{42}]^T\)
给定独立变量 \(q_i(t)\) 的瞬时值，可以通过求解非线性方程组 \(C(q_i(t), q_d) = 0\) 来得到对应的非独立变量 \(q_d(t)\)。这通常需要数值方法，如代码中使用的 scipy.optimize.fsolve (基于牛顿-拉夫逊法或其变种)。
6. 运动学方程（速度和加速度分析）¶
6.1 速度分析与雅可比矩阵¶
对约束方程 \(C(q)=0\) 关于时间 \(t\) 求一阶导数，得到速度级约束方程：
其中：
\(\dot{q} = \frac{dq}{dt}\) 是广义速度向量 (12x1)。
\(C_q = \frac{\partial C}{\partial q}\) 是约束雅可比矩阵 (10x12)。它的每一行是对应标量约束方程对所有广义坐标的偏导数。
将广义坐标 \(q\) 和广义速度 \(\dot{q}\) 划分为独立部分和非独立部分： \(q = \begin{pmatrix} q_i \\ q_d \end{pmatrix}\), \(\dot{q} = \begin{pmatrix} \dot{q}_i \\ \dot{q}_d \end{pmatrix}\) 相应地，雅可比矩阵 \(C_q\) 也可分块： \(C_q = \begin{bmatrix} \frac{\partial C}{\partial q_i} & \frac{\partial C}{\partial q_d} \end{bmatrix} = \begin{bmatrix} C_{q_i} & C_{q_d} \end{bmatrix}\) 其中 \(C_{q_i}\) 是 \(10 \times 2\) 矩阵，\(C_{q_d}\) 是 \(10 \times 10\) 矩阵。
速度级约束方程可写为： \(C_{q_i} \dot{q}_i + C_{q_d} \dot{q}_d = 0\) 假设 \(C_{q_d}\) 是非奇异的（即机构未处于奇异位形），则可以解出非独立速度 \(\dot{q}_d\) 与独立速度 \(\dot{q}_i\) 之间的关系：
这里，矩阵 \(J_v = -(C_{q_d})^{-1} C_{q_i}\) 称为速度传递雅可比矩阵，它建立了从独立（驱动）速度到非独立（从动）速度的线性映射。在代码中，这一步通过求解线性方程组 Cq_d_k * dq_d_k = -Cq_i_k * dq_i_k_vec 实现。
6.2 加速度分析与伽马项¶
对速度级约束方程 \(C_q \dot{q} = 0\) 再次对时间 \(t\) 求导，得到加速度级约束方程：
其中 \(\ddot{q} = \frac{d\dot{q}}{dt}\) 是广义加速度向量 (12x1)。 上式可整理为： \(C_q \ddot{q} = - \dot{C}_q \dot{q}\) 右端项通常被称为伽马项 \(\gamma\):
代码中 gamma_full = -gamma_term_jacobian_full * dq_all_sym，其中 gamma_term_jacobian_full = (Cq_full * dq_all_sym).jacobian(q_all_sym)，这与 \(\gamma\) 的定义一致。
将加速度向量 \(\ddot{q}\) 和雅可比矩阵 \(C_q\) 分块： \(C_{q_i} \ddot{q}_i + C_{q_d} \ddot{q}_d = \gamma\) 假设 \(C_{q_d}\) 非奇异，则非独立加速度 \(\ddot{q}_d\) 可由独立加速度 \(\ddot{q}_i\) 和当前系统的状态 \((q, \dot{q})\) 确定：
在代码中，这一步通过求解线性方程组 Cq_d_k * ddq_d_k = gamma_full_k - Cq_i_k * ddq_i_k_vec 实现。
通过以上步骤，我们建立了完整的2-DOF并联机械手运动学模型，包括位置约束、速度关系和加速度关系。这些方程是进行后续动力学分析和控制系统设计的基础。
五、实验步骤¶
熟悉机构与参数定义：
理解五杆机构的构型，包括各连杆的连接方式、长度参数 \((L_1, L_2, L_3, L_4, L_5)\)。
明确广义坐标的选取：四个连杆转角 \((a_1, a_2, a_3, a_4)\) 和四个连杆质心坐标 \((r_{11}, r_{12}), \dots, (r_{41}, r_{42})\)。
确定独立驱动关节 (例如，
DRIVEN_JOINTS = ['a1', 'a2'])。
符号推导运动学方程 (基于Python/SymPy)：
使用
sympy定义符号变量，包括时间 \(t\)，广义坐标 \(q_{all\_sym}\)，广义速度 \(dq_{all\_sym}\)，广义加速度 \(ddq_{all\_sym}\)，以及连杆长度 \(L_i\)。根据几何关系，建立5个矢量约束方程 (共10个标量约束方程) \(C(q) = 0\)。
计算约束雅可比矩阵 \(C_q = \frac{\partial C}{\partial q}\)。
计算加速度分析中的伽马项 \(\gamma = - \frac{\partial (C_q \dot{q})}{\partial q} \dot{q}\)。
将符号表达式转换为可进行数值计算的函数 (
lambdify)。
设置仿真参数：
定义连杆的实际数值长度。
设置仿真时间范围 \((t_{start}, t_{end})\) 和时间步长 \(dt\)。
为所选的独立驱动关节定义输入运动规律，即 \(q_i(t)\), \(\dot{q}_i(t)\), \(\ddot{q}_i(t)\)。
数值求解与仿真：
初始位置求解： 给定独立关节的初始角度 \(q_i(t_0)\)，使用数值求解器 (如
fsolve) 求解约束方程 \(C(q_i(t_0), q_d) = 0\)，得到非独立坐标的初始值 \(q_d(t_0)\)。循环求解： 在每个时间步 \(t_k\):
获取当前时刻独立关节的位移 \(q_i(t_k)\)、速度 \(\dot{q}_i(t_k)\) 和加速度 \(\ddot{q}_i(t_k)\)。
位置分析： 以 \(q_d(t_{k-1})\) 作为初猜，求解 \(C(q_i(t_k), q_d) = 0\) 得到 \(q_d(t_k)\)。更新完整的 \(q(t_k)\)。
速度分析： 利用 \(q(t_k)\) 计算 \(C_{q_i}(t_k)\) 和 \(C_{q_d}(t_k)\)。求解线性方程组 \(C_{q_d}(t_k) \dot{q}_d(t_k) = -C_{q_i}(t_k) \dot{q}_i(t_k)\) 得到 \(\dot{q}_d(t_k)\)。更新完整的 \(\dot{q}(t_k)\)。
加速度分析： 利用 \(q(t_k)\) 和 \(\dot{q}(t_k)\) 计算 \(\gamma(t_k)\)。求解线性方程组 \(C_{q_d}(t_k) \ddot{q}_d(t_k) = \gamma(t_k) - C_{q_i}(t_k) \ddot{q}_i(t_k)\) 得到 \(\ddot{q}_d(t_k)\)。更新完整的 \(\ddot{q}(t_k)\)。
存储当前时刻的所有运动学结果。
结果可视化与分析：
绘制各关节角度、角速度、角加速度随时间变化的曲线。
绘制特定点（如连杆质心）的运动轨迹。
生成机构运动的动画。
建模结果 (预期)¶
通过上述步骤，可以得到以下建模成果：
运动学约束方程组： 一组包含10个标量代数方程的 \(C(q)=0\)，精确描述了系统中12个广义坐标之间的几何关系。
约束雅可比矩阵 \(C_q\)： 一个 \(10 \times 12\) 的矩阵，其元素是约束方程对广义坐标的偏导数。该矩阵是进行速度和加速度分析的关键。
伽马项 \(\gamma\) 的表达式： 一个 \(10 \times 1\) 的列向量，用于加速度分析，包含了与速度相关的项。
独立与非独立变量间的运动传递关系：
速度层面：\(\dot{q}_d = -(C_{q_d})^{-1} C_{q_i} \dot{q}_i\)
加速度层面：\(\ddot{q}_d = (C_{q_d})^{-1} (\gamma - C_{q_i} \ddot{q}_i)\) 这些关系明确了从主动件运动到从动件运动的传递规律。
这些模型和公式构成了对2-DOF并联机械手进行运动学仿真和分析的数学基础。
六、实验数据¶
(此部分通常填写仿真中使用的具体参数值和得到的关键数据点或曲线图的引用。由于目前仅关注模型建立，此处从略。)
例如：
连杆长度：\(L_1 = 1.3m, L_2 = 0.6m, L_3 = 1.8m, L_4 = 1.8m, L_5 = 2.9m\)。
驱动关节：\(a_1, a_2\)。
\(a_1\) 输入运动：\(a_1(t) = 1.0 \cdot e^{-t/5.0} \cos(20t)\) rad。
\(a_2\) 输入运动：\(a_2(t) = (\pi/0.1)t + (\pi/10) \sin(300t)\) rad。
(仿真结果图表等)
七、实验结果与讨论¶
(此部分通常对仿真结果进行分析，并讨论模型的特点、局限性等。由于目前仅关注模型建立，此处从略。)
从多体动力学原理与机器人应用角度讨论机器人系统运动学建模的步骤：
机器人系统的运动学建模是理解和控制机器人运动的基础。从多体动力学原理出发，其一般步骤可概括如下：
明确机构构型与自由度：
首先，清晰定义机器人的机械结构，包括连杆、关节类型（转动副、移动副等）及其连接关系。
绘制机构简图，建立适当的坐标系（全局坐标系和连杆局部坐标系）。
通过Grübler公式或其他方法分析机构的自由度（DOF），这决定了独立驱动关节的数量。
选取广义坐标：
选择一组足以描述系统所有可能位形的独立参数作为广义坐标。对于复杂系统，可能会选择一组冗余的坐标，然后通过约束方程来关联它们。
常见的广义坐标包括关节角度、关节位移、连杆质心坐标、连杆姿态角等。
建立运动学约束方程：
基于机构的几何连接关系，建立代数约束方程 \(C(q)=0\)。这些方程表达了广义坐标之间的依赖关系。
对于闭环机构，约束方程尤为重要。例如，环路闭合条件、特定点的固定条件等。
位置分析：
给定独立（驱动）广义坐标 \(q_i\) 的值，通过求解非线性代数方程组 \(C(q_i, q_d)=0\) 来确定从动（非独立）广义坐标 \(q_d\) 的值。
这是运动学正解的一部分，即已知驱动关节的运动，求解机器人末端或其他关键点的位姿。
速度分析：
对约束方程 \(C(q)=0\) 对时间求导，得到速度级约束 \(C_q \dot{q} = 0\)。
\(C_q = \frac{\partial C}{\partial q}\) 是约束雅可比矩阵。
通过求解 \(C_{q_d} \dot{q}_d = -C_{q_i} \dot{q}_i\)，可以得到从动速度 \(\dot{q}_d\) 与驱动速度 \(\dot{q}_i\) 的关系。
雅可比矩阵在机器人速度分析、奇异位形判断、静力分析等方面有重要应用。
加速度分析：
对速度级约束再次对时间求导，得到加速度级约束 \(C_q \ddot{q} = \gamma\)，其中 \(\gamma = -\dot{C}_q \dot{q}\)。
通过求解 \(C_{q_d} \ddot{q}_d = \gamma - C_{q_i} \ddot{q}_i\)，可以得到从动加速度 \(\ddot{q}_d\) 与驱动加速度 \(\ddot{q}_i\) 的关系。
加速度分析是进行动力学分析的基础。
运动学逆解 (可选但重要)：
与正解相反，逆解是已知机器人末端执行器的期望位姿（和可能的姿态），求解所需的驱动关节值。这通常比正解更复杂，可能存在多解或无解的情况。
在机器人应用中的意义：
工作空间分析： 通过运动学模型可以确定机器人末端执行器能够到达的空间范围。
轨迹规划： 在关节空间或笛卡尔空间为机器人规划运动路径，确保平稳、无碰撞地到达目标。
仿真与验证： 在实际制造和部署机器人之前，通过运动学仿真可以验证设计、检测潜在问题。
控制系统设计： 运动学模型是机器人控制器（如PID控制器、计算力矩控制器等）设计的基础，用于精确控制机器人的运动。
奇异性分析： 通过雅可比矩阵可以识别机器人的奇异位形，在这些位形下机器人会失去某些方向的运动能力或产生极大的关节速度。
本实验通过对一个具体的2-DOF并联机械手进行建模，实践了上述通用步骤中的核心环节，特别是约束方程的建立和基于雅可比矩阵的速度/加速度分析，为更深入的机器人动力学研究和控制打下了基础。