机器人机械臂经典PID轨迹跟踪控制仿真

资 源 简 介

本项目基于MATLAB平台开发，旨在实现针对多自由度机械臂（通常以二自由度平面机械臂为例）的经典PID轨迹跟踪控制。系统首先利用拉格朗日动力学方程建立机械臂的非线性数学模型，包含惯性矩阵、科里奥利力及离心力项、重力项和摩擦项。核心控制逻辑采用独立关节PID控制策略，不依赖于复杂的模型前馈补偿，而是通过计算各关节期望轨迹与实际轨迹之间的位置误差，利用比例（P）、积分（I）、微分（D）三个环节计算出所需的驱动力矩。项目代码包括了被控对象模型文件、控制器算法实现文件以及主仿真脚本。在仿真过程中，系统能够处理正弦波跟踪或阶跃响应等任务，并通过修正PID增益系数（Kp, Ki, Kd）来演示参数对系统稳定性、收敛速度和稳态精度的影响。该项目能够完整展示从动力学建模到闭环控制实现的全部流程，验证经典PID算法在非线性机器人系统中的有效性与鲁棒性。

详 情 说 明

机器人机械臂经典PID轨迹跟踪控制仿真系统

1. 项目介绍

本项目是一个基于MATLAB开发的二自由度平面机械臂仿真系统。项目旨在通过非线性动力学建模与经典的PID控制算法，演示机械臂在特定轨迹下的跟踪性能。系统完整实现了从物理参数定义、动力学方程构建、控制回路设计到数值积分求解的全过程，并提供了丰富的数据可视化与动态仿真功能。

该项目不仅仅是一个简单的控制演示，更是一个包含惯性矩阵、科里奥利力、离心力、重力项以及摩擦力模型的完整物理仿真环境，展示了经典控制理论在处理非线性机器人系统时的有效性。

2. 功能特性

• 高保真动力学模型：基于拉格朗日方程建立二自由度机械臂动力学模型，详细计算了惯性矩阵 $M(q)$、科里奥利与离心力矩阵 $C(q, dot{q})$ 以及重力向量 $G(q)$。
• 摩擦力仿真：模型中明确引入了非线性摩擦项，包含粘滞摩擦（Viscous Friction）和库仑摩擦（Coulomb Friction），使仿真更贴近真实物理环境。
• 经典PID控制：采用独立关节PID控制策略，包含比例（P）、积分（I）、微分（D）三个环节，并实现了控制力矩的物理限幅（Saturation）。
• 高精度数值积分：未使用MATLAB内置的 ode45，而是手动实现了四阶龙格-库塔（Runge-Kutta 4th Order）算法进行系统状态更新，确保了仿真的可控性与透明度。
• 多维度可视化：

* 波形分析：提供位置跟踪对比、速度响应对比、跟踪误差收敛曲线以及控制力矩变化曲线。 * 动态演示：包含机械臂运动的实时动画，展示期望姿态（虚线）与实际姿态（实线）的对比，并绘制末端执行器的运动轨迹。

3. 系统环境要求

• MATLAB R2016b 或更高版本
• 不需要额外的工具箱（Toolbox），代码仅依赖MATLAB基础函数库。

4. 使用方法

1. 确保MATLAB当前工作路径包含 main.m 文件。
2. 直接运行 main 函数。
3. 系统将自动执行仿真计算，控制台会输出仿真进度。
4. 仿真结束后，会自动弹出三个数据分析图表窗口和一个机械臂运动动画窗口。

5. 代码实现与逻辑分析

本项目的核心逻辑全部封装在 main.m 文件中，采用了主函数加子函数的设计模式。

5.1 系统初始化与参数配置

在主程序入口处，代码定义了详细的物理参数和控制参数：

• 机械臂参数：定义了两连杆的质量（m1, m2）、长度（l1, l2）、质心距离（lc1, lc2）及转动惯量（I1, I2）。重力加速度设为 9.81。
• PID增益：采用对角矩阵形式定义 $K_p, K_i, K_d$，分别为关节1和关节2设置了独立的控制参数。例如关节1的 $K_p$ 为150，关节2为100。
• 仿真设置：总时长 10.0秒，步长 0.005秒。

5.2 仿真主循环

主循环（For Loop）是仿真的核心驱动引擎，主要执行以下步骤：

1. 状态获取：从状态向量中提取当前的关节角度和角速度。
2. 轨迹规划：调用 get_desired_trajectory 函数，根据当前时间 $t$ 生成期望的位置、速度和加速度。关节1规划为正弦运动，关节2规划为余弦运动。
3. 误差计算：计算期望轨迹与实际轨迹的偏差，并累加积分误差。
4. 控制器计算：根据 PID 公式 $tau = K_p e + K_i int e + K_d dot{e}$ 计算所需力矩，并对力矩进行 $pm 200 N cdot m$ 的限幅处理，模拟真实电机的输出限制。
5. 数值积分（RK4）：利用 robot_dynamics 函数计算系统状态导数，通过四阶龙格-库塔法更新下一时刻的系统状态（角度与角速度）。

5.3 动力学核心 (robot_dynamics 函数)

这是该项目中最关键的数学模型实现部分，它求解方程 $M(q)ddot{q} + C(q,dot{q})dot{q} + G(q) = tau - F$：

• 惯性矩阵 M：计算了包含耦合项的 $2 times 2$ 对称矩阵。
• C 矩阵：计算了科里奥利力和离心力项。
• 重力项 G：根据关节角度计算重力对各关节产生的力矩，假设重力沿Y轴负方向。
• 摩擦项 F：混合了与速度成正比的粘滞摩擦和与速度方向相关的库仑摩擦。
• 加速度求解：通过左除运算（M RHS）求解角加速度 $ddot{q}$，避免了直接求逆矩阵带来的数值稳定性问题。

5.4 轨迹生成 (get_desired_trajectory 函数)

实现了特定时间函数的轨迹规划：

• 关节1：执行周期为5秒的正弦波运动。
• 关节2：执行周期为5秒的余弦波运动。

该函数同时输出位置、速度和加速度的理论期望值，用于前馈控制（虽然本项目仅用了PID）或误差分析。

5.5 可视化模块 (plot_results 与 animate_robot)

• 数据绘图：

* 位置跟踪：子图分别展示关节1和关节2的期望角度与实际角度。 * 速度响应：展示角速度的跟踪情况。 * 误差与力矩：展示随着时间推移误差趋近于零的过程，以及控制力矩的输出波形。

• 动画仿真：

* 利用正运动学公式将关节空间坐标映射到笛卡尔空间。 * 使用“影子模式”（Shadow Mode）：同时绘制期望的机械臂（红色虚线）和实际的机械臂（蓝/绿色实线），以此直观展示滞后或超调现象。 * 绘制末端执行器的运动轨迹留痕。