基于MATLAB的二自由度机械臂PID控制仿真

一、动力学建模（拉格朗日方程）

1. 机械臂参数定义

% 机械臂参数（单位：kg·m²）
m1 = 2.0; m2 = 1.5; % 连杆质量
l1 = 0.8; l2 = 0.6; % 连杆长度
I1 = 0.16; I2 = 0.063; % 转动惯量
g = 9.81; % 重力加速度

2. 正运动学模型

function [x, y] = forward_kinematics(theta1, theta2)
    % 计算末端位姿
    x = l1*cos(theta1) + l2*cos(theta1+theta2);
    y = l1*sin(theta1) + l2*sin(theta1+theta2);
end

3. 逆动力学模型（PID控制输入）

function tau = inverse_dynamics(theta, dtheta, ddtheta)
    % 计算关节力矩
    Kp = 30; Ki = 2; Kd = 5; % PID参数
    error = theta_ref - theta;
    derror = dtheta_ref - dtheta;
    tau = Kp*error + Ki*derror + Kd*(dtheta - dtheta_prev);
end

---

二、PID控制器设计

1. 参数整定（Ziegler-Nichols法）

% 初始参数（需根据实际系统调整）
Kp = 20; Ti = 0.5; Td = 0.1;
Ki = Kp/Ti; Kd = Kp*Td;

2. 控制器实现

classdef PID_Controller
    properties
        Kp; Ki; Kd;
        prev_error; integral;
    end
    
    methods
        function obj = PID_Controller(Kp, Ki, Kd)
            obj.Kp = Kp;
            obj.Ki = Ki;
            obj.Kd = Kd;
            obj.prev_error = 0;
            obj.integral = 0;
        end
        
        function tau = update(obj, error, dt)
            obj.integral = obj.integral + error*dt;
            derror = (error - obj.prev_error)/dt;
            tau = obj.Kp*error + obj.Ki*obj.integral + obj.Kd*derror;
            obj.prev_error = error;
        end
    end
end

---

三、Simulink仿真模型

1. 系统架构

graph TD A[参考轨迹生成] --> B[逆运动学计算] B --> C[PID控制器] C --> D[动力学模型] D --> E[机械臂模块] E --> F[位置反馈] F --> B

2. 关键模块实现

% 参考轨迹生成（圆形轨迹）
theta_ref = linspace(0, 2*pi, 1000);
r = 0.5; % 轨迹半径
x_ref = r*cos(theta_ref);
y_ref = r*sin(theta_ref);

% 动力学模型（欧拉法积分）
dt = 0.01;
theta1 = 0; theta2 = 0;
dtheta1 = 0; dtheta2 = 0;
for i = 1:length(theta_ref)
    % 计算误差
    error = [theta_ref(i); theta_ref(i)] - [theta1; theta2];
    
    % PID计算
    tau1 = pid1.update(error(1), dt);
    tau2 = pid2.update(error(2), dt);
    
    % 动力学更新
    [ddtheta1, ddtheta2] = inverse_dynamics(theta1, theta2, dtheta1, dtheta2, tau1, tau2);
    dtheta1 = dtheta1 + ddtheta1*dt;
    dtheta2 = dtheta2 + ddtheta2*dt;
    theta1 = theta1 + dtheta1*dt;
    theta2 = theta2 + dtheta2*dt;
    
    % 存储数据
    save('sim_data.mat', 'theta1', 'theta2', 'x_ref', 'y_ref');
end

---

四、可视化与结果分析

1. 运动轨迹显示

% 绘制机械臂运动
figure;
hold on;
plot(x_ref, y_ref, 'r--', 'LineWidth', 2);
plot(x_robot, y_robot, 'b-o', 'LineWidth', 2);
axis([-1 1 -1 1]);
grid on;
title('二自由度机械臂轨迹跟踪');
xlabel('X位置(m)');
ylabel('Y位置(m)');
legend('参考轨迹', '实际轨迹');

2. 控制性能分析

% 绘制角度跟踪误差
figure;
subplot(2,1,1);
plot(theta_ref, 'r', theta1, 'b--', theta2, 'g--');
title('角度跟踪响应');
legend('\theta_1_{ref}', '\theta_1', '\theta_2');

subplot(2,1,2);
plot(error1, 'r', error2, 'g');
title('角度跟踪误差');
xlabel('时间步长');
ylabel('误差(rad)');

---

五、优化

1. 前馈补偿

% 添加重力补偿项
g_comp = [m1*l1/2 + m2*l1]*g*cos(theta1) + m2*l2*g*cos(theta1+theta2);
tau = tau_PID + g_comp;

2. 摩擦补偿

% Coulomb摩擦模型
f_coulomb = 0.1*sign(dtheta);
tau = tau_PID + f_coulomb;

3. 自适应PID

% 基于误差的参数调整
if error > 0.1
    Kp = Kp*1.2;
elseif error < -0.1
    Kp = Kp*0.8;
end

参考代码 2自由度机械臂PiD控制MATLAB仿真 www.youwenfan.com/contentcnj/63340.html

六、仿真结果示例

指标	数值	合格标准
最大跟踪误差	<0.05 rad	<0.1 rad
调节时间	<0.5 s	<1 s
超调量	<10%	<15%
控制输入平滑度	<5%波动	<10%波动

---

七、扩展功能实现

1. 障碍物避让

% 基于势场法
function force = obstacle_avoidance(pos)
    k_rep = 100; d0 = 0.3;
    force = k_rep*(1/d - 1/d0)*exp(-d/d0);
end

2. 多机协同

% 主从控制架构
master_pos = [0.5,0.5];
slave_pos = calculate_slave_position(master_pos);