JP6515828B2 - 干渉回避方法 - Google Patents

Description

本発明は干渉回避方法に関し、特に、ロボットアームの干渉を回避する干渉回避方法に関する。

複数の関節を有するロボットアームの動作を制御してロボットアームをリアルタイムに操作する方法として、例えばマスタスレーブ手法がある。マスタスレーブ手法では、マスタ側装置（制御装置）が制御値を指定して、その指定された制御値に応じて、スレーブ側装置（ロボットアーム）が動作する。指定される制御値としてロボットアームの手先位置が指定される場合、逆運動学計算を用いて各関節の関節角度が算出される。

ここで、ロボットの周囲には、ロボットの胴体等の障害物が存在する。したがって、手先位置を指定してロボットアームを制御する場合、各関節の関節角度を算出するだけでなく、ロボットアームが胴体等の障害物と干渉しないような解（干渉回避解）を算出する必要がある。そして、ロボットアームをリアルタイムに操作する場合、上述した関節角度の算出と干渉回避解の算出とを、リアルタイムに行う必要がある。

上記技術に関連して、特許文献１には、ロボットアームの手先部のハンドアイセンサの存在を考慮した周辺との干渉防止を図る、ハンドアイ式ビンピッキングロボットの制御装置が開示されている。特許文献１にかかる制御装置は、干渉パターンテーブルを記憶するテーブル記憶手段と、前記ハンドアイセンサと前記ロボットアームとが干渉するか否かを判定する移動先干渉判定手段と、姿勢再決定手段とを有する。

ここで、干渉パターンテーブルは、ロボットアームの各自由軸における回転角度の、前記ハンドアイセンサと前記ロボットアームとが干渉する組み合わせパターンを定義する。また、移動先干渉判定手段は、エンドエフェクタがワークの位置において決定した姿勢となるように前記ロボットアームを駆動させた場合の、前記各自由軸における回転角度の組み合わせパターンと、前記干渉パターンテーブルにおいて定義された組み合わせパターンとの照合に基づいて、ハンドアイセンサと前記ロボットアームとが干渉するか否かを判定する。姿勢再決定手段は、干渉すると判定された場合に、前記ワークのピッキングに適した前記エンドエフェクタの姿勢を新たに決定する。

特開２０１１−０９３０１５号公報

特許文献１にかかる技術では、事前に干渉パターンテーブルを準備しておく必要がある。このパターンテーブルを準備するために、膨大な時間を要するおそれがある。したがって、特許文献１にかかる技術を用いても、干渉回避解をリアルタイムに算出することは困難である。

本発明は、このような課題を解決するためになされたものであり、ロボットアームの干渉を回避するための計算を高速に行うことが可能な干渉回避方法を提供することにある。

本発明にかかる干渉回避方法は、肩部と、前記肩部に接続された上腕部と、肘部と、前腕部と、手首部と、手先部とを有するロボットアームであって、前記肘部を介して前記上腕部と前記前腕部とが接続され、前記手首部を介して前記前腕部と前記手先部とが接続され、前記手首部を構成する３つの関節部のうちの前記手先部の側の第１の関節部と前記第１の関節部と隣り合う第２の関節部との間にオフセット部が設けられるように構成されたロボットアームの障害物に対する干渉を回避するロボットアームの干渉回避方法であって、前記手先部の目標位置及び目標姿勢を指定し、前記ロボットアームの両端が固定された状態で、前記第２の関節部が前記第１の関節部と直交する位置にあるとみなし、前記前腕部と前記オフセット部とを一体とみなして構成された前腕リンク部及び前記上腕部の可動領域を算出し、前記可動領域において前記ロボットアームが前記障害物を含む障害物領域に干渉しない干渉回避範囲を決定し、前記決定された干渉回避範囲の中で前記肘部を第１の位置に設定し、前記前腕部と前記オフセット部とを分離して、前記設定された前記肘部の第１の位置と前記ロボットアームの両端とを含む平面上で、前記手先部の目標姿勢を満たすように、前記肘部の位置を示すパラメータを算出し、前記算出されたパラメータを用いて、前記ロボットアームを構成する関節の角度を算出する。

本発明によれば、ロボットアームの干渉を回避するための計算を高速に行うことが可能な干渉回避方法を提供できる。

実施の形態１にかかるロボットシステムを示す概略図である。 実施の形態１にかかるアームアングルについて説明するための図である。 実施の形態１にかかる制御装置の構成を示す機能ブロック図である。 実施の形態１にかかる制御装置の処理を示すフローチャートである。 実施の形態１にかかる可動領域の算出方法を説明するための図である。 実施の形態１にかかる可動領域算出部によって算出された円錐の各寸法を定義するための図である。 実施の形態１にかかる可動領域算出部によって算出された円錐を示す図である。 実施の形態１にかかる可動領域算出部によって算出された円錐を示す図である。 実施の形態１にかかるロボットの胴体とロボットアームとの関係について示す図である。 実施の形態１にかかるロボットの胴体とロボットアームとの関係について示す図である。 実施の形態１にかかるロボットの胴体とロボットアームとの関係について示す図である。 図７Ａに示した円錐の側面と干渉判定線分との交点が２つ存在する場合を例示する図である。 図９の例におけるアームアングルの存在範囲の判定方法について説明するための図である。 図７Ａに示した円錐の底面と干渉判定線分との交点が１つ存在する場合を例示する図である。 図１１の例におけるアームアングルφの存在範囲の判定方法について説明するための図である。 図７Ａに示した円錐の側面と干渉判定線分との交点が１つ存在する場合を例示する図である。 逆運動学計算を用いた関節角度の算出方法を説明するための図である。 手首部におけるオフセットを考慮した演算について説明するための図である。 基準座標系Σ_０におけるロボットアームを示した図である。 座標系Σ_ｗにおけるロボットアームを示した図である。 座標系Σ_７におけるロボットアームを示した図である。

（実施の形態１）
以下、図面を参照して本発明の実施の形態について説明する。
図１は、実施の形態１にかかるロボットシステム１を示す概略図である。ロボットシステム１は、ロボット１００と、ロボットの動作を制御する制御装置２とを有する。ロボット１００は、胴体１１０とロボットアーム１２０とを有する。胴体１１０は、胴体前面１１２と胴体側面１１４とを有する。

なお、実際のロボット１００のボディ（実胴体）は、任意の複数の曲面で構成された形状で形成されているが、本実施の形態では、模式的に、胴体１１０を、実胴体を内包する直方体Ｂｄと考える。また、本実施の形態においては、胴体１１０を、ロボットアーム１２０との干渉対象となる障害物としている。したがって、本実施の形態においては、胴体１１０は、障害物を含む障害物領域（上記「直方体Ｂｄ」に対応）であり、胴体前面１１２及び胴体側面１１４は、障害物を含む障害物領域の境界面である。

ロボットアーム１２０は、胴体側面１１４に接続されている。なお、図１は、ロボット１００の右腕に対応するロボットアーム１２０のみが示されているが、ロボット１００は、左腕に対応するロボットアーム１２０を有してもよい。さらに、ロボット１００は、１つ以上（例えば２つ）の脚を有してもよい。以下、ロボット１００の右腕に対応するロボットアーム１２０についての制御方法（干渉回避方法）について説明するが、これに限定されない。本実施の形態にかかる制御方法は、ロボット１００の左腕に対応するロボットアーム、及び、ロボット１００の脚に対しても適用可能である。つまり、「アーム（ロボットアーム）」とは、腕に相当するアーム、脚に相当するアーム、及びアーム型ロボットのアームを含む。

また、ロボットアーム１２０は、胴体１１０に近い方から順に、肩部１３２と、上腕部１２２と、肘部１３４と、前腕部１２５と、手首部１３６と、手先部１２６とから構成されている。胴体１１０（胴体側面１１４）には肩部１３２が接続されている。また、肘部１３４を介して、上腕部１２２と前腕部１２５とが接続されている。また、手首部１３６を介して前腕部１２５と手先部１２６とが接続されている。また、上腕部１２２の長さをＬ_１とし、前腕部１２５の長さをＬ_２とする。なお、後述する制御装置２における計算では、上腕部１２２及び前腕部１２５は、それぞれ、長さＬ_１及びＬ_２の線分であると仮定されている。

また、肩部１３２は、互いに直交した３軸（ピッチ、ロール、ヨー）の周りをそれぞれ回転する３個の関節（第一関節部１４１，第二関節部１４２，第三関節部１４３）を有している。また、肘部１３４は、１軸の周りを回転する１個の関節（第四関節部１４４）を有している。また、手首部１３６は、３軸（ヨー、ピッチ、ロール）の周りをそれぞれ回転する３個の関節（第五関節部１４５，第六関節部１４６，第七関節部１４７）を有している。

なお、各関節部のヨー、ピッチ、ロールの関係については、これらに限定されるものではない。ここで、隣り合う第六関節部１４６と第七関節部１４７との間には、オフセット部１２７が設けられている。つまり、手首部１３６においては、関節の一部がオフセットしている。また、第五関節部１４５及び第六関節部１４６によって手首関節部１３５が構成されている。さらに、前腕部１２５及びオフセット部１２７によって前腕リンク部１２４が構成されている。なお、後述する制御装置２における計算では、オフセット部１２７は、長さＬ_３の線分であると仮定されている。さらに、後述する制御装置２における計算において、前腕部１２５及びオフセット部１２７が一体とみなされる場合に、前腕リンク部１２４は長さＬ_２’の線分であると仮定されている。この場合、Ｌ_２’＝Ｌ_２＋Ｌ_３である。

また、第一関節部１４１の関節角度をθ_１とし、第二関節部１４２の関節角度をθ_２とし、第三関節部１４３の関節角度をθ_３とする。同様に、第四関節部１４４の関節角度をθ_４とし、第五関節部１４５の関節角度をθ_５とし、第六関節部１４６（第２の関節部）の関節角度をθ_６とし、第七関節部１４７（第１の関節部）の関節角度をθ_７とする。

なお、本実施の形態においては、後述する制御装置２における計算では、肩部１３２を１点で模擬している。つまり、第一関節部１４１，第二関節部１４２，第三関節部１４３が、１つの点で交差していると仮定している。同様に、後述する制御装置２における計算では、手首関節部１３５を１点で模擬している。つまり、第五関節部１４５及び第六関節部１４６が、１つの点で交差していると仮定している。この場合、肩部１３２の３個の関節（第一関節部１４１，第二関節部１４２，第三関節部１４３）及び手首関節部１３５の２個の関節（第五関節部１４５，第六関節部１４６）は、それぞれ、仮想的な球面構造を構成しているとみなし得る。一方、第七関節部１４７は、オフセット部１２７によって、手首関節部１３５からオフセットしている。さらに、本実施の形態におけるロボットアーム１２０は、７個の関節を有していることから、７自由度を有するマニピュレータである。

ここで、肩部１３２を原点Ｏとする基準座標系Σ_０を定義する。原点Ｏを通り、前側（胴体前面１１２側）方向（図１の紙面奥から手前方向）を正とする座標軸をｘ軸とする。また、原点Ｏを通り、胴体の右側から左側へと向かう方向（図１の左から右の方向）を正とする座標軸をｙ軸とする。また、原点Ｏを通り、上側へ向かう方向（図１の上方向）を正とする座標軸をｚ軸とする。

制御装置２は、例えばコンピュータとしての機能を有する。制御装置２は、ロボット１００の内部に搭載されてもよいし、ロボット１００と有線又は無線を介して通信可能に接続されてもよい。制御装置２は、ロボット１００の動作、特に、ロボットアーム１２０の動作を制御する。つまり、ロボットシステム１において、制御装置２はマスタ装置としての機能を有し、ロボット１００（ロボットアーム１２０）はスレーブ装置としての機能を有する。ここで、制御装置２は、ロボットアーム１２０の胴体１１０等の障害物に対する干渉を回避するように、ロボットアーム１２０の各関節角度を決定する。詳しくは後述する。

ここで、本実施の形態において使用される「アームアングル」という指標について説明する。「アームアングル」については、文献「関節の可動範囲を考慮に入れた７自由度冗長マニピュレータの解析的逆運動学解法」（日本ロボット学会誌Vol. 25 No. 4, pp.606〜617, 2007）に記載されている。以下、簡単に説明する。

一般に、空間内におけるマニピュレータの手先の位置及び姿勢は、６つのパラメータで一意に表現され得る。したがって、７自由度マニピュレータでは、余分な自由度が１つ存在する。この冗長自由度により、７自由度マニピュレータでは、手先の位置及び姿勢に影響を及ぼさない機構内部における１自由度のセルフモーションが可能である。ここで、上述したように、本実施の形態にかかるロボットアーム１２０は、７自由度を有する。したがって、本実施の形態にかかるロボットアーム１２０は、１つの冗長自由度を有している。本実施の形態では、この冗長自由度を表すパラメータとして、「アームアングル」という指標が用いられる。

図２は、実施の形態１にかかるアームアングルについて説明するための図である。上述したように、肩部１３２の３個の関節及び手首部１３６の３個の関節は、それぞれ、仮想的な球面構造を構成している。これにより、手先部１２６の位置及び姿勢を指定して、ロボットアーム１２０の両端、つまり肩部１３２の位置（点Ｏ）と手先部１２６の位置（点Ｐ）とを固定しても、冗長自由度により、肘部１３４の位置（点ｅ）は変化し得る。なお、以下、手先部１２６の位置は、仮想的に点で示される。ここで、第七関節部１４７及び手先部１２６の位置は、互いに同じであると仮定する。また、アームアングルを考慮する場合、前腕部１２５及びオフセット部１２７は一体とみなされる。そして、手首部１３６におけるオフセットが無視され、手首関節部１３５と第七関節部１４７とが一体であるとみなされる。

図２に示すように、手先部１２６の位置（点Ｐ）を指定すると、肩部１３２（点Ｏ）と、肘部１３４（点ｅ）と、手先部１２６（点Ｐ）とを頂点とし、上腕部１２２と、前腕リンク部１２４と、線分ＯＰとを３辺とする三角形Ｏ−ｅ−Ｐが定まる。このとき、三角形Ｏ−ｅ−Ｐは、線分ＯＰを中心として、矢印Ａに示すように回転可能である。このように、線分ＯＰを中心として肘部１３４の位置（点ｅ）を回転させた場合の肘部１３４の位置（点ｅ）の回転角度を、アームアングルφとする。また、予め定められた参照平面上に肘部１３４の位置がある場合にその位置を点ｅ_０とすると、三角形Ｏ−ｅ_０−Ｐにおけるアームアングルをφ＝０とする。そして、点Ｐから点Ｏを見て点ｅを時計回りに回転させる方向（矢印Ａの方向）を、アームアングルの正方向とする。言い換えると、アームアングルの正方向は、点Ｐから点Ｏの方向に親指を立てた右手の法則で定義される。アームアングルφを正方向に回転させると、肘部１３４の位置ｅは、φ＝０である点ｅ_０から、ｅ_１、ｅ_２、ｅ_３と移動する。

また、このように、線分ＯＰを中心として肘部１３４の位置（点ｅ）を回転させた場合に上腕部１２２及び前腕リンク部１２４が描く軌跡が、それぞれ、手先部１２６の位置（点Ｐ）が指定された場合の上腕部１２２及び前腕リンク部１２４の可動領域である。上腕部１２２及び前腕リンク部１２４の可動領域は、肘部１３４を回転した軌跡で形成された円を底面とし、上腕部１２２及び前腕リンク部１２４が回転した軌跡で形成された回転面をそれぞれ側面とする円錐で形成される。

図３は、実施の形態１にかかる制御装置２の構成を示す機能ブロック図である。制御装置２は、主要なハードウェア構成として、ＣＰＵ（Central Processing Unit）４と、ＲＯＭ（Read Only Memory）６と、ＲＡＭ（Random Access Memory）８とを有する。ＣＰＵ４は、制御処理及び演算処理等を行う演算装置としての機能を有する。ＲＯＭ６は、ＣＰＵ４によって実行される制御プログラム及び演算プログラム等を記憶するための機能を有する。ＲＡＭ８は、処理データ等を一時的に記憶するための機能を有する。

また、制御装置２は、情報格納部１２、手先位置決定部１４、可動領域算出部１６、交点算出部１８、干渉回避範囲決定部２０、関節角度算出部２２、オフセット演算部２４及びロボットアーム制御部２６（以下、「各構成要素」と称する）を有する。情報格納部１２、手先位置決定部１４、可動領域算出部１６、交点算出部１８、干渉回避範囲決定部２０、関節角度算出部２２、オフセット演算部２４及びロボットアーム制御部２６は、それぞれ、情報格納手段、手先位置決定手段、可動領域算出手段、交点算出手段、干渉回避範囲決定手段、関節角度算出手段、オフセット演算手段及びロボットアーム制御手段としての機能を有する。各構成要素は、例えば、ＣＰＵ４がＲＯＭ６に記憶されたプログラムを実行することによって実現可能である。また、必要なプログラムを任意の不揮発性記録媒体に記録しておき、必要に応じてインストールするようにしてもよい。なお、各構成要素は、上記のようにソフトウェアによって実現されることに限定されず、何らかの回路素子等のハードウェアによって実現されてもよい。各構成要素の機能については、図４を用いて説明する。

図４は、実施の形態１にかかる制御装置２の処理を示すフローチャートである。まず、ステップＳ１０２において、制御装置２は、ロボットアーム１２０の制御に必要な情報を情報格納部１２に格納させることによって、事前設定を行う。情報格納部１２は、上腕部１２２の長さＬ_１、前腕部１２５の長さＬ_２、オフセット部１２７の長さＬ_３及び前腕リンク部１２４の長さＬ_２’を格納する。また、情報格納部１２は、胴体１１０（障害物領域）に関する位置情報を格納する。具体的には、情報格納部１２は、肩部１３２を原点Ｏとするｘｙｚ座標系における、胴体１１０の位置情報（座標データ）を記憶している。さらに具体的には、情報格納部１２は、胴体前面１１２を示す位置情報（座標データ）及び胴体側面１１４を示す位置情報（座標データ）を格納する。さらに、情報格納部１２は、図１に示す線分ＡＢを示す位置情報（座標データ）を格納する。この線分ＡＢは、後述するように、干渉の回避を判定するための干渉判定線分Ｌｄ（第１の線）として使用される。

なお、情報格納部１２に記憶される胴体１１０の座標データは、実際のロボットアーム１２０の太さを考慮した、干渉を検出するために使用される胴体１１０に対応する障害物の三次元形状を示している。また、胴体１１０の座標データは、実際の胴体１１０を内包する障害物領域の境界面の座標を示している。つまり、胴体１１０に対応する障害物領域の座標データは、ロボットアーム１２０の太さに対応するマージンが含まれている。

次に、ステップＳ１０４において、制御装置２は、手先部１２６の位置（目標手先位置）及び姿勢（目標手先姿勢）を決定する。具体的には、制御装置２の手先位置決定部１４は、操作者がキーボード、マウス又はハプティックデバイス等の入力手段を用いて入力した目標手先位置及び目標手先姿勢のデータを受け付ける。これにより、手先部１２６の位置及び姿勢が指定される。

次に、ステップＳ１０６において、制御装置２は、手先部１２６の干渉の判定を行う。ここで、Ｓ１０６における干渉の判定は、手先部１２６が胴体１１０と干渉するか否かの判定である。具体的には、手先位置決定部１４は、指定された目標手先位置から、手先部１２６が胴体１１０と干渉するか否かを判定する。例えば、手先位置決定部１４は、手先部１２６及び胴体１１０をポリゴンで表し、ＯＢＢ（Oriented Bounding Box：有向境界ボックス）を用いることによって、手先部１２６が胴体１１０と干渉するか否かを判定してもよい。

手先部１２６の干渉が発生する場合（Ｓ１０６のＹＥＳ）、ステップＳ１４０において、制御装置２は、目標手先位置及び目標手先姿勢の修正処理を行う。具体的には、制御装置２は、Ｓ１０２の処理で指定された目標手先位置及び目標手先姿勢をキャンセルする。そして、制御装置２は、操作者が指定した目標手先位置及び目標手先姿勢では干渉が発生するので再度目標手先位置及び目標手先姿勢を指定するように、操作者に対して通知する。

一方、手先部１２６の干渉が発生しない場合（Ｓ１０６のＮＯ）、ステップＳ１０８において、制御装置２は、ロボットアーム１２０の可動領域を算出する。具体的には、制御装置２の可動領域算出部１６は、上述したアームアングルφを用いて、上腕部１２２及び前腕リンク部１２４の可動領域を算出する。なお、このときに算出される「可動領域」は、ロボットアーム１２０の周囲の障害物との干渉については考慮されていない。以下、可動領域算出部１６における計算方法を説明する。なお、Ｓ１０８〜Ｓ１３２においては、前腕部１２５及びオフセット部１２７は一体とみなされる。そして、手首部１３６におけるオフセットが無視され、手首関節部１３５と第七関節部１４７とが一体であるとみなされる。そして、手首関節部１３５と第七関節部１４７とが一体である手首部１３６の位置及び姿勢は、手先部１２６の位置及び姿勢とそれぞれ同一であるとみなされる。

図５は、実施の形態１にかかる可動領域の算出方法を説明するための図である。Ｓ１０４において、手先部１２６の位置ＰがＰ_０［Ｘ，Ｙ，Ｚ］^Ｔに指定されたとする。可動領域算出部１６は、図５の（Ａ）に示すように、肩部１３２の位置（原点Ｏ）と手先部１２６の位置Ｐとの間の距離（√（Ｘ^２＋Ｙ^２＋Ｚ^２））を保ったまま、手先部１２６の位置Ｐをｚ軸の鉛直下方向の位置（点Ｐ’）に置く。このとき、点Ｐ’の座標は、［０，０，−√（Ｘ^２＋Ｙ^２＋Ｚ^２）］である。さらに、可動領域算出部１６は、肘部１３４の位置ｅをｘｚ平面上のｘ＜０の領域の位置（ｅ’）に置く。これにより、肩部１３２（点Ｏ）と、肘部１３４（点ｅ）と、手先部１２６（点Ｐ）とを頂点とする三角形Ｏ−ｅ−Ｐ（Ｏ−ｅ’−Ｐ’）は、ｘｚ平面上に存在することとなる。

そして、可動領域算出部１６は、図５の（Ｂ）に示すように、手先部１２６の位置ＰがＳ１０４の処理で指定された手先位置Ｐ_０に一致するように、三角形Ｏ−ｅ−Ｐを、ｘ軸周りに回転角αで回転し、ｙ軸周りに回転角βで回転する。具体的には、可動領域算出部１６は、以下の式１及び式２を用いて、点Ｐ’及び点ｅ’を、それぞれ点Ｐ_０及び点ｅ_０に変換する。

（式１）

（式２）

ここで、式１及び式２において、Ｐ’、ｅ’、Ｐ_０及びｅ_０は、それぞれ点Ｐ’、点ｅ’、点Ｐ_０及び点ｅ_０に関する位置ベクトルである。また、Ｒ_αは、回転角αを示す回転行列を示す。また、Ｒ_βは、回転角βを示す回転行列を示す。また、ロボットアーム１２０のリンク長（Ｌ_１及びＬ_２’）は予め定められているので、ｅ_０及びｅ’を算出でき、上記式１及び式２から、α及びβは一意に算出可能である。

可動領域算出部１６は、図５の（Ｂ）に示した回転後の三角形Ｏ−ｅ_０−Ｐ_０（斜線で示す）を含む平面を、アームアングルの基準となる参照平面とする。つまり、可動領域算出部１６は、肘部１３４の位置ｅが点ｅ_０にある場合に、アームアングルをφ＝０とする。

そして、図５の（Ｃ）に示すように、可動領域算出部１６は、肩部１３２を原点Ｏに固定し、手先部１２６を点Ｐ_０に固定した状態で、ＯＰ周りに上腕部１２２及び前腕リンク部１２４を矢印Ａの方向に回転させた軌跡を算出する。この軌跡は、手先部１２６の位置（点Ｐ）が点Ｐ_０に指定された場合の上腕部１２２及び前腕リンク部１２４の可動領域である。そして、可動領域は、肘部１３４を回転した軌跡で形成された円を底面とし、上腕部１２２及び前腕リンク部１２４が回転した軌跡で形成された回転面をそれぞれ側面とする円錐で形成される。言い換えると、ロボットアーム１２０の可動領域は、底面が互いに合わさった２つの円錐で形成される。

図６は、実施の形態１にかかる可動領域算出部１６によって算出された円錐の各寸法を定義するための図である。可動領域算出部１６は、前腕リンク部１２４の軌跡で形成された円錐ＣＬ（第１の円錐）及び上腕部１２２の軌跡で形成された円錐ＣＵ（第２の円錐）それぞれについての寸法を算出する。ここで、線分ＯＰの長さをＬ_０とする。また、円錐ＣＵの底面は、円錐ＣＬの底面と共通しているので、それぞれの底面である円の半径ｒは同じである。ここで、円錐ＣＵの高さをｋ_１とし、円錐ＣＬの高さをｋ_２とする。このとき、ｋ_１＋ｋ_２＝Ｌ_０である。また、このとき、肘部１３４の位置ｅから線分ＯＰに下ろした垂線の足をＨとすると、ｋ_１＝ＯＨであり、ｋ_２＝ＰＨである。なお、円錐ＣＵは、三角形Ｏ−ｅ−Ｈを線分ＯＨの周りに回転させた軌跡である。同様に、円錐ＣＬは、三角形Ｐ−ｅ−Ｈを線分ＰＨの周りに回転させた軌跡である。

ここで、上述したように、上腕部１２２の長さＬ_１及び前腕リンク部１２４の長さＬ_２’は、予め定められている。また、手先部１２６の位置が点Ｐ_０に指定されることによって、線分ＯＰの長さも定められている。したがって、三角形Ｏ−ｅ−Ｐは、アームアングルに寄らないで一意に定められる。したがって、可動領域算出部１６は、余弦定理を用いて、ｒ，ｋ_１，ｋ_２を算出することができる。

さらに、可動領域算出部１６は、前腕リンク部１２４の軌跡である円錐ＣＬについて、頂点（点Ｐ）がｚ’軸上に置かれ、底面がｘ’ｙ’平面上に置かれるように、座標系（ｘ，ｙ，ｚ）から座標系（ｘ’，ｙ’，ｚ’）への座標変換を行う。具体的には、可動領域算出部１６は、以下の式３を用いて、円錐ＣＬについての座標変換を行う。

（式３）

式３において、上記式１及び式２で用いられた回転行列Ｒ_α及び回転行列Ｒ_βを用いることによって、線分ＰＨがｚ’軸上となるように、座標系が変換される。これは、図５の（Ａ）に示した座標系を（ｘ，ｙ，ｚ）から（ｘ’，ｙ’，ｚ’）に置き換え、前腕リンク部１２４をＯＰ周りに回転させた軌跡に相当する。また、頂点（点Ｐ）をｚ’軸の正の側に置くために、座標系は、ｚ軸（ｚ’軸）の符号が正から負に変換される。さらに、底面をｘ’ｙ’平面（つまりｚ’＝０の平面）上に移動させるために、座標系は、ｚ’軸方向に−ｋ_１オフセットされている。これにより、図６に示した点Ｈが、座標系（ｘ’，ｙ’，ｚ’）の原点Ｏ’に一致する。

さらに、可動領域算出部１６は、変換後の円錐底面の式を以下の式４と算出し、変換後の円錐面（側面）の式を以下の式５と算出する。

（式４）

（式５）

同様に、可動領域算出部１６は、算出された円錐ＣＵについて、頂点（点Ｏ）がｚ’軸上に置かれ、底面がｘ’ｙ’平面上に置かれるように、座標系（ｘ，ｙ，ｚ）から座標系（ｘ’，ｙ’，ｚ’）への座標変換を行う。具体的には、可動領域算出部１６は、以下の式６を用いて、円錐ＣＵについての座標変換を行う。

（式６）

式６において、式３と同様に、上記式１及び式２で用いられた回転行列Ｒ_α及び回転行列Ｒ_βを用いることによって、線分ＯＰがｚ’軸上となるように、座標系が変換される。また、底面をｘ’ｙ’平面（つまりｚ’＝０の平面）上に移動させるために、座標系は、ｚ’軸方向にｋ_１オフセットされている。これにより、図６に示した点Ｈが、座標系（ｘ’，ｙ’，ｚ’）の原点Ｏ’に一致する。

さらに、可動領域算出部１６は、変換後の円錐底面の式を上記式４と算出し、変換後の円錐面（側面）の式を以下の式７と算出する。

（式７）

図７Ａ及び図７Ｂは、実施の形態１にかかる可動領域算出部１６によって算出された円錐を示す図である。以上のようにして、可動領域算出部１６は、図７Ａに示すような円錐ＣＬを算出し、図７Ｂに示すような円錐ＣＵを算出する。ここで、肘部１３４の位置ｅが参照平面にある場合（図５の（Ｂ））の位置ｅ_０を座標変換すると、変換後のｅ_０’の座標は［−ｒ，０，０］となる。この位置において、アームアングルがφ＝０となる。また、アームアングルの正方向は、図７Ｂに示すように、円錐ＣＵについては、図２の矢印Ａより、ｚ’軸の正方向から見て反時計回りとなる。一方、円錐ＣＬについては、座標変換の際にｚ軸（ｚ’軸）の符号が正から負に変換されたことに伴い、アームアングルの正方向は、図７Ａに示すように、円錐ＣＵとは逆の、ｚ’軸の正方向から見て時計回りとなる。

次に、ステップＳ１１０において、制御装置２は、可動領域と、干渉判定線分Ｌｄとの交点を算出する。さらに、ステップＳ１１２〜Ｓ１２０において、制御装置２は、算出された交点を用いて、ロボットアーム１２０が可動領域において胴体１１０に干渉しない範囲（干渉回避範囲）を決定する。以下、本実施の形態における、アームアングルを用いた干渉回避範囲を決定するメカニズムを説明する。

上述したように、手先部１２６の位置を指定した場合に、ロボットアーム１２０の可動領域は、アームアングルφを回転させた軌跡である２つの円錐ＣＵ及びＣＬで表される。ここで、胴体１１０とロボットアーム１２０との干渉を回避するためには、胴体１１０の外部にロボットアーム１２０が存在するようにすればよい。言い換えると、円錐ＣＵの側面上の、肩部１３２と肘部１３４とを結ぶ線分（円錐ＣＵの母線）、及び、円錐ＣＬの側面上の、肘部１３４と手先部１２６とを結ぶ線分（円錐ＣＬの母線）が、胴体１１０を模擬した直方体Ｂｄの内部に侵入しないようにすればよい。つまり、円錐ＣＵの母線及び円錐ＣＬの母線が直方体Ｂｄの内部に侵入しないようなアームアングルの範囲を決定すればよい。

図８Ａ，図８Ｂ及び図８Ｃは、実施の形態１にかかるロボット１００の胴体１１０とロボットアーム１２０との関係について示す図である。図８Ａ，図８Ｂ及び図８Ｃにおいては、手先部１２６の位置に応じた、可動領域（円錐ＣＵ及びＣＬ）と、胴体１１０（直方体Ｂｄ）との位置関係について示されている。手先部１２６の位置が図８Ａに示す位置に指定された場合、可動領域（円錐ＣＵ及びＣＬ）の一部が、直方体Ｂｄの内部に侵入している。したがって、この場合には、可動領域（円錐ＣＵ及びＣＬ）のうち、直方体Ｂｄの内部に侵入しないアームアングルの範囲（干渉回避範囲）を決定すればよい。

ここで、図８Ａに示すようなケースでは、可動領域（円錐ＣＵ及びＣＬ）は、胴体側面１１４及び胴体前面１１２の両方と交わっている。これにより、可動領域（円錐ＣＵ及びＣＬ）は、線分ＡＢ（干渉判定線分Ｌｄ）と交わっている。したがって、本実施の形態においては、この可動領域（円錐ＣＵ及びＣＬ）と線分ＡＢ（干渉判定線分Ｌｄ）との交点を用いて、干渉回避範囲が決定される。

また、手先部１２６の位置が図８Ｂに示す位置に指定された場合、肩部１３２近傍及び手先部１２６の近傍を除き、可動領域（円錐ＣＵ及びＣＬ）の全てが、直方体Ｂｄの内部に侵入している。この場合は、胴体１１０とロボットアーム１２０との干渉を回避できない。したがって、干渉回避範囲は存在しないと決定される。なお、図８Ｂに示すようなケースでは、可動領域（円錐ＣＵ及びＣＬ）は、線分ＡＢ（干渉判定線分Ｌｄ）と交わらない。

また、手先部１２６の位置が図８Ｃに示す位置に指定された場合においても、可動領域（円錐ＣＵ及びＣＬ）は、線分ＡＢ（干渉判定線分Ｌｄ）と交わらない。一方、図８Ｃのケースでは、可動領域（円錐ＣＵ及びＣＬ）の全てが直方体Ｂｄの外側に存在しているか、又は、可動領域（円錐ＣＵ及びＣＬ）が胴体側面１１４のみと交わっている。したがって、この場合では、可動領域（円錐ＣＵ及びＣＬ）と胴体側面１１４との関係から干渉回避範囲が決定される。

ステップＳ１１０について説明する。制御装置２の交点算出部１８は、図７Ａ及び図７Ｂに示した円錐と、干渉判定線分Ｌｄ（図１の線分ＡＢ）との交点を算出する。交点算出部１８は、円錐ＣＬと干渉判定線分Ｌｄとの交点を算出し、円錐ＣＵと干渉判定線分Ｌｄとの交点を算出する。言い換えると、交点算出部１８は、円錐ＣＬと円錐ＣＵとで別個に交点を算出する。以下、円錐ＣＬと円錐ＣＵとで、分けて説明する。

具体的には、交点算出部１８は、円錐ＣＬと干渉判定線分Ｌｄとの交点を算出する際に、上記式３を用いて、点Ａの座標及び点Ｂの座標を座標系（ｘ，ｙ，ｚ）から座標系（ｘ’，ｙ’，ｚ’）に変換する。これによって、交点算出部１８は、点Ａを変換した点Ａ’の座標及び点Ｂを変換した点Ｂ’の座標を得る。さらに、交点算出部１８は、以下の式８のように、干渉判定線分Ｌｄ（線分Ａ’Ｂ’）上の点Ｐ_Ｌｄを表す式を算出する。

（式８）

ここで、式８において、ｔは媒介変数である。また、式８において、Ｐ_Ｌｄは点Ｐ_Ｌｄの位置ベクトル［ｘ’，ｙ’，ｚ’］^Ｔである。同様に、式８において、Ａ’及びＢ’は、それぞれ、Ａ’及びＢ’の位置ベクトルである。交点算出部１８は、式８を式４及び式５に代入し、得られた２次方程式におけるｔの解を算出する。式４を満たすｔの実数が存在する場合、交点算出部１８は、円錐ＣＬの底面と干渉判定線分Ｌｄとの交点が存在すると判定する。また、式５を満たすｔの実数が存在する場合、交点算出部１８は、円錐ＣＬの側面（円錐面）と干渉判定線分Ｌｄとの交点が存在すると判定する。そして、交点算出部１８は、得られたｔの値から、交点を算出する。

同様に、交点算出部１８は、円錐ＣＵと干渉判定線分Ｌｄとの交点を算出する際に、上記式６を用いて、点Ａの座標及び点Ｂの座標を座標系（ｘ，ｙ，ｚ）から座標系（ｘ’，ｙ’，ｚ’）に変換する。これによって、交点算出部１８は、点Ａを変換した点Ａ’の座標及び点Ｂを変換した点Ｂ’の座標を得る。さらに、交点算出部１８は、円錐ＣＬの場合と同様にして、上記式８のように、干渉判定線分Ｌｄ（線分Ａ’Ｂ’）上の点Ｐ_Ｌｄを表す式を算出する。そして、交点算出部１８は、円錐ＣＬの場合と同様に、式８を式４及び式７に代入し、ｔの解を算出する。式４を満たすｔの実数が存在する場合、交点算出部１８は、円錐ＣＵの底面と干渉判定線分Ｌｄとの交点が存在すると判定する。また、式７を満たすｔの実数が存在する場合、交点算出部１８は、円錐ＣＵの側面（円錐面）と干渉判定線分Ｌｄとの交点が存在すると判定する。そして、交点算出部１８は、得られたｔの値から、交点を算出する。

ステップＳ１１２において、制御装置２の交点算出部１８は、可動領域（円錐ＣＵ及びＣＬ）と線分Ａ’Ｂ’（干渉判定線分Ｌｄ）との交点が存在するか否かを判定する。交点が存在しないと判定された場合（Ｓ１１２のＮＯ）、処理はステップＳ１４２に進む。一方、交点が存在すると判定された場合（Ｓ１１２のＹＥＳ）、処理はステップＳ１２０に進む。

ステップＳ１４２において、制御装置２は、干渉回避範囲があるか否かを判定する。具体的には、制御装置２の干渉回避範囲決定部２０は、可動領域（円錐ＣＵ及びＣＬ）と胴体１１０との位置関係が図８Ｃに示す関係にあるか、又は図８Ｂに示す関係にあるかを判定する。例えば、干渉回避範囲決定部２０は、肩部１３２と手先部１２６とを結ぶ線分（ＯＰ）が胴体前面１１２と交わる場合に、図８Ｂに示す位置関係であると判定してもよい。一方、干渉回避範囲決定部２０は、線分ＯＰが胴体前面１１２と交わらない場合に、図８Ｃに示す位置関係であると判定してもよい。

干渉回避範囲がないと判定された場合（Ｓ１４２のＮＯ）、つまり図８Ｂの状態であると判定された場合、処理はＳ１４０に進む。一方、干渉回避範囲があると判定された場合（Ｓ１４２のＹＥＳ）、つまり図８Ｃの状態であると判定された場合、ステップＳ１４４において、干渉回避範囲決定部２０は、可動領域（円錐ＣＵ及びＣＬ）と胴体側面１１４との関係から、干渉を回避可能なアームアングルの範囲（干渉回避範囲φｓ）を決定する。この場合、干渉回避範囲決定部２０は、可動領域（円錐ＣＵ及びＣＬ）と胴体側面１１４との交点から、胴体側面１１４の外側の領域となるアームアングルの範囲を、干渉回避範囲として決定する。なお、可動領域（円錐ＣＵ及びＣＬ）と胴体側面１１４との交点がない場合は、干渉回避範囲決定部２０は、アームアングルの全周を、干渉回避範囲φｓとして決定する。

ステップＳ１２０において、干渉回避範囲決定部２０は、可動領域と干渉判定線分Ｌｄとの交点から、干渉回避範囲φｓを決定する。具体的には、干渉回避範囲決定部２０は、前腕リンク部１２４におけるアームアングルの存在範囲（ロボットアーム１２０が胴体１１０と干渉しないアームアングルφの範囲）を判定する。具体的には、干渉回避範囲決定部２０は、円錐ＣＬの底面と干渉判定線分Ｌｄ（線分Ａ’Ｂ’）との交点が存在するか否か判定する。さらに、干渉回避範囲決定部２０は、円錐ＣＬの側面（円錐面）と干渉判定線分Ｌｄとの交点がいくつあるかを判定する。

図９は、図７Ａに示した円錐ＣＬの側面と干渉判定線分Ｌｄとの交点が２つ存在する場合を例示する図である。式８を式５に代入して得られたｔの２次方程式が２つの実数解を持つ場合に、図９に示すように、円錐ＣＬの側面と干渉判定線分Ｌｄ（線分Ａ’Ｂ’）との交点が２つ存在する。なお、この場合、底面と干渉判定線分Ｌｄとの交点は存在しない。

干渉回避範囲決定部２０は、交点算出部１８によって算出された交点ａ’及びｂ’の座標を取得する。そして、干渉回避範囲決定部２０は、交点ａ’の座標を用いて、交点ａ’を円錐ＣＬの底面に投影（正射影）した写像である点ａ”を算出する。なお、円錐ＣＬは直円錐であり、底面はｘ’ｙ’平面上にあるため、交点ａ’の座標が［ｘ_ａ’，ｙ_ａ’，ｚ_ａ’］である場合、点ａ”の座標は［ｘ_ａ’，ｙ_ａ’，０］となる。同様に、干渉回避範囲決定部２０は、交点ｂ’の座標を用いて、交点ｂ’を円錐ＣＬの底面に投影（正射影）した写像である点ｂ”を算出する。

さらに、干渉回避範囲決定部２０は、円錐ＣＬの底面において、ｘ’軸の負方向（点ｅ_０’［−ｒ，０，０］を通る半径）を基準とした、点ａ”を通る半径Ｒａの角度φ１Ｌを算出する。また、同様に、干渉回避範囲決定部２０は、円錐ＣＬの底面において、ｘ’軸の負方向を基準とした、点ｂ”を通る半径Ｒｂの角度φ２Ｌを算出する。ここで、図７Ａを用いて上述したように、アームアングルφの基準点つまりφ＝０の点は、点ｅ_０’である。したがって、算出された角度φ１Ｌ及びφ２Ｌが、アームアングルφの限界角度（境界角度）となる。なお、上述したように、円錐ＣＬにおいては、アームアングルの正方向は、ｚ’軸正方向から見て時計回りである。したがって、図９のように点ａ’及び点ｂ’が位置している場合、−π≦φ≦πとすると、φ１Ｌ＞０であり、φ２Ｌ＜０である。

次に、干渉回避範囲決定部２０は、算出された角度φ１Ｌ及びφ２Ｌを用いて、前腕リンク部１２４におけるアームアングルの存在範囲φＬ（第１の範囲）を判定する。言い換えると、干渉回避範囲決定部２０は、ロボットアーム１２０の前腕リンク部１２４が胴体１１０と干渉しないアームアングルφの存在範囲を決定する。ここで、角度φ１Ｌ及びφ２Ｌは、アームアングルの存在範囲φＬの境界角度である。したがって、円錐ＣＬにおけるアームアングルの存在範囲φＬは、φ２Ｌから時計回りにφ１Ｌへと至る範囲（点ｅ_０’を含む側）か、又は、φ１Ｌから時計回りにφ２Ｌへと至る範囲（点ｅ_０’を含まない側）のどちらかである。以下、その判定方法について説明する。

図１０は、図９の例におけるアームアングルの存在範囲φＬの判定方法について説明するための図である。図１０においては、胴体前面１１２及び胴体側面１１４が式３を用いてそれぞれ座標変換された平面Ｆ’及び平面Ｓ’が、円錐ＣＬに重ねられている。ここで、アームアングルの存在範囲φＬとは、胴体１１０と干渉しない範囲であるから、円錐ＣＬが胴体にめり込まない側を、アームアングルの存在範囲φＬとすればよい。

例えば、干渉回避範囲決定部２０は、線分ａ’ｂ’上の任意の点（例えば中点ｇ’）を始点とする、胴体前面１１２上の任意の長さのベクトルＶｆを算出する。そして、干渉回避範囲決定部２０は、ベクトルＶｆを円錐ＣＬの底面（つまりｘ’ｙ’平面）に投影（正射影）する。そして、干渉回避範囲決定部２０は、ベクトルＶｆの写像と交わらない側の範囲を、アームアングルの存在範囲φＬとして決定する。図９及び図１０の例では、アームアングルの存在範囲φＬは、φ２Ｌから時計回りにφ１Ｌへと至る範囲（φ２Ｌ≦φ≦φ１Ｌ）となる。また、例えば、干渉回避範囲決定部２０は、φ＝０を含む側（点ｅ_０’を含む側）を、アームアングルの存在範囲φＬとしてもよい。

図１１は、図７Ａに示した円錐ＣＬの底面と干渉判定線分Ｌｄとの交点が１つ存在する場合を例示する図である。式４を満たす式８のｔが１つ存在する場合に、図１１に示すように、円錐ＣＬの底面と干渉判定線分Ｌｄ（線分Ａ’Ｂ’）との交点（ａ’）が１つ存在する。さらに、この場合、円錐ＣＬの側面と干渉判定線分Ｌｄとの交点（ｂ’）は、１つ存在する。つまり、式８を式５に代入して得られたｔの２次方程式が１つの実数解を持つ。

図９の場合と同様に、干渉回避範囲決定部２０は、交点算出部１８によって算出された交点ａ’及びｂ’の座標を取得する。図１１では、点ａ’が底面上にあり、点ｂ’が側面上にある。この場合、干渉回避範囲決定部２０は、交点ｂ’の座標を用いて、交点ｂ’を円錐ＣＬの底面に投影（正射影）した写像である点ｂ”を算出する。また、干渉回避範囲決定部２０は、円錐ＣＬの底面において、ｘ’軸の負方向を基準とした、点ｂ”を通る半径Ｒｂの角度φ２Ｌを算出する。この角度φ２Ｌが、アームアングルφの境界角度の１つとなる。以下、もう１つの境界角度φ１Ｌの算出方法について説明する。

図１２は、図１１の例におけるアームアングルの存在範囲φＬの判定方法について説明するための図である。上述したように、アームアングルφの境界角度の１つは、φ２Ｌである。ここで、胴体前面１１２のｘ’ｙ’ｚ’座標系への座標変換後の平面Ｆ’は、点ａ’を通る。このとき、干渉回避範囲決定部２０は、平面Ｆ’と円錐ＣＬの底面を形成する円周との交点Ｄ’を算出する。さらに、干渉回避範囲決定部２０は、円錐ＣＬの底面において、ｘ’軸の負方向を基準とした、交点Ｄ’を通る半径Ｒｄの角度を算出する。干渉回避範囲決定部２０は、この角度を、境界角度φ１Ｌと決定する。さらに、干渉回避範囲決定部２０は、ｘ’軸の負方向を基準とした、交点ａ’を通る半径Ｒａの角度φａを含む側の範囲、つまり、φ２Ｌから時計回りにφ１Ｌへと至る範囲（φ２Ｌ≦φ≦φ１Ｌ）を、アームアングルの存在範囲φＬと決定する。

図１３は、図７Ａに示した円錐ＣＬの側面と干渉判定線分Ｌｄとの交点が１つ存在する場合を例示する図である。式８を式５に代入して得られたｔの２次方程式が１つの重解を持つ場合に、図１３に示すように、円錐ＣＬの側面と干渉判定線分Ｌｄ（線分Ａ’Ｂ’）との交点が１つ存在する。言い換えると、干渉判定線分Ｌｄは、円錐ＣＬの側面と接する。この干渉判定線分Ｌｄと円錐ＣＬ側面との接点が、交点となる。なお、この場合、底面と干渉判定線分Ｌｄとの交点は存在しない。

この場合、干渉回避範囲決定部２０は、肩部１３２と手先部１２６とを結ぶ線分（ＯＰ）が干渉判定線分Ｌｄよりも胴体１１０の内側を通っているか否かを判定する。この判定は、座標変換後の座標系ｘ’ｙ’ｚ’で行われても、座標変換前の座標系ｘｙｚで行われてもよい。線分（ＯＰ）が干渉判定線分Ｌｄよりも胴体１１０の内側を通っている場合、全てのアームアングルφの範囲において胴体１１０との干渉を回避することはできない。したがって、干渉回避範囲決定部２０は、アームアングルの存在範囲φＬは存在しないと決定する。なお、この場合、ロボットアーム１２０が胴体１１０に接することが許容される条件であれば、干渉回避範囲決定部２０は、図９の例と同様にして交点（接点）からアームアングルφを算出し、そのアームアングルφのみをアームアングルの存在範囲φＬと決定してもよい。

一方、線分（ＯＰ）が干渉判定線分Ｌｄよりも胴体１１０の外側を通っている場合、全てのアームアングルφの範囲において胴体１１０と干渉しない。したがって、干渉回避範囲決定部２０は、アームアングルの全周を、アームアングルの存在範囲φＬと決定する。なお、この場合、ロボットアーム１２０が胴体１１０に接することが許容されない条件であれば、干渉回避範囲決定部２０は、図９の例と同様にして交点（接点）からアームアングルφを算出し、そのアームアングルφのみを除く範囲を、アームアングルの存在範囲φＬと決定してもよい。

次に、干渉回避範囲決定部２０は、上腕部１２２におけるアームアングルの存在範囲φＵ（第２の範囲）を判定する。具体的には、アームアングルの存在範囲φＬについての処理と同様に、干渉回避範囲決定部２０は、円錐ＣＵの底面と干渉判定線分Ｌｄ（線分Ａ’Ｂ’）との交点が存在するか否か判定する。さらに、アームアングルの存在範囲φＬについての処理と同様に、干渉回避範囲決定部２０は、円錐ＣＵの側面（円錐面）と干渉判定線分Ｌｄとの交点がいくつあるかを判定する。

アームアングルの存在範囲φＵについての具体的な処理については、アームアングルの存在範囲φＬについての処理と同様であるので、説明を省略する。但し、円錐ＣＵにおいては、アームアングルφの正方向の向きが、円錐ＣＬとは異なる。また、上腕部１２２は胴体側面１１４の側に存在するので、アームアングルの存在範囲φＬについての処理における「胴体前面１１２」は、「胴体側面１１４」に置き換えられる。

具体的には、アームアングルの存在範囲φＵについては、図１０を用いて説明した「ベクトルＶｆ」は、胴体側面１１４上の任意の長さの「ベクトルＶｓ」に置き換えられる。つまり、干渉回避範囲決定部２０は、線分ａ’ｂ’上の任意の点（例えば中点ｇ’）を始点とする、胴体側面１１４上の任意の長さのベクトルＶｓを算出する。そして、干渉回避範囲決定部２０は、そのベクトルＶｓを用いて、アームアングルの存在範囲φＵを決定する。

また、図１２では、干渉回避範囲決定部２０は、胴体前面１１２が座標変換された平面Ｆ’と円錐ＣＬの底面を形成する円周との交点Ｄ’を算出した。一方、アームアングルの存在範囲φＵについては、干渉回避範囲決定部２０は、胴体側面１１４が座標変換された平面Ｓ’と円錐ＣＵの底面を形成する円周との交点Ｄ’を算出して、同様の処理を行う。

なお、アームアングルの存在範囲φＬについて、前腕リンク部１２４に関する円錐ＣＬにおいて図９又は図１３のような状態である場合、つまり干渉判定線分Ｌｄが円錐ＣＬの側面とのみ交点を有し、円錐ＣＬの底面と干渉判定線分Ｌｄとの交点が存在しない場合、上腕部１２２に関する円錐ＣＵでは、干渉判定線分Ｌｄとの交点は存在しない。この場合、アームアングルの存在範囲φＵについての処理では、干渉回避範囲決定部２０は、アームアングルφの全周を、円錐ＣＵにおけるアームアングルの存在範囲φＵと決定する。同様に、アームアングルの存在範囲φＵについて干渉判定線分Ｌｄが円錐ＣＵの側面とのみ交点を有し、円錐ＣＵの底面と干渉判定線分Ｌｄとの交点が存在しない場合も、アームアングルの存在範囲φＬについての処理では、干渉回避範囲決定部２０は、アームアングルφの全周を、円錐ＣＬにおけるアームアングルの存在範囲φＬと決定する。

次に、干渉回避範囲決定部２０は、判定されたφＬとφＵとを用いて、アームアングルφの干渉回避範囲φｓを決定する。具体的には、干渉回避範囲決定部２０は、φＬとφＵとが重複する範囲を、干渉回避範囲φｓと決定する。

なお、上述したように、円錐ＣＬにおいて、干渉判定線分Ｌｄが円錐ＣＬの側面とのみ交点を有し、円錐ＣＬの底面と干渉判定線分Ｌｄとの交点が存在しない場合、円錐ＣＵにおけるアームアングルの存在範囲φＵはアームアングルφの全周である。したがって、干渉回避範囲決定部２０は、円錐ＣＬにおけるアームアングルの存在範囲φＬを、干渉回避範囲φｓと決定する。同様に、円錐ＣＵにおいて、干渉判定線分Ｌｄが円錐ＣＵの側面とのみ交点を有し、円錐ＣＵの底面と干渉判定線分Ｌｄとの交点が存在しない場合、円錐ＣＬにおけるアームアングルの存在範囲φＬはアームアングルφの全周である。したがって、干渉回避範囲決定部２０は、円錐ＣＵにおけるアームアングルの存在範囲φＵを、干渉回避範囲φｓと決定する。

一方、円錐ＣＬにおいて、干渉判定線分Ｌｄが円錐ＣＬの側面及び底面と交点を有する場合、円錐ＣＵにおいても、干渉判定線分Ｌｄが円錐ＣＵの側面及び底面と交点を有する。このような場合、φＬの一部とφＵの一部とが互いに重複する。したがって、干渉回避範囲決定部２０は、このφＬとφＵとが重複する範囲を、干渉回避範囲φｓと決定する。このような場合においては、アームアングルの干渉回避範囲φｓの一方の境界角度で前腕リンク部１２４が胴体１１０（胴体１１０上の干渉判定線分Ｌｄ）と干渉し、他方の境界角度で上腕部１２２が胴体１１０（胴体１１０上の干渉判定線分Ｌｄ）と干渉している。

次に、ステップＳ１３０において、干渉回避範囲決定部２０は、目標アームアングルφｔを決定する。具体的には、干渉回避範囲決定部２０は、干渉回避範囲φｓを満たすアームアングルφを、目標アームアングルφｔとする。例えば、干渉回避範囲決定部２０は、干渉回避範囲φｓの中央値を、目標アームアングルφｔとしてもよい。

また、Ｓ１０２の処理で事前設定を行うとき、又は、Ｓ１０４の処理で目標手先位置及び目標手先姿勢が決定したときに、干渉回避範囲決定部２０は、ロボットアーム１２０の各関節の許容関節角度から、ロボットアーム１２０の基準姿勢を規定する最適アームアングルφ_０を決定してもよい。そして、干渉回避範囲決定部２０は、その最適アームアングルφ_０を用いて目標アームアングルφｔを決定してもよい。例えば、最適アームアングルφ_０は、許容関節角度の中間値を用いて設定されてもよい。ここで、最適アームアングルφ_０が干渉回避範囲φｓに含まれる場合には、干渉回避範囲決定部２０は、最適アームアングルφ_０を目標アームアングルφｔとして決定してもよい。一方、最適アームアングルφ_０が干渉回避範囲φｓに含まれない場合には、干渉回避範囲決定部２０は、最適アームアングルφ_０に近い側の干渉回避範囲φｓの境界角度を、目標アームアングルφｔとして決定してもよい。

次に、ステップＳ１３２において、制御装置２は、目標アームアングルφｔにおけるロボットアーム１２０の各関節の関節角度を算出する。これにより、制御装置２は、肘部１３４の位置を設定する。具体的には、制御装置２の関節角度算出部２２は、決定されたアームアングルφにおける目標アームアングルφｔを用いて、逆運動学計算によって、各関節（第一関節部１４１〜第七関節部１４７）の関節角度θ_１〜θ_７を算出する。詳細には、関節角度算出部２２は、以下に示す方法によって、各関節角度θ_１〜θ_７を算出する。

図１４は、逆運動学計算を用いた関節角度の算出方法を説明するための図である。なお、手先部１２６の位置及び姿勢と手首部１３６の位置及び姿勢とはそれぞれ同一であると仮定していたので、以下、手先位置を手首位置として説明し、手先姿勢を手首姿勢として説明する。Ｓ１０４において、手先位置決定部１４が、手首部１３６の目標位置^０ｘ_７と、手首部１３６の目標姿勢^０Ｒ_７とを決定したとする。ここで、「^ｉｘ_ｋ」は、座標系Σ_ｉから見た座標系Σ_ｋの位置（位置ベクトル）を示す。なお、ｉが基準となる関節の座標系を示す場合、「^ｉｘ_ｋ」は、ｉ番目の関節から見たｋ番目の関節の位置を示す。また、「^ｉＲ_ｋ」は、座標系Σ_ｉから見た座標系Σ_ｋの姿勢を示す。なお、ｉが基準となる関節の座標系を示す場合、「^ｉＲ_ｋ」は、ｉ番目の関節から見たｋ番目の関節の姿勢を示す。ここで、ｉ＝０の場合は、肩部１３２の位置つまり原点Ｏから見た場合を示す。

ここで、図１４の（Ａ）に示すように、関節角度算出部２２は、肘部１３４をｘｚ平面上のｘ≦０の領域に置き、手先部１２６（手首部１３６）の位置をｚ軸上に置くように、ロボットアーム１２０の位置を仮想的に設定する。このとき、関節角度算出部２２は、肘部１３４の関節である第四関節部１４４の関節角度θ_４を、余弦定理により以下の式９によって算出する（図６参照）。

（式９）

また、関節角度算出部２２は、上腕部１２２とｚ軸との角度θｐ、つまり、上腕部１２２と、肩部１３２と手首部１３６を結ぶ線分との角度θｐを、余弦定理により以下の式１０によって算出する。
（式１０）

このとき、肘部１３４の位置に対応する位置ベクトルｅ_０及び手首部１３６の位置に対応する位置ベクトルｗ_０は、以下の式１１で表される。
（式１１）

関節角度算出部２２は、図１４の（Ａ）に示した仮想的なロボットアーム１２０（仮想ロボットアームＯ−ｅ_０−ｗ_０）を、図１４の（Ｂ）に示すように、ｚ軸周りに目標アームアングルφｔだけ回転させる。さらに、関節角度算出部２２は、図１４の（Ｂ）に示した仮想ロボットアームＯ−ｅ_１−ｗ_０を、図１４の（Ｃ）に示すように、ｘ軸周りに角度α回転させる。さらに、関節角度算出部２２は、図１４の（Ｃ）に示した仮想ロボットアームＯ−ｅ_２−ｗ_２を、図１４の（Ｄ）に示すように、ｙ軸周りに角度β回転させる。ここで、α及びβは、上記式１及び式２から得られる。このようにして仮想ロボットアームを回転させると、ｗ_３は目標手先位置^０ｘ_７と等しくなる。さらに、ｅ_３は、目標アームアングルφｔにおける肘部１３４の位置となる。

ここで、関節角度θ_１〜θ_７に対応する回転行列をそれぞれＲ_１〜Ｒ_７とする。また、関節角度算出部２２は、各関節角度を０としたときの肩部１３２から見た肘部１３４の位置に対応する位置ベクトル^３Ｉ_ｓｅ、及び、肘部１３４から見た手首部１３６の位置に対応する位置ベクトル^４Ｉ_ｅｗを、以下の式１２を用いて算出する。

（式１２）

また、手首部１３６の位置は、手首部１３６の各関節角度とは関係なく決定されるので、関節角度算出部２２は、手首部１３６の位置に対応する位置ベクトルを、以下の式１３によって算出する。
（式１３）

また、手首部１３６の姿勢については、肩部１３２、肘部１３４及び手首部１３６で分けて、以下の式１４で表される。
（式１４）

ここで、関節角度算出部２２は、上記のα、β、φｔ及びθｐを用いて、式１４から以下の式１５を算出する。
（式１５）

また、位置ベクトルｅ_３は、θ_１及びθ_２のみからなる^０Ｒ_２を用いて算出可能である。したがって、関節角度算出部２２は、肘部１３４の位置ベクトルｅ_３を、以下の式１６によって算出する。関節角度算出部２２は、式１６にα、β、φｔ及びθｐを代入して解くことによって、θ_１及びθ_２を算出する。

（式１６）

また、肩部１３２の３自由度による姿勢を考慮すると、以下の式１７のように、第三関節部１４３の姿勢（^０Ｒ_３）は、α、β、φｔ及びθｐによる姿勢と一致する。したがって、関節角度算出部２２は、式１７を用いて、θ_３を算出する。
（式１７）

さらに、手首部１３６の３自由度による姿勢を考慮すると、以下の式１８のように、^４Ｒ_７が定義される。関節角度算出部２２は、式１８を用いて、θ_５〜θ_７を算出する。
（式１８）

Ｓ１３２の処理までは、制御装置２は、手首部１３６におけるオフセットを無視して、前腕部１２５及びオフセット部１２７を一体とみなして演算を行った。つまり、制御装置２は、手首関節部１３５と第七関節部１４７とが一体であるとみなして演算を行った。一方、Ｓ１５２の処理からは、制御装置２は、手首部１３６におけるオフセットを考慮した演算を行う。

図１５は、手首部１３６におけるオフセットを考慮した演算について説明するための図である。図１５において、手首部１３６におけるオフセットが無視された演算、つまりＳ１３２の処理によって得られたロボットアーム１２０の位置及び姿勢が、破線で示されている。Ｓ１３２の処理に関するロボットアーム１２０は、長さＬ_１の上腕部１２２と、肘部１３４と、（前腕部１２５及びオフセット部１２７が一体とみなされた）長さＬ_２’の前腕リンク部１２４と、手先部１２６とで構成されている。

一方、手首部１３６におけるオフセットを考慮した演算によって得られたロボットアーム１２０の位置及び姿勢は、実線で示されている。実線で示されたロボットアーム１２０においては、前腕部１２５とオフセット部１２７とが分離されている。つまり、手首部１３６におけるオフセットを考慮した演算に関するロボットアーム１２０は、長さＬ_１の上腕部１２２と、肘部１３４と、長さＬ_２の前腕部１２５と、手首関節部１３５と、長さＬ_３のオフセット部１２７と、手先部１２６とで構成されている。

ここで、Ｓは、肩部１３２の位置を示す。また、Ｅ_２（第１の位置）は、Ｓ１３２の処理によって得られた肘部１３４の位置（ｅ_３）を示す。また、Ｅ_３は、手首部１３６におけるオフセットを考慮した演算によって得られた肘部１３４の位置を示す。また、Ｗ_ｒは、手首部１３６（手先部１２６）の位置、つまり目標手先位置（ｗ_３）を示す。ここで、オフセットを考慮した演算においては、Ｗ_ｒは、第七関節部１４７の位置（手先部１２６の位置）となる。また、Ｗ_ｐは、ピッチ軸周りに回転する第六関節部１４６の位置、つまり手首関節部１３５の位置を示す。

手首部１３６におけるオフセットを考慮した演算では、矢印Ｂで示すように、Ｓ，Ｅ_２，Ｗ_ｒを含む平面上、つまり肘部１３４の位置Ｅ_２とロボットアーム１２０の両端とを含む平面上で、肘部１３４の位置をＥ_２からＥ_３に調整する。また、このとき、Ｓ１０４の処理で設定された目標手先姿勢が満たされるようにする。これにより、Ｓ１３０で決定されたアームアングルφｔで定められる平面上で、手首部１３６におけるオフセットを考慮した肘部１３４の位置を決定することができる。したがって、ロボットアーム１２０の干渉を回避しやすい位置に肘部１３４の位置を決定することができる。

ステップＳ１５２において、制御装置２は、ロボットアーム１２０について座標系の変換を行う。具体的には、オフセット演算部２４は、図１６に示した基準座標系Σ_０から、第七関節部１４７を基準とした座標系Σ_ｗ，及びΣ_７に座標変換を行う。以下、詳述する。

図１６は、基準座標系Σ_０におけるロボットアーム１２０を示した図である。上述したように、基準座標系Σ_０においては、肩部１３２の位置Ｓを原点Ｏとしている。オフセット演算部２４は、まず、この基準座標系Σ_０から、図１７に示すような、Ｓ，Ｅ_２，Ｗ_ｒをｘｚ平面上に置いた座標系Σ_ｗに座標変換を行う。

図１７は、座標系Σ_ｗにおけるロボットアーム１２０を示した図である。オフセット演算部２４は、座標系Σ_ｗにおいて、Ｗ_ｒを原点Ｏに置き、Ｓをｚ軸上に置く。なお、ｙ軸の正方向は、紙面の手前から奥に向かう方向である。このとき、肘部１３４の位置Ｅ_３も、ｘｚ平面上にある。つまり、オフセット演算部２４は、ｘｚ平面上で、矢印Ｃで示すように、肘部１３４の位置をＥ_２からＥ_３に調整する。一方、手首関節部１３５（第六関節部１４６）の位置Ｗ_ｐは、ｘｚ平面上にあるとは限らない。

オフセット演算部２４は、式１９を用いて、手先部１２６の姿勢を、基準座標系Σ_０から座標系Σ_ｗに変換する。
（式１９）

なお、式１９を用いて手先部１２６の位置を変換すると、手先部１２６の位置は、式２０で示すように、原点Ｏに移動する。
（式２０）

ここで、Ｌ_０は、線分ＳＯ（線分ＳＷ_ｒ）の長さである。また、θ_０は、線分ＳＯ（線分ＳＷ_ｒ）と線分ＳＥ_２のなす角である。θは、線分ＳＯ（線分ＳＷ_ｒ）と線分ＳＥ_３のなす角である。つまり、θは、Ｓ，Ｅ_２，Ｗ_ｒを含む平面上において肘部１３４の位置Ｅ_３を示すパラメータである。また、γは、線分ＯＷ_ｐと線分ＯＥ_３のなす角である。また、Ｋ（θ）は、線分ＯＥ_３の長さである。

このとき、オフセット演算部２４は、以下の式２１を用いて、座標系Σ_ｗにおける肘部１３４の位置ベクトルを算出する。また、オフセット演算部２４は、余弦定理により、以下の式２２を用いてＫ（θ）を算出する。さらに、オフセット演算部２４は、式１０より、以下の式２３を用いてθ_０を算出する。

（式２１）

（式２２）

（式２３）

ここで、^ｗＲ_７の各成分を以下の式２４のように表すと、オフセット演算部２４は、^ｗＲ_７の逆行列^ｗＲ_７ ^−１を、以下の式２５のように算出する。
（式２４）

（式２５）

次に、オフセット演算部２４は、座標系Σ_ｗから、図１８に示すような、手先部１２６の姿勢が単位行列となるような座標系Σ_７に、座標変換を行う。つまり、座標系Σ_７では、手先部１２６の目標姿勢は、座標軸と一致したものとなる。

図１８は、座標系Σ_７におけるロボットアーム１２０を示した図である。図１８には、第六関節部１４６の回転軸がピッチ軸であり、第七関節部１４７の回転軸がロール軸である例が示されている。図１８に示すように、座標系Σ_７において、手先部１２６の目標姿勢は、座標軸と一致したものとなっている。図１８に示すように、座標系Σ_７において、第七関節部１４７の回転軸は、ｘ軸に一致したものとなっている。また、第六関節部１４６は、ｙｚ平面上にある。ここで、オフセット演算部２４は、座標系Σ_７における肘部１３４の位置を、Ｅ_３（ｘ_７，ｙ_７，ｚ_７）とする。また、Ｅ_３からｙｚ平面に下ろした垂線の足は、Ｈ’（０，ｙ_７，ｚ_７）である。ここで、第六関節部１４６は、線分ＯＨ’上にある。

ここで、式２１及び式２５から、オフセット演算部２４は、座標系Σ_７における肘部１３４の位置Ｅ_３（ｘ_７，ｙ_７，ｚ_７）を、以下の式２６を用いて算出する。
（式２６）

また、図１７において、三角形ＯＷ_ｐＥ_３に対する余弦定理によって以下の式２７が成り立つ。
（式２７）

また、図１８において、三角形ＯＨ’Ｅ_３は直角三角形であることから、以下の式２８が成り立つ。なお、ｓｇｎ（ｋ）は、ｋの符号を表す関数である。
（式２８）

ここで、式２７の右辺及び式２８の右辺が等しいことから、オフセット演算部２４は、以下の式２９のｆ（θ）が０となるθを求めることで、肘部１３４の位置を算出することができる。
（式２９）

ここで、式２９には、三角関数で表されるＫ（θ）及びｚ_７が平方根の中に現れるので、式２９のみを用いてｆ（θ）＝０を解くことは困難である。したがって、オフセット演算部２４は、ｆ（θ）＝０を満たすθを算出する際に、収束演算（反復法）を用いる。

ステップＳ１５４において、制御装置２は、収束演算の初期値を導出する。具体的には、オフセット演算部２４は、θの初期値θ_０を、式２３を用いて算出する。次に、ステップＳ１５６において、制御装置２のオフセット演算部２４は、以下に示すように、収束演算処理を行う。以下の説明では、ニュートン法による収束演算処理の方法を示すが、収束演算処理の方法については、ニュートン法に限定されない。

式２９を微分すると、以下の式３０が得られる。
（式３０）

オフセット演算部２４は、以下の式３１で表される漸化式により、θ_ｎをｎ＝０から順に式２９のｆ（θ_ｎ）に代入して、ｆ（θ_ｎ）を計算する。
（式３１）

まず、ｎ＝０について、オフセット演算部２４は、式２９に初期値θ_０を代入してｆ（θ_０）を算出する。次に、ｎ＝１について、オフセット演算部２４は、式２６、式３０及び式３１を用いて、θ_１を算出する。そして、オフセット演算部２４は、算出されたθ_１を式２９に代入して、ｆ（θ_１）を算出する。以下同様にして、オフセット演算部２４は、ｎ＝２以降についての演算を行う。

ステップＳ１５８において、オフセット演算部２４は、ｆ（θ）が閾値以下となったか否かを判定する。ここで、ｆ（θ）の閾値とは、ｆ（θ）に関する許容誤差を示す。この閾値は、手先部１２６の目標位置^０ｘ_７及び目標姿勢^０Ｒ_７の許容誤差、及びロボットシステム１における制約条件（制御装置２の計算速度、ロボット１００の動作速度等）などに応じて、予め定められる。つまり、閾値が小さい（０に近い）ほど、手先部１２６の位置及び姿勢は目標位置及び目標姿勢に近づくことができるが、ｆ（θ）が閾値以下となるまでの計算時間が膨大となりうる。例えば、ｆ（θ）の閾値は、１０^−１０と定められてもよいが、これに限られない。

ｆ（θ）が閾値以下でない場合（Ｓ１５８のＮＯ）、ステップＳ１６０において、オフセット演算部２４は、規定回数繰り返したか否かを判定する。具体的には、オフセット演算部２４は、ｎの値が規定値以上となったか否かを判定する。この規定回数（規定値）は、ｆ（θ）が閾値と同様に、ロボットシステム１における制約条件（制御装置２の計算速度、ロボット１００の動作速度等）などに応じて、予め定められる。規定回数が大きいほど、手先部１２６の位置及び姿勢は目標位置及び目標姿勢に近づくことができるが、計算時間が膨大となりうる。例えば、繰り返し計算の規定回数は、８回としてもよいが、これに限られない。

規定回数繰り返した場合（Ｓ１６０のＹＥＳ）、オフセット演算部２４は、この収束演算処理を打ち切る。つまり、オフセット演算部２４は、規定回数反復計算を行ってもｆ（θ）が閾値以内に収束しない場合は、Ｓ１０４で決定された目標位置及び目標姿勢では計算不能であると判定する。そして、処理はＳ１４０に進み、目標位置及び目標姿勢の修正処理がなされる。一方、規定回数繰り返していない場合（Ｓ１６０のＮＯ）、ｎをインクリメントして、上述したＳ１５６の収束計算処理を行う。

ｆ（θ）が閾値以下となった場合（Ｓ１５８のＹＥＳ）、ステップＳ１６２において、制御装置２は、第七関節部１４７の関節角度θ_７を算出する。具体的には、オフセット演算部２４（または関節角度算出部２２）は、このとき算出されたθ（ｆ（θ）が閾値以下となるθ）の値から、上記式２６及び以下の式３２を用いて、関節角度θ_７を算出する。

（式３２）

次に、ステップＳ１６４において、制御装置２は、第六関節部１４６の位置及び姿勢を算出する。具体的には、オフセット演算部２４（または関節角度算出部２２）は、以下の式３３を用いて、基準座標系Σ_０における第六関節部１４６の位置^０ｘ_６及び姿勢^０Ｒ_６を算出する。ここで、^ｉＴ_ｋは、ｉ番目の関節から見たｋ番目の関節の位置及び姿勢を示す行列である。なお、目標位置^０ｘ_７及び目標姿勢^０Ｒ_７は、上述したように、Ｓ１０４の処理で予め決定されている。

（式３３）

次に、ステップＳ１７２において、制御装置２は、各関節部の位置及び姿勢を算出する。具体的には、関節角度算出部２２は、算出された第六関節部１４６の位置^０ｘ_６及び姿勢^０Ｒ_６を用いて、ロボットアーム１２０の各関節の関節角度を算出する。ここで、第七関節部１４７の関節角度θ_７は既知であるから、残りの関節自由度は６自由度となる。つまり、目標位置３自由度及び目標姿勢３自由度の計６自由度と等しくなり、冗長自由度がなくなる。したがって、他の関節角度は、一意に決定され得る。

第六関節部１４６の位置^０ｘ_６及び姿勢^０Ｒ_６を用いた６自由度の関係式は、以下の式３４で表される。
（式３４）

関節角度算出部２２は、肘部１３４の関節である第四関節部１４４の関節角度θ_４を、余弦定理により以下の式３５によって算出する。
（式３５）

ここで、第六関節部１４６の位置及び姿勢を基準として肩部１３２の位置及び姿勢^６Ｔ_０は、式３６で表される。
（式３６）

式３６より、^６Ｔ_０における^６ｘ_０の成分は、^３Ｔ_０によらないで、θ_４、θ_５及びθ_６のみで表されることがわかる。ここで、^６ｘ_０は、既知である^０Ｔ_６の逆行列によって求められる。また、式３５によってθ_４は既知である。したがって、関節角度算出部２２は、以下の式３７によって、θ_５及びθ_６を算出する。

（式３７）

また、関節角度算出部２２は、式３７より、算出されたθ_４、θ_５及びθ_６を用いて、^６Ｒ_３を算出する。ここで、^０Ｒ_６は既知であるので、関節角度算出部２２は、以下の式３８を用いて、θ_１、θ_２及びθ_３を算出する。

（式３８）

ステップＳ１７４において、制御装置２は、ロボットアーム１２０を動作させる。具体的には、制御装置２のロボットアーム制御部２６は、Ｓ１７２で算出された各関節角度となるように、ロボットアーム１２０の各関節を制御する。なお、その際、ロボットアーム制御部２６は、ロボットアーム１２０の各関節の許容される動作域、及び速度域のチェックを行ってもよい。そして、動作域又は速度域を満たさない場合、制御装置２は、目標アームアングルφｔ及びを再度決定し直してもよい。

本実施の形態においては、ロボットアーム１２０の可動領域を円錐で模擬し、その円錐（円錐ＣＬ及びＣＵ）と、胴体１１０の表面上にある干渉判定線分Ｌｄとの交点に応じて、干渉回避範囲を決定している。ここで、円錐と線分との交点は、単純な幾何学的手法によって容易に算出可能である。したがって、本実施の形態においては、ロボットアーム１２０の干渉回避範囲を、高速に決定することが可能である。

ロボットアームの干渉を回避する他の方法として、例えば、一連の腕軌道に関して事前にポテンシャル場を設定し、そのポテンシャル場を用いて干渉を回避可能な経路を探索する方法がある。しかしながら、このような数値解析方法では、膨大な反復計算を行う必要があり、最終的に経路が探索されるように解が収束するまでの時間が膨大となるおそれがある。また、干渉を回避可能な各関節角度を示すテーブルを記憶しておいて、そのテーブルを随時参照する方法もある。しかしながら、この方法では、テーブルを事前に設定する必要があり、干渉を回避可能な解を都度算出するものではない。

一方、本実施の形態においては、上述したように、単純な幾何学的手法によって、ロボットアーム１２０の干渉回避範囲を決定することが可能である。したがって、本実施の形態においては、ロボットアームの干渉を回避するための計算を高速に行うことが可能となる。言い換えると、ロボットアームの干渉回避解をリアルタイムに決定することが可能となる。さらに、本実施の形態においては、高速に（リアルタイムに）干渉回避解を決定することができるので、ロボットの制御周期ごとに、干渉回避解を決定することが可能となる。さらに、高速に干渉回避解を決定できることから、ロボットの制御周期を短くすることが可能となり、それによって、力制御の応答性能を向上させることも可能となる。つまり、本実施の形態においては、ロボットアーム１２０と胴体１１０との干渉回避を考慮して各関節角度を高速に求解できることで、力制御を高周期で行うことを可能とし、したがって、高応答な力インタラクション可能なロボットを実現することが可能となる。

また、本実施の形態においては、最適アームアングルφ_０を事前に決定しておき、その最適アームアングルφ_０を用いて目標アームアングルφｔを決定してもよい。これによって、ロボットアームの干渉を回避しつつ、ロボットアームの基準姿勢に近くなるように、ロボットアームを制御することが可能となる。これによって、ロボットアームの干渉を回避しつつ、ロボットアームの動作がより自然となるように制御することが可能となる。

また、手首部１３６の各関節を設計するに際し、３軸が互いに直交した構造とすると、関節構造が非常に複雑となる。一方、本実施の形態にように、第六関節部１４６（ピッチ）と第七関節部１４７（ロール）との間にオフセット部１２７を設けることで、手首部１３６の設計が容易となる。このように、手首部１３６にオフセット部１２７が設けられている場合であっても、本実施の形態においては、ロボットアーム１２０の干渉を回避することが可能となる。

また、従来、手首部にオフセットを有する７軸マニピュレータの逆運動学計算を行うためには、いずれかの１軸の関節角度を恣意的に指定しなければならなかった。一方、本実施の形態においては、制御装置２は、ロボットアーム１２０の可動領域において干渉を回避するように決定されたアームアングルによって設定された肘部１３４の位置と、肩部１３２の位置と、手先部１２６の位置とを含む平面上で、オフセットを考慮して肘部１３４の位置を再設定する。これにより、可動領域において干渉を回避し、かつオフセットを考慮した指標で、手先部１２６の１自由度の角度（第七関節部１４７の関節角度）を算出することができ、この角度を用いて残りの６自由度の角度も、解析的手法で算出することができる。

なお、本実施の形態においては、オフセット演算部２４が収束演算処理を行っている。一方、オフセット部１２７の長さＬ_３は、動きの親和性の観点から、上腕部１２２の長さＬ_１及び前腕部１２５の長さＬ_２と比較して非常に短い。したがって、ｆ（θ）の収束解（ｆ（θ）を閾値以下とするθ）は、θの初期値θ_０と非常に近くなる。また、ｆ（θ）の収束演算において使用されるのは、ｓｉｎθ及びｃｏｓθのみであり、演算コストは非常に小さい。したがって、本実施の形態における収束演算処理は、収束時間も早く、計算に要する負荷も軽いものである。したがって、本実施の形態における収束演算処理は、解析的解法のみで求解する手法と比較しても遜色ない収束時間も及び計算負荷となる。したがって、手首部１３６にオフセット部１２７が設けられている場合であっても、高速に干渉回避解を決定できることから、ロボットの制御周期を短くすることが可能となる。したがって、本実施の形態においては、手首部１３６にオフセット部１２７が設けられている場合であっても、高応答な力インタラクション可能なロボットを実現することが可能となる。

なお、本実施の形態においては、図１５等に示すように、ロボットアーム１２０の干渉を回避するように決定されたアームアングルによる肘部１３４の位置Ｅ_２を、オフセットを考慮してＥ_３に移動する。したがって、ロボットアーム１２０が、Ｓ１３２で計算された干渉回避位置から、干渉位置に移動してしまうことが懸念される。しかしながら、上述したように、オフセット部１２７の長さＬ_３つまりオフセット量は、極力短く設計される。したがって、仮に、肘部１３４の位置をＥ_２からＥ_３に移動することによりロボットアーム１２０が干渉領域に入り込む場合であっても、その入り込む量はわずかである。したがって、制御装置２は、胴体１１０に対応する障害物領域（直方体Ｂｄ）を、その入り込みを考慮して大きめに設定しておいてもよい。つまり、障害物領域（直方体Ｂｄ）及び干渉判定線分Ｌｄは、オフセット量及び胴体１１０の外形を考慮して設定され得る。

（変形例）
なお、本発明は上記実施の形態に限られたものではなく、趣旨を逸脱しない範囲で適宜変更することが可能である。例えば、図４及び図１８に示したフローチャートにおいて、複数の処理の順序は、適宜、変更可能である。例えば、図４において、Ｓ１０２〜Ｓ１０６の処理、及びＳ１４０〜Ｓ１４４の処理は、省略されてもよい。また、アームアングルの存在範囲φＬについての処理及びアームアングルの存在範囲φＵについての処理の順序は、逆であってもよい。

また、上述した実施の形態においては、障害物の例としてロボット１００の胴体１１０としたが、障害物は、ロボット１００の胴体でなくてもよい。例えば、障害物は、柱又は壁等の、ロボットアーム１２０が干渉し得る任意の物体であってもよい。この場合、制御装置２は、各障害物の位置を示すデータを取得する手段を有してもよい。例えば、ロボット１００に、障害物の位置データ（座標データ）を取得するためのセンサを設けてもよい。また、障害物は１つとは限らず、複数あってもよい。この場合、複数の障害物それぞれについて、上述した処理を行ってもよい。このとき、複数の障害物それぞれについて干渉回避範囲が決定されるが、この決定された複数の干渉回避範囲が互いに重複する範囲を、ロボットアームが複数の障害物に対して干渉しない干渉回避範囲と決定してもよい。

また、第一関節部１４１，第二関節部１４２，第三関節部１４３の順序、及び、第五関節部１４５，第六関節部１４６，第七関節部１４７の順序は、図１に示したものに限られず、適宜変更可能である。なお、各関節の順序を変更した場合には、関節角度算出部２２における計算方法を適宜変更すればよい。

また、上述した実施の形態においては、干渉判定線分Ｌｄは胴体１１０の角部１１０ａ又は角部１１０ｂにあるとしたが、このような構成に限られない。干渉判定線分Ｌｄは、胴体１１０（障害物）の表面上（境界面上）の任意の線分であってもよい。しかしながら、干渉判定線分Ｌｄを角部１１０ａ又は角部１１０ｂにある線分とすることで、干渉回避範囲の判定がより容易となる。

また、上述した実施の形態においては、ロボットアーム１２０と障害物（胴体等）との干渉について説明したが、ロボットアーム１２０はロボットの腕に限られない。例えば、本実施形態にかかるロボットアームは、ロボットの脚に対しても適用可能である。この場合、上腕部は上腿部、前腕部は下腿（脛）部、肘は膝に、それぞれ置き換え可能である。また、本実施形態にかかるロボットアームは、任意のマニピュレータに対しても適用可能である。

また、上述した実施の形態においては、ロボットアーム１２０（及びロボット１００）は、物理的な実体のあるロボット１００に設けられているとしているが、このような構成に限られない。例えば、ロボットアーム１２０（及びロボット１００）は、仮想的なものであってもよい。つまり、本実施の形態は、例えばバーチャルリアリティに対しても適用可能である。具体的には、本実施の形態は、例えばゲームにおいて、任意の仮想的なリンク状の物体と任意の仮想的な障害物との干渉を回避するための計算に対しても適用可能である。また、本実施の形態は、何らかのリンク状の物体と障害物との干渉の有無をシミュレーションするものに対しても適用可能である。

また、上述した実施の形態においては、基準座標系Σ_０を図１に示すように定義したが、基準座標系Σ_０をロボットアーム１２０に対してどのように定義するかは、適宜変更可能である。他の座標系についても同様である。また、上述した実施の形態においては、ｘｙｚ座標系（基準座標系Σ_０）の原点を肩部１３２に設定したが、原点位置は任意である。この場合、肩部１３２と原点とのオフセットを考慮して、計算を行うようにすればよい。

また、Ｓ１５４の処理で、θの初期値θ_０が式２３を用いて算出されるとしたが、このような構成に限られない。θの初期値は、前回にロボットアーム１２０を動作させたときの逆運動学計算の過程で算出されたθとしてもよい。

また、上述した実施の形態においては、第六関節部１４６の回転軸がピッチ軸であり、第七関節部１４７の回転軸がロール軸であるとしたが、このような構成に限られない。第六関節部１４６の回転軸がロール軸であり、第七関節部１４７の回転軸がピッチ軸としてもよい。この場合、座標系Σ_７において、第七関節部１４７の回転軸は、ｙ軸に一致したものとなる。また、この場合、第六関節部１４６は、ｘｙ平面上にある。

１ ロボットシステム
２ 制御装置
１２ 情報格納部
１４ 手先位置決定部
１６ 可動領域算出部
１８ 交点算出部
２０ 干渉回避範囲決定部
２２ 関節角度算出部
２４ オフセット演算部
２６ ロボットアーム制御部
３０ 線分決定部
１００ ロボット
１１０ 胴体
１１０ａ 角部
１１０ｂ 角部
１１２ 胴体前面
１１４ 胴体側面
１２０ ロボットアーム
１２２ 上腕部
１２４ 前腕リンク部
１２５ 前腕部
１２６ 手先部
１２７ オフセット部
１３２ 肩部
１３４ 肘部
１３５ 手首関節部
１３６ 手首部
１４１ 第一関節部
１４２ 第二関節部
１４３ 第三関節部
１４４ 第四関節部
１４５ 第五関節部
１４６ 第六関節部
１４７ 第七関節部

Claims (1)

1. 肩部と、前記肩部に接続された上腕部と、肘部と、前腕部と、手首部と、手先部とを有するロボットアームであって、前記肘部を介して前記上腕部と前記前腕部とが接続され、前記手首部を介して前記前腕部と前記手先部とが接続され、前記手首部を構成する３つの関節部のうちの前記手先部の側の第１の関節部と前記第１の関節部と隣り合う第２の関節部との間にオフセット部が設けられるように構成されたロボットアームの障害物に対する干渉を回避するロボットアームの干渉回避方法であって、
   前記手先部の目標位置及び目標姿勢を指定し、
   前記ロボットアームの両端が固定された状態で、前記第２の関節部が前記第１の関節部と直交する位置にあるとみなし、前記前腕部と前記オフセット部とを一体とみなして構成された前腕リンク部及び前記上腕部の可動領域を算出し、
   前記可動領域において前記ロボットアームが前記障害物を含む障害物領域に干渉しない干渉回避範囲を決定し、
   前記決定された干渉回避範囲の中で前記肘部を第１の位置に設定し、
   前記前腕部と前記オフセット部とを分離して、前記設定された前記肘部の第１の位置と前記ロボットアームの両端とを含む平面上で、前記手先部の目標姿勢を満たすように、前記肘部の位置を示すパラメータを算出し、
   前記算出されたパラメータを用いて、前記ロボットアームを構成する関節の角度を算出する
   干渉回避方法。

Images