JPH05116081A - 力制御作業機械の重量・重心位置補正装置 - Google Patents

Abstract

(57)【要約】 【目的】 力制御の作業機械を設置する時に従来必要と
された繁雑な調整作業又は測定作業をなくし、更に装置
構成を簡易化し、制御誤差を小さくする。 【構成】 任意の姿勢をとる機構部の一部に力データを
検出する力センサを介し作業ツールを取付け、力制御に
基づいて作業ツールに力作業を行わせる作業機械に適用
され、その制御手段は、姿勢入力手段と、力入力手段
と、重量・重心位置演算手段を含み、機構部に複数の姿
勢をとらせ、各姿勢のデータと力データを取込み、重量
・重心位置演算手段が、姿勢データと力データを用い
て、作業ツールの重量パラメータをベース座標系又は他
の固定座標系で表現したベクトル量として算出すると共
に、作業ツールの重心位置パラメータをセンサ座標系で
表現したベクトル量として算出し、これらのパラメータ
を記憶部に格納する。

Description

【発明の詳細な説明】

【０００１】

【産業上の利用分野】本発明は、力制御作業機械の重量
・重心位置補正装置に関し、特に、力制御によって作動
する作業機械に力センサを介して装着される作業ツール
について、このツールの重量及び重心位置の各パラメー
タを求め、これらのパラメータで作業時に力センサの検
出値が重力補正される重量・重心位置補正装置に関す
る。

【０００２】

【従来の技術】グラインダがけ装置、バリ取り装置、嵌
め合わせ装置等の如く、力センサを介して作業ツールを
装置機構部のアーム先部に取付け、力制御に基づいて機
構部全体の姿勢を変化させて作業ツールに作業を行わせ
るように構成された力制御作業機械では、力センサに固
定した作業ツール及びそのホルダ等（以下ツールと記
す）の重量及び重心位置をパラメータとして設定するこ
とが必要である。その理由は、次の通りである。

【０００３】ツールがワークに対して所定作業を行うと
き、力センサで検出される力データは、力センサの検出
中心位置に加わる力である。これに対して、本来、知り
たい力情報は、ツールとワークが接触する箇所でのワー
クからツールに対し作用する力である。またツールが空
中に存在し、いかなる外力も受けない状態を想定する
と、ツールには、その重心位置に重力が加わり、重量を
有する。従って、ワークに接触して作業を行っているツ
ールには、作業に伴って生じる前記接触箇所での作用力
とツール自身の重量による力が複合された状態で、この
複合力が加わっている。力センサは、かかる複合力を検
出していることになる。そこで、ツールの接触箇所にお
ける作業に関連する力情報のみを検出するためには、ツ
ールの重量とツールの重心位置に関するパラメータを予
め用意し、力センサで検出された力データを補償しなけ
ればならない。そのために、ツールの重量及び重心位置
を重力補正用パラメータとして設定することが必要とな
る。

【０００４】従来、ツールの重量と重心位置のパラメー
タを算出する方法としては、特開昭６２−８４９９１号
公報に開示される方法がある。この先行文献で開示され
る方法では、ツールの重量パラメータをスカラー量とし
て算出し、同ツールの重心位置パラメータをベクトル量
として算出している。

【０００５】

【発明が解決しようとする課題】前述の従来のパラメー
タ演算方法では、重力がスカラー量で与えられるので、
ツールに対し重力が加わる方向を、作業機械の台座部す
なわち設置箇所に設定されるベース座標系の例えばＺ軸
方向と一致させるか、又は重力方向をベース座標系で表
現する式を別途に入力する必要があった。このため、力
制御による作業機械を適当な箇所に設置する時、各作業
機械ごとに、前述した重力方向と軸方向の一致のための
調整を行うか、又は前記の表現式を求めるため各設置箇
所に対応して重力方向と作業機械のベース座標系との関
係を正確に測定しなければならなかった。力制御作業機
械における前記の調整又は測定が不正確に行われると、
作業機械において力制御に基づき所定の作業を実行する
とき、重力補償演算の精度に悪影響を及ぼし、その結
果、最終的な力制御の精度そのものにも悪影響を及ぼ
す。

【０００６】本発明の目的は、力制御の作業機械を設置
する時に従来必要とされた繁雑な調整作業又は測定作業
をなくし、更に装置構成を簡易化し、制御誤差を小さく
した力制御作業機械の重量・重心位置補正装置を提供す
ることにある。

【０００７】

【課題を解決するための手段】本発明に係る力制御作業
機械の重量・重心位置補正装置は、動作する時に任意の
姿勢をとる構造を有する機構部の一部に、任意方向の力
と基準点回りのモーメントを検出する力センサを介し
て、作業ツールを取付け、制御手段で実行される力制御
に基づいて機構部を動作させ、作業ツールに所定の作業
を行わせる作業機械に適用されることを前提とし、更に
この作業機械では、機構部の設置箇所に対応する第１の
座標系（ベース座標系）と、力センサに対応する第２の
座標系（センサ座標系）とが設定され、制御手段は、機
構部の姿勢に関するデータを入力する姿勢入力手段と、
力センサの検出する力データを入力する力入力手段と、
重量・重心位置演算手段を含むように構成され、更に、
制御手段が、機構部を動作させて機構部に複数の姿勢を
とらせるとき、姿勢入力手段は、複数の姿勢のそれぞれ
に対応する姿勢に関するデータを入力し、力入力手段は
複数の姿勢のそれぞれに対応する力データを入力し、重
量・重心位置演算手段は、姿勢入力手段が入力した姿勢
に関するデータと力入力手段が入力した力データを用い
て、作業ツールの重量パラメータを第１の座標系で表現
したベクトル量として算出すると共に、作業ツールの重
心位置パラメータを第２の座標系で表現したベクトル量
として算出し、これらのパラメータをその後の作業動作
で重力補正値として使用するため、記憶部に格納するよ
うに構成される。前記の構成において、好ましくは、複
数の姿勢が、少なくとも３つの姿勢である。前記力制御
作業機械における力制御に基づく作業では、力センサで
検出され且つ出力される重量データ及び重心位置データ
を、前述の如くして得られた重量パラメータと重心位置
パラメータを用いて補償し、力制御において正確な力デ
ータをフィードバックする。

【０００８】前記の構成において、ベース座標系の代わ
りにその他の固定座標系を用いることもできる。

【０００９】

【作用】本発明では、作業時に力センサが作業ツールに
加わる力を検出し、この力データを制御手段にフィード
バックして、作業ツールに目標とする力作業を行わせる
とき、力センサで検出された力データを重力補正する必
要が生じるが、この場合において、重力補正に使用され
る作業ツールの重量パラメータをロボット本体の設置箇
所に対応するベース座標系等で表現したベクトル量とし
て求める。そのため従来の力制御作業機械における重量
・重心位置補正方法で必要とされた、ベース座標系で表
現された重力方向の単位ベクトル（^B 〈ｕ_g 〉）を測定
する必要がなくなり、従って、他の方法で計測する手
間、或いは調整する手間が不要となり、力制御作業機械
の設置時における繁雑さが軽減される。

【００１０】

【実施例】以下に、本発明の実施例を添付図面に基づい
て説明する。図１は、力制御で作動する作業機械と、こ
の作業機械の動作を制御する制御装置と、力センサで得
られる作業ツール（ツールホルダを含む）の重量及び重
心位置を補正するための重力補正値（重量パラメータ，
重心位置パラメータ）を求める補正装置を示し、併せて
本図中に重力補正値を算出するための３つのツール姿勢
を示す。制御装置及び補正装置は、コンピュータによる
演算処理装置として一体的に構成される。図２は制御装
置及び補正装置を構成する演算処理装置の内部構成を示
している。以下では、力制御作業機械をロボット本体と
記す。

【００１１】１は、６自由度を有し且つ垂直多関節型の
構造を有するロボット本体である。ロボット本体１は、
台座部３において、重力方向２に対して角度θ傾斜した
姿勢で設置される。ロボット本体１は、アーム部分の手
先部に力センサ４を有し、この力センサに、更にグライ
ンダ等の作業ツール５が装着される。力センサによっ
て、直交する３つの軸に関し各軸方向の力とモーメン
ト、すなわち、計６軸方向の力を計測できる。ツール５
が、図示しないワークに接触し、ワークに対して所定の
作業を行うとき、力センサ４は、ツール５に加わる力と
ツール５自身の重量が複合された力を検出する。

【００１２】１０はロボット制御装置である。ロボット
制御装置１０は、ロボット本体１の各関節部に取り付け
られたモータ等のアクチュエータの動作を制御する信号
を生成し、この制御信号を各アクチュエータに与えるこ
とにより、ロボット本体１に必要な姿勢をとらせ、作業
に必要な動作を行わせる。ロボット制御装置１０が実行
する制御は、少なくとも力を制御情報として扱ういわゆ
る力制御を含むものである。力制御では、ツール５に加
わる力を、前述の力センサ４で検出し、この検出した力
データをフィードバックしながら、所定の力が発生する
ように制御が行われる。力制御に基づいて動作するロボ
ット本体１の基本的制御、及びこの制御を行うためのロ
ボット制御装置の基本的構成は既に知られている。力制
御では、力センサ４に取り付けられるツール５及びその
ホルダ等の重力値を補償する必要がある。ツール５の重
力値の補償演算を行うには、ツールの重量や重心位置の
パラメータが必要とされる。

【００１３】作業ツールの重量及び重心位置の各パラメ
ータを求めるための装置を構成する機能要素として、力
入力装置１１、姿勢入力装置１２、重量・重心位置演算
装置１３、重量及び重心位置の各パラメータを格納する
記憶装置１４が設けられる。これらの装置の機能の説明
は、力センサ４で検出される作業ツールの重量及び重心
位置の各パラメータを演算する動作説明の時に行われ
る。

【００１４】ロボット制御装置１０、力入力装置１１、
姿勢入力装置１２、重量・重心位置演算装置１３、記憶
装置１４は、具体的に、コンピュータによる構成によっ
て実現される。そのブロック構成は図２に示される。

【００１５】次に、力センサ４によって検出されるツー
ルの重量及び重心位置のデータを補償するための重量及
び重心位置の各パラメータを算出する補正装置の動作に
ついて説明する。力センサ４には、力検出中心（モーメ
ントの基準点）を原点とする力センサ固有の直交座標
系、すなわちセンサ座標系Ｓが設定される。力センサ４
で検出される前述のツール５に加わる力及びツール５自
身の重量は、センサ座標系Ｓを基準にして表現される。
センサ座標系Ｓを用いて表現された力を^s 〈Ｆ_s 〉と表
す。ここで、〈Ｆ_s 〉はＦ_s がベクトル量であることを
意味し、左上の添字ｓが表現座標系を意味する。一方、
ロボット本体１では、その設置箇所に対応してロボット
本体固有の直交座標系であるベース座標系Ｂが定義され
る。

【００１６】センサ座標系Ｓ自体はロボット本体１に固
定された座標系であるので、センサ座標系Ｓの姿勢（傾
き）を、ロボット本体１の各関節部に取り付けられたエ
ンコーダ等の位置変位センサ（図示せず）を用いて各軸
の方向余弦を演算することにより、ベース座標系Ｂで表
現することができる。

【００１７】ここで、ｘ軸の方向余弦を^B 〈ｎ_s 〉、ｙ
軸の方向余弦を^B 〈ｏ_s 〉、ｚ軸の方向余弦を
^B 〈ａ_s 〉と表すと、ベース座標系Ｂで表したセンサ座
標系Ｓの姿勢（^B Ｔ_s ）は、 ^B Ｔ_s ＝〔^B 〈ｎ_s 〉，^B 〈ｏ_s 〉，^B 〈ａ_s 〉〕 …（１） となる。実際に、ツール５の重量及び重心位置の演算に
使用するのは、センサ座標系Ｓで表したベース座標系の
姿勢（^s Ｔ_B ）であるので、これは、 ^s Ｔ_B ＝（^B Ｔ_s ）^-1＝（^B Ｔ_s ）^T …（２） で演算することができる。ここで、（ ）^T は転置行列
を意味する。

【００１８】次に、ロボット制御装置１０による制御の
下で、ロボット本体１に少なくとも３つの姿勢をとらせ
る。図１において、第１から第３の姿勢状態が部分的に
示されている。この３つの姿勢は、力センサ４及び作業
ツールを含む周辺部分の構成に関するものである。これ
らの３つの姿勢に対応して、それぞれ、力センサに基く
力データと位置変位センサに基づく姿勢データが発生す
る。第１の姿勢状態に対応して発生する力データを
^s 〈Ｆ_S1〉、姿勢データを^s 〈Ｔ_B1〉、第２の姿勢状態
に対応する力データを^s 〈Ｆ_S2〉、姿勢データを^s 〈Ｔ
_B2〉、第３の姿勢状態に対応する力データを
^s 〈Ｆ_S3〉、姿勢データを^s 〈Ｔ_B3〉とする。第１から
第３の各姿勢状態に対応する力データ及び姿勢データ
は、それぞれ、姿勢ごとに組として力入力装置１１及び
姿勢入力装置１２から取り込まれ、一時的に記憶され
る。取り込まれた前記の各データを用いて、重量・重心
位置演算装置１３は、下記に説明するように、ツール５
の重量及び重心位置に関しての演算を行い、重力補正値
（パラメータ）としての重量^B 〈Ｗ〉と重心位置
^s 〈ｒ〉を算出する。これらの補正値は、記憶装置１４
に格納される。記憶装置１４に格納された重力補正値と
しての重量^B 〈Ｗ〉と重心位置^s 〈ｒ〉は、その後、ロ
ボット制御装置８による力制御に基づいてロボット本体
１及びツール５に所定の力作業を行わせる時に、記憶装
置１４から取り出され、ロボット制御装置１０内におい
て力センサ４で検出される力データに関して重力補償演
算を行う際に利用される。

【００１９】ツール５の重量^B 〈Ｗ〉及び重心位置
^s 〈ｒ〉の各パラメータの算出方法について詳述する。
まず、量^s 〈Ｆ_0s〉，^s 〈Ｆ_of〉，^s 〈Ｆ_g 〉，^s 〈Ｆ
_s 〉を定義する。^s 〈Ｆ_0s〉：力センサで検出される力^s 〈Ｆ_of〉：力センサのオフセット力補正値（温度ドリ
フト等によるゼロ点ずれ量）^s 〈Ｆ_g 〉：ツールの重力成分^s 〈Ｆ_s 〉：ツールがワークに接触したことにより生じ
る力 上記の量は、それぞれ、軸力ベクトル〈ｆ〉、とモーメ
ントベクトル〈ｍ〉により、次のように表される。^s 〈Ｆ_0s〉＝（^s 〈ｆ_0s〉，^s 〈ｍ_0s〉）^s 〈Ｆ_of〉＝（^s 〈ｆ_of〉，^s 〈ｍ_of〉）^s 〈Ｆ_g 〉＝（^s 〈ｆ_g 〉，^s 〈ｍ_g 〉）^s 〈Ｆ_s 〉＝（^s 〈ｆ_s 〉，^s 〈ｍ_s 〉） 上式において、^s 〈ｆ_g 〉，^s 〈ｍ_g〉は、 ^s 〈ｆ_g 〉＝^s Ｔ_B ・^B 〈Ｗ〉 …（３） ^s 〈ｍ_g 〉＝^s 〈ｒ〉×^s 〈ｆ_g 〉 …（４） また、^s 〈Ｆ_0s〉は、 ^s 〈Ｆ_0s〉＝^s 〈Ｆ_of〉＋^s 〈Ｆ_g 〉＋^s 〈Ｆ_s 〉 …（５） として表現される。ここで^s 〈ｒ〉と^B 〈Ｗ〉に関し
て、意味のない^s 〈ｒ′〉と^B 〈Ｗ′〉が初期値として
記憶されていたとする。このときロボット制御装置１０
内で、力センサのオフセット力補正値を設定するための
オフセット更新ルーチンにより重力補償として、 ^s 〈ｆ_g ′〉＝^s Ｔ_B ・^B 〈Ｗ′〉 …（６） ^s 〈ｍ_g ′〉＝^s 〈ｒ′〉×^s 〈ｆ_g ′〉 …（７） が演算され、オフセット力として、^s 〈Ｆ_of′〉＝^s 〈Ｆ_0s′〉−^s 〈Ｆ_g ′〉 がセットされているものとする。ツールにかかる力
^s 〈Ｆ_s 〉は０であり、^s 〈Ｆ_0s′〉は、この時の力セ
ンサの検出力である。次にツールに、非接触状態で任意
（独立な）の３つの姿勢をとらせる。このとき、重量・
重心位置演算装置１３は、力センサ４から与えられる力
計測データで重量と重心を求める。力計測データの座標
系はセンサ座標系Ｓである。 第１の姿勢： 変換行列 ^s Ｔ_B1 ^s 〈ｆ_s1〉＝^s 〈ｆ_of′〉＋^s 〈ｆ_g1〉^s 〈ｍ_s1〉＝^s 〈ｍ_of′〉＋^s 〈ｍ_g1〉 第２の姿勢： 変換行列 ^s Ｔ_B2 ^s 〈ｆ_s2〉＝^s 〈ｆ_of′〉＋^s 〈ｆ_g2〉^s 〈ｍ_s2〉＝^s 〈ｍ_of′〉＋^s 〈ｍ_g2〉 第３の姿勢： 変換行列 ^s Ｔ_B3 ^s 〈ｆ_s3〉＝^s 〈ｆ_of′〉＋^s 〈ｆ_g3〉^s 〈ｍ_s3〉＝^s 〈ｍ_of′〉＋^s 〈ｍ_g3〉 まず重量を求める。上式より、 ^s 〈ｆ_s1〉−^s 〈ｆ_s2〉＝^s 〈ｆ_g1〉−^s 〈ｆ_g2〉 ＝（^s Ｔ_B1−^s Ｔ_B2）^B 〈Ｗ〉 …（８） ^s 〈ｆ_s1〉−^s 〈ｆ_s3〉＝^s 〈ｆ_g1〉−^s 〈ｆ_g3〉 ＝（^s Ｔ_B1−^s Ｔ_B3）^B 〈Ｗ〉 …（９） ここで、 ^s 〈Δｆ₁ 〉＝^s 〈ｆ_s1〉−^s 〈ｆ_s2〉^s 〈Δｆ₂ 〉＝^s 〈ｆ_s1〉−^s 〈ｆ_s3〉 とおき、更に、^s 〈ΔＦ〉と^s 〈Ａ〉を次のようにおく
と、

【００２０】

【数１】

【００２１】上記の（８）式と（９）式は、 ^s 〈ΔＦ〉＝^s 〈Ａ〉・^B 〈Ｗ〉 …（１０） とまとめることができる。これにより、 ^s 〈Ａ〉^T ・〈ΔＦ〉＝^s 〈Ａ〉^T ・^s 〈Ａ〉・^B 〈Ｗ〉 …（１１） 故に、重量は、 ^B 〈Ｗ〉＝（^s 〈Ａ〉^T ・^s 〈Ａ〉）^-1・^s 〈Ａ〉^T ・〈ΔＦ〉…（１３） として求められる。

【００２２】次に、重心位置を求める。前述の第１から
第３の姿勢に関する式を用いて、 ^s 〈ｍ_s1〉−^s 〈ｍ_s2〉＝^s 〈ｍ_g1〉−^s 〈ｍ_g2〉 ＝^s 〈ｒ〉×^s 〈ｆ_g1〉−^s 〈ｒ〉×^s 〈ｆ_g2〉 ＝^s 〈ｒ〉×（^s 〈ｆ_g1〉−^s 〈ｆ_g2〉） ＝^s 〈ｒ〉×（^s 〈ｆ_s1〉−^s 〈ｆ_s2〉） ＝^s 〈ｒ〉×^s 〈Δｆ₁ 〉 …（１４） 同様に、 ^s 〈ｍ_s1〉−^s 〈ｍ_s3〉＝^s 〈ｒ〉×^s 〈Δｆ₂ 〉 …（１５） ここで、^s 〈Δｍ₁ 〉＝^s 〈ｍ_s1〉−^s 〈ｍ_s2〉^s 〈Δｍ₂ 〉＝^s 〈ｍ_s1〉−^s 〈ｍ_s3〉 とおき、更に、次のようにおく。

【００２３】

【数２】

【００２４】これによって、前記（１４）式及び（１
５）式は、次のように書き換えることができる。

【００２５】

【数３】

【００２６】ここで、

【００２７】

【数４】

【００２８】とおくと、（数３）の式は、^s 〈ΔＭ〉＝^s 〈Ｂ〉・^s 〈ｒ〉 となる。これにより、 ^s 〈Ｂ〉^T ・^s 〈ΔＭ〉＝^s 〈Ｂ〉^T ・^s 〈Ｂ〉・^s 〈ｒ〉 故に、重心位置は、 ^s 〈ｒ〉＝（^s 〈Ｂ〉^T ・^s 〈Ｂ〉）^-1・^s 〈Ｂ〉^T ・〈ΔＭ〉 として求めることができる。

【００２９】以上によって、重量^B 〈Ｗ〉と重心位置^s
〈ｒ〉をパラメータとしてロボット制御装置１０にセッ
トすれば、天吊りなど任意の姿勢にロボット本体１を設
置した場合に、容易に対応することができる。

【００３０】ここで前記の如く、重量をベクトルで求め
たときに、ベース座標系Ｂと重力方向との関係を明らか
にする、すなわち従来の如く測定及び調整を行わなくと
も良い理由を説明する。ロボット本体１では、前述の如
くその台座部にベース座標系Ｂが設定されている。この
ベース座標系は、ツール及び各関節部の位置や姿勢を表
現するための固定座標系として必要である。ロボット本
体１の各部の位置や姿勢はベース座標系で表現され、力
センサ４の位置・姿勢についても同様である。ベース座
標系で表現したセンサ座標系の姿勢は、前記（１）式で
示した通りである。ツール５は力センサ４に固定され、
ツール５の重量は力センサ４で検出されるため、その検
出値^s 〈Ｆ_0s〉は、前記式（５）で示された通り、セン
サ座標系で表現された値となる。力センサ４のオフセッ
ト力はセンサ自身の姿勢によって値の変化はないが、ツ
ール５の重力成分については、ツール５の向く姿勢、す
なわち力センサ４の姿勢によって値が変化する。本発明
よる補正装置は^s 〈Ｆ_0s〉に基づき^s 〈Ｆ_s 〉を求める
のに必要な^s 〈Ｆ_g 〉を求める装置に関するものである
ので、^s 〈Ｆ_g 〉について述べる。^s 〈Ｆ_g 〉＝（^s 〈ｆ_g 〉，^s 〈ｍ_g 〉） として、^s 〈ｆ_g 〉：力センサ４で検出されるツール重力の軸方
向成分^s 〈ｍ_g 〉：力センサ４で検出されるツール重力の回転
モーメント成分 とすると、前記（３）式及び（４）式で明らかなよう
に、^s 〈ｆ_g 〉＝^s Ｔ_B ・Ｗｅ・^B 〈ｕ_g 〉^s 〈ｍ_g 〉＝^s 〈ｒ〉×^s 〈ｆ_g 〉 で表される。ここで、Ｗｅはツール５の重量（スカラー
量）、^B 〈ｕ_g 〉はベース座標系で表現された重力方向
の単位ベクトルである。

【００３１】以上の式で明らかなように、ツール重量を
スカラー量で求めるように構成された重量・重心位置補
正装置では、別途に重力方向の単位ベクトル^B 〈ｕ_g 〉
を求めなければならない。この^B 〈ｕ_g 〉は３次元量で
あるため、測定が難しく、ロボット本体をユーザの使用
箇所に設置した後に、これらの測定を行うことは非常に
煩わしい。また測定において誤差が生じれば、その誤差
は力制御の精度に影響する。また、^B 〈ｕ_g 〉を予め
（０，０，１）のように単純な形で表現しておく方法も
存在するが、この場合には、ロボット本体の設置を、そ
のベース座標系が^B 〈ｕ_g 〉として定めた方向と重力方
向が一致するように、調整を行う必要がある。本発明で
は、前述の実施例で明らかなようにツール５の重量パラ
メータをベース座標系Ｂで表現したベクトル量として算
出する、すなわち、^B 〈Ｗ〉＝Ｗｅ・^B 〈ｕ_g 〉を直接
に求めるので、前述した測定又は調整の繁雑さがなくな
る。

【００３２】次に、図２に従って、ロボット制御装置１
０及び重量・重心位置補正装置からなる演算処理装置
（広い意味で制御手段）の動作を説明する。力センサ４
の出力した検出値は力入力装置１１に入力される。力セ
ンサ４の出力がアナログ信号であるときには、Ａ／Ｄ変
換器で構成される。また前記出力がシリアル通信（ＲＳ
４２２，ＲＳ２３２−Ｃ等）である場合、通信に適した
レシーバで構成される。力入力装置１１に取り込まれた
データは、バス２１を通してＲＡＭ２２に蓄えられる。
他方、姿勢入力装置１２は、各関節部に対応して配置さ
れたエンコーダ２３の発生する各軸の角度データを入力
するカウンタ２４として構成される。各軸の角度データ
が初期的な姿勢データとなる。カウンタ２４を介して取
り込まれた角度データは、バス２１を介してＲＡＭ２２
に蓄えられる。次にＣＰＵ２５で、前記角度データに基
づきキネマティックス演算を実行し、姿勢データ^B Ｔ_s
に変換し、再度ＲＡＭ２２に姿勢データを蓄える。更
に、ＣＰＵ２５は、ＲＯＭ２６に格納された、重量・重
心位置演算装置１３としての動作を実行するプログラム
を呼出し、このプログラムを用いて、且つＲＡＭ２２に
蓄えた前述の力データ及び姿勢データを用いて前述の演
算を実行し、重力と重心位置を算出する。この計算で求
められた重力と重心位置の各値は、ＲＡＭ２２に蓄えら
れる。この場合、ＲＡＭ２２は記憶装置１４として機能
する。その後、力制御による作業をロボット本体１に行
わせる時、ＣＰＵ２５は、力センサ４及び各エンコーダ
２３の出力を取り込み、取り込んだ力データをＲＡＭ２
２に格納した重力と重心位置で補償し、設定された制御
目標値と比較しながら動作指令値を設定する。動作指令
値は、動作指令値出力装置２７から出力され、サーボア
ンプ２８を介して、ロボット本体１の関節部に配置され
た所要のモータ２９に供給される。こうして、ＣＰＵ２
５はロボット制御装置１０として機能する。

【００３３】なお上記実施例では、第１の座標系をロボ
ットのベース座標系として述べたが、これに限らず、任
意の箇所を基準とする固定座標系を用いても、本発明が
成立することは明らかである。

【００３４】

【発明の効果】本発明によれば、力センサを備えた力制
御作業機械において、重力補償用の重量パラメータを、
設置場所に対応するベース座標系等に基づくベクトル表
現で求めるように構成したため、作業機械の設置状態を
作業ツールの重量表現のうちに自動的に含ませることが
でき、従来の測定又は調整ための手段及びプロセスが不
要となった。これによってかかる作業機械の設置作業が
簡便となった。また測定及び調整のための装置構成及び
作業が不要となったため、装置の構成が簡易となり、且
つ測定及び調整に起因する誤差がなくなり、力制御の精
度が常に一定範囲に収まり、全体として向上する。更
に、かかる誤差に起因して発生する作業機械の暴走的動
作を避けることができる。

【図面の簡単な説明】

【図１】本発明に係る力制御作業機械の重量・重心位置
補正装置の一実施例の全体的構成を示す構成図である。

【図２】本発明による装置を実現するコンピュータ構成
を示すブロック図である。

【符号の説明】

１ ロボット本体 ２ 重力方向 ４ 力センサ ５ 作業ツール

Claims (3)

【特許請求の範囲】
1. 【請求項１】 動作する時に任意の姿勢をとる構造を有
   する機構部に、任意方向の力と基準点回りのモーメント
   を検出する力センサを介して、作業ツールを取付け、制
   御手段で実行される力制御に基づいて前記機構部を動作
   させ、前記作業ツールに所定の作業を行わせる作業機械
   であって、更に、前記機構部の設置箇所に対応する第１
   の座標系と、前記力センサに対応する第２の座標系とが
   設定される前記作業機械において、 前記制御手段は、前記機構部の姿勢に関するデータを入
   力する姿勢入力手段と、前記力センサの検出する力デー
   タを入力する力入力手段と、重量・重心位置演算手段を
   含み、 前記制御手段が、前記機構部を動作させて前記機構部に
   複数の姿勢をとらせるとき、前記姿勢入力手段は、前記
   複数の姿勢のそれぞれに対応する姿勢に関するデータを
   入力し、前記力入力手段は前記複数の姿勢のそれぞれに
   対応する力データを入力し、前記重量・重心位置演算手
   段は、前記姿勢入力手段が入力した姿勢に関する前記デ
   ータと前記力入力手段が入力した前記力データを用い
   て、前記作業ツールの重量パラメータをベクトル量とし
   て算出すると共に、前記作業ツールの重心位置パラメー
   タを算出し、これらのパラメータをその後の作業動作で
   重力補正値として使用するため、記憶部に格納すること
   を特徴とする力制御作業機械の重量・重心位置補正装
   置。
2. 【請求項２】 請求項１記載の力制御作業機械の重量・
   重心位置補正装置において、前記複数の姿勢は、少なく
   とも３つの姿勢であることを特徴とする力制御作業機械
   の重量・重心位置補正装置。
3. 【請求項３】 請求項１記載の力制御作業機械の重量・
   重心位置補正装置において、前記重量パラメータを前記
   第１の座標系で表現し、前記重心パラメータを前記第２
   の座標系で表現したことを特徴とする力制御作業機械の
   重量・重心位置補正装置。