简介在机器人柔顺控制中阻抗控制通过调节位置偏差与接触力的动态关系使机械臂既能保证跟踪精度又能约束交互力。串联弹性执行器SEA在关节中引入弹性缓冲进一步提升碰撞安全性但系统自由度翻倍、非线性强耦合传统控制难以兼顾稳定性与性能。动态面控制以低通滤波替代虚拟控制量的解析求导有效简化高阶非线性系统的控制器设计自适应律在线估计连杆参数增强对负载变化的鲁棒性一阶变阻抗策略平滑调节刚度与阻尼适应自由空间与接触工况的切换。基于Simulink搭建SEA两杆机械臂仿真模型可实现上述算法的快速验证与参数调试。本文从动力学建模到控制律实现完整解析DSC-自适应-变阻抗的整合方案为协作机械臂及人形机器人关节的柔顺控制提供可落地的仿真框架。 最近我在调一个带SEA串联弹性执行器的两杆机械臂仿真目标很明确在末端和外界有接触的情况下既要保证跟踪性能又要让交互力可控。这套东西放在一起就是标题里那串关键词的组合——动态面控制、SEA、两杆、自适应、一阶变阻抗、阻抗控制。说实话这几个词单独拎出来都不算新但把它们串到一个系统里坑比想象中多。这篇博文想把整个方案的来龙去脉、建模过程、控制设计、Simulink实现还有我调参过程中踩过的雷一次性说清楚。这套方案适合谁参考如果你在做人形机器人关节、协作机械臂的柔顺控制或者刚接触SEA和阻抗控制想找一个“能落地”的仿真框架那这篇文章应该对你有用。我会从动力学模型一路讲到控制律推导再讲到仿真模块怎么搭、参数怎么给争取让没做过SEA的人也能顺着思路把系统搭出来。1. SEA驱动的两杆机械臂为什么阻抗控制非得绕这一个弯先说说为什么要在机械臂上装SEA。传统的刚性关节电机通过减速器直接驱动连杆优点是刚度高、带宽高但代价是一旦发生碰撞冲击力会在极短时间传递到整个结构上力矩传感器还没反应过来机械部分可能已经受伤了。SEA的思路很简单在减速器输出和连杆之间串一个弹性元件让力传递经过一个物理上的缓冲。你推它的时候它先压缩弹簧再推动连杆。这样一来碰撞瞬间的峰值力被大幅削低同时我们还能通过测量弹簧变形量直接估计关节力矩不需要额外装贵重的多维力传感器。但这个物理柔顺性是有代价的。系统的自由度翻倍了——原来刚性关节只需要知道电机转角一个状态现在你必须同时知道电机侧转角和连杆侧转角两者之间的差值就是弹簧变形量。两杆机械臂一共四个自由度每杆两个状态如果考虑速度就是八个状态。控制对象变成高阶、强耦合、非线性的系统普通的PID很难同时兼顾柔顺性和跟踪精度。这时候就需要阻抗控制登场。阻抗控制的本质不是直接去控制力或者位置而是控制“位置偏差”和“接触力”之间的动态关系。经典阻抗模型写作[ M_d \ddot{\tilde{q}}_l B_d \dot{\tilde{q}}_l K_d \tilde{q}l \tau{ext} ]其中 (\tilde{q}l q_l - q{ld}) 是连杆侧实际位置和期望任务位置之间的偏差(\tau_{ext}) 是外部施加的关节力矩。你可以把这个式子理解成一个虚拟的弹簧-阻尼-质量系统环境推你多大力你就让机械臂偏离期望位置多少偏离的形态由 (M_d, B_d, K_d) 决定。(K_d) 大关节“硬”跟踪精度高但接触力大(K_d) 小关节“软”接触时能退让但自由空间的跟踪就变差。传统做法是固定 (K_d) 和 (B_d)在“要精度”和“要柔顺”之间取一个折中。但实际任务往往不是单一工况机械臂空行程的时候希望硬一点、快速到位接触打磨的时候希望软一点、力稳定。这种需求催生了变阻抗控制也就是让目标刚度、目标阻尼随着交互状态在线调节。不过“变阻抗”不能乱变。我在初版仿真里直接把目标刚度从 500 跳到 50结果期望加速度瞬间出现一个尖峰整个系统直接发散。后来才意识到变阻抗的更新必须是有动态过程的最常见的就是加一阶惯性环节让刚度、阻尼平滑过渡——这就是标题里“一阶变阻抗”的实际含义。再来说说为什么是动态面控制。如果走传统的反步法backstepping设计SEA两杆系统的控制器第二步要对的虚拟控制量求符号导数那表达式会膨胀得非常恐怖。两杆系统的惯性矩阵带 (\sin(q_2))、(\cos(q_2)) 这些耦合项加上SEA多出来的状态手推四阶导数根本没法看。动态面控制的核心改进是在每一步虚拟控制量后面加一个一阶低通滤波器用滤波后的信号替代原信号从而避免对虚拟控制量进行解析求导。付出的代价是滤波误差但通过Lyapunov分析可以证明它和跟踪误差一起最终一致有界UUB滤波器时间常数取得越小这个界越小。实际仿真里只要时间常数取 0.01 到 0.05 之间效果就很接近反步法实现复杂度却降了一个量级。2. 动力学建模从连杆到SEA弹簧那一层的变量怎么处理动手写控制律之前必须先把被控对象的模型摆清楚。带SEA的两杆机械臂一般分成两个环节建模电机侧和连杆侧中间通过弹簧力矩 ( \tau_s K_s(q_m - q_l) ) 耦合。连杆侧动力学[ M_l(q_l)\ddot{q}_l C_l(q_l,\dot{q}_l)\dot{q}l G_l(q_l) K_s(q_m - q_l) \tau{ext} ]电机侧动力学[ M_m\ddot{q}_m B_m\dot{q}_m K_s(q_m - q_l) \tau_m ]这里 (q_l [q_{l1}, q_{l2}]^T) 是连杆侧角度(q_m [q_{m1}, q_{m2}]^T) 是电机侧角度(K_s \operatorname{diag}(K_{s1}, K_{s2})) 是弹簧刚度矩阵(\tau_m) 是电机输入力矩。(M_m) 通常取对角阵元素是电机转子折算之后的等效惯量(B_m) 是电机侧粘滞摩擦系数矩阵。有一个细节容易忽略(M_l(q_l))、(C_l(q_l,\dot{q}_l))、(G_l(q_l)) 这些量只和连杆侧状态有关不含 (q_m)。原因是我们把弹簧当作力矩源它给连杆的输入就是 (K_s(q_m - q_l))所以连杆侧的模型结构和刚性机械臂是一样的只不过把“关节驱动力矩 (\tau)”替换成了“弹簧力矩 (\tau_s)”。这个替换直接决定了后续控制器的设计思路我们没法直接控制连杆侧的关节力矩只能控制电机力矩把弹簧拧到某个期望变形量从而间接控制连杆侧。2.1 非线性项展开两杆机械臂的惯性矩阵 (M_l(q_l)) 的标准形式[ M_l(q_l) \begin{bmatrix} m_{11} m_{12} \ m_{21} m_{22} \end{bmatrix} ]其中 (m_{12} m_{21})且非对角项里含 (\cos(q_{l2})) 这种耦合项。科氏力和离心力矩 (C_l(q_l, \dot{q}_l)\dot{q}_l) 可以用Christoffel符号计算但实际写代码时直接用Lagrange方法对动能求导得到各项系数更不容易出错。对一个两杆系统计算量不大强烈建议先把 (M_l, C_l, G_l) 的符号表达式推出来再写进MATLAB Function而不是直接用数值微分。我见过有人为了省事用差分去估计 (M_l)结果每个控制周期都在做数值近似仿真一跑就飘调试半天才发现是模型精度问题。2.2 状态向量的组织方式做Simulink仿真状态向量顺序建议按照方便观察和排错的原则来[ x [q_{l1}, q_{l2}, \dot{q}{l1}, \dot{q}{l2}, q_{m1}, q_{m2}, \dot{q}{m1}, \dot{q}{m2}]^T ]需要特别提醒的是弹簧变形量 (q_m - q_l) 不是一个独立状态但它往往是后续变阻抗控制和力估计的关键信号。在仿真里要单独引出来观察后续如果要做什么“基于弹簧变形的力矩估计”直接用它就行不用额外装传感器模型。这一点在实验平台上也成立实际SEA关节一般装两个编码器分别测电机端和连杆端差值就是变形量。2.3 对控制设计的影响系统矩阵的阶数比刚性两杆翻了一倍这意味着控制器的输入通道(\tau_m)到被控输出(q_l)之间隔了一座“弹簧桥”。如果希望直接让连杆侧达到期望阻抗特性最简单的分层方法是外层根据阻抗模型计算连杆侧期望加速度 (\ddot{q}_{ld})积分得到期望速度、期望位置内层动态面控制器计算需要的弹簧力矩期望值 (\tau_{s,d})最内层由 (K_s(q_m - q_l) \to \tau_{s,d}) 反解出期望电机位置 (q_{md} q_l K_s^{-1}\tau_{s,d})再用电机侧的DSC跟踪这个期望电机位置。这样就把“连杆侧控制”和“电机侧控制”解耦了每一层都有明确的物理意义。实际仿真里每一步都很清晰和后面Simulink模块划分也能一一对应。3. 动态面控制设计比反步法少了哪一项为什么少动态面控制DSC这个名称里的“面”字指的是每个误差面error surface。反步法和动态面控制都是基于误差面逐层递推区别只在于虚拟控制量的导数怎么处理。反步法在第 (i) 步得到虚拟控制量 (\alpha_i) 后下一步必须计算 (\dot{\alpha}_i)这个导数会包含上一层的误差导数、模型导数、参考轨迹的导数表达式一层套一层指数级膨胀。在SEA两杆系统上即使只做到内力环三层手推(\dot{\alpha}_i)也要写满两页纸。动态面控制的做法是先把 (\alpha_i) 扔进一个一阶低通滤波器[ \tau_i \dot{x}{i1} x{i1} \alpha_i, \quad x_{i1}(0) \alpha_i(0) ]然后用滤波输出 (x_{i1}) 代替 (\alpha_i) 参与下一步计算而 (\dot{x}{i1}) 直接写成 ((\alpha_i - x{i1})/\tau_i)。这样每步只需要算代数运算不需要对 (\alpha_i) 做解析求导。代价是引入了一组滤波误差 (y_{i1} x_{i1} - \alpha_i)。在Lyapunov稳定性分析里这些滤波误差会作为额外的状态项出现最终结论是系统一致最终有界边界大小与滤波器时间常数 (\tau_i)、扰动上界、参考轨迹三阶导数的界有关。仿真中我们能直接感受到的规律是(\tau_i) 越小滤波误差越小跟踪越准但控制输入的噪声被放大(\tau_i) 越大虚拟控制越平滑但跟踪相位滞后越明显。我通常从 (\tau_i 0.02) 起步看力矩曲线再微调。3.1 误差面和虚拟控制量的设计具体到SEA两杆系统控制器分三层误差面第一层连杆位置跟踪误差[ s_1 q_l - q_{ld} ]这里 (q_{ld}) 是阻抗外层给出的期望连杆轨迹。取虚拟控制[ \alpha_2 \dot{q}_{ld} - c_1 s_1 ]经过滤波器得到 (x_2)作为连杆速度的期望值。第二层连杆速度误差[ s_2 \dot{q}_l - x_2 ]结合连杆侧动力学设计期望弹簧力矩[ \tau_{s,d} M_l(q_l)(\dot{x}_2 - c_2 s_2) C_l(q_l,\dot{q}_l)\dot{q}_l G_l(q_l) - \hat{D}(q_l,\dot{q}_l) ]其中 (\hat{D}) 是自适应项补偿模型不确定性后面第五节详细讲。这里的 (c_2 0) 是设计增益决定速度面收敛速度。第三层弹簧力矩到电机位置。由 (\tau_{s,d}) 直接解出期望弹簧变形[ \Delta_d K_s^{-1}\tau_{s,d} ]期望电机轨迹[ q_{md} q_l \Delta_d ]然后把 (q_{md}) 作为电机侧的位置参考再做一次DSC也就是电机侧误差面[ s_3 q_m - q_{md}, \quad s_4 \dot{q}_m - x_4 ]最终控制律[ \tau_m M_m(\dot{x}_4 - c_4 s_4) B_m\dot{q}_m K_s(q_m - q_l) ]这个式子看起来很简洁但它背后的信息量很大电机侧的控制只需要知道 (M_m, B_m)而(K_s(q_m - q_l)) 是弹簧力矩的实测值直接从状态计算。整个控制器把模型不确定性的重心从电机侧转移到了连杆侧——这正好是自适应控制要处理的地方。3.2 稳定性分析的粗略思路完整的Lyapunov函数是各误差面能量、滤波误差能量和参数估计误差能量的和[ V \frac{1}{2}s_1^T M_l s_1 \frac{1}{2}s_2^T M_l s_2 \frac{1}{2}s_3^T M_m s_3 \frac{1}{2}s_4^T M_m s_4 \frac{1}{2}\sum_i y_{i1}^2 \frac{1}{2}\tilde{\theta}^T \Gamma^{-1}\tilde{\theta} ]对时间求导后通过设计增益 (c_i) 大于某个与滤波器时间常数相关的下界可以保证 (\dot{V} 0) 在某个紧集外成立从而得到UUB稳定性。实际做仿真不需要把每一步推导都写出来但有一点必须记住控制增益 (c_i) 和滤波时间常数 (\tau_i) 不是互相独立的(\tau_i) 取得太小会让稳定性条件更苛刻表现为系统会在某组参数下发散但另一组参数下稳定不是没道理是稳定裕度不够。4. 一阶变阻抗怎么嵌进控制律刚度与阻尼的更新律阻抗控制的目标模型是二阶微分方程(M_d, B_d, K_d) 三个参数分别决定了期望阻抗的惯性、阻尼和刚度。在变阻抗控制里(B_d, K_d) 不再是常数而是随外部交互状态变化的量。问题来了变化本身不能是阶跃信号否则期望加速度会出现理论上的冲激数值仿真里等价于一步跳变系统马上炸。4.1 一阶变阻抗的实际含义这里说的“一阶变阻抗”第一层意思是目标刚度和目标阻尼的变化经过一个一阶惯性环节。给定期望的目标刚度 (K_d^{des})它由上层策略决定比如根据交互力大小调整实际用于阻抗模型的刚度[ \tau_K \dot{K}_d K_d K_d^{des} ]阻尼同理[ \tau_B \dot{B}_d B_d B_d^{des} ]这样即使上层策略给出的 (K_d^{des}) 瞬间从一个值跳到另一个值实际 (K_d(t)) 也会按指数形式平滑过渡物理上相当于给刚度变化本身加了一个“惯性”。(\tau_K, \tau_B) 的取值范围一般取10到20倍的内环控制周期不要太小。第二层意思可以理解为阻抗模型本身保持一阶导数的连续性。你可以把它看成位置误差、速度误差作为状态变量目标阻抗参数按一阶动力学更新整个外环在时间域上没有跳变。这也是为什么文献里常把这种方案叫“一阶变阻抗”而不是“多项式变阻抗”或别的。4.2 变阻抗策略的工程实现在仿真里我用的策略是交互力矩前馈调节[ K_d^{des} K_d^{min} \frac{K_d^{max} - K_d^{min}}{1 \beta |\tau_{ext}|} ]其中 (\tau_{ext}) 是外部交互力矩没有接触时约等于零此时 (K_d) 接近最大值保证自由空间的跟踪精度接触力增大时(K_d) 接近最小值关节变软降低接触力。(\beta) 是调节灵敏度的系数。阻尼的更新我直接跟刚度关联[ B_d 2\zeta\sqrt{M_d K_d} ]这样能保证不同刚度下系统都接近临界阻尼不要让阻抗模型出现欠阻尼振荡。(\zeta) 一般取 0.8 到 1.2。外部力矩 (\tau_{ext}) 怎么获取在纯仿真里可以直接从动力学方程算出来但在更接近实际的设置里建议用弹簧变形来估计(\hat{\tau}_{ext} \approx K_s(q_m - q_l))。注意这个估计在自由空间里不严格为零因为弹簧变形还包含惯性驱动力矩如果需要更准要把连杆侧动力学补偿掉但这些细节第一次搭仿真时可以以后再处理。4.3 阻抗外层如何参与控制变阻抗模型最终要输出一个期望加速度给内层DSC使用。把阻抗模型改写成[ \ddot{\tilde{q}}l M_d^{-1}(\tau{ext} - B_d \dot{\tilde{q}}_l - K_d \tilde{q}_l) ]于是期望连杆加速度[ \ddot{q}{ld} \ddot{q}{des} M_d^{-1}(\tau_{ext} - B_d \dot{\tilde{q}}_l - K_d \tilde{q}_l) ]其中 (q_{des}) 是任务层给出的名义参考轨迹(\tilde{q}l q_l - q{des})。积分两次得到 (q_{ld}, \dot{q}_{ld}) 后作为内层DSC的参考输入。这里有一个关键点阻抗外层的更新频率不需要和内层控制器一样但仿真里为了省事往往直接统一步长。如果你发现高频噪声影响变阻抗效果把外层的变阻抗参数更新单独放到一个采样率更低的子系统中计算效果会更好。这个分层采样率的设计思路在实验平台上更实用因为上位机通信带宽往往不够。5. 自适应项的实现细节估计什么、不估计什么、初始值怎么给SEA两杆系统的自适应控制听起来是把所有不确定参数一起估实际上好的做法是“能确定的就别让自适应去猜”。模型里哪些参数确定、哪些不确定要分清楚。5.1 参数线性化和回归矩阵把连杆侧动力学中的物理参数提取出来[ M_l(q_l)\ddot{q}_l C_l(q_l,\dot{q}_l)\dot{q}_l G_l(q_l) Y(q_l,\dot{q}_l,\ddot{q}_l)\theta ]这里 (\theta) 是由连杆质量、转动惯量、质心位置、摩擦系数组成的常值参数向量。(Y) 是只依赖状态和参考轨迹的回归矩阵。对两杆系统(\theta) 最多取6到8个参数就够两个连杆的质量、两个转动惯量、两个质心距离、两个额外的摩擦系数。如果你把质心的 (x,y) 坐标分开会有10个以上参数但真实情况下 (x,y) 坐标往往同时与质量和质心位置合力出现没必要拆开。实际仿真建模时我建议先不把 (\theta) 全部设为未知而是分成两部分确定项如电机惯量、弹簧刚度用真值直接用于控制器不确定项如连杆质量、末端负载由自适应律在线估计。这既符合工程直觉也能减小自适应压力。5.2 自适应律与稳定性耦合自适应律的标准形式[ \dot{\hat{\theta}} -\Gamma Y^T s_2 ]其中 (s_2) 是速度误差面(\Gamma) 是对角正定自适应增益矩阵。这里的 (s_2) 和动态面控制第二层用的误差信号是同一个这样自适应项才能和DSC的Lyapunov函数配合保证参数估计误差和跟踪误差耦合稳定。增益 (\Gamma) 的经验对角元素从 0.1 到 10 不等分别对应不同的物理参数不要所有参数都用同一个值。比如末端负载质量的估计增益可以取大一些因为它对控制性能影响大、需要快速跟上而摩擦系数的增益小一些因为摩擦项变化慢增益太大容易导致参数抖动。初始值怎么给如果你的负载是固定已知的初始值直接给真实值自适应根本不用启动系统也能稳定运行。但自适应存在的意义是负载变化。我的做法初始值给真实值的80%然后故意在一秒后突变负载相当于一个0.5kg的物体掉到末端观察参数估计能否在2秒内收敛到真实值附近。这种情况下自适应增益必须足够大否则末端负载改变后的稳态跟踪误差会一直存在。5.3 参数漂移和投影算子的必要性自适应最怕的是无激励情况下的参数漂移。机械臂在做微小运动、或者轨迹是常值时回归矩阵 (Y) 不满足持续激励条件参数估计在噪声的影响下会慢慢漂移。到后期估计的质心位置可能跑到一个物理上完全不可能的数值但控制仍然可能稳定——这更危险因为一旦参考轨迹突然变化这些离谱的参数会瞬间产生巨大的控制力矩。破解方法是在自适应律上加投影算子[ \dot{\hat{\theta}} \operatorname{Proj}{\Omega\theta}\left(-\Gamma Y^T s_2\right) ](\Omega_\theta) 是每个参数的允许范围比如连杆质量必须在0.5到10之间质心距离必须在0到杆长之间。Simulink里用MATLAB Function写一个投影函数就是在每步更新后检查参数是否越界越界则把梯度信号切到与边界相切的方向上。这个小改动看似不起眼但它能让仿真从“有时能跑”变成“一直能跑”。我强烈建议加上。6. Simulink搭建与调参从模块连线到数值发散排查模型和控制律都准备好之后就到了动手搭Simulink的环节。这个环节最容易浪费时间的地方不是控制律本身而是状态组织、代数环和数值发散。下面按我的习惯给出一种可复现的搭建方式。6.1 整体模块划分建议把整个仿真拆成五个部分期望轨迹生成模块生成名义参考轨迹 (q_{des}(t))比如正弦轨迹或S形点到点轨迹阻抗外层模块计算变阻抗后的期望加速度 (\ddot{q}{ld})积分得到 (\dot{q}{ld}, q_{ld})动态面控制器模块实现3.1节的控制律输出电机力矩 (\tau_m)自适应模块更新参数估计 (\hat{\theta})让控制器模块直接使用被控对象模块解算SEA两杆的8个状态微分方程。如果你的Simulink版本支持建议用MATLAB Function块写控制律避免一大张连线图。被控对象模块用S-Function或MATLAB Function都行我习惯用Level-2 MATLAB S-Function因为状态多的时候调试更方便每个状态都有明确的输出序号。6.2 被控对象模块的代码要点被控对象模块本质上是在解下面这个方程组[ \ddot{q}_l M_l^{-1}(q_l)\left[K_s(q_m - q_l) - C_l(q_l,\dot{q}_l)\dot{q}l - G_l(q_l) \tau{ext}\right] ][ \ddot{q}_m M_m^{-1}\left[\tau_m - B_m\dot{q}_m - K_s(q_m - q_l)\right] ]注意 (M_l^{-1}) 的求逆不要用符号求逆之后再代入数值直接用数值方法做2×2矩阵求逆效率高也稳定。每次计算求逆之前最好检查一下行列式是否接近0。两杆机械臂在 (\sin(q_{l2}) \approx 0) 的时候惯性矩阵会接近奇异也就是两个连杆折叠成一条直线时某些方向的运动变得不可控。仿真中如果参考轨迹会经过这种姿态要么避开要么在求逆时加一个小正则项[ M_l^{-1} \approx (M_l \epsilon I)^{-1} ](\epsilon) 取0.001到0.01之间代价是控制精度损失一点点但避免了数值爆炸。6.3 参数初始化表这是我最初跑通这套系统的初始化参数可以直接抄去试参数数值单位连杆1长度 (l_1)0.40m连杆2长度 (l_2)0.30m连杆1质量 (m_1)2.0kg连杆2质量 (m_2)1.0kg连杆1转动惯量 (I_1)0.04kg·m²连杆2转动惯量 (I_2)0.02kg·m²弹簧刚度 (K_{s1}, K_{s2})200, 150N·m/rad电机侧惯量 (M_m)diag(0.05, 0.04)kg·m²电机侧阻尼 (B_m)diag(1.0, 0.8)N·m·s/rad控制增益 (c_1, c_2, c_4)20, 20, 10-滤波时间常数 (\tau_2, \tau_4)0.02, 0.02s自适应增益 (\Gamma)diag(2, 2, 1, 1, 0.2)-期望质量 (M_d)diag(0.5, 0.5)kg·m²期望阻尼范围 (B_d)1.0~3.0N·m·s/rad期望刚度范围 (K_d)30~300N·m/rad注意这里把 (K_d) 的最小值设为30最大值300。如果你的任务接触力特别大可以把最大值再降一档但要注意自由空间跟踪精度也会变差。6.4 求解器设置求解器建议用固定步长ode4四阶龙格-库塔步长设0.001秒。变步长仿真在控制律含自适应、变阻抗这些强非线性环节时经常会因为局部误差估计过紧而把步长压到很小仿真速度慢到没法调参。固定步长配合ode4在大多数情况下精度足够而且行为可预测。如果你的模型里有开关环节或饱和非线性可以考虑ode3减少数值振荡。如果发现固定步长0.001下系统发散首先要排除的不是控制律而是代数环和采样时间问题。检查“代数环”是否断开建议在控制器输出和被控对象输入之间加一个单位Memory模块或者在MATLAB Function块里把输出延迟一步这能解决大部分一碰就飞的问题。6.5 变阻抗参数调节的方向感我调变阻抗参数时的经验是先固定 (K_d) 和 (B_d)把内层DSC和自适应调稳了再开变阻抗。开变阻抗时(\tau_K) 和 (\tau_B) 先给大一点0.1秒以上等系统稳定后逐步减小。如果 (\tau_K) 太小系统会出现一种很奇怪的振荡——低频大幅度来回摆不是高频抖动那是刚度变化跟不上接触力变化导致的。另一个容易踩的坑是阻抗外层的 (\ddot{q}{ld}) 直接积分两次得到位置积分饱和会导致期望轨迹漂移。仿真中如果长时间接触位置偏差会越积越大最终 (q{ld}) 漂到一个离谱的区域。解决办法是给积分器加限幅或者改用PD形式的阻抗模型而不是对加速度积分后者精度差一些但更稳。两种方案我都试过PD形式在Simulink里更好调但跟踪性能会打折扣取舍取决于你的任务。7. 我踩过的问题与改进方向7.1 弹簧刚度选择的两难SEA弹簧刚度的选取直接决定了整个系统的性能天花板。刚度太大SEA的柔顺优势消失碰撞力衰减不明显动态面控制器跟踪起来还算轻松刚度太小机械臂的带宽严重下降轻轻一碰就偏移一大截内环控制器再猛也拉不回来。我试过把 (K_s) 从300改到80系统的自然频率下降了一大截同样的控制增益下跟踪误差明显变大最后不得不把内环增益 (c_1, c_2) 调大但又引发了输入饱和。仿真时注意观察电机输入力矩是否频繁饱和如果饱和了说明弹簧刚度或者控制增益之间的匹配不合理。7.2 参数自适应和变阻抗打架自适应项在估计连杆参数变阻抗在调整目标刚度两者都会改变闭环动态如果调参顺序不对很容易互相打架。我遇到过一种情况参数估计在正常跟踪时收敛得好好的一旦进入接触阶段、变阻抗把 (K_d) 调小了参数估计突然开始发散。原因是接触阶段外部力矩 ( \tau_{ext} ) 进入了动力学方程但自适应模型里把末端负载也作为估计对象外部接触力被误认为负载变化的一部分。解决办法是把自适应项的目标限定在“自由空间模型不确定”上接触阶段暂停参数更新也就是根据交互力矩的大小做一个调度门控。Simulink里用Switch模块就能实现接触判断用 (|\tau_{ext}|) 是否超过阈值。7.3 离散化对DSC的影响Simulink连续仿真结果很好不等于控制器在真实硬件上也能工作。动态面控制的滤波器是连续时间的一阶惯性环节离散化之后滤波器时间常数必须远大于控制周期否则离散化误差会增大滤波误差项导致稳定性边界变差。我建议在做任何实机移植之前先把整个控制器改成本文之前提到的离散形式控制周期取1kHz滤波器时间常数至少取20个控制周期以上。初步测试发现同样的增益参数离散控制器的跟踪误差会比连续仿真大5%到10%这是正常现象不用过度慌张。7.4 后续可以扩展的方向这个框架的扩展性很好。一个方向是在阻抗外层加摩擦力矩观测器现在很多接触场景下用弹簧变形估计外部力矩已经够用但如果末端有额外的工具重力估计会偏很多另一个方向是变阻抗策略从查表式改成更聪明的方式比如用交互功率来决定刚度方向或者用强化学习在线调 (K_d, B_d)。还有一种更实用的改进把电机侧的行星减速器摩擦也纳入自适应范围减少SEA动态面的建模误差。这些方向我都试过一部分感觉最值得先做的还是力矩观测器因为它能直接提高变阻抗在接触力估计上的可靠性整个系统的鲁棒性会提升一个台阶。我自己调这套系统绕了不少弯路最深的体会是SEA两杆加变阻抗加自适应这种组合仿真想单次跑通几乎不可能但模块拆得清晰、参数按先固定后可变、先连续后离散的节奏来调其实每一步都是有迹可循的。上面这些经验如果能在你搭模型的时候帮你省下一两天调试时间这篇博文就没白写。本文还有配套的精品资源点击获取 SEA两杆机械臂的动态面控制与自适应变阻抗仿真
简介在机器人柔顺控制中阻抗控制通过调节位置偏差与接触力的动态关系使机械臂既能保证跟踪精度又能约束交互力。串联弹性执行器SEA在关节中引入弹性缓冲进一步提升碰撞安全性但系统自由度翻倍、非线性强耦合传统控制难以兼顾稳定性与性能。动态面控制以低通滤波替代虚拟控制量的解析求导有效简化高阶非线性系统的控制器设计自适应律在线估计连杆参数增强对负载变化的鲁棒性一阶变阻抗策略平滑调节刚度与阻尼适应自由空间与接触工况的切换。基于Simulink搭建SEA两杆机械臂仿真模型可实现上述算法的快速验证与参数调试。本文从动力学建模到控制律实现完整解析DSC-自适应-变阻抗的整合方案为协作机械臂及人形机器人关节的柔顺控制提供可落地的仿真框架。 最近我在调一个带SEA串联弹性执行器的两杆机械臂仿真目标很明确在末端和外界有接触的情况下既要保证跟踪性能又要让交互力可控。这套东西放在一起就是标题里那串关键词的组合——动态面控制、SEA、两杆、自适应、一阶变阻抗、阻抗控制。说实话这几个词单独拎出来都不算新但把它们串到一个系统里坑比想象中多。这篇博文想把整个方案的来龙去脉、建模过程、控制设计、Simulink实现还有我调参过程中踩过的雷一次性说清楚。这套方案适合谁参考如果你在做人形机器人关节、协作机械臂的柔顺控制或者刚接触SEA和阻抗控制想找一个“能落地”的仿真框架那这篇文章应该对你有用。我会从动力学模型一路讲到控制律推导再讲到仿真模块怎么搭、参数怎么给争取让没做过SEA的人也能顺着思路把系统搭出来。1. SEA驱动的两杆机械臂为什么阻抗控制非得绕这一个弯先说说为什么要在机械臂上装SEA。传统的刚性关节电机通过减速器直接驱动连杆优点是刚度高、带宽高但代价是一旦发生碰撞冲击力会在极短时间传递到整个结构上力矩传感器还没反应过来机械部分可能已经受伤了。SEA的思路很简单在减速器输出和连杆之间串一个弹性元件让力传递经过一个物理上的缓冲。你推它的时候它先压缩弹簧再推动连杆。这样一来碰撞瞬间的峰值力被大幅削低同时我们还能通过测量弹簧变形量直接估计关节力矩不需要额外装贵重的多维力传感器。但这个物理柔顺性是有代价的。系统的自由度翻倍了——原来刚性关节只需要知道电机转角一个状态现在你必须同时知道电机侧转角和连杆侧转角两者之间的差值就是弹簧变形量。两杆机械臂一共四个自由度每杆两个状态如果考虑速度就是八个状态。控制对象变成高阶、强耦合、非线性的系统普通的PID很难同时兼顾柔顺性和跟踪精度。这时候就需要阻抗控制登场。阻抗控制的本质不是直接去控制力或者位置而是控制“位置偏差”和“接触力”之间的动态关系。经典阻抗模型写作[ M_d \ddot{\tilde{q}}_l B_d \dot{\tilde{q}}_l K_d \tilde{q}l \tau{ext} ]其中 (\tilde{q}l q_l - q{ld}) 是连杆侧实际位置和期望任务位置之间的偏差(\tau_{ext}) 是外部施加的关节力矩。你可以把这个式子理解成一个虚拟的弹簧-阻尼-质量系统环境推你多大力你就让机械臂偏离期望位置多少偏离的形态由 (M_d, B_d, K_d) 决定。(K_d) 大关节“硬”跟踪精度高但接触力大(K_d) 小关节“软”接触时能退让但自由空间的跟踪就变差。传统做法是固定 (K_d) 和 (B_d)在“要精度”和“要柔顺”之间取一个折中。但实际任务往往不是单一工况机械臂空行程的时候希望硬一点、快速到位接触打磨的时候希望软一点、力稳定。这种需求催生了变阻抗控制也就是让目标刚度、目标阻尼随着交互状态在线调节。不过“变阻抗”不能乱变。我在初版仿真里直接把目标刚度从 500 跳到 50结果期望加速度瞬间出现一个尖峰整个系统直接发散。后来才意识到变阻抗的更新必须是有动态过程的最常见的就是加一阶惯性环节让刚度、阻尼平滑过渡——这就是标题里“一阶变阻抗”的实际含义。再来说说为什么是动态面控制。如果走传统的反步法backstepping设计SEA两杆系统的控制器第二步要对的虚拟控制量求符号导数那表达式会膨胀得非常恐怖。两杆系统的惯性矩阵带 (\sin(q_2))、(\cos(q_2)) 这些耦合项加上SEA多出来的状态手推四阶导数根本没法看。动态面控制的核心改进是在每一步虚拟控制量后面加一个一阶低通滤波器用滤波后的信号替代原信号从而避免对虚拟控制量进行解析求导。付出的代价是滤波误差但通过Lyapunov分析可以证明它和跟踪误差一起最终一致有界UUB滤波器时间常数取得越小这个界越小。实际仿真里只要时间常数取 0.01 到 0.05 之间效果就很接近反步法实现复杂度却降了一个量级。2. 动力学建模从连杆到SEA弹簧那一层的变量怎么处理动手写控制律之前必须先把被控对象的模型摆清楚。带SEA的两杆机械臂一般分成两个环节建模电机侧和连杆侧中间通过弹簧力矩 ( \tau_s K_s(q_m - q_l) ) 耦合。连杆侧动力学[ M_l(q_l)\ddot{q}_l C_l(q_l,\dot{q}_l)\dot{q}l G_l(q_l) K_s(q_m - q_l) \tau{ext} ]电机侧动力学[ M_m\ddot{q}_m B_m\dot{q}_m K_s(q_m - q_l) \tau_m ]这里 (q_l [q_{l1}, q_{l2}]^T) 是连杆侧角度(q_m [q_{m1}, q_{m2}]^T) 是电机侧角度(K_s \operatorname{diag}(K_{s1}, K_{s2})) 是弹簧刚度矩阵(\tau_m) 是电机输入力矩。(M_m) 通常取对角阵元素是电机转子折算之后的等效惯量(B_m) 是电机侧粘滞摩擦系数矩阵。有一个细节容易忽略(M_l(q_l))、(C_l(q_l,\dot{q}_l))、(G_l(q_l)) 这些量只和连杆侧状态有关不含 (q_m)。原因是我们把弹簧当作力矩源它给连杆的输入就是 (K_s(q_m - q_l))所以连杆侧的模型结构和刚性机械臂是一样的只不过把“关节驱动力矩 (\tau)”替换成了“弹簧力矩 (\tau_s)”。这个替换直接决定了后续控制器的设计思路我们没法直接控制连杆侧的关节力矩只能控制电机力矩把弹簧拧到某个期望变形量从而间接控制连杆侧。2.1 非线性项展开两杆机械臂的惯性矩阵 (M_l(q_l)) 的标准形式[ M_l(q_l) \begin{bmatrix} m_{11} m_{12} \ m_{21} m_{22} \end{bmatrix} ]其中 (m_{12} m_{21})且非对角项里含 (\cos(q_{l2})) 这种耦合项。科氏力和离心力矩 (C_l(q_l, \dot{q}_l)\dot{q}_l) 可以用Christoffel符号计算但实际写代码时直接用Lagrange方法对动能求导得到各项系数更不容易出错。对一个两杆系统计算量不大强烈建议先把 (M_l, C_l, G_l) 的符号表达式推出来再写进MATLAB Function而不是直接用数值微分。我见过有人为了省事用差分去估计 (M_l)结果每个控制周期都在做数值近似仿真一跑就飘调试半天才发现是模型精度问题。2.2 状态向量的组织方式做Simulink仿真状态向量顺序建议按照方便观察和排错的原则来[ x [q_{l1}, q_{l2}, \dot{q}{l1}, \dot{q}{l2}, q_{m1}, q_{m2}, \dot{q}{m1}, \dot{q}{m2}]^T ]需要特别提醒的是弹簧变形量 (q_m - q_l) 不是一个独立状态但它往往是后续变阻抗控制和力估计的关键信号。在仿真里要单独引出来观察后续如果要做什么“基于弹簧变形的力矩估计”直接用它就行不用额外装传感器模型。这一点在实验平台上也成立实际SEA关节一般装两个编码器分别测电机端和连杆端差值就是变形量。2.3 对控制设计的影响系统矩阵的阶数比刚性两杆翻了一倍这意味着控制器的输入通道(\tau_m)到被控输出(q_l)之间隔了一座“弹簧桥”。如果希望直接让连杆侧达到期望阻抗特性最简单的分层方法是外层根据阻抗模型计算连杆侧期望加速度 (\ddot{q}_{ld})积分得到期望速度、期望位置内层动态面控制器计算需要的弹簧力矩期望值 (\tau_{s,d})最内层由 (K_s(q_m - q_l) \to \tau_{s,d}) 反解出期望电机位置 (q_{md} q_l K_s^{-1}\tau_{s,d})再用电机侧的DSC跟踪这个期望电机位置。这样就把“连杆侧控制”和“电机侧控制”解耦了每一层都有明确的物理意义。实际仿真里每一步都很清晰和后面Simulink模块划分也能一一对应。3. 动态面控制设计比反步法少了哪一项为什么少动态面控制DSC这个名称里的“面”字指的是每个误差面error surface。反步法和动态面控制都是基于误差面逐层递推区别只在于虚拟控制量的导数怎么处理。反步法在第 (i) 步得到虚拟控制量 (\alpha_i) 后下一步必须计算 (\dot{\alpha}_i)这个导数会包含上一层的误差导数、模型导数、参考轨迹的导数表达式一层套一层指数级膨胀。在SEA两杆系统上即使只做到内力环三层手推(\dot{\alpha}_i)也要写满两页纸。动态面控制的做法是先把 (\alpha_i) 扔进一个一阶低通滤波器[ \tau_i \dot{x}{i1} x{i1} \alpha_i, \quad x_{i1}(0) \alpha_i(0) ]然后用滤波输出 (x_{i1}) 代替 (\alpha_i) 参与下一步计算而 (\dot{x}{i1}) 直接写成 ((\alpha_i - x{i1})/\tau_i)。这样每步只需要算代数运算不需要对 (\alpha_i) 做解析求导。代价是引入了一组滤波误差 (y_{i1} x_{i1} - \alpha_i)。在Lyapunov稳定性分析里这些滤波误差会作为额外的状态项出现最终结论是系统一致最终有界边界大小与滤波器时间常数 (\tau_i)、扰动上界、参考轨迹三阶导数的界有关。仿真中我们能直接感受到的规律是(\tau_i) 越小滤波误差越小跟踪越准但控制输入的噪声被放大(\tau_i) 越大虚拟控制越平滑但跟踪相位滞后越明显。我通常从 (\tau_i 0.02) 起步看力矩曲线再微调。3.1 误差面和虚拟控制量的设计具体到SEA两杆系统控制器分三层误差面第一层连杆位置跟踪误差[ s_1 q_l - q_{ld} ]这里 (q_{ld}) 是阻抗外层给出的期望连杆轨迹。取虚拟控制[ \alpha_2 \dot{q}_{ld} - c_1 s_1 ]经过滤波器得到 (x_2)作为连杆速度的期望值。第二层连杆速度误差[ s_2 \dot{q}_l - x_2 ]结合连杆侧动力学设计期望弹簧力矩[ \tau_{s,d} M_l(q_l)(\dot{x}_2 - c_2 s_2) C_l(q_l,\dot{q}_l)\dot{q}_l G_l(q_l) - \hat{D}(q_l,\dot{q}_l) ]其中 (\hat{D}) 是自适应项补偿模型不确定性后面第五节详细讲。这里的 (c_2 0) 是设计增益决定速度面收敛速度。第三层弹簧力矩到电机位置。由 (\tau_{s,d}) 直接解出期望弹簧变形[ \Delta_d K_s^{-1}\tau_{s,d} ]期望电机轨迹[ q_{md} q_l \Delta_d ]然后把 (q_{md}) 作为电机侧的位置参考再做一次DSC也就是电机侧误差面[ s_3 q_m - q_{md}, \quad s_4 \dot{q}_m - x_4 ]最终控制律[ \tau_m M_m(\dot{x}_4 - c_4 s_4) B_m\dot{q}_m K_s(q_m - q_l) ]这个式子看起来很简洁但它背后的信息量很大电机侧的控制只需要知道 (M_m, B_m)而(K_s(q_m - q_l)) 是弹簧力矩的实测值直接从状态计算。整个控制器把模型不确定性的重心从电机侧转移到了连杆侧——这正好是自适应控制要处理的地方。3.2 稳定性分析的粗略思路完整的Lyapunov函数是各误差面能量、滤波误差能量和参数估计误差能量的和[ V \frac{1}{2}s_1^T M_l s_1 \frac{1}{2}s_2^T M_l s_2 \frac{1}{2}s_3^T M_m s_3 \frac{1}{2}s_4^T M_m s_4 \frac{1}{2}\sum_i y_{i1}^2 \frac{1}{2}\tilde{\theta}^T \Gamma^{-1}\tilde{\theta} ]对时间求导后通过设计增益 (c_i) 大于某个与滤波器时间常数相关的下界可以保证 (\dot{V} 0) 在某个紧集外成立从而得到UUB稳定性。实际做仿真不需要把每一步推导都写出来但有一点必须记住控制增益 (c_i) 和滤波时间常数 (\tau_i) 不是互相独立的(\tau_i) 取得太小会让稳定性条件更苛刻表现为系统会在某组参数下发散但另一组参数下稳定不是没道理是稳定裕度不够。4. 一阶变阻抗怎么嵌进控制律刚度与阻尼的更新律阻抗控制的目标模型是二阶微分方程(M_d, B_d, K_d) 三个参数分别决定了期望阻抗的惯性、阻尼和刚度。在变阻抗控制里(B_d, K_d) 不再是常数而是随外部交互状态变化的量。问题来了变化本身不能是阶跃信号否则期望加速度会出现理论上的冲激数值仿真里等价于一步跳变系统马上炸。4.1 一阶变阻抗的实际含义这里说的“一阶变阻抗”第一层意思是目标刚度和目标阻尼的变化经过一个一阶惯性环节。给定期望的目标刚度 (K_d^{des})它由上层策略决定比如根据交互力大小调整实际用于阻抗模型的刚度[ \tau_K \dot{K}_d K_d K_d^{des} ]阻尼同理[ \tau_B \dot{B}_d B_d B_d^{des} ]这样即使上层策略给出的 (K_d^{des}) 瞬间从一个值跳到另一个值实际 (K_d(t)) 也会按指数形式平滑过渡物理上相当于给刚度变化本身加了一个“惯性”。(\tau_K, \tau_B) 的取值范围一般取10到20倍的内环控制周期不要太小。第二层意思可以理解为阻抗模型本身保持一阶导数的连续性。你可以把它看成位置误差、速度误差作为状态变量目标阻抗参数按一阶动力学更新整个外环在时间域上没有跳变。这也是为什么文献里常把这种方案叫“一阶变阻抗”而不是“多项式变阻抗”或别的。4.2 变阻抗策略的工程实现在仿真里我用的策略是交互力矩前馈调节[ K_d^{des} K_d^{min} \frac{K_d^{max} - K_d^{min}}{1 \beta |\tau_{ext}|} ]其中 (\tau_{ext}) 是外部交互力矩没有接触时约等于零此时 (K_d) 接近最大值保证自由空间的跟踪精度接触力增大时(K_d) 接近最小值关节变软降低接触力。(\beta) 是调节灵敏度的系数。阻尼的更新我直接跟刚度关联[ B_d 2\zeta\sqrt{M_d K_d} ]这样能保证不同刚度下系统都接近临界阻尼不要让阻抗模型出现欠阻尼振荡。(\zeta) 一般取 0.8 到 1.2。外部力矩 (\tau_{ext}) 怎么获取在纯仿真里可以直接从动力学方程算出来但在更接近实际的设置里建议用弹簧变形来估计(\hat{\tau}_{ext} \approx K_s(q_m - q_l))。注意这个估计在自由空间里不严格为零因为弹簧变形还包含惯性驱动力矩如果需要更准要把连杆侧动力学补偿掉但这些细节第一次搭仿真时可以以后再处理。4.3 阻抗外层如何参与控制变阻抗模型最终要输出一个期望加速度给内层DSC使用。把阻抗模型改写成[ \ddot{\tilde{q}}l M_d^{-1}(\tau{ext} - B_d \dot{\tilde{q}}_l - K_d \tilde{q}_l) ]于是期望连杆加速度[ \ddot{q}{ld} \ddot{q}{des} M_d^{-1}(\tau_{ext} - B_d \dot{\tilde{q}}_l - K_d \tilde{q}_l) ]其中 (q_{des}) 是任务层给出的名义参考轨迹(\tilde{q}l q_l - q{des})。积分两次得到 (q_{ld}, \dot{q}_{ld}) 后作为内层DSC的参考输入。这里有一个关键点阻抗外层的更新频率不需要和内层控制器一样但仿真里为了省事往往直接统一步长。如果你发现高频噪声影响变阻抗效果把外层的变阻抗参数更新单独放到一个采样率更低的子系统中计算效果会更好。这个分层采样率的设计思路在实验平台上更实用因为上位机通信带宽往往不够。5. 自适应项的实现细节估计什么、不估计什么、初始值怎么给SEA两杆系统的自适应控制听起来是把所有不确定参数一起估实际上好的做法是“能确定的就别让自适应去猜”。模型里哪些参数确定、哪些不确定要分清楚。5.1 参数线性化和回归矩阵把连杆侧动力学中的物理参数提取出来[ M_l(q_l)\ddot{q}_l C_l(q_l,\dot{q}_l)\dot{q}_l G_l(q_l) Y(q_l,\dot{q}_l,\ddot{q}_l)\theta ]这里 (\theta) 是由连杆质量、转动惯量、质心位置、摩擦系数组成的常值参数向量。(Y) 是只依赖状态和参考轨迹的回归矩阵。对两杆系统(\theta) 最多取6到8个参数就够两个连杆的质量、两个转动惯量、两个质心距离、两个额外的摩擦系数。如果你把质心的 (x,y) 坐标分开会有10个以上参数但真实情况下 (x,y) 坐标往往同时与质量和质心位置合力出现没必要拆开。实际仿真建模时我建议先不把 (\theta) 全部设为未知而是分成两部分确定项如电机惯量、弹簧刚度用真值直接用于控制器不确定项如连杆质量、末端负载由自适应律在线估计。这既符合工程直觉也能减小自适应压力。5.2 自适应律与稳定性耦合自适应律的标准形式[ \dot{\hat{\theta}} -\Gamma Y^T s_2 ]其中 (s_2) 是速度误差面(\Gamma) 是对角正定自适应增益矩阵。这里的 (s_2) 和动态面控制第二层用的误差信号是同一个这样自适应项才能和DSC的Lyapunov函数配合保证参数估计误差和跟踪误差耦合稳定。增益 (\Gamma) 的经验对角元素从 0.1 到 10 不等分别对应不同的物理参数不要所有参数都用同一个值。比如末端负载质量的估计增益可以取大一些因为它对控制性能影响大、需要快速跟上而摩擦系数的增益小一些因为摩擦项变化慢增益太大容易导致参数抖动。初始值怎么给如果你的负载是固定已知的初始值直接给真实值自适应根本不用启动系统也能稳定运行。但自适应存在的意义是负载变化。我的做法初始值给真实值的80%然后故意在一秒后突变负载相当于一个0.5kg的物体掉到末端观察参数估计能否在2秒内收敛到真实值附近。这种情况下自适应增益必须足够大否则末端负载改变后的稳态跟踪误差会一直存在。5.3 参数漂移和投影算子的必要性自适应最怕的是无激励情况下的参数漂移。机械臂在做微小运动、或者轨迹是常值时回归矩阵 (Y) 不满足持续激励条件参数估计在噪声的影响下会慢慢漂移。到后期估计的质心位置可能跑到一个物理上完全不可能的数值但控制仍然可能稳定——这更危险因为一旦参考轨迹突然变化这些离谱的参数会瞬间产生巨大的控制力矩。破解方法是在自适应律上加投影算子[ \dot{\hat{\theta}} \operatorname{Proj}{\Omega\theta}\left(-\Gamma Y^T s_2\right) ](\Omega_\theta) 是每个参数的允许范围比如连杆质量必须在0.5到10之间质心距离必须在0到杆长之间。Simulink里用MATLAB Function写一个投影函数就是在每步更新后检查参数是否越界越界则把梯度信号切到与边界相切的方向上。这个小改动看似不起眼但它能让仿真从“有时能跑”变成“一直能跑”。我强烈建议加上。6. Simulink搭建与调参从模块连线到数值发散排查模型和控制律都准备好之后就到了动手搭Simulink的环节。这个环节最容易浪费时间的地方不是控制律本身而是状态组织、代数环和数值发散。下面按我的习惯给出一种可复现的搭建方式。6.1 整体模块划分建议把整个仿真拆成五个部分期望轨迹生成模块生成名义参考轨迹 (q_{des}(t))比如正弦轨迹或S形点到点轨迹阻抗外层模块计算变阻抗后的期望加速度 (\ddot{q}{ld})积分得到 (\dot{q}{ld}, q_{ld})动态面控制器模块实现3.1节的控制律输出电机力矩 (\tau_m)自适应模块更新参数估计 (\hat{\theta})让控制器模块直接使用被控对象模块解算SEA两杆的8个状态微分方程。如果你的Simulink版本支持建议用MATLAB Function块写控制律避免一大张连线图。被控对象模块用S-Function或MATLAB Function都行我习惯用Level-2 MATLAB S-Function因为状态多的时候调试更方便每个状态都有明确的输出序号。6.2 被控对象模块的代码要点被控对象模块本质上是在解下面这个方程组[ \ddot{q}_l M_l^{-1}(q_l)\left[K_s(q_m - q_l) - C_l(q_l,\dot{q}_l)\dot{q}l - G_l(q_l) \tau{ext}\right] ][ \ddot{q}_m M_m^{-1}\left[\tau_m - B_m\dot{q}_m - K_s(q_m - q_l)\right] ]注意 (M_l^{-1}) 的求逆不要用符号求逆之后再代入数值直接用数值方法做2×2矩阵求逆效率高也稳定。每次计算求逆之前最好检查一下行列式是否接近0。两杆机械臂在 (\sin(q_{l2}) \approx 0) 的时候惯性矩阵会接近奇异也就是两个连杆折叠成一条直线时某些方向的运动变得不可控。仿真中如果参考轨迹会经过这种姿态要么避开要么在求逆时加一个小正则项[ M_l^{-1} \approx (M_l \epsilon I)^{-1} ](\epsilon) 取0.001到0.01之间代价是控制精度损失一点点但避免了数值爆炸。6.3 参数初始化表这是我最初跑通这套系统的初始化参数可以直接抄去试参数数值单位连杆1长度 (l_1)0.40m连杆2长度 (l_2)0.30m连杆1质量 (m_1)2.0kg连杆2质量 (m_2)1.0kg连杆1转动惯量 (I_1)0.04kg·m²连杆2转动惯量 (I_2)0.02kg·m²弹簧刚度 (K_{s1}, K_{s2})200, 150N·m/rad电机侧惯量 (M_m)diag(0.05, 0.04)kg·m²电机侧阻尼 (B_m)diag(1.0, 0.8)N·m·s/rad控制增益 (c_1, c_2, c_4)20, 20, 10-滤波时间常数 (\tau_2, \tau_4)0.02, 0.02s自适应增益 (\Gamma)diag(2, 2, 1, 1, 0.2)-期望质量 (M_d)diag(0.5, 0.5)kg·m²期望阻尼范围 (B_d)1.0~3.0N·m·s/rad期望刚度范围 (K_d)30~300N·m/rad注意这里把 (K_d) 的最小值设为30最大值300。如果你的任务接触力特别大可以把最大值再降一档但要注意自由空间跟踪精度也会变差。6.4 求解器设置求解器建议用固定步长ode4四阶龙格-库塔步长设0.001秒。变步长仿真在控制律含自适应、变阻抗这些强非线性环节时经常会因为局部误差估计过紧而把步长压到很小仿真速度慢到没法调参。固定步长配合ode4在大多数情况下精度足够而且行为可预测。如果你的模型里有开关环节或饱和非线性可以考虑ode3减少数值振荡。如果发现固定步长0.001下系统发散首先要排除的不是控制律而是代数环和采样时间问题。检查“代数环”是否断开建议在控制器输出和被控对象输入之间加一个单位Memory模块或者在MATLAB Function块里把输出延迟一步这能解决大部分一碰就飞的问题。6.5 变阻抗参数调节的方向感我调变阻抗参数时的经验是先固定 (K_d) 和 (B_d)把内层DSC和自适应调稳了再开变阻抗。开变阻抗时(\tau_K) 和 (\tau_B) 先给大一点0.1秒以上等系统稳定后逐步减小。如果 (\tau_K) 太小系统会出现一种很奇怪的振荡——低频大幅度来回摆不是高频抖动那是刚度变化跟不上接触力变化导致的。另一个容易踩的坑是阻抗外层的 (\ddot{q}{ld}) 直接积分两次得到位置积分饱和会导致期望轨迹漂移。仿真中如果长时间接触位置偏差会越积越大最终 (q{ld}) 漂到一个离谱的区域。解决办法是给积分器加限幅或者改用PD形式的阻抗模型而不是对加速度积分后者精度差一些但更稳。两种方案我都试过PD形式在Simulink里更好调但跟踪性能会打折扣取舍取决于你的任务。7. 我踩过的问题与改进方向7.1 弹簧刚度选择的两难SEA弹簧刚度的选取直接决定了整个系统的性能天花板。刚度太大SEA的柔顺优势消失碰撞力衰减不明显动态面控制器跟踪起来还算轻松刚度太小机械臂的带宽严重下降轻轻一碰就偏移一大截内环控制器再猛也拉不回来。我试过把 (K_s) 从300改到80系统的自然频率下降了一大截同样的控制增益下跟踪误差明显变大最后不得不把内环增益 (c_1, c_2) 调大但又引发了输入饱和。仿真时注意观察电机输入力矩是否频繁饱和如果饱和了说明弹簧刚度或者控制增益之间的匹配不合理。7.2 参数自适应和变阻抗打架自适应项在估计连杆参数变阻抗在调整目标刚度两者都会改变闭环动态如果调参顺序不对很容易互相打架。我遇到过一种情况参数估计在正常跟踪时收敛得好好的一旦进入接触阶段、变阻抗把 (K_d) 调小了参数估计突然开始发散。原因是接触阶段外部力矩 ( \tau_{ext} ) 进入了动力学方程但自适应模型里把末端负载也作为估计对象外部接触力被误认为负载变化的一部分。解决办法是把自适应项的目标限定在“自由空间模型不确定”上接触阶段暂停参数更新也就是根据交互力矩的大小做一个调度门控。Simulink里用Switch模块就能实现接触判断用 (|\tau_{ext}|) 是否超过阈值。7.3 离散化对DSC的影响Simulink连续仿真结果很好不等于控制器在真实硬件上也能工作。动态面控制的滤波器是连续时间的一阶惯性环节离散化之后滤波器时间常数必须远大于控制周期否则离散化误差会增大滤波误差项导致稳定性边界变差。我建议在做任何实机移植之前先把整个控制器改成本文之前提到的离散形式控制周期取1kHz滤波器时间常数至少取20个控制周期以上。初步测试发现同样的增益参数离散控制器的跟踪误差会比连续仿真大5%到10%这是正常现象不用过度慌张。7.4 后续可以扩展的方向这个框架的扩展性很好。一个方向是在阻抗外层加摩擦力矩观测器现在很多接触场景下用弹簧变形估计外部力矩已经够用但如果末端有额外的工具重力估计会偏很多另一个方向是变阻抗策略从查表式改成更聪明的方式比如用交互功率来决定刚度方向或者用强化学习在线调 (K_d, B_d)。还有一种更实用的改进把电机侧的行星减速器摩擦也纳入自适应范围减少SEA动态面的建模误差。这些方向我都试过一部分感觉最值得先做的还是力矩观测器因为它能直接提高变阻抗在接触力估计上的可靠性整个系统的鲁棒性会提升一个台阶。我自己调这套系统绕了不少弯路最深的体会是SEA两杆加变阻抗加自适应这种组合仿真想单次跑通几乎不可能但模块拆得清晰、参数按先固定后可变、先连续后离散的节奏来调其实每一步都是有迹可循的。上面这些经验如果能在你搭模型的时候帮你省下一两天调试时间这篇博文就没白写。本文还有配套的精品资源点击获取