Обратная кинематика с ориентацией конечного эффектора?

Question

Я пытаюсь реализовать решатель обратной кинематики, но на этот раз даже с ориентацией конечного эффектора. Мне удалось с тем случаем, когда конечный эффектор требует только положение.

Я узнал, что в этом случае вы можете построить Якобиану Матрицу следующим образом: где w_i - это ось вращения i_th в глобальном пространстве, а p_i - вектор от оси i_th до целевой позиции.

Проблема в том, когда я должен вычислить x_dot здесь в уравнении ниже.

Когда у x_dot были только позиции, которые нужно принять во внимание, и без ориентации, это было довольно просто. Но теперь, когда x_dot требуется 6 записей (позиция, ориентация), я не знаю, что мне следует делать для части ориентации. Я использовал углы Эйлера для представления ориентации в моей программе.

Идея, которая у меня есть на данный момент, состоит в том, чтобы просто вычитать текущий рывок, тангаж и крен текущего конечного эффектора из рыскания, тангажа и крена цели и делить каждый результат на 100. Но это кажется немного сложным. Есть ли лучшие способы решения этой проблемы? Любые идеи будут высоко оценены!

Answer 1

Вы должны представить ориентацию вашего конечного эффектора в виде матрицы вращения 3 на 3. Вы вычисляете ориентацию конечного эффектора на текущем векторе соединений (тета), а затем добавляете небольшое приращение к каждому элементу в векторе соединений (в вашем случае 6 приращений, так как шесть соединений). В простом случае, когда вас интересовало изменение положения острия, которое возникает в результате каждого небольшого изменения сустава, вы рассчитывали изменение в положениях X, Y и Z, которое представляло собой простое векторное вычитание положения в тета, и для каждого возмущенная тета. Чтобы сделать то же самое для угла, вам нужно найти матрицу вращения R, которая переводит 3x3@Theta (A) в 3x3@ThetaPrime(B).

так как A * R = B

AInvA R=AInv* B

AInve * A = личность

R=AInv* В

Из R вы можете извлечь дельта ролл, тангаж, углы Эйлера. Формула здесь

https://pdfs.semanticscholar.org/6681/37fa4b875d890f446e689eea1e334bcf6bf6.pdf

Тета-значения - это изменение рыскания, тангажа и крена, вызванное изменением каждой тэты.