JP7109075B2 - 演算装置及びプログラム - Google Patents

Description

本発明は、演算装置及びプログラムに関する。

例えば、ワイヤー、ハーネス、ケーブルなどの弾性体を製品に組み込む際に、弾性体の一端を固定端として他の部品に固定し、他端をロボットなどにより把持しながら移動させる場合がある。このような製品の組み立て工程において、ロボットに把持された状態の弾性体の形状や、固定端に加わる拘束力及び拘束モーメントを推定することができれば有用である。
これに関し、仮想３次元空間内において弾性体の形状、さらには拘束力及び拘束モーメントを推定する技術が開示されている（例えば、特許文献１を参照）。

特開２０１６－１３３９０２号公報

しかしながら、上述した従来技術によると、弾性体の形状、さらには拘束力及び拘束モーメントを推定する演算の計算量が比較的多くなり、演算に時間を要する場合があるという課題があった。

本発明の一実施形態は、弾性体の一端であるベース部の位置及び姿勢と、前記弾性体の他端であるティップ部の目標位置及び目標姿勢とをそれぞれ取得する取得部と、前記取得部が取得する前記ベース部の位置及び姿勢と、前記ベース部の位置及び姿勢を固定する固定部に前記ベース部から加わる力及びモーメントと、前記弾性体が複数のばね関節と剛体リンクが交互に連なる構造体として離散化モデリングされたモデル情報とに基づいて、前記ティップ部の推定位置及び推定姿勢と、前記ベース部から前記ティップ部までの前記弾性体の推定形状とを算出する形状算出部と、前記取得部が取得する前記ティップ部の目標位置及び目標姿勢と、前記形状算出部が算出する前記ティップ部の推定位置及び推定姿勢との誤差を算出する誤差算出部と、前記ティップ部の位置及び姿勢のイタレーション演算毎の変化と、前記弾性体の弾性力によって前記固定部に加わる力及びモーメントのイタレーション演算毎の変化との間の関係を示すヤコビ行列と、前記誤差算出部が算出する前記誤差とに基づいて、前記固定部に加わる力及びモーメントを算出する力モーメント算出部と、今回のイタレーション演算によって算出された前記誤差に基づいて前記力モーメント算出部によって算出された前記固定部に加わる力及びモーメントを前記形状算出部に対して供給することにより、当該力及びモーメントに基づく前記ティップ部の推定位置及び推定姿勢を次回のイタレーション演算として前記形状算出部に算出させるイタレーション制御部と、を備える演算装置である。

本発明の一実施形態は、上述の演算装置において、前記誤差算出部が算出する前記誤差が所定の許容範囲内であるか否かを判定する判定部をさらに備え、前記イタレーション制御部は、今回のイタレーション演算によって算出された前記誤差が、前記許容範囲内でないと前記判定部によって判定された場合に、前記次回のイタレーション演算として前記ティップ部の推定位置及び推定姿勢を算出させる。

本発明の一実施形態は、上述の演算装置において、前記イタレーション制御部は、前記力モーメント算出部が算出する前記力及びモーメントが前記弾性体の状態の特異点を示す場合には、前記力及びモーメントを初期値に戻して、前記形状算出部に前記ティップ部の推定位置及び推定姿勢を算出させる。

本発明の一実施形態は、上述の演算装置において、前記形状算出部は、所定の分割数の積分演算によって前記推定形状を算出し、前記力モーメント算出部は、前記所定の分割数以下の分割数による積分演算によって前記ヤコビ行列の演算を行うことにより、前記固定部に加わる力及びモーメントを算出する。

本発明の一実施形態は、演算装置が備えるコンピュータに、弾性体の一端であるベース部の位置及び姿勢と、前記弾性体の他端であるティップ部の目標位置及び目標姿勢とをそれぞれ取得する取得ステップと、前記取得ステップにおいて取得される前記ベース部の位置及び姿勢と、前記ベース部の位置及び姿勢を固定する固定部に前記ベース部から加わる力及びモーメントと、前記弾性体が複数のばね関節と剛体リンクとが交互に連なる構造体として離散化モデリングされたモデル情報とに基づいて、前記ティップ部の推定位置及び推定姿勢と、前記ベース部から前記ティップ部までの前記弾性体の推定形状とを算出する形状算出ステップと、前記取得ステップにおいて取得される前記ティップ部の目標位置及び目標姿勢と、前記形状算出ステップにおいて算出される前記ティップ部の推定位置及び推定姿勢との誤差を算出する誤差算出ステップと、前記ティップ部の位置及び姿勢のイタレーション演算毎の変化と、前記弾性体の弾性力によって前記固定部に加わる力及びモーメントのイタレーション演算毎の変化との間の関係を示すヤコビ行列と、前記誤差算出ステップにおいて算出される前記誤差とに基づいて、前記固定部に加わる力及びモーメントを算出する力モーメント算出ステップと、今回のイタレーション演算によって算出された前記誤差に基づいて前記力モーメント算出ステップにおいて算出された前記固定部に加わる力及びモーメントに基づく前記ティップ部の推定位置及び推定姿勢を次回のイタレーション演算として算出させるイタレーション制御ステップとを実行させるためのプログラムである。

本発明によれば、弾性体の状態を推定するための計算量を低減することができる演算装置及びプログラムを提供することができる。

本実施形態の演算装置の機能構成の一例を示す図である。 本実施形態の弾性体を把持しているロボットの外観の一例を示す図である。 本実施形態の弾性体の外観の一例を示す図である。 本実施形態の演算装置の動作の一例を示す図である。 本実施形態の演算装置に実装されるアルゴリズムの一例を示す図である。 本実施形態の演算装置による演算結果の一例を示す図である。

［実施形態］
以下、図面を参照して本実施形態の演算装置１０について説明する。
図１は、本実施形態の演算装置１０の機能構成の一例を示す図である。この演算装置１０は、弾性体ＥＲの推定形状ＳＰや、弾性体ＥＲのティップ部ＴＰの推定位置ｐｎ及び推定姿勢Ｆｎ、弾性体ＥＲのベース部ＢＳを固定するためのベース力ｆｂ、ベースモーメントｍｂを算出する。以下の説明において、弾性体ＥＲの推定形状ＳＰ、弾性体ＥＲのティップ部ＴＰの推定位置ｐｎ、推定姿勢Ｆｎ、弾性体ＥＲのベース部ＢＳを固定するためのベース力ｆｂ、ベースモーメントｍｂを総称して「弾性体ＥＲの状態」とも記載する。
図２を参照して弾性体ＥＲについて説明する。

［弾性体ＥＲについて］
図２は、本実施形態の弾性体ＥＲを把持しているロボットＲＢの外観の一例を示す図である。この一例では、ロボットＲＢとは、単腕多関節ロボットであり、腕の先端に把持部ＨＤを備えている。この把持部ＨＤは、種々の物体を把持可能である。この一例では、把持部ＨＤは、弾性体ＥＲのティップ部ＴＰを把持する。
この一例において、弾性体ＥＲは、ポリエチレンテレフタレート（ＰＥＴ）やポリイミドなどの合成樹脂や、鋼板などの金属などの可撓性を有する材料によって構成され、例えば、全体が均一な厚みと幅によって形成されている。弾性体ＥＲの端部のうち、位置及び姿勢が固定されている側をベース部ＢＳと、ロボットＲＢの把持部ＨＤに把持されて移動可能である側をティップ部ＴＰという。以下の説明において、ベース部ＢＳを弾性体ＥＲの固定端ともいい、ティップ部ＴＰを弾性体ＥＲの移動端ともいう。この弾性体ＥＲは、その全長にわたって各部が互いにほぼ等しいばね乗数を有する。
ベース部ＢＳの位置及び姿勢が固定されている状態においてティップ部ＴＰの位置及び姿勢を変化させると、弾性体ＥＲは、弾性体ＥＲの性状（形状、材質など）と、ベース部ＢＳの位置及び姿勢とティップ部ＴＰの位置及び姿勢との相対関係によって定まる形状に変化する。
弾性体ＥＲのベース部ＢＳ（固定端）の位置及び姿勢を固定する固定部には、弾性体ＥＲの弾性力に基づく力及びモーメントが加わる。この弾性体ＥＲから固定部に対して加わる力及びモーメントを、ベース力ｆｂ及びベースモーメントｍｂとよぶ。このベース力ｆｂ及びベースモーメントｍｂは、ティップ部ＴＰの位置及び姿勢の変化に伴い、すなわち弾性体ＥＲの形状変化に伴い変化する。ここで、ベース部ＢＳ及びティップ部ＴＰの位置及び姿勢について図３を参照して説明する。

図３は、本実施形態の弾性体ＥＲの外観の一例を示す図である。弾性体ＥＲのベース部ＢＳ及びティップ部ＴＰの位置及び姿勢は、次のようにして示される。すなわち、ベース部ＢＳの位置は、グローバル３次元直交座標系の座標軸（ｘｇ、ｙｇ、ｚｇ）におけるベース部ＢＳの座標によって示される。また、ベース部ＢＳの姿勢は、座標軸（ｘｇ、ｙｇ、ｚｇ）に対する、ベース部ＢＳの局所３次元直交座標系の座標軸（ｘｂ、ｙｂ、ｚｂ）の方向によって示される。同様に、ティップ部ＴＰの位置は、グローバル３次元直交座標系の座標軸（ｘｇ、ｙｇ、ｚｇ）におけるティップ部ＴＰの座標によって示される。また、ティップ部ＴＰの姿勢は、座標軸（ｘｇ、ｙｇ、ｚｇ）に対する、ティップ部ＴＰの局所３次元直交座標系の座標軸（ｘｔ、ｙｔ、ｚｔ）の方向によって示される。

なお、以下では、ベース部ＢＳの位置及び姿勢を示す座標軸（ｘｂ、ｙｂ、ｚｂ）と、グローバル３次元直交座標系の座標軸（ｘｇ、ｙｇ、ｚｇ）とが一致しており、ベース部ＢＳの位置及び姿勢を基準にしてティップ部ＴＰの位置及び姿勢が表されるものとして説明する。

［演算装置１０の機能構成及び動作］
演算装置１０の具体的な機能構成及び動作について説明する。
図１に戻り、演算装置１０は、取得部１１０と、形状算出部１２０と、誤差算出部１３０と、力モーメント算出部１４０と、イタレーション制御部１５０と、判定部１６０と、出力部１７０と、記憶部１８０とを備える。なお、この一例においては、演算装置１０が記憶部１８０を内蔵するものとして説明するが、これに限られない。例えば、記憶部１８０がクラウドサーバなどによって実現されるなど、演算装置１０と記憶部１８０とが別々の装置として構成されてもよい。

［モデル情報ＭＤＬについて］
記憶部１８０には、モデル情報ＭＤＬが記憶されている。モデル情報ＭＤＬとは、弾性体ＥＲを複数のばね関節と剛体リンクとが交互に連なる構造体として離散化モデリングした情報である。具体的には、モデル情報ＭＤＬは、弾性体ＥＲを疑似的に、ある単位長さｌの疑似剛体と、疑似剛体どうしを繋ぐ関節と、この関節において疑似剛体どうしを３次元方向にわたって繋ぐ所定のばね定数を有するばねとを含む、ばね関節と剛体リンクとが交互に連なる構造体と見なしてモデリングされている。
弾性体ＥＲのベース部ＢＳからティップ部ＴＰまでの長さ（すなわち弾性体ＥＲの全長）をＬとし、弾性体ＥＲを上述のばね関節によって疑似的に分割する所定の分割数をｎとする。この場合、上述の疑似剛体の長さｌはそれぞれＬ／ｎである。なお、疑似剛体の長さｌは、疑似剛体ごとに互いに異なっていてもよい。
この一例において各ばね関節は３自由度を有し、それぞれの角度を（θ_Ｔ，ｉ）、（θ_Ｎ，ｉ）、（θ_Ｂ，ｉ）∈Ｃとする。ここでインデックスｉは、ばね関節および剛体リンクの配列の順番を示し、その値は１（ベース部ＢＳ側）からｎ（ティップ部ＴＰ側）までである。ばね関節の角度をθ_ｉによって表す。ここで、θ_ｉ＝［θ_Ｔ，ｉθ_Ｎ，ｉθ_Ｂ，ｉ］^Ｔである。ベース部ＢＳ側から数えてｉ番目の剛体リンクの姿勢及び（ｉ－１）番目のばね関節の位置は、式（１）及び式（２）によって示される。

以降の説明において、ベース部ＢＳ側から数えてｉ番目のばね関節のことを単に「ｉ番目のばね関節」とも記載する。ｉ番目のばね関節の回転ＲJ（θ_ｉ）は、単位長方向ベクトルａを軸として角度θだけ回転させる回転作用素Ｒ（ａ，θ）を用いて式（３）によって示される。

ここで、eｘ＝［１ ０ ０］^Ｔ、eｙ＝［０ １ ０］^Ｔ、eｚ＝［０ ０ １］^Ｔである。ここで、［ ］^Ｔとは、行列［ ］の転置行列を示す。
なお、以下の説明において、ベース部ＢＳを０番目の疑似剛体とし、ティップ部ＴＰを（ｎ＋１）番目の疑似剛体とする。また、ベース部ＢＳの位置を基準位置ｐ０と、ベース部ＢＳの姿勢を基準姿勢Ｆ０とも称する。なお、ｐｎはティップ部の位置となる。

図４は、本実施形態の演算装置１０の動作の一例を示す図である。以下、図１及び図４を参照しつつ、演算装置１０の動作の一例について説明する。

（ステップＳ１０）取得部１１０は、弾性体ＥＲの一端である固定端の基準位置ｐ０及び基準姿勢Ｆ０（すなわち、ベース部ＢＳの位置及び姿勢）と、弾性体ＥＲの他端であるティップ部ＴＰの目標位置ｐ＊及び目標姿勢Ｆ＊とをそれぞれ取得する。これら、基準位置ｐ０、基準姿勢Ｆ０、目標位置ｐ＊及び目標姿勢Ｆ＊は、演算装置１０に接続された他の装置などから供給される。演算装置１０に接続された他の装置には、例えば、演算装置１０の上位のコンピュータ装置や、演算装置１０のキーボードやタッチパネルなどの操作デバイスが含まれる（いずれも不図示）。
取得部１１０は、取得した基準位置ｐ０及び基準姿勢Ｆ０を形状算出部１２０に出力する。また、取得部１１０は、取得した目標位置ｐ＊及び目標姿勢Ｆ＊を誤差算出部１３０に出力する。

（ステップＳ２０）形状算出部１２０は、ティップ部ＴＰの推定位置ｐｎ及び推定姿勢Ｆｎと、ベース部ＢＳからティップ部ＴＰまでの弾性体ＥＲの推定形状ＳＰとを算出する。

ここで、ティップ部ＴＰの目標位置ｐ＊及び目標姿勢Ｆ＊とは、ティップ部ＴＰの推定位置ｐｎ及び推定姿勢Ｆｎの目標値である。すなわち、ティップ部ＴＰの位置と姿勢の推定が完璧である場合、式（４）及び式（５）に示す関係が成り立つ。

各ばね関節におけるモーメントのバランスより、式（６）が得られる。

ここで、Ａｉは、式（７）～式（９）によって示されるｉ番目の関節の３方向のベクトルである。すなわち、Ａｉ：＝［ａ_Ｔ，ｉ ａ_Ｎ，ｉ ａ_Ｂ，ｉ］である。

また、式（６）のＫ_ｄは、（ｋ_ｄｔ，ｋ_ｄｎ，ｋ_ｄｂ）を要素とする対角行列、すなわち、Ｋ_ｄ＝diag（ｋ_ｄｔ，ｋ_ｄｎ，ｋ_ｄｂ）であり、上述したばね関節に含まれる３方向のばねのそれぞれのばね定数を示す。

Ａｉ^－１（Ａｉの逆行列）をＡｉ^Ｔ（Ａｉの転置行列）で近似した上で、この式（６）をθｉについて解くと、式（１０）が得られる。

この式（１０）は、ベース部ＢＳ側からティップ部ＴＰまでの各ばね関節の位置及び姿勢をベース部ＢＳ側から順に求めることにより、ティップ部ＴＰの推定位置ｐｎ及び推定姿勢Ｆｎを算出することができることを示している。
形状算出部１２０は、上述の式（１０）に基づいて、ベース部ＢＳ側からティップ部ＴＰまでの各ばね関節の位置及び姿勢をベース部ＢＳ側から順に求めることにより、ティップ部ＴＰの推定位置ｐｎ及び推定姿勢Ｆｎを算出する。また、形状算出部１２０は、算出した各ばね関節の位置及び姿勢（すなわち、θｉ）をインデックス順に算出することにより、ベース部ＢＳからティップ部ＴＰまでの弾性体ＥＲの推定形状ＳＰを算出する。

すなわち、形状算出部１２０は、取得部１１０が取得する基準位置ｐ０及び基準姿勢Ｆ０（すなわち、ベース部ＢＳの位置及び姿勢）と、ベース部ＢＳを把持する把持部ＨＤに把持部ＨＤに加わるベース力ｆｂ及びベースモーメントｍｂ（すなわち、ベース部ＢＳから加わる力及びモーメント）と、弾性体ＥＲが複数のばね関節と剛体リンクとが交互に連なる構造体として離散化モデリングされたモデル情報ＭＤＬとに基づいて、ティップ部ＴＰの推定位置ｐｎ及び推定姿勢Ｆｎと、ベース部ＢＳからティップ部ＴＰまでの弾性体ＥＲの推定形状ＳＰとを算出する。
形状算出部１２０は、算出したティップ部ＴＰの推定位置ｐｎ、推定姿勢Ｆｎ及び弾性体ＥＲの推定形状ＳＰを出力部１７０に出力する。

（ステップＳ３０）誤差算出部１３０は、取得部１１０が取得するティップ部ＴＰの目標位置ｐ＊及び目標姿勢Ｆ＊と、形状算出部１２０が算出するティップ部ＴＰの推定位置ｐｎ及び推定姿勢Ｆｎとの誤差ｅを算出する。

具体的には、誤差算出部１３０は、取得部１１０が出力するティップ部ＴＰの目標位置ｐ＊及び目標姿勢Ｆ＊と、形状算出部１２０が出力するティップ部ＴＰの推定位置ｐｎ及び推定姿勢Ｆｎとをそれぞれ取得する。誤差算出部１３０は、取得した目標位置ｐ＊及び目標姿勢Ｆ＊と、推定位置ｐｎ及び推定姿勢Ｆｎとの誤差ｅを算出する。ここで、誤差ｅを式（１１）に示す。

ここで、式（１１）中の（ ）^Ｖの定義を説明する前に、作用素[ ×]について説明する。３次元ベクトルa、ｂに対する外積a×ｂを考えたとき、Ｘｂ＝a×ｂとなるような３×３の行列Ｘを見つけることができる。３次元ベクトルaに対して、上記の行列Ｘを割り当てる作用素が[ ×]であり、すなわち、Ｘ＝[a×]である。これとは逆に、行列Ｘに対して、ベクトルaを割り当てる作用素が（ ）^Ｖであり、すなわち、a＝（Ｘ）^Ｖである。また、log（ ）は、行列指数関数（matrix exponential）の逆写像である、行列対数（matrix logarithm）を示す。
誤差算出部１３０は、算出した誤差ｅを力モーメント算出部１４０及び判定部１６０に出力する。

（ステップＳ４０）判定部１６０は、誤差算出部１３０が算出する誤差ｅの大きさが所定の許容範囲ＴＯＬ内であるか否かを判定する。判定部１６０は、誤差ｅの大きさが所定の許容範囲ＴＯＬ内であると判定した場合（ステップＳ４０；ＹＥＳ）には、処理をステップＳ６０に進める。
また、判定部１６０は、誤差ｅの大きさが所定の許容範囲ＴＯＬ内でないと判定した場合（ステップＳ４０；ＮＯ）には、処理をステップＳ５０に進める。

（ステップＳ５０）力モーメント算出部１４０は、ティップ部ＴＰの推定位置ｐｎ及び推定姿勢Ｆｎのイタレーション演算毎の変化と、弾性体ＥＲの弾性力によってベース部ＢＳから固定部に加わるベース力ｆｂ及びベースモーメントｍｂのイタレーション演算毎の変化との間の関係を示すヤコビ行列Ｊと、誤差算出部１３０が算出する誤差ｅとに基づいて、ベース部ＢＳから固定部に加わるベース力ｆｂ及びベースモーメントｍｂを算出する。

［ヤコビ行列Ｊについて］
ここで、記憶部１８０には、ヤコビ行列Ｊが記憶されている。本実施形態のヤコビ行列Ｊとは、ベース部ＢＳにおいて固定部に加わるベース力ｆｂ及びベースモーメントｍｂと、ティップ部ＴＰの推定位置ｐｎ及び推定姿勢Ｆｎとの関係を示す写像のヤコビ行列であり、６行×６列の行列である。
このヤコビ行列Ｊは、式（１２）～式（１９）に示すように、３ｎ行×３ｎ列の行列Ｍと、３ｎ行×６列の行列ｂ及びｃからなる。

ここで、｛Мij｝は、３×３行列Мijを(i,j)ブロック要素に持つブロック行列を意味する。また、Iｍは、ｍ次の単位行列である。
演算装置１０の機能構成について説明を続ける。イタレーション制御部１５０は、イタレーション演算の実行を制御する。本実施形態でいうイタレーションとは、上述したステップＳ２０からステップＳ５０までを反復して実行することにより、ティップ部ＴＰの推定位置ｐｎ及び推定姿勢Ｆｎを、ステップＳ１０において取得されるティップ部ＴＰの目標位置ｐ＊及び目標姿勢Ｆ＊に近づけていく手順をいう。また、イタレーション演算とは、イタレーション１回あたりに実行されるステップＳ２０からステップＳ５０までの一連の演算をいう。

より具体的には、イタレーション制御部１５０は、イタレーション回数Ｎを計数する。イタレーション制御部１５０は、イタレーション演算を実行するごとにイタレーション回数Ｎを加算（インクリメント）する。一例として、イタレーション回数Ｎの初期値は０（ゼロ）である。
力モーメント算出部１４０は、今回（例えば、イタレーション回数Ｎ＝０）のイタレーション演算によって算出された誤差ｅに基づいて固定部に加わるベース力ｆｂ及びベースモーメントｍｂを算出する。
イタレーション制御部１５０は、力モーメント算出部１４０によってベース力ｆｂ及びベースモーメントｍｂが算出されると、イタレーション回数Ｎに１を加算（例えば、イタレーション回数Ｎ＝１に）する。イタレーション制御部１５０は、今回（例えば、イタレーション回数Ｎ＝０）のイタレーション演算によって算出されたベース力ｆｂ及びベースモーメントｍｂを形状算出部１２０に対して供給することにより、次回（例えば、イタレーション回数Ｎ＝１）のイタレーション演算を実行させる。すなわち、イタレーション制御部１５０は、次回のイタレーション演算として、処理をステップＳ２０に戻して各ステップＳを反復して実行させる。

換言すれば、イタレーション制御部１５０は、今回（（Ｎ－１）回目）のイタレーション演算によって算出された誤差ｅに基づいて力モーメント算出部１４０によって算出されたベース力ｆｂ及びベースモーメントｍｂを形状算出部１２０に対して供給することにより、当該ベース力ｆｂ及びベースモーメントｍｂに基づくティップ部ＴＰの推定位置ｐｎ及び推定姿勢Ｆｎを次回（Ｎ回目）のイタレーション演算として形状算出部１２０に算出させる。

なお、演算装置１０が判定部１６０を備える場合には、イタレーション制御部１５０は、今回のイタレーション演算によって算出された誤差ｅの大きさが、許容範囲ＴＯＬ内でないと判定部１６０によって判定された場合に、次回のイタレーション演算としてティップ部ＴＰの推定位置ｐｎ及び推定姿勢Ｆｎを算出させるように構成されてもよい。

（ステップＳ６０）出力部１７０は、例えば、液晶ディスプレイやプリンタなどの出力デバイスに接続されている。出力部１７０は、形状算出部１２０が算出したティップ部ＴＰの推定位置ｐｎ、推定姿勢Ｆｎ及び弾性体ＥＲの推定形状ＳＰを、これら出力デバイスに対して出力する。
また、出力部１７０は、上述のイタレーション演算が収束した場合の、力モーメント算出部１４０が算出したベース力ｆｂ及びベースモーメントｍｂを、出力デバイスに対して出力してもよい。ここで、イタレーション演算が収束した場合とは、ティップ部ＴＰの目標位置ｐ＊及び目標姿勢Ｆ＊に対して、算出された推定位置ｐｎ及び推定姿勢Ｆｎが許容範囲ＴＯＬ内になった場合をいう。すなわちこの場合、出力部１７０は、推定位置ｐｎ及び推定姿勢Ｆｎが許容範囲ＴＯＬ内になった場合の、ベース力ｆｂ及びベースモーメントｍｂを出力する。

［アルゴリズム例］
図５は、本実施形態の演算装置１０に実装されるアルゴリズムの一例を示す図である。ここで、記号||ｅ||とは、式（１１）に示した誤差ｅのノルムである。演算装置１０は、誤差ｅのノルムが、許容範囲ＴＯＬ（同図中に示すε_ｔｏｌ）以上である場合には、以下の演算を繰り返す。すなわち、演算装置１０は、ベース力ｆｂ及びベースモーメントｍｂについての漸化式（すなわち、式（２０））によってベース力ｆｂ及びベースモーメントｍｂを更新する。

ここで、λは非負の実数、また、Ｋｐは６×６の正定行列であり、状況に応じて適度な値に設定する。変数の横に付いている（ｋ）は、ｋ回目のイタレーション演算によって算出された、その変数の値を意味する。
演算装置１０は、ｉ番目のばね関節において生じる局所的なトルクτ_ｉ、ばね関節の角度θ_ｉ＝［θ_Ｔ，ｉθ_Ｎ，ｉθ_Ｂ，ｉ］^Ｔ、（ｉ－１）番目の関節の推定位置（すなわち、推定位置ｐ_ｉ）及びｉ番目の剛体リンクの推定姿勢（すなわち、推定姿勢Ｆ_ｉ）をすべてのばね関節および剛体リンクについて順次算出することにより、ティップ部ＴＰの推定位置ｐｎ及び推定姿勢Ｆｎを算出する。
さらに、演算装置１０は、算出したティップ部ＴＰの推定位置ｐｎ及び推定姿勢Ｆｎと、目標位置ｐ＊及び目標姿勢Ｆ＊との誤差ｅを、上述した式（１１）に基づいて算出する。
演算装置１０は、算出した誤差ｅのノルムが、許容範囲ＴＯＬ（同図中に示すε_ｔｏｌ）未満になれば、算出したティップ部ＴＰの推定位置ｐｎ及び推定姿勢Ｆｎを演算結果ＲＳとして出力する。

［演算結果例］
図６は、本実施形態の演算装置１０による演算結果ＲＳの一例を示す図である。演算対象の弾性体ＥＲの初期形状を図６（ａ）に示す。図６（ａ）に示す初期形状に対して、ベース部ＢＳの位置及び姿勢を変化させることなく、ティップ部ＴＰの位置及び姿勢を変化させた場合の、変化後の弾性体ＥＲの形状を図６（ｂ）に示す。本実施形態の演算装置１０は、図６（ａ）に示すベース部ＢＳの基準位置ｐ０と基準姿勢Ｆ０、及び図６（ｂ）に示すティップ部ＴＰの位置と姿勢（すなわち、目標位置ｐ＊と目標姿勢Ｆ＊）が与えられた場合に、ベース部ＢＳからティップ部ＴＰまでの弾性体ＥＲの形状を「推定形状ＳＰ」として算出する。演算装置１０は、算出した推定形状ＳＰに基づいて図６（ｂ）に示す弾性体ＥＲの形状を描画する。

以上説明したように、演算装置１０は、弾性体ＥＲが複数のばね関節と剛体リンクとが交互に連なる構造体として離散化モデリングされたモデル情報ＭＤＬと、陽に表現されたヤコビ行列Ｊとに基づいて、弾性体ＥＲの形状などの推定演算を行う。

なお、ベース部ＢＳとティップ部ＴＰとの間の相対位置・相対方向や、弾性体ＥＲの材質などによっては、力モーメント算出部１４０が算出するベース力ｆｂ及びベースモーメントｍｂが弾性体ＥＲの状態の特異点を示す場合がある。ここで、弾性体ＥＲの状態の特異点には、例えば、飛び移り座屈（スナップスルー）を起こす際のベース力ｆｂ及びベースモーメントｍｂがある。このような特異点が生じると演算結果ＲＳの精度が低下することがある。
この場合、イタレーション制御部１５０は、ベース力ｆｂ及びベースモーメントｍｂを初期値に戻して、形状算出部１２０にティップ部ＴＰの推定位置ｐｎ及び推定姿勢Ｆｎを算出させる。このように構成することにより、イタレーション制御部１５０は、特異点の存在により演算結果ＲＳの精度が低下する状況を回避できるため、演算結果ＲＳの精度を向上させることができる。

また、上述したように、モデル情報ＭＤＬにおいて、弾性体ＥＲは、所定の分割数ｎのばね関節と剛体リンクとが交互に連なる構造体として離散化モデリングされている。この場合、形状算出部１２０は、所定の分割数ｎの積分演算によって推定形状ＳＰを算出する。ここで、積分演算には、特殊ユークリッド群ＳＥ（３）上の積分が含まれる。
この場合において、力モーメント算出部１４０は、所定の分割数ｎ以下の分割数ｎ＿Ｊによる積分演算によってヤコビ行列Ｊの演算を行うことにより、ベース力ｆｂ及びベースモーメントｍｂを算出する。一例として、所定の分割数ｎ＝５０とした場合、力モーメント算出部１４０は、分割数ｎ＿Ｊ＝２５としてベース力ｆｂ及びベースモーメントｍｂを算出する。

ここで、所定の分割数ｎの値が大きいほど、弾性体ＥＲの形状を高精度に推定することができる。一方で、有限差分によってヤコビ行列Ｊを数値計算的に計算する従来手法によると、所定の分割数ｎの値が大きくなるに従い、ヤコビ行列Ｊの計算量も増加してしまう。すなわち、従来手法によると、弾性体ＥＲの形状推定の高精度化と、形状推定のための計算量の低減とを両立することが困難であった。
一方、本実施形態の演算装置１０は、ヤコビ行列Ｊを陽に表現し、かつヤコビ行列Ｊの積分演算の分割数ｎ＿Ｊを、弾性体ＥＲの離散化モデリングの分割数ｎ以下にしているため、有限差分によってヤコビ行列Ｊを数値計算的に計算する従来手法に比べて、計算量を低減させることができる。
すなわち、本実施形態の演算装置１０は、弾性体ＥＲの形状推定の高精度化と、形状推定のための計算量の低減とを両立することができる。

以上、本発明の実施形態を、図面を参照して詳述してきたが、具体的な構成はこの実施形態に限られるものではなく、本発明の趣旨を逸脱しない範囲で適宜変更を加えることができる。

なお、上述の各装置は内部にコンピュータを有している。そして、上述した各装置の各処理の過程は、プログラムの形式でコンピュータ読み取り可能な記録媒体に記憶されており、このプログラムをコンピュータが読み出して実行することによって、上記処理が行われる。ここでコンピュータ読み取り可能な記録媒体とは、磁気ディスク、光磁気ディスク、ＣＤ－ＲＯＭ、ＤＶＤ－ＲＯＭ、半導体メモリ等をいう。また、このコンピュータプログラムを通信回線によってコンピュータに配信し、この配信を受けたコンピュータが当該プログラムを実行するようにしてもよい。

また、上記プログラムは、前述した機能の一部を実現するためのものであってもよい。
さらに、前述した機能をコンピュータシステムにすでに記録されているプログラムとの組み合わせで実現できるもの、いわゆる差分ファイル（差分プログラム）であってもよい。

１０…演算装置、１１０…取得部、１２０…形状算出部、１３０…誤差算出部、１４０…力モーメント算出部、１５０…イタレーション制御部、１６０…判定部、１７０…出力部、１８０…記憶部、ＥＲ…弾性体、ＢＳ…ベース部、ＴＰ…ティップ部、ＲＢ…ロボット、ＨＤ…把持部、Ｊ…ヤコビ行列、ＭＤＬ…モデル情報

Claims (5)

1. 弾性体の一端であるベース部の位置及び姿勢と、前記弾性体の他端であるティップ部の目標位置及び目標姿勢とをそれぞれ取得する取得部と、
   前記取得部が取得する前記ベース部の位置及び姿勢と、前記ベース部の位置及び姿勢を固定する固定部に前記ベース部から加わる力及びモーメントと、前記弾性体が複数のばね関節と剛体リンクとが交互に連なる構造体として離散化モデリングされたモデル情報とに基づいて、前記ティップ部の推定位置及び推定姿勢と、前記ベース部から前記ティップ部までの前記弾性体の推定形状とを算出する形状算出部と、
   前記取得部が取得する前記ティップ部の目標位置及び目標姿勢と、前記形状算出部が算出する前記ティップ部の推定位置及び推定姿勢との誤差を算出する誤差算出部と、
   前記ティップ部の位置及び姿勢のイタレーション演算毎の変化と、前記弾性体の弾性力によって前記固定部に加わる力及びモーメントのイタレーション演算毎の変化との間の関係を示すヤコビ行列と、前記誤差算出部が算出する前記誤差とに基づいて、前記固定部に加わる力及びモーメントを算出する力モーメント算出部と、
   今回のイタレーション演算によって算出された前記誤差に基づいて前記力モーメント算出部によって算出された前記固定部に加わる力及びモーメントを前記形状算出部に対して供給することにより、当該力及びモーメントに基づく前記ティップ部の推定位置及び推定姿勢を次回のイタレーション演算として前記形状算出部に算出させるイタレーション制御部と、
   を備える演算装置。
2. 前記誤差算出部が算出する前記誤差が所定の許容範囲内であるか否かを判定する判定部
   をさらに備え、
   前記イタレーション制御部は、
   今回のイタレーション演算によって算出された前記誤差が、前記許容範囲内でないと前記判定部によって判定された場合に、前記次回のイタレーション演算として前記ティップ部の推定位置及び推定姿勢を算出させる
   請求項１に記載の演算装置。
3. 前記イタレーション制御部は、
   前記力モーメント算出部が算出する前記力及びモーメントが前記弾性体の状態の特異点を示す場合には、前記力及びモーメントを初期値に戻して、前記形状算出部に前記ティップ部の推定位置及び推定姿勢を算出させる
   請求項１または請求項２に記載の演算装置。
4. 前記形状算出部は、
   所定の分割数の積分演算によって前記推定形状を算出し、
   前記力モーメント算出部は、
   前記所定の分割数以下の分割数による積分演算によって前記ヤコビ行列の演算を行うことにより、前記固定部に加わる力及びモーメントを算出する
   請求項１から請求項３のいずれか一項に記載の演算装置。
5. 演算装置が備えるコンピュータに、
   弾性体の一端であるベース部の位置及び姿勢と、前記弾性体の他端であるティップ部の目標位置及び目標姿勢とをそれぞれ取得する取得ステップと、
   前記取得ステップにおいて取得される前記ベース部の位置及び姿勢と、前記ベース部の位置及び姿勢を固定する固定部に前記ベース部から加わる力及びモーメントと、前記弾性体が複数のばね関節と剛体リンクとが交互に連なる構造体として離散化モデリングされたモデル情報とに基づいて、前記ティップ部の推定位置及び推定姿勢と、前記ベース部から前記ティップ部までの前記弾性体の推定形状とを算出する形状算出ステップと、
   前記取得ステップにおいて取得される前記ティップ部の目標位置及び目標姿勢と、前記形状算出ステップにおいて算出される前記ティップ部の推定位置及び推定姿勢との誤差を算出する誤差算出ステップと、
   前記ティップ部の位置及び姿勢のイタレーション演算毎の変化と、前記弾性体の弾性力によって前記固定部に加わる力及びモーメントのイタレーション演算毎の変化との間の関係を示すヤコビ行列と、前記誤差算出ステップにおいて算出される前記誤差とに基づいて、前記固定部に加わる力及びモーメントを算出する力モーメント算出ステップと、
   今回のイタレーション演算によって算出された前記誤差に基づいて前記力モーメント算出ステップにおいて算出された前記固定部に加わる力及びモーメントに基づく前記ティップ部の推定位置及び推定姿勢を次回のイタレーション演算として算出させるイタレーション制御ステップと、
   を実行させるためのプログラム。

Images