Преобразуйте силу в крутящие моменты
Помним, что уравнение преобразования управляющего сигнала из операционного пространства включает в себя 2 элемента помимо желаемой силы, а именно, Якобиан и матрица инерции в операционном пространстве:
где 
– Якобиан для интересующей точки, 
– матрица инерции в операционном пространстве для интересующей точки и 
– сила, что определена чуть выше.
Вычисление Якобиана для неопределённой точки
Итак, первое, что нужно вычислить, это Якобиан для этой точки на руке. Есть несколько способов, остановлюсь на методе из поста о SymPy автоматизации вывода Якобиана. Определим преобразования из исходной/базовой (origin) системы отсчёта в систему первого звена, а из неё в систему второго звена и т.д., пока не достигнем системы отсчёта захвата. Затем, когда бы ни понадобился Якобиан, можно связать все преобразования вместе, чтобы получить преобразование от начала координат до той точки руки, и взять частную производную относительно суставов (используя метод производной SymPy).
Например, скажем: хотели бы вычислить Якобиан для 3го сочленения, то сначала вычислим:
где 
читается, как преобразование из системы отсчёта m в систему отсчёта n.
Получив эту матрицу преобразования 
, умножим её на интересующую точку в системе отсчёта 3, которая раньше обычно была 
. Другими словами, обычно мы заинтересованы просто в переходе из базовой в систему отсчёта 3.
Якобиан есть
|
|
|
Как найти неуказанную заранее точку на звене 3? В SymPy установим 
вместо 
(напомню, что 1 на конце в векторах просто, чтобы математика работала), и делаем из Якобиана функцию, а SymPy генерирует её для нас, зависящей от обоих переменных 
и 
.
В коде это выглядит так:
Теперь можно вычислить Якобиан для любой точки на звене 3, просто изменив параметры, которые передаются в J3_func!
Превосходно, мы приближаемся к цели.
Примечание: эта параметризация может значительно увеличить время сборки. Чтобы уменьшить время сборки, можно попробовать закомментировать в коде вызовы simplify, что может немного замедлить время выполнения, но значительно сократит время генерации.
Где находится ближайшая точка в системе отсчёта данного звена?
При вычислении ближайшей точки к препятствию каждого звена руки возникает хитрая проблема: ближайшие точки вычислены в базовой системе отсчёта, а нам нужны они относительно собственных систем отсчёта каждого звена. К счастью, всё, что нужно – вычислить обратное преобразование для интересующего звена.
Например, обратное преобразование преобразует точку из базовой системы отсчёта, в систему отсчёта 3-го соединения.
В конце статьи о матрицах прямого преобразования рассказывается о расчёте обратного преобразования, но чтобы не возвращаться, просто просмотрите на этот код:
|
|
|
Теперь, чтобы найти ближайшую точку в координатах системы отсчёта 3, сделаем:
x – параметры функции J3_func, чтобы найти Якобиан для ближайшей точки на звена 3.
Дата добавления: 2019-11-16; просмотров: 68; Мы поможем в написании вашей работы! |
Мы поможем в написании ваших работ!