特異点低感度行列を用いた逆運動学　アルゴリズム編

運動学周りを一度ちゃんとまとめよう
Arduino(ESP32 only)と標準的なC++環境で併用できるKinematicライブラリを作っておきましたー
github.com
ESP32のコンパイルオプションで別れているので，コピペでPC環境でも使える

ESP32だと下記の線形代数ライブラリが必要
GitHub - tomstewart89/BasicLinearAlgebra: A library for using matrices and linear algebra on Arduino

Standard C++だとEigenが必要＋Kinematicライブラリ内のAffine3d.cpp & Affine3d.hppを削除（アフィン変換）

順運動学

ロボット先端位置・姿勢 $\boldsymbol r$ ，関節角度 $\boldsymbol q$ とすると下記のようになる
$\boldsymbol r = f(\boldsymbol q)$

逆運動学

下記のような非線形運動方程式 $g(\boldsymbol q)$ を満たすような $\boldsymbol q$ を求める
$g(\boldsymbol q) = \boldsymbol r - f(\boldsymbol q) = 0$
非線形方程式をニュートンラプソン(NR)法で解くと，更新式は下記のようになる
$\boldsymbol q_{i+1} = \boldsymbol q_{i} - \nabla g(\boldsymbol q_{i})^{-1} g(\boldsymbol q_{i})$
ここで $\nabla g(\boldsymbol q) = -\nabla f(\boldsymbol q) = -\boldsymbol J(\boldsymbol q)$ とすると
$\boldsymbol q_{i+1} = \boldsymbol q_{i} + J(\boldsymbol q_{i})^{-1} \Delta \boldsymbol r(\boldsymbol q_{i})$
ただし $\Delta \boldsymbol r(\boldsymbol q_{i}) = \boldsymbol r - f(\boldsymbol q_{i})$ ， $\boldsymbol r = const$ ， $\boldsymbol J(\boldsymbol q)$ はヤコビアン

特異点

$\boldsymbol J^{-1}$ が存在するための条件は $det \boldsymbol J \neq 0$ であるが
$\boldsymbol J^{-1} = \frac{1}{\rm{det} \boldsymbol J} \tilde{\boldsymbol J}$ であるから $det \boldsymbol J$ が小さいと
関節変化が非常に大きくなり運動として実現できない
そこで数学的な厳密さと運動実現性を考慮した逆運動学がいくつも提案されている

Levenberg-Marquardt 法による可解性を問わない逆運動学[杉原 2011]

今回は特異点低感度運動分解を用いて逆運動学を解く

特異点低感度運動分解

微小関節角度変化を $\delta \boldsymbol q$ ，それに伴う微小位置変化を $\delta \boldsymbol r$ とし，
最小二乗法より最小化する誤差関数を下記のように定義する
$\boldsymbol E = \frac{1}{2} ( \delta \boldsymbol r - \boldsymbol J \delta \boldsymbol q)^T \boldsymbol W_r ( \delta \boldsymbol r - \boldsymbol J \delta \boldsymbol q) + \frac{1}{2} \delta \boldsymbol q^T \boldsymbol W_q \delta \boldsymbol q \rightarrow min$
誤差関数の1項目で解の厳密さ，2項目で運動実現性を評価している
また $\boldsymbol W$ は重みを表す対角行列（ $\boldsymbol W^T = \boldsymbol W$ ）
誤差が最小となるような $\delta \boldsymbol q$ を求める
$\frac{\partial \boldsymbol E}{\partial \delta \boldsymbol q} = 0$ となるような $\delta \boldsymbol q$ を計算すると
$\delta \boldsymbol q = (\boldsymbol J^T \boldsymbol W_r \boldsymbol J + \boldsymbol W_q)^{-1} \boldsymbol J^T \boldsymbol W_r \delta \boldsymbol r$
ベクトル・行列の微分は下記で復習
線形代数の基礎 — ディープラーニング入門：Chainer チュートリアル
簡単のため $\boldsymbol W_r = \boldsymbol E$ ， $\boldsymbol W_q = k \boldsymbol E$ とすると
$\delta \boldsymbol q = (\boldsymbol J^T \boldsymbol J + k \boldsymbol E)^{-1} \boldsymbol J^T \delta \boldsymbol r$ となる
k は数学的な厳密さと運動実現性の比を表す
整理すると $\delta \boldsymbol q = \boldsymbol J^T (\boldsymbol J \boldsymbol J^T + k \boldsymbol E)^{-1} \delta \boldsymbol r$ となるので，下記の更新式を用いる
$\boldsymbol q_{i+1} = \boldsymbol q_{i} + \boldsymbol J^T (\boldsymbol J \boldsymbol J^T + k \boldsymbol E)^{-1} \Delta \boldsymbol r$

可制御性を用いた係数kの決定

可制御性 $\omega = \sqrt{det \boldsymbol J \boldsymbol J^T}$ が0に近いほど関節角度変化が大きくなるためk を大きくすればよい。閾値となる可制御度を $\omega_0$ 、閾値での係数 $k$ を $k_0$ とすると
$k = \begin{array}{} k_0 (1 - \frac{\omega}{\omega_0})^2 & (\omega > \omega_0) \\ 0 & (\omega \leqq \omega_0)\end{array}$

今日はここまで！
次回は係数kを3自由度アームを用いて求めてみます．あとライブラリの紹介もしないと，，，
----------------------------------------------------