Инверсная кинематика на плоскости с 2 суставами

[nailxx]

Возникла задача несложной инверсной кинематики, над которой я уже бьюсь 3 дня и где-то сильно туплю, поэтому не могу решить.



Имеется манипулятор из трёх плеч длинами $r_1, r_2, r_3$. Есть 2 сустава, которые их крутят. Они обозначены синими кружочками. Есть конечная точка $P$, которая обозначена красным.

Задача в том, чтобы получить аналитический ответ на вопрос: «какими должны быть углы поворота $\theta_1, \theta_2$, чтобы конечная точка оказалась в заданных, известных декартовых координатах.

Ход моего решения:

$$\theta_{12} = \theta_1 + \theta_2$$ $$p_x = r_1 \cos \theta_1 + r_2 \cos \theta_{12} - r_3 \sin \theta_{12}$$ $$p_y = r_1 \sin \theta_1 + r_2 \sin \theta_{12} + r_3 \cos \theta_{12}$$

Возводим оба уравнения в квадрат и складываем:

$$ \[p_x^2 = r_1^2 \cos^2 \theta_1 + 2r_1\cos\theta_1(r_2\cos\theta_{12}-r_3\sin\theta_{12}) + r_2^2 \cos^2 \theta_{12} - 2r_2r_3 \cos\theta_{12} \sin \theta_{12} + r_3^2\sin^2\theta_{12}\] \[p_y^2 = r_1^2 \sin^2 \theta_1 + 2r_1\sin\theta_1(r_2\sin\theta_{12}-r_3\cos\theta_{12}) + r_2^2 \sin^2 \theta_{12} + 2r_2r_3 \sin\theta_{12} \cos \theta_{12} + r_3^2\cos^2\theta_{12}\] \[p_x^2=r_1^2+r_2^2+r_3^2+2r_1r_2\cos\theta_1\cos\theta_{12}-2r_1r_3\cos\theta_1\sin\theta_{12} + 2r_1r_2\sin\theta_1\sin\theta_{12}+2r_1r_3\cos\theta_{12}\] $$

Заменяем $\theta_{12}$ на $\theta_1 + \theta_2$ в последнем уравнении, и пользуясь формулами синуса и косинуса суммы получаем:

$$ p_x^2+p_y^2=r_1^2+r_2^2+r_3^2+2r_1r_2\cos\theta_2 + 2r_1r_3(-\sin\theta_2) $$

или

$$ 2r_1r_2\cos\theta_2 - 2r_1r_3\sin\theta_2 = p_x^2+p_y^2 - r_1^2 - r_2^2 - r_3^2 $$

А это стандартное уравнение, решение которого:

$$ \theta_2 = \arctg \frac{A}{B} \pm \arctg\frac{\sqrt{A^2+B^2-C^2}}{C} $$

где A — коэффициент перед сокинусом, B — перед синусом, C — то, что справа.

Всё, $\theta_2$ известна, теперь дело за $\theta_1$. Ещё раз распишем:

$$p_x = r_1 \cos \theta_1 + r_2\cos\theta_1\cos\theta_2 - r_2\sin\theta_1\sin\theta_2 - r_3\sin\theta_1\cos\theta_2 - r_3\cos\theta_1\sin\theta_2$$ $$p_y = r_1 \sin \theta_1 + r_2\sin\theta_1\cos\theta_2 + r_2\cos\theta_1\sin\theta_2 + r_3\cos\theta_1\cos\theta_2 - r_3\sin\theta_1\sin\theta_2$$

Домножаем верхнее на $\cos\theta_1$, а нижнее на $\sin\theta_1$ и складываем вместе:

$$p_x\cos\theta_1 = r_1 \cos^2 \theta_1 + r_2\cos^2\theta_1\cos\theta_2 - r_2\sin\theta_1\sin\theta_2\cos\theta_1 - r_3\sin\theta_1\cos\theta_2\cos\theta_1 - r_3\cos^2\theta_1\sin\theta_2$$ $$p_y\sin\theta_1 = r_1 \sin^2 \theta_1 + r_2\sin^2\theta_1\cos\theta_2 + r_2\cos\theta_1\sin\theta_2\sin\theta_1 + r_3\cos\theta_1\cos\theta_2\sin\theta_1 - r_3\sin^2\theta_1\sin\theta_2$$ $$p_x\cos\theta_1 + p_y\sin\theta_1 = r_1 + r_2\cos\theta_2 - r_3\sin\theta_2$$

Аналогично, если домножать на $-\sin\theta_1$ и на $\cos\theta_1$, а затем сложить:

$$p_y\cos\theta_1 - p_x\sin\theta_1 = r_2\sin\theta_2 + r_3\cos\theta_2$$

В двух последних полученных уравнениях если первое домножить на $p_x$, а нижнее на $p_y$ и сложить, получим:

$$p_x^2\cos\theta_1 + p_xp_y\sin\theta_1 = p_x(r_1 + r_2\cos\theta_2 - r_3\sin\theta_2)$$ $$p_y^2\cos\theta_1 - p_xp_y\sin\theta_1 = p_y(r_2\sin\theta_2 + r_3\cos\theta_2)$$ $$\cos\theta_1 = \frac{p_x(r_1+r_2\cos\theta_2-r_3\sin\theta_2)+p_y(r_2\sin\theta_2+r_3\cos\theta_2)}{p_x^2+p_y^2}$$

Берём аркосинус и та да! Казалось бы оба угла известны, всё лепо. Однако если провести элементарную проверку в экселе, становится понятно, что решение не верно. Например, если проверить координаты: $p_x=r_1+r_2, p_y=r_3$, т.е. разогнутый сустав, угол $\theta_2$ равен -90°, что похоже на правду, но вот $\theta_1$ какой-то не адекватный и с изменением параметров положения точки P значение $\theta_1$ ведёт себя странно.

Уважаемые форумчане, быть может кто-то увидел ошибку в рассуждениях? Выручайте: мой мозг уже вскипел.

[TOTAL]

Найдите точку пересечения окружностей радиусов $r_1$ и $\sqrt{r_2^2 + r_3^2}$ соответственно с центрами в понятно каких точках. Дальше очевидно.

Или так: в треуголнике (сустав, сустав, т.$P$) все стороны известны, легко находим углы, дальше очевидно.

[nailxx]

OMG, Total. Как же хорошо взглянуть на задачу с другой стороны!

Попробовал решить по совету с треугольником. Всё сошлось, решение поместилось на одном листике, жизнь прекрасна, большое спасибо! :)