ホーム > 特許ランキング > 本田技研工業株式会社
知財求人 - 知財ポータルサイト「IP Force」
(19)【発行国】日本国特許庁(JP)
(12)【公報種別】公開特許公報(A)
(11)【公開番号】P2024123546
(43)【公開日】2024-09-12
(54)【発明の名称】姿勢推定装置、姿勢推定方法、およびプログラム
(51)【国際特許分類】
G06T 7/70 20170101AFI20240905BHJP
G06T 7/00 20170101ALI20240905BHJP
G06V 10/82 20220101ALI20240905BHJP
G06V 20/64 20220101ALI20240905BHJP
【ＦＩ】
G06T7/70 Z
G06T7/00 350C
G06T7/00 C
G06V10/82
G06V20/64
【審査請求】未請求
【請求項の数】8
【出願形態】ＯＬ
(21)【出願番号】P 2023031054
(22)【出願日】2023-03-01
(71)【出願人】
【識別番号】000005326
【氏名又は名称】本田技研工業株式会社
(74)【代理人】
【識別番号】100165179
【弁理士】
【氏名又は名称】田▲崎▼ 聡
(74)【代理人】
【識別番号】100126664
【弁理士】
【氏名又は名称】鈴木 慎吾
(74)【代理人】
【識別番号】100154852
【弁理士】
【氏名又は名称】酒井 太一
(74)【代理人】
【識別番号】100194087
【弁理士】
【氏名又は名称】渡辺 伸一
(72)【発明者】
【氏名】奈良村 五十志
【テーマコード（参考）】
5L096
【Ｆターム（参考）】
5L096BA05
5L096FA67
5L096HA11
5L096KA04
(57)【要約】
【課題】インハンド操作のような自己遮蔽が発生する環境であっても、物体の姿勢、および物体を操作する操作装置の姿勢のうちのすくなくとも１つを高精度、かつ効率的に推定することが可能になる姿勢推定装置、姿勢推定方法、およびプログラムを提供することを目的とする。
【解決手段】姿勢推定装置は、実データと動作環境で得られたデータとの一貫性が保たれる操作装置の姿勢および操作対象物体の姿勢のうちの少なくとも１つを逐次的に推定で用いるレンダラーに微分可能レンダラーを用い、操作装置のニューラル三次元表現と、操作対象物体毎のニューラル三次元表現とをそれぞれに学習し、それぞれの学習結果を、占有空間を利用して組み合わせたデータを前記レンダラーに入力することで、操作対象物体の姿勢、および操作装置の姿勢のうちの少なくとも１つを推定する姿勢推定部、を備える。
【選択図】図８
【解決手段】姿勢推定装置は、実データと動作環境で得られたデータとの一貫性が保たれる操作装置の姿勢および操作対象物体の姿勢のうちの少なくとも１つを逐次的に推定で用いるレンダラーに微分可能レンダラーを用い、操作装置のニューラル三次元表現と、操作対象物体毎のニューラル三次元表現とをそれぞれに学習し、それぞれの学習結果を、占有空間を利用して組み合わせたデータを前記レンダラーに入力することで、操作対象物体の姿勢、および操作装置の姿勢のうちの少なくとも１つを推定する姿勢推定部、を備える。
【選択図】図８
【特許請求の範囲】
【請求項1】
実データと動作環境で得られたデータとの一貫性が保たれる操作装置の姿勢および操作対象物体の姿勢のうちの少なくとも１つを逐次的に推定で用いるレンダラーに微分可能レンダラーを用い、前記操作装置のニューラル三次元表現と、前記操作対象物体毎のニューラル三次元表現とをそれぞれに学習し、それぞれの学習結果を、占有空間を利用して組み合わせたデータを前記レンダラーに入力することで、前記操作対象物体の姿勢、および前記操作装置の姿勢のうちの少なくとも１つを推定する姿勢推定部、
を備える姿勢推定装置。
を備える姿勢推定装置。
【請求項2】
前記動作環境の対象は、関節を有する物体であり、
前記微分可能レンダラーはＥｉｇｅｎ ＮｅＲＦまたはＮｅＲＦである、
請求項１に記載の姿勢推定装置。
前記微分可能レンダラーはＥｉｇｅｎ ＮｅＲＦまたはＮｅＲＦである、
請求項１に記載の姿勢推定装置。
【請求項3】
前記姿勢推定部は、
外界センサの姿勢を取得し、
物体座標系基準の外界センサ座標変換行列によって、前記外界センサの原点から所定の画素を結んだ直線である各Ｒａｙの座標系を前記物体座標系基準に座標変換して前記レンダラーに入力して操作対象物体の画像をレンダリングし、
操作装置座標系基準における前記操作装置の関節角度情報を取得して前記レンダラーに入力して前記操作装置の画像をレンダリングし、
レンダリングした前記操作対象物体の画像と前記操作装置の画像と、観測画像との輝度差に基づく第１損失関数を算出し、
前記占有空間において、前記操作装置の占有空間に、前記操作対象物体が侵入しているか否かを示す第２損失関数を算出し、
前記第１損失関数と前記第２損失関数を加算し、関した損失関数が最小となる前記操作対象物体の姿勢をロボット座標系基準で算出する、
請求項１または請求項２に記載の姿勢推定装置。
外界センサの姿勢を取得し、
物体座標系基準の外界センサ座標変換行列によって、前記外界センサの原点から所定の画素を結んだ直線である各Ｒａｙの座標系を前記物体座標系基準に座標変換して前記レンダラーに入力して操作対象物体の画像をレンダリングし、
操作装置座標系基準における前記操作装置の関節角度情報を取得して前記レンダラーに入力して前記操作装置の画像をレンダリングし、
レンダリングした前記操作対象物体の画像と前記操作装置の画像と、観測画像との輝度差に基づく第１損失関数を算出し、
前記占有空間において、前記操作装置の占有空間に、前記操作対象物体が侵入しているか否かを示す第２損失関数を算出し、
前記第１損失関数と前記第２損失関数を加算し、関した損失関数が最小となる前記操作対象物体の姿勢をロボット座標系基準で算出する、
請求項１または請求項２に記載の姿勢推定装置。
【請求項4】
前記姿勢推定部は、
操作対象物体座標系基準の前記操作対象物体の占有マップと、操作装置座標系基準の前記操作装置の占有マップと、をワールド座標系基準に座標変換する、
請求項１または請求項２に記載の姿勢推定装置。
操作対象物体座標系基準の前記操作対象物体の占有マップと、操作装置座標系基準の前記操作装置の占有マップと、をワールド座標系基準に座標変換する、
請求項１または請求項２に記載の姿勢推定装置。
【請求項5】
前記姿勢推定部は、背景のニューラル三次元表現を学習し、学習した前記背景のニューラル三次元表現も前記レンダラーに入力することで、前記操作対象物体の姿勢、および前記操作装置の姿勢のうちの少なくとも１つを推定する、
請求項１または請求項２に記載の姿勢推定装置。
請求項１または請求項２に記載の姿勢推定装置。
【請求項6】
前記姿勢推定部は、外界センサの原点から所定の画素を結んだ直線である各Ｒａｙの座標系をワールド座標系における外界センサ座標系に座標変換して前記背景の画像をレンダリングし、
レンダリングした前記操作対象物体の画像と前記操作装置の画像と前記背景の画像と、観測画像との輝度差に基づく第１損失関数を算出する、
請求項５に記載の姿勢推定装置。
レンダリングした前記操作対象物体の画像と前記操作装置の画像と前記背景の画像と、観測画像との輝度差に基づく第１損失関数を算出する、
請求項５に記載の姿勢推定装置。
【請求項7】
姿勢推定部が、実データと動作環境で得られたデータとの一貫性が保たれる操作装置の姿勢および操作対象物体の姿勢のうちの少なくとも１つを逐次的に推定で用いるレンダラーに微分可能レンダラーを用い、前記操作装置のニューラル三次元表現と、前記操作対象物体毎のニューラル三次元表現とをそれぞれに学習し、
前記姿勢推定部が、それぞれの学習結果を、占有空間を利用して組み合わせたデータを前記レンダラーに入力することで、前記操作対象物体の姿勢、および前記操作装置の姿勢のうちの少なくとも１つを推定する、
姿勢推定方法。
前記姿勢推定部が、それぞれの学習結果を、占有空間を利用して組み合わせたデータを前記レンダラーに入力することで、前記操作対象物体の姿勢、および前記操作装置の姿勢のうちの少なくとも１つを推定する、
姿勢推定方法。
【請求項8】
姿勢推定装置のコンピュータに、
実データと動作環境で得られたデータとの一貫性が保たれる操作装置の姿勢および操作対象物体の姿勢のうちの少なくとも１つを逐次的に推定で用いるレンダラーに微分可能レンダラーを用い、前記操作装置のニューラル三次元表現と、前記操作対象物体毎のニューラル三次元表現とをそれぞれに学習させ、
それぞれの学習結果を、占有空間を利用して組み合わせたデータを前記レンダラーに入力することで、前記操作対象物体の姿勢、および前記操作装置の姿勢のうちの少なくとも１つを推定させる、
プログラム。
実データと動作環境で得られたデータとの一貫性が保たれる操作装置の姿勢および操作対象物体の姿勢のうちの少なくとも１つを逐次的に推定で用いるレンダラーに微分可能レンダラーを用い、前記操作装置のニューラル三次元表現と、前記操作対象物体毎のニューラル三次元表現とをそれぞれに学習させ、
それぞれの学習結果を、占有空間を利用して組み合わせたデータを前記レンダラーに入力することで、前記操作対象物体の姿勢、および前記操作装置の姿勢のうちの少なくとも１つを推定させる、
プログラム。
【発明の詳細な説明】
【技術分野】
【0001】
本発明は、姿勢推定装置、姿勢推定方法、およびプログラムに関する。
【背景技術】
【0002】
物体の姿勢を推定する手法としては、操作対象の特徴ベクトルを検出しＰｎＰ（Perspective-n-Point）問題として物体姿勢を推定する古典的な手法や、画像と物体姿勢を直接回帰するＥ２Ｅ（End-to-End）な手法がある。また、近年、ｉＮｅＲＦのようなＮｅＲＦ表現を用いてカメラ姿勢（カメラ座標と物体座標は逆行列で表現できる）を推定する方法が提案されている（例えば、非特許文献１参照）。
【先行技術文献】
【非特許文献】
【0003】
【非特許文献1】Lin Yen-Chen1, Pete Florence, et al., “iNeRF: Inverting Neural Radiance Fields for Pose Estimation”, IROS 2021, 2021
【発明の概要】
【発明が解決しようとする課題】
【0004】
しかしながら、従来技術では、ロボットハンドによる自己遮蔽に弱い、姿勢推定が手と干渉した結果を出力するという課題があった。また、ｉＮｅＲＦでは、カメラ姿勢と輝度差について勾配を見ているだけなので学習が安定しない（ハイパーパラメータ設定が煩雑）。また、ロボットハンドと複数物体の姿勢推定を行うためには、ロボットハンドと物体の組み合わせで学習データを取得する必要があるためデータ収集が困難であった。
【0005】
本発明は、上記の問題点に鑑みてなされたものであって、インハンド操作のような自己遮蔽が発生する環境であっても、物体の姿勢、および物体を操作する操作装置の姿勢のうちのすくなくとも１つを高精度、かつ効率的に推定することが可能になる姿勢推定装置、姿勢推定方法、およびプログラムを提供することを目的とする。
【課題を解決するための手段】
【0006】
（１）上記目的を達成するため、本発明の一態様に係る姿勢推定装置は、実データと動作環境で得られたデータとの一貫性が保たれる操作装置の姿勢および操作対象物体の姿勢のうちの少なくとも１つを逐次的に推定で用いるレンダラーに微分可能レンダラーを用い、前記操作装置のニューラル三次元表現と、前記操作対象物体毎のニューラル三次元表現とをそれぞれに学習し、それぞれの学習結果を、占有空間を利用して組み合わせたデータを前記レンダラーに入力することで、前記操作対象物体の姿勢、および前記操作装置の姿勢のうちの少なくとも１つを推定する姿勢推定部、を備える姿勢推定装置である。
【0007】
（２）（１）の姿勢推定装置において、前記動作環境の対象は、関節を有する物体であり、前記微分可能レンダラーはＥｉｇｅｎ ＮｅＲＦまたはＮｅＲＦであるようにしてもよい。
【0008】
（３）（１）または（２）の姿勢推定装置において、前記姿勢推定部は、外界センサの姿勢を取得し、物体座標系基準の外界センサ座標変換行列によって、前記外界センサの原点から所定の画素を結んだ直線である各Ｒａｙの座標系を前記物体座標系基準に座標変換して前記レンダラーに入力して操作対象物体の画像をレンダリングし、操作装置座標系基準における前記操作装置の関節角度情報を取得して前記レンダラーに入力して前記操作装置の画像をレンダリングし、レンダリングした前記操作対象物体の画像と前記操作装置の画像と、観測画像との輝度差に基づく第１損失関数を算出し、前記占有空間において、前記操作装置の占有空間に、前記操作対象物体が侵入しているか否かを示す第２損失関数を算出し、前記第１損失関数と前記第２損失関数を加算し、関した損失関数が最小となる前記操作対象物体の姿勢をロボット座標系基準で算出するようにしてもよい。
【0009】
（４）（１）から（３）のうちのいずれか１つの姿勢推定装置において、前記姿勢推定部は、操作対象物体座標系基準の前記操作対象物体の占有マップと、操作装置座標系基準の前記操作装置の占有マップと、をワールド座標系基準に座標変換するようにしてもよい。
【0010】
（５）（１）から（４）のうちのいずれか１つの姿勢推定装置において、前記姿勢推定部は、背景のニューラル三次元表現を学習し、学習した前記背景のニューラル三次元表現も前記レンダラーに入力することで、前記操作対象物体の姿勢、および前記操作装置の姿勢のうちの少なくとも１つを推定するようにしてもよい。
【0011】
（６）（５）の姿勢推定装置において、前記姿勢推定部は、外界センサの原点から所定の画素を結んだ直線である各Ｒａｙの座標系をワールド座標系における外界センサ座標系に座標変換して前記背景の画像をレンダリングし、レンダリングした前記操作対象物体の画像と前記操作装置の画像と前記背景の画像と、観測画像との輝度差に基づく第１損失関数を算出するようにしてもよい。
【0012】
（７）上記目的を達成するため、本発明の一態様に係る姿勢推定方法は、姿勢推定部が、実データと動作環境で得られたデータとの一貫性が保たれる操作装置の姿勢および操作対象物体の姿勢のうちの少なくとも１つを逐次的に推定で用いるレンダラーに微分可能レンダラーを用い、前記操作装置のニューラル三次元表現と、前記操作対象物体毎のニューラル三次元表現とをそれぞれに学習し、前記姿勢推定部が、それぞれの学習結果を、占有空間を利用して組み合わせたデータを前記レンダラーに入力することで、前記操作対象物体の姿勢、および前記操作装置の姿勢のうちの少なくとも１つを推定する、姿勢推定方法である。
【0013】
（８）上記目的を達成するため、本発明の一態様に係るプログラムは、姿勢推定装置のコンピュータに、実データと動作環境で得られたデータとの一貫性が保たれる操作装置の姿勢および操作対象物体の姿勢のうちの少なくとも１つを逐次的に推定で用いるレンダラーに微分可能レンダラーを用い、前記操作装置のニューラル三次元表現と、前記操作対象物体毎のニューラル三次元表現とをそれぞれに学習させ、それぞれの学習結果を、占有空間を利用して組み合わせたデータを前記レンダラーに入力することで、前記操作対象物体の姿勢、および前記操作装置の姿勢のうちの少なくとも１つを推定させる、プログラムである。
【発明の効果】
【0014】
（１）～（８）によれば、インハンド操作のような自己遮蔽が発生する環境であっても、物体の姿勢、および物体を操作する操作装置の姿勢のうちのすくなくとも１つを高精度、かつ効率的に推定することが可能になる。
【図面の簡単な説明】
【0015】
【図1】実施形態の概要を説明するための図である。
【図2】ＮｅＲＦの概要を説明するための図である。
【図3】Ｐ－ＮｅＲＦを模式的に表す図である。
【図4】ＮｅＲＦによるシーン表現と微分可能レンダリングの概要を示す図である。
【図5】Ｐ－ＮｅＲＦのネットワーク構成の一例を表す図である。
【図6】第１実施形態に係るシステムの構成例と処理の概要を示す図である。
【図7】各座標系と変換について説明するための図である。
【図8】第１実施形態に係る姿勢推定装置の構成例を示す図である。
【図9】第１実施形態に係る姿勢推定の処理手順のフローチャートである。
【図10】第１実施形態に係る姿勢推定の処理手順のフローチャートである。
【図11】第１実施形態に係る姿勢推定の処理手順のフローチャートである。
【図12】第１実施形態に係るＯｃｃｕｐａｎｃｙ Ｇｒｉｄの構成手順のフローチャートである。
【図13】各座標系と座標変換行列の定義を示す図である。
【図14】第２実施形態に係る処理の概要を示す図である。
【図15】第２実施形態に係る姿勢推定の処理手順のフローチャートである。
【図16】第２実施形態に係る姿勢推定の処理手順のフローチャートである。
【図17】第２実施形態に係るＯｃｃｕｐａｎｃｙ Ｇｒｉｄの構成手順のフローチャートである。
【発明を実施するための形態】
【0016】
以下、本発明の実施の形態について図面を参照しながら説明する。なお、以下の説明に用いる図面では、各部材を認識可能な大きさとするため、各部材の縮尺を適宜変更している。
なお、実施形態を説明するための全図において、同一の機能を有するものは同一符号を用い、繰り返しの説明は省略する。
また、本願でいう「ＸＸに基づいて」とは、「少なくともＸＸに基づく」ことを意味し、ＸＸに加えて別の要素に基づく場合も含む。また、「ＸＸに基づいて」とは、ＸＸを直接に用いる場合に限定されず、ＸＸに対して演算や加工が行われたものに基づく場合も含む。「ＸＸ」は、任意の要素（例えば、任意の情報）である。
なお、実施形態を説明するための全図において、同一の機能を有するものは同一符号を用い、繰り返しの説明は省略する。
また、本願でいう「ＸＸに基づいて」とは、「少なくともＸＸに基づく」ことを意味し、ＸＸに加えて別の要素に基づく場合も含む。また、「ＸＸに基づいて」とは、ＸＸを直接に用いる場合に限定されず、ＸＸに対して演算や加工が行われたものに基づく場合も含む。「ＸＸ」は、任意の要素（例えば、任意の情報）である。
【0017】
（実施形態の概要）
図１は、実施形態の概要を説明するための図である。
図１の例では、作業環境でロボットｒｏｂが、作業の例として対象物体ｏｂｊを把持している。ロボットｒｏｂは、例えば２つ以上の指部を備えるロボットハンドである。このような作業環境には、環境ｓｔａｔ（含む背景）も存在している。実施形態では、環境とロボットハンドと対象物体それぞれについて、ＮｅＲＦ（Neural Radiance Fields）表現を予め学習させる。なお、ＮｅＲＦについては後述する。実施形態では、環境ｓｔａｔ、ロボットｒｏｂ、対象物体ｏｂｊそれぞれにＮｅＲＦ表現を用いる。なお、Ｆθ＊は、ＭＬＰ（Multi Layer Perceptron；多層パーセプトロン）である。
図１は、実施形態の概要を説明するための図である。
図１の例では、作業環境でロボットｒｏｂが、作業の例として対象物体ｏｂｊを把持している。ロボットｒｏｂは、例えば２つ以上の指部を備えるロボットハンドである。このような作業環境には、環境ｓｔａｔ（含む背景）も存在している。実施形態では、環境とロボットハンドと対象物体それぞれについて、ＮｅＲＦ（Neural Radiance Fields）表現を予め学習させる。なお、ＮｅＲＦについては後述する。実施形態では、環境ｓｔａｔ、ロボットｒｏｂ、対象物体ｏｂｊそれぞれにＮｅＲＦ表現を用いる。なお、Ｆθ＊は、ＭＬＰ（Multi Layer Perceptron；多層パーセプトロン）である。
【0018】
なお、環境とロボットハンドの基準座標は同じであるか、または座標変換行列が既知であるとする。ロボットハンドと実際のカメラ姿勢は、例えばカメラ較正やロボットハンドが備えるエンコーダの検出値におって計算可能である。また、ロボットハンドのＮｅＲＦ表現には、後述するＥｉｇｅｎ ＮｅＲＦを用いる。
【0019】
（ＮｅＲＦの概要）
次に、ＮｅＲＦの概要を説明する。ＮｅＲＦは、主に、複数の視点の画像から、任意の視点（新たな視点）の画像を生成するタスク（新規視点画像生成（Novel View Synthesis）という）を実現するために提案された手法である。図２は、ＮｅＲＦの概要を説明するための図である。ＮｅＲＦは、ある視点の三次元位置を示す位置ベクトルｘ→＝（ｘ，ｙ，ｚ）と、その視点からシーンを観測する際の視線の方向を示す方向ベクトルｄ→＝（θ，φ）とが指定されると、視線上においてＮ個のサンプリング点を定め、各サンプリング点の色を示す色ベクトルｃ→＝（Ｒ，Ｇ，Ｂ）と色の密度σを出力する。上付き矢印→はベクトルを表している（図と数式では、太字で示している）。例えば、ｘは三次元空間の幅を表し、ｙは奥行きを表し、ｚは高さを表し。θ及びφのうち一方は仰角を表し、θ及びφのうち他方は方位角を表す。Ｒは赤色を表し、Ｇは緑色を表し、Ｂは青色を表している。
次に、ＮｅＲＦの概要を説明する。ＮｅＲＦは、主に、複数の視点の画像から、任意の視点（新たな視点）の画像を生成するタスク（新規視点画像生成（Novel View Synthesis）という）を実現するために提案された手法である。図２は、ＮｅＲＦの概要を説明するための図である。ＮｅＲＦは、ある視点の三次元位置を示す位置ベクトルｘ→＝（ｘ，ｙ，ｚ）と、その視点からシーンを観測する際の視線の方向を示す方向ベクトルｄ→＝（θ，φ）とが指定されると、視線上においてＮ個のサンプリング点を定め、各サンプリング点の色を示す色ベクトルｃ→＝（Ｒ，Ｇ，Ｂ）と色の密度σを出力する。上付き矢印→はベクトルを表している（図と数式では、太字で示している）。例えば、ｘは三次元空間の幅を表し、ｙは奥行きを表し、ｚは高さを表し。θ及びφのうち一方は仰角を表し、θ及びφのうち他方は方位角を表す。Ｒは赤色を表し、Ｇは緑色を表し、Ｂは青色を表している。
【0020】
図３は、ＮｅＲＦによるシーン表現と微分可能レンダリングの概要を示す図である。ＭＬＰ（Ｆθ）には、観測画像ｇ１１から５次元の入力（位置と視線）が入力され、ＲＧＢと色の密度σを出力し、カメラ光線に沿ったレンダリング画像ｇ１２が合成される。損失関数Ｌは、この観測ピクセル画像ｇ２１におけるＣ（ｒ→）と、レンダリングピクセル画像ｇ２２におけるＣ＾（ｒ→）との差の合計である。なお、Ｃ（ｒ→）は、光線ｒの焦点から見た色であり、色と密度を書けた物を光線上で積分したものである。ＮｅＲＦでは、ボリュームレンダリングを使って三次元オブジェクトを画像としてレンダリングし、得られた画像と、学習データ中の画像とを比較して損失関数を計算し、この損失関数を最小化するようにする（例えば参考文献１参照）。なお、レンダリングは、微分可能レンダラーを用いて行う。
【0021】
参考文献１；Ben Mildenhall, Pratul P. Srinivasan, :NeRF : Representing Scenes as Neural Radiance Fields for View Synthesis”, 10.48550/arXiv.2003.08934 , 2020.
【0022】
（Ｅｉｇｅｎ ＮｅＲＦの概要）
次に、Ｅｉｇｅｎ ＮｅＲＦの概要を説明する。図４は、Ｐ－ＮｅＲＦを模式的に表す図である。図４のように、Ｐ－ＮｅＲＦに対して、高次元化されたベクトルγ（ｘ→）、γ（ｄ→）、γ（ｅ→）が入力される。これを受けてＰ－ＮｅＲＦは、それら高次元化されたベクトルが入力されたことに応じて色ベクトルｃ→と密度σを出力する。Ｐ－ＮｅＲＦによって出力された色ベクトルｃ→と密度σに基づいて、別視点画像データが生成される。
次に、Ｅｉｇｅｎ ＮｅＲＦの概要を説明する。図４は、Ｐ－ＮｅＲＦを模式的に表す図である。図４のように、Ｐ－ＮｅＲＦに対して、高次元化されたベクトルγ（ｘ→）、γ（ｄ→）、γ（ｅ→）が入力される。これを受けてＰ－ＮｅＲＦは、それら高次元化されたベクトルが入力されたことに応じて色ベクトルｃ→と密度σを出力する。Ｐ－ＮｅＲＦによって出力された色ベクトルｃ→と密度σに基づいて、別視点画像データが生成される。
【0023】
図５は、Ｐ－ＮｅＲＦのネットワーク構成の一例を表す図である。Ｐ－ＮｅＲＦ（エンドエフェクタの関節の姿勢によって条件付けられたＮｅＲＦ）は、図５のように、複数の中間層を含む。中間層は、ＦＣ（Fully Connected）層であり、密度σを出力する出力層と、色ベクトルｃ→を出力する出力層とに途中で分岐している。例えば、各中間層の次元数は２５６であってよく、活性化関数はＲｅＬＵであってよい。
【0024】
次元圧縮され次元数が一定のベクトルである固有把持表現ｅ→により条件付けられたＰ－ＮｅＲＦは、次式（１）によって表される。実施形態では、固有把持表現ｅ→により条件付けられたＰ－ＮｅＲＦを「Ｅｉｇｅｎ ＮｅＲＦ（Eigengrasp-conditioned NeRF）」と言う。
【0025】
【数1】
【0026】
Ｅｉｇｅｎ ＮｅＲＦは、γ（ｘ→）、γ（ｄ→）、γ（ｅ→）が入力されたことに応じて色ベクトルｃ→と密度σを出力する。Ｅｉｇｅｎ ＮｅＲＦによって出力された色ベクトルｃ→と密度σを視線方向（方向ベクトルｄ→）上においてボリュームレンダリングすることで、位置ベクトルｘ→が示す三次元位置から観測したときに得られるであろうシーンの別視点画像データが生成される。例えば、ボリュームレンダリングとして、人間が知覚する視覚が網膜に入射する光の輝度から決定されるように、視線方向（方向ベクトルｄ→）上の色を密度に応じて積算することで、別視点画像中の画素値が決定される。
【0027】
３次元空間上を進む光線ｒ上の各サンプル点において、ＮｅＲＦによって決定された各サンプル点の色ベクトルｃ→と密度σから、画像空間上の画素値がボリュームレンダリングによって決定される。この一連のレンダリング手順を定式化する。光線ｒに沿った画像空間上の色Ｃ（ｒ）を決定するために、ボリュームレンダリングでは、オブジェクトＯＢが位置する３次元空間上を通る光線ｒを画像空間上の一点を決定する方向ｄに沿って飛ばす。この方向ｄは、カメラのパラメータから決定される。光線上の密度σ（ｒ→（ｔ））と色ｃ（ｒ→（ｔ），ｄ→）をｔで積分することで、画像面上の色を決定できる。なお、パラメータｔは、カメラの位置を原点としたとき、光線ｒ上の任意の点ｒ（ｔ）はカメラ原点から方向ｄ→に向かってどれだけ進むかを示すパラメータである。
【0028】
また、Ｅｉｇｅｎ ＮｅＲＦでは、ＭＬＰへの入力として、上記のカメラの位置と方向（ｘ，ｙ，ｚ，θ，φ）に加えてロボットハンドの各関節の位置とロール、ピッチ（ｘ，ｙ，ｚ，ｒｏｌｌ，ｐｉｔｃｈ，ｙａｗ）を用いる（例えば、特願２０２２－１４８９７４号参照）。
【0029】
＜第１実施形態＞
本実施形態では、ロボットハンドにＥｉｇｅｎ ＮｅＲＦを用い、対象物体にＮｅＲＦ表現を用いる。本実施形態で求めたいものは、例えばロボットから見た物体の姿勢である。
本実施形態では、ロボットハンドにＥｉｇｅｎ ＮｅＲＦを用い、対象物体にＮｅＲＦ表現を用いる。本実施形態で求めたいものは、例えばロボットから見た物体の姿勢である。
【0030】
図６は、本実施形態に係るシステムの構成例と処理の概要を示す図である。なお、カメラ６０の光線方向ｒは、光線方向Ｒ（二重線文字）の集合の要素である。ロボットハンド５０の関節ｊは、ロボットハンド５０の関節Ｊ（二重線文字）の集合の要素である。ｄ→は光線方向のベクトルである。カメラ６０の位置ｘ→は、［ｘ ｙ ｚ］Ｔ（Ｔは倒置を表す）である。光線ｒは、（ｘ→，ｄ→）＝（ｘ，ｙ，ｚ，θ，φ）の集合に含まれている。座標変換Ｇ→は、（Ｒ→，ｔ）の集合に含まれている。カメラ座標の原点は、Ｏｃａｍｅｒａである。ロボット座標の原点は、Ｏｒｏｂｏｔである。対象物体ｏｂｊ座標の原点は、Ｏｏｂｊｅｃｔである。
【0031】
ロボットハンド５０に関する第１のＭＬＰのＦθ＿ｒｏｂｏｔの入力は、カメラ６０の位置と方向と、ロボットハンド５０の関節毎のパラメータ（ｉは関節の番号であり、１～Ｐの間の整数）であり、出力は（ｒ，ｇ，ｂ，σ）である。
対象物体ｏｂｊに関する第２のＭＬＰのＦθ＿ｏｂｊｅｃｔの入力は、（γ（ｘ→）（Ｇ→）），（γ（ｄ→）（Ｇ→））であり、出力は（ｒ，ｇ，ｂ，σ）である。
対象物体ｏｂｊに関する第２のＭＬＰのＦθ＿ｏｂｊｅｃｔの入力は、（γ（ｘ→）（Ｇ→）），（γ（ｄ→）（Ｇ→））であり、出力は（ｒ，ｇ，ｂ，σ）である。
【0032】
この２つのＭＬＰの出力に重み付けされ加算され、レンダリングピクセル画像が生成される。そして、レンダリングピクセル画像における色Ｃ＾（ｒ→）と、観測ピクセル画像における色Ｃ（ｒ→）との損失関数を最小にするようにバックプロパゲーション（Ｂａｃｋｐｒｏｐａｇａｔｉｏｎ）によってＭＬＰが学習される。
【0033】
（座標系と変換）
しかしながら、本実施形態では、複数のＮｅＲＦを用いるにあたり、座標系が異なっているため、座標系の変換が必要である。
図７は、各座標系と変換について説明するための図である。図７のように、ロボットハンド５０の作業空間には、センサ（カメラ６０）の座標系（以下、「外界センサの座標系」ともいう）と、ワールド座標系と、ロボット座標系と、対象物体の座標系がある。
しかしながら、本実施形態では、複数のＮｅＲＦを用いるにあたり、座標系が異なっているため、座標系の変換が必要である。
図７は、各座標系と変換について説明するための図である。図７のように、ロボットハンド５０の作業空間には、センサ（カメラ６０）の座標系（以下、「外界センサの座標系」ともいう）と、ワールド座標系と、ロボット座標系と、対象物体の座標系がある。
【0034】
ｗｏｒｌｄＴｒｏｂｏｔは、ワールド座標からロボット座標への変換である。ｗｏｒｌｄＴｓｅｎｓｏｒは、ワールド座標からセンサ座標への変換である。ｒｏｂｏｔＴｏｂｊｅｃｔは、ロボット座標から対象物体の座標への変換である。ｒｏｂｏｔＴｓｅｎｓｏｒは、センサ座標からロボット座標への変換である。ｏｂｊｅｃｔＴｓｅｎｓｏｒは、対象物体の座標からセンサ座標への変換である。
【0035】
ここで、ロボット座標から見たセンサの位置は、ロボットハンド５０が備えるエンコーダの値を用いて求めるか、ｉＮｅＲＦの手法で求めるか、予めカメラ６０とロボットハンド５０の関係をキャリブレーションすることで求めることができるため既知である。このため、ｗｏｒｌｄＴｒｏｂｏｔと、ｒｏｂｏｔＴｓｅｎｓｏｒは既知である。
ワールド座標からセンサ座標への変換ｗｏｒｌｄＴｓｅｎｓｏｒは、図７のように、ｗｏｒｌｄＴｒｏｂｏｔとｒｏｂｏｔＴｓｅｎｓｏｒをかけることで求めることができるので既知である。
ワールド座標からセンサ座標への変換ｗｏｒｌｄＴｓｅｎｓｏｒは、図７のように、ｗｏｒｌｄＴｒｏｂｏｔとｒｏｂｏｔＴｓｅｎｓｏｒをかけることで求めることができるので既知である。
【0036】
これに対して、ロボット座標から対象物体の座標への変換ｒｏｂｏｔＴｏｂｊｅｃｔは未知である。
また、対象物体の座標からセンサ座標への変換ｏｂｊｅｃｔＴｓｅｎｓｏｒは、ｒｏｂｏｔＴｏｂｊｅｃｔ） －１・ｒｏｂｏｔＴｓｅｎｓｏｒで求められるがｒｏｂｏｔＴｏｂｊｅｃｔが未知のため、ｏｂｊｅｃｔＴｓｅｎｓｏｒも未知である。
なお、本実施形態では、ワールド座標系から見たロボット座標系の移動量を、自己位置推定アルゴリズムを用いて推定する。なお、自己位置推定アルゴリズムには、例えばセンサオドメトリを用いる。または、Ｌｉｄａｒセンサを用いたＡｄａｐｔｉｖｅ Ｍｏｎｔｅ Ｃａｒｌｏ Ｌｏｃａｌｉｚａｔｉｏｎ法や、Ｖｉｓｕａｌ ＳＬＡＭ の様にＭＡＰ生成と同時に行ってもよい。また、ロボット座標系のオフセット値は任意の設計値を用いる。そして、ロボット座標系から見たセンサ座標系は、順キネマティック（順運動学）およびセンサ取付位置較正によって計算する。そして、本実施形態では、この２つの座標変換を既知として扱う。
また、対象物体の座標からセンサ座標への変換ｏｂｊｅｃｔＴｓｅｎｓｏｒは、ｒｏｂｏｔＴｏｂｊｅｃｔ） －１・ｒｏｂｏｔＴｓｅｎｓｏｒで求められるがｒｏｂｏｔＴｏｂｊｅｃｔが未知のため、ｏｂｊｅｃｔＴｓｅｎｓｏｒも未知である。
なお、本実施形態では、ワールド座標系から見たロボット座標系の移動量を、自己位置推定アルゴリズムを用いて推定する。なお、自己位置推定アルゴリズムには、例えばセンサオドメトリを用いる。または、Ｌｉｄａｒセンサを用いたＡｄａｐｔｉｖｅ Ｍｏｎｔｅ Ｃａｒｌｏ Ｌｏｃａｌｉｚａｔｉｏｎ法や、Ｖｉｓｕａｌ ＳＬＡＭ の様にＭＡＰ生成と同時に行ってもよい。また、ロボット座標系のオフセット値は任意の設計値を用いる。そして、ロボット座標系から見たセンサ座標系は、順キネマティック（順運動学）およびセンサ取付位置較正によって計算する。そして、本実施形態では、この２つの座標変換を既知として扱う。
【0037】
（Ｏｃｃｕｐａｎｃｙ Ｇｒｉｄ Ｍａｐ）
ここで、Ｏｃｃｕｐａｎｃｙ Ｇｒｉｄマップ（占有格子地図）の概要を説明する。
Ｏｃｃｕｐａｎｃｙ Ｇｒｉｄを用いた手法では、空間を２次元の格子状に区切り、各格子（セル）内にどれだけの確率で物体が存在するかを逐次計算する。なお、格子サイズ、パラメータは予め設定しておく（例えば、参考文献２参照）。
ここで、Ｏｃｃｕｐａｎｃｙ Ｇｒｉｄマップ（占有格子地図）の概要を説明する。
Ｏｃｃｕｐａｎｃｙ Ｇｒｉｄを用いた手法では、空間を２次元の格子状に区切り、各格子（セル）内にどれだけの確率で物体が存在するかを逐次計算する。なお、格子サイズ、パラメータは予め設定しておく（例えば、参考文献２参照）。
【0038】
参考文献２；Payam Nikdel, Richard Vaughan, “Recognizing and Tracking High-Level, Human-Meaningful Navigation Features of Occupancy Grid Maps”, arXiv:1903.03669v1 [cs.RO] , 2019
【0039】
後述するＯｃｃｕｐａｎｃｙ Ｇｒｉｄの構成では、次式（２）を用いて占有マップの座標をワールド座標系基準に変換する。なお、式（２）においてＡＯＢ（Ａはｏｂｊｅｃｔ，ｗｏｒｌｄ，ｒｏｂｏｔ、Ｂはｏｂｊｅｃｔ，ｗｏｒｌｄ，ｒｏｂｏｔ）は、占有マップを表している。なお、ロボット座標から対象物体の座標への変換ｒｏｂｏｔＴｏｂｊｅｃｔが未知であるため、Ｔの上に「～」を付けて表している。なお、左上の添え字は、基準座標系を示している。
【0040】
【数2】
【0041】
なお、ｏｂｊｅｃｔＯｏｂｊｅｃｔは、物体座標系基準における物体の占有マップである。ｒｏｂｏｔＯｒｏｂｏｔは、ロボット座標系基準におけるロボットの占有マップである。ｗｏｒｌｄＯｗｏｒｌｄは、ワールド座標系における背景の占有マップである。ｒｏｒｌｄＯｒ
ｂｊｅｃｔは、ロボット座標系における物体の占有マップである。ｗｏｒｌｄＯｏｂｊｅｃｔは、ワールド座標系における物体の占有マップである。ｗｏｒｌｄＯｒｏｂｏｔは、ワールド座標系におけるロボットの占有マップである。
【0042】
この占有空間における損失関数Ｌｉｎｔｅｒｕｐｔ（Ｇ）（第２損失関数）は、次式（３）で求めることができる。この式の意味合いは、占有マップを用いて物体の重なり具合を損失関数として利用している。
【0043】
【数3】
【0044】
なお、Ωｒｏｂｏｔは、ワールド座標系において、Ｏｃｃｕｐａｎｃｙ Ｇｒｉｄ（占有格子）における占有マップＯｒｏｂｏｔのうちロボットが存在する領域の集合を意味する。また、ｘはΩｏｂｊのボクセルである。
【0045】
また、対象物体の座標からセンサ座標への座標変換Ｇ→による損失関数Ｌｐｈｏｔｏｍｅｔｈｉｃ（Ｇ）（第１損失関数）は、次式（４）となる。
【0046】
【数4】
【0047】
なお、Ｃ（＊ｒ→）は観測画像における画素値であり、Ｃ＾（＊ｒ→）はレンダリング画像における画素値である。＊ｒは、＊座標系におけるｒを表している。
【0048】
本実施形態では、上記の損失関数Ｌｐｈｏｔｏｍｅｔｈｉｃ（Ｇ）と損失関数Ｌｉｎｔｅｒｕｐｔ（Ｇ）を重み付けして加算してＢａｃｋｐｒｏｐａｇａｔｉｏｎで用いる。
【0049】
（姿勢推定装置）
図８は、本実施形態に係る姿勢推定装置の構成例を示す図である。図８のように、姿勢推定装置１は、例えば、Ｅｉｇｅｎ ＮｅＲＦモデル格納部１１と、ＮｅＲＦモデル格納部１２と、外界センサ１３と、姿勢推定部１４と、意図推定部１５と、動作生成部１６と、内界センサ１７と、制御部１８と、駆動部１９と、アクチュエータ２０と、記憶部２１を備える。
図８は、本実施形態に係る姿勢推定装置の構成例を示す図である。図８のように、姿勢推定装置１は、例えば、Ｅｉｇｅｎ ＮｅＲＦモデル格納部１１と、ＮｅＲＦモデル格納部１２と、外界センサ１３と、姿勢推定部１４と、意図推定部１５と、動作生成部１６と、内界センサ１７と、制御部１８と、駆動部１９と、アクチュエータ２０と、記憶部２１を備える。
【0050】
なお、操作対象を操作する操作装置は、例えば、Ｅｉｇｅｎ ＮｅＲＦモデル格納部１１と、ＮｅＲＦモデル格納部１２と、外界センサ１３と、意図推定部１５と、動作生成部１６と、内界センサ１７（カメラ６０）と、制御部１８と、駆動部１９と、アクチュエータ２０と、記憶部２１を備えていてもよい。操作装置は、姿勢推定部１４を備えていてもよい。
【0051】
姿勢推定装置１は、例えば、アームや多指を備えるハンドを備えるロボットハンド、またはロボット等である。なお、ロボットやロボットハンドが、姿勢推定装置１の全ての機能または一部の機能を備えていてもよい。
【0052】
Ｅｉｇｅｎ ＮｅＲＦモデル格納部１１は、ロボットに関して学習済みのＥｉｇｅｎ ＮｅＲＦモデルを格納する。
【0053】
ＮｅＲＦモデル格納部１２は、物体に関するＮｅＲＦモデルを物体毎に格納する。
【0054】
外界センサ１３は、例えば、ロボットが作業を行う環境に設置されているカメラ、またはロボットが備えるカメラである。外界センサ１３は、例えばＲＧＢ（赤緑青）撮影装置であり、撮影したＲＧＢ画像を姿勢推定部１４に出力する。
【0055】
姿勢推定部１４には、外界センサ１３からＲＧＢ画像が入力され、内界センサ１７からロボットの関節角情報が入力される。姿勢推定部１４は、Ｅｉｇｅｎ ＮｅＲＦモデルを用いてロボット画像をレンダリングする。姿勢推定部１４は、ＮｅＲＦモデルを用いて物体画像をレンダリングする。姿勢推定部１４は、実データと動作環境で得られたデータとの一貫性が保たれる操作装置の姿勢および操作対象物体の姿勢のうちの少なくとも１つを逐次的に推定で用いるレンダラーに微分可能レンダラーを用いる。姿勢推定部１４は、操作装置のニューラル三次元表現と、操作対象物体毎のニューラル三次元表現とをそれぞれに学習する。姿勢推定部１４は、それぞれの学習結果を、占有空間を利用して組み合わせたデータをレンダラーに入力することで、操作対象物体の姿勢、およびの姿勢のうちの少なくとも１つを推定する。物理シミュレータの対象は、関節を有する物体であり、例えばロボットやロボットハンドである。微分可能レンダラーは、Ｅｉｇｅｎ ＮｅＲＦまたはＮｅＲＦである。なお、姿勢推定部１４の処理の詳細は後述する。
【0056】
意図推定部１５は、作業者が入力した操作入力情報に基づいて、作業者の作業に関する意図推定を行って手先目標姿勢を生成する。意図推定部１５は、生成した手先目標姿勢を動作生成部１６に出力する。作業者は、例えば手にデータグローブを付けて手を動かして操作入力してもよい。なお、意図推定方法は後述する。
【0057】
動作生成部１６は、姿勢推定部１４が出力する物体姿勢情報と、意図推定部１５が出力する手先目標姿勢を用いて、ロボットに対する軌道列を生成する。
【0058】
内界センサ１７は、関節毎に取り付けられているセンサである。内界センサ１７は、指先に付けられている６軸センサである。内界センサ１７は、アクチュエータに取り付けられているエンコーダである。
【0059】
制御部１８は、動作生成部１６から軌道列を取得し、内界センサ１７から検出値を受け取る。制御部１８は、受け取った軌道列と検出値を用いて、ロボットハンドの手先で対象物体を把持させる等、手先目標姿勢となるようにアクチュエータを駆動する動作指示を生成する。
【0060】
駆動部１９は、制御部１８が出力する動作指示に応じてアクチュエータ２０を駆動する。
【0061】
アクチュエータ２０は、駆動部１９の駆動に応じて動作する。アクチュエータ２０は、例えば、ロボットハンドの指部の関節、掌の関節、手首、腕等に取り付けられている。
【0062】
記憶部２１は、初期値、座標変換式、損失関数の式やパラメータ等を記憶する。
【0063】
（意図推定）
次に、意図推定方法を説明する。
意図推定部１５は、取得した操作入力情報を用いて、作業内容であるタスクを推定する。なお、タスクは、例えば、物体の把持、物体を把持して移動、物体を把持して移動後に置く等である。意図推定部１５は、例えば特願２０２１－０５８９５２号の手法を用いて意図推定を行う。意図推定部１５は、学習済みのモデルを備えていてもよい。この場合、また、意図推定部１５は、例えば、作業者の指先の位置情報と、操作者の視線情報に基づいて対象物体を推定するようにしてもよい。この場合、作業者は、操作入力を、データグローブに加えて、視線検出可能手段を備えるＨＭＤ（ヘッドマウントディスプレイ）を装着して作業を行う。また、推定されるタスクは、例えば作業者が行おうとしている作業の分類（タクソノミー（参考文献３参照））である。
次に、意図推定方法を説明する。
意図推定部１５は、取得した操作入力情報を用いて、作業内容であるタスクを推定する。なお、タスクは、例えば、物体の把持、物体を把持して移動、物体を把持して移動後に置く等である。意図推定部１５は、例えば特願２０２１－０５８９５２号の手法を用いて意図推定を行う。意図推定部１５は、学習済みのモデルを備えていてもよい。この場合、また、意図推定部１５は、例えば、作業者の指先の位置情報と、操作者の視線情報に基づいて対象物体を推定するようにしてもよい。この場合、作業者は、操作入力を、データグローブに加えて、視線検出可能手段を備えるＨＭＤ（ヘッドマウントディスプレイ）を装着して作業を行う。また、推定されるタスクは、例えば作業者が行おうとしている作業の分類（タクソノミー（参考文献３参照））である。
【0064】
参考文献３；Thomas Feix, Javier Romero,et al., “The GRASP Taxonomy of Human GraspTypes” IEEE Transactions on Human-Machine Systems (Volume: 46, Issue: 1, Feb.2016),IEEE,p66-77
【0065】
【0066】
（ステップＳ１０１）姿勢推定部１４は、外界センサ１３から観測画像を取得する。
【0067】
（ステップＳ１０２）姿勢推定部１４は、取得した観測画像に対して周知の画像処理を行って物体を抽出し、抽出した物体毎に物体ラベルを付与する。姿勢推定部１４は、観測画像に対して物体ラベル毎に領域を分割する。
【0068】
（ステップＳ１０３）姿勢推定部１４は、対象物体が存在しているか否かを判別する。なお、対象物体の個数は１つに限らず、２つ以上であってもよい。姿勢推定部１４は、対象物体が存在する場合（ステップＳ１０４；ＹＥＳ）、ステップＳ１０４の処理に進める。姿勢推定部１４は、対象物体が存在しない場合（ステップＳ１０４；ＮＯ）、処理を終了する。
【0069】
（ステップＳ１０４）姿勢推定部１４は、ロボット座標系基準で外界センサ１３の姿勢を取得する。なお、外界センサ１３（カメラ）の姿勢は、後述するように最適化によって求める。
姿勢推定部１４は、以下の処理のうち、ステップＳ１０５～Ｓ１０９と、ステップＳ１１０～Ｓ１１２の処理を例えば並列で行う。
姿勢推定部１４は、以下の処理のうち、ステップＳ１０５～Ｓ１０９と、ステップＳ１１０～Ｓ１１２の処理を例えば並列で行う。
【0070】
（ステップＳ１０５）姿勢推定部１４は、ＮｅＲＦモデル格納部１２から、物体ラベルに紐付くＮｅＲＦモデルを取得する。
【0071】
（ステップＳ１０６）姿勢推定部１４は、物体姿勢値を初期化する。
【0072】
（ステップＳ１０７）姿勢推定部１４は、物体座標系基準の外界センサ座標系の座標変換行列ｏｂｊＴｓｅｎｓｏｒを記憶部２１から取得する。
【0073】
（ステップＳ１０８）姿勢推定部１４は、ＮｅＲＦモデルへの入力である各Ｒａｙ（センサ原点からある画素を結んだ直線）を、物体座標系基準の外界センサ座標系の座標変換行列ｏｂｊｅｃｔＴｓｅｎｓｏｒを用いて、ロボット座標系から物体座標基準に座標変換する。なお、Ｒａｙは、外界センサ座標系基準であるため、得られた座標変換行列を左から掛ければ物体座標系から見たＲａｙを得ることができる。なお、未知のロボット座標系から見た物体座標系への座標変換行列ｒｏｂｏｔＴｏｂｊｅｃｔの逆行列を、既知のロボット座標系から見た外界センサ座標系ｒｏｂｏｔＴｓｅｎｓｏｒへの座標変換行列に左から掛けることで、物体座標系から見た外界センサ座標系への座標変換行列を得る。なお、ロボット座標系から見た物体座標系への座標変換行列が未知であるため、本実施形態では、適切な初期値で初期化する。この初期値は、後述する最適化演算によって修正されていく。これにより、ロボット座標系から見た物体座標と、ロボット座標系から見た外界センサ座標が与えられる。ロボット座標系から見た物体座標は、初期値を適当に決定し最適化演算によって修正される。ロボット座標系から見た外界センサ座標は、例えばＦＫ（Forward Kinematics；順運動学）を解くなどで計算する。ここで、物体座標系から見た外界センサ座標は、ロボット座標系から見た物体座標の逆行列をロボット座標系から見た外界センサ座標の変換行列に左側から掛けることで得ることができる。
【0074】
（ステップＳ１０９）姿勢推定部１４は、座標変換したＲａｙに基づく情報をＮｅＲＦモデルへ入力して、対象物体の画像をレンダリングする。姿勢推定部１４は、処理後、ステップＳ１１３の処理に進める。
【0075】
（ステップＳ１１０）姿勢推定部１４は、Ｅｉｇｅｎ ＮｅＲＦモデル格納部１１から、Ｅｉｇｅｎ ＮｅＲＦモデルを取得する。
【0076】
（ステップＳ１１１）姿勢推定部１４は、内界センサ１７から、例えばロボットハンドの指部と腕等の関節角度を取得する。
【0077】
（ステップＳ１１２）姿勢推定部１４は、取得した関節角度と外界センサから取得した画像に基づく情報をＥｉｇｅｎ ＮｅＲＦモデルへ入力して、ロボットの画像をレンダリングする。姿勢推定部１４は、処理後、ステップＳ１１３の処理に進める。
【0078】
（ステップＳ１１３）姿勢推定部１４は、ステップＳ１０９で作成した物体のレンダリング画像と、ステップＳ１１２で作成したロボットのレンダリング画像を加算する。なお、それぞれのＭＬＰに入力されるｘ，ｄは、座標変換によって各座標系基準の外界センサ位置姿勢を意味する。なお、図１４において、物体のＭＬＰに入力しているｘ，ｄに入力するＧは、座標変換行列を意味する。この場合は、ロボット座標系基準の外界センサ座標とロボット座標系基準の物体座標との座標変換行列を意味する。なお、後述する背景のＭＬＰにも、ロボット座標系基準の外界センサ座標とロボット座標系基準の世界座標との座標変換行列を入力する必要がある。推定姿勢が正しければ単純に足し合わせるだけでもよく、重みを掛けてから足し合わせてもよい。なお、重みは、各ＭＬＰから出力されるσを正規化するために使用する。
【0079】
（ステップＳ１１４）姿勢推定部１４は、合成した画像と観測画像との輝度差Ｌ１である損失関数Ｌｐｈｏｔｏｍｅｔｈｉｃ（Ｇ）を計算する。
【0080】
（ステップＳ１１５）姿勢推定部１４は、Ｏｃｃｕｐａｎｃｙ Ｇｒｉｄを構成する。なお、Ｏｃｃｕｐａｎｃｙ Ｇｒｉｄの構成方法は後述する。
【0081】
（ステップＳ１１６）姿勢推定部１４は、ロボットのＯｃｃｕｐａｎｃｙ Ｇｒｉｄに物体がどの程度侵入しているかを示すＬ２である損失関数Ｌｉｎｔｅｒｕｐｔ（Ｇ）を計算する。そして、後述するステップＳ４０４（図１７）と同様に、各占有マップの座標系を同じにするように座標変換を行う。
【0082】
（ステップＳ１１７）姿勢推定部１４は、ロボット座標系基準で損失関数が最小となる物体姿勢を推定する。なお、損失関数は、損失関数Ｌｉｎｔｅｒｕｐｔ（Ｇ）と、損失関数Ｌｐｈｏｔｏｍｅｔｈｉｃ（Ｇ）を重み付けして加算したものである。なお、姿勢推定部１４は、損失関数が最小となるように、例えば後述するステップＳ１１９（図１３）の条件のように、閾値として設定した残差より小さくなるまで処理を繰り返す。
【0083】
（ステップＳ１１８）姿勢推定部１４は、検出された全ての対象物体についての処理が終了したか否かを判別する。姿勢推定部１４は、検出された全ての対象物体についての処理が終了した場合（Ｓ１１８；ＹＥＳ）、処理を終了する。姿勢推定部１４は、検出された全ての対象物体についての処理が終了していない場合（Ｓ１１８；ＮＯ）、ステップＳ１０７の処理に戻す。
【0084】
【0085】
（ステップＳ２０１）姿勢推定部１４は、物体座標系における対象物体の占有マップを取得する。
【0086】
（ステップＳ２０２）姿勢推定部１４は、ロボット座標系におけるロボットの占有マップを取得する。
【0087】
（ステップＳ２０３）姿勢推定部１４は、ワールド座標系基準で、物体の占有マップとロボットの占有マップの座標変換を行う。なお、姿勢推定部１４は、ロボット座標と物体座標が未知であるため、初期値を与える。初期値は、例えば他の物体姿勢推定法を用いて設定する。姿勢推定部１４は、例えば、物体姿勢推定法に、ＤｅｎｓｅＦｕｓｉｏｎ法（例えばChen Wang, Danfei Xu, et al, “DenseFusion: 6D Object Pose Estimation by Iterative Dense Fusion”,
＜https://sites.google.com/view/densefusion＞参照）を用いる。通常のＶｉｓｉｏｎベースの手法では自己遮蔽等により精度が悪化するため、ＤｅｎｓｅＦｕｓｉｏｎ法を用いて精度向上させることが期待される。
＜https://sites.google.com/view/densefusion＞参照）を用いる。通常のＶｉｓｉｏｎベースの手法では自己遮蔽等により精度が悪化するため、ＤｅｎｓｅＦｕｓｉｏｎ法を用いて精度向上させることが期待される。
【0088】
【0089】
ここで、ワールド座標系のｒは、ｗｏｒｌｄｒ→
ｉ＝ｗｏｒｌｄＴｓｅｎｓｏｒ・ｓｅｎｓｏｒｒ→
ｉで変換可能である。ロボット座標系のｒは、ｒｏｂｏｔｒ→
ｉ＝ｒｏｂｏｔＴｓｅｎｓｏｒ・ｓｅｎｓｏｒｒ→
ｉで変換可能である。物体のｒは、ｏｂｊｒ→
ｉ＝ｏｂｊＴｓｅｎｓｏｒ・ｓｅｎｓｏｒｒ→
ｉで変換可能である。
なお、ｓｅｎｓｏｒｒ→ ｉは、外界センサ１３が撮影した観測画像の画素値のベクトルである。
なお、ｓｅｎｓｏｒｒ→ ｉは、外界センサ１３が撮影した観測画像の画素値のベクトルである。
【0090】
図１３では、未知の座標変換であるロボット座標系から物体座標系をｒｏｂｏｔＴ～
ｏｂｊで示している。また、未知の座標変換である物体座標系からセンサ座標系をｏｂｊＴ～
ｓｅｎｓｏｒで示している。また、図１３において、ベクトルはＲａｙと同じ意味で用いている。
【0091】
本実施形態では、上述したように、ワールド座標系から見たロボット座標系の移動量は、自己位置推定アルゴリズムを用いて推定する。本実施形態では、ロボット座標系のオフセット値に、任意の設計値を用いる。本実施形態では、ロボット座標系から見たセンサ座標系を、順キネマ及びセンサ取付位置較正によって計算する。すなわち、本実施形態では、ワールド座標系から見たセンサ座標系への座標変換を計算によって求める。また、上述したように、本実施形態では、ロボット座標系から見た物体座標系への座標変換行列ｒｏｂｏｔＴｏｂｊｅｃｔの逆行列を、ロボット座標系から見たセンサ座標系への座標変換行列ｒｏｂｏｔＴｓｅｎｓｏｒに左から掛けることで、物体座標系から見たセンサ座標系への座標変換行列を得る。なお、本実施形態では、この２つの座標変換を既知として扱った。
なお、図７を用いて説明したように、ｗｏｒｌｄＴｒｏｂｏｔと、ｒｏｂｏｔＴｓｅｎｓｏｒ、 ｗｏｒｌｄＴｓｅｎｓｏｒは既知である。これに対して、ｒｏｂｏｔＴｏｂｊｅｃｔ、ｏｂｊｅｃｔＴｓｅｎｓｏｒは未知であるため、本実施形態では、適切な初期値で初期化する。この初期値は、最適化演算によって修正されていく。
なお、図７を用いて説明したように、ｗｏｒｌｄＴｒｏｂｏｔと、ｒｏｂｏｔＴｓｅｎｓｏｒ、 ｗｏｒｌｄＴｓｅｎｓｏｒは既知である。これに対して、ｒｏｂｏｔＴｏｂｊｅｃｔ、ｏｂｊｅｃｔＴｓｅｎｓｏｒは未知であるため、本実施形態では、適切な初期値で初期化する。この初期値は、最適化演算によって修正されていく。
【0092】
以上のように、本実施形態では、ロボットハンドのＥｉｇｅｎ ＮｅＲＦ表現と物体のＮｅＲＦ表現を組み合わせた。また、本実施形態では、複数のNeRF表現を用いるため、座標変換を行った後にレンダリングを行うようにした。また本実施形態では、占有空間を用いてデータを組み合わせてレンダラーに入力するようにした。
【0093】
これにより、本実施形態によれば、インハンド操作のような自己遮蔽が発生する環境であっても、物体の姿勢を高精度、かつ効率的、レンダリングが高速であれば高速に推定することが可能になる。また、本実施形態によれば、ロボットハンドと物体のＮｅＲＦ 表現はそれぞれ別で学習して組み合わせるため、様々な物体の組み合わせにも対応できる。
【0094】
＜第２実施形態＞
本実施形態では、図１４のように、ロボットハンドにＥｉｇｅｎ ＮｅＲＦを用い、対象物体と背景にＮｅＲＦ表現を用いる。図１４は、本実施形態に係る処理の概要を示す図である。
なお、背景については、ＮｅＲＦには、（γ（ｘ→）、γ（ｄ→））が入力され、（ｒ、ｇ、ｂ、σ）が出力される。そして、ロボットのＥｉｇｅｎ ＮｅＲＦの出力と、物体のＮｅＲＦの出力と、背景のＮｅＲＦの出力とを加算してレンダリング画像を生成する。なお、背景のＮｅＲＦモデルは、例えばＮｅＲＦモデル格納部１２に格納されている。
本実施形態では、図１４のように、ロボットハンドにＥｉｇｅｎ ＮｅＲＦを用い、対象物体と背景にＮｅＲＦ表現を用いる。図１４は、本実施形態に係る処理の概要を示す図である。
なお、背景については、ＮｅＲＦには、（γ（ｘ→）、γ（ｄ→））が入力され、（ｒ、ｇ、ｂ、σ）が出力される。そして、ロボットのＥｉｇｅｎ ＮｅＲＦの出力と、物体のＮｅＲＦの出力と、背景のＮｅＲＦの出力とを加算してレンダリング画像を生成する。なお、背景のＮｅＲＦモデルは、例えばＮｅＲＦモデル格納部１２に格納されている。
【0095】
（姿勢推定の処理手順）
次に、対象物体の姿勢の推定手順例を、図１１、１５、１６を用いて説明する。
図１５～１６は、本実施形態に係る姿勢推定の処理手順のフローチャートである。なお、第１実施例と同様の処理については、同じ符号を用いて説明を省略する。
次に、対象物体の姿勢の推定手順例を、図１１、１５、１６を用いて説明する。
図１５～１６は、本実施形態に係る姿勢推定の処理手順のフローチャートである。なお、第１実施例と同様の処理については、同じ符号を用いて説明を省略する。
【0096】
（ステップＳ１０１～Ｓ１０４）姿勢推定部１４は、ステップＳ１０１～Ｓ１０４の処理を行う。
姿勢推定部１４は、以下の処理のうち、ステップＳ１０５～Ｓ１０９と、ステップＳ１１０～Ｓ１１２と、ステップＳ３０１～Ｓ３０４の処理を例えば並列で行う。
姿勢推定部１４は、以下の処理のうち、ステップＳ１０５～Ｓ１０９と、ステップＳ１１０～Ｓ１１２と、ステップＳ３０１～Ｓ３０４の処理を例えば並列で行う。
【0097】
（ステップＳ１０５～Ｓ１０９）姿勢推定部１４は、ステップＳ１０５～Ｓ１０９の処理を行う。姿勢推定部１４は、処理後、ステップＳ１１３の処理に進める。
【0098】
（ステップＳ１１０～Ｓ１１２）姿勢推定部１４は、ステップＳ１１０～Ｓ１１２の処理を行う。姿勢推定部１４は、処理後、ステップＳ１１３の処理に進める。
【0099】
（ステップＳ３０１）姿勢推定部１４は、例えばＮｅＲＦモデル格納部１２から、背景のＮｅＲＦモデルを取得する。
【0100】
（ステップＳ３０２）姿勢推定部１４は、ワールド座標系基準の外界センサ座標系の座標変換行列ｗｏｒｌｄＴｓｅｎｓｏｒを記憶部２１から取得する。
【0101】
（ステップＳ３０３）姿勢推定部１４は、ＮｅＲＦモデルへの入力である各Ｒａｙを、ワールド座標系基準の外界センサ座標系の座標変換行列ｗｏｒｌｄＴｓｅｎｓｏｒを用いて、ワールド座標基準に座標変換する。
【0102】
（ステップＳ３０４）姿勢推定部１４は、外界センサ１３の画像に基づく情報をＮｅＲＦモデルへ入力して、背景の画像をレンダリングする。姿勢推定部１４は、処理後、ステップＳ１１３の処理に進める。
【0103】
（ステップＳ１１３）姿勢推定部１４は、ステップＳ１０９で作成した物体のレンダリング画像と、ステップＳ１１２で作成したロボットのレンダリング画像と、ステップＳ３０４で作成した背景のレンダリング画像を合成する。
【0104】
（ステップＳ１１４～Ｓ１１９）姿勢推定部１４は、ステップＳ１１４～Ｓ１１９（図１１）の処理を行う。
【0105】
ここで、仮に、各ＭＬＰに正確な外界センサ位置姿勢が入力されると観測画像と同等（輝度差がない）の画像を合成することになる。これに対して、本実施形態では、レンダリングの段階での座標系が、各ＭＬＰでそれぞれ異なる。本実施形態では、座標変換によって間接的に紐づけられているため、レンダリングされた画像が外界センサ座標からの視点に基づいているようにした。推定した物体姿勢が真値からズレていると、座標変換行列もズレ、その結果、レンダリング結果も観測画像と輝度差が大きくなる。本実施形態では、この輝度差および占有マップの干渉を利用して最適な物体姿勢を推定する。
【0106】
【0107】
（ステップＳ４０１）姿勢推定部１４は、物体座標系における対象物体の占有マップを取得する。
【0108】
（ステップＳ４０２）姿勢推定部１４は、物体座標系における対象物体の占有マップを取得する。
【0109】
（ステップＳ４０３）姿勢推定部１４は、ロボット座標系におけるロボットの占有マップを取得する。
【0110】
（ステップＳ４０４）姿勢推定部１４は、ワールド座標系基準で、背景の占有マップと物体の占有マップとロボットの占有マップの座標変換を行う。なお、上述したステップＳ２０３と同様に、姿勢推定部１４は、ロボット座標と物体座標が未知であるため、初期値を与える。初期値は、例えば他の物体姿勢推定法を用いて設定する。
【0111】
以上のように、本実施形態では、第１実施形態に加えて背景画像情報を用い、背景画像情報をＮｅＲＦモデルに入力するようにした。また、本実施形態では、この背景のＮｅＲＦも用いてレンダリング画像を生成するようにした。
【0112】
これにより、本実施形態によれば、背景も考慮して物体の姿勢を精度良く推定できる。また、本実施形態によれば、静的環境において、背景（つまり、ロボットと物体以外）との干渉を用いることで、推定される姿勢に制約が掛かるので演算量を削減できる。なお、背景を含めない場合は、観測画像から背景部分を除去する処理が追加される。
【0113】
＜変形例＞
なお、上述した各実施形態では、物体の姿勢を推定する例を説明したが、姿勢推定装置１は、例えばロボットの姿勢を推定するようにしてもよい。
例えば、ロボットの占有マップは、ロボットの関節角が変化するたびに変化する。なお、ＮｅＲＦとしてモデル化できていれば、占有マップは変化しない。また、モデル化された以外のものが現れた場合には、外界センサ１３の検出値に基づいて、占有マップを構成するようにしてもよい。
なお、上述した各実施形態では、物体の姿勢を推定する例を説明したが、姿勢推定装置１は、例えばロボットの姿勢を推定するようにしてもよい。
例えば、ロボットの占有マップは、ロボットの関節角が変化するたびに変化する。なお、ＮｅＲＦとしてモデル化できていれば、占有マップは変化しない。また、モデル化された以外のものが現れた場合には、外界センサ１３の検出値に基づいて、占有マップを構成するようにしてもよい。
【0114】
なお、本発明における姿勢推定装置１の全てまたは一部の機能を実現するためのプログラムをコンピュータ読み取り可能な記録媒体に記録して、この記録媒体に記録されたプログラムをコンピュータシステムに読み込ませ、実行することにより姿勢推定装置１が行う処理の全てまたは一部を行ってもよい。なお、ここでいう「コンピュータシステム」とは、ＯＳや周辺機器等のハードウェアを含むものとする。また、「コンピュータシステム」は、ホームページ提供環境（あるいは表示環境）を備えたＷＷＷシステムも含むものとする。また、「コンピュータ読み取り可能な記録媒体」とは、フレキシブルディスク、光磁気ディスク、ＲＯＭ、ＣＤ－ＲＯＭ等の可搬媒体、コンピュータシステムに内蔵されるハードディスク等の記憶装置のことをいう。さらに「コンピュータ読み取り可能な記録媒体」とは、インターネット等のネットワークや電話回線等の通信回線を介してプログラムが送信された場合のサーバやクライアントとなるコンピュータシステム内部の揮発性メモリ（ＲＡＭ）のように、一定時間プログラムを保持しているものも含むものとする。
【0115】
また、上記プログラムは、このプログラムを記憶装置等に格納したコンピュータシステムから、伝送媒体を介して、あるいは、伝送媒体中の伝送波により他のコンピュータシステムに伝送されてもよい。ここで、プログラムを伝送する「伝送媒体」は、インターネット等のネットワーク（通信網）や電話回線等の通信回線（通信線）のように情報を伝送する機能を有する媒体のことをいう。また、上記プログラムは、前述した機能の一部を実現するためのものであってもよい。さらに、前述した機能をコンピュータシステムにすでに記録されているプログラムとの組み合わせで実現できるもの、いわゆる差分ファイル（差分プログラム）であってもよい。
【0116】
以上、本発明を実施するための形態について実施形態を用いて説明したが、本発明はこうした実施形態に何等限定されるものではなく、本発明の要旨を逸脱しない範囲内において種々の変形および置換を加えることができる。
【符号の説明】
【0117】
１…姿勢推定装置、１１…Ｅｉｇｅｎ ＮｅＲＦモデル格納部、１２…ＮｅＲＦモデル格納部、１３…外界センサ、１４…姿勢推定部、１５…意図推定部、１６…動作生成部、１７…内界センサ、１８…制御部、１９…駆動部、２０…アクチュエータ、２１…記憶部、５０…ロボットハンド、６０…カメラ
【図1】
【図2】
【図3】
【図4】
【図5】
【図6】
【図7】
【図8】
【図9】
【図10】
【図11】
【図12】
【図13】
【図14】
【図15】
【図16】
【図17】