Я працюю над кадротором. Я знаю його положення - , куди я хотів би піти - цільове положення , і з цього обчислюю вектор - одиничний вектор, який приведе мене до моєї цілі:$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$

Заголовка проти Яу

Якщо ви використовуєте компас для "кута похилу" вашого автомобіля, це також може бути вашою помилкою; заголовок і позіхання - це не те саме . Заголовок компаса дорівнює нулю вздовж позитивної осі , збільшуючись у міру повороту за годинниковою стрілкою :$y$

Коса дорівнює нулю вздовж позитивної осі , збільшуючись у міру повернення проти годинникової стрілки :$x$

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 (і знову ж таки, тому я підозрюю, нормалізація - це більше, ніж у вас як).