6軸ロボットで、エンドエフェクタの位置と方向の範囲を指定して、最適な関節の値を見つける方法

エンドエフェクターでツールを保持している6軸の多関節ロボットアームがある場合、希望するツールの位置と方向がある場合、ロボットがその位置に到達するための逆運動学方程式のソリューションは1つだけです。
（または、関節の範囲に応じて、最大16の異なるソリューション）

しかし、ロボットがペンのようなものを持ち、ロボットがターゲット上のそのペンで特定のポイントをマークするようにしたい場合、マークされた表面に垂直である限り、ペンの向きを気にしません。

したがって、逆運動学方程式は無限に多くの解を持つことになります。

これらのソリューションの中から、現在の構成に最も近いジョイント構成を選択するにはどうすればよいですか？
（または、すべての関節角度が最大値と最小値から最も離れているなど、他の同様の基準に従って最適である関節構成）

— HugoRune
ソース

回答:

まず、最適なを定義する必要があります。あなたはあなたが最適だと思うことを言わないので、ほとんどの人は二次式を選びます。たとえば、現在の関節角度がベクトルによって与えられているとします。必要な移動を最小化することを検討できます-エラーで、コスト関数定義できますいくつかの行列。通常、対角行列を使用しますが、正定行列でも使用できます。 $\vec{\alpha}$ $\vec{x} = \vec{\alpha} - \vec{\alpha}_{start}$ $J=\vec{x}^TQ\vec{x}$ $Q$

2つの関節角度を持つ単純化された例では、関節がより安価なモーター（おそらくエンドエフェクターに近い）を持っている場合、 $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]$ 、つまり関節の動き関節として高価として2倍である。 $b$ $a$

さて、運動方程式は行列式であり、Denavit-Hartenberg表記では次のようになります。

$\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]$

左辺を乗算すると、3つの方程式が得られます。パラメータが線形であれば、簡単に解決できます。これは、すべてのアクチュエータがリニアアクチュエータの場合です。この場合、問題は実際には2次プログラムです。左辺を並べ替えて、次の式を取得できます。

$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}$

これを解決するために、たとえば内点、アクティブセットなど、使用できるアルゴリズムがいくつかあります。適切なライブラリを見つけるだけで、解決されます。

方程式の非線形システムは解くのがより困難です。これは非線形プログラミングと呼ばれますが、回転ジョイントがある場合はそうです。

基本的に、行列方程式の代わりに、非線形関数があります。

条件として最小化し、（制約のRHSをゼロにする必要がある場合は再配置します） $f(x)$ $\vec{h}(x) = 0$ $\vec{g}(x) \leq 0$

これを解決するために使用されるアルゴリズムはさらに複雑ですが、内点逐次二次計画法（SQP）、アクティブセット、信頼領域反射アルゴリズムが含まれます。明らかに、これらのアルゴリズムがどのように機能するかについての説明は非常に長く、私はそれをこの回答の範囲から外します。二次計画法だけに使用されるアルゴリズムのコンテンツの量は、それ自体で全体のコースになる可能性があると言うだけで十分です。

問題を解決するためのライブラリを見つけるだけで、効率的な実装をコード化するには長い時間がかかり、効率的な実装は一度に100（またはそれ以上）の変数を処理できます。たとえば、MATLABを使用している場合、Optimization Toolboxの関数fminconの使用方法に関するドキュメントがあります。

オンラインで解決するには、C ++または他のネイティブ実装（NLoptなど）が必要になる場合があります。これはマイクロコントローラーがすぐに解決できるものではない場合があり、多くのライブラリーには他の依存関係がある可能性があることに注意してください（マイクロコントローラーで使用するのは簡単ではありません）。

効率を気にせず、自分でコーディングできるものが必要な場合は、逆運動学の問題を解決するために呼び出すことができる関数があると仮定して、勾配降下法を実行できます。たとえば、ランダムな開始方向を任意に選択し、逆の問題を解決してから、コスト関数を確認します。次に、摂動分析を使用して、方向をどのように変えるかを確認できます。たとえば、現在の向き（つまり、立方体グリッドの8ポイント）を中心に同様の向きを確認すると、コスト関数が各方向でどのように変化するかを2次近似で求めることができます。

2次近似（多変量であるためヘッセ行列と呼ばれる-方向は3次元）を使用すると、コスト関数の勾配のゼロ交差（つまり、予測される極小）を見つけることができます。

新しく予測された方向で、逆ソルバーに再度通し、精度が十分になるまで繰り返します。

インバースキネマティック問題自体は反復的に解決する必要があるため、これはおそらくそれほど効率的ではないことに注意してください（そのため、解決にしばらく時間がかかる関数を繰り返し使用しています）。また、含まれるコードは完全な最適化アルゴリズムよりも少ないかもしれませんが、それでもかなり重要であり、時間の取るに足らない投資ではありません。

どちらの方法（形式的には非線形プログラムとして解くか、関数を繰り返し使用して逆問題を解く）を使用しても、複数の極小値がある場合、解は最適ではない可能性があります。この場合、さまざまなアプローチを使用して、グローバルミニマムを見つけることができます。非線形プログラミングソルバーを使用しても、初期値（ジョイント角度など）をシードすることが期待されます。さまざまな方法で生成されたシードを使用して、いずれかのメソッドを繰り返し実行できます。

ランダム再起動（ランダムに生成されます）
グリッドベース

または他のカスタムメソッド。

ただし、極小値が多数ある場合、グローバルな極小値を見つけることを保証する良い方法はないことに注意してください。可能性を高めることができるだけです。

— ronalchn
ソース

質問は産業用ロボットに関するものであるため、おそらくロボットのダイナミクスのモデルがないので、運動学的基準のみを最適化するソリューションを探していると想定しています。

ロボットには、インバースキネマティクスのための閉じた形式のソリューションがありますが、残念ながら、エンドエフェクターには余分な回転自由度があります。つまり、ロボットには本質的に7つの自由度があります。ただし、これはもう1つの自由度なので、思っているほど問題にはなりません。

このようなほとんど冗長性のないロボットの一般的なトリックは、余分な自由度をロックして、残りの関節の値を解析的に解決することです。たとえば、平均ミリ秒で解を返す6 自由度ロボット用の閉じた形式のIKソルバーを作成するとします。したがって、からまで反復することにより、ペン角度を約ミリ秒で離散化するための最適なソリューションを見つけることができます。これは、アプリケーションがおそらく十分すぎると考えられます。 $0.05$ $1$ $360$ $1^\circ$ $18$

ほとんどの場合、ペンが少ししか動かない場合（たとえば、線を描くとき）、検索を高速化する別のトリックは、数値IKを使用することです。たとえば、疑似逆法：

ましょう、ロボットの現在の構成であるものとするそのヤコビアンであり、およびlet現在のエンドエフェクタへの変換対象の変位です。解決用して、新しいコンフィギュレーションを計算。ここでは詳細をスキップしますが、のソリューションは最小化する必要があります適切に選択されたメトリック。 $q_1$ $J$ $\Delta x$ $\Delta x = J \, \Delta q$ $\Delta q$ $q_2 = q_1 + \Delta q$ $\Delta q$ $\|\Delta q\|$

これは7 自由度のロボットに対して行われ、ここでも数ミリ秒しかかかりません。けれども有効な構成ではないかもしれません（共同値が範囲外にすることができます）と（あなたがいますが、より多くの擬似逆の手順を取ることができます）正確なIKソリューションではないかもしれない、ほとんどの時間は、それがAのための出発点は良いだろう閉形式ソルバーを使用して検索します。 $q_2$

— アントナコス
ソース

これには素敵な閉じた形があります。が何であるかを気にしていないとしましょう（つまり、どのように変更するかは気にしません）。 $r_z$

J^{- 1} \dot{X} = \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{x} \\ \dot{y} \\ \dot{z} \\ \dot{r_{x}} \\ \dot{r_{y}} \\ \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{x} \dots \dot{r_{y}}} + {\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}$ これで、ゲームは最小化されましたは、ronalchnのような対角行列上記。私が使用するつもりだと見やすくするための。

({\dot{Θ}}_{\dot{x} \dots \dot{r_{y}}} + {\dot{Θ}}_{\dot{r_{z}}})^{T} D ({\dot{Θ}}_{\dot{x} \dots \dot{r_{y}}} + {\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}^*}$

これを

A^{T} D A + 2 B^{T} D A + B^{T} D B or A^{T} D A + 2 \dot{r_{z}} {\vec{j_{6}}}^{T} D A + {\dot{r_{z}}}^{2} {\vec{j_{6}}}^{T} 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}}$

これで、簡単に区別できる方程式ができました。に関する導関数を見つけ、それを設定し。これにより、2つのポーズ間の関節角度の「距離」が最小化されます。 $\dot{r_z}$ $\dot{r_z}$ $0$

2 {\vec{j_{6}}}^{T} D A + 2 \dot{r_{z}} {\vec{j_{6}}}^{T} D \vec{j_{6}} = 0 \dot{r_{z}} = \frac{- {\vec{j_{6}}}^{T} D A}{{\vec{j_{6}}}^{T} 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}}$

— Joshkarges
ソース