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

Abstract

【課題】ユーザがロボットアーム型治療台を直感的に安定に操作することのできるロボット制御装置を提供する。
【解決手段】本実施形態のロボット制御装置は、ロボットの動作方向が制限されているときに、関節角差分が閾値を超えない速度関節角速度指令値が監視部から出力され、関節角差分が閾値により制限された場合に、その過大割合が速度抑制部によって算出され、この過大割合を考慮して目標値が目標値整形部によって整形されるとともに、ユーザに対して制限された動作方向を速度制限提示部によって表示する。
【選択図】図２

Description

本発明の実施形態は、ロボット制御装置に関する。

重粒子線などの放射線治療においては、患者に対して様々な方向から放射線を照射して治療を実施している。放射線の照射方向が固定されている場合、患者を搭載している天板を照射部位に合わせて正確に位置決めする必要がある。このため、放射線治療においては、多リンクで構成されたロボットアームの先端に天板を搭載した治療台は効果的である。技師や医師などの操作者（ユーザ）が照射位置を決定するために、患者が搭載された状態で天板の目標位置を入力する必要がある。従来は産業用ロボットにおいて広く使用されているティーチングペンダントにより、直交座標系の各軸ずつに目標値を与えていた。

しかし、操作性の面からより直感的に入力できることが望ましい。ジョイスティックのようなポインティングデバイスを用いると、天板の動作を直感的に入力することが可能となるだけでなく、その操作量に従って動作量を連続的に変化させることが可能となるため、ユーザの意図を反映させやすい。しかし、ロボットがユーザの意図した通りに動作できない場合がある。例えば、ロボットの関節の可動範囲および目標位置姿勢を満たす解が無限に存在する特異点、または特異点近傍の目標値が与えられた時の関節角速度の急変などが挙げられる。ロボットの関節の動きと天板の動きは直接リンクしていないため、ロボットの動作に詳しくないユーザにとっては操作が困難となる。

またリンク構成が目標位置のもつ自由度よりも多くなる、冗長自由度アームの場合は、関節目標値を算出する逆運動学の解は無数に存在する。しかし、上記の可動限界や特異点などの機構上の制約条件を充たした解が存在する。さらにユーザの作業スペースを確保するための関節の可動域に制限を設けるなどの要望も満たすことが望ましい。

こうした制約条件を充たすために、関節の可動域に制限を設け、可動限界から遠ざかるような目標値を与えることで解決を図ることが知られている。しかし、冗長性を十分に生かすならば、あえて可動限界に近づく方向に動作させた方が、速度の急変を防げる場合がある。また、速度過大の場合にユーザに分かりやすく提示できなければ、ユーザは異常に気付かずに誤った入力をし続ける可能性がある。

また、力覚センサを用いて、ユーザの操作力を連続的な動作量に変換し利用する例も提案されている。しかし、現状では力覚センサは高価で衝撃に弱く故障しやすいため、産業用ロボットで広く使用されているティーチングペンダントのように、直交座標系の各軸ずつに割り当てられた動作方向をＯＮとＯＦＦのみで離散的に指示するボタンが具備されているものが多い。

力覚センサを用いる例では、力制御を用いた動作プログラムをユーザの誘導で修正している。この例では、ティーチングペンダントに力制御を設定する手段を備えており、教示データを再生中において、ユーザが位置または力指令を与えると、動作プログラムの動作に指令を重畳して動作を修正している。これにより、ユーザの誘導により教示データが修正されることから、ユーザの意図した動作を実現することができる、としている。

しかし、ユーザが与える位置または力指令の修正量は、ティーチングペンダントに付属の「高」や「低」などのボタンを用いて一括で変更している。このため、ユーザの操作性は画一的に設定されることになる。例えば、ある軸の修正量だけ増やしたい時などは、軸ごとに修正量調整のボタンが必要になるだけでなく、状況に応じてユーザは事前にすべての軸の修正量を設定する必要がある。このため、かえってユーザの操作性を悪化させていることになる。

特開２００５−１９９４１２号公報 特開２０１１−２２４６９６号公報

本実施形態は、ユーザがロボットアーム型治療台を直感的に安定に操作することのできるロボット制御装置を提供する。

本実施形態のロボット制御装置は、アーム型治療台を備えたロボットの関節軸を制御周期毎に駆動するアクチュエータと、前記関節軸の角度を検出する関節角度検出部と、前記関節角度とリンクパラメータとに基づいて前記ロボットの制御点の位置座標を算出する制御点位置算出部と、検出された前記関節角度および前記リンクパラメータに基づいて前記各関節における関節座標系と作業座標系との間のヤコビ行列を算出するヤコビ行列算出部と、前記制御点の目標位置および制御点の目標値を入力する目標値入力部と、前記制御点の目標値を整形する目標値整形部と、前記目標値整形部により整形された制御点の目標値と前記制御点位置算出部により算出された制御点の現在値との差分を算出し、前記ヤコビ行列を用いて逆運動学計算を行い、前記制御点における目標速度を算出する目標速度算出部と、前記目標速度を監視し、算出された前記目標速度が閾値を超えている場合に前記目標速度が前記閾値以下となるように制限する速度監視部と、前記算出された目標速度が前記閾値を超えている場合に前記制御点の目標値を修正する速度抑制部と、前記速度抑制部によって前記ロボットの動作方向が制限されているときに、ユーザに対して制限された動作方向を提示する速度制限提示部と、前記速度監視部から出力された目標速度と前記関節角度とに基づいて指令値を算出し、指令値に基づいて前記アクチュエータを駆動する第１駆動部と、を備え、前記目標値整形部は、前記速度抑制部によって前記制御点の目標値が修正された場合に、この修正された目標値を整形することを特徴とする。

制御対象であるロボットの一例を示す外観図。 第１実施形態によるロボット制御装置を示すブロック図。 速度制限提示部の一例を示す図。 第２実施形態によるロボット制御装置を示すブロック図。 第２実施形態に係る入力信号評価部の動作を示すフローチャート。 第２実施形態に係る入力信号評価部における信号の一例を示す図。 第２実施形態の第１変形例によるロボット制御装置を示すブロック図。

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

（第１実施形態）
第１実施形態によるロボット制御装置について図１および図２を参照して説明する。図１は、本実施形態のロボット制御装置によって制御されるロボットの外観図であり、図２は、本実施形態のロボット制御装置を示すブロック図である。

本実施形態のロボット制御装置は、例えば図１に示す、シリアルにリンク（関節軸）が連結されているアームの先端に患者を搭載するロボット１０を制御対象としている。このロボット１０は、第１乃至第７関節軸１１、１２、１３、１４、１５、１６、１７と、患者を搭載する天板２０と、を備えている。

第１関節軸１１は、ロボット１０が設けられている室に対してターンテーブルのように水平方向に回転動作する。第２関節軸１２は、第１関節軸に接続され、第１関節軸１１に対して水平方向に回転動作する。なお、第１および第２関節軸１１、１２の回転動作は、上から見たときに時計回りの回転および反時計回りの回転が可能な構成となっている。第３関節軸１３は、第２関節軸１２に接続され、垂直方向に動作、すなわち上下方向に動作する。第４関節軸１４は、第３関節軸１３に接続され水平方向に回転動作する。この回転動作は、上から見たときに時計回りの回転および反時計回りの回転が可能な構成となっている。

第５関節軸１５は、第４関節軸１４に接続されて軸が水平方向に延在し、この延在する軸の周りに回転動作する。この回転動作は、上記延在する軸から見たときに時計回りの回転および反時計回りの回転が可能な構成となっている。第６関節軸１６は、第５関節軸１５に接続され、垂直方向に動作、すなわち上下方向に動作する。第７関節軸１７は、第６関節軸１６に接続され、水平方向に回転動作する。この回転動作は、上から見たときに時計回りの回転および反時計回りの回転が可能な構成となっている。天板２０は第７関節軸１７に接続される。このように構成されたロボット１０においては、第５関節軸１５が回転動作することにより、天板２０を水平方向に対して傾いた状態にすることが可能となる。

本実施形態のロボット制御装置は、図２に示すように、中央演算処理装置（ＣＰＵ）１と、アクチュエータ１００と、駆動部（アンプ）１０１と、関節角度検出部１０２と、目標値入力部１０３と、速度制限提示部１０４と、リンクパラメータ記憶部２０１と、最大速度記憶部２０２と、を備えている。

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

目標値入力部１０３は、ロボット１０の最終到達目標位置姿勢（目標値）を設定する。目標値はデータベースに予め格納されるか、もしくはユーザがジョイスティックなどのポインティングデバイスで与えてもよい。

速度制限提示部１０４は、後述する速度抑制部１１０によってロボット１０の動作方向が制限されているときに、ユーザに対して制限された動作方向を分かりやすく提示する。例えば、目標値入力部１０３がポインティングデバイス２００である場合に、図３に示す領域２０２が、ロボット１０の動作方向が制限されている方向となる。

図２に示したＣＰＵ１は、制御点位置算出部１０５と、ヤコビ行列算出部１０６と、目標値整形部１０７と、目標速度算出部１０８と、速度監視部１０９と、速度抑制部１１０と、冗長性評価部１１１と、をそれぞれモジュールとして備えている。すなわち、ＣＰＵ１を構成する各要素は、ハードウェアで実現してもよいし、ソフトウェアで実現してもよい。

制御点位置算出部１０５は、リンクパラメータ記憶部２０１からロボット１０のリンクパラメータを読み出し、関節角度検出部１０２により検出された関節角度から、リンクパラメータを用いて順運動学計算によりロボット１０の作業座標系における制御点の位置姿勢を算出する。

ヤコビ行列算出部１０６は、関節角度検出部１０２により算出された関節角度と、リンクパラメータ記憶部２０１に記憶されたリンクパラメータとに基づいて、ヤコビ行列を算出する。ヤコビ行列とは、ロボット１０の作業座標系と関節座標系との間の微小変位関係を表現した行列である。ヤコビ行列をＪとすると、位置の変化Δｘと関節角差分Δθは、下記の式（１）の関係を満たしている。

Δｘ＝ＪΔθ … （１）

目標値整形部１０７は、目標値入力部１０３により入力されたロボット１０の目標値を、フィルタなどを用いてより滑らかに整形する。また目標値整形部１０７は、後述する速度抑制部１１０によってロボット１０の関節角差分Δθが制限された場合に、その過大割合を考慮して目標値を整形する。

目標速度算出部１０８は、目標値整形部１０７で整形された目標値と、制御点位置算出部１０５により算出された制御点位置の現在値とを用いて、位置差分Δｘを算出し、ヤコビ行列Ｊの逆行列Ｊ^-1を用いて関節角差分Δθを、次の式（２）を用いて算出する。

Δθ＝Ｊ^-1Δｘ … （２）

関節角差分Δθの算出は、所定の周期毎、例えば後述するアクチュエータ１００の制御周期毎に行われる。このため、関節角差分Δθは上記周期の間における目標角速度を表している。

速度監視部１０９は、最大速度記憶部２０２から関節の角速度の最大値を読み出し、目標速度算出部１０８において算出された関節角差分Δθが、設定された最大角速度（上記周期における最大角度）θ_maxを超えないようにリミットを設け、関節角速度指令値とする。

速度抑制部１１０は、速度監視部１０９において関節角差分Δθが最大角速度θ_maxにより制限された場合に、算出された関節角差分Δθの過大割合αを算出し、目標値整形部１０７へ伝達する。過大割合αは例えば以下のように算出する。

α＝|Δθ/θ_max| … （３）

駆動部１０１は、速度監視部１０９により算出された関節角速度指令値にしたがってアクチュエータ１００を制御周期毎に駆動する。

本実施形態のロボット制御装置においては、制御対象となるロボット１０は図１に示すように冗長自由度を有することが考えられる。ここで、冗長自由度を有するとは、入力された目標値の次元よりも、ロボット１０のアクチュエータの方が多いことを意味する。この場合、ヤコビ行列Ｊに逆行列は存在しない。このため、式（２）を用いて関節角差分Δθを算出できない。そこで、ヤコビ行列Ｊの擬似逆行列Ｊ⁺を用いて以下（４）式に示すように関節角差分Δθを目標速度算出部１０８が算出する。

Δθ＝Ｊ⁺Δx + (I−ＪＪ⁺)ｋ … （４）

ここで、Iは単位行列であり、ｋは冗長性パラメータである。この冗長性パラメータｋは冗長性評価部１１１により算出される。冗長性評価部１１１は、ロボット１０が可動限界や特異点に近づいた場合に目標速度が算出されるように、関節の可動範囲およびヤコビ行列の特異点近傍度を評価関数として算出される冗長性パラメータを算出する。特異点近傍度κとして以下に示す可操作度や条件数など、さまざまな指標が提案されている。可操作度は以下の式に示すようにＪとその転置行列との積の行列式に対し平方根を取ったものである。

κ＝√（ｄｅｔ（ＪＪ^Ｔ））

ここで、ｄｅｔ（Ｋ）は行列Ｋの行列式である。特異点に近づくと急激に減少し、特異点では０となる。

また条件数は以下の式に示すように、Ｊの固有値の比で定義される。

κ＝σ_１／σ_ｍ

ここで、σ_１はＪの固有値の中で最小のもの、σ_ｍはＪの固有値の中で最大のものである。条件数が１に近いほど特異値からは離れていると言える。

以上のような特異値近傍度κを用いて、冗長性パラメータｋを設定する。

ｋ ＝ ｋ_０ ＊ ｆ（κ）

ここで、ｋ_０は冗長性パラメータの基準値であり、動作によって選択してもよい。例えばユーザによる作業エリアを確保するために、第１軸を大きく動かすことでアームを回り込ませたいときは、以下のように設定するとよい。

ｋ_０＝［ｋ_１， ０…， ０］

また、ｆは特異点近傍度に基づく評価関数である。特異点から遠ければ１でよく，特異点に近づいたときは関節速度を抑制する値を出力することで関節角速度が急激に大きくなるのを防ぐ。

また動作開始時や停止時においてΔｘ＝０であり、Δθ＝０とならなければならない。このとき、（Ｉ−ＪＪ^＋）≠０であればｋ＝０としておかなければΔθ≠０となって、急激な加速度が発生してしまい、危険である。よって（Ｉ−ＪＪ^＋）の項にかかわらず、以下の式で定義するような時間に依存する加減速係数βをｋにかけることで、滑らかな目標軌道を生成するようにする。

β＝ｔ／Ｔ_０ （０＜ｔ＜Ｔ_０）
β＝１ （Ｔ_０＜ｔ＜Ｔ−Ｔ_０）
β＝（Ｔ−ｔ）／Ｔ_０ （Ｔ−Ｔ_０＜ｔ＜Ｔ）

ここで、Ｔ_０は加減速時間、Ｔは動作完了に必要な時間である。

以上説明したように、本実施形態においては、ロボット１０の動作方向が制限されているときに、関節角差分Δθが最大角速度（上記周期における最大角度）θ_ｍａｘを超えない速度関節角速度指令値が監視部１０９から出力され、関節角差分Δθが最大角速度θ_ｍａｘにより制限された場合に、その過大割合が速度抑制部１１０によって算出され、この過大割合を考慮して目標値が目標値整形部１０７によって整形されるとともに、ユーザに対して制限された動作方向を速度制限提示部１０４によって分かりやすく表示するので、ユーザがロボットアーム型治療台を直感的に安定に操作することができる。

なお、上記実施形態において、ＣＰＵ１、リンクパラメータ記憶部２０１、最大速度記憶部２０２等が、制御対象であるロボットの内部に埋め込まれて一体化されていても良い。また、ＣＰＵ１、リンクパラメータ記憶部２０１、最大速度記憶部２０２等が、制御対象であるロボットの外部にあり、ロボットを有線或いは無線で遠隔制御することも可能である。

（第２実施形態）
第２実施形態によるロボット制御装置を図４に示す。この第２実施形態のロボット制御装置は、第１実施形態と同様に、例えば図１に示すロボット１０を制御対象としている。この実施形態のロボット制御装置は、第１実施形態の制御装置において、駆動部１０１を駆動部１０１Ａに置き換えるとともにＣＰＵ１をＣＰＵ１Ａに置き換え、動作方向指定部３０２を新たに設けた構成となっている。この第２実施形態のＣＰＵ１Ａは、図２に示すＣＰＵ１において、先端速度目標値算出部３０３と、関節速度目標値算出部３０５と、入力信号評価部３０７と、慣性モーメント算出部３０８と、フィードフォワード指令値算出部３０９と、を新たに設けた構成となっている。なお、このＣＰＵ１Ａは、図２に示すヤコビ行列算出部１０６以外のＣＰＵ１の各構成要素を図示していない。なお、ＣＰＵ１Ａを構成する各構成要素はモジュールであって、ハードウェアで実現してもよいし、ソフトウェアで実現してもよい。

動作方向指定部３０２は、ユーザが入力する制御点の動作方向を指定するものであり、正負の方向を示すモメンタリ動作のボタン、または正方向、停止、負方向を示すプッシュ・プル動作のボタンなどの離散的な入力を与えるものであってもよい。また、ジョイスティックや力覚センサなどのポインティングデバイスで連続的な入力を与えてもよい。

先端速度目標値算出部３０３は、動作方向指定部３０２で得られた制御点における動作方向から、所定の最大加速度を超えない範囲で制御点における制御周期毎の先端速度目標値（制御点における速度目標値）を算出する。前回の先端速度目標値をｖ_Ｒｉ−１、今回の先端速度目標値をｖ_Ｒｉ、最大加速度をａ、制御周期をΔＴとすると、例えば式（５）に示すように算出される。

ｖ_Ｒｉ＝ｖ_Ｒｉ−１ ＋ ａΔＴ … （５）

実際には、前回の先端速度目標値によって到達すべき目標位置に到達していない場合がある。そこで想定していた目標位置と現在位置との誤差Δｘを算出しておき、式（６）に示すように補正しておくと良い。

ｖ_Ｒｉ＝ｖ_Ｒｉ−１ ＋ ａΔＴ ＋ Δｘ … （６）

得られた先端速度目標値に対してフィルタ処理を施すことで、より滑らかな動作となるようにしてもよい。

ヤコビ行列算出部１０６によって算出されたヤコビ行列をＪとすると、制御点の速度ｖと関節速度ωは式（７）に示す関係を満たしている。

ｖ＝Ｊω … （７）

関節速度目標値算出部３０５は、先端速度目標値算出部３０３で算出された先端速度目標値ｖ_Ｒｉと、ヤコビ行列Ｊの逆行列Ｊ^−１を用いて関節速度目標値ω_Ｒｉを、次の式（８）を用いて算出する。

ω_Ｒｉ＝Ｊ^−１ｖ_Ｒｉ … （８）

なお、ロボット１０が図１に示すように冗長自由度を有する場合には、第１実施形態で説明したと同様に、ヤコビ行列Ｊの擬似逆行列Ｊ⁺を用いて関節速度目標値角差分ω_Ｒｉを関節速度目標値算出部３０５が算出する。

駆動部１０１は、関節速度目標値算出部３０５により算出された関節速度目標値ω_Ｒｉにしたがってアクチュエータ１００を制御周期毎に駆動する。駆動部１０１は、速度指令型のドライバであることが多い。例えば、２自由度制御系である速度ＦＦ−Ｉ−Ｐ制御系で構成されると、式（９）に示すような駆動トルク指令値τ_Ｒｉを算出する。

τ_Ｒｉ＝Ｋ_Ｐ（Ｋ_Ｉ∫（ω_Ｒｉ−ω）ｄｔ−ω＋Ｋ_ＦＦω_Ｒｉ） … （９）

ここで、Ｋ_Ｉ、Ｋ_Ｐ、Ｋ_ＦＦはそれぞれ速度偏差積分フィードバック制御ゲイン、速度比例フィードバック制御ゲイン、速度目標値フィードフォワード制御ゲインを表す。

入力信号評価部３０７は、動作方向指定部３０２で入力された動作方向とその変化した時間を記録し、離散的な入力信号を連続的な関数で近似したものとして評価する。その上で、制御点の最大速度や最大加速度などの軌道パラメータを変更して、この変更された軌道パラメータを先端速度目標値算出部３０３に与えるとともに、ゲインを変更して変更されたゲインをフィードフォワード指令値算出部３０９へ与える。すなわち、入力信号評価部３０７は、動作方向の履歴や操作時間に基づいて、制御点の最大速度や最大加速度などの軌道パラメータを変更する。

次に、第２実施形態に係る入力信号評価部３０７の動作を図５および図６を参照して説明する。図５は、入力信号評価部３０７の動作を示すフローチャート、図６は、入力信号評価部３０７における入力信号の波形を示す図である。

（ａ）入力信号が入力信号評価部３０７に入力されると、処理が開始される（図５のＳ３００）。

（ｂ）次に入力信号を符号化する（図５のＳ３０１）。例えば上下方向の動作方向について、例えば、動作方向指定部３０２において、上昇（ＵＰ）を「１」、下降（ＤＯＷＮ）を「―１」、停止（ＳＴＯＰ）を「０」のように符号を割り当てたとする。スイッチが押されている間は同じ数値が入力され続ける（モメンタリ動作）。入力された信号の一例を図６の波形４０１に示す。この波形４０１においては、最初に、動作ＵＰと動作ＳＴＯＰとが交互に入力され、３秒後に動作ＤＯＷＮが入力されたのち、動作ＵＰと動作ＳＴＯＰとが素早く切り替わっている。

（ｃ）入力信号が変化していなければ（図５のＳ３０２）、入力信号評価部３０７において、ある連続的な関数でこの入力履歴を近似する。近似式として例えば式（１０）に示す関数を用意する（図５のＳ３０３）．

ｙ_ｉ＝Ｋ（ｕ_ｉ−ｙ_ｉ−１）＋ｙ_ｉ−１ … （１０）

ここでｕ_ｉは入力、ｙ_ｉは出力、ｙ_ｉ−１は前回の出力、Ｋは係数である。この式は、一次遅れ要素（ゲイン１）の式を差分形式で表現したものである。この関数は入力が変化しなければ、時間の経過とともに出力は入力に収束する。

（ｄ）ここで、入力と出力の差を誤差とし、所定の範囲になっていれば（図５のＳ３０４）、収束時間ｄＴを算出する（図５のＳ３０６）。ここで収束時間とは、入力信号が変化した時刻から誤差が所定の範囲内に収まった時刻までに経過した時間とする。誤差が大きければ処理を終了する（図５のＳ３０５）。

（ｅ）もしステップＳ３０２で入力信号が変化すると、入力が変化するまでに経過した時間ＴとステップＳ３０６で算出した収束時間ｄＴを比較し、入力信号が変化するごとに係数Ｋを変化させる。例えば式（１１）に示すルールが考えられる。

Ｋ_ｊ＝Ｋ_ｊ−１／α （ｄＴ＜βＴ の場合）
Ｋ_ｊ＝αＫ_ｊ−１ （ｄＴ＞Ｔ の場合） … （１１）
Ｋ_ｊ＝Ｋ_ｊ−１ （その他の場合）

ここでα＞１、０＜β＜１である。

出力の一例を図６の波形４０２に示す。初期値では収束時間が短かったので、係数が小さくなり徐々に収束時間が長くなっている。しかし、入力が素早く変化するときには係数が大きくなって収束時間が短くなっているのが分かる。なお、近似式は他にもＡＲＸ（Auto Regressive with eXogenous inputs またはAuto Regressive with eXternal inputs）モデルなどが考えられる。

慣性モーメント算出部３０８は、各リンクの質量、重心位置、重心周りの慣性モーメントなどのリンクパラメータから各関節軸周りの慣性モーメントを算出する。無負荷の場合は初期値として設計データから予め算出したものを記憶部に記録しておけばよい。しかし、治療台アームの天板２０に患者が移乗されるケースのように付加荷重が動的に変化すると、慣性モーメントも変化してしまう。また、重心位置は各ケースで異なる可能性がある。外力Ｆと各関節に作用するトルクτ_ｄには仮想仕事の原理から式（１２）に示す関係が成り立つ。

Ｆ＝（Ｊ^Ｔ）^−１τ_ｄ … （１２）

ここでＪ^Ｔはヤコビ行列Ｊの転置行列である。ロボット１０が図１に示すように冗長自由度を有する場合には、第１実施形態で説明したと同様に、ヤコビ行列Ｊの転置行列Ｊ^Ｔの擬似逆行列Ｊ^Ｔを用いて外力Ｆを算出する。τ_ｄは、関節駆動トルクから、無負荷でかつ静止時における関節駆動トルク（≒重力トルク）を差し引けば算出される。このため、式（１２）で外力が算出される。さらにアームの姿勢を非干渉化すれば計算量を削減され、荷重および重心位置が算出される。あるいはアームの姿勢を変えながら、逐次的にパラメータを推定し続ける、という方法もある。

フィードフォワード指令値評価部３０９は、慣性モーメント算出部３０８で算出された慣性モーメントから、駆動部１０１Ａで使用するフィードフォワード制御ゲインＫ_ＦＦを変更する。式（９）に示す制御系の場合、ゲインは０〜１の数値を取りうる。

以上説明したように、この第２実施形態は、第１実施形態と同様のロボット制御装置を構成する各要素を備えているので、第１実施形態と同様に、ユーザがロボットアーム型治療台を直感的に安定に操作することができる。

さらに、第２実施形態においては、動作方向の履歴や操作時間に基づいて、制御点の最大速度や最大加速度などの軌道パラメータが入力信号評価部３０７によって変更され、関節角度とリンクパラメータと駆動トルク指令値から、ロボットに搭載されている荷重（患者の体重など）が慣性モーメント算出部によって推定されてリンクの慣性モーメントが算出され、この慣性モーメントや入力信号評価部の評価値に基づいて、駆動部で使用されるフィードフォワードゲインがフィードフォワード指令値算出部によって算出されるので、ユーザが指示する動作方向がＯＮとＯＦＦのみのような離散的な信号であっても、ロボットアーム型治療台を滑らかに応答性良く動作させることができる。

また、ユーザの入力信号が離散的であっても、履歴データから応答性を自動的に調整することでユーザの意図に応じて簡便に操作することができる。

（第１変形例）
第２実施形態の第１変形例によるロボット制御装置を図７に示す。この第１変形例のロボット制御装置は、図４に示す第２実施形態のロボット制御装置において、駆動部１０１Ａを駆動部１０１Ｂに置き換えるとともに、ＣＰＵ１ＡをＣＰＵ１Ｂに置き換えた構成となっている。駆動部１０１Ｂは駆動トルク指令値を受けてアクチュエータを駆動する電流を生成するトルク指令型の駆動部である。ＣＰＵ１Ｂは、ＣＰＵ１Ａにおいて、制御指令算出部３１０を新たに設けた構成となっている。なお、制御指令算出部３１０は、モジュールであって、ハードウェアで実現してもよいし、ソフトウェアで実現してもよい。

この第１変形例においては、図７に示すように、制御指令算出部３１０が、関節速度目標値算出部３０５で算出された関節速度目標値と、関節角度検出部１０２で検出された関節角度と、フィードフォワード指令値算出部３０９で算出されたフィードバックゲインから、駆動トルク指令値を式（９）に示すように算出し、駆動部１０１Ｂに送る。

駆動部１０１Ｂは、制御指令算出部３１０で算出された駆動トルク指令値に基づき、アクチュエータ１００を駆動する。

慣性モーメント算出部３０８が制御指令算出部３１０で算出された駆動トルク指令値からロボットに搭載された負荷荷重を算出し、これをもとに慣性モーメントを算出する。

この第１変形例も第２実施形態と同様の効果を奏することができる。

上記のように、各実施形態を説明したが、この開示の一部をなす論述及び図面はこの発明を限定するものであると理解すべきではない。この開示から当業者には様々な代替実施の形態、実施例及び運用技術が明らかとなろう。例えば上記実施形態では回転関節を対象に説明をしているが、直動関節であっても同様に適用できることは明らかである。

また、ＣＰＵ１Ａ、１Ｂ、リンクパラメータ記憶部２０１等が制御対象であるロボットの内部に埋め込まれて一体化されていても良い。また、ＣＰＵ１Ａ、１Ｂ、リンクパラメータ記憶部２０１等が、制御対象であるロボットの外部にあり、ロボットを有線或いは無線で遠隔制御することも可能である。

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

１、１Ａ、１Ｂ 中央演算処理装置（ＣＰＵ）
１０ ロボット
１１ 第１関節軸
１２ 第２関節軸
１３ 第３関節軸
１４ 第４関節軸
１５ 第５関節軸
１６ 第６関節軸
１７ 第７関節軸
２０ 天板
１００ アクチュエータ
１０１、１０１Ａ、１０１Ｂ 駆動部
１０２ 関節角度検出部
１０３ 目標値入力部
１０４ 速度制限提示部
１０５ 制御点位置算出部
１０６ ヤコビ行列算出部
１０７ 目標値整形部
１０８ 目標速度算出部
１０９ 速度監視部
１１０ 速度抑制部
１１１ 冗長性評価部
２０１ リンクパラメータ記憶部
２０２ 最大速度記憶部
３０２ 動作方向指定部
３０３ 先端速度目標値算出部
３０５ 関節速度目標値算出部
３０７ 入力信号評価部
３０８ 慣性モーメント算出部
３０９ フィードフォワード指令値算出部
３１０ 制御指令算出部

Claims (5)

1. アーム型治療台を備えたロボットの関節軸を制御周期毎に駆動するアクチュエータと、
   前記関節軸の角度を検出する関節角度検出部と、
   前記関節角度とリンクパラメータとに基づいて前記ロボットの制御点の位置座標を算出する制御点位置算出部と、
   検出された前記関節角度および前記リンクパラメータに基づいて前記各関節における関節座標系と作業座標系との間のヤコビ行列を算出するヤコビ行列算出部と、
   前記制御点の目標位置および制御点の目標値を入力する目標値入力部と、
   前記制御点の目標値を整形する目標値整形部と、
   前記目標値整形部により整形された制御点の目標値と前記制御点位置算出部により算出された制御点の現在値との差分を算出し、前記ヤコビ行列を用いて逆運動学計算を行い、前記制御点における目標速度を算出する目標速度算出部と、
   前記目標速度を監視し、算出された前記目標速度が閾値を超えている場合に前記目標速度が前記閾値以下となるように制限する速度監視部と、
   前記算出された目標速度が前記閾値を超えている場合に前記制御点の目標値を修正する速度抑制部と、
   前記速度抑制部によって前記ロボットの動作方向が制限されているときに、ユーザに対して制限された動作方向を提示する速度制限提示部と、
   前記速度監視部から出力された目標速度と前記関節角度とに基づいて指令値を算出し、指令値に基づいて前記アクチュエータを駆動する第１駆動部と、
   を備え、
   前記目標値整形部は、前記速度抑制部によって前記制御点の目標値が修正された場合に、この修正された目標値を整形することを特徴とするロボット制御装置。
2. 前記ロボットの制御点の動作方向を指示する動作方向指定部と、
   指示された前記動作方向に基づいて、前記制御点における前記制御周期毎の先端速度目標値を算出する先端速度目標値算出部と、
   算出された前記先端速度目標値と前記ヤコビ行列を用いて逆運動学計算を行い、前記関節軸の速度目標値を算出する関節速度目標値算出部と、
   前記動作方向の履歴や操作時間に基づいて軌道パラメータを変更する入力信号評価部と、
   検出された前記関節角度と、前記リンクパラメータと、前記駆動部の指令値とに基づいて、前記ロボットに搭載されている患者の荷重を推定し、リンクの慣性モーメントを算出する慣性モーメント算出部と、
   前記慣性モーメントや前記入力信号評価部の評価値に基づいて、フィードフォワード指令値を算出するフィードフォワード指令値算出部と、
   算出された前記速度目標値と、前記フィードフォワード指令値に基づいて、前記アクチュエータを駆動する第２駆動部とを
   を備えていることを特徴とする請求項１記載のロボット制御装置。
3. 算出された前記速度目標値と、前記フィードフォワード指令値に基づいて駆動トルク指令値を算出する制御指令算出部を更に備え、
   前記慣性モーメント算出部は、検出された前記関節角度と、前記リンクパラメータと、前記駆動トルク指令値とに基づいて、前記ロボットに搭載されている患者の荷重を推定し、リンクの慣性モーメントを算出し、
   前記第２駆動部は、前記駆動トルク指令値に基づいて、前記アクチュエータを駆動することを特徴とする請求項２記載のロボット制御装置。
4. 前記入力信号評価部は、入力信号の変化パターンを記録し、設計した近似関数に含まれる応答パラメータをユーザの使用状況に応じて変化させることを特徴とする請求項２または３記載のロボット制御装置。
5. 前記ロボットは、前記目標値入力部において設定される前記制御点の目標値の自由度よりも冗長なアクチュエータを有し、
   前記ロボットが可動限界や特異点に近づいた場合に目標速度が算出されるように、前記関節の可動範囲、前記ヤコビ行列の特異点近傍度を評価関数として算出される冗長性パラメータを算出する冗長性評価部を前記ロボット制御装置は更に備え、
   前記目標速度算出部は、前記冗長性パラメータに基づいて、冗長自由度の逆運動学計算を行うことにより前記目標速度を算出することを特徴とする請求項１乃至４のいずれかに記載のロボット制御装置。

Images