(57)【特許請求の範囲】
【請求項１】
三次元方向の力とトルクを検出する６軸センサと、
前記６軸センサに取り付けられ、先端に加わる外力によって変形する弾性体と、
前記弾性体の三次元形状を推定する座標演算部と
を具備し、
前記座標演算部は、前記弾性体を、所定の長さを有する疑似剛体とバネ定数を有する関節の集合体よりなる離散化モデルとして見做し、前記バネ定数と、前記６軸センサから得られる前記力の情報と前記トルクの情報に基づいて、前記弾性体が前記外力に対して静的つり合いをなす屈曲形状を推定演算する、
三次元形状推定装置。
【請求項２】
前記座標演算部は、前記弾性体を、所定の長さを有する疑似剛体とバネ定数を有する関節の集合体よりなる、
τｉ＝ｍｓ−ｐｉ−１×ｆｓ
を満たす離散化モデルとして見做し、前記弾性体が前記外力に対して静的つり合いをなす屈曲形状を推定演算する、請求項１に記載の三次元形状推定装置。
但し、ｉは自然数であり、τｉはｉ番目の前記関節である第ｉ関節にかかる局所的なトルクであり、ｍｓは前記６軸センサより読み取ったトルクベクトルであり、ｐｉ−１は前記第ｉ関節の位置ベクトルであり、ｆｓは前記６軸センサから読み取った力ベクトルである。
【請求項３】
更に、
前記座標演算部の演算結果に基づき、前記弾性体の推定形状を描画する画像描画処理部と
を具備する、請求項２に記載の三次元形状推定装置。
【請求項４】
前記座標演算部は、
演算処理の初期値として、前記６軸センサに前記弾性体が据え付けられている三次元方向の位置・姿勢と、前記バネ定数と、前記疑似剛体の長さを設定し、
前記６軸センサから得られる前記力の情報と前記トルクの情報を取得し、
漸化計算のインデックスを初期値に設定し、前記関節に加わるトルクを算出し、
前記関節の第１軸の向き、前記関節の第１軸の角度、前記関節の第１軸の回転の影響を受ける第１サブフレーム姿勢を算出し、
前記関節の第２軸の向き、前記関節の第２軸の角度、前記関節の第２軸の回転の影響を受ける第２サブフレーム姿勢を算出し、
前記関節の第３軸の向き、前記関節の第３軸の角度、前記関節の第３軸の回転の影響を受ける手先側の疑似剛体姿勢を算出し、
前記手先側の疑似剛体の長さと前記姿勢から前記関節の次の手先側関節位置を算出する、
請求項３に記載の三次元形状推定装置。
【請求項５】
三次元方向の力とトルクを検出する６軸センサに取り付けられる弾性体を、所定の長さを有する疑似剛体とバネ定数を有する関節の集合体よりなる離散化モデルとして見做し、前記バネ定数と、前記６軸センサから得られる前記力の情報と前記トルクの情報に基づいて、前記弾性体の屈曲形状を推定演算する、可撓性弾性体の三次元形状推定方法であり、
前記三次元形状推定方法は、
演算処理の初期値として、前記６軸センサに前記弾性体が据え付けられている三次元方向の位置・姿勢と、前記バネ定数と、前記疑似剛体の長さを設定する初期値設定ステップと、
前記６軸センサから得られる前記力の情報と前記トルクの情報を取得するセンシングステップと、
漸化計算のインデックスを初期値に設定するインデックス初期化ステップと、
前記関節に加わるトルクを算出する局所トルク算出ステップと、
前記関節の第１軸の向きを算出する第１軸方向算出ステップと、
前記関節の第１軸の角度を算出する第１軸角度算出ステップと、
前記関節の第１軸の回転の影響を受ける第１サブフレーム姿勢算出ステップと、
前記関節の第２軸の向きを算出する第２軸方向算出ステップと、
前記関節の第２軸の角度を算出する第２軸角度算出ステップと、
前記関節の第２軸の回転の影響を受ける第２サブフレーム姿勢算出ステップと、
前記関節の第３軸の向きを算出する第３軸方向算出ステップと、
前記関節の第３軸の角度を算出する第３軸角度算出ステップと、
前記関節の第３軸の回転の影響を受ける手先側の疑似剛体姿勢を算出する疑似剛体姿勢算出ステップと、
前記手先側の疑似剛体の長さと前記姿勢から前記関節の次の手先側関節位置を算出する関節位置算出ステップと
を含む、可撓性弾性体の三次元形状推定方法。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、三次元形状推定装置及び可撓性弾性体の三次元形状推定方法に関する。
【背景技術】
【０００２】
高度なロボット技術の発展には、可動体の姿勢や力を検出するための、センサ技術の進化が不可欠である。
非特許文献１及び２には、そのようなロボットに適用されるセンサが開示されている。
【先行技術文献】
【非特許文献】
【０００３】
【非特許文献１】「シェイプテープ」株式会社ソリッドレイ研究所［２０１５年４月２３日検索］、インターネット＜URL:http://www.solidray.co.jp/product/3dglove/shapetape.html＞
【非特許文献２】「マスタースレーブロボット指令用フレキシブルセンサーチューブ」旭光電機株式会社［２０１５年４月２３日検索］、インターネット＜URL:http://www.kyokko.co.jp/technology/fst-03.html＞
【発明の概要】
【発明が解決しようとする課題】
【０００４】
上記の非特許文献１及び非特許文献２に開示されるセンサ装置は、多数のセンサモジュールを内蔵して、可動体の姿勢を検出する。このようなセンサの数を低減させることができれば、より低コストなセンサの実現に繋がる。
【０００５】
本発明はかかる点に鑑みてなされたものであり、６軸センサ１個で、６軸センサに取り付けられた可撓性弾性体の形状を迅速に推定できる、三次元形状推定装置及び可撓性弾性体の三次元形状推定方法を提供することを目的とする。
【課題を解決するための手段】
【０００６】
上記課題を解決するために、本発明の三次元形状推定装置は、三次元方向の力とトルクを検出する６軸センサと、６軸センサに取り付けられ、先端に加わる外力によって変形する弾性体と、弾性体の三次元形状を推定する座標演算部とを具備する。
座標演算部は、弾性体を、所定の長さを有する疑似剛体とバネ定数を有する関節の集合体よりなる離散化モデルとして見做し、バネ定数と、６軸センサから得られる力の情報とトルクの情報に基づいて、弾性体が外力に対して静的つり合いをなす屈曲形状を推定演算する。
【発明の効果】
【０００７】
本発明により、６軸センサ１個で、６軸センサに取り付けられた可撓性弾性体の形状を迅速に推定できる、三次元形状推定装置及び可撓性弾性体の三次元形状推定方法を提供することができる。
上記した以外の課題、構成及び効果は、以下の実施形態の説明により明らかにされる。
【図面の簡単な説明】
【０００８】
【図１】本発明の実施形態の例である、弾性体形状推定システムの全体構成を示す概略図である。
【図２】情報処理装置のハードウェア構成を示すブロック図である。
【図３】情報処理装置のソフトウェア機能を示すブロック図である。
【図４】座標演算部における演算処理の概念図である。
【図５】情報処理装置における演算処理を説明するフローチャートである。
【発明を実施するための形態】
【０００９】
［全体構成］
図１は、本発明の実施形態の例である、弾性体形状推定システム１０１の全体構成を示す概略図である。
弾性体形状推定システム１０１は、６軸センサ１０２と、パソコン等の情報処理装置１０５よりなる。
６軸センサ１０２には、弾性体形状推定システム１０１による推定の対象となる、帯状弾性体１０３の一端が取り付けられている。
帯状弾性体１０３は、例えば周知のポリエチレンテレフタレート（ＰＥＴ樹脂）やポリイミド等の合成樹脂の他、鋼板等の金属でもよいが、全て可撓性を有する材料にて形成される弾性体であり、全体が均一な厚みと幅にて形成されている。したがって、帯状弾性体１０３はその全長にわたってほぼ等しいバネ定数を有する。
【００１０】
６軸センサ１０２は、ｘ軸、ｙ軸、ｚ軸の各軸方向の力と、ｘ軸、ｙ軸、ｚ軸の各軸を中心とするトルクを出力するセンサである。６軸センサ１０２が出力するアナログ信号は、Ａ／Ｄ変換器２０７を内蔵するインターフェースユニット１０４を通じて、情報処理装置１０５に接続される。インターフェースユニット１０４と情報処理装置１０５との間を接続するインターフェースは特に限定されない。一般的なパソコンである情報処理装置１０５が装備するＰＣＩバスに装着される形態や、ＵＳＢ、ＬＡＮケーブル等、種々のインターフェースが利用可能である。
情報処理装置１０５は、予め入力された種々の初期値と、６軸センサ１０２の信号を受けて演算処理を行い、６軸センサ１０２に取り付けられた帯状弾性体１０３の形状を模倣する画像を表示部１０６に表示する。
なお、本実施形態の弾性体形状推定システム１０１は、バネ定数の設定が容易な帯状弾性体１０３を用いたが、必ずしも弾性体が帯状である必要はない。
【００１１】
図２は、情報処理装置１０５のハードウェア構成を示すブロック図である。
一般的なパソコンよりなる情報処理装置１０５は、ＣＰＵ２０１、ＲＯＭ２０２、ＲＡＭ２０３、不揮発性ストレージ２０４、表示部１０６、そして操作部２０５が、バス２０６に接続されている。この他、６軸センサ１０２が出力するアナログ信号をデジタルデータに変換する、インターフェースユニット１０４に内蔵されるＡ／Ｄ変換器２０７も、バス２０６に接続される。
【００１２】
図３は、情報処理装置１０５のソフトウェア機能を示すブロック図である。
Ａ／Ｄ変換器２０７を通じて入力される６軸センサ１０２のデータは、入出力制御部３０１の一機能である座標演算部３０２に入力される。
座標演算部３０２は、操作部２０５から入力される初期値と共に、後述する演算処理を行い、６軸センサ１０２に取り付けられている帯状弾性体１０３の形状を推定する。推定結果は、三次元空間内における複数の点の座標情報として出力される。座標演算部３０２が出力する複数の点の座標情報は、入出力制御部３０１の一機能である画像描画処理部３０３に入力される。
画像描画処理部３０３は、複数の点の座標情報を基に、三次元空間における帯状弾性体１０３のシミュレーション画像を作成する。そして、画像描画処理部３０３が作成したシミュレーション画像は、表示部１０６に表示される。
【００１３】
［原理］
図４は、座標演算部３０２における演算処理の概念図である。
本実施形態の弾性体形状推定システム１０１は、基本的に全長にわたって均等なバネ定数を有する弾性体を推定演算の対象としている。この弾性体は連続体であるが、この連続体を離散化することで、計算機による演算が可能になる。つまり、弾性体はその離散化モデルとして、疑似的にある長さの疑似剛体４０１と、疑似剛体４０１同士を繋ぐ関節４０２と、疑似剛体４０１同士を三次元方向にわたって繋ぐ、所定のバネ定数を有するバネ４０３よりなるバネ関節４０４の集合体と見做すことができる。疑似剛体４０１の一本一本はベクトルであり、ベクトルの回転を繋ぎ合わせた構成であると考えると、帯状弾性体１０３の形状を推定演算することができる。
【００１４】
帯状弾性体１０３の全長をＬ、帯状弾性体１０３をバネ関節４０４で疑似的に分割する分割数をｎとする。各疑似剛体の長さをｌ＝Ｌ／ｎとする。各関節４０２は一般的に３自由度を有し，それぞれの角度をθT,I、θN,i、θB,I∈Ｃとする。インデックスであるｉは１から始まり、ｎまでである。
【００１５】
先ず、本実施形態における推定演算の準備として、回転作用素Ｒ（ａ，θ）の計算式を以下に記す。
【００１６】
【数１】
0006558727.tif 000002
【００１７】
次に、本実施形態における推定演算に使用する定数を以下に記す。
【００１８】
【数２】
0006558727.tif 000003
【００１９】
次に、本実施形態における推定演算に使用する設定値を以下に記す。
【００２０】
【数３】
0006558727.tif 000004
【００２１】
ただし、基準座標系は６軸センサ座標系と向きが一致しており、原点も６軸センサ１０２の原点に一致しているとする。
【００２２】
【数４】
0006558727.tif 000005
【００２３】
次に、本実施形態における推定演算において、６軸センサから得る値を以下に記す。
【００２４】
【数５】
0006558727.tif 000006
【００２５】
本実施形態における推定演算は、漸化計算である。繰り返し演算処理の指標（漸化計算のインデックス）となるｉの初期値は１である。
【００２６】
【数６】
0006558727.tif 000007
【００２７】
漸化計算は、以下に記す式（１４）から式（２４）までを繰り返す。
【００２８】
【数７】
0006558727.tif 000008
【００２９】
最終的に、式（２４）で得られる疑似関節の位置ｐｉと疑似剛体４０１の姿勢Ｆｉを求めることで、帯状弾性体１０３の形状を推定することができる。つまり、コンピュータグラフィクスで三次元空間内にｐ０，ｐ１，ｐ２…ｐｎをプロットし，Ｆ０，Ｆ１，Ｆ２…Ｆｎに対応するフレームを描画することで、帯状弾性体１０３の形状を描画できる。
【００３０】
［動作］
図５は、情報処理装置１０５における演算処理を説明するフローチャートである。
処理を開始すると（Ｓ５０１）、入出力制御部３０１は、操作部２０５から入力された、演算に必要な初期値であるバネ定数、ｐ０、Ｆ０、そして疑似関節の長さｌを座標演算部３０２に設定する（Ｓ５０２）。次に、座標演算部３０２はＡ／Ｄ変換器２０７からｆｓ、ｍｓを取得する（Ｓ５０３）。以上、ステップＳ５０２及びＳ５０３が、初期値設定ステップとなる。そして、漸化計算のインデックスともいえるカウンタ変数ｉを１に初期化する（Ｓ５０４）。ステップＳ５０４はインデックス初期化ステップともいえる。
【００３１】
【数８】
0006558727.tif 000009
【００３２】
なお、ステップＳ５０５にて実行する式（１４）は、関節に加わるトルクを算出する局所トルク算出ステップといえる。
同様に、ステップＳ５０６にて実行する式（１５）は、関節の第１軸の向きを算出する第１軸方向算出ステップといえる。
同様に、ステップＳ５０７にて実行する式（１６）は、関節の第１軸の角度を算出する第１軸角度算出ステップといえる。
同様に、ステップＳ５０８にて実行する式（１７）は、関節の第１軸の回転の影響を受ける第１サブフレーム姿勢算出ステップといえる。
同様に、ステップＳ５０９にて実行する式（１８）は、関節の第２軸の向きを算出する第２軸方向算出ステップといえる。
同様に、ステップＳ５１０にて実行する式（１９）は、関節の第２軸の角度を算出する第２軸角度算出ステップといえる。
同様に、ステップＳ５１１にて実行する式（２０）は、関節の第２軸の回転の影響を受ける第２サブフレーム姿勢算出ステップといえる。
同様に、ステップＳ５１２にて実行する式（２１）は、関節の第３軸の向きを算出する第３軸方向算出ステップといえる。
同様に、ステップＳ５１３にて実行する式（２２）は、関節の第３軸の角度を算出する第３軸角度算出ステップといえる。
同様に、ステップＳ５１４にて実行する式（２３）は、関節の第３軸の回転の影響を受ける手先側の疑似剛体姿勢を算出する疑似剛体姿勢算出ステップといえる。
同様に、ステップＳ５１５にて実行する式（２４）は、手先側疑似剛体の長さと姿勢から関節の次の手先側関節位置を算出する関節位置算出ステップといえる。
【００３３】
次に、入出力制御部３０１はカウンタ変数ｉを１インクリメントして（Ｓ５１６）、ｉが分割数ｎを超えているか否かを確認する（Ｓ５１７）。ｉが分割数ｎを超えていない場合には（Ｓ５１７のＮＯ）、入出力制御部３０１は再度ステップＳ５０５から演算処理を繰り返す。
ステップＳ５１７において、ｉが分割数ｎを超えている場合は（Ｓ５１７のＹＥＳ）、帯状弾性体１０３の描画に必要なｐｉが全て揃ったので、画像描画処理部３０３は、ｌとｐｉに基づいて、帯状弾性体１０３を表示部１０６に表示する（Ｓ５１８）。
次に、入出力制御部３０１は、操作部２０５等から処理の終了が指示されているか否かを確認する（Ｓ５１９）。処理の終了が指示されていなければ（Ｓ５１９のＮＯ）、入出力制御部３０１は再度ステップＳ５０３から処理を繰り返す。すなわち、再びＡ／Ｄ変換器２０７からｆｓ、ｍｓを取得して、計算処理（Ｓ５０５〜Ｓ５１５）と描画処理（Ｓ５１８）を繰り返す。
ステップＳ５１９で処理の終了が指示されていれば（Ｓ５１９のＹＥＳ）、入出力制御部３０１は一連の処理を終了する（Ｓ５１１）。
【００３４】
なお、ステップＳ５１８の処理は、ステップＳ５１５の後、かつＳ５１７の前でもよい。その場合、表示部１０６には、６軸センサ１０２から疑似剛体４０１が１個ずつ伸びていく画像が表示される。
以上より、離散バネ定数、６軸センサ１０２上に設置される弾性体の設置位置及び姿勢を既知の値として、６軸センサ１０２で取得できる力とトルクの情報から、式（１４）〜（２４）を演算することで、弾性体の形状及び弾性体先端の位置ｐnと姿勢Ｆnを出力することができる。
【００３５】
本実施形態は、以下の様な応用が可能である。
（１）上述の実施形態では、弾性体の離散バネ定数が全長にわたって一様であることを前提に演算していたが、バネ定数が一様でない弾性体であっても、離散化する際にバネ定数が導出できるのであれば、座標演算部３０２による演算処理が可能である。例えば根元がテーパ状に形成されている弾性体等、バネ定数の変化が弾性体長さの関数で表現できる形態のものは、原理的に演算処理が可能である。
【００３６】
（２）図１から図３までに示した三次元形状推定システムは、実在する弾性体の形状を推定するシステムであったが、実在しない弾性体の形状を推定することも当然に可能である。すなわち、弾性体の形状をシミュレートし、また弾性体に加わる力を推定することも可能である。
【００３７】
本実施形態においては、弾性体形状推定システム１０１を開示した。
６軸センサ１０２上に設置される弾性体の先端に力を加えると、離散バネ定数、６軸センサ１０２上に設置される弾性体の設置位置及び姿勢を既知の値として、６軸センサ１０２で取得できる、弾性体の設置位置にかかる力とトルクの情報から、式（１４）〜（２４）を演算することで、弾性体の形状及び弾性体先端の位置と姿勢を出力することができる。
演算処理は非常に短時間で完遂するため、弾性体に加わる力が変化する度、リアルタイムで推定演算の結果である画像が変化する。
【００３８】
以上、本発明の実施形態について説明したが、本発明は上記実施形態に限定されるものではなく、特許請求の範囲に記載した本発明の要旨を逸脱しない限りにおいて、他の変形例、応用例を含む。
例えば、上記した実施形態は本発明をわかりやすく説明するために装置及びシステムの構成を詳細かつ具体的に説明したものであり、必ずしも説明した全ての構成を備えるものに限定されるものではない。また、ある実施形態の構成の一部を他の実施形態の構成に置き換えることは可能であり、更にはある実施形態の構成に他の実施形態の構成を加えることも可能である。また、各実施形態の構成の一部について、他の構成の追加・削除・置換をすることも可能である。
また、上記の各構成、機能、処理部等は、それらの一部又は全部を、例えば集積回路で設計するなどによりハードウェアで実現してもよい。また、上記の各構成、機能等は、プロセッサがそれぞれの機能を実現するプログラムを解釈し、実行するためのソフトウェアで実現してもよい。各機能を実現するプログラム、テーブル、ファイル等の情報は、メモリや、ハードディスク、ＳＳＤ（Solid State Drive）等の揮発性あるいは不揮発性のストレージ、または、ＩＣカード、光ディスク等の記録媒体に保持することができる。
また、制御線や情報線は説明上必要と考えられるものを示しており、製品上必ずしもすべての制御線や情報線を示しているとは限らない。実際には殆ど全ての構成が相互に接続されていると考えてもよい。
【符号の説明】
【００３９】
１０１…弾性体形状推定システム、１０２…６軸センサ、１０３…帯状弾性体、１０４…インターフェースユニット、１０５…情報処理装置、１０６…表示部、２０１…ＣＰＵ、２０２…ＲＯＭ、２０３…ＲＡＭ、２０４…不揮発性ストレージ、２０５…操作部、２０６…バス、２０７…Ａ／Ｄ変換器、３０１…入出力制御部、３０２…座標演算部、３０３…画像描画処理部、４０１…疑似剛体、４０２…関節、４０３…バネ、４０４…バネ関節
【図１】
0006558727.tif 000010
【図２】
0006558727.tif 000011
【図３】
0006558727.tif 000012
【図４】
0006558727.tif 000013
【図５】
0006558727.tif 000014