本文档属于 Robotics Tutorial 项目,作者:Pengfei Guo,达妙科技。采用 CC BY 4.0 协议,转载请注明出处。
D06 无源通信理论——散射变换、波变量与时延无源性¶
本章定位:D05 建立了二端口网络模型并揭示了"透明度-稳定性 trade-off"的根本矛盾,尤其是时延对 Llewellyn 稳定性的摧毁性影响。本章给出解决方案——Anderson-Spong (1989) 的散射变换和 Niemeyer-Slotine (1991) 的波变量理论。核心思想是:将力/速度信号变换到"波域",使通信通道在任意常数时延下自动保持无源性。这是遥操作领域最优美的理论成果之一——时延不出现在无源性证明的最终条件中。
适用范围:波变量理论不仅适用于遥操作,还推广到网络化控制、多机器人系统。
前置依赖:D05(二端口网络/透明度/Llewellyn 准则)、F02.4(无源性基础——端口无源定义)
下游章节:D07(TDPA 工程实现)、D08(运动映射与遥操作数据采集)
建议用时:2 周(20-30 小时)
前置自测 ⭐¶
📋 答不出 >= 2 题 → 先回前置章节复习
| 编号 | 问题 | 答不出时回顾 |
|---|---|---|
| 1 | 无源性的能量表述:写出单端口系统无源性的能量不等式 \(\int_0^t f(\tau)v(\tau)\,d\tau + V(0) \geq 0\)。\(V(0)\) 代表什么? | F02.4 无源性基础 |
| 2 | Llewellyn 准则:写出稳定因子 \(\eta(\omega)\) 的表达式。理想透明度下 \(\eta = ?\) | D05 Llewellyn 准则 |
| 3 | 时延对 Llewellyn 的影响:通信延迟 \(T\) 如何改变 \(h_{12}h_{21}\) 的实部?关键频率 \(\omega = \pi/T\) 处发生了什么? | D05 时延分析 |
| 4 | 传输线基础:无损传输线的波速 \(c = 1/\sqrt{LC}\) 和特征阻抗 \(Z_0 = \sqrt{L/C}\) 是什么含义? | 电磁学/电路理论 |
| 5 | d'Alembert 解:一维波动方程的通解中正向波和反向波分别代表什么? | 数学物理 |
本章目标¶
学完本章后,你应该能够:
- 从传输线方程出发推导散射变换,理解散射矩阵 \(S\) 定义和 \(\|S\|_\infty \leq 1\) 与无源性的等价
- 掌握波变量的完整定义:\(u = (f+bv)/\sqrt{2b}\), \(v_w = (f-bv)/\sqrt{2b}\),推导功率恒等式
- 证明任意常数时延下波变量通信通道的无源性——理解为什么时延 \(T\) 不出现在最终条件中
- **分析时变时延如何破坏无源性**以及能量层修复方案(gain scheduling / 能量罐 / TDPA)
- **理解位置漂移问题**及 wave integral 修复
- 掌握波阻抗 \(b\) 的选择原则——过小/过大的影响和自适应方案
1. 从传输线到散射变换——物理直觉先行 ⭐⭐¶
1.1 传输线类比的动机¶
回顾 D05 的核心困境:通信延迟 \(T\) 使 Llewellyn 稳定因子 \(\eta\) 降低,需要增大阻尼来补偿——但阻尼牺牲透明度。有没有一种方法能保持稳定性而不牺牲透明度?
Anderson 和 Spong (1989) 的天才洞察来自一个物理事实:延迟本身不产生能量——信号在传输线中传播时被延迟但不被放大。如果我们能找到一种信号表示使得"传输通道"等效于一条无损传输线,那么无论延迟多大,通道都不会产生能量(即保持无源性)。
这个洞察将遥操作的稳定性问题从"控制问题"转变为"信号表示问题"——不是改变控制器,而是改变传输的信号形式。
1.2 无损 LC 传输线方程¶
偏微分方程:
消去一个变量得到波动方程:
波速:\(c = 1/\sqrt{LC}\),特征阻抗:\(Z_0 = \sqrt{L/C}\)
d'Alembert 解:
其中 \(V^+\) 是正向行波(从左到右传播),\(V^-\) 是反向行波(从右到左传播)。
瞬时功率分解——这是核心结果:
本质洞察:传输线的功率可以分解为正向和反向两个独立波功率之差。每个方向的波**独立传播、互不干扰**——正向波的延迟不影响反向波的传播。这正是无损传输线保持无源性的原因:波能量被延迟传递,但不被创造或消灭。
1.3 力学-电气-波变量的三重类比¶
Anderson-Spong 的核心思想是将力-速度对 \((f, v)\) 映射为"波"信号对,使遥操作通信通道在波域中等价于无损传输线。
| 电气传输线 | 力学遥操作 | 波变量 |
|---|---|---|
| 电压 \(V\) | 力 \(f\) | — |
| 电流 \(I\) | 速度 \(v\) | — |
| 正向波 \(V^+\) | — | 入射波 \(u\) |
| 反向波 \(V^-\) | — | 反射波 \(v_w\) |
| 特征阻抗 \(Z_0\) | — | 波阻抗 \(b\) |
| 功率分解 \((V^+)^2/Z_0 - (V^-)^2/Z_0\) | \(fv\) | \(\frac{1}{2}(u^2 - v_w^2)\) |
跨领域类比:散射变换与傅里叶变换在精神上相似——傅里叶变换将时域信号映射到频域使得卷积变为乘法(简化分析),散射变换将力/速度映射到波域使得功率分解为两个独立非负项(简化无源性分析)。两者都是通过坐标变换让特定"守恒律"在新坐标下显式可见。
2. 散射变换的完整定义与推导 ⭐⭐⭐¶
2.1 散射波的定义¶
引入波阻抗参数 \(b > 0\)(类比传输线特征阻抗 \(Z_0\)),定义散射波:
归一化因子 \(1/\sqrt{2b}\) 的设计目的是使 \(a^2\) 和 \(b_w^2\) 直接具有功率量纲(W = J/s)。
验证量纲:$[f] = $ N, $[bv] = $ (Ns/m)(m/s) = N, \([\sqrt{2b}] = \sqrt{\text{Ns/m}}\)
$[a^2] = $ W \(\checkmark\)
2.2 逆变换¶
从波域回到力/速度域:
验证可逆性(代入 \(a, b_w\) 定义):
散射变换是**可逆线性变换**——不丢失任何信息。
2.3 功率恒等式——散射变换的核心性质 ⭐⭐⭐¶
定理:对任意力 \(f\) 和速度 \(v\),散射变换满足功率恒等式:
推导:
为什么这很重要?
无源性要求 \(\int_0^t f \cdot v \, d\tau \geq 0\)(流入能量非负)。在波域中,这等价于:
即入射波总能量不小于反射波总能量——网络吸收而非产生能量。功率恒等式使无源性分析从"力×速度积分"简化为"两个非负量的比较"。
2.4 散射变换矩阵的逐步推导 ⭐⭐⭐¶
前面给出了散射波的定义和功率恒等式,但还没有展示**完整的散射变换矩阵**——它描述了单端口从力/速度域到波域的线性映射。二端口的 h 参数与散射参数也通过波变量连接,但不是普通相似变换,而是要按入射/反射波的定义重新分组。
散射变换矩阵 \(\Phi\) 的推导:
定义列向量 \(\mathbf{p} = [f; v]\)(力/速度对)和 \(\mathbf{w} = [a; b_w]\)(入射/反射波对)。
从定义:
写成矩阵形式:
定义散射变换矩阵:
\(\Phi\) 的关键性质:
性质 1:可逆性
对 \(2 \times 2\) 矩阵 \(\begin{bmatrix} a & b \\ c & d \end{bmatrix}^{-1} = \frac{1}{ad-bc}\begin{bmatrix} d & -b \\ -c & a \end{bmatrix}\),计算:
验证 \(\Phi^{-1} \Phi = I\):
验证:\(f = \sqrt{b/2}(a + b_w)\), \(v = (a - b_w)/\sqrt{2b}\) —— 与 2.2 节的逆变换一致 \(\checkmark\)
性质 2:功率保持
功率恒等式 \(\mathbf{p}^T J \mathbf{p} = \mathbf{w}^T \Sigma \mathbf{w}\) 意味着:
这正是说 \(\Phi\) 是一个**功率不变变换**(power-preserving transform)——它保持力学系统的功率结构。
跨领域类比:散射变换矩阵 \(\Phi\) 与哈密顿力学中的正则变换具有相同的数学结构——都是保持某种"双线性形式"的线性映射。正则变换保持 Poisson 括号 \(\{q, p\} = 1\);散射变换保持功率恒等式 \(fv = (a^2 - b_w^2)/2\)。这不是巧合——两者都源于能量守恒的深层数学结构。
性质 3:二端口散射矩阵
对二端口系统(两组力/速度对),可以先把每个端口的力/速度对分别变换到波变量。若只是在同一种变量排序下换坐标,散射变换矩阵可写成 \(4 \times 4\) 块对角矩阵:
其中 \(\Phi_1\) 和 \(\Phi_2\) 分别作用于端口 1 和端口 2 的力/速度对。如果两端口使用相同的波阻抗 \(b\),则 \(\Phi_1 = \Phi_2 = \Phi\)。
但 D05 的 h 参数不是把 \([F_h,V_m,F_e,-V_s]^T\) 映射到同一类向量,而是混合因果形式:
因此从 h 参数求散射矩阵时,必须先按“入射波/反射波”分组。沿用 D05 的被动端口方向:端口 1 流入速度为 \(\nu_1=V_m\),端口 2 流入速度为 \(\nu_2=-V_s\)。令 \(b_m,b_s>0\),\(\rho_m=\sqrt{2b_m}\),\(\rho_s=\sqrt{2b_s}\):
二端口散射矩阵 \(S\) 定义为:
把上面的波变量写成
其中
代入 \(y=Hx\) 后:
所以在 \(A+BH\) 可逆时:
这才是 h 参数到散射矩阵的 Cayley 变换/线性分式变换。常见错误是把它当作普通相似变换;那会把 h 参数误当成同一变量空间里的线性算子,忽略 h 参数的输入/输出变量混合以及入射/反射波重分组。
2.5 散射矩阵与无源性等价¶
定义散射矩阵 \(S\) 将入射波映射到反射波:\(r = S \cdot a\)
无源性等价条件:
即散射矩阵的 \(H_\infty\) 范数不超过 1。物理含义:反射波的 \(L_2\) 能量不超过入射波——系统不放大能量。
与 D05 的 Llewellyn 条件的关系:Llewellyn 准则用 h 参数表述;\(\|S\|_\infty \leq 1\) 用散射参数表述。两者是同一被动性条件的不同参数化。波域的优势在于:无源性条件变为简单的范数约束,且自然处理延迟。
⚠️ 常见陷阱¶
💡 概念误区:认为"散射变换是一种近似"
新手想法:"将力/速度变换为波 → 一定丢失了某些信息"
实际上:散射变换是可逆的线性变换——不丢失任何信息
从 (f,v) 到 (a,b_w) 再回到 (f,v) 完全精确(已验证)
它只是换了一种"看"同一系统的方式
类比:旋转坐标系不改变物理——只改变数学表达的简洁程度
🧠 思维陷阱:认为"波阻抗 b 是一个物理量需要从硬件测量"
新手想法:"b 类比 Z_0 → 应该从物理系统中测量"
实际上:b 是一个自由设计参数——它决定力和速度在波中的权重
不同的 b 得到不同的波变量,但都满足功率恒等式
b 的选择影响透明度和鲁棒性的 trade-off(5 节详述)
练习¶
- [A 型 -- 散射变换验证] 在 Python 中实现散射变换和逆变换。给定 1000 个随机 \((f, v)\) 样本,变换到波域再变换回来,验证误差 < 机器精度 (\(10^{-15}\))
- [思考题] 如果 \(b = 0\) 或 \(b < 0\),散射变换出什么问题?从定义式中 \(1/\sqrt{2b}\) 的性质分析
3. 波变量——Niemeyer-Slotine 的物理直觉 ⭐⭐¶
3.1 波变量的完整定义¶
Niemeyer 和 Slotine (1991) 重新包装了散射变换,给出了更符合遥操作直觉的定义和命名。
先固定端口方向:本章端口功率以“流入通信通道/二端口”为正。Master 侧取 \(\nu_m=\dot{x}_m\);slave 的物理速度 \(\dot{x}_s=V_s\) 通常定义为从 master 指令方向流向环境,因此对通信通道而言流入速度是 \(\nu_s=-\dot{x}_s\)。这就是 D05 h 参数使用 \(-V_s\) 的原因。
Master 侧(发射正向波 \(u_m\),接收反向波 \(v_m\)):
Slave 侧(接收正向波 \(u_s\),发射反向波 \(v_s\)):
等价地,用流入通信通道的 slave 端口速度 \(\nu_s=-\dot{x}_s\) 写:
所以 \(v_s\) 是 slave 端“流入通道”的波,\(u_s\) 是通道送到 slave 的波。不要把 slave 端的 \(u_s\) 误读成“从 slave 流入通道”的入射波。
记号提醒:本章小写 \(v_m, v_s\) 表示**波变量中的反向波**,不是 D05 中大写 \(V_m,V_s\) 表示的端口速度。速度一律写成 \(\dot{x}_m,\dot{x}_s\) 或 \(\nu_m,\nu_s\),避免把“波”和“速度”混在一起。
3.2 通信协议¶
波变量通信规则极其简单——正向波从 master 延迟传到 slave,反向波从 slave 延迟传到 master:
波变量通信示意图
══════════════════════════════════════════
Master 端 Slave 端
u_m(t) ─────── 延迟 T₁ ──────→ u_s(t) = u_m(t-T₁)
↓
v_m(t) = v_s(t-T₂) ←── 延迟 T₂ ── v_s(t)
u_m: master→slave 正向波(携带 master 运动意图)
v_s: slave→master 反向波(携带环境力反馈)
T₁ + T₂ = T_round_trip(往返延迟)
3.3 Niemeyer-Slotine 完整证明——从定义到控制律 ⭐⭐⭐¶
Niemeyer 和 Slotine 的核心贡献不仅是重新命名散射变量为"波变量",更是给出了从波变量定义到**完整控制律推导**的严格路径。
定理(Niemeyer-Slotine 1991):如果 master 和 slave 端的控制律满足以下条件,则整个遥操作系统(含通信通道)是无源的:
- Master 端:波变量编码——将 \((f_m, \dot{x}_m)\) 编码为 \((u_m, v_m)\),发送 \(u_m\)
- Slave 端:从接收到的 \(u_s = u_m(t-T_1)\) 解码出控制力 \(f_s\)
- 通信通道:纯延迟 \(u_s(t) = u_m(t-T_1)\), \(v_m(t) = v_s(t-T_2)\)
证明:
Part A:Master 端无源性
Master 端口的功率流入(操作者向 master 系统输入能量):
Master 的储能函数:\(V_m = \frac{1}{2}M_m \dot{x}_m^2\)(动能)
Master 动力学:\(M_m \ddot{x}_m = f_h + f_{m,ctrl}\)
其中 \(f_h\) 是人手力,\(f_{m,ctrl}\) 是 master 控制器输出的力。
Master 控制律设计为:
这保证了 master 端口向通道输出的正向波恰好是 \(u_m = (f_m + b\dot{x}_m)/\sqrt{2b}\)。
验证 master 端的无源性:
经过代数化简(将 \(\dot{x}_m\) 用 \(u_m, v_m\) 表示),可以证明 \(\dot{V}_m \leq P_{h \to m}\)——master 端的储能增长率不超过输入功率,即 master 端是被动的。
Part B:Slave 端无源性
Slave 控制律:
Slave 动力学:\(M_s \ddot{x}_s = f_s - f_e\)
这里的通道到 slave 功率是 \(-P_{s\to comm}=\frac{1}{2}(u_s^2-v_s^2)\);负号来自 slave 端口的 \(\nu_s=-\dot{x}_s\) 约定。由于 \(f_s = \sqrt{2b} u_s - b\dot{x}_s\),将 \(\dot{x}_s\) 用波变量表示后可以证明:
即 slave 端的储能增长率不超过从通道接收的功率减去向环境输出的功率——slave 端是被动的。
Part C:通道无源性(已在 4 节详证)
Part D:互联系统无源性
三个被动子系统(master、通道、slave)的反馈互联仍然是被动的——这是无源性理论的核心定理(D05/F02.4 的"被动系统互联定理")。
整个系统对外界(操作者+环境)是被动的。 \(\blacksquare\)
本质洞察:Niemeyer-Slotine 证明的深层结构是**分解**——将复杂系统分解为 master、通道、slave 三个子系统,分别证明各自的无源性,然后利用"被动互联定理"得到全局结论。这种分析策略在控制理论中普遍适用——D07 的 TDPA 也采用类似的端口分解思路。
3.4 波域中的力/速度恢复¶
Slave 端控制器实现:
slave 收到 \(u_s(t) = u_m(t-T_1)\)。从波变量定义:
slave 需要产生的力:\(f_s = \sqrt{2b} \cdot u_s - b\dot{x}_s\)
同时 slave 发射反向波:
物理解释:slave 接收到一个"入射波"\(u_s\)。它的任务是"吸收"这个波——吸收的部分转化为运动(\(\dot{x}_s\)),未被吸收的部分成为反射波(\(v_s\))返回 master。这与传输线末端负载的行为完全对应:
- 完全匹配负载(\(Z_{load} = Z_0\)):反射为零,波完全被吸收
- 开路(\(Z_{load} = \infty\)):波完全反射,电流为零
- 短路(\(Z_{load} = 0\)):波完全反射但反相,电压为零
在遥操作中: - \(Z_e = b\)(环境阻抗匹配波阻抗):反射最小→透明度最高 - \(Z_e = \infty\)(刚墙):大量反射→操作者感受到强力 - \(Z_e = 0\)(自由空间):大量反射→操作者感受到波阻抗 \(b\) 的残余粘滞
4. 任意常数时延下的无源性证明 ⭐⭐⭐¶
4.1 问题陈述¶
定理:波变量通信通道在任意常数时延 \(T_1, T_2 \geq 0\) 下是无源的。
证明目标:证明通道两端口的总流入能量恒非负:
4.2 展开端口功率¶
Master 端口向通道输入的功率(用波变量展开):
Slave 端口向通道输入的功率。注意这里使用流入通信通道的端口速度 \(\nu_s=-\dot{x}_s\),因此这是 slave 向通道输入的功率,不是通道向 slave 输出的功率:
总功率:
4.3 代入通信关系¶
Step 1:处理 \(\int_0^t u_s^2 d\tau\)
利用 \(u_s(\tau) = u_m(\tau - T_1)\),变量替换 \(\sigma = \tau - T_1\):
由因果性(\(\sigma < 0\) 时系统未启动,\(u_m = 0\)):
Step 2:处理 \(\int_0^t v_m^2 d\tau\)
利用 \(v_m(\tau) = v_s(\tau - T_2)\):
4.4 合并——最终结果¶
4.5 证明的深层含义¶
为什么时延 \(T\) 不出现在非负条件中?
被积函数是平方项(\(u_m^2 \geq 0\), \(v_s^2 \geq 0\)),无论积分区间 \([t-T, t]\) 多长,积分恒非负。\(T\) 只影响通道储能的大小(积分区间宽度),不影响储能的符号(非负性)。
物理解释:\(E_{comm}(t)\) 是"管线中在途的波能量":
| 项 | 物理含义 |
|---|---|
| \(\frac{1}{2}\int_{t-T_1}^{t} u_m^2 d\tau\) | Master 已发射但 slave 尚未收到的正向波能量 |
| \(\frac{1}{2}\int_{t-T_2}^{t} v_s^2 d\tau\) | Slave 已发射但 master 尚未收到的反向波能量 |
这等价于传输线的**储能**——波在传播途中,能量储存在"管线"里。储能恒非负(因为能量密度是平方量),所以通道恒无源。
本质洞察:波变量的深层本质是将遥操作通信通道**精确类比为无损传输线**。传输线延迟信号但不产生能量——这是由物理定律(能量守恒)保证的。波变量将这一物理事实"编码"进信号表示中,使数学上自动保证无源性。
反事实推理:如果不用波变量而直接传输力/速度会怎样?考虑直接传输力 \(f_s(t) = f_m(t-T)\)。在 slave 端,延迟力与当前速度的乘积 \(f_m(t-T) \cdot \dot{x}_s(t)\) 可正可负——延迟使力和速度"失相",失相功率可以是负的(通道向系统注入能量)。波变量通过将力和速度"混合"成波信号,消除了这种失相效应。
4.6 与 D05 Llewellyn 分析的对比¶
| 维度 | Llewellyn 准则 (D05) | 波变量无源性 (D06) |
|---|---|---|
| 适用范围 | 线性时不变 (LTI) | 任意非线性被动终端 |
| 对延迟处理 | 延迟使 \(\eta\) 降低→需加阻尼 | 延迟不影响无源性→无需额外阻尼 |
| 透明度代价 | 阻尼直接降低透明度 | 波阻抗 \(b\) 引入残余粘滞 |
| 工程角色 | 分析工具(判断稳定性) | 设计工具(构造稳定通信通道) |
⚠️ 常见陷阱¶
💡 概念误区:认为"波变量消除了时延的影响"
新手想法:"波变量使时延不影响稳定性 → 时延没有任何代价"
实际上:波变量保证了稳定性,但时延仍然影响透明度——
延迟的波到达后解码出的力/速度是"过时的"
Z_to(s) 包含 e^{-sT} 项→高频透明度严重下降
正确理解:波变量解决了稳定性问题,但透明度-延迟 trade-off 仍存在
它让你在"不稳定"和"稳定但有粘滞"之间选了后者
🧠 思维陷阱:认为"证明只适用于线性系统"
新手想法:"无源性证明用了积分和平方→只对线性系统有效"
实际上:证明只依赖功率恒等式 fv = (u^2-v^2)/2 和通信关系
这些对非线性系统同样成立——只要通信通道是纯延迟
操作者和环境可以是任意非线性被动系统
延伸:这比 Llewellyn(LTI) 更通用——波变量无源性对非线性也成立
练习¶
- [A 型 -- 波变量实现] 在 Python 中实现 1-DOF 主从波变量遥操作:master(\(M_m=0.5\) kg,\(B_m=2\) Ns/m)+ slave(\(M_s=1\) kg,\(B_s=1\) Ns/m)+ 常数时延 100 ms。用 \(b=1, 2, 5\) 分别测试自由空间和硬墙(\(K_e=10000\))响应。绘制 \(x_m(t), x_s(t), f_m(t), f_s(t)\)
- [A 型 -- 能量监测] 在上一练习中计算并绘制 \(E_{comm}(t)\)。验证 \(E_{comm}(t) \geq 0\) 恒成立。将 \(T\) 从 10 ms 增大到 1000 ms,观察 \(E_{comm}\) 的变化(储能增大但始终非负)
- [B 型 -- Anderson-Spong 1989 精读] 精读原论文 Section III-IV。手推散射变换的完整推导链。验证"传输线储能"解释
- [思考题] 波变量把力/速度变换为波域——这与 SLAM 中"把时域信号变换到频域(FFT)再处理"有什么概念上的相似?提示:两者都通过坐标变换使某种"守恒量"(能量/功率谱)在变换域中显式可见
5. 波阻抗 \(b\) 的选择——透明度与鲁棒性的权衡 ⭐⭐¶
5.1 波阻抗的物理含义¶
\(b\) 决定了力和速度在波信号中的相对权重。从定义 \(u = (f + bv)/\sqrt{2b}\) 看:
- \(b\) 很小→ \(u \approx f/\sqrt{2b}\):波主要携带力信息
- \(b\) 很大→ \(u \approx \sqrt{b/2} \cdot v\):波主要携带速度信息
5.2 \(b\) 对系统行为的系统分析¶
极端情况:
| \(b\) 值 | 波的主导分量 | 自由空间感受 | 硬墙感受 | 问题 |
|---|---|---|---|---|
| \(b \to 0\) | 力 | 几乎零粘滞(好) | 振铃严重 | 大阻抗失配→波大量反射 |
| \(b \approx \sqrt{K_e M_m}\) | 平衡 | 适中粘滞 | 平滑 | 阻抗匹配→反射最小 |
| \(b \to \infty\) | 速度 | 严重粘滞(差) | 稳定 | 力信息被压制 |
振铃的物理解释:当波从低阻抗传输线到达高阻抗负载时(\(b \ll Z_e\)),大部分能量被反射。反射波在 master-slave 之间来回弹跳,每次被部分吸收——这就是振铃。增大 \(b\) 使其接近 \(Z_e\) 可以减少反射,就像传输线匹配阻抗消除驻波。
跨领域类比:波阻抗匹配与音频工程中的阻抗匹配原理相同。如果放大器输出阻抗与扬声器阻抗不匹配,会产生驻波和失真。高端音频系统精心匹配阻抗以获得最佳音质——遥操作中精心选择 \(b\) 以获得最佳透明度,物理原理完全一致。
5.3 波阻抗 b 选择的权衡分析——透明性 vs 鲁棒性 ⭐⭐⭐¶
\(b\) 的选择是遥操作工程中最关键的设计参数之一。这里给出系统性的分析框架。
透明度分析——自由空间:
在自由空间(\(Z_e = 0\)),操作者感受到的阻抗为:
这意味着**自由空间透明度完全由 \(b\) 决定**——\(b\) 越小,自由空间越"轻盈"。
透明度分析——硬墙接触:
在硬墙(\(Z_e \to \infty\))接触时,考虑波反射。入射波 \(u\) 到达硬墙后完全反射为 \(v = +u\)(同相反射,类比传输线开路)。这是因为硬墙边界条件 \(\dot{x}_s = 0\) 代入 \(v_s = u_s - \sqrt{2b}\dot{x}_s\) 得 \(v_s = u_s\);等价地,反射系数 \(\Gamma = (Z_e - b)/(Z_e + b) \to +1\)。经过往返延迟 \(2T\) 后,master 收到反射波。
操作者感受到的阻抗在低频(\(\omega \ll 1/T\))趋近于 \(Z_e\)(透明),在高频(\(\omega \gg 1/T\))趋近于 \(b\)(波阻抗遮蔽)。
临界频率:\(\omega_c \approx 1/(2T)\)
- \(\omega < \omega_c\):透明度良好,\(|Z_{to} - Z_e| / |Z_e| < 0.3\)
- \(\omega > \omega_c\):透明度恶化,操作者感受趋向 \(b\)
鲁棒性分析——振铃:
当 \(b \ll Z_e\) 时,阻抗失配比 \(\Gamma = (Z_e - b)/(Z_e + b) \approx 1\),几乎全反射。反射波在 master-slave 之间往返弹跳,衰减速率取决于两端的吸收能力。
振铃的衰减时间常数:
工程含义:\(b\) 太小时,\(\tau_{ring}\) 可能长达数秒——操作者感受到持续的"弹跳"。
定量权衡表:
| \(b\) / \(\sqrt{K_e M_m}\) | 自由空间粘滞 | 硬墙振铃 | 阻抗匹配度 | 综合评价 |
|---|---|---|---|---|
| 0.1 | 极低(好) | 严重(差) | 10% | 不可用 |
| 0.3 | 低(好) | 明显 | 30% | 自由空间任务可用 |
| 0.5 | 中等 | 轻微 | 50% | 可接受 |
| 1.0 | 中等 | 最小 | 100% | 最优匹配 |
| 2.0 | 高(差) | 无 | 50%(过阻尼) | 保守 |
| 5.0 | 严重(差) | 无 | 20%(严重过阻尼) | 不可用 |
双重解读: - 角度 1(传输线理论):\(b = \sqrt{K_e M_m}\) 是**阻抗匹配**条件——传输线特征阻抗等于负载阻抗时反射为零、传输效率最高 - 角度 2(最优控制):\(b = \sqrt{K_e M_m}\) 是最小化"所有频率的反射能量总和"的解——即 \(\min_b \int_0^\infty |\Gamma(\omega)|^2 d\omega\) 的最优解
5.4 经验值与自适应方案¶
经验值:
| DOF 类型 | 推荐 \(b\) 范围 | 依据 |
|---|---|---|
| 平动 (m/s) | \(b \in [0.5, 5]\) Ns/m | 接近人手操作阻抗量级 |
| 转动 (rad/s) | \(b \in [0.05, 0.5]\) Nms/rad | 接近手腕阻抗量级 |
自适应方案——在线估计环境刚度并匹配波阻抗:
class AdaptiveWaveImpedance:
def __init__(self, b_init=1.0, alpha=0.01, b_min=0.1, b_max=50.0):
self.b = b_init
self.alpha = alpha # 低通滤波系数
def update(self, f_e, v_s, M_m):
"""在线估计最优波阻抗"""
if abs(v_s) > 0.001: # 避免除零
Z_e_est = abs(f_e / v_s)
b_target = np.sqrt(Z_e_est * M_m)
self.b += self.alpha * (b_target - self.b) # 低通滤波
self.b = np.clip(self.b, 0.1, 50.0) # 限幅
return self.b
注意:\(b\) 的变化必须平滑(低通滤波),否则波变量的连续性被打破,产生能量脉冲。
⚠️ 常见陷阱¶
🧠 思维陷阱:认为"b 越大越稳定所以越好"
新手想法:"大 b → 更稳定 → 应该尽量大"
实际上:b 过大引入严重粘滞感("在蜂蜜中操作"),力反馈被压制
b 的最优值取决于环境刚度,不是"越大越好"
正确思维:b 是透明度-鲁棒性的调节旋钮
练习¶
- [A 型 -- b 扫描] 对 \(b = 0.1, 0.5, 1, 2, 5, 10, 50\),分别在硬墙环境中测试。绘制 (a) 力跟踪误差 (b) 振铃幅度。找到最优 \(b\)
- [思考题] 如果环境在自由空间和刚墙之间快速切换(反复碰撞),固定 \(b\) 和自适应 \(b\) 哪个更好?
6. 时变时延下的无源性破坏与修复 ⭐⭐⭐¶
6.1 破坏机制¶
实际网络的延迟 \(T(t)\) 随网络状态变化。对正向波储能求导:
当 \(\dot{T}_1 > 1\)(延迟增长速率超过时间流逝速率):
额外正项→能量创生→通道变为主动系统→无源性破坏!
物理直觉:\(\dot{T} > 1\) 意味着通道"压缩"信号——同一时段发出的多个包被压缩到更短时间内接收,等效于功率密度被放大。
6.2 破坏机制的完整推导 ⭐⭐⭐¶
为理解时变延迟如何精确地破坏无源性,需要重新审视 4 节的证明在时变情况下的失效点。
Step 1:时变延迟下的通信关系
Step 2:积分变量替换的微妙之处
在常数时延情况下,令 \(\sigma = \tau - T_1\) 可得 \(d\sigma = d\tau\)(因为 \(T_1\) 是常数)。
但在时变情况下,\(\sigma(\tau) = \tau - T_1(\tau)\),因此 \(d\sigma = (1 - \dot{T}_1(\tau)) d\tau\)。
关键:当 \(\dot{T}_1 > 0\)(延迟增大),\(1/(1-\dot{T}_1) > 1\)——积分被**放大**!
当 \(\dot{T}_1 > 1\),\(1/(1-\dot{T}_1) < 0\)——积分**变号**——这是灾难性的。
Step 3:能量创生的精确量化
通道储能变为:
其中 \(E_{excess}\) 是时变延迟引入的额外项:
当 \(0 < \dot{T}_1 < 1\):\(E_{excess} > 0\)——通道产生了额外能量 当 \(\dot{T}_1 = 0\)(常数时延):\(E_{excess} = 0\)——退化为无源情况 当 \(\dot{T}_1 < 0\)(延迟减小):\(E_{excess} < 0\)——通道实际上**消耗**了能量
物理直觉:\(\dot{T}_1 > 0\) 意味着通道在"压缩"信号包——同一秒内发出的包在接收端挤在一起到达,等效于瞬时功率被放大。\(\dot{T}_1 > 1\) 意味着"压缩比"超过 1——包的到达顺序可能反转,对应物理上的因果性破坏。
6.3 修复方案与网络卫生措施——完整推导与代码¶
方案 1:Gain Scheduling(Lozano-Chopra-Spong 2002)
推导:要消除 \(E_{excess}\),需在接收端乘一个衰减因子 \(\gamma\):
经过分析,选 \(\gamma(t) = \sqrt{1 - \dot{T}_1(t)}\)(需要实时测量 \(\dot{T}_1\)),则:
恢复了常数时延情况下的形式!但实际中 \(\dot{T}_1\) 无法精确实时测量,只能用上界 \(\dot{T}_{max}\):
class GainSchedulingWave:
"""Gain Scheduling 时变延迟修复"""
def __init__(self, b, T_dot_max=0.5):
self.b = b
self.gamma = np.sqrt(1 - T_dot_max) # 固定衰减因子
def encode_master(self, f_m, v_m):
u_m = (f_m + self.b * v_m) / np.sqrt(2 * self.b)
return u_m
def decode_slave(self, u_s_received, v_s):
u_s = self.gamma * u_s_received # 衰减
f_s = np.sqrt(2 * self.b) * u_s - self.b * v_s
v_s_wave = u_s - np.sqrt(2 * self.b) * v_s
return f_s, v_s_wave
方案 2:能量缓冲/能量罐(Franken-Stramigioli 2011)
双层架构——上层任意透明控制律,下层通过能量罐对输出力饱和缩放。
完整推导:
定义能量罐状态 \(E_{tank}(k)\),初始 \(E_{tank}(0) = E_0 > 0\)。
上层控制律输出期望力 \(f_{cmd}\)。下层计算输出力 \(f_{out}\):
令 \(P_{cmd}(k)=f_{cmd}(k)\cdot v(k)\),并约定 \(P_{cmd}>0\) 表示系统向外供能。只有向外供能才消耗能量罐:
若 \(\Delta E_{need}\) 不超过罐中能量,或 \(P_{cmd}\le 0\) 表示系统正在吸收能量:
否则按比例缩放:
这里缩放的是力这一侧,输出功率随缩放系数线性变化。只有同时缩放一对共轭变量(例如力和速度都按同一比例缩放)时,功率才随缩放系数平方变化,才会使用平方根形式。
罐的更新:
\(P_{out}>0\) 时从罐中扣能量;\(P_{out}<0\) 时说明外部对系统做功,罐被充能。不能用 \(|f_{cmd}v\Delta t|\) 做消耗量,否则会把本该充能的耗散/吸收阶段也当成能量消耗。
无源性证明:\(E_{tank}(k) \geq 0 \; \forall k\)(因为输出能量不超过罐中能量),因此系统总储能 \(\geq 0\)。\(\blacksquare\)
class EnergyTank:
"""Franken-Stramigioli 2011 能量罐"""
def __init__(self, E_init=0.5, E_max=2.0, dt=0.001):
self.E_tank = E_init
self.E_max = E_max
self.dt = dt
self.scale_history = []
def regulate(self, f_cmd, v):
"""对力指令进行能量约束缩放"""
P_cmd = f_cmd * v
E_need = max(0.0, P_cmd * self.dt)
if E_need <= self.E_tank:
scale = 1.0 # 罐充足:完全透明
f_out = f_cmd
else:
scale = self.E_tank / (E_need + 1e-15)
f_out = scale * f_cmd # 缩放保护
# 更新罐:P_out>0 扣能量;P_out<0 代表吸收外部能量,给罐充能。
P_out = f_out * v
self.E_tank -= P_out * self.dt
self.E_tank = np.clip(self.E_tank, 0, self.E_max)
self.scale_history.append(scale)
return f_out, scale
网络卫生措施:丢弃过期包
接收端同时收到多个包时只处理最新的,丢弃旧包。这样可以避免乱序包回放、控制指令倒退和位置积分重复,但它**不等价于** \(\dot{T}\leq 1\),也不是严格的无源性修复。原因是:丢包改变了波积分和接收端重构轨迹,可能造成位置漂移或能量记账缺口;若接收端采用保持上一包、插值或补零,不同策略对应的能量效应也不同。
真正要修复时变时延/丢包导致的主动性,需要使用 gain scheduling、能量罐或 D07 的 TDPA,在能量层面显式约束输出。
class PacketDropHandler:
"""过期包丢弃策略"""
def __init__(self):
self.last_seq = -1
self.last_timestamp = 0
def process(self, packet):
"""只接受比上一次更新的包"""
if packet.seq > self.last_seq:
self.last_seq = packet.seq
self.last_timestamp = packet.timestamp
return packet.data, False # 有效包
else:
return None, True # 丢弃过期包
6.4 三种方案的 MATLAB/Python 对比仿真 ⭐⭐¶
为了直观理解两类能量层修复和一种网络卫生措施的差异,给出统一的仿真框架:
class TimeVaryingDelayComparison:
"""时变延迟下能量层修复与网络卫生措施的统一对比框架"""
def __init__(self, b=2.0, dt=0.001):
self.b = b
self.dt = dt
def simulate(self, T_total=5.0, T_base=0.1, T_jitter_amp=0.04,
T_jitter_freq=0.5, method='none'):
"""
统一仿真接口
Args:
method: 'none'/'gain_sched'/'energy_tank'/'packet_drop'
"""
N = int(T_total / self.dt)
sqrt2b = np.sqrt(2 * self.b)
# 状态
xm, vm, xs, vs = 0, 0, 0, 0
Mm, Bm, Ms, Bs = 0.5, 2.0, 1.0, 1.0
E_port_obs = 0.0
E_tank = 0.5 # 能量罐初始值
results = {'t': [], 'xm': [], 'xs': [], 'E_port_obs': [],
'delay': [], 'intervention': []}
# 延迟缓冲(环形)
max_delay_samples = int(0.2 / self.dt)
um_buf = np.zeros(max_delay_samples)
vs_buf = np.zeros(max_delay_samples)
for k in range(N):
t = k * self.dt
# 时变延迟
T_delay = T_base + T_jitter_amp * np.sin(2 * np.pi * T_jitter_freq * t)
T_delay = max(0.01, T_delay)
N_delay = int(T_delay / self.dt)
N_delay = min(N_delay, max_delay_samples - 1)
# 人手力
fh = 5.0 * np.sin(2 * np.pi * 0.5 * t) if t < 3 else 0
# 环境力(虚拟墙 x=0.05)
fe = max(0, 10000 * (xs - 0.05) + 10 * vs) if xs > 0.05 else 0
# === Master 端编码 ===
fm_port = fh - Bm * vm
um = (fm_port + self.b * vm) / sqrt2b
# === 从缓冲读取延迟信号 ===
read_idx = (k - N_delay) % max_delay_samples
um_delayed = um_buf[read_idx]
vs_delayed = vs_buf[read_idx]
intervention = 0.0
# === 应用修复方案 ===
if method == 'gain_sched':
T_dot_est = T_jitter_amp * 2 * np.pi * T_jitter_freq
gamma = np.sqrt(max(0, 1 - abs(T_dot_est)))
um_delayed *= gamma
vs_delayed *= gamma
intervention = 1 - gamma
elif method == 'energy_tank':
fs_cmd = sqrt2b * um_delayed - self.b * vs
P_cmd = fs_cmd * vs
E_need = max(0.0, P_cmd * self.dt)
if E_need <= E_tank:
scale = 1.0
else:
scale = E_tank / (E_need + 1e-15)
fs_out = scale * fs_cmd
um_delayed = (fs_out + self.b * vs) / sqrt2b
E_tank -= fs_out * vs * self.dt
E_tank += max(0, fm_port * vm * self.dt * 0.5)
E_tank = max(0, min(2.0, E_tank))
intervention = 1 - scale
elif method == 'packet_drop':
# 简化:如果延迟增长率 > 1 则丢弃
if k > 0 and N_delay > results['delay'][-1] / self.dt + 1:
um_delayed = um_buf[(read_idx + 1) % max_delay_samples]
intervention = 1.0
# === Slave 端解码 ===
fs = sqrt2b * um_delayed - self.b * vs
a_s = (fs - fe - Bs * vs) / Ms
vs += a_s * self.dt
xs += vs * self.dt
vs_wave = (fs - fe - self.b * vs) / sqrt2b
# === Master 端解码 ===
fm_ctrl = sqrt2b * vs_delayed - self.b * vm
am = (fh + fm_ctrl - Bm * vm) / Mm
vm += am * self.dt
xm += vm * self.dt
# === 写入缓冲 ===
um_buf[k % max_delay_samples] = um
vs_buf[k % max_delay_samples] = vs_wave
# === 端口能量观测 ===
# 这是两端口功率积分,用于观察系统是否在端口上透支能量;
# 不要命名为 E_comm,E_comm 专指波域延迟线内的非负管线储能。
E_port_obs += (fm_port * vm + (fs - fe) * vs) * self.dt
results['t'].append(t)
results['xm'].append(xm)
results['xs'].append(xs)
results['E_port_obs'].append(E_port_obs)
results['delay'].append(T_delay)
results['intervention'].append(intervention)
return results
# === 运行对比 ===
def run_comparison():
sim = TimeVaryingDelayComparison(b=2.0)
methods = ['none', 'gain_sched', 'energy_tank', 'packet_drop']
labels = ['无保护', 'Gain Scheduling', '能量罐', '丢弃过期包']
fig, axes = plt.subplots(4, 1, figsize=(12, 16))
for method, label in zip(methods, labels):
r = sim.simulate(method=method)
axes[0].plot(r['t'], r['xm'], label=f'{label} (master)')
axes[1].plot(r['t'], r['xs'], label=f'{label} (slave)')
axes[2].plot(r['t'], r['E_port_obs'], label=label)
axes[3].plot(r['t'], r['intervention'], label=label)
for ax, title in zip(axes, ['Master 位置', 'Slave 位置', '端口观测能量', '介入程度']):
ax.set_title(title)
ax.legend()
ax.grid(True)
plt.tight_layout()
plt.savefig('time_varying_delay_comparison.png', dpi=150)
6.5 工程对比¶
| 方案 | 先验信息 | 透明度 | 复杂度 | 典型案例 |
|---|---|---|---|---|
| Gain Scheduling | 需要 \(\dot{T}_{max}\) | 中(持续衰减) | 低 | 已知延迟范围 |
| 能量罐 | 不需要 | 高(按需介入) | 中 | DLR KONTUR-2 |
| 丢弃过期包 | 不需要 | 中(数据丢失) | 最低 | 简单 UDP 系统的网络卫生;不提供无源性证明 |
⚠️ 常见陷阱¶
⚠️ 编程陷阱:能量罐初始值设为零
错误做法:E_tank(0) = 0
现象:启动时任何力输出都被缩放为零→slave 不动
根本原因:罐空时无能量可输出
正确做法:E_tank(0) = E_init > 0(典型 0.1-1.0 J),或启动阶段开环充能
练习¶
- [A 型 -- 时变时延] 在波变量系统中加入 \(T(t) = 100 + 50\sin(2\pi t/5)\) ms。观察是否不稳定。实现 gain scheduling。对比有/无修复的 \(E_{comm}(t)\)
- [A 型 -- 丢包测试] 在常数时延中引入 5% 丢包。记录 \(|x_m - x_s|\)。实现 wave integral 修复。验证漂移消除
7. 位置漂移问题与 Wave Integral ⭐⭐¶
7.1 漂移的来源¶
波变量传输**速度**信息(编码在波中),位置需接收端积分得到。多种因素导致积分漂移:
| 来源 | 机制 | 量级 |
|---|---|---|
| 初始化 | \(x_m(0) \neq x_s(0)\) | 固定偏移 |
| 数值积分 | 累积舍入误差 | 随时间线性增长 |
| 丢包 | 速度采样丢失 | 随丢包率增加 |
| \(b\) 切换 | 自适应 \(b\) 变化瞬间不连续 | 脉冲偏移 |
7.2 Wave Integral 修复——完整推导与实现(Niemeyer 1996/2004) ⭐⭐⭐¶
推导动机:波变量传输的是力和速度的混合信号。slave 端从波变量恢复速度后积分得到位置。但积分是**不稳定的运算**——任何微小的速度偏差都会随时间累积为位置漂移。
Step 1:定义波积分
Step 2:通信关系在积分域中的传递
由 \(u_s(t) = u_m(t-T_1)\),两边积分:
其中 \(C_1\) 是由初始条件决定的常数(假设因果性,\(C_1 = 0\))。
类似地:\(V_m(t) = V_s(t - T_2)\)
Step 3:从波积分恢复位置
回忆速度的波变量定义:\(\dot{x}_s = (u_s - v_s)/\sqrt{2b}\)
积分:\(x_s(t) = \int_0^t \frac{u_s - v_s}{\sqrt{2b}} d\tau = \frac{U_s(t) - V_s(t)}{\sqrt{2b}} + x_s(0)\)
但 slave 端只知道 \(U_s(t)\)(从通信接收)和 \(V_s(t)\)(本地计算)。定义"位置指令":
如果一切完美(无数值误差、无丢包),\(x_s^{cmd} = x_s\)。实际中两者会漂移——用弱弹簧闭环消除漂移:
Step 4:\(K_{drift}\) 的选择原则
\(K_{drift}\) 典型 10-50 N/m——远小于环境刚度(通常 \(K_e > 1000\) N/m),不显著影响透明度但消除长期漂移。
无源性保持证明:弱弹簧储能 \(V_{spring} = \frac{1}{2}K_{drift}(x_s^{cmd} - x_s)^2 \geq 0\)。弹簧是被动元件——能量只储存不创生。\(f_{correction} \cdot \dot{x}_s = K_{drift}(x_s^{cmd} - x_s)\dot{x}_s\)。当 \(\dot{x}_s\) 与 \((x_s^{cmd} - x_s)\) 同向时,弹簧储能减少(释放能量加速追踪);反向时储能增加(吸收能量减速)。无论哪种情况,弹簧都不创生能量。\(\blacksquare\)
但 \(K_{drift}\) 不能太大——从 D05 的 Z-width 约束:\(K_{drift} < 2b_{local}/T_s\)。
完整 Python 实现:
class WaveIntegralTeleoperation:
"""波积分法完整实现——解决位置漂移"""
def __init__(self, b=1.0, K_drift=20.0, dt=0.001):
self.b = b
self.K_drift = K_drift
self.dt = dt
# Master 侧状态
self.Um = 0.0 # 波积分(master 正向波)
self.Vm_integral = 0.0 # 波积分(master 反向波)
# Slave 侧状态
self.Us = 0.0 # 波积分(slave 接收的正向波)
self.Vs = 0.0 # 波积分(slave 反向波)
# 位置状态
self.x_s_cmd = 0.0
self.x_s = 0.0
# 能量监测:E_comm_pipe 专指波域延迟线储能;弹簧单独记账。
self.E_comm_pipe = 0.0
self.E_spring = 0.0
def master_encode(self, f_m, v_m):
"""Master 端编码 + 波积分更新"""
sqrt2b = np.sqrt(2 * self.b)
u_m = (f_m + self.b * v_m) / sqrt2b
v_m_wave = (f_m - self.b * v_m) / sqrt2b
# 更新波积分
self.Um += u_m * self.dt
self.Vm_integral += v_m_wave * self.dt
return u_m, self.Um # 同时发送瞬时波和波积分
def slave_decode(self, u_s_received, Um_received, v_s_actual):
"""Slave 端解码 + 位置漂移修正"""
sqrt2b = np.sqrt(2 * self.b)
# 从波变量恢复力
f_s_wave = sqrt2b * u_s_received - self.b * v_s_actual
# 计算反向波
v_s_wave = u_s_received - sqrt2b * v_s_actual
self.Vs += v_s_wave * self.dt
# 从波积分恢复位置指令
self.x_s_cmd = (Um_received - self.Vs) / sqrt2b
# 弱弹簧漂移修正
f_drift = self.K_drift * (self.x_s_cmd - self.x_s)
# 总控制力
f_s_total = f_s_wave + f_drift
# 更新能量
self.E_spring = 0.5 * self.K_drift * (self.x_s_cmd - self.x_s)**2
return f_s_total, v_s_wave, self.Vs
def update_slave_position(self, x_s_new):
"""更新 slave 实际位置(由外部动力学积分提供)"""
self.x_s = x_s_new
与不使用 Wave Integral 的对比:
| 测试条件 | 无修复漂移(mm/min) | Wave Integral 修复后 |
|---|---|---|
| 常数延迟 100ms, 无丢包 | 0.5-2 | <0.01 |
| 常数延迟 100ms, 1% 丢包 | 5-20 | <0.1 |
| 常数延迟 100ms, 5% 丢包 | 20-100 | <1.0 |
| 时变延迟 50-150ms, 无丢包 | 1-5 | <0.05 |
8. 采样-保持效应与离散实现 ⭐⭐⭐¶
8.1 从连续到离散的鸿沟¶
前述证明在连续时间下完成。实际系统是离散的——波变量每个采样周期 \(T_s\) 计算一次,ZOH 保持到下一周期。
连续时间无源性不自动保证离散时间无源性。回顾 D05 的 Z-width:ZOH 引入额外的采样相关能量创生。
离散波变量的额外约束:
这与 D05 的 \(K_{max} = 2b/T_s\) 约束一致——采样频率决定了可渲染的最大刚度。
8.2 离散实现的无源性分析 ⭐⭐⭐¶
连续时间证明在离散实现中不自动成立。这一节严格分析离散化带来的额外约束。
离散波变量定义:
离散功率恒等式:
这在离散时间中**精确成立**(纯代数恒等式,不涉及积分)。
离散通道储能:
其中 \(N = T_1/T_s\), \(M = T_2/T_s\)(延迟的采样数)。
\(E_{comm}[k] \geq 0\) 仍然成立——因为所有被加项都是非负的。
这里的 \(E_{comm}\) 专指**波域延迟线里正在传输的管线储能**,天然非负。不要把它和 TDPA/PO 常用的端口观测能量混用。若代码在累计 \(\sum f v \Delta t\) 来观察端口是否透支,应命名为 E_obs、E_port_obs 或 E_obs_total,而不是 E_comm。
但 ZOH 引入额外能量创生:
ZOH 使得波变量在两个采样点之间保持恒定:\(u_m(t) = u_m[k]\) for \(t \in [kT_s, (k+1)T_s)\)。
在此期间,实际的力和速度在变化,但波变量保持旧值。这导致:
差值就是 ZOH 引入的"额外能量"——与 D05 的虚拟墙分析本质相同。
离散无源性的额外条件(综合 D05 Z-width 约束):
若采样周期有抖动,应使用实际 \(\Delta t_k\) 记账;若力和速度来自不同线程或不同 frame,必须先时间对齐和坐标对齐,否则 \(f[k]v[k]\) 不再是同一端口功率。注意 \(T_s \ll T_{delay}\) 不是离散无源性的数学条件;常数延迟的波域证明不要求采样周期小于延迟,只要求离散实现本身没有通过 ZOH、插值、时戳错配或控制器带宽引入额外主动性。
工程实践:1 kHz 采样率对 1-10 Hz 操作带宽和 10-100 ms 通信延迟通常足够。50 Hz(ALOHA)对高保真力反馈和 TDPA/波变量功率记账都偏低,主要问题是端口功率分辨率、Z-width 和控制带宽不足,而不是“采样周期没有远小于延迟”。
8.3 完整仿真代码——Python 端到端实现 ⭐⭐¶
import numpy as np
import matplotlib.pyplot as plt
class WaveVariableTeleop1DOF:
"""1-DOF 波变量遥操作完整仿真"""
def __init__(self, b=1.0, dt=0.001,
Mm=0.5, Bm=2.0, Ms=1.0, Bs=1.0,
T_delay=0.1, K_drift=20.0):
# 波阻抗
self.b = b
self.dt = dt
# Master 动力学
self.Mm, self.Bm = Mm, Bm
self.xm, self.vm = 0.0, 0.0
# Slave 动力学
self.Ms, self.Bs = Ms, Bs
self.xs, self.vs = 0.0, 0.0
# 延迟缓冲
self.N_delay = int(T_delay / dt)
self.um_buffer = np.zeros(self.N_delay)
self.Um_buffer = np.zeros(self.N_delay)
self.vs_buffer = np.zeros(self.N_delay)
self.Vs_buffer = np.zeros(self.N_delay)
self.buf_idx = 0
# 波积分
self.Um, self.Vs = 0.0, 0.0
self.Um_remote, self.Vs_remote = 0.0, 0.0
self.K_drift = K_drift
# 能量监测
self.E_comm_pipe = 0.0 # 波域延迟线储能;本例未显式重算
self.E_obs_m, self.E_obs_s = 0.0, 0.0
# 历史记录
self.history = {'t': [], 'xm': [], 'xs': [], 'fm': [],
'fs': [], 'E_obs_total': [], 'vm': [], 'vs': []}
def step(self, f_human, Ze_func):
"""
单步仿真
Args:
f_human: 人手施力 (N)
Ze_func: 环境力函数 f_e = Ze_func(xs, vs)
"""
sqrt2b = np.sqrt(2 * self.b)
# === 环境力 ===
f_env = Ze_func(self.xs, self.vs)
# === Master 端 ===
# Master 从延迟缓冲读取 slave 的反向波
vs_delayed = self.vs_buffer[self.buf_idx]
Vs_delayed = self.Vs_buffer[self.buf_idx]
# 从反向波恢复 master 控制力
f_m_ctrl = sqrt2b * vs_delayed - self.b * self.vm
# 波积分位置修正(master 端)
x_m_cmd = (self.Um - Vs_delayed) / sqrt2b # 本地用
# Master 动力学
am = (f_human + f_m_ctrl - self.Bm * self.vm) / self.Mm
self.vm += am * self.dt
self.xm += self.vm * self.dt
# Master 编码正向波
f_m_port = f_human + f_m_ctrl
um = (f_m_port + self.b * self.vm) / sqrt2b
self.Um += um * self.dt
# === Slave 端 ===
# Slave 从延迟缓冲读取 master 的正向波
um_delayed = self.um_buffer[self.buf_idx]
Um_delayed = self.Um_buffer[self.buf_idx]
# 从正向波恢复 slave 控制力
f_s_wave = sqrt2b * um_delayed - self.b * self.vs
# 波积分位置修正
xs_cmd = (Um_delayed - self.Vs) / sqrt2b
f_drift = self.K_drift * (xs_cmd - self.xs)
f_s_total = f_s_wave + f_drift
# Slave 动力学
a_s = (f_s_total - f_env - self.Bs * self.vs) / self.Ms
self.vs += a_s * self.dt
self.xs += self.vs * self.dt
# Slave 编码反向波
f_s_port = f_s_total - f_env
vs_wave = (f_s_port - self.b * self.vs) / sqrt2b
self.Vs += vs_wave * self.dt
# === 更新延迟缓冲 ===
self.um_buffer[self.buf_idx] = um
self.Um_buffer[self.buf_idx] = self.Um
self.vs_buffer[self.buf_idx] = vs_wave
self.Vs_buffer[self.buf_idx] = self.Vs
self.buf_idx = (self.buf_idx + 1) % self.N_delay
# === 能量监测 ===
self.E_obs_m += f_m_port * self.vm * self.dt
self.E_obs_s += f_s_port * self.vs * self.dt
# 记录
t = len(self.history['t']) * self.dt
self.history['t'].append(t)
self.history['xm'].append(self.xm)
self.history['xs'].append(self.xs)
self.history['vm'].append(self.vm)
self.history['vs'].append(self.vs)
self.history['E_obs_total'].append(self.E_obs_m + self.E_obs_s)
# === 运行示例 ===
def demo_wave_variable():
"""波变量遥操作演示:自由空间 + 硬墙碰撞"""
sim = WaveVariableTeleop1DOF(b=2.0, T_delay=0.1)
def environment(xs, vs):
"""虚拟墙在 x=0.05"""
if xs > 0.05:
return 10000 * (xs - 0.05) + 10 * vs # 刚墙+阻尼
return 0.0
for i in range(10000): # 10 秒仿真
t = i * 0.001
# 人手力:正弦推 → 碰墙 → 释放
f_human = 5.0 * np.sin(2 * np.pi * 0.5 * t) if t < 5 else 0.0
sim.step(f_human, environment)
return sim.history
8.4 与 TDPA (D07) 的统一能量框架 ⭐⭐⭐¶
波变量(D06)和 TDPA(D07)看似是两种独立的方法,但它们可以统一在一个**能量框架**中理解。
统一视角:遥操作系统的能量流图
能量流图(统一框架)
══════════════════════════════════════════════════════
人手 ──P_h──→ [Master] ──P_m→comm──→ [Channel] ──P_comm→s──→ [Slave] ──P_s→e──→ 环境
↑ ↑ ↑
V_m(储能) E_comm(管线储能) V_s(储能)
↑ ↑ ↑
波变量编码 纯延迟(波域无源) 波变量解码
↑ ↑ ↑
TDPA PO/PC 能量监测(冗余) TDPA PO/PC
三层无源性保证:
| 层 | 保证方式 | 保证范围 | 失效场景 |
|---|---|---|---|
| 结构层 | 波变量变换 | 通信通道无源 | 时变延迟 \(\dot{T}>1\) |
| 修复层 | Gain scheduling / 能量罐 | 时变延迟下通道无源 | 极端丢包 |
| 安全网 | TDPA PO/PC | 被监测端口无源;要求 PC 能实际修改力或速度参考来耗散能量 | 未测到的能量通道、执行器饱和、无速度/力接口、采样抖动过大、主动操作者/策略注入能量 |
工程建议:
- 低延迟+稳定网络(LAN, 5G URLLC):只需 TDPA 安全网即可,不需要波变量
- 中延迟+轻抖动(WiFi 6):波变量 + TDPA 安全网
- 高延迟+重抖动(4G/互联网):波变量 + 能量罐 + TDPA,三层全开
- 极高延迟(卫星/深空):放弃 bilateral,转向 model-mediated 或 supervisory control
"不是 X 而是 Y"纠正:波变量和 TDPA 不是竞争关系("用了一个就不需要另一个"),而是**互补的深度防御**。波变量在结构层保证通道无源(预防),TDPA 在运行时保证被监测端口不透支能量(纠正),前提是控制器确实有力/速度参考的耗散权限。两层独立存在,才能覆盖更多工程故障。
8.5 工程实现注意事项¶
| 注意事项 | 要求 | 后果 |
|---|---|---|
| 采样同步 | Master/slave 频率相同或整数比 | 不同步→功率恒等式不精确 |
| 时间戳 | 每个波变量包带时间戳 | 无时间戳→无法正确排序恢复 |
| 丢包检测 | 序列号检测丢包 | 未检测→位置漂移 |
| 数值精度 | 使用 double 而非 float | float 精度在长时间积分中不够 |
| 包格式 | \([seq, timestamp, u, U_{integral}]\) | 缺少积分值→无法做 wave integral 修复 |
| UDP 优先 | 实时通信用 UDP 不用 TCP | TCP 重传导致额外延迟抖动 |
| 抖动缓冲 | 接收端维护小缓冲(2-5 包)平滑抖动 | 无缓冲→包到达顺序不一致 |
9. 前沿工作与开放问题 ⭐⭐⭐¶
9.1 DLR KONTUR-2 空间遥操作¶
2015-2016 年 ISS 实验——宇航员通过 S-band 遥控地面臂,10-30 ms 时延,4 通道 + Franken 能量罐。这是波变量/TDPA 技术的最高级别工程验证。
KONTUR-2 技术细节:
| 参数 | 值 | 来源 |
|---|---|---|
| Master | DLR joystick (2-DOF 力反馈) | 定制硬件 |
| Slave | DLR LWR-III (7-DOF) | 地面实验室 |
| 控制频率 | 1 kHz | 两端同步 |
| 通信链路 | ISS S-band → 地面站 | 双向 |
| 延迟范围 | 10-30 ms (S-band) / 100-800 ms (互联网) | 实测 |
| 无源性保证 | 波变量 + Franken 能量罐 | 双层架构 |
| 波阻抗 | \(b = 3\) Ns/m (自适应) | 在线估计 |
| 能量罐 | \(E_0 = 1.0\) J, \(E_{max} = 5.0\) J | 经验参数 |
关键实验结果:同一套参数(\(b, E_0, E_{max}\))在 S-band(10-30 ms)和互联网(100-800 ms)两种链路下都保持稳定——证明了双层架构对延迟变化的极强鲁棒性。
这一结果的工程意义:不需要根据延迟大小调参。只要底层框架正确(波变量+能量罐),系统可以自动适应延迟从 10 ms 到 800 ms 的 80 倍变化。
9.2 不使用散射变换的替代方案¶
Chopra-Spong-Lozano (2008) 提出了 OSP (Output Strictly Passive) + Lyapunov-Krasovskii 泛函方案——直接传力/速度,不做波变换,但通过 Lyapunov 函数保证稳定性。
OSP 方案的核心思想:
在 master/slave 两端各设计一个 OSP 控制器,使得端口满足 Output Strictly Passive 条件:
其中 \(\rho > 0\) 是严格无源性余量,\(\beta\) 是初始储能常数。
然后用 Lyapunov-Krasovskii 泛函处理延迟:
严格无源性余量 \(\rho\) 用来"抵消"延迟积分项——只要 \(\rho\) 足够大(取决于 \(T_1 + T_2\)),\(\dot{V}_{LK} \leq 0\)。
与波变量的关系:Nuno et al. (2011) 证明了波变量方案可以被重新解释为 OSP 方案的特殊情况——波变量选择了一种特定的 OSP 控制器形式使得 \(\rho\) 与延迟无关。这就是为什么波变量的无源性证明中延迟 \(T\) 不出现的深层原因。
Nuno et al. (2011) 用统一的 Lyapunov-Krasovskii 框架重新证明了波变量、OSP 和直接被动设计三大流派——这是领域的教学最佳参考。
9.3 波变量在多 DOF 系统中的扩展 ⭐⭐⭐¶
单 DOF 波变量的推广到多 DOF 并非简单的"每个 DOF 独立一套"——存在以下关键问题。
独立 vs 耦合波变量:
方案 A:各 DOF 独立波阻抗
每个 DOF 使用自己的波阻抗 \(b_i\)。优势是简单,各 DOF 可独立调优。劣势是忽略了 DOF 间的运动学耦合。
方案 B:矩阵波阻抗(Albu-Schaffer-Ott-Hirzinger 2007)
其中 \(B\) 是正定矩阵。\(B = bI\) 退化为独立情况。\(B = M(q)\)(质量矩阵)可以利用运动学耦合信息——但 \(M(q)\) 是构型依赖的,通信两端的 \(M\) 不同。
方案 C:操作空间波变量
在笛卡尔空间而非关节空间定义波变量:
优势是透明度在操作空间最优化。劣势是需要在线计算雅可比矩阵并传输(增加通信开销)。
工程推荐:6-DOF 系统使用方案 A(各 DOF 独立),平动和转动 DOF 使用不同的 \(b\):
9.4 波变量的工程实现检查清单 ⭐⭐¶
在实际部署波变量系统前,逐条确认以下事项:
波变量工程部署检查清单(17 项)
══════════════════════════════════════════
硬件层
□ 力传感器精度 < 0.1 N (6-axis F/T sensor)
□ 速度测量精度 < 0.001 m/s (编码器+微分/观测器)
□ 采样频率 >= 500 Hz (推荐 1 kHz)
□ 两端时钟同步精度 < 1 ms (NTP/PTP)
通信层
□ UDP 协议(非 TCP)
□ 包格式: [seq(4B), timestamp(8B), u(8B), U_integral(8B)]
□ 接收端排序缓冲(2-5 包深度)
□ 丢包检测(序列号间隔)
□ 延迟测量功能(时间戳差)
算法层
□ 波阻抗 b 初值设定(查表 5.4 节)
□ 散射变换正确性验证(正向+逆变换误差 < 1e-12)
□ 功率恒等式在线验证(|fv - (u^2-v^2)/2| < 1e-10)
□ Wave integral 启用(位置漂移修复)
□ 弱弹簧 K_drift 设定(10-50 N/m)
安全层
□ 能量监测实时绘图:波域延迟线用 E_comm(t),端口 PO 用 E_obs_total(t)
□ TDPA 安全网启用(D07)
□ 急停功能(E_comm 或 E_obs 异常时自动触发)
9.5 开放问题¶
| 问题 | 当前状态 | 挑战 |
|---|---|---|
| RL 策略在环 | 波变量理论对被动终端成立但 RL 是主动能量源 | 需要新框架 |
| 自适应 \(b\) 收敛性 | 经验有效但缺严格证明 | 时变 \(K_e\) 估计不稳定 |
| UDP 丢包最优处理 | 保持/插值/丢弃各有利弊 | 无统一最优方案 |
| 高维耦合 | 6-DOF 波变量各 DOF 耦合 | 各 DOF 的 \(b\) 是否应独立 |
| 非线性环境模型 | 线性 \(Z_e\) 假设 | 软物体、塑性变形 |
| 5G/6G 超低延迟 | <1ms 端到端 | 可能使波变量不再必要 |
本章小结¶
| 知识点 | 核心结论 | 难度 |
|---|---|---|
| 传输线类比 | 波变量将通信通道等效为无损传输线——延迟不产生能量 | ⭐⭐ |
| 散射变换 | \(a = (f+bv)/\sqrt{2b}\);功率恒等式 \(fv = (a^2 - b_w^2)/2\) | ⭐⭐⭐ |
| 波变量通信 | 传 \(u_m, v_s\);接收端延迟解码力/速度 | ⭐⭐ |
| 常数时延无源性 | \(E_{comm} = \frac{1}{2}\int_{t-T}^t u_m^2 + \frac{1}{2}\int_{t-T}^t v_s^2 \geq 0\) 恒成立 | ⭐⭐⭐ |
| 波阻抗 \(b\) | 过小→振铃,过大→粘滞;最优 \(b = \sqrt{K_e M_m}\) | ⭐⭐ |
| 时变时延 | \(\dot{T}>1\) 能量创生;gain scheduling / 能量罐 / TDPA 才是能量层修复,丢弃过期包只是网络卫生 | ⭐⭐⭐ |
| 位置漂移 | 速度积分累积→wave integral + 弱弹簧修复 | ⭐⭐ |
| 离散实现 | ZOH 引入额外被动性约束,需足够高采样频率 | ⭐⭐⭐ |
累积项目:本章新增模块¶
| 章节 | 新增模块 | 功能 |
|---|---|---|
| D05 | 二端口分析 + Z-width | Llewellyn 稳定分析 |
| D06 | 波变量通信模块 | 力/速度→波变换→延迟通道→波逆变换→力/速度 |
| D06 | 无源性监测器 | 区分 \(E_{comm}(t)\)(波域管线储能)与 \(E_{obs}(t)\)(端口功率观测),在线检测能量创生 |
| D06 | 自适应波阻抗 | 在线估计 \(K_e\)→更新 \(b = \sqrt{K_e M_m}\) |
延伸阅读¶
| 资源 | 类型 | 难度 | 关注点 |
|---|---|---|---|
| Anderson-Spong 1989, "Bilateral Control of Teleoperators with Time Delay" | 论文 | ⭐⭐⭐ | 散射变换原始推导——必读 |
| Niemeyer-Slotine 1991, "Stable Adaptive Teleoperation" | 论文 | ⭐⭐⭐ | 波变量定义和物理直觉——与 1989 对读 |
| Niemeyer-Slotine 2004, "Telemanipulation with Time Delays" | 论文 | ⭐⭐⭐ | Wave integral 解决位置漂移 |
| Nuno et al. 2011, "Bilateral Teleoperation of Flexible-Joint Manipulators with Dynamic Gravity Compensation and Time-Varying Delay", Automatica (DOI: 10.1016/j.automatica.2011.01.067) | 论文 | ⭐⭐ | Lyapunov-Krasovskii 柔性关节遥操作——教学参考 |
| Franken et al. 2011, "Bilateral Telemanipulation With Time Delays: A Two-Layer Approach Combining Passivity and Transparency", IEEE Trans. Robot. (DOI: 10.1109/TRO.2011.2142430) | 论文 | ⭐⭐⭐⭐ | 能量罐双层架构——KONTUR-2 采用 |
| Chopra, Spong, Lozano 2008, "Synchronization of Bilateral Teleoperators with Time Delay", Automatica (DOI: 10.1016/j.automatica.2008.01.012) | 论文 | ⭐⭐⭐⭐ | 不使用散射变换的替代方案 |
故障排查手册¶
| 症状 | 可能原因 | 排查步骤 | 相关章节 |
|---|---|---|---|
| 自由空间粘滞感严重 | 波阻抗 \(b\) 过大 | 1. 降低 \(b\) 2. 测量 \(Z_{to}\) 3. 用自适应 \(b\) | 本章 5 |
| 硬墙接触振铃 | \(b\) 过小(阻抗失配) | 1. 增大 \(b\) 2. 估计 \(K_e\) 3. 自适应 \(b\) | 本章 5 |
| 两端位置逐渐漂移 | 速度积分误差 / 丢包 | 1. 检查丢包率 2. 实现 wave integral 3. 加弱弹簧 | 本章 7 |
| 时变延迟下不稳定 | \(\dot{T} > 1\) 能量创生 | 1. 监测波域 \(E_{comm}(t)\) 与端口 \(E_{obs}(t)\) 2. gain scheduling 3. 能量罐或 TDPA 4. 丢弃过期包仅用于乱序治理 | 本章 6, D07 |
| 波变量编解码有噪声 | \(1/\sqrt{2b}\) 放大量化噪声 | 1. 检查力/速度测量精度 2. 波域加低通滤波 3. 增大 \(b\) | 本章 5 |
| 自适应 \(b\) 导致力跳变 | \(b\) 切换时波连续性断裂 | 1. 加 \(b\) 变化率限制 2. 低通滤波 \(b(t)\) 3. 减小自适应增益 \(\alpha\) | 本章 5.4 |
| 包乱序导致波积分错误 | UDP 包到达顺序不一致 | 1. 检查序列号 2. 加接收端排序缓冲 3. 丢弃乱序包 | 本章 8.5 |
| 能量罐持续缩放 | 罐初始值太低或充能不足 | 1. 增大 \(E_0\) 2. 检查充能率 3. 确认 master 端正常输入能量 | 本章 6.3 |
| Wave integral 修复后仍有微漂移 | 数值精度累积 | 1. 使用 double 精度 2. 定期同步位置 3. 增大 \(K_{drift}\) | 本章 7.2 |
D06 到 D07 承接:本章给出了波变量这一"结构性"无源保证方案——在波域中,通信通道天然无源。但工程实践中还面临 ZOH 离散化、控制器 bug、传感器噪声等问题,波变量无法覆盖这些"非结构性"的能量异常。D07 的 TDPA 正是为此设计——它是系统级的"运行时安全网",在线监测能量并实时纠正任何异常。工业系统通常**组合使用**波变量(结构层安全)和 TDPA(运行层安全)——两层独立、互为备份。
D06 的核心收获:散射变换将功率分解为两个非负项之差(\(fv = (u^2-v^2)/2\)),使无源性分析从"力×速度积分"简化为"两个非负量的比较"。这一数学变换是遥操作理论中最优美的成果——时延 \(T\) 不出现在非负性条件中,因为波能量密度(平方量)的积分恒非负,与积分区间宽度无关。理解了这一点,就理解了为什么"传输线不产生能量"——波变量只是将这一物理事实编码进信号表示中。