ロボットソフトウェア特論 (3)suehiro/class2019/presen/lecture...2019年度前学期...

ロボットソフトウェア特論 (3)

2019.4.24

電気通信大学

大学院情報理工学研究科

末廣尚士

2019年度前学期電気通信大学大学院情報理工学研究科情報学専攻ロボットソフトウェア特論

3. ロボットアームの運動学

順運動学：ロボットの関節角度が与えられたとき，手先の位置（および姿勢）を求める

逆運動学：ロボットの手先の位置（および姿勢）が与えられたとき，それを実現する関節角度を求める

アームのヤコビ行列：ロボットの関節の微小な動き（もしくは速度）とロボットの手先の位置（および姿勢）の微小な動き（もしくは速度）との関係を求める

2


- ロボット作業の表現

table上のplace_Aにあるboxをplace_Bに動かす。

3


- ロボットアームの座標系表現

4

リンクに貼り付けられた座標系でその位置・姿勢などの状態を表現する

運動学では，それら（特にbaseとwrist(もしくはhand)）の関係を求める

𝑙3

𝑙2

𝑙1𝑙𝑏


- 3自由度アームの順運動学

関節角度から手首、手先、ツールの位置や姿勢を計算する

角度は何でもよいの

だが 𝜃1 =𝜋

6，𝜃2 =

𝜋

4，

𝜃3 =𝜋

2で図示して

みる

5

𝑙2𝑙3

𝑙1

𝑙𝑏


- 手首位置の計算(1)

base𝑧wrist = 𝑙𝑏 + 𝑙1 + 𝑙2 cos 𝜃2 + 𝑙3 cos 𝜃2 + 𝜃3

L1𝑥wrist = 𝑙2 sin 𝜃2 + 𝑙3 sin 𝜃2 + 𝜃36

𝑙2𝑙3

𝑙1

𝑙𝑏


- 手首位置の計算(2)

L1𝑥wrist = 𝑙2 sin 𝜃2 + 𝑙3 sin 𝜃2 + 𝜃3

base𝑥wrist =L1𝑥wrist cos 𝜃1 = 𝑙2 sin 𝜃2 + 𝑙3 sin 𝜃2 + 𝜃3 cos 𝜃1

base𝑦wrist =L1𝑥wrist sin 𝜃1 = 𝑙2 sin 𝜃2 + 𝑙3 sin 𝜃2 + 𝜃3 sin 𝜃1

7


- 3自由度アームの順運動学まとめ

base𝑥wrist = 𝑙2 sin 𝜃2 + 𝑙3 sin 𝜃2 + 𝜃3 cos 𝜃1

base𝑦wrist = 𝑙2 sin 𝜃2 + 𝑙3 sin 𝜃2 + 𝜃3 sin 𝜃1

base𝑧wrist = 𝑙𝑏 + 𝑙1 + 𝑙2cos 𝜃2 + 𝑙3 cos 𝜃2 + 𝜃3

8


- 3自由度アームの逆運動学

手先の位置が

base𝑥wristbase𝑦wrist

base𝑧wrist

のとき，関節角度𝜃1，

𝜃2，𝜃3を求める．

9


- 幾何学的(解析的)解法まずは順運動学base𝑧wrist = 𝑙𝑏 + 𝑙1 + 𝑙2 cos 𝜃2 + 𝑙3 cos 𝜃2 + 𝜃3base𝑥wrist =

L1𝑥hand cos 𝜃1 = 𝑙2 sin 𝜃2 + 𝑙3sin 𝜃2 + 𝜃3 cos 𝜃1base𝑦wrist =

L1𝑥hand sin 𝜃1 = 𝑙2 sin 𝜃2 + 𝑙3sin 𝜃2 + 𝜃3 sin 𝜃1

where L1𝑥wrist = 𝑙2 sin 𝜃2 + 𝑙3 sin 𝜃2 + 𝜃3

10

𝑙2𝑙3

𝑙1

𝑙𝑏


- 𝜃1の計算base𝑥wrist =

L1𝑥wrist cos 𝜃1base𝑦wrist =

L1𝑥wrist sin 𝜃1

よって，tan 𝜃1 = ൗbase𝑦wristbase𝑥wrist

𝜃1 =

tan−1 ൗbase𝑦wristbase𝑥wrist − 𝜋 𝑥 < 0, 𝑦 < 0

− Τ𝜋 2 𝑥 = 0, 𝑦 < 0

tan−1 ൗbase𝑦wristbase𝑥wrist 𝑥 > 0

Τ𝜋 2 𝑥 = 0, 𝑦 > 0

tan−1 ൗbase𝑦wristbase𝑥wrist + 𝜋 𝑥 < 0, 𝑦 ≥ 0

unknown 𝑥 = 0, 𝑦 = 0

もしくは，𝜃1 = atan2 base𝑦wrist ,base𝑥wrist

11


- 𝜃3の計算base𝑧wrist = 𝑙𝑏 + 𝑙1 + 𝑙2 cos 𝜃2 + 𝑙3 cos 𝜃2 + 𝜃3L1𝑥wrist = 𝑙2 sin 𝜃2 + 𝑙3 sin 𝜃2 + 𝜃3

ここで，

𝑋 = L1𝑥wrist (𝑋2 = base𝑥wrist2 + base𝑦wrist

2 )

𝑍 = base𝑧wrist − 𝑙𝑏 − 𝑙1

とすると，

𝑋 = 𝑙2 sin 𝜃2 + 𝑙3 sin 𝜃2 + 𝜃3𝑍 = 𝑙2 cos 𝜃2 + 𝑙3 cos 𝜃2 + 𝜃3

両辺を2乗して加える，

𝑋2 = 𝑙22 sin2 𝜃2 + 2𝑙2𝑙3 sin 𝜃2 sin 𝜃2 + 𝜃3 + 𝑙3

2 sin2 𝜃2 + 𝜃3

𝑍2 = 𝑙22 cos2 𝜃2 + 2𝑙2𝑙3 cos 𝜃2 cos 𝜃2 + 𝜃3 + 𝑙3

2 cos2 𝜃2 + 𝜃3

𝑋2 + 𝑍2 = 𝑙22 + 𝑙3

2 + 2𝑙2𝑙3 cos 𝜃2 cos 𝜃2 + 𝜃3 + sin 𝜃2 sin 𝜃2 + 𝜃312

𝑙𝑏

𝑙1

𝑙2𝑙3


- 𝜃3の計算(つづき)𝑋2 + 𝑍2 = 𝑙2

2 + 𝑙32 + 2𝑙2𝑙3 cos 𝜃2 cos 𝜃2 + 𝜃3 + sin 𝜃2 sin 𝜃2 + 𝜃3

ここで cos 𝛼 − 𝛽 = cos 𝛼 cos 𝛽 + sin 𝛼 sin 𝛽 を思い出して，

𝛼 = 𝜃2 + 𝜃3 ，𝛽 = 𝜃2 とすると，

𝑋2 + 𝑍2 = 𝑙22 + 𝑙3

2 + 2𝑙2𝑙3 cos 𝜃3 (余弦定理の変種)

cos 𝜃3 =𝑋2+𝑍2−𝑙2

2−𝑙32

2𝑙2𝑙3

よって，

𝜃3 = ±cos−1𝑋2+𝑍2−𝑙2

2−𝑙32

2𝑙2𝑙3

13


- 𝜃2の計算

14

𝜃3が，「+」の場合「-」の場合

𝜓 = sin−1𝑍

𝑋2 + 𝑍2

𝜙 = sin−1𝑙3 sin 𝜃3

𝑋2 + 𝑍2

𝜃2 = Τ𝜋 2 − 𝜓 − 𝜙

𝜓 = sin−1𝑍

𝑋2 + 𝑍2


𝑋2 + 𝑍2

𝜃2 = Τ𝜋 2 − 𝜓 − 𝜙

アームの姿勢が変わっても実は式は同じ．ここの符号が自動的に変わる．


- 結果のグラフィックス表示1

𝜓 = sin−1𝑍

𝑋2+𝑍2


𝑋2+𝑍2

𝜃2 = Τ𝜋 2 − 𝜓 − 𝜙

15


- 結果のグラフィックス表示2

𝜓 = sin−1𝑍

𝑋2+𝑍2


𝑋2+𝑍2

𝜃2 = Τ𝜋 2 − 𝜓 − 𝜙

16


- 実はまだ他に解がある

𝜃1を裏に回して，𝜃2と𝜃3を反対に曲げる．

𝜃1 = atan2 −base𝑦wrist , −base𝑥wrist

17


まとめ(1)

𝜃1 = atan2 base𝑦wrist ,base𝑥wrist

𝜃3 = ±cos−1𝑋2 + 𝑍2 − 𝑙2

2 − 𝑙32

2𝑙2𝑙3

𝜃2 = Τ𝜋 2 − 𝜓 − 𝜙

𝜓 = sin−1𝑍

𝑋2 + 𝑍2


𝑋2 + 𝑍218


まとめ(2)

𝜃1 = atan2 −base𝑦wrist , −base𝑥wrist

𝜃3 = ±cos−1𝑋2 + 𝑍2 − 𝑙2

2 − 𝑙32

2𝑙2𝑙3

𝜃2 = − Τ𝜋 2 + 𝜓 − 𝜙

𝜓 = sin−1𝑍

𝑋2 + 𝑍2


𝑋2 + 𝑍219


- アームのヤコビ行列

ロボットの関節の微小な動き（もしくは速度）とロボットの手先の位置（および姿勢）の微小な動き（もしくは速度）との関係

微小な動きの関係は線形関係であらわされるすなわち行列（偏微分係数行列，ヤコビ行列）で表現できる

これを使うと逆運動学を数値的に解けるニュートン法の応用

20


- 原理(2リンクアーム)

まずは簡単な2リンクアームで考える．

関節角が𝜽 =𝜃1𝜃2

のとき，

手先位置が𝒑 =𝑥𝑦 だとする．

21

𝜃1

𝜃2

𝒑 =𝑥𝑦

𝑥

𝑦


- 原理(𝜃1を少しだけ動かす)

ここで𝜃1を少しだけ動かす．

𝜃1を𝜃1 + ∆𝜃1にしたとき

𝒑が𝒑 + ∆𝒑1 =𝑥 + ∆𝑥1𝑦 + ∆𝑦1

になったとする．

これは，∆𝑥1 ≃𝜕𝑥

𝜕𝜃1∆𝜃1，∆𝑦1 ≃

𝜕𝑦

𝜕𝜃1∆𝜃1と

書くことができる．

つまり∆𝒑1 ≃

𝜕𝑥

𝜕𝜃1𝜕𝑦

𝜕𝜃1

∆𝜃1

22

𝜃1

𝜃2

𝒑 =𝑥𝑦

𝑥

𝑦 ∆𝒑1

∆𝜃1


- 原理(𝜃2を少しだけ動かす)

こんどは𝜃2を少しだけ動かす．

𝜃2を𝜃2 + ∆𝜃2にしたとき

𝒑が𝒑 + ∆𝒑2 =𝑥 + ∆𝑥2𝑦 + ∆𝑦2

になったとする．

これは，∆𝑥2 ≃𝜕𝑥

𝜕𝜃2∆𝜃2，∆𝑦2 ≃

𝜕𝑦

𝜕𝜃2∆𝜃2と

書くことができる．

つまり∆𝒑2 ≃

𝜕𝑥

𝜕𝜃2𝜕𝑦

𝜕𝜃2

∆𝜃2

23

𝜃1

𝜃2

𝒑 =𝑥𝑦

𝑥

𝑦∆𝒑2

∆𝜃2


- 原理(𝜃1と𝜃2を少しだけ動かす)

そして𝜃1と𝜃2を少し動かしてみる．

それぞれの動きが十分小さいとすると

手先の変化は∆𝒑 ≃ ∆𝒑1 + ∆𝒑2となる．

すなわち

∆𝒑 ≃

𝜕𝑥

𝜕𝜃1𝜕𝑦

𝜕𝜃1

𝜕𝑥

𝜕𝜃2𝜕𝑦

𝜕𝜃2

∆𝜃1∆𝜃2

これを∆𝒑 ≃ 𝐽∆𝜽と書く．

この 𝐽をヤコビ行列（ヤコビアン）と呼ぶ．24

𝜃1

𝜃2

𝒑 =𝑥𝑦

𝑥

𝑦 ∆𝒑1

∆𝜃1

∆𝒑2

∆𝜃2


- 分解運動（速度）制御

逆に∆𝒑が与えられたら関節角はどうなるか？

それぞれの動きが十分小さいとすると∆𝒑 = 𝐽∆𝜽

これを解くと∆𝜽 = 𝐽−1∆𝒑

つまり動きが小さければ任意の場所(方向)に狙って動かせるということ

これを微小時間で考えると

𝝎 =𝒅𝜽

𝑑𝑡= 𝐽−1

𝒅𝒑

𝑑𝑡= 𝑱−𝟏𝒗

これを分解運動速度制御と呼ぶ．25

𝜃1

𝜃2

𝒑 =𝑥𝑦

𝑥

𝑦 ∆𝒑

∆𝜃1

∆𝜃2


- 逆運動学解の求め方

(0) まず適当な関節角𝜽0 =𝜃0,1𝜃0,2

を開始状態とする．

(1) 𝜽𝑖の順運動学解𝒑𝑖を求める．

(2) 𝒑𝑖が𝒑𝑇に十分一致すれば終了．

十分近ければその差を∆𝒑𝑖とし，離れている場合は

その方向の微小ステップを∆𝒑𝑖とする．

(3) ∆𝜽 = 𝐽−1∆𝒑を計算し関節角に加え，𝜽𝑖+1とする．

(4) 終了するまで(1)から(3)を繰り返す．

終了時点での𝜽𝑖が逆運動学解𝜽𝑇となっている．

26

𝜃0,1

𝜃0,2

𝒑0

𝑥

𝑦

∆𝒑

𝒑𝑇

∆𝜃2

∆𝜃1

𝜃𝑇,2

𝜃𝑇,1

目標𝒑𝑇が与えられたら関節角はどうなるか？

順運動学とヤコビ行列があれば逆運動学は数値的に解くことができる．


- ニュートン法としてのまとめ

関数𝑓 𝑥 の𝑥0回りでのテーラー展開を考える．

𝑓 𝑥 = 𝑓 𝑥0 + 𝑓′ 𝑥0 𝑥 − 𝑥0 ⋯

近似的に𝑓 𝑥 = 0とする𝑥1を求める．

0 = 𝑓 𝑥0 + 𝑓′ 𝑥0 𝑥 − 𝑥0

これを解いて

𝑥1 = 𝑥0 − Τ𝑓 𝑥0 𝑓′ 𝑥1

これを繰り返すことで近似解を

求めることができる．

つまり微分を使うと非線形問題が簡単に解ける

27


- 微分の復習

偏微分：着目しているパラメタ以外は変化ないとしたときの、そのパラメタについての微分。1変数の微分で計算できる。

良く使う微分の公式◼ 積の微分

◼ 合成関数の微分

28


- 積の微分

積の微分

𝑓 𝒙 = 𝑓1 𝒙 𝑓2 𝒙 のとき𝜕𝑓

𝜕𝒙=

𝜕𝑓1

𝜕𝒙𝑓2 + 𝑓1

𝜕𝑓2

𝜕𝒙

とりわけ1つのパラメタが1つの部分にしか関係していない場合，つまり

𝑓 𝒙 = 𝑓1 𝑥1 𝑓2 𝑥2 ⋯𝑓𝑖 𝑥𝑖 ⋯𝑓𝑛 𝑥𝑛 のとき

𝜕𝑓

𝜕𝑥𝑖= 𝑓1𝑓2⋯

𝜕𝑓𝑖

𝜕𝑥𝑖⋯𝑓𝑛

なぜなら𝜕𝑓𝑗

𝜕𝑥𝑖= 0 (𝑖 ≠ 𝑗) だから

29


- 合成関数の微分

合成関数の微分

𝑓 𝑥 = 𝑓 𝑔 𝑥 のとき

𝜕𝑓

𝜕𝑥=

𝜕𝑓

𝜕𝑔

𝜕𝑔

𝜕𝑥

たとえば手先のイメージヤコビアンを考える場合，

アームから手先の位置：𝒑 = 𝒈 𝜽

手先からカメラ画像への写像：𝒖 = 𝒇 𝒑

このような場合でも，それぞれ別々にヤコビ行列を計算しておいて，掛ければよいことが分かる．

30


- 3自由度アームのヤコビアン(求め方1)

順運動学の式を素直に偏微分するbase𝑧wrist = 𝑙𝑏 + 𝑙1 + 𝑙2 cos 𝜃2 + 𝑙3 cos 𝜃2 + 𝜃3base𝑥wrist =

L1𝑥hand cos 𝜃1 = 𝑙2 sin 𝜃2 + 𝑙3sin 𝜃2 + 𝜃3 cos 𝜃1base𝑦wrist =

L1𝑥hand sin 𝜃1 = 𝑙2 sin 𝜃2 + 𝑙3sin 𝜃2 + 𝜃3 sin 𝜃1

where L1𝑥wrist = 𝑙2 sin 𝜃2 + 𝑙3 sin 𝜃2 + 𝜃3

31

𝑙2𝑙3

𝑙1

𝑙𝑏


- 𝑥についての計算

𝑥 = 𝑙2 sin 𝜃2 + 𝑙3 sin 𝜃2 + 𝜃3 cos 𝜃1

𝜕𝑥

𝜕𝜃1= − 𝑙2 sin 𝜃2 + 𝑙3 sin 𝜃2 + 𝜃3 sin𝜃1

𝜕𝑥

𝜕𝜃2= 𝑙2 cos 𝜃2 + 𝑙3 cos 𝜃2 + 𝜃3 cos 𝜃1

𝜕𝑥

𝜕𝜃3= 𝑙3 cos 𝜃2 + 𝜃3 cos 𝜃1

32


- 𝑦についての計算

𝑦 = 𝑙2 sin 𝜃2 + 𝑙3 sin 𝜃2 + 𝜃3 sin 𝜃1

𝜕𝑦

𝜕𝜃1= 𝑙2 sin 𝜃2 + 𝑙3 sin 𝜃2 + 𝜃3 cos 𝜃1

𝜕𝑦

𝜕𝜃2= 𝑙2 cos 𝜃2 + 𝑙3 cos 𝜃2 + 𝜃3 sin𝜃1

𝜕𝑦

𝜕𝜃3= 𝑙3 cos 𝜃2 + 𝜃3 sin 𝜃1

33


- 𝑧についての計算

𝑧 = 𝑙𝑏 + 𝑙1 + 𝑙2 cos 𝜃2 + 𝑙3 cos 𝜃2 + 𝜃3

𝜕𝑧

𝜕𝜃1= 0

𝜕𝑧

𝜕𝜃2= −𝑙2 sin 𝜃2 − 𝑙3 sin 𝜃2 + 𝜃3

𝜕𝑧

𝜕𝜃3= −𝑙3 sin 𝜃2 + 𝜃3

34


- 求め方1(まとめ)

ቌ− 𝑙2 sin 𝜃2 + 𝑙3 sin 𝜃2 + 𝜃3 sin 𝜃1𝑙2 sin 𝜃2 + 𝑙3 sin 𝜃2 + 𝜃3 cos 𝜃1

0

𝑙2 cos 𝜃2 + 𝑙3 cos 𝜃2 + 𝜃3 cos 𝜃1𝑙2 cos 𝜃2 + 𝑙3 cos 𝜃2 + 𝜃3 sin 𝜃1−𝑙2 sin 𝜃2 − 𝑙3 sin 𝜃2 + 𝜃3

ቍ

𝑙3 cos 𝜃2 + 𝜃3 cos 𝜃1𝑙2 cos 𝜃2 + 𝑙3 cos 𝜃2 + 𝜃3 sin 𝜃1

−𝑙3 sin 𝜃2 + 𝜃3

35


- 3自由度アームのヤコビアンの直感的意味

𝜕 0𝒑wrist

𝜕𝜽= 𝒓1 × 𝒍1 𝒓2 × 𝒍2 𝒓3 × 𝒍3

36


課題2（詳細は別紙）

簡単なアーム（２関節）について，

順運動学

逆運動学

ヤコビ行列

分解運動速度制御

を求める．

締切 5/7

37


- 課題2のやり方

提出は回答のpdfファイル元は手書き，パワポ，ワード，etc．なんでもOK.

ファイルを添付して[email protected]

メールの表題は「ロボットソフトウェア特論:課題2」

〆切，5月7日(火)

38

ロボットソフトウェア特論 (3)suehiro/class2019/presen/lecture...2019年度前学期...

Documents