松尾孝美＠大分大学工学部福祉環境工学科 ·...

福祉ロボット工学講義資料その４

担当松尾孝美＠大分大学工学部福祉環境工学科

1

順運動学，仮想仕事の原理，逆運動学

１．アームの先端位置に関する順運動学

ホモジニアス変換行列を用いることにより，ハンド座標系からワールド座標系に変換し

たとき，手先の位置をワールド座標系の成分に変換できる．これホモジニアス変換行列は

各関節角を用いることにより表現される．これから，手先の座標が関節角を用いて表され

ることになる．これが順運動学である．導出例は，テキスト p.24から p.25の 2.4.3を参照

すること．

２．アーム先端の速度・加速度に関する順運動学：ヤコビ行列

●テキスト p.28(2.34)式の説明：2変数 , の関数 ),(),,( yx を考える．ただし， ,

は時間 tの関数とすると， yx, は，また時間の関数である．

)(sin)())(),((

)(cos)())(),((

tttty

ttttx

これらの時間微分は次式のようになる．

dt

tdy

dt

tdytty

dt

d

dt

tdy

dt

tdx

dt

tdxttx

dt

d

dt

tdx

)(),()(),())(),((

)(

)(),()(),())(),((

)(

これをベクトル表現すると，次式のようになる．

dt

ddt

d

J

dt

ddt

d

yy

xx

dt

dydt

dx

),(

上式は位置座標における速度と角度座標における速度（角速度）の関係を表している．行

列 Jをヤコビ(Jacobi)行列という．ここで，ベクトルを

dt

tddt

td

t

dt

tdydt

tdx

t

t

tt

ty

txt

)(

)(

)(,)(

)(

)(

)(

)()(,

)(

)()(

qr

qr

と定義すると，

)()()( tJt qqr

さらに，もう一度時間で微分すると，加速度どうしの関係が出てくる．

)()()()()( tJtJt qqqqr

課題： Jがどのような形になるか書き出せ．



2

微小時間 dt が経過した際の )(),( tytx の微小変化量を )(),( tdytdx および )(),( tt の微小

変化量を dd , とすると，単位時間当りの変化率がdt

dy

dt

dx, であることから，次式のように

書ける．

dtdt

tdtddt

dt

tdtd

dtdt

tdytdydt

dt

tdxtdx

)()(,

)()(

)()(,

)()(

したがって，次式が成り立つ．

dy

dy

dy

dx

dx

dx

),(),(

),(),(

ここで，ベクトルを

)(

)()(,

)(

)()(

td

tdtd

tdy

tdxtd

qr

とおくと，次式が成立する．

)()()( tdJtd qqr

とくに，

)(sin)())(),((

)(cos)())(),((

tttty

ttttx

(◎)

のとき，偏微分は一方の変数を定数と見た微分であることから次式のようになる．

cos),(

,sin),(

sin),(

,cos),(

yy

xx

課題：(◎)式で Jがどのような形になるか計算せよ．

課題：図の 2自由度ロボットアームに対して，ヤコビ行列を計算せよ．

手先 ),( pp yx

1

)(2 t

x

y

リンク長さ1l

2l



3

３．仮想仕事の原理とロボットアームの力とトルクの関係

ここでは一般の仮想仕事の原理について紹介し，さらにロボットアーム先端で発生する

力・トルクと関節トルクの関係について述べる．

仮想仕事の原理：

物体が力の平衡状態にあるための必要十分条件は，あらゆる方向の仮想位置について仮

想仕事がゼロになることである．つまり，ロボットアームが外部になした仕事と外部から

受けた仕事は等しい．ここで，仮想変位とは，任意の微小変化分（時間は関係ない）で，

仮想仕事は仮想変位による仕事をいう．

ロボットアームにおいて，先端での力とモーメント（付録参照）をベクトルで f

方向角モーメント



方向力

方向力

方向力

ｙ

ｘ

zz

y

x

z

y

x

z

y

x

m

m

m

f

f

f

f

とし，仮想変位をベクトルで p

方向角変位

方向角変位

方向角変位

方向変位

方向変位

方向変位

ｙ

ｘ

zz

y

x

z

y

x

z

y

x

p

p

p

p

とし，n個の関節でのトルクをベクトルで τ

のトルク関節

のトルク関節

のトルク関節

nn

2

1

2

1

τ

とし，関節の仮想変位をベクトルで qδ

の角変位関節

の角変位関節

の角変位関節

nq

q

q

n

2

1

2

1

q



4

とすると，これらの仕事はひとしくなることから，次式が成り立つ．

zzyyxxzxzyyxxnn

n

n

z

y

x

z

y

x

zyxzyx

TT

mmmfffqq

q

qp

p

p

mmmfff

11

1

1

qτpf

ただし，肩字Tはベクトルや行列の転置（行と列を入れかえること）を意味している．

一方，仮想変位は微小変位を考えていることとヤコビ行列の性質から，次式が成り立つ．

qqp )(J

これから次式が成り立つことがわかる．

fqτ

qfτ

T

TT

J

J

)(

)(

上式はアーム先端での力とモーメントと各関節トルクの関係を表している．

例題：図の２自由度ロボットアームがベース座標系(OXY)で f の力を外界に及ぼしているも

のとする．関節変位ベクトルと手先座標系の間のヤコビ行列を J とし，このときの f に等価

な関節駆動トルク τを求めよう．

２自由度ロボットアームのヤコビ行列は前述の課題より求めた．次のような式になる．

object

y

x

f

ff

2

1

1l

2l

1

2

O X

Y



5

)cos(),sin(,cos,sin 211221121111

12212211

12212211

CSCSwhere

ClClCl

SlSlSlJ

(◆)

f に等価な τは次式で計算できる．

fτTJ

４．逆運動学

ロボットアーム先端の位置・速度・加速度が与えられたときに，アームの関節の変位，

速度，加速度を求めることを逆運動学という．

●位置の逆運動学とニュートン法

リンク座標系が決まり，隣接するリンク座標系間のホモジニアス変換行列 Tii 1 が決まれば，

座標系 0 と n の関係はつぎのホモジニアス変換行列で表せる．

TTTT n

nn

11

2

0

1

0

0 をベース座標系， n をハンド座標系とみなせばよい．ホモジニアス変換行列 Tn

0は次式

のようになる．

1

00

0

0

LRT nn

n

このとき， Rn

0が手先の姿勢を表し， Ln

0が手先の位置を表すことになる． Rn

0が与えられれ

ば，姿勢はオイラー角で書き直すことができるので，結局，手先の位置，姿勢は６次元ベ

クトルで表せることになる．これをrとかくことにする．rが各関節の変位 niqi ,,2,1;

の関数であることから，次式のような一般的な式になる．

)(qfuncr

ただし，qは関節変位ベクトルであり，次式のように置いている．

nq

q

1

q

qからrを求めることを位置の順運動学といい，この逆を逆運動学といい，一般には上記の

逆関数を求めることを意味する．

)(1 rfuncq



6

希望の手先位置・姿勢にアームを追従させるためには，各関節をそれに対応する変位に制

御する要求から位置の逆運動学が必要になる．

テキスト p.30の 2.5.1では，幾何学的な関係より逆運動学を直接解いている．しかしな

がら，関節数（自由度）が増えると，このようなやり方は困難になるので，ニュートン法

による数値計算により関節角を求めるほうが実用的な方法といえる．前述のようにヤコビ

行列を用いた微小変位同士の関係は，次式のようになる．

qqr )(J

)(qJ の逆行列 1)( qJ が存在するとき，次式のようになる．

rqq 1)( J

ここで， rq , は微小変化量を表しているので，次式のように変位の差として近似すること

ができる．

ii

ii

rrr

qqq

1

1

これから次式が成立する．これがニュートン法の基礎式である．

)()( 1

1

1 iiiii J rrqqq

この式を用いて，次式の逆運動学を計算する手順を求める．

)( ** qfuncr

逆運動学では，*r が与えられたときに， *q を求める．ニュートン法の基礎式において，

*1*1 , rrqq ii

とおくと，次式のようになる．

)()( *

1

* iii J rrqqq

ニュートン法の計算手順はつぎのとおりである．

手順１）初期近似解 0q を適当に与える．ただし，この選定は収束性能に影響を与える．さ

らに，順運動学により， 0q に対応する 0r を計算する．

手順 2）次式により近似解1q を計算する．

)()( 0*

1

001 rrqqq J

さらに，順運動学により，1q に対応する

1r を計算する．

手順３）この手順をニュートン法の基礎式で， ,3,2,1i と繰り返すことにより，*q を数

値的に求めることができる．

ii

qq

lim*

●分解速度制御法

ニュートン法の基礎式を応用すると，アーム先端の初期状態の位置と姿勢ならびにアー



7

ム移動経路が与えられたときにも，その移動経路に対応する関節変位はニュートン法の基

礎式を用いて計算できる．これを分解速度制御法という．詳細はテキスト p.34 から p.36

の例題 2.4を参照すること．

●速度の逆運動学

手先の位置・姿勢の速度ベクトルと関節速度ベクトルには，次式の関係がある．

qqr )(J

q が与えられたときにrを求める問題を速度の順運動学といい，その逆を逆運動学という．

手先に適当なスピードを与えるためには各関節速度を制御しなけらばならないので，速度

の逆運動学が必要になる．ヤコビ行列が正則（逆行列が存在する，つまり行列式がゼロで

ない）ならば，速度の逆運動学の解は次式のようになる．

rqq 1)( J

ヤコビ行列が正則でない場合には，どのような関節速度q を選んでも，目標の手先速度rは

実現できないので，ヤコビ行列の行列式がゼロ，つまり，

0)(det qJ

となる位置・姿勢qを特異点といい，そのようなアームの位置形状を特異姿勢という．ヤコ

ビ行列式がゼロから遠いことが手先の操作のしやすさ（速度を自由に設定できるという意

味）につながることから，次式を可操作度と呼ぶ．

)(det qJw

課題： 2 自由度ロボットアームの特異点を(◆)式より求め，どのような特異姿勢になって

いるか図示せよ．

●加速度の逆運動学

加速度の逆運動学の解は次式のようになる．

))(()( 1 qqrqq JJ

手先の力を制御するためには手先の加速度（∵加速度×質量＝力）を制御しなければなら

ない．このためには関節角速度を対応する値に制御することにより力を制御できることに

なる．

●冗長自由度

空間上では６自由度あれば任意の位置決めは可能であるが，もっと多くの自由度をもて

ば，複雑な動きををすることができる．たとえば，人間の腕は７自由度あり，１自由度た

めに手と肩を拘束しても肘の位置を動かすことができる．このような７自由度以上の多関

節アームを冗長自由度アームあるいは冗長マニピュレータという．最近ではもっと多くの

自由度をもつロボット（大自由度ロボット）で複雑な動きをアニメーションで表示するこ



8

とも行われている．これを一般の場合として定式化してみよう．手先速度ベクトルrの次元

をmとし，関節速度ベクトルq の次元をnとし，冗長マニピュレータでは， nm になっ

ていることから，ヤコビ行列が横長の nm 行列になっていることに注意しよう．

qqr )(J (□)

ここでは，ヤコビ行列はフルランク，つまり次式が成り立っていると仮定する．

mJ )(rank q

ここで， rankは行列の階数（ランク）のことである．この場合には，目標の手先速度を与

える関節速度は無数に存在する．(□)式の一般解は次式のように表すことができる．

1)(

)(

TT JJJJ

JJIJ vrq (□□)式

ただし， vは任意のベクトルである．

課題：どうして無数に存在するのか理由を具体例を用いて説明せよ．また，(□□)式が(□)

式の解になっていることを確認せよ．

ここでは，無数の解の中から，関節速度のある意味での大きさであるつぎの量

qqq WG T)(

を最小にする解を求める方法を紹介する．ただし，上式は２次形式とよばれ，W は 0q を

満たすすべてのベクトルに対して 0)( qG となる nn 行列（これを正定値行列という）と

する．

課題：２次形式はどういう意味をもつか説明せよ．また，正定値行列の例をあげよ．

問題はつぎの制約条件付き最適化問題に帰着される．

問題）制約条件 0)( qqr J のもとで， )(qG を最小にするq を求めよ．

これはラグランジュの未定係数法で，制約条件を最適化関数にとりいれることで，解くこ

とができる．ラグランジュ乗数λ

m

T 1λ

を用いて，つぎの関数を定義する．

))((),( qqrλqqλq JWG TT

),( λqG を最小にする条件は，次式である．

0λ

λq0

q

λq

),(,

),(

GG

これを計算すると，次式のようになる．

0qqr0λq )(,2 JJW T (▲)

上式でq を消去すると，次式が出てくる．

rλ 2)( 1 TJJW

これより



9

rλ 11 )(2 TJJW

となり，これを(▲)の１番目の式に代入すると，次式が得られる．

rq 111 )( TT JJWJW

ここで， 111 )( TT JJWJW は逆行列を一般化したものとみなすことができる．これを J の

ノルム最小化逆行列という．特に，W を単位行列，つまり， IW としたときには，ノル

ム最小化逆行列は次式のようになる．

とかく JJJJ TT 1)(

これを J の擬似逆行列という．

課題：擬似逆行列を用いて，次式の解を求めよ．

1321

3

2

1

x

x

x

５．力・モーメント座標変換の応用 5)

図のようにハンドで円柱棒を穴に挿入する仕事を考えてみよう．

本資料 p.4 で述べたように関節の駆動力 τを調整して手先あるいは棒の先に適切な力やモ

ーメントFを与えることができる．ここでは，逆に棒の先に加わる外力をアーム先端に装

着した力センサの計測データから求める方法について述べる．ただし，棒はアーム先端に

しっかりと固定されたまま穴の一部と接触して静止しており，重力の影響は無視するとす

円柱棒

s

0 p

p0

力センサ

全体を１つ

の剛体とみ

なす



10

る．座標系を 3つ与える．

・ベース座標系（ロボット本体ベース）： 0

・センサ座標系（力センサ部分）： s

・部品座標系（棒の先端部分）： p

p の原点に作用する力とモーメントを 0 座標の表現したものを pp nf 00 , とする．ここで，

力とモーメントの記号の添え字のつけ方はつぎのようにしている．

かかる場所座標系

f

力センサを含む手先と掴んだ部品を１つの剛体とみなすと， pp nf 00 , に等価なセンサ座標系

の原点に作用する力とモーメント ss nf00 , の間には，つぎの関係が成り立つ．

ssp

sp

fpnn

ff

0000

00

ただし， p0は s の原点の p の原点に対する相対ベクトルを 0 で表したものとする．ベー

ス座標系と部品座標系の座標変換の回転行列 Rp

0 とすると，次式のように力ベクトルを表す

ことができる．

s

p

s

p

p

p

p

p RR ffff 0

0

0

0 ,

センサ座標系と部品座標系の座標変換の回転行列 Rp

s とするセンサ原点に働く力の座標変

換の関係は次式のようになる．

s

sp

ss

p R ff

ここで， s

sf がセンサにより計測される力になる．このことからつぎの変形が可能である．

s

sp

ss

p

p

p

s

p

p

p

sp

R

RR

fff

ff

ff

0

0

0

0

00



11

これにより，センサで測れるデータである s

sf と知りたい力 p

pf の関係が得られる．つぎに

モーメントについて考える．次式のような変形が可能である．

)(

)()(

)(

0

0

0

0

00

0

0

0

0

0

0000

s

sp

s

p

s

sp

sp

p

s

pp

s

p

p

p

s

pp

s

p

p

p

s

p

s

p

p

p

ssp

RR

RR

RRR

fpnn

fpnn

fpnn

fpnn

fpnn

これにより，センサで測れるデータである s

s

s

snf , と知りたいモーメント p

p n の関係が得ら

れる．

課題：次式が成り立つことを説明せよ．

)()()( 0

0

0

0

00

0 s

pp

s

p RRR fpfp

外積計算を行列演算表示するために，行列を定義する．任意の 3次元ベクトル

z

y

x

a

a

a

a

のとき，つぎのような行列を定義する．

0

0

0

xy

xz

yz

aa

aa

aa

a

このとき，外積はつぎのように表現できる．

baba ][

したがって，力センサによる力・モーメントの計測データと棒の先の力・モーメントとの

関係は次式のようにまとめることができる．

s

s

s

s

p

s

p

s

p

p

s

p

p

p

p

RRp

R

n

f

n

f

][

0

A付録 1：力の釣り合いと張力

天井から質量 m の物体が糸で吊り下げられていて，その物体を水平方向の力 S で引張っ

たとき，糸は角度θで釣り合ったとする．このとき，物体と糸は静止することから糸にも



12

引張り力が生じることになる．これを張力という．

X方向

0sin TS

Y方向

0cos WT

B付録２：力のモーメント 7）

質点とは質量をもった点であるが，剛体とは大きさをもつ物体である．このため力の働

きに注意が必要で，力の大きさと方向の他に，力が作用する点を考える必要がある．力が

作用する点を着力点という．着力点を考えないと，剛体は回転してしまう．図のように点 a

と点 bに Aと B の力が働いているとする．ベクトル Aとベクトル Bの延長線を引き，この

交点 cが着力点である．着力点に，ベクトル A とベクトル B に釣り合う力 C を加えると，

剛体は静止する．

ところが，次図のように働く力が平行の場合には，剛体が静止する力を加える着力点をみ

つけることはできない．このため，モーメントの概念が必要になる．

T

gW m

S

a b

A B

c

A B

C

着力点

r1

A B

r2



13

ある点のまわりの力のモーメントとは，その点から力のベクトルの延長線上におろした垂

線までの距離と力の大きさをかけた量のことである．図の点が着力点になるためには，2つ

の力のモーメントが等しくなければならない．

BrAr 21

ただし，つぎのようにおいている．

BA BA ,

モーメントはつぎの図のように考えればよい．一端を固定した棒をの片方に力を加えると，

棒は回転する．これがモーメントである．力の方向で回転は変わるので，モーメントも符

号を考えなければならない．

モーメントベクトル

FrN

C.付録 3：仮想仕事の原理：物体に働く 2つの力の場合

3次元空間内の物体（質点）に３つの力 cba FFF ,,

が働いており，これがつりあっているとする．この

状態で，仮に，物体が微小変位 ld だけ移動すると仮

定する．ただし， ld をベクトルの成分表現したもの

を次式とする．

dz

dy

dx

dl

このときの微小仕事は次式のように書ける．

lFFFlFFFlFlFlF ddddddW T

cbacbacba )()(

固定点

位置ベクトル r 力 F

N r

F

Fa

Fb

Fc

dl



14

ただし，上式のは内積を意味している．ベクトルの内積はつぎの表せることに注意しよう．

zzyyxx

z

y

x

zyx

z

y

x

T

z

y

x

z

y

x

z

y

x

bababa

b

b

b

aaa

b

b

b

a

a

a

b

b

b

a

a

a

また，上式を全微分の式と考えると，次式が成り立つ．

dz

dy

dx

z

W

y

W

x

WdW

したがって，力が釣り合っていることから，次式がなりたつ．

0

cba

z

W

y

W

x

WFFF

したがって，釣り合いの状態にあるときは，仕事は極値をとる（微分がゼロ）ことになる．

また，次式も成立する．

0dW

これを仮想仕事の原理という．さらに，運動している物体の仮想仕事の原理を考えてみよ

う．質量 mの質点がｘ方向に働く力 Fにより dx

だけ移動した場合を考える．仮想仕事の原理を

適用するためには，運動している質点を静止し

た力の釣り合いとみなさなければならない．こ

のため，

xm をみかけの力とみなすと，つぎのような釣り合い式が成り立つ．

0Fxm

この状態で dxだけ変位したときの仮想仕事は次式になる．

0)( dxFxmdW

ここで，次式が成り立つ．

Fxmdx

dW

これより，質点が t=0 で x=0 から t=t で x=xまで移動したときの仕事は次式のように書け

ることがわかる．

xt xxt

x xx

FdxxmFdxdtxxmFdxdtdt

dxxm

FdxdxxmdxFxmW

0

2

0 000

0 00

2

1

)(

F

xm

dx



15

参考文献

１）下嶋，佐藤：ロボット工学，森北出版（１９９９）（講義テキスト）

２）若松，田村編：ロボット制御，計測自動制御学会（１９８５）

３）計測自動制御学会編：ロボット制御の実際，コロナ社（１９９７）

４）松日楽，大明：わかりやすいロボットシステム入門，オーム社（１９９９）

５）宮崎，升谷，西川：ロボティクス入門，共立出版（２０００）

６）広瀬：ロボット工学，裳華房（１９８７）

７）鷹尾：力と数学のはなし，日化技連（１９９９）

松尾孝美＠大分大学工学部福祉環境工学科 ·...

Documents