1. 项目定位与整体设计思路1.1 为什么选迭代学习重复性任务的最优解双臂控制本身就是机器人领域里一个“看着不难、做起来一堆事”的方向。两条手臂一起搬东西、装配零件、打磨表面听起来只要把两个单臂的控制器拼在一起就行但真上手做会发现负载分配、内力约束、双臂同步精度全都会冒出来。我这次分享的项目是一个偏教学和验证性质的MATLAB仿真核心方法是迭代学习控制Iterative Learning ControlILC场景则是我自己搭的一个平面二连杆双臂搬运模型。先说清楚为什么选ILC而不是大家更熟悉的PID或者自适应控制。工业现场里大批量的装配、搬运、喷涂都是同一个轨迹一遍遍重复执行系统遇到的扰动往往也是重复性的比如重力、摩擦、固定的装配力。ILC的思路非常直接上一遍跑完发现哪个时间点偏了下一遍就在那个时间点多补偿一点跑几轮之后轨迹精度可以压得很好。这种“用重复性换精度”的思路在双臂这类强耦合、建模难度大的系统上尤其划算不需要精确的动力学模型也不需要在线辨识参数只要满足基本收敛条件几轮迭代就能看到明显改善。这个项目适合两类人看。一类是刚接触ILC、想在机器人臂上验证一下理论的同学可以把它当成一个能跑、能改、能复现的起步模板另一类是有单臂控制经验、想往双臂协同方向拓展的工程师可以通过这个例子理解“双臂协同很多时候可以拆成两个带耦合约束的轨迹跟踪问题”再决定要不要加阻抗控制、力控制那些更重的方案。1.2 双臂控制拆解从“双系统”到“双轨迹”很多人一听到双臂控制就默认要上主从控制、力位混合控制那一套其实做项目之前得先想清楚任务到底是什么。这个项目里我设计的场景是双夹持搬运左右臂末端同时夹住一个刚性负载沿着规划好的一条轨迹运动。这个任务有个关键性质——负载是刚性的两臂末端之间的相对距离基本不变因此只要两条手臂各自精准跟踪它们的那条末端轨迹配合关系自然就保证了。在这个前提下双臂控制就被拆成了一个“双轨迹跟踪”问题左右臂各自有各自的位置参考曲线ILC也各自独立工作。拆完之后系统结构非常清晰每个臂是一个两自由度的平面机械臂用角度作为输出关节力矩作为输入左右臂动力学参数略有差异用来模拟两侧负载不均或者关节磨损不一致的真实情况。这样做的意图是即使左右臂模型不一样ILC的迭代学习能力也应该能把两条轨迹的跟踪误差同时压下去这比两个完全相同的臂更有说服力也更贴近实际。1.3 方案选型为什么不用PID、自适应或鲁棒控制在做这个项目之前我其实先用传统PID在仿真里跑过一遍目的就是想看看ILC到底能强多少。PID在单次轨迹跟踪上能做到稳态误差但如果参考轨迹比较复杂比如包含多个加速减速段纯反馈就很难兼顾响应速度和超调量。你要把增益调大去追动态误差噪声就会被放大调小了动态跟踪又跟不上。这是反馈控制的通病不是调参能完全解决的。自适应控制和鲁棒控制也能处理模型不确定性但它们要么需要在线参数估计涉及稳定性分析和激励条件要么是在最坏情况下做保守设计控制量往往偏大。对搬运、装配这种重复性场景来说这两种方案都有点“杀鸡用牛刀”的感觉还增加了很多调试难度。ILC只需要一条初始控制序列和一组学习增益在重复运行中自动修正工程实现极其简单这正好符合“分享一个简单的迭代学习机器人双臂控制”这个项目名的定位。当然ILC也有自己的限制最典型的就是要求任务重复、初始状态一致这两个条件在后续章节我会分别展开讲。2. 双臂系统建模与ILC原理推导2.1 二连杆双臂动力学模型怎么搭仿真的基础是机械臂动力学模型。我不建议上来就套复杂的模型库先把一个最基本的二连杆模型搞清楚后面换成六轴甚至双臂双臂平台时思路是一样的。一个平面二连杆机械臂的动力学方程可以写成M(q)q̈ C(q, q̇)q̇ G(q) τ其中q是两关节角度向量M(q)是惯性矩阵C(q, q̇)是科氏力与离心力矩阵G(q)是重力项τ是关节输入力矩。这个方程描述的核心思想是驱动力矩一部分用来克服惯量、一部分用来补偿速度耦合项、一部分平衡重力。惯性矩阵的表达式是M(q) [m1l1² m2(l1² l2² 2l1l2cos(q2)) I1 I2, m2(l2² l1l2cos(q2)) I2; m2(l2² l1l2cos(q2)) I2, m2l2² I2]科氏力矩阵和重力项分别推导这里不展开太多公式MATLAB实现时会直接写成函数读者可以照着抄。有一点值得注意科氏力矩阵的取法不唯一不同教材差一个零空间项但仿真效果没区别选最简单的构造方式就行。重力项在平面臂里只跟cos(q1)、cos(q1q2)有关如果做的是水平面运动重力项可以去掉模型会更简单。左右臂我这里取的是不完全相同的参数左侧臂m11.0、m20.8右侧臂m11.2、m21.0。可能有人觉得差别不大但在高精度轨迹跟踪任务里这种细微差别会直接影响跟踪误差的量级刚好能检验ILC对不同对象的适应能力。2.2 迭代学习律设计P型、PD型到底怎么选ILC的核心表达非常简洁。假设第k次迭代的控制输入是u_k(t)跟踪误差是e_k(t) q_d(t) - q_k(t)那么经典的学习律可以写成u_{k1}(t) u_k(t) Γ_p · e_k(t) Γ_d · ė_k(t)这就是PD型迭代学习律。Γ_p和Γ_d是学习增益矩阵分别对应位置误差和速度误差的补偿权重。控制器只做了一件事把本次运行中“没跟住”的那部分误差信号以一定比例叠加到下一次的控制输入上。整个控制器不需要知道系统的精确模型这正是它工程实现门槛低的原因。这里有一个初学者特别容易踩的坑如果系统输出的是关节位置而输入是关节力矩系统的相对阶是2也就是位置信号不直接受输入影响输入要先经过两重积分才能体现到输出上。这种情况下纯P型ILC通常收敛性很差甚至发散因为位置误差和力矩之间隔了两层积分关系直接往控制量里加比例位置误差相位上就对不上。我一开始用P型ILC仿真误差不降反升折腾了很久才意识到要加误差的差分项也就是D型补偿用来间接反映速度误差。所以这个项目里用的是PD型学习律实际调试中也是它最稳。如果任务对收敛速度要求更高或者系统本身带有明显的输出延迟可以考虑加入二阶或高超前项的ILC但工程上PD型已经能覆盖绝大多数机械臂轨迹跟踪场景。2.3 收敛条件学习增益的选择依据ILC不是无脑增大增益就能收敛它有一条硬性的收敛条件。对线性时不变系统在PD型ILC下收敛条件可以写成频域形式核心是要求下面的范数小于1sup_ω |1 - Γ_p · G(jω)| 1其中G(jω)是系统传递函数。工程上的一个粗略理解是学习增益不能太大否则误差信号会被过度放大也不能太小否则收敛慢。实际调参时我会先用很小的Γ_p比如0.2左右再加上一个很小的Γ_d仿真观察误差变化效果不好再逐步增加。对单输入单输出系统还可以做一个更直观的推导。如果系统满足全局Lipschitz连续条件且学习增益满足||I - Γ · C · B|| 1那么迭代学习过程就会收敛。这里B和C是状态空间模型的输入和输出矩阵。这也是我经常推荐读者先跑一遍线性化模型、验证学习增益合理范围的原因线性模型算起来方便算出来的增益范围可以作为非线性模型的初始参考值。我这边实测中采样时间设置成0.01秒PD型学习增益Γ_p在0.3到0.8之间都能收敛Γ_d在0.01到0.05之间比较稳。增益超出这个范围误差曲线就会震荡甚至发散。具体数值后面代码部分会写清楚。3. MATLAB完整实现与代码解析3.1 工程结构与初始化整个仿真我拆成了三个文件逻辑比较清晰主程序ilc_dual_arm_main.m负责参数设置、参考轨迹生成、迭代循环和结果绘图动力学函数文件dual_arm_dynamics.m实现左右臂的动力学模型ILC控制器直接在主程序里实现不单独封装方便读者边看边改。主程序的初始化部分包含这几块内容左右臂的物理参数、运动学参数、控制周期、轨迹时长、迭代次数、初始状态。我把左右臂参数分别存成结构体方便后续函数调用。代码如下%% ilc_dual_arm_main.m 主程序 clear; clc; close all; % 时间与采样设置 T 2.0; % 单次运行时长秒 Ts 0.01; % 采样周期秒 t 0:Ts:T; N length(t); % 离散点数 % 左臂参数 pL.m1 1.0; pL.m2 0.8; pL.l1 0.5; pL.l2 0.4; pL.I1 0.02; pL.I2 0.015; pL.g 9.81; % 右臂参数与左臂不同模拟负载差异与关节差异 pR.m1 1.2; pR.m2 1.0; pR.l1 0.5; pR.l2 0.4; pR.I1 0.025; pR.I2 0.02; pR.g 9.81; % 初始状态保持左右臂初始关节角度一致 q0 [deg2rad(30); deg2rad(60)]; qd0 [0; 0];参考轨迹这里我选用的是一个带平滑过渡的多项式轨迹从初始位置运动到目标位置途中经过一个中途点。之所以不用简单的阶跃或者正弦是为了让轨迹包含持续的加减速过程这样ILC要补的误差特征更丰富也更能看出学习效果。轨迹生成代码如下%% 参考轨迹生成 q_ref zeros(2, N); q_ref(1, :) deg2rad(30) 20 * (t/T).^2 .* (3 - 2*(t/T)); q_ref(2, :) deg2rad(60) 10 * (t/T).^2 .* (3 - 2*(t/T));这段其实就是三次多项式插值位置从初始角平滑过渡到目标角可以直观理解为“关节从A点转到B点且起止速度为零”。3.2 动力学仿真函数实现动力学函数是我这个项目里最基础也最需要仔细核对的部分。函数输入是当前状态和力矩输出是状态导数然后由主程序里的RK4积分器推进。关于积分器选择MATLAB自带的ode45虽然精度高但有两点麻烦一是变步长会让离散时间点对不齐ILC需要每个时间点都能拿到对应的误差所以最好用固定步长积分器二是ILC每次迭代都要完整跑一遍仿真固定步长RK4计算效率更高也更容易定位问题。%% dual_arm_dynamics.m function dz dual_arm_dynamics(t, z, tau, p) q z(1:2); qd z(3:4); M inertia_matrix(q, p); C coriolis_matrix(q, qd, p); G gravity_vector(q, p); qdd M \ (tau - C*qd - G); dz [qd; qdd]; end function M inertia_matrix(q, p) q1 q(1); q2 q(2); l1 p.l1; l2 p.l2; m1 p.m1; m2 p.m2; I1 p.I1; I2 p.I2; M11 m1*l1^2 m2*(l1^2 l2^2 2*l1*l2*cos(q2)) I1 I2; M12 m2*(l2^2 l1*l2*cos(q2)) I2; M22 m2*l2^2 I2; M [M11, M12; M12, M22]; end function C coriolis_matrix(q, qd, p) q2 q(2); h -p.m2 * p.l1 * p.l2 * sin(q2); C [h*qd(2), h*(qd(1)qd(2)); -h*qd(1), 0]; end function G gravity_vector(q, p) q1 q(1); q2 q(2); l1 p.l1; l2 p.l2; m1 p.m1; m2 p.m2; G [(m1m2)*p.g*l1*cos(q1) m2*p.g*l2*cos(q1q2); m2*p.g*l2*cos(q1q2)]; end这里想提醒一个细节科氏力矩阵的构造方式很多我这个版本用了常见的反对称形式推导结果读者如果和自己平时用的形式不一样不用太纠结只要仿真里状态导数的数值合理、能量不发散模型基本没问题。防呆的办法是跑一次零输入自由落体仿真看看系统的能量变化是否正常。3.3 ILC主循环与学习律更新代码ILC实现的核心流程是初始化一条零控制序列然后循环迭代。每一次迭代都用当前控制序列驱动仿真得到实际轨迹计算误差再根据学习律更新控制序列。这里有一个容易被忽略的细节误差序列和控制序列的长度不同。仿真的输出状态有N个点但控制信号是零阶保持的每个采样周期内保持不变所以在计算误差差分时要用相邻两个采样点的误差差除以采样周期。我的实现里让控制序列长度同样为N但在最后一点的学习更新上做了边界处理保证序列不越界。下面是ILC主循环代码%% ILC迭代参数 gamma_p 0.5; % P型学习增益 gamma_d 0.02; % D型学习增益 iter_max 30; % 最大迭代次数 % 初始控制量设为重力补偿项 tau_L zeros(2, N); tau_R zeros(2, N); % 记录每次迭代的最大误差 err_L_max zeros(iter_max, 1); err_R_max zeros(iter_max, 1); for iter 1:iter_max % 左臂仿真 zL [q0; qd0]; z_out_L zeros(4, N); z_out_L(:, 1) zL; for k 1:N-1 zL rk4step(dual_arm_dynamics, t(k), zL, tau_L(:, k), Ts, pL); z_out_L(:, k1) zL; end qL z_out_L(1:2, :); % 右臂仿真 zR [q0; qd0]; z_out_R zeros(4, N); z_out_R(:, 1) zR; for k 1:N-1 zR rk4step(dual_arm_dynamics, t(k), zR, tau_R(:, k), Ts, pR); z_out_R(:, k1) zR; end qR z_out_R(1:2, :); % 计算误差 eL q_ref - qL; eR q_ref - qR; err_L_max(iter) max(max(abs(eL))); err_R_max(iter) max(max(abs(eR))); % PD型ILC学习律更新 for k 1:N-1 deL (eL(:, k1) - eL(:, k)) / Ts; deR (eR(:, k1) - eR(:, k)) / Ts; tau_L(:, k) tau_L(:, k) gamma_p * eL(:, k) gamma_d * deL; tau_R(:, k) tau_R(:, k) gamma_p * eR(:, k) gamma_d * deR; end % 末尾点直接用前一点的更新量近似 tau_L(:, N) tau_L(:, N-1); tau_R(:, N) tau_R(:, N-1); endRK4积分函数很短这里不单独贴了就是标准的四阶龙格库塔公式。整个主循环我实测在普通笔记本上跑30次迭代大概二十秒左右性能完全可以接受。有意思的是我给左右臂设置的初始控制量都是零这会导致第一次迭代时机械臂在重力作用下直接下垂误差大到离谱。但这正是ILC的魅力所在初始控制量不需要准确几轮迭代之后控制器会自己把轨迹“拉”回参考轨迹。3.4 参数设置与调参经验关于学习增益的选取我积累了一条比较实用的经验先用线性化模型做开环扫描再用非线性模型精调。具体做法是取工作点附近的线性化系统设置一组候选增益计算收敛判据||I - ΓCB||的数值从最小的增益开始往上扫找到满足条件并且有一定裕度的范围然后代入非线性模型跑3次迭代看最大误差是降还是升。如果降说明增益趋势对如果升说明接近边界了要往回退。% 简单的线性化检查示意需要根据实际模型替换A B C A_lin [0 0 1 0; 0 0 0 1; 10 0 0 0; 0 5 0 0]; B_lin [0 0; 0 0; 1 0; 0 1]; C_lin [1 0 0 0; 0 1 0 0]; G_lp C_lin * inv(eye(4) - (eye(4) A_lin*Ts)) * B_lin * Ts; % 检查 ||I - gamma * C*B|| 是否小于1 gamma_test 0.5; rho norm(eye(2) - gamma_test * C_lin * B_lin, 2); fprintf(谱半径指标: %.4f\n, rho);很多人一上来直接调非线性模型误差曲线发散就怀疑模型错了其实很多时候是收敛条件根本没满足。先跑一遍线性化检查能排除大量无效调试。调参顺序上我建议先调Γ_p等误差曲线呈现“持续下降然后平缓”的趋势后再微调Γ_d来压制高频抖动。如果你发现误差曲线一路下降但最后停在某个残差附近往往是学习增益太小或者存在系统性的常值扰动可以考虑增大Γ_p或者加入误差的积分项。4. 仿真结果分析与问题排查4.1 三次迭代看效果误差收敛曲线解读跑完30次迭代先看最大误差随迭代次数的曲线。我实测的结果是第一次迭代误差大约十几个度量级的量级因为初始力矩为零机械臂直接下垂第二次迭代误差明显缩小第三次更加缩小到第十次左右基本进入稳定平台。左右臂的收敛速度略有差异右臂由于质量大、惯性大同样增益下收敛稍慢但最终精度都能到10^(-3)弧度级别。如果再把第1次、第5次、第15次的关节角度轨迹画在一起对比可以非常直观地看到轨迹逐次逼近参考轨迹的过程。我强烈建议读者在仿真完成后把这三组曲线用不同颜色画在同一张图里这是理解ILC“逐次修正”特性的最佳方式。我第一次跑通的时候看到那个过程还挺激动的比什么收敛理论都直观。另外还要看控制力矩的曲线。ILC学习出来的控制力矩不完全是光滑的尤其是误差小到一定程度后控制量里会掺杂高频成分这是因为误差差分项放大了数值噪声。遇到这种情况通常对误差信号先做一次滑动平均滤波然后再送入学习律更新能明显改善控制量的平滑性。4.2 实操中常见的5个坑与解决办法我前后调试这个项目大概花了两天时间踩过的坑总结起来就五个这里直接整理成速查表现象可能原因解决办法误差发散或来回震荡Γ_p 或 Γ_d 过大不满足收敛条件降低学习增益用线性化模型先算收敛区间前几轮收敛、后面突然恶化初始状态未严格重置每次迭代起点不一致每次迭代前强制 q(0)q_ref(0)、q̇(0)0残差一直降不下去学习增益太小或存在常值扰动增大 Γ_p或加入误差积分项控制力矩高频抖振误差差分项放大数值噪声对误差做滑动平均滤波后再更新左右臂收敛速度差异大两侧动力学参数差异明显同一增益“偏科”左右臂分别调整增益不用强求相同这些坑里最阴间的是第二个初始状态未严格重置。ILC理论本身有个隐含假设就是每次迭代起点完全相同。如果某次迭代因为上一个状态残留导致初始位置偏了控制器会试图去补这个初始偏置结果就是前几轮好不容易学到的控制量被带偏误差曲线呈现先降后升的诡异形态。排查方法也很简单把每次迭代的初始状态打印出来看看就知道。4.3 从仿真到实机还要注意什么仿真跑通了别急着觉得万事大吉转到实体机器人之前有几个问题必须提前考虑。第一是力矩饱和。实际关节电机输出力矩有上限学习律更新出来的控制量如果在某个时间点超出饱和限制误差反而不降反升。解决办法是在学习律更新后强制做钳位tau_new max(min(tau_new, tau_max), -tau_max);。我建议在仿真阶段就把这一步加上否则实机阶段控制器行为会和仿真差很多。第二是模型不确定性。仿真里模型是精确的实机里模型参数、摩擦、间隙全都不同ILC虽然不依赖精确模型但初始控制的零力矩策略在实机上会让机械臂摔得很惨。实机上第一次运行至少要有一路反馈控制器兜底ILC负责在反馈基础上做前馈补偿工程上这叫“反馈辅助ILC”是工业界最常用的做法。第三是采样周期。仿真的采样周期可以随意设置实机受控周期受控制器频率限制学习增益的取值范围也会变。通常实机的采样周期比仿真长误差差分精度下降Γ_d的取值要相应调小。5. 后续扩展方向5.1 加入阻抗控制处理双夹持力这个项目里双臂是通过“各自精确跟踪同一组轨迹”来间接保证协同的。如果负载不是完全刚性或者两臂之间存在装配力约束单纯的位置控制会产生很大的内力轻则轨迹偏差重则损坏负载或机械臂。这时候就需要引入阻抗控制或者力位混合控制让双臂在保持位置精度的同时对接触力有一定的柔顺性。ILC在这种场景下依然有用只是学习的不再是纯力矩补偿而是“位置环力补偿”里的前馈部分。具体做法是外层用阻抗关系把期望力映射为位置修正量内层用ILC学习消除重复性的力跟踪误差。感兴趣的话可以在当前项目里先给负载加一个弹簧约束模拟两臂夹持一个柔性物体跑一遍纯位置控制和位置阻抗控制的对比效果差别会很明显。5.2 与优化工具箱结合自动调参学习增益Γ_p和Γ_d的手动调节虽然可用但最优性没有保障。MATLAB优化工具箱里的fminsearch或者全局优化算法可以在这里派上用场优化目标是整个迭代过程结束后轨迹误差的某个指标比如最大绝对误差的积分或者均方根误差。变量的维数不复杂就是两个增益目标函数则需要完整跑一次ILC迭代闭环所以单次优化会比较耗时。我实际跑过的经验是用fminsearch大概需要几百次迭代才能收敛到比较优的增益组合仿真跑得动但每次都要完整跑30轮ILC整体耗时十几分钟属于可以接受的离线调参成本。这样调出来的增益基本接近理论最优再用人工微调去处理实机差异就很快了。5.3 空间双臂与姿态误差的扩展平面二连杆模型验证完下一步自然是想把方法搬到空间六自由度双臂上。空间机械臂的动力学和运动学比平面复杂很多但ILC的控制结构不用变唯一需要特别处理的是姿态误差的表示。关节空间的误差交换很方便直接用角度误差就行。但如果你要在任务空间直接控制末端位姿姿态误差不能简单用欧拉角做线性加减因为三维旋转不在向量空间里。常见做法是把期望姿态和实际姿态的相对旋转矩阵转化成李代数向量也就是旋转向量形式的误差ILC的PD型学习律在这个误差向量上实施更新完再映射回旋转矩阵。这个思路在当前项目里可以先在平面内把末端笛卡尔位置作为输出跑一遍理解“工作任务空间的参考轨迹如何生成关节空间轨迹”之后再升级到三维会顺滑很多。我在实际使用中还有个比较深的体会ILC极其依赖参考轨迹的时间参数化。同样的期望路径不同的速度规划会导致学习效果差异很大尤其是轨迹曲率变化剧烈的地方ILC学出来的力矩曲线会跟着变抖。所以做双臂协同之前先花时间把轨迹规划做平滑比在控制器里纠结增益划算得多。这也是这个项目我故意选用五次多项式平滑轨迹的原因——把不必要的麻烦提前规避掉控制器的收敛性才能直观体现。