프롬프트 설명

제어 블록 다이어그램 생성: 이 제어 블록 다이어그램은 '내부 속도 루프 + 외부 힘 루프'의 하이브리드 힘-위치 전략을 구현합니다. 궤적 추적 모듈은 접선 운동을 위한 원하는 엔드 이펙터 속도 명령 v_pos를 제공하며, 이는 내부 서보 루프의 주요 참조 역할을 합니다. 동시에, 6차원 힘/토크 센서는 접촉력 데이터를 획득하고, 이는 공구 좌표계에서 법선력 F_z를 얻기 위해 변환됩니다. 이 F_z는 영점 오프셋 보정, 진폭 제한 및 2차 IIR 저역 통과 필터를 거쳐 노이즈를 줄입니다. 이어서, 칼만 필터가 느리게 변하는 드리프트 b(k)를 온라인으로 분리하는 데 사용되어 안정적인 법선력 피드백 F_z,f를 생성합니다. 외부 루프는 원하는 법선력 F_z,d와 F_z,f를 사용하여 힘 오차 e_f를 형성하고, 이는 1차원 2차 임피던스/어드미턴스 모델 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=2ms 주기로 실행을 위해 로봇의 서보 인터페이스로 전송됩니다. 이를 통해 시스템은 접선 궤적 추적을 유지하면서 공구의 z 방향으로 일정한 힘 접촉을 달성할 수 있으며, 접촉력은 센서로 피드백되어 폐쇄 루프를 형성합니다.