Численная обратная кинематика: метод Ньютона

Когда геометрических формул нет, IK решают итеративно — шагая к цели по якобиану.

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

Идея метода Ньютона для IK: пусть текущие углы дают позицию $\mathbf{p}(q)$, а цель — $\mathbf{p}^*$. Ошибка $\mathbf{e} = \mathbf{p}^* - \mathbf{p}(q)$. Связь малого изменения углов и положения — через якобиан $J$:

$$ \Delta \mathbf{p} \approx J \, \Delta q \;\Rightarrow\; \Delta q = J^{-1} \mathbf{e} $$

Шагаем $q \leftarrow q + J^{-1}\mathbf{e}$ и повторяем, пока $\|\mathbf{e}\|$ не станет мал. Якобиан двухзвенника:

$$ J = \begin{bmatrix} -L_1 s_1 - L_2 s_{12} & -L_2 s_{12} \\\\ L_1 c_1 + L_2 c_{12} & L_2 c_{12} \end{bmatrix} $$

где $s_1 = \sin\theta_1$, $s_{12} = \sin(\theta_1 + \theta_2)$ и аналогично косинусы.

Считаем

Та же цель $(1.2, 0.5)$. Ждём схождения к тем же $-9.58^\circ, 81.79^\circ$, что дала геометрия.

import math

L1, L2 = 1.0, 0.7

def fk(a, b):
    return (L1*math.cos(a) + L2*math.cos(a+b),
            L1*math.sin(a) + L2*math.sin(a+b))

def jac(a, b):
    return [[-L1*math.sin(a) - L2*math.sin(a+b), -L2*math.sin(a+b)],
            [ L1*math.cos(a) + L2*math.cos(a+b),  L2*math.cos(a+b)]]

tx, ty = 1.2, 0.5
a, b = 0.5, 0.5  # начальное приближение
for it in range(6):
    x, y = fk(a, b)
    ex, ey = tx - x, ty - y
    err = math.hypot(ex, ey)
    print(f"шаг {it}: theta=({math.degrees(a):6.2f},{math.degrees(b):6.2f}) ошибка={err:.6f}")
    if err < 1e-6:
        break
    J = jac(a, b)
    det = J[0][0]*J[1][1] - J[0][1]*J[1][0]
    inv = [[ J[1][1]/det, -J[0][1]/det], [-J[1][0]/det, J[0][0]/det]]
    a += inv[0][0]*ex + inv[0][1]*ey
    b += inv[1][0]*ex + inv[1][1]*ey

Вывод:

шаг 0: theta=( 28.65, 28.65) ошибка=0.571187
шаг 1: theta=(-32.12,144.30) ошибка=0.726803
шаг 2: theta=(-20.14, 87.58) ошибка=0.197994
шаг 3: theta=( -9.82, 83.41) ошибка=0.016944
шаг 4: theta=( -9.60, 81.80) ошибка=0.000206
шаг 5: theta=( -9.58, 81.79) ошибка=0.000000

Как работает под капотом

Метод сходится квадратично вблизи решения — посмотрите, как ошибка падает $0.017 \to 0.0002 \to 0$: число верных знаков примерно удваивается за шаг. Универсальность — главный плюс: тот же алгоритм работает для 3, 6, 7 звеньев, нужен лишь якобиан (его легко получить численно). Минус — зависимость от начального приближения и проблемы у сингулярностей, где $\det J \to 0$.

Частые ошибки

Стартовать у сингулярности или у вытянутой руки — $\det J$ близок к нулю, шаг «взрывается».
Не ограничивать размер шага: при больших ошибках метод может уйти в сторону (как видно на шаге 1).
Делить на $\det J$ без проверки — в сингулярности это деление на ноль.

Итог

Численная IK шагает к цели через $\Delta q = J^{-1} \mathbf{e}$.
Метод Ньютона сходится квадратично вблизи решения.
Подходит для любых роботов, где геометрия не выводится вручную.
Чувствителен к начальному приближению и к сингулярностям.