Promptの説明

制御ブロック図の作成：この制御ブロック図は、「内部速度ループ＋外部力ループ」のハイブリッド力-位置制御戦略を具現化している。軌道追従モジュールは、接線運動のための目標エンドエフェクタ速度指令v_posを提供し、内部サーボループの主要な基準として機能する。同時に、6軸力/トルクセンサが接触力データを取得し、ツール座標系における法線力F_zを得るために変換される。このF_zは、ゼロオフセット校正、振幅制限、およびノイズを低減するための2次IIRローパスフィルタ処理を受ける。その後、カルマンフィルタが、緩やかに変化するドリフトb(k)をオンラインで分離するために使用され、安定した法線力フィードバックF_z,fが得られる。外部ループは、目標法線力F_z,dとF_z,fを使用して力誤差e_fを形成し、これは一次元の2次インピーダンス/アドミタンスモデルMd ẍe + Bd ẋe + Kd xe = e_fによって法線方向の動的応答を計算するために使用される。2回の離散積分後、法線速度補正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方向で一定の力接触を実現でき、接触力はセンサにフィードバックされて閉ループを形成する。