制御

用語定義 †

  • 注意点
    • ’’直感のために基本的にはCraig先生の表記を採用する’’
    • Craig先生と中村先生とで，座標系定義が異なるため，動力学計算の式が異なる
    • 具体的には逆動力学，Craig 5.43とロボットモーション3.3.67あたりで異なる
  • 中村先生とCraig先生の比較
Craig先生中村先生
ジョイント回転軸・直動軸の両方の立式回転軸のみに特化した立式
立式ベースリンクを介さずに漸化式を導いているほぼ全ての立式がベースリンクを介す（本には書いていないが運動学動力学統合化計算の要請）
座標系定義ジョイントiのz軸に$q_{i}$の回転ジョイントが乗っている．関節iの動きによって{i}が動くジョイントiのz軸に$q_{i-1}$の回転ジョイントが乗っている．関節iの動きによって{i}が動く
  • 用語定義
Craig中村意味
{i}{i}座標系i
$\theta_i$$\theta_i$軸iの一般化座標
$^{A}_{B}R$$^{A}_{B}R${A}から見た，{B}への回転行列
$^{A}X_{B}${A}から見た，{B}の何らか属性であるベクトルX
$^{A}P_{B}$$^{A}_{B}P${A}から見た，{A}から{B}への原点への並進ベクトル
$^{A}\hat{Z}_{B}${A}から見た，{B}のZ軸．A=Bの時，(0, 0, 1)^t
$^{A}f_{i}, ^{A}n_{i}${A}から見た，リンク{i-1}がリンク{i}に与える力とトルク
{C_i}リンクiの重心を原点として{i}と同じ姿勢の座標系
↑

ロボティクス計算の種類 †

順(Forward)逆(Inverse)
運動学(Kinematics)$q \to ^{0}_{i}R, ^{0}_{i}P $
微分運動学(Differential Kinematics)$\dot{q} \to ^{0}\omega_{i}, ^{0}\dot{P}_{i} $$^{0}\omega_{i}, ^{0}\dot{P}_{i} \to \dot{q} $
動力学(Dynamics)$q, \dot{q}, \ddot{q} \to \ddot{q}$$q, \dot{q}, \ddot{q} \to \tau$
↑

基本定理 †

  • 座標変換の基本定理
  • 変数定義
$Q$時変する点Q．{B}でgiven
$^{A}V_{Q} = ^{A} \dot{Q}${A}から見た点Qの速度．なぜVQと表現するかというと，逆動力学でvを前向きニュートンオイラーで求めるから
$^{A}\dot{V}_{Q} = ^{A} \ddot{Q}${A}から見た点Qの加速度
$^{A}\Omega_{B}${A}から見た．{B}の角速度
  • 定理
位置$^{B}Q = ^{B}_{A}R ^{A}Q$
姿勢$^{A}_{C}R = ^{A}_{B}R ^{B}_{C}R$
速度$^{A}V_{Q} = ^{A}V_{BORG} + ^{A}_{B}R ^{B}V_{Q} + ^{A}\Omega_{B} \times ^{A}_{B}R ^{B}Q$(5.13)
角速度$^{A}\Omega_{C} = ^{A}\Omega_{B}+ ^{A}_{B}R ^{B}\Omega_{C}$
加速度$^{A}\dot{V}_{Q} = ^{A}\dot{V}_{BORG} + ^{A}_{B}R ^{B}\dot{V}_{Q} + 2 ^{A}\Omega_{B} \times ^{A}_{B}R ^{B}V_{Q} + ^{A}\dot{\Omega}_{B} \times ^{A}_{B}R ^{B}Q + ^{A}\Omega_{B} \times (^{A}\Omega_{B} \times ^{A}_{B}R ^{B}Q)$(6.10)
角加速度$^{A}\dot{\Omega}_{C} = ^{A}\dot{\Omega}_{B} + ^{A}_{B}R ^{B} \dot{\Omega}_{C} + ^{A}\Omega_{B} \times ^{A}_{B}R ^{B}\Omega_{C}$(6.15)
↑

逆運動学 †

リンクが枝分かれしている場合はどうするの？

  • 座標系伝播
回転軸直動軸
一般化座標$q_i = \theta_i$$q_i = d_i$
一般化力$\tau_i$$\tau_i$
速度$^{i+1}v_{i+1} = ^{i+1}_{i}R (^{i}v_{i} + ^{i}\omega_{i} \times ^{i}P_{i+1})$ (5.47)$^{i+1}_{i}R (^{i}v_{i} + ^{i}\omega_{i} \times ^{i}P_{i+1}) + \dot{d_{i+1}} ^{i+1}\hat{Z}_{i+1}$ (5.48)
角速度$^{i+1}\omega_{i+1}=^{i+1}_{i}R ^{i}\omega_{i} + \dot{\theta} ^{i+1}\hat{Z}_{i+1}$ (5.45)$^{i+1}\omega_{i+1}=^{i+1}_{i}R ^{i}\omega_{i}$ (5.48)
加速度$^{i+1}\dot{v}_{i+1} = ^{i+1}_{i}R (^{i}\dot{\omega}_{i} \times ^{i}P_{i+1} + ^{i}\omega_{i} \times (^{i}\omega_{i} \times ^{i}P_{i+1}) + ^{i}\dot{v}_{i})$$^{i+1}\dot{v+1}_{i} = ^{i+1}_{i}R (^{i}\dot{\omega}_{i} \times ^{i}P_{i+1} + ^{i}\omega_{i} \times (^{i}\omega_{i} \times ^{i}P_{i+1}) + ^{i}\dot{v}_{i}) + 2 ^{i+1}\omega_{i+1} \times \dot{d}_{i+1} \, ^{i+1}\hat{Z}_{i+1} + \ddot{d}_{i+1} \, ^{i+1}\hat{Z}_{i+1}$
角加速度$^{i+1}\dot{\omega}_{i+1} = ^{i+1}_{i}R ^{i}\dot{\omega}_{i} + ^{i+1}_{i}R ^{i}\omega_{i} \times \dot{\theta_{i+1}} ^{i+1}\hat{Z}_{i+1} + \ddot{\theta_{i+1}} ^{i+1}\hat{Z}_{i+1}$(6.32)$^{i+1}\dot{\omega}_{i+1} = ^{i+1}_{i}R ^{i}\dot{\omega}_{i}$(6.33)
重心加速度（直動・回転のみ成立する式）$^{i}\dot{v_{Ci}} = ^{i}\omega_{i} \times (^{i}\omega_{i} \times ^{i}P_{C_i}) + ^{i}\dot{v}_{i}$$^{i}\dot{v_{Ci}} = ^{i}\omega_{i} \times (^{i}\omega_{i} \times ^{i}P_{C_i}) + ^{i}\dot{v}_{i}$
合力$^{i+1}F_{i+1} = m_{i+i} ^{i+1}\dot{v}_{C_{i+1}}$$^{i+1}F_{i+1} = m_{i+i} ^{i+1}\dot{v}_{C_{i+1}}$
合モーメント$^{i+1}N_{i+1} = ^{C_{i+1}}I_{i+1} ^{i+1}\dot{\omega}_{i+1} + ^{i+1}\omega_{i+1} \times ^{C_{i+1}}I_{i+1} ^{i+1}\omega_{i+1}$$^{i+1}N_{i+1} = ^{C_{i+1}}I_{i+1} ^{i+1}\dot{\omega}_{i+1} + ^{i+1}\omega_{i+1} \times ^{C_{i+1}}I_{i+1} ^{i+1}\omega_{i+1}$
リンクi-1がリンクiに与える力$^{i}f_{i} = ^{i}_{i+1}R ^{i+1}f_{i+1} + ^{i}F_{i}$$^{i}f_{i} = ^{i}_{i+1}R ^{i+1}f_{i+1} + ^{i}F_{i}$
リンクi-1がリンクiに与えるモーメント$^{i}n_{i} = ^{i}N_{i} + ^{i}_{i+1}R ^{i+1}n_{i+1} + ^{i}P_{C_i} \times ^{i}F_{i} + ^{i}P_{i+1} \times ^{i}_{i+1}R ^{i+1}f_{i+1}$$^{i}n_{i} = ^{i}N_{i} + ^{i}_{i+1}R ^{i+1}n_{i+1} + ^{i}P_{C_i} \times ^{i}F_{i} + ^{i}P_{i+1} \times ^{i}_{i+1}R ^{i+1}f_{i+1}$
一般化力$\tau_{i} = ^{i}n_{i}^T ^{i}\hat{Z}_{i}$$\tau_{i} = ^{i}f_{i}^T ^{i}\hat{Z}_{i}$
↑

順運動学 †

  • $\theta, \dot{\theta}, \tau$が既知の時、$\ddot{\theta}$を求める
    • 運動方程式の変数定義：$\bf{\tau} = A(\bf{\theta}) \bf{\ddot{\theta}} + b(\theta, \dot{\theta})$
    • 下記2つによって、運動方程式が求まるので、$\ddot{\theta}$は逆行列で求まる。
前処理成り立つ式
$\ddot{\theta} = 0$で逆動力学を解く$b = \bf{\tau}$
$\ddot{\theta} = e_i$で逆動力学を解く$A.col(i) = \bf{\tau} - b$
↑

閉リンク拘束条件と一般化座標の選択 †

  • 変数定義
    v仮想リンク
    r仮想リンクが指す実リンク
    $J_v${v}の$\theta_O$に対するヤコビアン
    $\theta_O=\theta_J, \theta_G, \theta_A, \theta_C$開リンクの一般化座標、閉リンクの一般化座標、駆動関節の一般化座標、拘束関節の一般化座標
    $I_G, I_A, I_C$閉リンクの一般化座標、駆動関節の一般化座標、拘束関節の一般化座標が、開リンクの一般化座標の何番目の添え字に相当しているか
  1. 切断点の座標系{i}における$^{0}_{i}v,^{0}_{i}\omega$が一致するので、ループx6個の拘束条件が立つ
    • $J_r \dot{\theta_O} = blockdiag(^{v}_{r}R, ^{v}_{r}R) J_v \dot{\theta_O}$
    • $J_C \dot{\theta_O} = (J_r - blockdiag(^{v}_{r}R, ^{v}_{r}R) J_v) \dot{\theta_O}$
  2. 行方向に、$J_C$から独立な拘束条件のみを抽出
    • $J_C$から独立行抽出により、$J_{Cm}$を構築する。ここではrank-reveilingな完全軸LU分解を利用する。
    • $J_C^T = P_C^{-1} L_C U_C Q_C^{-1}$
    • ここで、$Q_C$の1列目から$rank(J_C)$列目を抽出する。この行列から、1を含む行の添字を集め、添字リスト$I_C$を作る。$J_{Cm}$は、$J_C$から$I_C$行を集めた行列である。
  3. 列方向に、$J_{Cm}$から独立な変数＝ループによって拘束されている変数を抽出
    • $J_Cm$から独立列抽出により、$J_S$を構築する。ここではrank-reveilingな完全軸LU分解を利用する。
    • $J_{Cm} = P_{Cm}^{-1} L_{Cm} U_{Cm} Q_{Cm}^{-1}$
    • ここで、$Q_{Cm}$の1列目から$rank(J_{Cm})$列目を抽出する。この行列から、1を含む行の添字を集め、添字リスト$I_S$を作る。1を含まない行の添え字を集め、添え字リスト$I_G$を作る。
    • $J_S$は、$J_{Cm}$から$I_S$列を集めた行列である。$J_G$は、$J_{Cm}$から$I_G$を集めた行列である。
    • $\theta_O$の、$I_G$行を抽出したベクトルを$\theta_G$として閉リンク一般化座標とする。また、$\theta_O$の、$I_S$である行を抽出したベクトルを$\theta_S$として閉リンク従属座標とする。
  4. 一般化座標と、従属・開・駆動・拘束座標の微分運動学
従属変数の、一般化座標に対するヤコビアン$H=\del \theta_S / \del \theta_G$$J_S^{-1} J_G$
開の全変数の、一般化座標に対するヤコビアン$W = H_O = H_J =\del \theta_O / \del \theta_G$Wのi行目は、$\theta_O$が一般化座標のj番目なら$e_j$、$\theta_O$が従属座標のj番目なら、H.col(j)
駆動関節の変数の、一般化座標に対するヤコビアン$S = H_A =\del \theta_A / \del \theta_G$Sのi行目は、$\theta_A$が一般化座標のj番目なら$e_j$、$\theta_A$が従属座標のj番目なら、H.col(j)
拘束関節の変数の、一般化座標に対するヤコビアン$H_C=\del \theta_C / \del \theta_G$$H_C$のi行目は、$\theta_C$が一般化座標のj番目なら$e_j$、$\theta_C$が従属座標のj番目なら、H.col(j)。
↑

力学的整合性：拘束条件をリンクで明示的に表現する場合の運動方程式 †

=拘束を明示的に軸iとして表現し、拘束条件として$\ddot{\theta_C}=0$を入れる方法。

運動方程式$\begin{pmatrix} A & -H_C^T & H_J^T \\ H_C & 0 & 0 \end{pmatrix} \begin{pmatrix} \ddot{\theta_G} \\ \tau_C \\ \tau_J \end{pmatrix} = \begin{pmatrix} -b \\ -\dot{H_C} \dot{\theta_G} \end{pmatrix}$
$\dot{H_C} \dot{\theta_G}$兵リンク運動方程式のbから、I_Cを抽出したもの
↑

メモ †

独立行抽出は、独立な拘束条件の抽出 独立列抽出は、独立な変数抽出

  • 微分運動学と静力学の双対性
    • $\dot{x}=J(\theta) \dot{\theta}$
    • $\tau=J(\theta)^T f$（fはマニピュレータが物体に及ぼす力）
    • これは$\delta W = \tau^t \delta \theta+ F^t \delta v + N^t \delta \omega$の仮想仕事が0、$\begin{pmatrix}\delta v \\ \delta w \end{pmatrix} = J(\theta) \delta \theta$による。
トップ   新規 一覧 単語検索 最終更新   ヘルプ   最終更新のRSS