简介这是一份面向机器人控制课程实验的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。对于平面二连杆所有关节都是转动关节sigma0。注意这里的0.45是连杆长度对应表1里的l1单位是米。实验文档里给的参数表要仔细对照杆1长0.45m杆2长0.55m但重心位置lc10.091m和lc20.105m是从关节轴线到质心的距离不是连杆长度的一半。质量m1在正文里写的是23.90kg摘要里写3.90kg这是原始资料里明显的笔误。实际仿真时建议以质量均匀、密度合理的数值为准但注意L{1}.m这个属性在link对象创建后是单独赋值的不会影响运动学参数。如果你用标准DHlink的第二个参数是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.xdrivebot函数可能被移除了但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 把模型嵌进Simulinkroblocks与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}_d0 )、一个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}_d0, \ddot{q}_d0 )前馈就退化成重力补偿。所以这个实验更常见的变体是给定正弦轨迹来验证前馈效果。Simulink搭建时需要额外三个Constant块分别给( q_d )、( \dot{q}_d )、( \ddot{q}_d )然后把这些信号接入一个逆动力学函数模块。逆动力学计算可以直接调用( M, C, G )的表达式但要注意输入维度。一个常见的错误是把前馈项里的( M(q_d) )算成标量矩阵了二自由度下它一定是2x2矩阵要和( \ddot{q}_d )向量相乘得到2x1力矩向量。3.4 Simulink控制框图搭建要点做实验时最容易在连线处出错。建议按照如下信号流组织期望位置Constant2x1矩阵输入到求和模块的正端。实际位置q从机器人模块的q输出口引出接到求和模块的负端构成关于位置的负反馈。速度信号qd从动力学模块引出乘以-Kd后接到力矩求和点。控制器输出力矩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} )。对关节1M对角线元素约( m_1l_{c1}^2 m_2l_1^2 I_1 I_2 \approx 23.90.00828 4.440.2025 1.51 \approx 0.1980.8991.51 2.6 )。取( K_p50 )则( 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增大到200Kd同样按系数缩放比如( 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控制器时的顺序是先把Kp设得非常小比如对角元素各为5Kd设为0看系统能不能稳定。如果发散说明模型或回环方向有问题先修正。逐渐增大Kp直到系统开始出现等幅振荡。记录这个Kp为临界增益。但二自由度系统是多变量振荡可能只在某个关节出现要分别判断。在临界增益的50%左右加入Kd。Kd从小到大直到响应曲线不超调或只超调一次。如果稳态误差不满足要求不要一味增大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控制实验升华了从关节空间控制延伸到操作空间控制。不过实验只要求位置控制这个技巧可以作为报告里的拓展内容显得有思考深度。本文还有配套的精品资源点击获取 二自由度机器人PD控制实战：从Robotics Toolbox建模到Simulink调参
简介这是一份面向机器人控制课程实验的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。对于平面二连杆所有关节都是转动关节sigma0。注意这里的0.45是连杆长度对应表1里的l1单位是米。实验文档里给的参数表要仔细对照杆1长0.45m杆2长0.55m但重心位置lc10.091m和lc20.105m是从关节轴线到质心的距离不是连杆长度的一半。质量m1在正文里写的是23.90kg摘要里写3.90kg这是原始资料里明显的笔误。实际仿真时建议以质量均匀、密度合理的数值为准但注意L{1}.m这个属性在link对象创建后是单独赋值的不会影响运动学参数。如果你用标准DHlink的第二个参数是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.xdrivebot函数可能被移除了但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 把模型嵌进Simulinkroblocks与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}_d0 )、一个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}_d0, \ddot{q}_d0 )前馈就退化成重力补偿。所以这个实验更常见的变体是给定正弦轨迹来验证前馈效果。Simulink搭建时需要额外三个Constant块分别给( q_d )、( \dot{q}_d )、( \ddot{q}_d )然后把这些信号接入一个逆动力学函数模块。逆动力学计算可以直接调用( M, C, G )的表达式但要注意输入维度。一个常见的错误是把前馈项里的( M(q_d) )算成标量矩阵了二自由度下它一定是2x2矩阵要和( \ddot{q}_d )向量相乘得到2x1力矩向量。3.4 Simulink控制框图搭建要点做实验时最容易在连线处出错。建议按照如下信号流组织期望位置Constant2x1矩阵输入到求和模块的正端。实际位置q从机器人模块的q输出口引出接到求和模块的负端构成关于位置的负反馈。速度信号qd从动力学模块引出乘以-Kd后接到力矩求和点。控制器输出力矩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} )。对关节1M对角线元素约( m_1l_{c1}^2 m_2l_1^2 I_1 I_2 \approx 23.90.00828 4.440.2025 1.51 \approx 0.1980.8991.51 2.6 )。取( K_p50 )则( 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增大到200Kd同样按系数缩放比如( 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控制器时的顺序是先把Kp设得非常小比如对角元素各为5Kd设为0看系统能不能稳定。如果发散说明模型或回环方向有问题先修正。逐渐增大Kp直到系统开始出现等幅振荡。记录这个Kp为临界增益。但二自由度系统是多变量振荡可能只在某个关节出现要分别判断。在临界增益的50%左右加入Kd。Kd从小到大直到响应曲线不超调或只超调一次。如果稳态误差不满足要求不要一味增大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控制实验升华了从关节空间控制延伸到操作空间控制。不过实验只要求位置控制这个技巧可以作为报告里的拓展内容显得有思考深度。本文还有配套的精品资源点击获取