提示詞描述

控制方塊圖生成：此控制方塊圖體現了一種「內速度迴路 + 外力迴路」的混合力-位置策略。軌跡追蹤模組提供期望的末端執行器速度指令 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 毫秒的週期發送到機器人的伺服介面執行。這使得系統能夠保持切向軌跡追蹤，同時在工具的 z 方向上實現恆力接觸，接觸力反饋到感測器以形成閉環。