关节位置模式，导纳算法实现末端柔顺贴合

索旭东

发布于 2026-09-15 20:19:44

960

文章被收录于专栏：具身小站具身小站

基于 CSP 位置模式实现导纳力控贴合，整个系统的核心在于“外环力反馈 → 位置修正 → 内环位置执行”这一协同框架。下面从原理、架构、数据流、同步时序、安全机制以及伪代码流程，完整展开。

一、系统硬件与通信架构

工控机（上位机）：运行导纳控制、运动学、轨迹规划、力信号处理。
总线（EtherCAT/CAN）：实时工业以太网，负责主站（工控机）与从站（伺服驱动器）之间的循环数据交换。
伺服驱动器：各关节工作在 CSP 模式，接收目标位置 qcmd，内部完成位置环、速度环、电流环。
六维力传感器：通过 EtherCAT、Ethernet 或模拟量采集卡与工控机通信，频率可达到 1–2 kHz。

周期设定：

EtherCAT 周期（总线周期）：1 ms（典型）。每个周期主站发送目标位置，接收实际位置、力矩等反馈。
力传感器读取周期：与总线周期同步，或通过硬件中断以 1 kHz 读取。
控制算法周期：与 EtherCAT 周期一致，固定在 1 ms，保证实时性。

二、控制环路的层级关系

[力传感器补偿] → [导纳控制器] → ΔX (笛卡尔)
                              ↓
[参考轨迹 X_ref] → (+) → X_cmd → [逆运动学(Pinocchio)] → q_cmd (关节)
                              ↓
                          [CSP驱动器] → 关节位置闭环 → 实际运动

外环（导纳）：以 1kHz 频率运行，根据力误差计算末端位姿修正量。
内环（CSP）：驱动器以更高频率（电流环 10–20 kHz，速度环/位置环 1–4 kHz）执行关节位置跟踪，确保末端实际运动符合修正后的指令。
运动学模块：在工控机侧实时完成正/逆解，将笛卡尔空间修正映射到关节空间。

三、核心算法模块详解

1. 六维力信号预处理（1 kHz）

每个控制周期，从传感器读取原始数据，依次进行：

零漂去除
重力补偿（利用已辨识的 m,rcom与当前末端姿态）
惯性补偿（利用 Isensor,ω,α,a）
低通滤波（30 Hz 截止）

得到净接触力 Fcontact 和力矩 Mcontact，统一在工具坐标系下表示。

2. 导纳控制器

目标：维持法向力 Fdes（如 3N），同时切向跟随轨迹。

导纳律（K=0 的特例）：

其中 Δx是工具坐标系下的位置修正量（沿法向主要是 Z 向）。

离散实现（双线性变换或欧拉法，1 ms 周期）：

3. 笛卡尔位姿修正与逆运动学

参考轨迹规划器输出当前时刻的期望末端位姿矩阵 Tref（4×4）。

修正后的位姿：将 Δx（工具坐标系 Z 向的平移，可能叠加微小的姿态修正）施加到 Tref：

其中 ΔR 可来自导纳力矩修正（如需调整姿态对齐法向）。

逆运动学：调用 pinocchio.inverseKinematics(model, data, T_{cmd}, q_prev) 求解目标关节角 qcmd。利用冗余度（J6小俯仰）进行零空间优化，使 J6 保持在工作范围中心。

4. CSP 指令发送

将 qcmd写入 EtherCAT 过程数据对象（PDO），在下一个总线周期下发至驱动器。

驱动器接收到新位置指令后，立即更新内部位置环给定，并开始轨迹插补（若支持插补模式）或直接跟踪。由于 CSP 是同步周期位置，驱动器会在每个周期根据最新指令计算速度前馈等，实现平滑运动。

四、完整程序流程（伪代码）

# 初始化
model, data = load_urdf("robot.urdf")  # Pinocchio
q_actual = get_joint_positions()
q_cmd = q_actual
dt = 0.001  # 1 ms

# 导纳状态
dx_adm = 0.0, dx_dot = 0.0
M, D = 2.0, 100.0  # 已调优的参数
F_des = 3.0  # 法向期望力

# 主循环，与 EtherCAT 周期同步
while running:
    # -------- 1. 传感器采集与补偿 --------
    F_raw, M_raw = read_ft_sensor()
    F_contact, M_contact = compensate(F_raw, M_raw, q_actual, dq_actual)  # 重力/惯性补偿

    # -------- 2. 导纳计算 (工具坐标系Z向) --------
    force_z = F_contact[2]  # 假定工具Z轴向外为法向
    e = F_des - force_z

    # 二阶导纳
    acc = (e - D * dx_dot) / M
    dx_dot += acc * dt
    dx_adm += dx_dot * dt
    dx_adm = clamp(dx_adm, -0.005, 0.005)  # 限制修正深度

    # -------- 3. 参考轨迹获取 --------
    T_ref = trajectory_planner(current_time)  # 来自通用模板（螺旋/弓字）

    # -------- 4. 位姿修正 --------
    # 构建修正变换：沿工具Z轴平移dx_adm，无旋转（或根据M_contact做姿态修正）
    T_corr = SE3.identity()
    T_corr.translation = np.array([0, 0, dx_adm])  # 在工具系下
    T_cmd = T_ref * T_corr

    # -------- 5. 逆运动学 --------
    q_cmd = pinocchio.inverse_kinematics(model, data, T_cmd, q_cmd, 
                                         max_iter=10, dt=dt)
    # 零空间优化：保持J6居中
    q_cmd = nullspace_optimization(q_cmd, weight_j6=1.0)

    # -------- 6. 安全检查 --------
    if any(abs(F_contact) > MAX_FORCE) or any(abs(M_contact) > MAX_TORQUE):
        trigger_soft_stop()
        break

    # -------- 7. 发送CSP指令 --------
    send_csp_command(q_cmd)  # 通过 EtherCAT PDO

    # -------- 8. 同步等待下一周期 --------
    wait_next_cycle()
    q_actual = get_joint_positions()
    dq_actual = get_joint_velocities()

五、控制器与驱动器协同的时序图

Time   工控机（主站）                    驱动器（从站）
--------------------------------------------------------------------
t0     发送 q_cmd (cycle N)             收到 q_cmd，开始位置闭环
t1     采集力传感器，执行导纳+IK          正在执行 q_cmd
t2     计算得出新的 q_cmd                 正在执行 q_cmd
t3     发送 q_cmd (cycle N+1)           收到新 q_cmd，平滑过渡
...

驱动器在每个 EtherCAT 周期（如1ms）收到新的目标位置，其内部位置环以更快的周期（如10µs）插补，保证运动平滑。
位置环刚度很高，确保了导纳输出的位置修正量被精确执行，接触力因此可调。

六、柔顺力控原理

导纳控制外环：根据力误差告诉机器人应该往哪移动一点，本质上是在线生成一个虚拟顺从轨迹。
CSP 内环：以极高的刚度和精度执行这个虚拟轨迹，让末端确实移动了那个微小的位移。
因为位移由力误差驱动，最终达到平衡时，力误差为零（或很小），接触力等于期望值。相当于用一个刚硬的机械臂模拟出了一个柔顺的弹簧行为，只不过弹簧是在软件里实现的。

局限性与注意事项：

内环位置环增益不能无限高，否则在刚性接触时可能引起力振荡，通过导纳的阻尼 D提供稳定裕度。
对通信延迟敏感，因此必须采用高速总线（EtherCAT）且控制周期固定。
导纳控制器输出量程需限幅，避免突然的力冲击导致位置跳变。

七、高级扩展（可选）

速度前馈：若轨迹规划器能提供参考速度 X˙ref，可将其转换为关节速度前馈，减轻位置环负担。
力控姿态修正：导纳控制器也可处理力矩误差，产生微小姿态旋转，确保刷头与釉面法向对齐。
过渡阶段管理：非接触时禁用导纳修正（Δx=0），或使用混合状态机平滑切换。

以上完整描述了从力感知到位置指令生成，再到驱动器执行的闭环逻辑，可直接用于工程实现。

本文参与腾讯云自媒体同步曝光计划，分享自微信公众号。

原始发表：2026-08-10，如有侵权请联系 cloudcommunity@tencent.com 删除

cmd