2014 dxdy logo

Научный форум dxdy

Математика, Физика, Computer Science, Machine Learning, LaTeX, Механика и Техника, Химия,
Биология и Медицина, Экономика и Финансовая Математика, Гуманитарные науки


Правила форума


В этом разделе нельзя создавать новые темы.

Если Вы хотите задать новый вопрос, то не дописывайте его в существующую тему, а создайте новую в корневом разделе "Помогите решить/разобраться (М)".

Если Вы зададите новый вопрос в существующей теме, то в случае нарушения оформления или других правил форума Ваше сообщение и все ответы на него могут быть удалены без предупреждения.

Не ищите на этом форуме халяву, правила запрещают участникам публиковать готовые решения стандартных учебных задач. Автор вопроса обязан привести свои попытки решения и указать конкретные затруднения.

Обязательно просмотрите тему Правила данного раздела, иначе Ваша тема может быть удалена или перемещена в Карантин, а Вы так и не узнаете, почему.



Начать новую тему Ответить на тему
 
 Инверсная кинематика на плоскости с 2 суставами
Сообщение19.04.2013, 12:45 
Аватара пользователя


19/04/13
2
Возникла задача несложной инверсной кинематики, над которой я уже бьюсь 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$ ведёт себя странно.

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

 Профиль  
                  
 
 Re: Инверсная кинематика на плоскости с 2 суставами
Сообщение19.04.2013, 12:57 
Заслуженный участник
Аватара пользователя


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

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

 Профиль  
                  
 
 Re: Инверсная кинематика на плоскости с 2 суставами
Сообщение19.04.2013, 15:59 
Аватара пользователя


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

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

 Профиль  
                  
Показать сообщения за:  Поле сортировки  
Начать новую тему Ответить на тему  [ Сообщений: 3 ] 

Модераторы: Модераторы Математики, Супермодераторы



Кто сейчас на конференции

Сейчас этот форум просматривают: нет зарегистрированных пользователей


Вы не можете начинать темы
Вы не можете отвечать на сообщения
Вы не можете редактировать свои сообщения
Вы не можете удалять свои сообщения
Вы не можете добавлять вложения

Найти:
Powered by phpBB © 2000, 2002, 2005, 2007 phpBB Group