2.1. Theory

PKM加工機に関するコンプライアンスの計算方法について述べる．

2.1.1. はじめに

PKM加工機の運動方程式を考えてみる．PKM加工機は，レールに沿って動くBaseと、Baseに接続された6本のリンクに沿って指示されるPlatformに沿って構成される．ここで，BaseとPlatformをつなぐリンクはそれぞれユニバーサルジョイント，ボールジョイントによって接続されている．具体的にイメージしてもらうために，次ページに記載した模式図を参照してほしい． par Platformに取り付けられた工具が加工反力を受けた時，リンクを伝ってPlatformだけでなくBaseも力を受け，運動する．したがって，PKM加工機の運動を考えるためには，これら2つの運動を考慮する必要がある．ここで，Baseの重心の位置，回転角を表すベクトルを絶対座標系で以下の様に定義する．

(1)\[ \begin{align}\begin{aligned}\boldsymbol{t}_{ob}&=[x_{ob}， y_{ob}，z_{ob} ]\\\boldsymbol{\theta}_{ob}&= [\theta_{ob_x}， \theta_{ob_y} ， \theta_{ob_z} ]\end{aligned}\end{align} \]

また．Platformの重心の位置，回転角を表すベクトルを以下のようにする．

(2)\[ \begin{align}\begin{aligned}\boldsymbol{t}_{op}&=[x_{op}， y_{op}，z_{op} ]\\\boldsymbol{\theta}_{op}&= [\theta_{op_x}， \theta_{op_y} ， \theta_{op_z} ]\end{aligned}\end{align} \]

ここで，上四つのベクトルをまとめて， $\boldsymbol{q}$ を $\boldsymbol{q} = [ \boldsymbol{t}_{op}， \boldsymbol{\theta}_{op}，\boldsymbol{t}_{ob}， \boldsymbol{\theta}_{ob} ]^T$ と表すことにすると，PKM加工機全体の運動方程式は以下の様に表すことができる．

(3)\[M\ddot{\boldsymbol{q}}+C\dot{\boldsymbol{q}}+K\boldsymbol{q} = \boldsymbol{F}\]

ここで， $M$ は質量マトリクス， $C$ は減衰マトリクス， $K$ は剛性マトリクスと呼ぶ．$\boldsymbol{q}$ は12成分のベクトルであるから，これらは $12×12$ の行列となる．これらを求めることで，PKM加工機の静剛性，動剛性を考えることができる．これから，$M，C，K$ は具体的にどのように表すことができるか考えていこう．

2.1.2. 記号の説明

この計算書に断りなく現れる記号について，以下にその一覧と説明を記載する．

$\{O\}$ : 絶対座標系

$\{P\}$ : Platformの重心にとりつけられた座標系，Platformと一緒に移動，回転する．

$\{B\}$ : Baseの重心にとりつけられた座標系，Baseと一緒に移動，回転する．

$\boldsymbol{t}_{op}$ : $\{ O \}$ におけるPlatform重心の位置ベクトル

$\boldsymbol{t}_{ob}$ : $\{ O \}$ におけるBase重心の位置ベクトル

$\boldsymbol{\theta}_{op}$ : $\{O \}$ における $\{P\}$ の回転ベクトル，Platformの回転角を表す．

$\boldsymbol{\theta}_{ob}$ : $\{O\}$ における $\{B\}$ の回転ベクトル，Baseの回転角を表す．

$\boldsymbol{q}$ : 一般化座標，PKM加工機全体の姿勢を表す．

$\boldsymbol{p}_{oi}$ : $\{O\}$ における $i$ 本目のリンクのボールジョイントの位置ベクトル．

$\boldsymbol{b}_{oi}$ : $\{O\}$ における $i$ 本目のリンクのユニバーサルジョイントの位置ベクトル．

$\boldsymbol{t}$ : $\{O\}$ におけるBaseの重心からPlatformの重心までのベクトル．

$\boldsymbol{q}_{si}$ : $\{P\}$ におけるPlatform重心からi本目のリンクのボールジョイントへの位置ベクトル， $\{P\}$ は移動，回転する座標系なので固定の値となる．

$\boldsymbol{q}_{bi}$ : $\{B\}$ におけるBase重心から $i$ 本目のリンクのユニバーサルジョイントへの位置ベクトル， $\{B\}$ は移動，回転する座標系なので固定の値となる．

$R_{ob}$ : $\{O\}$ を基準としたB座標系の回転行列 $\{B\}$ で書かれたベクトル $\boldsymbol{a}$ は $R_{ob}\boldsymbol{a}$ とすると $\{O\}$ で書かれたベクトルになる．

$R$ : $\{B\}$ を基準としたP座標系の回転行列 $\{P\}$ で書かれたベクトル $\boldsymbol{a}$ は $R\boldsymbol{a}$ とすると $\{R\}$ で書かれたベクトルになる．

$l_t$ : $i$ 本目のリンクの重心から，ボールジョイントまでの長さ

$\rho_i$ : $i$ 本目のリンクの長さ

$\boldsymbol{n}_{i}$ : $\{O\}$ におけるi本目のリンクの軸方向ベクトル，ノルムは1(単位ベクトル)になるよう設定．

$m_{b}，m_{p}，m_{l}$ : それぞれBase，Platform，リンクの質量

$I_{b}，I_{p}，I_{l}$ : それぞれBase，Platform，リンクの慣性テンソル行列( $3 \times 3$ )

$Q_{bi}，Q_{si}$ : それぞれ $\boldsymbol{q}_{bi}$ ， $\boldsymbol{q}_{si}$ の $\text{skev-symmetric matrix}$ ，例えば $Q_{bi}$ の場合， $\{B\}$ で書かれたベクトル $\boldsymbol{a}$ について $Q_{bi}\boldsymbol{a}=\boldsymbol{q}_{bi} \times \boldsymbol{a}$ ， $Q_{si}$ の場合は $\{P\}$ で書かれたベクトル $\boldsymbol{b}$ について $Q_{si}\boldsymbol{b}=\boldsymbol{q}_{si} \times \boldsymbol{b}$ を満たす．

$N_{i}$ : $\boldsymbol{n}_{i}$ の $\text{skev-symmetrix matrix}$ ．性質は上述の通り．

$\hat{\dot{\theta}}_{ob}，\hat{\dot{\theta}}_{op}$ : それぞれ $\dot{\boldsymbol{\theta}_{ob}}，\dot{\boldsymbol{\theta}_{op}}$ の $\text{skev-symmetrix matrix}$ ．性質は上述の通り．

2.1.3. リンクの運動

はじめにで示した各マトリクスを導出する前の準備として，リンクの運動を考えてみる．具体的にはリンクの速度を $\dot{\boldsymbol{q}}$ で表すことを考える．そのこれから現れる記号は前ページで説明しているので，それを見ながら式を追ってほしい．まず，i本目のリンクのユニバーサルジョイント，ボールジョイントの位置は絶対座標系で以下のように表すことができる． $\boldsymbol{q}_{si}$ ， $\boldsymbol{q}_{bi}$ はそれぞれ $\{P\}$ ， $\{B\}$ で書かれたベクトルなので，$\{O\}$ で表すために回転行列をかけている．

(4)\[ \begin{align}\begin{aligned}\boldsymbol{b}_{oi} &= \boldsymbol{t}_{ob} + R_{ob}\boldsymbol{q}_{bi}\\\boldsymbol{p}_{oi} &= \boldsymbol{t}_{ob} + R_{ob}(\boldsymbol{t}+ R\boldsymbol{q}_{si})\end{aligned}\end{align} \]

(4) の両辺を時間微分すると以下のようになる． ( $\dot{\boldsymbol{\theta}}$ の外積をとる理由は機械力学テキストを参照)

(5)\[ \begin{align}\begin{aligned}\dot{\boldsymbol{b}}_{oi} &=\dot{\boldsymbol{t}}_{ob} + \dot{\boldsymbol{\theta}}_{ob}\times R_{ob}\boldsymbol{q}_{bi}\\\dot{\boldsymbol{p}}_{oi} &= \dot{\boldsymbol{t}}_{ob} + \dot{(R_{ob}\boldsymbol{t})}+ \dot{\boldsymbol{\theta}_{op}}\times R_{ob}R\boldsymbol{q}_{si}\end{aligned}\end{align} \]

ここで $\dot{\boldsymbol{\theta}}_{ob}\times R_{ob}\boldsymbol{q}_{bi}$ と $\dot{\boldsymbol{\theta}_{ob}}\times R_{ob}R\boldsymbol{q}_{si}$ は以下の様に変形することができる．

(6)\[\begin{split}\dot{\boldsymbol{\theta}}_{ob}\times R_{ob}\boldsymbol{q}_{bi} &= -R_{ob}\boldsymbol{q}_{si} \times \dot{\boldsymbol{\theta}}_{ob}\\ &=-R_{ob}(R_{ob}Q_{bi})\dot{\boldsymbol{\theta}}_{ob} \\ &=R_{ob}(R_{ob}Q_{bi})^T \dot{\boldsymbol{\theta}}_{ob} \\ &= R_{ob}Q_{bi}^T R_{ob}^T \dot{\boldsymbol{\theta_{ob}}}\\ \\ \dot{\boldsymbol{\theta}_{op}}\times R_{ob}R\boldsymbol{q}_{si} &= (R_{ob}R)Q_{si}^T (R_{ob}R)^T \dot{\boldsymbol{\theta_{op}}}\end{split}\]

(6) を (5) に代入することで，以下の式を得る．

(7)\[\begin{split}\begin{align} \dot{\boldsymbol{b}}_{oi} &= \dot{\boldsymbol{t}}_{ob} + \dot{\boldsymbol{\theta}}_{ob}\times R_{ob}\boldsymbol{q}_{bi} \\ &= \dot{\boldsymbol{t}}_{ob} +R_{ob}Q_{bi}^T R_{ob}^T \dot{\boldsymbol{\theta_{ob}}} \\ &= [ O_{3×3} ， O_{3×3 }， I_{3×3} ， R_{ob}Q_{bi}^T R_{ob}^T] \cdot \begin{pmatrix} \dot{\boldsymbol{t}_{op}} \\ \dot{\boldsymbol{\theta}_{op}} \\ \dot{\boldsymbol{t}_{ob}} \\ \dot{\boldsymbol{\theta}_{ob}} \end{pmatrix} \\ &= [ O_{3×3} ， O_{3×3} ， I_{3×3} ， R_{ob}Q_{bi}^T R_{ob}^T] \cdot \dot{\boldsymbol{q}} \\ \\ \dot{\boldsymbol{p}}_{oi} &= \dot{\boldsymbol{t}}_{ob} + \dot{(R_{ob}\boldsymbol{t})}+ \dot{\boldsymbol{\theta}_{op}}\times R_{ob}R\boldsymbol{q}_{si} \\ &= \dot{\boldsymbol{t}}_{ob}+ \dot{(R_{ob}\boldsymbol{t})}+(R_{ob}R)Q_{si}^T (R_{ob}R)^T \dot{\boldsymbol{\theta_{op}}} \\ &= [ I_{3×3} ， (R_{ob}R)Q_{bi}^T (R_{ob}R)^T，O_{3×3} ，O_{3×3}] \cdot \begin{pmatrix} \dot{\boldsymbol{t}_{ob}}+\dot{(R_{ob}\boldsymbol{t})} \\ \dot{\boldsymbol{\theta}_{op}} \\ \dot{\boldsymbol{t}_{ob}} \\ \dot{\boldsymbol{\theta}_{ob}} \end{pmatrix} \\ &= [ I_{3×3} ， R_{ob}RQ_{bi}^T (R_{ob}R)^T，O_{3×3} ，O_{3×3}] \cdot \dot{\boldsymbol{q}} \end{align}\end{split}\]

ここで， $I_{3×3}$ ， $O_{3×3}$ はそれぞれ3×3の単位行列，ゼロ行列である． (7) 式より，i番目のリンクの回転角速度ベクトル $\boldsymbol{\omega_{li}}$ は絶対座標系で以下のようになる．

(8)\[\begin{split}\begin{equation} \begin{split} \boldsymbol{\omega_{li}}&= \frac{\boldsymbol{n_{i}} \times (\dot{\boldsymbol{p}_{oi}} - \dot{\boldsymbol{b}_{oi}})}{ρ_{i}}=\frac{N_{i}}{ρ_{i}}[I_{3×3}， (R_{ob}R)Q_{bi}^T (R_{ob}R)^T， -I_{3×3}， -R_{ob}Q_{bi}^T R_{ob}^T] \dot{\boldsymbol{q}}\\ &\equiv \frac{N_{i}}{ρ_{i}}M_{1i}\dot{\boldsymbol{q}} \end{split} \end{equation} \\\end{split}\]

したがって，i番目のリンクの速度ベクトル $\boldsymbol{v_{i}}$ は絶対座標系で以下のように書くことができる．

(9)\[\begin{split}\begin{equation} \begin{split} \boldsymbol{v_{i}}&=\dot{\boldsymbol{p}_{oi}} + \boldsymbol{\omega_{li}} \times (-l_{t}\boldsymbol{n_{i}}) =\dot{\boldsymbol{p}_{oi}} + \frac{(\boldsymbol{n_{i}} \times (\dot{\boldsymbol{p}_{oi}} - \dot{\boldsymbol{b}_{oi}})) \times (-l_{t}\boldsymbol{n_{i}})}{\rho_{i}}\\ &=\dot{\boldsymbol{p}_{oi}}+\frac{l_{t}}{\rho_{i}}N_{i}^2(\dot{\boldsymbol{p}_{oi}} - \dot{\boldsymbol{b}_{oi}})\\ &=\{[I_{3×3} ， (R_{ob}R)Q_{bi}^T (R_{ob}R)^T，O_{3×3} ，O_{3×3}]+ \\ & \qquad \frac{l_{t}}{\rho_{i}}N_{i}^2[I_{3×3}， (R_{ob}R)Q_{bi}^T (R_{ob}R)^T， -I_{3×3}， -R_{ob}Q_{bi}^T R_{ob}^T]\}\cdot \dot{\boldsymbol{q}}\\ &\equiv M_{2i}\dot{\boldsymbol{q}} \end{split} \end{equation}\end{split}\]

これで，リンクの速度ベクトル $\boldsymbol{v_{i}}$ を $\dot{\boldsymbol{q}}$ で表すことができた．次に，この結果を利用して質量マトリクス $M$ を導出する．

2.1.4. 質量マトリクスの導出

次に，質量マトリクス $M$ を導出しよう．BaseやPlatformだけではなく，各リンクの質量を考慮しないといけないためその表式は複雑となる．ここで，工作機械全体の運動エネルギー $T_{total}$ を考える． $T$ と $M$ ，そして $\boldsymbol{q}$ には以下の関係式がある．

(10)\[\begin{equation} T_{total}=\frac{\boldsymbol{q}^T M\boldsymbol{q}}{2} \end{equation}\]

したがって，質量マトリクスは以下のように表すことができる．

(11)\[\begin{equation} M=\dfrac{\partial^2 T_{total}}{\partial \boldsymbol{q}^2} \end{equation}\]

よって，質量マトリクスを求めるためには工作機械全体の運動エネルギー $T$ を求めればよい，ここで，PKM加工機の運動要素はBase，Platform，リンクの3つであるから，Tは以下のように表すことができる．

(12)\[\begin{equation} T_{total}= T_{leg} + T_{b} +T_{p} \end{equation}\]

ここで， $T_{leg}$ ， $T_{b}，T_{p}$ はそれぞれ6本のリンク，Base，Platformの運動エネルギーである．$これらをひとつづつ求めていこう．まず，リンクの運動エネルギーを求める．並進と回転の運動エネルギーを考えることで， $T_{leg}$ は以下のように表すことができる．

(13)\[\begin{equation} T_{leg}=\frac{1}{2}\sum_{i=1}^{6} (\boldsymbol{v_{i}}^T m_{l} \boldsymbol{v_{i}} +\boldsymbol{\omega_{li}}^T I_{l} \boldsymbol{\omega_{li}}) \end{equation}\]

ここで，リンクの速度ベクトル $\boldsymbol{v}_{i}$ と回転角速度ベクトル $\boldsymbol{\omega}_{li}$ は式 (9) ， (8) で示した通りであるから，これを式 (13) に代入すると以下の式を得る

(14)\[\begin{split}\begin{equation} \begin{split} T_{leg}&=\frac{1}{2}\sum_{i=1}^{6} ( \boldsymbol{q}^T M_{2i}^T m_{l} M_{2i} \boldsymbol{q} + \boldsymbol{q}^T M_{1i}^T I_{l} M_{1i} \boldsymbol{q})\\ &=\frac{1}{2}\sum_{i=1}^{6} \boldsymbol{q}^T (M_{2i}^T m_{l} M_{2i} + M_{1i}^T I_{l} M_{1i} ) \boldsymbol{q} \end{split} \end{equation}\end{split}\]

つぎに $T_{p}$ を求める．これも並進と回転を考えることで以下のように書くことができる．

(15)\[\begin{split}\begin{equation} \begin{split} T_{p} & = \frac{1}{2} \dot{\boldsymbol{t}_{op}}^T m_{p} \dot{\boldsymbol{t}_{op}} + \frac{1}{2} \dot{\boldsymbol{\theta}_{op}}^T I_{l} \dot{\boldsymbol{\theta}_{op}} \\ &= \frac{1}{2} \dot{\boldsymbol{q}}^T \begin{bmatrix} m_{p} & & & & & \\ & m_{p} & & & \huge{0} & \\ & & m_{p} & & & \\ & & & 0 & & \\ & \huge{0} & & & \ddots & \\ & & & & & 0 \end{bmatrix} \boldsymbol{q} + \frac{1}{2} \dot{\boldsymbol{q}}^T \begin{bmatrix} 0 & & & & & \\ & 0 & & & \huge{0} & \\ & & 0 & & & \\ & & & [I_{p}] & & \\ & \huge{0} & & & \ddots & \\ & & & & & 0 \end{bmatrix} \dot{\boldsymbol{q}} \\ &= \frac{1}{2} \dot{\boldsymbol{q}}^T \begin{bmatrix} m_{p} & & & & & \\ & m_{p} & & & \huge{0} & \\ & & m_{p} & & & \\ & & & [I_{p}] & & \\ & \huge{0} & & & \ddots & \\ & & & & & 0 \end{bmatrix} \dot{\boldsymbol{q}} \end{split} \end{equation}\end{split}\]

最後に，$T_{b}$ を求める．これは $T_{p}$ と同様に考えることで以下のようになる．

(16)\[\begin{split}\begin{equation} T_{b} = \frac{1}{2} \dot{\boldsymbol{q}}^T \begin{bmatrix} 0 & & & & & \\ & \ddots & & & \huge{0} & \\ & & m_{b} & & & \\ & & & m_{b} & & \\ & \huge{0} & & & m_{b} & \\ & & & & & [I_{b}] \end{bmatrix} \dot{\boldsymbol{q}} \end{equation}\end{split}\]

式 (14) ， (15) ， (16) より，質量マトリクスは次のように計算できる．

(17)\[\begin{split}\begin{equation} \begin{split} M & = \dfrac{\partial^2 (T_{leg}+T_{b}+T_{p})}{\partial \boldsymbol{q}^2} \\ &= \sum_{i=1}^{6} (M_{2i}^T m_{l} M_{2i} + M_{1i}^T I_{l} M_{1i} ) + \begin{bmatrix} m_{p} & & & & & & & \\ & m_{p}& & & & & & \\ & & m_{p}& & & \huge{\huge{0}} & & \\ & & & [I_{p}] & & & & \\ & & & & m_{b} & & & \\& & \huge{\huge{0}} & & & m_{b} & & \\& & & & & & m_{b} & \\& & & & & & & [I_{b}] \end{bmatrix} \end{split} \end{equation}\end{split}\]

2.1.5. 剛性マトリクスの導出

次に，剛性マトリクスを導出する．剛性マトリクス $K$ とは，静的加重 $\boldsymbol{F}$ をかけた際にPKM加工機の位置ベクトル $\boldsymbol{q}$ との間に以下の関係式が成り立つ行列である．

(18)\[\begin{equation} \boldsymbol{F}=K\boldsymbol{q} \end{equation}\]

今， Baseの剛性および各リンクの剛性は分かっているものとする．すなわち， Baseの位置ベクトル $\boldsymbol{q}_{ob}= [\boldsymbol{t}_{ob}， \boldsymbol{\theta}_{ob}]^T$ とBaseにかかる静加重 $\boldsymbol{F}_{b}$ および各リンクの長さをまとめたベクトル $\boldsymbol{\rho}=[\rho_{1}，\rho_{2}， \ldots， \rho_{6}]^T$ と各リンクに軸方向にかかる静加重 $\boldsymbol{F}_{l}$ には，以下の関係があるとする．

(19)\[\begin{split}\begin{equation} \begin{split} \boldsymbol{F}_{b} &= \begin{bmatrix} k_{bx} & & & & & \\ & k_{by}& & & \huge{0} & \\ & &k_{bz} & & & \\ & & &k_{b\theta x} & & \\ & \huge{0} & & &k_{b\theta y} & \\ & & & & &k_{b\theta z} \end{bmatrix} \boldsymbol{q}_{ob} \\ & \equiv K_{b} \boldsymbol{q}_{ob} \\ \\ \boldsymbol{F}_{l} &=\begin{bmatrix} k_{1} & & & & & \\ & k_{2}& & & \huge{0} & \\ & &k_{3} & & & \\ & & &k_{4} & & \\ & \huge{0} & & & k_{5} & \\ & & & & &k_{6} \end{bmatrix} \boldsymbol{\rho} \\ & \equiv K_{l} \boldsymbol{\rho} \end{split} \end{equation}\end{split}\]

各リンクの長さと，Baseの位置をまとめたベクトルを $\hat{\boldsymbol{q}}=[\boldsymbol{\rho}$ ， $\boldsymbol{q}_{ob}]^T$ として，式 (19) をまとめると以下の様になる．

(20)\[\begin{split}\begin{equation} \begin{split} \begin{bmatrix} \boldsymbol{F}_{l} \\ \boldsymbol{F}_{b} \end{bmatrix} &= \begin{bmatrix} K_{l} \ \ \ O_{6\times 6} \\ O_{6\times 6} \ \ \ K_{b} \end{bmatrix} \begin{bmatrix} \boldsymbol{\rho} \\ \boldsymbol{q}_{ob} \end{bmatrix} \\ & \equiv \hat{K} \hat{\boldsymbol{q}} \end{split} \end{equation}\end{split}\]

ここで， $K$ と $\hat{K}$ の関係を考えてみる，まず，$\boldsymbol{q}$ と $\hat{\boldsymbol{q}}$ の関係はヤコビ行列Jを使って $\dot{\hat{\boldsymbol{q}}}=J\dot{\boldsymbol{q}}$ と表されるとする．両辺の微小変化を考えると $\delta \hat{\boldsymbol{q}}=J \delta \boldsymbol{q}$ である．さらに， $\boldsymbol{q}→\boldsymbol{q}+\delta \boldsymbol{q}$ ， $\hat{\boldsymbol{q}}→\hat{\boldsymbol{q}}+\delta \hat{\boldsymbol{q}}$ となった際の仕事 $\delta W$ ， $\delta \hat{W}$ を考えると以下のように書くことができる．

(21)\[\begin{split}\delta W &= (K \delta \boldsymbol{q})^T \delta \boldsymbol{q} = \delta \boldsymbol{q}^T K^T \delta \boldsymbol{q} \\ \\ \delta \hat{W} &= (\hat{K} \delta \hat{\boldsymbol{q}})^T \delta \hat{\boldsymbol{q}} =\delta \hat{\boldsymbol{q}}^T \hat{K}^T \delta \hat{\boldsymbol{q}} \\ &= \delta \boldsymbol{q}^T J^T \hat{K}^T J \delta \boldsymbol{q}\end{split}\]

式 (21) の2式における右辺の値は仮想仕事の原理より一致するため，$K$ と $\hat{K}$ の関係は以下のようになる．

(22)\[\begin{equation} K = J^T \hat{K} J \end{equation}\]

したがって，ヤコビ行列 $J$ を求めることができれば剛性マトリクス $K$ を導出することができる．いま， $i$ 本目のリンクの長さ $\rho_{i}$ は以下の式で表される．

(23)\[\begin{equation} \rho_{i}^2=(\boldsymbol{p}_{oi} - \boldsymbol{b}_{oi})^T (\boldsymbol{p}_{oi} - \boldsymbol{b}_{oi}) \end{equation}\]

両辺を時間微分することで，以下の式を得る．

(24)\[\begin{equation} 2\rho_{i} \dot{\rho_{i}} = 2(\boldsymbol{p}_{oi} - \boldsymbol{b}_{oi})^T (\dot{\boldsymbol{p}_{oi}} - \dot{\boldsymbol{b}_{oi}}) \end{equation}\]

ここで， $\dot{\boldsymbol{p}_{oi}}$ ， $\dot{\boldsymbol{b}_{oi}}$ は以下の式で表される．

(25)\[\begin{split}\dot{\boldsymbol{p}_{oi}} &= \dot{(\boldsymbol{t}_{op} + R_{ob}R\boldsymbol{q}_{si})}=\dot{\boldsymbol{t}_{op}} + \dot{(R_{ob}R)}\boldsymbol{q}_{si} \\ &=\dot{\boldsymbol{t}_{op}} + \hat{\dot{\theta_{op}}}R_{ob}R\boldsymbol{q}_{si} \\ \\ \dot{\boldsymbol{b}_{oi}} &= \dot{(\boldsymbol{t}_{ob} + R_{ob}\boldsymbol{q}_{bi})}=\dot{\boldsymbol{t}_{ob}} + \hat{\dot{\theta_{ob}}} R_{ob} \boldsymbol{q}_{bi}\end{split}\]

式 (25) を式 (24) に代入することにより，以下の式を得ることができる．

(26)\[\begin{split}\begin{equation} \begin{split} \rho_{i} \dot{\rho}_{i} &= (\boldsymbol{p}_{oi} - \boldsymbol{b}_{oi})^T ( \dot{\boldsymbol{t}_{op}} - \dot{\boldsymbol{t}_{ob}}+ \hat{\dot{\theta_{op}}}R_{ob}R\boldsymbol{q}_{si} - \hat{\dot{\theta_{ob}}} R_{ob} \boldsymbol{q}_{bi})\\ &= (\boldsymbol{p}_{oi} - \boldsymbol{b}_{oi})^T \dot{\boldsymbol{t}_{op}} + (R_{ob}R\boldsymbol{q}_{si}) \times (\boldsymbol{p}_{oi} - \boldsymbol{b}_{oi}) \dot(\boldsymbol{\theta}_{op} - (\boldsymbol{p}_{oi} - \boldsymbol{b}_{oi})^T \dot{\boldsymbol{t}_{ob}} + (R_{ob}\boldsymbol{q}_{bi}) \times (\boldsymbol{p}_{oi} - \boldsymbol{b}_{oi}) \dot{\boldsymbol{\theta}_{ob}} \\ &= [ (\boldsymbol{p}_{oi} - \boldsymbol{b}_{oi})^T， (R_{ob}R\boldsymbol{q}_{si}) \times (\boldsymbol{p}_{oi} - \boldsymbol{b}_{oi})， - (\boldsymbol{p}_{oi} - \boldsymbol{b}_{oi})^T， (R_{ob}\boldsymbol{q}_{bi}) \times (\boldsymbol{p}_{oi} - \boldsymbol{b}_{oi})] \begin{bmatrix} \dot{\boldsymbol{t}_{op}} \\ \dot{\boldsymbol{\theta}_{op}} \\ \dot{\boldsymbol{t}_{ob}} \\ \dot{\boldsymbol{\theta}_{ob}} \end{bmatrix} \\ & \equiv m_{i}^T \dot{\boldsymbol{q}} \end{split} \end{equation}\end{split}\]

したがって， $i=1～6$ について式 (26) をまとめて書くと以下の様になる．

(27)\[\begin{split}\begin{equation} \begin{bmatrix} \rho_{1} & & \\ & \ddots & \\ & & \rho_{6} \end{bmatrix} \dot{\boldsymbol{\rho}} = \begin{bmatrix} m_{1}^T \\ \vdots \\ m_{6}^T \end{bmatrix} \dot{\boldsymbol{q}} \end{equation}\end{split}\]

以上の結果より，$\boldsymbol{q}$ と $\hat{\boldsymbol{q}}$ の関係$は以下のようになる．

(28)\[\begin{split}\begin{equation} \begin{split} \hat{\boldsymbol{q}} &= \begin{bmatrix} \dot{\boldsymbol{\rho}} \\ \dot{\boldsymbol{q}_{ob}} \end{bmatrix} = \begin{bmatrix} \begin{bmatrix} \rho_{1} & & \\ & \ddots & \\ & & \rho_{6} \end{bmatrix}^{-1} & \begin{bmatrix} m_{1}^T \\ \vdots \\ m_{6}^T \end{bmatrix} \\ \\ O_{6\times6} & I_{6\times6} \end{bmatrix} \dot{\boldsymbol{q}}\\ &= J \dot{\boldsymbol{q}} \end{split} \end{equation}\end{split}\]

これにより，ヤコビ行列 $J$ を求めることができた．これを式 (22) に代入して剛性マトリクス $K$ を得ることができる．

2.1.6. 粘性マトリクスの導出

粘性マトリクス $C$ は $K$ に比例するものと考える．比例定数を $\lambda$ とし，式 (22) に注意すると $C$ は以下のように書くことができる．

(29)\[\begin{split}\begin{equation} C= J^T \begin{bmatrix} \lambda k_{1} & & 0 \\ & \ddots & \\ 0 & & \lambda k_{b\theta z} \end{bmatrix}J \end{equation}\end{split}\]

これにより，$M$ ， $K$ ， $C$ の全てを求めることができた．ここで，はじめにで示したパラレル加工機の運動方程式に戻る．両辺をラプラス変換すると．以下の式を得る

(30)\[\begin{equation} (s^2 M + s C + K) Q(s) = F(s) \end{equation}\]

ここで， $\frac{Q(s)}{F(s)}$ をコンプライアンスと呼ぶ．コンプライアンスは以下の式で表される．

(31)\[\begin{equation} <Receptance> = \frac{Q(s)}{F(s)} = (s^2 M + s C +K )^{-1} \end{equation}\]

この値を求めることで，PKM加工機の静剛性，動剛性を考察することができる．

2.1.7. References

1. Zhang, Parallel Robotic Machine Tools , vol. 53, no. 9.
1. Ma et al., “Parametric vibration analysis and validation for a novel portable hexapod machine tool attached to surfaces with unequal stiffness,” J. Manuf. Process., vol.47, no. September, pp. 192 201, 2019, doi: 10.1016/j.jmapro.2019.10.003.
1. 1. Guo and H. R. Li, “Dynamic analysis and simulation of a six degree of freedom Stewart platform manipulator,” Proc. Inst. Mech. Eng. Part C, vol. 220, no. 1, pp. 61 72, 2006, doi: 10.1243/095440605X32075.
1. Pedrammehr, M. Mahboubkhah, and N. Kha ni, “Improved dynamic equations for the generally configured Stewart platform manipulator,” J. Mech. Sci. Technol., vol. 26, no. 3, pp. 711 721, 2012, doi: 10.1007/s12206 011 1231 0.

2.1.8. 補足

2.1.8.1. 固定角の微分

2.1.8.1.1. ZYX固定角における座標変換行列 $R_{ZYX}$

今回は，剛体の姿勢を与えるベクトルとして，ZYX固定角 ( $\vb*{\theta}\equiv\bmqty{\theta_x \theta_y \theta_z}^T$ )を考える．このときの座標変換行列( $\vb*{R}_{ZYX}$ )は，以下のとおりである．

(32)\[\begin{split}\begin{align} \vb*{R}_{ZYX}(\vb*\theta) & = R_X R_Y R_Z \\ & =\bmqty { 1 & 0 & 0 \\ 0 & \cos\theta_x & -\sin\theta_x \\ 0 & \sin\theta_x & \cos\theta_x }\bmqty{ \cos\theta_y & 0 & \sin\theta_y \\ 0 & 1 & 0 \\ -\sin\theta_y & 0 & \cos\theta_y }\bmqty{ \cos\theta_z & -\sin\theta_z & 0 \\ \sin\theta_z & \cos\theta_z & 0 \\ 0 & 0 & 1 } \\ & = \bmqty { \cos\theta_y \cos\theta_z & -\cos\theta_y \sin\theta_z & \sin\theta_y \\ \cos\theta_x\sin\theta_z+\sin\theta_x\sin\theta_y\cos\theta_z & \cos\theta_x\cos\theta_z-\sin\theta_x\sin\theta_y\sin\theta_z & -\sin\theta_x\cos\theta_y \\ \sin\theta_x\sin\theta_z-\cos\theta_x\sin\theta_y\cos\theta_z & \cos\theta_x\sin\theta_y\sin\theta_z+\sin\theta_x\cos\theta_z & \cos\theta_x\cos\theta_y }\label{equ:Rxyz} \end{align}\end{split}\]

Note

各軸周りの座標変換行列

$x$ 軸を中心とした回転行列 $R_x$ ， $y$ 軸を中心とした回転行列 $R_y$ ， $z$ 軸を中心とした回転行列 $R_z$ はそれぞれ次式で表される．

(33)\[\begin{split}\begin{align} R_x(\theta_x) & = \bmqty { 1 & 0 & 0 \\ 0 & \cos\theta_x & -\sin\theta_x \\ 0 & \sin\theta_x & \cos\theta_x } \label{equ:Rx} \\ R_y(\theta_y) & = \bmqty { \cos\theta_y & 0 & \sin\theta_y \\ 0 & 1 & 0 \\ -\sin\theta_y & 0 & \cos\theta_y } \label{equ:Ry} \\ R_z(\theta_z) & = \bmqty { \cos\theta_z & -\sin\theta_z & 0 \\ \sin\theta_z & \cos\theta_z & 0 \\ 0 & 0 & 1 } \label{equ:Rz} \end{align}\end{split}\]

2.1.8.1.2. 回転行列の時間微分と角速度ベクトル

一般に，座標変換行列 $\vb*{R}$ は直交行列であるから，

(34)\[\begin{align} \vb*R\vb*R^T=\vb*E\qquad(\vb*E\text{は単位行列}) \end{align}\]

が成立する．両辺を時間で微分することで，次式が得られる．

(35)\[\begin{equation} \dot{\vb*R}\vb*R^T + \vb*R\dot{\vb*R}^T =\vb*{O} \label{equ:RRtdot} \end{equation}\]

ここで， $\vb*\Omega\equiv\dot{\vb*R}\vb*R^T$ と定義すれば，上式は $\vb*\Omega+\vb*\Omega^T=\vb*{O}$ と表され， $\vb*\Omega$ は歪対称行列（交代行列，skew-symmetric matrix, alternating matrix）であることが確かめられる．ただし，歪対称行列とは， $A^T=-A$ を満たす正方行列のことである．歪対称行列 $\vb*\Omega$ は，3つの要素 $\omega_x, \omega_y, \omega_z$ からなり，

(36)\[\begin{split}\begin{align} \vb*\Omega = \bmqty { 0 & -\omega_z & \omega_y \\ \omega_z & 0 & -\omega_x \\ -\omega_y & \omega_x & 0 \\ } \end{align}\end{split}\]

と書き下すことができる．これらの3つの要素をまとめたベクトルを $\vb*\omega\equiv\bmqty{\omega_x & \omega_y & \omega_z}^T$ と定義すれば，左から行列 $\vb*\Omega$ を乗じることは，ベクトル $\vb*\omega$ の外積を取ることと等しい．

式 (35) と $\vb*\Omega$ の定義より，

(37)\[\begin{split}\begin{align} \dot{\vb*R} & = -\vb*R\dot{\vb*R}^T\vb*R = -{(\underbrace{\dot{\vb*R}\vb*R^T}_{\vb*\Omega})}^T\vb*R \notag \\ & = \vb*\Omega\vb*R = \vb*\omega\cross \vb*R \end{align}\end{split}\]

を得る．

2.1.8.1.3. ZYX固定角での角速度ベクトル $\vb*\omega$ の具体的な計算

前節で定義した角速度ベクトル(vb*omega )を，式 (32) の $\vb*R_{ZYX}$ の場合について求める．はじめに， $\dot{\vb*R}_{ZYX}$ を計算する．

(38)\[\begin{split}\begin{equation} \dot{\vb*R}_{ZYX} = \bmqty{ \dot{R_{11}} & \dot{R_{12}} & \dot{R_{13}} \\ \dot{R_{21}} & \dot{R_{22}} & \dot{R_{23}} \\ \dot{R_{31}} & \dot{R_{32}} & \dot{R_{33}} } \end{equation}\end{split}\]

各成分については地道に計算を行う．

(39)\[\begin{split}\begin{align} \dot{R_{11}} & = -\dot{\theta}_{z}\cos\theta_y\sin\theta_z - \dot{\theta}_{y}\sin\theta_y\cos\theta_z \\ \dot{R_{12}} & = \dot{\theta}_{y}\sin\theta_y\sin\theta_z - \dot{\theta}_{z}\cos\theta_y\cos\theta_z \\ \dot{R_{13}} & = \dot{\theta}_{y}\cos\theta_y \\ \dot{R_{21}} & = -\dot{\theta}_{z}\sin\theta_x\sin\theta_y\sin\theta_z + \dot{\theta}_{z}\cos\theta_x\cos\theta_z \notag \\ & \phantom{=} -\dot{\theta}_{x}\sin\theta_x\sin\theta_z + \dot{\theta}_{y}\sin\theta_x\cos\theta_y\cos\theta_z + \dot{\theta}_{x}\cos\theta_x\sin\theta_y\cos\theta_z \\ \dot{R_{22}} & = -\dot{\theta}_{z}\cos\theta_x\sin\theta_z - \dot{\theta}_{z}\sin\theta_x\sin\theta_y\cos\theta_z \notag \\ & \phantom{=} -\dot{\theta}_{y}\sin\theta_x \cos\theta_y\sin\theta_z - \dot{\theta}_{x}\cos\theta_x\sin\theta_y\sin\theta_z - \dot{\theta}_{x}\sin\theta_x\cos\theta_z \\ \dot{R_{23}} & = \dot{\theta}_{y}\sin\theta_x\sin\theta_y - \dot{\theta}_{x}\cos\theta_x\cos\theta_y \notag \\ \dot{R_{31}} & = \dot{\theta}_{z}\cos\theta_x\sin\theta_y\sin\theta_z + \dot{\theta}_{z}\sin\theta_x\cos\theta_z \\ & \phantom{=} + \dot{\theta}_{x}\cos\theta_x\sin\theta_z - \dot{\theta}_{y}\cos\theta_x\cos\theta_y\cos\theta_z + \dot{\theta}_{x}\sin\theta_x\sin\theta_y\cos\theta_z \\ \dot{R_{32}} & = -\dot{\theta}_{z}\sin\theta_x\sin\theta_z + \dot{\theta}_{z}\cos\theta_x\sin\theta_y\cos\theta_z \notag \\ & \phantom{=} + \dot{\theta}_{y}\cos\theta_x\cos\theta_y\sin\theta_z - \dot{\theta}_{x}\sin\theta_x\sin\theta_y\sin\theta_z + \dot{\theta}_{x}\cos\theta_x \cos\theta_z \\ \dot{R_{33}} & = - \dot{\theta}_{y}\cos\theta_x\sin\theta_y - \dot{\theta}_{x}\sin\theta_x\cos\theta_y \end{align}\end{split}\]

次に， $\dot{\vb*R}_{ZYX}$ に対して，右から $\vb*R^T$ を乗ずることで， $\vb*\Omega_{ZYX}\equiv\dot{\vb*R}_{ZYX}{\vb*{R}_{ZYX}}^T$ を求める．

(40)\[\begin{split}\begin{align} \vb*\Omega_{ZYX} & \equiv \dot{\vb*R}_{ZYX}{\vb*{R}_{ZYX}}^T \\ & =\begin{bmatrix} 0 & -\dot{\theta}_{z}\cos\theta_x\cos\theta_y -\dot{\theta}_{y}\sin\theta_x & \dot{\theta}_{y}\cos\theta_x -\dot{\theta}_{z}\sin\theta_x\cos\theta_y \\ \dot{\theta}_{z}\cos\theta_x\cos\theta_y +\dot{\theta}_{y}\sin\theta_x & 0 & -\dot{\theta}_{z}\sin\theta_y - \dot{\theta}_{x} \\ \dot{\theta}_{z}\sin\theta_x\cos\theta_y -\dot{\theta}_{y}\cos\theta_x & \dot{\theta}_{z}\sin\theta_y + \dot{\theta}_{x} & 0 \end{bmatrix} \end{align}\end{split}\]

したがって，

(41)\[\begin{split}\begin{align} \vb*\omega & = \bmqty { \dot{\theta}_{z}\sin\theta_y + \dot{\theta}_{x} \\ \dot{\theta}_{y}\cos\theta_x - \dot{\theta}_{z}\sin\theta_x\cos\theta_y \\ \dot{\theta}_{z}\cos\theta_x\cos\theta_y + \\dot{\theta}_{y}\sin\theta_x } \\ & = \underbrace{ \bmqty { 1 & 0 & \sin\theta_y \\ 0 & \cos\theta_x & -\sin\theta_x\cos\theta_y \\ 0 & \sin\theta_x & \cos\theta_x\cos\theta_y \\ }}_{\text{固定角の微分と角速度ベクトルの変換行列 $\vb*\Psi $}} \bmqty { \dot{\theta}_{x} \\ \dot{\theta}_{y} \\ \dot{\theta}_{z} } \\ & = \vb*\Psi\dot{\vb*\theta} \end{align}\end{split}\]