Part II · 第 9 章 机器人形态 · 9.4

执行器与传感器

前面所有章节里的力矩 \(\boldsymbol\tau\) 都是凭空给出的，所有的 \(q,\dot q\) 都是精确读到的。真实的机器人上，每个 \(\tau\) 都要由一台有电压、电流、转速和温度限制的电机加减速器产生，每个 \(q,\dot q\) 都来自一个带量化、噪声、零偏和延迟的传感器。这一节是从数学走向硬件的桥：直流电机的转矩-转速直线、传动比与折算惯量、串联弹性执行器，编码器的量化与差分测速，IMU 的积分漂移与互补滤波，以及延迟如何吃掉相位裕度。仿真到现实的差距，大半就出在这里。

约 35 分钟 前置：4.3 方差 · 4.4 高斯 · 4.8 卡尔曼滤波 · 2.5 ODE · 7.6 静力学 · 8.3 质量矩阵 6 个交互

    1为什么机器人需要它

    你在仿真里训练好一个腿式机器人的策略，放到真机上，机器人抖、软、慢半拍，甚至把电机烧了。几乎每一次，原因都是仿真里的关节是一个理想的力矩源，而真机的关节不是：

    • 力矩有上限，而且随转速下降。电机转得越快，反电动势越大，能推出的电流越小（FIG 1）。"峰值力矩"只在低速时成立。
    • 电机的力矩不是关节的力矩。减速器把力矩放大 \(N\) 倍，却把转子惯量放大 \(N^2\) 倍，还带来摩擦和效率损失（FIG 2）。8.3 节的质量矩阵 \(M(q)\) 的对角线上要加上这一项。
    • 电机会发热。发热正比于电流的平方，所以"持续力矩"远小于"峰值力矩"。
    • 你读到的 \(q\) 和 \(\dot q\) 是估计，不是真值。编码器有量化，差分测速噪声很大；陀螺仪有零偏，积分后越漂越远（FIG 4、5）。
    • 从传感器到电机有延迟。几毫秒的延迟就能让一个在仿真里很稳的 PD 控制器振荡起来（FIG 6）。

    这一节把这五件事逐个做成可以拖动的小实验。它们是 Part IV（PID/阻抗控制、状态估计、视觉惯性里程计）和 Part VI（仿真到现实：执行器模型与执行器网络）的硬件前提。

    2直觉：电机是一个越转越弱的力矩源

    直流电机（无刷电机经磁场定向控制后，等效成同样的模型）只有两条定律。电流产生力矩：\(\tau = k_t\, i\)。转动产生反电动势：绕组在磁场里转，会发出一个方向与供电相反、大小正比于转速的电压 \(k_e\omega\)。电压方程是 \(V = R\,i + k_e\,\omega\)，电感的作用很快（毫秒级），先忽略。

    于是电流 \(i = (V - k_e\omega)/R\)：转得越快，反电动势抵消的电压越多，剩下的电流越少，力矩也越小。转速为 0 时（堵转）电流最大，力矩 \(\tau_s = k_tV/R\) 最大；转到 \(\omega_0 = V/k_e\)（空载转速）时电流为 0，力矩也为 0。力矩-转速关系是一条直线，两个端点就决定了一切：功率、效率和发热都由它导出。

    下图用一台中型无刷电机（参数是示意性的，量级合理）。青色实线是电机能给出的力矩，紫色线是负载要的力矩（恒定部分模拟重力或库仑摩擦，斜率部分模拟黏性阻力），两线的交点就是稳态工作点。左图看力矩，右图看功率、发热和效率。

    FIG 1一台直流电机的全部
    力矩-转速
    功率、铜损、效率 vs 转速
    参数：\(k_t=k_e=0.06\) N·m/A，\(R=0.5\ \Omega\)，绕组热阻 5 K/W、温升上限 90 K，因此持续电流 \(I_c=\sqrt{90/(5\cdot0.5)}=6\) A，持续力矩 0.36 N·m。左图：虚线是不限流的理想直线，实线是驱动器限流后的实际力矩，绿色底是持续区，琥珀色底是只能短时使用的峰值区。右图：琥珀色是机械功率 \(\tau\omega\)，红色是铜损 \(i^2R\)，青色虚线是效率（右轴）。热模型是集总一阶：\(\Delta T_{ss}=i^2R\,R_{th}\)，热容 40 J/K。
    试一试
    • 加大负载力矩，直到电机堵转（转速为 0）。读数里的电流是多少？热模型说它多久会烧到温升上限？
    • 找到机械功率最大的工作点（达到理论值 \(V^2/4R\) 的 95% 以上）。此时转速是空载转速的多少？效率是多少？
    • 不烧电机地长期工作：让电流低于持续电流 6 A，同时输出机械功率超过 100 W。需要高转速还是大力矩？
    核心洞见

    机械功率 \(\tau\omega\) 在半个空载转速处最大，\(P_{\max}=\tau_s\omega_0/4\)，而那里的效率恰好只有 50%：一半的电功率变成了铜损发热。高效率只存在于高转速、小力矩的区域。机器人在抬腿起跳时恰恰处在低速大力矩区，所以"峰值力矩"一用就是热。

    3动手玩：传动比，力矩的杠杆与惯量的平方

    电机的力矩太小、转得太快，机器人关节要的是低速大力矩，于是要加减速比为 \(N\) 的减速器。理想的减速器是一根杠杆：关节侧力矩 \(\tau_j = N\eta\,\tau_m\)（\(\eta\) 是效率），关节侧转速 \(\omega_j=\omega_m/N\)，功率只损失一个 \(\eta\)。

    代价藏在惯量里。转子的动能是 \(\tfrac12J_m\omega_m^2=\tfrac12(N^2J_m)\,\omega_j^2\)，从关节这一侧看，转子的惯量被放大了 \(N^2\) 倍，叫折算惯量（reflected inertia）。给定电机力矩 \(\tau_m\)，让一根转动惯量为 \(J_L\) 的连杆（不受其他外力矩）加速，关节角加速度是

    $$ \alpha_j \;=\; \frac{\hl{tj}{N\eta\,\tau_m}}{J_L + \hl{jrefl}{N^2J_m}} \;\;\Longrightarrow\;\; \text{maximal at}\quad \hl{nstar}{N^\star=\sqrt{J_L/J_m}} . $$

    \(N\) 太小，力矩杠杆不够；\(N\) 太大，转子自己的 \(N^2J_m\) 成了负担，加速度反而下降。最佳点上折算的转子惯量刚好等于负载惯量（"惯量匹配"）。下图左边是 \(\alpha_j\) 随 \(N\) 变化的曲线（双对数），右边是关节侧的力矩-转速包络，也就是 FIG 1 的直线经 \(N\) 变换后的样子。拖动琥珀色的点，设定你的关节需要的力矩和速度。

    FIG 2传动比实验室
    关节角加速度 α vs 传动比 N
    关节侧力矩-转速包络（双对数）
    转子惯量 \(J_m=10^{-4}\) kg·m²，电机同 FIG 1（24 V，峰值电流 30 A，持续电流 6 A），左图用峰值电机力矩 1.8 N·m。虚线是 \(J_L\) 缩小/放大 4 倍时的曲线，圆点是 \(N^\star\)：负载越重，最佳传动比越大。右图：青色区域是峰值能力，绿色是持续能力，灰色虚线是 \(N=1\)，紫色虚线是电机的功率上限 \(\eta V^2/4R\)：改变 \(N\) 只是让包络沿这条功率双曲线滑动，并不增加功率。"足端折算质量"取腿长 0.25 m：\(N^2J_m/\ell^2\)。
    试一试
    • 把 \(N\) 调到最佳传动比 \(N^\star\)（误差 10% 以内）。此时折算的转子惯量和负载惯量之比是多少？再改变 \(J_L\)，\(N^\star\) 怎么变？
    • 让需求点（至少 50 W）落在绿色持续区内。依次试"准直驱""行星""谐波"三个预设：默认的需求点只有哪一个能持续做到？为什么传动比太小和太大都不行？
    核心洞见

    传动比是在力矩与速度之间做交易，并以 \(N^2\) 的价格出售惯量。腿式机器人偏偏不选最佳的 \(N^\star\)，而选很小的 \(N\approx6\text{–}10\)（准直驱，QDD）：折算惯量小，关节几乎可以被外力反向驱动（backdrivable），电流直接反映关节力矩（于是 7.6 节的 \(\boldsymbol\tau=J^\top F\) 可以直接用来估计足端力），落地冲击由连杆自己"让"掉，而不是靠齿轮硬扛。加速度峰值不是唯一的指标；足式机器人的整体设计见 9.2。

    回到 8.3 节的运动方程：把每个电机的转子并进去，有效的质量矩阵是 \(M_{\text{eff}}(q)=M(q)+\operatorname{diag}(N_i^2J_{m,i})\)（忽略转子旋转带来的陀螺耦合）。当 \(N\) 很大时，对角线上的常数项压倒 \(M(q)\) 的位形相关部分，每个关节"看到"的惯量几乎不随姿态变化，关节之间的耦合也变得很弱，这正是工业机械臂可以逐关节独立做 PD 控制的原因；而 QDD 的腿，\(M(q)\) 的位形相关部分和耦合都很显著，需要基于模型的全身控制（Part IV）。

    传动Nη回差可反向驱动特点
    直驱1≈1无是最透明，力矩小而重（大直径力矩电机）
    准直驱 QDD（单级行星）≈ 6–10≈ 0.9小是电流≈力矩，耐冲击；持续力矩受发热限制
    多级行星≈ 20–100≈ 0.7–0.85角分量级费力力矩密度高，摩擦与折算惯量显著
    谐波（应变波）≈ 30–160≈ 0.6–0.85几乎为零很难单级大减速比、体积小；柔性、力矩波动，效率随负载和温度变化
    摆线≈ 10–100+≈ 0.8–0.9小中等抗冲击，刚度高

    表中数字是常见量级，具体型号差别很大，选型时以数据手册为准。

    4第二、第三视角：弹簧、编码器与陀螺仪

    串联弹性执行器：把力矩变成位移

    刚性的减速器很难精确控制力：输出端稍有位移，力就变化很大，摩擦和折算惯量又把真实力矩藏了起来。Pratt 和 Williamson（1995）的办法是在电机与负载之间故意串一根弹簧：弹簧的形变正比于力矩，\(\tau_s=k(\theta_m-\theta_l)\)，于是"测力"变成了"测角度差"，只要两个编码器；力控制问题变成了让电机去追一个弹簧形变。

    代价是带宽。电机转子的惯量与弹簧组成一个二阶系统，固有频率 \(\omega_n=\sqrt{k/J_m}\) 限制了力矩的响应速度；弹簧越软，力矩分辨率越好（同样的编码器分辨率对应更细的力矩 \(k\,\delta\theta\)），耐冲击越强（输出端被撞时，弹簧先吃掉动能，齿轮箱承受的力矩上限是 \(\omega\sqrt{kJ_m}\)），可带宽越低。下图在输出端被锁住（撞墙）或自由两种情形下，用 RK4（2.5 节）仿真对力矩阶跃命令的响应。

    FIG 3串联弹性执行器
    机构与弹簧力矩响应
    带宽、冲击力矩、力矩分辨率 vs 刚度 k
    关节侧等效转子惯量 \(N^2J_m=0.01\) kg·m²（\(N=10\)），电机摩擦 \(b=0.02\) N·m·s/rad，命令是 \(t=0.05\) s 时 0→5 N·m 的阶跃。控制律 \(u=(1+K_p)\tau_{des}-K_p\tau_s-K_d\dot\delta\)（前馈加比例反馈，再对形变速度 \(\dot\delta=\dot\theta_m-\dot\theta_l\) 加阻尼），\(K_d\) 按目标阻尼比 \(\zeta\) 自动整定。"自由"情形负载惯量 0.5 kg·m²，会被拖着转。右图三条线共用纵轴数值：青色为闭环带宽 (Hz)，红色为转子 10 rad/s 撞上锁死输出时弹簧承受的力矩 (N·m)，琥珀色为 14 位关节编码器对应的力矩分辨率 (N·m)；虚线竖线标出刚性齿轮传动的量级。
    • 找到最硬的弹簧，使冲击力矩不超过 15 N·m（落在 13–15 N·m 之间）。你买到了多少带宽？力矩分辨率变成多少？

    执行器家族

    类型力矩/力的来源典型特点代价
    高减速比电动电机 + 减速器，力矩 ≈ \(N\eta k_ti\)力矩密度高，位置控制精确，工业机械臂的主流摩擦、回差、折算惯量，力控制需要力矩传感器
    准直驱 (QDD)低减速比，电流直接估计力矩透明、耐冲击、力矩带宽高（电流环 kHz 量级）发热是瓶颈，需要大电机
    串联弹性 (SEA)弹簧形变 \(k\,\delta\theta\)力矩测量干净，耐冲击，力矩带宽通常是几十 Hz 的量级体积、位置带宽受限，需要第二个编码器
    变刚度 (VSA)弹簧 + 第二个电机改变刚度可调节柔顺性，适合碰撞安全与储能机构复杂、质量大，多为研究原型
    液压油压（约 20 MPa，即 3000 psi 的量级）× 活塞面积功率密度和力密度极高，伺服阀带宽可达几十到上百 Hz泵、油管、泄漏、噪声，效率较低
    气动 / 气动肌肉气压（约 0.3–0.8 MPa）天然柔顺，质量轻，力/重比高空气可压缩，迟滞明显，精度与带宽低（几 Hz 量级）
    人工肌肉（介电弹性体、HASEL、形状记忆合金、绞合聚合物纤维）静电、热或相变驱动应变大、质量轻，仿生高压 (kV)，或热驱动而缓慢，效率低，大多处于研究阶段

    编码器：位置被量化，速度被差分放大

    关节编码器给出的是角度的整数倍：\(B\) 位编码器把一圈分成 \(2^B\) 份，分辨率 \(\Delta=2\pi/2^B\)。量化误差在 \(\pm\Delta/2\) 内，近似均匀分布，标准差 \(\Delta/\sqrt{12}\)。位置上这很小（12 位约 0.0004 rad），但控制器要的往往是速度，而最朴素的做法是差分：\(\hat\omega_k=(\theta_k-\theta_{k-W})/(W T_s)\)。两个量化误差相减、再除以很小的时间，噪声被放大：速度估计只能取 \(\Delta/(WT_s)\) 的整数倍，低速时就是一排 0 与一个脉冲交替的"锯齿"。

    加大窗口 \(W\) 能降噪声（反比于 \(W\)），代价是延迟 \(WT_s/2\)：速度在变化时，估计滞后了。最优的 \(W\) 是噪声误差和滞后误差相等的地方，下图右侧是这条 U 形曲线。

    FIG 4编码器与差分测速
    量化误差（单位 LSB）与速度估计
    速度估计的均方根误差 vs 窗口 W（双对数）
    真实运动：转速 \(v(t)=v_0(1+0.5\sin 4\pi t)\)，即平均 \(v_0\)、2 Hz 起伏。上图上半部分是每个采样点的量化误差 \((\theta_q-\theta)/\Delta\)，永远在 \(\pm0.5\) 个 LSB 内；下半部分紫色是真实速度，琥珀色阶梯是 \(W\) 点差分的估计。右图：红色是仿真测得的均方根误差，青色虚线是量化噪声模型 \(\Delta/(\sqrt6\,WT_s)\)，紫色虚线是滞后模型 \(a_{rms}WT_s/2\)，绿色竖线是两者平衡点的预测 \(W^\star\)。
    • 把 \(W\) 调到误差曲线的最低点附近（与最优值相差不超过 15%）。再把转速 \(v_0\) 降到很低，让估计变成 0 与脉冲交替：最优的 \(W\) 怎么变？

    IMU：陀螺仪准但会漂，加速度计不漂但有噪声

    惯性测量单元（IMU）里的陀螺仪测角速度 \(\omega_g=\dot\theta+b+n\)（零偏 \(b\)，白噪声 \(n\)）。积分得到角度，零偏变成线性增长的漂移 \(b\,t\)，白噪声变成随机游走，标准差 \(N\sqrt t\)（\(N\) 叫角度随机游走系数，常用 °/√s 或 °/√h；数据手册里的"噪声密度"，单位 °/s/√Hz，数值上就是 \(N\)）。几十秒之后，漂移大到不能用。

    加速度计在静止或匀速时测到的是重力方向，\(\theta_{acc}=\operatorname{atan2}(a_x,a_z)\)：不漂，但噪声大，而且一旦机器人自己有加速度（起步、落地、振动）就被污染。两者的缺陷在频率上是互补的：陀螺仪的高频好、低频差，加速度计的低频好、高频差。于是用一个交叉频率 \(f_c\) 把它们接起来：低通的加速度计 + 高通的积分陀螺仪，叫互补滤波器。图中的真实姿态是 \(15^\circ\sin(2\pi\cdot0.2t)+4^\circ\sin(2\pi\cdot1.1t)\)，第 11–12 秒有 0.15 g 的水平加速度干扰。

    FIG 5陀螺仪 + 加速度计 → 互补滤波
    俯仰角 (°) vs 时间 (s)
    融合误差均方根 vs 交叉频率 fc
    采样 100 Hz，随机数由固定种子生成（点"换一组噪声"换种子）。紫色是真实角度，红色是只积分陀螺仪，青色是只用加速度计，琥珀色是互补滤波。默认零偏 0.5 °/s 是出厂校准之后还会剩下的开机零偏的量级；加速度计噪声 0.03 g 包含了振动，远大于电子噪声本身。右图：融合误差随 \(f_c\) 变化，U 形的左边是零偏没被压住，右边是加速度计的噪声和干扰漏了进来。
    • 找到一个 \(f_c\)，使融合误差同时小于"只用陀螺仪"和"只用加速度计"，并且第 11–12 秒的加速度干扰造成的最大误差小于 5°。
    • 把 \(f_c\) 提到 3 Hz 以上，看干扰期间融合角度被拉偏多少（超过 8°）。滤波器此时几乎"相信"加速度计。
    互补滤波是最简单的卡尔曼滤波

    固定的 \(f_c\) 等于固定的卡尔曼增益。4.8 节的卡尔曼滤波把状态扩成"角度 + 陀螺零偏"，按两个传感器的噪声方差自动算出最优增益，并且把零偏估计出来：稳态零偏误差 \(b\tau\) 会变成 0，不用在"压制漂移"和"抵抗干扰"之间折中。真实机器人的姿态估计（EKF、Mahony/Madgwick 滤波器，三维时作用在 \(\SO\) 上，见 5.2 节与 5.5 节）都是这个想法的推广。

    传感器分类

    按"测的是机器人自己还是外部世界"分为本体感觉（proprioceptive）和外部感觉（exteroceptive）。数字是常见量级，以具体型号为准。

    传感器测什么典型速率 / 量级主要误差
    关节编码器（光电 / 磁）\(q\)随控制环读取（kHz）；12–20 位 / 圈量化、非线性、读取延迟
    电机电流\(\tau\approx N\eta k_ti\)电流环 10 kHz 量级摩擦、效率 \(\eta\)、\(k_t\) 随温度漂移
    关节力矩传感器 / SEA 弹簧\(\tau\)约 1 kHz温漂、交叉耦合、过载
    IMU\(\omega,\ a\)100 Hz 到 1 kHz 以上；MEMS 陀螺噪声密度约 0.005–0.02 °/s/√Hz零偏、温漂、振动
    六维力/力矩传感器wrench \(F\)约 1 kHz温漂、重力补偿、零点
    触觉（阵列、视触觉）接触位置、压力、形变10–100 Hz 量级迟滞、标定、耐久
    相机 / 立体 / 深度相机图像、深度30–120 Hz；立体深度误差随距离平方增长曝光、运动模糊、同步、反光
    LiDAR三维点云机械旋转式 10–20 Hz；量程百米以上，测距误差厘米级运动畸变、雨雾、镜面
    动作捕捉外部标定的位姿百 Hz 量级，亚毫米精度（实验室）遮挡、标记点脱落、无法出实验室

    5再看数学

    提示：把鼠标悬停在公式中带底色的符号上，FIG 1 与 FIG 2 中对应的位置会被高亮。

    直流电机方程

    电与机械两个方程，通过 \(k_t\) 与 \(k_e\) 耦合：

    $$ V = R\,i + L\,\frac{di}{dt} + k_e\,\omega,\qquad \tau = k_t\,i,\qquad J_m\dot\omega = \tau - \tau_L . $$

    在国际单位制里 \(k_t\)（N·m/A）与 \(k_e\)（V·s/rad）数值相同：反电动势功率 \(k_e\omega\, i\) 就是机械功率 \(\tau\omega = k_t i\,\omega\)，能量守恒要求 \(k_e=k_t\)。稳态时（\(di/dt=0\)）得到力矩-转速直线，两个端点和最大功率是

    $$ \tau(\omega) = \frac{k_t}{R}\big(V - k_e\omega\big),\qquad \hl{stall}{\tau_s = \frac{k_tV}{R}},\qquad \hl{noload}{\omega_0 = \frac{V}{k_e}},\qquad \hl{pmax}{P_{\max} = \max_\omega \tau\omega = \frac{\tau_s\omega_0}{4} = \frac{V^2}{4R}}\ \ \text{at}\ \ \omega=\tfrac{\omega_0}{2}. $$

    推导：\(P(\omega)=\tau_s\,\omega\,(1-\omega/\omega_0)\) 是开口向下的抛物线，在 \(\omega_0/2\) 处取最大值 \(\tau_s\omega_0/4\)。数值检查（FIG 1 的参数）：\(\tau_s=0.06\cdot24/0.5=2.88\) N·m，\(\omega_0=24/0.06=400\) rad/s，\(P_{\max}=2.88\cdot400/4=288\) W \(=24^2/(4\cdot0.5)\)。铜损与效率：

    $$ P_{cu} = i^2R = \frac{\tau^2}{K_m^2},\quad K_m=\frac{k_t}{\sqrt R};\qquad \eta = \frac{\tau\omega}{\tau\omega + i^2R} = \frac{\omega}{\omega_0}\ \ (\text{voltage-limited}),\qquad T_{mech}=\frac{R J_m}{k_tk_e}. $$

    \(K_m\)（N·m/√W）是电机的"品质因数"：要的力矩一定，发热就是 \(\tau^2/K_m^2\)，与绕组怎么绕（匝数换 \(k_t\) 与 \(R\)）无关。力矩加倍，发热变 4 倍，所以持续力矩由 \(I_c=\sqrt{\Delta T_{\max}/(R\,R_{th})}\) 决定，远低于峰值。\(T_{mech}\) 是机电时间常数：给电机一个电压阶跃，转速以此为时间常数指数趋近 \(\omega_0\)（FIG 1 的电机约 14 ms）。驱动器限流时，等效电压被降低，效率不再是 \(\omega/\omega_0\) 而是用 \(\tau\omega/(\tau\omega+i^2R)\) 计算，图中的曲线用的就是这个。无刷电机在磁场定向控制（FOC）下，把三相电流变换到转子的 d-q 坐标，力矩只由 q 轴电流决定，上面的公式照用，只要用等效的 \(k_t\)、\(R\)。

    传动比、折算惯量与最佳 \(N\)

    理想减速器的约束 \(\theta_m=N\theta_j\)。功率守恒（含效率）给出 \(\tau_j\omega_j=\eta\,\tau_m\omega_m\)，即 \(\tau_j=N\eta\tau_m\)。把转子的动能折算到关节一侧，得到关节侧的运动方程（转子与齿轮的惯量并入 \(J_m\)）：

    $$ \big(J_L + N^2J_m\big)\,\dot\omega_j \;=\; N\eta\,\tau_m - \tau_{ext},\qquad \alpha_j(N)=\frac{N\eta\tau_m}{J_L+N^2J_m},\qquad \frac{d\alpha_j}{dN}=0\ \Rightarrow\ N^\star=\sqrt{\frac{J_L}{J_m}},\quad \alpha_j^\star=\frac{\eta\,\tau_m}{2\sqrt{J_LJ_m}} . $$

    推导：\(\frac{d}{dN}\frac{N}{J_L+N^2J_m}=\frac{J_L-N^2J_m}{(J_L+N^2J_m)^2}\)，分子为零即 \(N^2=J_L/J_m\)，此时 \(N^2J_m=J_L\)。数值检查：\(J_L=0.03,\ J_m=10^{-4}\) 得 \(N^\star=17.3\)，\(\alpha^\star=0.9\cdot1.8/(2\sqrt{0.03\cdot10^{-4}})=467\) rad/s²，在 node 里对 \(N\) 做网格搜索得到同样的峰值。前提要写清楚：负载只有惯性（无重力、无摩擦），电机力矩与效率不随 \(N\) 变化；如果还要抵抗重力力矩 \(\tau_g\)，需要 \(N\eta\tau_m\ge\tau_g\)，如果关节要转得快，还需要 \(N\le\omega_0/\omega_{j,\max}\)。在 8.3 节的符号下，所有关节合起来是 \(M_{\text{eff}}(q)=M(q)+\operatorname{diag}(N_i^2J_{m,i})\)。折算也适用于摩擦：电机侧的库仑摩擦 \(\tau_f\) 在关节侧是 \(N\tau_f\)，它正是不能反向驱动的原因。

    串联弹性执行器的传递函数

    令 \(\delta=\theta_m-\theta_l\)。运动方程：

    $$ J_m\ddot\theta_m = u - k\,\delta - b\dot\theta_m,\qquad J_L\ddot\theta_l = k\,\delta - \tau_{ext},\qquad \tau_s = k\,\delta . $$

    输出锁死（\(\theta_l=0\)）时，开环 \(\tau_s/u=k/(J_ms^2+bs+k)\)，固有频率 \(\omega_n=\sqrt{k/J_m}\)。取控制律 \(u=(1+K_p)\tau_{des}-K_p\tau_s-K_d\dot\delta\)，闭环为

    $$ \frac{\tau_s(s)}{\tau_{des}(s)}=\frac{\omega_c^2}{s^2+2\zeta\omega_c s+\omega_c^2},\quad \omega_c^2=\frac{k(1+K_p)}{J_m},\quad 2\zeta\omega_c=\frac{b+K_d}{J_m},\quad \omega_{bw}=\omega_c\sqrt{1-2\zeta^2+\sqrt{(1-2\zeta^2)^2+1}} . $$

    直流增益为 1（前馈项保证了稳态无偏差）。\(\zeta=0.7\) 时 \(\omega_{bw}\approx1.01\,\omega_c\)，我们用 node 对 \(k=100\)、\(K_p=2\) 的仿真验证过：\(|H(j\omega_{bw})|=0.707\)，超调 4.6%，与二阶系统的 \(e^{-\pi\zeta/\sqrt{1-\zeta^2}}=4.6\%\) 一致。带宽 \(\propto\sqrt k\)。力矩分辨率：编码器分辨率 \(\Delta_\theta=2\pi/2^B\)，对应 \(\delta\tau=k\,\Delta_\theta\propto k\)。冲击：转子以关节侧速度 \(\omega\) 撞上锁死的输出，能量 \(\tfrac12J_m\omega^2=\tfrac12\tau_{imp}^2/k\)，所以 \(\tau_{imp}=\omega\sqrt{kJ_m}\propto\sqrt k\)。带宽、冲击与分辨率都随 \(k\) 变，没有免费的午餐。

    编码器：量化噪声与差分

    量化误差 \(e\sim U(-\Delta/2,\Delta/2)\)，\(\operatorname{Var}(e)=\Delta^2/12\)（4.3 节：均匀分布的方差 \((b-a)^2/12\)）。\(W\) 点差分 \(\hat\omega=(\theta_q[k]-\theta_q[k-W])/(WT_s)\) 含两个独立的量化误差，方差 \(2\Delta^2/12=\Delta^2/6\)。再有加速度 \(a\) 时的滞后（估计的是窗口中点的速度）：

    $$ q=\frac{\Delta}{WT_s},\qquad \sigma_{\text{noise}}=\frac{q}{\sqrt6},\qquad e_{\text{lag}}\approx a\,\frac{WT_s}{2},\qquad W^\star=\Big(\tfrac23\Big)^{1/4}\sqrt{\frac{\Delta}{a_{rms}T_s^2}} . $$

    \(W^\star\) 来自最小化 \(\Delta^2/(6W^2T_s^2)+a^2W^2T_s^2/4\)。FIG 4 的默认参数（12 位，1 kHz，\(v_0=3\) rad/s，\(a_{rms}=4.44\,v_0\)）预测 \(W^\star\approx9.7\)，仿真测得的最低点在 \(W=10\)。注意 \(q/\sqrt6\) 是对所有速度取平均的结果：真实速度恰在两个量化台阶之间的某处时，估计在相邻两个值之间跳，均方根是 \(q\sqrt{p(1-p)}\)，\(p\) 是小数部分；对 \(p\) 均匀分布平均得 \(q/\sqrt6\)（我们用 node 对近千个恒定速度验证为 \(0.419\,q\)，理论 \(0.408\,q\)）。

    陀螺积分与互补滤波

    陀螺读数 \(\omega_g=\dot\theta+b+n\)，\(\mathbb E[n(t)n(t')]=N^2\delta(t-t')\)（白噪声，4.4 节）。积分后

    $$ \theta_g(t)-\theta(t) = b\,t + N\,B(t),\qquad \operatorname{std}=N\sqrt t\ \ (B\ \text{a Wiener process}),\qquad \text{bias dominates once}\ t>(N/b)^2 . $$

    互补滤波器的连续形式与离散形式（\(\tau=1/(2\pi f_c)\)，\(\alpha=\tau/(\tau+T_s)\)）：

    $$ \hat\theta=\underbrace{\frac{1}{\tau s+1}}_{G_{lp}}\theta_{acc}+\underbrace{\frac{\tau s}{\tau s+1}}_{G_{hp}}\frac{\omega_g}{s},\quad G_{lp}+G_{hp}=1;\qquad \hat\theta_k=\alpha\big(\hat\theta_{k-1}+\omega_{g,k}T_s\big)+(1-\alpha)\,\theta_{acc,k} . $$

    因为 \(G_{lp}+G_{hp}=1\)，真实的姿态信号无失真地通过（精确传感器下没有滞后）；只有传感器的缺陷被各自的滤波器处理。零偏造成稳态误差：令 \(e=\hat\theta-\theta\)，得 \(\dot e=b-e/\tau\)，所以 \(e_{ss}=b\,\tau\)（\(b=0.5^\circ\)/s、\(f_c=0.1\) Hz 时 0.80°，仿真 0.794°）。加速度计的白噪声 \(\sigma_a\) 经低通后方差为 \(\sigma_a^2(1-\alpha)/(1+\alpha)\approx\sigma_a^2T_s/(2\tau)\)。减小 \(f_c\) 压低噪声与干扰，增大 \(f_c\) 压低零偏误差，U 形曲线的最低点是折中。

    延迟与相位裕度

    单关节惯量 \(J\) 由 PD 控制 \(u=K_p(q_d-q)-K_d\dot q\)，总回路延迟 \(T_d\)（传感器滤波 + 计算 + 通信 + 放大器 + 零阶保持的半个采样周期），开环传递函数是

    $$ L(s)=\frac{(K_p+K_ds)\,e^{-sT_d}}{Js^2},\quad \omega_c^2=\frac{K_d^2+\sqrt{K_d^4+4J^2K_p^2}}{2J^2},\quad \mathrm{PM}=\arctan\frac{K_d\omega_c}{K_p}-\omega_cT_d,\quad T_{d,\max}=\frac{\arctan(K_d\omega_c/K_p)}{\omega_c} . $$

    \(\omega_c\) 来自 \(|L(j\omega_c)|=1\)，即 \(J^2\omega^4-K_d^2\omega^2-K_p^2=0\)。延迟不改变幅值，只让相位线性下降 \(\omega T_d\)。取 \(\zeta=0.7\)（\(K_p=J\omega_n^2\)，\(K_d=2\zeta\omega_nJ\)）：\(\omega_c=1.54\,\omega_n\)，无延迟的 \(\mathrm{PM}=65^\circ\)，延迟裕度 \(T_{d,\max}\approx0.74/\omega_n\)。\(\omega_n=40\) rad/s 时是 18.4 ms，FIG 6 的仿真验证了：延迟取裕度的 0.8 倍衰减、1.2 倍发散。经验：回路延迟乘以截止频率，\(\omega_cT_d\)，要远小于 0.5 rad，才能留下像样的裕度。

    6机器人中的它：真实的关节是一个有延迟、会饱和的闭环

    把前面几块拼起来，一个真实关节的力矩链条是：传感器 → 滤波 → 控制器 → 通信 → 放大器与电流环 → 电机 → 减速器 → 连杆。每一段都有延迟，最后一段还有 FIG 1 的力矩-转速限制。下图让一根惯量 \(J=0.05\) kg·m² 的连杆做 PD 位置阶跃（0.5 rad）：滑块改变回路延迟和控制带宽 \(\omega_n\)，开关可选"理想力矩源"或"饱和在 12 N·m"。下半部分是相位裕度随延迟的变化。三个预设对应三种典型的软硬件栈，延迟的构成（毫秒）是示意性的量级。

    FIG 6延迟预算与相位裕度
    阶跃响应 q(t) 与相位裕度 PM(Td)
    控制器 \(K_p=J\omega_n^2\)，\(K_d=2\cdot0.7\,\omega_nJ\)。延迟用环形缓冲区实现（步长 0.2 ms，半隐式欧拉）。上图：琥珀色是带延迟的响应，灰色细线是无延迟的响应，紫色虚线是目标。下图：\(\mathrm{PM}(T_d)=\mathrm{PM}_0-\omega_cT_d\) 是一条直线，与零线的交点就是延迟裕度；红色区域是不稳定的延迟。饱和之后，线性分析不再严格成立：延迟超过裕度时系统不会发散到无穷，而是被力矩限制"夹"成一个极限环。
    • 把延迟拉过延迟裕度，让回路失稳（或进入极限环）。裕度是多少毫秒？它是否等于 \(0.74/\omega_n\)？
    • 保持 10 ms 以上的延迟，通过降低带宽 \(\omega_n\) 把相位裕度恢复到 40° 以上。这就是"为什么真机的增益比仿真里调得低"。

    最简单的执行器模型就是把仿真器给出的 PD 力矩按直流电机特性裁剪：\(\tau=\operatorname{clip}\big(\tau_{des},\,\pm\tau_{\max}(\omega)\big)\)，\(\tau_{\max}(\omega)=\min\big(\tau_{pk},\ \tau_s(1-|\omega|/\omega_0)\big)\)，再加一个延迟缓冲区。Isaac Lab 等仿真框架里的直流电机执行器模型做的就是这件事。更精确的办法是 Hwangbo 等（2019）的执行器网络：在真机（ANYmal，串联弹性执行器）上记录"位置误差、速度的短历史 → 实际力矩"，训练一个小网络代替解析模型，放进仿真后策略几乎无需调整就能迁移。

    真实系统里

    关节力矩传感：Franka Emika Panda 和 KUKA LBR iiwa 在每个关节里装了力矩传感器，控制环 1 kHz，因此能做阻抗控制和碰撞检测；UR 系列主要从电机电流估计关节力矩，e 系列在法兰处另有一个六维力/力矩传感器。腿式机器人：MIT Cheetah / Mini Cheetah 把低减速比（Mini Cheetah 为 6:1 行星减速）与大尺寸无刷电机结合，称为"本体感觉执行器"：靠电流估计足端力，靠小折算惯量承受落地冲击（Seok et al. 2013；Wensing et al. 2017；Katz et al. 2019）；Unitree 的四足与人形关节采用同一思路，减速比约 6–10；ANYmal 用串联弹性执行器（ANYdrive）；较早的 Atlas 用液压。伺服与驱动器：Dynamixel 一类的智能舵机把编码器、减速器、驱动和位置环封装在一起，可选位置、速度或电流（力矩）模式；无刷电机的驱动器（ODrive、VESC、moteus 等）用 FOC，电流环在 10 kHz 量级。总线与实时：EtherCAT 把多关节的控制环固定在 1 kHz 左右并保证抖动在微秒量级；CAN 与 USB 串口抖动较大。外部传感：RealSense、ZED 提供 30–90 Hz 的立体/深度图像；动作捕捉（Vicon、OptiTrack）提供实验室里的"真值"，用于标定和评估。IMU 与相机的融合（视觉惯性里程计）见 Part IV。

    7常见误区

    误区 1：把电机力矩当成关节力矩

    关节力矩是 \(N\eta\,\tau_m\) 再减去摩擦（电机侧的摩擦在关节侧被放大 \(N\) 倍），要加速关节还要同时推动 \(N^2J_m\) 的转子惯量。用电流估计关节力矩在 QDD 上很好（\(N\) 小、\(\eta\) 高），在 100:1 的谐波减速器上，效率随负载、温度和方向变化 10–30 个百分点，电流估计出来的力矩可能偏差很大，这时才需要真正的力矩传感器。

    误区 2：峰值力矩可以一直用

    发热 \(\propto\tau^2\)：FIG 1 里的 1.8 N·m 峰值力矩是持续力矩 0.36 N·m 的 5 倍，发热是 25 倍，堵转时只能撑 8 秒左右。机器人站立不动时，关节在重力下保持姿态，电机堵转似的发热却一点也不少，这是腿式机器人"站着不动也会烫"的原因。设计和仿真都应该给出持续力矩、峰值力矩、以及占空比（或热模型）。

    误区 3：直接对量化的编码器求导

    一步差分的速度只能取 \(\Delta/T_s\) 的整数倍，低速时是 0 与脉冲交替，PD 控制器的 D 项把它放大成力矩抖动，电机发出"嘶嘶"声。加长窗口、低通滤波、用观测器（卡尔曼滤波）、或者改用高分辨率编码器，都是在噪声和滞后之间折中（FIG 4）。反过来，在仿真里直接用精确的 \(\dot q\) 训练的策略，到了真机上会因为速度反馈噪声而失效。

    误区 4：直接积分陀螺仪，或在机器人加速时相信加速度计

    零偏 0.5 °/s 积分 60 秒就是 30°；而加速度计在起步、急停、落地时测到的是"重力 + 自身加速度"，倾角估计错得离谱（FIG 5 里 0.15 g 就错 8.5°）。两者要融合，并且要估计零偏。三维旋转要积分在 \(\SO\) 上（5.2 节、5.6 节），不能把三个角速度分量直接当欧拉角的导数积分。陀螺零偏也会随温度和时间漂（"零偏不稳定性"），不是一次标定就能解决。

    误区 5：忽略延迟和抖动，把仿真里的关节当成理想力矩源

    仿真里的关节通常是"今天算出力矩，当步立刻施加，且无限大也施加得出来"。真机上有几毫秒的延迟、不均匀的采样（抖动）、力矩饱和、摩擦、回差和柔性。FIG 6 里 18 ms 的延迟就能让带宽 40 rad/s 的环失稳；而一个"位置控制"的舵机在仿真里常被建模成"直接到达目标位置"，真实的内部位置环有自己的带宽和饱和。仿真到现实的训练要在仿真里随机化延迟、摩擦、电机常数，或者像 Hwangbo 等那样直接学出执行器模型。

    8小练习

    1. 理想直流电机：\(V=12\) V，\(R=1\ \Omega\)，\(k_t=k_e=0.1\)。它能输出的最大机械功率，以及此时的转速和效率是？
    \(\tau_s=k_tV/R=1.2\) N·m，\(\omega_0=V/k_e=120\) rad/s，\(P_{\max}=\tau_s\omega_0/4=V^2/(4R)=36\) W，发生在 \(\omega_0/2=60\) rad/s，那里的电流是堵转电流的一半，效率 \(\omega/\omega_0=50\%\)。在 \(\omega_0\) 处电流和力矩都是 0，功率也是 0。
    2. 转子惯量 \(J_m=10^{-4}\) kg·m²，要加速的连杆惯量 \(J_L=0.09\) kg·m²（只受电机力矩）。让关节角加速度最大的传动比是多少？此时折算转子惯量是多少？
    \(N^\star=\sqrt{J_L/J_m}=\sqrt{900}=30\)，折算转子惯量 \(N^2J_m=900\cdot10^{-4}=0.09=J_L\)：惯量匹配。\(N\) 更大时转子的 \(N^2J_m\) 超过负载，加速度反而降低。
    3. 12 位编码器，1 kHz 采样，用相邻两次读数的一步差分估计速度。速度估计的最小"台阶"（量化步长）是多少？
    \(\Delta=2\pi/4096=1.534\times10^{-3}\) rad，\(q=\Delta/T_s=1.534\times10^{-3}\cdot1000=1.53\) rad/s（0.0015 是 \(\Delta\) 本身，忘了除以 \(T_s\)）。关节以 1 rad/s 转动时，估计会在 0 与 1.53 rad/s 之间跳动，平均值正确，但瞬时值噪声巨大。
    4. 陀螺零偏 \(b=0.5^\circ\)/s，用时间常数 \(\tau=2\) s 的互补滤波器（其余传感器完美）融合，稳态倾角误差是多少？
    \(\dot e=b-e/\tau\)，稳态 \(e_{ss}=b\tau=0.5\cdot2=1^\circ\)。加速度计把误差"拉住"了，所以不再无限增长，但固定增益的互补滤波器并没有估计出零偏，留下一个与 \(\tau\) 成正比的常值误差；想让它为 0，要把零偏放进状态里（卡尔曼滤波，或加积分项的 Mahony 滤波器）。
    5. 一个 PD 位置环的截止频率是 \(\omega_c=40\) rad/s。通信链路又增加了 10 ms 的延迟，相位裕度损失多少？
    延迟 \(T\) 在截止频率处贡献相位 \(-\omega_cT=-40\cdot0.01=-0.4\) rad \(=-22.9^\circ\)（0.4 是弧度，不是度）。延迟不改变幅值，只让相位随频率线性下降，所以高带宽的回路对延迟最敏感。

    9延伸阅读

    • M. W. Spong, S. Hutchinson & M. Vidyasagar, Robot Modeling and Control, "独立关节控制"一章（2006 年版第 6 章） — 直流电机方程、齿轮折算、执行器动力学与单关节控制
    • B. Siciliano, L. Sciavicco, L. Villani & G. Oriolo, Robotics: Modelling, Planning and Control, Ch. 5 — 执行器与传感器：电机、传动、编码器、力传感器、视觉
    • G. A. Pratt & M. M. Williamson, Series elastic actuators, IEEE/RSJ IROS 1995 — 串联弹性执行器的原始论文
    • S. Seok, A. Wang, M. Y. Chuah, D. Otten, J. Lang & S. Kim, Design principles for highly efficient quadrupeds and implementation on the MIT Cheetah robot, IEEE ICRA 2013 — 为什么腿式机器人需要大力矩电机与小传动比
    • P. M. Wensing, A. Wang, S. Seok, D. Otten, J. Lang & S. Kim, Proprioceptive actuator design in the MIT Cheetah: impact mitigation and high-bandwidth physical interaction for dynamic legged robots, IEEE Trans. Robotics 33(3), 2017 — 折算惯量、冲击与本体感觉执行器
    • J. Hwangbo et al., Learning agile and dynamic motor skills for legged robots, Science Robotics 4(26), 2019 — 用神经网络建模执行器（执行器网络），实现仿真到现实的迁移
    • O. J. Woodman, An introduction to inertial navigation, University of Cambridge Computer Laboratory Technical Report UCAM-CL-TR-696, 2007 — 陀螺与加速度计的误差模型：噪声、零偏、积分漂移
    • R. Mahony, T. Hamel & J.-M. Pflimlin, Nonlinear complementary filters on the special orthogonal group, IEEE Trans. Automatic Control 53(5), 2008 — 把互补滤波推广到三维姿态，并估计陀螺零偏