쿼드 로터에서 일하고 있습니다. - 나는 그것의 위치를 알고 , 내가 가고 싶은 곳 - 목표 위치 , 및으로부터 I 계산 벡터 - 내 목표에 데려다 것 단위 벡터 :$a$$b$$c$

c = b - a
c = normalize(c)

쿼드 로터는 회전없이 어떤 방향 으로든 움직일 수 있기 때문에 내가 시도한 것은

1. 로봇 요 각도로 회전$c$
2. 성분 으로 나눕니다$x, y$
3. 롤과 피치 각도로 로봇에 전달하십시오.

문제는 요가 0 ° ± 5이면 작동하지만 요가 +90 또는 -90에 가까워지면 실패하고 잘못된 방향으로 조정됩니다. 내 질문은 여기에 분명한 것이 빠져 있습니까?

솔루션을 다시 구현하면 다음과 같은 결과를 얻습니다.

벡터 사이의 각도

먼저 점 와 사이의 각도 ( 특히 단위 벡터가 아닌 )를 원합니다 . $A$$B$

( Fx 프로그래밍 사용 ) : $\theta = math.atan2(B_{x}-A_{x}, B_{y}-A_{y})$

차량 요 각도

다음으로 (그리고 이것이 당신의 문제라고 생각 합니다) 계산 된 에서 차량의 요 각도 를 빼야 합니다 . $\psi$$\theta$

제목 대 요

차량의 "요잉 각도"에 나침반을 사용하는 경우 이는 실수 일 수도 있습니다. 제목과 요는 동일하지 않습니다 . 나침반 방향은 양의 축을 따라 0이며 시계 방향으로 회전함에 따라 증가합니다 .$y$

Yaw는 양의 축을 따라 0이며 시계 반대 방향 으로 회전함에 따라 증가합니다 .$x$

원하는 요 (yaw)에서 비히클 요 (빼기 대신)를 추가하는 것과 결합 된 이러한 측정 사이의 90도 겹침은 대상이 ± 5 ° 이내이고 ± 90 °에서 잘못 작동했을 때 작동하는 이유 일 수 있습니다.

성분 X 및 Y로 변환

여기에서이 결과 를 및 구성 요소로 변환하여 롤 및 피치 각도로 로봇에 전달한다고합니다. 위의 수정으로이 시점에서 원하는 결과를 얻을 수 있습니다. 그러나 차량의 속도 (실제로 운동량)가 아니라 위치 차이 만 고려하기 때문에 이러한 구성 요소를 경사 각도에 직접 매핑하는 것은 문제가 될 수 있습니다.$(\theta-\psi)$$x$$y$

PID 제어

차량의 롤 및 피치에 PID 제어 루프를 사용하는 것이 가장 좋습니다. 즉, 일단 코드를 수정하고 목표를 달성 할 수 있다면, 대신에 오버 슈트를 시작하여 앞뒤로 진동하는 것 같습니다. 올바르게 조정 된 PID는 대상에 빠르게 접근 할 수 있도록하면서 발생하지 않도록합니다.

와 를 롤과 피치 에 연결하는 대신 롤과 피치 PID가 입력으로 허용 하는 오류 값으로 간주하십시오 .$x$$y$

여기서 3D 벡터에 대해 이야기한다고 가정하겠습니다. 그렇게 일반화 할 수 있습니까 normalize()? 그것은 흔한 일입니까 (그렇다면 그것을 본 적이 없다면 그때 나에게 뉴스를 보내십시오). 그렇지 않으면 명백한 나침반 랩 문제가 각 X 및 Y 구성 요소에 적용됩니다. 왜 그들을 롤 및 / 또는 피치 및 / 또는 요라고 부릅니까? (3D와 2D 명명법을 혼합하면 문제가 혼동됩니다).

내 2D 정규화는 다음과 같습니다.

int Pilot_QuickestTurnTo(int hdgNow, int hdgNew)
{
    hdgNow = Pilot_Hdg360(hdgNow);
    hdgNew = Pilot_Hdg360(hdgNew);
    if (hdgNow < hdgNew)
        hdgNow += 360;
    int left = hdgNow - hdgNew;
        return (left < 181 ? -left : 360 - left);
}

실제로 쿼드 인 경우 X 및 Y 구성 요소가 실제로 YAW, Altitude ((X, Y) & Z)라고 가정합니다. YAW(X, Y)2D 로 처리하고 Z에 대해 단순히 고도를 떨어 뜨리거나 높이를 가져와야합니다. 다시 말해서 정규화가 원래보다 더 많은 것으로 생각됩니다.