修正DH法のわかりやすい解説

# まえがき

修正DH法を用いてパラメータを設計する機会があり、思ったよりも手こずったのでまた今度設計する機会があるかもしれませんので、備忘録として残しておきます。
前提として、通常のDH法によるパラメータ設計が行えることを想定しています。
修正DH法に関して自分は以下のサイトや動画を参考にしました。

https://qiita.com/takeshiD/items/7bd5cf88a66a3ff59358

https://www.youtube.com/watch?v=RO_DaYSW_J8

# DH法の説明
|　　　　　　　　   　　　　![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/3512710/4c78f4bf-eb00-4fd8-e550-2b76ad04bc2b.png)　　　　　　　　　　　   　|
|:-:|


DH法はロボットの関節位置を4つのパラメータを用いて表す方法で、パラメータから得られる同次座標変換行列を原点座標に対いて順にかけていくことで各リンクの座標を求められます。上図はDH法による変換のイメージ図で、現在のリンクの座標と前のリンクの座標をもとにパラメータを求めていきます。実際には変換ごとに必ず関節座標と一致するわけではないですが、イメージとしてはこんな感じです。そのため、初めに示した動画では全リンクの$\alpha$を一気に決めていますが、各リンク毎にパラメータを決めていく方が個人的には分かりやすいと思います。

DHパラメータから得られる同次座標変換行列は以下の様に定まります。
```math
T^{0}_{1} = \mathbf{R}(x_{i-1}, \alpha_{i-1})
\mathbf{R}(x_{i-1}, a_{i-1})
\mathbf{R}(z_{i}, \theta_{i})
\mathbf{R}(z_{i}, d_{i})
```

この変換は修正DH法におけるものであり、DH法とは異なりますので注意してください。これらの変換の意味としては、$\sum_0$における座標を$\alpha_{i-1}$だけ$x_{i-1}$を回転軸として回転させ$x_{i-1}$軸に向けて$a_{i-1}$だけ平行移動させた後に、$z_i$を回転軸として$\theta_i$回転させて$z_i$方向に平行移動させるといったものになっています。

DHパラメータはロボットのスケルトン表示から求め、以下の表にまとめていきます。

|$\mathrm{L}$|$\alpha_{i-1}$|$a_{i-1}$|$\theta_i$|$d_i$|
|:---:|:---:|:---:|:---:|:---:|
|1|||||
|2|||||

# DHパラメータの導出例
文字だけの説明ではわかりづらいと思いますので、実際にDHパラメータを導出する様子をそのままお見せします。

1.　スケルトン表示を描く。
    DHパラメータを求めたいロボットのスケルトン表示を描きます。

|　　　　　　　　   　　　　![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/3512710/a0c56492-02cc-c415-c26a-eb6101cf3994.png)　　　　　　　　　　　   　|
|:-:|

2.　原点座標を決める。
    スケルトン表示をもとに、自分が実環境でロボットを配置させたい方向に合うように原点座標$(x_0, y_0, z_0)$を決めましょう。$z_0$をリンク方向に決めさえすれば、$x_0$は任意の方向で構いません。

|　　　　　　　　   　　　　![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/3512710/26d30e3e-eced-5a7a-47e3-2da8c9a4de5e.png)　　　　　　　　　　　   　|
|:-:|

3.　各関節の$Z$軸を決める
    回転関節であれば回転軸方向に、直動関節であれば直動方向に$Z$軸を決定していきます。

|　　　　　　　　   　　　　![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/3512710/26c742c2-33ad-5e27-190f-6256a32df2a1.png)　　　　　　　　　　　   　|
|:-:|

4.　各関節の$X$軸を決める。
    $i$を関節番号として、$Z_{i}\perp X_{i-1}$の条件を満たす様に各関節$X$軸を決定します。

|　　　　　　　　   　　　　![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/3512710/83d75bb5-c0b0-79b7-8499-29c2c1c92e4f.png)　　　　　　　　　　　   　|
|:-:|

5.　$Y$軸を決める。
    DHパラメータを導出するだけであれば必要はないですが、y軸を決定する場合はy軸が右手系を満たす様に決定してください。今回は省略します。

6.　原点座標から順にDHパラメータを決定していく。
    先ほど決定した座標を基に各関節の座標を求めるためのDHパラメータを決定していきます。ここで、関節$i$での座標を$\sum_i$と表し、関節$i$から関節$i+1$への変換を$\sum_i → \sum_{i+1}$と表すことします。また、角度は記述が楽な度数表記にしておきます。


**①　$\sum_1 → \sum_2$の導出**



![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/3512710/72dc9350-a853-3b44-ab3c-f5a8b3687cc5.png)


次に$a$を決定します。今回原点座標である$\sum_0$と$\sum_1$が一致していますので、$x_0$方向への移動は不要とで0となります。

![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/3512710/c8a2af58-1d5b-8bc7-bcfd-f81a08109012.png)

$θ$は次の関節の$x_i+1$の基準位置と$x_i$を一致させるために、$z_i$を回転軸として回転させる角度になります。$z$軸はすべて関節の回転軸を基準として決定していますので、関節の回転角度を入れてやればよいです。関節の回転角度は$\theta_i$と表記することにします。

![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/3512710/d5b918d5-75a3-3be3-e5c0-66ae3946bb5b.png)

最後に$d$になるのですが、今回$z_1$軸方向にいくら移動をさせても$\sum_2$に到達することができません。そのため0となります。

![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/3512710/d3683f7e-c5ed-1de8-9ed4-2c2f895c66dc.png)

あれ？結局このDHパラメータでは座標の変換ができていないのでは？
そう思われた方。その通りです。
このようにDHパラメータでは一回の変換では次の関節の座標に到達できない場合が存在します。こういった場合は座標の角度のみを変換する仮想関節が存在すると考えてパラメータを求めていきます。
例えば今回の場合、$x$軸を一度回転させてから平行移動し、その後$x_2$軸の基準位置と一致するように回転させれば$\sum_2$に到達できます。下図は$\sum_1 → \sum_2$を実現するために仮想関節を挿入した場合の等価的なスケルトン表示とそのDHパラメータで、固定角度だけ回転移動を行う関節が挿入したと考えることでDHパラメータを用いた座標の変換が可能となります。

![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/3512710/cf2b7182-1037-fe83-df2b-1f20a4dd31db.png)

ちなみに、下図のような変換でも大丈夫です。このようにDHパラメータは同じロボットでも異なるパラメータになることがあります。

![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/3512710/2816e775-84e3-c9fa-ac60-3fd9351bdad4.png)

**②　$\sum_2 → \sum_3$の導出**

   $z$軸を回転させて同じ方向にした後に、$z$軸方向に対して移動を行えば大丈夫です。

![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/3512710/ef6d3710-e4c8-8b74-3315-60eab7730493.png)

**③　確認**
さて、求められたDHパラメータが正しいか確認してみましょう。$\theta_2$の回転が分かりやすいようにエンドエフェクタの$x$軸方向に棒を付け加えて$\theta_1$と$\theta_2$を変化させて挙動を確認してみます。

|　　　　　　　　   　　　　![parameter0_0.gif](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/3512710/5bcea763-1429-e2b0-f394-e3b822546131.gif)　　　　　　　　　　　   　|
|:-:|
|$\theta_1$を変化させた様子|

|　　　　　　　　   　　　　![parameter3_0.gif](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/3512710/a25bf0cf-a279-4653-3401-51dfaa0c6f6f.gif)　　　　　　　　　　　   　|
|:-:|
|$\theta_2$を変化させた様子|

赤い点が動かしている関節になるのですが、うまくいっている様子が確認できます。