JP4743699B2 - 位置制御装置、位置制御方法及び位置制御プログラム - Google Patents

Description

本発明は、関節を介して複数のリンクを連結したリンク機構の所定部位、例えば、先端部を目標３次元位置及び目標方向に制御する位置制御装置、位置制御方法及び位置制御プログラムに関する。

従来のロボットアームの制御方式としては、スレーブアームと同じ軸構成等を有するマスターアームを操作者が操作してスレーブアームを制御するマスターアーム方式、操作者の腕と手の主要部（各関節）にセンサを装着して操作者の腕と手の関節の動きを検出してロボットアームを制御するモーションキャプチャー方式、操作者の手首・手の甲等に付けた３次元位置方向センサにより操作者の手首・手の甲の３次元位置及び方向を検出し、このデータを用いてアーム（リンク）の各関節角度を求める逆変換を行ってロボットアームを制御する逆変換方式等の種々の方式がある。

上記のマスターアーム方式では、操作者側には、制御されるスレーブアームだけでなく、マスターアームが必要となり、システムが重装備になる。また、モーションキャプチャー方式では、操作者の腕と手の複数箇所にセンサを装着する必要があり、操作者にとって煩わしいものとなる。

一方、逆変換方式では、操作者は手の甲等に１個のセンサのみを装着するだけで済み、操作者にとって煩わしさがないが、操作者の手首の位置と方向データから各関節角度を求める逆変換が必要となる。この逆変換では、人の腕のような高い自由度（６自由度以上）を持つ軸構成における各関節角度を求める必要があるため、逆変換が非線形連立方程式となり、複雑な計算となる。

このため、本願発明者は、逆変換が非線形連立方程式とならず、単純且つ少ない演算量で厳密解を求めることができるロボットアーム・ハンドの操作方法を提案した（特許文献１参照）。この操作方法では、ロボットアームの操作者の手首に付けたセンサにより検出された３次元位置及び方向からアームの各関節角度を求めるために、浮動小数点演算を用いた誤差有り演算を用いている。この浮動小数点フォーマットを用いた実数表現は、非常に小さな数から大きな数まで表現でき、計算効率がよいという優れた特徴があるため、逆変換のような実数の数値計算に適している。
特開２００５−４６９３１号公報

しかしながら、上記の浮動小数点演算を用いた数値制御では、以下のような問題がある。
（１）浮動小数点演算では、数値演算誤差が発生し、且つその蓄積が起こるため、演算結果に大きな誤差が含まれる場合があり、ロボットアームの先端位置が大幅にずれる場合がある。
（２）上記の数値演算誤差とその蓄積のために、当初想定していなかった状態が発生し、結果として制御が破綻してロボットアーム動作が暴走する状況が発生する。
（３）上記のロボットアーム動作の暴走を防止するためには、暴走対策を施す必要があり、制御が複雑化する。

本発明の目的は、リンク機構の制御動作を安定化及び高信頼化することができるとともに、リンク機構を高精度に制御することができる位置制御装置、位置制御方法及び位置制御プログラムを提供することである。

本発明に係る位置制御装置は、関節を介して複数のリンクを連結したリンク機構の所定部位を目標３次元位置及び目標方向に制御する位置制御装置であって、リンク機構の所定部位の目標３次元位置データ及び目標方向データからなる目標データをＰ、リンク機構の各関節角度をθとし、目標データＰから関節角度θを求める解法処理に使用されるデータを固定長数値データ形式から可変長整数データ形式へ変換する変換手段と、上記解法処理において、変換手段により変換された可変長整数データ形式のデータを用いて、除算を除く加減算及び乗算からなる無誤差演算を実行する無誤差演算手段とを備え、前記変換手段は、前記無誤差演算手段の演算結果を可変長整数データ形式から固定長数値データ形式へ変換し、前記解法処理において、前記変換手段により変換された固定長数値データ形式の演算結果を用いて１回のみの除算を含む固定長数値演算を実行することにより固定長数値データ形式の関節角度θを求める誤差演算手段をさらに備えるものである。

本発明に係る位置制御装置では、目標データＰから関節角度θを求める解法処理において、この解法処理に使用されるデータが固定長数値データ形式から可変長整数データ形式へ変換され、変換された可変長整数データ形式のデータを用いて、除算を除く加算、減算及び乗算からなる無誤差演算が実行される。この無誤差演算においては、数値演算誤差が発生せず、その蓄積もないため、リンク機構の制御動作を安定化及び高信頼化することができるとともに、リンク機構を高精度に制御することができる。

この場合、目標データＰから関節角度θを求める解法処理において、可変長整数データ形式の係数データを用いて、除算を除く加算、減算及び乗算からなる無誤差演算が実行された後、この演算結果が可変長整数データ形式から固定長数値データ形式へ変換され、変換された固定長数値データ形式の演算結果を用いて１回のみの除算を含む固定長数値演算が実行され、固定長数値データ形式の関節角度θが算出されるので、上記解法処理において、数値演算誤差の発生を多くとも１回の除算によるものだけに制限して数値演算誤差を非常に小さくすることができるとともに、その蓄積を防止することができる。

無誤差演算手段は、θ_ｉ＝θ’_ｉ＋Δθ_ｉ（ｉ＝１，…，ｎ）（ここで、θ_ｉは関節ｉの求める関節角度、θ’_ｉは微小時間Δｔ前の関節ｉの関節角度、Δθ_ｉは微小時間Δｔ経過後の関節ｉの微小関節角度変化量、ｎは求める関節角度θの数）とし、ｎ元連立一次方程式ＡΔθ＝Ｂ（ここで、Ａはその要素ａ_ｉｊ（ｉ＝１，…，ｎ、ｊ＝１，…，ｎ）がリンク機構のリンク長及び微小時間Δｔ前の関節ｉの関節角度θ’_ｉにより決定される係数行列、Ｂはその要素ｂ_ｉ（ｉ＝１，…，ｎ）が目標データＰ及び微小時間Δｔ前の位置方向データＰ’により決定されるベクトル）をＡｎΔθ＝Ｂｎ（ここで、Ａｎは対角要素ａ^ｎ＋１ _ｉｉ（ｉ＝１，…，ｎ）以外が０となる対角行列、Ｂｎは要素ｂ^ｎ＋１ _ｉ（ｉ＝１，…，ｎ）のベクトル）に変形したときの係数データａ^ｎ＋１ _ｉｉ及び係数データｂ^ｎ＋１ _ｉを、除算を除く加算、減算及び乗算からなる無誤差演算を用いて求め、変換手段は、無誤差演算手段により求められた係数データａ^ｎ＋１ _ｉｉ及び係数データｂ^ｎ＋１ _ｉを可変長整数データ形式から固定長数値データ形式へ変換し、誤差演算手段は、変換手段により変換された固定長数値データ形式の係数データａ^ｎ＋１ _ｉｉ及び係数データｂ^ｎ＋１ _ｉを用いて、θ_ｉ＝θ’_ｉ＋ｂ^ｎ＋１ _ｉ／ａ^ｎ＋１ _ｉｉ（ｉ＝１，…，ｎ）を固定長数値演算により求めることが好ましい。

この場合、目標データＰから関節角度θを求める解法処理において、θ_ｉ＝θ’_ｉ＋ｂ^ｎ＋１ _ｉ／ａ^ｎ＋１ _ｉｉ以外の演算を、除算を除く加算、減算及び乗算からなる無誤差演算により実行するとともに、数値演算誤差の発生を多くともθ_ｉ＝θ’_ｉ＋ｂ^ｎ＋１ _ｉ／ａ^ｎ＋１ _ｉｉにおける１回の除算だけに制限しているので、関節角度θを高精度に求めることができる。

上記位置制御装置は、操作者の所定部位の３次元位置及び方向を検出する検出手段をさらに備え、検出手段により検出された３次元位置及び方向をリンク機構の所定部位の目標３次元位置データ及び目標方向データとして用いて、リンク機構の所定部位を目標３次元位置及び目標方向に制御することが好ましい。

この場合、リンク機構の所定部位を操作者の所定部位の動きに応じた目標３次元位置及び目標方向に高精度に制御することができる。

本発明に係る位置制御方法は、関節を介して複数のリンクを連結したリンク機構の所定部位を目標３次元位置及び目標方向に制御するロボットアーム制御方法であって、リンク機構の所定部位の目標３次元位置データ及び目標方向データからなる目標データをＰ、リンク機構の各関節角度をθとし、目標データＰから関節角度θを求める解法処理に使用されるデータを固定長数値データ形式から可変長整数データ形式へ変換する変換ステップと、上記解法処理において、変換された可変長整数データ形式のデータを用いて、除算を除く加算、減算及び乗算からなる無誤差演算を実行する無誤差演算ステップとを含み、前記変換ステップは、前記無誤差演算ステップにおける演算結果を可変長整数データ形式から固定長数値データ形式へ変換するステップを含み、前記解法処理において、前記変換ステップにおいて変換された固定長数値データ形式の演算結果を用いて１回のみの除算を含む固定長数値演算を実行することにより固定長数値データ形式の関節角度θを求める誤差演算ステップをさらに含むものである。

本発明に係る位置制御プログラムは、関節を介して複数のリンクを連結したリンク機構の所定部位を目標３次元位置及び目標方向に制御するための位置制御プログラムであって、リンク機構の所定部位の目標３次元位置データ及び目標方向データからなる目標データをＰ、リンク機構の各関節角度をθとし、目標データＰから関節角度θを求める解法処理に使用されるデータを固定長数値データ形式から可変長整数データ形式へ変換する変換手段と、上記解法処理において、変換手段により変換された可変長整数データ形式の係数データを用いて、除算を除く加算、減算及び乗算からなる無誤差演算を実行する無誤差演算手段としてコンピュータを機能させ、前記変換手段は、前記無誤差演算手段の演算結果を可変長整数データ形式から固定長数値データ形式へ変換し、前記解法処理において、前記変換手段により変換された固定長数値データ形式の演算結果を用いて１回のみの除算を含む固定長数値演算を実行することにより固定長数値データ形式の関節角度θを求める誤差演算手段として前記コンピュータをさらに機能させるものである。

本発明によれば、目標データＰから関節角度θを求める解法処理において、可変長整数データ形式のデータを用いて、除算を除く加算、減算及び乗算からなる無誤差演算が実行され、この無誤差演算においては、数値演算誤差が発生せず、その蓄積もないため、リンク機構の制御動作を安定化及び高信頼化することができるとともに、リンク機構を高精度に制御することができる。

以下、本発明の一実施の形態による位置制御装置の一例であるロボットアーム制御装置をいたロボットアーム制御システムについて図面を参照しながら説明する。

図１は、本発明の一実施の形態によるロボットアーム制御装置を用いたロボットアーム制御システムの構成を示すブロック図である。図１に示すロボットアーム制御システムは、ロボットアーム制御装置１、ロボットアーム部２、３次元操作グローブ３、３次元位置方向センサ４及び３次元ディスプレイ５を備える。

ロボットアーム部２は、一対のロボットアーム２１及びハンド２２、胴体部２３並びにカメラ部２４を備え、人の上半身（頭部を除く）と同程度の大きさと自由度とを有し、両腕と両手の機能を実現している。

ロボットアーム２１は、肩、肘及び手首に関節を有し、これらの関節を介してリンクとなる上腕２１ａ及び前腕２１ｂが連結されたリンク機構（マニプレータ）であり、人の腕と同程度の大きさ及び自由度を有する。ハンド２２は、人の手と同程度の大きさと自由度を有する触覚付きの５本指ロボットハンドであり、前腕２１ｂの先端に取り付けられる。ハンド２２には触覚センサ（図示省略）が設けられており、物をつかんだ時の触覚（圧力）がロボットアーム制御装置１を介して３次元操作グローブ３へ伝達される。

なお、図示を省略しているが、ロボットアーム２１及びハンド２２は、その内部にモータ等の駆動源を有し、この駆動源により任意の角度に駆動される。また、本実施の形態では、ロボットアーム２１を人の腕に模倣させ、ハンド２２を人の手指に模倣させているが、ロボットアーム及びハンドの構成は、この例に特に限定されず、ハンド２２の指の数を増減させる等の種々の変更が可能である。また、リンク機構の関節数及びリンク数も、上記の例に特に限定されず、種々の変更が可能である。

カメラ部２４は、３次元カメラ（ステレオカメラ）等から構成され、ロボットアーム制御装置１を介して、撮影した画像を３次元ディスプレイ５へ出力する。３次元ディスプレイ５は、カメラ部２４により撮影された画像を表示し、操作者Ｍに提示する。なお、撮影された画像の提示方法は、この例に特に限定されず、３次元ディスプレイに替えて、操作者Ｍに装着させたヘッドマウントディスプレイに表示させる等の種々の変更が可能である。

３次元操作グローブ３は、公知の３次元操作用データグローブから構成され、操作者Ｍの両手にそれぞれ装着される。３次元操作グローブ３は、操作者Ｍの手の甲及び指の動き（関節角度）を検出し、検出した関節角度データをロボットアーム制御装置１へ出力する。３次元操作グローブ３の手首の位置には、３次元位置方向センサ４が取り付けられ、３次元位置方向センサ４は、前腕２１ｂの先端部の目標３次元位置及び目標方向として、操作者Ｍの手首の３次元位置及び方向を検出し、検出した３次元位置データ及び方向データをロボットアーム制御装置１へ出力する。

なお、目標３次元位置及び目標方向を検出又は入力する方法は、上記の例に特に限定されず、データグローブ又はデータスーツ等から操作者の所定部位の３次元位置及び方向を検出したり、ハプティクス・デバイス等を用いて入力したりしてもよい。

ロボットアーム制御装置１は、操作者Ｍの両手に装着された３次元操作グローブ３及び３次元位置方向センサ４からの検出結果を基に、ロボットアーム２１及びハンド２２の各関節角度を算出する。ハンド２２に関しては、３次元操作グローブ３が手の甲の曲がり（手首の）角度及び指の関節角度を検出し、ロボットアーム制御装置１は、これらの角度データに基づいてハンド２２を制御する。

一方、ロボットアーム２１に関しては、操作者Ｍの手首に装着した３次元位置方向センサ４から操作者Ｍの手首の三次元位置及びその方向、すなわち、ロボットアーム２１の先端部の目標３次元位置及び目標方向が得られるだけである。このため、ロボットアーム２１の先端部の目標３次元位置データ及び目標方向データから、ロボットアーム２１の各関節角度を計算する必要がある。このように、ロボットアーム２１の先端部の３次元位置及び方向からロボットアーム２１の各関節角度を計算することを逆変換と呼び、これとは逆に、ロボットアーム２１の各関節角度からロボットアーム２１の先端部の３次元位置及び方向を計算することを順変換と呼ぶことにする。

図２は、図１に示すロボットアーム制御装置１の構成を示すブロック図である。図２に示すように、ロボットアーム制御装置１は、外部インターフェース部１１、浮動小数点データ記憶部１２、データ変換処理部１３、可変長整数データ記憶部１４、誤差有制御処理部１５及び無誤差制御処理部１６を備える。誤差有制御処理部１５は、座標変換処理部１５ａ及び浮動小数点演算部１５ｂを備え、無誤差制御処理部１６は、方程式解法処理部１６ａ及び可変長整数演算部１６ｂを備える。

ロボットアーム制御装置１は、ＣＰＵ（中央演算処理装置）、ＲＯＭ（リードオンリメモリ）、ＲＡＭ（ランダムアクセスメモリ）、外部記憶装置、記録媒体駆動装置、入力装置、表示装置、外部機器インターフェース等を備えるコンピュータから構成することができる。この場合、後述するロボットアーム制御処理（位置制御処理）を実行するためのロボットアーム制御プログラム（位置制御プログラム）を、ＣＰＵ等を用いて実行することにより、外部インターフェース部１１、浮動小数点データ記憶部１２、データ変換処理部１３、可変長整数データ記憶部１４、誤差有制御処理部１５及び無誤差制御処理部１６としてコンピュータを機能させることができる。なお、ロボットアーム制御装置１の構成は、この例に特に限定されず、上記の各機能の一部又は全部を専用のハードウエアから構成する等の種々の変更が可能である。

外部インターフェース部１１は、３次元位置方向センサ４から３次元位置データ及び方向データを取得して浮動小数点データ記憶部１２に格納する。また、外部インターフェース部１１は、ロボットアーム制御処理により求められた関節角度を浮動小数点データ記憶部１２から読み出してロボットアーム部２へ出力し、ロボットアーム２１の各関節角度がロボットアーム制御処理により求められた関節角度に制御される。

浮動小数点データ記憶部１２は、誤差有制御処理部１５において処理される種々のデータを浮動小数点データ形式で記憶し、可変長整数データ記憶部１４は、無誤差制御処理部１６において処理される種々のデータを可変長整数データ形式で記憶する。

図３は、浮動小数点データ及び可変長整数データのデータ構成の一例を示す図である。図３の（ａ）に示すように、固定長数値データの一例である浮動小数点データは、符号部、指数部及び仮数部から構成され、このようなデータ形式で浮動小数点データ記憶部１２に格納される。また、図３の（ｂ）に示すように、可変長整数データは、語長部、符号部、及び数値部（整数表現）から構成され、このようなデータ形式で可変長整数データ記憶部１４に格納される。

データ変換処理部１３は、浮動小数点データ記憶部１２から浮動小数点データを読み出し、読み出した浮動小数点データを可変長整数データへ変換し、変換した可変長整数データを可変長整数データ記憶部１４に格納する。また、データ変換処理部１３は、可変長整数データ記憶部１４から可変長整数データを読み出し、読み出した可変長整数データを浮動小数点データへ変換し、変換した浮動小数点データを浮動小数点データ記憶部１２に格納する。

なお、本発明に用いられる固定長数値データは、上記の例に特に限定されず、例えば、固定長整数データを用いることもできる。図３の（ｃ）に示すように、固定長整数データは、符号部及び数値部（整数表現）から構成され、この固定長整数データ形式も、固定長であるため、誤差（数値のオーバーフロー及びそのまるめによる誤差）が発生し、本発明を同様に適用することができる。

例えば、３次元位置方向センサ４が出力する値（角度）が、−２５５〜＋２５５等の整数値であり、ロボットアーム２１の関節を動作させるモータに対する出力（回転角度）も、−１０００〜＋１０００パルス等の整数値である場合、浮動小数点データ記憶部１２を固定長数値データ記憶部に変更し、固定長整数データ形式で各データを格納するようにしてもよい。また、固定長整数データ形式でデータを取り扱う場合、浮動小数点演算部１５ｂを固定長整数演算部に変更し、固定長整数演算部により固定長整数演算を実行するようにしてもよい。

誤差有制御処理部１５は、ロボットアーム部２のロボットアーム２１及びハンド２２の制御処理のうち、固定長数値演算の一例である浮動小数点演算を用いた誤差有制御処理を実行する。座標変換処理部１５ａは、浮動小数点データ記憶部１２から浮動小数点データを読み出し、浮動小数点演算部１５ｂを用いて座標変換処理等（ｓｉｎ、ｃｏｓ演算）を浮動小数点演算で実行し、演算結果を浮動小数点データ記憶部１２に格納する。浮動小数点演算部１５ｂは、加算器、減算器、乗算器及び除算器としての機能を有し、浮動小数点演算を用いて、加算、減算、乗算及び除算を実行する。

無誤差制御処理部１６は、ロボットアーム部２のロボットアーム２１及びハンド２２の制御処理のうち、可変長整数演算を用いた無誤差制御処理を実行する。方程式解法処理部１６ａは、可変長整数データ記憶部１４から可変長整数データを読み出し、制御方程式の求解等を可変長整数演算部１６ｂを用いて可変長整数演算で実行し、演算結果を可変長整数データ記憶部１４に格納する。可変長整数演算部１６ｂは、加算器、減算器及び乗算器としての機能を有し、可変長整数演算を用いて、加算、減算及び乗算を無誤差で実行する。

ここで、本実施の形態の制御対象となるロボットアーム２１について詳細に説明する。図４は、図１に示すロボットアーム部２における右側のロボットアーム２１の構成を示す模式図であり、図５は、図１に示す右側のロボットアーム２１の座標系を説明するための図である。なお、左側のロボットアーム２１も、図４及び図５に示す右側のロボットアーム２１と同様に構成されているので、詳細な説明は省略する。

図４に示すように、グローバル座標系の原点Ｏを上腕２１ａの右肩（第１関節）に、グローバル座標系のＸ座標軸を胴体部２３（図１参照）の正面方向に沿って床面と平行方向に、グローバル座標系のＹ座標軸を右肩から左肩方向に、グローバル座標系のＺ座標軸を床面から天井への垂直方向にそれぞれ設定する。また、ロボットアーム２１の上腕２１ａの長さをｅ_１、前腕２１ｂの長さをｅ_２とし、上腕２１ａの肩関節のうち第１関節（Ｙ軸回り）の関節角度θ_１、第２関節（Ｘ軸回り）の関節角度θ_２、第３関節（Ｚ軸回り）の関節角度θ_３とし、上腕２１ａと前腕２１ｂとの間の第４関節（Ｙ軸回り）の関節角度θ_４、第５関節（Ｚ軸回り）の関節角度θ_５とし、前腕２１ｂとハンド２２との間の第６関節（Ｙ軸回り）の関節角度θ_６とする。

このとき、前腕２１ｂの先端位置・方向Ｐ_ｓすなわち操作者Ｍの手首に装着した３次元位置方向センサ４が示すグローバル座標に対する変換行列をＨ_０ｓとすると、以下の式（１）のように表現することができる。

ここで、図５に示すように、３次元位置方向センサ４の３次元位置はＰ_ｓ（ｘ_ｓ，ｙ_ｓ，ｚ_ｓ）、３次元位置方向センサ４の方向を表すＸ軸方向の単位ベクトルはｄ_ｘｓ（ｕ_ｘｓ，ｖ_ｘｓ，ｗ_ｘｓ）、Ｙ軸方向の単位ベクトルはｄ_ｙｓ（ｕ_ｙｓ，ｖ_ｙｓ，ｗ_ｙｓ）、Ｚ軸方向の単位ベクトルはｄ_ｚｓ（ｕ_ｚｓ，ｖ_ｚｓ，ｗ_ｚｓ）となる。これらの値はすべて、３次元位置方向センサ４から取得されるので、既知となる。一方、Ｈ_０ｓの各要素は、各アーム（上腕２１ａ及び前腕２１ｂ）の軸の長さｅ_１，ｅ_２と各関節角度θ_１，θ_２，θ_３，θ_４，θ_５を用いて、以下のように表現することができる。

ここで、Ｃ_ｎ＝ｃｏｓ（θ_ｎ）、Ｓ_ｎ＝ｓｉｎ（θ_ｎ）（ｎ＝１，…，５）であり、関節角度θ_ｎは、アーム動作により刻々と変化するので、時間ｔの関数となり、θ_ｎ’を微小時間Δｔ前の関節角度（直前の関節角度）とし、微小時間Δｔ経過後における微小関節角度変化量をΔθ_ｎとすると、以下のように表現することができる。
θ_ｎ＝θ_ｎ’＋Δθ_ｎ （６）
ここで、微小時間Δｔ前の関節角度値θ_ｎ’は既知であり、微小時間Δｔは制御時間間隔である。

上記の前提を基に、以下のように近似することができる。なお、下記式では、（Δθ）^２は微小であるので、（Δθ）^２以上の高次項は無視できるとした。
ｓｉｎ（θ）＝ｓｉｎ（θ’＋Δθ）≒ｓｉｎ（θ’）＋（ｓｉｎ（θ’））’Δθ＝ｓｉｎ（θ’）＋ｃｏｓ（θ’）Δθ （７ａ）
ｃｏｓ（θ）＝ｃｏｓ（θ’＋Δθ）≒ｃｏｓ（θ’）＋（ｃｏｓ（θ’））’Δθ＝ｃｏｓ（θ’）−ｓｉｎ（θ’）Δθ （７ｂ）
上記の式（７ａ）、（７ｂ）を式（２）〜（５）へ代入すると、以下となる。なお、ここでも、Δθ_１Δθ_２等の高次のΔθは微小であり、無視できるとした。

上記各式より、式（１）は、以下のように表現することができる。

そこで、Δθ_１，Δθ_２，Δθ_３，Δθ_４，Δθ_５は、以下の５元連立一次方程式の解として求めることができる。

また、上式の左辺の行列の各要素を係数ａ_ｉｊ（ｉ＝１，…，５、ｊ＝１，…，５）、右辺のベクトルの各要素を係数ｂ_ｉ（ｉ＝１，…，５）で表すと、以下のように表現することができる。

上記の５元連立一次方程式の解法では、加減乗除算が繰り返されるために、浮動小数点演算を用いた数値計算では、数値演算誤差とその蓄積のために解の誤差が大きくなり、連立方程式の解を求める処理が演算誤差のために暴走及び破綻することが多々ある。このように解の誤差が大きくなること、及び連立方程式の解を求める処理が暴走及び破綻することは、ロボットアームが人に危害を加える恐れがあるため、ロボットアームの制御においては許されない。また、除算を使用する場合、数値計算に計算精度の低下等の様々な悪影響を及ぼすことはもちろんのこと、無誤差演算自体を行うことはできない。

このため、本実施の形態では、ガウス・ジョルダンの消去法を基本にし、最後の１回を除いて除算が発生しない無誤差連立一次方程式解法処理を考案し、式（１５）の解法に誤差を発生させない無誤差演算を用い、各関節角度θ_１，θ_２，θ_３，θ_４，θ_５を以下のようにして求めている。

まず、式（１５）の係数ａ_ｉｊ（ｉ＝１，…，５、ｊ＝１，…，５）及び係数ｂ_ｉ（ｉ＝１，…，５）を用いて、ｋ＝１，…，５として、下記の式を繰り返し演算することにより係数ａ^６ _ｉｊ及び係数ｂ^６ _ｉを求める。
ａ^ｋ＋１ _ｉｊ＝ａ^ｋ _ｉｊ （ｉ＝ｋ） （１６ａ）
ａ^ｋ＋１ _ｉｊ＝ａ^ｋ _ｉｊａ^ｋ _ｋｋ−ａ^ｋ _ｋｊａ^ｋ _ｉｋ （ｉ≠ｋ） （１６ｂ）
ｂ^ｋ＋１ _ｉ＝ｂ^ｋ _ｉ （ｉ＝ｋ） （１６ｃ）
ｂ^ｋ＋１ _ｉ＝ｂ^ｋ _ｉａ^ｋ _ｋｋ−ｂ^ｋ _ｋａ^ｋ _ｉｋ （ｉ≠ｋ） （１６ｄ）
上記の係数ａ^６ _ｉｊ及び係数ｂ^６ _ｉにより、式（１５）は以下の形式となる。

上記のように、式（１７）の左辺の係数行列は、対角要素以外が０となる対角行列となる。上記の演算までは、除算を使用する必要がないため、本実施の形態では、式（１５）の連立一次方程式の解法処理のうち上記の演算までの処理を、無誤差制御処理部１６による無誤差演算を用いて実行し、ここまでの演算を無誤差で行っている。

次に、各関節角度θ_１，θ_２，θ_３，θ_４，θ_５は、以下の式により求めることができる。

上記の式（１８）の演算だけは、除算が回避できないために、本実施の形態では、誤差有制御処理部１５による浮動小数点演算を用いて実行している。この浮動小数点演算により誤差が発生するが、この処理は最終処理であり、且つ１回の除算を行うだけであるため、除算に充分な精度が確保されていれば、誤差が問題になることはなく、誤差の蓄積も起こらない。なお、手首の角度θ_６は、３次元操作グローブ３により直接取得することができるので、上記の無誤差連立一次方程式解法処理からは求めていない。

ロボットアーム制御装置１は、上記の無誤差連立一次方程式解法処理（目標データＰから関節角度θを求める解法処理）を以下のようにして実行している。

外部インターフェース部１１は、ロボットアームの２１の先端部の目標３次元位置（３次元位置方向センサ４の３次元位置Ｐ_ｓ）及び目標方向（Ｘ、Ｙ、Ｚ軸方向の単位ベクトルｄ_ｘｓ、ｄ_ｙｓ、ｄ_ｚｓ）からなる目標データＰを３次元位置方向センサ４から取得し、浮動小数点データ記憶部１２に浮動小数点データ形式で記憶させる。このとき、浮動小数点データ記憶部１２には、微小時間Δｔ前の位置方向データＰ’及び微小時間Δｔ前の関節角度データθ’_ｉが記憶されており、また、上腕２１ａの長さｅ_１、前腕２１ｂの長さｅ_２等の初期値は予め記憶されているものとする。

ここで、上記の微小時間Δｔ前の位置方向データＰ’は、微小時間Δｔ前の目標データではなく、微小時間Δｔ前の実際の３次元位置データ及び方向データであり、微小時間Δｔ前の関節角度データθ’_ｉもアームの実際の関節角度である。これは、後述するように、Ｐ’、θ’及びＰからΔθを求め（線形近似後、連立一次方程式を無誤差で解く）、そして、θ＝θ’＋Δθからθを求めており、ここで（線形）近似しているため、このθから求めた実際の位置方向データは目標データと一致せず、次の時刻の計算では、微小時間Δｔ前の目標データ等ではなく、微小時間Δｔ前の実際の３次元位置データ及び方向データ、並びに微小時間Δｔ前の実際の関節角度データを用いているためである。

座標変換処理部１５ａは、浮動小数点データ記憶部１２から微小時間Δｔ前の関節角度θ’_ｉ、上腕２１ａの長さｅ_１、前腕２１ｂの長さｅ_２を読み出し、浮動小数点演算部１５ｂを用いて浮動小数点演算を実行することにより式（１５）の係数データａ_ｉｊを算出し、浮動小数点データ記憶部１２に記憶させる。また、座標変換処理部１５ａは、浮動小数点データ記憶部１２から目標データＰ及び微小時間Δｔ前の位置方向データＰ’を読み出し、浮動小数点演算部１５ｂを用いて浮動小数点演算を実行することにより式（１５）の係数データｂ_ｉを算出し、浮動小数点データ記憶部１２に記憶させる。

データ変換処理部１３は、浮動小数点データ記憶部１２から係数ａ_ｉｊ及び係数ｂ_ｉを順次読み出し、浮動小数点データ形式から可変長整数データ形式に変換した係数ａ_ｉｊ及び係数ｂ_ｉを、係数ａ^１ _ｉｊ及び係数ｂ^１ _ｉとして可変長整数データ記憶部１４に格納する。

方程式解法処理部１６ａは、可変長整数データ記憶部１４から係数ａ^１ _ｉｊ及び係数ｂ^１ _ｉを読み出し、可変長整数演算部１６ｂを用いて可変長整数演算（無誤差演算）を実行することにより、式（１６ａ）乃至（１６ｄ）を用いて係数ａ^ｋ＋１ _ｉｊ及びｂ^ｋ＋１ _ｉを順次算出し、最終的に係数ａ^６ _ｉｊ及びｂ^６ _ｉを可変長整数データ記憶部１４に格納する。

データ変換処理部１３は、可変長整数データ記憶部１４から係数ａ^６ _ｉｉ及び係数ｂ^６ _ｉを順次読み出し、可変長整数データ形式から浮動小数点データ形式に変換した係数ａ^６ _ｉｉ及び係数ｂ^６ _ｉを浮動小数点データ記憶部１２に格納する。

座標変換処理部１５ａは、浮動小数点データ記憶部１２から係数ａ^６ _ｉｉ及び係数ｂ^６ _ｉを順次読み出し、浮動小数点演算部１５ｂを用いて浮動小数点演算の除算を１回だけ実行することにより、微小関節角度変化量Δθ_ｉ＝ｂ^６ _ｉ／ａ^６ _ｉｉを算出し、浮動小数点データ記憶部１２に格納する。そして、座標変換処理部１５ａは、浮動小数点データ記憶部１２から直前の関節角度値θ’_ｉ及び微小関節角度変化量Δθ_ｉを順次読み出し、浮動小数点演算部１５ｂを用いて浮動小数点演算を実行することにより式（１８）を用いて関節角度θ_ｉを算出し、浮動小数点データ記憶部１２に格納する。

外部インターフェース部１１は、浮動小数点データ記憶部１２から関節角度θ_ｉを読み出してロボットアーム部２へ出力する。ロボットアーム部２は、ロボットアーム２１の各関節角度が関節角度θ_ｉとなるようにロボットアーム２１を動作させる。

上記の制御動作により、操作者Ｍは、３次元ディスプレイ５を用いて、カメラ部２４により撮影された画像を見ながら、両手に装着した３次元操作グローブ３を用いて、ロボットアーム２１及びハンド２２を操作し、操作者Ｍの腕と手の動きとがロボットアーム２１及びハンド２２により忠実に再現される。

本実施の形態では、データ変換処理部１３が変換手段の一例に相当し、無誤差制御処理部１６（方程式解法処理部１６ａ及び可変長整数演算部１６ｂ）が無誤差演算手段の一例に相当し、誤差有制御処理部１５（座標変換処理部１５ａ及び浮動小数点演算部１５ｂ）が誤差演算手段の一例に相当する。

次に、上記のように構成されたロボットアーム制御装置１によるロボットアーム制御処理について説明する。図６は、図１に示すロボットアーム制御装置１によるロボットアーム制御処理を説明するためのフローチャートである。

図６に示すように、まず、ステップＳ１１において、外部インターフェース部１１は、３次元位置方向センサ４から３次元位置データＰ_ｓ（ｘ_ｓ，ｙ_ｓ，ｚ_ｓ）、Ｘ軸方向の単位ベクトルｄ_ｘｓ（ｕ_ｘｓ，ｖ_ｘｓ，ｗ_ｘｓ）、Ｙ軸方向の単位ベクトルｄ_ｙｓ（ｕ_ｙｓ，ｖ_ｙｓ，ｗ_ｙｓ）、及びＺ軸方向の単位ベクトルｄ_ｚｓ（ｕ_ｚｓ，ｖ_ｚｓ，ｗ_ｚｓ）を取得して浮動小数点データ記憶部１２に格納する。

次に、ステップＳ１２において、座標変換処理部１５ａは、浮動小数点データ記憶部１２から３次元位置データＰ_ｓ（ｘ_ｓ，ｙ_ｓ，ｚ_ｓ）等を順次読み出し、浮動小数点演算部１５ｂを用いて、式（１５）の係数ａ_ｉｊ（ｉ＝１，…，５、ｊ＝１，…，５）及び係数ｂ_ｉ（ｉ＝１，…，５）を求め、浮動小数点データ記憶部１２に格納する。

次に、ステップＳ１３において、データ変換処理部１３は、浮動小数点データ記憶部１２から係数ａ_ｉｊ及び係数ｂ_ｉを順次読み出し、浮動小数点データ形式から可変長整数データ形式に変換した係数ａ_ｉｊ及び係数ｂ_ｉを、係数ａ^１ _ｉｊ及び係数ｂ^１ _ｉとして可変長整数データ記憶部１４に格納する。

なお、上記のステップＳ１２及びＳ１３では、浮動小数点演算を用いて３次元位置データＰ_ｓ（ｘ_ｓ，ｙ_ｓ，ｚ_ｓ）等から係数ａ_ｉｊ及び係数ｂ_ｉを求めた後に、可変長整数データへフォーマット変換したが、この例に特に限定されず、データ変換処理部１３により３次元位置データＰ_ｓ（ｘ_ｓ，ｙ_ｓ，ｚ_ｓ）、Ｘ軸方向の単位ベクトルｄ_ｘｓ（ｕ_ｘｓ，ｖ_ｘｓ，ｗ_ｘｓ）、Ｙ軸方向の単位ベクトルｄ_ｙｓ（ｕ_ｙｓ，ｖ_ｙｓ，ｗ_ｙｓ）、及びＺ軸方向の単位ベクトルｄ_ｚｓ（ｕ_ｚｓ，ｖ_ｚｓ，ｗ_ｚｓ）を浮動小数点データから可変長整数データに変換し、その後、方程式解法処理部１６ａにより可変長整数データの３次元位置データＰ_ｓ（ｘ_ｓ，ｙ_ｓ，ｚ_ｓ）等から可変長整数データの係数ａ_ｉｊ及び係数ｂ_ｉを求めるようにしてもよい。

次に、ステップＳ１４〜Ｓ１６において、方程式解法処理部１６ａは、初期設定として、ｋ＝１、ｉ＝１、ｊ＝１に設定する。次に、ステップＳ１７において、方程式解法処理部１６ａは、可変長整数データ記憶部１４から係数ａ^ｋ _ｉｊ，ａ^ｋ _ｋｋ，ａ^ｋ _ｋｊ，ａ^ｋ _ｉｋを読み出し、可変長整数演算部１６ｂを用いて、ｉ＝ｋの場合はａ^ｋ＋１ _ｉｊ＝ａ^ｋ _ｉｊに設定し、ｉ≠ｋの場合はａ^ｋ＋１ _ｉｊ＝ａ^ｋ _ｉｊａ^ｋ _ｋｋ−ａ^ｋ _ｋｊａ^ｋ _ｉｋを算出し、係数ａ^ｋ＋１ _ｉｊを可変長整数データ記憶部１４に格納する。

次に、ステップＳ１８において、方程式解法処理部１６ａは、ｊ＜５であるか否かを判断し、ｊ＜５の場合、ステップＳ２７においてｊを１だけインクリメントしてステップＳ１７以降の処理を継続し、ｊ＜５でない（ｊ＝５）場合、ステップＳ１９へ処理を移行する。

ｊ＜５でない場合、ステップＳ１９において、方程式解法処理部１６ａは、可変長整数データ記憶部１４から係数ｂ^ｋ _ｉ，ａ^ｋ _ｋｋ，ｂ^ｋ _ｋ，ａ^ｋ _ｉｋを読み出し、可変長整数演算部１６ｂを用いて、ｉ＝ｋの場合はｂ^ｋ＋１ _ｉ＝ｂ^ｋ _ｉに設定し、ｉ≠ｋの場合はｂ^ｋ＋１ _ｉ＝ｂ^ｋ _ｉａ^ｋ _ｋｋ−ｂ^ｋ _ｋａ^ｋ _ｉｋを算出し、係数ｂ^ｋ＋１ _ｉを可変長整数データ記憶部１４に格納する。

次に、ステップＳ２０において、方程式解法処理部１６ａは、ｉ＜５であるか否かを判断し、ｉ＜５の場合、ステップＳ２６においてｉを１だけインクリメントしてステップＳ１６以降の処理を継続し、ｉ＜５でない（ｉ＝５）場合、ステップＳ２１へ処理を移行する。

ｉ＜５でない場合、ステップＳ２１において、方程式解法処理部１６ａは、ｋ＜５であるか否かを判断し、ｋ＜５の場合、ステップＳ２５においてｋを１だけインクリメントしてステップＳ１５以降の処理を継続し、ｋ＜５でない（ｋ＝５）場合、ステップＳ２２へ処理を移行する。このように、ステップＳ１４〜Ｓ２１及びＳ２５〜Ｓ２７までの処理が無誤差演算を用いて実行される。

次に、ステップＳ２２において、データ変換処理部１３は、可変長整数データ記憶部１４から係数ａ^６ _ｉｉ及び係数ｂ^６ _ｉ（ｉ＝１，…，５）を順次読み出し、可変長整数データ形式から浮動小数点データ形式に変換し、変換した係数ａ^６ _ｉｉ及び係数ｂ^６ _ｉを浮動小数点データ記憶部１２に格納する。

次に、ステップＳ２３において、座標変換処理部１５ａは、浮動小数点データ記憶部１２から係数ａ^６ _ｉｉ及び係数ｂ^６ _ｉ（ｉ＝１，…，５）を順次読み出し、浮動小数点演算部１５ｂを用いて、微小関節角度変化量Δθ_ｉ＝ｂ^６ _ｉ／ａ^６ _ｉｉ（ｉ＝１，…，５）を算出し、浮動小数点データ記憶部１２に格納する。

次に、ステップＳ２４において、座標変換処理部１５ａは、浮動小数点データ記憶部１２から直前の関節角度値θ’_ｉ及び微小関節角度変化量Δθ_ｉ（ｉ＝１，…，５）を順次読み出し、浮動小数点演算部１５ｂを用いて、式（１８）に従って関節角度θ_ｉ＝θ’_ｉ＋Δθ_ｉ（ｉ＝１，…，５）を算出し、浮動小数点データ記憶部１２に格納する。このとき、外部インターフェース部１１は、浮動小数点データ記憶部１２から関節角度θ_１，θ_２，θ_３，θ_４，θ_５を読み出してロボットアーム部２へ出力する。

ロボットアーム部２は、ロボットアーム２１の各関節角度が関節角度θ_１，θ_２，θ_３，θ_４，θ_５となるようにロボットアーム２１を動作させる。なお、手首の関節角度θ_６は、３次元操作グローブ３から直接取得することができるので、３次元操作グローブ３により検出された関節角度θ_６がロボットアーム制御装置１を介してロボットアーム部２へ出力され、ロボットアーム部２は、ロボットアーム２１の手首の関節角度が関節角度θ_６となるようにロボットアーム２１を動作させる。上記の処理が制御時間間隔ごとに繰り返され、ロボットアーム２１の先端位置が操作者Ｍの手首の位置に応じて正確に制御される。

上記の処理により、本実施の形態では、ロボットアーム制御処理において用いられる逆変換制御演算（連立一次方程式解法）に使用される数値を可変長の整数により表現するとともに、除算を実行することなく（最後の除算を除く）、加算、減算及び乗算のみを用いた無誤差演算処理を実行し、制御演算誤差をなくすことができる。

この結果、ロボットアーム２１の先端部を高精度に制御することができるとともに、制御処理の破綻をなくしてロボットアーム２１の動作の暴走等をなくすことができ、ロボットアーム２１の動作の安定化及び高信頼化を図ることができる。また、ロボットアーム２１の暴走防止対策等が不要となるので、ロボットアーム制御装置１の構成を単純化することができ、装置の低コスト化を実現することができる。

なお、本実施の形態では、操作者の手首の位置及び方向を検出し、これらを目標位置及び目標方向として制御したが、本発明による制御例は、この例に特に限定されず、３次元操作グローブ３、３次元位置方向センサ４及び３次元ディスプレイ５を省略し、テーチングを行うことにより浮動小数点データ記憶部１２に記憶した目標位置及び目標方向となるようにロボットアーム２１を制御する等の他の制御例にも適用可能である。また、ロボットアーム２１を制御対象としたが、この例に特に限定されず、ハンド２２を制御対象にする等の種々の変更が可能である。

本発明の一実施の形態によるロボットアーム制御装置を用いたロボットアーム制御システムの構成を示すブロック図である。 図１に示すロボットアーム制御装置の構成を示すブロック図である。 浮動小数点データ及び可変長整数データのデータ構成の一例を示す図である。 図１に示すロボットアーム部における右側のロボットアームの構成を示す模式図である。 図３に示す右側のロボットアームの座標系を説明するための図である。 図１に示すロボットアーム制御装置によるロボットアーム制御処理を説明するためのフローチャートである。

符号の説明

１ ロボットアーム制御装置
２ ロボットアーム部
３ ３次元操作グローブ
４ ３次元位置方向センサ
５ ３次元ディスプレイ
１１ 外部インターフェース部
１２ 浮動小数点データ記憶部
１３ データ変換処理部
１４ 可変長整数データ記憶部
１５ 誤差有制御処理部
１５ａ 座標変換処理部
１５ｂ 浮動小数点演算部
１６ 無誤差制御処理部
１６ａ 方程式解法処理部
１６ｂ 可変長整数演算部
２１ ロボットアーム
２１ａ 上腕
２１ｂ 前腕
２２ ハンド
２３ 胴体部
２４ カメラ部

Claims (5)

1. 関節を介して複数のリンクを連結したリンク機構の所定部位を目標３次元位置及び目標方向に制御する位置制御装置であって、
   リンク機構の所定部位の目標３次元位置データ及び目標方向データからなる目標データをＰ、リンク機構の各関節角度をθとし、目標データＰから関節角度θを求める解法処理に使用されるデータを固定長数値データ形式から可変長整数データ形式へ変換する変換手段と、
   前記解法処理において、前記変換手段により変換された可変長整数データ形式のデータを用いて、除算を除く加算、減算及び乗算からなる無誤差演算を実行する無誤差演算手段とを備え、
   前記変換手段は、前記無誤差演算手段の演算結果を可変長整数データ形式から固定長数値データ形式へ変換し、
   前記解法処理において、前記変換手段により変換された固定長数値データ形式の演算結果を用いて１回のみの除算を含む固定長数値演算を実行することにより固定長数値データ形式の関節角度θを求める誤差演算手段をさらに備えることを特徴とする位置制御装置。
2. 前記無誤差演算手段は、θ_ｉ＝θ’_ｉ＋Δθ_ｉ（ｉ＝１，…，ｎ）（ここで、θ_ｉは関節ｉの求める関節角度、θ’_ｉは微小時間Δｔ前の関節ｉの関節角度、Δθ_ｉは微小時間Δｔ経過後の関節ｉの微小関節角度変化量、ｎは求める関節角度θの数）とし、ｎ元連立一次方程式ＡΔθ＝Ｂ（ここで、Ａはその要素ａ_ｉｊ（ｉ＝１，…，ｎ、ｊ＝１，…，ｎ）がリンク機構のリンク長及び微小時間Δｔ前の関節ｉの関節角度θ’_ｉにより決定される係数行列、Ｂはその要素ｂ_ｉ（ｉ＝１，…，ｎ）が目標データＰ及び微小時間Δｔ前の位置方向データＰ’により決定されるベクトル）をＡｎΔθ＝Ｂｎ（ここで、Ａｎは対角要素ａ^ｎ＋１ _ｉｉ（ｉ＝１，…，ｎ）以外が０となる対角行列、Ｂｎは要素ｂ^ｎ＋１ _ｉ（ｉ＝１，…，ｎ）のベクトル）に変形したときの係数データａ^ｎ＋１ _ｉｉ及び係数データｂ^ｎ＋１ _ｉを、除算を除く加算、減算及び乗算からなる無誤差演算を用いて求め、
   前記変換手段は、前記無誤差演算手段により求められた係数データａ^ｎ＋１ _ｉｉ及び係数データｂ^ｎ＋１ _ｉを可変長整数データ形式から固定長数値データ形式へ変換し、
   前記誤差演算手段は、前記変換手段により変換された固定長数値データ形式の係数データａ^ｎ＋１ _ｉｉ及び係数データｂ^ｎ＋１ _ｉを用いて、θ_ｉ＝θ’_ｉ＋ｂ^ｎ＋１ _ｉ／ａ^ｎ＋１ _ｉｉ（ｉ＝１，…，ｎ）を固定長数値演算により求めることを特徴とする請求項１記載の位置制御装置。
3. 操作者の所定部位の３次元位置及び方向を検出する検出手段をさらに備え、
   前記検出手段により検出された３次元位置及び方向を前記リンク機構の所定部位の目標３次元位置データ及び目標方向データとして用いて、前記リンク機構の所定部位を目標３次元位置及び目標方向に制御することを特徴とする請求項１又は２に記載の位置制御装置。
4. 関節を介して複数のリンクを連結したリンク機構の所定部位を目標３次元位置及び目標方向に制御するロボットアーム制御方法であって、
   前記リンク機構の所定部位の目標３次元位置データ及び目標方向データからなる目標データをＰ、前記リンク機構の各関節角度をθとし、目標データＰから関節角度θを求める解法処理に使用されるデータを固定長数値データ形式から可変長整数データ形式へ変換する変換ステップと、
   前記解法処理において、変換された可変長整数データ形式の係数データを用いて、除算を除く加算、減算及び乗算からなる無誤差演算を実行する無誤差演算ステップとを含み、
   前記変換ステップは、前記無誤差演算ステップにおける演算結果を可変長整数データ形式から固定長数値データ形式へ変換するステップを含み、
   前記解法処理において、前記変換ステップにおいて変換された固定長数値データ形式の演算結果を用いて１回のみの除算を含む固定長数値演算を実行することにより固定長数値データ形式の関節角度θを求める誤差演算ステップをさらに含むことを特徴とする位置制御方法。
5. 関節を介して複数のリンクを連結したリンク機構の所定部位を目標３次元位置及び目標方向に制御するための位置制御プログラムであって、
   前記リンク機構の所定部位の目標３次元位置データ及び目標方向データからなる目標データをＰ、前記リンク機構の各関節角度をθとし、目標データＰから関節角度θを求める解法処理に使用されるデータを固定長数値データ形式から可変長整数データ形式へ変換する変換手段と、
   前記解法処理において、前記変換手段により変換された可変長整数データ形式の係数データを用いて、除算を除く加算、減算及び乗算からなる無誤差演算を実行する無誤差演算手段としてコンピュータを機能させ、
   前記変換手段は、前記無誤差演算手段の演算結果を可変長整数データ形式から固定長数値データ形式へ変換し、
   前記解法処理において、前記変換手段により変換された固定長数値データ形式の演算結果を用いて１回のみの除算を含む固定長数値演算を実行することにより固定長数値データ形式の関節角度θを求める誤差演算手段として前記コンピュータをさらに機能させることを特徴とする位置制御プログラム。

Images