とめ研究所若手研究者懸賞論文

論

1

文 ======================================================

移動距離と全身の運動を一つの系として記述可能な
ロボット運動学の定式化手法
関口叡範
Sekiguchi Masanori

東京都立大学Tokyo Metropolitan University============================================================
1. 緒

不都合をもたらすことである．場合によっては，ヤコビ行

言列の計算式が A4 用紙数十枚分に相当する長さにまで肥大

少子高齢化による労働人口の減少が続く近年において，化することもある．二つ目は，習慣的にヤコビ行列としてロボットの活躍は益々期待されている．そこで本論文では，扱われている行列が，微分積分学におけるヤコビ行列の定ロボットの制御における核心技術の一つである運動学（キ

義に順守していないことである．この問題点の原因は，手ネマティクス）について議論する．ロボットの運動学は，近先効果器の姿勢を表すベクトル（オイラー角など）を関節

年注目を集めている機械学習の性能にも大きく影響を及ぼ

変位ベクトルで偏微分しても，角速度に関する微分運動学

す．例えば，逆運動学を計算可能な場合と，そうでない場

は導出できないことにある [11]．偏微分によって定義され合では，学習方式あるいは学習効率に大きな差が生じる．

る本来のヤコビ行列と，そうでないものが区別されていな

運動学はロボット工学における基本的な概念の一つであいという点で，ヤコビ行列という名称を使用することは厳り，古くから多くの文献で取り上げられている．大抵の場

密には適切ではない．この問題点を考慮した一部の文献で

合，順運動学の計算には Denavit-Hartenberg の表記法（DHは，基礎ヤコビ行列（the basic Jacobian matrix）[12] [13]法）[1] が用いられ，微分運動学（ヤコビ行列）の定式化にはや幾何ヤコビ行列（geometric Jacobian matrix）[14]〜[16]偏微分が用いられる．これらの方法は，ロボット工学が一といった他の名称が使用されている．しかし，名前にヤコ

つの学問分野として認知され始めた 1980 年代から既に使ビ行列という名称が含まれているため，誤解の余地を与え用されており，それが現在のロボット工学にまで引き継が得るという問題点は依然として残されている．

れている．1980 年代では，多関節ロボットの主な用途が画

そこで本論文では，順運動学とヤコビ行列の基本を見直一的な単純作業に限定されていたことから，自由度が 4 ∼ 6

し，前述した問題点を解決するための新たな定式化手法を程度の簡素なロボットが主流となっていた．しかし近年で提案する．まず，第 2 章と第 3 章では，それぞれ順運動学

は，移動マニピュレータや腰に自由度を持った双腕型のロと微分運動学（ヤコビ行列）の定式化手法について述べる．

ボットなどが普及し，ロボットの構造や自由度が大きく多本論文で提案する定式化手法を用いることで，分岐点や直様化・冗長化した．これにより，順運動学やヤコビ行列の進対偶，移動能力を有するロボットにおいても，同一の手計算過程に従来から潜在していた問題点が，より顕著に現順で運動学を計算することが可能となる．第 4 章では，提れるようになった．

案した運動学の定式化手法を逆運動学の数値解法に適用しDH 法における問題点の一つは，隣接する二つの関節軸が互いに平行であった場合，DH パラメータの一つが不定となることである [2]．この問題は DH 法における特異点問た事例を示す．第 5 章では，双腕型の移動マニピュレータ

題として古くから知られており，既に多くの解決策が提案

当性を検証する．第 6 章の結言では，まとめと今後の課題

されている [3]〜[9]．しかし，DH 法には特異点の問題だけについて述べる．

を用いたシミュレーション実験を行い，提案手法の具体的な応用例を示すと共に，提案手法に含まれる各計算式の妥でなく，各関節の位置・姿勢が計算結果に残らないことや，

2. 順 運 動 学

DH パラメータの計算手順が複雑であるといった実用上の問題点があることも指摘されている [10]．これらの問題点

本章では，本論文で提案する順運動学の定式化手法につに加え，ロボットの構造が多様化した近年においては，理論

いて説明した後，提案手法を DH 法と比較したときの利点の表記に関する他の問題点も顕在化している．例えば，双と欠点について述べる．

腕型のロボットを対象とした場合，右腕か左腕かを判別す

2. 1 順運動学の定式化

るための添え字を各変数に追加する必要があるが，従来の大抵の場合，多関節ロボットの機械的な構造は，分岐点定式化手法では既に多くの添え字が使用されており，数式を有する開リンク機構の一種として扱うことができる．人として表現することが困難である．

型ロボットのように，ロボットが三次元空間上を任意に移微分運動学の計算過程においても二つの問題点がある．

動する場合においても，ロボットの位置と姿勢をそれぞれ一つ目は，ロボットの自由度が大きくなるにつれて，ヤコ3 つの直進対偶と 1 つの球対偶を用いて表現することで，ビ行列の計算式が複雑化し，実装コストや計算時間の点で運動学モデルを開リンク機構の一種として扱うことができ

とめ研究所若手研究者懸賞論文—1—2021 年

関口叡範

2: Joint
図1: Joint

基準姿勢（q = 0）のときの運動学パラメータ

図 2 任意の q に対する運動学パラメータ
R R q , ϕ
bij
ij
ij
Ri,j+1 =Rij

る [17]．そこで本節では，一つの幹と複数の枝によって構成されている樹状型の剛体リンク系を対象に，その順運動学について考える．幹と各枝はそれぞれ直鎖状のリンク連鎖であるとし，それらが一点で連結することによって，リンク系が構成されているものとする．
枝の本数を r とし，幹の自由度を n0 ，各枝の自由度を順に n1 , n2 , · · · , nr とする．図 1 のリンク系では，r = 3，n0 = 4, n1 = 3, n2 = 1, n3 = 3 である．リンク系全体の自由度は n := n0 + n1 + · · · + nr となる．リンク系全体の関節変位ベクトル q を次のように定義する．
h

qT
0

qT
1

···

qT
r

iT

∈ Rn

（1）

(直進対偶)（2）bij は回転角度 q ij と回転軸ベクトル ϕbij
ここで，R q ij , ϕ
による回転変換を回転行列形式で表したものである（付録

A 参照）．ただし，各直鎖リンクの 0 番リンクにおける回転
行列 Ri0 は，上式では計算できないため次式を用いる．

Ri0 =


I

(i=0)

R0n

0

q=

(回転対偶)( i ̸= 0 )（3）

ここで，I は単位行列である．擬似コードで回転行列の計算手順を表すと以下のようになる．

ここで，q 0 は幹の各関節変位を表し，q 1 , q 2 , · · · , q r は各枝の各関節変位を表す．また，幹と枝の両方をまとめて直ク i における j 番関節の変位（q i の第 j 成分）を q ij とす

Algorithm 1
Procedure for computing the Rij .
1: for i = 0, 1,
· · · , r do

る．本章では，以降でも i は直鎖リンクの番号を表し，j は2:鎖リンクと呼ぶことにし，その番号を i とする．直鎖リン関節またはリンクの番号を表す．

Ri0 =

I
R0n0

(i=0)
( i ̸= 0 )for j = 0, 1, 
· · · , ni − 1 dobij
Rij R q ij , ϕ
4:
Ri,j+1 =Rij
5:
end for
6: end for
3:

まず，リンク系が基準姿勢（q = 0）のときにおける運
動学パラメータを定義する．リンクベクトル ℓbij ∈ R3 と関

bij ∈ R3 を図 1 のように定める．リンクベ
節軸ベクトル ϕ

クトル ℓbij は隣接する関節の相対的な位置関係を表すベク(回転対偶)(直進対偶)bij は，関節 j が回転対偶でトルである．関節軸ベクトル ϕ
あれば単位回転軸ベクトルを表し，直進対偶であれば単位回転行列 Rij が既知となることで，任意の q に対する各

bij はリンク系の構造を表す
方向ベクトルを表す．ℓbij と ϕ

リンクベクトル ℓij ∈ R3 と各関節軸ベクトル ϕij ∈ R3 も定数のベクトルであり，既知の変数となる．

それぞれ次式によって計算可能となる．

図 2 のように，任意の q に対する各リンクの姿勢を表す
回転行列を Rij とする．回転行列 Rij は関節変位ベクトルℓij = Rij ℓbij

（4）

q によって生じる姿勢の変化を表しており，q = 0 のときは全ての Rij が単位行列となる．j + 1 番リンクの姿勢を表す回転行列は，j 番関節の種類（回転対偶または直進対

bij
ϕij = Rij ϕ（5）偶）に応じて次のように表される．

とめ研究所若手研究者懸賞論文

擬似コードで ℓij と ϕij の計算手順を表すと次のようになる．

—2—2021 年

移動距離と全身の運動を一つの系として記述可能なロボット運動学の定式化手法

3

Algorithm 2
Procedure for computing the ϕij and ℓij .
1: for i = 0, 1, · · · , r do
2:
for j = 0, 1, · · · , ni − 1 dobij
3:
ϕij = Rij ϕ
4:ℓij = Rij ℓbij
5:
end for6:ℓini = Rini ℓbini
7: end for

次に，絶対座標系（ベース）に対する各関節の位置ベク
トルを pij ∈ R3 とする．ただし，j = ni のときのみは pij
は関節の位置ではなく，各直鎖リンク（幹または枝）の終端
位置となる．それぞれの位置ベクトルは，j 番関節の種類（回転対偶または直進対偶）に応じて次のように表される．


ℓ
00
pi0 =
ℓi0 + p0n

(i=0)
0

( i ̸= 0 )p + ℓi,j+1
ij
pi,j+1 =pij + ℓi,j+1 + q ϕijij(回転対偶)(直進対偶)（6）

図 4 順運動学の計算に使用した変数（7）

j 番関節が直進対偶のときにおける関節間の相対的な位置関係を図 3 に示す．
擬似コードで位置ベクトルの計算手順を表すと次のよう

2. 2 DH 法と比較したときの利点と欠点
DH 法と比較したとき，前述した順運動学の定式化手法には以下のような利点がある．
（1）プログラミング言語との互換性が高く，実装が容易で
あること．

になる．

（2）分岐点や直進対偶を含むロボットにおいても，同一の手順で順運動学を計算可能であること．
Algorithm 3
Procedure for computing the pij .
1: for i = 0,
1, · · · , r do
ℓ00
ℓi0 + p0n0に含まれる変数の数は基本的に二つ以下に収まること．
（4）リンクベクトルや関節軸ベクトルの向きが，絶対座標(i=0)( i ̸= 0 )

2:

pi0 =

3:

for j = 0, 1,
· · · , ni − 1 do
pij + ℓi,j+1
pi,j+1 =pij + ℓi,j+1 + q ij ϕij

4:（3）分岐点を有するロボットを対象とした場合でも，添え字

系の座標軸と平行でない場合でも適用可能であること．
（5）絶対座標系に対する各関節の位置と各リンクの姿勢が(回転対偶)(直進対偶)

明確となるため，計算結果を干渉チェックやヤコビ行列の計算に再利用できること．

5:
end for
6: end for

一方で，提案した順運動学の定式化手法には，DH 法よりも多くの変数を計算の過程で要するという欠点がある．

順運動学を計算する過程で必要となった全ての変数を図

3. 微 分 運 動 学

4 に示す．例えば，これらの変数を C 言語で実装する場合，本章では，分岐点や直進対偶を含む多関節ロボットを対一つのブロックをクラスとして定義し，それを二次元配列象とした微分運動学の定式化手法について述べる．本章でにすることなどが考えられる．

述べる定式化手法には三つの利点がある．一つ目は，計算過程に偏微分が含まれないため，実装が容易となる点である．二つ目は，第 2 章で述べた順運動学の計算結果が再利用されるため，新たに必要となる変数や演算処理が少ないことである．三つ目は，ロボットの構造に関わらず，同じ理論・同じプログラムで微分運動学を計算できることである．

:

各効果器（幹と各枝の終端位置）の速度と角速度を表すベクトルを次のように定める．

h
図 3 j 番関節が直進対偶のときにおける関節間の相対的な位置
関係

とめ研究所若手研究者懸賞論文—3—v(q) =v0T(q) v1T(q) · · ·vrT(q)

iT∈ R6(r+1)（8）2021 年

関口叡範

4

ここで，v0 (q) ∈ R6 は幹の終端位置（分岐点）における速ここで，0 ∈ R3 は零ベクトルである．関節変位が可動域

度と角速度を表し，v1 (q), v2 (q), · · · , vr (q) ∈ R6 は各枝の限界値に達しているときなどでは，関節は可動でなくなの終端位置における速度と角速度を表す．

る．擬似コードで速度比行列の計算手順を表すと次のよう以降では，次式を満たすような行列 J (q) を求めること

になる．

を考える．
（9）

v(q) = J (q) q̇

Algorithm 4
Procedure for computing the s jij and b jij .
1: for i = 0, 1, · · · , r do
2:
for j = 0, 1, · · · , n0 − 1 do
3:Calculate the s j ij .
4:
end for5:
for j = 0, 1, · · · , ni − 1 do
6:
Calculate the b j ij .
7:
end for
8: end for

行列 J (q) は，習慣的にヤコビ行列と呼ばれている．しかし，特殊な機構を除き，行列 J (q) は多変数ベクトル関数の偏微分で求めることは不可能なため，厳密には行列 J (q)はヤコビ行列ではない．そこで本論文では，J (q) を速度比行列（velocity ratio matrix）と呼ぶことにする．この名称は，行列 J (q) が各効果器と各関節の速度比を表していることに由来する．速度比行列 J (q) はブロック下三角行列となり，以下のようにブロック分割することができる．

s
J0
 sJ
 1
s
J
J (q) = 
 2
 ..
 .
s
Jr

O
b
J1
O
..
.
O

O
O
b
J2
..
.
O

···
···
···
..
.


O
O


O

.. 
. 

···

b

ただし，上記の擬似コードでは sJ0 と bJ0 の両方が計算されるが，sJ0 = bJ0 であり，実際にはどちらか一方の計算のみでも十分である．すなわち，式 (10) の sJ0 は bJ0 に（10）置き換えても同じ計算結果となる．なお，i ̸= 0 のときはJi ̸= bJi となる．また，r = 0 のときは J (q) = sJ0 とな
り，このときの行列 J (q) は杉原の解説記事 [18] で述べらsJr

ここで O は零行列であり，各ブロックを表す変数では引数および(q) が略記されている．さらに，行列 Ji ∈ Rb
Ji ∈ R6×ni を以下のように列ブロック分割する．
ihsJi = s j i0 s j i1 · · · s j i,n0 −1 ∈ R6×n0 （11）
s

b

Ji =

h

bj

i0

bj

i1

それぞれの列ベクトル

···
sj

ij と

i
i,ni −1bj

ij は，j 番関節の種類（回




p
p
ϕ
×
(
−
)
 0j
ini
0j

 (回転対偶)




ϕ0j











ϕ
s
 0j 
j ij =(直進対偶) （13）

0






 





0

 

(可動でない)
0

とめ研究所若手研究者懸賞論文

本章では，文献 [19] [20] で述べられている逆運動学の数値解法に対し，前述した順運動学と速度比行列を適用したや直進対偶が含まれている場合においても，逆運動学の数

転対偶または直進対偶）と状態（可動か否か）に応じて以下のように表される．

4. 逆運動学への応用例

事例を示す．提案手法を用いることで，ロボットに分岐点∈ R6×ni （12）

bj




p
p
)
−
ϕ
×
(

ij
ij
in
i







ϕ
ij






 



ϕ
b
 ij 
j ij =

0






 





0

 



0

れている基礎ヤコビ行列と等価になる．

6×n0

値解法を一つの理論に定式化できることを示す．

4. 1 誤差ベクトル
直鎖リンク i の終点における位置・姿勢をそれぞれ pi (q) ∈R3 ，Ri (q) ∈ SO(3) とする．pi (q) は第 2 章の順運動学で求めた pini と一致し，Ri (q) は Rini hRi と一致する．ここで，hRi はハンドの姿勢を表す定数の回転行列である．リ
ンクベクトルの姿勢とハンドの姿勢は一致しているとは限らないため，リンクベクトルとハンドの相対的な姿勢関係を表す回転行列 hRi を導入した．また，直鎖リンク i の終点に対して与える目標の位置と姿勢をそれぞれ d pi ∈ R3 ，Ri ∈ SO(3) とする．これらを用いて誤差ベクトル e(q)dを次のように定める．

he(q) =

TeT
0 (q) e1 (q)"(回転対偶)ei (q) =···i − pi (q)dϕ Ri RiT(q)
dp

eT
r (q)
#
∈ R6

iT

∈ R6(r+1)
（15）
（16）

ここで，ϕ( ∗ ) は回転行列を角軸ベクトル [21] に変換する(直進対偶)（14）関数である．

4. 2 仮想バネと仮想トルク
誤差ベクトル e(q) に対して仮想的な直進バネと回転バ(可動でない)ネが張られていると考える．仮想直進バネは位置の誤差に対して張られ，仮想回転バネは姿勢の誤差に対して張られ—4—2021 年

移動距離と全身の運動を一つの系として記述可能なロボット運動学の定式化手法

5

Target 2
Target 0++ +UpdateForward kinematics

図 6 逆運動学の計算手順を表すブロック線図

q k+1 = q k + ∠|q k（21）

また，q k+1 は次のように書くこともできる．

Target 1

q k+1 = q 0 +Target 3: Joint

をそれぞれ次のように定めた．

省略されている）．バネ定数を表す行列 K を次のように定

O
K1
..
.
O

···
···
..
.
···

（22）

q 0 は初期関節変位である．上記の反復計算をブロック線図にまとめると図 6 のようになる．図 6 では，集合 d χ，χk

る．図 5 に仮想直進バネを示す（図中では仮想回転バネは


K0

O
K=
 ..
 .
O

∠|q i

i=0

図 5 仮想直進バネの概念図

める．

k
X

dχ



O

O
.. 

. 
Kr

χk =
（17）

d

p0 , dR0 , · · · , d pr , dRr（23）p0 (q k ), R0 (q k ), · · · , pr (q k ), Rr (q k ) （24）

4. 4 仮想バネの弾性エネルギー
仮想バネの弾性エネルギー Vk は以下のように表される．

ここで，対角行列 Ki ∈ R6×6 は誤差ベクトル ei (q) に対Vk =して張られる仮想バネのバネ定数を表す．また，仮想バネ
によって各関節に生じる仮想トルク τ (q) は次式によって計算できる．

τ (q) = J T(q) K e(q)



=

（18）

上式の J (q) は第 3 章で定式化した速度比行列である．仮

1 Te K ek ∈ R
2 k

（25）

Vk が最小となるような各関節変位が逆運動学解となる．
よって，∠|q k に含まれる行列 Dk は Vk の減少性を考慮して選定する必要がある．Vk を減少させるための Dk の選定
例として，以下のものが提案されている [20]．

想トルク τ (q) に対し，関節ベースでのダンピング効果に

Dk = JkT KJk +より，各関節に速度 q̇ が発生すると見なせば，q̇ は次のよ

12

V k + δ In

（26）

ここで，δ ∈ R は Vk ≃ 0 のときに行列 Dk の非正則化を防うに表される．

q̇ = D−1(q) τ (q)

ぐための微小な正の定数である．行列 JkT KJk はインピー（19）

ここで，D(q) は関節ダンピング係数行列である．D(q) の
具体的な定め方は後述する．

数値解法 [24] などで頻繁に現れる．行列 JkT KJk のブロック分割については付録 B を参照されたい．式 (26) に示し

4. 3 逆運動学の数値解法への展開
反復計算により，逆運動学の数値解を求めることを考える．反復計算 k 回目における各関節変位を q k とし，以降では，e(q k )，τ (q k )，D(q k )，J (q k ) をそれぞれ ek ，τk ，

Dk ，Jk と略記する．
仮想バネと関節ダンピング効果により各関節に速度が生じたとき，その速度によって各関節変位が 1 秒間当たりに変化する量を ∠|q k とする．このとき，∠|q k は次のように

表される．
−1
−1 T
∠|q k = Dk τk = Dk Jk K ek

ダンス制御 [22] やコンプライアンス制御 [23]，逆運動学のた行列 Dk は正定値対称行列となるため，その逆行列を計算する際は，Cholesky 分解 [25]〜[28] を用いることで計算量を半減することができる．また，行列 Dk は Vk+1 ≤ Vkとなるための近似的な十分条件 [19] を満足している．
以上のように，第 2 章と第 3 章で提案した運動学の定式化手法を用いることで，分岐点や直進対偶を含む多関節ロ
ボットにおいても，逆運動学の数値解法を一つの理論にまとめることができるようになる．

5. シミュレーション実験（20）

本章では，第 4 章で述べた逆運動学の数値解法を双腕型

各関節変位 q k に ∠|q k を加えたものを次のステップにおけの移動マニピュレータに適用し，シミュレーション実験をる各関節変位とすれば，関節変位の更新則は次のように表行う．その結果から，提案手法のより具体的な応用例や各される．

計算式の妥当性を示す．

とめ研究所若手研究者懸賞論文—5—2021 年

関口叡範

6Target 2

Target 1

図7

シミュレーション実験で用いる多関節ロボットの運動学モデル（双腕型の移動マニピュレータ）(a) 初期姿勢（k = 0）

5. 1 実験に使用した運動学モデルとパラメータ

Target 2

シミュレーション実験に使用する多関節ロボットの運動学モデルを図 7 に示す．幹の自由度は 4，枝（片腕）の自
由度は 7 であり，全体の自由度は 18 である．片腕は 2 つTarget 1の球対偶と 1 つの回転対偶によって構成されており，移動
台車の変位は仮想的な 2 つの直進対偶と 1 つの回転対偶によって表現されている．ロボットの高さは 1.4 [m]，肩幅は0.8 [m] である．また，ハンドの姿勢を表す回転行列は以下のようになる．

1hR1 = hR2 =  0
0

0
−1
0


0

0 
−1

(b) 反復計算 4 回目（k = 4）

（27）

Target 2ロボットが基準姿勢（q = 0）のとき，リンクの姿勢を表す回転行列は全て単位行列であるのに対し，図 7 のロボッ
Target 1

トではハンドが真下を向いている．これを補正するための回転行列が hR1 と hR2 である．
本実験で使用したバネ定数行列 K と定数 δ の値を以下
に示す．



O

K=O
O

O
K1
O


O

O ,
K2

δ = 0.02（28）(c) 反復計算 8 回目（k = 8）

K1 = K2 = diag( 1, 1, 1, 4.13, 4.13, 4.13 ) （29）Target 2

本実験では，幹の終点（分岐点）に対しては目標の位置・姿勢を与えないため，K0 = O とした．

5. 2 逆運動学の数値計算例

Target 1

目標の位置・姿勢は，右腕の手先と左腕の手先に対して与えることにする．目標の位置・姿勢はそれぞれ次のよう
に定めた．




1.8

d
p1 = 
 0.8  [m],
0.8



0.8

d
p2 = 
 2.4  [m],1.6

とめ研究所若手研究者懸賞論文




0 0 1


d
R1 =  0 1 0 −1 0 0

0 0 1


d
R2 =  0 1 0 −1 0 0（30）(d) 反復計算 15 回目（k = 15）



（31）

図 8 逆運動学の数値解法を実行したときに手先が目標の位置・姿勢に収束するまでの過程—6—2021 年

移動距離と全身の運動を一つの系として記述可能なロボット運動学の定式化手法

手先が目標の位置・姿勢に収束するまでの過程を図 8 に示す．仮想バネの弾性エネルギー Vk がゼロ（0.001 以下）となるまでに 15 回の反復回数を要した．
移動台車の位置・姿勢は q 00 ，q 01 ，q 02 から得られるため，逆運動学を解くことで移動台車の位置決め問題も同時に解くことができる．ただし，逆運動学の計算結果から得られるのは，移動台車が最終的に到達する位置・姿勢であり，そこに至るまでの経路を計算するためには，逆運動学とは別に移動台車の経路計画を行う必要がある．
以上より，提案した順運動学と速度比行列（ヤコビ行列）の定式化手法を導入した場合においても，シミュレーション
実験で逆運動学の数値解を計算できることが示された．すなわち，本論文で提案した各計算式の妥当性がシミュレー
ション実験の結果からも示された．
なお，2 つの目標が離れた位置に与えられた場合，ロボッ
トはどちらか一方の目標にしか到達することができない．
このような状況を想定して，それぞれの目標に優先度を付与する方法も提案されている [29]〜[31]．これらは優先度付
き逆運動学と呼ばれており，例えば文献 [31] に示されている方法では，バネ定数行列 K に変数を１つ追加するだけで，それぞれの目標に優先度を付与することができる．

6. 結

言

本論文では，ロボットの構造が多様化した近年においても，汎用的に実用化可能な運動学を確立するため，ロボッ
ト運動学の基本を見直し，再定義を行った．提案手法を用いることで，分岐点や直進対偶，移動能力を有するロボッ
トにおいても，同一の手順で運動学を計算することが可能となる．第 5 章の数値計算例では，双腕型の移動マニピュレータを例に用いたが，同様な手順で四足歩行ロボットや人型ロボットなどの逆運動学を計算することも可能である．
本論文ではロボットに含まれる分岐点の数を 1 つに限定して定式化を行ったため，分岐点を 2 つ以上有するロボットに対しては，提案手法を適用することができない．分岐点の数を一般化した形式に拡張し，汎用性をより高めることが本研究における今後の課題となっている．
参 考 文 献[ 1 ] R.S. Hartenberg and J. Denavit: “A Kinematic Notation forLower-Pair Mechanisms Based on Matrices”, Journal of Mechatronics, vol.77, no.2, pp.215-221, 1955.
[ 2 ] J.M. Renders, E. Rossignol, M. Becquet and R. Hanus: “Kinematic Calibration and Geometrical Parameter Identiﬁcation
for Robots”, IEEE Transactions on Robotics and Automation,
vol.7, no.6, pp.721-732, 1991.
[ 3 ] W. Veitschegger and Chi-Haur Wu: “Robot Accuracy Analysis
Based on Kinematics”, IEEE Journal on Robotics and Automation, vol.2, no.3, pp.171-179, 1986.
[ 4 ] S. Hayati and M. Mirmirani: “Improving the Absolute Positioning Accuracy of Robot Manipulators”, Journal of Robotic
Systems, vol.2, no.4, pp.397-413, 1985.
[ 5 ] S. Hayati, K. Tso and G. Roston: “Robot Geometry Calibration”, Proceedings. 1988 IEEE International Conference on
Robotics and Automation, vol.2, pp.947-951, 1988.
[ 6 ] H. Zhuang and Z.S. Roth: “A Linear Solution to the Kinematic
Parameter Identiﬁcation of Robot Manipulators”, IEEE Trans-

とめ研究所若手研究者懸賞論文—7—

7

actions on Robotics and Automation, vol.9, no.2, pp.174-185,
1993.
[ 7 ] I.M. Chen, G. Yang, C.T. Tan and S.H. Yeo: “Local POE
Model for Robot Kinematic Calibration”, Mechanism and Machine Theory, vol.36, no.11-12, pp.1215-1239, 2001.
[ 8 ] F. C. Park and K. Okamura: “Kinematic Calibration and theProduct of Exponentials Formula”, Advances in Robot Kinematics and Computational Geometry, MIT press, pp.119-128,
1994.
[ 9 ] C. Wu: “The Kinematic Error Model for the Design of Robot
Manipulator”, 1983 American Control Conference, pp.497-502,
1983.
[10] P. I. Corke: “A Simple and Systematic Approach to Assigning Denavit–Hartenberg Parameters”, IEEE Transactions on
Robotics, vol.23, no.3, pp.590-594, 2007.
[11] 大隅久: “ロボット工学の基礎”, 精密工学会誌, vol.73, no.10,
pp.1123-1126, 2007.
[12] A. Codourey and E. Burdet: “A Body-oriented Method for
Finding a Linear Form of the Dynamic Equation of Fully
Parallel Robots”, Proceedings of International Conference on
Robotics and Automation, vol.2, pp.1612-1618, 1997.
[13] O. Khatib: “Inertial Properties in Robotic Manipulation:
An Object-Level Framework”, The International Journal of
Robotics Research, vol.14, no.1, pp.19-36, 1995.
[14] B. Siciliano: “The Tricept robot: Inverse Kinematics, Manipulability Analysis and Closed-Loop Direct Kinematics Algorithm”, Robotica, vol.17, no.4, pp.437-445, 1999.
[15] G. Tevatia and S. Schaal: “Inverse Kinematics for Humanoid
Robots”, IEEE International Conference on Robotics and Automation, vol.1, pp.294-299, 2000.
[16] R. Campa, K. Camarillo and L. Arias: “Kinematic Modelingand Control of Robot Manipulators via Unit Quaternions: Application to a Spherical Wrist”, Proceedings of the 45th IEEE
Conference on Decision and Control, pp.6474-6479, 2006.
[17] T. Sugihara and M. Morisawa: “A Survey: Dynamics of Humanoid Robots”, Advanced Robotics, vol.34, no.21-22, pp.13381352, 2020.
[18] 杉原知道: “逆運動学の数値解法”, 日本ロボット学会誌, vol.34, no.3,
pp.167-173, 2016.
[19] 関口叡範, 武居直行: “仮想バネの弾性エネルギー最小化に基づいた特異姿勢・非可解な問題に対して安定な逆運動学の数値解法”, 日本ロボット学会誌, vol.36, no.9, pp.645-653, 2018.
[20] T. Sugihara: “Solvability-unconcerned Inverse Kinematics
by Levenberg-Marquardt Method”, IEEE Transaction on
Robotics, vol.27, no.5, pp.984-991, 2011.
[21] 関口叡範, 武居直行: “特異点における角軸ベクトルの計算法と拡張角
軸ベクトルの提案”, 日本ロボット学会誌, vol.37, no.8, pp.726-734,
2019.
[22] C. Ott, A. Albu-Schaﬀer, A. Kugi and G. Hirzinger: “On the
Passivity-Based Impedance Control of Flexible Joint Robots”,
IEEE Transactions on Robotics, vol.24, no.2, pp.416-429, 2008.
[23] J. K. Salisbury: “Active Stiﬀness Control of a Manipulator in
Cartesian Coordinates”, 19th IEEE Conference on Decision andControl including the Symposium on Adaptive Processes, 1980.
[24] A. S. Deo and I. D. Walker: “Overview of Damped LeastSquares Methods for Inverse Kinematics of Robot Manipulators”, Journal of Intelligent and Robotic Systems, vol.14, pp.4368, 1995.
[25] A. Krishnamoorthy and D. Menon: “Matrix inversion using
Cholesky decomposition”, 2013 Signal Processing: Algorithms,
Architectures, Arrangements, and Applications, pp.70-72, 2013.
[26] A. W. Bojanczyk, R. P. Brent, P. van Dooren, and F. R. de
Hoog : “A Note on Downdating the Cholesky Factorization”,
SIAM Journal on Scientiﬁc and Statistical Computing, vol.8,
no.3, pp.210-221, 1987.
[27] N. J. Higham : “Cholesky factorization”, WIREs Computational Statistics, vol.1, no.2, pp.251-254, 2009.
[28] J. Li and O. B. Widlund : “FETI-DP, BDDC, and Block
Cholesky Methods”, International Journal for Numerical Methods in Engineering, vol.66, no.2, pp.250-271, 2006.
[29] T. Sugihara: “Robust Solution of Prioritized Inverse Kinematics Based on Hestenes-Powell Multiplier Method”, IEEE/RSJ

2021 年関口叡範

8International Conference on Intelligent Robots and Systems,
pp.510-515, 2014.
[30] 関口叡範, 武居直行: “切断法による優先度付き逆運動学の数値解法”,
日本ロボット学会誌, vol.37, no.8, pp.711-717, 2019.
[31] M. Sekiguchi and N. Takesue: “Fast and Robust Numerical
Method for Inverse Kinematics with Prioritized Multiple Targets for Redundant Robots”, Advanced Robotics, vol.34, no.16,
pp.1068-1078, 2020.

付録 A.

付録：Rodrigues の回転公式

回転角度 q と単位回転軸ベクトル ϕ = [ ϕx ϕy ϕz ]T による回転変換を回転行列で表すと以下のようになる．

hiR(q, ϕ) =

r1

r2

r3

∈ SO(3)

（A.1）




cos q + ϕ2x (1 − cos q)
r1 = ϕx ϕy (1 − cos q) + ϕz sin q  ∈ R3
ϕz ϕx (1 − cos q) − ϕy sin q

ϕx ϕy (1 − cos q) − ϕz sin q
r2 =  cos q + ϕ2y (1 − cos q)  ∈ R3
ϕy ϕz (1 − cos q) + ϕx sin q
ϕz ϕx (1 − cos q) + ϕy sin q
r3 = ϕy ϕz (1 − cos q) − ϕx sin q  ∈ R3
cos q + ϕ2z (1 − cos q)

（A.2）

（A.3）

（A.4）

上記の式は Rodrigues の回転公式と呼ばれている．

付録 B.

行列 J T KJ のブロック分割

行列 J T KJ は二重縁付きブロック対角行列であり，次のようにブロック分割することができる．



C00
C
 10

T
C
J KJ = 
 20
 ..
 .
Cr0

T
C10
C11
O
..
.
O

T
C20
O
C22
..
.
O

···
···
···
..
.


T
Cr0
O 


O  ∈ Rn×n

.. 
. 

···

Crr
（B.5）

ここで，

C00 =

r
X

s T
Ji Ki sJi ∈ Rn0 ×n0

（B.6）

i=0

Cii = bJiT Ki bJi ∈ Rni ×ni
Ci0 = bJiT Ki sJi ∈ Rni ×n0( i = 1, 2, · · · , r )（B.7）( i = 1, 2, · · · , r )（B.8）

である．行列 J T KJ は半正定値行列であり，正則とは限らない．例えば，行列 J が正方行列でないならば，行列

J T KJ は正則にはならない．よって，逆行列を計算する際
は，減衰因子を追加するなどの正則化が必要である．

とめ研究所若手研究者懸賞論文—8—2021 年