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

Description

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

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

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

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

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

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

また、数値演算処理は、加算、減算、乗算及び除算の四則演算を用いて行われるが、除算は、割り切れない場合に必ず誤差が発生し、除算を含む四則演算すべてを無誤差で実行することは困難である。この場合、除算を使用しないことも考えられるが、通常、数値制御処理には加減乗除算の四則演算が用いられるため、除算が使えないということは、適用できる範囲が狭くなり、実用性が低下する。

本発明の目的は、制御動作を安定化及び高信頼化することができるとともに、制御対象を高精度に制御することができ、さらに、汎用性の高い且つ適用範囲の広い制御演算を実現することができる制御装置、制御方法及び制御プログラムを提供することである。

本発明に係る制御装置は、制御対象を制御するために用いられる制御データを求めるデータ演算処理に使用されるデータを固定長数値データ形式から可変長整数データ形式、又は可変長整数データ形式の分母データ及び分子データから構成される可変長分数データ形式へ変換する変換手段と、前記変換手段により変換された可変長整数データ形式又は可変長分数データ形式のデータを用いて、前記データ演算処理を無誤差で実行する無誤差演算手段とを備え、前記無誤差演算手段は、前記データ演算処理における除算を可変長分数データ形式で表現し、前記制御対象は、関節を介して複数のリンクを連結したリンク機構であり、前記変換手段は、前記リンク機構の所定部位の目標３次元位置データ及び目標方向データからなる目標データをＰ、前記リンク機構の各関節角度をθとし、目標データＰから関節角度θを求めるデータ演算処理に使用されるデータを固定長数値データ形式から可変長整数データ形式、又は可変長整数データ形式の分母データ及び分子データから構成される可変長分数データ形式へ変換するものである。

本発明に係る制御装置では、制御対象を制御するために用いられる制御データを求めるデータ演算処理に使用されるデータが固定長数値データ形式から可変長整数データ形式、又は可変長整数データ形式の分母データ及び分子データから構成される可変長分数データ形式へ変換され、変換された可変長整数データ形式又は可変長分数データ形式のデータを用いて、前記データ演算処理が無誤差で実行される。ここで、データ演算処理において除算が発生した場合、当該除算が保留されて可変長分数データ形式で表現されるので、可変長分数データ形式で表現された数値に対するその後の演算を継続することができ、データ演算処理を最後まで実行することができる。また、誤差が発生する除算を実際には実行していないので、数値演算誤差が発生することがなく、その蓄積も起こらない。この結果、除算を含む四則演算すべてを無誤差演算の枠組みで処理することができるので、制御動作を安定化及び高信頼化することができるとともに、制御対象を高精度に制御することができ、さらに、汎用性の高い且つ適用範囲の広い制御演算を実現することができる。
また、目標データＰから関節角度θを求めるデータ演算処理において、除算を含む四則演算すべてを無誤差演算の枠組みで処理することができるので、リンク機構の制御動作を安定化及び高信頼化することができるとともに、リンク機構を高精度に制御することができ、さらに、汎用性の高い且つ適用範囲の広い制御演算を実現することができる。

前記無誤差演算手段は、前記データ演算処理において除算の結果に余りが発生しない場合は当該除算を実行することが好ましい。

この場合、データ演算処理において除算の結果に余りが発生しないときは当該除算が実行されるので、数値演算誤差の発生及びその蓄積を防止しながら、誤差が発生しない除算を実行して除算の結果を表すデータ量を削減することができ、データ演算処理におけるデータ量の削減及び計算効率の向上を図ることができる。

前記無誤差演算手段は、前記可変長分数データ形式で表現された分母データ及び分子データを約分することが好ましい。

この場合、可変長分数データ形式で表現された分母データ及び分子データが約分されるので、数値データの桁数及びデータ量を削減することができ、データ演算処理におけるデータ量の削減及び計算効率の向上を図ることができる。

前記変換手段は、前記データ演算処理に使用されるデータを固定長数値データ形式から可変長分数データ形式へ変換し、前記無誤差演算手段は、前記変換手段により変換された可変長分数データ形式のデータを用いて、前記データ演算処理を無誤差で実行することが好ましい。

この場合、可変長分数データ形式のデータを用いてデータ演算処理が無誤差で実行されるので、使用するデータ形式が固定長数値データ形式と可変長分数データ形式との２種類のみに限定され、データの管理が容易となり、また、無誤差演算手段では可変長分数データ形式のデータのみを用いることとなり、無誤差演算手段の構成も簡略化することができる。

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

この場合、目標データＰから関節角度θを求めるデータ演算処理を、可変長分数データ形式の係数データを用いて無誤差で解くことができるので、データ演算処理が除算を含む場合でも、データ演算処理における数値演算誤差の発生及びその蓄積を完全に排除することができる。

前記変換手段は、前記無誤差演算手段により求められた可変長分数データ形式の関節角度θを固定長数値データ形式へ変換することが好ましい。

この場合、データ演算処理により求められた関節角度θが可変長分数データ形式から固定長数値データ形式へ変換されるので、固定長数値データ形式の関節角度θを用いて通常の制御を行うことができ、種々の制御対象を容易に制御することができる。

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

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

本発明に係る制御方法は、制御対象を制御するために用いられる制御データを求めるデータ演算処理に使用されるデータを固定長数値データ形式から可変長整数データ形式、又
は可変長整数データ形式の分母データ及び分子データから構成される可変長分数データ形式へ変換する第１のステップと、変換された可変長整数データ形式又は可変長分数データ形式のデータを用いて、前記データ演算処理を無誤差で実行する第２のステップとを含み、前記第２のステップは、前記データ演算処理における除算を可変長分数データ形式で表現するステップを含み、前記制御対象は、関節を介して複数のリンクを連結したリンク機構であり、前記第１のステップは、前記リンク機構の所定部位の目標３次元位置データ及び目標方向データからなる目標データをＰ、前記リンク機構の各関節角度をθとし、目標データＰから関節角度θを求めるデータ演算処理に使用されるデータを固定長数値データ形式から可変長整数データ形式、又は可変長整数データ形式の分母データ及び分子データから構成される可変長分数データ形式へ変換するステップを含むものである。

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

本発明によれば、制御対象を制御するために用いられる制御データを求めるデータ演算処理において、除算が発生した場合、当該除算が保留されて可変長分数データ形式で表現されるので、データ演算処理を最後まで実行することができるとともに、数値演算誤差が発生することがなく、その蓄積も起こらないため、除算を含む四則演算すべてを無誤差演算の枠組みで処理することができ、制御動作を安定化及び高信頼化することができるとともに、制御対象を高精度に制御することができ、さらに、汎用性の高い且つ適用範囲の広い制御演算を実現することができる。

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

なお、本発明に用いられる固定長数値データは、上記の浮動小数点データに特に限定されず、固定長整数データを用いてもよく、この場合、浮動小数点演算部１５ｂを固定長整数演算部に変更して固定長整数演算部により固定長整数演算を実行するようにしてもよい。例えば、３次元位置方向センサ４が出力する値（角度）が、−２５５〜＋２５５等の整数値であり、ロボットアーム２１の関節を動作させるモータに対する出力（回転角度）も、−１０００〜＋１０００パルス等の整数値である場合に固定長整数データを用いることができる。

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

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

可変長分数演算部１６ｂは、加算器、減算器及び乗算器としての機能を有し、可変長分数演算を用いて、加算、減算及び乗算を無誤差で実行する。また、可変長分数演算部１６ｂは、除算器としての機能をも有し、割り切れる場合は除算を行い、その結果を分母を１とする可変長分数データ形式で保持し、割り切れない場合は除算を保留し、そのまま可変長分数データ形式で保持し、割り切れないが、約分できる場合は、約分を実行し、その結果を可変長分数データ形式で保持し、以降の演算を進める。

図３は、数値データの種々のフォーマットの例を示す図である。数値データは、固定長数値データと可変長数値データとに大別される。前者（固定長数値データ）の代表例には、計算機においてよく用いられる、図３の（ａ）に示す固定長整数データ（例えば、３２ビット）と、図３の（ｂ）に示す浮動小数点データ（例えば、６４ビット）とが該当する。図３の（ａ）に示すように、固定長整数データは、符号部及び数値（整数）部から構成される。図３の（ｂ）に示すように、浮動小数点データは、符号部、指数部及び仮数部から構成され、このようなデータ形式で固定長数値データ記憶部１２に格納される。これらの固定長数値データは、固定長であるため、計算効率がよい反面、数値表現精度が限定され、演算誤差の発生とその蓄積とが起こる。

一方、可変長数値データには、図３の（ｃ）に示す可変長整数データと、図３の（ｄ）に示す可変長分数データとが該当する。図３の（ｃ）ように、可変長整数データは、語長部、符号部及び数値（整数）部から構成される。図３の（ｄ）ように、可変長分数データは、語長部、符号部及び数値（整数）部から構成される分子データと、語長部、符号部及び数値（整数）部から構成される分母データとが一対にされたデータであり、このようなデータ形式で可変長分数データ記憶部１４に格納される。これらの可変長数値データでは、数値表現精度に限界がないため、加減乗算を無誤差で演算することができ、演算誤差の発生とその蓄積とが起こらないという利点がある。

しかしながら、可変長数値データでは、データ長が固定でないため、計算効率が劣る。また、除算の場合は、割り切れずに循環小数となる場合がある。この場合、無限桁数の数値となるので、可変長数値データ形式でも対応することができず、無誤差演算を行うことはできない。

そこで、本実施の形態では、可変長分数データ記憶部１４及び無誤差制御処理部１６を用いて、可変長整数データを分子と分母とに用いた分数形式で表現して記憶及び演算を行っている。すなわち、被除数ａ／ｂ（ここで、ａは可変長整数データ形式の分子データ、ｂは可変長整数データ形式の分母データ）を除数ｃ／ｄ（ここで、ｃは可変長整数データ形式の分子データ、ｄは可変長整数データ形式の分母データ）で割る除算を実行する場合、除数ｃ／ｄの逆数ｄ／ｃを乗算してａ×ｄ（＝ｅ）及びｂ×ｃ（＝ｆ）の分子部及び分母部の乗算を実行する。次に、乗算後の被除数ｅを除数ｆで割る除算において、割り切れる場合は除算を行い、その結果を、分母を１とする可変長分数データ形式で表現して記憶及び演算を行い、一方、割り切れない場合は除算を保留し、そのまま可変長分数データ形式で記憶し、以降の演算を進める。また、除算において、割り切れないが、約分できる場合は、約分を実行し、約分後の可変長分数データを記憶してその後の演算に用いている。

上記の方法の利点としては、以下が挙げられる。
（１）加減乗算に加えて、除算も無誤差演算の枠組みで行うことができるため、非常に汎用性の高い且つ適用性の広い無誤差演算を実現できる。
（２）除算を分数形式データで表現するので、割り切れる場合は除算を無誤差で実行することができ、一方、割り切れない場合は、除算を分数形式データとして保留したまま無誤差演算を進めることができる。
（３）除算を分数形式データで表現するので、割り切れない場合でも、約分することができ、数値桁数を削減できる場合が多いため、データ量の削減及び計算効率の向上に大きく寄与することができる。

なお、除算において割り切れる場合、上記の可変長分数データ形式で表現することなく、除算結果を可変長整数データ形式で表現するようにしてもよく、可変長整数データ記憶部、可変長整数データ演算処理部及び可変長整数演算部を付加して、可変長整数データ演算処理部及び可変長整数演算部を用いて可変長整数演算を実行するようにしてもよい。この場合、分母データを可変長整数データ記憶部に記憶する必要がなくなり、データ量をより削減することができる。

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

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

このとき、３次元位置方向センサ４が装着されている前腕の手首のローカル座標系からグローバル座標への変換行列をＨ_ｓとすると、以下の式（１）のように表現することができる。

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

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

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

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

そこで、Δθ_１，Δθ_２，Δθ_３，Δθ_４，Δθ_５は、未知数が５個あるので、式（１３）の中から適切な５つの式を選択し、例えば、以下の５元連立一次方程式の解として求めることができる。

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

上記の５元連立一次方程式の解法では、加減乗除算が繰り返されるために、浮動小数点演算（固定長数値演算）を用いた数値計算では、数値演算誤差とその蓄積のために解の誤差が大きくなる場合が発生する。また、ゼロで除算が行われる場合（以下、この場合を「ゼロ割」という）も発生する。結果として、これらの演算誤差、その蓄積及びゼロ割のために、ロボットアーム動作が不安定になり、暴走及び破綻が発生する。

このような数値演算の誤差が大きくなること、ロボットアームの動作が暴走及び破綻することは、ロボットアーム制御処理においては許されない。ロボットアームの動作精度が低下したり、又は動作が暴走及び破綻すると、人に危害を加えたり、周辺の物を壊してしまうこととなる。また、除算があると、除算は数値演算に計算精度の低下等様々な悪影響を及ぼすことはもちろんのこと、除算では割り切れない場合が発生し（無限桁数が必要な循環小数の発生）、無誤差演算を行うことができない。

このため、本願発明者は、特願２００５−１６４５６７号明細書において、上記連立方程式の解法処理に関して、可変長数値（整数）データを用い且つ除算を一切使用せず、誤差を発生させない無誤差演算を行う無誤差連立一次方程式解法処理方法を考案した。これにより、安定且つ高信頼性なロボット制御方法を実現した。

しかしながら、除算を用いない無誤差演算方法は、適用できる範囲が非常に限定され、且つ計算を進めていくと、際限なく数値桁数が増大し、計算効率が著しく低下していく。結果として、実用的ではない場合も多々ある。このため、本願発明者は、上記の特願２００５−１６４５６７号明細書に記載の方法をベースにして、この方法の欠点をなくす方法を考案した。

すなわち、本実施の形態では、式（１５）の連立一次方程式の解法演算において、上記した可変長整数を用いた分数形式のデータフォーマット（可変長分数データ形式）で除算を表現して無誤差演算を行う。例えば、ガウス・ジョルダンの消去法を用いて、連立一次方程式の解を求める処理を可変長分数データ処理演算により行い、各関節角度θ_１，θ_２，θ_３，θ_４，θ_５を以下のようにして求めている。

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

上記のように、式（１７）の左辺の係数行列は、対角要素以外が０となる対角行列となる。式（６）及び式（１７）から、各関節角度θ_１，θ_２，θ_３，θ_４，θ_５は、以下の式により求めることができる。なお、手首の角度θ_６は、３次元操作グローブ３により直接取得することができるので、上記の無誤差連立一次方程式解法処理からは求めていない。

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

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

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

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

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

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

また、可変長分数データ演算処理部１６ａは、可変長分数データ記憶部１４から係数ａ^６ _ｉｉ及び係数ｂ^６ _ｉを読み出し、可変長分数演算部１６ｂを用いて可変長分数演算（無誤差演算）を実行することにより、式（１７）を基に微小関節角度変化量Δθ_ｉ＝ｂ^６ _ｉ／ａ^６ _ｉｉを算出し、さらに、式（１８）に従って関節角度θ_ｉ＝θ_ｉ’＋Δθ_ｉを算出し、可変長分数データ記憶部１４に格納する。

これらの可変長分数演算において、可変長分数データを用いて、加算、減算及び乗算はすべて実行され、除算は割り切れる場合に実行され、割り切れない場合には保留され、さらに、約分ができる場合は約分が実行される。

データ変換処理部１３は、可変長分数データ記憶部１４から関節角度θ_ｉを順次読み出して可変長分数データ形式から浮動小数点データ形式に変換し、浮動小数点データ形式の関節角度θ_ｉを固定長数値データ記憶部１２に格納する。

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

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

本実施の形態では、データ変換処理部１３が変換手段の一例に相当し、無誤差制御処理部１６（可変長分数データ演算処理部１６ａ及び可変長分数演算部１６ｂ）が無誤差演算手段の一例に相当し、３次元位置方向センサ４が検出手段の一例に相当する。

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

図６に示すように、まず、ステップＳ１１において、外部インターフェース部１１は、３次元位置方向センサ４から目標となる３次元位置方向データＨ_ｓを取得して固定長数値データ記憶部１２に格納する。

次に、ステップＳ１２において、誤差有制御処理部１５の固定長数値データ演算処理部１５ａは、固定長数値データ記憶部１２から３次元位置方向データＨ_ｓ等を読み出し、浮動小数点演算部１５ｂを用いて、式（１５）の係数ａ_ｉｊ（ｉ＝１，…，５、ｊ＝１，…，５）及び係数ｂ_ｉ（ｉ＝１，…，５）を求め、固定長数値データ記憶部１２に格納する。

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

なお、上記のステップＳ１２及びＳ１３では、浮動小数点演算を用いて３次元位置方向データＨ_ｓ等から係数ａ_ｉｊ及び係数ｂ_ｉを求めた後に、可変長分数データへフォーマット変換したが、この例に特に限定されず、データ変換処理部１３により３次元位置方向データＨ_ｓを浮動小数点データから可変長分数データに変換し、その後、無誤差制御処理部１６により可変長分数データの３次元位置方向データＨ_ｓ等から可変長分数データの係数ａ^１ _ｉｊ及び係数ｂ^１ _ｉを求めるようにしてもよい。

ここで、図６に示すステップＳ１３における固定長数値データ（浮動小数点数値データ）から可変長分数データへの変換処理を詳細に説明する。図７は、図６に示すステップＳ１３における浮動小数点数値データから可変長分数データへの変換処理を説明するためのフローチャートである。

図７に示すように、まず、ステップＳ３１において、データ変換処理部１３は、固定長数値データ記憶部１２から浮動小数点数値データｄを読み出し、浮動小数点数値データｄを上位から（ｎ＋１）桁目で四捨五入をする（ステップＳ５１）。ここで、桁数ｎは要求される数値精度に応じて任意に設定することができる。例えば、ｄ＝１２３４５．６７８９、ｎ＝７とすると、四捨五入した結果はｄ＝１２３４５．６８となる。

次に、ステップＳ３２において、データ変換処理部１３は、四捨五入後の数値ｄが整数か小数点以下の数値を持つかどうかを判定し、整数である場合は、ステップＳ３３に処理を移行して可変長分数データの分子ａ＝ｄ、分母ｂ＝ｌに設定し、この可変長分数データａ／ｂを、可変長分数データ記憶部１４に格納した後、図６に示すステップＳ１４へ処理を戻す。

一方、数値ｄが小数点以下の数値を持つ場合は、データ変換処理部１３は、ステップＳ３４において、数値ｄの小数点以下の桁数ｎ’を求め、ステップＳ３５において、分子ａ＝ｄ×１０^ｎ’、分母ｂ＝１０^ｎ’とし、この可変長分数データａ／ｂを可変長分数データ記憶部１４に格納した後、図６に示すステップＳ１４へ処理を戻す。上記の例では、小数点以下の数値を持ち、ｎ’＝２となるので、分子ａ＝１２３４５．６８×１０^２＝１２３４５６８、分母ｂ＝１０^２＝１００となり、これらのデータが一対のデータとして可変長分数データ記憶部１４に記憶される。

再び、図６を参照して、上記のステップＳ１３の処理が終了した後、ステップＳ１４、Ｓ１５及びＳ１６において、可変長分数データ演算処理部１６ａは、初期設定として、ｋ＝１、ｉ＝１、ｊ＝１に設定する。次に、ステップＳ１７において、可変長分数データ演算処理部１６ａは、可変長分数データ記憶部１４から係数ａ^ｋ _ｉｊ，ａ^ｋ _ｋｋ，ａ^ｋ _ｋｊ，ａ^ｋ _ｉｋを読み出し、可変長分数演算部１６ｂを用いて、ｉ＝ｋの場合はａ^ｋ＋１ _ｉｊ＝ａ^ｋ _ｉｊに設定し、ｉ≠ｋの場合はａ^ｋ＋１ _ｉｊ＝ａ^ｋ _ｉｊ−ａ^ｋ _ｋｊａ^ｋ _ｉｋ／ａ^ｋ _ｋｋを算出し、係数ａ^ｋ＋１ _ｉｊを可変長分数データ記憶部１４に格納する。

次に、ステップＳ１８において、可変長分数データ演算処理部１６ａは、ｊ＜５であるか否かを判断し、ｊ＜５の場合、ステップＳ２７においてｊを１だけインクリメントしてステップＳ１７以降の処理を継続し、ｊ＜５でない（ｊ＝５）場合、ステップＳ１９へ処理を移行する。

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

次に、ステップＳ２０において、可変長分数データ演算処理部１６ａは、ｉ＜５であるか否かを判断し、ｉ＜５の場合、ステップＳ２６においてｉを１だけインクリメントしてステップＳ１６以降の処理を継続し、ｉ＜５でない（ｉ＝５）場合、ステップＳ２１へ処理を移行する。

ｉ＜５でない場合、ステップＳ２１において、可変長分数データ演算処理部１６ａは、ｋ＜５であるか否かを判断し、ｋ＜５の場合、ステップＳ２５においてｋを１だけインクリメントしてステップＳ１５以降の処理を継続し、ｋ＜５でない（ｋ＝５）場合、ステップＳ２２へ処理を移行する。

上記の処理により、可変長分数データ記憶部１４からａ^ｋ _ｉｊ及びｂ^ｋ _ｉが読み出され、無誤差制御処理部１６により、可変長分数演算部１６ｂを用いて式（１６ａ）乃至（１６ｄ）の演算処理が行われ、演算結果であるａ^ｋ＋１ _ｉｊ及びｂ^ｋ＋１ _ｉが、再び可変長分数データ記憶部１４に記憶される。ここでの演算の除算において、割り切れる場合は除算が実行され（分母が１となる）、割り切れない場合は、除算は保留される（分数形式のままとなる）。また、割り切れない場合で、約分ができる場合は約分が行われ、分子データ及び分母データの数値桁数の削減が行われる。これらの点に関して以下のステップＳ２２における除算も同様である。

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

ここで、図６に示すステップＳ１７、Ｓ１９及びＳ２２の除算における約分処理を詳細に説明する。本実施の形態では、ロボットアーム制御処理における除算において、例えば、可変長整数データａ及びｂに対する除算ａ÷ｂが発生すると、可変長分数データａ／ｂとして表現され、分子データａと分母データｂとが１つのペアデータとして可変長分数データ記憶部１４に記憶され、割り切れる場合は除算が実行され、さらに、約分ができる場合は約分が行われる。

図８は、図６に示すステップＳ１７、Ｓ１９及びＳ２２の除算における約分処理を説明するためのフローチャートである。図８に示すように、約分処理では、まず、ステップＳ４１において、可変長分数データ演算処理部１６ａは、可変長分数データａ／ｂを可変長分数データ記憶部１４から読み込む。

次に、ステップＳ４２において、可変長分数データ演算処理部１６ａは、可変長分数演算部１６ｂを用いて、分子データａ及び分母データｂの下位ビットにおいて「０」が連続するビット数ｎ_ａ及びｎ_ｂを求める。例えば、ａ＝１２＝１１００（２進数）、ｂ＝６＝１１０（２進数）であるとすると、ｎ_ａ＝２、ｎ_ｂ＝１となる。

次に、ステップＳ４３において、可変長分数データ演算処理部１６ａは、可変長分数演算部１６ｂを用いて、ビット数ｎ_ａとｎ_ｂとを比較し、大きくない方をｎとする。上記の例では、ｎ＝ｎ_ｂ＝１となる。

次に、ステップＳ４４において、可変長分数データ演算処理部１６ａは、可変長分数演算部１６ｂを用いて、分子データａ及び分母データｂのビットを下位方向にｎビットシフトさせ、それぞれａ’及びｂ’とする。上記の例では、ｎ＝１であるため、分子データａ及び分母データｂのビットを下位方向に１ビットシフトし、それぞれａ’＝６＝１１０（２進数）及びｂ’＝３＝１１（２進数）となる。

次に、可変長分数データ演算処理部１６ａは、可変長分数演算部１６ｂを用いて、ステップＳ４５において、ａ’とｂ’とを比較し、小さくない方をＡ、大きくない方をＢとし、ステップＳ４６において、両者の差Ｃ＝Ａ−Ｂを求める。上記の例では、Ａ＝６、Ｂ＝３となり、Ｃ＝６−３＝３となる。

次に、可変長分数データ演算処理部１６ａは、可変長分数演算部１６ｂを用いて、ステップＳ４７において、Ｃ＝０であるか否かを判断し、Ｃ＝０の場合は、ステップＳ５１において、Ａが最大公約数となり、分子データａ’及び分母データｂ’をＡで除算し、約分された分数データａ／ｂを再び可変長分数データ記憶部１４に記憶させた後、図６に示すステップＳ１７、Ｓ１９及びＳ２２へ処理を戻す。

一方、Ｃ＝０でない場合、可変長分数データ演算処理部１６ａは、可変長分数演算部１６ｂを用いて、ステップＳ４８において、Ａ＝Ｂ、Ｂ＝Ｃに設定し、ステップＳ４９において、Ａ＜Ｂであるか否かを判断し、Ａ＜Ｂでない場合は、ステップＳ４６に戻って以降の処理を継続し、Ａ＜Ｂである場合は、ステップＳ５０において、ＡとＢとを入れ替えた後、ステップ４６に戻って以降の処理を継続する。この処理では、ＢとＣとを比較し、小さくない方をＡ、大きくない方をＢとし、Ｃ＝０となるまで、処理を繰り返す。上記の例では、Ｃ＝０となるのは、Ａ＝３、Ｂ＝３となるときであり、最大公約数は３となり、ａ／ｂを約分すると、ａ／ｂ＝１２／６＝ａ’／ｂ’＝６／３＝（６÷３）／（３÷３）＝２／１となる。

再び、図６を参照して、上記のステップＳ２２の処理が終了した後、ステップＳ２３において、可変長分数データ演算処理部１６ａは、可変長分数データ記憶部１４から直前の関節角度値θ’_ｉ及び微小関節角度変化量Δθ_ｉ（ｉ＝１，…，５）を順次読み出し、可変長分数演算部１６ｂを用いて、式（１８）に従って関節角度θ_ｉ＝θ_ｉ’＋Δθ_ｉ（ｉ＝１，…，５）を算出し、可変長分数データ記憶部１４に格納する。このように、ステップＳ１４〜Ｓ２４及びＳ２５〜Ｓ２７までの処理が無誤差演算を用いて実行される。

次に、ステップＳ２４において、データ変換処理部１３は、可変長分数データ記憶部１４から関節角度θ_ｉ（ｉ＝１，…，５）を順次読み出して可変長分数データ形式から浮動小数点データ形式に変換し、浮動小数点データ形式の関節角度θ_ｉを固定長数値データ記憶部１２に格納する。この処理では、分数データを浮動小数点数値に変換するために除算すなわち分子を分母で割る処理が行われる。この演算により誤差が発生する場合があるが、この処理は１回の除算を行うだけであるので、除算に十分な精度が確保されていれば、誤差が問題になることはなく、誤差の管理が可能であり、誤差の蓄積も起こらない。

また、ステップＳ２４において、外部インターフェース部１１は、固定長数値データ記憶部１２から関節角度θ_１，θ_２，θ_３，θ_４，θ_５を読み出してロボットアーム部２へ出力する。ロボットアーム部２は、ロボットアーム２１の各関節角度が関節角度θ_１，θ_２，θ_３，θ_４，θ_５となるようにロボットアーム２１を動作させる。

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

ここで、図６に示すステップＳ２４における可変長分数データから浮動小数点数値データへの変換処理を詳細に説明する。図９は、図６に示すステップＳ２４における可変長分数データから浮動小数点数値データへの変換処理を説明するためのフローチャートである。

図９に示すように、データ変換処理部１３は、まず、ステップＳ６１において、可変長分数データ記憶部１４から可変長分数データａ／ｂを読み出して、分子データａの上位から（ｎ＋１）桁目で四捨五入をし、これを仮数部ａ’とし、ステップＳ６２において、分子データａの指数部ｉを求める。ここで、桁数ｎは要求される数値精度に応じて任意に設定することができる。例えば、ａ／ｂ＝１２３４５６／７８９、ｎ＝３とすると、四捨五入した結果は、ａ’＝１．２３となり、ｉ＝５となる。

次に、データ変換処理部１３は、ステップＳ６３において、分母データｂの上位から（ｎ＋１）桁目で四捨五入をし、これを仮数部ｂ’とし、ステップＳ６４において、分母データの指数部ｊを求める。上記の例では、ｂ’＝７．８９、ｊ＝２となる。

次に、データ変換処理部１３は、ステップＳ６５において、ａ’をｂ’で割った仮数部ｃを求め、ステップＳ６６において、その指数部ｋ＝ｉ−ｊを求める。上記の例では、ｃ＝ａ’／ｂ’＝１．２３／７．８９＝０．ｌ５６、ｋ＝ｉ−ｊ＝５−２＝３となる。

最後に、ステップＳ６７において、データ変換処理部１３は、求めた仮数部と指数部とで構成される浮動小数点数値ｄを正規化して、固定長数値データ記憶部１２に格納して処理を終了する。上記の例では、正規化は、ｄ＝０．１５６×１０^３＝１．５６×１０^２となる。なお、この例では、説明を分かりやすくするために、１０進数を用いて説明してきたが、実際の実装は２進数を用いて行われる。

上記の処理により、本実施の形態では、可変長分数データ演算を用いて無誤差演算制御を実現しているので、以下の効果を奏することができる。
（１）除算を含む四則演算すべてを用いて、無誤差演算で制御処理を行うことができ、汎用性、実用性が高い。
（２）可変長分数データ処理では、約分をすることができる。これにより、無誤差演算の欠点であった数値桁数の際限のない増大を抑えることができ、データ量を削減し、演算処理を効率化及び高速化することができる。
（３）制御演算誤差及びその蓄積をなくすことにより、制御処理の破綻をなくし、高精度な位置制御を実現することができる。
（４）リンク機構（ロボットアーム）動作の暴走等をなくすことができ、ロボット動作を安定化及び高信頼化することができる。
（５）上記との関連で、ロボットアームの暴走防止対策等が不要となり、制御プログラム及び制御システムを単純化することができる。

なお、本実施の形態では、浮動小数点データを可変長分数データへ変換して無誤差演算を実行したが、この例に特に限定されず、浮動小数点データを可変長整数データへ変換し、除算が発生した場合に可変長整数データを可変長分数データへ変換して表現する等の種々の変更が可能である。

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

本発明の一実施の形態によるロボットアーム制御装置を用いたロボットアーム制御システムの構成を示すブロック図である。 図１に示すロボットアーム制御装置の構成を示すブロック図である。 数値データの種々のフォーマットの例を示す図である。 図１に示すロボットアーム部における右側のロボットアームの構成を示す模式図である。 図４に示す右側のロボットアームの座標系を説明するための図である。 図１に示すロボットアーム制御装置によるロボットアーム制御処理を説明するためのフローチャートである。 図６に示すステップＳ１３における浮動小数点数値データから可変長分数データへの変換処理を説明するためのフローチャートである。 図６に示すステップＳ１７、Ｓ１９及びＳ２２の除算における約分処理を説明するためのフローチャートである。 図６に示すステップＳ２４における可変長分数データから浮動小数点数値データへの変換処理を説明するためのフローチャートである。

符号の説明

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

Claims (9)

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

Images