位置制御装置、位置制御方法及び位置制御プログラム
【課題】リンク機構の制御動作を安定化及び高信頼化することができるとともに、リンク機構を高精度に制御することができる位置制御装置を提供する。
【解決手段】データ変換処理部１３は、ロボットアーム２１の先端部の目標データＰから関節角度θを求める解法処理に使用される係数ａｉｊ及び係数ｂｉを浮動小数点データ形式から可変長整数データ形式へ変換し、無誤差制御処理部１６は、可変長整数データ形式の係数ａｉｊ及び係数ｂｉを用いて除算を除く加減算及び乗算からなる無誤差演算を実行して係数ａ６ｉｉ及び係数ｂ６ｉを求め、データ変換処理部１３は、係数ａ６ｉｉ及び係数ｂ６ｉを可変長整数データ形式から浮動小数点データ形式へ変換し、誤差有制御処理部１５は、上記解法処理において浮動小数点データ形式の係数ａ６ｉｉ及び係数ｂ６ｉを用いて１回のみの除算を含む浮動小数点演算を実行することにより浮動小数点データ形式の関節角度θを求める。
【解決手段】データ変換処理部１３は、ロボットアーム２１の先端部の目標データＰから関節角度θを求める解法処理に使用される係数ａｉｊ及び係数ｂｉを浮動小数点データ形式から可変長整数データ形式へ変換し、無誤差制御処理部１６は、可変長整数データ形式の係数ａｉｊ及び係数ｂｉを用いて除算を除く加減算及び乗算からなる無誤差演算を実行して係数ａ６ｉｉ及び係数ｂ６ｉを求め、データ変換処理部１３は、係数ａ６ｉｉ及び係数ｂ６ｉを可変長整数データ形式から浮動小数点データ形式へ変換し、誤差有制御処理部１５は、上記解法処理において浮動小数点データ形式の係数ａ６ｉｉ及び係数ｂ６ｉを用いて１回のみの除算を含む浮動小数点演算を実行することにより浮動小数点データ形式の関節角度θを求める。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、関節を介して複数のリンクを連結したリンク機構の所定部位、例えば、先端部を目標３次元位置及び目標方向に制御する位置制御装置、位置制御方法及び位置制御プログラムに関する。
【背景技術】
【０００２】
従来のロボットアームの制御方式としては、スレーブアームと同じ軸構成等を有するマスターアームを操作者が操作してスレーブアームを制御するマスターアーム方式、操作者の腕と手の主要部（各関節）にセンサを装着して操作者の腕と手の関節の動きを検出してロボットアームを制御するモーションキャプチャー方式、操作者の手首・手の甲等に付けた３次元位置方向センサにより操作者の手首・手の甲の３次元位置及び方向を検出し、このデータを用いてアーム（リンク）の各関節角度を求める逆変換を行ってロボットアームを制御する逆変換方式等の種々の方式がある。
【０００３】
上記のマスターアーム方式では、操作者側には、制御されるスレーブアームだけでなく、マスターアームが必要となり、システムが重装備になる。また、モーションキャプチャー方式では、操作者の腕と手の複数箇所にセンサを装着する必要があり、操作者にとって煩わしいものとなる。
【０００４】
一方、逆変換方式では、操作者は手の甲等に１個のセンサのみを装着するだけで済み、操作者にとって煩わしさがないが、操作者の手首の位置と方向データから各関節角度を求める逆変換が必要となる。この逆変換では、人の腕のような高い自由度（６自由度以上）を持つ軸構成における各関節角度を求める必要があるため、逆変換が非線形連立方程式となり、複雑な計算となる。
【０００５】
このため、本願発明者は、逆変換が非線形連立方程式とならず、単純且つ少ない演算量で厳密解を求めることができるロボットアーム・ハンドの操作方法を提案した（特許文献１参照）。この操作方法では、ロボットアームの操作者の手首に付けたセンサにより検出された３次元位置及び方向からアームの各関節角度を求めるために、浮動小数点演算を用いた誤差有り演算を用いている。この浮動小数点フォーマットを用いた実数表現は、非常に小さな数から大きな数まで表現でき、計算効率がよいという優れた特徴があるため、逆変換のような実数の数値計算に適している。
【特許文献１】特開２００５−４６９３１号公報
【発明の開示】
【発明が解決しようとする課題】
【０００６】
しかしながら、上記の浮動小数点演算を用いた数値制御では、以下のような問題がある。
（１）浮動小数点演算では、数値演算誤差が発生し、且つその蓄積が起こるため、演算結果に大きな誤差が含まれる場合があり、ロボットアームの先端位置が大幅にずれる場合がある。
（２）上記の数値演算誤差とその蓄積のために、当初想定していなかった状態が発生し、結果として制御が破綻してロボットアーム動作が暴走する状況が発生する。
（３）上記のロボットアーム動作の暴走を防止するためには、暴走対策を施す必要があり、制御が複雑化する。
【０００７】
本発明の目的は、リンク機構の制御動作を安定化及び高信頼化することができるとともに、リンク機構を高精度に制御することができる位置制御装置、位置制御方法及び位置制御プログラムを提供することである。
【課題を解決するための手段】
【０００８】
本発明に係る位置制御装置は、関節を介して複数のリンクを連結したリンク機構の所定部位を目標３次元位置及び目標方向に制御する位置制御装置であって、リンク機構の所定部位の目標３次元位置データ及び目標方向データからなる目標データをＰ、リンク機構の各関節角度をθとし、目標データＰから関節角度θを求める解法処理に使用されるデータを固定長数値データ形式から可変長整数データ形式へ変換する変換手段と、上記解法処理において、変換手段により変換された可変長整数データ形式のデータを用いて、除算を除く加減算及び乗算からなる無誤差演算を実行する無誤差演算手段とを備えるものである。
【０００９】
本発明に係る位置制御装置では、目標データＰから関節角度θを求める解法処理において、この解法処理に使用されるデータが固定長数値データ形式から可変長整数データ形式へ変換され、変換された可変長整数データ形式のデータを用いて、除算を除く加算、減算及び乗算からなる無誤差演算が実行される。この無誤差演算においては、数値演算誤差が発生せず、その蓄積もないため、リンク機構の制御動作を安定化及び高信頼化することができるとともに、リンク機構を高精度に制御することができる。
【００１０】
変換手段は、無誤差演算手段の演算結果を可変長整数データ形式から固定長数値データ形式へ変換し、上記位置制御装置は、上記解法処理において、変換手段により変換された固定長数値データ形式の演算結果を用いて１回のみの除算を含む固定長数値演算を実行することにより固定長数値データ形式の関節角度θを求める誤差演算手段をさらに備えることが好ましい。
【００１１】
この場合、目標データＰから関節角度θを求める解法処理において、可変長整数データ形式の係数データを用いて、除算を除く加算、減算及び乗算からなる無誤差演算が実行された後、この演算結果が可変長整数データ形式から固定長数値データ形式へ変換され、変換された固定長数値データ形式の演算結果を用いて１回のみの除算を含む固定長数値演算が実行され、固定長数値データ形式の関節角度θが算出されるので、上記解法処理において、数値演算誤差の発生を多くとも１回の除算によるものだけに制限して数値演算誤差を非常に小さくすることができるとともに、その蓄積を防止することができる。
【００１２】
無誤差演算手段は、θｉ＝θ’ｉ＋Δθｉ（ｉ＝１，…，ｎ）（ここで、θｉは関節ｉの求める関節角度、θ’ｉは微小時間Δｔ前の関節ｉの関節角度、Δθｉは微小時間Δｔ経過後の関節ｉの微小関節角度変化量、ｎは求める関節角度θの数）とし、ｎ元連立一次方程式ＡΔθ＝Ｂ（ここで、Ａはその要素ａｉｊ（ｉ＝１，…，ｎ、ｊ＝１，…，ｎ）がリンク機構のリンク長及び微小時間Δｔ前の関節ｉの関節角度θ’ｉにより決定される係数行列、Ｂはその要素ｂｉ（ｉ＝１，…，ｎ）が目標データＰ及び微小時間Δｔ前の位置方向データＰ’により決定されるベクトル）をＡｎΔθ＝Ｂｎ（ここで、Ａｎは対角要素ａｎ＋１ｉｉ（ｉ＝１，…，ｎ）以外が０となる対角行列、Ｂｎは要素ｂｎ＋１ｉ（ｉ＝１，…，ｎ）のベクトル）に変形したときの係数データａｎ＋１ｉｉ及び係数データｂｎ＋１ｉを、除算を除く加算、減算及び乗算からなる無誤差演算を用いて求め、変換手段は、無誤差演算手段により求められた係数データａｎ＋１ｉｉ及び係数データｂｎ＋１ｉを可変長整数データ形式から固定長数値データ形式へ変換し、誤差演算手段は、変換手段により変換された固定長数値データ形式の係数データａｎ＋１ｉｉ及び係数データｂｎ＋１ｉを用いて、θｉ＝θ’ｉ＋ｂｎ＋１ｉ／ａｎ＋１ｉｉ（ｉ＝１，…，ｎ）を固定長数値演算により求めることが好ましい。
【００１３】
この場合、目標データＰから関節角度θを求める解法処理において、θｉ＝θ’ｉ＋ｂｎ＋１ｉ／ａｎ＋１ｉｉ以外の演算を、除算を除く加算、減算及び乗算からなる無誤差演算により実行するとともに、数値演算誤差の発生を多くともθｉ＝θ’ｉ＋ｂｎ＋１ｉ／ａｎ＋１ｉｉにおける１回の除算だけに制限しているので、関節角度θを高精度に求めることができる。
【００１４】
上記位置制御装置は、操作者の所定部位の３次元位置及び方向を検出する検出手段をさらに備え、検出手段により検出された３次元位置及び方向をリンク機構の所定部位の目標３次元位置データ及び目標方向データとして用いて、リンク機構の所定部位を目標３次元位置及び目標方向に制御することが好ましい。
【００１５】
この場合、リンク機構の所定部位を操作者の所定部位の動きに応じた目標３次元位置及び目標方向に高精度に制御することができる。
【００１６】
本発明に係る位置制御方法は、関節を介して複数のリンクを連結したリンク機構の所定部位を目標３次元位置及び目標方向に制御するロボットアーム制御方法であって、リンク機構の所定部位の目標３次元位置データ及び目標方向データからなる目標データをＰ、リンク機構の各関節角度をθとし、目標データＰから関節角度θを求める解法処理に使用されるデータを固定長数値データ形式から可変長整数データ形式へ変換するステップと、上記解法処理において、変換された可変長整数データ形式のデータを用いて、除算を除く加算、減算及び乗算からなる無誤差演算を実行するステップとを含むものである。
【００１７】
本発明に係る位置制御プログラムは、関節を介して複数のリンクを連結したリンク機構の所定部位を目標３次元位置及び目標方向に制御するための位置制御プログラムであって、リンク機構の所定部位の目標３次元位置データ及び目標方向データからなる目標データをＰ、リンク機構の各関節角度をθとし、目標データＰから関節角度θを求める解法処理に使用されるデータを固定長数値データ形式から可変長整数データ形式へ変換する変換手段と、上記解法処理において、変換手段により変換された可変長整数データ形式の係数データを用いて、除算を除く加算、減算及び乗算からなる無誤差演算を実行する無誤差演算手段としてコンピュータを機能させるものである。
【発明の効果】
【００１８】
本発明によれば、目標データＰから関節角度θを求める解法処理において、可変長整数データ形式のデータを用いて、除算を除く加算、減算及び乗算からなる無誤差演算が実行され、この無誤差演算においては、数値演算誤差が発生せず、その蓄積もないため、リンク機構の制御動作を安定化及び高信頼化することができるとともに、リンク機構を高精度に制御することができる。
【発明を実施するための最良の形態】
【００１９】
以下、本発明の一実施の形態による位置制御装置の一例であるロボットアーム制御装置をいたロボットアーム制御システムについて図面を参照しながら説明する。
【００２０】
図１は、本発明の一実施の形態によるロボットアーム制御装置を用いたロボットアーム制御システムの構成を示すブロック図である。図１に示すロボットアーム制御システムは、ロボットアーム制御装置１、ロボットアーム部２、３次元操作グローブ３、３次元位置方向センサ４及び３次元ディスプレイ５を備える。
【００２１】
ロボットアーム部２は、一対のロボットアーム２１及びハンド２２、胴体部２３並びにカメラ部２４を備え、人の上半身（頭部を除く）と同程度の大きさと自由度とを有し、両腕と両手の機能を実現している。
【００２２】
ロボットアーム２１は、肩、肘及び手首に関節を有し、これらの関節を介してリンクとなる上腕２１ａ及び前腕２１ｂが連結されたリンク機構（マニプレータ）であり、人の腕と同程度の大きさ及び自由度を有する。ハンド２２は、人の手と同程度の大きさと自由度を有する触覚付きの５本指ロボットハンドであり、前腕２１ｂの先端に取り付けられる。ハンド２２には触覚センサ（図示省略）が設けられており、物をつかんだ時の触覚（圧力）がロボットアーム制御装置１を介して３次元操作グローブ３へ伝達される。
【００２３】
なお、図示を省略しているが、ロボットアーム２１及びハンド２２は、その内部にモータ等の駆動源を有し、この駆動源により任意の角度に駆動される。また、本実施の形態では、ロボットアーム２１を人の腕に模倣させ、ハンド２２を人の手指に模倣させているが、ロボットアーム及びハンドの構成は、この例に特に限定されず、ハンド２２の指の数を増減させる等の種々の変更が可能である。また、リンク機構の関節数及びリンク数も、上記の例に特に限定されず、種々の変更が可能である。
【００２４】
カメラ部２４は、３次元カメラ（ステレオカメラ）等から構成され、ロボットアーム制御装置１を介して、撮影した画像を３次元ディスプレイ５へ出力する。３次元ディスプレイ５は、カメラ部２４により撮影された画像を表示し、操作者Ｍに提示する。なお、撮影された画像の提示方法は、この例に特に限定されず、３次元ディスプレイに替えて、操作者Ｍに装着させたヘッドマウントディスプレイに表示させる等の種々の変更が可能である。
【００２５】
３次元操作グローブ３は、公知の３次元操作用データグローブから構成され、操作者Ｍの両手にそれぞれ装着される。３次元操作グローブ３は、操作者Ｍの手の甲及び指の動き（関節角度）を検出し、検出した関節角度データをロボットアーム制御装置１へ出力する。３次元操作グローブ３の手首の位置には、３次元位置方向センサ４が取り付けられ、３次元位置方向センサ４は、前腕２１ｂの先端部の目標３次元位置及び目標方向として、操作者Ｍの手首の３次元位置及び方向を検出し、検出した３次元位置データ及び方向データをロボットアーム制御装置１へ出力する。
【００２６】
なお、目標３次元位置及び目標方向を検出又は入力する方法は、上記の例に特に限定されず、データグローブ又はデータスーツ等から操作者の所定部位の３次元位置及び方向を検出したり、ハプティクス・デバイス等を用いて入力したりしてもよい。
【００２７】
ロボットアーム制御装置１は、操作者Ｍの両手に装着された３次元操作グローブ３及び３次元位置方向センサ４からの検出結果を基に、ロボットアーム２１及びハンド２２の各関節角度を算出する。ハンド２２に関しては、３次元操作グローブ３が手の甲の曲がり（手首の）角度及び指の関節角度を検出し、ロボットアーム制御装置１は、これらの角度データに基づいてハンド２２を制御する。
【００２８】
一方、ロボットアーム２１に関しては、操作者Ｍの手首に装着した３次元位置方向センサ４から操作者Ｍの手首の三次元位置及びその方向、すなわち、ロボットアーム２１の先端部の目標３次元位置及び目標方向が得られるだけである。このため、ロボットアーム２１の先端部の目標３次元位置データ及び目標方向データから、ロボットアーム２１の各関節角度を計算する必要がある。このように、ロボットアーム２１の先端部の３次元位置及び方向からロボットアーム２１の各関節角度を計算することを逆変換と呼び、これとは逆に、ロボットアーム２１の各関節角度からロボットアーム２１の先端部の３次元位置及び方向を計算することを順変換と呼ぶことにする。
【００２９】
図２は、図１に示すロボットアーム制御装置１の構成を示すブロック図である。図２に示すように、ロボットアーム制御装置１は、外部インターフェース部１１、浮動小数点データ記憶部１２、データ変換処理部１３、可変長整数データ記憶部１４、誤差有制御処理部１５及び無誤差制御処理部１６を備える。誤差有制御処理部１５は、座標変換処理部１５ａ及び浮動小数点演算部１５ｂを備え、無誤差制御処理部１６は、方程式解法処理部１６ａ及び可変長整数演算部１６ｂを備える。
【００３０】
ロボットアーム制御装置１は、ＣＰＵ（中央演算処理装置）、ＲＯＭ（リードオンリメモリ）、ＲＡＭ（ランダムアクセスメモリ）、外部記憶装置、記録媒体駆動装置、入力装置、表示装置、外部機器インターフェース等を備えるコンピュータから構成することができる。この場合、後述するロボットアーム制御処理（位置制御処理）を実行するためのロボットアーム制御プログラム（位置制御プログラム）を、ＣＰＵ等を用いて実行することにより、外部インターフェース部１１、浮動小数点データ記憶部１２、データ変換処理部１３、可変長整数データ記憶部１４、誤差有制御処理部１５及び無誤差制御処理部１６としてコンピュータを機能させることができる。なお、ロボットアーム制御装置１の構成は、この例に特に限定されず、上記の各機能の一部又は全部を専用のハードウエアから構成する等の種々の変更が可能である。
【００３１】
外部インターフェース部１１は、３次元位置方向センサ４から３次元位置データ及び方向データを取得して浮動小数点データ記憶部１２に格納する。また、外部インターフェース部１１は、ロボットアーム制御処理により求められた関節角度を浮動小数点データ記憶部１２から読み出してロボットアーム部２へ出力し、ロボットアーム２１の各関節角度がロボットアーム制御処理により求められた関節角度に制御される。
【００３２】
浮動小数点データ記憶部１２は、誤差有制御処理部１５において処理される種々のデータを浮動小数点データ形式で記憶し、可変長整数データ記憶部１４は、無誤差制御処理部１６において処理される種々のデータを可変長整数データ形式で記憶する。
【００３３】
図３は、浮動小数点データ及び可変長整数データのデータ構成の一例を示す図である。図３の（ａ）に示すように、固定長数値データの一例である浮動小数点データは、符号部、指数部及び仮数部から構成され、このようなデータ形式で浮動小数点データ記憶部１２に格納される。また、図３の（ｂ）に示すように、可変長整数データは、語長部、符号部、及び数値部（整数表現）から構成され、このようなデータ形式で可変長整数データ記憶部１４に格納される。
【００３４】
データ変換処理部１３は、浮動小数点データ記憶部１２から浮動小数点データを読み出し、読み出した浮動小数点データを可変長整数データへ変換し、変換した可変長整数データを可変長整数データ記憶部１４に格納する。また、データ変換処理部１３は、可変長整数データ記憶部１４から可変長整数データを読み出し、読み出した可変長整数データを浮動小数点データへ変換し、変換した浮動小数点データを浮動小数点データ記憶部１２に格納する。
【００３５】
なお、本発明に用いられる固定長数値データは、上記の例に特に限定されず、例えば、固定長整数データを用いることもできる。図３の（ｃ）に示すように、固定長整数データは、符号部及び数値部（整数表現）から構成され、この固定長整数データ形式も、固定長であるため、誤差（数値のオーバーフロー及びそのまるめによる誤差）が発生し、本発明を同様に適用することができる。
【００３６】
例えば、３次元位置方向センサ４が出力する値（角度）が、−２５５〜＋２５５等の整数値であり、ロボットアーム２１の関節を動作させるモータに対する出力（回転角度）も、−１０００〜＋１０００パルス等の整数値である場合、浮動小数点データ記憶部１２を固定長数値データ記憶部に変更し、固定長整数データ形式で各データを格納するようにしてもよい。また、固定長整数データ形式でデータを取り扱う場合、浮動小数点演算部１５ｂを固定長整数演算部に変更し、固定長整数演算部により固定長整数演算を実行するようにしてもよい。
【００３７】
誤差有制御処理部１５は、ロボットアーム部２のロボットアーム２１及びハンド２２の制御処理のうち、固定長数値演算の一例である浮動小数点演算を用いた誤差有制御処理を実行する。座標変換処理部１５ａは、浮動小数点データ記憶部１２から浮動小数点データを読み出し、浮動小数点演算部１５ｂを用いて座標変換処理等（ｓｉｎ、ｃｏｓ演算）を浮動小数点演算で実行し、演算結果を浮動小数点データ記憶部１２に格納する。浮動小数点演算部１５ｂは、加算器、減算器、乗算器及び除算器としての機能を有し、浮動小数点演算を用いて、加算、減算、乗算及び除算を実行する。
【００３８】
無誤差制御処理部１６は、ロボットアーム部２のロボットアーム２１及びハンド２２の制御処理のうち、可変長整数演算を用いた無誤差制御処理を実行する。方程式解法処理部１６ａは、可変長整数データ記憶部１４から可変長整数データを読み出し、制御方程式の求解等を可変長整数演算部１６ｂを用いて可変長整数演算で実行し、演算結果を可変長整数データ記憶部１４に格納する。可変長整数演算部１６ｂは、加算器、減算器及び乗算器としての機能を有し、可変長整数演算を用いて、加算、減算及び乗算を無誤差で実行する。
【００３９】
ここで、本実施の形態の制御対象となるロボットアーム２１について詳細に説明する。図４は、図１に示すロボットアーム部２における右側のロボットアーム２１の構成を示す模式図であり、図５は、図１に示す右側のロボットアーム２１の座標系を説明するための図である。なお、左側のロボットアーム２１も、図４及び図５に示す右側のロボットアーム２１と同様に構成されているので、詳細な説明は省略する。
【００４０】
図４に示すように、グローバル座標系の原点Ｏを上腕２１ａの右肩（第１関節）に、グローバル座標系のＸ座標軸を胴体部２３（図１参照）の正面方向に沿って床面と平行方向に、グローバル座標系のＹ座標軸を右肩から左肩方向に、グローバル座標系のＺ座標軸を床面から天井への垂直方向にそれぞれ設定する。また、ロボットアーム２１の上腕２１ａの長さをｅ１、前腕２１ｂの長さをｅ２とし、上腕２１ａの肩関節のうち第１関節（Ｙ軸回り）の関節角度θ１、第２関節（Ｘ軸回り）の関節角度θ２、第３関節（Ｚ軸回り）の関節角度θ３とし、上腕２１ａと前腕２１ｂとの間の第４関節（Ｙ軸回り）の関節角度θ４、第５関節（Ｚ軸回り）の関節角度θ５とし、前腕２１ｂとハンド２２との間の第６関節（Ｙ軸回り）の関節角度θ６とする。
【００４１】
このとき、前腕２１ｂの先端位置・方向Ｐｓすなわち操作者Ｍの手首に装着した３次元位置方向センサ４が示すグローバル座標に対する変換行列をＨ０ｓとすると、以下の式（１）のように表現することができる。
【００４２】
【数１】
【００４３】
ここで、図５に示すように、３次元位置方向センサ４の３次元位置はＰｓ（ｘｓ，ｙｓ，ｚｓ）、３次元位置方向センサ４の方向を表すＸ軸方向の単位ベクトルはｄｘｓ（ｕｘｓ，ｖｘｓ，ｗｘｓ）、Ｙ軸方向の単位ベクトルはｄｙｓ（ｕｙｓ，ｖｙｓ，ｗｙｓ）、Ｚ軸方向の単位ベクトルはｄｚｓ（ｕｚｓ，ｖｚｓ，ｗｚｓ）となる。これらの値はすべて、３次元位置方向センサ４から取得されるので、既知となる。一方、Ｈ０ｓの各要素は、各アーム（上腕２１ａ及び前腕２１ｂ）の軸の長さｅ１，ｅ２と各関節角度θ１，θ２，θ３，θ４，θ５を用いて、以下のように表現することができる。
【００４４】
【数２】
【００４５】
ここで、Ｃｎ＝ｃｏｓ（θｎ）、Ｓｎ＝ｓｉｎ（θｎ）（ｎ＝１，…，５）であり、関節角度θｎは、アーム動作により刻々と変化するので、時間ｔの関数となり、θｎ’を微小時間Δｔ前の関節角度（直前の関節角度）とし、微小時間Δｔ経過後における微小関節角度変化量をΔθｎとすると、以下のように表現することができる。
θｎ＝θｎ’＋Δθｎ （６）
ここで、微小時間Δｔ前の関節角度値θｎ’は既知であり、微小時間Δｔは制御時間間隔である。
【００４６】
上記の前提を基に、以下のように近似することができる。なお、下記式では、（Δθ）２は微小であるので、（Δθ）２以上の高次項は無視できるとした。
ｓｉｎ（θ）＝ｓｉｎ（θ’＋Δθ）≒ｓｉｎ（θ’）＋（ｓｉｎ（θ’））’Δθ＝ｓｉｎ（θ’）＋ｃｏｓ（θ’）Δθ （７ａ）
ｃｏｓ（θ）＝ｃｏｓ（θ’＋Δθ）≒ｃｏｓ（θ’）＋（ｃｏｓ（θ’））’Δθ＝ｃｏｓ（θ’）−ｓｉｎ（θ’）Δθ （７ｂ）
上記の式（７ａ）、（７ｂ）を式（２）〜（５）へ代入すると、以下となる。なお、ここでも、Δθ１Δθ２等の高次のΔθは微小であり、無視できるとした。
【００４７】
【数３】
【００４８】
【数４】
【００４９】
【数５】
【００５０】
【数６】
【００５１】
上記各式より、式（１）は、以下のように表現することができる。
【００５２】
【数７】
【００５３】
そこで、Δθ１，Δθ２，Δθ３，Δθ４，Δθ５は、以下の５元連立一次方程式の解として求めることができる。
【００５４】
【数８】
【００５５】
また、上式の左辺の行列の各要素を係数ａｉｊ（ｉ＝１，…，５、ｊ＝１，…，５）、右辺のベクトルの各要素を係数ｂｉ（ｉ＝１，…，５）で表すと、以下のように表現することができる。
【００５６】
【数９】
【００５７】
上記の５元連立一次方程式の解法では、加減乗除算が繰り返されるために、浮動小数点演算を用いた数値計算では、数値演算誤差とその蓄積のために解の誤差が大きくなり、連立方程式の解を求める処理が演算誤差のために暴走及び破綻することが多々ある。このように解の誤差が大きくなること、及び連立方程式の解を求める処理が暴走及び破綻することは、ロボットアームが人に危害を加える恐れがあるため、ロボットアームの制御においては許されない。また、除算を使用する場合、数値計算に計算精度の低下等の様々な悪影響を及ぼすことはもちろんのこと、無誤差演算自体を行うことはできない。
【００５８】
このため、本実施の形態では、ガウス・ジョルダンの消去法を基本にし、最後の１回を除いて除算が発生しない無誤差連立一次方程式解法処理を考案し、式（１５）の解法に誤差を発生させない無誤差演算を用い、各関節角度θ１，θ２，θ３，θ４，θ５を以下のようにして求めている。
【００５９】
まず、式（１５）の係数ａｉｊ（ｉ＝１，…，５、ｊ＝１，…，５）及び係数ｂｉ（ｉ＝１，…，５）を用いて、ｋ＝１，…，５として、下記の式を繰り返し演算することにより係数ａ６ｉｊ及び係数ｂ６ｉを求める。
ａｋ＋１ｉｊ＝ａｋｉｊ （ｉ＝ｋ） （１６ａ）
ａｋ＋１ｉｊ＝ａｋｉｊａｋｋｋ−ａｋｋｊａｋｉｋ （ｉ≠ｋ） （１６ｂ）
ｂｋ＋１ｉ＝ｂｋｉ （ｉ＝ｋ） （１６ｃ）
ｂｋ＋１ｉ＝ｂｋｉａｋｋｋ−ｂｋｋａｋｉｋ （ｉ≠ｋ） （１６ｄ）
上記の係数ａ６ｉｊ及び係数ｂ６ｉにより、式（１５）は以下の形式となる。
【００６０】
【数１０】
【００６１】
上記のように、式（１７）の左辺の係数行列は、対角要素以外が０となる対角行列となる。上記の演算までは、除算を使用する必要がないため、本実施の形態では、式（１５）の連立一次方程式の解法処理のうち上記の演算までの処理を、無誤差制御処理部１６による無誤差演算を用いて実行し、ここまでの演算を無誤差で行っている。
【００６２】
次に、各関節角度θ１，θ２，θ３，θ４，θ５は、以下の式により求めることができる。
【００６３】
【数１１】
【００６４】
上記の式（１８）の演算だけは、除算が回避できないために、本実施の形態では、誤差有制御処理部１５による浮動小数点演算を用いて実行している。この浮動小数点演算により誤差が発生するが、この処理は最終処理であり、且つ１回の除算を行うだけであるため、除算に充分な精度が確保されていれば、誤差が問題になることはなく、誤差の蓄積も起こらない。なお、手首の角度θ６は、３次元操作グローブ３により直接取得することができるので、上記の無誤差連立一次方程式解法処理からは求めていない。
【００６５】
ロボットアーム制御装置１は、上記の無誤差連立一次方程式解法処理（目標データＰから関節角度θを求める解法処理）を以下のようにして実行している。
【００６６】
外部インターフェース部１１は、ロボットアームの２１の先端部の目標３次元位置（３次元位置方向センサ４の３次元位置Ｐｓ）及び目標方向（Ｘ、Ｙ、Ｚ軸方向の単位ベクトルｄｘｓ、ｄｙｓ、ｄｚｓ）からなる目標データＰを３次元位置方向センサ４から取得し、浮動小数点データ記憶部１２に浮動小数点データ形式で記憶させる。このとき、浮動小数点データ記憶部１２には、微小時間Δｔ前の位置方向データＰ’及び微小時間Δｔ前の関節角度データθ’ｉが記憶されており、また、上腕２１ａの長さｅ１、前腕２１ｂの長さｅ２等の初期値は予め記憶されているものとする。
【００６７】
ここで、上記の微小時間Δｔ前の位置方向データＰ’は、微小時間Δｔ前の目標データではなく、微小時間Δｔ前の実際の３次元位置データ及び方向データであり、微小時間Δｔ前の関節角度データθ’ｉもアームの実際の関節角度である。これは、後述するように、Ｐ’、θ’及びＰからΔθを求め（線形近似後、連立一次方程式を無誤差で解く）、そして、θ＝θ’＋Δθからθを求めており、ここで（線形）近似しているため、このθから求めた実際の位置方向データは目標データと一致せず、次の時刻の計算では、微小時間Δｔ前の目標データ等ではなく、微小時間Δｔ前の実際の３次元位置データ及び方向データ、並びに微小時間Δｔ前の実際の関節角度データを用いているためである。
【００６８】
座標変換処理部１５ａは、浮動小数点データ記憶部１２から微小時間Δｔ前の関節角度θ’ｉ、上腕２１ａの長さｅ１、前腕２１ｂの長さｅ２を読み出し、浮動小数点演算部１５ｂを用いて浮動小数点演算を実行することにより式（１５）の係数データａｉｊを算出し、浮動小数点データ記憶部１２に記憶させる。また、座標変換処理部１５ａは、浮動小数点データ記憶部１２から目標データＰ及び微小時間Δｔ前の位置方向データＰ’を読み出し、浮動小数点演算部１５ｂを用いて浮動小数点演算を実行することにより式（１５）の係数データｂｉを算出し、浮動小数点データ記憶部１２に記憶させる。
【００６９】
データ変換処理部１３は、浮動小数点データ記憶部１２から係数ａｉｊ及び係数ｂｉを順次読み出し、浮動小数点データ形式から可変長整数データ形式に変換した係数ａｉｊ及び係数ｂｉを、係数ａ１ｉｊ及び係数ｂ１ｉとして可変長整数データ記憶部１４に格納する。
【００７０】
方程式解法処理部１６ａは、可変長整数データ記憶部１４から係数ａ１ｉｊ及び係数ｂ１ｉを読み出し、可変長整数演算部１６ｂを用いて可変長整数演算（無誤差演算）を実行することにより、式（１６ａ）乃至（１６ｄ）を用いて係数ａｋ＋１ｉｊ及びｂｋ＋１ｉを順次算出し、最終的に係数ａ６ｉｊ及びｂ６ｉを可変長整数データ記憶部１４に格納する。
【００７１】
データ変換処理部１３は、可変長整数データ記憶部１４から係数ａ６ｉｉ及び係数ｂ６ｉを順次読み出し、可変長整数データ形式から浮動小数点データ形式に変換した係数ａ６ｉｉ及び係数ｂ６ｉを浮動小数点データ記憶部１２に格納する。
【００７２】
座標変換処理部１５ａは、浮動小数点データ記憶部１２から係数ａ６ｉｉ及び係数ｂ６ｉを順次読み出し、浮動小数点演算部１５ｂを用いて浮動小数点演算の除算を１回だけ実行することにより、微小関節角度変化量Δθｉ＝ｂ６ｉ／ａ６ｉｉを算出し、浮動小数点データ記憶部１２に格納する。そして、座標変換処理部１５ａは、浮動小数点データ記憶部１２から直前の関節角度値θ’ｉ及び微小関節角度変化量Δθｉを順次読み出し、浮動小数点演算部１５ｂを用いて浮動小数点演算を実行することにより式（１８）を用いて関節角度θｉを算出し、浮動小数点データ記憶部１２に格納する。
【００７３】
外部インターフェース部１１は、浮動小数点データ記憶部１２から関節角度θｉを読み出してロボットアーム部２へ出力する。ロボットアーム部２は、ロボットアーム２１の各関節角度が関節角度θｉとなるようにロボットアーム２１を動作させる。
【００７４】
上記の制御動作により、操作者Ｍは、３次元ディスプレイ５を用いて、カメラ部２４により撮影された画像を見ながら、両手に装着した３次元操作グローブ３を用いて、ロボットアーム２１及びハンド２２を操作し、操作者Ｍの腕と手の動きとがロボットアーム２１及びハンド２２により忠実に再現される。
【００７５】
本実施の形態では、データ変換処理部１３が変換手段の一例に相当し、無誤差制御処理部１６（方程式解法処理部１６ａ及び可変長整数演算部１６ｂ）が無誤差演算手段の一例に相当し、誤差有制御処理部１５（座標変換処理部１５ａ及び浮動小数点演算部１５ｂ）が誤差演算手段の一例に相当する。
【００７６】
次に、上記のように構成されたロボットアーム制御装置１によるロボットアーム制御処理について説明する。図６は、図１に示すロボットアーム制御装置１によるロボットアーム制御処理を説明するためのフローチャートである。
【００７７】
図６に示すように、まず、ステップＳ１１において、外部インターフェース部１１は、３次元位置方向センサ４から３次元位置データＰｓ（ｘｓ，ｙｓ，ｚｓ）、Ｘ軸方向の単位ベクトルｄｘｓ（ｕｘｓ，ｖｘｓ，ｗｘｓ）、Ｙ軸方向の単位ベクトルｄｙｓ（ｕｙｓ，ｖｙｓ，ｗｙｓ）、及びＺ軸方向の単位ベクトルｄｚｓ（ｕｚｓ，ｖｚｓ，ｗｚｓ）を取得して浮動小数点データ記憶部１２に格納する。
【００７８】
次に、ステップＳ１２において、座標変換処理部１５ａは、浮動小数点データ記憶部１２から３次元位置データＰｓ（ｘｓ，ｙｓ，ｚｓ）等を順次読み出し、浮動小数点演算部１５ｂを用いて、式（１５）の係数ａｉｊ（ｉ＝１，…，５、ｊ＝１，…，５）及び係数ｂｉ（ｉ＝１，…，５）を求め、浮動小数点データ記憶部１２に格納する。
【００７９】
次に、ステップＳ１３において、データ変換処理部１３は、浮動小数点データ記憶部１２から係数ａｉｊ及び係数ｂｉを順次読み出し、浮動小数点データ形式から可変長整数データ形式に変換した係数ａｉｊ及び係数ｂｉを、係数ａ１ｉｊ及び係数ｂ１ｉとして可変長整数データ記憶部１４に格納する。
【００８０】
なお、上記のステップＳ１２及びＳ１３では、浮動小数点演算を用いて３次元位置データＰｓ（ｘｓ，ｙｓ，ｚｓ）等から係数ａｉｊ及び係数ｂｉを求めた後に、可変長整数データへフォーマット変換したが、この例に特に限定されず、データ変換処理部１３により３次元位置データＰｓ（ｘｓ，ｙｓ，ｚｓ）、Ｘ軸方向の単位ベクトルｄｘｓ（ｕｘｓ，ｖｘｓ，ｗｘｓ）、Ｙ軸方向の単位ベクトルｄｙｓ（ｕｙｓ，ｖｙｓ，ｗｙｓ）、及びＺ軸方向の単位ベクトルｄｚｓ（ｕｚｓ，ｖｚｓ，ｗｚｓ）を浮動小数点データから可変長整数データに変換し、その後、方程式解法処理部１６ａにより可変長整数データの３次元位置データＰｓ（ｘｓ，ｙｓ，ｚｓ）等から可変長整数データの係数ａｉｊ及び係数ｂｉを求めるようにしてもよい。
【００８１】
次に、ステップＳ１４〜Ｓ１６において、方程式解法処理部１６ａは、初期設定として、ｋ＝１、ｉ＝１、ｊ＝１に設定する。次に、ステップＳ１７において、方程式解法処理部１６ａは、可変長整数データ記憶部１４から係数ａｋｉｊ，ａｋｋｋ，ａｋｋｊ，ａｋｉｋを読み出し、可変長整数演算部１６ｂを用いて、ｉ＝ｋの場合はａｋ＋１ｉｊ＝ａｋｉｊに設定し、ｉ≠ｋの場合はａｋ＋１ｉｊ＝ａｋｉｊａｋｋｋ−ａｋｋｊａｋｉｋを算出し、係数ａｋ＋１ｉｊを可変長整数データ記憶部１４に格納する。
【００８２】
次に、ステップＳ１８において、方程式解法処理部１６ａは、ｊ＜５であるか否かを判断し、ｊ＜５の場合、ステップＳ２７においてｊを１だけインクリメントしてステップＳ１７以降の処理を継続し、ｊ＜５でない（ｊ＝５）場合、ステップＳ１９へ処理を移行する。
【００８３】
ｊ＜５でない場合、ステップＳ１９において、方程式解法処理部１６ａは、可変長整数データ記憶部１４から係数ｂｋｉ，ａｋｋｋ，ｂｋｋ，ａｋｉｋを読み出し、可変長整数演算部１６ｂを用いて、ｉ＝ｋの場合はｂｋ＋１ｉ＝ｂｋｉに設定し、ｉ≠ｋの場合はｂｋ＋１ｉ＝ｂｋｉａｋｋｋ−ｂｋｋａｋｉｋを算出し、係数ｂｋ＋１ｉを可変長整数データ記憶部１４に格納する。
【００８４】
次に、ステップＳ２０において、方程式解法処理部１６ａは、ｉ＜５であるか否かを判断し、ｉ＜５の場合、ステップＳ２６においてｉを１だけインクリメントしてステップＳ１６以降の処理を継続し、ｉ＜５でない（ｉ＝５）場合、ステップＳ２１へ処理を移行する。
【００８５】
ｉ＜５でない場合、ステップＳ２１において、方程式解法処理部１６ａは、ｋ＜５であるか否かを判断し、ｋ＜５の場合、ステップＳ２５においてｋを１だけインクリメントしてステップＳ１５以降の処理を継続し、ｋ＜５でない（ｋ＝５）場合、ステップＳ２２へ処理を移行する。このように、ステップＳ１４〜Ｓ２１及びＳ２５〜Ｓ２７までの処理が無誤差演算を用いて実行される。
【００８６】
次に、ステップＳ２２において、データ変換処理部１３は、可変長整数データ記憶部１４から係数ａ６ｉｉ及び係数ｂ６ｉ（ｉ＝１，…，５）を順次読み出し、可変長整数データ形式から浮動小数点データ形式に変換し、変換した係数ａ６ｉｉ及び係数ｂ６ｉを浮動小数点データ記憶部１２に格納する。
【００８７】
次に、ステップＳ２３において、座標変換処理部１５ａは、浮動小数点データ記憶部１２から係数ａ６ｉｉ及び係数ｂ６ｉ（ｉ＝１，…，５）を順次読み出し、浮動小数点演算部１５ｂを用いて、微小関節角度変化量Δθｉ＝ｂ６ｉ／ａ６ｉｉ（ｉ＝１，…，５）を算出し、浮動小数点データ記憶部１２に格納する。
【００８８】
次に、ステップＳ２４において、座標変換処理部１５ａは、浮動小数点データ記憶部１２から直前の関節角度値θ’ｉ及び微小関節角度変化量Δθｉ（ｉ＝１，…，５）を順次読み出し、浮動小数点演算部１５ｂを用いて、式（１８）に従って関節角度θｉ＝θ’ｉ＋Δθｉ（ｉ＝１，…，５）を算出し、浮動小数点データ記憶部１２に格納する。このとき、外部インターフェース部１１は、浮動小数点データ記憶部１２から関節角度θ１，θ２，θ３，θ４，θ５を読み出してロボットアーム部２へ出力する。
【００８９】
ロボットアーム部２は、ロボットアーム２１の各関節角度が関節角度θ１，θ２，θ３，θ４，θ５となるようにロボットアーム２１を動作させる。なお、手首の関節角度θ６は、３次元操作グローブ３から直接取得することができるので、３次元操作グローブ３により検出された関節角度θ６がロボットアーム制御装置１を介してロボットアーム部２へ出力され、ロボットアーム部２は、ロボットアーム２１の手首の関節角度が関節角度θ６となるようにロボットアーム２１を動作させる。上記の処理が制御時間間隔ごとに繰り返され、ロボットアーム２１の先端位置が操作者Ｍの手首の位置に応じて正確に制御される。
【００９０】
上記の処理により、本実施の形態では、ロボットアーム制御処理において用いられる逆変換制御演算（連立一次方程式解法）に使用される数値を可変長の整数により表現するとともに、除算を実行することなく（最後の除算を除く）、加算、減算及び乗算のみを用いた無誤差演算処理を実行し、制御演算誤差をなくすことができる。
【００９１】
この結果、ロボットアーム２１の先端部を高精度に制御することができるとともに、制御処理の破綻をなくしてロボットアーム２１の動作の暴走等をなくすことができ、ロボットアーム２１の動作の安定化及び高信頼化を図ることができる。また、ロボットアーム２１の暴走防止対策等が不要となるので、ロボットアーム制御装置１の構成を単純化することができ、装置の低コスト化を実現することができる。
【００９２】
なお、本実施の形態では、操作者の手首の位置及び方向を検出し、これらを目標位置及び目標方向として制御したが、本発明による制御例は、この例に特に限定されず、３次元操作グローブ３、３次元位置方向センサ４及び３次元ディスプレイ５を省略し、テーチングを行うことにより浮動小数点データ記憶部１２に記憶した目標位置及び目標方向となるようにロボットアーム２１を制御する等の他の制御例にも適用可能である。また、ロボットアーム２１を制御対象としたが、この例に特に限定されず、ハンド２２を制御対象にする等の種々の変更が可能である。
【図面の簡単な説明】
【００９３】
【図１】本発明の一実施の形態によるロボットアーム制御装置を用いたロボットアーム制御システムの構成を示すブロック図である。
【図２】図１に示すロボットアーム制御装置の構成を示すブロック図である。
【図３】浮動小数点データ及び可変長整数データのデータ構成の一例を示す図である。
【図４】図１に示すロボットアーム部における右側のロボットアームの構成を示す模式図である。
【図５】図３に示す右側のロボットアームの座標系を説明するための図である。
【図６】図１に示すロボットアーム制御装置によるロボットアーム制御処理を説明するためのフローチャートである。
【符号の説明】
【００９４】
１ ロボットアーム制御装置
２ ロボットアーム部
３ ３次元操作グローブ
４ ３次元位置方向センサ
５ ３次元ディスプレイ
１１ 外部インターフェース部
１２ 浮動小数点データ記憶部
１３ データ変換処理部
１４ 可変長整数データ記憶部
１５ 誤差有制御処理部
１５ａ 座標変換処理部
１５ｂ 浮動小数点演算部
１６ 無誤差制御処理部
１６ａ 方程式解法処理部
１６ｂ 可変長整数演算部
２１ ロボットアーム
２１ａ 上腕
２１ｂ 前腕
２２ ハンド
２３ 胴体部
２４ カメラ部
【技術分野】
【０００１】
本発明は、関節を介して複数のリンクを連結したリンク機構の所定部位、例えば、先端部を目標３次元位置及び目標方向に制御する位置制御装置、位置制御方法及び位置制御プログラムに関する。
【背景技術】
【０００２】
従来のロボットアームの制御方式としては、スレーブアームと同じ軸構成等を有するマスターアームを操作者が操作してスレーブアームを制御するマスターアーム方式、操作者の腕と手の主要部（各関節）にセンサを装着して操作者の腕と手の関節の動きを検出してロボットアームを制御するモーションキャプチャー方式、操作者の手首・手の甲等に付けた３次元位置方向センサにより操作者の手首・手の甲の３次元位置及び方向を検出し、このデータを用いてアーム（リンク）の各関節角度を求める逆変換を行ってロボットアームを制御する逆変換方式等の種々の方式がある。
【０００３】
上記のマスターアーム方式では、操作者側には、制御されるスレーブアームだけでなく、マスターアームが必要となり、システムが重装備になる。また、モーションキャプチャー方式では、操作者の腕と手の複数箇所にセンサを装着する必要があり、操作者にとって煩わしいものとなる。
【０００４】
一方、逆変換方式では、操作者は手の甲等に１個のセンサのみを装着するだけで済み、操作者にとって煩わしさがないが、操作者の手首の位置と方向データから各関節角度を求める逆変換が必要となる。この逆変換では、人の腕のような高い自由度（６自由度以上）を持つ軸構成における各関節角度を求める必要があるため、逆変換が非線形連立方程式となり、複雑な計算となる。
【０００５】
このため、本願発明者は、逆変換が非線形連立方程式とならず、単純且つ少ない演算量で厳密解を求めることができるロボットアーム・ハンドの操作方法を提案した（特許文献１参照）。この操作方法では、ロボットアームの操作者の手首に付けたセンサにより検出された３次元位置及び方向からアームの各関節角度を求めるために、浮動小数点演算を用いた誤差有り演算を用いている。この浮動小数点フォーマットを用いた実数表現は、非常に小さな数から大きな数まで表現でき、計算効率がよいという優れた特徴があるため、逆変換のような実数の数値計算に適している。
【特許文献１】特開２００５−４６９３１号公報
【発明の開示】
【発明が解決しようとする課題】
【０００６】
しかしながら、上記の浮動小数点演算を用いた数値制御では、以下のような問題がある。
（１）浮動小数点演算では、数値演算誤差が発生し、且つその蓄積が起こるため、演算結果に大きな誤差が含まれる場合があり、ロボットアームの先端位置が大幅にずれる場合がある。
（２）上記の数値演算誤差とその蓄積のために、当初想定していなかった状態が発生し、結果として制御が破綻してロボットアーム動作が暴走する状況が発生する。
（３）上記のロボットアーム動作の暴走を防止するためには、暴走対策を施す必要があり、制御が複雑化する。
【０００７】
本発明の目的は、リンク機構の制御動作を安定化及び高信頼化することができるとともに、リンク機構を高精度に制御することができる位置制御装置、位置制御方法及び位置制御プログラムを提供することである。
【課題を解決するための手段】
【０００８】
本発明に係る位置制御装置は、関節を介して複数のリンクを連結したリンク機構の所定部位を目標３次元位置及び目標方向に制御する位置制御装置であって、リンク機構の所定部位の目標３次元位置データ及び目標方向データからなる目標データをＰ、リンク機構の各関節角度をθとし、目標データＰから関節角度θを求める解法処理に使用されるデータを固定長数値データ形式から可変長整数データ形式へ変換する変換手段と、上記解法処理において、変換手段により変換された可変長整数データ形式のデータを用いて、除算を除く加減算及び乗算からなる無誤差演算を実行する無誤差演算手段とを備えるものである。
【０００９】
本発明に係る位置制御装置では、目標データＰから関節角度θを求める解法処理において、この解法処理に使用されるデータが固定長数値データ形式から可変長整数データ形式へ変換され、変換された可変長整数データ形式のデータを用いて、除算を除く加算、減算及び乗算からなる無誤差演算が実行される。この無誤差演算においては、数値演算誤差が発生せず、その蓄積もないため、リンク機構の制御動作を安定化及び高信頼化することができるとともに、リンク機構を高精度に制御することができる。
【００１０】
変換手段は、無誤差演算手段の演算結果を可変長整数データ形式から固定長数値データ形式へ変換し、上記位置制御装置は、上記解法処理において、変換手段により変換された固定長数値データ形式の演算結果を用いて１回のみの除算を含む固定長数値演算を実行することにより固定長数値データ形式の関節角度θを求める誤差演算手段をさらに備えることが好ましい。
【００１１】
この場合、目標データＰから関節角度θを求める解法処理において、可変長整数データ形式の係数データを用いて、除算を除く加算、減算及び乗算からなる無誤差演算が実行された後、この演算結果が可変長整数データ形式から固定長数値データ形式へ変換され、変換された固定長数値データ形式の演算結果を用いて１回のみの除算を含む固定長数値演算が実行され、固定長数値データ形式の関節角度θが算出されるので、上記解法処理において、数値演算誤差の発生を多くとも１回の除算によるものだけに制限して数値演算誤差を非常に小さくすることができるとともに、その蓄積を防止することができる。
【００１２】
無誤差演算手段は、θｉ＝θ’ｉ＋Δθｉ（ｉ＝１，…，ｎ）（ここで、θｉは関節ｉの求める関節角度、θ’ｉは微小時間Δｔ前の関節ｉの関節角度、Δθｉは微小時間Δｔ経過後の関節ｉの微小関節角度変化量、ｎは求める関節角度θの数）とし、ｎ元連立一次方程式ＡΔθ＝Ｂ（ここで、Ａはその要素ａｉｊ（ｉ＝１，…，ｎ、ｊ＝１，…，ｎ）がリンク機構のリンク長及び微小時間Δｔ前の関節ｉの関節角度θ’ｉにより決定される係数行列、Ｂはその要素ｂｉ（ｉ＝１，…，ｎ）が目標データＰ及び微小時間Δｔ前の位置方向データＰ’により決定されるベクトル）をＡｎΔθ＝Ｂｎ（ここで、Ａｎは対角要素ａｎ＋１ｉｉ（ｉ＝１，…，ｎ）以外が０となる対角行列、Ｂｎは要素ｂｎ＋１ｉ（ｉ＝１，…，ｎ）のベクトル）に変形したときの係数データａｎ＋１ｉｉ及び係数データｂｎ＋１ｉを、除算を除く加算、減算及び乗算からなる無誤差演算を用いて求め、変換手段は、無誤差演算手段により求められた係数データａｎ＋１ｉｉ及び係数データｂｎ＋１ｉを可変長整数データ形式から固定長数値データ形式へ変換し、誤差演算手段は、変換手段により変換された固定長数値データ形式の係数データａｎ＋１ｉｉ及び係数データｂｎ＋１ｉを用いて、θｉ＝θ’ｉ＋ｂｎ＋１ｉ／ａｎ＋１ｉｉ（ｉ＝１，…，ｎ）を固定長数値演算により求めることが好ましい。
【００１３】
この場合、目標データＰから関節角度θを求める解法処理において、θｉ＝θ’ｉ＋ｂｎ＋１ｉ／ａｎ＋１ｉｉ以外の演算を、除算を除く加算、減算及び乗算からなる無誤差演算により実行するとともに、数値演算誤差の発生を多くともθｉ＝θ’ｉ＋ｂｎ＋１ｉ／ａｎ＋１ｉｉにおける１回の除算だけに制限しているので、関節角度θを高精度に求めることができる。
【００１４】
上記位置制御装置は、操作者の所定部位の３次元位置及び方向を検出する検出手段をさらに備え、検出手段により検出された３次元位置及び方向をリンク機構の所定部位の目標３次元位置データ及び目標方向データとして用いて、リンク機構の所定部位を目標３次元位置及び目標方向に制御することが好ましい。
【００１５】
この場合、リンク機構の所定部位を操作者の所定部位の動きに応じた目標３次元位置及び目標方向に高精度に制御することができる。
【００１６】
本発明に係る位置制御方法は、関節を介して複数のリンクを連結したリンク機構の所定部位を目標３次元位置及び目標方向に制御するロボットアーム制御方法であって、リンク機構の所定部位の目標３次元位置データ及び目標方向データからなる目標データをＰ、リンク機構の各関節角度をθとし、目標データＰから関節角度θを求める解法処理に使用されるデータを固定長数値データ形式から可変長整数データ形式へ変換するステップと、上記解法処理において、変換された可変長整数データ形式のデータを用いて、除算を除く加算、減算及び乗算からなる無誤差演算を実行するステップとを含むものである。
【００１７】
本発明に係る位置制御プログラムは、関節を介して複数のリンクを連結したリンク機構の所定部位を目標３次元位置及び目標方向に制御するための位置制御プログラムであって、リンク機構の所定部位の目標３次元位置データ及び目標方向データからなる目標データをＰ、リンク機構の各関節角度をθとし、目標データＰから関節角度θを求める解法処理に使用されるデータを固定長数値データ形式から可変長整数データ形式へ変換する変換手段と、上記解法処理において、変換手段により変換された可変長整数データ形式の係数データを用いて、除算を除く加算、減算及び乗算からなる無誤差演算を実行する無誤差演算手段としてコンピュータを機能させるものである。
【発明の効果】
【００１８】
本発明によれば、目標データＰから関節角度θを求める解法処理において、可変長整数データ形式のデータを用いて、除算を除く加算、減算及び乗算からなる無誤差演算が実行され、この無誤差演算においては、数値演算誤差が発生せず、その蓄積もないため、リンク機構の制御動作を安定化及び高信頼化することができるとともに、リンク機構を高精度に制御することができる。
【発明を実施するための最良の形態】
【００１９】
以下、本発明の一実施の形態による位置制御装置の一例であるロボットアーム制御装置をいたロボットアーム制御システムについて図面を参照しながら説明する。
【００２０】
図１は、本発明の一実施の形態によるロボットアーム制御装置を用いたロボットアーム制御システムの構成を示すブロック図である。図１に示すロボットアーム制御システムは、ロボットアーム制御装置１、ロボットアーム部２、３次元操作グローブ３、３次元位置方向センサ４及び３次元ディスプレイ５を備える。
【００２１】
ロボットアーム部２は、一対のロボットアーム２１及びハンド２２、胴体部２３並びにカメラ部２４を備え、人の上半身（頭部を除く）と同程度の大きさと自由度とを有し、両腕と両手の機能を実現している。
【００２２】
ロボットアーム２１は、肩、肘及び手首に関節を有し、これらの関節を介してリンクとなる上腕２１ａ及び前腕２１ｂが連結されたリンク機構（マニプレータ）であり、人の腕と同程度の大きさ及び自由度を有する。ハンド２２は、人の手と同程度の大きさと自由度を有する触覚付きの５本指ロボットハンドであり、前腕２１ｂの先端に取り付けられる。ハンド２２には触覚センサ（図示省略）が設けられており、物をつかんだ時の触覚（圧力）がロボットアーム制御装置１を介して３次元操作グローブ３へ伝達される。
【００２３】
なお、図示を省略しているが、ロボットアーム２１及びハンド２２は、その内部にモータ等の駆動源を有し、この駆動源により任意の角度に駆動される。また、本実施の形態では、ロボットアーム２１を人の腕に模倣させ、ハンド２２を人の手指に模倣させているが、ロボットアーム及びハンドの構成は、この例に特に限定されず、ハンド２２の指の数を増減させる等の種々の変更が可能である。また、リンク機構の関節数及びリンク数も、上記の例に特に限定されず、種々の変更が可能である。
【００２４】
カメラ部２４は、３次元カメラ（ステレオカメラ）等から構成され、ロボットアーム制御装置１を介して、撮影した画像を３次元ディスプレイ５へ出力する。３次元ディスプレイ５は、カメラ部２４により撮影された画像を表示し、操作者Ｍに提示する。なお、撮影された画像の提示方法は、この例に特に限定されず、３次元ディスプレイに替えて、操作者Ｍに装着させたヘッドマウントディスプレイに表示させる等の種々の変更が可能である。
【００２５】
３次元操作グローブ３は、公知の３次元操作用データグローブから構成され、操作者Ｍの両手にそれぞれ装着される。３次元操作グローブ３は、操作者Ｍの手の甲及び指の動き（関節角度）を検出し、検出した関節角度データをロボットアーム制御装置１へ出力する。３次元操作グローブ３の手首の位置には、３次元位置方向センサ４が取り付けられ、３次元位置方向センサ４は、前腕２１ｂの先端部の目標３次元位置及び目標方向として、操作者Ｍの手首の３次元位置及び方向を検出し、検出した３次元位置データ及び方向データをロボットアーム制御装置１へ出力する。
【００２６】
なお、目標３次元位置及び目標方向を検出又は入力する方法は、上記の例に特に限定されず、データグローブ又はデータスーツ等から操作者の所定部位の３次元位置及び方向を検出したり、ハプティクス・デバイス等を用いて入力したりしてもよい。
【００２７】
ロボットアーム制御装置１は、操作者Ｍの両手に装着された３次元操作グローブ３及び３次元位置方向センサ４からの検出結果を基に、ロボットアーム２１及びハンド２２の各関節角度を算出する。ハンド２２に関しては、３次元操作グローブ３が手の甲の曲がり（手首の）角度及び指の関節角度を検出し、ロボットアーム制御装置１は、これらの角度データに基づいてハンド２２を制御する。
【００２８】
一方、ロボットアーム２１に関しては、操作者Ｍの手首に装着した３次元位置方向センサ４から操作者Ｍの手首の三次元位置及びその方向、すなわち、ロボットアーム２１の先端部の目標３次元位置及び目標方向が得られるだけである。このため、ロボットアーム２１の先端部の目標３次元位置データ及び目標方向データから、ロボットアーム２１の各関節角度を計算する必要がある。このように、ロボットアーム２１の先端部の３次元位置及び方向からロボットアーム２１の各関節角度を計算することを逆変換と呼び、これとは逆に、ロボットアーム２１の各関節角度からロボットアーム２１の先端部の３次元位置及び方向を計算することを順変換と呼ぶことにする。
【００２９】
図２は、図１に示すロボットアーム制御装置１の構成を示すブロック図である。図２に示すように、ロボットアーム制御装置１は、外部インターフェース部１１、浮動小数点データ記憶部１２、データ変換処理部１３、可変長整数データ記憶部１４、誤差有制御処理部１５及び無誤差制御処理部１６を備える。誤差有制御処理部１５は、座標変換処理部１５ａ及び浮動小数点演算部１５ｂを備え、無誤差制御処理部１６は、方程式解法処理部１６ａ及び可変長整数演算部１６ｂを備える。
【００３０】
ロボットアーム制御装置１は、ＣＰＵ（中央演算処理装置）、ＲＯＭ（リードオンリメモリ）、ＲＡＭ（ランダムアクセスメモリ）、外部記憶装置、記録媒体駆動装置、入力装置、表示装置、外部機器インターフェース等を備えるコンピュータから構成することができる。この場合、後述するロボットアーム制御処理（位置制御処理）を実行するためのロボットアーム制御プログラム（位置制御プログラム）を、ＣＰＵ等を用いて実行することにより、外部インターフェース部１１、浮動小数点データ記憶部１２、データ変換処理部１３、可変長整数データ記憶部１４、誤差有制御処理部１５及び無誤差制御処理部１６としてコンピュータを機能させることができる。なお、ロボットアーム制御装置１の構成は、この例に特に限定されず、上記の各機能の一部又は全部を専用のハードウエアから構成する等の種々の変更が可能である。
【００３１】
外部インターフェース部１１は、３次元位置方向センサ４から３次元位置データ及び方向データを取得して浮動小数点データ記憶部１２に格納する。また、外部インターフェース部１１は、ロボットアーム制御処理により求められた関節角度を浮動小数点データ記憶部１２から読み出してロボットアーム部２へ出力し、ロボットアーム２１の各関節角度がロボットアーム制御処理により求められた関節角度に制御される。
【００３２】
浮動小数点データ記憶部１２は、誤差有制御処理部１５において処理される種々のデータを浮動小数点データ形式で記憶し、可変長整数データ記憶部１４は、無誤差制御処理部１６において処理される種々のデータを可変長整数データ形式で記憶する。
【００３３】
図３は、浮動小数点データ及び可変長整数データのデータ構成の一例を示す図である。図３の（ａ）に示すように、固定長数値データの一例である浮動小数点データは、符号部、指数部及び仮数部から構成され、このようなデータ形式で浮動小数点データ記憶部１２に格納される。また、図３の（ｂ）に示すように、可変長整数データは、語長部、符号部、及び数値部（整数表現）から構成され、このようなデータ形式で可変長整数データ記憶部１４に格納される。
【００３４】
データ変換処理部１３は、浮動小数点データ記憶部１２から浮動小数点データを読み出し、読み出した浮動小数点データを可変長整数データへ変換し、変換した可変長整数データを可変長整数データ記憶部１４に格納する。また、データ変換処理部１３は、可変長整数データ記憶部１４から可変長整数データを読み出し、読み出した可変長整数データを浮動小数点データへ変換し、変換した浮動小数点データを浮動小数点データ記憶部１２に格納する。
【００３５】
なお、本発明に用いられる固定長数値データは、上記の例に特に限定されず、例えば、固定長整数データを用いることもできる。図３の（ｃ）に示すように、固定長整数データは、符号部及び数値部（整数表現）から構成され、この固定長整数データ形式も、固定長であるため、誤差（数値のオーバーフロー及びそのまるめによる誤差）が発生し、本発明を同様に適用することができる。
【００３６】
例えば、３次元位置方向センサ４が出力する値（角度）が、−２５５〜＋２５５等の整数値であり、ロボットアーム２１の関節を動作させるモータに対する出力（回転角度）も、−１０００〜＋１０００パルス等の整数値である場合、浮動小数点データ記憶部１２を固定長数値データ記憶部に変更し、固定長整数データ形式で各データを格納するようにしてもよい。また、固定長整数データ形式でデータを取り扱う場合、浮動小数点演算部１５ｂを固定長整数演算部に変更し、固定長整数演算部により固定長整数演算を実行するようにしてもよい。
【００３７】
誤差有制御処理部１５は、ロボットアーム部２のロボットアーム２１及びハンド２２の制御処理のうち、固定長数値演算の一例である浮動小数点演算を用いた誤差有制御処理を実行する。座標変換処理部１５ａは、浮動小数点データ記憶部１２から浮動小数点データを読み出し、浮動小数点演算部１５ｂを用いて座標変換処理等（ｓｉｎ、ｃｏｓ演算）を浮動小数点演算で実行し、演算結果を浮動小数点データ記憶部１２に格納する。浮動小数点演算部１５ｂは、加算器、減算器、乗算器及び除算器としての機能を有し、浮動小数点演算を用いて、加算、減算、乗算及び除算を実行する。
【００３８】
無誤差制御処理部１６は、ロボットアーム部２のロボットアーム２１及びハンド２２の制御処理のうち、可変長整数演算を用いた無誤差制御処理を実行する。方程式解法処理部１６ａは、可変長整数データ記憶部１４から可変長整数データを読み出し、制御方程式の求解等を可変長整数演算部１６ｂを用いて可変長整数演算で実行し、演算結果を可変長整数データ記憶部１４に格納する。可変長整数演算部１６ｂは、加算器、減算器及び乗算器としての機能を有し、可変長整数演算を用いて、加算、減算及び乗算を無誤差で実行する。
【００３９】
ここで、本実施の形態の制御対象となるロボットアーム２１について詳細に説明する。図４は、図１に示すロボットアーム部２における右側のロボットアーム２１の構成を示す模式図であり、図５は、図１に示す右側のロボットアーム２１の座標系を説明するための図である。なお、左側のロボットアーム２１も、図４及び図５に示す右側のロボットアーム２１と同様に構成されているので、詳細な説明は省略する。
【００４０】
図４に示すように、グローバル座標系の原点Ｏを上腕２１ａの右肩（第１関節）に、グローバル座標系のＸ座標軸を胴体部２３（図１参照）の正面方向に沿って床面と平行方向に、グローバル座標系のＹ座標軸を右肩から左肩方向に、グローバル座標系のＺ座標軸を床面から天井への垂直方向にそれぞれ設定する。また、ロボットアーム２１の上腕２１ａの長さをｅ１、前腕２１ｂの長さをｅ２とし、上腕２１ａの肩関節のうち第１関節（Ｙ軸回り）の関節角度θ１、第２関節（Ｘ軸回り）の関節角度θ２、第３関節（Ｚ軸回り）の関節角度θ３とし、上腕２１ａと前腕２１ｂとの間の第４関節（Ｙ軸回り）の関節角度θ４、第５関節（Ｚ軸回り）の関節角度θ５とし、前腕２１ｂとハンド２２との間の第６関節（Ｙ軸回り）の関節角度θ６とする。
【００４１】
このとき、前腕２１ｂの先端位置・方向Ｐｓすなわち操作者Ｍの手首に装着した３次元位置方向センサ４が示すグローバル座標に対する変換行列をＨ０ｓとすると、以下の式（１）のように表現することができる。
【００４２】
【数１】
【００４３】
ここで、図５に示すように、３次元位置方向センサ４の３次元位置はＰｓ（ｘｓ，ｙｓ，ｚｓ）、３次元位置方向センサ４の方向を表すＸ軸方向の単位ベクトルはｄｘｓ（ｕｘｓ，ｖｘｓ，ｗｘｓ）、Ｙ軸方向の単位ベクトルはｄｙｓ（ｕｙｓ，ｖｙｓ，ｗｙｓ）、Ｚ軸方向の単位ベクトルはｄｚｓ（ｕｚｓ，ｖｚｓ，ｗｚｓ）となる。これらの値はすべて、３次元位置方向センサ４から取得されるので、既知となる。一方、Ｈ０ｓの各要素は、各アーム（上腕２１ａ及び前腕２１ｂ）の軸の長さｅ１，ｅ２と各関節角度θ１，θ２，θ３，θ４，θ５を用いて、以下のように表現することができる。
【００４４】
【数２】
【００４５】
ここで、Ｃｎ＝ｃｏｓ（θｎ）、Ｓｎ＝ｓｉｎ（θｎ）（ｎ＝１，…，５）であり、関節角度θｎは、アーム動作により刻々と変化するので、時間ｔの関数となり、θｎ’を微小時間Δｔ前の関節角度（直前の関節角度）とし、微小時間Δｔ経過後における微小関節角度変化量をΔθｎとすると、以下のように表現することができる。
θｎ＝θｎ’＋Δθｎ （６）
ここで、微小時間Δｔ前の関節角度値θｎ’は既知であり、微小時間Δｔは制御時間間隔である。
【００４６】
上記の前提を基に、以下のように近似することができる。なお、下記式では、（Δθ）２は微小であるので、（Δθ）２以上の高次項は無視できるとした。
ｓｉｎ（θ）＝ｓｉｎ（θ’＋Δθ）≒ｓｉｎ（θ’）＋（ｓｉｎ（θ’））’Δθ＝ｓｉｎ（θ’）＋ｃｏｓ（θ’）Δθ （７ａ）
ｃｏｓ（θ）＝ｃｏｓ（θ’＋Δθ）≒ｃｏｓ（θ’）＋（ｃｏｓ（θ’））’Δθ＝ｃｏｓ（θ’）−ｓｉｎ（θ’）Δθ （７ｂ）
上記の式（７ａ）、（７ｂ）を式（２）〜（５）へ代入すると、以下となる。なお、ここでも、Δθ１Δθ２等の高次のΔθは微小であり、無視できるとした。
【００４７】
【数３】
【００４８】
【数４】
【００４９】
【数５】
【００５０】
【数６】
【００５１】
上記各式より、式（１）は、以下のように表現することができる。
【００５２】
【数７】
【００５３】
そこで、Δθ１，Δθ２，Δθ３，Δθ４，Δθ５は、以下の５元連立一次方程式の解として求めることができる。
【００５４】
【数８】
【００５５】
また、上式の左辺の行列の各要素を係数ａｉｊ（ｉ＝１，…，５、ｊ＝１，…，５）、右辺のベクトルの各要素を係数ｂｉ（ｉ＝１，…，５）で表すと、以下のように表現することができる。
【００５６】
【数９】
【００５７】
上記の５元連立一次方程式の解法では、加減乗除算が繰り返されるために、浮動小数点演算を用いた数値計算では、数値演算誤差とその蓄積のために解の誤差が大きくなり、連立方程式の解を求める処理が演算誤差のために暴走及び破綻することが多々ある。このように解の誤差が大きくなること、及び連立方程式の解を求める処理が暴走及び破綻することは、ロボットアームが人に危害を加える恐れがあるため、ロボットアームの制御においては許されない。また、除算を使用する場合、数値計算に計算精度の低下等の様々な悪影響を及ぼすことはもちろんのこと、無誤差演算自体を行うことはできない。
【００５８】
このため、本実施の形態では、ガウス・ジョルダンの消去法を基本にし、最後の１回を除いて除算が発生しない無誤差連立一次方程式解法処理を考案し、式（１５）の解法に誤差を発生させない無誤差演算を用い、各関節角度θ１，θ２，θ３，θ４，θ５を以下のようにして求めている。
【００５９】
まず、式（１５）の係数ａｉｊ（ｉ＝１，…，５、ｊ＝１，…，５）及び係数ｂｉ（ｉ＝１，…，５）を用いて、ｋ＝１，…，５として、下記の式を繰り返し演算することにより係数ａ６ｉｊ及び係数ｂ６ｉを求める。
ａｋ＋１ｉｊ＝ａｋｉｊ （ｉ＝ｋ） （１６ａ）
ａｋ＋１ｉｊ＝ａｋｉｊａｋｋｋ−ａｋｋｊａｋｉｋ （ｉ≠ｋ） （１６ｂ）
ｂｋ＋１ｉ＝ｂｋｉ （ｉ＝ｋ） （１６ｃ）
ｂｋ＋１ｉ＝ｂｋｉａｋｋｋ−ｂｋｋａｋｉｋ （ｉ≠ｋ） （１６ｄ）
上記の係数ａ６ｉｊ及び係数ｂ６ｉにより、式（１５）は以下の形式となる。
【００６０】
【数１０】
【００６１】
上記のように、式（１７）の左辺の係数行列は、対角要素以外が０となる対角行列となる。上記の演算までは、除算を使用する必要がないため、本実施の形態では、式（１５）の連立一次方程式の解法処理のうち上記の演算までの処理を、無誤差制御処理部１６による無誤差演算を用いて実行し、ここまでの演算を無誤差で行っている。
【００６２】
次に、各関節角度θ１，θ２，θ３，θ４，θ５は、以下の式により求めることができる。
【００６３】
【数１１】
【００６４】
上記の式（１８）の演算だけは、除算が回避できないために、本実施の形態では、誤差有制御処理部１５による浮動小数点演算を用いて実行している。この浮動小数点演算により誤差が発生するが、この処理は最終処理であり、且つ１回の除算を行うだけであるため、除算に充分な精度が確保されていれば、誤差が問題になることはなく、誤差の蓄積も起こらない。なお、手首の角度θ６は、３次元操作グローブ３により直接取得することができるので、上記の無誤差連立一次方程式解法処理からは求めていない。
【００６５】
ロボットアーム制御装置１は、上記の無誤差連立一次方程式解法処理（目標データＰから関節角度θを求める解法処理）を以下のようにして実行している。
【００６６】
外部インターフェース部１１は、ロボットアームの２１の先端部の目標３次元位置（３次元位置方向センサ４の３次元位置Ｐｓ）及び目標方向（Ｘ、Ｙ、Ｚ軸方向の単位ベクトルｄｘｓ、ｄｙｓ、ｄｚｓ）からなる目標データＰを３次元位置方向センサ４から取得し、浮動小数点データ記憶部１２に浮動小数点データ形式で記憶させる。このとき、浮動小数点データ記憶部１２には、微小時間Δｔ前の位置方向データＰ’及び微小時間Δｔ前の関節角度データθ’ｉが記憶されており、また、上腕２１ａの長さｅ１、前腕２１ｂの長さｅ２等の初期値は予め記憶されているものとする。
【００６７】
ここで、上記の微小時間Δｔ前の位置方向データＰ’は、微小時間Δｔ前の目標データではなく、微小時間Δｔ前の実際の３次元位置データ及び方向データであり、微小時間Δｔ前の関節角度データθ’ｉもアームの実際の関節角度である。これは、後述するように、Ｐ’、θ’及びＰからΔθを求め（線形近似後、連立一次方程式を無誤差で解く）、そして、θ＝θ’＋Δθからθを求めており、ここで（線形）近似しているため、このθから求めた実際の位置方向データは目標データと一致せず、次の時刻の計算では、微小時間Δｔ前の目標データ等ではなく、微小時間Δｔ前の実際の３次元位置データ及び方向データ、並びに微小時間Δｔ前の実際の関節角度データを用いているためである。
【００６８】
座標変換処理部１５ａは、浮動小数点データ記憶部１２から微小時間Δｔ前の関節角度θ’ｉ、上腕２１ａの長さｅ１、前腕２１ｂの長さｅ２を読み出し、浮動小数点演算部１５ｂを用いて浮動小数点演算を実行することにより式（１５）の係数データａｉｊを算出し、浮動小数点データ記憶部１２に記憶させる。また、座標変換処理部１５ａは、浮動小数点データ記憶部１２から目標データＰ及び微小時間Δｔ前の位置方向データＰ’を読み出し、浮動小数点演算部１５ｂを用いて浮動小数点演算を実行することにより式（１５）の係数データｂｉを算出し、浮動小数点データ記憶部１２に記憶させる。
【００６９】
データ変換処理部１３は、浮動小数点データ記憶部１２から係数ａｉｊ及び係数ｂｉを順次読み出し、浮動小数点データ形式から可変長整数データ形式に変換した係数ａｉｊ及び係数ｂｉを、係数ａ１ｉｊ及び係数ｂ１ｉとして可変長整数データ記憶部１４に格納する。
【００７０】
方程式解法処理部１６ａは、可変長整数データ記憶部１４から係数ａ１ｉｊ及び係数ｂ１ｉを読み出し、可変長整数演算部１６ｂを用いて可変長整数演算（無誤差演算）を実行することにより、式（１６ａ）乃至（１６ｄ）を用いて係数ａｋ＋１ｉｊ及びｂｋ＋１ｉを順次算出し、最終的に係数ａ６ｉｊ及びｂ６ｉを可変長整数データ記憶部１４に格納する。
【００７１】
データ変換処理部１３は、可変長整数データ記憶部１４から係数ａ６ｉｉ及び係数ｂ６ｉを順次読み出し、可変長整数データ形式から浮動小数点データ形式に変換した係数ａ６ｉｉ及び係数ｂ６ｉを浮動小数点データ記憶部１２に格納する。
【００７２】
座標変換処理部１５ａは、浮動小数点データ記憶部１２から係数ａ６ｉｉ及び係数ｂ６ｉを順次読み出し、浮動小数点演算部１５ｂを用いて浮動小数点演算の除算を１回だけ実行することにより、微小関節角度変化量Δθｉ＝ｂ６ｉ／ａ６ｉｉを算出し、浮動小数点データ記憶部１２に格納する。そして、座標変換処理部１５ａは、浮動小数点データ記憶部１２から直前の関節角度値θ’ｉ及び微小関節角度変化量Δθｉを順次読み出し、浮動小数点演算部１５ｂを用いて浮動小数点演算を実行することにより式（１８）を用いて関節角度θｉを算出し、浮動小数点データ記憶部１２に格納する。
【００７３】
外部インターフェース部１１は、浮動小数点データ記憶部１２から関節角度θｉを読み出してロボットアーム部２へ出力する。ロボットアーム部２は、ロボットアーム２１の各関節角度が関節角度θｉとなるようにロボットアーム２１を動作させる。
【００７４】
上記の制御動作により、操作者Ｍは、３次元ディスプレイ５を用いて、カメラ部２４により撮影された画像を見ながら、両手に装着した３次元操作グローブ３を用いて、ロボットアーム２１及びハンド２２を操作し、操作者Ｍの腕と手の動きとがロボットアーム２１及びハンド２２により忠実に再現される。
【００７５】
本実施の形態では、データ変換処理部１３が変換手段の一例に相当し、無誤差制御処理部１６（方程式解法処理部１６ａ及び可変長整数演算部１６ｂ）が無誤差演算手段の一例に相当し、誤差有制御処理部１５（座標変換処理部１５ａ及び浮動小数点演算部１５ｂ）が誤差演算手段の一例に相当する。
【００７６】
次に、上記のように構成されたロボットアーム制御装置１によるロボットアーム制御処理について説明する。図６は、図１に示すロボットアーム制御装置１によるロボットアーム制御処理を説明するためのフローチャートである。
【００７７】
図６に示すように、まず、ステップＳ１１において、外部インターフェース部１１は、３次元位置方向センサ４から３次元位置データＰｓ（ｘｓ，ｙｓ，ｚｓ）、Ｘ軸方向の単位ベクトルｄｘｓ（ｕｘｓ，ｖｘｓ，ｗｘｓ）、Ｙ軸方向の単位ベクトルｄｙｓ（ｕｙｓ，ｖｙｓ，ｗｙｓ）、及びＺ軸方向の単位ベクトルｄｚｓ（ｕｚｓ，ｖｚｓ，ｗｚｓ）を取得して浮動小数点データ記憶部１２に格納する。
【００７８】
次に、ステップＳ１２において、座標変換処理部１５ａは、浮動小数点データ記憶部１２から３次元位置データＰｓ（ｘｓ，ｙｓ，ｚｓ）等を順次読み出し、浮動小数点演算部１５ｂを用いて、式（１５）の係数ａｉｊ（ｉ＝１，…，５、ｊ＝１，…，５）及び係数ｂｉ（ｉ＝１，…，５）を求め、浮動小数点データ記憶部１２に格納する。
【００７９】
次に、ステップＳ１３において、データ変換処理部１３は、浮動小数点データ記憶部１２から係数ａｉｊ及び係数ｂｉを順次読み出し、浮動小数点データ形式から可変長整数データ形式に変換した係数ａｉｊ及び係数ｂｉを、係数ａ１ｉｊ及び係数ｂ１ｉとして可変長整数データ記憶部１４に格納する。
【００８０】
なお、上記のステップＳ１２及びＳ１３では、浮動小数点演算を用いて３次元位置データＰｓ（ｘｓ，ｙｓ，ｚｓ）等から係数ａｉｊ及び係数ｂｉを求めた後に、可変長整数データへフォーマット変換したが、この例に特に限定されず、データ変換処理部１３により３次元位置データＰｓ（ｘｓ，ｙｓ，ｚｓ）、Ｘ軸方向の単位ベクトルｄｘｓ（ｕｘｓ，ｖｘｓ，ｗｘｓ）、Ｙ軸方向の単位ベクトルｄｙｓ（ｕｙｓ，ｖｙｓ，ｗｙｓ）、及びＺ軸方向の単位ベクトルｄｚｓ（ｕｚｓ，ｖｚｓ，ｗｚｓ）を浮動小数点データから可変長整数データに変換し、その後、方程式解法処理部１６ａにより可変長整数データの３次元位置データＰｓ（ｘｓ，ｙｓ，ｚｓ）等から可変長整数データの係数ａｉｊ及び係数ｂｉを求めるようにしてもよい。
【００８１】
次に、ステップＳ１４〜Ｓ１６において、方程式解法処理部１６ａは、初期設定として、ｋ＝１、ｉ＝１、ｊ＝１に設定する。次に、ステップＳ１７において、方程式解法処理部１６ａは、可変長整数データ記憶部１４から係数ａｋｉｊ，ａｋｋｋ，ａｋｋｊ，ａｋｉｋを読み出し、可変長整数演算部１６ｂを用いて、ｉ＝ｋの場合はａｋ＋１ｉｊ＝ａｋｉｊに設定し、ｉ≠ｋの場合はａｋ＋１ｉｊ＝ａｋｉｊａｋｋｋ−ａｋｋｊａｋｉｋを算出し、係数ａｋ＋１ｉｊを可変長整数データ記憶部１４に格納する。
【００８２】
次に、ステップＳ１８において、方程式解法処理部１６ａは、ｊ＜５であるか否かを判断し、ｊ＜５の場合、ステップＳ２７においてｊを１だけインクリメントしてステップＳ１７以降の処理を継続し、ｊ＜５でない（ｊ＝５）場合、ステップＳ１９へ処理を移行する。
【００８３】
ｊ＜５でない場合、ステップＳ１９において、方程式解法処理部１６ａは、可変長整数データ記憶部１４から係数ｂｋｉ，ａｋｋｋ，ｂｋｋ，ａｋｉｋを読み出し、可変長整数演算部１６ｂを用いて、ｉ＝ｋの場合はｂｋ＋１ｉ＝ｂｋｉに設定し、ｉ≠ｋの場合はｂｋ＋１ｉ＝ｂｋｉａｋｋｋ−ｂｋｋａｋｉｋを算出し、係数ｂｋ＋１ｉを可変長整数データ記憶部１４に格納する。
【００８４】
次に、ステップＳ２０において、方程式解法処理部１６ａは、ｉ＜５であるか否かを判断し、ｉ＜５の場合、ステップＳ２６においてｉを１だけインクリメントしてステップＳ１６以降の処理を継続し、ｉ＜５でない（ｉ＝５）場合、ステップＳ２１へ処理を移行する。
【００８５】
ｉ＜５でない場合、ステップＳ２１において、方程式解法処理部１６ａは、ｋ＜５であるか否かを判断し、ｋ＜５の場合、ステップＳ２５においてｋを１だけインクリメントしてステップＳ１５以降の処理を継続し、ｋ＜５でない（ｋ＝５）場合、ステップＳ２２へ処理を移行する。このように、ステップＳ１４〜Ｓ２１及びＳ２５〜Ｓ２７までの処理が無誤差演算を用いて実行される。
【００８６】
次に、ステップＳ２２において、データ変換処理部１３は、可変長整数データ記憶部１４から係数ａ６ｉｉ及び係数ｂ６ｉ（ｉ＝１，…，５）を順次読み出し、可変長整数データ形式から浮動小数点データ形式に変換し、変換した係数ａ６ｉｉ及び係数ｂ６ｉを浮動小数点データ記憶部１２に格納する。
【００８７】
次に、ステップＳ２３において、座標変換処理部１５ａは、浮動小数点データ記憶部１２から係数ａ６ｉｉ及び係数ｂ６ｉ（ｉ＝１，…，５）を順次読み出し、浮動小数点演算部１５ｂを用いて、微小関節角度変化量Δθｉ＝ｂ６ｉ／ａ６ｉｉ（ｉ＝１，…，５）を算出し、浮動小数点データ記憶部１２に格納する。
【００８８】
次に、ステップＳ２４において、座標変換処理部１５ａは、浮動小数点データ記憶部１２から直前の関節角度値θ’ｉ及び微小関節角度変化量Δθｉ（ｉ＝１，…，５）を順次読み出し、浮動小数点演算部１５ｂを用いて、式（１８）に従って関節角度θｉ＝θ’ｉ＋Δθｉ（ｉ＝１，…，５）を算出し、浮動小数点データ記憶部１２に格納する。このとき、外部インターフェース部１１は、浮動小数点データ記憶部１２から関節角度θ１，θ２，θ３，θ４，θ５を読み出してロボットアーム部２へ出力する。
【００８９】
ロボットアーム部２は、ロボットアーム２１の各関節角度が関節角度θ１，θ２，θ３，θ４，θ５となるようにロボットアーム２１を動作させる。なお、手首の関節角度θ６は、３次元操作グローブ３から直接取得することができるので、３次元操作グローブ３により検出された関節角度θ６がロボットアーム制御装置１を介してロボットアーム部２へ出力され、ロボットアーム部２は、ロボットアーム２１の手首の関節角度が関節角度θ６となるようにロボットアーム２１を動作させる。上記の処理が制御時間間隔ごとに繰り返され、ロボットアーム２１の先端位置が操作者Ｍの手首の位置に応じて正確に制御される。
【００９０】
上記の処理により、本実施の形態では、ロボットアーム制御処理において用いられる逆変換制御演算（連立一次方程式解法）に使用される数値を可変長の整数により表現するとともに、除算を実行することなく（最後の除算を除く）、加算、減算及び乗算のみを用いた無誤差演算処理を実行し、制御演算誤差をなくすことができる。
【００９１】
この結果、ロボットアーム２１の先端部を高精度に制御することができるとともに、制御処理の破綻をなくしてロボットアーム２１の動作の暴走等をなくすことができ、ロボットアーム２１の動作の安定化及び高信頼化を図ることができる。また、ロボットアーム２１の暴走防止対策等が不要となるので、ロボットアーム制御装置１の構成を単純化することができ、装置の低コスト化を実現することができる。
【００９２】
なお、本実施の形態では、操作者の手首の位置及び方向を検出し、これらを目標位置及び目標方向として制御したが、本発明による制御例は、この例に特に限定されず、３次元操作グローブ３、３次元位置方向センサ４及び３次元ディスプレイ５を省略し、テーチングを行うことにより浮動小数点データ記憶部１２に記憶した目標位置及び目標方向となるようにロボットアーム２１を制御する等の他の制御例にも適用可能である。また、ロボットアーム２１を制御対象としたが、この例に特に限定されず、ハンド２２を制御対象にする等の種々の変更が可能である。
【図面の簡単な説明】
【００９３】
【図１】本発明の一実施の形態によるロボットアーム制御装置を用いたロボットアーム制御システムの構成を示すブロック図である。
【図２】図１に示すロボットアーム制御装置の構成を示すブロック図である。
【図３】浮動小数点データ及び可変長整数データのデータ構成の一例を示す図である。
【図４】図１に示すロボットアーム部における右側のロボットアームの構成を示す模式図である。
【図５】図３に示す右側のロボットアームの座標系を説明するための図である。
【図６】図１に示すロボットアーム制御装置によるロボットアーム制御処理を説明するためのフローチャートである。
【符号の説明】
【００９４】
１ ロボットアーム制御装置
２ ロボットアーム部
３ ３次元操作グローブ
４ ３次元位置方向センサ
５ ３次元ディスプレイ
１１ 外部インターフェース部
１２ 浮動小数点データ記憶部
１３ データ変換処理部
１４ 可変長整数データ記憶部
１５ 誤差有制御処理部
１５ａ 座標変換処理部
１５ｂ 浮動小数点演算部
１６ 無誤差制御処理部
１６ａ 方程式解法処理部
１６ｂ 可変長整数演算部
２１ ロボットアーム
２１ａ 上腕
２１ｂ 前腕
２２ ハンド
２３ 胴体部
２４ カメラ部
【特許請求の範囲】
【請求項１】
関節を介して複数のリンクを連結したリンク機構の所定部位を目標３次元位置及び目標方向に制御する位置制御装置であって、
リンク機構の所定部位の目標３次元位置データ及び目標方向データからなる目標データをＰ、リンク機構の各関節角度をθとし、目標データＰから関節角度θを求める解法処理に使用されるデータを固定長数値データ形式から可変長整数データ形式へ変換する変換手段と、
前記解法処理において、前記変換手段により変換された可変長整数データ形式のデータを用いて、除算を除く加算、減算及び乗算からなる無誤差演算を実行する無誤差演算手段とを備えることを特徴とする位置制御装置。
【請求項２】
前記変換手段は、前記無誤差演算手段の演算結果を可変長整数データ形式から固定長数値データ形式へ変換し、
前記解法処理において、前記変換手段により変換された固定長数値データ形式の演算結果を用いて１回のみの除算を含む固定長数値演算を実行することにより固定長数値データ形式の関節角度θを求める誤差演算手段をさらに備えることを特徴とする請求項１記載の位置制御装置。
【請求項３】
前記無誤差演算手段は、θｉ＝θ’ｉ＋Δθｉ（ｉ＝１，…，ｎ）（ここで、θｉは関節ｉの求める関節角度、θ’ｉは微小時間Δｔ前の関節ｉの関節角度、Δθｉは微小時間Δｔ経過後の関節ｉの微小関節角度変化量、ｎは求める関節角度θの数）とし、ｎ元連立一次方程式ＡΔθ＝Ｂ（ここで、Ａはその要素ａｉｊ（ｉ＝１，…，ｎ、ｊ＝１，…，ｎ）がリンク機構のリンク長及び微小時間Δｔ前の関節ｉの関節角度θ’ｉにより決定される係数行列、Ｂはその要素ｂｉ（ｉ＝１，…，ｎ）が目標データＰ及び微小時間Δｔ前の位置方向データＰ’により決定されるベクトル）をＡｎΔθ＝Ｂｎ（ここで、Ａｎは対角要素ａｎ＋１ｉｉ（ｉ＝１，…，ｎ）以外が０となる対角行列、Ｂｎは要素ｂｎ＋１ｉ（ｉ＝１，…，ｎ）のベクトル）に変形したときの係数データａｎ＋１ｉｉ及び係数データｂｎ＋１ｉを、除算を除く加算、減算及び乗算からなる無誤差演算を用いて求め、
前記変換手段は、前記無誤差演算手段により求められた係数データａｎ＋１ｉｉ及び係数データｂｎ＋１ｉを可変長整数データ形式から固定長数値データ形式へ変換し、
前記誤差演算手段は、前記変換手段により変換された固定長数値データ形式の係数データａｎ＋１ｉｉ及び係数データｂｎ＋１ｉを用いて、θｉ＝θ’ｉ＋ｂｎ＋１ｉ／ａｎ＋１ｉｉ（ｉ＝１，…，ｎ）を固定長数値演算により求めることを特徴とする請求項２記載の位置制御装置。
【請求項４】
操作者の所定部位の３次元位置及び方向を検出する検出手段をさらに備え、
前記検出手段により検出された３次元位置及び方向を前記リンク機構の所定部位の目標３次元位置データ及び目標方向データとして用いて、前記リンク機構の所定部位を目標３次元位置及び目標方向に制御することを特徴とする請求項１〜３のいずれかに記載の位置制御装置。
【請求項５】
関節を介して複数のリンクを連結したリンク機構の所定部位を目標３次元位置及び目標方向に制御するロボットアーム制御方法であって、
前記リンク機構の所定部位の目標３次元位置データ及び目標方向データからなる目標データをＰ、前記リンク機構の各関節角度をθとし、目標データＰから関節角度θを求める解法処理に使用されるデータを固定長数値データ形式から可変長整数データ形式へ変換するステップと、
前記解法処理において、変換された可変長整数データ形式の係数データを用いて、除算を除く加算、減算及び乗算からなる無誤差演算を実行するステップとを含むことを特徴とする位置制御方法。
【請求項６】
関節を介して複数のリンクを連結したリンク機構の所定部位を目標３次元位置及び目標方向に制御するための位置制御プログラムであって、
前記リンク機構の所定部位の目標３次元位置データ及び目標方向データからなる目標データをＰ、前記リンク機構の各関節角度をθとし、目標データＰから関節角度θを求める解法処理に使用されるデータを固定長数値データ形式から可変長整数データ形式へ変換する変換手段と、
前記解法処理において、前記変換手段により変換された可変長整数データ形式の係数データを用いて、除算を除く加算、減算及び乗算からなる無誤差演算を実行する無誤差演算手段としてコンピュータを機能させることを特徴とする位置制御プログラム。
【請求項１】
関節を介して複数のリンクを連結したリンク機構の所定部位を目標３次元位置及び目標方向に制御する位置制御装置であって、
リンク機構の所定部位の目標３次元位置データ及び目標方向データからなる目標データをＰ、リンク機構の各関節角度をθとし、目標データＰから関節角度θを求める解法処理に使用されるデータを固定長数値データ形式から可変長整数データ形式へ変換する変換手段と、
前記解法処理において、前記変換手段により変換された可変長整数データ形式のデータを用いて、除算を除く加算、減算及び乗算からなる無誤差演算を実行する無誤差演算手段とを備えることを特徴とする位置制御装置。
【請求項２】
前記変換手段は、前記無誤差演算手段の演算結果を可変長整数データ形式から固定長数値データ形式へ変換し、
前記解法処理において、前記変換手段により変換された固定長数値データ形式の演算結果を用いて１回のみの除算を含む固定長数値演算を実行することにより固定長数値データ形式の関節角度θを求める誤差演算手段をさらに備えることを特徴とする請求項１記載の位置制御装置。
【請求項３】
前記無誤差演算手段は、θｉ＝θ’ｉ＋Δθｉ（ｉ＝１，…，ｎ）（ここで、θｉは関節ｉの求める関節角度、θ’ｉは微小時間Δｔ前の関節ｉの関節角度、Δθｉは微小時間Δｔ経過後の関節ｉの微小関節角度変化量、ｎは求める関節角度θの数）とし、ｎ元連立一次方程式ＡΔθ＝Ｂ（ここで、Ａはその要素ａｉｊ（ｉ＝１，…，ｎ、ｊ＝１，…，ｎ）がリンク機構のリンク長及び微小時間Δｔ前の関節ｉの関節角度θ’ｉにより決定される係数行列、Ｂはその要素ｂｉ（ｉ＝１，…，ｎ）が目標データＰ及び微小時間Δｔ前の位置方向データＰ’により決定されるベクトル）をＡｎΔθ＝Ｂｎ（ここで、Ａｎは対角要素ａｎ＋１ｉｉ（ｉ＝１，…，ｎ）以外が０となる対角行列、Ｂｎは要素ｂｎ＋１ｉ（ｉ＝１，…，ｎ）のベクトル）に変形したときの係数データａｎ＋１ｉｉ及び係数データｂｎ＋１ｉを、除算を除く加算、減算及び乗算からなる無誤差演算を用いて求め、
前記変換手段は、前記無誤差演算手段により求められた係数データａｎ＋１ｉｉ及び係数データｂｎ＋１ｉを可変長整数データ形式から固定長数値データ形式へ変換し、
前記誤差演算手段は、前記変換手段により変換された固定長数値データ形式の係数データａｎ＋１ｉｉ及び係数データｂｎ＋１ｉを用いて、θｉ＝θ’ｉ＋ｂｎ＋１ｉ／ａｎ＋１ｉｉ（ｉ＝１，…，ｎ）を固定長数値演算により求めることを特徴とする請求項２記載の位置制御装置。
【請求項４】
操作者の所定部位の３次元位置及び方向を検出する検出手段をさらに備え、
前記検出手段により検出された３次元位置及び方向を前記リンク機構の所定部位の目標３次元位置データ及び目標方向データとして用いて、前記リンク機構の所定部位を目標３次元位置及び目標方向に制御することを特徴とする請求項１〜３のいずれかに記載の位置制御装置。
【請求項５】
関節を介して複数のリンクを連結したリンク機構の所定部位を目標３次元位置及び目標方向に制御するロボットアーム制御方法であって、
前記リンク機構の所定部位の目標３次元位置データ及び目標方向データからなる目標データをＰ、前記リンク機構の各関節角度をθとし、目標データＰから関節角度θを求める解法処理に使用されるデータを固定長数値データ形式から可変長整数データ形式へ変換するステップと、
前記解法処理において、変換された可変長整数データ形式の係数データを用いて、除算を除く加算、減算及び乗算からなる無誤差演算を実行するステップとを含むことを特徴とする位置制御方法。
【請求項６】
関節を介して複数のリンクを連結したリンク機構の所定部位を目標３次元位置及び目標方向に制御するための位置制御プログラムであって、
前記リンク機構の所定部位の目標３次元位置データ及び目標方向データからなる目標データをＰ、前記リンク機構の各関節角度をθとし、目標データＰから関節角度θを求める解法処理に使用されるデータを固定長数値データ形式から可変長整数データ形式へ変換する変換手段と、
前記解法処理において、前記変換手段により変換された可変長整数データ形式の係数データを用いて、除算を除く加算、減算及び乗算からなる無誤差演算を実行する無誤差演算手段としてコンピュータを機能させることを特徴とする位置制御プログラム。
【図１】
【図２】
【図３】
【図４】
【図５】
【図６】
【図２】
【図３】
【図４】
【図５】
【図６】
【公開番号】特開２００７−７８３７（Ｐ２００７−７８３７Ａ）
【公開日】平成１９年１月１８日（２００７．１．１８）
【国際特許分類】
【出願番号】特願２００５−３２５２７７（Ｐ２００５−３２５２７７）
【出願日】平成１７年１１月９日（２００５．１１．９）
【国等の委託研究の成果に係る記載事項】（出願人による申告）平成１７年度独立行政法人情報通信研究機構、研究テーマ「超高速知能ネットワーク社会に向けた新しいインタラクション・メディアの研究開発」に関する委託研究、産業活力再生特別措置法第３０条の適用を受ける特許出願
【出願人】（３９３０３１５８６）株式会社国際電気通信基礎技術研究所 (905)
【Ｆターム（参考）】
【公開日】平成１９年１月１８日（２００７．１．１８）
【国際特許分類】
【出願日】平成１７年１１月９日（２００５．１１．９）
【国等の委託研究の成果に係る記載事項】（出願人による申告）平成１７年度独立行政法人情報通信研究機構、研究テーマ「超高速知能ネットワーク社会に向けた新しいインタラクション・メディアの研究開発」に関する委託研究、産業活力再生特別措置法第３０条の適用を受ける特許出願
【出願人】（３９３０３１５８６）株式会社国際電気通信基礎技術研究所 (905)
【Ｆターム（参考）】
[ Back to top ]