一、动力学建模（拉格朗日方程）
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_ControllerpropertiesKp; Ki; Kd;prev_error; integral;endmethodsfunction obj = PID_Controller(Kp, Ki, Kd)obj.Kp = Kp;obj.Ki = Ki;obj.Kd = Kd;obj.prev_error = 0;obj.integral = 0;endfunction 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;endend
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.1Kp = Kp*1.2;
elseif error < -0.1Kp = 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);