JP6137155B2 - 干渉回避方法、制御装置及びプログラム - Google Patents

Description

本発明は干渉回避方法、制御装置及びプログラムに関し、特に、ロボットアームの干渉を回避する干渉回避方法、制御装置及びプログラムに関する。

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

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

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

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

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

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

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

本発明にかかる干渉回避方法は、関節部を介して第１アームと第２アームとが接続されて構成されたロボットアームの障害物に対する干渉を回避するロボットアームの干渉回避方法であって、前記ロボットアームの両端の位置が固定された状態における、前記第１アーム及び前記第２アームの可動領域を算出し、前記可動領域と、前記障害物を含む障害物領域の境界面上の第１の線との交点を算出し、前記算出された交点に基づいて、前記可動領域において前記ロボットアームが前記障害物領域に干渉しない干渉回避範囲を決定する。

また、本発明にかかる制御装置は、関節部を介して第１アームと第２アームとが接続されて構成されたロボットアームを制御する制御装置であって、前記ロボットアームの両端の位置が固定された状態における、前記第１アーム及び前記第２アームの可動領域を算出する可動領域算出手段と、前記可動領域と、障害物を含む障害物領域の境界面上の第１の線との交点を算出する交点算出手段と、前記算出された交点に基づいて、前記可動領域において前記ロボットアームが前記障害物領域に干渉しない干渉回避範囲を決定する干渉回避範囲決定手段とを有する。

また、本発明にかかるプログラムは、関節部を介して第１アームと第２アームとが接続されて構成されたロボットアームの障害物に対する干渉を回避するロボットアームの干渉回避方法を実現するプログラムであって、前記ロボットアームの両端の位置が固定された状態における、前記第１アーム及び前記第２アームの可動領域を算出するステップと、前記可動領域と、前記障害物を含む障害物領域の境界面上の第１の線との交点を算出するステップと、前記算出された交点に基づいて、前記可動領域において前記ロボットアームが前記障害物領域に干渉しない干渉回避範囲を決定するステップとをコンピュータに実行させる。

本発明は、上述したように構成されていることによって、単純な幾何学的手法によって、ロボットアームの干渉回避範囲を決定することが可能である。したがって、本発明は、ロボットアームの干渉を回避するための計算を高速に行うことが可能となる。

また、好ましくは、前記可動領域は、前記ロボットアームの両端の位置が固定された状態で前記関節部が回転した軌跡で形成された円を底面とし、前記第１アーム及び前記第２アームがそれぞれ回転した軌跡で形成された回転面を円錐側面とする円錐で形成され、前記第２アームの軌跡によって形成された第１の円錐と前記第１アームの軌跡によって形成された第２の円錐とで、別個に前記交点を算出する。
別個に計算を行うことによって、単純に、円錐と線との交点を算出するという簡易な計算手法を適用できる。そのため、本発明は、ロボットアームの干渉回避範囲を、さらに高速に決定することが可能である。

また、好ましくは、前記第１の線との交点が前記底面上に存在する場合、前記第１の円錐に基づいて判定された第１の範囲と、前記第２の円錐に基づいて判定された第２の範囲とが重複する範囲を、前記ロボットアームの前記干渉回避範囲と決定する。
上記のような構成とすることによって、干渉回避範囲の一方の境界で第２アームが障害物と干渉し、他方の境界で第１アームが障害物と干渉している場合であっても、簡易な幾何学的手法によって、干渉回避範囲を決定することが可能である。したがって、本発明は、ロボットアームの干渉回避範囲を、さらに高速に決定することが可能である。

また、好ましくは、前記第１の円錐に関する前記円錐側面上の交点と前記底面上の交点とに基づいて前記第１の範囲を判定し、前記第２の円錐に関する前記円錐側面上の交点と前記底面上の交点とに基づいて前記第２の範囲を判定する。
上記のように構成することによって、簡易な幾何学的手法によって、第１の円錐及び第２の円錐それぞれについて、第１の範囲及び第２の範囲を判定することが可能である。したがって、本発明は、ロボットアームの干渉回避範囲を、さらに高速に決定することが可能である。

また、好ましくは、前記障害物領域の前記境界面の少なくとも一部は曲面で形成される。
境界面の向きが不連続に形成されていると、ロボットアームの動きが急峻となるおそれがある。このような場合、ロボットアームの速度超過又は過負荷が発生するおそれがある。一方、本発明は、上記のような構成とすることによって、ロボットアームの動きの変化が緩和される。したがって、ロボットアームの速度超過又は過負荷を抑制することが可能となる。

また、好ましくは、前記ロボットアームの手先の位置を通り前記曲面に接する接線における接点を含む線を、前記曲面上の前記第１の線と決定する。
上記のような構成とすることによって、前記境界面の少なくとも一部が曲面で形成されている場合であっても、簡易な幾何学的手法によって、容易に、干渉回避範囲を決定することが可能となる。したがって、本発明は、前記境界面の少なくとも一部が曲面で形成されている場合であっても、ロボットアームの干渉回避範囲を、高速に決定することが可能である。

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

実施の形態１にかかるロボットシステムを示す概略図である。 実施の形態１にかかるアームアングルについて説明するための図である。 実施の形態１にかかる制御装置の構成を示す機能ブロック図である。 実施の形態１にかかる制御装置の処理を示すフローチャートである。 実施の形態１にかかる可動領域の算出方法を説明するための図である。 実施の形態１にかかる可動領域算出部によって算出された円錐の各寸法を定義するための図である。 実施の形態１にかかる可動領域算出部によって算出された円錐を示す図である。 実施の形態１にかかる可動領域算出部によって算出された円錐を示す図である。 実施の形態１にかかるロボットの胴体とロボットアームとの関係について示す図である。 実施の形態１にかかるロボットの胴体とロボットアームとの関係について示す図である。 実施の形態１にかかるロボットの胴体とロボットアームとの関係について示す図である。 図７Ａに示した円錐の側面と干渉判定線分との交点が２つ存在する場合を例示する図である。 図９の例におけるアームアングルの存在範囲の判定方法について説明するための図である。 図７Ａに示した円錐の底面と干渉判定線分との交点が１つ存在する場合を例示する図である。 図１１の例におけるアームアングルφの存在範囲の判定方法について説明するための図である。 図７Ａに示した円錐の側面と干渉判定線分との交点が１つ存在する場合を例示する図である。 逆運動学計算を用いた関節角度の算出方法を説明するための図である。 実施の形態１にかかる、目標手先位置が胴体の角部近傍である場合のロボットアームの動きについて示す上面図である。 実施の形態２にかかる、目標手先位置が胴体の角部近傍である場合のロボットアームの動きについて示す上面図である。 実施の形態２にかかる制御装置の構成を示す機能ブロック図である。 実施の形態２にかかる制御装置の処理を示すフローチャートである。 実施の形態２にかかる円錐ＣＬ（前腕部の軌跡）における干渉判定線分を決定する方法を説明するための図である。 実施の形態２にかかる接点を決定する方法を説明するための図である。 実施の形態２にかかる円錐ＣＵ（上腕部の軌跡）における干渉判定線分を決定する方法を説明するための図である。 実施の形態２にかかる接点を決定する別の方法を説明するための図である。 本実施の形態を採用したロボットの動作を再現したシミュレーション結果を示す図である。 本実施の形態を採用したロボットの動作を再現したシミュレーション結果を示す図である。 本実施の形態を採用したロボットの動作を再現したシミュレーション結果を示す図である。 本実施の形態を採用したロボットの動作を再現したシミュレーション結果を示す図である。

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

一方、手先部１２６の干渉が発生しない場合（Ｓ１０６のＮＯ）、ステップＳ１０８において、制御装置２は、ロボットアーム１２０の可動領域を算出する。具体的には、制御装置２の可動領域算出部１６は、上述したアームアングルφを用いて、上腕部１２２及び前腕部１２４の可動領域を算出する。なお、このときに算出される「可動領域」は、ロボットアーム１２０の周囲の障害物との干渉については考慮されていない。以下、可動領域算出部１６における計算方法を説明する。

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

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

（式１）

（式２）

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

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

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

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

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

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

（式３）

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

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

（式４）

（式５）

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

（式６）

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

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

（式７）

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

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

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

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

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

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

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

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

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

（式８）

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

（式９）

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

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

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

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

（式１２）

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

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

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

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

（式１６）

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

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

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

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

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

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

また、本実施の形態においては、上腕部１２２の可動領域の軌跡（円錐ＣＵ）と、前腕部１２４の可動領域の軌跡（円錐ＣＬ）とで、別個に計算を行っている。このように別個に計算を行うことによって、単純に、円錐と線（線分）との交点を算出するという、簡易な計算手法を適用できる。そのため、ロボットアーム１２０の干渉回避範囲を、さらに高速に決定することが可能である。

また、本実施の形態においては、円錐の底面がｘ’ｙ’平面上に、頂点がｚ’軸上になるように座標変換が行われる。これによって、ロボットアーム１２０の干渉を回避するための計算をさらに簡易にすることが可能となる。したがって、ロボットアーム１２０の干渉回避範囲を、さらに高速に決定することが可能である。

また、本実施の形態においては、円錐ＣＬにおいて、干渉判定線分Ｌｄが円錐ＣＬの側面及び底面と交点を有し、円錐ＣＵにおいて、干渉判定線分Ｌｄが円錐ＣＵの側面及び底面と交点を有する場合、干渉回避範囲決定部２０によって判定されたアームアングルの存在範囲φＬとφＵとが重複する範囲が、干渉回避範囲φｓと決定される。このような構成とすることによって、アームアングルの干渉回避範囲φｓの一方の境界角度で前腕部１２４が胴体１１０（胴体１１０上の干渉判定線分Ｌｄ）と干渉し、他方の境界角度で上腕部１２２が胴体１１０（胴体１１０上の干渉判定線分Ｌｄ）と干渉している場合であっても、簡易な幾何学的手法によって、干渉回避範囲を決定することが可能である。したがって、ロボットアーム１２０の干渉回避範囲を、さらに高速に決定することが可能である。

また、このような場合、前腕部１２４にかかるアームアングルの存在範囲φＬの境界角度φ１Ｌを、円錐ＣＬの底面と干渉判定線分Ｌｄとの交点を用いて算出している。同様に、上腕部１２２にかかるアームアングルの存在範囲φＵの境界角度φ１Ｕを、円錐ＣＵの底面と干渉判定線分Ｌｄとの交点を用いて算出している。このように構成することによって、簡易な幾何学的手法によって、各円錐ＣＬ及びＣＵそれぞれについて、アームアングルの存在範囲φＬ及びφＬを判定することが可能である。したがって、ロボットアーム１２０の干渉回避範囲を、さらに高速に決定することが可能である。

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

（実施の形態２）
次に、実施の形態２について説明する。実施の形態１においては、胴体１１０を模式的に直方体としていた。一方、実施の形態２については、胴体１１０の角部を曲面形状に模擬した点で、実施の形態１と異なる。以下、図を用いて実施の形態１と実施の形態２との比較について説明する。

図１５は、実施の形態１にかかる、目標手先位置が胴体１１０の角部１１０ａ近傍である場合のロボットアーム１２０の動きについて示す上面図である。また、図１６は、実施の形態２にかかる、目標手先位置が胴体１１０の角部１１０ｂ近傍である場合のロボットアーム１２０の動きについて示す上面図である。図１５及び図１６においては、ロボット１００の上側つまりｚ軸の正方向から見たロボット１００が示されている。

上述したように、実施の形態１においては、胴体１１０が直方体に模擬されているので、角部１１０ａは例えば略９０度で交わる２つの平面で形成されている。このような場合に、角部１１０ａ近傍で手先部１２６を矢印Ａ方向に胴体１１０中央側にわずかに移動させると、ロボットアーム１２０は、胴体１１０との干渉を回避するために、矢印Ｂに示すように大きく移動してしまう。言い換えると、実施の形態１においては、手先部１２６がわずかに移動しただけで、ロボットアーム１２０が急峻に移動することとなる可能性がある。このように、ロボットアーム１２０を急峻に移動させる場合、ロボットアーム１２０の各関節も急峻に動作することとなる。この結果、各関節のモータ（図示せず）において、速度超過及び過負荷が生じてしまい、各関節が劣化するおそれがある。

一方、実施の形態２においては、胴体１１０の角部１１０ｂが曲面で形成されている。したがって、図１６に示すように、図１５と同じ変化量で、矢印Ａ方向に手先部１２６を移動させた場合、矢印Ｃに示すように、ロボットアーム１２０の動きの変化が緩和される。したがって、ロボットアーム１２０の各関節のモータにおける速度超過及び過負荷を抑制することが可能となる。

図１７は、実施の形態２にかかる制御装置２の構成を示す機能ブロック図である。実施の形態２にかかる制御装置２のハードウェア構成は、実施の形態１にかかる制御装置２のハードウェア構成と実質的に同じである。一方、実施の形態２にかかる制御装置２は、実施の形態１にかかる制御装置２の各構成要素に加え、線分決定部３０をさらに有する点で、実施の形態１と異なる。

線分決定部３０は、干渉判定線分Ｌｄを決定する。つまり、線分決定部３０は、干渉判定線分Ｌｄを決定する線分決定手段としての機能を有する。実施の形態１においては、干渉判定線分Ｌｄは、常に線分ＡＢであった。言い換えると、実施の形態１においては、手先部１２６の位置によらず不変であった。一方、実施の形態２においては、後述するように、干渉判定線分Ｌｄは、手先部１２６の位置に応じて変化する。線分決定部３０は、手先部１２６の位置に応じて（例えば手先部１２６の位置が指定されるごとに）、干渉判定線分Ｌｄを決定する。このように、手先部１２６の位置に応じて干渉判定線分Ｌｄを決定することにより、角部１１０ｂを曲面で形成した場合であっても、実施の形態１と同様の方法で、簡易な幾何学的手法によって、容易に、干渉回避範囲を決定することが可能となる。

図１８は、実施の形態２にかかる制御装置２の処理を示すフローチャートである。図１８においては、図４の各処理に加え、ステップＳ２０９が追加されている。図１８に示すように、実施の形態２においても、制御装置２は、図４に示したＳ１０２〜Ｓ１０８とそれぞれ実質的に同様の処理Ｓ２０２〜Ｓ２０８を行う。したがって、Ｓ２０２〜Ｓ２０８についての説明は省略する。

ステップＳ２０９において、制御装置２は、干渉判定線分Ｌｄを決定する。具体的には、制御装置２の線分決定部３０は、手先部１２６の位置（目標手先位置）に応じて、円錐ＣＬ（前腕部１２４の軌跡）及び円錐ＣＵ（上腕部１２２の軌跡）それぞれについて、干渉判定線分Ｌｄを決定する。線分決定部３０は、円錐ＣＬについて、手先部１２６の位置を通り、角部１１０ｂにかかる曲面に接する接線における接点を含む線を、干渉判定線分Ｌｄ_Ｌと決定する。また、線分決定部３０は、円錐ＣＵについて、肩部１３２の位置を通り、角部１１０ｂにかかる曲面に接する接線における接点を含む線を、干渉判定線分Ｌｄ_Ｕと決定する。以下、具体的に説明する。

図１９は、実施の形態２にかかる円錐ＣＬ（前腕部１２４の軌跡）における干渉判定線分Ｌｄ_Ｌを決定する方法を説明するための図である。図１９においては、角部１１０ｂを、半径Ｒの円弧で形成された曲面としている。線分決定部３０は、手先部１２６が位置Ａにある場合に、位置Ａから角部１１０ｂにかかる曲面に引いた接線の接点ａを通り、ｚ軸に平行である線を、干渉判定線分Ｌｄ_Ｌと決定する。同様に、線分決定部３０は、手先部１２６が位置Ｂ（又は位置Ｃ）にある場合に、位置Ｂ（又は位置Ｃ）から角部１１０ｂにかかる曲面に引いた接線の接点ｂ（又は接点ｃ）を通り、ｚ軸に平行である線を、干渉判定線分Ｌｄ_Ｌと決定する。なお、手先部１２６が位置Ｄにある場合、位置Ｄから、角部１１０ｂにかかる曲面に接線を引くことはできない。このような場合、線分決定部３０は、角部１１０ｂにかかる曲面の端点ｄを通り、ｚ軸に平行である線を、干渉判定線分Ｌｄ_Ｌと決定する。

図２０は、実施の形態２にかかる接点を決定する方法を説明するための図である。ここで、接点を決定する際、ｚ軸成分については無視している。また、角部１１０ｂにかかる曲面を形成する円弧の中心を原点とし、胴体１１０の前方向（図２０の紙面上方向）をｘ軸正方向とし、胴体１１０の左方向（図２０の紙面左方向）をｙ軸正方向とする。このような座標系において、手先部１２６の位置が（ｈ_ｘ，ｈ_ｙ）に決定されたとする。このとき、線分決定部３０は、以下に示すように、接点（ｃ_ｘ，ｃ_ｙ）を決定する。

接点（ｃ_ｘ，ｃ_ｙ）は半径Ｒの円周上にあるので、線分決定部３０は、以下の式１９を得る。
（式１９）

また、線分決定部３０は、以下の式２０のように、接線を示す方程式を算出する。
（式２０）

ここで、接線は手先位置（ｈ_ｘ，ｈ_ｙ）を通るので、線分決定部３０は、（ｈ_ｘ，ｈ_ｙ）を式２０に代入して、以下の式２１を得る。
（式２１）

さらに、線分決定部３０は、式２１で得られたｃ_ｙを式１９に代入して得られたｃ_ｘの２次方程式を解くことによって、ｃ_ｘを算出する。さらに、線分決定部３０は、算出されたｃ_ｘを式２０に代入することによって、ｃ_ｙを算出する。これによって、線分決定部３０は、接点（ｃ_ｘ，ｃ_ｙ）を決定する。なお、接点は複数求まるが、線分決定部３０は、図２０の太線部分（ｘ≧０，ｙ≦０）にある接点を、決定対象とする。なお、実際には、ｘｙｚ座標系の原点は肩部１３２の位置であるので、線分決定部３０は、算出された接点（ｃ_ｘ，ｃ_ｙ）について原点位置を変換するための座標変換を行う。

図２１は、実施の形態２にかかる円錐ＣＵ（上腕部１２２の軌跡）における干渉判定線分Ｌｄ_Ｕを決定する方法を説明するための図である。線分決定部３０は、肩部１３２から角部１１０ｂにかかる曲面に引いた接線の接点ｅを通り、ｚ軸に平行である線を、干渉判定線分Ｌｄ_Ｕと決定する。ここで、肩部１３２の位置は、手先部１２６の位置によらず一定である。したがって、線分決定部３０は、手先部１２６の位置が指定される前に、予め、干渉判定線分Ｌｄ_Ｕを決定しておいてよい。

ステップＳ２１０において、制御装置２の交点算出部１８は、図４に示したＳ１１０と同様に、図７Ａ及び図７Ｂに示した円錐と、干渉判定線分Ｌｄとの交点を算出する。このとき、交点算出部１８は、線分決定部３０によって算出された干渉判定線分Ｌｄ_Ｌと、円錐ＣＬとの交点を算出する。同様に、交点算出部１８は、線分決定部３０によって算出された干渉判定線分Ｌｄ_Ｕと、円錐ＣＵとの交点を算出する。それ以外の処理については、Ｓ１１０と同様であるので、説明を省略する。

また、図１８に示すように、実施の形態２においても、制御装置２は、図４に示したＳ１１２〜Ｓ１３４，Ｓ１４０〜Ｓ１４４とそれぞれ実質的に同様の処理Ｓ２１２〜Ｓ２３４，Ｓ２４０〜Ｓ２４４を行う。したがって、Ｓ２１２〜Ｓ２３４，Ｓ２４０〜Ｓ２４４についての説明は省略する。

なお、Ｓ２１４において、図１０を用いて説明したアームアングルの存在範囲φＬの決定方法については、平面Ｆ’の代わりに、胴体前面１１２に平行でかつ接点を通る平面を座標変換した平面が用いられる。同様に、平面Ｓ’の代わりに、胴体側面１１４に平行でかつ接点を通る平面を座標変換した平面が用いられる。例えば、接点が、図１９における接点ｂと決定された場合、平面Ｆ’の代わりに、胴体前面１１２に平行でかつ接点ｂを通る平面１１２ｂを座標変換した平面が用いられる。同様に、平面Ｓ’の代わりに、胴体側面１１４に平行でかつ接点ｂを通る平面１１４ｂを座標変換した平面が用いられる。

また、Ｓ２１４において、図１２を用いて説明したアームアングルφの決定方法については、平面Ｆ’の代わりに、胴体前面１１２に平行でかつ接点を通る平面を座標変換した平面が用いられる。例えば、接点が、図１９における接点ｂと決定された場合、平面Ｆ’の代わりに、胴体前面１１２に平行でかつ接点ｂを通る平面１１２ｂを座標変換した平面が用いられる。

また、Ｓ２１６において、図１０を用いて説明したアームアングルの存在範囲φＬの決定方法については、平面Ｆ’の代わりに、胴体前面１１２に平行でかつ接点ｅを通る平面を座標変換した平面が用いられる。同様に、平面Ｓ’の代わりに、胴体側面１１４に平行でかつ接点ｅを通る平面を座標変換した平面が用いられる。また、Ｓ２１６において、図１２を用いて説明したアームアングルφの決定方法についても、平面Ｓ’の代わりに、胴体側面１１４に平行でかつ接点ｅを通る平面を座標変換した平面が用いられる。

図２２は、実施の形態２にかかる接点を決定する別の方法を説明するための図である。図２２には、図２０と異なり、角部１１０ｂにかかる曲面がだ円弧である場合について示されている。ここで、接点を決定する際、ｚ軸成分については無視している。また、角部１１０ｂにかかる曲面を形成するだ円弧の中心を原点とし、胴体１１０の前方向をｘ軸正方向とし、胴体１１０の左方向をｙ軸正方向とする。ここで、だ円のｘ軸方向の軸を２ａ、ｙ軸方向の軸を２ｂとする。このような座標系において、手先部１２６の位置が（ｈ_ｘ，ｈ_ｙ）に決定されたとする。このとき、線分決定部３０は、以下に示すように、接点（ｃ_ｘ，ｃ_ｙ）を決定する。

接点（ｃ_ｘ，ｃ_ｙ）はだ円周上にあるので、線分決定部３０は、以下の式２２を得る。
（式２２）

また、線分決定部３０は、以下の式２３のように、接線を示す方程式を算出する。
（式２３）

ここで、接線は手先位置（ｈ_ｘ，ｈ_ｙ）を通るので、線分決定部３０は、（ｈ_ｘ，ｈ_ｙ）を式２３に代入して、以下の式２４を得る。
（式２４）

さらに、線分決定部３０は、式２４で得られたｃ_ｙを式２２に代入して得られたｃ_ｘの２次方程式を解くことによって、ｃ_ｘを算出する。さらに、線分決定部３０は、算出されたｃ_ｘを式２３に代入することによって、ｃ_ｙを算出する。これによって、線分決定部３０は、接点（ｃ_ｘ，ｃ_ｙ）を決定する。なお、接点は複数求まるが、線分決定部３０は、図２２の太線部分（ｘ≧０，ｙ≦０）にある接点を、決定対象とする。なお、実際には、ｘｙｚ座標系の原点は肩部１３２の位置であるので、線分決定部３０は、算出された接点（ｃ_ｘ，ｃ_ｙ）について原点位置を変換するための座標変換を行う。

なお、実施の形態２においては、角部１１０ｂについてのみ曲面であるとしたが、このような構成に限られない。例えば、胴体１１０の全周が曲面で形成されてもよい。このような場合、単に、実施の形態２において胴体前面１１２及び胴体側面１１４の平面部分をなくすようにすればよい。言い換えると、角部１１０ｂだけでなく、胴体前面１１２及び胴体側面１１４の全てを、曲面で表すようにすればよい。

図２３〜図２６は、本実施の形態を採用したロボット１００の動作を再現したシミュレーション結果を示す図である。まず、図２３に示すように、操作者の操作によって、手先部１２６の位置が、胴体側面１１４の外側の位置Ａに指定される。次に、図２４に示すように、操作者の操作によって、手先部１２６の位置が、胴体前面１１２の前側の位置Ｂに指定される。次に、図２５に示すように、操作者の操作によって、手先部１２６の位置が、位置Ｂよりも胴体前面１１２に近い位置Ｃに指定される。さらに、図２６に示すように、操作者の操作によって、手先部１２６の位置が、位置Ｃよりも胴体前面１１２に近い位置Ｄに指定される。位置Ａ〜Ｄのいずれの位置に手先部１２６の位置が指定された場合であっても、上腕部１２２及び前腕部１２４は、胴体１１０と干渉しないように動作している。さらに、ロボット１００の胴体１１０の角部１１０ｂが曲面で形成されているので、図２３〜図２６に示した一連の動作において、ロボットアーム１２０の動き（特に肘部１３４の移動）が滑らかに（つまり急峻でないように）なっている。

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

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

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

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

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

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

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

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

Claims (8)

1. 関節部を介して第１アームと第２アームとが接続されて構成されたロボットアームの障害物に対する干渉を回避するロボットアームの干渉回避方法であって、
   前記ロボットアームの両端の位置が固定された状態における、前記第１アーム及び前記第２アームの可動領域を算出し、
   前記可動領域と、前記障害物を含む障害物領域の境界面上の第１の線との交点を算出し、
   前記算出された交点に基づいて、前記可動領域において前記ロボットアームが前記障害物領域に干渉しない干渉回避範囲を決定する
   干渉回避方法。
2. 前記可動領域は、前記ロボットアームの両端の位置が固定された状態で前記関節部が回転した軌跡で形成された円を底面とし、前記第１アーム及び前記第２アームがそれぞれ回転した軌跡で形成された回転面を円錐側面とする円錐で形成され、
   前記第２アームの軌跡によって形成された第１の円錐と前記第１アームの軌跡によって形成された第２の円錐とで、別個に前記交点を算出する
   請求項１に記載の干渉回避方法。
3. 前記第１の線との交点が前記底面上に存在する場合、前記第１の円錐に基づいて判定された第１の範囲と、前記第２の円錐に基づいて判定された第２の範囲とが重複する範囲を、前記ロボットアームの前記干渉回避範囲と決定する
   請求項２に記載の干渉回避方法。
4. 前記第１の円錐に関する前記円錐側面上の交点と前記底面上の交点とに基づいて前記第１の範囲を判定し、前記第２の円錐に関する前記円錐側面上の交点と前記底面上の交点とに基づいて前記第２の範囲を判定する
   請求項３に記載の干渉回避方法。
5. 前記障害物領域の前記境界面の少なくとも一部は曲面で形成される
   請求項１から４のいずれか１項に記載の干渉回避方法。
6. 前記ロボットアームの手先の位置を通り前記曲面に接する接線における接点を含む線を、前記曲面上の前記第１の線と決定する
   請求項５に記載の干渉回避方法。
7. 関節部を介して第１アームと第２アームとが接続されて構成されたロボットアームを制御する制御装置であって、
   前記ロボットアームの両端の位置が固定された状態における、前記第１アーム及び前記第２アームの可動領域を算出する可動領域算出手段と、
   前記可動領域と、障害物を含む障害物領域の境界面上の第１の線との交点を算出する交点算出手段と、
   前記算出された交点に基づいて、前記可動領域において前記ロボットアームが前記障害物領域に干渉しない干渉回避範囲を決定する干渉回避範囲決定手段と
   を有する制御装置。
8. 関節部を介して第１アームと第２アームとが接続されて構成されたロボットアームの障害物に対する干渉を回避するロボットアームの干渉回避方法を実現するプログラムであって、
   前記ロボットアームの両端の位置が固定された状態における、前記第１アーム及び前記第２アームの可動領域を算出するステップと、
   前記可動領域と、前記障害物を含む障害物領域の境界面上の第１の線との交点を算出するステップと、
   前記算出された交点に基づいて、前記可動領域において前記ロボットアームが前記障害物領域に干渉しない干渉回避範囲を決定するステップと
   をコンピュータに実行させるプログラム。

Images