初めてのロボットアーム作り

[日本Androidの会秋葉原支部ロボット部 Advent Calendar 2020](https://qiita.com/advent-calendar/2020/akbrobot) 9日目です。

映画のアイアンマンやアップグレード(アマゾンプライムビデオ)を見て、ロボットアームが欲しい！と思ったので、作ってみることにしました。

# 作ったもの
[FS90Controller - Github](https://github.com/BiggieBoo18/FS90Controller)
[1リンクアーム - YouTube](https://youtu.be/XyIVZsw_kic)
[2リンクアーム - YouTube](https://youtu.be/tgkxtIfXRqM)

# 材料
(2リンクアームの場合)

* サーボモータ FS90 2個
* ESP32DevKitCv4 1個
* 単3×4電池ボックス 1個
* 単3電池 4本
* マイクロUSBケーブル 1本
* アームは3Dプリンタで作成

# ロボットアームって？
人の腕のような働きをする機械だそうです。

# どうやって実現する？
運動学、動力学・・・。とりあえず運動学から勉強することに決めました。

# 運動学
物体の運動を、変位(位置や速度など)で記述する古典的な力学の一部門。ロボットの運動学には、順運動学と逆運動学があります。

* 順運動学 : 各関節の変位から手先の位置や姿勢を求める
* 逆運動学 : 手先の位置や姿勢から各関節の変位を求める

以降、1リンク~3リンクの運動学の式を淡々と見ていきます。

## 1リンクの運動学
![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/254464/bfdd25e5-a1fa-3bf4-be4c-1f1abb2236d5.png)

* 順運動学

```math
x = Lcos\theta\\
y = Lsin\theta
```

* 逆運動学

```math
\theta = tan^{-1}(\frac{y}{x})
```

## 2リンクの運動学
![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/254464/bd07dc8f-73bf-14a3-c26d-d1e5e2010ea8.png)

* 順運動学

```math
x = L_1cos\theta_1 + L_2cos(\theta_1 + \theta_2)\\
y = L_1sin\theta_1 + L_2sin(\theta_1 + \theta_2)
```

* 逆運動学

逆運動学の導出には2つの方法があるようです。

* 順運動学の式から変形
* 余弦定理

以下は順運動学の式からの変形で導出した逆運動学の式です。

```math
\begin{aligned}
\theta_1 &= \pm cos^{-1}(\frac{x^2 + y^2 + L^2_1 + L^2_2}{2L_1\sqrt{x^2 + y^2}}) + tan^{-1}(\frac{y}{x})\\
\theta_2 &= tan^{-1}(\frac{y - L_1sin\theta_1}{x - L_1cos\theta_1}) - \theta_1
\end{aligned}
```

# 3リンクアームの運動学
![3dof.jpg](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/254464/3d0ab24b-3b1c-3117-3d62-e400cb465c5f.jpeg)

## 順運動学

```math
\begin{aligned}
x &= (L_2cos(\theta_2) + L_3cos(\theta_2 + \theta_3))cos(\theta_1)\\
y &= (L_2cos(\theta_2) + L_3cos(\theta_2 + \theta_3))sin(\theta_1)\\
z &= L_2sin(\theta_2) + L_3sin(\theta_2 + \theta_3) + L_1
\end{aligned}
```

## 逆運動学
余弦定理を使って求めました。

```math
\begin{aligned}
\overline{OP} &= \sqrt{x^2 + y^2}\\
\alpha &= cos^{-1}(\frac{L_2^2 + L_3^2 - \overline{OP}^2}{2L_2L_3})\\
\beta &= cos^{-1}(\frac{L_2^2 + \overline{OP}^2 - L_3^2}{2L_2\overline{OP}})\\
\gamma &= sin^{-1}(\frac{z - L_1}{\overline{OP}})\\
\theta_1 &= tan^{-1}(\frac{y}{x})\\
\theta_2 &= \gamma \mp\beta\\
\theta_3 &= \pm(\pi - \alpha)
\end{aligned}
```

# アームとアプリケーション作成
サーボモータは秋月電子で購入したFS90を使いました。アーム部分は3Dプリンタでデザインしたものを使っています。ついでにスマホで操作するためにAndroidアプリを作りました。動作は下の図のようになります。
![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/254464/e1cf0c71-8fb3-a329-19ac-3a000d8c9aae.png)


# ブログ記事
* [detamamoruのブログ - ロボット](https://detamamoru.hateblo.jp/archive/category/%E3%83%AD%E3%83%9C%E3%83%83%E3%83%88)

# 参考
* [イラストで学ぶ ロボット工学 (KS情報科学専門書) | 木野 仁, 谷口 忠大 |本 | 通販 | Amazon](https://www.amazon.co.jp/%E3%82%A4%E3%83%A9%E3%82%B9%E3%83%88%E3%81%A7%E5%AD%A6%E3%81%B6-%E3%83%AD%E3%83%9C%E3%83%83%E3%83%88%E5%B7%A5%E5%AD%A6-KS%E6%83%85%E5%A0%B1%E7%A7%91%E5%AD%A6%E5%B0%82%E9%96%80%E6%9B%B8-%E6%9C%A8%E9%87%8E-%E4%BB%81/dp/4061538349/ref=sr_1_1?__mk_ja_JP=%E3%82%AB%E3%82%BF%E3%82%AB%E3%83%8A&crid=1TCYQL6UOOZU0&dchild=1&keywords=%E3%82%A4%E3%83%A9%E3%82%B9%E3%83%88%E3%81%A7%E5%AD%A6%E3%81%B6%E3%83%AD%E3%83%9C%E3%83%83%E3%83%88%E5%B7%A5%E5%AD%A6&qid=1601251377&sprefix=%E3%82%A4%E3%83%A9%E3%82%B9%E3%83%88%E3%81%A7%E5%AD%A6%E3%81%B6%2Caps%2C395&sr=8-1)
* [2リンクモデルの逆運動学を求める！順運動学の式から算出する方法｜Tajima Robotics](https://tajimarobotics.com/kinematics-two-link-model-2/)
* [pythonで2リンクの逆運動学を解いて表示する - ロボット、電子工作、IoT、AIなどの開発記録](http://weekendproject9.hatenablog.com/entry/2017/11/17/013434)
* [2リンクマニピュレータの軌道追従制御 - Qiita](https://qiita.com/fumiya_sato/items/9167f12bf182fa585968)