JP5306313B2 - ロボット制御装置 - Google Patents

Description

本発明の実施形態は、カメラで撮影した画像情報を用いて、モータなどのアクチュエータを用いてロボットを駆動するロボット制御装置に関する。

ロボットがカメラで撮影した画像情報に基づいて位置姿勢を制御する技術はビジュアルフィードバックと呼ばれている。ロボットのハンドまたはこのハンド付近に搭載したカメラで撮影する場合、俯瞰した位置から撮影した場合に比べて対象物を大きく捉えることが可能となるため、画像の分解能の範囲において正確に位置姿勢を制御することができる。

対象物の画像特徴量の変化をロボットのフィードバックループに直接取り込んだ手法は特徴ベース法と呼ばれており、画像特徴量の変化とロボットの関節角度の変化との割合である画像ヤコビアンを求める必要がある。しかし一般的なカメラのフレームレートは３０ｆｐｓ〜６０ｆｐｓ程度であり、ロボットの制御周期に比べて圧倒的に低い。したがって、例えば画像特徴量に対してロボットの制御周期に合うような時間的な補間を行い、ロボットの動作による画像上の運動も考慮した画像特徴量の推定が必要となる。

またロボットのハードウェアの制限条件により、目標軌道に沿ってロボットが動作できない場合がある。例えばロボットの関節の可動範囲、目標位置姿勢を満たす解が無限に存在する特異点または特異点近傍の目標値が与えられた時の関節角速度の急変などがある。このようにロボットが制御できない場合、ロボットの移動禁止領域として記憶することで解決を図ることが知られている。

特許３９０１００６号公報

しかし、この移動禁止領域を避けて新たな目標値をどのように算出するかについては、現在のところ知られていない。また、特異点近傍の目標値が与えられた場合は目標軌道の速度を十分に抑制すれば通過することが可能となるので、画像上の目標軌道の算出が重要となる。

そこで、本発明が解決しようとする課題は、ロボットのハードウェア制限条件を考慮しながら、安定したビジュアルフィードバックを実現することのできるロボット制御装置を提供することである。

本実施形態によれば、カメラで撮影した画像情報に基づいて多関節ロボットを制御するロボット制御装置であって、前記ロボットの関節軸がアクチュエータによって制御周期毎に駆動され、前記関節軸の関節角度が関節角度検出部によって検出され、前記カメラで撮影した画像が画像取得部によって取得される。前記カメラで撮影された画像から対象物の特徴を表す画像特徴量が画像特徴量算出部によって算出され、前記画像特徴量が時間的に補間されて前記ロボットの制御周期ごとに画像特徴量推定部によって推定され、この推定された画像特徴量の変化と前記関節角の変化の割合である画像ヤコビアンが画像ヤコビアン算出部によって算出される。また、前記対象物の画像上での目標値が目標値入力部によって設定され、この設定された目標値と前記画像特徴量とを用いて前記ロボットの制御周期毎の画像上の目標軌道が目標軌道生成部によって生成され、この生成された目標軌道と算出された画像ヤコビアンとを用いて前記関節角度の差分が関節角差分算出部によって算出される。そして、算出された前記関節角度の差分に基づいて前記関節角度の指令値がトルク指令値算出部において求められるとともに、この関節角度の指令値と、検出された前記関節角度とを用いて前記関節軸のトルク指令値が算出される。このトルク指令値に基づいて前記アクチュエータが駆動部によって駆動される。また、検出された前記関節角度から、前記ロボットのハードウェア制約条件をもとに前記画像特徴量の目標軌道の修正量が位置姿勢評価部によって算出され、この算出された目標軌道の修正量を用いて前記ロボットがフィードバック制御される。

第１実施形態によるロボット制御装置を示すブロック図。 第１実施形態によるロボット制御装置の動作を示すフローチャート。 第２変形例の制御ブロック図。 冗長自由度ロボットの一例を示す図。 第２実施形態によるロボット制御装置を示すブロック図。 第２実施形態によるロボット制御装置の動作を示すフローチャート。

実施形態を以下に図面を参照して説明する。以下の図面の記載において、同一又は類似の部分には同一又は類似の符号を付している。ただし、図面は模式的なものであり、具体的な寸法は以下の説明を参酌して判断すべきものである。また、図面相互間においても互いの寸法の関係や比率が異なる部分が含まれていることは言うまでもない。

（第１実施形態）
第１実施形態によるロボット制御装置は、多関節を有するロボットを制御するものであって、図１に示すように、中央演算処理装置（以下、ＣＰＵともいう）１と、アクチュエータ１００と、駆動部（アンプとも呼ばれる）１０１と、関節角度検出部１０２と、カメラを有する画像取得部１０３と、目標値入力部１０４と、を備えている。

関節角度検出部１０２は、エンコーダ等の位置センサが使用可能であり、所定の周波数成分を除去するフィルタを備えていても良い。関節軸が駆動軸から伝達機構を介して構成されていて、駆動軸に検出部が備えられている場合、関節角度は、駆動軸の位置の変位量（駆動軸角度）から伝達機構のもつ駆動軸と関節軸の比とに応じて算出される。

目標値入力部１０４は、ロボットの最終到達目標となる位置姿勢を設定する。最終到達目標値はデータベースに予め格納されているか、もしくはユーザがＧＵＩ（Graphical User Interface）などで対話的に与えてもよい。

図１に示したＣＰＵ１は、画像特徴量算出部１０５と、画像特徴量推定部１０６と、画像ヤコビアン算出部１０７と、目標軌道生成部１０８と、関節角差分算出部１０９と、トルク指令値算出部１１０と、位置姿勢評価部１１１と、をハードウェア資源であるモジュール（論理回路）として論理的に備えている。

画像特徴量算出部１０５は、画像取得部１０３で撮影された画像から画像処理を行うことにより、対象物の特徴を表す量である画像特徴量を算出する。画像特徴量は対象物の画像上の重心位置、姿勢、色分布、輝度勾配パターン分布など数値ベクトル化したものである。画像処理は一般的に演算処理が重いことから、画像取得部１０３および画像特徴量算出部１０５は、ＣＰＵ１とは異なる他のＣＰＵに設けられ、画像特徴量算出部１０５によって算出された結果（画像特徴量）を、ネットワークなどを介して伝達するように構成してもよい。

画像特徴量推定部１０６は、画像特徴量算出部１０５により算出された画像特徴量を時間的に補間し、次のステップ（次の制御周期後の制御動作）における画像特徴量を推定または予測する。画像取得部のカメラによる画像の取得周期はロボットの制御周期に比べてずっと低い。したがって、滑らかな画像上の目標軌道を生成するためには、過去のステップ（過去の取得周期における取得動作）にて取得した画像特徴量から次のステップ（次の取得周期後における取得動作）で取得されるまでの間の画像特徴量を推定または予測しなければならない。推定または予測する方法は最も単純な線形補間や、カルマンフィルタを用いた推定方法などがあり、これらのうちのいずれかの方法を用いることができる。

画像ヤコビアン算出部１０７は、画像特徴量推定部１０６により算出された画像特徴量から、画像ヤコビアンを算出する。画像ヤコビアンとは、ロボットの各制御周期間における画像特徴量の変化（差分）と、上記制御周期間における制御ロボットの関節角変化との微小変位関係を表現した行列である。画像ヤコビアンをＪ_ｉｍｇとすると、画像特徴量の変化をΔξとすると、ロボットの関節角度の差分Δθは式（１）の関係を満たしている。

Δξ＝Ｊ_ｉｍｇΔθ … （１）

ここで、制御対象となる画像特徴量がｍ個、関節軸がｎ個であるとすると、ΔξおよびΔθはそれぞれｍ次元列ベクトルおよびｎ次元列ベクトルを表している。そして、画像ヤコビアンＪ_ｉｍｇの第ｉ（１≦ｉ≦ｍ）行かつ第ｊ（１≦ｊ≦ｎ）列成分をＪ_ｉｊとし、Δξの第ｉ行成分をΔξ_ｉとし、Δθの第ｊ行成分をΔξ_ｊとすると、Δξ_ｉは以下の式で表される。

カメラやロボットの座標系を規定するパラメータが不明であるかもしくは誤差を含んでいる場合、画像ヤコビアンはオンラインで推定する必要がある。例えば逐次最小二乗法を用いれば、画像特徴量ξと関節角θのデータから推定することができる（例えば、細田耕，浅田稔「構造やパラメータに関する先験的な知識を必要としないフィードフォワード補償器を持つ適応型ビジュアルサーボ系の構成」日本ロボット学会誌 vol.14, No.2, pp.159−165,1996参照）。

目標軌道生成部１０８は、目標値入力部１０４で設定された最終到達目標値と、画像特徴量推定部１０６により推定された画像特徴量とを用いて、ロボットの制御周期毎に画像上における画像特徴量の目標軌道を生成する。

関節角差分算出部１０９は、目標軌道生成部１０８によって生成された目標軌道と推定された画像特徴量との差分Δξと、画像ヤコビアンＪ_ｉｍｇの疑似逆行列Ｊ^＋ _ｉｍｇとを用いて関節角度の差分Δθを、次の式（２）を用いて算出する。

Δθ＝Ｊ^＋ _ｉｍｇΔξ … （２）

なお、行列からその疑似逆行列を算出することは、公知の方法を用いて行う（例えば、ムーア・ペンローズ法）。

トルク指令値算出部１１０は、関節角差分算出部１０９により算出された関節角度の差分を積分することにより関節角の指令値を算出し、この関節角の指令値と、関節角度検出部１０２により検出されたロボットの関節角度とに基づいてトルク指令値（制御目標値）を算出する。

駆動部１０１は、トルク指令値算出部１１０により算出されたトルク指令値にしたがってアクチュエータ１００を制御周期毎に駆動する。

位置姿勢評価部１１１は、関節角度検出部１０２によって検出されたロボットの関節角度から、ロボットのハードウェア制約条件をもとに画像特徴量の目標軌道の修正量を算出する。例えば、ロボットの第ｉ番目の関節軸の関節角度θ_ｉが可動限界最大値θ_{ｉ、ｍａｘ}に近づいている場合、次の式（３）に示すように限界値から遠ざかるような関節角度の補正量Δθ_ｃｏｍｐを、補正量Δθ_ｉを用いて算出する。

Δθ_ｃｏｍｐ＝［０、…、 −Δθ_ｉ、…、０］^Ｔ … （３）

なお、補正量Δθ_ｉは固定値もしくは次の式（４）に示すように可動限界までの距離に応じた関数で与えられる。

Δθ_ｉ＝ｆ（θ_ｉ−θ_{ｉ、ｍａｘ}） … （４）

算出された関節角度の補正量Δθ_ｃｏｍｐから画像特徴量の目標軌道の修正量Δξ_ｃｏｍｐを、画像ヤコビアン算出部１０７で算出した画像ヤコビアンを用いて次の式（５）に示すように算出する。

Δξ_ｃｏｍｐ＝Ｊ_ｉｍｇΔθ_ｃｏｍｐ … （５）

可動限界最小値θ_{ｉ、ｍｉｎ}へ近づいている場合も同様に、関節角度の補正量Δθ_ｃｏｍｐを、式（６）に示すように、補正量Δθ_ｉを用いて算出する。

Δθ_ｃｏｍｐ＝［０、…、 Δθ_ｉ、…、０］^Ｔ … （６）

上記補正量Δθ_ｉは次の式（７）に示すように可動限界までの距離に応じた関数で与えられる。

Δθ_ｉ＝ｆ（θ_ｉ−θ_{ｉ、ｍｉｎ}） … （７）

となる。

（制御装置の動作）
次に、本実施形態によるロボット制御装置の動作を、図２に示すフローチャートを参照して説明する。

（ａ）まず、制御演算を開始し、目標値入力部１０４によって、画像上の目標値が生成される（Ｓ１００、Ｓ１０１）。次に、画像取得部１０３のカメラから画像を取得する（Ｓ１０２）。続いて、画像特徴量が画像特徴量算出部１０５によって算出される（Ｓ１０３）。続いて、画像特徴量算出部１０５により算出された画像特徴量を時間的に補間し（Ｓ１０４、）、次の制御周期における画像特徴量を画像特徴量推定部１０６によって推定または予測する。

次に、目標値入力部１０４から入力された画像上の目標値と、画像特徴量推定部１０６により算出された画像特徴量とから、後述する画像特徴量に関する目標軌道の修正量を考慮した画像特徴量の変化Δξが目標軌道生成部１０８によって生成される（Ｓ１０５）。続いて、目標軌道生成部１０８により生成された画像特徴量の変化Δξに対して、画像ヤコビアン算出部１０７で算出された画像ヤコビアンを用いて式（２）に示すように、逆運動学計算を行って関節角度の差分Δθが算出される（Ｓ１０６）。

その後、関節角差分算出部１０９により算出された関節角度の差分Δθを積分することにより関節角の目標値を算出し、後述する関節角度検出部１０２により検出された関節角度から、トルク指令値がトルク指令値算出部１１０によって算出される（Ｓ１０７）。

次に、トルク指令値算出部１１０により算出されたトルク指令値を制御目標値として駆動部１０１によってアクチュエータ１００が駆動されることにより、ロボットが制御される（Ｓ１０８）。

続いて、制御動作を終了するか否かを確認し（Ｓ１０９）、制御動作を終了する場合はサーボ処理を終了する（Ｓ１１０）。Ｓ１０９において制御動作を終了しない場合はＳ１１２またはＳ１１１に進む。

（ｂ）Ｓ１１２に進んだ場合は、関節角度検出部１０２によって関節角度を検出する。続いて、関節角度検出部１０２により算出された関節角度と、画像特徴量推定部１０６により算出された画像特徴量を用いて、画像ヤコビアン算出部１０７によって画像ヤコビアンが算出される（Ｓ１１３）。次に、関節角度検出部１０２により検出された関節角度と、画像ヤコビアン算出部１０７によって算出された画像ヤコビアンを用いて、位置姿勢評価部１１１によって画像特徴量の目標軌道の修正量が算出される（Ｓ１１４）。続いて、Ｓ１０５に戻り、画像特徴量の目標軌道を算出し、Ｓ１０５〜Ｓ１０９の動作を繰り返す。

（ｃ）Ｓ１１１に進んだ場合は、画像取得のタイミングであるか否かを確認し、画像取得のタイミングである場合はＳ１０２に戻る。画像取得のタイミングでなければＳ１０４に戻り、前回の画像取得のタイミングで取得した画像から推定または予測した画像特徴量から次の制御周期における画像特徴量を時間的に補間するとともに推定を行い、Ｓ１０５において目標軌道の生成に使用する画像特徴量とする。

以上説明したように、本実施形態によれば、ロボットのハードウェア制約条件をもとに画像特徴量の目標軌道の修正量を算出し、この目標軌道の修正量を用いて制御しているので、ロボットを安定に制御することができる。すなわち、ロボットのハードウェア制限条件を考慮しながら、安定したビジュアルフィードバックを実現することができる。

（第１変形例）
本実施形態の第１変形例によるロボットの制御装置を説明する。この第１変形例の制御装置は、第１実施形態とは、位置姿勢評価部１１１における評価方法が異なっている。第１実施形態では、位置姿勢評価部１１１における評価方法は、関節の可動範囲に着目し、関節が可動限界に近づいている場合にその反対方向へと修正するような関節角の補正量から画像特徴量の目標軌道の修正量を算出している。一般に、ロボットのハードウェア制約条件としては、特異点姿勢がある。特異点姿勢とはロボットの目標位置姿勢を実現する関節角度が無数に存在し不定となる状態のことである。特異点姿勢への接近度は可操作度を用いて評価することが知られている。可操作度κ（Ｊ）はヤコビ行列Ｊから次の式（８）を用いて算出される。

κ（Ｊ）＝（ｄｅｔ（ＪＪ^Ｔ））^１／２ …（８）

ここで、Ｊ^Ｔはヤコビ行列Ｊの転置行列を表し、ｄｅｔ（ＪＪ^Ｔ）は行列ＪＪ^Ｔの行列式を表す。

この可操作度を画像ヤコビアンＪ_ｉｍｇにも適用することで、特異点姿勢の評価による目標軌道の修正を行う。特異点近傍では関節角速度が急激に増加するため、目標速度を抑制する必要がある。そこで、次の式（９）に示すように設定した抑制ゲインＫを求める。

κ（Ｊ_ｉｍｇ）＜κ（Ｊ_０）のとき Ｋ＝（κ（Ｊ_ｉｍｇ）／κ（Ｊ_０））^２
κ（Ｊ_ｉｍｇ）≧κ（Ｊ_０）のとき Ｋ＝１ …（９）

すなわち、Ｋは０より大きく１以下の値である。なお、閾値となる画像ヤコビアンＪ_０は予め設定しておく。

その後、求められた抑制ゲインＫを、関節角差分算出部１０９に入力される画像特徴量の差分Δξに対して乗算し、この乗算した値（＝Ｋ・Δξ）を修正された画像特徴量の差分として、式（２）に代入し、以下、第１実施形態で説明したと同様にして、画像特徴量の目標軌道の修正量を求める。

以上説明したように、第１変形例の制御装置は、特異点姿勢の評価による目標軌道を修正し、これにより目標速度を抑制しているので、目標軌道が特異点近傍に与えられた場合でも、ロボットを安定に制御することができる。すなわち、ロボットのハードウェア制限条件を考慮しながら、安定したビジュアルフィードバックを実現することができる。

（第２変形例）
第１実施形態の第２変形例によるロボットの制御装置を、図３を参照して説明する。

上記第１変形例のように、関節が特異点姿勢に接近している場合、速度の抑制だけで充分に関節角の速度を抑制できない場合がある。これは画像ヤコビアンがランク不足になり、疑似逆行列を算出できなくなることに起因する。

そこで、この課題を解決するために、第２変形例の制御装置は、第１実施形態の制御装置において、画像ヤコビアン修正量算出部１１２と、修正量記憶部２００とを新たに設けた構成となっている。なお、この第２変形例における位置姿勢評価部１１１は、第１変形例で説明した画像ヤコビアンの可操作度κを算出することが第１実施形態の位置姿勢評価部１１１とは異なっている。画像ヤコビアン修正量算出部１１２は、ＣＰＵ１内に設けられ、位置姿勢評価部１１１において算出された画像ヤコビアンの可操作度κから、関節が特異点姿勢に接近していると判断すると次の式（１０）示す修正量αを算出する。

κ（Ｊ_ｉｍｇ）＜κ（Ｊ_０）のとき α＝α_０（１−κ（Ｊ_ｉｍｇ）／κ（Ｊ_０））^２
κ（Ｊ_ｉｍｇ）≧κ（Ｊ_０）のとき α＝０ …（１０）

ここで、パラメータα_０はあらかじめ適当に設定しておく。修正量記憶部２００は、画像ヤコビアン修正量算出部１１２が算出した画像ヤコビアンの修正量αを記憶している。

関節角差分算出部１０９は、画像ヤコビアンＪ_ｉｍｇに対して画像ヤコビアンの修正量αを加えた上で第１実施形態と同様にして算出する。具体的には、以下のように画像ヤコビアンＪ_ｉｍｇに対して画像ヤコビアン修正量αを加える。

まず、式（１）に対し両辺に画像ヤコビアンＪ_ｉｍｇの転置行列Ｊ^Ｔ _ｉｍｇを乗算する。

Ｊ^Ｔ _ｉｍｇΔξ＝Ｊ^Ｔ _ｉｍｇＪ_ｉｍｇΔθ … （１１）

ここで、式（１０）を用いて算出した画像ヤコビアン修正量αを式（１１）の右辺の正方行列Ｊ^Ｔ _ｉｍｇＪ_ｉｍｇの対角成分に加えた行列（＝Ｊ^Ｔ _ｉｍｇＪ_ｉｍｇ＋αＩ）を、式（１１）の右辺の正方行列Ｊ^Ｔ _ｉｍｇＪ_ｉｍｇと、置き換える。すなわち、

Ｊ^Ｔ _ｉｍｇΔξ＝（Ｊ^Ｔ _ｉｍｇＪ_ｉｍｇ＋αＩ）Δθ … （１２）

ここで、Ｉは単位行列を表す。この行列（Ｊ^Ｔ _ｉｍｇＪ_ｉｍｇ＋αＩ）の逆行列を用いて関節角度の差分Δθを、次の式（１３）を用いて算出する。

Δθ＝（Ｊ^Ｔ _ｉｍｇＪ_ｉｍｇ＋αＩ）^−１Ｊ^Ｔ _ｉｍｇΔξ … （１３）

したがって、第２変形例においては、行列（Ｊ^Ｔ _ｉｍｇＪ_ｉｍｇ＋αＩ）^−１Ｊ^Ｔ _ｉｍｇが画像ヤコビアンの修正された擬似逆行列となる。

修正量記憶部２００としては、例えば半導体メモリ、磁気ディスク、光ディスク、光磁気ディスクや磁気テープ等が採用可能である。

以上説明したように、この第２変形例においては、修正量αを用いて、正方行列Ｊ^Ｔ _ｉｍｇＪ_ｉｍｇにαＩを加算し、この加算された正方行列（＝Ｊ^Ｔ _ｉｍｇＪ_ｉｍｇ＋αＩ）を用いて、Δθを求めている。そして、加算された正方行列（＝Ｊ^Ｔ _ｉｍｇＪ_ｉｍｇ＋αＩ）は、正規行列となるので、画像ヤコビアンがランク不足になることはない。このため、修正された擬似逆行列は算出することができる。これにより、関節が特異点姿勢に接近していても、充分に関節角の速度を抑制することができる。

すなわち、第２変形例も第１実施形態と同様に、ロボットのハードウェア制限条件を考慮しながら、安定したビジュアルフィードバックを実現することができる。

なお、第１変形例と第２変形例は同時に適用してもよい。すなわち画像特徴量の目標軌道での速度を抑制しながら、画像ヤコビアンも修正することで、より安定にロボットを動作させるように制御してもよい。

（第３変形例）
本実施形態の第３変形例として、冗長自由度のロボットによる制御について説明する。冗長自由度ロボットの一例を図４に模式的に示す。図４に示すロボットはその手先に対して３次元の位置と姿勢を制御できるが、７個の関節２００_１〜２００_７を有している。すなわち、制御対象の６自由度（位置Ｘ、Ｙ、Ｚの３自由度と，それらの各軸周りの回転による姿勢の３自由度）であるよりも多い７自由度を持っており、冗長自由度ロボットと呼ばれる。この冗長自由度ロボットでは、目標位置姿勢を実現することのできる関節角度は無数にある。したがって余剰の自由度を用いれば、関節の可動範囲や特異点近傍を避ける姿勢を与えることができる。これによれば画像上の目標値を修正することなくロボット内部の制御のみで動作を実現することができる。この冗長自由度のロボットによる制御は、第２変形例のロボット制御装置を用いることが好ましい。

（第１実施形態の適用例）
第１実施形態を適用したロボットでは、例えば以下のような作業および動作を実現することができる。先端部に物体把持用のハンドとカメラを搭載した多関節ロボットが、カメラ画像内のマーカーを検出し、指定された位置および姿勢になるように接近する。ロボットは物体に近づくとハンドを開いて物体を把持する。次に把持した物体を別の場所へ運び、はめ込む作業を行う。作業場所に貼られた先のものとは別のマーカーを検出し、指定された位置および姿勢になるように接近し、把持物体を押し込んではめ込み作業を完了する。

（第２実施形態）
次に、第２実施形態によるロボット制御装置について図５を参照して説明する。

第２実施形態では、第１実施形態の適用例で示したような作業において、ロボットに作用する外力を検出して、設定したコンプライアンスモデルに基づいた力制御を用いることで、柔軟な動作を行う。これによりビジュアルフィードバックを用いた作業をより安全に実施することができる。

第２実施形態によるロボット制御装置を図５に示す。この第２実施形態の制御装置は、第１実施形態のロボット制御装置において、力／モーメントデータ記憶部２０１と、コンプライアンスモデル記憶部２０２と、力制御切替部３００と、力指令値生成部３０１と、外力検出部３０２と、ヤコビ行列算出部３０３と、力制御による関節角修正量算出部３０４と、を新たに設けた構成となっている。力指令値生成部３０１と、ヤコビ行列算出部３０３と、力制御による関節角修正量算出部３０４と、は、ＣＰＵ１に設けられる。

力制御切替部３００は、力制御を適用するかどうかをＧＵＩやスクリプトなどでユーザにより指定されると、力／モーメントデータ記憶部２０１に格納されている力またはモーメントの目標値に追従するようトリガーを送る。

力指令値生成部３０１は、力制御切替部３００からトリガーを受けると、力／モーメントデータ記憶部２０１に格納された力またはモーメントの目標値に追従するような力指令値を制御周期毎に生成する。

外力検出部３０２は、先端部付近に力覚センサを備えており、ロボットに作用する外力を検出する。この外力は、センサを使用せずに、トルク指令値と関節角度から動力学モデルを用いて算出した外乱トルクから推定してもよい。

ヤコビ行列算出部３０３は、関節角度検出部１０２により検出された関節角度からヤコビ行列を算出する。

力制御による関節角修正量算出部３０４は、コンプライアンスモデル記憶部２０２からコンプライアンスモデルパラメータを読みだして、力指令値生成部３０１により生成された力指令値と、外力検出部３０２により検出された外力との差である力誤差Δｆに応じた位置の修正量を算出する。もし力制御が有効でない場合は、力指令値は生成されないので力誤差Δｆは外力と同じになる。ここでコンプライアンスモデルとは、例えば式（１４）に示すように接触対象との間において仮想的に慣性、粘性、剛性を想定したものである。

Ｍｄ^２Δｘ／ｄｔ^２＋ＤｄΔｘ／ｄｔ＋ＫΔｘ＝Ｋ_ｆΔｆ （１４）

ここで、Δｘは外力作用関節に設定された作業座標系での位置誤差ベクトル、ｄΔｘ／ｄｔは作業座標系での速度ベクトル、ｄ^２Δｘ／ｄｔ^２は作業座標系での加速度ベクトル、Ｍは慣性行列、Ｄは粘性係数行列、Ｋは剛性係数行列、Ｋ_ｆは力フィードバックゲイン行列である。力と位置の作用させる軸を切り替えるコンプライアンス選択行列は力フィードバックゲイン行列Ｋ_ｆに含まれた形になっている。誤差の速度ベクトルｄΔｘ／ｄｔ及び加速度ベクトルｄ^２Δｘ／ｄｔ^２は、それぞれ位置誤差ベクトルΔｘの時間に関する一回差分、二回差分で近似できることから、外力が作用する関節でのコンプライアンス修正量Δｘ_ｃｏｍｐは、次の式（１５）を用いて算出することができる。

Δｘ_ｃｏｍｐ＝１／Ｋ（Ｋ_ｆΔｆ−Ｍｄ^２Δｘ／ｄｔ^２−ＤｄΔｘ／ｄｔ） （１５）

このコンプライアンス修正量Δｘ_ｃｏｍｐからヤコビ行列算出部３０３により算出されたヤコビ行列Ｊの逆行列を用いて関節角度の修正量Δθ_ｃｏｍｐを、次の式（１６）を用いて算出する。
Δθ_ｃｏｍｐ＝Ｊ^−１Δｘ_ｃｏｍｐ …（１６）

関節角差分算出部１０９は、力制御切替部３００からトリガーを受けると、関節角度の差分の算出処理を実行せず、関節角度の差分として「０」を出力する。

トルク指令値算出部１１０は、関節角差分算出部１０９により算出された関節角度の差分を積分して算出した関節角度の指令値に、力制御による関節角修正量算出部３０４から算出された関節角度の修正量Δθ_ｃｏｍｐを加えたものを新たな関節角度の指令値とし、関節角度検出部１０２により算出された関節角度との差分からトルク指令値（制御目標値）を生成する。すなわち、トルク指令値算出部１１０は、力制御が適用されない場合（トリガーが生成されない場合）には、第１実施形態と同様に、関節角差分算出部１０９により算出された関節角度の差分を積分して算出された関節角度の指令値と、関節角度検出部１０２によって検出された関節角度とに基づいてトルク指令値を算出する。これに対して、力制御が適用される場合（トリガーが生成される場合）には、関節角度の修正量Δθ_ｃｏｍｐを関節角度の指令値とし、この関節角度の指令値と、関節角度検出部１０２によって検出された関節角度とに基づいてトルク指令値を算出する。

これにより、ビジュアルフィードバックを実行しているときにおいても外力を検出すれば、設定したコンプライアンスモデルに基づいて柔軟な制御を行うことができる。また力制御が有効になると、画像特徴量の差分が発生しても、関節角度の差分を算出せず、設定した力またはモーメントの目標値に追従するように外力検出部３０２により検出された外力に基づいて関節角度の修正量が算出される。すなわち接触状態においては、画像よりも力の情報を優先させる。画像データにはカメラパラメータ誤差やキャリブレーション誤差が含まれており、外力を無視して作業を続行すれば対象物の破損を招く恐れがあるからである。

（ロボット制御方法）
次に、第２実施形態によるロボット制御装置の動作を、図６に示すフローチャートを参照して説明する。なお、図２と重複する部分は省略する。
（ａ）まず、制御演算を開始し、外力検出部３０２に外力が算出される。（Ｓ１００、Ｓ２００）。次に、Ｓ２０１において、力制御切替部３００により力制御が有効になっているかどうか確認する。有効である場合、後述するＳ２０２に進む。無効の場合、後述するＳ２０４へ進む。
Ｓ２０２において、力指令値生成部３０１により力指令値が算出される。
（ｂ）次に、Ｓ２０３において、Ｓ１１２で算出された関節角度からヤコビ行列算出部３０３によりヤコビ行列が算出される。
（ｃ）次に、Ｓ２０４において、力制御による関節角修正量算出部３０４により、力指令値生成部３０１で生成された力指令値と、外力検出部３０２で検出された外力と、コンプライアンスモデル記憶部２０２に格納されたコンプライアンスモデルのパラメータと、ヤコビ行列算出部３０３により算出されたヤコビ行列から、式（１４）、（１５）、（１６）を用いて関節角の修正量が算出される。
（ｄ）次にＳ１０７において、関節角差分算出部１０９により算出された関節角差分を積分することにより算出された関節角目標値に、力制御による関節角修正量算出部３０４により算出された関節角修正量を加えて、新たな関節角目標値が算出され、これと関節角度検出部１０２により算出された関節角度とから、トルク指令値がトルク指令値算出部１１０によって算出される。

以上説明したように、第２実施形態においては、力制御が適用されない場合には、第１実施形態と同じ制御が行われるので、第１実施形態と同様に、ロボットのハードウェア制限条件を考慮しながら、安定したビジュアルフィードバックを実現することができる。また、力制御が適用される場合には、ビジュアルフィードバックを実行しているときにおいても外力を検出すれば、設定したコンプライアンスモデルに基づいて柔軟な制御を行うことができる。また、力制御が有効になると、画像特徴量の差分が発生しても、関節角度の差分を算出せず、設定した力またはモーメントの目標値に追従するように外力検出部３０２により検出された外力に基づいて関節角の修正量が算出される。すなわち接触状態においては、画像よりも力の情報を優先させるため、対象物の破損を防止することができる。

（その他の変形例）
第２実施形態は、第１実施形態に対して、力制御を有効にした時のロボットの制御動作を付加したものである。よって、第１実施形態の第１乃至第３変形例と同様な制御を行うことができる。

（その他の実施の形態）
例えば、上記実施形態において、ＣＰＵ１、画像取得部１０３、目標値入力部１０４、修正量記憶部２００、力・モーメントデータ記憶部２０１、コンプライアンスモデル記憶部２０２、等が制御対象であるロボットの内部に埋め込まれて一体化されていても良い。また、ＣＰＵ１、画像取得部１０３、目標値入力部１０４、修正量記憶部２００、力・モーメントデータ記憶部２０１、コンプライアンスモデル記憶部２０２等が制御対象であるロボットの外部にあり、ロボットを有線或いは無線で遠隔制御することも可能である。

本発明のいくつかの実施形態を説明したが、これらの実施形態は、例として提示したものであり、発明の範囲を限定することは意図していない。これらの実施形態は、その他の様々な形態で実施されることが可能であり、発明の要旨を逸脱しない範囲で、種々の省略、置き換え、変更を行うことができる。これらの実施形態やその変形は、発明の範囲や要旨に含まれると同様に、特許請求の範囲に記載された発明とその均等の範囲に含まれるものである。

１ 中央演算処理装置（ＣＰＵ）
１００ アクチュエータ
１０１ 駆動部
１０２ 関節角度検出部
１０３ 画像取得部
１０４ 目標値入力部
１０５ 画像特徴量算出部
１０６ 画像特徴量推定部
１０７ 画像ヤコビアン算出部
１０８ 目標軌道生成部
１０９ 関節角差分算出部
１１０ トルク指令値算出部
１１１ 位置姿勢評価部
１１２ 画像ヤコビアン修正量算出部
２００ 修正量記憶部
２０１ 力／モーメントデータ記憶部
２０２ コンプライアンスモデル記憶部
３００ 力制御切替部
３０１ 力指令値生成部
３０２ 外力検出部
３０３ ヤコビ行列算出部
３０４ 力制御による関節角修正量算出部

Claims (5)

1. カメラで撮影した画像情報に基づいて多関節ロボットを制御するロボット制御装置であって、
   前記ロボットの関節軸を制御周期毎に駆動するアクチュエータと、
   前記関節軸の関節角度を検出する関節角度検出部と、
   前記カメラで撮影した画像を取得する画像取得部と、
   前記カメラで撮影された画像から対象物の特徴を表す画像特徴量を算出する画像特徴量算出部と、
   前記画像特徴量を時間的に補間して前記ロボットの制御周期ごとに推定する画像特徴量推定部と、
   前記推定された画像特徴量の変化と前記関節角の変化の割合である画像ヤコビアンを算出する画像ヤコビアン算出部と、
   前記対象物の画像上での目標値を設定する目標値入力部と、
   設定された前記目標値と前記画像特徴量とを用いて前記ロボットの制御周期毎の画像上の目標軌道を生成する目標軌道生成部と、
   生成された前記目標軌道と算出された画像ヤコビアンとを用いて前記関節角度の差分を算出する関節角差分算出部と、
   算出された前記関節角度の差分に基づいて前記関節角度の指令値を求め、この関節角度の指令値と、検出された前記関節角度とを用いて前記関節軸のトルク指令値を算出するトルク指令値算出部と、
   前記トルク指令値に基づいて前記アクチュエータを駆動する駆動部と、
   検出された前記関節角度から、前記ロボットのハードウェア制約条件をもとに前記画像特徴量の目標軌道の修正量を算出する位置姿勢評価部と、
   を備え、
   この算出された目標軌道の修正量を用いて前記ロボットをフィードバック制御することを特徴とするロボット制御装置。
2. 前記位置姿勢評価部は、前記ロボットの可動限界および特異点姿勢への接近度示す画像ヤコビアンの可操作度を算出し、前記可動限界および前記可操作度を用いて画像上の目標軌道の修正量を算出することを特徴とする請求項１記載のロボット制御装置。
3. 前記位置姿勢評価部は、前記ロボットの可動限界および特異点姿勢への接近度示す画像ヤコビアンの可操作度を算出し、
   前記関節角差分算出部は、
   算出された前記可操作度を用いて前記画像ヤコビアンの修正量を算出する画像ヤコビアン修正量算出部と、
   算出された前記画像ヤコビアンの修正量を保持する修正量記憶部と、
   を更に備え、
   前記関節角差分算出部は、修正された前記画像ヤコビアンを用いて関節角度の差分を算出することを特徴とする請求項１記載のロボット制御装置。
4. 前記ロボットは、冗長自由度ロボットであることを特徴とする請求項３記載のロボット制御装置。
5. 力制御の有無を切り替えるトリガー信号を発生する力制御切替部と、
   前記ロボットが対象物との接触時において発生させるべき力またはモーメントの目標値を記憶する力／モーメントデータ記憶部と、
   前記トリガー信号を受けて、前記力／モーメントデータ記憶部に格納された前記力またはモーメントの目標値に追従する力指令値を算出する力指令値生成部と、
   前記ロボットに作用する外力を検出する外力検出部と、
   検出された前記関節角度からヤコビ行列を算出するヤコビ行列算出部と、
   前記ロボットと対象物との接触状態を記述したコンプライアンスモデルのパラメータを記憶するコンプライアンスモデル記憶部と、
   前記力指令値と、前記外力と、前記コンプライアンスモデルのパラメータと、前記ヤコビ行列に基づいて、関節角度の修正量を算出する関節角修正量算出部と、
   を備え、
   前記トルク指令値算出部は、
   前記トリガー信号が生成されない場合には、前記関節角度の指令値と、検出された前記関節角度とに基づいてトルク指令値を算出し、
   前記トリガー信号が生成される場合には、前記関節角修正量算出部によって算出された前記関節角度の修正量を前記関節角度の指令値とし、この関節角度の指令値と、検出された前記関節角度とに基づいてトルク指令値を算出することを特徴とする請求項１乃至４のいずれかに記載のロボット制御装置。

Images