솔루션을 다시 구현하면 다음과 같은 결과를 얻습니다.
벡터 사이의 각도
먼저 점 와 사이의 각도 ( 특히 단위 벡터가 아닌 )를 원합니다 .
AB
( Fx 프로그래밍 사용 ) :
θ=math.atan2(Bx−Ax,By−Ay)
차량 요 각도
다음으로 (그리고 이것이 당신의 문제라고 생각 합니다) 계산 된 에서 차량의 요 각도 를 빼야 합니다 . ψθ
제목 대 요
차량의 "요잉 각도"에 나침반을 사용하는 경우 이는 실수 일 수도 있습니다. 제목과 요는 동일하지 않습니다 . 나침반 방향은 양의 축을 따라 0이며 시계 방향으로 회전함에 따라 증가합니다 .y
Yaw는 양의 축을 따라 0이며 시계 반대 방향 으로 회전함에 따라 증가합니다 .x
원하는 요 (yaw)에서 비히클 요 (빼기 대신)를 추가하는 것과 결합 된 이러한 측정 사이의 90도 겹침은 대상이 ± 5 ° 이내이고 ± 90 °에서 잘못 작동했을 때 작동하는 이유 일 수 있습니다.
성분 X 및 Y로 변환
여기에서이 결과 를 및 구성 요소로 변환하여 롤 및 피치 각도로 로봇에 전달한다고합니다. 위의 수정으로이 시점에서 원하는 결과를 얻을 수 있습니다. 그러나 차량의 속도 (실제로 운동량)가 아니라 위치 차이 만 고려하기 때문에 이러한 구성 요소를 경사 각도에 직접 매핑하는 것은 문제가 될 수 있습니다.(θ−ψ)xy
PID 제어
차량의 롤 및 피치에 PID 제어 루프를 사용하는 것이 가장 좋습니다. 즉, 일단 코드를 수정하고 목표를 달성 할 수 있다면, 대신에 오버 슈트를 시작하여 앞뒤로 진동하는 것 같습니다. 올바르게 조정 된 PID는 대상에 빠르게 접근 할 수 있도록하면서 발생하지 않도록합니다.
와 를 롤과 피치 에 연결하는 대신 롤과 피치 PID가 입력으로 허용 하는 오류 값으로 간주하십시오 .xy