7/15/2014

1

東北文化学園大学 科学技術学部 知能情報システム学科

費 仙鳳

1

ロボットの概要数学的基礎

‣ 座標変換‣ 同次変換‣ オイラー角‣ ロールピッチヨウ角‣ 座標系設定‣ リンクパラメータ

腕型ロボットの構造腕型ロボットの順運動学腕型ロボットの逆運動学腕型ロボットのヤコビアン・速度・特異姿勢

2

3 4

7/15/2014

2

5

三角関数

ベクトルと行列

並進変換と回転変換

同次変換行列の導入

オイラー角（Z-Y-Z）

ロール･ピッチ･ヨウ角

6

三角関数‣ cos, sin, tan, acos, asin, atanの意味、グラフ‣ メ、ジャーな角度の値：π(180deg)、π/2(90deg)、π/3(60deg)、2π/3(120deg)、π/6(30deg)

‣ 微分、積分

ベクトル‣ 内積‣ 外積‣ 単位ベクトル‣ 座標系変換

行列‣ 3ｘ3、4ｘ4演算‣ 転置‣ 逆行列

7

対辺

隣辺

斜辺

8

7/15/2014

3

9 10

11

大きさと向きで定まる量をベクトルという縦ベクトル

横ベクトル: 転置

ベクトルの長さ

単位ベクトル（i、j、k）を使う

i = (1, 0, 0)T12

7/15/2014

4

座標や状態量を表す‣２次元位置

‣２次元位置＋姿勢

‣３次元位置

‣３次元位置＋姿勢

13

加算

スカラ倍

加算の可換律

14

定義

成分表示

15

定義

16

7/15/2014

5

成分表示

特徴

17

に対して

18

例

用途：‣座標変換(回転)や関節の角度とロボットの姿勢の関係を表すなどに使用されます。

19

行列の和

スカラ倍

20

7/15/2014

6

行列の積

21

行列の積

22

行列の積の特徴

‣ 1.

‣ 2. にたして

は成立します。

23

行列の転置

24

7/15/2014

7

逆行列

逆行列は、ロボット工学においては、「逆変換」の意味で用いられます。ある座標系から別の座標系に行列Ｒで変換できる場合に、Ｒの逆行列を用いると逆の座標変換ができます。

25

行列によるベクトルの変換

26

点Q ( X,Y ) は 点P ( x,y ) を β 回転させたものである。

回転行列

27

点の並進変換（ベクトルの加算）

点の回転変換（ベクトルの一次変換）

座標の並進変換

座標の回転変換

28

7/15/2014

8

！一次変換にはならない

29

点が原点周りにθだけ回転すると

平面回転変換行列

逆方向の回転変換行列

30

点の並進変換（ベクトルの加算）

点の回転変換（ベクトルの一次変換）

座標の並進変換

座標の回転変換

31 32

7/15/2014

9

X

Y

X

Y

1. 回転

= +

地面に固定された座標系

ロポットの座標系

X

Y

2. 平行移動

33

X

Y

U

V

BpAp

q

34

ベクトルの写像の復習

35 36

7/15/2014

10

37

座標軸AとBの関係を入れ替えると

すなわち

38

平行移動と回転‣以上のことをまとめると、二つの座標系の間の座標変換は次の式で得られることがわかります

同次変換‣ロボットで座標の関係を扱うとき、多くの座標系がでてきて、変換ごとにこのように書くと面倒です。 そこで、この式を行列とベクトルの乗算だけに書き換えます。

39

まとめると

回転＋平行という一般的な座標変換

‣ 本来の座標ベクトルの次数を一つ増やし、「1」を追加する。‣ 回転変換の行列の行と列を一つ増やし、増えた列に平行移動のベクトルと「１」を追加し、のこりは０をうめる。

40

7/15/2014

11

「 R で回転、 q で移動」という手順を逆変換するには、「 q だけ戻す、 R で逆回転」という手順になります。

‣結合して

‣「あとでする変換を前に書く」ことです

41

？

42

3次元空間での姿勢は自由度３‣＝3個のパラメータ

ところが、回転行列は３×３＝９個のパラメータ‣⇒この9個は独立ではない

‣各ベクトルは単位ベクトル

‣各ベクトルは直交

‣合計６個の拘束条件を含んでいる

よって、実質のパラメータ数＝９－６＝３個

43

いずれも任意の姿勢を3つの回転角変数で表す

以下の2つの表現法が知られている‣①オイラー角

‣②ロール・ピッチ・ヨウ角

44

7/15/2014

12

①z回りにΦ回転してy’を作る

②ｙ’回りにθ回転してz'' を作る

③ｚ''回りにψ回転してΣC を作る

略記法 45

よって、

46

も簡素な要素として、R33に着目すると

?47 48

7/15/2014

13

49 50

51 52

7/15/2014

14

53 54

55

回転の順序は重要で、順序を変えると異なる姿勢となる（オイラー角、ロール･ピッチ･ヨウ角共に）。

例 X→Y→Zの順に、φ、θ、ψ回転するとすれば

となり、前式と全く異なることがわかる。

56

7/15/2014

15

57 58

59

文字式の足し算・引き算‣ (例)3x + 2x = 5x 4a - a = 3a 5x2 + 6x2 = 11x2

2a + 3b は計算できない。 4x + 3x2 も計算できない

文字式の掛け算・割り算‣ 文字式を表すときには ×が省略されます。‣ 3a × 5b = 3×a×5×b = 15ab 12a÷2=12×a×12=6a

同類項・・・文字の部分が全く同じ項を同類項という。‣ (例)－9xと8x 5abcと12abc 3x2と-8x2

※累乗の指数まで完全に同じでないと同類項ではない‣ 同類項をまとめる・・・分配法則の逆を使うと同類項をまとめることができる

‣ 分配法則 (a+b)x = ax+bx分配法則の逆 ax+bx = (a+b)x

60

7/15/2014

16

等式の両辺に同じ数を足しても、等式が成り立ちます。

等式の両辺から同じ数をひいても、等式が成り立ちます。

等式の両辺に同じ数をかけても、等式が成り立つます。

等式の両辺を同じ数で割っても、等式が成り立ちます。

移項して解く‣ 移項とは、等式の反対側へ、項を移動することを言います。

‣ xに係数が付いている場合

61 62

空間での‣位置＝3自由度（ｘ，ｙ，ｚ）

‣姿勢＝3自由度(φ,θ,ψ)

‣ ⇒合計6自由度（≒６関節）

必要 小限以上の余分の自由度＝「冗長自由度」

63

ロボットの座標系

リンク座標系‣①座標系番号のルール

‣②Ｚ軸の設定ルール

‣③Ｘ軸の設定ルール

‣④Ｙ軸の設定ルール

‣⑤ベース座標系の設定ルール

座標系設定手順のまとめ

64

7/15/2014

17

65

よく用いられる設定ルール

Denavit-hartenbergの表記法（略してD-H法）

66

ベースをリンク番号０とし、手先に向かって順にリンク番号１～n をつける

リンクi-1 とiとの連結部を関節iとする

座標系番号＝関節番号

（ただし、座標系０を固定の基準座標系とする）

67

関節軸の軸方向をＺ軸とする

正方向は先端を向く方向

（どちらでも良いとき

は全体で統一する)

68

7/15/2014

18

69

あまり使わないので、省略する

もし記入するなら、Z・X軸に対して右手系で設定

70

座標系１における関節変数（角度)がゼロのときの座標系をベース座標系Σ0とする（原点共通）

71 72

7/15/2014

19

73 74

隣接する２座標系間の関係を４つのパラメータで表わす

75

４つのパラメータのうち、‣３つは固定値（リンクの構造に依存）

‣ 1つは変数（１自由度)

関節変数‣回転関節＝

‣直動関節＝

76

7/15/2014

20

77

前記パラメータは、座標系の並進・回転を表わすから、これらを連続的に演算すると隣同士の関係を得る

重要78

79

ロボットの各リンク機構を記号化

各リンクに座標系を設定（DH法）

リンクパラメータ表の作成

各座標系間の関係 を計算

アーム全体の同次変換行列 を計算

を与え、手先の位置・姿勢を計算

80

7/15/2014

21

81 82

83 84

7/15/2014

22

85 86

87

幾何学的解法(2D)

代数的解法(3D)

数値的な方法(ヤコビアン)

88

7/15/2014

23

３関節平面の逆運動学

式（11.1）

89

式（11.1）より

ただし、

それぞれ２乗して加えると、

変数はＣ1、Ｓ1に含まれるθ1のみ。定数項をまとめると

=1

90

この形の式を解くため、図のような直角三角形を考える。

これを先の式に代入すると

91

であるから

であるから、分母を払うと

また、 を代入すると

ただし、

92

7/15/2014

24

ただし、

式(11.1)にもどって、

93

両辺を2乗してくわえると、

と同様に解くと、

は、 が既知であることから

ただし、

94

95

をまとめために、左から を乗じると

……

逆運動学では次のような式がよく出てくる

96

7/15/2014

25

97

1

倍角の公式より、

また、 であるから

ここで、

98

これを、 に代入すると

について整理すると、

２次方程式の解より、

9910

0

7/15/2014

26

101

①z回りにΦ回転してy’を作る

②ｙ’回りにθ回転してz'' を作る

③ｚ''回りにψ回転してΣC を作る

Euler角による姿勢表示は次のように与えられている。

そこで、

左から を乗じると、

ここで、

102

の時には、第(2,3) 要素に着目して、

よっで、

さらに、第(1,3)、(3,3) 要素、および、第(2,1)、(2,2) 要素より、

の時には、であるから、それに従って、

103

104

7/15/2014

27

RPY角による姿勢表示は次のように与えられている。

そこで、

左から を乗じると、

ここで、

105

の時には、第(2,1) 要素に着目して、

よっで、

さらに、第(1,1)、(3,1) 要素、および、第(2,2)、(2,3) 要素より、

の時には、であるから、それに従って、

106

107

i ai αi di θi1 0 0 d1 θ12 a1 0 0 θ23 a2 180 d3 0

関節変位は

で、手先についてはその位置だけに着目して、座標系３の原点とします.

108

7/15/2014

28

仮の を定める。

順運動学を用いて を得る。

目的とする との差を求め、それをもとに を微修正する

の誤差が十分小さくなるまで繰り返す。

109

110

111

関節変位

手先の位置

ヤコビアン

112

7/15/2014

29

オイラー角の時間微分で表す方法

行の数＝マニピュレータの運動の自由度列の数＝マニピュレータの関節数

関節速度手先速度

113

関節変位

手先の位置

ヤコビアン

x,y,z軸の速度成分

オイラー角の角速度

114

115

角速度ベクトル

ヤコビアン

(3-1)速度ベクトル＝位置ベクトルで表わされる点の時間的変化率‣位置ベクトル

‣速度ベクトル

116

7/15/2014

30

117

の への垂線

時間後の回転角

よって

で の向きは 平面に対して垂直

すなわち、速度ベクトルはベクトル と の外積で表わされることを示している。

118

逆に

⇒問題は、ヤコビアンの逆行列が存在するかどうか

119

問題１：‣ 歯車列の駆動歯車Ｚ１が１分間に１，０００回転する時の被動歯車Ｚ３の毎分の回転数の求め方。ただし、Ｚ１の歯数を３０、Ｚ２の歯数を６０、Ｚ３歯数を１５０とする。

7/15/2014

31

問題２：‣ロボットを構成する要素は、大きく分けてどうのような四つですか？

ロボットの動力システム

ロボットのボディメカニズム

ロボットのセンサシステム

ロボットのコンピュータシステム

、

であるとき の値を求めよ。

方針： と

の両辺を平方して加え式を変形する。

０座標系をｚ軸回りに30°回転してできたA座標系で表わされた点が、Aｐ=［４３７Tのとき、これを元の０座標系で表わすとどのよう になるか］

7/15/2014

32

座標系設定

128

7/15/2014

33

129

①座標系の設定

リンクパラメータ表の作成

?θ1 θ2 ｄ3 θ4 が

関節変数

?13

0

131

i ai αi di θi1 0 0 0 θ12 l1 0 0 θ23 l2 0 0 0

132

座標

既知

7/15/2014

34

文字式の足し算・引き算‣ (例)3x + 2x = 5x 4a - a = 3a 5x2 + 6x2 = 11x2

2a + 3b は計算できない。 4x + 3x2 も計算できない

文字式の掛け算・割り算‣ 文字式を表すときには ×が省略されます。‣ 3a × 5b = 3×a×5×b = 15ab 12a÷2=12×a×12=6a

同類項・・・文字の部分が全く同じ項を同類項という。‣ (例)－9xと8x 5abcと12abc 3x2と-8x2

※累乗の指数まで完全に同じでないと同類項ではない‣ 同類項をまとめる・・・分配法則の逆を使うと同類項をまとめることができる

‣ 分配法則 (a+b)x = ax+bx分配法則の逆 ax+bx = (a+b)x

133

等式の両辺に同じ数を足しても、等式が成り立ちます。

等式の両辺から同じ数をひいても、等式が成り立ちます。

等式の両辺に同じ数をかけても、等式が成り立つます。

等式の両辺を同じ数で割っても、等式が成り立ちます。

移項して解く‣ 移項とは、等式の反対側へ、項を移動することを言います。

‣ xに係数が付いている場合

134

135

左から を乗じると ここで、

136

第(1,4) 要素より、

第(2,4) 要素より、

上の式をそれぞれ２乗すると、

上の二つ式をたすと、

7/15/2014

35

ので、

ので、

平面マニピュレータで、先端がx軸上を1.0m/sで移動している。θ2→0 のとき、各関節の角速度を求めよ。

平面マニピュレータで、先端がx軸上を1.0m/sで移動している。θ2→0 のとき、各関節の角速度を求めよ。