Углы ориентации

Материал из BrSTU Robotics Wiki
Перейти к: навигация, поиск
Рис. 1 - Система отсчета угла, относительно декартовой системы координат

Определение угла цели робота

Когда определяется направление робота - возникает множество подводных камней, решение которых предоставляет простой математический аппарат.

Рис. 2. - Графическое отображение положений робота и его цели

Предположим что у нас есть робот в координатах ($x_{1}$,$y_{1}$). Цель робота, приехать в координаты ($x_{2}$,$y_{2}$). В таком случае простой алгоритм достижения робота цели будет состоять из следующих шагов:

  1. Повернуться в сторону препятствия ($x_{2}$,$y_{2}$)
  2. Ехать до тех пор, пока координаты робота не станут ($x_{2}$,$y_{2}$)

На первом этапе алгоритма нам необходимо определить направление движения робота. Решим данную задачу.

Первое что нам необходимо определить это под каким уголм отностиельно нашей системы координат распологается препятствие (см. рис. 1).

arctan()

Для определения угла препятствия можно использовать arctan():

$

\begin{align} \varphi & = 90 - arctan(\frac{y_{2} - y_{1}}{x_{2} - x_{1}}) \\ \end{align} $

Но такое решение даст нам угол только от 0° до 180°, что не даем нам знания где точно находится препятствия (сверху или снизу). Для того что бы определить где же оно, необходимо использовать следующий алгоритм:

dx = x2 - x1
dy = y2 - y1
    if dx > 0 then
        phi = 90 - (180 / PI) * arctan(dy / dx)
    if dx < 0 then
        phi = 270 - (180 / PI) * arctan(dy / dx)
    if dx = 0 then
        if dy > 0 then phi = 0
        if dy < 0 then phi = 180
        if dy == 0 then point 1 == point 2 and there phi = 0

atan2()

Второй вариант более простой - использовать функцию atan2():

$

\varphi = (\frac{180}{\pi}) * atan2(y_{2} - y_{1}, x_{2} - x_{1}) $

Функция atan2(dy, dx) вернет необходимый угол в диапазоне от 0 до 180 или от 0 до -180.

Обработка полученного угла до [-$\pi$,$\pi$]

Вторая ситуация, когда удобно применить atan2() - когда вы получили угол больший $\pi$ или меншьий $-\pi$. Для этого используется стандартный трюк:

$ \varphi = arctan2(sin(\varphi), cos(\varphi)) $

В результате вы получите угол в диапазоне [-$\pi$,$\pi$], соответствующий вашей системе координат.