制御
用語定義 †
- 注意点
- 直感のために基本的にはCraig先生の表記を採用する（修正D-H記法というらしい）
- Craig先生と中村先生とで，座標系定義が異なるため，動力学計算の式が異なる
- 具体的には逆動力学，Craig 5.43とロボットモーション3.3.67あたりで異なる
| 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}, \tau \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}_{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}) + 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}F_{i} = m_{i} ^{i}\dot{v}_{C_{i}} \) | \( ^{i}F_{i} = m_{i} ^{i}\dot{v}_{C_{i}} \) | | 合モーメント | \( ^{i}N_{i} = ^{C_{i}}I_{i} ^{i}\dot{\omega}_{i} + ^{i}\omega_{i} \times ^{C_{i}}I_{i} ^{i}\omega_{i} \) | \( ^{i}N_{i} = ^{C_{i}}I_{i} ^{i}\dot{\omega}_{i} + ^{i}\omega_{i} \times ^{C_{i}}I_{i} ^{i}\omega_{i} \) | | リンク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 \) |
閉リンク拘束条件と一般化座標の選択 †
- 閉ループ構造は，機構の剛性を高める効果がある一方，一般に関節の可動範囲をせばめ，作業空間を減少させる（Craig, p.231）
- 変数定義
| v | 仮想リンク | | r | 仮想リンクが指す実リンク | | \( J_v \) | {v}の\( q_O \)に対するヤコビアン | | \( q_O=q_J, q_G, q_A, q_C \) | 開リンクの一般化座標、閉リンクの一般化座標、駆動関節の一般化座標、拘束関節の一般化座標 | | \( I_G, I_A, I_C \) | 閉リンクの一般化座標、駆動関節の一般化座標、拘束関節の一般化座標が、開リンクの一般化座標の何番目の添え字に相当しているか |
- 切断点の座標系{i}における\( ^{0}_{i}v,^{0}_{i}\omega \)が一致するので、ループx6個の拘束条件が立つ
- \( J_r \dot{q_O} = blockdiag(^{v}_{r}R, ^{v}_{r}R) J_v \dot{q_O} \)
- \( J_C \dot{q_O} = (J_r - blockdiag(^{v}_{r}R, ^{v}_{r}R) J_v) \dot{q_O} \)
- 行方向に、\( 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 \)行を集めた行列である。
- 列方向に、\( 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 \)を集めた行列である。
- \( q_O \)の、\( I_G \)行を抽出したベクトルを\( q_G \)として閉リンク一般化座標とする。また、\( q_O \)の、\( I_S \)である行を抽出したベクトルを\( q_S \)として閉リンク従属座標とする。
- 一般化座標と、従属・開・駆動・拘束座標の微分運動学
| 従属変数の、一般化座標に対するヤコビアン | \( H=\delta q_S / \delta q_G \) | \( J_S^{-1} J_G \) | | 開の全変数の、一般化座標に対するヤコビアン | \( W = H_O = H_J =\delta q_O / \delta q_G \) | Wのi行目は、\( q_O \)が一般化座標のj番目なら\( e_j \)、\( q_O \)が従属座標のj番目なら、H.col(j) | | 駆動関節の変数の、一般化座標に対するヤコビアン | \( S = H_A =\delta q_A / \delta q_G \) | Sのi行目は、\( q_A \)が一般化座標のj番目なら\( e_j \)、\( q_A \)が従属座標のj番目なら、H.col(j) | | 拘束関節の変数の、一般化座標に対するヤコビアン | \( H_C=\delta q_C / \delta q_G \) | \( H_C \)のi行目は、\( q_C \)が一般化座標のj番目なら\( e_j \)、\( q_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を抽出したもの |
ロボットダイナミクスのLie群的解釈でO(N)ダイナミクス計算 †
メモ †
独立行抽出は、独立な拘束条件の抽出
独立列抽出は、独立な変数抽出
- 微分運動学と静力学の双対性
- \( \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 \)による。
英語表現 †
- \( ^{i+1}_{i}A \) is the matrix transformation between coordinates frame i and i+1.
- 同次変換行列 a homogeneous transformation matrix
- 座標系i = coordinate frame i
最初からきちんと勉強する試み †
表記 †
位置 †
- 回転行列には対応する歪対称行列分解が一意に存在する(2.56)
- オイラー角(2.90, 2.91, 2.92)
- オイラーは四次元上の点
- 相互変換の方法が一意に定まる。
速度(5章) †
- 速度は、どの座標系で微分するのかが非常に重要
- \( ^B V_Q \)を、座標系Bから見たQの速度と定義する (5.1)
- 本当は\( ^B V_Q \)は\( ^B\dot{Q} \)と書くべきなのだろうと思う…
- \( ^A (^B V_Q) \)を、座標系Aから見た「座標系Bから見たQの速度」と定義する (5.2)
- \( ^A (^B V_Q)=^A_B R\ ^B V_Q \) (5.4)
- 糖衣構文: \( v_C = ^U v_C = ^U V_{CORG} \)を、ベース座標系から見た座標系Cの原点の速度、と定義する (5.5)
- \( ^C (^A V_B) \)を、座標系Cから見た「座標系Aから見た座標系Bの角速度ベクトル」と定義する (5.6の上)
- 糖衣構文: \( \omega_C = ^U \Omega_C \)を、「座標系Uから見た、座標系Cの角速度ベクトル」と定義する(5.6)
- 速度の連鎖律
- 求：座標系Aから点Pの速度を知りたい
- 与：座標系Bから点Pの速度 \( ^B V_P \)
- 与：座標系Aに対する、座標系Bの速度 \( ^A V_{BORG} \)
- 与：座標系Aに対する、座標系Bの角速度 \( ^A \Omega_{B} \)
- 速度は線形なので、与えられたものの3つのパターンを独立に足せば良い
- 与：座標系Bから点Pの速度 \( ^B V_P \)→見てる座標系が違うので、座標系Aから見ると\( ^A_B R ^B V_P \)
- 与：座標系Aに対する、座標系Bの速度 \( ^A V_{BORG} \)→座標系が同じなので、そのまま\( ^A V_{BORG} \)足す。
- 与：座標系Aに対する、座標系Bの角速度\( ^A \Omega_{B} \)→\( ^A \Omega_{B} \times (A^P_{BORG} + ^A_B R ^B Q) \)じゃないの？？（！！！！！！！！！！！）どうやら前半がないらしい。なんで？？？→何で座標系ごと回転してると思っているんですかね…(5.13)
|
![[PukiWiki]](image/pukiwiki.png "[PukiWiki]")
