機構学

編集履歴

制御

用語定義

  • 注意点

    • 直感のために基本的にはCraig先生の表記を採用する（修正D-H記法というらしい）
    • Craig先生と中村先生とで，座標系定義が異なるため，動力学計算の式が異なる
    • 具体的には逆動力学，Craig 5.43とロボットモーション3.3.67あたりで異なる

  • 中村先生とCraig先生の比較

Craig先生中村先生
ジョイント回転軸・直動軸の両方の立式回転軸のみに特化した立式
立式ベースリンクを介さずに漸化式を導いているほぼ全ての立式がベースリンクを介す（本には書いていないが運動学動力学統合化計算の要請）
座標系定義ジョイントiのz軸にqiq_{i}の回転ジョイントが乗っている．関節iの動きによって{i}が動くジョイントiのz軸にqi−1q_{i-1}の回転ジョイントが乗っている．関節iの動きによって{i}が動く
  • 用語定義
Craig中村意味
{i}{i}座標系i
θi\theta_iθi\theta_i軸iの一般化座標
BAR^{A}_{B}RBAR^{A}_{B}R{A}から見た，{B}への回転行列
AXB^{A}X_{B}{A}から見た，{B}の何らか属性であるベクトルX
APB^{A}P_{B}BAP^{A}_{B}P{A}から見た，{A}から{B}への原点への並進ベクトル
AZ^B^{A}\hat{Z}_{B}{A}から見た，{B}のZ軸．A=Bの時，(0, 0, 1)^t
Afi,Ani^{A}f_{i}, ^{A}n_{i}{A}から見た，リンク{i-1}がリンク{i}に与える力とトルク
{C_i}リンクiの重心を原点として{i}と同じ姿勢の座標系

ロボティクス計算の種類

順(Forward)逆(Inverse)
運動学(Kinematics)q→i0R,i0Pq \to ^{0}_{i}R, ^{0}_{i}P
微分運動学(Differential Kinematics)q˙→0ωi,0P˙i\dot{q} \to ^{0}\omega_{i}, ^{0}\dot{P}_{i} 0ωi,0P˙i→q˙^{0}\omega_{i}, ^{0}\dot{P}_{i} \to \dot{q}
動力学(Dynamics)q,q˙,τ→q¨q, \dot{q}, \tau \to \ddot{q}q,q˙,q¨→τq, \dot{q}, \ddot{q} \to \tau

基本定理

  • 座標変換の基本定理

  • 変数定義

QQ時変する点Q．{B}でgiven
AVQ=AQ˙^{A}V_{Q} = ^{A} \dot{Q}{A}から見た点Qの速度．なぜVQと表現するかというと，逆動力学でvを前向きニュートンオイラーで求めるから
AV˙Q=AQ¨^{A}\dot{V}_{Q} = ^{A} \ddot{Q}{A}から見た点Qの加速度
AΩB^{A}\Omega_{B}{A}から見た．{B}の角速度
  • 定理
位置BQ=ABRAQ^{B}Q = ^{B}_{A}R ^{A}Q
姿勢CAR=BARCBR^{A}_{C}R = ^{A}_{B}R ^{B}_{C}R
速度AVQ=AVBORG+BARBVQ+AΩB×BARBQ^{A}V_{Q} = ^{A}V_{BORG} + ^{A}_{B}R ^{B}V_{Q} + ^{A}\Omega_{B} \times ^{A}_{B}R ^{B}Q(5.13)
角速度AΩC=AΩB+BARBΩC^{A}\Omega_{C} = ^{A}\Omega_{B}+ ^{A}_{B}R ^{B}\Omega_{C}
加速度AV˙Q=AV˙BORG+BARBV˙Q+2AΩB×BARBVQ+AΩ˙B×BARBQ+AΩB×(AΩB×BARBQ)^{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Ω˙C=AΩ˙B+BARBΩ˙C+AΩB×BARBΩC^{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)

逆運動学

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

  • 座標系伝播
回転軸直動軸
一般化座標qi=θiq_i = \theta_iqi=diq_i = d_i
一般化力τi\tau_iτi\tau_i
速度i+1vi+1=ii+1R (ivi+iωi×iPi+1)^{i+1}v_{i+1} = ^{i+1}_{i}R \, (^{i}v_{i} + ^{i}\omega_{i} \times ^{i}P_{i+1}) (5.47)ii+1R (ivi+iωi×iPi+1)+di+1˙i+1Z^i+1^{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ωi+1=ii+1R iωi+θ˙i+1Z^i+1^{i+1}\omega_{i+1}=^{i+1}_{i}R \, ^{i}\omega_{i} + \dot{\theta} ^{i+1}\hat{Z}_{i+1} (5.45)i+1ωi+1=ii+1R iωi^{i+1}\omega_{i+1}=^{i+1}_{i}R \, ^{i}\omega_{i} (5.48)
加速度i+1v˙i+1=ii+1R (iω˙i×iPi+1+iωi×(iωi×iPi+1)+iv˙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})i+1v˙i+1=ii+1R (iω˙i×iPi+1+iωi×(iωi×iPi+1)+iv˙i)+2i+1ωi+1×d˙i+1 i+1Z^i+1+d¨i+1 i+1Z^i+1^{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ω˙i+1=ii+1R iω˙i+ii+1R iωi×θi+1˙i+1Z^i+1+θi+1¨i+1Z^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ω˙i+1=ii+1R iω˙i^{i+1}\dot{\omega}_{i+1} = ^{i+1}_{i}R \, ^{i}\dot{\omega}_{i}(6.33)
重心加速度（直動・回転のみ成立する式）ivCi˙=iωi×(iωi×iPCi)+iv˙i^{i}\dot{v_{Ci}} = ^{i}\omega_{i} \times (^{i}\omega_{i} \times ^{i}P_{C_i}) + ^{i}\dot{v}_{i}ivCi˙=iωi×(iωi×iPCi)+iv˙i^{i}\dot{v_{Ci}} = ^{i}\omega_{i} \times (^{i}\omega_{i} \times ^{i}P_{C_i}) + ^{i}\dot{v}_{i}
合力iFi=miiv˙Ci^{i}F_{i} = m_{i} ^{i}\dot{v}_{C_{i}}iFi=miiv˙Ci^{i}F_{i} = m_{i} ^{i}\dot{v}_{C_{i}}
合モーメントiNi=CiIiiω˙i+iωi×CiIiiωi^{i}N_{i} = ^{C_{i}}I_{i} ^{i}\dot{\omega}_{i} + ^{i}\omega_{i} \times ^{C_{i}}I_{i} ^{i}\omega_{i}iNi=CiIiiω˙i+iωi×CiIiiω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に与える力ifi=i+1iR i+1fi+1+iFi^{i}f_{i} = ^{i}_{i+1}R \, ^{i+1}f_{i+1} + ^{i}F_{i}ifi=i+1iR i+1fi+1+iFi^{i}f_{i} = ^{i}_{i+1}R \, ^{i+1}f_{i+1} + ^{i}F_{i}
リンクi-1がリンクiに与えるモーメントini=iNi+i+1iR i+1ni+1+iPCi×iFi+iPi+1×i+1iR i+1fi+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}ini=iNi+i+1iR i+1ni+1+iPCi×iFi+iPi+1×i+1iR i+1fi+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}
一般化力τi=iniT iZ^i\tau_{i} = ^{i}n_{i}^T \, ^{i}\hat{Z}_{i}τi=ifiT iZ^i\tau_{i} = ^{i}f_{i}^T \, ^{i}\hat{Z}_{i}

順運動学

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

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

  • 閉ループ構造は，機構の剛性を高める効果がある一方，一般に関節の可動範囲をせばめ，作業空間を減少させる（Craig, p.231）
  • 変数定義 | | | | --- | --- | | v | 仮想リンク | | r | 仮想リンクが指す実リンク | | JvJ_v | {v}のqOq_Oに対するヤコビアン | | qO=qJ,qG,qA,qCq_O=q_J, q_G, q_A, q_C | 開リンクの一般化座標、閉リンクの一般化座標、駆動関節の一般化座標、拘束関節の一般化座標 | | IG,IA,ICI_G, I_A, I_C | 閉リンクの一般化座標、駆動関節の一般化座標、拘束関節の一般化座標が、開リンクの一般化座標の何番目の添え字に相当しているか |
  1. 切断点の座標系{i}におけるi0v,i0ω^{0}_{i}v,^{0}_{i}\omegaが一致するので、ループx6個の拘束条件が立つ
  • JrqO˙=blockdiag(rvR,rvR)JvqO˙J_r \dot{q_O} = blockdiag(^{v}_{r}R, ^{v}_{r}R) J_v \dot{q_O}
  • JCqO˙=(Jr−blockdiag(rvR,rvR)Jv)qO˙J_C \dot{q_O} = (J_r - blockdiag(^{v}_{r}R, ^{v}_{r}R) J_v) \dot{q_O}
  1. 行方向に、JCJ_Cから独立な拘束条件のみを抽出
  • JCJ_Cから独立行抽出により、JCmJ_{Cm}を構築する。ここではrank-reveilingな完全軸LU分解を利用する。
  • JCT=PC−1LCUCQC−1J_C^T = P_C^{-1} L_C U_C Q_C^{-1}
  • ここで、QCQ_Cの1列目からrank(JC)rank(J_C)列目を抽出する。この行列から、1を含む行の添字を集め、添字リストICI_Cを作る。JCmJ_{Cm}は、JCJ_CからICI_C行を集めた行列である。
  1. 列方向に、JCmJ_{Cm}から独立な変数＝ループによって拘束されている変数を抽出
  • JCmJ_Cmから独立列抽出により、JSJ_Sを構築する。ここではrank-reveilingな完全軸LU分解を利用する。
  • JCm=PCm−1LCmUCmQCm−1J_{Cm} = P_{Cm}^{-1} L_{Cm} U_{Cm} Q_{Cm}^{-1}
  • ここで、QCmQ_{Cm}の1列目からrank(JCm)rank(J_{Cm})列目を抽出する。この行列から、1を含む行の添字を集め、添字リストISI_Sを作る。1を含まない行の添え字を集め、添え字リストIGI_Gを作る。
  • JSJ_Sは、JCmJ_{Cm}からISI_S列を集めた行列である。JGJ_Gは、JCmJ_{Cm}からIGI_Gを集めた行列である。
  • qOq_Oの、IGI_G行を抽出したベクトルをqGq_Gとして閉リンク一般化座標とする。また、qOq_Oの、ISI_Sである行を抽出したベクトルをqSq_Sとして閉リンク従属座標とする。
  1. 一般化座標と、従属・開・駆動・拘束座標の微分運動学
従属変数の、一般化座標に対するヤコビアンH=δqS/δqGH=\delta q_S / \delta q_GJS−1JGJ_S^{-1} J_G
開の全変数の、一般化座標に対するヤコビアンW=HO=HJ=δqO/δqGW = H_O = H_J =\delta q_O / \delta q_GWのi行目は、qOq_Oが一般化座標のj番目ならeje_j、qOq_Oが従属座標のj番目なら、H.col(j)
駆動関節の変数の、一般化座標に対するヤコビアンS=HA=δqA/δqGS = H_A =\delta q_A / \delta q_GSのi行目は、qAq_Aが一般化座標のj番目ならeje_j、qAq_Aが従属座標のj番目なら、H.col(j)
拘束関節の変数の、一般化座標に対するヤコビアンHC=δqC/δqGH_C=\delta q_C / \delta q_GHCH_Cのi行目は、qCq_Cが一般化座標のj番目ならeje_j、qCq_Cが従属座標のj番目なら、H.col(j)。

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

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

運動方程式(A−HCT−HJTHC00)(θG¨τCτJ)=(−b−HC˙θG˙)\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}
HC˙θG˙\dot{H_C} \dot{\theta_G}兵リンク運動方程式のbから、I_Cを抽出したもの

ロボットダイナミクスのLie群的解釈でO(N)ダイナミクス計算

メモ

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

  • 微分運動学と静力学の双対性
    • x˙=J(θ)θ˙\dot{x}=J(\theta) \dot{\theta}
    • τ=J(θ)Tf\tau=J(\theta)^T f（fはマニピュレータが物体に及ぼす力）
    • これはδW=τtδθ+Ftδv+Ntδω\delta W = \tau^t \delta \theta+ F^t \delta v + N^t \delta \omegaの仮想仕事が0、(δvδw)=J(θ)δθ\begin{pmatrix}\delta v \\ \delta w \end{pmatrix} = J(\theta) \delta \thetaによる。

英語表現

  • ii+1A^{i+1}_{i}A is the matrix transformation between coordinates frame i and i+1.
  • 同次変換行列 a homogeneous transformation matrix
  • 座標系i = coordinate frame i

最初からきちんと勉強する試み

表記

  • Uはベースフレーム

位置

  • 回転行列には対応する歪対称行列分解が一意に存在する(2.56)
    • 正規直交行列に関するケーリーの定理
  • オイラー角(2.90, 2.91, 2.92)
    • オイラーは四次元上の点
    • 相互変換の方法が一意に定まる。

速度(5章)

  • 速度は、どの座標系で微分するのかが非常に重要

  • BVQ^B V_Qを、座標系Bから見たQの速度と定義する (5.1)

    • 本当はBVQ^B V_QはBQ˙^B\dot{Q}と書くべきなのだろうと思う…

  • A(BVQ)^A (^B V_Q)を、座標系Aから見た「座標系Bから見たQの速度」と定義する (5.2)

    • A(BVQ)=BAR BVQ^A (^B V_Q)=^A_B R\ ^B V_Q (5.4)

  • 糖衣構文: vC=UvC=UVCORGv_C = ^U v_C = ^U V_{CORG}を、ベース座標系から見た座標系Cの原点の速度、と定義する (5.5)

  • C(AVB)^C (^A V_B)を、座標系Cから見た「座標系Aから見た座標系Bの角速度ベクトル」と定義する (5.6の上)

  • 糖衣構文: ωC=UΩC\omega_C = ^U \Omega_Cを、「座標系Uから見た、座標系Cの角速度ベクトル」と定義する(5.6)

  • 速度の連鎖律

    • 求：座標系Aから点Pの速度を知りたい
      • 与：座標系Bから点Pの速度 BVP^B V_P
      • 与：座標系Aに対する、座標系Bの速度 AVBORG^A V_{BORG}
      • 与：座標系Aに対する、座標系Bの角速度 AΩB^A \Omega_{B}
    • 速度は線形なので、与えられたものの3つのパターンを独立に足せば良い
      1. 与：座標系Bから点Pの速度 BVP^B V_P→見てる座標系が違うので、座標系Aから見るとBARBVP^A_B R ^B V_P
      2. 与：座標系Aに対する、座標系Bの速度 AVBORG^A V_{BORG}→座標系が同じなので、そのままAVBORG^A V_{BORG}足す。
      3. 与：座標系Aに対する、座標系Bの角速度AΩB^A \Omega_{B}→AΩB×(ABORGP+BARBQ)^A \Omega_{B} \times (A^P_{BORG} + ^A_B R ^B Q)じゃないの？？（！！！！！！！！！！！）どうやら前半がないらしい。なんで？？？→何で座標系ごと回転してると思っているんですかね…(5.13)

最終更新: 2020-01-01