企业官网建设流程全解析

简介：这是一份面向机器人控制课程实验的PDF文档，解决二自由度机器人位置PD控制的建模与仿真问题。文档以Matlab语言、Simulink环境及Robot工具箱为基础，先给出二连杆的杆长、质量、质心、转动惯量等参数配置，再逐步讲解运动学模型和动力学模型的搭建方法，并针对控制器设计提供了简单PD控制率与PD加重力补偿两种方案，对比了稳态误差与算法复杂度。同时文档从M文件创建、drivebot动态位姿观察，到Simulink中连接robot模块与动力学函数的操作均有清晰说明，便于按步骤复现实验。包体内包含1个PDF文件，大小约245KB，篇幅紧凑、步骤完整，适合高校本科生或研究生在机器人控制实验中参考。目前已有1221人学习/下载，读者可通过Simulink搭建控制回路，调整比例、微分及重力补偿参数，直观观察二自由度机械臂的位姿响应，从而加深对PD控制原理的理解。

1. 二自由度机器人PD控制：先看清模型，再调增益

做机器人控制实验的人，最容易踩的坑不是控制器写不出来，而是模型和控制器对不上。这个实验用Matlab Robotics Toolbox建一个两连杆机械臂，再用Simulink搭PD位置控制，听着简单，但实际跑起来会发现：同样的Kp、Kd，别人曲线很漂亮，你的却抖得厉害，或者稳态误差大得离谱。原因往往不在控制器，而在你构建的连杆参数、动力学函数或者Simulink回环接法。本文把整个链条拆开讲清楚——从机器人工具箱里的link对象，到拉格朗日方程写出的M、C、G矩阵，再到PD、重力补偿、前馈补偿三种控制结构，最后落到Kp、Kd怎么调、响应曲线怎么判读。适合正在做机器人控制实验的本科生，也适合刚接触Robotics Toolbox的工程师，看一遍能少走很多弯路。

2. 用Robotics Toolbox搭建二连杆模型：从连杆参数到动力学M函数

2.1 连杆参数不是随便填的：标准DH和惯性参数

Robotics Toolbox里link([theta d a alpha sigma], 'standard')这行代码，很多人只记得顺序，忘了每个位置的含义。这里的[0 0.45 0 0 0]对应的是：关节转角theta、连杆偏距d、连杆长度a、连杆扭转角alpha、关节类型sigma。对于平面二连杆，所有关节都是转动关节，sigma=0。注意，这里的0.45是连杆长度，对应表1里的l1，单位是米。

实验文档里给的参数表要仔细对照：杆1长0.45m，杆2长0.55m，但重心位置lc1=0.091m和lc2=0.105m是从关节轴线到质心的距离，不是连杆长度的一半。质量m1在正文里写的是23.90kg，摘要里写3.90kg，这是原始资料里明显的笔误。实际仿真时，建议以质量均匀、密度合理的数值为准，但注意L{1}.m这个属性在link对象创建后是单独赋值的，不会影响运动学参数。如果你用标准DH，link的第二个参数是'standard'，这个参数在Robotics Toolbox 9.x和10.x版本里依然有效，不过新版推荐用SerialLink，但实验要求用老写法也完全能跑。

2.2 运动学模型M文件：从link到drivebot

实验要求把运动学模型写成名字首字母_twolink的M文件，核心代码就几行：

% 文件名：WJB_twolink.m % 构造连杆一 L{1} = link([0 0.45 0 0 0], 'standard'); L{1}.m = 23.9; % 质量 kg，注意和表1统一 L{1}.r = [0.091 0 0]; % 质心在连杆坐标系中的位置 [x y z] L{1}.I = [0 0 0; 0 0 0; 0 0 0]; % 惯性张量，这里先给零 L{1}.Jm = 0; % 电机转子惯量 L{1}.G = 1; % 减速比 % 构造连杆二 L{2} = link([0 0.55 0 0 0], 'standard'); L{2}.m = 4.44; L{2}.r = [0.105 0 0]; L{2}.I = [0 0 0; 0 0 0; 0 0 0]; L{2}.Jm = 0; L{2}.G = 1; % 创建机器人对象 WJB = robot(L); WJB.name = 'WJB_twolink'; qz = [0 0]; % 零位 qr = [0 pi/2]; % 参考位形

这里link函数的第二个参数'standard'表示使用标准DH参数。如果你用'modified'，坐标变换顺序会变，后面动力学计算的对角线元素都会变，所以不要混用。L{1}.I给的是3x3惯性张量，实验里为了简化直接给了零，但这样动力学仿真时转动惯量就只靠后面M函数里的I1、I2来补，要注意别重复计入。

写完M文件后，在命令窗口运行drivebot(WJB)，会弹出一个交互式图形界面，拖拽滑块就能看到二连杆在平面内的位形变化。这个功能本质上是调用SerialLink.plot，但drivebot的好处是直接绑定sldemo滑块，不需要自己写回调。如果你用的是Robotics Toolbox 10.x，drivebot函数可能被移除了，但teach函数可以替代，效果一样。

2.3 动力学模型M函数：拉格朗日方程里的M、C、G

二自由度机器人的动力学方程是：

[ M(q)\ddot{q} + C(q,\dot{q})\dot{q} + G(q) = \tau ]

实验文档里给的M函数把内部所有变量都直接写在函数体里，没有用link对象的动力学属性，原因是控制实验的核心是看控制器效果，而工具箱自带的rne函数在Simulink里封装后不好调试。自己写M函数有个好处，可以清楚看到每一项如何随关节角度变化。

% 文件名：WJB_dl.m function qdd = WJB_dl(u) % 输入u = [q1; q2; qd1; qd2; tau1; tau2] % 输出qdd = [qdd1; qdd2] q = u(1:2); qd = u(3:4); tau = u(5:6); g = 9.8; m1 = 23.9; m2 = 4.44; l1 = 0.45; l2 = 0.55; lc1 = 0.091; lc2 = 0.105; I1 = 1.27; I2 = 0.24; % 惯性矩阵M M11 = m1*lc1^2 + m2*(l1^2 + lc2^2 + 2*l1*lc2*cos(q(2))) + I1 + I2; M12 = m2*(lc2^2 + l1*lc2*cos(q(2))) + I2; M21 = m2*(lc2^2 + l1*lc2*cos(q(2))) + I2; M22 = m2*lc2^2 + I2; M = [M11 M12; M21 M22]; % 科氏力和向心力矩阵C C11 = -m2*l1*lc2*sin(q(2))*qd(2); C12 = -m2*l1*lc2*sin(q(2))*(qd(1) + qd(2)); C21 = m2*l1*lc2*sin(q(2))*qd(1); C22 = 0; C = [C11 C12; C21 C22]; % 重力项G G1 = (m1*lc1 + m2*l1)*g*sin(q(1)) + m2*lc2*g*sin(q(1)+q(2)); G2 = m2*lc2*g*sin(q(1)+q(2)); G = [G1; G2]; % 计算角加速度 qdd = inv(M) * (tau - G - C*qd);

这段代码最关键的是inv(M)。对于二连杆，M矩阵是2x2且对称正定，直接求逆没问题。但如果你想把它移植到更高自由度的机器人，不要用inv()，应该用M\b，数值稳定性更好。另外注意C矩阵的写法：这里把( C(q,\dot{q})\dot{q} )展开成两个元素，C11对应( c_1 )中的第一项，C12对应第二项，然后整体乘以qd。这个形式是根据拉格朗日方程推导出来的，不是随便凑的。很多人在这一步出错，导致仿真发散，可以检查C矩阵是否满足( \dot{M} - 2C )的斜对称性，但实验里不要求。

2.4 把模型嵌进Simulink：roblocks与Look Under Mask

在Matlab命令窗口输入roblocks，会打开一个机器人工具箱的Simulink模块库。把robot模块拖进模型，双击它，在Robot object栏填上你定义的机器人变量名，比如WJB。然后右键这个模块，选Look Under Mask，会看到内部结构——里面有一个S-Function，它调用了机器人的运动学函数，但你要把里面的S-Function替换成你自己的动力学M函数。

具体操作是：删掉原有的S-Function，换成Simulink标准库里的MATLAB Function模块，双击写入qdd = WJB_dl(u)。注意这个模块的输入u必须是一个向量，输出qdd是二元素向量。为了把这步做好，你的WJB_dl函数必须严格定义输入顺序。另一种常见做法是直接用Interpreted MATLAB Function，但那个模块每次仿真都要解释一遍，速度慢，而且不能直接处理矩阵。我一般用MATLAB Function，因为可以写成function qdd = WJB_dl(u)，内部再用coder.extrinsic调试，但实验里没必要那么复杂，直接写就行。

3. PD控制率的三种实现：从纯PD到重力补偿和前馈

3.1 纯PD控制：为什么稳态误差是必然的

最简单的关节空间位置控制就是：

[ \tau = K_p(q_d - q) - K_d \dot{q} ]

在Simulink里，这个式子用两个Constant块（一个给期望位置( q_d )，另一个给( \dot{q}_d=0 )）、一个Matrix Sum、两个Matrix Gain搭建。注意这里( -K_d\dot{q} )不是误差的微分，因为期望速度是零，所以直接用实际速度反馈的负值。如果你偷懒用Derivative模块对( q )求导，反而会把测量噪声放大，而且容易代数环。正确做法是把动力学模块输出的qd（即关节速度）直接引回来。

纯PD控制下，系统稳态时有( \dot{q}=0, \ddot{q}=0 )，代入动力学方程：

[ K_p(q_d - q) = G(q) ]

也就是说，只要重力项不等于零，稳态误差就满足( q_d - q = K_p^{-1}G(q) )。二连杆有两个关节，重力项和两个角度都有关，所以第一个关节的稳态误差不仅取决于自身重力，还受第二个关节角度影响。这就是为什么实验要求至少两组Kp、Kd比较——误差和负载有关，不是单纯调大Kp就能消除的。

3.2 PD加重力补偿：消除大部分稳态误差

改进的控制率：

[ \tau = K_p(q_d - q) - K_d\dot{q} + G(q) ]

这里的( G(q) )是当前角度下计算出的重力项。注意是当前角度，不是期望角度。如果设为( G(q_d) )，在跟踪变轨迹时会引入额外的力矩误差。在Simulink里实现时，你需要在控制器模块里也放置一个MATLAB Function计算G，或者复用动力学模块里的G输出。我习惯的做法是，把WJB_dl函数拆成WJB_G和WJB_MC两个函数，在控制器里调用WJB_G(u)，但实验为了快速验证，可以直接把G的计算复制到控制器模块里。

加重力补偿后，稳态时( \tau = K_p(q_d - q) )，如果系统无摩擦且模型准确，稳态误差理论上为零。但实际仿真里依然会有微小误差，原因是数值积分误差和增益有限。这时候可以通过增大Kp来进一步压缩误差，但Kp过大会导致系统刚度过高，响应振荡甚至失稳。

3.3 PD加前馈补偿：逆动力学模型全补偿

如果目标轨迹不是定值，而是连续变化的轨迹，那么重力补偿不够。需要前馈项包含M、C、G：

[ \tau = K_p(q_d - q) + K_d(\dot{q}_d - \dot{q}) + M(q_d)\ddot{q}_d + C(q_d,\dot{q}_d)\dot{q}_d + G(q_d) ]

注意前馈计算用的是期望轨迹的三要素( (q_d,\dot{q}_d,\ddot{q}_d) )，不是实际状态。这样设计的原因是，前馈项相当于给系统一个“开环引力”，让系统在跟踪轨迹时不需要完全靠反馈误差来产生力矩。实验里如果只做阶跃响应，( \dot{q}_d=0, \ddot{q}_d=0 )，前馈就退化成重力补偿。所以这个实验更常见的变体是给定正弦轨迹来验证前馈效果。

Simulink搭建时，需要额外三个Constant块分别给( q_d )、( \dot{q}_d )、( \ddot{q}_d )，然后把这些信号接入一个逆动力学函数模块。逆动力学计算可以直接调用( M, C, G )的表达式，但要注意输入维度。一个常见的错误是：把前馈项里的( M(q_d) )算成标量矩阵了，二自由度下它一定是2x2矩阵，要和( \ddot{q}_d )向量相乘得到2x1力矩向量。

3.4 Simulink控制框图搭建要点

做实验时最容易在连线处出错。建议按照如下信号流组织：

1. 期望位置Constant（2x1矩阵）输入到求和模块的正端。
2. 实际位置q从机器人模块的q输出口引出，接到求和模块的负端，构成关于位置的负反馈。
3. 速度信号qd从动力学模块引出，乘以-Kd后接到力矩求和点。
4. 控制器输出力矩tau接入动力学模块的tau输入口。

机器人模块内部的动力学MATLAB Function输出qdd，然后经过两个积分器得到qd和q。这里注意积分器初值要设成机器人的初始关节位置，否则仿真开始时会有一个瞬态跳变。如果你发现仿真一开始力矩很大，多半是积分器初值没和运动学模型的初始位形对齐。

另外，所有常数块里的值，如果是矩阵，必须在Constant块里写[kp1 0; 0 kp2]这种形式，而不是向量。因为K_p是增益矩阵，不是标量。很多人把Kp设成标量，然后直接乘误差向量，这样两个关节耦合在一起，控制效果会很怪。

4. 调参实战：Kp和Kd对响应曲线的影响与常见误区

4.1 初始参数选择和增益矩阵设置

实验要求至少给出两组PD参数对应的响应曲线。合理的起始点可以这么估：把每个关节看成独立的二阶系统，( M\ddot{q} + K_d\dot{q} + K_p(q_d-q) = 0 )。如果忽略耦合，那么等效阻尼比( \zeta = K_d/(2\sqrt{K_p M}) )。想得到临界阻尼，( K_d = 2\sqrt{K_p M} )。对关节1，M对角线元素约( m_1l_{c1}^2 + m_2l_1^2 + I_1 + I_2 \approx 23.90.00828 + 4.440.2025 + 1.51 \approx 0.198+0.899+1.51 = 2.6 )。取( K_p=50 )，则( K_d\approx 2*\sqrt{502.6}=2\sqrt{130}\approx22.8 )。所以一组基础参数可以是( K_p=\text{diag}(50,50) )，( K_d=\text{diag}(23,5) )，关节2的惯量小，Kd不需要那么大。

第二组参数可以取Kp增大到200，Kd同样按系数缩放，比如( K_p=\text{diag}(200,150) )，( K_d=\text{diag}(46,10) )，这样能看到响应更快但可能超调。注意Kd不是Kp的导数，而是独立参数，调大Kd可以抑制振荡，但会减慢响应速度。

4.2 响应曲线判读：超调、振荡、稳态误差和耦合

Scope里输出的是关节角度随时间的变化。给定期望( q_r=[0; \pi/2] )，实际响应分三段看：

• 上升阶段：关节2从0转到90度，关节1保持0度。如果关节1有明显波动，说明两个关节的动力学耦合很严重，M矩阵的非对角元素M12在关节2运动时对关节1产生反作用力矩。此时应该增加关节1的Kp或Kd，但更根本的办法是增加前馈补偿。
• 稳定阶段：纯PD控制下，两个关节都有稳态误差。如果关节1误差比关节2大，因为重力项G1更大。增大Kp可以减小误差，但误差与Kp成反比，不是线性趋零。
• 振荡阶段：如果曲线开始衰减振荡，说明Kd相对Kp太小。如果等幅振荡甚至发散，可能是积分器步长太大或者Kp过大。

实验报告里建议至少截取两组曲线比较。最有效的比较方式是画在同一张图里，用不同线型。Matlab里可以用plot(t, q(:,1))，然后hold on，再画另一组。注意Scope输出的数据是结构体格式，要先用to workspace模块或Scope的历史数据导出。

4.3 参数调整的步骤：从低增益到临界阻尼

我调这类PD控制器时的顺序是：

1. 先把Kp设得非常小，比如对角元素各为5，Kd设为0，看系统能不能稳定。如果发散，说明模型或回环方向有问题，先修正。
2. 逐渐增大Kp，直到系统开始出现等幅振荡。记录这个Kp为临界增益。但二自由度系统是多变量，振荡可能只在某个关节出现，要分别判断。
3. 在临界增益的50%左右，加入Kd。Kd从小到大，直到响应曲线不超调或只超调一次。
4. 如果稳态误差不满足要求，不要一味增大Kp，可以考虑增加重力补偿项。这也是实验内容之一，纯PD只是对照组。

一个实用技巧：把Kp和Kd的每个元素单独用一个Gain模块，而不是直接改矩阵值。这样你可以用tunable参数配合Simulink的Signal Editor做批扫，但实验阶段直接改Constant块数值更快。

4.4 常见坑：步长、代数环和矩阵维度

仿真时如果报“Algebraic loop detected”，通常是因为把控制器输出直接反馈到动力学模块，而动力学模块内部又直接用了控制力矩，中间没有经过积分器。检查方法：在力矩信号线上串一个Memory模块断开代数环。不过对于本实验，动力学是积分级联的，不应该出现代数环，除非你用了Derivative模块。

另一个常见问题是，MATLAB Function模块里写qdd = inv(M)*(tau - G - C*qd)时，输入u的顺序和Simulink信号线接入顺序不一致。如果提示维度错误，可以在模块里加一条size(u)检查。

仿真步长建议固定步长1ms或更小。变步长求解器在机器人模型里会因为积分器状态剧烈变化而自动加密步长，但碰到关节接近奇异位形时（比如二连杆完全伸直），M矩阵接近奇异，变步长可能卡死。固定步长配合ode4（四阶龙格库塔）在这个实验里完全够用。

5. 验证与进阶技巧：用drivebot观察位姿和量化控制性能

如果你只是看Scope曲线，很难直观感受二连杆的姿态变化。这里有个进阶技巧：在Simulink里把q信号引出来，通过To Workspace模块存成变量，然后在命令窗口调用drivebot(WJB)，再手动修改机器人的位置。但更直接的是用plot回放机器人动画：

% 假设仿真数据存在out.q和out.t中 t = out.t; % 时间向量 q = out.q; % 每一时刻的关节角度，Nx2矩阵 figure; for i = 1:length(t) WJB.q = q(i,:); % 直接修改机器人对象的角度 plot(WJB); % 这个命令会画出当前位姿 drawnow; end

这段代码能让你像看动画一样看到机械臂从初始位形运动到期望位形。如果你嫌循环太慢，可以每隔10帧画一次。注意plot(WJB)会每次清空图窗，如果想保留轨迹，可以先用held_on或者画一个静态的末端轨迹线。

量化控制性能时，除了看稳态误差，还可以计算误差的积分或RMS值。在Matlab里：

e = q - repmat([0, pi/2], length(q), 1); % 期望位形 [0, pi/2] rms_e = sqrt(mean(e.^2)); % 每个关节的均方根误差 max_overshoot = max(e(:,1)); % 关节1的最大超调量

对比两组PD参数时，用这些量化指标比肉眼看曲线更有说服力。实验报告里如果能附上误差RMS对比表，顺手还能分析出增益矩阵对角元与非对角元的影响。我做过一个经验总结：对于这个二连杆模型，关节2的响应速度比关节1快很多，因为其有效惯量小，所以如果想让两个关节同时到达，需要给关节1更大的Kp，而Kd要相应调大以保证稳定。这就是M矩阵对角元差异带来的直接后果。

另外，如果你对Robotics Toolbox比较熟，可以用WJB.jacob0(q)算出雅可比矩阵，然后通过( \dot{x} = J\dot{q} )在笛卡尔空间分析末端轨迹。这算是把PD控制实验升华了：从关节空间控制延伸到操作空间控制。不过实验只要求位置控制，这个技巧可以作为报告里的拓展内容，显得有思考深度。

本文还有配套的精品资源，点击获取