实验二 2dof（五连杆）仿真¶
一、实验名称¶
2-DOF并联机械手运动学仿真
二、实验目的¶
通过实际操作Matlab软件及应用多体动力学，基于系统运动副约束方程，掌握2-DOF并联机械手系统运动学模型求解基本方法与步骤。
熟悉Matlab语言编程，特别是系统约束非线性方程组求解函数与雅可比矩阵函数的调用与结果分析的技巧。
三、实验设备¶
装有Matlab软件运行环境的计算机。 (注：本报告基于提供的Python代码进行仿真原理阐述，其核心数值求解方法和步骤与在Matlab等其他科学计算环境中实现是相通的。)
四、实验原理（或实验方案）¶
本实验基于已建立的2-DOF并联机械手（五杆机构）的运动学模型进行数值仿真。该模型主要包括：
广义坐标 \(q\): 一个包含12个变量的向量，描述了系统的位形。 \(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\)
运动学约束方程 \(C(q) = 0\): 一组包含10个标量代数方程，描述了广义坐标间的几何关系。
速度级约束方程 \(C_q \dot{q} = 0\): 其中 \(C_q = \frac{\partial C}{\partial q}\) 是约束雅可比矩阵。
加速度级约束方程 \(C_q \ddot{q} = \gamma\): 其中 \(\gamma = -\dot{C}_q \dot{q}\) 是与速度相关的项。
由于系统的自由度为2，我们选取其中2个广义坐标（例如 \(a_1, a_2\)）作为独立变量 (驱动变量) \(q_i\)，其余10个为非独立变量 (从动变量) \(q_d\)。仿真过程的核心是：在给定驱动变量 \(q_i(t)\)、\(\dot{q}_i(t)\)、\(\ddot{q}_i(t)\) 随时间变化的规律后，通过数值方法求解非独立变量 \(q_d(t)\)、\(\dot{q}_d(t)\)、\(\ddot{q}_d(t)\)。
1. 仿真流程概述¶
仿真过程采用时间步进法，在每个离散的时间点 \(t_k\) 上依次求解位置、速度和加速度。
2. 位置分析 (Position Analysis)¶
在每个时间步 \(t_k\)，已知独立坐标 \(q_i(t_k)\)，需要求解非独立坐标 \(q_d(t_k)\)。这涉及到求解一个非线性代数方程组：
这是一个包含10个方程和10个未知数 (\(q_d\) 的分量) 的方程组。数值上，通常采用迭代法求解，如牛顿-拉夫逊法。在提供的Python代码中，scipy.optimize.fsolve 函数用于此目的。该函数需要一个目标函数 (在此为约束方程的残差 position_residuals_flexible) 和一个对 \(q_d\) 的初始猜测值 (通常使用上一时间步的解 \(q_d(t_{k-1})\))。
position_residuals_flexible(qd_unknown, qi_known, params_k, driven_indices, dependent_indices, num_all_coords):
该函数将已知的 \(q_i\) 和待求的 \(q_d\) 组合成完整的 \(q_{full}\)，然后代入已lambdify的约束方程 func_num(*q_full, *params_k) 并返回其结果（残差）。fsolve 会尝试找到使这个残差向量趋近于零的 qd_unknown。
3. 速度分析 (Velocity Analysis)¶
一旦位置 \(q(t_k)\) (包含 \(q_i(t_k)\) 和求解得到的 \(q_d(t_k)\)) 确定，就可以进行速度分析。已知独立速度 \(\dot{q}_i(t_k)\)，需求解非独立速度 \(\dot{q}_d(t_k)\)。 根据速度级约束方程 \(C_{q_i} \dot{q}_i + C_{q_d} \dot{q}_d = 0\)，可以得到：
这是一个关于 \(\dot{q}_d(t_k)\) 的线性方程组。 其中：
\(C_{q_d}\) 是约束雅可比矩阵 \(C_q\) 中对应于非独立坐标 \(q_d\) 的列组成的 \(10 \times 10\) 矩阵。
\(C_{q_i}\) 是约束雅可比矩阵 \(C_q\) 中对应于独立坐标 \(q_i\) 的列组成的 \(10 \times 2\) 矩阵。 这些雅可比矩阵在当前时刻 \(t_k\) 的值 \(C_q(q(t_k))\) 可以通过已lambdify的
Cq_full_num(*q_all_k, *param_vals_kin_num)计算得到，然后进行相应的列提取。 如果 \(C_{q_d}\) 非奇异（即机构未处于奇异位形），则可以直接求解该线性方程组得到 \(\dot{q}_d(t_k)\)。代码中使用np.linalg.solve(Cq_d_k, rhs_vel)。
4. 加速度分析 (Acceleration Analysis)¶
在位置 \(q(t_k)\) 和速度 \(\dot{q}(t_k)\) 都已知后，可以进行加速度分析。已知独立加速度 \(\ddot{q}_i(t_k)\)，需求解非独立加速度 \(\ddot{q}_d(t_k)\)。 根据加速度级约束方程 \(C_{q_i} \ddot{q}_i + C_{q_d} \ddot{q}_d = \gamma(q(t_k), \dot{q}(t_k))\)，可以得到：
这是一个关于 \(\ddot{q}_d(t_k)\) 的线性方程组。
右端的 \(\gamma\) 项 \(\gamma(q(t_k), \dot{q}(t_k))\) 可以通过已lambdify的 gamma_full_num(*q_all_k, *dq_all_k, *param_vals_kin_num) 计算得到。
同样，如果 \(C_{q_d}\) 非奇异，则可以求解该线性方程组得到 \(\ddot{q}_d(t_k)\)。代码中使用 np.linalg.solve(Cq_d_k, rhs_accel)。
通过在每个时间步重复这三个分析步骤，就可以得到整个仿真时间内所有广义坐标的位置、速度和加速度。
五、实验步骤¶
配置仿真环境与参数 (Python 代码
CONFIGURATION SECTION)：选择驱动关节 (
DRIVEN_JOINTS): 从连杆角度 \(\{a_1, a_2, a_3, a_4\}\) 中选择两个作为独立驱动关节。例如，DRIVEN_JOINTS = ['a1', 'a2']。设定连杆长度 (
L1_val至L5_val): 为机构的五个连杆指定数值长度。设定仿真时间 (
t_start,t_end,dt): 定义仿真的起始时间、结束时间和时间步长。定义输入运动参数 (
input_params): 为所有潜在的驱动关节（即使当前未被选为驱动）定义运动规律的参数。运动规律可以是角度、角速度、角加速度随时间变化的函数。代码中提供了一个灵活的函数get_input_motion，可以生成包含常数速度、正弦振荡和指数衰减的组合运动。
符号模型加载与数值化 (Python 代码
Symbolic Setup和Lambdify部分的成果)：程序首先进行符号推导（如上一份报告所述），得到约束方程
func、完整雅可比矩阵Cq_full和伽马项gamma_full的符号表达式。使用
sympy.lambdify将这些符号表达式转换为可高效进行数值计算的Python函数 (func_num,Cq_full_num,gamma_full_num)。这些函数接受广义坐标、广义速度（伽马项需要）和连杆长度作为输入。
初始化 (Python 代码
Run Numerical Simulation Loop初期)：时间向量生成: 根据
t_start,t_end,dt生成时间序列t_m。结果数组初始化: 为所有广义坐标的位置、速度、加速度创建存储数组 (例如
results[name+'_m'])。确定初始构型:
为所有广义坐标设置一个初始猜测值
q_all_guess_t0。其中，驱动关节的角度由其在 \(t_0\) 时刻的输入运动函数确定。提取非独立坐标的初始猜测值
q_d_guess。调用
fsolve(position_residuals_flexible, q_d_guess, ...)求解在 \(t_0\) 时刻的非独立坐标q_d(t_0)，以满足约束方程 \(C(q_i(t_0), q_d(t_0)) = 0\)。组合得到完整的初始位置向量 \(q(t_0)\) (代码中为
q_all_k)。
(可选) 计算初始速度:
获取驱动关节的初始速度 \(\dot{q}_i(t_0)\)。
计算 \(t_0\) 时刻的雅可比矩阵 \(C_q(q(t_0))\)，并分解为 \(C_{q_i}\) 和 \(C_{q_d}\)。
求解 \(C_{q_d} \dot{q}_d(t_0) = -C_{q_i} \dot{q}_i(t_0)\) 得到非独立关节的初始速度 \(\dot{q}_d(t_0)\)。
组合得到完整的初始速度向量 \(\dot{q}(t_0)\) (代码中为
dq_all_k)。
执行时间步进仿真循环 (Python 代码
Simulation Loop)： 对于时间序列t_m中的每一个时间点 \(t_k\) (从k=0到nn-1)：a. 获取驱动输入： 调用
get_input_motion(t_k, driver_name, input_params)函数，获取当前时刻 \(t_k\) 下两个独立驱动关节的位置 \(q_i(t_k)\) (即 \(a\_i\_k\))、速度 \(\dot{q}_i(t_k)\) (即 \(da\_i\_k\)) 和加速度 \(\ddot{q}_i(t_k)\) (即 \(dda\_i\_k\))。 将驱动关节的运动学量存入结果数组。b. 位置分析：
使用上一时刻的非独立坐标解 \(q_d(t_{k-1})\) (存储在
q_all_k[dependent_idx]) 作为当前时刻 \(q_d(t_k)\) 的初始猜测值。调用
fsolve(position_residuals_flexible, q_d_guess, args=(a_i_k, ...))求解非线性方程组 \(C(q_i(t_k), q_d) = 0\)，得到当前时刻的非独立坐标 \(q_d(t_k)\) (即sol)。如果求解失败 (
ier != 1)，则打印警告并可能中止仿真。更新完整的广义坐标向量 \(q(t_k)\) (即
q_all_k)，并将所有分量存入结果数组。
c. 速度分析：
使用当前时刻的 \(q(t_k)\) 和连杆长度参数，调用
Cq_full_num(*q_all_k, *param_vals_kin_num)计算完整的雅可比矩阵 \(C_q(t_k)\) (即Cq_full_k)。从
Cq_full_k中提取 \(C_{q_d}(t_k)\) (即Cq_d_k) 和 \(C_{q_i}(t_k)\) (即Cq_i_k)。计算右端项 \(b_v = -C_{q_i}(t_k) \dot{q}_i(t_k)\) (即
rhs_vel)。求解线性方程组 \(C_{q_d}(t_k) \dot{q}_d(t_k) = b_v\) 得到 \(\dot{q}_d(t_k)\) (即
dq_d_k)，使用np.linalg.solve。如果求解失败（例如 \(C_{q_d}\) 奇异），则打印错误并中止仿真。
更新完整的广义速度向量 \(\dot{q}(t_k)\) (即
dq_all_k)，并将所有分量存入结果数组。
d. 加速度分析：
使用当前时刻的 \(q(t_k)\)、\(\dot{q}(t_k)\) 和连杆长度参数，调用
gamma_full_num(*q_all_k, *dq_all_k, *param_vals_kin_num)计算伽马项 \(\gamma(t_k)\) (即gamma_full_k)。计算右端项 \(b_a = \gamma(t_k) - C_{q_i}(t_k) \ddot{q}_i(t_k)\) (即
rhs_accel)。求解线性方程组 \(C_{q_d}(t_k) \ddot{q}_d(t_k) = b_a\) 得到 \(\ddot{q}_d(t_k)\) (即
ddq_d_k)，使用np.linalg.solve。如果求解失败，则打印错误并中止仿真。
更新完整的广义加速度向量 \(\ddot{q}(t_k)\) (即
ddq_all_k)，并将所有分量存入结果数组。
e. 进度更新 (可选)： 定期打印仿真进度。
结果后处理与可视化 (Python 代码
Post-processing和Animation Setup,Display Static Plots)：数据整理: 检查仿真是否提前终止，如果是，则截断结果数组以匹配实际完成的步数。
静态绘图:
绘制驱动关节和从动关节的角度随时间变化的曲线。
绘制特定连杆质心的运动轨迹。
绘制驱动关节的运动学曲线（位移、速度、加速度）。
动画生成:
设置动画图形界面。
定义动画更新函数
update_anim(frame)：在每一帧，根据该帧对应的已存储的广义坐标 \(q(t_{frame})\)，计算各连杆端点和质心的位置，并更新绘图对象的显示。使用
matplotlib.animation.FuncAnimation创建动画对象。显示动画。
仿真结果 (预期输出)¶
执行上述仿真步骤后，程序会生成以下类型的结果：
数值数据：
存储在
results字典中的时间序列数据，包括所有12个广义坐标 \((a_1, \dots, r_{42})\) 及其一阶导数 (速度) 和二阶导数 (加速度) 在每个仿真时间步的值。fsolve函数的求解信息 (results['fsolve_info'])，可用于诊断收敛问题。
静态图表：
输入角度 vs 时间图： 显示所选驱动关节的角度随时间的变化。
从动角度 vs 时间图： 显示计算得到的两个从动关节的角度随时间的变化。
连杆质心轨迹图： 例如，连杆3和连杆4质心在XY平面上的运动路径。
驱动关节运动学图： 分别显示两个驱动关节的角度、角速度、角加速度随时间变化的曲线。
动态动画：
一个2D动画，实时展示五杆机构在仿真时间内的运动过程。动画会显示各连杆的连接、运动以及质心的位置变化。
这些结果可以用于分析机械手的运动特性，例如：
从动关节的运动范围和变化规律。
末端执行器（或特定点）的运动轨迹、速度和加速度。
机构是否会遇到奇异位形（表现为
np.linalg.solve失败或 \(C_{q_d}\) 的条件数过大）。不同驱动输入对整体运动的影响。
六、实验数据¶
仿真使用以下参数（示例，具体值见代码 CONFIGURATION SECTION）：
连杆长度 (Link Lengths):
\(L_1 = 1.3\)
\(L_2 = 0.6\)
\(L_3 = 1.8\)
\(L_4 = 1.8\)
\(L_5 = 2.9\) (地面)
仿真时间 (Simulation Time):
\(t_{start} = 0.0 s\)
\(t_{end} = 10.0 s\)
\(dt = 0.001 s\)
驱动关节 (Driven Joints):
例如
DRIVEN_JOINTS = ['a1', 'a2']
输入运动参数 (Input Motion Parameters for
a1anda2):a1:{'w': 20.0, 'amp': 1.0, 'decay': 5.0, 'vel': 0.0}\(a_1(t) = 1.0 \cdot e^{-t/5.0} \cos(20t)\)a2:{'w': 300.0, 'amp': np.pi/10, 'decay': 0.0, 'vel': np.pi/0.1}\(a_2(t) = (\pi/0.1)t + (\pi/10) \sin(300t)\)
(仿真得到的具体图表和动画是实验数据的核心部分，此处不重复粘贴，应引用实际运行程序生成的图像。)
七、实验结果与讨论¶
(此部分通常对仿真得到的具体图表进行详细分析，例如描述从动关节角度的变化趋势、质心轨迹的形状、速度/加速度曲线的特征，并解释这些现象。由于目前侧重于仿真步骤的描述，详细的结果分析从略。)
从多体动力学原理与机器人应用角度讨论机器人系统运动学仿真的步骤：
机器人系统运动学仿真是指在计算机上模拟机器人各组成部分在给定驱动输入下的运动行为，而不考虑引起运动的力或力矩。其核心是求解描述系统几何约束的代数方程以及由其导出的一阶（速度）和二阶（加速度）微分代数方程。一般步骤如下：
建立数学模型：
运动学描述： 这是仿真的前提。如上一份报告所述，包括定义广义坐标、建立约束方程 \(C(q)=0\)、推导雅可比矩阵 \(C_q\) 和伽马项 \(\gamma\)。这些构成了仿真的基础方程。
驱动定义： 明确系统的独立自由度（驱动关节），并为其设定随时间变化的运动规律 \(q_i(t), \dot{q}_i(t), \ddot{q}_i(t)\)。
选择数值求解器与设定仿真参数：
非线性方程组求解器： 对于位置分析 \(C(q_i, q_d)=0\)，需要选择合适的数值方法（如牛顿-拉夫逊法及其变种）。在Matlab中常使用
fsolve，Python中为scipy.optimize.fsolve。线性方程组求解器： 对于速度和加速度分析（均为形如 \(Ax=b\) 的线性系统），标准线性代数库函数即可（如Matlab的
\运算符，Python的numpy.linalg.solve）。时间步长 (\(dt\))： 选取合适的 \(dt\) 至关重要。过大会导致数值不稳定或精度不足；过小则增加计算时间。通常需要根据系统动态特性和求解器稳定性进行调整。
仿真时长 (\(t_{start}, t_{end}\)): 根据需要观察的运动周期或现象确定。
初始条件： 需要为系统的非独立坐标提供一个合理的初始猜测值，以便启动非线性求解器。有时也需要求解一致的初始速度。
初始化：
设定初始驱动状态： 计算 \(t=t_{start}\) 时驱动关节的 \(q_i(t_0), \dot{q}_i(t_0), \ddot{q}_i(t_0)\)。
求解初始构型： 利用非线性求解器，根据 \(q_i(t_0)\) 和约束方程 \(C(q)=0\) 求解出非独立坐标 \(q_d(t_0)\)，确保系统在初始时刻满足所有几何约束。这是非常关键的一步，不一致的初始条件会导致仿真失败。
(可选) 求解初始速度/加速度： 如果需要，可以进一步求解 \(\dot{q}_d(t_0)\) 和 \(\ddot{q}_d(t_0)\) 以确保初始状态的运动学一致性。
时间步进迭代求解： 这是仿真的核心循环。在每个时间步 \(t_k = t_{k-1} + dt\)：
更新驱动输入： 计算当前时刻驱动关节的 \(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)\) 和 \(\dot{q}_i(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)\)。
加速度分析： 利用已知的 \(q(t_k)\), \(\dot{q}(t_k)\) 和 \(\ddot{q}_i(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)\)。
数据存储： 保存当前时刻的 \(q(t_k), \dot{q}(t_k), \ddot{q}(t_k)\)。
结果处理与可视化：
数据提取与转换： 将原始数据转换为易于理解的物理量（如角度转为度，弧度/秒等）。
绘图： 生成关节变量、末端执行器轨迹、速度、加速度等随时间变化的曲线图。
动画： 创建机构运动的动态可视化，直观展示运动过程。
分析与验证：
检查物理合理性： 结果是否符合预期，有无突变、不连续等异常现象。
奇异性检测： 监控 \(C_{q_d}\) 矩阵的条件数或行列式，当接近奇异时，求解会变得困难或不稳定。
与理论或其他软件对比： 如果可能，将仿真结果与解析解（若存在）或商业多体动力学软件（如ADAMS, RobotStudio）的结果进行比较验证。
在机器人应用中的意义：
运动学仿真是机器人设计、分析和控制流程中不可或缺的一环：
设计验证： 在物理样机制造前，通过仿真验证机器人设计的运动学性能，如工作空间、可达性、关节运动范围等。
轨迹规划与验证： 规划机器人末端或关节的运动轨迹，并通过仿真验证轨迹的可行性、平滑性以及是否会导致奇异。
碰撞检测： 结合环境模型，仿真可以用于检测机器人运动过程中是否会与自身或其他物体发生碰撞。
控制算法开发与测试： 在将控制算法部署到实际机器人之前，可以在仿真环境中进行测试和调试，降低风险和成本。
人机交互研究： 仿真可以用于研究和优化人机协作任务中的运动协调。
教学与培训： 提供一个直观、可交互的平台来学习和理解机器人运动学原理。
本实验通过Python代码实现了一个完整的运动学仿真流程，从参数配置、初始条件求解到时间步进迭代和结果可视化，体现了多体系统运动学仿真的核心思想和技术步骤。