適応的身体表象のための持続的な感覚運動変換調整法
○鍋嶌厚太 國吉康夫 大村吉幸 (東京大学)
A Sustainable Coordination between Sensation and Body Motion
for Autonomous Adaptation of Body Representation
*Cota NABESHIMA, Yasuo KUNIYOSHI, Yoshiyuki OHMURA (The University of Tokyo)
Abstract— The body representation should adapt to the change of the body and objects it wears (e.g. tool)
anytime they happen. It requires sustainable coordination between sensation and body motion: observation
of discrepancy and identification. For the sustainability, robots should autonomously judge the reliability of
the identification. In this paper, we propose an sustainable coordination method with a clear criterion of
the reliability, and its applications: vision-motion coordination and kinesthesia-motion coordination. Simulational experiments show the validity of the criterion.
Key Words: Adaptive body schema, Tool-body assimilation, Marker-free hand/head-eye calibration, Orientation of force-torque sensor, Inertia identification
1.
序論
ヒトは視覚や力覚などの感覚を頼りに, 身体や道具を
使う. このとき, 感覚-身体運動間の空間的な変換 (感覚
運動変換) が必要となる. しかし感覚運動変換は, 成長
や怪我による身体変化や, 手に持つ物体や身に着ける
物体 (付着物体) によって容易に変化する. ヒト脳内の
感覚運動変換は身体図式と呼ばれ, 成長や怪我だけで
なく, 付着物体による変化にも自律的に適応する [1, 2].
このような適応能力は, 初めて持つ荷物や道具を運び,
使うロボットに必要である.
身体変化により, 感覚運動変換には “ずれ” が生じる.
身体変化は常に起こりうるので, その検出のためには
“ずれ” を常に観測する必要がある. 身体変化が検出さ
れれば, 感覚運動変換を新たに同定しうる. “ずれ” の観
測と変換の同定によって, 感覚運動変換が調整される.
適切な感覚運動変換を維持するためには, 持続的な調
整が必要となる. そのためにロボットは, 同定の適切さ
を自律的に判定する必要がある. 同定の適切さが判る
ことで, 得られた感覚運動変換に基づく行動が可能とな
る. “ずれ” の観測と変換の同定を行ない, 同時に同定
の適切さを判定する計算法を Sec.2. に示す.
視覚運動変換は従来, 身体上に既知パターンを置き,
オフラインに同定していた [3, 4]. しかし付着物体によ
り身体変化するとき, 身体上の既知パターンは仮定で
きず, “ずれ” の観測と変換の同定は困難となる. Sec.3.
では, 既知パターンを用いない方法を示す. Sec.2. の計
算法と組み合わせ, 持続的な調整が可能となる.
力覚運動変換は, 慣性パラメータ (質量, 慣性テンソ
ル, 重心位置) で律される. これらを同定することは, 重
力や身体運動による慣性力を予測/無視するために必須
である. 特に付着物体による身体変化の検出および慣
性パラメータの同定は, 付着物体上の接触推定に必須で
ある (触覚延長 [5]). 慣性パラメータを同定する方法は
種々ある [6, 7, 8, 9] が, 同定の適切さを自律的に判定
する方法は明らかでない. Sec.4. では, Sec.2. の計算法
を慣性パラメータ同定に適用する.
2.
“ずれ” の観測/変換の同定/適切さの判定
ある時刻のデータ入力 Ai (M × N 行列) と, 変換を
律するパラメータ x (N 次元列ベクトル) を結ぶ方程式
が, Ai x = 0 と線形化可能とする. x が定まったとき,
“ずれ” は ||Ai x|| で表せる. この方法は軽量かつ計算
量が固定なので, 常に行なうべき “ずれ” の観測に適し
ている. この “ずれ” の大きさによって, 新たに x を同
定するかを決める.
誤差行列を C = i ATi Ai (半正定値対称行列) とす
ると, 入力全体の二乗誤差 i ||Ai x||2 を最小化する x
は, C x = 0 の解となる. すなわちオンラインに更新さ
れる C の零空間の次元 n(C) が 1 となるとき, その基底
が x となる. ただし得られた x のスカラー倍の任意性
は, その他の拘束条件を用いる正規化により解消する.
n(C) は解の安定性を意味し, 同定の適切さの指標と
なる. 数値的には C を特異値分解し, 最小特異値が 0 に
近く, かつ 0 とみなす特異値が 1 つであれば, n(C) = 1
と見なし, 同定が適切と判定する. このとき, 右特異行
列の特異値 0 に対応する列ベクトルが x となる. 以上
のすべての計算は実時間で行なえる.
パラメータ同定には従来, [方法 a] 擬似逆行列, [方法
b] ロバスト推定, [方法 c] カルマンフィルタなどが用い
られてきた. [方法 a] はデータの適切なサンプリングが
必要である. [方法 b] は最適なサンプリングを選ぶため
計算量が多く, オンライン用途に適さない. [方法 c] は,
ノイズの分散行列の適切な設定が必要で, さらに同定す
べき次元が多いとき, 収束が困難になる. またこれらの
方法では, 同定の適切さを自律的に判定する方法が無
い. 本節では, 事前に設定する変数無しに同定し, 同時
に解の適切さを判定する方法を示した.
3.
視覚運動変換の調整法
視覚運動変換の同定を稼働前に行なうとき, ハンドア
イ校正の問題となる. このときハンド上に固定された
既知パターンを仮定できる. 従来ではカメラに対する
既知パターンの運動を推定し, Fig.1-A に示す行列方程
式をオフラインに解いていた [3, 4]. これらの方法では
同定の適切さを自律的に判定できず, 持続的な調整に
適さなかった. また付着物体により身体変化するとき,
付着物体の形が一般には未知なため, 身体表面に既知パ
ターンを仮定できない. このとき “ずれ” の検出および
変換の同定は困難となる.
(A) Conventional formulation
(
(B) Proposed formulation
(
)
x
る方法 [7] があった. Sujan と Dubowsky は, カルマン
フィルタの収束を早める身体運動を相互情報量により
求める方法を示した [9]. しかしこれらの方法では, 同
定の適切さを自律的に判定できなかった. そこで本節で
は, 慣性パラメータ同定の適切さを得るために, Sec.2.
の計算法を適用する方法を示す. この方法により慣性
パラメータの持続的な調整が可能となる.
以下では, マニピュレータに備えられた力覚センサよ
り先の物体の慣性パラメータが変化したとする. また
ワールド座標系における力覚センサの座標系 (力覚座標
W
系) の原点位置 W
Kr および姿勢 KR を可変で既知とす
る (Sec.4·2 この条件を満たす方法を示す).
b)
Fig.1 Conventional and proposed formulation of
vision-motion coordination.
我々は [10] の中で, 身体運動に同起する視覚特徴の
変化のみが, 視覚と身体運動を関連付ける手がかりであ
ると示した. またこの手がかりを利用し, 身体形状と背
景が未知であっても, カメラ中の身体領域を発見する方
法を示した. この方法と Sec.2. の計算法を組み合わせ,
本節では既知パターンが不要な視覚運動変換の持続的
な調整法を提案する.
[10] の方法によって, 身体 (ハンド) 表面に固定され
たある一点 i を発見し, 追跡可能なことが仮定できる.
以下では, 点 i のカメラ座標系での位置 Cr i がステレオ
カメラなどによって継続的に得られたとする (Fig.1-B).
点 i の座標をハンド座標系で Hr i (固定; 未知) と表す.
このときハンド座標系とカメラ座標系の関係は,
W
W
C R C ro
C
ri 1
T
=
W
W
H R H ro
H
ri 1
T
(1)
W
CR
T
C
W
W
= [ W
C e1 C e2 C e3 ] および r i =
[ Cxi Cyi Czi ] とし, 既知量と未知量をまとめ,
となる.
A=
x=
C
xi E
W T
C e1
C
yi E
W T
C e2
C
W
zi E E −W
H R −H r o
W T
C e3
W T
C ro
H T
ri
,
T
1
とすると eq.(1) を Ax = 0 の形式で表せる. これに
Sec.2. の計算法を適用する. ただし E を 3 × 3 単位行
列とする. 得られた x には最終要素を 1 にする正規化
T W T W T
を行う. また W
C e1 , C e2 , C e3 は正規直行化する.
この方法は従来に比べ, 身体や付着物体の形状を仮定
せずにオンラインに計算でき, 事前に設定する変数無し
に同定可能で, その適切さも自律的に判定可能である.
4.
力覚運動変換の調整法
4·1 持続的な慣性パラメータの調整
力覚運動変換は慣性パラメータによって律される. 物
体を手にするとき, 身体の慣性パラメータが変化する.
この変化に適応することで, 物体への触覚延長が可能に
なる [5]. 物体の付着などによって慣性パラメータは容
易に変化するため, その場で即座に適応しなければなら
ない.
マニピュレータの慣性パラメータの同定法には従来,
擬似逆行列による方法 [6, 8] や, カルマンフィルタによ
(
)
Fig.2 Formulation of inertia identification.
物体は力覚座標系に対して固定されているとする
(Fig.2). 物体の質量を m (固定; 未知), 重心回りの
慣性テンソルを I (固定; 未知) とする. 物体の重心を
物体座標系の原点とし, その座標をワールド座標系で
W
K
T r (可変; 未知), 力覚座標系で T r (固定; 未知) と表す.
W
座標軸はすべて力覚座標系と平行に設定し, W
T R = KR
(可変; 既知) とする. 物体と手リンク, 力覚センサは互
いに固定されているので, これらの角速度および角加速
度は等しく, それぞれ ω, ω
˙ (可変; 既知) と表す. 力覚
センサが物体に与えた力とトルクを力覚座標系で Kf ,
K
τ と表す.
物体のワールド座標系における運動方程式は,
¨ =
mW
Tr
Iω
˙ =
W K
KR f
W
Kr
−
+ m g,
W
Tr
(2)
W K
KR f
×
+
W K
KR τ
(3)
となる. ただし g はワールド座標系における重力加速
度 (固定; 既知) とした.
W
W
˙ W
¨ は可変で未知であるが,
T r とその時間微分 T r,
Tr
W
Tr
W
T r˙
W
¨
Tr
=
W K
KR T r
=
K
[ ω× ] W
KR T r
=
K
WW
KR T r
+
W
Kr,
+
+
(4)
W
˙
Kr,
(5)
W
¨
Kr
(6)
と, 可変既知量および固定未知量で表すことができる.
ただし W = ( [ ω×
˙ ] + [ ω× ]2 ) とした.
eq.(2) の運動方程式に eq.(6) を代入して変形し,
eq.(3) と合わせて既知量と未知量をまとめ, Ax = 0
の形式で表す. そのとき,
A=
x=
W K
KR f
0
1/m
0
W T ˙
KR Ω
IT
K T
Tr
−W W
KR
−
K
f×
−W
¨+g
Kr
−Kτ
,
T
1
となる. これに Sec.2. の計算法を適用する. 得られ
た x には最終要素を 1 にする正規化を行う. ただし
T
となる. これに Sec.2. の計算法を適用する. 得られた x
には最終要素を 1 とする正規化を行なう. 要素 H
Ke1 /m,
H
H
H
e
/m,
e
/m
を正規直交化し
,
R
が得られる
. また
K 2
K 3
K
H
H
T r も得られる. m は得られた KR および x の対応する
要素の比の平均によって求める.
以上の同定が適切と判定されたとする. 運動方程式
eq.(3) に eq.(7) を代入し, 既知量と未知量を B y = 0 の
形式で表す. このとき,
I = { Iij }, ω
˙ = [ ω˙ 1 ω˙ 2 ω˙ 3 ] として,
I=
T
I11
I22
I33
I12
I23
I31
ω˙ 1
˙ = ⎣ 0
Ω
0
0
ω˙ 2
0
0
0
ω˙ 3
ω˙ 2
ω˙ 1
0
0
ω˙ 3
ω˙ 2
ω˙ 3
0 ⎦
ω˙ 1
⎡
⎤
,
と置いた.
4·2 力覚センサ配置の調整法
Sec.4·1 ではワールド座標系における力覚センサの運
W
動 (W
Kr, KR) を既知と仮定した. この条件を満たすに
は, 身体運動に対する力覚センサの配置の調整が不可
欠と言える. 以下では運動が既知の座標系に対して力
覚座標系が固定されているとする. 本節では慣性パラ
メータの調整を伴う力覚センサ配置の調整に, Sec.2. の
計算法を適用する方法を示す.
W
T r˙
W
¨
Tr
=
W H
HR T r
=
H
[ ω× ] W
HR T r
=
H
WW
HR T r
+
W
Hr,
+
+
Fig.4 Robot
configuration
for
simulation.
(8)
W
˙
Hr,
(9)
W
¨
Hr
(10)
と書ける. eq.(2) に eq.(7) と eq.(10) を代入すると,
H
m WW
HRT r +
W
¨
Hr
=
W H K
HRKR f
+ mg
(11)
T
T
H
K
K
H
H
K
K
となる. H
KR = [ Ke1 Ke2 Ke3 ] , f = [ f x f y f z ]
を用いて eq.(11) の未知量をまとめるように変形し,
Ax = 0 の形式で表す. このとき,
A=
K
f xW
HR
x=
K
f yW
HR
H T
Ke1 / m
K
W
W
f zW
¨+ g
HR −W HR − Hr
H T
Ke2 / m
H T
Ke3 / m
H T
Tr
T
1
K
f×
−Kτ
T
K T
Tr
1
実験
(7)
と表される. また W
T r およびその時間微分は,
W
Tr
IT
Average expended time [sec]
以下では既知座標系を便宜的にハンド座標系と呼ぶ.
まずハンド座標系に対する力覚座標系の姿勢 H
KR, ハン
r
力覚センサか
ド座標系における物体座標系の原点 H
T
ら先の質量 m を調整する. 次に得られたパラメータを
利用して, 慣性テンソル I, 力覚座標系における物体座
標系の原点 K
T r を調整し, ハンド座標系における力覚座
標系の原点 H
Kr を求める (Fig.3). 慣性パラメータの変
化に比べ, 力覚センサの配置は頻繁に変化しないと考え
られる. そのため物体の把持などによって動的に変化
する慣性パラメータを持続的に調整するには, Sec.4·1
の方法が適している.
座標系間の姿勢の関係は,
W H
HRKR
y=
−
提案法の実行時間と収束性について実験し, 同定の
適切さの判定に n(C) = 1 を用いる妥当性を示す. 実
験は Neuronics 社製 Katana アーム [11] の自由度構成
(Fig.4) および関節角限界をシミュレートして行なう.
各自由度は関節角限界の範囲で周期運動する (周期 90
± 15 [sec]). 末端リンクの根元には力覚センサを設定
し, シミュレーションステップは 0.01 [sec] とする. ま
たハンド座標系に固定された点は, 実験中つねに視覚セ
ンサによって追跡されるものとする. ノイズは視覚セ
ンサから得られた手先位置 Cr i および力覚センサの出
力 Kf , Kτ に対して印加する. ノイズは一様乱数を用い
て生成する.
Fig.3 Formulation of orientation of kinesthetic sensor.
=
H TW T ˙
KR HR Ω
となる. これに Sec.2. の計算法を適用する. 得られ
た y には最終要素を 1 とする正規化を行なう. 以上で
H
H
H
得られた K
T r, KR, T r を用いて, 最後の未知量が Kr =
H K
H
H
H
−KRT r + T r と求まる. ここで得られた KR と Kr に
よって, 身体運動に対する力覚センサの配置が定まる.
5.
W
KR
B=
,
1.2e-4
8.0e-5
4.0e-5
0
Vision-motion
Inertia
Orientation of
coordination identificaiton kinesthetic sensor
Fig.5 Average expended time
in 10,000 calculation (100
[sec]).
各方法の計算時間を検証するため, 0 から 100 [sec] の
各計算ステップに費された時間を計測した. 視覚運動変
換, 慣性パラメータ, 力覚センサ配置の各同定法につい
て, 計算に Intel 社の Xeon X5482 搭載機を用いた場合
の結果を Fig.5 に示す. 縦軸を時間として, 棒グラフで
平均計算時間を, 誤差棒でその最大値と最小値を表す.
計算時間はすべての方法において, 0.12 [msec] 以内
であった. この結果により, 十分な実時間性があると言
える. 各方法における計算時間の差は, 同定すべきパラ
メータの次元の違いが原因と思われる (視覚運動変換:
16 次元, 慣性パラメータ: 11 次元, 力覚センサ配置: 13
次元 +10 次元). 力覚センサ配置の同定ではまず 13 次
元の調整を行なうので, その際の最小計算時間が Fig.5
に表れている.
Number of
Minumum singular value
smaller singular value
同定の適切さの判定に n(C) = 1 を用いる妥当性を示
すため, 視覚運動変換, 慣性パラメータそれぞれの同定
法について, SN 比を 20 から 200 [dB] に変化させ, そ
の際の n(C) の変化を実験した. 時刻 1, 10, 100 [sec]
での結果を Fig.6 に示す. 横軸はすべて SN 比を, 縦軸
は上のグラフが誤差行列の最小特異値, 下のグラフが最
小特異値の 100 倍以内に入る特異値の数を示している.
これは最小特異値が十分 0 に近い場合, n(C) を表す.
Vision-motion coordination
Inertia identification
t = 1 [sec]
t = 10
t = 100
1.6e-4
1.2e-5
1.2e-4
8.0e-6
8.0e-5
4.0e-6
4.0e-5
0
0
4
6
3
4
2
3
1
1
0
20
60
100
140
200
0
20
Signal-noise ratio [dB]
60
100
140
200
Signal-noise ratio [dB]
Fig.6 Resultant convergence against SNR.
SN 比が低くとも, 視覚運動変換の同定は十分時間を
かければ収束し, n(C) = 1 となった. 一方, 慣性パラ
メータの同定は 60 [dB] 程度より低くなると, 100 [sec]
経過しても収束せず, n(C) = 1 であった. これは力覚セ
ンサへのノイズが大きいことによる同定の失敗を, n(C)
によって判定可能なことを示している. SN 比が低いと
き, 最小特異値は時間経過に従って大きくなっている.
これは零空間が不安定になっていく様子と言える.
SN 比が 70 [dB] のときの時間的推移を Fig.7 に示す.
横軸は時間 (0 から 10 [sec]), 縦軸は Fig.6 と同様であ
る. 上のグラフ中で最小特異値が 0 に近いため, 下のグ
ラフは n(C) を示していると言える. 視覚運動変換, 慣
性パラメータの同定ともに, 時間経過に従って n(C) = 1
に収束した様子が分かる.
Number of
Minimum singular value
smaller singlar value
Vision-motion coordination
Inertia identification
4.0e-8
2.0e-10
3.0e-8
1.0e-10
2.0e-8
5.0e-11
1.0e-8
0
0
5
3
3
2
1
1
0
0
0
2
4
6
8
Elapsed time [sec]
10
0
2
4
6
8
10
Elapsed time [sec]
Fig.7 Resultant convergence against time.
各同定法の真値への収束を調べる. 時刻 10, 100 [sec]
における真値との二乗誤差を Table 1 に示す. すべて
の同定法について 0 に近く, 正しく解が求められたと
言える. また時間を追うごとにより 0 に収束している.
以上の結果により, すべての同定法において, 同定の適
切さの判定に n(C) = 1 を用いるのは妥当だと言える.
Table 1 Time course of square
Time
10 [sec]
Vision-motion coord.
2.32426e-08
Inertia identification
2.63747e-07
Orientation of
kinesthetic sensor
3.25699e-07
6.
error
100 [sec]
1.03752e-10
7.88522e-10
8.29628e-09
結論
身体変化への自律的適応のためには, 持続的な感覚運
動変換の調整—身体変化によって生じる感覚運動変換
の “ずれ” の観測と, 新たな感覚運動変換の同定—が必
要となる. 本研究では持続的に調整を行なうために, 同
定の適切さを判定可能な計算法を示した (Sec.2.). その
計算法の適用として, 既知パターンを仮定しない視覚運
動変換の調整法 (Sec.3.) および力覚運動変換の調整法
(Sec.4.) も示した. 数値実験により, 提案した誤差行列
の零空間の次元による収束判定が有効であることを示
した. また実時間で計算可能なことも示した.
Sujan と Dubowsky の慣性パラメータの同定法 [9] の
ように, より調整しやすい身体運動を生成する方法と提
案法を組み合わせれば, 持続的かつ素早い調整が可能と
考える. 実ロボットに適用し, ロボットの自律性をより
高めることが今後の課題である.
参考文献
[1] G. Berlucchi and S. Aglioti. The body in the brain:
neural bases of corporeal awareness. Trends in Neurosciences, 20, 12, pp. 560–564, 1997.
[2] P. Haggard and D.M. Wolpert. Higher-Order Motor
Disorders, chapter Disorders of Body Scheme. Oxford
Univ. Press, 2005.
[3] F. Dornaika and R. Horaud. Simultaneous robotworld and hand-eye calibration. IEEE Trans. on Rob.
and Autom., 14, 4, pp. 617–622, 1998.
[4] I. Fassi and G. Legnani. Hand to sensor calibration:
A geometrical interpretation of the matrix equation
AX=XB. J. of Robot. Syst., 22, 9, pp. 497–506, 2005.
[5] 鍋嶌厚太, 國吉康夫. 対象同定に基づく道具身体化の計
算論. 日本ロボット学会誌, 25, 6, pp. 897–905, 2007.
[6] G. Liu, K. Iagnemma, S. Dubowsky, and G. Morel. A
base force/torque sensor approach to robot manipulator inertialparameter estimation. In Proc. of 1998
IEEE Intl. Conf. on Rob. and Autom., 4, pp. 3316–
3321, 1998.
[7] M. Gautier and Ph. Poignet. Extended kalman filtering and weighted least squares dynamic identification
of robot. Contr. Eng. Practice, 9, 12, pp. 1361–1372,
2001.
[8] T. Kodek and M. Munih. An identification of body
segment parameters in the upper extremity. In Proc.
of IEEE Region 8 EUROCON 2003 conf.: comput. as
a tool, 2, pp. 197–201, 2003.
[9] V.A. Sujan and S. Dubowsky. An optimal information
method for mobile manipulator dynamic parameter
identification. IEEE/ASME Trans. on Mech., 8, 2,
pp. 215–225, 2003.
[10] 鍋嶌厚太, 國吉康夫. 運動同起による身体視覚像の発見
と体性感覚との結合. 第 7 回計測自動制御学会システム
インテグレーション部門講演会, 2N4-1, 2006.
[11] Neuronics. Katana—intelligent personal robot. http:
//www.neuronics.ch/cms en/web/.
© Copyright 2024 ExpyDoc
