特開2020-42406(P2020-42406A)IP Force 特許公報掲載プロジェクト 2015.5.11 β版

知財求人 - 知財ポータルサイト「IP Force」

▶ 国立大学法人 筑波大学の特許一覧
<>
  • 特開2020042406-演算装置及びプログラム 図000023
  • 特開2020042406-演算装置及びプログラム 図000024
  • 特開2020042406-演算装置及びプログラム 図000025
  • 特開2020042406-演算装置及びプログラム 図000026
  • 特開2020042406-演算装置及びプログラム 図000027
  • 特開2020042406-演算装置及びプログラム 図000028
< >
(19)【発行国】日本国特許庁(JP)
(12)【公報種別】公開特許公報(A)
(11)【公開番号】特開2020-42406(P2020-42406A)
(43)【公開日】2020年3月19日
(54)【発明の名称】演算装置及びプログラム
(51)【国際特許分類】
   G06F 30/18 20200101AFI20200225BHJP
   G06F 30/20 20200101ALI20200225BHJP
【FI】
   G06F17/50 650Z
   G06F17/50 612G
【審査請求】未請求
【請求項の数】5
【出願形態】OL
【全頁数】15
(21)【出願番号】特願2018-167877(P2018-167877)
(22)【出願日】2018年9月7日
(71)【出願人】
【識別番号】504171134
【氏名又は名称】国立大学法人 筑波大学
(74)【代理人】
【識別番号】100106909
【弁理士】
【氏名又は名称】棚井 澄雄
(74)【代理人】
【識別番号】100188558
【弁理士】
【氏名又は名称】飯田 雅人
(74)【代理人】
【識別番号】100169764
【弁理士】
【氏名又は名称】清水 雄一郎
(72)【発明者】
【氏名】望山 洋
【テーマコード(参考)】
5B046
【Fターム(参考)】
5B046AA04
5B046AA07
5B046JA07
(57)【要約】
【課題】弾性体の状態を推定するための計算量を低減する。
【解決手段】演算装置は、取得部が取得するベース部の位置及び姿勢と、固定部にベース部から加わる力及びモーメントと、弾性体が離散化モデリングされたモデル情報とに基づいて、ティップ部の推定位置及び推定姿勢と、弾性体の推定形状とを算出する形状算出部と、ティップ部の目標位置及び目標姿勢とティップ部の推定位置及び推定姿勢との誤差を算出する誤差算出部と、ティップ部の位置及び姿勢のイタレーション演算毎の変化と、固定部に加わる力及びモーメントのイタレーション演算毎の変化との間の関係を示すヤコビ行列と、誤差算出部が算出する誤差とに基づいて、固定部に加わる力及びモーメントを算出する力モーメント算出部と、当該力及びモーメントに基づくティップ部の推定位置及び推定姿勢を次回のイタレーション演算として形状算出部に算出させるイタレーション制御部とを備える。
【選択図】図1
【特許請求の範囲】
【請求項1】
弾性体の一端であるベース部の位置及び姿勢と、前記弾性体の他端であるティップ部の目標位置及び目標姿勢とをそれぞれ取得する取得部と、
前記取得部が取得する前記ベース部の位置及び姿勢と、前記ベース部の位置及び姿勢を固定する固定部に前記ベース部から加わる力及びモーメントと、前記弾性体が複数のばね関節と剛体リンクとが交互に連なる構造体として離散化モデリングされたモデル情報とに基づいて、前記ティップ部の推定位置及び推定姿勢と、前記ベース部から前記ティップ部までの前記弾性体の推定形状とを算出する形状算出部と、
前記取得部が取得する前記ティップ部の目標位置及び目標姿勢と、前記形状算出部が算出する前記ティップ部の推定位置及び推定姿勢との誤差を算出する誤差算出部と、
前記ティップ部の位置及び姿勢のイタレーション演算毎の変化と、前記弾性体の弾性力によって前記固定部に加わる力及びモーメントのイタレーション演算毎の変化との間の関係を示すヤコビ行列と、前記誤差算出部が算出する前記誤差とに基づいて、前記固定部に加わる力及びモーメントを算出する力モーメント算出部と、
今回のイタレーション演算によって算出された前記誤差に基づいて前記力モーメント算出部によって算出された前記固定部に加わる力及びモーメントを前記形状算出部に対して供給することにより、当該力及びモーメントに基づく前記ティップ部の推定位置及び推定姿勢を次回のイタレーション演算として前記形状算出部に算出させるイタレーション制御部と、
を備える演算装置。
【請求項2】
前記誤差算出部が算出する前記誤差が所定の許容範囲内であるか否かを判定する判定部
をさらに備え、
前記イタレーション制御部は、
今回のイタレーション演算によって算出された前記誤差が、前記許容範囲内でないと前記判定部によって判定された場合に、前記次回のイタレーション演算として前記ティップ部の推定位置及び推定姿勢を算出させる
請求項1に記載の演算装置。
【請求項3】
前記イタレーション制御部は、
前記力モーメント算出部が算出する前記力及びモーメントが前記弾性体の状態の特異点を示す場合には、前記力及びモーメントを初期値に戻して、前記形状算出部に前記ティップ部の推定位置及び推定姿勢を算出させる
請求項1または請求項2に記載の演算装置。
【請求項4】
前記形状算出部は、
所定の分割数の積分演算によって前記推定形状を算出し、
前記力モーメント算出部は、
前記所定の分割数以下の分割数による積分演算によって前記ヤコビ行列の演算を行うことにより、前記固定部に加わる力及びモーメントを算出する
請求項1から請求項3のいずれか一項に記載の演算装置。
【請求項5】
演算装置が備えるコンピュータに、
弾性体の一端であるベース部の位置及び姿勢と、前記弾性体の他端であるティップ部の目標位置及び目標姿勢とをそれぞれ取得する取得ステップと、
前記取得ステップにおいて取得される前記ベース部の位置及び姿勢と、前記ベース部の位置及び姿勢を固定する固定部に前記ベース部から加わる力及びモーメントと、前記弾性体が複数のばね関節と剛体リンクとが交互に連なる構造体として離散化モデリングされたモデル情報とに基づいて、前記ティップ部の推定位置及び推定姿勢と、前記ベース部から前記ティップ部までの前記弾性体の推定形状とを算出する形状算出ステップと、
前記取得ステップにおいて取得される前記ティップ部の目標位置及び目標姿勢と、前記形状算出ステップにおいて算出される前記ティップ部の推定位置及び推定姿勢との誤差を算出する誤差算出ステップと、
前記ティップ部の位置及び姿勢のイタレーション演算毎の変化と、前記弾性体の弾性力によって前記固定部に加わる力及びモーメントのイタレーション演算毎の変化との間の関係を示すヤコビ行列と、前記誤差算出ステップにおいて算出される前記誤差とに基づいて、前記固定部に加わる力及びモーメントを算出する力モーメント算出ステップと、
今回のイタレーション演算によって算出された前記誤差に基づいて前記力モーメント算出ステップにおいて算出された前記固定部に加わる力及びモーメントに基づく前記ティップ部の推定位置及び推定姿勢を次回のイタレーション演算として算出させるイタレーション制御ステップと、
を実行させるためのプログラム。
【発明の詳細な説明】
【技術分野】
【0001】
本発明は、演算装置及びプログラムに関する。
【背景技術】
【0002】
例えば、ワイヤー、ハーネス、ケーブルなどの弾性体を製品に組み込む際に、弾性体の一端を固定端として他の部品に固定し、他端をロボットなどにより把持しながら移動させる場合がある。このような製品の組み立て工程において、ロボットに把持された状態の弾性体の形状や、固定端に加わる拘束力及び拘束モーメントを推定することができれば有用である。
これに関し、仮想3次元空間内において弾性体の形状、さらには拘束力及び拘束モーメントを推定する技術が開示されている(例えば、特許文献1を参照)。
【先行技術文献】
【特許文献】
【0003】
【特許文献1】特開2016−133902号公報
【発明の概要】
【発明が解決しようとする課題】
【0004】
しかしながら、上述した従来技術によると、弾性体の形状、さらには拘束力及び拘束モーメントを推定する演算の計算量が比較的多くなり、演算に時間を要する場合があるという課題があった。
【課題を解決するための手段】
【0005】
本発明の一実施形態は、弾性体の一端であるベース部の位置及び姿勢と、前記弾性体の他端であるティップ部の目標位置及び目標姿勢とをそれぞれ取得する取得部と、前記取得部が取得する前記ベース部の位置及び姿勢と、前記ベース部の位置及び姿勢を固定する固定部に前記ベース部から加わる力及びモーメントと、前記弾性体が複数のばね関節と剛体リンクが交互に連なる構造体として離散化モデリングされたモデル情報とに基づいて、前記ティップ部の推定位置及び推定姿勢と、前記ベース部から前記ティップ部までの前記弾性体の推定形状とを算出する形状算出部と、前記取得部が取得する前記ティップ部の目標位置及び目標姿勢と、前記形状算出部が算出する前記ティップ部の推定位置及び推定姿勢との誤差を算出する誤差算出部と、前記ティップ部の位置及び姿勢のイタレーション演算毎の変化と、前記弾性体の弾性力によって前記固定部に加わる力及びモーメントのイタレーション演算毎の変化との間の関係を示すヤコビ行列と、前記誤差算出部が算出する前記誤差とに基づいて、前記固定部に加わる力及びモーメントを算出する力モーメント算出部と、今回のイタレーション演算によって算出された前記誤差に基づいて前記力モーメント算出部によって算出された前記固定部に加わる力及びモーメントを前記形状算出部に対して供給することにより、当該力及びモーメントに基づく前記ティップ部の推定位置及び推定姿勢を次回のイタレーション演算として前記形状算出部に算出させるイタレーション制御部と、を備える演算装置である。
【0006】
本発明の一実施形態は、上述の演算装置において、前記誤差算出部が算出する前記誤差が所定の許容範囲内であるか否かを判定する判定部をさらに備え、前記イタレーション制御部は、今回のイタレーション演算によって算出された前記誤差が、前記許容範囲内でないと前記判定部によって判定された場合に、前記次回のイタレーション演算として前記ティップ部の推定位置及び推定姿勢を算出させる。
【0007】
本発明の一実施形態は、上述の演算装置において、前記イタレーション制御部は、前記力モーメント算出部が算出する前記力及びモーメントが前記弾性体の状態の特異点を示す場合には、前記力及びモーメントを初期値に戻して、前記形状算出部に前記ティップ部の推定位置及び推定姿勢を算出させる。
【0008】
本発明の一実施形態は、上述の演算装置において、前記形状算出部は、所定の分割数の積分演算によって前記推定形状を算出し、前記力モーメント算出部は、前記所定の分割数以下の分割数による積分演算によって前記ヤコビ行列の演算を行うことにより、前記固定部に加わる力及びモーメントを算出する。
【0009】
本発明の一実施形態は、演算装置が備えるコンピュータに、弾性体の一端であるベース部の位置及び姿勢と、前記弾性体の他端であるティップ部の目標位置及び目標姿勢とをそれぞれ取得する取得ステップと、前記取得ステップにおいて取得される前記ベース部の位置及び姿勢と、前記ベース部の位置及び姿勢を固定する固定部に前記ベース部から加わる力及びモーメントと、前記弾性体が複数のばね関節と剛体リンクとが交互に連なる構造体として離散化モデリングされたモデル情報とに基づいて、前記ティップ部の推定位置及び推定姿勢と、前記ベース部から前記ティップ部までの前記弾性体の推定形状とを算出する形状算出ステップと、前記取得ステップにおいて取得される前記ティップ部の目標位置及び目標姿勢と、前記形状算出ステップにおいて算出される前記ティップ部の推定位置及び推定姿勢との誤差を算出する誤差算出ステップと、前記ティップ部の位置及び姿勢のイタレーション演算毎の変化と、前記弾性体の弾性力によって前記固定部に加わる力及びモーメントのイタレーション演算毎の変化との間の関係を示すヤコビ行列と、前記誤差算出ステップにおいて算出される前記誤差とに基づいて、前記固定部に加わる力及びモーメントを算出する力モーメント算出ステップと、今回のイタレーション演算によって算出された前記誤差に基づいて前記力モーメント算出ステップにおいて算出された前記固定部に加わる力及びモーメントに基づく前記ティップ部の推定位置及び推定姿勢を次回のイタレーション演算として算出させるイタレーション制御ステップとを実行させるためのプログラムである。
【発明の効果】
【0010】
本発明によれば、弾性体の状態を推定するための計算量を低減することができる演算装置及びプログラムを提供することができる。
【図面の簡単な説明】
【0011】
図1】本実施形態の演算装置の機能構成の一例を示す図である。
図2】本実施形態の弾性体を把持しているロボットの外観の一例を示す図である。
図3】本実施形態の弾性体の外観の一例を示す図である。
図4】本実施形態の演算装置の動作の一例を示す図である。
図5】本実施形態の演算装置に実装されるアルゴリズムの一例を示す図である。
図6】本実施形態の演算装置による演算結果の一例を示す図である。
【発明を実施するための形態】
【0012】
[実施形態]
以下、図面を参照して本実施形態の演算装置10について説明する。
図1は、本実施形態の演算装置10の機能構成の一例を示す図である。この演算装置10は、弾性体ERの推定形状SPや、弾性体ERのティップ部TPの推定位置pn及び推定姿勢Fn、弾性体ERのベース部BSを固定するためのベース力fb、ベースモーメントmbを算出する。以下の説明において、弾性体ERの推定形状SP、弾性体ERのティップ部TPの推定位置pn、推定姿勢Fn、弾性体ERのベース部BSを固定するためのベース力fb、ベースモーメントmbを総称して「弾性体ERの状態」とも記載する。
図2を参照して弾性体ERについて説明する。
【0013】
[弾性体ERについて]
図2は、本実施形態の弾性体ERを把持しているロボットRBの外観の一例を示す図である。この一例では、ロボットRBとは、単腕多関節ロボットであり、腕の先端に把持部HDを備えている。この把持部HDは、種々の物体を把持可能である。この一例では、把持部HDは、弾性体ERのティップ部TPを把持する。
この一例において、弾性体ERは、ポリエチレンテレフタレート(PET)やポリイミドなどの合成樹脂や、鋼板などの金属などの可撓性を有する材料によって構成され、例えば、全体が均一な厚みと幅によって形成されている。弾性体ERの端部のうち、位置及び姿勢が固定されている側をベース部BSと、ロボットRBの把持部HDに把持されて移動可能である側をティップ部TPという。以下の説明において、ベース部BSを弾性体ERの固定端ともいい、ティップ部TPを弾性体ERの移動端ともいう。この弾性体ERは、その全長にわたって各部が互いにほぼ等しいばね乗数を有する。
ベース部BSの位置及び姿勢が固定されている状態においてティップ部TPの位置及び姿勢を変化させると、弾性体ERは、弾性体ERの性状(形状、材質など)と、ベース部BSの位置及び姿勢とティップ部TPの位置及び姿勢との相対関係によって定まる形状に変化する。
弾性体ERのベース部BS(固定端)の位置及び姿勢を固定する固定部には、弾性体ERの弾性力に基づく力及びモーメントが加わる。この弾性体ERから固定部に対して加わる力及びモーメントを、ベース力fb及びベースモーメントmbとよぶ。このベース力fb及びベースモーメントmbは、ティップ部TPの位置及び姿勢の変化に伴い、すなわち弾性体ERの形状変化に伴い変化する。ここで、ベース部BS及びティップ部TPの位置及び姿勢について図3を参照して説明する。
【0014】
図3は、本実施形態の弾性体ERの外観の一例を示す図である。弾性体ERのベース部BS及びティップ部TPの位置及び姿勢は、次のようにして示される。すなわち、ベース部BSの位置は、グローバル3次元直交座標系の座標軸(xg、yg、zg)におけるベース部BSの座標によって示される。また、ベース部BSの姿勢は、座標軸(xg、yg、zg)に対する、ベース部BSの局所3次元直交座標系の座標軸(xb、yb、zb)の方向によって示される。同様に、ティップ部TPの位置は、グローバル3次元直交座標系の座標軸(xg、yg、zg)におけるティップ部TPの座標によって示される。また、ティップ部TPの姿勢は、座標軸(xg、yg、zg)に対する、ティップ部TPの局所3次元直交座標系の座標軸(xt、yt、zt)の方向によって示される。
【0015】
なお、以下では、ベース部BSの位置及び姿勢を示す座標軸(xb、yb、zb)と、グローバル3次元直交座標系の座標軸(xg、yg、zg)とが一致しており、ベース部BSの位置及び姿勢を基準にしてティップ部TPの位置及び姿勢が表されるものとして説明する。
【0016】
[演算装置10の機能構成及び動作]
演算装置10の具体的な機能構成及び動作について説明する。
図1に戻り、演算装置10は、取得部110と、形状算出部120と、誤差算出部130と、力モーメント算出部140と、イタレーション制御部150と、判定部160と、出力部170と、記憶部180とを備える。なお、この一例においては、演算装置10が記憶部180を内蔵するものとして説明するが、これに限られない。例えば、記憶部180がクラウドサーバなどによって実現されるなど、演算装置10と記憶部180とが別々の装置として構成されてもよい。
【0017】
[モデル情報MDLについて]
記憶部180には、モデル情報MDLが記憶されている。モデル情報MDLとは、弾性体ERを複数のばね関節と剛体リンクとが交互に連なる構造体として離散化モデリングした情報である。具体的には、モデル情報MDLは、弾性体ERを疑似的に、ある単位長さlの疑似剛体と、疑似剛体どうしを繋ぐ関節と、この関節において疑似剛体どうしを3次元方向にわたって繋ぐ所定のばね定数を有するばねとを含む、ばね関節と剛体リンクとが交互に連なる構造体と見なしてモデリングされている。
弾性体ERのベース部BSからティップ部TPまでの長さ(すなわち弾性体ERの全長)をLとし、弾性体ERを上述のばね関節によって疑似的に分割する所定の分割数をnとする。この場合、上述の疑似剛体の長さlはそれぞれL/nである。なお、疑似剛体の長さlは、疑似剛体ごとに互いに異なっていてもよい。
この一例において各ばね関節は3自由度を有し、それぞれの角度を(θT,i)、(θN,i)、(θB,i)∈Cとする。ここでインデックスiは、ばね関節および剛体リンクの配列の順番を示し、その値は1(ベース部BS側)からn(ティップ部TP側)までである。ばね関節の角度をθによって表す。ここで、θ=[θT,iθN,iθB,iである。ベース部BS側から数えてi番目の剛体リンクの姿勢及び(i−1)番目のばね関節の位置は、式(1)及び式(2)によって示される。
【0018】
【数1】
【0019】
【数2】
【0020】
以降の説明において、ベース部BS側から数えてi番目のばね関節のことを単に「i番目のばね関節」とも記載する。i番目のばね関節の回転RJ(θ)は、単位長方向ベクトルaを軸として角度θだけ回転させる回転作用素R(a,θ)を用いて式(3)によって示される。
【0021】
【数3】
【0022】
ここで、ex=[1 0 0]、ey=[0 1 0]、ez=[0 0 1]である。ここで、[ ]とは、行列[ ]の転置行列を示す。
なお、以下の説明において、ベース部BSを0番目の疑似剛体とし、ティップ部TPを(n+1)番目の疑似剛体とする。また、ベース部BSの位置を基準位置p0と、ベース部BSの姿勢を基準姿勢F0とも称する。なお、pnはティップ部の位置となる。
【0023】
図4は、本実施形態の演算装置10の動作の一例を示す図である。以下、図1及び図4を参照しつつ、演算装置10の動作の一例について説明する。
【0024】
(ステップS10)取得部110は、弾性体ERの一端である固定端の基準位置p0及び基準姿勢F0(すなわち、ベース部BSの位置及び姿勢)と、弾性体ERの他端であるティップ部TPの目標位置p*及び目標姿勢F*とをそれぞれ取得する。これら、基準位置p0、基準姿勢F0、目標位置p*及び目標姿勢F*は、演算装置10に接続された他の装置などから供給される。演算装置10に接続された他の装置には、例えば、演算装置10の上位のコンピュータ装置や、演算装置10のキーボードやタッチパネルなどの操作デバイスが含まれる(いずれも不図示)。
取得部110は、取得した基準位置p0及び基準姿勢F0を形状算出部120に出力する。また、取得部110は、取得した目標位置p*及び目標姿勢F*を誤差算出部130に出力する。
【0025】
(ステップS20)形状算出部120は、ティップ部TPの推定位置pn及び推定姿勢Fnと、ベース部BSからティップ部TPまでの弾性体ERの推定形状SPとを算出する。
【0026】
ここで、ティップ部TPの目標位置p*及び目標姿勢F*とは、ティップ部TPの推定位置pn及び推定姿勢Fnの目標値である。すなわち、ティップ部TPの位置と姿勢の推定が完璧である場合、式(4)及び式(5)に示す関係が成り立つ。
【0027】
【数4】
【0028】
【数5】
【0029】
各ばね関節におけるモーメントのバランスより、式(6)が得られる。
【0030】
【数6】
【0031】
ここで、Aiは、式(7)〜式(9)によって示されるi番目の関節の3方向のベクトルである。すなわち、Ai:=[aT,iN,iB,i]である。
【0032】
【数7】
【0033】
【数8】
【0034】
【数9】
【0035】
また、式(6)のKは、(kdt,kdn,kdb)を要素とする対角行列、すなわち、K=diag(kdt,kdn,kdb)であり、上述したばね関節に含まれる3方向のばねのそれぞれのばね定数を示す。
【0036】
Ai−1(Aiの逆行列)をAi(Aiの転置行列)で近似した上で、この式(6)をθiについて解くと、式(10)が得られる。
【0037】
【数10】
【0038】
この式(10)は、ベース部BS側からティップ部TPまでの各ばね関節の位置及び姿勢をベース部BS側から順に求めることにより、ティップ部TPの推定位置pn及び推定姿勢Fnを算出することができることを示している。
形状算出部120は、上述の式(10)に基づいて、ベース部BS側からティップ部TPまでの各ばね関節の位置及び姿勢をベース部BS側から順に求めることにより、ティップ部TPの推定位置pn及び推定姿勢Fnを算出する。また、形状算出部120は、算出した各ばね関節の位置及び姿勢(すなわち、θi)をインデックス順に算出することにより、ベース部BSからティップ部TPまでの弾性体ERの推定形状SPを算出する。
【0039】
すなわち、形状算出部120は、取得部110が取得する基準位置p0及び基準姿勢F0(すなわち、ベース部BSの位置及び姿勢)と、ベース部BSを把持する把持部HDに把持部HDに加わるベース力fb及びベースモーメントmb(すなわち、ベース部BSから加わる力及びモーメント)と、弾性体ERが複数のばね関節と剛体リンクとが交互に連なる構造体として離散化モデリングされたモデル情報MDLとに基づいて、ティップ部TPの推定位置pn及び推定姿勢Fnと、ベース部BSからティップ部TPまでの弾性体ERの推定形状SPとを算出する。
形状算出部120は、算出したティップ部TPの推定位置pn、推定姿勢Fn及び弾性体ERの推定形状SPを出力部170に出力する。
【0040】
(ステップS30)誤差算出部130は、取得部110が取得するティップ部TPの目標位置p*及び目標姿勢F*と、形状算出部120が算出するティップ部TPの推定位置pn及び推定姿勢Fnとの誤差eを算出する。
【0041】
具体的には、誤差算出部130は、取得部110が出力するティップ部TPの目標位置p*及び目標姿勢F*と、形状算出部120が出力するティップ部TPの推定位置pn及び推定姿勢Fnとをそれぞれ取得する。誤差算出部130は、取得した目標位置p*及び目標姿勢F*と、推定位置pn及び推定姿勢Fnとの誤差eを算出する。ここで、誤差eを式(11)に示す。
【0042】
【数11】
【0043】
ここで、式(11)中の( )の定義を説明する前に、作用素[ ×]について説明する。3次元ベクトルa、bに対する外積a×bを考えたとき、Xb=a×bとなるような3×3の行列Xを見つけることができる。3次元ベクトルaに対して、上記の行列Xを割り当てる作用素が[ ×]であり、すなわち、X=[a×]である。これとは逆に、行列Xに対して、ベクトルaを割り当てる作用素が( )であり、すなわち、a=(X)である。また、log( )は、行列指数関数(matrix exponential)の逆写像である、行列対数(matrix logarithm)を示す。
誤差算出部130は、算出した誤差eを力モーメント算出部140及び判定部160に出力する。
【0044】
(ステップS40)判定部160は、誤差算出部130が算出する誤差eの大きさが所定の許容範囲TOL内であるか否かを判定する。判定部160は、誤差eの大きさが所定の許容範囲TOL内であると判定した場合(ステップS40;YES)には、処理をステップS60に進める。
また、判定部160は、誤差eの大きさが所定の許容範囲TOL内でないと判定した場合(ステップS40;NO)には、処理をステップS50に進める。
【0045】
(ステップS50)力モーメント算出部140は、ティップ部TPの推定位置pn及び推定姿勢Fnのイタレーション演算毎の変化と、弾性体ERの弾性力によってベース部BSから固定部に加わるベース力fb及びベースモーメントmbのイタレーション演算毎の変化との間の関係を示すヤコビ行列Jと、誤差算出部130が算出する誤差eとに基づいて、ベース部BSから固定部に加わるベース力fb及びベースモーメントmbを算出する。
【0046】
[ヤコビ行列Jについて]
ここで、記憶部180には、ヤコビ行列Jが記憶されている。本実施形態のヤコビ行列Jとは、ベース部BSにおいて固定部に加わるベース力fb及びベースモーメントmbと、ティップ部TPの推定位置pn及び推定姿勢Fnとの関係を示す写像のヤコビ行列であり、6行×6列の行列である。
このヤコビ行列Jは、式(12)〜式(19)に示すように、3n行×3n列の行列Mと、3n行×6列の行列b及びcからなる。
【0047】
【数12】
【0048】
【数13】
【0049】
【数14】
【0050】
【数15】
【0051】
【数16】
【0052】
【数17】
【0053】
【数18】
【0054】
【数19】
【0055】
ここで、{Мij}は、3×3行列Мijを(i,j)ブロック要素に持つブロック行列を意味する。また、Imは、m次の単位行列である。
演算装置10の機能構成について説明を続ける。イタレーション制御部150は、イタレーション演算の実行を制御する。本実施形態でいうイタレーションとは、上述したステップS20からステップS50までを反復して実行することにより、ティップ部TPの推定位置pn及び推定姿勢Fnを、ステップS10において取得されるティップ部TPの目標位置p*及び目標姿勢F*に近づけていく手順をいう。また、イタレーション演算とは、イタレーション1回あたりに実行されるステップS20からステップS50までの一連の演算をいう。
【0056】
より具体的には、イタレーション制御部150は、イタレーション回数Nを計数する。イタレーション制御部150は、イタレーション演算を実行するごとにイタレーション回数Nを加算(インクリメント)する。一例として、イタレーション回数Nの初期値は0(ゼロ)である。
力モーメント算出部140は、今回(例えば、イタレーション回数N=0)のイタレーション演算によって算出された誤差eに基づいて固定部に加わるベース力fb及びベースモーメントmbを算出する。
イタレーション制御部150は、力モーメント算出部140によってベース力fb及びベースモーメントmbが算出されると、イタレーション回数Nに1を加算(例えば、イタレーション回数N=1に)する。イタレーション制御部150は、今回(例えば、イタレーション回数N=0)のイタレーション演算によって算出されたベース力fb及びベースモーメントmbを形状算出部120に対して供給することにより、次回(例えば、イタレーション回数N=1)のイタレーション演算を実行させる。すなわち、イタレーション制御部150は、次回のイタレーション演算として、処理をステップS20に戻して各ステップSを反復して実行させる。
【0057】
換言すれば、イタレーション制御部150は、今回((N−1)回目)のイタレーション演算によって算出された誤差eに基づいて力モーメント算出部140によって算出されたベース力fb及びベースモーメントmbを形状算出部120に対して供給することにより、当該ベース力fb及びベースモーメントmbに基づくティップ部TPの推定位置pn及び推定姿勢Fnを次回(N回目)のイタレーション演算として形状算出部120に算出させる。
【0058】
なお、演算装置10が判定部160を備える場合には、イタレーション制御部150は、今回のイタレーション演算によって算出された誤差eの大きさが、許容範囲TOL内でないと判定部160によって判定された場合に、次回のイタレーション演算としてティップ部TPの推定位置pn及び推定姿勢Fnを算出させるように構成されてもよい。
【0059】
(ステップS60)出力部170は、例えば、液晶ディスプレイやプリンタなどの出力デバイスに接続されている。出力部170は、形状算出部120が算出したティップ部TPの推定位置pn、推定姿勢Fn及び弾性体ERの推定形状SPを、これら出力デバイスに対して出力する。
また、出力部170は、上述のイタレーション演算が収束した場合の、力モーメント算出部140が算出したベース力fb及びベースモーメントmbを、出力デバイスに対して出力してもよい。ここで、イタレーション演算が収束した場合とは、ティップ部TPの目標位置p*及び目標姿勢F*に対して、算出された推定位置pn及び推定姿勢Fnが許容範囲TOL内になった場合をいう。すなわちこの場合、出力部170は、推定位置pn及び推定姿勢Fnが許容範囲TOL内になった場合の、ベース力fb及びベースモーメントmbを出力する。
【0060】
[アルゴリズム例]
図5は、本実施形態の演算装置10に実装されるアルゴリズムの一例を示す図である。ここで、記号||e||とは、式(11)に示した誤差eのノルムである。演算装置10は、誤差eのノルムが、許容範囲TOL(同図中に示すεtol)以上である場合には、以下の演算を繰り返す。すなわち、演算装置10は、ベース力fb及びベースモーメントmbについての漸化式(すなわち、式(20))によってベース力fb及びベースモーメントmbを更新する。
【0061】
【数20】
【0062】
ここで、λは非負の実数、また、Kpは6×6の正定行列であり、状況に応じて適度な値に設定する。変数の横に付いている(k)は、k回目のイタレーション演算によって算出された、その変数の値を意味する。
演算装置10は、i番目のばね関節において生じる局所的なトルクτ、ばね関節の角度θ=[θT,iθN,iθB,i、(i−1)番目の関節の推定位置(すなわち、推定位置p)及びi番目の剛体リンクの推定姿勢(すなわち、推定姿勢F)をすべてのばね関節および剛体リンクについて順次算出することにより、ティップ部TPの推定位置pn及び推定姿勢Fnを算出する。
さらに、演算装置10は、算出したティップ部TPの推定位置pn及び推定姿勢Fnと、目標位置p*及び目標姿勢F*との誤差eを、上述した式(11)に基づいて算出する。
演算装置10は、算出した誤差eのノルムが、許容範囲TOL(同図中に示すεtol)未満になれば、算出したティップ部TPの推定位置pn及び推定姿勢Fnを演算結果RSとして出力する。
【0063】
[演算結果例]
図6は、本実施形態の演算装置10による演算結果RSの一例を示す図である。演算対象の弾性体ERの初期形状を図6(a)に示す。図6(a)に示す初期形状に対して、ベース部BSの位置及び姿勢を変化させることなく、ティップ部TPの位置及び姿勢を変化させた場合の、変化後の弾性体ERの形状を図6(b)に示す。本実施形態の演算装置10は、図6(a)に示すベース部BSの基準位置p0と基準姿勢F0、及び図6(b)に示すティップ部TPの位置と姿勢(すなわち、目標位置p*と目標姿勢F*)が与えられた場合に、ベース部BSからティップ部TPまでの弾性体ERの形状を「推定形状SP」として算出する。演算装置10は、算出した推定形状SPに基づいて図6(b)に示す弾性体ERの形状を描画する。
【0064】
以上説明したように、演算装置10は、弾性体ERが複数のばね関節と剛体リンクとが交互に連なる構造体として離散化モデリングされたモデル情報MDLと、陽に表現されたヤコビ行列Jとに基づいて、弾性体ERの形状などの推定演算を行う。
【0065】
なお、ベース部BSとティップ部TPとの間の相対位置・相対方向や、弾性体ERの材質などによっては、力モーメント算出部140が算出するベース力fb及びベースモーメントmbが弾性体ERの状態の特異点を示す場合がある。ここで、弾性体ERの状態の特異点には、例えば、飛び移り座屈(スナップスルー)を起こす際のベース力fb及びベースモーメントmbがある。このような特異点が生じると演算結果RSの精度が低下することがある。
この場合、イタレーション制御部150は、ベース力fb及びベースモーメントmbを初期値に戻して、形状算出部120にティップ部TPの推定位置pn及び推定姿勢Fnを算出させる。このように構成することにより、イタレーション制御部150は、特異点の存在により演算結果RSの精度が低下する状況を回避できるため、演算結果RSの精度を向上させることができる。
【0066】
また、上述したように、モデル情報MDLにおいて、弾性体ERは、所定の分割数nのばね関節と剛体リンクとが交互に連なる構造体として離散化モデリングされている。この場合、形状算出部120は、所定の分割数nの積分演算によって推定形状SPを算出する。ここで、積分演算には、特殊ユークリッド群SE(3)上の積分が含まれる。
この場合において、力モーメント算出部140は、所定の分割数n以下の分割数n_Jによる積分演算によってヤコビ行列Jの演算を行うことにより、ベース力fb及びベースモーメントmbを算出する。一例として、所定の分割数n=50とした場合、力モーメント算出部140は、分割数n_J=25としてベース力fb及びベースモーメントmbを算出する。
【0067】
ここで、所定の分割数nの値が大きいほど、弾性体ERの形状を高精度に推定することができる。一方で、有限差分によってヤコビ行列Jを数値計算的に計算する従来手法によると、所定の分割数nの値が大きくなるに従い、ヤコビ行列Jの計算量も増加してしまう。すなわち、従来手法によると、弾性体ERの形状推定の高精度化と、形状推定のための計算量の低減とを両立することが困難であった。
一方、本実施形態の演算装置10は、ヤコビ行列Jを陽に表現し、かつヤコビ行列Jの積分演算の分割数n_Jを、弾性体ERの離散化モデリングの分割数n以下にしているため、有限差分によってヤコビ行列Jを数値計算的に計算する従来手法に比べて、計算量を低減させることができる。
すなわち、本実施形態の演算装置10は、弾性体ERの形状推定の高精度化と、形状推定のための計算量の低減とを両立することができる。
【0068】
以上、本発明の実施形態を、図面を参照して詳述してきたが、具体的な構成はこの実施形態に限られるものではなく、本発明の趣旨を逸脱しない範囲で適宜変更を加えることができる。
【0069】
なお、上述の各装置は内部にコンピュータを有している。そして、上述した各装置の各処理の過程は、プログラムの形式でコンピュータ読み取り可能な記録媒体に記憶されており、このプログラムをコンピュータが読み出して実行することによって、上記処理が行われる。ここでコンピュータ読み取り可能な記録媒体とは、磁気ディスク、光磁気ディスク、CD−ROM、DVD−ROM、半導体メモリ等をいう。また、このコンピュータプログラムを通信回線によってコンピュータに配信し、この配信を受けたコンピュータが当該プログラムを実行するようにしてもよい。
【0070】
また、上記プログラムは、前述した機能の一部を実現するためのものであってもよい。
さらに、前述した機能をコンピュータシステムにすでに記録されているプログラムとの組み合わせで実現できるもの、いわゆる差分ファイル(差分プログラム)であってもよい。
【符号の説明】
【0071】
10…演算装置、110…取得部、120…形状算出部、130…誤差算出部、140…力モーメント算出部、150…イタレーション制御部、160…判定部、170…出力部、180…記憶部、ER…弾性体、BS…ベース部、TP…ティップ部、RB…ロボット、HD…把持部、J…ヤコビ行列、MDL…モデル情報
図1
図2
図3
図4
図5
図6