提示词描述

控制框图生成：该控制框图体现了一种“内环速度环+外环力环”的混合力/位置策略。轨迹跟踪模块提供期望的末端执行器速度指令v_pos用于切向运动，作为内环伺服的主要参考。同时，六维力/力矩传感器采集接触力数据，经过转换得到工具坐标系下的法向力F_z。该F_z经过零点偏移校准、幅值限制和二阶IIR低通滤波器以降低噪声。随后，采用卡尔曼滤波器在线分离慢变漂移b(k)，从而得到稳定的法向力反馈F_z,f。外环使用期望法向力F_z,d和F_z,f形成力误差e_f，然后通过一维二阶阻抗/导纳模型Md ẍe + Bd ẋe + Kd xe = e_f计算法向动态响应。经过两次离散积分后，得到法向速度修正v_z，从而构建v_force=[0,0,v_z,0,0,0]^T。最后，使用选择矩阵在速度层面对v_pos和v_force进行合成，得到v_cmd=Sx v_pos+Sf v_force，并以Ts=2 ms的周期发送到机器人的伺服接口执行。这使得系统能够在保持切向轨迹跟踪的同时，在工具的z方向上实现恒力接触，接触力反馈到传感器形成闭环。