Навчання №44: Кінематика роботів — від кутів суглобів до декартового простору і назад

Коли хірургічний робот-асистент протягує нитку крізь петлю завтовшки 2 мм, або зварювальний робот для кузовів автомобілів веде шов зі швидкістю 1,2 м/с, жоден із них не звіряється з таблицею поз. Обидва спираються на два взаємообернені перетворення, що лежать в основі робототехніки: пряму кінематику (кути суглобів → декартова поза) та обернену кінематику (бажана поза → кути суглобів). Правильно реалізувати їх — і знати, коли вони перестають працювати — це різниця між роботом, що функціонує, і роботом, що руйнує сам себе в сингулярності.

Пряма кінематика і конвенція Денавіта-Хартенберга

Послідовний робот-маніпулятор — це кінематичний ланцюг: послідовність жорстких ланок, з'єднаних суглобами. Кожен суглоб має один ступінь вільності — обертальний (revolute) або поступальний (prismatic). Щоб описати позу (положення й орієнтацію) кожної ланки відносно попередньої, робототехніки використовують параметризацію Денавіта-Хартенберга (DH), запропоновану Жаком Денавітом і Річардом Хартенбергом у 1955 році.

Кожне перетворення між ланками задається рівно чотирма параметрами:

• θ_i — кут суглоба (обертання навколо z_i−1)
• d_i — зміщення ланки (переміщення вздовж z_i−1)
• a_i — довжина ланки (переміщення вздовж x_i)
• α_i — кручення ланки (обертання навколо x_i)

Однорідна матриця перетворення від системи координат i−1 до системи координат i є добутком чотирьох елементарних перетворень:

Для маніпулятора з n суглобів поза робочого органа в базовій системі координат є добутком усіх матриць ланок:

Кожна матриця Ti — це матриця 4×4, що містить блок обертання 3×3 і вектор перенесення 3×1, усе в однорідних координатах. Тому пряма кінематика — це замкнене множення матриць — швидке, точне і завжди має єдину відповідь для заданого набору кутів суглобів.

Обернена кінематика через транспоновану матрицю Якобі

Пряма кінематика проста; обернена кінематика (IK) складна. Маючи бажане положення й орієнтацію робочого органа xd, потрібно знайти кути суглобів q такі, що f(q) = xd, де f — відображення прямої кінематики. Задача, як правило, нелінійна і може мати нуль, одне або нескінченно багато розв'язків.

Найпоширеніший чисельний підхід базується на матриці Якобі J(q), яка відображає швидкості суглобів у швидкості робочого органа:

Матриця Якобі має розмір m×n, де m — кількість вимірів робочого простору (зазвичай 6 для 3D-положення та орієнтації), а n — кількість суглобів. Кожен стовпець Ji описує, як суглоб i впливає на рух робочого органа.

Метод транспонованої матриці Якобі наближається до мети за допомогою оновлення у стилі градієнтного спуску:

де Δx = xd − f(q) — декартова похибка, а α — коефіцієнт кроку. Це оновлення обчислювально дешеве — воно уникає обернення матриці J — але збігається повільно поблизу розв'язків і може осцилювати без ретельного демпфування. Надійнішою альтернативою є метод затухаючих найменших квадратів (damped least-squares, DLS) з псевдооберненою матрицею:

Коефіцієнт затухання λ запобігає великим швидкостям суглобів поблизу сингулярностей, де визначник JJT інакше наближався б до нуля.

Обмеження суглобів і обрізання значень

Кожен фізичний робот має обмеження кутів суглобів — механічні упори, за які маніпулятор не може повернутися без пошкодження. Після кожної ітерації IK оновлені кути суглобів потрібно обрізати:

// Псевдокод — обрізання кожного суглоба після оновлення
for i in 0..n_joints:
    q[i] = q[i] + delta_q[i]
    q[i] = clamp(q[i], q_min[i], q_max[i])

Наївне обрізання може призвести до «застрягання» розв'язувача: ціль вимагає, щоб суглоб перевищив своє обмеження, тому маніпулятор досягає межі й перестає збігатися. Більш витончені підходи використовують проєкцію в нуль-простір, щоб скористатися надлишковістю маніпулятора (коли n > m) для виконання вторинних цілей — наприклад, утримання суглобів ближче до середини їхнього діапазону — не впливаючи на основну мету щодо положення робочого органа.

Сингулярності, межі робочого простору та числа обумовленості

Кінематична сингулярність виникає, коли матриця Якобі втрачає ранг — коли дві або більше осей суглобів стають колінеарними, або коли маніпулятор повністю витягнутий чи повністю складений. У сингулярності робочий орган втрачає здатність рухатися в одному або кількох декартових напрямках, незалежно від того, як швидко обертаються суглоби. Визначник JJT падає до нуля:

Практичною мірою близькості маніпулятора до сингулярності є індекс маніпульованості, запропонований Цунео Йошикавою у 1985 році:

Коли w → 0, малі декартові похибки вимагають величезних швидкостей суглобів, насичуючи приводи й спричиняючи хаотичний рух. Стратегії керування, що утримують w вище порогу — або використовують DLS з адаптивним λ, що зростає в міру зменшення w — дозволяють уникнути найгірших наслідків.

Межі робочого простору

Досяжний робочий простір — це множина всіх положень робочого органа, досяжних за будь-якої комбінації кутів суглобів. Для плоского дволанкового маніпулятора з довжинами ланок l₁ та l₂ досяжний робочий простір є кільцем:

Зовнішня межа (r = l1 + l2) — це сингулярна конфігурація (повністю витягнутий стан). Внутрішня межа також сингулярна (повністю складений стан). У 3D обмеження суглобів вирізають додаткові порожнини з цього простору, залишаючи придатний для маніпуляцій робочий простір — підмножину положень, які можна досягти з повною свободою орієнтації — зазвичай значно меншу за досяжний робочий простір.

Сингулярність зап'ястка: конкретний приклад

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

Спробуйте самі

Наведені нижче симуляції mysimulator.uk дають змогу дослідити ці концепції інтерактивно. Перетягуйте повзунки, спостерігайте, як матриці оновлюються в реальному часі, і навмисно заганяйте маніпулятор у сингулярність, щоб побачити, що станеться.

Досліджуючи симуляцію кінематики робота, спробуйте такі експерименти:

1. Встановіть усі кручення ланок DH αi = 0, щоб отримати плоский маніпулятор, а потім перевірте кільцеву межу робочого простору аналітично.
2. Приведіть суглоб 5 у 6-ступеневому маніпуляторі до 0° і спостерігайте, як індекс маніпульованості падає до нуля при наближенні до сингулярності зап'ястка.
3. У симуляції IK порівняйте, наскільки швидко кожен розв'язувач збігається, коли ціль розташована поблизу межі робочого простору порівняно з центром придатного для маніпуляцій робочого простору.

Насамкінець

Кінематика роботів — це галузь, де елегантна математика зустрічається з жорсткою фізичною реальністю. Конвенція DH — це чудове стиснення геометрії твердого тіла до чотирьох чисел на суглоб. Матриця Якобі з'єднує простір суглобів і декартів простір однією матрицею. Проте обидва підходи ламаються саме там, де вони найпотрібніші — у сингулярностях і на межах робочого простору — тому практичні розв'язувачі IK ніколи не є просто формулою з підручника. Вони — формула з підручника плюс десятиліття інженерної роботи над її вадами.

Розуміння того, чому математика дає збій — а не просто того, що це трапляється — ось що відрізняє програміста-робототехніка від інженера-робототехніка. Наступного разу, коли ви побачите, як роботизований маніпулятор зупиняється посеред траєкторії й перебудовує шлях, ви знатимете, що він щойно уникнув сингулярності, яку контролер виявив за падінням індексу маніпульованості. Ця коротка пауза — це матриця Якобі, що рятує двигуни зап'ястка.