JP6465289B2 - 柔軟物の状態計算方法、ロボット軌道生成装置、ロボット装置、プログラム及び記録媒体 - Google Patents

Description

本発明は、柔軟物（例えば、ワイヤー、ハーネス、ケーブルなど）の状態（形状、拘束力、拘束トルク）を計算する計算方法、ロボット軌道生成装置、ロボット装置、プログラム及び記録媒体に関する。

製品を設計し製造する上で、ワイヤー、ハーネス、ケーブルなどの柔軟物を配置する場合がある。通常、製品の設計は、仮想３次元空間内における柔軟物と他の剛体などの部品との配置を考慮して行う。

ワイヤー、ハーネス、ケーブルなどの柔軟物は、弾性特性を有するが、弾性論で前提とする微小変形の範囲を越えて大きく変形する。大変形した形状は１次元幾何物体である曲線で特徴づけられるため、このような大変形柔軟物を以下では線条弾性体とよぶ。

製品や装置の配置設計においては、線条弾性体を部分的に拘束（例えば把持）する拘束部（例えばロボットハンド）を空間内で移動させる際に、拘束部の移動により線条弾性体の長さや形状を変化させて設計する必要がある。また、線条弾性体を拘束する拘束部において拘束に必要な拘束力及び拘束トルクから拘束部の選定や設計をする必要がある。

そのような設計において、線条弾性体の形状特性、拘束条件を与え、線条弾性体を強制変位させた場合の予測形状を有限要素法により算出することが知られている（特許文献１参照）。しかしながら、有限要素法により線条弾性体の形状を算出する場合、線条弾性体の大変形に対応するには、非線形の境界値問題を解く必要があるが、境界値問題を解くには収束計算が必要となり、収束計算によっては計算コストが大きくなるという問題があった。

そこで、有限要素法を用いない構造解析手法として、線条弾性体の形状を特徴づける空間曲線に沿って質点とバネが連続的に分布するモデルを使用することも考えられる。このモデルに基づけば、線条弾性体の大変形は計算できるが、非線形の偏微分方程式に対する境界値問題を解くことが必要であるため、何らかの数値計算法に頼らなければならない。

一方、初期値を推測した上で、初期値問題に対して数値積分を行うことで最終値を求め、最終値が境界条件に収束するまで初期値の推定を繰り返すＳｈｏｏｔｉｎｇ法（狙い撃ち法）という計算手法が知られている（非特許文献１参照）。

特許第４３９２２９７号公報

Chao-Chieh Lan and Kok-Meng Lee: Generalized Shooting Method for Analyzing Compliant Mechanisms with Curved Members, ASME Journal of Mechanical Design, 128, 765.775, 2006.

しかしながら、非特許文献１には、平面変形の計算例については記載されているものの、３次元空間への拡張方法については記載されていない。したがって、３次元空間で大変形する柔軟物の状態（形状、拘束力及び拘束トルク）の収束計算に、非特許文献１の計算方法をそのまま適用することは困難であった。

そこで、本発明は、柔軟物の形状、拘束力及び拘束トルクを計算する際の計算コストを小さくしながら、柔軟物の形状、拘束力及び拘束トルクを短時間で求めることを目的とするものである。

本発明の柔軟物の状態計算方法は、演算部が、少なくとも２つの拘束部により位置及び姿勢が拘束される柔軟物について前記少なくとも２つの拘束部の内、拘束される２つの拘束箇所の間の形状と、前記２つの拘束箇所のうち一方の拘束箇所の位置及び姿勢を固定したときに前記２つの拘束箇所のうち他方の拘束箇所に生じる拘束力及び拘束トルクと、を計算する柔軟物の状態計算方法であって、前記演算部が、前記柔軟物を仮想３次元空間内で複数のバネ関節及び複数のリンクでモデル化し、前記仮想３次元空間内における前記他方の拘束箇所の目標位置、前記他方の拘束箇所の目標姿勢、前記目標位置に対して許容する位置ずれ量を示す位置許容誤差、前記目標姿勢に対して許容する姿勢ずれ量を示す姿勢許容誤差、前記拘束力の初期値、及び前記拘束トルクの初期値を設定する設定工程と、前記演算部が、前記拘束力及び前記拘束トルクに基づいて、前記バネ関節の角度、前記リンクの姿勢、及び前記バネ関節の位置を、前記一方の拘束箇所から前記他方の拘束箇所に向かって順次計算することで、前記柔軟物の前記２つの拘束箇所の間の形状を求める形状計算工程と、前記演算部が、前記形状計算工程により求めた形状計算結果に基づく前記他方の拘束箇所の位置の前記目標位置に対する位置誤差が、前記位置許容誤差以下であるか否かを判断する位置判断工程と、前記演算部が、前記位置判断工程にて前記位置誤差が前記位置許容誤差を上回っていると判断した場合、前記拘束力を更新する拘束力更新工程と、前記演算部が、前記形状計算工程により求めた形状計算結果に基づく前記他方の拘束箇所の前記目標姿勢に対する姿勢誤差が、前記姿勢許容誤差以下であるか否かを判断する姿勢判断工程と、前記演算部が、前記姿勢判断工程にて前記姿勢誤差が前記姿勢許容誤差を上回っていると判断した場合、前記拘束トルクを更新する拘束トルク更新工程と、を備え、前記演算部は、前記位置判断工程にて前記位置誤差が前記位置許容誤差以下と判断するまで、前記拘束力更新工程、前記形状計算工程及び前記位置判断工程を繰り返し、且つ前記姿勢判断工程にて前記姿勢誤差が前記姿勢許容誤差以下と判断するまで、前記拘束トルク更新工程、前記形状計算工程及び前記姿勢判断工程を繰り返し、前記拘束トルク更新工程では、前記演算部が、前記姿勢誤差に比例する値を用いて、前記拘束トルクを更新し、前記拘束力更新工程では、前記演算部が、前記位置誤差を前記拘束トルクのベクトル方向の軸まわりに回転変換した値に比例する値を用いて、前記拘束力を更新することを特徴とする。

本発明によれば、柔軟物の形状、拘束力及び拘束トルクを計算する際の計算コストが小さくなり、柔軟物の形状、拘束力及び拘束トルクを短時間で求めることが可能となる。

本発明の第１実施形態に係る柔軟物を示す模式図である。 本発明の第１実施形態に係る柔軟物の状態を計算する柔軟物状態計算装置を有する計算システム示すブロック図である。 本発明の第１実施形態に係る柔軟物の状態計算方法の各工程を示すフローチャートである。 図１に示す柔軟物を離散化したモデル図である。 第１実施形態のアルゴリズムによるシミュレーション結果を示すグラフである。 比較例１のアルゴリズムによるシミュレーション結果を示すグラフである。 比較例２のアルゴリズムによるシミュレーション結果を示すグラフである。 本発明の第２実施形態に係るロボット装置を示す斜視図である。 （ａ）は、本発明の第２実施形態に係るロボット装置による製造作業を説明するためのロボットハンド近傍を示す斜視図である。（ｂ）は、本発明の第２実施形態に係るロボット装置による製造作業を説明するためのロボットハンド近傍を示す正面図である。 本発明の第２実施形態に係る柔軟物の状態計算方法の各工程を示すフローチャートである。 本発明の第３実施形態に係る柔軟物の状態計算方法の各工程を示すフローチャートである。 （ａ）は、本発明の第４実施形態において計算対象となる実装基板の斜視図である。（ｂ）は、本発明の第４実施形態において計算対象となる実装基板の上面図である。 本発明の第４実施形態に係る柔軟物の状態計算方法の各工程を示すフローチャートである。 （ａ）は、変形例として、ロボット装置による製造作業を説明するためのロボットハンド近傍を示す斜視図である。（ｂ）は、変形例として、ロボット装置による製造作業を説明するためのロボットハンド近傍を示す正面図である。

以下、本発明を実施するための形態を、図面を参照しながら詳細に説明する。

［第１実施形態］
図１は、本発明の第１実施形態に係る柔軟物を示す模式図である。図１に示す柔軟物は線条弾性体（帯条の弾性体も含む）３であり、大変形する。線条弾性体３は、ＸＹＺの直交座標で規定された空間内に配置される。線条弾性体３は、例えばワイヤー、ハーネス、ケーブル（例えば、フレキシブルフラットケーブル）である。線条弾性体３の長手方向の両端１，２が、拘束部６，７により拘束される拘束箇所である。２つの拘束箇所（両端）１，２のうち、一方の拘束箇所をベース１、他方の拘束箇所を手先２とする。線条弾性体３は、２つの拘束部６，７のうち一方の拘束部６によりベース１の位置及び姿勢が拘束され、他方の拘束部７により手先２の位置及び姿勢が拘束される。線条弾性体３のベース１と手先２との間（２つの拘束箇所間）の長手方向の長さをＬとする。第１実施形態では、ベース１と手先２との間の部分が大変形する場合について、ベース１の位置及び姿勢を固定したときの、ベース１と手先２との間の形状、並びに手先２に生じる拘束力及び拘束トルクを計算する。以下、形状、拘束力及び拘束トルクを総称して、「状態」という。

図２は、本発明の第１実施形態に係る柔軟物の状態を計算する柔軟物状態計算装置を有する計算システム示すブロック図である。計算システム１００は、柔軟物状態計算装置（以下、「計算装置」という）２００と、計算装置２００に接続された操作装置３０１、表示装置３０２及び外部記憶装置３０３と、を有する。計算装置２００は、コンピュータで構成されている。

計算装置２００は、演算部としてのＣＰＵ（Central Processing Unit）２０１を備えている。また、計算装置２００は、記憶部として、ＲＯＭ（Read Only Memory）２０２、ＲＡＭ（Random Access Memory）２０３、ＨＤＤ（Hard Disk Drive）２０４を備えている。また、計算装置２００は、記録ディスクドライブ２０５、及び各種のインタフェース２１１〜２１３を備えている。

ＣＰＵ２０１には、ＲＯＭ２０２、ＲＡＭ２０３、ＨＤＤ２０４、記録ディスクドライブ２０５及び各種のインタフェース２１１〜２１３が、バス２２０を介して接続されている。ＲＯＭ２０２には、ＢＩＯＳ等の基本プログラムが格納されている。ＲＡＭ２０３は、ＣＰＵ２０１の演算処理結果等、各種データを一時的に記憶する記憶装置である。

ＨＤＤ２０４は、ＣＰＵ２０１の演算処理結果や外部から取得した各種データ等を記憶する記憶装置であると共に、ＣＰＵ２０１に、後述する各種演算処理を実行させるためのプログラム２４０を記録するものである。ＣＰＵ２０１は、ＨＤＤ２０４に記録（格納）されたプログラム２４０に基づいて線条弾性体（柔軟物）３の状態（形状、拘束力及び拘束トルク）の計算方法の各工程を実行する。

記録ディスクドライブ２０５は、記録ディスク２４１に記録された各種データやプログラム等を読み出すことができる。

インタフェース２１１には、ユーザが操作可能な操作装置３０１が接続されている。操作装置３０１は、例えばキーボードやマウス等であり、ＣＰＵ２０１にユーザの操作に応じた信号を出力する。

インタフェース２１２には、表示装置３０２が接続されている。表示装置３０２は、ＣＰＵ２０１の制御の下、各種画像を表示する。

インタフェース２１３には、外部記憶装置３０３が接続されている。外部記憶装置３０３は、書き換え可能な不揮発性メモリや外付けＨＤＤ等の記憶装置である。

なお、本実施形態では、コンピュータ読み取り可能な記録媒体がＨＤＤ２０４であり、ＨＤＤ２０４にプログラム２４０が格納される場合について説明するが、これに限定するものではない。プログラム２４０は、コンピュータ読み取り可能な記録媒体であれば、いかなる記録媒体に記録されていてもよい。例えば、プログラム２４０を供給するための記録媒体としては、図２に示すＲＯＭ２０２や外部記憶装置３０３、記録ディスク２４１等を用いてもよい。具体例を挙げて説明すると、記録媒体として、フレキシブルディスク、ハードディスク、光ディスク、光磁気ディスク、ＣＤ−ＲＯＭ、ＣＤ−Ｒ、磁気テープ、不揮発性メモリ、ＲＯＭ等を用いることができる。

ＣＰＵ２０１は、線条弾性体３のベース１と手先２との間の形状と、ベース１の位置及び姿勢を固定したときに手先２に生じる拘束力及び拘束トルクとを計算する。以下、線条弾性体３の状態（形状、拘束力及び拘束トルク）の計算方法について詳細に説明する。

図３は、本発明の第１実施形態に係る柔軟物の状態（形状、拘束力及び拘束トルク）の計算方法の各工程を示すフローチャートである。ここで、図２に示す演算部としてのＣＰＵ２０１は、プログラム２４０を実行することにより、図３に示す各工程を実行する。

まず、ＣＰＵ２０１は、線条弾性体３の離散モデル化を行う（Ｓ１：設定工程）。図４は、図１に示す線条弾性体（柔軟物）３を離散化したモデル図である。ＣＰＵ２０１は、図４に示すように、線条弾性体３を、ＸＹＺの直交座標で規定された仮想３次元空間内で複数（ｎ個：ｎは２以上の正の整数）のバネ関節４_１〜４_ｎ及び複数（ｎ個）のリンク５_１〜５_ｎでモデル化する。各リンク５_１〜５_ｎは、軸状の剛体とする。線条弾性体３の離散化モデルは、リンク５_１〜５_ｎをバネ関節４_１〜４_ｎにより直列に接続した剛体・バネ関節直列モデルである。

なお、第１実施形態では、バネ関節４_１〜４_ｎを繋ぐリンク５_１〜５_ｎを剛体としたが、直動バネとして線条弾性体３をモデル化することも可能である。

実際には図１に示すような曲線状の線条弾性体３を、剛体・バネ関節直列モデルでは、図４に示すように、複数（ｎ個）のバネ関節４_１〜４_ｎを節点とし複数（ｎ個）のリンク５_１〜５_ｎを線分とする、折れ線で表される。

各リンク５_１〜５_ｎの長さは同一とし、長手方向の長さをｌ＝Ｌ／ｎで表す。なお、計算の簡略化のために各リンク５_１〜５_ｎの長さを同一としたが、同一でなくてもよい。また、ｎは線条弾性体３の分割数とする。

各バネ関節（節点）４_１〜４_ｎで角度（折れ線の向き）が変化する。第１実施形態では空間変形であるので、各バネ関節４_１〜４_ｎは３自由度の関節である。

バネ関節４_１〜４_ｎ及びリンク５_１〜５_ｎの番号を、図４に示すように、ベース１（根元）側から手先２側に向かって順に付す。即ち、ベース１側から手先２側に向かって、第１バネ関節４_１，第２バネ関節４_２，…，第ｎバネ関節４_ｎとする。また、ベース１側から手先２側に向かって、第１リンク５_１，第２リンク５_２，…，第ｎリンク５_ｎとする。

第ｉバネ関節４_ｉ（ｉは１〜ｎ）の位置をｐ_ｉ−１∈Ｒ^３（Ｒ^３は３次の実数空間）とする。第ｉリンク５_ｉの姿勢をＦ_ｉ∈ＳＯ（３）（ＳＯ（３）：３次の特殊直交群｛Ｆ∈Ｒ^３×３｜Ｆ^ＴＦ＝Ｉ_３，ｄｅｔＦ＝１｝，Ｉ_３は３次の単位行列）とする。

ＣＰＵ２０１は、ステップＳ１においてモデル化するにあたり、以下の値を予め設定する。
１．バネ関節及びリンクの変形モデルの物性値及び線条弾性体の物性値から、又は同定実験から求まるバネ定数Ｋｄ
バネ定数Ｋｄ＝ｄｉａｇ｛ｋ_ｄｔ，ｋ_ｄｎ，ｋ_ｄｂ｝（３次の対角行列）である。各対角要素ｋ_ｄｔ，ｋ_ｄｎ，ｋ_ｄｂは、３自由度のバネ関節の各軸回りのバネ定数を表す。連続モデルの場合のバネ定数をＫ＝ｄｉａｇ｛ｋ_ｔ，ｋ_ｎ，ｋ_ｂ｝（ｋ_ｄｔ，ｋ_ｄｎ，ｋ_ｄｂは３自由度関節の各軸回りのバネ定数）とすると、Ｋｄ＝Ｋ／ｌの関係がある。
２．仮想３次元空間内における線条弾性体３の手先２の目標位置ｐ^＊及び目標姿勢Ｆ^＊
３．目標位置ｐ^＊に対して許容する位置ずれ量を示す位置許容誤差Ｅ_Ｐ及び目標姿勢Ｆ^＊に対して許容する姿勢ずれ量を示す姿勢許容誤差Ｅ_Ｏ
４．線状弾性体の手先位置・姿勢を拘束するための拘束力λの初期値及び拘束トルクτの初期値
５．仮想３次元空間内における線条弾性体３のベース１の位置ｐ_０及び拘束姿勢Ｆ_０

次に、ＣＰＵ２０１は、第ｉバネ関節４_ｉの角度θ_ｉを計算する（Ｓ２）。θ_ｉ∈Ｒ^３は、ベース１側から第ｉ番目の３自由度のバネ関節４_ｉの角度ベクトルであり、次式（１）で計算する。

ここで、式（１）中‘×’は外積演算であり、θ_ｔ，ｉ，θ_ｎ，ｉ，θ_ｂ，ｉ∈Ｒはそれぞれ第ｉ番目のバネ関節４_ｉの３自由度の各軸まわりの関節角度である。なお、式（１）において、ｐ_ｎはｐ^＊と置き換えてもよい。

また、Ａｉ∈Ｒ^３×３は、第ｉバネ関節４_ｉにおける３自由度の関節の各軸方向の単位ベクトルａ_ｔ，ｉ，ａ_ｎ，ｉ，ａ_ｂ，ｉ∈Ｒ^３を並べてできる行列であり、次式（２）で定義される。

次に、ＣＰＵ２０１は、第ｉリンク５_ｉの姿勢Ｆ_ｉを計算する（Ｓ３）。具体的に説明すると、ＣＰＵ２０１は、第ｉリンク５_ｉの姿勢Ｆ_ｉ∈ＳＯ（３）を、次式（３）で計算する。

ここで、Ｒ_Ｊ∈ＳＯ（３）は、３自由度のバネ関節４_ｉの回転作用を表す行列であり、次式（４）で定義される。

ここで、Ｒ（ａ，θ）は、ａ軸まわりに角度θ回転させる回転作用素（回転行列）である。

次に、ＣＰＵ２０１は、第（ｉ＋１）バネ関節４_ｉ＋１の位置ｐ_ｉを計算する（Ｓ４）。具体的に説明すると、ＣＰＵ２０１は、第（ｉ＋１）バネ関節４_ｉ＋１の位置ｐ_ｉ∈Ｒ^３を次式（５）で計算する。

ここでｅ_ｘ＝［１ ０ ０］^Ｔはｘ方向の単位ベクトルである。

これらステップＳ２〜Ｓ４を、ｉ＝１からｉ＝ｎまで繰り返す（Ｓ５：形状計算工程）。即ちステップＳ５は、ステップＳ２の第ｉバネ関節の角度θ_ｉの計算、ステップＳ３の第ｉリンクの姿勢Ｆ_ｉの計算、ステップＳ４の第（ｉ＋１）関節の位置ｐ_ｉの計算（第（ｎ＋１）関節位置ｐ_ｎは手先２の位置に相当）をｉ＝１からｉ＝ｎまで繰り返す。以上、ＣＰＵ２０１は、拘束力λ及び拘束トルクτに基づいて、バネ関節の角度、リンクの姿勢、及びバネ関節の位置を、ベース１側から手先２側に向かって順次計算することで、線条弾性体３のベース１と手先２との間の形状を求める。ここで、線条弾性体３のベース１と手先２との間の形状とは、バネ関節４_１〜４_ｎの角度、リンク５_１〜５_ｎの姿勢及びバネ関節４_１〜４_ｎの位置のことである。

次にＣＰＵ２０１は、ステップＳ５により求めた形状計算結果に基づく手先２の位置ｐ_ｎの目標位置ｐ^＊に対する位置誤差ΔＰ（＝ｐ^＊−ｐ_ｎ）の大きさが、位置許容誤差Ｅ_Ｐ以下であるか否かを判断する（Ｓ６：位置判断工程）。判断は次式（６）で行う。

ＣＰＵ２０１は、式（６）において許容誤差Ｅ_Ｐ内であれば（Ｓ６：Ｙｅｓ）、ステップＳ７に移行する。ＣＰＵ２０１は、ステップＳ５により求めた形状計算結果に基づく手先２の姿勢Ｆ_ｎの目標姿勢Ｆ^＊に対する姿勢誤差ΔＯ（＝θ_ｅｒｒω）の大きさが、姿勢許容誤差Ｅ_Ｏ以下であるか否かを判断する（Ｓ７：姿勢判断工程）。判断は次式（７）で行う。

ここで、θ_ｅｒｒω∈Ｒ^３は姿勢誤差を表すベクトルであり、次式（８）で定義される。

θ_ｅｒｒ∈[−π π]は等価回転角度であり、次式（９）により求められる。

また、行列Φは次式（１０）で与えられる。

また、ω∈Ｒ^３（｜｜ω｜｜＝１）は等価回転軸であり、次式（１１）で与えられる。

式（６）及び式（７）において許容値内であれば（Ｓ６，Ｓ７：Ｙｅｓ）、線条弾性体３の形状、拘束力及び拘束トルクの計算を終了する。

ＣＰＵ２０１は、ステップＳ６において、位置誤差ΔＰの大きさが位置許容誤差Ｅ_Ｐよりも大きいと判断した場合、即ち位置誤差ΔＰの大きさが位置許容誤差Ｅ_Ｐを上回っていると判断した場合（Ｓ６：Ｎｏ）、拘束力を更新する（Ｓ８：拘束力更新工程）。次に、ＣＰＵ２０１は、ステップＳ２の処理に移行し、位置誤差の大きさが位置許容誤差以下になるまで、繰り返し計算を行う。

ＣＰＵ２０１はステップＳ７において姿勢誤差ΔＯの大きさが姿勢許容誤差Ｅ_Ｏよりも大きいと判断した場合、即ち姿勢誤差ΔＯの大きさが姿勢許容誤差Ｅ_Ｏを上回っていると判断した場合（Ｓ７：Ｎｏ）、拘束トルクを更新する（Ｓ９：拘束トルク更新工程）。次に、ステップＳ２の処理に移行し、位置誤差の大きさが位置許容誤差以下となり、かつ姿勢誤差の大きさが姿勢許容誤差以下となるまで繰り返し計算を行う。

即ちＣＰＵ２０１は、ステップＳ６にて位置誤差ΔＰの大きさが位置許容誤差Ｅ_Ｐ以下となるまで、ステップＳ８、ステップＳ５及びステップＳ６を繰り返す。

また、ＣＰＵ２０１は、ステップＳ７にて姿勢誤差ΔＯの大きさが姿勢許容誤差Ｅ_Ｏ以下となるまで、ステップＳ９、ステップＳ５及びステップＳ７を繰り返す。第１実施形態では、ステップＳ６にて位置誤差ΔＰの大きさが位置許容誤差Ｅ_Ｐであると判断した場合に、ステップＳ７に移行する。したがって、姿勢誤差ΔＯの大きさが姿勢許容誤差Ｅ_Ｏ以下となるまで、ステップＳ９、ステップＳ５、ステップＳ６及びステップＳ７を繰り返すことになる。

第１実施形態では、ステップＳ８における拘束力の更新則は次式（１２）のアルゴリズムで定義される。

即ち、ＣＰＵ２０１は、位置誤差ΔＰを拘束トルクτのベクトル方向の軸まわりに回転変換した値に比例する値（更新値）を用いて、拘束力λを更新する。つまり、更新値を拘束力λに加算して新たな拘束力λとする。具体的に説明すると、ＣＰＵ２０１は、位置誤差ΔＰを拘束トルクτのベクトル方向の軸まわりに拘束トルクτの大きさに比例する量で逆回転した値に比例する値（更新値）を加算して、拘束力λを更新する。

ここで、逆回転とは、式（１２）中マイナスの符号で表され、拘束トルクτのベクトル方向に見て、ベクトル方向の軸まわりに右回転を正としたときの逆、即ち左回転をいう。

また、第１実施形態では、ステップＳ９における拘束トルクの更新則は次式（１３）のアルゴリズムで定義される。

即ち、ＣＰＵ２０１は、姿勢誤差ΔＯに比例する値（更新値）を用いて、拘束トルクτを更新する。つまり、更新値を拘束トルクτに加算して新たな拘束トルクτとする。

式（１２）及び式（１３）中、‘←’は更新を表す。ｃ_Ｐおよびｃ_Ｏは、更新則の調整パラメータであり、正の定数である。ｃ_Ｔも正の定数であるが、このアルゴリズム特有の調整パラメータである。

拘束力、拘束トルクの更新則として手先の位置及び姿勢に関する誤差情報を利用して、拘束力、拘束トルクを更新する際の最もシンプルなアルゴリズムは、位置誤差、姿勢誤差に比例した値を、更新増分とすることである。

比較例１では、拘束トルクの更新則は式（１３）と同じとした上で、拘束力の更新側は、式（１４）に示すように、位置誤差に比例した値を更新増分とする。

この更新則を比例アルゴリズムとよぶことにする。この比例アルゴリズムでは、収束性に問題がある。この問題を解決したのが，第１実施形態においてステップＳ８，Ｓ９で使用した式（１２）及び式（１３）の更新アルゴリズムである。手先２の姿勢を収束させるための計算プロセスにおいて、拘束トルクの更新増分を姿勢誤差としている点は前述比較例１の比例アルゴリズムと同じである。第１実施形態の特徴は、手先２の位置を収束させるための計算プロセスにある。

前述比較例１の比例アルゴリズムとの違いは位置誤差に回転作用が加わっている点である。より具体的には、拘束力の更新増分を、位置誤差を拘束トルクτのベクトル方向の軸まわりに逆回転させた量としている。

手先２の位置誤差に回転を加えることが効果的であるならば、トルクベクトルの暫定値ではなく、姿勢誤差に対応して回転させることが妥当のように考えられる。比較例２では、次式（１５）のようなアルゴリズムとしている。

比較例２でも、手先２の拘束トルクの更新則は式（１３）と同じである。ここで、ｃ_Ｒは正の定数であり、このアルゴリズム特有の調整パラメータである。比較例２のアルゴリズムと第１実施形態のアルゴリズムとの違いは、τの代わりに姿勢誤差ΔＯを用いている点である。この比較例２のアルゴリズムを単純回転アルゴリズムとよぶ。単純回転アルゴリズムでも収束性は改善されない。これは姿勢誤差に対応して回転させることだけでは収束性の改善には繋がらないことを示している。

図５は、第１実施形態のアルゴリズムによるシミュレーション結果を示すグラフである。図６は、比較例１のアルゴリズムによるシミュレーション結果を示すグラフである。図７は、比較例２のアルゴリズムによるシミュレーション結果を示すグラフである。

シミュレーション条件は以下である。
・弾性体材料特性
線条弾性体３の全長Ｌ＝１５０［ｍｍ］
材料特性：ヤング率Ｅ＝２１０［ＧＰａ］＝２１０，０００［Ｎ／ｍｍ^２］、ポワソン比ν＝０．３
断面形状：幅ｗ＝２０［ｍｍ］、厚さｔ＝０．１［ｍｍ］の長方形
離散バネ定数ｋ_ｄｔ，ｋ_ｄｎ，ｋ_ｄｂは以下の式（１６）及び式（１７）からｋ_ｄｔ＝１６２［ｍＮｍ／ｒａｄ］、ｋ_ｄｎ＝４．６７［ｋＮｍ／ｒａｄ］、ｋ_ｄｂ＝１６７［ｍＮｍ／ｒａｄ］とした。

ここで、式（１６）のＪは断面捩りモーメントであり、断面形状が長方形のため、Ｊ＝ｋ２ｗｔ^３であり、ｋ２＝０．３とした。また、式（１７）のＩは断面２次モーメントであり、断面形状が長方形のため、ｋ_ｄｎにおいてはＩ＝ｗｔ^３／１２から求め、ｋ_ｄｂにおいてはＩ＝ｗ^３ｔ／１２から求めた。
・シミュレーションパラメータ
分割数ｎ＝５０
位置許容誤差Ｅ_Ｐ＝０．０１［ｍｍ］
姿勢許容誤差Ｅ_Ｏ＝１．０［ｄｅｇ］＝π／１８０［ｒａｄ］
・初期条件
初期形状：手先位置［９０；０；７８．９７］，手先姿勢［１ ０ ０；０ ０ １；０ −１ ０］となるような形状および拘束力・拘束トルクを予め求めて与えた。
・目標条件
目標手先位置［９０；０；５８．９７］＝初期手先位置よりもｚ方向に−２０［ｍｍ］の位置。
目標手先姿勢［１ ０ ０；０ ０．３４２ ０．９３９７；０ −０．９３９７ ０．３４２０］＝初期手先姿勢をｘ軸回りに２０［ｄｅｇ］回転させた姿勢。
・アルゴリズムパラメータ
位置更新の比例定数ｃ_Ｐ＝０．０００５
姿勢更新の比例定数ｃ_Ｏ＝１．０
パラメータｃ_Ｔ＝０．１
パラメータｃ_Ｒ＝０．１

なお、これらのアルゴリズムのパラメータは、挙動が不安定にならない範囲で大きな値に設定した。

図５〜図７の実験結果に示されるように、３つのアルゴリズムにおいて、図５に示す第１実施形態のアルゴリズムのみ収束し、線条弾性体３の形状、拘束力及び拘束トルクを求めることが可能となる。また、離散化モデルを用いて線条弾性体３の形状、拘束力及び拘束トルクを計算したので、有限要素法による従来例と比較しても、線条弾性体３の形状、拘束力及び拘束トルクを計算する際の計算コストが小さくなる。よって、線条弾性体３の形状、拘束力及び拘束トルクの計算結果を短時間で収束させることができる。換言すれば、線条弾性体３の形状、拘束力及び拘束トルクを短時間で求めることが可能となる。

［第２実施形態］
次に、本発明の第２実施形態に係るロボット装置の計算装置による柔軟物の状態計算方法について説明する。図８は、本発明の第２実施形態に係るロボット装置を示す斜視図である。なお、第２実施形態において、上記第１実施形態と同様の構成については、同一符号を用い、詳細な説明は省略する。図８に示すロボット装置１０００は、ロボット６００と、ロボット６００の動作を制御するロボット制御部である制御装置４００と、制御装置４００に接続されたティーチングペンダント５００と、を備えている。また、ロボット装置１０００は、上記第１実施形態と同様の構成の柔軟物状態計算装置（計算装置）２００を備えている。第２実施形態では、計算装置２００は、ロボット６００の軌道を生成するロボット軌道生成装置である。制御装置４００は、生成された軌道に基づきロボット６００の動作を制御する。第２実施形態では計算装置２００と制御装置４００を説明の簡略化のため、物理的に分けて表現しているが、装置として一体としても構わない。

ロボット６００は、多関節のロボットである。ロボット６００は、垂直多関節型のロボットアーム６０１と、ロボットアーム６０１の先端に接続された拘束部であるロボットハンド６０２と、を備えている。

ロボットアーム６０１は、作業台に固定されるベース部（基端ロボットリンク）６０３と、変位や力を伝達する複数のロボットリンク６２１〜６２６と、を有している。ベース部６０３及び複数のロボットリンク６２１〜６２６は、複数の関節Ｊ１〜Ｊ６で旋回又は回転可能に互いに連結されている。また、ロボットアーム６０１は、各関節Ｊ１〜Ｊ６に設けられた、関節を駆動する関節駆動装置６１０を備えている。各関節Ｊ１〜Ｊ６に配置された関節駆動装置６１０は、必要なトルクの大きさに合わせて適切な出力のものが用いられる。

ロボットハンド６０２は、柔軟物を把持する複数（第２実施形態では２つ）の把持爪６０４と、複数の把持爪６０４を駆動する不図示の駆動部と、駆動部の回転角度を検出する不図示のエンコーダと、回転を把持動作に変換する不図示の機構とを有している。この不図示の機構は、カム機構やリンク機構などで必要な把持動作に合わせて設計される。なお、ロボットハンド６０２に用いる駆動部に必要なトルクは、ロボットアーム６０１の関節用と異なるが、基本構成は同じである。また、ロボットハンド６０２は、把持爪６０４等に作用する力（反力）やモーメントを検出可能な不図示の力覚センサを有している。

ティーチングペンダント５００は、制御装置４００に接続可能に構成され、制御装置４００に接続された際に、ロボットアーム６０１やロボットハンド６０２を駆動制御する指令を制御装置４００に送信可能に構成されている。

計算装置２００のＣＰＵ２０１（図２）は、与えられた２つ以上の教示点間を繋ぐロボット６００の軌道を生成して制御装置４００に軌道データを出力する。教示点は、ロボット６００の先端の位置及び姿勢を示すパラメータ値、又は関節Ｊ１〜Ｊ６の角度を示すパラメータ値で設定される。ロボット６００の軌道とは、時間をパラメータとしたロボット６００の経路を表したもの、即ち、動作時刻毎（所定時間間隔）のロボット６００の各関節Ｊ１〜Ｊ６の角度（つまり、関節駆動装置６１０のモータの回転角度）である。

図９（ａ）は、本発明の第２実施形態に係るロボット装置による製造作業を説明するためのロボットハンド近傍を示す斜視図である。図９（ｂ）は、本発明の第２実施形態に係るロボット装置による製造作業を説明するためのロボットハンド近傍を示す正面図である。なお、図９（ａ）において、ロボット６００については、ロボットハンド６０２と、ロボットアーム６０１の先端周辺のみ図示した。また、図９（ｂ）において、ロボット６００については、ロボットハンド６０２のみ図示した。

ここで、第２実施形態において、線条弾性体（柔軟物）は、フレキシブルフラットケーブル（以下、「ケーブル」）１３である。

ケーブル１３の長手方向の一端の拘束箇所（一方の拘束箇所）１１が、基板１８に固定（実装）された一方の拘束部（ロボットハンド以外の拘束部）であるコネクタ１６に挿入されて、コネクタ１６に拘束される。また、ケーブル１３の長手方向の他端の近傍の拘束箇所（他方の拘束箇所）１２が、他方の拘束部であるロボットハンド６０２の把持爪６０４に把持されて、ロボットハンド６０２に拘束される。なお、ケーブル１３をロボットハンド６０２で把持した際のケーブル１３の拘束箇所１２の位置及び姿勢は予めわかっていることとする。

コネクタ１６に挿入されたケーブル１３の拘束箇所１１が、上記第１実施形態のベース１に相当し、ケーブル１３の拘束箇所１２が、上記第１実施形態の手先２に相当する。基板１８の近傍には、基板１７が配置され、基板１７には、ケーブル１３の他端が挿入されるコネクタ１９が固定（実装）されている。基板１７は剛体である。第２実施形態では、ロボットハンド６０２にケーブル１３を把持させて、ロボット６００にケーブル１３の他端をコネクタ１９に挿入させる挿入動作を行わせる。その際、コネクタ１９が固定された基板１７が、ケーブル１３の周辺にある障害物となる。

図１０は、本発明の第２実施形態に係る柔軟物の状態計算方法の各工程を示すフローチャートである。ここで、図２に示す演算部としてのＣＰＵ２０１は、プログラム２４０を実行することにより、図１０に示す各工程を実行する。

図１０に示すフローチャートは、図９のようにロボットハンド６０２に把持された線条弾性体であるケーブル１３が障害物である基板１７に干渉しているか否かを判断する手順を示す。

なお、軌道データに従ってロボット６００を動作させると、ロボット６００の先端の位置及び姿勢、即ちロボット６００の各関節Ｊ１〜Ｊ６の角度は動作時刻毎に変化することが考えられる。第２実施形態では、ロボット６００が静止している、又はある動作時刻のロボット６００の動作状態で、ケーブル１３が障害物である基板１７に干渉しているか否かを判断する手順を示す。

第２実施形態では、計算装置２００は、３次元モデルを用い、干渉確認が可能なシミュレーション装置を想定しており、干渉が発生した場合は、図２に示す表示装置３０２の画面に３次元モデルを表示する機能があるものとする。

まず、計算装置２００のＣＰＵ２０１は、ケーブル１３のモデルが設定される仮想３次元空間の座標基準で、障害物の位置及び形状を設定する（Ｓ１１：障害物設定工程）。第２実施形態では、基板１７を障害物とし、基板１７の形状及び位置を設定する。

次に、ＣＰＵ２０１は、ケーブル１３が障害物に干渉する可能性のある干渉領域を設定する（Ｓ１２）。干渉領域は、ステップＳ１１で設定した基板１７の位置及び形状に基づいて設定する。

次に、ＣＰＵ２０１は、ケーブル１３の物性値の設定を行う（Ｓ１３：設定工程）。ケーブル１３の物性値とは、ケーブル１３の形状、拘束力及び拘束トルクの計算に必要な物性値であるケーブル１３の全長、ヤング率、ポワソン比、断面形状である幅、厚さ、断面捩りモーメントを決定する捩りの比例定数のことである。

次に、ＣＰＵ２０１は、ケーブル１３の状態計算用のパラメータの設定を行う（Ｓ１４：設定工程）。パラメータにはケーブル１３の形状、拘束力及び拘束トルクの計算に必要な分割数ｎ、拘束位置の許容誤差Ｅ_Ｐ、拘束姿勢の許容誤差Ｅ_Ｏ、初期条件としてケーブル１３の拘束箇所１２の位置及び姿勢を設定する。なお、これらステップＳ１３，Ｓ１４は、上記第１実施形態において説明した図３のステップＳ１で行ったものを、説明の都合上、別のステップとして分けて図示したものである。したがって、ステップＳ１３，Ｓ１４の設定動作を、ステップＳ１で行ってもよい。

次に、ＣＰＵ２０１は、ケーブル１３の拘束箇所１１，１２間の形状の計算を行う（Ｓ１５）。ケーブル１３の形状の計算は、上記第１実施形態において図３を用いて説明した、線条弾性体３の形状、拘束力及び拘束トルクの計算方法を用いる。最終的にステップＳ５で求めた線条弾性体３の形状の計算結果を以下のステップで用いる。

次に、ＣＰＵ２０１は、干渉確認を行う（Ｓ１６）。干渉確認は、ステップＳ１５で計算した形状のケーブル１３が、ステップＳ１２で設定した干渉領域に侵入するか否かで確認する。なお、図１０には、ケーブル１３が干渉領域に侵入していることを条件とするフローチャートが図示されているが、ケーブル１３が干渉領域に侵入しない場合には、そのまま終了する。

次に、ＣＰＵ２０１は、図３のステップＳ５による形状計算結果に基づき、ケーブル１３が障害物である基板１７と干渉するか否かを判断する（Ｓ１７）。

ＣＰＵ２０１は、干渉ありと判断した場合（Ｓ１７：Ｙｅｓ）、干渉していることを示す画像を、表示装置３０２に表示させるよう画像データを表示装置３０２に出力する。ＣＰＵ２０１は、干渉なしと判断した場合（Ｓ１７：Ｎｏ）は干渉確認を終了する。

以上、第３実施形態では、ロボットハンド６０２がケーブル１３のような柔軟物を把持した際に基板１７のような障害物と干渉するか否かの判断を、上記第１実施形態のステップＳ５で求めた形状計算結果を用いて行う。これにより、ケーブル１３が障害物に干渉するか否かの干渉判断を、迅速かつ正確に行うことができる。その結果、ロボット６００をオフライン（シミュレーション）で教示するオフライン教示の作業性が向上する。

［第３実施形態］
次に、本発明の第３実施形態に係るロボット装置の計算装置による柔軟物の状態計算方法について説明する。図１１は、本発明の第３実施形態に係る柔軟物の状態計算方法の各工程を示すフローチャートである。ロボット装置の構成は、上記第２実施形態と同様であり、詳細な説明を省略する。

第３実施形態では、上記第２実施形態の干渉確認手順を基に、動的に形状及び拘束箇所１２の位置及び姿勢が変化するケーブル１３が、障害物である基板１７に干渉するか否かを判断するまでに発展させたものである。

ロボット６００が軌道データに従って動作した際に、ロボットハンド６０２が把持したケーブル１３の拘束箇所１２の位置及び姿勢が動的に変化することにより、ケーブル１３の拘束箇所１１と拘束箇所１２との間の形状が動的に変化する。

第３実施形態では、その際に障害物である基板１７に、動的に形状が変化するケーブル１３が干渉するか否かを判断する。

以下、図１１に示すフローチャートに沿って詳細に説明する。ここで、図２に示す演算部としてのＣＰＵ２０１は、プログラム２４０を実行することにより、図１１に示す各工程を実行する。なお、ケーブル１３をロボットハンド６０２で把持した際の初期のケーブル１３の拘束箇所１２の位置及び姿勢、更にケーブル１３の拘束箇所１２の位置及び姿勢とロボット６００との相対位置は予めわかっていることとする。

図１１に示すステップＳ２１〜Ｓ２４は、上記第２実施形態で説明した図１０のステップＳ１１〜Ｓ１４と同じであり、詳細な説明は省略する。

次に、ＣＰＵ２０１は、ロボット６００の軌道計算を行う（Ｓ２５）。ロボット６００の軌道は、上述したように、２つ以上の教示点を結ぶ際のロボット６００に対する指令である。ロボット６００の移動を開始する教示点と移動を終了する教示点を結ぶ際のロボット６００の軌道の分割数は、ロボット６００の制御周期と加速度、速度、トルク等によって変化する。

次に、ＣＰＵ２０１は、ロボット６００の軌道に応じて移動するケーブル１３の拘束箇所（手先）１２の位置及び姿勢の計算を行う（Ｓ２６）。即ち、ＣＰＵ２０１は、拘束箇所１２を仮想３次元空間内で仮想的に移動させることとなる（移動工程）。ロボット６００の軌道におけるケーブル１３の拘束箇所１２の位置及び姿勢の計算は、ステップＳ２５で求めたロボット６００の軌道の開始点から終了点まで、軌道の分割数毎に実施する。

次に、ＣＰＵ２０１は、ケーブル１３の初期形状の更新を行う（Ｓ２７）。ケーブル１３の初期形状の更新は、ステップＳ２６で計算した軌道の分割数毎のケーブル１３の拘束箇所１２の位置及び姿勢から行う。

次に、ＣＰＵ２０１は、ケーブル１３の拘束箇所１１，１２間の形状計算を行う（Ｓ２８）。ケーブル１３の形状の計算は、上記第１実施形態において図３を用いて説明した、線条弾性体３の形状、拘束力及び拘束トルクの計算方法を用いる。最終的にステップＳ５で求めた線条弾性体３の形状の計算結果を、以下のステップで用いる。この計算は、軌道の分割数毎に行う。

次に、ＣＰＵ２０１は、干渉確認を行う（Ｓ２９）。干渉確認は、ステップＳ２８で計算した形状のケーブル１３が、ステップＳ２２で設定した干渉領域に侵入するか否かで確認する。なお、図１１には、ケーブル１３が干渉領域に侵入していることを条件とするフローチャートが図示されているが、ケーブル１３が干渉領域に侵入しない場合には、ステップＳ３２の処理に移行する。

次に、ＣＰＵ２０１は、ステップＳ３０の処理に移行するが、ステップＳ３０，Ｓ３１は、上記第２実施形態で説明した図１０のステップＳ１７，Ｓ１８と同じ内容のため、説明を省略する。

次に、ＣＰＵ２０１は、ロボット６００の軌道の終点か否かを確認する（Ｓ３２）。ＣＰＵ２０１は、終点でなければ（Ｓ３２：Ｎｏ）、ステップＳ２６に移行し、ロボット軌道の終点になるまで（Ｓ３２：Ｙｅｓ）、ステップＳ２６〜ステップＳ３２を繰り返す。これにより、ＣＰＵ２０１は、ステップＳ３０の処理を、ステップＳ２６において仮想３次元空間内で移動させた拘束箇所１２の位置及び姿勢を算出する度に、即ち拘束箇所１２を仮想３次元空間内で移動させる度に行う。

ＣＰＵ２０１は、ステップＳ３２において、軌道の終点が確認された場合、干渉確認を終了する。そして、ＣＰＵ２０１は、作業者の指示に従って又はプログラムに従って、干渉する軌道について修正する。このように、ＣＰＵ２０１は、ケーブル１３の状態に基づいてロボット６００の軌道を生成する。このように生成された軌道のデータは、計算装置２００から制御装置４００に送信され、制御装置４００にて設定される。そして、制御装置４００は、製造工程において、設定した軌道に従ってロボット６００の動作を制御する。

以上、第３実施形態によれば、ロボットハンド６０２で把持したケーブル１３の拘束箇所１２の位置又は姿勢がロボット６００の移動により動的に変化した場合にも、干渉を判断することが可能となる。そして、ケーブル１３が障害物に干渉するか否かの干渉判断を、迅速かつ正確に行うことができる。その結果、ロボット６００をオフライン（シミュレーション）で教示するオフライン教示の作業性が向上する。

なお、第３実施形態では、干渉確認を実施するロボット６００の軌道として、教示点（始点）から教示点（終点）までの軌道全区間としたが、任意の区間の軌道としてもよい。また、軌道の分割数に関しても任意の分割数でもよい。また、干渉の確認方法は、上記第１実施形態及び第２実施形態共に干渉確認が可能なシミュレーション装置内に組み込まれ、干渉領域と重なった線条弾性体を３次元データで画面表示することが望ましいが、干渉の有無を画面表示するのみでもよい。干渉が確認された場合、ロボット６００の位置及び姿勢やロボットハンド６０２による拘束箇所１２の変更、干渉回避のための教示点を作成してケーブル１３が基板１７と干渉することを回避することが可能となる。

［第４実施形態］
次に、本発明の第４実施形態に係るロボット装置の計算装置による柔軟物の状態計算方法について説明する。図１２（ａ）は、本発明の第４実施形態において計算対象となる実装基板の斜視図であり、図１２（ｂ）は、本発明の第４実施形態において計算対象となる実装基板の上面図である。

基板２７の上には、フレキシブルフラットケーブル（以下、「ケーブル」という）が接続されるコネクタ２５ａ，２５ｂと、コンデンサ等の電子部品２６ａ〜２６ｃとがレイアウト（実装）されている。

図１２（ａ）に示すように、コネクタ２５ａ，２５ｂに接続されるケーブルの設計を行う際に、電子部品２６ａ〜２６ｃを避けて接続しなければならない場合、ケーブルの形状が複雑なものになる。そのため、設計したケーブルの形状において、コネクタ２５ａ，２５ｂからケーブルが抜ける力又はトルクや、ケーブルに断線が発生する力又はトルクが、ケーブルの長手方向の端部にかかっているか否かを判断する必要がある。

第４実施形態では、ケーブルのような線条弾性体を設計する際に、コネクタからケーブルが抜ける力又はトルクや、ケーブルに断線が発生する力又はトルクが、ケーブルの端にかかっているか否かを確認する。以下、ケーブルの形状を決定する設計支援手順について説明する。なお、第４実施形態においても、上記実施形態と同様、計算装置２００（図２）を用いて行う。

図１３は、本発明の第４実施形態に係る柔軟物の状態計算方法の各工程を示すフローチャートである。ここで、図２に示す演算部としてのＣＰＵ２０１は、プログラム２４０を実行することにより、図１３に示す各工程を実行する。

ＣＰＵ２０１は、コネクタ２５ａ，２５ｂの位置及び姿勢の設定を行う（Ｓ４１：設定工程）。第４実施形態では、コネクタ２５ａが一方の拘束部であり、コネクタ２５ｂが他方の拘束部である。コネクタ２５ａの位置及び姿勢が、ケーブルの一端（一方の拘束箇所）の位置及び姿勢、コネクタ２５ｂの位置及び姿勢が、ケーブルの他端（他方の拘束箇所）の位置及び姿勢である。ケーブルの一端（一方の拘束箇所）が上記第１実施形態のベース１、ケーブルの他端（他方の拘束箇所）が上記第１実施形態の手先２に相当する。

次に、ＣＰＵ２０１は、線条弾性体であるケーブルの物性値の設定を行う（Ｓ４２：設定工程）。ケーブルの物性値は、ケーブルの形状、拘束力及び拘束トルクの計算に必要な物性値であるヤング率、ポワソン比、断面形状である幅、厚さ、断面捩りモーメントを決定する捩りの比例定数を設定する。

次に、ＣＰＵ２０１は、線条弾性体であるケーブルの状態（形状、拘束力及び拘束トルク）の計算用のパラメータの設定を行う（Ｓ４３：設定工程）。パラメータには、ケーブルの形状、拘束力及び拘束トルクの計算に必要な分割数ｎ、拘束位置の許容誤差Ｅ_Ｐ、拘束姿勢の許容誤差Ｅ_Ｏを設定する。

次に、ＣＰＵ２０１は、ケーブルの端にかかる拘束力及び拘束トルクの上限値（負荷力及び負荷トルク）を設定する（Ｓ４４：設定工程）。上限値はケーブルの接続不良が発生するコネクタからケーブルが抜けない力及びトルクや、ケーブルに断線が発生しない力及びトルクを設定するのが望ましい。

次に、ＣＰＵ２０１は、設計したケーブルの初期形状を設定する（Ｓ４５：設定工程）。

次に、ＣＰＵ２０１は、ケーブルの拘束力及び拘束トルクの計算を行う（Ｓ４６）。ケーブルの拘束力及び拘束トルクの計算方法は、上記第１実施形態で説明した線条弾性体３の形状、拘束力及び拘束トルクの計算方法とする。

ＣＰＵ２０１は、拘束力が、予め設定した負荷力（上限値）を超えるか否かを判断する（Ｓ４７：拘束力判断工程）。ＣＰＵ２０１は、ステップＳ４７において、上限値を超えていると判断した場合（Ｓ４７：Ｙｅｓ）、拘束力の計算結果を出力し（Ｓ４８）、次のステップＳ４９に移行する。出力先は、例えば図２に示す表示装置３０２であり、計算結果が表示装置３０２に表示される。ＣＰＵ２０１は、ステップＳ４７において、上限値を超えていないと判断した場合（Ｓ４７：Ｎｏ）、そのまま次のステップＳ４９に移行する。

ＣＰＵ２０１は、拘束トルクが、予め設定した負荷トルク（上限値）を超えるか否かを判断する（Ｓ４９：拘束トルク判断工程）。ＣＰＵ２０１は、ステップＳ４９において、上限値を超えていると判断した場合（Ｓ４９：Ｙｅｓ）、拘束トルクの計算結果を出力して（Ｓ５０）終了し、上限値を超えていないと判断した場合（Ｓ４９：Ｎｏ）そのまま終了する。出力先は、例えば図２に示す表示装置３０２であり、計算結果が表示装置３０２に表示される。

ステップＳ４７及びステップＳ４９において、上限値を超えていない場合は、ケーブルの形状が決定する。

以上、第４実施形態によれば、コネクタからケーブルが抜ける力又はトルクや、ケーブルに断線が発生する力又はトルクが、ケーブルにかからないように、ケーブルの形状を設計することが可能となる。

なお、第４実施形態では、拘束力、拘束トルクの上限値に対し、計算したケーブルの拘束力、拘束トルクが超えている場合、拘束力、拘束トルクの計算結果を出力したが、単純にＯＫ、ＮＧなど、ユーザが判断できる結果の出力のみでもよい。

また、第４実施形態では、コネクタの位置及び姿勢を固定してケーブルの形状を決定する設計支援方法を提供しているが、これに限定するものではない。図１３のステップＳ４１のコネクタの位置及び姿勢の設定を変更し、図１３のステップＳ４５において、ケーブルの形状を固定し、コネクタの配置を決定する設計支援方法としてもよい。

また、第４実施形態では、図１３に示すステップＳ４７，Ｓ４９において、上限値を超えている場合に図１３に示すステップＳ４８，Ｓ５０において、拘束力、拘束トルクの計算結果を出力することでケーブルの設計者がケーブルの設計変更することができる。しかし、線条弾性体の端にかかる拘束力、拘束トルクの上限を超えた場合に、ケーブルの設計変更ではなく、コネクタの再選定やコネクタの仕様変更をしてもよい。

また、第４実施形態では、コネクタの位置及び姿勢を固定してケーブルの形状を決定する設計支援方法を提供した。しかし、一方の拘束部をロボットハンドとすることで、ロボットの動作により他方の拘束部であるコネクタからケーブルが抜ける力や、ケーブルに断線が発生する力がかかっていないことを確認することも可能となる。その方法として、まず、図１３のステップＳ４５のケーブルの形状を固定する。次に、ステップＳ４１のコネクタ位置姿勢設定の内、片方を第３実施形態の図９のようにケーブル１３の片方をロボットハンド６０２で把持する。次に、ロボット６００が動作した際のケーブル１３の動的に変化する手先位置及び姿勢と置き換える。それにより、ロボットの動作によりコネクタからケーブルが抜ける力や、ケーブルに断線が発生する力がかかっていないことを確認することが可能となる。

第１〜第４実施形態では、フレキシブルフラットケーブルを含め、断面形状が長方形のものを線条弾性体としたが、断面形状は三角形以上の多角形、円や楕円のような丸い形状のワイヤーや管、ゴムチューブ等であってもよい。また、線条弾性体が、束線のような複数の線から構成されていてもよい。

また、第２〜第４実施形態では、ケーブルの初期形状は直線を想定しているが、Ｌ字やＮ字のように、途中部位が曲がった形状、Ｙ字、Ｅ字のように２股以上に分かれた形状であってもよい。

また、第２〜第４実施形態では、ベースの位置及び姿勢を固定とした。図１４（ａ）は、変形例として、ロボット装置による製造作業を説明するためのロボットハンド近傍を示す斜視図である。図１４（ｂ）は、変形例として、ロボット装置による製造作業を説明するためのロボットハンド近傍を示す正面図である。

図１４（ａ）及び図１４（ｂ）のように、ケーブル１３の一端がコネクタ１６に拘束され、更に、ケーブル１３の途中の箇所が、固定部位５８で固定されている。固定部位５８により拘束される箇所を、ベース及び手先のうち一方とし、ケーブル１３の一端又は他端を、ベース及び手先のうち他方として、線条弾性体の形状、拘束力、拘束トルクを求めてもよい。つまり、拘束される箇所は、線条弾性体の両端に限るものではなく、長手方向の任意の２箇所が拘束されればよい。この場合、２箇所の間の形状を求め、２箇所の一方に対して他方に働く拘束力及び拘束トルクを求めればよい。

また、固定部位５８が複数になり、フレキシブルフラットケーブルが順次固定されることでベース又は手先の位置及び姿勢を、固定部位間や固定部位とケーブルの端部等に変更させて線条弾性体の形状、拘束力及び拘束トルクを求めてもよい。つまり、拘束される箇所が３つ以上あってもよく、隣接する２つの拘束箇所の間の形状、２つの拘束箇所のうち他方の拘束箇所の拘束力及び拘束トルクを求めればよい。

また、第２〜第４実施形態では、計算装置２００が、ロボット制御部である制御装置４００とは別のロボット軌道生成装置である場合について説明したが、これに限定するものではない。制御装置４００が計算装置２００の機能を有する場合であってもよい。この場合、制御装置４００の演算部（ＣＰＵ）が、第２〜第４実施形態で説明した演算処理を行う。

以上、本発明は、例えばケーブルやワイヤーハーネスの設計支援、ロボットによる柔軟物マニピュレーションにおける軌道計画支援、弾性受動関節の設計支援、閉ループ弾性体に基づく瞬発力発生機構の設計支援、カテーテルの設計支援等に適用可能である。

なお、本発明は、以上説明した実施形態に限定されるものではなく、本発明の技術的思想内で多くの変形が可能である。また、本発明の実施形態に記載された効果は、本発明から生じる最も好適な効果を列挙したに過ぎず、本発明による効果は、本発明の実施形態に記載されたものに限定されない。

［その他の実施形態］
本発明は、上述の実施形態の１以上の機能を実現するプログラムを、ネットワーク又は記憶媒体を介してシステム又は装置に供給し、そのシステム又は装置のコンピュータにおける１つ以上のプロセッサーがプログラムを読出し実行する処理でも実現可能である。また、１以上の機能を実現する回路（例えば、ＡＳＩＣ）によっても実現可能である。

１…ベース（一方の拘束箇所）、２…手先（他方の拘束箇所）、３…線条弾性体（柔軟物）、４_１〜４_ｎ…バネ関節、５_１〜５_ｎ…リンク、２００…計算装置、２０１…ＣＰＵ（演算部）

Claims (11)

1. 演算部が、少なくとも２つの拘束部により位置及び姿勢が拘束される柔軟物について前記少なくとも２つの拘束部の内、拘束される２つの拘束箇所の間の形状と、前記２つの拘束箇所のうち一方の拘束箇所の位置及び姿勢を固定したときに前記２つの拘束箇所のうち他方の拘束箇所に生じる拘束力及び拘束トルクと、を計算する柔軟物の状態計算方法であって、
   前記演算部が、前記柔軟物を仮想３次元空間内で複数のバネ関節及び複数のリンクでモデル化し、前記仮想３次元空間内における前記他方の拘束箇所の目標位置、前記他方の拘束箇所の目標姿勢、前記目標位置に対して許容する位置ずれ量を示す位置許容誤差、前記目標姿勢に対して許容する姿勢ずれ量を示す姿勢許容誤差、前記拘束力の初期値、及び前記拘束トルクの初期値を設定する設定工程と、
   前記演算部が、前記拘束力及び前記拘束トルクに基づいて、前記バネ関節の角度、前記リンクの姿勢、及び前記バネ関節の位置を、前記一方の拘束箇所から前記他方の拘束箇所に向かって順次計算することで、前記柔軟物の前記２つの拘束箇所の間の形状を求める形状計算工程と、
   前記演算部が、前記形状計算工程により求めた形状計算結果に基づく前記他方の拘束箇所の位置の前記目標位置に対する位置誤差が、前記位置許容誤差以下であるか否かを判断する位置判断工程と、
   前記演算部が、前記位置判断工程にて前記位置誤差が前記位置許容誤差を上回っていると判断した場合、前記拘束力を更新する拘束力更新工程と、
   前記演算部が、前記形状計算工程により求めた形状計算結果に基づく前記他方の拘束箇所の前記目標姿勢に対する姿勢誤差が、前記姿勢許容誤差以下であるか否かを判断する姿勢判断工程と、
   前記演算部が、前記姿勢判断工程にて前記姿勢誤差が前記姿勢許容誤差を上回っていると判断した場合、前記拘束トルクを更新する拘束トルク更新工程と、を備え、
   前記演算部は、前記位置判断工程にて前記位置誤差が前記位置許容誤差以下と判断するまで、前記拘束力更新工程、前記形状計算工程及び前記位置判断工程を繰り返し、且つ前記姿勢判断工程にて前記姿勢誤差が前記姿勢許容誤差以下と判断するまで、前記拘束トルク更新工程、前記形状計算工程及び前記姿勢判断工程を繰り返し、
   前記拘束トルク更新工程では、前記演算部が、前記姿勢誤差に比例する値を用いて、前記拘束トルクを更新し、
   前記拘束力更新工程では、前記演算部が、前記位置誤差を前記拘束トルクのベクトル方向の軸まわりに回転変換した値に比例する値を用いて、前記拘束力を更新することを特徴とする柔軟物の状態計算方法。
2. λを前記拘束力、τを前記拘束トルク、ΔＰを前記位置誤差、ΔＯを前記姿勢誤差、ｃ_Ｐ，ｃ_Ｔ，ｃ_Ｏを正の定数とし、Ｒ（ａ，θ）をａ軸まわりに角度θ回転させる回転行列としたとき、
   前記拘束力更新工程では、前記演算部が、
   

   

   の式に従って前記拘束力を更新し、
   前記拘束トルク更新工程では、前記演算部が、
   

   

   の式に従って前記拘束トルクを更新することを特徴とする請求項１に記載の柔軟物の状態計算方法。
3. 前記設定工程では、前記演算部が前記リンクを前記バネ関節で直列に接続したモデルで前記柔軟物をモデル化することを特徴とする請求項１又は２に記載の柔軟物の状態計算方法。
4. 前記演算部が、前記柔軟物の周辺の障害物を前記仮想３次元空間内で設定する障害物設定工程と、
   前記演算部が、前記形状計算工程による形状計算結果に基づき、前記柔軟物が前記障害物と干渉するか否かを判断する干渉判断工程と、を更に備えたことを特徴とする請求項１乃至３のいずれか１項に記載の柔軟物の状態計算方法。
5. 前記演算部が、前記他方の拘束箇所を前記仮想３次元空間内で仮想的に移動させる移動工程を更に備え、
   前記干渉判断工程は、前記演算部が、前記移動工程で前記他方の拘束箇所を前記仮想３次元空間内で移動させる度に行うことを特徴とする請求項４に記載の柔軟物の状態計算方法。
6. 前記演算部が、前記拘束力が、予め設定した負荷力を超えるか否かを判断する拘束力判断工程を更に備えたことを特徴とする請求項１乃至５のいずれか１項に記載の柔軟物の状態計算方法。
7. 前記演算部が、前記拘束トルクが、予め設定した負荷トルクを超えるか否かを判断する拘束トルク判断工程を更に備えたことを特徴とする請求項１乃至６のいずれか１項に記載の柔軟物の状態計算方法。
8. 柔軟物の状態を計算する演算部を備え、
   前記演算部は、柔軟物の２つの拘束箇所のうち一方の拘束箇所がロボットハンド以外の拘束部に拘束され、他方の拘束箇所が前記ロボットハンドに拘束されるものとして、請求項１乃至７のいずれか１項に記載の柔軟物の状態計算方法の各工程を実行して柔軟物の状態を求め、求めた柔軟物の状態に基づいてロボットの軌道を生成することを特徴とするロボット軌道生成装置。
9. ロボットハンドを有するロボットと、
   前記ロボットの動作を制御するロボット制御部と、を備え、
   前記ロボット制御部は、柔軟物の状態を計算する演算部を有し、
   前記演算部は、柔軟物の２つの拘束箇所のうち一方の拘束箇所が前記ロボットハンド以外の拘束部に拘束され、他方の拘束箇所が前記ロボットハンドに拘束されるものとして、請求項１乃至７のいずれか１項に記載の柔軟物の状態計算方法の各工程を実行して柔軟物の状態を求め、
   前記ロボット制御部は、前記ロボットハンドに柔軟物の他方の拘束箇所を拘束させた状態で、求めた柔軟物の状態に基づき生成された前記ロボットの軌道に従って前記ロボットの動作を制御することを特徴とするロボット装置。
10. コンピュータに、請求項１乃至７のいずれか１項に記載の柔軟物の状態計算方法の各工程を実行させるためのプログラム。
11. 請求項１０に記載のプログラムを記録したコンピュータ読み取り可能な記録媒体。

Images