最適化問題の局所的探索法のひとつに共役勾配法がある。この共役な勾配について簡単にまとめる。
いま、最小化すべき関数を$f\left(\vec{x}\right)$とする。$\vec{x}$ 近傍の微小量 $\vec{u}$ と $\vec{v}$ を考え、
$f\left(\vec{x}+\vec{u}+\vec{v}\right)$ を $\vec{x}$ 近傍でTayler展開し、2次の項までとる。
\begin{eqnarray}
f\left(\vec{x}+\vec{u}+\vec{v}\right)&=&f\left(\vec{x}\right)+\sum_{i}\frac{\partial f\left(\vec{x}\right)}{\partial x_i}\left(u_i + v_i\right)+\frac{1}{2}\sum_{ij}\frac{\partial^2 f\left(\vec{x}\right)}{\partial x_i \partial x_j}\left(u_i + v_i\right)\left(u_j + v_j\right)\nonumber \\
&=& f\left(\vec{x}\right)
+ \sum_{i}\frac{\partial f\left(\vec{x}\right)}{\partial x_i}u_i + \frac{1}{2}\sum_{ij}\frac{\partial^2 f\left(\vec{x}\right)}{\partial x_i \partial x_j}u_i\; u_j \nonumber \\
&+& \sum_{i}\frac{\partial f\left(\vec{x}\right)}{\partial x_i}v_i + \frac{1}{2}\sum_{ij}\frac{\partial^2 f\left(\vec{x}\right)}{\partial x_i \partial x_j}v_i\; v_j \nonumber \\
&+& \sum_{ij}\frac{\partial^2 f\left(\vec{x}\right)}{\partial x_i \partial x_j}u_i\;v_j
\end{eqnarray}
ここで、$b_i=\frac{\partial f}{\partial x_i}$、$A_{ij}=\frac{\partial^2 f}{\partial x_i \partial x_j}$と書くと
\begin{equation}
\delta f = \left(\vec{b}^{\;T}\vec{u} + \frac{1}{2}\vec{u}^{\;T} A \;\vec{u}\right)
+\left(\vec{b}^{\;T}\vec{v} + \frac{1}{2}\vec{v}^{\;T} A \;\vec{v}\right) + \vec{u}^{\;T}\;A\;\vec{v}
\end{equation}
となる。上式右辺の最後の項が存在しなければ、$\vec{u}$ と $\vec{v}$ について独立に局所的探索法を適用できる。
$\vec{u}^{\;T}\;A\;\vec{v}= 0$ となるような組 $\{\vec{u},\vec{v}\}$ を共役な関係にあるベクトルと呼ぶ。共役な関係にある勾配の集合を適当に作り、互いに干渉することなく効率良く最適解を探索する手法が共役勾配法である。
2013年9月15日日曜日
Kinematics 〜 Implementation 〜
in Japanese
In the previous page, I described a brief explanation on the kinematics. Here I show you my implementation of the algorithm of a six-axis-arm system in C++.
A green sphere indicates a target position. When the end point of the arm arrives at it, the target is moved to the next position.
Here is my source code.
A file
Introduction
In the previous page, I described a brief explanation on the kinematics. Here I show you my implementation of the algorithm of a six-axis-arm system in C++.
Demo
Development Environment
- Mac OS X 10.8.4
- Processor：3.06 GHz Intel Core 2 Duo
- Memory：4GB
- Xcode4.6.2 with Apple LLVM 4.2（C++ Language Dialect → C++11, C++ Standard Library → libc++）
- boost-1.54.0（built by Apple LLVM 4.2 compiler）
Source
Here is my source code.
Usage
A file
robot.dat passed as an argument determines a form of the arm system.
The format of the file is expressed in 1.
Using keyboard,
- 'c' starts the program,
- 'b' inverts the movement of the target sphere,
- 'a' restores the movement,
- and
escapestops the program.
References
- OpenGL 3Dグラフィックス入門 第2版 三浦憲二郎 朝倉書店 (In Japanese)
Kinematics 〜 Theory 〜
in Japanese
In this page, I describe a brief explanation on the kinematics used in the robotics. See also the next page as to the implementation of a simple six-axis-arm system in C++.
Let us consider the 3-dimensional homogeneous coordinate system. In the system, the rotation matrices around x, y, and z axises are given by \begin{equation} R_{x}(\theta) = \left( \begin{array}{cccc} 1 & 0 & 0 & 0 \\ 0 & \cos{\theta} & -\sin{\theta} & 0 \\ 0 & \sin{\theta} & \cos{\theta} & 0 \\ 0 & 0 & 0 & 1 \end{array} \right), \end{equation} \begin{equation} R_{y}(\theta) = \left( \begin{array}{cccc} \cos{\theta} & 0 & \sin{\theta} & 0 \\ 0 & 1 & 0 & 0 \\ -\sin{\theta} & 0 & \cos{\theta} & 0 \\ 0 & 0 & 0 & 1 \end{array} \right), \end{equation} and \begin{equation} R_{z}(\theta) = \left( \begin{array}{cccc} \cos{\theta} & -\sin{\theta} & 0 & 0 \\ \sin{\theta} & \cos{\theta} & 0 & 0 \\ 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1 \end{array} \right). \end{equation} Moreover, the translation matrix $L(\vec{l})$ with a directional vector $\vec{l}$ is obtained by \begin{equation} L(\vec{l}) = \left( \begin{array}{cccc} 1 & 0 & 0 & l_x \\ 0 & 1 & 0 & l_y \\ 0 & 0 & 1 & l_z \\ 0 & 0 & 0 & 1 \end{array} \right). \end{equation} Suppose that we have an initial state of a three-axis-arm system shown in the below figure.
In the figure, $n_i$ and $l_i$ indicate a joint and a length of each component, respectively.
$n_1$ rotates $\theta_1$ in counterclockwise direction around $x$ axis, $n_2$ rotates $\theta_2$ in counterclockwise direction around $y$ axis, and $n_3$ rotates $\theta_3$ in counterclockwise direction around $x$ axis. $\vec{l}_1=(0,0,l_1)$, $\vec{l}_2=(0,0,l_2)$, and $\vec{l}_3=(0,0,l_3)$.
A position vector of an end point $n_4$ takes the form
\begin{equation}
\vec{p}_{4} = R_x(\theta_1) L(\vec{l}_1) R_y(\theta_2) L(\vec{l}_2) R_x(\theta_3) L(\vec{l}_3)
\left(
\begin{array}{c}
0 \\
0 \\
0 \\
1
\end{array}
\right).
\label{sample-case}
\end{equation}
It must be noted that parameters $\theta_i$ and $\vec{l}_i$ are defined in the local coordinate system of the $i$-th joint.
We can expand eq.(\ref{sample-case}) to the case of $n$-axis-arm system as
\begin{equation}
\vec{p} = R_{a_{1}}(\theta_1) L(\vec{l}_1) R_{a_{2}}(\theta_2) L(\vec{l}_2) \cdots R_{a_{n}}(\theta_n) L(\vec{l}_n)
\left(
\begin{array}{c}
0 \\
0 \\
0 \\
1
\end{array}
\right),
\end{equation}
where $a_{i}\in \{x,y,z\}$.
Setting lengths to constant values, the vector $\vec{p}$ becomes a function with $n$ angles in the form \begin{equation} \vec{p}= \vec{f}(\theta_1,\theta_2,\cdots,\theta_n). \end{equation} Its deviation with respect to $\{\theta_1,\theta_2,\cdots,\theta_n\}$ is written as \begin{eqnarray} \delta\vec{p} &=& \vec{f}(\theta_1+\delta\theta_1,\theta_2+\delta\theta_2,\cdots,\theta_n+\delta\theta_n) - \vec{f}(\theta_1,\theta_2,\cdots,\theta_n)\nonumber \\ &=&\sum_{i=1}^{n}\;\frac{\partial\vec{f}}{\partial\theta_i}\;\delta\theta_i + \frac{1}{2}\sum_{i=1}^{n}\sum_{j=1}^{n}\;\frac{\partial^2\vec{f}}{\partial\theta_i\partial\theta_j}\;\delta\theta_i\;\delta\theta_j +\cdots. \end{eqnarray} Ignoring the terms more than first derivative, we can obtain the form \begin{eqnarray} \left( \begin{array}{c} \delta p_x \\ \delta p_y \\ \delta p_z \end{array} \right) &=& \left(\frac{\partial\vec{f}}{\partial\theta_1},\frac{\partial\vec{f}}{\partial\theta_2},\cdots,\frac{\partial\vec{f}}{\partial\theta_n}\right) \left( \begin{array}{c} \delta \theta_1 \\ \delta \theta_2 \\ \vdots \\ \delta \theta_n \end{array} \right) \nonumber \\ &=& \left( \begin{array}{cccc} \frac{\partial f_1}{\partial \theta_1} & \frac{\partial f_1}{\partial \theta_2} & \cdots & \frac{\partial f_1}{\partial \theta_n} \\ \frac{\partial f_2}{\partial \theta_1} & \frac{\partial f_2}{\partial \theta_2} & \cdots & \frac{\partial f_2}{\partial \theta_n} \\ \frac{\partial f_3}{\partial \theta_1} & \frac{\partial f_3}{\partial \theta_2} & \cdots & \frac{\partial f_3}{\partial \theta_n} \end{array} \right) \left( \begin{array}{c} \delta \theta_1 \\ \delta \theta_2 \\ \vdots \\ \delta \theta_n \end{array} \right) \nonumber \\ &\equiv& J \left( \begin{array}{c} \delta \theta_1 \\ \delta \theta_2 \\ \vdots \\ \delta \theta_n \end{array} \right), \end{eqnarray} where $J$ is called the Jacobian matrix. In this case, it is $3 \times n$ matrix. A partial differential $\frac{\partial \vec{f}}{\partial \theta_i}$ is calculated as \begin{equation} \frac{\partial \vec{f}}{\partial \theta_i} = R_{a_{1}}(\theta_1) L(\vec{l}_1) \cdots \frac{dR_{a_{i}}(\theta_i)}{d\theta_i} L(\vec{l}_i) \cdots R_{a_{n}}(\theta_n) L(\vec{l}_n) \left( \begin{array}{c} 0 \\ 0 \\ 0 \\ 1 \end{array} \right). \end{equation} By introducing the inverse matrix $J^{-1}$, we can obtain the deviation vector $\delta \vec{\theta}$ of angles as \begin{equation} \delta \vec{\theta} = J^{-1}\;\delta\vec{p}. \end{equation} When ${\rm rank}(J)>3$, the inverse matrix $J^{-1}$ is not uniquely determined. In other words, in a case where the number of joints are greater than 3, the end position ($\vec{p}$) is achieved by different postures. The typical method to determine a unique posture is to use a pseudoinverse matrix $J^{\#}$ as \begin{equation} \delta \vec{\theta} = J^{\#} \delta\vec{p}, \end{equation} where \begin{equation} J^{\#}=J^{T}\;(J\;J^{T})^{-1}. \end{equation} The procedure to solve the inverse kinematics problem is as follows:
Introduction
In this page, I describe a brief explanation on the kinematics used in the robotics. See also the next page as to the implementation of a simple six-axis-arm system in C++.
Forward Kinematics
Let us consider the 3-dimensional homogeneous coordinate system. In the system, the rotation matrices around x, y, and z axises are given by \begin{equation} R_{x}(\theta) = \left( \begin{array}{cccc} 1 & 0 & 0 & 0 \\ 0 & \cos{\theta} & -\sin{\theta} & 0 \\ 0 & \sin{\theta} & \cos{\theta} & 0 \\ 0 & 0 & 0 & 1 \end{array} \right), \end{equation} \begin{equation} R_{y}(\theta) = \left( \begin{array}{cccc} \cos{\theta} & 0 & \sin{\theta} & 0 \\ 0 & 1 & 0 & 0 \\ -\sin{\theta} & 0 & \cos{\theta} & 0 \\ 0 & 0 & 0 & 1 \end{array} \right), \end{equation} and \begin{equation} R_{z}(\theta) = \left( \begin{array}{cccc} \cos{\theta} & -\sin{\theta} & 0 & 0 \\ \sin{\theta} & \cos{\theta} & 0 & 0 \\ 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1 \end{array} \right). \end{equation} Moreover, the translation matrix $L(\vec{l})$ with a directional vector $\vec{l}$ is obtained by \begin{equation} L(\vec{l}) = \left( \begin{array}{cccc} 1 & 0 & 0 & l_x \\ 0 & 1 & 0 & l_y \\ 0 & 0 & 1 & l_z \\ 0 & 0 & 0 & 1 \end{array} \right). \end{equation} Suppose that we have an initial state of a three-axis-arm system shown in the below figure.
Inverse Kinematcis
Setting lengths to constant values, the vector $\vec{p}$ becomes a function with $n$ angles in the form \begin{equation} \vec{p}= \vec{f}(\theta_1,\theta_2,\cdots,\theta_n). \end{equation} Its deviation with respect to $\{\theta_1,\theta_2,\cdots,\theta_n\}$ is written as \begin{eqnarray} \delta\vec{p} &=& \vec{f}(\theta_1+\delta\theta_1,\theta_2+\delta\theta_2,\cdots,\theta_n+\delta\theta_n) - \vec{f}(\theta_1,\theta_2,\cdots,\theta_n)\nonumber \\ &=&\sum_{i=1}^{n}\;\frac{\partial\vec{f}}{\partial\theta_i}\;\delta\theta_i + \frac{1}{2}\sum_{i=1}^{n}\sum_{j=1}^{n}\;\frac{\partial^2\vec{f}}{\partial\theta_i\partial\theta_j}\;\delta\theta_i\;\delta\theta_j +\cdots. \end{eqnarray} Ignoring the terms more than first derivative, we can obtain the form \begin{eqnarray} \left( \begin{array}{c} \delta p_x \\ \delta p_y \\ \delta p_z \end{array} \right) &=& \left(\frac{\partial\vec{f}}{\partial\theta_1},\frac{\partial\vec{f}}{\partial\theta_2},\cdots,\frac{\partial\vec{f}}{\partial\theta_n}\right) \left( \begin{array}{c} \delta \theta_1 \\ \delta \theta_2 \\ \vdots \\ \delta \theta_n \end{array} \right) \nonumber \\ &=& \left( \begin{array}{cccc} \frac{\partial f_1}{\partial \theta_1} & \frac{\partial f_1}{\partial \theta_2} & \cdots & \frac{\partial f_1}{\partial \theta_n} \\ \frac{\partial f_2}{\partial \theta_1} & \frac{\partial f_2}{\partial \theta_2} & \cdots & \frac{\partial f_2}{\partial \theta_n} \\ \frac{\partial f_3}{\partial \theta_1} & \frac{\partial f_3}{\partial \theta_2} & \cdots & \frac{\partial f_3}{\partial \theta_n} \end{array} \right) \left( \begin{array}{c} \delta \theta_1 \\ \delta \theta_2 \\ \vdots \\ \delta \theta_n \end{array} \right) \nonumber \\ &\equiv& J \left( \begin{array}{c} \delta \theta_1 \\ \delta \theta_2 \\ \vdots \\ \delta \theta_n \end{array} \right), \end{eqnarray} where $J$ is called the Jacobian matrix. In this case, it is $3 \times n$ matrix. A partial differential $\frac{\partial \vec{f}}{\partial \theta_i}$ is calculated as \begin{equation} \frac{\partial \vec{f}}{\partial \theta_i} = R_{a_{1}}(\theta_1) L(\vec{l}_1) \cdots \frac{dR_{a_{i}}(\theta_i)}{d\theta_i} L(\vec{l}_i) \cdots R_{a_{n}}(\theta_n) L(\vec{l}_n) \left( \begin{array}{c} 0 \\ 0 \\ 0 \\ 1 \end{array} \right). \end{equation} By introducing the inverse matrix $J^{-1}$, we can obtain the deviation vector $\delta \vec{\theta}$ of angles as \begin{equation} \delta \vec{\theta} = J^{-1}\;\delta\vec{p}. \end{equation} When ${\rm rank}(J)>3$, the inverse matrix $J^{-1}$ is not uniquely determined. In other words, in a case where the number of joints are greater than 3, the end position ($\vec{p}$) is achieved by different postures. The typical method to determine a unique posture is to use a pseudoinverse matrix $J^{\#}$ as \begin{equation} \delta \vec{\theta} = J^{\#} \delta\vec{p}, \end{equation} where \begin{equation} J^{\#}=J^{T}\;(J\;J^{T})^{-1}. \end{equation} The procedure to solve the inverse kinematics problem is as follows:
- Suppose that a target position of the end point is $\vec{p}_{\rm G}$.
- Calculate a displacement $\vec{d} = \vec{p}_{\rm G}-\vec{p}$ where $\vec{p}$ indicates the current positoin of the end point. Using $\vec{d}$, we make a small deviation $\delta \vec{p}=\alpha\;\vec{d}/|\vec{d}|$ where $\alpha$ is an appropriate positive value.
- Using current angles, we calculate $J^{\#}$.
- Calculate $\delta \vec{\theta}=J^{\#} \delta\vec{p}$.
- Update the quantities, $\vec{\theta} \leftarrow \vec{\theta} + \delta \vec{\theta}, \vec{p} \leftarrow \vec{p} + \delta \vec{p}$.
- After updating them, if the displacement $|\vec{d}| = |\vec{p}_{\rm G}-\vec{p}|$ is not in an acceptable error range $\epsilon$, return to 2.
References
- Forward Kinematics (in Japanese)
- Inverse Kinematics by means of Jacobian (in Japanese)
- Kinematics Problem of Robot (in Japanese)
2013年9月7日土曜日
運動学 〜 実装 〜
in English
先のページで解説したアルゴリズムをC++11で実装した。
緑の球が目的の位置を表す。ロボットの手の先がそれに到達すると、球は次の位置へ移動する。
ソースはここ。
引数に渡しているファイルrobot.datによりロボットの形状を決定している。 そのフォーマットは文献1のものを使用した。 キーボードから
はじめに
先のページで解説したアルゴリズムをC++11で実装した。
デモ
開発環境
- Mac OS X 10.8.4
- プロセッサ：3.06 GHz Intel Core 2 Duo
- メモリ：4GB
- Xcode4.6.2 with Apple LLVM 4.2（C++ Language Dialect → C++11, C++ Standard Library → libc++）
- boost-1.54.0（Apple LLVM 4.2でコンパイルしたもの。）
ソース
ソースはここ。
実行方法
引数に渡しているファイルrobot.datによりロボットの形状を決定している。 そのフォーマットは文献1のものを使用した。 キーボードから
- 'c'を入力してスタート
- 'b'を入力すると逆向き
- 'a'を入力すると元の向き
- escapeキーで終了
参考文献
- OpenGL 3Dグラフィックス入門 第2版 三浦憲二郎 朝倉書店
2013年8月23日金曜日
C++メモ 〜 std::regex 〜
大文字小文字を区別せずに検索するには？
出力：
raw string literalを使えばエスケープせずにダブルクオーテーションを利用できる（７行目）。
空白以外の文字列を拾うには？ 出力：
数字を拾うには？ 出力：
検索文字の先頭と末尾に特定の文字を挿入するには？ 出力： 参照
空白以外の文字列を拾うには？ 出力：
std::regexのオブジェクトを作る際（10行目）、raw string literalを使えばエスケープを行う必要はない。
数字を拾うには？ 出力：
検索文字の先頭と末尾に特定の文字を挿入するには？ 出力： 参照
2013年8月6日火曜日
C++メモ 〜 boost::coroutine 〜
Macではlibboost_coroutine.aとlibboost_context.dylib(.a)をリンクする必要があります。
出力：
出力：
出力：
おまけ。 出力：
追記：2014/1/12 ファイルa.txtの内容は以下の通り。 出力： 関数
Routine1のコンストラクタが呼ばれた時点でloop1の中の
yieldが一度呼ばれます。
出力：
loop2の先頭にあるyield(1)が出力の先頭にある1を生成します。
yield(1)がないと、15行目で落ちます。
Routine2のコンストラクタが呼ばれた時点ではAのオブジェクトが設定されていないからです。
出力：
おまけ。 出力：
追記：2014/1/12 ファイルa.txtの内容は以下の通り。 出力： 関数
read_fileのプロトタイプがないと警告がでるようになった(Apple LLVM 5.0, boost-1.55.0)。
2013年8月4日日曜日
運動学
in English
ロボット工学で使われる順運動学と逆運動学についてまとめる。 実装編はこちら。
３次元空間右手系の同次座標で議論を行う。各軸周りの回転行列は以下の通りである。
$x$軸周りの回転行列 $R_{x}(\theta)$:
\begin{equation} R_{x}(\theta) = \left( \begin{array}{cccc} 1 & 0 & 0 & 0 \\ 0 & \cos{\theta} & -\sin{\theta} & 0 \\ 0 & \sin{\theta} & \cos{\theta} & 0 \\ 0 & 0 & 0 & 1 \end{array} \right) \end{equation} $y$軸周りの回転行列 $R_{y}(\theta)$: \begin{equation} R_{y}(\theta) = \left( \begin{array}{cccc} \cos{\theta} & 0 & \sin{\theta} & 0 \\ 0 & 1 & 0 & 0 \\ -\sin{\theta} & 0 & \cos{\theta} & 0 \\ 0 & 0 & 0 & 1 \end{array} \right) \end{equation} $z$軸周りの回転行列 $R_{z}(\theta)$: \begin{equation} R_{z}(\theta) = \left( \begin{array}{cccc} \cos{\theta} & -\sin{\theta} & 0 & 0 \\ \sin{\theta} & \cos{\theta} & 0 & 0 \\ 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1 \end{array} \right) \end{equation} また、方向 $\vec{l}$ への並進移動行列 $L(\vec{l})$ は次式で与えられる。 \begin{equation} L(\vec{l}) = \left( \begin{array}{cccc} 1 & 0 & 0 & l_x \\ 0 & 1 & 0 & l_y \\ 0 & 0 & 1 & l_z \\ 0 & 0 & 0 & 1 \end{array} \right) \end{equation} いま各関節 $n_i$ の初期状態が図のように与えられたとする。
関節 $n_1$ は$x$軸周りに $\theta_1$、関節 $n_2$ は$y$軸周りに $\theta_2$、関節 $n_3$ は$x$軸周りに $\theta_3$ だけ回転するとき、端点$n_4$の位置ベクトルは次式で計算される。
\begin{equation}
\vec{p}_{4} = R_x(\theta_1) L(\vec{l}_1) R_y(\theta_2) L(\vec{l}_2) R_x(\theta_3) L(\vec{l}_3)
\left(
\begin{array}{c}
0 \\
0 \\
0 \\
1
\end{array}
\right)
\end{equation}
各関節での局所座標は、その関節に到るまでの変換行列の影響を受ける。回転行列、並進移動行列のパラメータは各関節の局所座標系で定義される量である。一般に関節が$n$個ある場合の端点(end-effector)の位置座標 $\vec{p}$ は
\begin{equation}
\vec{p} = R_{a_{1}}(\theta_1) L(\vec{l}_1) R_{a_{2}}(\theta_2) L(\vec{l}_2) \cdots R_{a_{n}}(\theta_n) L(\vec{l}_n)
\left(
\begin{array}{c}
0 \\
0 \\
0 \\
1
\end{array}
\right)
\end{equation}
となる。ここで$a_{i}\in \{x,y,z\}$である。
腕（リンク）の長さが定数であるとき位置ベクトル $\vec{p}$ は $n$ 個の角度の関数である。 \begin{equation} \vec{p}= \vec{f}(\theta_1,\theta_2,\cdots,\theta_n) \end{equation} 変分をとると \begin{eqnarray} \delta\vec{p} &=& \vec{f}(\theta_1+\delta\theta_1,\theta_2+\delta\theta_2,\cdots,\theta_n+\delta\theta_n) - \vec{f}(\theta_1,\theta_2,\cdots,\theta_n)\nonumber \\ &=&\sum_{i=1}^{n}\;\frac{\partial\vec{f}}{\partial\theta_i}\;\delta\theta_i + \frac{1}{2}\sum_{i=1}^{n}\sum_{j=1}^{n}\;\frac{\partial^2\vec{f}}{\partial\theta_i\partial\theta_j}\;\delta\theta_i\;\delta\theta_j +\cdots \end{eqnarray} 2次以降の項を無視して書き下ろすと \begin{eqnarray} \left( \begin{array}{c} \delta p_x \\ \delta p_y \\ \delta p_z \end{array} \right) &=& \left(\frac{\partial\vec{f}}{\partial\theta_1},\frac{\partial\vec{f}}{\partial\theta_2},\cdots,\frac{\partial\vec{f}}{\partial\theta_n}\right) \left( \begin{array}{c} \delta \theta_1 \\ \delta \theta_2 \\ \vdots \\ \delta \theta_n \end{array} \right) \nonumber \\ &=& \left( \begin{array}{cccc} \frac{\partial f_1}{\partial \theta_1} & \frac{\partial f_1}{\partial \theta_2} & \cdots & \frac{\partial f_1}{\partial \theta_n} \\ \frac{\partial f_2}{\partial \theta_1} & \frac{\partial f_2}{\partial \theta_2} & \cdots & \frac{\partial f_2}{\partial \theta_n} \\ \frac{\partial f_3}{\partial \theta_1} & \frac{\partial f_3}{\partial \theta_2} & \cdots & \frac{\partial f_3}{\partial \theta_n} \end{array} \right) \left( \begin{array}{c} \delta \theta_1 \\ \delta \theta_2 \\ \vdots \\ \delta \theta_n \end{array} \right) \nonumber \\ &\equiv& J \left( \begin{array}{c} \delta \theta_1 \\ \delta \theta_2 \\ \vdots \\ \delta \theta_n \end{array} \right) \end{eqnarray} となる。ここで行列 $J$ はヤコビアン（Jacobian）である。いまの場合 $3 \times n$ の行列となる。要素に現れる偏微分は次式で計算できる。 \begin{equation} \frac{\partial \vec{f}}{\partial \theta_i} = R_{a_{1}}(\theta_1) L(\vec{l}_1) \cdots \frac{dR_{a_{i}}(\theta_i)}{d\theta_i} L(\vec{l}_i) \cdots R_{a_{n}}(\theta_n) L(\vec{l}_n) \left( \begin{array}{c} 0 \\ 0 \\ 0 \\ 1 \end{array} \right) \end{equation} 逆行列 $J^{-1}$を使えば位置の変位量 $\delta \vec{p}$ から角度の変位量 $\delta \vec{\theta}$ を求めることができる。 \begin{equation} \delta \vec{\theta} = J^{-1}\;\delta\vec{p} \end{equation} ${\rm rank}(J)>3$のとき $J$ の逆行列は無数に存在する。 すなわち、関節の数が3より大きい場合、1つの端点（end effector）の位置に対応する腕の姿勢は無数に存在する。したがって、任意の拘束条件を導入して $\delta \vec{\theta}$ を一意に決定する必要がある。その代表的な方法が、擬似逆行列（pseudoinverse）$J^{\#}$ を用いる方法である。 \begin{equation} \delta \vec{\theta} = J^{\#} \delta\vec{p} \end{equation} ここで、 \begin{equation} J^{\#}=J^{T}\;(J\;J^{T})^{-1} \end{equation} である。 逆運動学問題を解く手順は以下の通りである。
はじめに
ロボット工学で使われる順運動学と逆運動学についてまとめる。 実装編はこちら。
順運動学
３次元空間右手系の同次座標で議論を行う。各軸周りの回転行列は以下の通りである。
$x$軸周りの回転行列 $R_{x}(\theta)$:
\begin{equation} R_{x}(\theta) = \left( \begin{array}{cccc} 1 & 0 & 0 & 0 \\ 0 & \cos{\theta} & -\sin{\theta} & 0 \\ 0 & \sin{\theta} & \cos{\theta} & 0 \\ 0 & 0 & 0 & 1 \end{array} \right) \end{equation} $y$軸周りの回転行列 $R_{y}(\theta)$: \begin{equation} R_{y}(\theta) = \left( \begin{array}{cccc} \cos{\theta} & 0 & \sin{\theta} & 0 \\ 0 & 1 & 0 & 0 \\ -\sin{\theta} & 0 & \cos{\theta} & 0 \\ 0 & 0 & 0 & 1 \end{array} \right) \end{equation} $z$軸周りの回転行列 $R_{z}(\theta)$: \begin{equation} R_{z}(\theta) = \left( \begin{array}{cccc} \cos{\theta} & -\sin{\theta} & 0 & 0 \\ \sin{\theta} & \cos{\theta} & 0 & 0 \\ 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1 \end{array} \right) \end{equation} また、方向 $\vec{l}$ への並進移動行列 $L(\vec{l})$ は次式で与えられる。 \begin{equation} L(\vec{l}) = \left( \begin{array}{cccc} 1 & 0 & 0 & l_x \\ 0 & 1 & 0 & l_y \\ 0 & 0 & 1 & l_z \\ 0 & 0 & 0 & 1 \end{array} \right) \end{equation} いま各関節 $n_i$ の初期状態が図のように与えられたとする。
逆運動学
腕（リンク）の長さが定数であるとき位置ベクトル $\vec{p}$ は $n$ 個の角度の関数である。 \begin{equation} \vec{p}= \vec{f}(\theta_1,\theta_2,\cdots,\theta_n) \end{equation} 変分をとると \begin{eqnarray} \delta\vec{p} &=& \vec{f}(\theta_1+\delta\theta_1,\theta_2+\delta\theta_2,\cdots,\theta_n+\delta\theta_n) - \vec{f}(\theta_1,\theta_2,\cdots,\theta_n)\nonumber \\ &=&\sum_{i=1}^{n}\;\frac{\partial\vec{f}}{\partial\theta_i}\;\delta\theta_i + \frac{1}{2}\sum_{i=1}^{n}\sum_{j=1}^{n}\;\frac{\partial^2\vec{f}}{\partial\theta_i\partial\theta_j}\;\delta\theta_i\;\delta\theta_j +\cdots \end{eqnarray} 2次以降の項を無視して書き下ろすと \begin{eqnarray} \left( \begin{array}{c} \delta p_x \\ \delta p_y \\ \delta p_z \end{array} \right) &=& \left(\frac{\partial\vec{f}}{\partial\theta_1},\frac{\partial\vec{f}}{\partial\theta_2},\cdots,\frac{\partial\vec{f}}{\partial\theta_n}\right) \left( \begin{array}{c} \delta \theta_1 \\ \delta \theta_2 \\ \vdots \\ \delta \theta_n \end{array} \right) \nonumber \\ &=& \left( \begin{array}{cccc} \frac{\partial f_1}{\partial \theta_1} & \frac{\partial f_1}{\partial \theta_2} & \cdots & \frac{\partial f_1}{\partial \theta_n} \\ \frac{\partial f_2}{\partial \theta_1} & \frac{\partial f_2}{\partial \theta_2} & \cdots & \frac{\partial f_2}{\partial \theta_n} \\ \frac{\partial f_3}{\partial \theta_1} & \frac{\partial f_3}{\partial \theta_2} & \cdots & \frac{\partial f_3}{\partial \theta_n} \end{array} \right) \left( \begin{array}{c} \delta \theta_1 \\ \delta \theta_2 \\ \vdots \\ \delta \theta_n \end{array} \right) \nonumber \\ &\equiv& J \left( \begin{array}{c} \delta \theta_1 \\ \delta \theta_2 \\ \vdots \\ \delta \theta_n \end{array} \right) \end{eqnarray} となる。ここで行列 $J$ はヤコビアン（Jacobian）である。いまの場合 $3 \times n$ の行列となる。要素に現れる偏微分は次式で計算できる。 \begin{equation} \frac{\partial \vec{f}}{\partial \theta_i} = R_{a_{1}}(\theta_1) L(\vec{l}_1) \cdots \frac{dR_{a_{i}}(\theta_i)}{d\theta_i} L(\vec{l}_i) \cdots R_{a_{n}}(\theta_n) L(\vec{l}_n) \left( \begin{array}{c} 0 \\ 0 \\ 0 \\ 1 \end{array} \right) \end{equation} 逆行列 $J^{-1}$を使えば位置の変位量 $\delta \vec{p}$ から角度の変位量 $\delta \vec{\theta}$ を求めることができる。 \begin{equation} \delta \vec{\theta} = J^{-1}\;\delta\vec{p} \end{equation} ${\rm rank}(J)>3$のとき $J$ の逆行列は無数に存在する。 すなわち、関節の数が3より大きい場合、1つの端点（end effector）の位置に対応する腕の姿勢は無数に存在する。したがって、任意の拘束条件を導入して $\delta \vec{\theta}$ を一意に決定する必要がある。その代表的な方法が、擬似逆行列（pseudoinverse）$J^{\#}$ を用いる方法である。 \begin{equation} \delta \vec{\theta} = J^{\#} \delta\vec{p} \end{equation} ここで、 \begin{equation} J^{\#}=J^{T}\;(J\;J^{T})^{-1} \end{equation} である。 逆運動学問題を解く手順は以下の通りである。
- 端点の目標位置を $\vec{p}_{\rm G}$ とする。
- 現在の端点の位置 $\vec{p}$ からの変位量 $\vec{d} = \vec{p}_{\rm G}-\vec{p}$ から微小変位量を求める。$\delta \vec{p}=\alpha\;\vec{d}/|\vec{d}|$。ここで $\alpha$ は適当な微小量である。
- 現在の $\vec{\theta}$ を使って $J^{\#}$ を計算する。
- $\delta \vec{\theta}=J^{\#} \delta\vec{p}$ を計算する。
- $\vec{\theta} \leftarrow \vec{\theta} + \delta \vec{\theta}, \vec{p} \leftarrow \vec{p} + \delta \vec{p}$
- 更新後の変位量 $|\vec{d}| = |\vec{p}_{\rm G}-\vec{p}|$が許容誤差 $\epsilon$ 以下なら計算終了。そうでなければ、2へ戻る。
参考文献
登録:
投稿 (Atom)