在线自适应前馈，实现连续空间的学习控制

针对用已清洁区域的接触力误差，预测修正后续区域轨迹，提供四种工程上可实现的算法方案，从极简到复杂的方案进行展开介绍。

一、问题建模与统一框架

清洁时，机器人末端沿一条空间路径 s 运动，从起点 s=0 到终点 s=L。在每个位置，导纳控制器会根据力误差 e(s)=F_des−F_contact(s)产生一个法向位置修正量 Δx_adm(s)，这个修正量反映了实际曲面与规划轨迹的偏差。

目标是：利用 s<s_cur 的误差信息，生成一个前馈偏移量 x_ff(s)，在 s>s_cur的区域提前修正参考轨迹，从而减小导纳的负担，让贴合力更平稳。

所有方案都遵循这个逻辑：

1. 将路径分段（按距离或时间）。
2. 在已走完的段上提取特征（平均修正量、拟合曲面参数、误差统计等）。
3. 用该特征预测下一段或未来几段的前馈量。
4. 将前馈量叠加到参考轨迹上，导纳只处理残余误差。

二、四种算法方案

方案1：分段均值前馈

原理：假设曲面偏差在空间上是缓变的，直接用上一段的平均导纳修正量作为下一段的前馈偏移。

实现步骤：

• 将路径等分为长度为 Δs（如5cm）的段 Seg_i。
• 在 Seg_i−1 内，记录每个控制周期导纳控制器输出的法向修正量 Δx_adm，求平均 Δ_ˉi−1。
• 在进入 Seg_i 时，将参考轨迹沿法向偏移 x_ff,i=α⋅Δˉ_i−1，其中 α 是遗忘因子（0.5~0.9）。
• 保持导纳控制器继续运行，处理剩余误差。

segment_length = 0.05  # 5cm
current_seg = 0
accum_dx = 0.0
count = 0
dx_ff = 0.0

# 在控制循环中，s 为当前路径位置
if s // segment_length > current_seg:
    # 段结束，计算平均修正
    avg_dx = accum_dx / max(count,1)
    dx_ff = alpha * avg_dx
    # 重置
    current_seg = s // segment_length
    accum_dx = 0.0
    count = 0

# 获取导纳修正量
dx_adm = admittance_control(F_contact)
# 前馈加导纳
total_offset = dx_ff + dx_adm
# 叠加到参考轨迹
X_cmd = X_ref + total_offset * normal_direction
# 记录本段修正量
accum_dx += dx_adm
count += 1

优势：极其简单，无需建模，计算量几乎为零。 劣势：滞后一段，对突变曲面适应性差；可能因噪声产生波动。

方案2：局部曲面拟合预测前馈

原理：利用已走过的几个点接触位置，拟合局部曲面（直线、圆弧、多项式），预测前方的法向和位置，直接修正参考轨迹。

实现步骤：

• 存储最近N个控制周期的实际接触点位置 P_contact（可通过正运动学和导纳修正量反算）。
• 当点数足够（如10个），用最小二乘法拟合一条直线或圆弧。
• 根据拟合模型，计算未来几个周期的预测接触点 P_future，以及与原始参考轨迹的偏差 e
• 将偏差的低频部分作为前馈偏移 x_ff 叠加到轨迹上。
• 更新采用滑动窗口，实时在线。

window_size = 20  # 点
contact_points.append(get_current_contact_point())
if len(contact_points) > window_size:
    contact_points.pop(0)
# 拟合平面曲线（假设外壁可近似为二维轮廓）
# 使用参数 s 作为自变量
z_coords = [p.z for p in contact_points]  # 法向
# 二次多项式拟合 z = a s^2 + b s + c
s_vals = [p.s for p in contact_points]
coeffs = polyfit(s_vals, z_coords, deg=2)
# 预测下几个点的位置
future_s = [s_current + i*ds for i in range(1,5)]
predicted_z = polyval(coeffs, future_s)
# 原始参考轨迹在 future_s 的 Z 坐标
ref_z = [traj_z(s) for s in future_s]
# 前馈偏移为预测偏差的平均
error = mean(predicted_z - ref_z)
dx_ff = lowpass_filter(error, alpha=0.3)

优势：可预判曲率变化，响应快；能处理非缓变曲面。 劣势：依赖位置精度和传感器；拟合可能受噪声影响，需要合适的窗口长度和多项式阶数。

方案3：滑动窗口均值前馈

原理：将空间误差信号通过一个低通滤波器处理，得到的平滑曲线作为前馈信号，相当于一种离线的空间域迭代学习，但因为只使用过去的误差，所以采用在线滤波器实现。

实现步骤：

• 定义一组沿路径的离散存储点（如每1cm一个），或者使用一个环形缓冲区存储最近的误差和修正量。
• 设计一个沿空间方向的低通滤波器（如移动平均或一阶惯性滤波）：

β 是空间滤波系数，决定了历史信息的影响长度；Δx_adm是沿路径距离的导纳修正量，是导纳控制器根据当前力误差实时计算出的位置修正量；x_ff是平滑的前馈输出量，对历史 x_adm序列进行空间低通滤波得到的平滑值

• 当前控制周期，取 x_ff(s)作为前馈，由于它是过去修正量的平滑版本，可以代表曲面的趋势。

# 初始化
dx_ff = 0.0
beta = 0.9  # 空间滤波系数，越大越平滑但滞后越多

# 控制循环
dx_adm = admittance_control(F_contact)  # 导纳修正量
# 更新前馈：用上一次的前馈和这一次的导纳修正做一阶滤波
dx_ff = beta * dx_ff + (1 - beta) * dx_adm
# 总偏移
total_offset = dx_ff + dx_adm
# 叠加到参考轨迹法向
X_cmd = X_ref + total_offset * n

优势：平滑、稳定，能够学习缓慢变化的曲面偏差；实现简单。 劣势：类似于方案1，但有更好的滤波特性；需要调整滤波系数以平衡响应和噪声。

方案4：自适应导纳参数调整（间接前馈）

原理：不直接修正轨迹，而是根据接触状态（力误差的波动幅度、方向）在线调整导纳控制器的阻尼 D 或期望力 F_des，使系统在曲面变化剧烈的区域自动软或硬，减少力尖峰。

实现步骤：

• 监测一段时间内的力误差均方根 RMS(e)。
• 如果 RMS(e) 大于阈值，说明曲面变化快，则降低切向速度或增大法向阻尼 D_z。
• 如果误差持续偏正，则微调期望力。
• 这不能主动预测，但可以提高对未知曲面的鲁棒性。

优势：无需修改参考轨迹，不会引入位置偏差；安全性高。 劣势：不是真正的前馈，无法补偿系统性的轨迹偏差，只能被动适应。

三、方案对比

四、工程落地建议

对于清洁通常为连续曲面，曲率变化不会特别剧烈，推荐优先采用方案1或方案3，因为它们实现简单且无需位置精度，具体步骤：

1. 先用方案3（滑动窗口滤波器）：
   • 实现一个带指数遗忘的滤波器。
   • 在清洁过程中，实时计算出滤波后的前馈偏移。
   • 配合导纳控制器，观察力误差是否更加平稳。
   • 调整滤波系数 β（0.7~0.9）以获得满意响应。
2. 如果效果仍不理想，可叠加方案4：当力波动过大时自动降速或增加阻尼。
3. 只有在有额外视觉或精确位置反馈时才考虑方案2，因为它能提供预测性，适合内壁等复杂曲面。

所有方案都是在线、计算量极小的，可以在 2ms 控制周期内轻松执行，选择其中一种，写几十行代码即可实现，完全不需要控制工程师，普通软件工程师足以胜任。