基于MATLAB的二自由度机械臂PD控制

一、二自由度机械臂动力学建模

1. 物理参数定义

% 机械臂参数（单位：kg, m）
m1 = 1;    % 连杆1质量
m2 = 1;    % 连杆2质量
l1 = 1;    % 连杆1长度
l2 = 1;    % 连杆2长度
lc1 = 0.5; % 连杆1质心到关节1距离
lc2 = 0.5; % 连杆2质心到关节2距离
I1 = 0.1;  % 连杆1转动惯量
I2 = 0.1;  % 连杆2转动惯量
g = 9.81;  % 重力加速度

2. 拉格朗日方程推导动力学模型

机械臂动力学方程：

\(M(q)q¨+C(q,q˙)q˙+G(q)=τ\)

其中 \(q=[q1,q2]^T\)为关节角度，\(τ=[τ1,τ2]^T\)为控制力矩。

MATLAB函数实现动力学计算：

function [M, C, G] = robot_dynamics(q, dq)
    % 输入：关节角度q(2x1)，关节角速度dq(2x1)
    % 输出：惯性矩阵M(2x2)，科里奥利矩阵C(2x2)，重力向量G(2x1)
    
    global m1 m2 l1 l2 lc1 lc2 I1 I2 g
    
    q1 = q(1); q2 = q(2);
    dq1 = dq(1); dq2 = dq(2);
    
    % 惯性矩阵M
    M11 = m1*lc1^2 + m2*(l1^2 + lc2^2 + 2*l1*lc2*cos(q2)) + I1 + I2;
    M12 = m2*(lc2^2 + l1*lc2*cos(q2)) + I2;
    M21 = M12;
    M22 = m2*lc2^2 + I2;
    M = [M11, M12; M21, M22];
    
    % 科里奥利矩阵C
    C11 = -m2*l1*lc2*sin(q2)*dq2;
    C12 = -m2*l1*lc2*sin(q2)*(dq1 + dq2);
    C21 = m2*l1*lc2*sin(q2)*dq1;
    C22 = 0;
    C = [C11, C12; C21, C22];
    
    % 重力向量G
    G1 = (m1*lc1 + m2*l1)*g*cos(q1) + m2*lc2*g*cos(q1+q2);
    G2 = m2*lc2*g*cos(q1+q2);
    G = [G1; G2];
end

二、PD控制器设计

1. PD控制律

采用PD+重力补偿控制（消除稳态误差）：

\(τ=K_p(qd−q)+K_d(q˙d−q˙)+G(q)\)

其中 \(K_p=diag(k_{p1},k_{p2})\)，\(K_d=diag(k_{d1},k_{d2})\)为对角增益矩阵。

2. MATLAB实现控制器

function tau = pd_controller(q, dq, qd, dqd, ddqd)
    % 输入：实际关节角度q，实际角速度dq，目标角度qd，目标角速度dqd，目标角加速度ddqd
    % 输出：控制力矩tau
    
    global Kp Kd
    
    % PD控制部分（含重力补偿）
    [~, ~, G] = robot_dynamics(q, dq);  % 计算重力项
    e = qd - q;         % 位置误差
    de = dqd - dq;      % 速度误差
    tau = Kp*e + Kd*de + G;  % PD+重力补偿
end

三、参数整定与仿真设置

1. 控制参数初始化

% PD增益（需根据实际系统调整）
Kp = diag([100, 50]);  % 比例增益（关节1>关节2，因连杆1更长）
Kd = diag([20, 10]);   % 微分增益

% 目标轨迹（示例：关节1从0→π/2，关节2从0→π/4）
t_total = 5;          % 总仿真时间5s
dt = 0.01;            % 采样时间0.01s
t = 0:dt:t_total;     % 时间序列
N = length(t);

% 目标轨迹生成（平滑S型曲线）
qd1 = linspace(0, pi/2, N);       % 关节1目标角度
qd2 = linspace(0, pi/4, N);       % 关节2目标角度
dqd1 = gradient(qd1)/dt;           % 目标角速度
dqd2 = gradient(qd2)/dt;           % 目标角速度
ddqd1 = gradient(dqd1)/dt;         % 目标角加速度
ddqd2 = gradient(dqd2)/dt;         % 目标角加速度
qd = [qd1; qd2]; dqd = [dqd1; dqd2]; ddqd = [ddqd1; ddqd2];

2. 仿真主程序（数值积分求解动力学）

% 初始化状态变量
q = zeros(2, N);      % 实际关节角度
dq = zeros(2, N);     % 实际关节角速度
q(:,1) = [0; 0];      % 初始角度
dq(:,1) = [0; 0];     % 初始角速度

% 仿真循环（欧拉积分）
for i = 1:N-1
    % 当前状态
    q_curr = q(:,i);
    dq_curr = dq(:,i);
    
    % 计算控制力矩
    tau = pd_controller(q_curr, dq_curr, qd(:,i), dqd(:,i), ddqd(:,i));
    
    % 计算动力学（M, C, G）
    [M, C, G] = robot_dynamics(q_curr, dq_curr);
    
    % 求解加速度：M*ddq = tau - C*dq - G → ddq = M\(tau - C*dq - G)
    ddq = M \ (tau - C*dq_curr - G);
    
    % 更新速度和位置（欧拉积分）
    dq(:,i+1) = dq_curr + ddq * dt;
    q(:,i+1) = q_curr + dq(:,i+1) * dt;
end

四、仿真结果可视化

1. 关节角度跟踪曲线

figure;
subplot(2,1,1);
plot(t, qd(1,:), 'r--', t, q(1,:), 'b');
xlabel('时间(s)'); ylabel('关节1角度(rad)');
legend('目标', '实际'); title('关节1角度跟踪');

subplot(2,1,2);
plot(t, qd(2,:), 'r--', t, q(2,:), 'b');
xlabel('时间(s)'); ylabel('关节2角度(rad)');
legend('目标', '实际'); title('关节2角度跟踪');

2. 控制力矩曲线

figure;
plot(t(1:end-1), tau(1,:), 'r', t(1:end-1), tau(2,:), 'b');
xlabel('时间(s)'); ylabel('控制力矩(N·m)');
legend('关节1力矩', '关节2力矩'); title('控制力矩输出');

3. 跟踪误差分析

e1 = qd(1,:) - q(1,:);  % 关节1误差
e2 = qd(2,:) - q(2,:);  % 关节2误差

figure;
subplot(2,1,1);
plot(t, e1, 'g');
xlabel('时间(s)'); ylabel('关节1误差(rad)'); title('关节1跟踪误差');

subplot(2,1,2);
plot(t, e2, 'g');
xlabel('时间(s)'); ylabel('关节2误差(rad)'); title('关节2跟踪误差');

参考代码 二自由度机械臂PD控制 www.youwenfan.com/contentcnm/82819.html

五、扩展：Simulink模块化实现

若需图形化建模，可在Simulink中搭建如下模块：

1. 机械臂模型：用S-Function封装动力学函数 robot_dynamics。
2. PD控制器：用Gain模块实现 Kp、Kd，加法器组合误差信号。
3. 轨迹生成：用Signal Builder生成目标轨迹 qd(t)。
4. 示波器：连接关节角度、力矩信号，实时观察跟踪效果。

六、预期仿真结果

• 跟踪误差：关节角度误差应小于0.05 rad（约3°），稳态时趋近于0。
• 力矩曲线：启动阶段力矩较大（加速需求），稳定后接近0（仅需克服摩擦力）。
• 动态特性：上升时间约0.5~1 s，超调量小于10%（取决于 Kp/Kd比值）。