За допомогою 6-осевого робота, заданого положення кінцевого ефектора та діапазону орієнтацій, як знайти оптимальні значення з'єднання

Враховуючи шестивісну шарнірну рукоятку робота, яка тримає інструмент на його кінцевому ефекторі, якщо у мене є бажане положення інструмента та орієнтація інструменту, рівно обернеться рівняння зворотної кінематики, щоб робот досягнув цього положення.
(а точніше до 16 різних розчинів, залежно від діапазону стиків)

Але якщо робот тримає щось подібне до ручки, і я хочу, щоб робот позначив певну точку цією ручкою на мішені, то мені не байдуже, як ручка орієнтована, доки вона перпендикулярна позначеній поверхні.

Тож рівняння зворотної кінематики матиме нескінченно багато рішень.

Як я можу вибрати серед цих рішень спільну конфігурацію, найбільш близьку до поточної конфігурації: та, яка потребуватиме найменшої кількості руху для досягнення?
(або конфігурація стику, яка є оптимальною за яким-небудь іншим аналогічним критерієм, наприклад, що всі кути з'єднання найбільш віддалені від їх максимуму та мінімуму?)

— HugoRune
джерело

Відповіді:

По-перше, нам потрібно визначити оптимальний . Оскільки ви не кажете те, що вважаєте оптимальним, більшість людей вибирають квадратичний вираз. Наприклад, припустимо, що ваші поточні кути суглоба задані вектором $\vec{\alpha}$ . Ми можемо розглянути можливість мінімізації необхідного руху - з помилкою $\vec{x} = \vec{\alpha} - \vec{\alpha}_{start}$ , Ви можете визначити функцію витрат $J=\vec{x}^TQ\vec{x}$ для деякої матриці $Q$ . Ми зазвичай використовуємо діагональну матрицю, але будь-яка матриця, що визначається позитивом, буде робити.

На спрощеному прикладі з двома суглобами кута, якщо спільними $a$ був дешевший мотор (можливо, ближче до кінцевого ефектора), ми можемо мати функцію витрат

$J=\left[\begin{matrix}x_a\\x_b\end{matrix} \right]\left[\begin{matrix}1&0\\0&2\\\end{matrix} \right]\left[\begin{matrix}x_a&x_b\end{matrix} \right]$ , тобто. рух суглоба $b$ вдвічі дорожче, ніж спільне $a$ .

Тепер кінематичне рівняння є матричною формулою, і в позначенні Денавіт-Гартенберга може бути:

$\prod{T_i}=\left[\begin{matrix}1&0&0&x\\0&1&0&y\\0&0&1&z\\0&0&0&1\end{matrix} \right]$ , де права рука представляє положення $(x,y,z)$ та орієнтація (в даний час встановлена нульовим обертанням), враховуючи кути з'єднання.

Оскільки нас не хвилює орієнтація, а лише положення, ми можемо усікати перші 3 стовпчики останньої матриці перетворення та останній рядок першої матриці перетворення. Ми можемо так само виразити цю формулу:

$\left[\begin{matrix}1&0&0&0\\0&1&0&0\\0&0&1&0\end{matrix}\right]\prod{T_i}\left[\begin{matrix}0\\0\\0\\1\end{matrix}\right]=\left[\begin{matrix}x\\y\\z\end{matrix} \right]$

Помноживши ліву частину, отримаємо три рівняння. Якби параметри були лінійними, вирішити їх було б просто. Це так, якщо всі приводи є лінійними приводами. У цьому випадку проблема насправді є квадратичною програмою . Ми можемо переставити ліву частину, щоб отримати рівняння:

$K\vec{x}=\left[\begin{matrix}x\\y\\z\end{matrix}\right]$ , для деякої матриці $K$ .

Квадратична програма - це проблема, яка може бути виражена у вигляді:

Мінімізуйте $J=\frac{1}{2}\vec{x}^TQ\vec{x}+\vec{c}^T\vec{x}$

На тему $A\vec{x}\leq\vec{b}$ , $E\vec{x}=\vec{d}$

Для вирішення цього питання існує ряд алгоритмів, якими ви можете скористатися, наприклад, внутрішня точка, активний набір, .... Просто знайдіть підходящу бібліотеку, і вона вирішить її для вас.

Нелінійну систему рівнянь вирішити важче. Це називається нелінійним програмуванням , але це те, що у вас є, якщо у вас є обертові суглоби.

По суті, замість матричних рівнянь у вас є нелінійні функції.

Мінімізуйте $f(x)$ на тему $\vec{h}(x) = 0$ , $\vec{g}(x) \leq 0$ (переупорядкуйте, якщо необхідно, щоб РЗС обмежень було нульовим)

Алгоритми, що використовуються для вирішення цього питання, ще складніші, але включають внутрішню точку, послідовне квадратичне програмування (SQP), активний набір, відображаючі алгоритми довіреної області. Очевидно, що пояснення того, як працюють ці алгоритми, є дуже тривалим, і я не залишатиму його поза рамками цієї відповіді. Досить сказати, що кількість вмісту в алгоритмах, що використовуються для просто квадратичного програмування, може скласти сам курс.

Вам слід просто знайти бібліотеку, щоб вирішити проблему, для кодування ефективної реалізації знадобиться багато часу, і ефективні реалізації можуть обробляти 100 (або більше) змінних одночасно. Наприклад, якщо ви використовуєте MATLAB, то є документація щодо використання функції fmincon з Панелі оптимізації.

Щоб вирішити це в Інтернеті, вам може знадобитися C ++ або інша власна реалізація, наприклад, NLopt. Зверніть увагу, що це може бути не те, що мікроконтролер може швидко вирішити, і багато бібліотек можуть мати інші залежності, які не просто використовувати у мікроконтролері (оскільки вони призначені для комп'ютера).

Якщо ви не переживаєте за ефективність, а просто хочете щось, що ви можете кодувати самостійно, то припускаючи, що є функція, яку ви можете зателефонувати для вирішення зворотної кінематичної проблеми , ви можете просто скористатися методом градієнтного спуску. Наприклад, довільно вибираючи випадкову початкову орієнтацію, вирішують зворотну задачу, а потім перевіряють функцію витрат. Потім ви можете скористатися аналізом збурень, щоб перевірити, як слід змінювати орієнтацію. Наприклад, якщо ви перевіряєте подібні орієнтації навколо вашої поточної орієнтації (наприклад, 8 балів у кубічній сітці), ви можете отримати приблизне наближення другого порядку до того, як змінюється функція витрат у кожному напрямку.

Використовуючи наближення другого порядку (відоме як матриця Гессі, оскільки вона є багатоваріантною - тривимірною для орієнтації), можна знайти нульове перетин градієнта функції витрат (тобто передбачувані локальні мінімуми).

З новою передбачуваною орієнтацією просто перенесіть її через інверсну розв'язку і повторіть, поки точність не буде достатньою.

Зверніть увагу, що це, ймовірно, не буде настільки ефективним, оскільки сама зворотна кінематична задача повинна бути ітеративно вирішена (тому ви неодноразово використовуєте функцію, яка сама потребує певного часу для вирішення). Також код може бути меншим, ніж повноцінний алгоритм оптимізації, але це все-таки досить вагомий і не незначний вкладення часу.

Використовуючи будь-який метод (формально розв’язуючи як нелінійну програму або використовуючи ітераційне використання функції для вирішення зворотної задачі), рішення може бути не оптимальним, якщо є кілька локальних мінімумів. У цьому випадку ви можете спробувати знайти глобальні мінімуми, використовуючи різні підходи. Навіть із нелінійним розв'язувачем програмування, від вас очікується засідання початкових значень (наприклад, кути з'єднання). Ви можете неодноразово запускати будь-який метод із створеним насінням різними способами:

випадковий перезапуск (генерується випадковим чином)
на основі сітки

або інші користувацькі методи.

Однак зауважте, що якщо мінімумів багато, не існує хорошого способу гарантувати, що ви знайдете глобальні мінімуми. Ви можете лише поліпшити свої шанси.

— ronalchn
джерело

Оскільки питання стосується промислового робота, ми, мабуть, не маємо моделі динаміки роботи, тому я припускаю, що ми шукаємо рішення, які оптимізують лише кінематичний критерій.

Робот має рішення закритої форми для своєї зворотної кінематики, але, на жаль, кінцевий ефектор має додаткову ступінь свободи обертання, а це означає, що робот має по суті 7 ступенів свободи. Але оскільки це лише ще один доф , це не стільки питання, скільки можна подумати.

Поширений трюк для таких майже не зайвих роботів - зафіксувати зайві ступені свободи та вирішити для решти спільні значення аналітично. Скажімо, ми пишемо ІК-вирішувач із закритою формою для робота 6 dof, який повертає рішення в $0.05$ мс в середньому Ітерацією від $1$ до $360$ тому ви можете знайти оптимальне рішення для дискретизації кута пера $1^\circ$ в о $18$ мс, що дано програмі, мабуть, більш ніж достатньо.

Якщо більшу частину часу ручка лише трохи рухається (наприклад, під час малювання лінійки), ще одним фокусом для прискорення пошуку є використання чисельного ІК, наприклад, метод псевдоінверсії:

Дозволяє $q_1$ будь поточна конфігурація робота, нехай $J$ будь його якобійським, і нехай $\Delta x$ - переміщення цілі відносно поточного перетворення кінцевого ефектора. Вирішити $\Delta x = J \, \Delta q$ для $\Delta q$ і обчислити нову конфігурацію $q_2 = q_1 + \Delta q$ . Я пропускаю тут деталі, але рішення $\Delta q$ повинні мінімізувати $\|\Delta q\|$ для правильно обраного показника.

Це робиться для робота 7 dof і знову потрібно брати лише частки мілісекунди. Хоча $q_2$ може не бути коректною конфігурацією (спільні значення можуть бути поза межами) і може бути не точним рішенням ІК (однак, ви можете зробити більше псевдо зворотних кроків), більшість часу це буде гарною відправною точкою для пошуку за допомогою вирішувач із закритою формою.

— антонакос
джерело

Для цього є приємна закрита форма. Скажімо, що нам байдуже, що $r_z$ є (тобто нам байдуже, як ми це змінимо ).

J^{- 1} \dot{Х} = \dot{Θ} = [\begin{matrix} \vec{j_{1}} & \vec{j_{2}} & \vec{j_{3}} & \vec{j_{4}} & \vec{j_{5}} & \vec{j_{6}} \end{matrix}] [\begin{matrix} \dot{х} \\ \dot{у} \\ \dot{z} \\ \dot{r_{х}} \\ \dot{r_{у}} \\ \dot{r_{z}} \end{matrix}] = [\begin{matrix} \dot{θ_{1}} \\ \dot{θ_{2}} \\ \dot{θ_{3}} \\ \dot{θ_{4}} \\ \dot{θ_{5}} \\ \dot{θ_{6}} \end{matrix}]

$\mathbf{J}^{-1}\dot{\mathbf{X}}= \dot{\mathbf{\Theta}}= \begin{bmatrix} \vec{j_{1}} & \vec{j_{2}} & \vec{j_{3}} & \vec{j_{4}} & \vec{j_{5}} & \vec{j_{6}} \end{bmatrix} \begin{bmatrix} \dot{x}\\ \dot{y}\\ \dot{z}\\ \dot{r_x}\\ \dot{r_y}\\ \dot{r_z} \end{bmatrix} = \begin{bmatrix} \dot{\theta_1}\\ \dot{\theta_2}\\ \dot{\theta_3}\\ \dot{\theta_4}\\ \dot{\theta_5}\\ \dot{\theta_6}\\ \end{bmatrix}$ Де

\vec{j_{i}}

$\vec{j_{i}}$ є

i^{t h}

$i^{th}$ стовпчик

J^{- 1}

$\mathbf{J}^{-1}$ . Ми можемо розійтися

\dot{Θ}

$\dot{\mathbf{\Theta}}$ в ту частину, від якої залежить

\dot{r_{z}}

$\dot{r_z}$ і частина, яка цього не робить.

\dot{Θ} = {\dot{Θ}}_{\dot{х} \dots \dot{r_{у}}} + {\dot{Θ}}_{\dot{r_{z}}} {\dot{Θ}}_{\dot{r_{z}}} = \vec{j_{6}} \dot{r_{z}}

$\dot{\mathbf{\Theta}}= \dot{\mathbf{\Theta}}_{\dot{x}\dots \dot{r_y}} + \dot{\mathbf{\Theta}}_{\dot{r_z}} \\ \dot{\mathbf{\Theta}}_{\dot{r_z}}= \vec{j_{6}} \dot{r_z}$ Отже тепер гра стала, давайте мінімізуємо

({\dot{Θ}}_{\dot{х} \dots \dot{r_{у}}} + {\dot{Θ}}_{\dot{r_{z}}})^{Т} D ({\dot{Θ}}_{\dot{х} \dots \dot{r_{у}}} + {\dot{Θ}}_{\dot{r_{z}}})

$(\dot{\mathbf{\Theta}}_{\dot{x}\dots \dot{r_y}} + \dot{\mathbf{\Theta}}_{\dot{r_z}})^T D(\dot{\mathbf{\Theta}}_{\dot{x}\dots \dot{r_y}} + \dot{\mathbf{\Theta}}_{\dot{r_z}})$ для деякої діагональної матриці

D

$D$ як Роналчн сказав вище. Я збираюся використовувати

A = {\dot{Θ}}_{\dot{x} \dots \dot{r_{y}}}

$A=\dot{\mathbf{\Theta}}_{\dot{x}\dots \dot{r_y}}$ і

B = {\dot{Θ}}_{{\dot{r_{z}}}^{*}}

$B=\dot{\mathbf{\Theta}}_{\dot{r_z}^*}$ для легшого перегляду

Ми можемо розширити це до

А^{Т} D А + 2 Б^{Т} D А + Б^{Т} D Б або А^{Т} D А + 2 \dot{r_{z}} {\vec{j_{6}}}^{Т} D А + {\dot{r_{z}}}^{2} {\vec{j_{6}}}^{Т} D \vec{j_{6}}

$A^TDA+2B^TDA+B^TDB\\ \text{or}\\ A^TDA+2\dot{r_z}\vec{j_{6}}^TDA+\dot{r_z}^2\vec{j_{6}}^TD\vec{j_{6}}$

Тепер у нас є просте рівняння для диференціації $\dot{r_z}$ . Ми знаходимо похідну відносно $\dot{r_z}$ і встановити його $0$ .

2 {\vec{j_{6}}}^{Т} D А + 2 \dot{r_{z}} {\vec{j_{6}}}^{Т} D \vec{j_{6}} = 0 \dot{r_{z}} = \frac{- {\vec{j_{6}}}^{Т} D А}{{\vec{j_{6}}}^{Т} D \vec{j_{6}}}

$2\vec{j_{6}}^TDA+2\dot{r_z}\vec{j_{6}}^TD\vec{j_{6}}=0\\ \dot{r_z} = \frac{-\vec{j_6}^TDA}{\vec{j_6}^TD\vec{j_6}}$ Це мінімізує кут з'єднання «відстань» між вашими двома позими.

— джошкарги
джерело