Геометрия SCARA робота (Решено)

[lonely-war.livejournal.com]

upd. Спасибо за помощь, получилось, в комментах выложил скрин решения

Помогите люди добрые, горю. Собрал конструкцию scara робота. Принцип управления немного отличается от общепринятого - на осях у меня стоят переменные резисторы, которые вращаются вместе с ними. В динамике измеряю сопротивление, аппроксимирую и знаю на какой угол в данный момент смещены рёбра конструкции.

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

Геометрически задача выглядит так:

Через координаты x и y нужно вычислить углы alfa и beta. Я пробовал через синусы косинусы.
Застрял на необходимости разложить арксинус разности.

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

math.sqrt((math.pow(y*y+x*x,2)-4*y*y*l*l)/(4*x*x-4*y*y))

Ещё нашёл вот такую картинку. Забитые по ней формулы дали примерно правильный результат, но они теряют знак. У меня вторая соединительная ось манипулятора может заходить в отрицательную зону, угол alfa может быть меньше нуля. А формула всегда даёт абсолютное значение.


beta = math.acos((x*x+y*y-l*l-l*l)/(2*l*l))

beta / math.pi * 180

gamma = math.acos((x*x+y*y+l*l-l*l)/(2*l*math.sqrt(x*x+y*y)))

gamma / math.pi * 180

alfa = beta / 2 - (math.pi / 2 - gamma)

alfa / math.pi * 180

Пробовал смотреть исходники Marlin от vitaminred. Место в коде, где вычисляются углы нашёл, но там какая-то вакханалия.

В общем если кто-то подскажет как решить, мы с Тыквой будем очень признательны.

Comments

[lonely-war.livejournal.com]

* уважаемые админы, если считаете запись неуместной, скажите, я удалю. Но мне кажется людям будет интересно

[aterentiev.livejournal.com]

Вполне инженерная задача, зачем удалять?

Предлагаю такой вариант:
Посчитать геометрически треугольник, с одной стороной (0,0)-(X,Y), а двумя другими L1 и L2, он раскладывается на два прямоугольных, это просто, надо найти углы.
А затем, зная угол к (0,0)-(X,Y), поворачиваем. Решения в общем виде два (нормальный треугольник), в пределе одно (вырожденный при L1+L2=lX,Yl, за пределами ни одного.

[matabuba.livejournal.com]

помойму в матлабе обратная кинематика есть

[urpek.livejournal.com]

Из центра (X,Y) проводишь окружность радиуса L2, из центра (0,0) - окружность радиуса L1, находишь точки их пересечения. Дальше определить углы - дело техники.

[aso.livejournal.com]

Через вектора решать не пробовале?
Ну или через комплексные числа/кватернионы.
Там обычно всё прекрасно алгебраизируется.

[lonely-war.livejournal.com]

Спасибо большое, получилось

[lonely-war.livejournal.com]

Да, пробовал так. Но видимо наделал ошибок запутавшись в квадратах и четвёртых степенях, потому как посчитанный результат оказался нереалистичным.
В общем решил через равнобедренный треугольник, выше в комментах скан прикрепил.

[aterentiev.livejournal.com]

Ура! ;)

[urpek.livejournal.com]

Можете пояснить, почему sin(gamma) равен sqrt(x*x+y*y)/2/L2 ? Кажется, что-то не то.
Так построение, кажется, более простое, чем с окружностями.

[lonely-war.livejournal.com]

ну синус гаммы это противолежащий катет (k) делённый на гипотенузу (l)
а катет (k) это половина длинны линии от 0,0 до x,y
то есть k = sqr(x^2+y^2)/2
делим на гипотенузу
sqrt(x^2+y^2)/2/L
остаётся отарксинусить

я проверял в ручную с чертежом, сходится

[urpek.livejournal.com]

Боюсь, что ваше утверждение про k верно только в случае L1==L2

[lonely-war.livejournal.com]

А, ну это да, забыл упомянуть. У меня в конструкции они как раз равны.

[urpek.livejournal.com]

Тогда всё верно, поздравляю с решением!

[lonely-war.livejournal.com]

благодарствую)