JP4743699B2 - POSITION CONTROL DEVICE, POSITION CONTROL METHOD, AND POSITION CONTROL PROGRAM - Google Patents

Description

本発明は、関節を介して複数のリンクを連結したリンク機構の所定部位、例えば、先端部を目標３次元位置及び目標方向に制御する位置制御装置、位置制御方法及び位置制御プログラムに関する。   The present invention relates to a position control device, a position control method, and a position control program for controlling a predetermined portion of a link mechanism in which a plurality of links are connected via joints, for example, a tip portion in a target three-dimensional position and a target direction.

従来のロボットアームの制御方式としては、スレーブアームと同じ軸構成等を有するマスターアームを操作者が操作してスレーブアームを制御するマスターアーム方式、操作者の腕と手の主要部（各関節）にセンサを装着して操作者の腕と手の関節の動きを検出してロボットアームを制御するモーションキャプチャー方式、操作者の手首・手の甲等に付けた３次元位置方向センサにより操作者の手首・手の甲の３次元位置及び方向を検出し、このデータを用いてアーム（リンク）の各関節角度を求める逆変換を行ってロボットアームを制御する逆変換方式等の種々の方式がある。   As a conventional robot arm control method, a master arm method in which an operator operates a master arm having the same axis configuration as the slave arm to control the slave arm, and the main part of the operator's arm and hand (each joint) A motion capture system that controls the robot arm by detecting movements of the operator's arm and hand joints by attaching a sensor to the operator's wrist, and a 3D position / direction sensor attached to the operator's wrist, back of the hand, etc. There are various methods such as a reverse conversion method for detecting the three-dimensional position and direction of the back of the hand and performing reverse conversion to obtain each joint angle of the arm (link) using this data to control the robot arm.

上記のマスターアーム方式では、操作者側には、制御されるスレーブアームだけでなく、マスターアームが必要となり、システムが重装備になる。また、モーションキャプチャー方式では、操作者の腕と手の複数箇所にセンサを装着する必要があり、操作者にとって煩わしいものとなる。   In the above master arm system, not only the slave arm to be controlled but also the master arm is required on the operator side, and the system becomes heavy equipment. In addition, in the motion capture method, it is necessary to attach sensors to a plurality of locations of the operator's arm and hand, which is troublesome for the operator.

一方、逆変換方式では、操作者は手の甲等に１個のセンサのみを装着するだけで済み、操作者にとって煩わしさがないが、操作者の手首の位置と方向データから各関節角度を求める逆変換が必要となる。この逆変換では、人の腕のような高い自由度（６自由度以上）を持つ軸構成における各関節角度を求める必要があるため、逆変換が非線形連立方程式となり、複雑な計算となる。   On the other hand, in the reverse conversion method, the operator only has to install one sensor on the back of the hand and the like, and there is no trouble for the operator. However, the inverse of obtaining each joint angle from the wrist position and direction data of the operator. Conversion is required. In this inverse transformation, since it is necessary to obtain each joint angle in a shaft configuration having a high degree of freedom (6 degrees of freedom or more) like a human arm, the inverse transformation becomes a nonlinear simultaneous equation and is a complicated calculation.

このため、本願発明者は、逆変換が非線形連立方程式とならず、単純且つ少ない演算量で厳密解を求めることができるロボットアーム・ハンドの操作方法を提案した（特許文献１参照）。この操作方法では、ロボットアームの操作者の手首に付けたセンサにより検出された３次元位置及び方向からアームの各関節角度を求めるために、浮動小数点演算を用いた誤差有り演算を用いている。この浮動小数点フォーマットを用いた実数表現は、非常に小さな数から大きな数まで表現でき、計算効率がよいという優れた特徴があるため、逆変換のような実数の数値計算に適している。
特開２００５−４６９３１号公報 For this reason, the inventor of the present application has proposed a robot arm / hand operation method in which the inverse transformation is not a nonlinear simultaneous equation, and an exact solution can be obtained with a simple and small amount of calculation (see Patent Document 1). In this operation method, in order to obtain each joint angle of the arm from the three-dimensional position and direction detected by the sensor attached to the wrist of the operator of the robot arm, the calculation with error using the floating point calculation is used. The real number expression using the floating point format is suitable for numerical calculation of a real number such as inverse transformation because it can express from a very small number to a large number and has a superior calculation efficiency.
JP 2005-46931 A

しかしながら、上記の浮動小数点演算を用いた数値制御では、以下のような問題がある。
（１）浮動小数点演算では、数値演算誤差が発生し、且つその蓄積が起こるため、演算結果に大きな誤差が含まれる場合があり、ロボットアームの先端位置が大幅にずれる場合がある。
（２）上記の数値演算誤差とその蓄積のために、当初想定していなかった状態が発生し、結果として制御が破綻してロボットアーム動作が暴走する状況が発生する。
（３）上記のロボットアーム動作の暴走を防止するためには、暴走対策を施す必要があり、制御が複雑化する。 However, the numerical control using the floating point arithmetic has the following problems.
(1) In floating point calculations, numerical calculation errors occur and accumulate, so that the calculation results may include large errors, and the tip position of the robot arm may be significantly displaced.
(2) Due to the above numerical calculation error and its accumulation, a state that was not initially assumed occurs, and as a result, a situation occurs in which the control is broken and the robot arm operation runs out of control.
(3) In order to prevent the robot arm operation from running out of control, it is necessary to take measures against the runaway, which complicates the control.

本発明の目的は、リンク機構の制御動作を安定化及び高信頼化することができるとともに、リンク機構を高精度に制御することができる位置制御装置、位置制御方法及び位置制御プログラムを提供することである。   An object of the present invention is to provide a position control device, a position control method, and a position control program capable of stabilizing and improving the control operation of a link mechanism and controlling the link mechanism with high accuracy. It is.

本発明に係る位置制御装置は、関節を介して複数のリンクを連結したリンク機構の所定部位を目標３次元位置及び目標方向に制御する位置制御装置であって、リンク機構の所定部位の目標３次元位置データ及び目標方向データからなる目標データをＰ、リンク機構の各関節角度をθとし、目標データＰから関節角度θを求める解法処理に使用されるデータを固定長数値データ形式から可変長整数データ形式へ変換する変換手段と、上記解法処理において、変換手段により変換された可変長整数データ形式のデータを用いて、除算を除く加減算及び乗算からなる無誤差演算を実行する無誤差演算手段とを備え、前記変換手段は、前記無誤差演算手段の演算結果を可変長整数データ形式から固定長数値データ形式へ変換し、前記解法処理において、前記変換手段により変換された固定長数値データ形式の演算結果を用いて１回のみの除算を含む固定長数値演算を実行することにより固定長数値データ形式の関節角度θを求める誤差演算手段をさらに備えるものである。 A position control device according to the present invention is a position control device that controls a predetermined portion of a link mechanism in which a plurality of links are connected via joints in a target three-dimensional position and a target direction, and includes a target 3 of the predetermined portion of the link mechanism. The target data consisting of the dimension position data and the target direction data is P, each joint angle of the link mechanism is θ, and the data used for the solution processing for obtaining the joint angle θ from the target data P is a variable length integer from the fixed length numerical data format. A conversion means for converting to a data format; and an error-free calculation means for executing an error-free calculation including addition / subtraction and multiplication excluding division using the data in the variable-length integer data format converted by the conversion means in the solution processing. The conversion means converts the calculation result of the error-free calculation means from a variable-length integer data format to a fixed-length numeric data format, and in the solution processing, Error calculation means for obtaining a joint angle θ in fixed-length numerical data format by executing fixed-length numerical calculation including only one division using the calculation result in fixed-length numerical data format converted by the conversion means It is to be prepared .

本発明に係る位置制御装置では、目標データＰから関節角度θを求める解法処理において、この解法処理に使用されるデータが固定長数値データ形式から可変長整数データ形式へ変換され、変換された可変長整数データ形式のデータを用いて、除算を除く加算、減算及び乗算からなる無誤差演算が実行される。この無誤差演算においては、数値演算誤差が発生せず、その蓄積もないため、リンク機構の制御動作を安定化及び高信頼化することができるとともに、リンク機構を高精度に制御することができる。   In the position control device according to the present invention, in the solution processing for obtaining the joint angle θ from the target data P, the data used for the solution processing is converted from the fixed-length numeric data format to the variable-length integer data format, and the converted variable Using data in the long integer data format, an error-free operation including addition, subtraction, and multiplication excluding division is executed. In this error-free calculation, no numerical calculation error occurs and no accumulation occurs, so that the control operation of the link mechanism can be stabilized and highly reliable, and the link mechanism can be controlled with high accuracy. .

この場合、目標データＰから関節角度θを求める解法処理において、可変長整数データ形式の係数データを用いて、除算を除く加算、減算及び乗算からなる無誤差演算が実行された後、この演算結果が可変長整数データ形式から固定長数値データ形式へ変換され、変換された固定長数値データ形式の演算結果を用いて１回のみの除算を含む固定長数値演算が実行され、固定長数値データ形式の関節角度θが算出されるので、上記解法処理において、数値演算誤差の発生を多くとも１回の除算によるものだけに制限して数値演算誤差を非常に小さくすることができるとともに、その蓄積を防止することができる。   In this case, in the solution processing for obtaining the joint angle θ from the target data P, error-free operations including addition, subtraction, and multiplication excluding division are performed using coefficient data in the variable-length integer data format, and then the calculation result Is converted from a variable-length integer data format to a fixed-length numeric data format, and a fixed-length numeric operation including only one division is executed using the operation result of the converted fixed-length numeric data format. Since the joint angle θ is calculated, in the above-described solution processing, the numerical calculation error can be limited to at most one division and the numerical calculation error can be made extremely small. Can be prevented.

無誤差演算手段は、θ_ｉ＝θ’_ｉ＋Δθ_ｉ（ｉ＝１，…，ｎ）（ここで、θ_ｉは関節ｉの求める関節角度、θ’_ｉは微小時間Δｔ前の関節ｉの関節角度、Δθ_ｉは微小時間Δｔ経過後の関節ｉの微小関節角度変化量、ｎは求める関節角度θの数）とし、ｎ元連立一次方程式ＡΔθ＝Ｂ（ここで、Ａはその要素ａ_ｉｊ（ｉ＝１，…，ｎ、ｊ＝１，…，ｎ）がリンク機構のリンク長及び微小時間Δｔ前の関節ｉの関節角度θ’_ｉにより決定される係数行列、Ｂはその要素ｂ_ｉ（ｉ＝１，…，ｎ）が目標データＰ及び微小時間Δｔ前の位置方向データＰ’により決定されるベクトル）をＡｎΔθ＝Ｂｎ（ここで、Ａｎは対角要素ａ^ｎ＋１ _ｉｉ（ｉ＝１，…，ｎ）以外が０となる対角行列、Ｂｎは要素ｂ^ｎ＋１ _ｉ（ｉ＝１，…，ｎ）のベクトル）に変形したときの係数データａ^ｎ＋１ _ｉｉ及び係数データｂ^ｎ＋１ _ｉを、除算を除く加算、減算及び乗算からなる無誤差演算を用いて求め、変換手段は、無誤差演算手段により求められた係数データａ^ｎ＋１ _ｉｉ及び係数データｂ^ｎ＋１ _ｉを可変長整数データ形式から固定長数値データ形式へ変換し、誤差演算手段は、変換手段により変換された固定長数値データ形式の係数データａ^ｎ＋１ _ｉｉ及び係数データｂ^ｎ＋１ _ｉを用いて、θ_ｉ＝θ’_ｉ＋ｂ^ｎ＋１ _ｉ／ａ^ｎ＋１ _ｉｉ（ｉ＝１，…，ｎ）を固定長数値演算により求めることが好ましい。 The error-free computing means is θ _i = θ ′ _i + Δθ _i (i = 1,..., N) (where θ _i is the joint angle obtained by the joint i, θ ′ _i is the joint of the joint i before the minute time Δt) The angle, Δθ _i is the minute joint angle change amount of the joint i after the minute time Δt has elapsed, n is the number of joint angles θ to be obtained, and n-ary simultaneous linear equations AΔθ = B (where A is the element a _ij ( i = 1,..., n, j = 1,..., n) is a coefficient matrix determined by the link length of the link mechanism and the joint angle θ ′ _i of the joint i before the minute time Δt, and B is its element b _i ( i = 1, ..., a vector) n) is determined by the target data P and minute time Δt before the position-direction data P 'AnΔθ = Bn (where, an is diagonal element _{^{a n + 1 ii (i =}} 1, ..., n) except becomes zero diagonal matrix, Bn elements _{^{b n + 1 i (i =}} 1, ..., deformed vectors) of n) Coefficient data a ^{n + 1} _ii and coefficient data b ^{n + 1} _i, adding except division time, calculated using a non-error operation consisting of subtraction and multiplication, conversion means, coefficient data obtained by an error calculating unit a ^{n + 1} _ii And the coefficient data b ^{n + 1} _i from the variable-length integer data format to the fixed-length numeric data format, and the error calculation means converts the coefficient data a ^{n + 1} _ii and coefficient data b ^{n + 1} _i in the fixed-length numeric data format converted by the conversion means. It is preferable to obtain θ _i = θ ′ _i + b ^{n + 1} _i / a ^{n + 1} _ii (i = 1,..., N) by a fixed-length numerical calculation.

この場合、目標データＰから関節角度θを求める解法処理において、θ_ｉ＝θ’_ｉ＋ｂ^ｎ＋１ _ｉ／ａ^ｎ＋１ _ｉｉ以外の演算を、除算を除く加算、減算及び乗算からなる無誤差演算により実行するとともに、数値演算誤差の発生を多くともθ_ｉ＝θ’_ｉ＋ｂ^ｎ＋１ _ｉ／ａ^ｎ＋１ _ｉｉにおける１回の除算だけに制限しているので、関節角度θを高精度に求めることができる。 In this case, in the solution processing for obtaining the joint angle θ from the target data P, operations other than θ _i = θ ′ _i + b ^{n + 1} _i / a ^{n + 1} _ii are executed by error-free operations including addition, subtraction and multiplication excluding division. In addition, since the occurrence of numerical calculation errors is limited to at most one division in θ _i = θ ′ _i + b ^{n + 1} _i / a ^{n + 1} _ii , the joint angle θ can be obtained with high accuracy.

上記位置制御装置は、操作者の所定部位の３次元位置及び方向を検出する検出手段をさらに備え、検出手段により検出された３次元位置及び方向をリンク機構の所定部位の目標３次元位置データ及び目標方向データとして用いて、リンク機構の所定部位を目標３次元位置及び目標方向に制御することが好ましい。   The position control device further includes detection means for detecting the three-dimensional position and direction of the predetermined part of the operator, and the target three-dimensional position data of the predetermined part of the link mechanism is detected from the three-dimensional position and direction detected by the detection means. It is preferable to control the predetermined part of the link mechanism to the target three-dimensional position and the target direction using the target direction data.

この場合、リンク機構の所定部位を操作者の所定部位の動きに応じた目標３次元位置及び目標方向に高精度に制御することができる。   In this case, the predetermined part of the link mechanism can be controlled with high accuracy in the target three-dimensional position and the target direction according to the movement of the predetermined part of the operator.

本発明に係る位置制御方法は、関節を介して複数のリンクを連結したリンク機構の所定部位を目標３次元位置及び目標方向に制御するロボットアーム制御方法であって、リンク機構の所定部位の目標３次元位置データ及び目標方向データからなる目標データをＰ、リンク機構の各関節角度をθとし、目標データＰから関節角度θを求める解法処理に使用されるデータを固定長数値データ形式から可変長整数データ形式へ変換する変換ステップと、上記解法処理において、変換された可変長整数データ形式のデータを用いて、除算を除く加算、減算及び乗算からなる無誤差演算を実行する無誤差演算ステップとを含み、前記変換ステップは、前記無誤差演算ステップにおける演算結果を可変長整数データ形式から固定長数値データ形式へ変換するステップを含み、前記解法処理において、前記変換ステップにおいて変換された固定長数値データ形式の演算結果を用いて１回のみの除算を含む固定長数値演算を実行することにより固定長数値データ形式の関節角度θを求める誤差演算ステップをさらに含むものである。 A position control method according to the present invention is a robot arm control method for controlling a predetermined part of a link mechanism in which a plurality of links are connected via joints to a target three-dimensional position and a target direction, and the target of the predetermined part of the link mechanism The target data consisting of three-dimensional position data and target direction data is P, each joint angle of the link mechanism is θ, and the data used for the solution processing for obtaining the joint angle θ from the target data P is variable length from the fixed length numerical data format. A conversion step for converting to an integer data format, and an error-free calculation step for performing error-free operations including addition, subtraction, and multiplication except division using the data in the variable-length integer data format converted in the above-described solution processing, only including, the converting step converts the calculation result in the no-error calculating step from the variable length integer data format to a fixed-length numeric data format A fixed-length numeric data format joint by performing a fixed-length numeric operation including only one division using the calculation result in the fixed-length numeric data format converted in the conversion step in the solution processing It further includes an error calculation step for obtaining the angle θ .

本発明に係る位置制御プログラムは、関節を介して複数のリンクを連結したリンク機構の所定部位を目標３次元位置及び目標方向に制御するための位置制御プログラムであって、リンク機構の所定部位の目標３次元位置データ及び目標方向データからなる目標データをＰ、リンク機構の各関節角度をθとし、目標データＰから関節角度θを求める解法処理に使用されるデータを固定長数値データ形式から可変長整数データ形式へ変換する変換手段と、上記解法処理において、変換手段により変換された可変長整数データ形式の係数データを用いて、除算を除く加算、減算及び乗算からなる無誤差演算を実行する無誤差演算手段としてコンピュータを機能させ、前記変換手段は、前記無誤差演算手段の演算結果を可変長整数データ形式から固定長数値データ形式へ変換し、前記解法処理において、前記変換手段により変換された固定長数値データ形式の演算結果を用いて１回のみの除算を含む固定長数値演算を実行することにより固定長数値データ形式の関節角度θを求める誤差演算手段として前記コンピュータをさらに機能させるものである。 A position control program according to the present invention is a position control program for controlling a predetermined part of a link mechanism in which a plurality of links are connected via a joint to a target three-dimensional position and a target direction, The target data consisting of the target three-dimensional position data and the target direction data is P, each joint angle of the link mechanism is θ, and the data used for the solution processing for obtaining the joint angle θ from the target data P is variable from the fixed-length numerical data format. A conversion means for converting to a long integer data format, and in the above solution processing, using the coefficient data in the variable length integer data format converted by the conversion means, an error-free operation including addition, subtraction and multiplication excluding division is executed. The computer functions as error-free computing means, and the converting means converts the computation result of the error-free computing means from a variable-length integer data format to a fixed length. Fixed-length numeric data by converting to a numeric data format and executing fixed-length numeric operations including only one division using the calculation results in the fixed-length numeric data format converted by the conversion means in the solution processing The computer is further caused to function as an error calculation means for obtaining a joint angle θ of a form .

本発明によれば、目標データＰから関節角度θを求める解法処理において、可変長整数データ形式のデータを用いて、除算を除く加算、減算及び乗算からなる無誤差演算が実行され、この無誤差演算においては、数値演算誤差が発生せず、その蓄積もないため、リンク機構の制御動作を安定化及び高信頼化することができるとともに、リンク機構を高精度に制御することができる。   According to the present invention, in the solution processing for obtaining the joint angle θ from the target data P, error-free operations including addition, subtraction, and multiplication excluding division are executed using data in the variable-length integer data format. In calculation, no numerical calculation error occurs and no accumulation occurs, so that the control operation of the link mechanism can be stabilized and highly reliable, and the link mechanism can be controlled with high accuracy.

以下、本発明の一実施の形態による位置制御装置の一例であるロボットアーム制御装置をいたロボットアーム制御システムについて図面を参照しながら説明する。   Hereinafter, a robot arm control system including a robot arm control device which is an example of a position control device according to an embodiment of the present invention will be described with reference to the drawings.

図１は、本発明の一実施の形態によるロボットアーム制御装置を用いたロボットアーム制御システムの構成を示すブロック図である。図１に示すロボットアーム制御システムは、ロボットアーム制御装置１、ロボットアーム部２、３次元操作グローブ３、３次元位置方向センサ４及び３次元ディスプレイ５を備える。   FIG. 1 is a block diagram showing a configuration of a robot arm control system using a robot arm control device according to an embodiment of the present invention. The robot arm control system shown in FIG. 1 includes a robot arm control device 1, a robot arm unit 2, a three-dimensional operation globe 3, a three-dimensional position / direction sensor 4, and a three-dimensional display 5.

ロボットアーム部２は、一対のロボットアーム２１及びハンド２２、胴体部２３並びにカメラ部２４を備え、人の上半身（頭部を除く）と同程度の大きさと自由度とを有し、両腕と両手の機能を実現している。   The robot arm unit 2 includes a pair of robot arms 21 and a hand 22, a body unit 23, and a camera unit 24. The robot arm unit 2 has a size and a degree of freedom similar to those of a human upper body (excluding the head). Realizes the function of both hands.

ロボットアーム２１は、肩、肘及び手首に関節を有し、これらの関節を介してリンクとなる上腕２１ａ及び前腕２１ｂが連結されたリンク機構（マニプレータ）であり、人の腕と同程度の大きさ及び自由度を有する。ハンド２２は、人の手と同程度の大きさと自由度を有する触覚付きの５本指ロボットハンドであり、前腕２１ｂの先端に取り付けられる。ハンド２２には触覚センサ（図示省略）が設けられており、物をつかんだ時の触覚（圧力）がロボットアーム制御装置１を介して３次元操作グローブ３へ伝達される。   The robot arm 21 is a link mechanism (manipulator) having joints on shoulders, elbows and wrists, and an upper arm 21a and a forearm 21b that are linked via these joints, and is as large as a human arm. And has flexibility. The hand 22 is a tactile five-fingered robot hand having the same size and flexibility as a human hand, and is attached to the tip of the forearm 21b. The hand 22 is provided with a tactile sensor (not shown), and a tactile sensation (pressure) at the time of grasping an object is transmitted to the three-dimensional operation glove 3 via the robot arm control device 1.

なお、図示を省略しているが、ロボットアーム２１及びハンド２２は、その内部にモータ等の駆動源を有し、この駆動源により任意の角度に駆動される。また、本実施の形態では、ロボットアーム２１を人の腕に模倣させ、ハンド２２を人の手指に模倣させているが、ロボットアーム及びハンドの構成は、この例に特に限定されず、ハンド２２の指の数を増減させる等の種々の変更が可能である。また、リンク機構の関節数及びリンク数も、上記の例に特に限定されず、種々の変更が可能である。   Although not shown, the robot arm 21 and the hand 22 have a driving source such as a motor inside thereof, and are driven at an arbitrary angle by the driving source. In the present embodiment, the robot arm 21 is imitated by a human arm and the hand 22 is imitated by a human finger. However, the configurations of the robot arm and the hand are not particularly limited to this example. Various changes, such as increasing or decreasing the number of fingers, are possible. Further, the number of joints and the number of links of the link mechanism are not particularly limited to the above example, and various changes can be made.

カメラ部２４は、３次元カメラ（ステレオカメラ）等から構成され、ロボットアーム制御装置１を介して、撮影した画像を３次元ディスプレイ５へ出力する。３次元ディスプレイ５は、カメラ部２４により撮影された画像を表示し、操作者Ｍに提示する。なお、撮影された画像の提示方法は、この例に特に限定されず、３次元ディスプレイに替えて、操作者Ｍに装着させたヘッドマウントディスプレイに表示させる等の種々の変更が可能である。   The camera unit 24 is configured by a three-dimensional camera (stereo camera) or the like, and outputs a photographed image to the three-dimensional display 5 via the robot arm control device 1. The three-dimensional display 5 displays an image photographed by the camera unit 24 and presents it to the operator M. The method of presenting the captured image is not particularly limited to this example, and various changes such as displaying on a head-mounted display worn by the operator M instead of the three-dimensional display are possible.

３次元操作グローブ３は、公知の３次元操作用データグローブから構成され、操作者Ｍの両手にそれぞれ装着される。３次元操作グローブ３は、操作者Ｍの手の甲及び指の動き（関節角度）を検出し、検出した関節角度データをロボットアーム制御装置１へ出力する。３次元操作グローブ３の手首の位置には、３次元位置方向センサ４が取り付けられ、３次元位置方向センサ４は、前腕２１ｂの先端部の目標３次元位置及び目標方向として、操作者Ｍの手首の３次元位置及び方向を検出し、検出した３次元位置データ及び方向データをロボットアーム制御装置１へ出力する。   The three-dimensional operation glove 3 is composed of a known three-dimensional operation data glove, and is attached to both hands of the operator M. The three-dimensional operation glove 3 detects the back of the hand of the operator M and the movement of the finger (joint angle), and outputs the detected joint angle data to the robot arm control device 1. The three-dimensional position / direction sensor 4 is attached to the wrist position of the three-dimensional operation glove 3, and the three-dimensional position / direction sensor 4 uses the wrist of the operator M as the target three-dimensional position and target direction of the tip of the forearm 21b. The three-dimensional position and direction are detected, and the detected three-dimensional position data and direction data are output to the robot arm control device 1.

なお、目標３次元位置及び目標方向を検出又は入力する方法は、上記の例に特に限定されず、データグローブ又はデータスーツ等から操作者の所定部位の３次元位置及び方向を検出したり、ハプティクス・デバイス等を用いて入力したりしてもよい。   Note that the method for detecting or inputting the target three-dimensional position and target direction is not particularly limited to the above example, and the three-dimensional position and direction of a predetermined part of the operator can be detected from a data glove, a data suit, or the like. -You may input using a device etc.

ロボットアーム制御装置１は、操作者Ｍの両手に装着された３次元操作グローブ３及び３次元位置方向センサ４からの検出結果を基に、ロボットアーム２１及びハンド２２の各関節角度を算出する。ハンド２２に関しては、３次元操作グローブ３が手の甲の曲がり（手首の）角度及び指の関節角度を検出し、ロボットアーム制御装置１は、これらの角度データに基づいてハンド２２を制御する。   The robot arm control device 1 calculates the joint angles of the robot arm 21 and the hand 22 based on the detection results from the three-dimensional operation glove 3 and the three-dimensional position / direction sensor 4 attached to both hands of the operator M. Regarding the hand 22, the three-dimensional operation glove 3 detects the bending angle of the back of the hand (the wrist) and the joint angle of the finger, and the robot arm control device 1 controls the hand 22 based on these angle data.

一方、ロボットアーム２１に関しては、操作者Ｍの手首に装着した３次元位置方向センサ４から操作者Ｍの手首の三次元位置及びその方向、すなわち、ロボットアーム２１の先端部の目標３次元位置及び目標方向が得られるだけである。このため、ロボットアーム２１の先端部の目標３次元位置データ及び目標方向データから、ロボットアーム２１の各関節角度を計算する必要がある。このように、ロボットアーム２１の先端部の３次元位置及び方向からロボットアーム２１の各関節角度を計算することを逆変換と呼び、これとは逆に、ロボットアーム２１の各関節角度からロボットアーム２１の先端部の３次元位置及び方向を計算することを順変換と呼ぶことにする。   On the other hand, with respect to the robot arm 21, the three-dimensional position and direction of the wrist of the operator M from the three-dimensional position / direction sensor 4 attached to the wrist of the operator M, that is, the target three-dimensional position of the tip of the robot arm 21 and Only the target direction is obtained. For this reason, it is necessary to calculate each joint angle of the robot arm 21 from the target three-dimensional position data and target direction data of the tip of the robot arm 21. In this way, calculating each joint angle of the robot arm 21 from the three-dimensional position and direction of the tip of the robot arm 21 is referred to as inverse transformation, and conversely, the robot arm is calculated from each joint angle of the robot arm 21. The calculation of the three-dimensional position and direction of the tip 21 is referred to as forward conversion.

図２は、図１に示すロボットアーム制御装置１の構成を示すブロック図である。図２に示すように、ロボットアーム制御装置１は、外部インターフェース部１１、浮動小数点データ記憶部１２、データ変換処理部１３、可変長整数データ記憶部１４、誤差有制御処理部１５及び無誤差制御処理部１６を備える。誤差有制御処理部１５は、座標変換処理部１５ａ及び浮動小数点演算部１５ｂを備え、無誤差制御処理部１６は、方程式解法処理部１６ａ及び可変長整数演算部１６ｂを備える。   FIG. 2 is a block diagram showing a configuration of the robot arm control device 1 shown in FIG. As shown in FIG. 2, the robot arm control device 1 includes an external interface unit 11, a floating-point data storage unit 12, a data conversion processing unit 13, a variable-length integer data storage unit 14, an error presence control processing unit 15, and no error control. A processing unit 16 is provided. The control unit with error 15 includes a coordinate conversion processing unit 15a and a floating point arithmetic unit 15b, and the error-free control processing unit 16 includes an equation solution processing unit 16a and a variable length integer arithmetic unit 16b.

ロボットアーム制御装置１は、ＣＰＵ（中央演算処理装置）、ＲＯＭ（リードオンリメモリ）、ＲＡＭ（ランダムアクセスメモリ）、外部記憶装置、記録媒体駆動装置、入力装置、表示装置、外部機器インターフェース等を備えるコンピュータから構成することができる。この場合、後述するロボットアーム制御処理（位置制御処理）を実行するためのロボットアーム制御プログラム（位置制御プログラム）を、ＣＰＵ等を用いて実行することにより、外部インターフェース部１１、浮動小数点データ記憶部１２、データ変換処理部１３、可変長整数データ記憶部１４、誤差有制御処理部１５及び無誤差制御処理部１６としてコンピュータを機能させることができる。なお、ロボットアーム制御装置１の構成は、この例に特に限定されず、上記の各機能の一部又は全部を専用のハードウエアから構成する等の種々の変更が可能である。   The robot arm control device 1 includes a CPU (Central Processing Unit), a ROM (Read Only Memory), a RAM (Random Access Memory), an external storage device, a recording medium driving device, an input device, a display device, an external device interface, and the like. It can consist of computers. In this case, a robot arm control program (position control program) for executing a robot arm control process (position control process), which will be described later, is executed using a CPU or the like, so that the external interface unit 11 and the floating point data storage unit are executed. 12, the computer can function as the data conversion processing unit 13, the variable-length integer data storage unit 14, the control unit 15 with error, and the control unit 16 without error. The configuration of the robot arm control device 1 is not particularly limited to this example, and various modifications such as configuring a part or all of the above-described functions from dedicated hardware are possible.

外部インターフェース部１１は、３次元位置方向センサ４から３次元位置データ及び方向データを取得して浮動小数点データ記憶部１２に格納する。また、外部インターフェース部１１は、ロボットアーム制御処理により求められた関節角度を浮動小数点データ記憶部１２から読み出してロボットアーム部２へ出力し、ロボットアーム２１の各関節角度がロボットアーム制御処理により求められた関節角度に制御される。   The external interface unit 11 acquires three-dimensional position data and direction data from the three-dimensional position / direction sensor 4 and stores them in the floating point data storage unit 12. Further, the external interface unit 11 reads out the joint angles obtained by the robot arm control processing from the floating point data storage unit 12 and outputs them to the robot arm unit 2, and each joint angle of the robot arm 21 is obtained by the robot arm control processing. The joint angle is controlled.

浮動小数点データ記憶部１２は、誤差有制御処理部１５において処理される種々のデータを浮動小数点データ形式で記憶し、可変長整数データ記憶部１４は、無誤差制御処理部１６において処理される種々のデータを可変長整数データ形式で記憶する。   The floating point data storage unit 12 stores various data processed by the control unit with error 15 in a floating point data format, and the variable length integer data storage unit 14 stores various data processed by the error-free control processing unit 16. Are stored in a variable-length integer data format.

図３は、浮動小数点データ及び可変長整数データのデータ構成の一例を示す図である。図３の（ａ）に示すように、固定長数値データの一例である浮動小数点データは、符号部、指数部及び仮数部から構成され、このようなデータ形式で浮動小数点データ記憶部１２に格納される。また、図３の（ｂ）に示すように、可変長整数データは、語長部、符号部、及び数値部（整数表現）から構成され、このようなデータ形式で可変長整数データ記憶部１４に格納される。   FIG. 3 is a diagram illustrating an example of the data configuration of floating point data and variable-length integer data. As shown in FIG. 3A, floating-point data, which is an example of fixed-length numeric data, includes a sign part, an exponent part, and a mantissa part, and is stored in the floating-point data storage part 12 in such a data format. Is done. Further, as shown in FIG. 3B, the variable-length integer data is composed of a word length part, a sign part, and a numerical value part (integer expression), and the variable-length integer data storage unit 14 in such a data format. Stored in

データ変換処理部１３は、浮動小数点データ記憶部１２から浮動小数点データを読み出し、読み出した浮動小数点データを可変長整数データへ変換し、変換した可変長整数データを可変長整数データ記憶部１４に格納する。また、データ変換処理部１３は、可変長整数データ記憶部１４から可変長整数データを読み出し、読み出した可変長整数データを浮動小数点データへ変換し、変換した浮動小数点データを浮動小数点データ記憶部１２に格納する。   The data conversion processing unit 13 reads the floating point data from the floating point data storage unit 12, converts the read floating point data into variable length integer data, and stores the converted variable length integer data in the variable length integer data storage unit 14. To do. The data conversion processing unit 13 reads the variable length integer data from the variable length integer data storage unit 14, converts the read variable length integer data into floating point data, and converts the converted floating point data into the floating point data storage unit 12. To store.

なお、本発明に用いられる固定長数値データは、上記の例に特に限定されず、例えば、固定長整数データを用いることもできる。図３の（ｃ）に示すように、固定長整数データは、符号部及び数値部（整数表現）から構成され、この固定長整数データ形式も、固定長であるため、誤差（数値のオーバーフロー及びそのまるめによる誤差）が発生し、本発明を同様に適用することができる。   The fixed-length numerical data used in the present invention is not particularly limited to the above example, and, for example, fixed-length integer data can be used. As shown in FIG. 3C, the fixed-length integer data is composed of a sign part and a numerical part (integer representation). Since this fixed-length integer data format is also a fixed length, an error (numeric overflow and An error due to rounding occurs, and the present invention can be applied similarly.

例えば、３次元位置方向センサ４が出力する値（角度）が、−２５５〜＋２５５等の整数値であり、ロボットアーム２１の関節を動作させるモータに対する出力（回転角度）も、−１０００〜＋１０００パルス等の整数値である場合、浮動小数点データ記憶部１２を固定長数値データ記憶部に変更し、固定長整数データ形式で各データを格納するようにしてもよい。また、固定長整数データ形式でデータを取り扱う場合、浮動小数点演算部１５ｂを固定長整数演算部に変更し、固定長整数演算部により固定長整数演算を実行するようにしてもよい。   For example, the value (angle) output by the three-dimensional position / direction sensor 4 is an integer value such as −255 to +255, and the output (rotation angle) to the motor that operates the joint of the robot arm 21 is also −1000 to +1000 pulses. For example, the floating-point data storage unit 12 may be changed to a fixed-length numeric data storage unit, and each data may be stored in a fixed-length integer data format. When handling data in the fixed-length integer data format, the floating-point arithmetic unit 15b may be changed to a fixed-length integer arithmetic unit, and the fixed-length integer arithmetic unit may execute the fixed-length integer arithmetic.

誤差有制御処理部１５は、ロボットアーム部２のロボットアーム２１及びハンド２２の制御処理のうち、固定長数値演算の一例である浮動小数点演算を用いた誤差有制御処理を実行する。座標変換処理部１５ａは、浮動小数点データ記憶部１２から浮動小数点データを読み出し、浮動小数点演算部１５ｂを用いて座標変換処理等（ｓｉｎ、ｃｏｓ演算）を浮動小数点演算で実行し、演算結果を浮動小数点データ記憶部１２に格納する。浮動小数点演算部１５ｂは、加算器、減算器、乗算器及び除算器としての機能を有し、浮動小数点演算を用いて、加算、減算、乗算及び除算を実行する。   The control unit with error 15 executes a control process with error using a floating-point calculation, which is an example of a fixed-length numerical calculation, among the control processes of the robot arm 21 and the hand 22 of the robot arm unit 2. The coordinate conversion processing unit 15a reads floating-point data from the floating-point data storage unit 12, performs coordinate conversion processing (sin, cos calculation), etc. by floating-point calculation using the floating-point calculation unit 15b, and floats the calculation result. Stored in the decimal point data storage unit 12. The floating point arithmetic unit 15b functions as an adder, a subtracter, a multiplier, and a divider, and executes addition, subtraction, multiplication, and division using floating point arithmetic.

無誤差制御処理部１６は、ロボットアーム部２のロボットアーム２１及びハンド２２の制御処理のうち、可変長整数演算を用いた無誤差制御処理を実行する。方程式解法処理部１６ａは、可変長整数データ記憶部１４から可変長整数データを読み出し、制御方程式の求解等を可変長整数演算部１６ｂを用いて可変長整数演算で実行し、演算結果を可変長整数データ記憶部１４に格納する。可変長整数演算部１６ｂは、加算器、減算器及び乗算器としての機能を有し、可変長整数演算を用いて、加算、減算及び乗算を無誤差で実行する。   The error-free control processing unit 16 executes error-free control processing using variable-length integer arithmetic among the control processing of the robot arm 21 and the hand 22 of the robot arm unit 2. The equation solution processing unit 16a reads the variable-length integer data from the variable-length integer data storage unit 14, executes solution of the control equation, etc. by variable-length integer calculation unit 16b using the variable-length integer calculation unit 16b, and sets the calculation result to variable length Stored in the integer data storage unit 14. The variable-length integer arithmetic unit 16b functions as an adder, a subtracter, and a multiplier, and performs addition, subtraction, and multiplication without error using variable-length integer arithmetic.

ここで、本実施の形態の制御対象となるロボットアーム２１について詳細に説明する。図４は、図１に示すロボットアーム部２における右側のロボットアーム２１の構成を示す模式図であり、図５は、図１に示す右側のロボットアーム２１の座標系を説明するための図である。なお、左側のロボットアーム２１も、図４及び図５に示す右側のロボットアーム２１と同様に構成されているので、詳細な説明は省略する。   Here, the robot arm 21 to be controlled in this embodiment will be described in detail. 4 is a schematic diagram showing a configuration of the right robot arm 21 in the robot arm unit 2 shown in FIG. 1, and FIG. 5 is a diagram for explaining a coordinate system of the right robot arm 21 shown in FIG. is there. The left robot arm 21 is configured in the same manner as the right robot arm 21 shown in FIGS.

図４に示すように、グローバル座標系の原点Ｏを上腕２１ａの右肩（第１関節）に、グローバル座標系のＸ座標軸を胴体部２３（図１参照）の正面方向に沿って床面と平行方向に、グローバル座標系のＹ座標軸を右肩から左肩方向に、グローバル座標系のＺ座標軸を床面から天井への垂直方向にそれぞれ設定する。また、ロボットアーム２１の上腕２１ａの長さをｅ_１、前腕２１ｂの長さをｅ_２とし、上腕２１ａの肩関節のうち第１関節（Ｙ軸回り）の関節角度θ_１、第２関節（Ｘ軸回り）の関節角度θ_２、第３関節（Ｚ軸回り）の関節角度θ_３とし、上腕２１ａと前腕２１ｂとの間の第４関節（Ｙ軸回り）の関節角度θ_４、第５関節（Ｚ軸回り）の関節角度θ_５とし、前腕２１ｂとハンド２２との間の第６関節（Ｙ軸回り）の関節角度θ_６とする。 As shown in FIG. 4, the origin O of the global coordinate system is on the right shoulder (first joint) of the upper arm 21a, and the X coordinate axis of the global coordinate system is on the floor surface along the front direction of the body portion 23 (see FIG. 1). In the parallel direction, the Y coordinate axis of the global coordinate system is set from the right shoulder to the left shoulder direction, and the Z coordinate axis of the global coordinate system is set in the vertical direction from the floor surface to the ceiling. Further, the length of the upper arm 21a of the robot arm 21 is e ₁ , the length of the forearm 21b is e _2, and the joint angle θ ₁ of the first joint (around the Y axis) out of the shoulder joints of the upper arm 21a, the second joint ( The joint angle θ ₂ around the X axis, the joint angle θ ₃ around the third joint (around the Z axis), and the joint angle θ ₄ around the fourth joint (around the Y axis) between the upper arm 21a and the forearm 21b, the fifth a joint angle theta ₅ of the joint (Z axis), and the sixth joint angle theta ₆ of the joint (Y axis) between a forearm 21b and the hand 22.

このとき、前腕２１ｂの先端位置・方向Ｐ_ｓすなわち操作者Ｍの手首に装着した３次元位置方向センサ４が示すグローバル座標に対する変換行列をＨ_０ｓとすると、以下の式（１）のように表現することができる。 In this case, when the transformation matrix for the global coordinates indicated by the 3-dimensional position-direction sensor 4 attached to the wrist front end position and direction P _s That operator M forearm 21b and H _{0 s,} expressed as the following equation (1) can do.

ここで、図５に示すように、３次元位置方向センサ４の３次元位置はＰ_ｓ（ｘ_ｓ，ｙ_ｓ，ｚ_ｓ）、３次元位置方向センサ４の方向を表すＸ軸方向の単位ベクトルはｄ_ｘｓ（ｕ_ｘｓ，ｖ_ｘｓ，ｗ_ｘｓ）、Ｙ軸方向の単位ベクトルはｄ_ｙｓ（ｕ_ｙｓ，ｖ_ｙｓ，ｗ_ｙｓ）、Ｚ軸方向の単位ベクトルはｄ_ｚｓ（ｕ_ｚｓ，ｖ_ｚｓ，ｗ_ｚｓ）となる。これらの値はすべて、３次元位置方向センサ４から取得されるので、既知となる。一方、Ｈ_０ｓの各要素は、各アーム（上腕２１ａ及び前腕２１ｂ）の軸の長さｅ_１，ｅ_２と各関節角度θ_１，θ_２，θ_３，θ_４，θ_５を用いて、以下のように表現することができる。 Here, as shown in FIG. 5, the three-dimensional position of the three-dimensional position / direction sensor 4 is P _s (x _s , y _s , z _s ), a unit vector in the X-axis direction that represents the direction of the three-dimensional position / direction sensor 4. Is d _xs (u _xs , v _xs , w _xs ), a unit vector in the Y-axis direction is d _ys (u _ys , v _ys , w _ys ), and a unit vector in the Z-axis direction is d _zs (u _zs , v _zs , w _zs ). These values are all known from the three-dimensional position / direction sensor 4. On the other hand, each element of H _0s uses the shaft lengths e ₁ and e ₂ and the joint angles θ ₁ , θ ₂ , θ ₃ , θ ₄ , and θ ₅ of each arm (the upper arm 21a and the forearm 21b), It can be expressed as follows.

ここで、Ｃ_ｎ＝ｃｏｓ（θ_ｎ）、Ｓ_ｎ＝ｓｉｎ（θ_ｎ）（ｎ＝１，…，５）であり、関節角度θ_ｎは、アーム動作により刻々と変化するので、時間ｔの関数となり、θ_ｎ’を微小時間Δｔ前の関節角度（直前の関節角度）とし、微小時間Δｔ経過後における微小関節角度変化量をΔθ_ｎとすると、以下のように表現することができる。
θ_ｎ＝θ_ｎ’＋Δθ_ｎ （６）
ここで、微小時間Δｔ前の関節角度値θ_ｎ’は既知であり、微小時間Δｔは制御時間間隔である。 Here, C _n = cos (θ _n ), S _n = sin (θ _n ) (n = 1,..., 5), and the joint angle θ _n changes every moment due to the arm operation. Assuming that θ _n ′ is the joint angle before the minute time Δt (the previous joint angle) and the minute joint angle change amount after the minute time Δt has elapsed is Δθ _n , it can be expressed as follows.
θ _n = θ _n '+ Δθ _n (6)
Here, the joint angle value θ _n ′ before the minute time Δt is known, and the minute time Δt is a control time interval.

上記の前提を基に、以下のように近似することができる。なお、下記式では、（Δθ）^２は微小であるので、（Δθ）^２以上の高次項は無視できるとした。
ｓｉｎ（θ）＝ｓｉｎ（θ’＋Δθ）≒ｓｉｎ（θ’）＋（ｓｉｎ（θ’））’Δθ＝ｓｉｎ（θ’）＋ｃｏｓ（θ’）Δθ （７ａ）
ｃｏｓ（θ）＝ｃｏｓ（θ’＋Δθ）≒ｃｏｓ（θ’）＋（ｃｏｓ（θ’））’Δθ＝ｃｏｓ（θ’）−ｓｉｎ（θ’）Δθ （７ｂ）
上記の式（７ａ）、（７ｂ）を式（２）〜（５）へ代入すると、以下となる。なお、ここでも、Δθ_１Δθ_２等の高次のΔθは微小であり、無視できるとした。 Based on the above assumptions, it can be approximated as follows. In the following expression, (Δθ) ² is very small, and therefore higher-order terms of (Δθ) ² or more can be ignored.
sin (θ) = sin (θ ′ + Δθ) ≈sin (θ ′) + (sin (θ ′)) ′ Δθ = sin (θ ′) + cos (θ ′) Δθ (7a)
cos (θ) = cos (θ ′ + Δθ) ≈cos (θ ′) + (cos (θ ′)) ′ Δθ = cos (θ ′) − sin (θ ′) Δθ (7b)
Substituting the above equations (7a) and (7b) into equations (2) to (5) yields the following. In this case as well, high-order Δθ such as Δθ ₁ Δθ ₂ is very small and can be ignored.

上記各式より、式（１）は、以下のように表現することができる。   From the above equations, equation (1) can be expressed as follows.

そこで、Δθ_１，Δθ_２，Δθ_３，Δθ_４，Δθ_５は、以下の５元連立一次方程式の解として求めることができる。 Therefore, Δθ ₁ , Δθ ₂ , Δθ ₃ , Δθ ₄ , and Δθ ₅ can be obtained as solutions of the following five-way simultaneous linear equations.

また、上式の左辺の行列の各要素を係数ａ_ｉｊ（ｉ＝１，…，５、ｊ＝１，…，５）、右辺のベクトルの各要素を係数ｂ_ｉ（ｉ＝１，…，５）で表すと、以下のように表現することができる。 Further, each element of the matrix on the left side of the above expression is a coefficient a _ij (i = 1,..., 5, j = 1,..., 5), and each element of the vector on the right side is a coefficient b _i (i = 1,. When expressed in 5), it can be expressed as follows.

上記の５元連立一次方程式の解法では、加減乗除算が繰り返されるために、浮動小数点演算を用いた数値計算では、数値演算誤差とその蓄積のために解の誤差が大きくなり、連立方程式の解を求める処理が演算誤差のために暴走及び破綻することが多々ある。このように解の誤差が大きくなること、及び連立方程式の解を求める処理が暴走及び破綻することは、ロボットアームが人に危害を加える恐れがあるため、ロボットアームの制御においては許されない。また、除算を使用する場合、数値計算に計算精度の低下等の様々な悪影響を及ぼすことはもちろんのこと、無誤差演算自体を行うことはできない。   In the solution of the above five-way simultaneous linear equations, addition / subtraction / multiplication / division is repeated. Therefore, in numerical calculation using floating point arithmetic, the numerical calculation error and the error due to its accumulation increase. In many cases, the process for obtaining a runaway and failure occurs due to a calculation error. Such an increase in the error of the solution and the runaway and failure of the processing for finding the solution of the simultaneous equations are not allowed in the control of the robot arm because the robot arm may cause harm to humans. In addition, when division is used, error-free calculation itself cannot be performed as well as various adverse effects such as a decrease in calculation accuracy in numerical calculations.

このため、本実施の形態では、ガウス・ジョルダンの消去法を基本にし、最後の１回を除いて除算が発生しない無誤差連立一次方程式解法処理を考案し、式（１５）の解法に誤差を発生させない無誤差演算を用い、各関節角度θ_１，θ_２，θ_３，θ_４，θ_５を以下のようにして求めている。 Therefore, in this embodiment, based on the Gauss-Jordan elimination method, an error-free simultaneous linear equation solving process that does not generate division except for the last one is devised, and an error is introduced into the solution of equation (15). Using the error-free calculation that does not occur, the joint angles θ ₁ , θ ₂ , θ ₃ , θ ₄ , and θ ₅ are obtained as follows.

まず、式（１５）の係数ａ_ｉｊ（ｉ＝１，…，５、ｊ＝１，…，５）及び係数ｂ_ｉ（ｉ＝１，…，５）を用いて、ｋ＝１，…，５として、下記の式を繰り返し演算することにより係数ａ^６ _ｉｊ及び係数ｂ^６ _ｉを求める。
ａ^ｋ＋１ _ｉｊ＝ａ^ｋ _ｉｊ （ｉ＝ｋ） （１６ａ）
ａ^ｋ＋１ _ｉｊ＝ａ^ｋ _ｉｊａ^ｋ _ｋｋ−ａ^ｋ _ｋｊａ^ｋ _ｉｋ （ｉ≠ｋ） （１６ｂ）
ｂ^ｋ＋１ _ｉ＝ｂ^ｋ _ｉ （ｉ＝ｋ） （１６ｃ）
ｂ^ｋ＋１ _ｉ＝ｂ^ｋ _ｉａ^ｋ _ｋｋ−ｂ^ｋ _ｋａ^ｋ _ｉｋ （ｉ≠ｋ） （１６ｄ）
上記の係数ａ^６ _ｉｊ及び係数ｂ^６ _ｉにより、式（１５）は以下の形式となる。 First, k = 1,..., Using coefficients a _ij (i = 1,..., 5, j = 1,..., 5) and coefficients b _i (i = 1,. 5, the coefficient a ⁶ _ij and the coefficient b ⁶ _i are obtained by repeatedly calculating the following equation.
a ^{k + 1} _ij = a ^k _ij (i = k) (16a)
a ^{k + 1} _ij = a ^k _ij a ^k _kk −a ^k _kj a ^k _ik (i ≠ k) (16b)
b ^{k + 1} _i = b ^k _i (i = k) (16c)
^{_{b k + 1 i = b k}} i a k kk -b k k a k ik (i ≠ k) (16d)
With the coefficient a ⁶ _ij and the coefficient b ⁶ _i , the expression (15) is in the following form.

上記のように、式（１７）の左辺の係数行列は、対角要素以外が０となる対角行列となる。上記の演算までは、除算を使用する必要がないため、本実施の形態では、式（１５）の連立一次方程式の解法処理のうち上記の演算までの処理を、無誤差制御処理部１６による無誤差演算を用いて実行し、ここまでの演算を無誤差で行っている。   As described above, the coefficient matrix on the left side of Expression (17) is a diagonal matrix in which the elements other than the diagonal elements are zero. Since it is not necessary to use division until the above calculation, in the present embodiment, the processing up to the above calculation in the solution processing of the simultaneous linear equations of Equation (15) is performed by the error-free control processing unit 16. The calculation is performed using error calculation, and the calculation so far is performed without error.

次に、各関節角度θ_１，θ_２，θ_３，θ_４，θ_５は、以下の式により求めることができる。 Next, the joint angles θ ₁ , θ ₂ , θ ₃ , θ ₄ , and θ ₅ can be obtained by the following equations.

上記の式（１８）の演算だけは、除算が回避できないために、本実施の形態では、誤差有制御処理部１５による浮動小数点演算を用いて実行している。この浮動小数点演算により誤差が発生するが、この処理は最終処理であり、且つ１回の除算を行うだけであるため、除算に充分な精度が確保されていれば、誤差が問題になることはなく、誤差の蓄積も起こらない。なお、手首の角度θ_６は、３次元操作グローブ３により直接取得することができるので、上記の無誤差連立一次方程式解法処理からは求めていない。 In the present embodiment, only the calculation of the above equation (18) cannot be avoided, and therefore, in the present embodiment, the calculation is performed using the floating point calculation by the control unit with error 15. Although an error occurs due to this floating-point operation, since this process is a final process and only one division is performed, the error may be a problem if sufficient accuracy is ensured for the division. There is no error accumulation. Since the wrist angle θ ₆ can be directly obtained by the three-dimensional operation glove 3, it is not obtained from the above error-free simultaneous linear equation solving process.

ロボットアーム制御装置１は、上記の無誤差連立一次方程式解法処理（目標データＰから関節角度θを求める解法処理）を以下のようにして実行している。   The robot arm control device 1 executes the error-free simultaneous linear equation solving process (the solving process for obtaining the joint angle θ from the target data P) as follows.

外部インターフェース部１１は、ロボットアームの２１の先端部の目標３次元位置（３次元位置方向センサ４の３次元位置Ｐ_ｓ）及び目標方向（Ｘ、Ｙ、Ｚ軸方向の単位ベクトルｄ_ｘｓ、ｄ_ｙｓ、ｄ_ｚｓ）からなる目標データＰを３次元位置方向センサ４から取得し、浮動小数点データ記憶部１２に浮動小数点データ形式で記憶させる。このとき、浮動小数点データ記憶部１２には、微小時間Δｔ前の位置方向データＰ’及び微小時間Δｔ前の関節角度データθ’_ｉが記憶されており、また、上腕２１ａの長さｅ_１、前腕２１ｂの長さｅ_２等の初期値は予め記憶されているものとする。 The external interface unit 11 includes a target three-dimensional position (three-dimensional position P _s of the three-dimensional position / direction sensor 4) and a target direction (unit vectors d _xs , d in the X, Y, and Z axis directions) of the tip of the robot arm 21. Target data P consisting of _ys , d _zs ) is acquired from the three-dimensional position / direction sensor 4 and stored in the floating point data storage unit 12 in the floating point data format. At this time, the floating-point data storage unit 12 stores the position direction data P ′ before the minute time Δt and the joint angle data θ ′ _i before the minute time Δt, and the length e _{1 of} the upper arm 21a. the initial value of such length e ₂ of the forearm 21b is assumed to be stored in advance.

ここで、上記の微小時間Δｔ前の位置方向データＰ’は、微小時間Δｔ前の目標データではなく、微小時間Δｔ前の実際の３次元位置データ及び方向データであり、微小時間Δｔ前の関節角度データθ’_ｉもアームの実際の関節角度である。これは、後述するように、Ｐ’、θ’及びＰからΔθを求め（線形近似後、連立一次方程式を無誤差で解く）、そして、θ＝θ’＋Δθからθを求めており、ここで（線形）近似しているため、このθから求めた実際の位置方向データは目標データと一致せず、次の時刻の計算では、微小時間Δｔ前の目標データ等ではなく、微小時間Δｔ前の実際の３次元位置データ及び方向データ、並びに微小時間Δｔ前の実際の関節角度データを用いているためである。 Here, the position direction data P ′ before the minute time Δt is not the target data before the minute time Δt but the actual three-dimensional position data and direction data before the minute time Δt, and the joint before the minute time Δt. The angle data θ ′ _{i is} also the actual joint angle of the arm. As will be described later, Δθ is obtained from P ′, θ ′ and P (after linear approximation, the simultaneous linear equations are solved without error), and θ is obtained from θ = θ ′ + Δθ. Since (linear) approximation is performed, the actual position / direction data obtained from θ does not match the target data, and the next time calculation is not the target data before the minute time Δt but the minute data Δt before. This is because actual three-dimensional position data and direction data, and actual joint angle data before a minute time Δt are used.

座標変換処理部１５ａは、浮動小数点データ記憶部１２から微小時間Δｔ前の関節角度θ’_ｉ、上腕２１ａの長さｅ_１、前腕２１ｂの長さｅ_２を読み出し、浮動小数点演算部１５ｂを用いて浮動小数点演算を実行することにより式（１５）の係数データａ_ｉｊを算出し、浮動小数点データ記憶部１２に記憶させる。また、座標変換処理部１５ａは、浮動小数点データ記憶部１２から目標データＰ及び微小時間Δｔ前の位置方向データＰ’を読み出し、浮動小数点演算部１５ｂを用いて浮動小数点演算を実行することにより式（１５）の係数データｂ_ｉを算出し、浮動小数点データ記憶部１２に記憶させる。 Coordinate conversion processing unit 15a reads the floating-point data minute time from the storage unit 12 Delta] t before joint angle theta _'i, the length _{e 1} of the upper arm 21a, the forearm 21b length _{e 2,} using a floating point arithmetic unit 15b Then, the coefficient data a _ij of the equation (15) is calculated by executing the floating point calculation, and is stored in the floating point data storage unit 12. Further, the coordinate conversion processing unit 15a reads the target data P and the position direction data P ′ before the minute time Δt from the floating point data storage unit 12, and executes the floating point calculation using the floating point calculation unit 15b. The coefficient data b _i of (15) is calculated and stored in the floating point data storage unit 12.

データ変換処理部１３は、浮動小数点データ記憶部１２から係数ａ_ｉｊ及び係数ｂ_ｉを順次読み出し、浮動小数点データ形式から可変長整数データ形式に変換した係数ａ_ｉｊ及び係数ｂ_ｉを、係数ａ^１ _ｉｊ及び係数ｂ^１ _ｉとして可変長整数データ記憶部１４に格納する。 The data conversion processing unit 13 sequentially reads the coefficient a _ij and the coefficient b _i from the floating-point data storage unit 12 and converts the coefficient a _ij and the coefficient b _i converted from the floating-point data format to the variable-length integer data format into the coefficient a ¹ _ij and coefficient b ¹ _i are stored in the variable-length integer data storage unit 14.

方程式解法処理部１６ａは、可変長整数データ記憶部１４から係数ａ^１ _ｉｊ及び係数ｂ^１ _ｉを読み出し、可変長整数演算部１６ｂを用いて可変長整数演算（無誤差演算）を実行することにより、式（１６ａ）乃至（１６ｄ）を用いて係数ａ^ｋ＋１ _ｉｊ及びｂ^ｋ＋１ _ｉを順次算出し、最終的に係数ａ^６ _ｉｊ及びｂ^６ _ｉを可変長整数データ記憶部１４に格納する。 The equation solution processing unit 16a reads the coefficient a ¹ _ij and the coefficient b ¹ _i from the variable length integer data storage unit 14, and executes a variable length integer calculation (error-free calculation) using the variable length integer calculation unit 16b. The coefficients a ^{k + 1} _ij and b ^{k + 1} _i are sequentially calculated using the equations (16a) to (16d), and finally the coefficients a ⁶ _ij and b ⁶ _i are stored in the variable-length integer data storage unit 14.

データ変換処理部１３は、可変長整数データ記憶部１４から係数ａ^６ _ｉｉ及び係数ｂ^６ _ｉを順次読み出し、可変長整数データ形式から浮動小数点データ形式に変換した係数ａ^６ _ｉｉ及び係数ｂ^６ _ｉを浮動小数点データ記憶部１２に格納する。 The data conversion processing unit 13 sequentially reads the coefficient a ⁶ _ii and the coefficient b ⁶ _i from the variable length integer data storage unit 14 and converts the coefficient a ⁶ _ii and the coefficient b ⁶ _i from the variable length integer data format to the floating point data format. Is stored in the floating-point data storage unit 12.

座標変換処理部１５ａは、浮動小数点データ記憶部１２から係数ａ^６ _ｉｉ及び係数ｂ^６ _ｉを順次読み出し、浮動小数点演算部１５ｂを用いて浮動小数点演算の除算を１回だけ実行することにより、微小関節角度変化量Δθ_ｉ＝ｂ^６ _ｉ／ａ^６ _ｉｉを算出し、浮動小数点データ記憶部１２に格納する。そして、座標変換処理部１５ａは、浮動小数点データ記憶部１２から直前の関節角度値θ’_ｉ及び微小関節角度変化量Δθ_ｉを順次読み出し、浮動小数点演算部１５ｂを用いて浮動小数点演算を実行することにより式（１８）を用いて関節角度θ_ｉを算出し、浮動小数点データ記憶部１２に格納する。 The coordinate conversion processing unit 15a sequentially reads the coefficient a ⁶ _ii and the coefficient b ⁶ _i from the floating-point data storage unit 12 and executes the division of the floating-point calculation only once using the floating-point calculation unit 15b. The joint angle change amount Δθ _i = b ⁶ _i / a ⁶ _ii is calculated and stored in the floating point data storage unit 12. Then, the coordinate conversion processing unit 15a sequentially reads the previous joint angle value θ ′ _i and the minute joint angle change amount Δθ _i from the floating point data storage unit 12, and executes the floating point calculation using the floating point calculation unit 15b. Thus, the joint angle θ _i is calculated using the equation (18) and stored in the floating point data storage unit 12.

外部インターフェース部１１は、浮動小数点データ記憶部１２から関節角度θ_ｉを読み出してロボットアーム部２へ出力する。ロボットアーム部２は、ロボットアーム２１の各関節角度が関節角度θ_ｉとなるようにロボットアーム２１を動作させる。 The external interface unit 11 reads the joint angle θ _i from the floating point data storage unit 12 and outputs it to the robot arm unit 2. The robot arm unit 2 operates the robot arm 21 so that each joint angle of the robot arm 21 becomes the joint angle θ _i .

上記の制御動作により、操作者Ｍは、３次元ディスプレイ５を用いて、カメラ部２４により撮影された画像を見ながら、両手に装着した３次元操作グローブ３を用いて、ロボットアーム２１及びハンド２２を操作し、操作者Ｍの腕と手の動きとがロボットアーム２１及びハンド２２により忠実に再現される。   By the above control operation, the operator M uses the 3D operation glove 3 attached to both hands while viewing the image taken by the camera unit 24 using the 3D display 5 and uses the robot arm 21 and the hand 22. The movement of the arm and hand of the operator M is faithfully reproduced by the robot arm 21 and the hand 22.

本実施の形態では、データ変換処理部１３が変換手段の一例に相当し、無誤差制御処理部１６（方程式解法処理部１６ａ及び可変長整数演算部１６ｂ）が無誤差演算手段の一例に相当し、誤差有制御処理部１５（座標変換処理部１５ａ及び浮動小数点演算部１５ｂ）が誤差演算手段の一例に相当する。   In the present embodiment, the data conversion processing unit 13 corresponds to an example of a conversion unit, and the error-free control processing unit 16 (the equation solution processing unit 16a and the variable length integer calculation unit 16b) corresponds to an example of an error-free calculation unit. The control unit 15 with error (coordinate conversion processing unit 15a and floating point calculation unit 15b) corresponds to an example of an error calculation unit.

次に、上記のように構成されたロボットアーム制御装置１によるロボットアーム制御処理について説明する。図６は、図１に示すロボットアーム制御装置１によるロボットアーム制御処理を説明するためのフローチャートである。   Next, robot arm control processing by the robot arm control device 1 configured as described above will be described. FIG. 6 is a flowchart for explaining the robot arm control processing by the robot arm control device 1 shown in FIG.

図６に示すように、まず、ステップＳ１１において、外部インターフェース部１１は、３次元位置方向センサ４から３次元位置データＰ_ｓ（ｘ_ｓ，ｙ_ｓ，ｚ_ｓ）、Ｘ軸方向の単位ベクトルｄ_ｘｓ（ｕ_ｘｓ，ｖ_ｘｓ，ｗ_ｘｓ）、Ｙ軸方向の単位ベクトルｄ_ｙｓ（ｕ_ｙｓ，ｖ_ｙｓ，ｗ_ｙｓ）、及びＺ軸方向の単位ベクトルｄ_ｚｓ（ｕ_ｚｓ，ｖ_ｚｓ，ｗ_ｚｓ）を取得して浮動小数点データ記憶部１２に格納する。 As shown in FIG. 6, first, in step S11, the external interface unit 11 receives the three-dimensional position data P _s (x _s , y _s , z _s ) from the three-dimensional position / direction sensor 4 and the unit vector d in the X-axis direction. _xs (u _xs , v _xs , w _xs ), unit vector d _ys (u _ys , v _ys , w _ys ) in the Y-axis direction, and unit vector d _zs (u _zs , v _zs , w _zs ) in the Z-axis direction Is stored in the floating-point data storage unit 12.

次に、ステップＳ１２において、座標変換処理部１５ａは、浮動小数点データ記憶部１２から３次元位置データＰ_ｓ（ｘ_ｓ，ｙ_ｓ，ｚ_ｓ）等を順次読み出し、浮動小数点演算部１５ｂを用いて、式（１５）の係数ａ_ｉｊ（ｉ＝１，…，５、ｊ＝１，…，５）及び係数ｂ_ｉ（ｉ＝１，…，５）を求め、浮動小数点データ記憶部１２に格納する。 Next, in step S12, the coordinate conversion processing unit 15a sequentially reads the three-dimensional position data P _s (x _s , y _s , z _s ) and the like from the floating point data storage unit 12 and uses the floating point calculation unit 15b. , The coefficient a _ij (i = 1,..., 5, j = 1,..., 5) and the coefficient b _i (i = 1,..., 5) in Expression (15) are obtained and stored in the floating-point data storage unit 12. To do.

次に、ステップＳ１３において、データ変換処理部１３は、浮動小数点データ記憶部１２から係数ａ_ｉｊ及び係数ｂ_ｉを順次読み出し、浮動小数点データ形式から可変長整数データ形式に変換した係数ａ_ｉｊ及び係数ｂ_ｉを、係数ａ^１ _ｉｊ及び係数ｂ^１ _ｉとして可変長整数データ記憶部１４に格納する。 Next, in step S13, the data conversion processing unit 13 sequentially reads the coefficient a _ij and the coefficient b _i from the floating point data storage unit 12, and converts the coefficient a _ij and the coefficient converted from the floating point data format to the variable length integer data format. b _i is stored in the variable-length integer data storage unit 14 as a coefficient a ¹ _ij and a coefficient b ¹ _i .

なお、上記のステップＳ１２及びＳ１３では、浮動小数点演算を用いて３次元位置データＰ_ｓ（ｘ_ｓ，ｙ_ｓ，ｚ_ｓ）等から係数ａ_ｉｊ及び係数ｂ_ｉを求めた後に、可変長整数データへフォーマット変換したが、この例に特に限定されず、データ変換処理部１３により３次元位置データＰ_ｓ（ｘ_ｓ，ｙ_ｓ，ｚ_ｓ）、Ｘ軸方向の単位ベクトルｄ_ｘｓ（ｕ_ｘｓ，ｖ_ｘｓ，ｗ_ｘｓ）、Ｙ軸方向の単位ベクトルｄ_ｙｓ（ｕ_ｙｓ，ｖ_ｙｓ，ｗ_ｙｓ）、及びＺ軸方向の単位ベクトルｄ_ｚｓ（ｕ_ｚｓ，ｖ_ｚｓ，ｗ_ｚｓ）を浮動小数点データから可変長整数データに変換し、その後、方程式解法処理部１６ａにより可変長整数データの３次元位置データＰ_ｓ（ｘ_ｓ，ｙ_ｓ，ｚ_ｓ）等から可変長整数データの係数ａ_ｉｊ及び係数ｂ_ｉを求めるようにしてもよい。 In steps S12 and S13, variable length integer data is obtained after the coefficients a _ij and the coefficients b _i are obtained from the three-dimensional position data P _s (x _s , y _s , z _s ) and the like using floating point arithmetic. However, the present invention is not particularly limited to this example, and the data conversion processing unit 13 uses the three-dimensional position data P _s (x _s , y _s , z _s ) and the unit vector d _xs (u _xs , v) in the X-axis direction. _xs , w _xs ), unit vector d _ys (u _ys , v _ys , w _ys ) in the Y-axis direction, and unit vector d _zs (u _zs , v _zs , w _zs ) in the Z-axis direction are variable from floating-point data converted to a long integer data, then, three-dimensional position data _P s of the variable length integer data by equation solving section _{16a (x s, y s,} z s) coefficients _{a ij} and engagement of the variable-length integer data from such I may be obtained b _i.

次に、ステップＳ１４〜Ｓ１６において、方程式解法処理部１６ａは、初期設定として、ｋ＝１、ｉ＝１、ｊ＝１に設定する。次に、ステップＳ１７において、方程式解法処理部１６ａは、可変長整数データ記憶部１４から係数ａ^ｋ _ｉｊ，ａ^ｋ _ｋｋ，ａ^ｋ _ｋｊ，ａ^ｋ _ｉｋを読み出し、可変長整数演算部１６ｂを用いて、ｉ＝ｋの場合はａ^ｋ＋１ _ｉｊ＝ａ^ｋ _ｉｊに設定し、ｉ≠ｋの場合はａ^ｋ＋１ _ｉｊ＝ａ^ｋ _ｉｊａ^ｋ _ｋｋ−ａ^ｋ _ｋｊａ^ｋ _ｉｋを算出し、係数ａ^ｋ＋１ _ｉｊを可変長整数データ記憶部１４に格納する。 Next, in steps S14 to S16, the equation solution processing unit 16a sets k = 1, i = 1, and j = 1 as initial settings. Next, in step S17, the equation solution processing unit 16a reads the coefficients a ^k _ij , a ^k _kk , a ^k _kj , and a ^k _ik from the variable length integer data storage unit 14, and uses the variable length integer calculation unit 16b. , If i = k, set a ^{k + 1} _ij = a ^k _ij , if i ≠ k, calculate a ^{k + 1} _ij = a ^k _ij a a ^k _kk −a ^k _kj a ^k _ik and calculate the coefficient a ^{k + 1} _ij It is stored in the variable length integer data storage unit 14.

次に、ステップＳ１８において、方程式解法処理部１６ａは、ｊ＜５であるか否かを判断し、ｊ＜５の場合、ステップＳ２７においてｊを１だけインクリメントしてステップＳ１７以降の処理を継続し、ｊ＜５でない（ｊ＝５）場合、ステップＳ１９へ処理を移行する。   Next, in step S18, the equation solution processing unit 16a determines whether or not j <5. If j <5, j is incremented by 1 in step S27 and the processing from step S17 is continued. If j <5 is not satisfied (j = 5), the process proceeds to step S19.

ｊ＜５でない場合、ステップＳ１９において、方程式解法処理部１６ａは、可変長整数データ記憶部１４から係数ｂ^ｋ _ｉ，ａ^ｋ _ｋｋ，ｂ^ｋ _ｋ，ａ^ｋ _ｉｋを読み出し、可変長整数演算部１６ｂを用いて、ｉ＝ｋの場合はｂ^ｋ＋１ _ｉ＝ｂ^ｋ _ｉに設定し、ｉ≠ｋの場合はｂ^ｋ＋１ _ｉ＝ｂ^ｋ _ｉａ^ｋ _ｋｋ−ｂ^ｋ _ｋａ^ｋ _ｉｋを算出し、係数ｂ^ｋ＋１ _ｉを可変長整数データ記憶部１４に格納する。 If j <5 is not satisfied, in step S19, the equation solution processing unit 16a reads the coefficients b ^k _i , a ^k _kk , b ^k _k , and a ^k _ik from the variable-length integer data storage unit 14, and the variable-length integer calculation unit 16b. Is set to b ^{k + 1} _i = b ^k _i if i = k, and b ^{k + 1} _i = b ^k _i a ^k _kk −b ^k _k a ^k _ik if i ≠ k, and the coefficient b ^{k + 1} _i is stored in the variable-length integer data storage unit 14.

次に、ステップＳ２０において、方程式解法処理部１６ａは、ｉ＜５であるか否かを判断し、ｉ＜５の場合、ステップＳ２６においてｉを１だけインクリメントしてステップＳ１６以降の処理を継続し、ｉ＜５でない（ｉ＝５）場合、ステップＳ２１へ処理を移行する。   Next, in step S20, the equation solution processing unit 16a determines whether i <5. If i <5, i is incremented by 1 in step S26, and the processing from step S16 is continued. If i <5 is not satisfied (i = 5), the process proceeds to step S21.

ｉ＜５でない場合、ステップＳ２１において、方程式解法処理部１６ａは、ｋ＜５であるか否かを判断し、ｋ＜５の場合、ステップＳ２５においてｋを１だけインクリメントしてステップＳ１５以降の処理を継続し、ｋ＜５でない（ｋ＝５）場合、ステップＳ２２へ処理を移行する。このように、ステップＳ１４〜Ｓ２１及びＳ２５〜Ｓ２７までの処理が無誤差演算を用いて実行される。   If i <5 is not satisfied, the equation solving processing unit 16a determines whether or not k <5 in step S21. If k <5, k is incremented by 1 in step S25, and the processing after step S15 is performed. If k <5 is not satisfied (k = 5), the process proceeds to step S22. In this way, the processes from steps S14 to S21 and S25 to S27 are executed using error-free computation.

次に、ステップＳ２２において、データ変換処理部１３は、可変長整数データ記憶部１４から係数ａ^６ _ｉｉ及び係数ｂ^６ _ｉ（ｉ＝１，…，５）を順次読み出し、可変長整数データ形式から浮動小数点データ形式に変換し、変換した係数ａ^６ _ｉｉ及び係数ｂ^６ _ｉを浮動小数点データ記憶部１２に格納する。 Next, in step S22, the data conversion processing unit 13 sequentially reads the coefficient a ⁶ _ii and the coefficient b ⁶ _i (i = 1,..., 5) from the variable length integer data storage unit 14, and from the variable length integer data format. The data is converted into the floating-point data format, and the converted coefficient a ⁶ _ii and coefficient b ⁶ _i are stored in the floating-point data storage unit 12.

次に、ステップＳ２３において、座標変換処理部１５ａは、浮動小数点データ記憶部１２から係数ａ^６ _ｉｉ及び係数ｂ^６ _ｉ（ｉ＝１，…，５）を順次読み出し、浮動小数点演算部１５ｂを用いて、微小関節角度変化量Δθ_ｉ＝ｂ^６ _ｉ／ａ^６ _ｉｉ（ｉ＝１，…，５）を算出し、浮動小数点データ記憶部１２に格納する。 Next, in step S23, the coordinate conversion processing unit 15a sequentially reads the coefficient a ⁶ _ii and the coefficient b ⁶ _i (i = 1,..., 5) from the floating point data storage unit 12, and uses the floating point calculation unit 15b. Then, the small joint angle change amount Δθ _i = b ⁶ _i / a ⁶ _ii (i = 1,..., 5) is calculated and stored in the floating-point data storage unit 12.

次に、ステップＳ２４において、座標変換処理部１５ａは、浮動小数点データ記憶部１２から直前の関節角度値θ’_ｉ及び微小関節角度変化量Δθ_ｉ（ｉ＝１，…，５）を順次読み出し、浮動小数点演算部１５ｂを用いて、式（１８）に従って関節角度θ_ｉ＝θ’_ｉ＋Δθ_ｉ（ｉ＝１，…，５）を算出し、浮動小数点データ記憶部１２に格納する。このとき、外部インターフェース部１１は、浮動小数点データ記憶部１２から関節角度θ_１，θ_２，θ_３，θ_４，θ_５を読み出してロボットアーム部２へ出力する。 Next, in step S24, the coordinate conversion processing unit 15a sequentially reads the previous joint angle value θ ′ _i and the minute joint angle change amount Δθ _i (i = 1,..., 5) from the floating point data storage unit 12, The joint angle θ _i = θ ′ _i + Δθ _i (i = 1,..., 5) is calculated using the floating point arithmetic unit 15 b according to the equation (18) and stored in the floating point data storage unit 12. At this time, the external interface unit 11 reads out the joint angles θ ₁ , θ ₂ , θ ₃ , θ ₄ , θ ₅ from the floating point data storage unit 12 and outputs them to the robot arm unit 2.

ロボットアーム部２は、ロボットアーム２１の各関節角度が関節角度θ_１，θ_２，θ_３，θ_４，θ_５となるようにロボットアーム２１を動作させる。なお、手首の関節角度θ_６は、３次元操作グローブ３から直接取得することができるので、３次元操作グローブ３により検出された関節角度θ_６がロボットアーム制御装置１を介してロボットアーム部２へ出力され、ロボットアーム部２は、ロボットアーム２１の手首の関節角度が関節角度θ_６となるようにロボットアーム２１を動作させる。上記の処理が制御時間間隔ごとに繰り返され、ロボットアーム２１の先端位置が操作者Ｍの手首の位置に応じて正確に制御される。 The robot arm unit 2 operates the robot arm 21 so that the joint angles of the robot arm 21 are the joint angles θ ₁ , θ ₂ , θ ₃ , θ ₄ , and θ ₅ . Since the wrist joint angle θ ₆ can be directly obtained from the three-dimensional operation glove 3, the joint angle θ ₆ detected by the three-dimensional operation glove 3 is obtained via the robot arm control device 1. is output to the robot arm unit 2 operates the robot arm 21 as wrist joint angles of the robot arm 21 is joint angle theta _6. The above process is repeated every control time interval, and the tip position of the robot arm 21 is accurately controlled according to the wrist position of the operator M.

上記の処理により、本実施の形態では、ロボットアーム制御処理において用いられる逆変換制御演算（連立一次方程式解法）に使用される数値を可変長の整数により表現するとともに、除算を実行することなく（最後の除算を除く）、加算、減算及び乗算のみを用いた無誤差演算処理を実行し、制御演算誤差をなくすことができる。   By the above processing, in this embodiment, the numerical value used for the inverse transformation control calculation (simultaneous linear equation solving) used in the robot arm control processing is expressed by a variable-length integer, and division is not performed ( Error-free arithmetic processing using only addition, subtraction, and multiplication can be executed (except for the last division), and control arithmetic errors can be eliminated.

この結果、ロボットアーム２１の先端部を高精度に制御することができるとともに、制御処理の破綻をなくしてロボットアーム２１の動作の暴走等をなくすことができ、ロボットアーム２１の動作の安定化及び高信頼化を図ることができる。また、ロボットアーム２１の暴走防止対策等が不要となるので、ロボットアーム制御装置１の構成を単純化することができ、装置の低コスト化を実現することができる。   As a result, the tip of the robot arm 21 can be controlled with high accuracy, the control process can be prevented from being broken, and the robot arm 21 can be prevented from running out of control. High reliability can be achieved. In addition, since it is not necessary to take measures to prevent the robot arm 21 from running away, the configuration of the robot arm control device 1 can be simplified, and the cost of the device can be reduced.

なお、本実施の形態では、操作者の手首の位置及び方向を検出し、これらを目標位置及び目標方向として制御したが、本発明による制御例は、この例に特に限定されず、３次元操作グローブ３、３次元位置方向センサ４及び３次元ディスプレイ５を省略し、テーチングを行うことにより浮動小数点データ記憶部１２に記憶した目標位置及び目標方向となるようにロボットアーム２１を制御する等の他の制御例にも適用可能である。また、ロボットアーム２１を制御対象としたが、この例に特に限定されず、ハンド２２を制御対象にする等の種々の変更が可能である。   In this embodiment, the position and direction of the wrist of the operator are detected and controlled as the target position and target direction. However, the control example according to the present invention is not particularly limited to this example, and the three-dimensional operation is performed. Other than the globe 3, the three-dimensional position / direction sensor 4 and the three-dimensional display 5, the robot arm 21 is controlled so that the target position and target direction stored in the floating-point data storage unit 12 are obtained by performing teaching. This example can also be applied. Although the robot arm 21 is the control target, the present invention is not particularly limited to this example, and various modifications such as making the hand 22 a control target are possible.

本発明の一実施の形態によるロボットアーム制御装置を用いたロボットアーム制御システムの構成を示すブロック図である。It is a block diagram which shows the structure of the robot arm control system using the robot arm control apparatus by one embodiment of this invention. 図１に示すロボットアーム制御装置の構成を示すブロック図である。It is a block diagram which shows the structure of the robot arm control apparatus shown in FIG. 浮動小数点データ及び可変長整数データのデータ構成の一例を示す図である。It is a figure which shows an example of the data structure of floating point data and variable-length integer data. 図１に示すロボットアーム部における右側のロボットアームの構成を示す模式図である。It is a schematic diagram which shows the structure of the robot arm of the right side in the robot arm part shown in FIG. 図３に示す右側のロボットアームの座標系を説明するための図である。It is a figure for demonstrating the coordinate system of the robot arm of the right side shown in FIG. 図１に示すロボットアーム制御装置によるロボットアーム制御処理を説明するためのフローチャートである。It is a flowchart for demonstrating the robot arm control processing by the robot arm control apparatus shown in FIG.

符号の説明Explanation of symbols

１ ロボットアーム制御装置
２ ロボットアーム部
３ ３次元操作グローブ
４ ３次元位置方向センサ
５ ３次元ディスプレイ
１１ 外部インターフェース部
１２ 浮動小数点データ記憶部
１３ データ変換処理部
１４ 可変長整数データ記憶部
１５ 誤差有制御処理部
１５ａ 座標変換処理部
１５ｂ 浮動小数点演算部
１６ 無誤差制御処理部
１６ａ 方程式解法処理部
１６ｂ 可変長整数演算部
２１ ロボットアーム
２１ａ 上腕
２１ｂ 前腕
２２ ハンド
２３ 胴体部
２４ カメラ部 DESCRIPTION OF SYMBOLS 1 Robot arm control apparatus 2 Robot arm part 3 3D operation glove 4 3D position direction sensor 5 3D display 11 External interface part 12 Floating point data storage part 13 Data conversion process part 14 Variable length integer data storage part 15 Control with error Processing unit 15a Coordinate transformation processing unit 15b Floating point arithmetic unit 16 Error-free control processing unit 16a Equation solution processing unit 16b Variable length integer arithmetic unit 21 Robot arm 21a Upper arm 21b Forearm 22 Hand 23 Body unit 24 Camera unit

Claims (5)

関節を介して複数のリンクを連結したリンク機構の所定部位を目標３次元位置及び目標方向に制御する位置制御装置であって、
リンク機構の所定部位の目標３次元位置データ及び目標方向データからなる目標データをＰ、リンク機構の各関節角度をθとし、目標データＰから関節角度θを求める解法処理に使用されるデータを固定長数値データ形式から可変長整数データ形式へ変換する変換手段と、
前記解法処理において、前記変換手段により変換された可変長整数データ形式のデータを用いて、除算を除く加算、減算及び乗算からなる無誤差演算を実行する無誤差演算手段とを備え、
前記変換手段は、前記無誤差演算手段の演算結果を可変長整数データ形式から固定長数値データ形式へ変換し、
前記解法処理において、前記変換手段により変換された固定長数値データ形式の演算結果を用いて１回のみの除算を含む固定長数値演算を実行することにより固定長数値データ形式の関節角度θを求める誤差演算手段をさらに備えることを特徴とする位置制御装置。 A position control device that controls a predetermined part of a link mechanism in which a plurality of links are connected via a joint in a target three-dimensional position and a target direction,
The target data consisting of target three-dimensional position data and target direction data of a predetermined part of the link mechanism is P, each joint angle of the link mechanism is θ, and the data used for the solution processing for obtaining the joint angle θ from the target data P is fixed. Conversion means for converting from a long numeric data format to a variable-length integer data format;
In the solution processing, using the data in the variable-length integer data format converted by the conversion means, comprising error-free calculation means for executing error-free calculation including addition, subtraction, and multiplication excluding division ,
The converting means converts the calculation result of the error-free calculating means from a variable-length integer data format to a fixed-length numeric data format,
In the solution processing, the joint angle θ in the fixed-length numeric data format is obtained by executing a fixed-length numeric calculation including only one division using the calculation result in the fixed-length numeric data format converted by the conversion means. A position control device further comprising error calculation means . 前記無誤差演算手段は、θ_ｉ＝θ’_ｉ＋Δθ_ｉ（ｉ＝１，…，ｎ）（ここで、θ_ｉは関節ｉの求める関節角度、θ’_ｉは微小時間Δｔ前の関節ｉの関節角度、Δθ_ｉは微小時間Δｔ経過後の関節ｉの微小関節角度変化量、ｎは求める関節角度θの数）とし、ｎ元連立一次方程式ＡΔθ＝Ｂ（ここで、Ａはその要素ａ_ｉｊ（ｉ＝１，…，ｎ、ｊ＝１，…，ｎ）がリンク機構のリンク長及び微小時間Δｔ前の関節ｉの関節角度θ’_ｉにより決定される係数行列、Ｂはその要素ｂ_ｉ（ｉ＝１，…，ｎ）が目標データＰ及び微小時間Δｔ前の位置方向データＰ’により決定されるベクトル）をＡｎΔθ＝Ｂｎ（ここで、Ａｎは対角要素ａ^ｎ＋１ _ｉｉ（ｉ＝１，…，ｎ）以外が０となる対角行列、Ｂｎは要素ｂ^ｎ＋１ _ｉ（ｉ＝１，…，ｎ）のベクトル）に変形したときの係数データａ^ｎ＋１ _ｉｉ及び係数データｂ^ｎ＋１ _ｉを、除算を除く加算、減算及び乗算からなる無誤差演算を用いて求め、
前記変換手段は、前記無誤差演算手段により求められた係数データａ^ｎ＋１ _ｉｉ及び係数データｂ^ｎ＋１ _ｉを可変長整数データ形式から固定長数値データ形式へ変換し、
前記誤差演算手段は、前記変換手段により変換された固定長数値データ形式の係数データａ^ｎ＋１ _ｉｉ及び係数データｂ^ｎ＋１ _ｉを用いて、θ_ｉ＝θ’_ｉ＋ｂ^ｎ＋１ _ｉ／ａ^ｎ＋１ _ｉｉ（ｉ＝１，…，ｎ）を固定長数値演算により求めることを特徴とする請求項１記載の位置制御装置。 The error-free computing means is: θ _i = θ ′ _i + Δθ _i (i = 1,..., N) (where θ _i is the joint angle obtained by the joint i, θ ′ _i is the joint i before the minute time Δt) The joint angle, Δθ _i is a minute joint angle change amount of the joint i after the minute time Δt has elapsed, n is the number of joint angles θ to be obtained, and n-ary simultaneous linear equations AΔθ = B (where A is an element a _ij (I = 1,..., N, j = 1,..., N) is a coefficient matrix determined by the link length of the link mechanism and the joint angle θ ′ _i of the joint i before the minute time Δt, and B is an element b _i (i = 1, ..., n ) is a vector) which is determined by the target data P and minute time Δt before the position-direction data P 'AnΔθ = Bn (where, an is diagonal element _{^{a n + 1 ii (i =}} 1 , ..., n) except becomes zero diagonal matrix, Bn elements _{^{b n + 1 i (i =}} 1, ..., n) varying in vector) of The coefficient data a ^{n + 1} _ii and coefficient data b ^{n + 1} _i when the, determined using summing except division, the error free operation consisting of subtraction and multiplication,
The conversion means converts the coefficient data a ^{n + 1} _ii and coefficient data b ^{n + 1} _i obtained by the error-free calculating means from a variable-length integer data format to a fixed-length numeric data format,
The error calculation means uses the coefficient data an ^{+ 1} _ii and coefficient data bn ^{+ 1} _i in the fixed-length numerical data format converted by the conversion means, and θ _i = θ ′ _i + b ^{n + 1} _i / a ^{n + 1} _ii (i = 1, ..., n) position control device according to claim 1, wherein the determination by the fixed length numerical operations to. 操作者の所定部位の３次元位置及び方向を検出する検出手段をさらに備え、
前記検出手段により検出された３次元位置及び方向を前記リンク機構の所定部位の目標３次元位置データ及び目標方向データとして用いて、前記リンク機構の所定部位を目標３次元位置及び目標方向に制御することを特徴とする請求項１又は２に記載の位置制御装置。 It further comprises detection means for detecting the three-dimensional position and direction of the predetermined part of the operator,
Using the three-dimensional position and direction detected by the detection means as target three-dimensional position data and target direction data of the predetermined part of the link mechanism, the predetermined part of the link mechanism is controlled to the target three-dimensional position and target direction. The position control device according to claim 1 or 2 , wherein 関節を介して複数のリンクを連結したリンク機構の所定部位を目標３次元位置及び目標方向に制御するロボットアーム制御方法であって、
前記リンク機構の所定部位の目標３次元位置データ及び目標方向データからなる目標データをＰ、前記リンク機構の各関節角度をθとし、目標データＰから関節角度θを求める解法処理に使用されるデータを固定長数値データ形式から可変長整数データ形式へ変換する変換ステップと、
前記解法処理において、変換された可変長整数データ形式の係数データを用いて、除算を除く加算、減算及び乗算からなる無誤差演算を実行する無誤差演算ステップとを含み、
前記変換ステップは、前記無誤差演算ステップにおける演算結果を可変長整数データ形式から固定長数値データ形式へ変換するステップを含み、
前記解法処理において、前記変換ステップにおいて変換された固定長数値データ形式の演算結果を用いて１回のみの除算を含む固定長数値演算を実行することにより固定長数値データ形式の関節角度θを求める誤差演算ステップをさらに含むことを特徴とする位置制御方法。 A robot arm control method for controlling a predetermined part of a link mechanism in which a plurality of links are connected via a joint to a target three-dimensional position and a target direction,
Data used for the solution processing for determining the joint angle θ from the target data P, where P is target data composed of target three-dimensional position data and target direction data of a predetermined part of the link mechanism, and θ is each joint angle of the link mechanism. A conversion step for converting from a fixed-length numeric data format to a variable-length integer data format;
In the solution process, using the coefficient data of the converted variable-length integer data format, seen including adding, a non-error calculation step of performing a non-error calculation consisting of subtraction and multiplication, except for division,
The conversion step includes a step of converting the calculation result in the error-free calculation step from a variable-length integer data format to a fixed-length numeric data format,
In the solution processing, the joint angle θ in the fixed-length numeric data format is obtained by executing a fixed-length numeric calculation including only one division using the calculation result in the fixed-length numeric data format converted in the conversion step. A position control method further comprising an error calculation step . 関節を介して複数のリンクを連結したリンク機構の所定部位を目標３次元位置及び目標方向に制御するための位置制御プログラムであって、
前記リンク機構の所定部位の目標３次元位置データ及び目標方向データからなる目標データをＰ、前記リンク機構の各関節角度をθとし、目標データＰから関節角度θを求める解法処理に使用されるデータを固定長数値データ形式から可変長整数データ形式へ変換する変換手段と、
前記解法処理において、前記変換手段により変換された可変長整数データ形式の係数データを用いて、除算を除く加算、減算及び乗算からなる無誤差演算を実行する無誤差演算手段としてコンピュータを機能させ、
前記変換手段は、前記無誤差演算手段の演算結果を可変長整数データ形式から固定長数値データ形式へ変換し、
前記解法処理において、前記変換手段により変換された固定長数値データ形式の演算結果を用いて１回のみの除算を含む固定長数値演算を実行することにより固定長数値データ形式の関節角度θを求める誤差演算手段として前記コンピュータをさらに機能させることを特徴とする位置制御プログラム。 A position control program for controlling a predetermined part of a link mechanism in which a plurality of links are connected via a joint to a target three-dimensional position and a target direction,
Data used for the solution processing for determining the joint angle θ from the target data P, where P is target data composed of target three-dimensional position data and target direction data of a predetermined part of the link mechanism, and θ is each joint angle of the link mechanism. Means for converting from a fixed-length numeric data format to a variable-length integer data format;
In the solution processing, using the coefficient data in the variable-length integer data format converted by the conversion means, the computer functions as error-free calculation means for performing error-free calculation including addition, subtraction, and multiplication excluding division ,
The converting means converts the calculation result of the error-free calculating means from a variable-length integer data format to a fixed-length numeric data format,
In the solution processing, the joint angle θ in the fixed-length numeric data format is obtained by executing a fixed-length numeric calculation including only one division using the calculation result in the fixed-length numeric data format converted by the conversion means. A position control program for causing the computer to further function as error calculation means .

Images