Формула 2D-вращения

Инструментарий tf от ros может легко реализовать преобразование координат между произвольными системами координат. Однако, если вы просто хотите легко проверить свои идеи и не хотите писать слишком сложный код, рассмотрите возможность написания собственной функции для 2D-вращения. Другой задачей, двойственной задаче двумерного вращения, является преобразование вращения двумерной системы координат.这两个问题的形式基本一样，只是旋转的角度相差一个负号。Это то, что легко спутать, так что сделайте пометку для дальнейшего использования.

1. Формула двумерного вращения (алгоритм)

И (эта статья только для двумерных) вращение должно представлять точку координат$(x_1, y_1)$Угол поворота против часовой стрелки (вперед) вокруг начала координат в определенной системе координат$\theta$После получения новых координат$(x_2, y_2)$.

Вывод: предполагаемый$v=(x, y)$,$v'=(x',y')$, как показано на рисунке выше$x=rcos(\phi),y=rsin(\phi),x'=rcos(\phi+\theta),y'=rsin(\phi+\theta)$(Обратите внимание, что на изображении выше есть несколько ошибок, та, что сбоку от оси координат$cos/sin(\theta)$следует изменить на$cos/sin(\phi+\theta$). расширять$x',y'$Доступный:$x'=rcos(\phi)cos(\theta)-rsin(\phi)sin(\theta)=xcos(\theta)-ysin(\theta)$ $y'=rsin(\phi)cos(\theta)+rcos(\phi)sin(\theta)=xsin(\theta)+ycos(\theta)$

Матричная форма:$\left[\begin{matrix}x' \\ y'\end{matrix}\right]=\left[\begin{matrix} cos(\theta) & -sin(\theta) \\ sin(\theta) & cos(\theta)\end{matrix}\right]\left[\begin{matrix}x \\ y\end{matrix}\right]$

Тогда двумерная матрица вращения:A=\left[\begin{matrix} cos(\theta) & -sin(\theta) \\ sin(\theta) & cos(\theta)\end{matrix}\right] \tag{1}

void Rotate2(double x1, double y1, double alpha, double& x2, double& y2)
{
	x2 = x1 * cos(alpha) - y1 * sin(alpha);
	y2 = x1 * sin(alpha) + y1 * cos(alpha);
}

2. Преобразование поворота двумерной системы координат

Предположим, что есть система координат$XOY$, через угол поворота против часовой стрелки (вперед)$\theta$После этого получите новую систему координат$Х'О'Й'$. получить координаты в исходной системе координат$(x,y)$Значения координат в новой системе координат называются преобразованием системы координат.

$x'=xcos(\theta)+ysin(\theta)=xcos(-\theta)-ysin(-\theta)$ $y'=-xsin(\theta)+ycos(\theta)=xsin(-\theta)+ycos(-\theta)$

Таким образом, двумерная матрица преобразования вращения координат имеет вид:B=\left[\begin{matrix} cos(\theta) & sin(\theta) \\ -sin(\theta) & cos(\theta)\end{matrix}\right]=\left[\begin{matrix} cos(-\theta) & -sin(-\theta) \\ sin(-\theta) & cos(-\theta)\end{matrix}\right] \tag{2}

в заключении

Сравнивая формулы (1) и (2), можно обнаружить, что двумерное вращение согласуется с двумерным координатным вращением, за исключением того, что когда оба вращения положительные (против часовой стрелки), угол отличается на отрицательный знак. То есть поставить точку под той же осью координат$(x,y)$Поверните угол в положительном направлении (против часовой стрелки) от начала координат$\theta$Новые координаты, полученные после$(x',y')$, что равносильно постановке точки$(x,y)$система координат$XOY$Обратный (по часовой стрелке) угол поворота$\theta$После этого в новой системе координат$X'O'Y'$Вниз,$(x,y)$Соответствующая новая точка координат$(x',y')$. Возьмите ручку и бумагу, поиграйте с двумя вышеприведенными поясняющими диаграммами, и все станет ясно.