東北文化学園大学 科学技術学部知能情報システム学科 費 仙鳳 ロボットの概要 数学的基礎 座標変換 同次変換 オイラー角 ロールピッチヨウ角 座標系設定 リンクパラメータ 腕型ロボットの構造 腕型ロボットの順運動学 腕型ロボットの逆運動学 腕型ロボットのヤコビアン 速度 特異姿勢 1 2 3 4 1
三角関数 ベクトルと行列 並進変換と回転変換 同次変換行列の導入 オイラー角 (Z-Y-Z) ロール ピッチ ヨウ角 5 6 三角関数 cos, sin, tan, acos, asin, atan の意味 グラフ メ ジャーな角度の値 :π(180deg) π/2(90deg) π/3(60deg) 2π/3(120deg) π/6(30deg) 微分 積分 ベクトル 内積 外積 単位ベクトル 座標系変換 行列 3x3 4x4 演算 転置 逆行列 斜辺 対辺 7 隣辺 8 2
9 10 大きさと向きで定まる量をベクトルという 縦ベクトル 横ベクトル : 転置 ベクトルの長さ 単位ベクトル (i j k) を使う 11 i = (1, 0, 0) T 12 3
座標や状態量を表す 2 次元位置 2 次元位置 + 姿勢 加算 スカラ倍 3 次元位置 3 次元位置 + 姿勢 加算の可換律 13 14 定義 定義 成分表示 15 16 4
成分表示 特徴 に対して 17 18 例 行列の和 用途 : 座標変換 ( 回転 ) や関節の角度とロボットの姿勢の関係を表すなどに使用されます スカラ倍 19 20 5
行列の積 行列の積 21 22 行列の積の特徴 行列の転置 1. 2. にたして は成立します 23 24 6
逆行列 行列によるベクトルの変換 逆行列は ロボット工学においては 逆変換 の意味で用いられます ある座標系から別の座標系に行列 R で変換できる場合に R の逆行列を用いると逆の座標変換ができます 25 26 点 Q ( X,Y ) は点 P ( x,y ) を β 回転させたものである 回転行列 点の並進変換 ( ベクトルの加算 ) 点の回転変換 ( ベクトルの一次変換 ) 座標の並進変換 座標の回転変換 27 28 7
点が原点周りに θ だけ回転すると 平面回転変換行列! 一次変換にはならない 逆方向の回転変換行列 29 30 点の並進変換 ( ベクトルの加算 ) 点の回転変換 ( ベクトルの一次変換 ) 座標の並進変換 座標の回転変換 31 32 8
Y Y 1. 回転 = + Y Y A p V q X B p U X X X 2. 平行移動 地面に固定された座標系 ロポットの座標系 33 34 ベクトルの写像の復習 35 36 9
座標軸 A と B の関係を入れ替えると すなわち 37 38 平行移動と回転 以上のことをまとめると 二つの座標系の間の座標変換は次の式で得られることがわかります 同次変換 ロボットで座標の関係を扱うとき 多くの座標系がでてきて 変換ごとにこのように書くと面倒です そこで この式を行列とベクトルの乗算だけに書き換えます まとめると 回転 + 平行という一般的な座標変換 本来の座標ベクトルの次数を一つ増やし 1 を追加する 回転変換の行列の行と列を一つ増やし 増えた列に平行移動のベクトルと 1 を追加し のこりは 0 をうめる 39 40 10
R で回転 q で移動 という手順を逆変換するには q だけ戻す R で逆回転 という手順になります? 結合して あとでする変換を前に書く ことです 41 42 3 次元空間での姿勢は自由度 3 =3 個のパラメータ ところが 回転行列は 3 3=9 個のパラメータ この 9 個は独立ではない 各ベクトルは単位ベクトル 各ベクトルは直交 合計 6 個の拘束条件を含んでいる いずれも任意の姿勢を 3 つの回転角変数で表す 以下の 2 つの表現法が知られている 1 オイラー角 2 ロール ピッチ ヨウ角 よって 実質のパラメータ数 =9-6=3 個 43 44 11
よって 1z 回りに Φ 回転して y を作る 2y 回りに θ 回転して z'' を作る 3z'' 回りに ψ 回転して ΣC を作る 略記法 45 46 最も簡素な要素として R33 に着目すると? 47 48 12
49 50 51 52 13
53 54 回転の順序は重要で 順序を変えると異なる姿勢となる ( オイラー角 ロール ピッチ ヨウ角共に ) 例 X Y Z の順に φ θ ψ 回転するとすれば となり 前式と全く異なることがわかる 55 56 14
57 58 文字式の足し算 引き算 ( 例 ) 3x + 2x = 5x 4a - a = 3a 5x 2 + 6x 2 = 11x 2 2a + 3b は計算できない 4x + 3x 2 も計算できない 文字式の掛け算 割り算 文字式を表すときには が省略されます 3a 5b = 3 a 5 b = 15ab 12a 2=12 a 12=6a 同類項 文字の部分が全く同じ項を同類項という ( 例 )-9xと8x 5abcと12abc 3x 2 と-8x 2 累乗の指数まで完全に同じでないと同類項ではない 同類項をまとめる 分配法則の逆を使うと同類項をまとめることができる 分配法則 (a+b)x = ax+bx 分配法則の逆 ax+bx = (a+b)x 59 60 15
等式の両辺に同じ数を足しても 等式が成り立ちます 等式の両辺から同じ数をひいても 等式が成り立ちます 等式の両辺に同じ数をかけても 等式が成り立つます 等式の両辺を同じ数で割っても 等式が成り立ちます 移項して解く 移項とは 等式の反対側へ 項を移動することを言います x に係数が付いている場合 61 62 空間での 位置 =3 自由度 (x,y,z) 姿勢 =3 自由度 (φ,θ,ψ) 合計 6 自由度 ( 6 関節 ) 必要最小限以上の余分の自由度 = 冗長自由度 ロボットの座標系 リンク座標系 1 座標系番号のルール 2Z 軸の設定ルール 3X 軸の設定ルール 4Y 軸の設定ルール 5ベース座標系の設定ルール 座標系設定手順のまとめ 63 64 16
よく用いられる設定ルール Denavit-hartenberg の表記法 ( 略して D-H 法 ) 65 66 ベースをリンク番号 0 とし 手先に向かって順にリンク番号 1~n をつける リンク i-1 と i との連結部を関節 i とする 座標系番号 = 関節番号 ( ただし 座標系 0 を固定の基準座標系とする ) 関節軸の軸方向を Z 軸とする 正方向は先端を向く方向 ( どちらでも良いときは全体で統一する ) 67 68 17
あまり使わないので 省略する もし記入するなら Z X 軸に対して右手系で設定 69 70 座標系 1 における関節変数 ( 角度 ) がゼロのときの座標系をベース座標系 Σ0 とする ( 原点共通 ) 71 72 18
73 74 隣接する 2 座標系間の関係を 4 つのパラメータで表わす 4 つのパラメータのうち 3 つは固定値 ( リンクの構造に依存 ) 1 つは変数 (1 自由度 ) 関節変数 回転関節 = 直動関節 = 75 76 19
前記パラメータは 座標系の並進 回転を表わすから これらを連続的に演算すると隣同士の関係を得る 重要 77 78 ロボットの各リンク機構を記号化 各リンクに座標系を設定 (DH 法 ) リンクパラメータ表の作成 各座標系間の関係 を計算 アーム全体の同次変換行列 を計算 を与え 手先の位置 姿勢を計算 79 80 20
81 82 83 84 21
85 86 幾何学的解法 (2D) 代数的解法 (3D) 数値的な方法 ( ヤコビアン ) 87 88 22
3 関節平面の逆運動学 式 (11.1) より 式 (11.1) ただし それぞれ 2 乗して加えると =1 変数は C1 S1 に含まれる θ1 のみ 定数項をまとめると 89 90 この形の式を解くため 図のような直角三角形を考える であるから これを先の式に代入すると であるから 分母を払うと また を代入すると ただし 91 92 23
両辺を 2 乗してくわえると と同様に解くと ただし 式 (11.1) にもどって ただし は が既知であることから 93 94 逆運動学では次のような式がよく出てくる をまとめために 左から を乗じると 95 96 24
倍角の公式より 1 また であるから ここで 97 98 これを に代入すると について整理すると 2 次方程式の解より 99 10 0 25
Euler 角による姿勢表示は次のように与えられている そこで 左から を乗じると 1z 回りに Φ 回転して y を作る 2y 回りに θ 回転して z'' を作る 3z'' 回りに ψ 回転して ΣC を作る ここで 10 1 10 2 の時には 第 (2,3) 要素に着目して よっで さらに 第 (1,3) (3,3) 要素 および 第 (2,1) (2,2) 要素より の時には であるから それに従って 10 3 10 4 26
RPY 角による姿勢表示は次のように与えられている の時には 第 (2,1) 要素に着目して そこで よっで 左から を乗じると さらに 第 (1,1) (3,1) 要素 および 第 (2,2) (2,3) 要素より の時には であるから それに従って ここで 10 5 10 6 i ai αi di θi 1 0 0 d1 θ1 2 a1 0 0 θ2 3 a2 180 d3 0 関節変位は で 手先についてはその位置だけに着目して 座標系 3 の原点とします. 10 7 10 8 27
仮のを定める 順運動学を用いてを得る 目的とするとの差を求め それをもとにを微修正する の誤差が十分小さくなるまで繰り返す 10 9 11 0 関節変位 手先の位置 ヤコビアン 11 1 11 2 28
オイラー角の時間微分で表す方法 関節変位 手先の位置 ヤコビアン 手先速度関節速度 行の数 = マニピュレータの運動の自由度 列の数 = マニピュレータの関節数 x,y,z 軸の速度成分 オイラー角の角速度 11 3 11 4 角速度ベクトル ヤコビアン (3-1) 速度ベクトル = 位置ベクトルで表わされる点の時間的変化率 位置ベクトル 速度ベクトル 11 5 11 6 29
のへの垂線 時間後の回転角 よって で の向きは 平面に対して垂直 すなわち 速度ベクトルはベクトル と の外 積で表わされることを示している 11 7 11 8 逆に 問題は ヤコビアンの逆行列が存在するかどうか 問題 1: 歯車列の駆動歯車 Z1 が 1 分間に 1,000 回転する時の被動歯車 Z3 の毎分の回転数の求め方 ただし Z1 の歯数を 30 Z 2 の歯数を 60 Z3 歯数を 150 とする 11 9 30
問題 2: ロボットを構成する要素は 大きく分けてどうのような四つですか? であるとき の値を求めよ ロボットの動力システム ロボットのボディメカニズム ロボットのセンサシステム ロボットのコンピュータシステム 方針 : と の両辺を平方して加え式を変形する 0 座標系を z 軸回りに 30 回転してできた A 座標系で表わされた点が Ap=[437T のとき これを元の 0 座標系で表わすとどのようになるか ] 31
座標系設定 12 8 32
1 座標系の設定 リンクパラメータ表の作成? θ1 θ2 d3 θ4 が関節変数? 13 12 9 0 i ai αi di θi 1 0 0 0 θ1 2 l1 0 0 θ2 3 l2 0 0 0 座標 既知 13 1 13 2 33
文字式の足し算 引き算 ( 例 ) 3x + 2x = 5x 4a - a = 3a 5x 2 + 6x 2 = 11x 2 2a + 3b は計算できない 4x + 3x 2 も計算できない 文字式の掛け算 割り算 文字式を表すときには が省略されます 3a 5b = 3 a 5 b = 15ab 12a 2=12 a 12=6a 同類項 文字の部分が全く同じ項を同類項という ( 例 )-9xと8x 5abcと12abc 3x 2 と-8x 2 累乗の指数まで完全に同じでないと同類項ではない 同類項をまとめる 分配法則の逆を使うと同類項をまとめることができる 分配法則 (a+b)x = ax+bx 分配法則の逆 ax+bx = (a+b)x 等式の両辺に同じ数を足しても 等式が成り立ちます 等式の両辺から同じ数をひいても 等式が成り立ちます 等式の両辺に同じ数をかけても 等式が成り立つます 等式の両辺を同じ数で割っても 等式が成り立ちます 移項して解く 移項とは 等式の反対側へ 項を移動することを言います x に係数が付いている場合 13 3 13 4 第 (1,4) 要素より 第 (2,4) 要素より 左からを乗じるとここで 上の式をそれぞれ 2 乗すると 上の二つ式をたすと 13 5 13 6 34
ので 平面マニピュレータで 先端が x 軸上を 1.0m/s で移動している θ2 0 のとき 各関節の角速度を求めよ ので 平面マニピュレータで 先端が x 軸上を 1.0m/s で移動している θ2 0 のとき 各関節の角速度を求めよ 35