JP2015174185A - ロボットのシミュレーション装置及び方法、制御装置、及びロボットシステム - Google Patents

Abstract

【課題】多関節マニピュレータの制御において、関節の可動範囲による運動の制限を緩和する技術を提供する。
【解決手段】多関節マニピュレータの各関節について、角度の可動範囲と、その内側に定義される安全域とを設定する。各関節の現在角度と、手先位置指令値とに基づいて、各関節の角度指令値を生成する。角度指令値が安全域を超えた場合、その関節の角度指令値を生成する際に、角度指令値の変化速度を小さくする障害回避制御を行う。
【選択図】図１

Description

本発明は、多関節マニピュレータを備えたロボットの制御に関する。

多関節マニピュレータ（多関節アーム）を備えたロボットが知られている。図１は、多関節マニピュレータの参考例を示す。多関節マニピュレータ１０１は、直列的に接続された複数のリンクＬ１０１〜Ｌ１０６を備える。隣接する一対のリンク（例えばリンクＬ１０１とＬ１０２）は、その間に設けられた関節（関節Ｊ１０２）によって互いに可動的に接続される。図１の例では、６つの回転関節（関節Ｊ１０１〜Ｊ１０６）を有する多関節マニピュレータ１０１が描かれている。

具体的には、固定された基部１０２に、支持部１０３の一端が取り付けられる。支持部１０３の他端に、第１の関節Ｊ１０１の一方側が取り付けられる。第１の関節Ｊ１０１の他方側に、第１のリンクＬ１０１の一端が取り付けられる。第１のリンクＬ１０１の他端に、第２の関節Ｊ１０２の一方側が取り付けられる。以下、同様にして、第５のリンクＬ１０５の他端に第６の関節Ｊ１０６の一方側が取り付けられる。第６の関節Ｊ１０６の他方側に第６のリンクＬ１０６の一端が取り付けられる。リンクＬ１０６の他端にエンドエフェクタ１０４が取り付けられる。

作業者は、手先（エンドエフェクタの先端などの位置）のワールド座標系における位置指令値を制御装置に対して指定する。制御装置は、逆運動学計算により、手先が位置指令値の方向に移動するように、各関節Ｊ１０１〜Ｊ１０６の角度指令値を計算する。各関節Ｊ１０１〜Ｊ１０６は、その角度指令値に従ってモータ等により駆動される。このような制御により、多関節マニピュレータ１０１の手先を所望の位置に移動することができる。

非特許文献１には、このような多関節マニピュレータの制御についての一般論が記載されている。

吉川恒夫『ロボット制御基礎論』、コロナ社、１９８８年１１月２５日発行

図２は、参考例における多関節マニピュレータ１０１を示す。手先１０５を目標点１１０に移動する制御を考える。作業者が目標点１１０を入力すると、制御装置は手先１０５が目標点１１０に移動するように関節角度の指令値を計算する。その指令値に従って、手先１０５が目標点１１０に到達するように各関節が駆動される。図２には、この動作において手先１０５が描く軌道１１１が描かれている。

このような制御においては、制御装置が手先１０５の位置及び姿勢について指令値を与え、その指令値に基づいて、各関節の角度の指令値を逆運動学により計算する。その各関節角度の指令値により、多関節マニピュレータ１０１が駆動される。

各関節角度を計算する逆運動学計算において、手先速度と関節角速度の関係付けを与えるヤコビ行列を用いると、容易かつシステマティックに指令値を計算することができる。この場合、ヤコビ行列を用いて算出された関節角速度を時間積分することにより、各関節の角度変位（角度の変化量）δθの指令値が求められる。この変化量δθの指令値と関節の角度の現在値を足すことにより、関節の角度の指令値が得られる。

次に、このように指令値を算出する場合に発生し得る問題について説明する。説明を簡単にするために、図２の多関節マニピュレータ１０１において３つの関節Ｊ１０２、Ｊ１０４、Ｊ１０６のみが駆動されることと仮定する。図３は、手先１０５が軌道１１１を描いて動く場合に、上記のような方法で算出された各関節Ｊ１０２、Ｊ１０４、Ｊ１０６の角度の例を示す。図３の横線は時間を示す。図３（ａ）に曲線で描かれている角度１２１は関節Ｊ１０２の角度θ１の時間変化を示す。図３（ｂ）に曲線で描かれている角度１２２は関節Ｊ１０４の角度θ２の時間変化を示す。図３（ｃ）に曲線で描かれている角度１２３は関節Ｊ１０６の角度θ３の時間変化を示す。

多関節マニピュレータ１０１の各関節Ｊ１０２、Ｊ１０４、Ｊ１０６の可動範囲が狭い場合などの場合を考えると、手先１０５が目標点１１０に接近することが難しい可能性がある。図３には、各関節Ｊ１０２、Ｊ１０４、Ｊ１０６の可動範囲が示されている。関節Ｊ１０２の角度θ１は、下限値ＬＬ１０１と上限値ＵＬ１０１の間の範囲で可動であり、その範囲を超えることは出来ないものとする。同様に、関節Ｊ１０４の角度θ２は、下限値ＬＬ１０２と上限値ＵＬ１０２の間の範囲で可動であり、関節Ｊ１０６の角度θ３は、下限値ＬＬ１０３と上限値ＵＬ１０３の間の範囲で可動である。

図３の例では、時刻ｔ１において関節Ｊ１０２の角度θ１が下限値ＬＬ１０１を下回っている。従って、順運動・逆運動計算に従って目標点１１０に到達するための関節角度を計算しても、時刻ｔ１において関節Ｊ１０２が可動範囲を超えてしまう。このような場合、その時刻で動作停止をさせるインターロックを設けることが普通である。そのため、ヤコビ行列を用いた順運動学及び逆運動学計算をリアルタイムに行いながら多関節マニピュレータ１０１を制御している場合には、時刻ｔ１で停止してしまう。

図３の例では、仮に関節Ｊ１０２、Ｊ１０４、Ｊ１０６の可動範囲に制限が無いと考えた場合に手先１０５が目標点１１０に到達する時刻ｔ２において、すべての関節の角度θ１〜θ３は、それぞれの可動範囲の内側に収まっている。すなわち、関節角度の指令値が一旦は可動範囲を超えているものの、手先１０５が目標点１１０に達した段階での多関節マニピュレータ１０１の姿勢においては、各関節Ｊ１０２、Ｊ１０４、Ｊ１０６の角度は可動範囲内である。従って、途中の軌道１１１が異なっていれば、手先１０５を目標点１１０に移動することができた可能性がある。

多関節マニピュレータの制御において、関節の可動範囲による運動の制限を緩和する技術が望まれる。

本発明の一側面において、シミュレーション装置は、複数の関節を備える多関節マニピュレータのシミュレーション装置であって、複数の関節の各々について、角度の可動範囲と、可動範囲の内側に定義される安全域とを記憶装置に記憶する記憶部と、複数の関節の各々の現在の角度を示す現在角度データと、多関節マニピュレータの手先位置指令値とに基づいて、複数の関節の各々の角度指令値を生成する角度指令値計算部とを備える。角度指令値計算部は、複数の関節のいずれかの内、角度指令値が安全域を超えた関節である停止候補関節が発生したとき、停止候補関節の角度指令値を生成する際に、角度指令値の変化速度を小さくする障害回避制御を行う。

本発明の他の側面において、制御装置は、本発明によるシミュレーション装置によって生成された角度指令値によって多関節マニピュレータを制御する。

本発明の更に他の側面において、ロボットシステムは、本発明による制御装置と、多関節マニピュレータとを備える。

本発明の更に他の側面において、シミュレーション方法は、複数の関節を備える多関節マニピュレータのシミュレーション方法であって、複数の関節の各々について、角度の可動範囲と、可動範囲の内側に定義される安全域とを記憶装置に記憶する工程と、複数の関節の各々の現在の角度を示す現在角度データと、多関節マニピュレータの手先位置指令値とに基づいて、複数の関節の各々の角度指令値を生成する工程とを備える。角度指令値を生成する工程において、複数の関節のいずれかの内、角度指令値が安全域を超えた関節である停止候補関節が発生したとき、停止候補関節の角度指令値を生成する際に、角度指令値の変化速度を小さくする障害回避制御を行う。

本発明の更に他の側面において、制御方法は、本発明によるシミュレーション方法で生成された角度指令値によって多関節マニピュレータを制御する。

本発明の更に他の側面において、プログラムは、本発明によるシミュレーション方法をコンピュータに実行させる。

本発明により、多関節マニピュレータの制御において、関節の可動範囲による運動の制限を緩和する技術が提供される。

図１は、多関節マニピュレータの参考例を示す。 図２は、多関節マニピュレータの参考例を示す。 図３は、関節の角度指令値の時間変化を示す。 図４は、ロボットシステムを示す。 図５は、関節の制御における計算の説明図である。 図６は、関節の角度指令値の時間変化を示す。 図７は、関節の角度指令値の時間変化を示す。 図８は、コンピュータによって実現される機能ブロックを示す。 図９は、安全域テーブルを示す。 図１０は、障害回避制御のフローチャートである。 図１１は、データテーブルを示す。 図１２は、多関節マニピュレータのシミュレーション計算の説明図である。 図１３は、多関節マニピュレータ画像の一例を示す。 図１４は、軌道修正処理のフローチャートである。 図１５は、多関節マニピュレータ画像の一例を示す。 図１６は、経由点設定処理のフローチャートである。 図１７は、経由点の設定方法の説明図である。

以下、添付図面を参照して、本発明の実施形態を説明する。図４は、本発明の一実施形態における多関節マニピュレータ１と、コンピュータＤ１と、表示装置Ｄ２とを備えたロボットシステムを示す。多関節マニピュレータ１は、床面等に固定された基部２を備える。基部２に、支持部３の一端が固定される。基部２が台車等によって移動可能な場合には、その移動についての座標変換を施せば、以下の説明は同様に適用できる。

支持部３の他端は、関節Ｊ１の一方側に固定される。関節Ｊ２の他方側に、第１のリンクＬ１の一端が取り付けられる。第１のリンクＬ１の他端に、第２の関節Ｊ２の一方側が取り付けられる。以下、同様にして、第５のリンクＬ５の他端に第６の関節Ｊ６の一方側が取り付けられる。第６の関節Ｊ６の他方側に、第６のリンクＬ６の一端が取り付けられる。第６のリンクＬ６の他端に、エンドエフェクタ４が取り付けられる。図４の例では、６個の関節Ｊ１〜Ｊ６を備える多関節マニピュレータ１が描かれているが、これより多い、またはこれより少ないｎ個の関節Ｊ１〜Ｊｎを備えたｎ自由度の多関節マニピュレータ１を用いてもよい。

作業者は、多関節マニピュレータ１のエンドエフェクタ４の先端などに設定される手先５のワールド座標系における位置及び姿勢を示す位置・姿勢指令値（最終的な目標値）を制御装置に対して指定する。制御装置は、手先５が位置・姿勢指令値に示された状態に向かうように、各関節Ｊ１〜Ｊ６の角度指令値を生成する。各関節Ｊ１〜Ｊ６は、その角度指令値に従ってモータ等により駆動される。このような制御により、多関節マニピュレータ１の手先５を所望の位置に移動することができる。

多関節マニピュレータ１に、コンピュータＤ１が接続される。コンピュータＤ１は、ハードディスク等の有体の非遷移的（ｔａｎｇｉｂｌｅ，ｎｏｎ−ｔｒａｎｓｉｔｏｒｙ）な記憶媒体を備える。コンピュータＤ１は、その記憶媒体に格納されたソフトウェアを実行することによって、多関節マニピュレータ１の動作をシミュレーションにより再現することができる。そのシミュレーションにより、多関節マニピュレータ１の動作が表示装置Ｄ２の画面上で再現される。作業者は、その画面上に表示された多関節マニピュレータ画像６を見て、グラフィカルユーザインタフェースによって画面上の手先５を所望の場所に移動し、更にその姿勢を指定する。このような画面操作により、多関節マニピュレータ１の手先５の位置・姿勢指令値を設定することができる。

コンピュータＤ１及び表示装置Ｄ２により、多関節マニピュレータ１のシミュレーションを行うシミュレーション装置、多関節マニピュレータ１の制御用データ生成装置、及び多関節マニピュレータ１の制御装置が実現される。コンピュータＤ１、表示装置Ｄ２、及び多関節マニピュレータ１により、ロボットシステムが構成される。

図５は、手先５の指令値を入力したときの関節Ｊ１〜Ｊ６の制御における計算の一般的な流れを示す。多関節マニピュレータ１は、エンコーダなどによって、関節Ｊ１〜Ｊ６の各々の現在の姿勢を示す関節角度θを検出することができる。コンピュータＤ１は、多関節マニピュレータ１から、各関節Ｊ１〜Ｊ６の関節角度θの検出値を取得する（Ａ０）。コンピュータＤ１は、その関節角度θと、予め記憶したリンクパラメータに基づいて順運動学計算を行うことにより、ワールド座標系における現在の手先位置及び手先姿勢を計算する（Ａ１）。

一方、作業者はコンピュータＤ１を用い、表示装置Ｄ２のシミュレーション画像を見ながら、手先５の最終的な目標位置及び目標姿勢を示す手先指令を入力する。コンピュータＤ１は、多関節マニピュレータ１の手先５が現在位置姿勢から最終目標位置姿勢に到達するまでの位置姿勢の軌道を生成する。この軌道により、現在の手先５に対して、次の制御周期で目標とすべき手先位置及び姿勢の指令値が与えられる（Ａ８）。コンピュータＤ１は、Ａ１で算出された現在の手先位置及び姿勢に対する、手先位置及び姿勢の指令値の偏差Ｅを計算する（Ａ２）。コンピュータＤ１は更に、予め設定された位置姿勢制御用の比例ゲインＫＰを偏差Ｅに掛ける（Ａ３）。

各関節Ｊ１〜Ｊ６の現在の角度の検出値に基づいて、関節の角速度と直交座標系における手先速度との関係を示すヤコビ行列Ｊを計算する（Ａ４）。更に、そのヤコビ行列の逆行列Ｊ^−１（多関節マニピュレータ１が冗長自由度を持っている場合は疑似逆行列）を計算する（Ａ５）。この逆ヤコビ行列を用いて、ゲインを掛けた位置姿勢偏差ＫＰ・Ｅから関節角速度の指令値を算出する（Ａ６）。

関節角速度の指令値を時間積分することにより、各関節Ｊ１〜Ｊ６の角度の変化量の指令値が算出される。ブロックＡ０で入力された各関節Ｊ１〜Ｊ６の角度の現在値に、この角度の変化量の指令値を足すことにより、関節角度の指令値が得られる（Ａ７）。コンピュータＤ１は、関節角度の指令値を多関節マニピュレータ１に送信する。多関節マニピュレータ１の制御装置は、その指令値に基づいて、各関節Ｊ１〜Ｊ６のモータ等を制御する。以上の制御により、作業者が指定した目標位置及び目標姿勢を取るようにエンドエフェクタ４を動かすことが可能である。

次に、本実施形態において関節Ｊ１〜Ｊ６の可動限界付近における制御について説明する。まず図６を参照して、本実施形態における制御の基本的な考えを説明する。図６は、或る関節について、可動範囲が制限されていないと仮定した場合の角度θの指令値の時間変化を示す。しかし実際には、この関節は、可動範囲が下限ＬＬ１と上限ＵＬ１との間の範囲に制限されているものとする。その内側に、仮想的なリミットを設定する。すなわち、下限ＬＬ１よりやや大きい値に仮想的な下限ＬＬ２を設定する。上限ＵＬ１よりやや小さい値に仮想的な上限ＵＬ２を設定する。このような仮想的な上限ＵＬ２、下限ＬＬ２は、各関節の特性に応じて、関節毎に設定される。

関節の角度２１が仮想的な上限ＵＬ２と下限ＬＬ２の間の値を取っている領域では、実際の上限ＵＬ１、下限ＬＬ１まで余裕幅があるので、通常の順運動学・逆運動学の計算によって指令値を算出して制御を行う。

図６の例では、時刻ｔ１において角度２１（指令値）が仮想的な下限ＬＬ２を下回り、時刻ｔ２において角度２１が実際の下限ＬＬ１を下回る。その後、時刻ｔ３において角度２１が実際の下限ＬＬ１を上回り、可動範囲に戻る。更に、時刻ｔ４において仮想的な下限ＬＬ２を上回り、安全域２２に入る。最終的に、時刻ｔ５において手先５が最終目標点に到達した段階では、角度２１は可動範囲の内側に入っている。

このような制御において、関節の角度２１が上限ＵＬ１又は下限ＬＬ１に近づいたときに、何らかの対処を取ることが望まれる。そのため、角度２１が仮想的な下限ＬＬ２を下回るか、又は仮想的な上限ＵＬ２を上回ったときに、障害回避制御が行われる。例えば図６では、時刻ｔ１において角度２１が仮想的な下限ＬＬ２を下回るので、通常制御から障害回避制御への切り替えが行われる。

図７は、障害回避制御における角度２１の変化の一例を示す。障害回避制御が開始される時刻ｔ１において、リミットに近づいた関節の角度指令値の変化速度を小さくする。このとき、その他の関節については通常制御が続行される。図７の時刻ｔ１以降の角度２１は、このような角度指令値による制御結果を示す。このような制御の結果、角度２１が下限ＬＬ１に接近するまでに時間の余裕が生じる。

図７において、点線で示した角度２３は、時刻ｔ１以降においても通常制御を続行した場合の角度の変化を示す。この場合、時刻ｔ２において関節の角度２３が下限ＬＬ１に達して、多関節マニピュレータ１の動作が停止してしまう。しかしながら、障害回避制御を適用した場合、関節の角度の変化速度が低下しているために、時刻ｔ２になってもまだ下限ＬＬ１に到達するまでには余裕幅がある。このような状態において、他の関節が回転することにより、多関節マニピュレータ１の姿勢が変化する。その結果、障害回避制御を行っている関節に対する角度指令値が変化するため、角度指令値がリミットに達することを回避できる可能性がある。図７の例では、障害回避制御を行っている期間に関節の角度２１の変化が下限ＬＬ１から離れる方向に転じ、時刻ｔ３において仮想的な下限ＬＬ２を上回って安全域２２に入る。この時刻ｔ３において、関節の制御が障害回避制御から通常の制御に戻される。

以上のような障害回避制御について、以下により具体的に説明する。図８は、障害回避制御を行うためにコンピュータＤ１によって実現される機能ブロックを示す。コンピュータＤ１は、記憶部１１、角度指令値計算部１２、障害回避制御部１３、軌道データ生成部１４、シミュレーション部１５、障害関節表示部１６、及び軌道修正部１７として機能する。これらの各機能ブロックは、コンピュータＤ１の演算制御装置が非遷移的な記憶媒体に格納されたソフトウェアを読み出して実行することによって実現することができる。

図９は、記憶部１１が記憶する安全域テーブル２５を示す。多関節マニピュレータ１が備えるｎ個の関節Ｊ１、Ｊ２、…Ｊｎの各々について、可動範囲の上限と下限（実際のリミット）と、それより内側に設定された安全域２２の上限と下限とが登録される。関節Ｊ１、Ｊ２、…Ｊｎのうちの一部にのみ安全域２２を登録して障害回避制御を適用してもよい。上限・下限ではなく右回転方向の限界角度と左回転方向の限界角度の対などによって可動範囲を登録してもよい。

［ステップＳ１：軌道データ入力］
図１０は、障害回避制御のフローチャートである。軌道データ生成部１４は、多関節マニピュレータ１の手先５の位置姿勢の指令値を示す軌道データを生成する。この軌道データは、多関節マニピュレータ１の手先を、現在手先位置から目標手先位置まで所定の軌道（例えば等速直線運動）に沿って移動させるために用いられる。このような軌道データは、例えば、多関節マニピュレータ１の制御周期毎の手先５の位置姿勢の指令値として与えられる。コンピュータＤ１が多関節マニピュレータ１の制御を行うとき、このようにして生成された軌道データを入力する。

図１１に、本実施形態における制御で用いるデータを表したデータテーブル２４を示す。このようなデータテーブル２４を記憶部１１に格納してもよい。データテーブル２４において、手先指令値（少なくとも手先の位置を示す手先位置指令値、より一般的には手先の姿勢も示す手先位置姿勢指令値）Ａ１〜Ａｎが、軌道データを示す。軌道データは、現在手先位置に近い第１番目の時刻Ｔ１における手先指令値Ａ１から目標手先位置に近い第ｎ番目の時刻Ｔｎにおける手先指令値Ａｎまでの一連の手先位置指令値Ａｉ（ｉは１以上ｎ以下の整数）からなる。各手先指令値Ａｉは、位置を示す３つの値（Ｘ、Ｙ、Ｚと記す）と、姿勢を示す３つの値（Ａ、Ｂ、Ｃと記し、オイラー角などにより表現できる三次元空間内の角度を示す）とを含む。

［ステップＳ２：手先の現在位置・姿勢の計算］
多関節マニピュレータ１の各関節Ｊ１〜Ｊ６には、その回転角度を検出するエンコーダ等の検出装置が取り付けられる。コンピュータＤ１は、各関節Ｊ１〜Ｊ６の回転角度の検出値を多関節マニピュレータ１からリアルタイムで（例えば１０ｍｓのオーダーの制御周期で）読み込む。この角度の検出値が、データテーブル２４に、複数の関節の各々の現在の角度を示す現在角度データＣ１〜Ｃｎとして示されている。

以下、ステップＳ４まで、角度指令値計算部１２は、図５に示した方法によって、関節角度指令値を算出するための計算を実行する。まず、多関節マニピュレータ１から読み込まれた関節角度の検出値から、順運動学計算によって、多関節マニピュレータ１の手先５の現在位置が算出される。角度指令値計算部１２は更に、手先５のリンク（図４ではエンドエフェクタ４を備えたリンクＬ６）の現在姿勢を算出する。これらは図５のＡ１の処理に相当する。

［ステップＳ３：関節角速度指令値の算出］
ステップＳ１で入力された軌道データ指令値と、ステップＳ２で生成された手先５の位置・姿勢の現在値とに基づいて、図５のＡ２〜Ａ５の計算を行うことにより、各関節Ｊ１〜Ｊ６に対する回転の速度を示す角速度指令値Ｖθ（単位はラジアン毎秒［ｒａｄ／ｓ］）が算出される。これは図５のＡ６に相当する。

［ステップＳ４：関節角度指令値の算出］
複数の関節Ｊ１〜Ｊ６の各々の関節角速度指令値Ｖθに、図５のＡ７のように係数Ｋを掛けて時間積分することにより、次の制御周期における関節の角度の変化量の指令値δθ［ｒａｄ］が算出される。ステップＳ２で入力された各関節Ｊ１〜Ｊ６の角度の現在値に、角度の変化量の指令値δθを加えることにより、複数の関節の各々の角度指令値θ［ｒａｄ］が生成される。この角度指令値θが、図１１のデータテーブル２４において、各時刻Ｔｉにおける角度指令値Ｂｉとして示されている。通常制御において、係数Ｋは例えばＫ＝１に設定される。

［ステップＳ５：関節角度指令値の仮想リミットをチェック］
障害回避制御部１３は、各関節Ｊ１〜Ｊ６について、ステップＳ５で算出された角度指令値が仮想リミットの範囲内（仮想の下限ＬＬ２と仮想の上限ＵＬ２との間の領域）に入っているか否かを判定する。この判定は、安全域テーブル２５に登録された各関節Ｊ１〜Ｊｎの安全域の値を用いて行われる。又は他の方法として、安全域テーブル２５に登録された各関節Ｊ１〜Ｊｎの可動範囲の上限と下限を用い、可動範囲のうちの上限又は下限に近い所定範囲（例えば可動範囲全体の幅の１０パーセント以内）の値に入ったときに、仮想リミットの範囲内に入ったと判定してもよい。

［ステップＳ６：指令値による分岐処理］
すべての関節Ｊ１〜Ｊ６について角度指令値が仮想リミット以内であった場合（ステップＳ７Ｙｅｓ）、すべての関節Ｊ１〜Ｊ６について通常制御を行うために、処理はステップＳ８に移行する。複数の関節Ｊ１〜Ｊ６のいずれかの内、角度指令値が仮想リミットを超えていた関節があった場合、停止する可能性のある停止候補関節であると判定する。その停止候補関節について障害回避制御を行うため、処理はステップＳ７に移行する。

［ステップＳ７：障害回避制御の積分係数設定］
ステップＳ６において角度指令値が仮想リミット外であると判定された関節に対して、障害回避制御が適用される。具体的には、ステップＳ４の計算で用いられた積分係数Ｋを小さくする。例えば、Ｋ＝０．１に設定する。他の関節については、通常制御が続けられる。このような設定により、ステップＳ４（図５のＡ７）で算出される関節の回転角度がリミットに近づく速度が遅くなり、図７の期間ｔ１〜ｔ３における角度２１のような制御を実現することができる。

［ステップＳ８：通常制御の積分係数設定］
ステップＳ５で関節角度指令値を生成する積分計算時に用いた係数Ｋとして、通常制御の値（例：Ｋ＝１）に設定する。前回の制御周期までに通常制御を行っていた関節については、係数Ｋ＝１は変化しない。前回の制御周期までに障害回避制御を行っていた関節については、ステップＳ７で今回新たに角度指令値が仮想リミット内であると判定された場合、障害回避制御時の積分係数の値（例：Ｋ＝０．１）が通常制御時の値Ｋ＝１に戻される。

［ステップＳ９：指令値をコントローラへ出力］
コンピュータＤ１は、ステップＳ４で算出された関節Ｊ１〜Ｊ６の角度指令値を多関節マニピュレータ１に出力する。多関節マニピュレータ１のコントローラは、その角度指令値を用いて各関節Ｊ１〜Ｊ６の角度を制御する。

［多関節マニピュレータの動作のシミュレーション］
次に、多関節マニピュレータ１の動作のシミュレーションについて説明する。上記の図１０では、多関節マニピュレータ１の実機の動作中に関節がリミットに近づいた場合の制御について説明した。それに対して、実機の動作を伴わずに、関節がリミットに近づいた場合の多関節マニピュレータ１の動作をシミュレーションによって再現することも可能である。

図８のシミュレーション部１５は、そのようなシミュレーション計算を実行する。図１２を参照して、そのようなシミュレーションについて説明する。図５と比較して、以下の２点が異なっている。
（１）ブロックＡ７で生成された関節の角度指令値θが各関節角度の現在値θとしてフィードバックされるフィードバック処理ＦＢが追加されている。
（２）図５のブロックＡ０において多関節マニピュレータ１の実機から各関節Ｊ１〜Ｊ６の角度の現在値を取得していた。その代わりに、図１２のブロックＡ９においては、時刻Ｔｉにおける関節の角度指令値θと手先指令値を用いてブロックＡ７で算出された関節角度の指令値θが、次の時刻Ｔｉ＋１における各関節角度の現在値θとして与えられる。

図５と図１２における計算の違いは、以下のような意味を有する。
（ａ）図５では、時刻Ｔｉにおける計算により、ブロックＡ７において関節角度の指令値を生成し、その指令値によって多関節マニピュレータ１を制御した。その制御の結果として得られた各関節角度の検出値が、次の時刻Ｔｉ＋１における制御計算に用いられた。
（ｂ）それに対して図１２では、疑似的にθ指令を更新して次の時刻におけるθの現在値として用い、ヤコビ行列の収束計算を実施する。このような計算により、多関節マニピュレータ１の動作をコンピュータＤ１の内部で疑似的に再現する。

このようなシミュレーションは、図１１のデータテーブル２４を参照して、以下のように行うこともできる。シミュレーション部１５は、手先指令値Ａ１からＡｎまでを順次用いて、多関節マニピュレータの動作の仮想的な再現を行う。シミュレーション部１５は更に、角度指令値計算部において手先指令値Ａｉに基づく計算結果として、時刻Ｔｉにおける複数の関節の各々の角度指令値Ｂｉを得る。その角度指令値Ｂｉを、次の時刻Ｔｉ＋１における複数の関節の各々の現在角度データＣｉ＋１として設定することにより、図１２のフィードバック処理ＦＢが行われる。

このようなシミュレーションにより、多関節マニピュレータ１の手先５が現在の位置姿勢から最終目標位置姿勢に到達するまでの動作を近似的に再現することができる。そのため、その動作において各関節について可動範囲を外れる可能性があるか否かを確認することができる。

図１３は、コンピュータＤ１によって計算され表示装置Ｄ２に表示される多関節マニピュレータ画像６の一例を示す。実線は制御の開始位置、点線は制御の途中位置における多関節マニピュレータ画像６を示す。制御の開始位置において、手先５の位置姿勢が与えられる。更に、目標点３２における最終目標位置姿勢が設定される。更に、開始位置と目標点３２との間を繋ぐ軌道３１が設定される。

コンピュータＤ１は、これらの設定と、リンクパラメータや各関節Ｊ１〜Ｊ４の可動範囲などの多関節マニピュレータ１を定義する情報とを用いることにより、図１２に示したシミュレーションを実行する。その結果、手先５が軌道３１を目標点３２まで移動するときの多関節マニピュレータ１の運動を仮想的に再現することができる。

このようなシミュレーションにおいて、関節が可動範囲のリミットに達したか否かの判定が行われる。図８の障害関節表示部１６は、多関節マニピュレータ１のシミュレーション上の運動について、各関節Ｊ１〜Ｊ４が可動範囲に入っているか否かを判定する。可動範囲に入っている場合は、シミュレーションを続行する。いずれかの関節Ｊ１〜Ｊ４の角度指令値（図１１の角度指令値Ｂｉ）が可動範囲を超えた場合は、障害関節が発生したと判定する。障害関節が発生した場合、シミュレーションを停止し、その時点Ｔｉでの多関節マニピュレータ画像６を表示する。図１３における点線は、手先が停止点３３に到達したときに、関節（例えば関節Ｊ２）が可動範囲を超えたため、シミュレーションが停止された状態での多関節マニピュレータ画像６を示す。

このようなシミュレーションにおいて、図１０を用いて説明した障害回避処理を仮想的に再現することも可能である。

［軌道修正処理］
このようなシミュレーションにおいて、いずれかの関節Ｊ１〜Ｊ４の角度が可動範囲を超える場合、すべての関節Ｊ１〜Ｊ４の角度が可動範囲内に収まるように、軌道３１を修正することが望まれる。図１４は、多関節マニピュレータ１のシミュレーション方法と、その方法中で実行される軌道修正処理のフローチャートである。

図１０のステップＳ１と同様に、軌道データ生成部１４は、軌道データを生成する（ステップＳ２１）。図１０のステップＳ２〜Ｓ４と同様に、運動学計算により、各関節Ｊ１〜Ｊ４の角度指令値が計算される。但し、図１０の場合では実機から取得した関節角度の現在値を用いて角度指令値を計算したのに対し、図１４の場合には、図１２に示した方法により、疑似的に多関節マニピュレータ１の動作が再現され、多関節マニピュレータ画像６の動作として表示される（ステップＳ２２）。

障害回避制御部１３は、各関節Ｊ１〜Ｊ４について、ステップＳ２２で算出された角度指令値が可動範囲に入っているか否かを判定する。この判定は、安全域テーブル２５に登録された可動範囲の上限と下限の値を用いて行われる（ステップＳ２３）。

すべての関節Ｊ１〜Ｊ４について角度指令値が可動範囲内であった場合（ステップＳ２４Ｙｅｓ）、処理はステップＳ２７に移行する。関節Ｊ１〜Ｊ４のいずれかについて、時刻Ｔｉにおける角度指令値Ｂｉが可動範囲を超えた場合、処理はステップＳ２５に移行する（ステップＳ２４Ｎｏ）。

［ステップＳ２５：ＣＧ画面での停止］
関節Ｊ１〜Ｊ４のいずれかの角度指令値Ｂｉが可動範囲を超えた場合、シミュレーションが停止され、その時点での多関節マニピュレータ画像６が表示される。図１５は、関節Ｊ３の角度指令値Ｂｉが上限を超えた場合の表示例を示す。障害関節表示部１６は、角度がリミットに達した関節Ｊ３について、分かりやすいように、画面上でその関節を指示する障害関節表示を行う。例えば、障害関節を他の関節と区別する表示（例えば違う色での表示）がなされる。更に、関節Ｊ３の角度が可動範囲の一方側の端を超えたか他方側の端を超えたかを示す障害方向表示を行う。図１５の例では、障害方向表示として、関節Ｊ３の角度が下限に達したのかを矢印等により示す上限／下限表示３４が表示されている。

［ステップＳ２６：手先指令値の記憶］
次に、角度がリミットに達した関節Ｊ３について、リミットから外れるように、すなわち可動範囲の中心側に向かうように手先指令値を修正して記憶する。具体的には、手先の軌道上に新たに経由点を設定することにより、関節Ｊ３の角度が可動範囲に収まるようにする。図１６は、そのような経由点設定処理のフローチャートである。まず、停止点３３における手先位置が読み込まれる（ステップＳ３１）。次に、関節Ｊ３の角度が可動範囲の下限か上限かが判定される（ステップＳ３２）。

次に、図１７に例示されるような手前点３５を設定する。手前点３５は、例えば、時刻Ｔｉにおける角度指令値Ｂｉによってシミュレーションが停止した場合、それよりも所定期間だけ以前の時刻Ｔｉ−ｋ（ｋは１以上の整数）における手先指令値Ａｉ−ｋによって設定される（ステップＳ３３）。

次に、経由点３６が設定される。経由点３６は、修正を施す前の軌道３１と比べて、停止点３３でリミットに達した関節Ｊ３がリミットから離れる方向（図１７の離脱方向３７）となるように設定される。言い換えれば、関節Ｊ３がその可動範囲の中心に近い角度となるように経由点が設定される。

例えば、手先５が停止点３３に位置する時点で関節Ｊ３の角度が上限に達してした場合、他の全ての関節Ｊ１、Ｊ２、Ｊ４を固定して、関節Ｊ３の角度を所定の角度Δθだけ小さい値にしたときの手先５の位置が、経由点３６として設定される（ステップＳ３４）。手前点３５と経由点３６とを直線などの軌道３８で繋ぎ、経由点３６と目標点３２とを直線などの軌道３９で繋ぐことにより、修正された軌道が生成される。このような経由点３６の設定は、軌道修正部１７が自動的に行ってもよいし、作業者による入力操作によって行ってもよい（ステップＳ３５）。

軌道修正部１７は、修正された軌道３８、３９の手先指令値を記憶する（図１４のステップＳ２６）。以上の軌道修正処理は、いずれかの関節Ｊ１〜Ｊ４がリミットに達する度に実行される。そのため、２以上の経由点３６が設定されることもある。

修正された軌道が生成されると、ステップＳ２１からの処理が繰り返される。但し、以後の計算は、手前点３５の時点の軌道３１まで戻って、修正された軌道を用いて実行される。

図１４に戻って、ステップＳ２４において全ての関節Ｊ１〜Ｊ４の角度指令値が可動範囲内であった場合（ステップＳ２４Ｙｅｓ）、手先５が目標点３２（最終手先目標値）に到達したか否か判定される（ステップＳ２７）。手先５が目標点３２に到達していない場合は、ステップＳ２１からの処理が繰り返される（ステップＳ２７Ｎｏ）。手先５が目標点３２に到達した場合（ステップＳ２７Ｙｅｓ）、処理はステップＳ２８に移行する。

ステップＳ２６で軌道修正が行われた場合、経由点３６を通る軌道（図１７の例では手前点３５までは軌道３１、手前点３５以降では修正された軌道３８、３９）を連結することにより、修正された軌道を描く手先指令値が生成される（ステップＳ２８）。

コンピュータＤ１は、この手先指令値を多関節マニピュレータ１に実動作指令として送信する（ステップＳ２９）。シミュレーション上、関節Ｊ１〜Ｊ４が可動範囲を超えないように手先指令値が修正されているため、実機でも動作が停止せずに手先５が目標点３２まで到達できる見込みが高い。

図１４を用いて説明した軌道修正処理において、図１０を用いて説明した障害回避処理を適用することもできる。その場合、ステップＳ２３において関節角度指令値が可動範囲内か否かを判定する前に、ステップＳ５、Ｓ６における関節角度指令値が仮想リミット以内か否かの判定が行われる。関節角度指令値が仮想リミットを超えていた場合、ステップＳ７における障害回避処理が実行される。その後、ステップＳ２３以降の処理が行われる。

このような処理により、いずれかの関節の角度が仮想リミットに近づいた場合には、まず障害回避処理が実行される。それでもその関節が実際のリミットに到達した場合にのみ、ステップＳ２６での軌道修正処理が行われる。そのため、軌道修正処理が少なくなり、当初の軌道に近い自然な経路で手先５を目標点３２に導くことができる。

１ 多関節マニピュレータ
２ 基部
３ 支持部
４ エンドエフェクタ
５ 手先
６ 多関節マニピュレータ画像
１１ 記憶部
１２ 角度指令値計算部
１３ 障害回避制御部
１４ 軌道データ生成部
１５ シミュレーション部
１６ 障害関節表示部
１７ 軌道修正部
２１ 角度
２２ 安全域
２３ 角度
２４ データテーブル
２５ 安全域テーブル
３１ 軌道
３２ 目標点
３３ 停止点
３４ 上限／下限表示
３５ 手前点
３６ 経由点
３７ 離脱方向
３８ 軌道
３９ 軌道
１０１ 多関節マニピュレータ
１０２ 基部
１０３ 支持部
１０４ エンドエフェクタ
１０５ 手先
１１０ 目標点
１１１ 軌道
１２１〜１２３ 角度
Ｄ１ コンピュータ
Ｄ２ 表示装置
Ｊ１〜Ｊ６ 関節
Ｊ１０１〜Ｊ１０６ 関節
Ｌ１〜Ｌ５ リンク
Ｌ１０１〜Ｌ１０６ リンク
ＬＬ１０１〜ＬＬ１０３ 下限
ＵＬ１０１〜ＵＬ１０３ 上限
ＬＬ１ 下限
ＬＬ２ 仮想的な下限
ＵＬ１ 上限
ＵＬ２ 仮想的な上限

Claims (13)

1. 複数の関節を備える多関節マニピュレータのシミュレーション装置であって、
   前記複数の関節の各々について、角度の可動範囲と、前記可動範囲の内側に定義される安全域とを記憶装置に記憶する記憶部と、
   前記複数の関節の各々の現在の角度を示す現在角度データと、前記多関節マニピュレータの手先位置指令値とに基づいて、前記複数の関節の各々の角度指令値を生成する角度指令値計算部とを具備し、
   前記角度指令値計算部は、前記複数の関節のいずれかの内、前記角度指令値が前記安全域を超えた関節である停止候補関節が発生したとき、前記停止候補関節の前記角度指令値を生成する際に、前記角度指令値の変化速度を小さくする障害回避制御を行う
   シミュレーション装置。
2. 請求項１に記載されたシミュレーション装置であって、
   前記角度指令値計算部は、前記複数の関節の各々の角速度の指令値に係数を掛けて時間積分することによって前記角度指令値を生成し、
   前記障害回避制御は、前記係数の値を小さくすることによって実行される
   シミュレーション装置。
3. 請求項１又は２に記載されたシミュレーション装置であって、
   更に、前記多関節マニピュレータの手先を、現在手先位置から目標手先位置まで所定の軌道に沿って移動させるための軌道データであって、前記現在手先位置に近い第１番目の時刻Ｔ１における手先位置指令値Ａ１から前記目標手先位置に近い第ｎ番目の時刻Ｔｎにおける手先位置指令値Ａｎまでの一連の手先位置指令値Ａｉ（ｉは１以上ｎ以下の整数）からなる軌道データを生成する軌道データ生成部と、
   前記手先位置指令値Ａ１からＡｎまでを順次用いて、前記多関節マニピュレータの動作の仮想的な再現を行うシミュレーション部とを具備し、
   前記シミュレーション部は、
   前記角度指令値計算部において前記手先位置指令値Ａｉに基づく計算結果として、時刻Ｔｉにおける前記複数の関節の各々の角度指令値Ｂｉを得、
   前記角度指令値Ｂｉを、次の時刻Ｔｉ＋１における前記複数の関節の各々の前記現在角度データＣｉ＋１として設定する
   シミュレーション装置。
4. 請求項３に記載されたシミュレーション装置であって、
   更に、前記仮想的な再現において、前記角度指令値Ｂｉが前記可動範囲を超えた関節である障害関節が発生したとき、時刻Ｔｉにおける前記多関節マニピュレータを示すシミュレーション画像を表示し、且つ、前記シミュレーション画像において前記障害関節を指示する障害関節表示を行う障害関節表示部
   を具備するシミュレーション装置。
5. 請求項４に記載されたシミュレーション装置であって、
   前記可動範囲は、一方側の端と他方側の端とによって定義され、
   前記障害関節表示部は更に、前記障害関節が、前記一方側の端と前記他方側の端とのうちのいずれを超えたかを示す障害方向表示を行う
   シミュレーション装置。
6. 請求項３から５のいずれかに記載されたシミュレーション装置であって、
   更に、前記角度指令値Ｂｉが前記可動範囲を超えたとき前記軌道データに修正を施す軌道修正部を具備し、
   前記修正において、時刻Ｔｉよりも以前の時刻Ｔｉ−ｋ（ｋは１以上の整数）における前記手先位置指令値Ａｉ−ｋが、前記修正を施す前と比べて前記複数の関節のうち前記可動範囲を超えた関節の角度が前記可動範囲の中心側に近い角度となるように修正される
   シミュレーション装置。
7. 請求項１から６のいずれかに記載されたシミュレーション装置によって生成された前記角度指令値によって前記多関節マニピュレータを制御する制御装置。
8. 請求項７に記載された制御装置と、前記多関節マニピュレータとを具備するロボットシステム。
9. 複数の関節を備える多関節マニピュレータのシミュレーション方法であって、
   前記複数の関節の各々について、角度の可動範囲と、前記可動範囲の内側に定義される安全域とを記憶装置に記憶する工程と、
   前記複数の関節の各々の現在の角度を示す現在角度データと、前記多関節マニピュレータの手先位置指令値とに基づいて、前記複数の関節の各々の角度指令値を生成する工程とを具備し、
   前記角度指令値を生成する工程において、前記複数の関節のいずれかの内、前記角度指令値が前記安全域を超えた関節である停止候補関節が発生したとき、前記停止候補関節の前記角度指令値を生成する際に、前記角度指令値の変化速度を小さくする障害回避制御を行う
   シミュレーション方法。
10. 請求項９に記載されたシミュレーション方法であって、
    前記角度指令値を生成する工程において、前記複数の関節の各々の角速度の指令値に係数を掛けて時間積分することによって前記角度指令値を生成し、
    前記障害回避制御は、前記係数の値を小さくすることによって実行される
    シミュレーション方法。
11. 請求項９又は１０に記載されたシミュレーション方法であって、
    更に、前記多関節マニピュレータの手先を、現在手先位置から目標手先位置まで所定の軌道に沿って移動させるための軌道データであって、前記現在手先位置に近い第１番目の時刻Ｔ１における手先位置指令値Ａ１から前記目標手先位置に近い第ｎ番目の時刻Ｔｎにおける手先位置指令値Ａｎまでの一連の手先位置指令値Ａｉ（ｉは１以上ｎ以下の整数）からなる軌道データを生成する工程と、
    前記手先位置指令値Ａ１からＡｎまでを順次用いて、前記多関節マニピュレータの動作の仮想的な再現を行う工程とを具備し、
    前記仮想的な再現を行う工程は、
    前記角度指令値を計算する工程において前記手先位置指令値Ａｉに基づく計算結果として、時刻Ｔｉにおける前記複数の関節の各々の角度指令値Ｂｉを得る工程と、
    前記角度指令値Ｂｉを、次の時刻Ｔｉ＋１における前記複数の関節の各々の前記現在角度データＣｉ＋１として設定する工程とを具備する
    シミュレーション方法。
12. 請求項９から１１のいずれかに記載されたシミュレーション方法で生成された前記角度指令値によって前記多関節マニピュレータを制御する制御方法。
13. 請求項９から１１のいずれかに記載されたシミュレーション方法をコンピュータに実行させるプログラム。

Images