JPH0732277A - ロボットの制御装置 - Google Patents

Abstract

(57)【要約】 （修正有） 【目的】 ロボットの許容最大速度や最大加速度の値を
越えない目標軌道を生成し、移動する物体の捕捉やあら
かじめ指定された目標到達位置に目標速度で到達するこ
とのできるロボットの制御装置を提供する。 【構成】 ロボット位置検出手段４により検出したロボ
ット３の位置、ロボット３が到達目標とする目標到達位
置、ロボット３が目標到達位置へ到達する時に目標とす
る目標速度、およびロボット３の制限最高速度または制
限最高加速度の各値を用いて、ロボット３が制限最高速
度または制限最高加速度を越えない速度をとるとともに
ロボット３の初期位置から目標位置へ到達するのに要す
る時間が等しい軌道の中ではロボットの加速度の二乗総
和が最小となるようにする。

Description

【発明の詳細な説明】

【０００１】

【産業上の利用分野】本発明はロボットの位置を制御す
るロボットの制御装置に係り、特にロボットの初期位置
から前記目標到達位置へ至るロボットの目標軌道を算出
する目標軌道算出手段を備えるロボットの制御装置に関
する。

【０００２】

【従来の技術】現在、工場の生産ラインなどで用いられ
る産業用ロボットには、搬送用のコンベア上に載せられ
た部品に追従して作業を行う機能を有するものがごく一
般的に使われている。こういったロボットの追従運動
は、追従対象が常に一定の経路や速度で移動する場合、
つまり追従対象を載せたコンベアが定常運転を行う場合
のみに有効であり、例えばコンベアの移送速度が不規則
に変化したり、またロボットとコンベアの位置関係が不
規則に変化したりすると、上述した追従運動が不可能と
なる。

【０００３】このような不具合を改善するために、特開
昭６２−５４６８８号公報に記載の産業用ロボットは、
コンベアの移送速度を検出することによって追従対象の
位置を演算し、これに基づいてロボット先端部の位置を
演算し、この演算結果に基づいてロボット先端部の移動
軌跡を決定するものである。この制御方法を用いると、
コンベアの移送速度の変化による追従対象の位置や速度
の変化に対して適確に対応することが可能となる。

【０００４】ところが例えば宇宙空間において人工衛星
のような浮遊物体（以下、ペイロードという）の追従を
考えた場合、ペイロードの運動経路や運動速度はかなら
ずしも正確に一定とは限らない上にペイロード位置や速
度の計測誤差も無視できないために、上記の制御方法で
は追従は不可能である。

【０００５】しかも、ペイロードの捕捉を行うにあたっ
ては、マニピュレータ先端部がペイロードに対してその
相対速度を正確に一致させておく必要があり、もしそう
しなければ、人工衛星等の大質量物を捕捉する際のマニ
ピュレータに加わる力が大きなものになり機器類に悪影
響を及ぼす可能性がある。

【０００６】特開昭６３−７４４３４号公報に記載のマ
ニピュレータの制御方法及び装置は、上記問題点を解決
するために、移動物体の位置情報を検知し、マニピュレ
ータが前記移動物体に対して移動加速度の二乗の累積が
最小となるようにして相対位置と相対速度を一致させる
マニピュレータの制御方法である。

【０００７】この方法では、移動物体に対して相対位置
および相対速度を一致させるべき時刻をあらかじめ指定
してマニピュレータの軌道を計算する。この結果生成さ
れる軌道は、運動物体が等速度運動しているとすれば、
指定された時刻で相対位置・速度を一致させる軌道の中
で加速度の二乗の累積が最小となることが保証されるの
で、滑らかな軌道となる。

【０００８】しかし、前記公報に記載のマニピュレータ
の制御方法及び装置には、移動物体の追跡途中の軌道の
速度や加速度の最大値については言及されていない。産
業用などのロボットでは、通常、制限最大速度を規定し
ており、ロボットの軌道はその最大速度を上回らないよ
うに生成する。一方、将来の宇宙空間で人工衛星の回収
や宇宙ステーションの保守点検作業などを行うようにな
ると考えられている、移動機能を有する宇宙ロボットで
は、スラスタの推進力に限界が存在するために、その移
動途中の加速度の最大値をスラスタ推進力による限界以
下に設定する必要がある。ところが、前記公報に記載の
マニピュレータの制御方法及び装置では、これらの制約
下での軌道の生成方法について述べられておらず、ロボ
ットやマニピュレータの許容最大加速度や速度を上回る
軌道を生成してしまい、実際にロボットが動作する際に
その目標軌道に追従できない可能性が存在した。

【０００９】また、組立作業などにおいても、部品を組
み立てる対象物体にある特定の方向からアプローチする
必要がある作業が数多く存在するが、そのような作業に
おいて、目標とする位置に指定した速度でロボット先端
を到達させることができると連続的に次の動作へスムー
ズに移行することを容易にする。

【００１０】特に、任意の速度をもった初期状態から任
意の速度で到達位置へ到達する軌道が、最大速度や最大
加速度の値がそのロボットの許容最大値以下となること
を保証しつつ生成できれば、ロボットに一連の連続的な
作業を行わせるのがより容易になる。これまでにも、ロ
ボットが移動すべき目標点を複数教示しておき、それら
の教示点をスプライン曲線などで補間する手法は存在し
ており、教示点の指定により特定方向からのロボットの
アプローチを実現することは可能であった。

【００１１】しかし、軌道の補間とは位置の補間であ
り、目標位置での速度や初期速度、作業途中の値などを
考慮して十分滑らかなロボットの時間軌道を生成するの
は必ずしも容易ではなかった。

【００１２】

【発明が解決しようとする課題】以上のように従来は、
移動物体を追跡し、移動物体に到達した時点でその移動
物体の速度とマニピュレータの速度を等しくするマニピ
ュレータの制御方法及び装置は存在したものの、追跡途
中の速度や加速度の値を許容値以下にできる保証がなか
った。

【００１３】そこで本発明の目的は、上記従来技術の有
する問題を解消し、ロボットの制限最大速度や制限最大
加速度の値を越えずに、移動する物体の捕捉やあらかじ
め指定された目標到達位置に目標速度で到達することの
できるロボットの制御装置の提供することである。

【００１４】

【課題を解決するための手段】上記の目的を達成するた
めに本発明においては、 （１）ロボットの位置を検出するためのロボット位置検
出手段と、前記ロボット位置検出手段により検出したロ
ボットの位置、ロボットが到達目標とする目標到達位
置、ロボットが前記目標到達位置へ到達する時に目標と
する目標速度、およびロボットの制限最高速度または制
限最高加速度の各値を用いて、ロボットが前記制限最高
速度または前記制限最高加速度を越えない速度をとると
ともにロボットの初期位置から前記目標位置へ到達する
のに要する時間が等しい軌道の中ではロボットの加速度
の二乗総和が最小となるように、ロボットの初期位置か
ら前記目標到達位置へ至るロボットの目標軌道を算出す
る目標軌道算出手段と、前記ロボット位置検出手段の出
力信号と前記目標軌道算出手段の出力信号とが入力され
てロボットを駆動する信号を出力するロボット制御手段
と、を備えたことを特徴とする。 （２）また、ロボットの制御装置は、ロボットの位置情
報を得るロボット位置検出手段と、物体の位置情報を得
る物体位置検出手段と、前記物体位置検出手段の出力信
号および軌道の最大速度値を入力とし、初期位置から物
体位置まで到達するとともに、物体位置到達時点での軌
道速度を物体の移動速度と等しくするロボットの目標軌
道の中を出力し、本軌道が、物体が等速度運動をしてい
る場合には、最大速度が入力された値を越えず、最大速
度の物体位置到達に要する時間が等しい軌道の中で移動
加速度の二乗の累積が最小となる目標軌道算出手段と、
前記ロボット位置検出手段の出力信号と前記目標軌道算
出手段の出力信号を入力し、ロボットを駆動する信号を
出力するロボット制御手段とを備えたことを特徴とす
る。 （３）また、ロボットの制御装置は、ロボットの位置情
報を得るロボット位置検出手段と、物体の位置・速度情
報を得る物体位置検出手段と、前記ロボット位置検出手
段の出力信号および軌道の最大加速度を入力とし、初期
位置から物体位置まで到達するとともに、物体位置到達
時点での軌道速度を物体の移動速度と等しくするロボッ
トの目標軌道を出力し、本軌道が、物体が等速度運動を
している場合には、最大加速度が入力された値を越え
ず、物体位置到達に要する時間が等しい軌道の中で移動
加速度の二乗の累積が最小となる軌道を出力する目標軌
道算出手段と、前記ロボット位置検出手段の出力信号と
前記目標軌道算出手段の出力信号を入力し、ロボットを
駆動する信号を出力するロボット制御手段とを備えたこ
とを特徴とする。

【００１５】

【作用】上記のようなロボットの制御装置とすれば、ロ
ボットは移動物体、あるいは目標到達位置に対して位置
と速度を無駄な動きなく徐々に一致させてゆく。また、
ロボットの軌道の速度や加速度は、あらかじめ入力され
た制限最大速度または制限最大加速度の値を上回らな
い。従って、ロボットは生成された軌道に追従しなが
ら、移動物体や目標到達位置に対して滑らかに接近し、
適確に捕捉することや、次の動作への連続的な移行を可
能とするロボットの制御装置が実現する。

【００１６】

【実施例】以下、図面に参照して本発明によるロボット
の制御装置の実施例を詳細に説明する。

【００１７】第１実施例 図１は本実施例のロボットの制御装置を示すシステムの
ブロック図である。ロボットの制御装置は、ロボット３
の目標軌道を算出する目標軌道算出部１、ロボット３の
位置を制御する信号を生成するロボット制御部２、およ
びロボット３の位置を検出するロボット位置検出センサ
４を備えている。

【００１８】目標軌道算出部１では、ロボット３の目標
到達位置、目標到達位置での目標速度、目標位置に到達
するまでの制限値としての最大速度、およびロボット位
置検出センサ４によって検出されたロボット３の位置の
各値を入力信号とし、ロボット制御部２の各制御周期に
おけるロボットの目標位置および目標速度の値を算出す
る。ロボット制御部２は、目標軌道算出部１の出力であ
るロボットの目標位置および、ロボット位置検出センサ
４の出力であるロボットの現在位置の値からロボットを
駆動する信号を生成する。

【００１９】従って、ロボット３は目標軌道算出部１で
生成された目標軌道に追従するように動作する。以上の
働きにより、ロボットの目標到達位置に対して滑らかに
接近し、目標到達位置で目標速度となり、次の動作への
連続的な移行を可能とする。

【００２０】以上のような構成の本発明において、目標
軌道算出部１で計算されてロボット制御部２に与えられ
るロボットの目標軌道は、以下の方法により決定され
る。

【００２１】今、目標到達位置ベクトルをＸｃ、目標位
置に到達時の目標速度ベクトルをＶｃとする。また、目
標到達位置への到達時刻を仮にｔｆとする。

【００２２】ここで、時刻ｔｆで目標到達位置Ｘｃに到
達する、速度Ｖｃで等速度運動している移動物体を仮定
する。本移動物体に、時刻ｔｆで位置および速度が一致
する接近軌道の中で加速度の二乗総和が最小となる軌道
は、特開６３−７４４３４号公報に記載のマニピュレー
タの制御方法及び装置にあるように、次の式で算出され
る。

【００２３】

【数１】 ただし、移動目標の位置ベクトルをＸｔ、ロボットの目
標位置ベクトルをＸｍ、これらの偏差ベクトルをＸ（＝
Ｘｔ−Ｘｍ）とする。また、同様に速度の偏差ベクトル
をＶ（＝Ｖｔ−Ｖｍ）とし、ロボットの目標加速度ベク
トルをＡｍとする。（１）式により求められたロボット
の目標加速度ベクトルを数値積分してゆくことにより、
ロボットの目標速度・目標位置の値が算出される。

【００２４】従って、目標到達位置Ｘｃ、目標位置到達
時の目標速度ベクトルＶｃおよび目標到達時刻ｔｆが与
えられた場合に、 Ｘｔ＝Ｘｃ＋Ｖｃ・（ｔ−ｔｆ） （２） Ｖｔ＝Ｖｃ （２）式により、Ｘｔ，Ｖｔを算出し、（１）式に代
入、Ａｍを求めた後に数値積分により目標軌道の速度、
位置を順次算出してゆくことにより、時刻ｔｆで目標到
達位置Ｘｃに到達し、目標位置到達時の目標速度ベクト
ルがＶｃとなる軌道の中で加速度の二乗総和が最小の軌
道が生成される。

【００２５】また、ロボット位置検出センサ４の出力で
あるロボット位置の初期時刻での値を軌道の初期位置、
ロボット位置の値を微分することにより初期時刻でのロ
ボット速度を求め、軌道の初期速度とすることにより、
ロボットの初期位置から目標位置に到達する軌道が生成
される。

【００２６】従って、入力された最大速度の値から目標
到達時刻ｔｆを算出することができれば、そのｔｆの値
により各制御周期のロボット目標位置・速度の値を
（１）式および（２）式により算出することができる。

【００２７】一方、ロボディックス・メカトロニクス講
演会予稿集（１９９１）に記載の片山氏らの「視覚情報
を用いたマニピュレータによる移動物体の捕獲〜高精度
位置計測と最適接近軌道〜」と題する発表の中の解決を
参照すると、（１）式を以下の初期条件のもとで解く
と、ロボットの目標位置ベクトルは時間の３次関数とな
り次のようになる。

【００２８】 Ｘｍ（τ）＝αγ³ ＋βγ² −ｖ_c γ＋ｘ_c （３） α＝｛２（ｘ_c −ｘ_o ）−ｔ_f （ｖ_c ＋ｖ_o ）｝／ｔ_f ³ β＝｛−３（ｘ_c −ｘ_o ）＋ｔ_f （２ｖ_c ＋ｖ_o ）｝／ｔ_f ²

【００２９】

【数２】 τ＝ｔｆ−ｔ従って、ロボットの軌道の速度は（３）式
を時間に関して微分することにより以下のようになる。

【００３０】

【数３】 （４）式は時間に関する２次式であるので、最大速度を
とるのは ・初期時刻 ・終端時刻 ・追跡途中（τ＝−β／３α） の３通りが存在しうる。ただし、初期時刻の軌道速度は
初期条件によりｖｏと決まっており、終端時刻の速度は
目標位置に到達時の速度ｖｃと決まっている。従って、
最大速度がｖｏおよびｖｃより大きい場合には、その最
大速度をとる時刻は追跡途中（τ＝−β／３α）とな
る。ロボットでは通常複数自由度を有しているが、最大
速度の値としては各自由度ごとの値を設定するものと
し、最大速度の値からｔｆの算出計算は各自由度ごとに
独立に以下のように行う。（４）式の値をＶ_max とする
と以下のような２次方程式になる。 ｛３（ｖ_max −ｖ_c ）（ｖ_c ＋ｖ_o ）＋（２ｖ_c ＋ｖ_o ）² ｝ｔ_f ² −６（ｘ_c −ｘ_o ）（ｖ_max ＋ｖ_c ＋ｖ_o ）ｔ_f ＋９（ｘ_c −ｘ_o ）² ＝０ （５） （５）式よりｔｆの値を求め、そのｔｆの値が初期時刻
から終端時刻の間に含まれれば、追跡軌道の最大速度が
入力された値であるＶ_max となる。ただし、通常、最大
速度が問題にされるのは、その絶対値であるので（５）
式のＶmax の値の符号を反転させて再びｔｆの値を求め
る必要がある。そのｔｆの値が初期時刻から終端時刻の
間に含まれれば、追跡軌道の最大速度が（−Ｖ_max ）と
なる到達時刻となる。以上の結果、複数の解が求められ
た場合には、それらの値の中から最も小さい値を選択す
ることにより、最大速度の絶対値が与えられた値Ｖ_max
となる軌道の中で最も到達時刻が早いものが求められ
る。

【００３１】一方、与えられた最大速度の値がＶ_c およ
びＶ_o の絶対値と等しい場合には、最大速度をとるのが ａ） 初期時刻（τ＝ｔｆ） ｂ） 終端時刻（τ＝０） ｃ） 追跡途中（τ＝−β／３α） の３通りが考えられる。

【００３２】ａ）およびｂ）の場合には、軌道速度が極
値をとる時刻 τ＝−β／３α が初期時刻から終端時刻の間に含まれていないというこ
とであるから τ＝−β／３α≦０ ｏｒ τ＝−β／３α≧ｔｆ ということになる。

【００３３】 τ＝−β／３α≦０→ （２ｖ_c ＋ｖ_o ）（ｖ_c ＋ｖ_o ）ｔ_f ² −（ｘ_c −ｘ_o ） （７ｖ_c ＋５ｖ_o ）ｔ_f ＋６（ｘ_c −ｘ_o ）² ≦０ （６） τ＝−β／３α≧ｔｆ→ （ｖ_c ＋ｖ_o ）（２ｖ_o ＋ｖ_c ）ｔ_f ² −（ｘ_c −ｘ_o ） （５ｖ_c ＋７ｖ_o ）ｔ_f ＋６（ｘ_c −ｘ_o ）² ≦０ （７） （６）式あるいは（７）式を満たすｔｆの中で最も小さ
な値は、それぞれの式で等号が成り立つ場合である。従
って、やはり軌道速度が極値をとる時刻 τ＝β／３α で最大速度をとることになるので、（５）式によりｔｆ
を求めればよいことになる。

【００３４】以下、図２を用いて到達時刻ｔｆの算出手
順を説明する。まずＳＴ１で、与えられた最大速度の値
Ｖ_max の値の絶対値と目標位置到達時の目標速度Ｖｃの
絶対値および初期速度Ｖｏの絶対値の大きさを比較す
る。Ｖ_max の絶対値の大きさがＶｃ、Ｖｏのいづれかの
絶対値の大きさより小さい場合には、そのような軌道は
生成できないので、軌道生成が不可能であると応答す
る。

【００３５】もし、Ｖ_max の絶対値の大きさがＶｃ，Ｖ
ｏの絶対値より小さくない場合には、ＳＴ２で、最大速
度記録時の目標時刻算出を行う。

【００３６】ＳＴ２では、まず与えられたＶ_max の値に
より（５）式からｔｆの値を計算し、求められたｔｆの
値から最大速度を記録する時刻τの値が初期時刻から終
端時刻の間に含まれているかどうかの以下に示す条件を
満たすｔｆを求める。

【００３７】 ０≦τ≦ｔｆ （８） τ＝β／３α 次に、Ｖ_max の値の符号を反転させて（５）式に代入
し、ｔｆの値を計算し、（８）式の条件を満たすｔｆを
求める。これらの求められたｔｆの値は、ＳＴ３で、候
補値として登録しておく。

【００３８】以上の処理の後にＳＴ４にて登録された候
補値が存在するか調べ、もし存在する場合には、ＳＴ５
で、それらの値の中で最小の値のものを求め、到達時刻
として出力する。また、もし登録された候補値が存在し
ない場合には、与えられた最大速度では目標到達位置に
目標速度で到達することは不可能であるので、軌道生成
不可能であると出力する。

【００３９】以上の処理により求められた到達時刻ｔｆ
の値により、各制御周期ごとに（１）式の計算を行うこ
とにより、ロボットの目標位置・目標速度の値を計算
し、それらの値に追従するようにロボット制御部にて制
御することにより、ロボットは、運動途中の最大速度が
与えられた値となる軌道に従いながら目標到達位置に目
標速度で到達することが可能となる。また、この軌道
は、同じ時間で到達する軌道の中では加速度の二乗の累
積が最小の軌道となっており、非常に滑らかな軌道とな
る。

【００４０】また、ロボットが並進３自由度、回転３自
由度を有しており、最大速度の値が並進合成速度および
回転合成速度で与えられる場合には、与えられた並進合
成速度の値を１／３^1/2 倍した値を各並進自由度の最大
速度とし、与えられた回転合成速度の値をやはり１／３
^1/2 倍した値を各回転自由度の最大速度として、同様の
処理を行い捕獲時刻ｔｆの値を求めることにより、軌道
の並進合成速度および回転合成速度の値が与えられた最
大値の値を越えない軌道が生成される。

【００４１】また、到達時速度ベクトルと初期速度ベク
トルの偏差ベクトルと、到達位置ベクトルと初期位置ベ
クトルの偏差ベクトルが平行の場合には、座標変換する
ことにより、運動する自由度を１自由度とすることがで
きるので、軌道の並進合成速度および回転合成速度の最
大値の値が与えられた最大値となる軌道を生成すること
ができる。

【００４２】図３は上記のロボットの制御装置のシミュ
レーションを行った結果を示した図である。本シミュレ
ーションでの設定条件は第１表に示すとおりである。

【００４３】 第 １ 表 ロボット初期位置 ０mm ロボット初期速度 ０mm／ｓ 目標到達位置 ５００mm 目標到達時速度 −１００mm／ｓ このように、ロボットが目標到達位置で目標速度となる
ように滑らかな軌道を描いて接近してゆくために、ロボ
ットの制御部でその目標軌道に追従させることも容易に
なる。

【００４４】また、このように目標到達位置での速度や
初期速度までを指定して軌道を生成できることにより、
ロボットが停止せずに連続的に次の動作に移る場合の軌
道生成も容易に行うことが可能になる。

【００４５】第２実施例 図４は本発明の第２実施例のロボットの制御装置を示す
システムのブロック図である。ロボットの制御装置は、
ロボット７の目標軌道を算出する目標軌道算出部５、ロ
ボット３の位置を制御する信号を生成するロボット制御
部６、およびロボット３の位置を検出するロボット位置
検出センサ８を備えている。

【００４６】目標軌道算出部５では、ロボットの目標到
達位置および目標到達位置での目標速度、さらに目標位
置に到達するまでの制限値としての最大加速度、および
ロボット位置検出センサ８の出力であるロボット位置を
入力とし、ロボット制御部６の各制御周期におけるロボ
ットの目標位置および目標速度の値を算出する。

【００４７】ロボット制御部６は、目標軌道算出部５の
出力であるロボットの目標位置および、ロボット位置検
出手段の出力であるロボットの現在位置の値からロボッ
トを駆動する信号を生成する。従って、ロボット７は目
標軌道算出部５で生成された目標軌道に追従するように
動作する。以上の働きにより、ロボットは目標到達位置
に対して滑らかに接近し、目標到達位置で目標速度とな
り、次の動作への連続的な移行を可能とする。

【００４８】以上のような構成の本実施例において、目
標軌道算出部５で計算されてロボット制御部６に与えら
れるロボットの目標軌道は、第１実施例と同様に、まず
制限最大加速度の値から到達時刻ｔｆを求め、そのｔｆ
の値により（１）式から算出することにより求めること
ができる。

【００４９】また最大加速度の値としては、やはり第１
実施例と同様に各自由度ごとの値を設定するものとし、
制限最大加速度の値からのｔｆの算出計算は各自由度ご
とに独立に以下のように行う。

【００５０】ロボットの軌道の加速度は（４）式をさら
に時間に関して微分することにより以下のように時間に
関する１次式となる。

【００５１】

【数４】 従って、加速度の値は初期時刻または終端時刻で最大値
となる。初期時刻または終端時刻での加速度の値を与え
たときの到達時刻ｔｆの値は以下のような２次方程式を
求めることにより算出される。

【００５２】 α_max ｔ_f ² ＋２（ｖ_c ＋２ｖ_o ）ｔ_f −６（ｘ_c −ｘ_o ）＝０ （１０） α_max ｔ_f ² −２（２ｖ_c ＋ｖ_o ）ｔ_f ＋６（ｘ_c −ｘ_o ）＝０ （１１） 以下に、最大加速度の値から到達時刻ｔｆを求める手順
について図５に従いながら説明する。

【００５３】ＳＴ６で、入力された最大加速度の値から
初期時刻での加速度が最大となる時の到達時刻ｔｆの算
出を行う。

【００５４】まず、入力された最大加速度の値を（１
０）式に代入してｔｆの値を求める。次に、それらの値
が（８）式の条件を満たしているかどうかを調べ、
（８）式の条件を満たしているものについては、そのｔ
ｆの値を（９）式に代入することにより終端時刻での加
速度を求め、初期時刻での加速度の値と終端時刻での加
速度の値を比較する。もし、初期時刻での加速度の絶対
値の方が大きければｔｆの候補となるので保存する。

【００５５】次に、入力された最大加速度の符号を反転
して同様の手続きによりｔｆの値を算出しｔｆの候補と
して保存する。

【００５６】以上の手続きにより求められたｔｆの候補
をＳＴ７で、候補値として登録する。次に、ＳＴ８で、
入力された最大加速度の値から終端時刻での加速度が最
大となる時の到達時刻ｔｆの算出を行う。まず、入力さ
れた最大加速度の値を（１１）式に代入してｔｆの値を
求める。

【００５７】次に、それらの値が（８）式の条件を満た
しているかどうかを調べ、（８）式の条件を満たしてい
るものについては、そのｔｆの値を（９）式に代入する
ことにより初期時刻での加速度を求め、終端時刻での加
速度の値と初期時刻での加速度の値を比較する。もし、
終端時刻での加速度の絶対値の方が大きければｔｆの候
補となるので保存する。

【００５８】次に、入力された最大加速度の符号を反転
して同様の手続きによりｔｆの候補として保存する。

【００５９】以上の手続きにより求められたｔｆの候補
をＳＴ９で、候補値として登録する。ＳＴ１０で、候補
値が登録されているかどうかを調べ、ひとつも登録され
ていなければ、入力された最大加速度の値で目標到達位
置に目標速度で到達することは不可能であるので、軌道
生成不可能であると出力する。一方、登録されているも
のがある場合には、ＳＴ１１で、それらの値の中で最小
の値のものを求め、出力する。

【００６０】以上の手続きにより求められた到達時刻ｔ
ｆの値により、各制御周期ごとに（１）式の計算を行う
ことにより、ロボットの目標位置および速度の値を計算
し、それらの値に追従するようにロボット制御部にて制
御することにより、ロボットは運動途中の最大加速度が
入力された値となる軌道に従いながら目標到達位置に目
標速度で到達することが可能となる。また、この軌道は
同じ時間で到達する軌道の中では加速度の二乗の累積が
最小の軌道となっており、非常に滑らかな軌道となる。
また、ロボットが複数の自由度を有し、最大加速度の値
がそれらの複数自由度に関しての合成最大速度として与
えられる場合には、第１実施例と同様に与えられた合成
最大加速度の値から各自由度の最大加速度を計算するこ
とにより、軌道の合成加速度が与えられた最大加速度の
値を越えないロボット軌道が生成される。

【００６１】さらに、ロボット本体に取り付けられたス
ラスタ推進力により移動する機能をもった宇宙ロボット
の目標軌道を上記の方法で生成し、その軌道に追従して
運動するように宇宙ロボットの慣性の値により推進力を
フィードフォワード的に算出、スラスタ推進力を制御す
ることにより、目標到達位置まで最小のエネルギーで到
達することも可能となる。

【００６２】図６は上記のロボットの制御装置のシミュ
レーションを行った結果を示した図である。本シミュレ
ーションでの設定条件は第２表に示すとおりである。

【００６３】 第 ２ 表 ロボット初期位置 ０mm ロボット初期速度 ０mm／ｓ 目標到達位置 ５００mm 目標到達時速度 −１００mm／ｓ2 最大加速度 １００mm／ｓ2 このように、ロボットが目標到達位置で目標速度となる
ように滑らかな軌道を描いて接近してゆくために、ロボ
ットの制御部でその目標軌道に追従させることも容易に
なる。また、このように目標到達位置での速度や初期速
度までを指定して軌道を生成できることにより、ロボッ
トが停止せずに連続的に次の動作に移る場合の軌道生成
も容易に行うことが可能になる。

【００６４】第３実施例 図７は本発明の第３実施例のロボットの制御装置を示す
システムのブロック図である。ロボット１２はその先端
で物体の捕捉が可能となっている。また、ロボット１２
には、ロボット位置検出センサ１３が取り付けられてい
る。さらに、ロボット１２には物体位置検出手段として
の物体位置検出センサ９も取り付けられている。

【００６５】物体位置検出センサ９の出力である物体位
置およびロボット位置検出センサ１３の出力および軌道
の最大速度の値が目標軌道算出部１０に入力され、物体
が等速度運動している場合には軌道の最大速度が入力さ
れた値を越えずに物体捕捉時の物体とロボットとの速度
が等しくなるような軌道を生成する。

【００６６】目標軌道算出部１０の出力はロボット制御
部１１に入力され、ロボット制御部１１では、目標軌道
算出部１０の出力であるロボットの目標位置および目標
速度の値と、ロボット位置検出センサ１３の出力である
ロボットの現在位置および速度の値からロボットの駆動
信号を生成する。従って、ロボット１２は目標軌道算出
部１０で生成された目標軌道に追従するように動作す
る。以上の働きにより、ロボットは目標物体に対して滑
らかに接近し、捕捉時に目標物体の速度と等しい速度と
なり、確実な捕捉を可能とする。

【００６７】以上のような構成の本実施例において、目
標軌道算出部１０で算出されてロボット制御部１１に入
力されるロボットの目標軌道は第１の実施例と同様に、
まず最大速度の値から捕獲時刻ｔｆの値を決定し、その
ｔｆの値により（１）式を各制御周期ごとに計算するこ
とにより算出する。ただし、（１）式における移動目標
の位置Ｘｔおよび速度Ｖｔを物体位置検出センサ９の出
力である物体位置およびその値を微分した値とする。

【００６８】捕獲時刻ｔｆの値の決定方法を以下に述べ
る。初期時刻での、物体位置検出センサの出力である物
体位置および物体速度の値をそれぞれｘｐ，ｖとする。
物体が等速度運動すると仮定し、捕獲時刻をｔｆとする
と、時刻ｔｆで物体に到達するとともに速度を等しくす
る軌道の中で、加速度の二乗の総和が最小になる軌道は
第１の実施例と同様に以下のように求められる。

【００６９】

【数５】 （１２）式を初期条件のもとに解くと（１３）式のよう
になる。

【００７０】 Ｘ_m (t) ＝ατ³ ＋βτ² −ｖ_c τ＋（ｘ_p ＋ｖ_c ｔ_f ） （１３） α＝｛２（ｘ_p −ｘ_o ）＋ｔ_f （ｖ_c −ｖ_o ）｝／ｔ_f ³ β＝｛−３（ｘ_p −ｘ_o ）−ｔ_f （ｖ_c −ｖ_o ）｝／ｔ_f ² （１３）式は、第１実施例と同様に時間に関する３次式
になっており、最大速度の値から捕獲時刻ｔｆを求める
手順は第１の実施例と同様になる。ただし、（５）式、
（６）式および（７）式が以下の（１４），（１５）式
および（１６）式に代わる。

【００７１】 （ｖ_c −ｖ_o ）（４ｖ_c −ｖ_o −３ｖ_max ）ｔ_f ² ＋６（ｘ_p −ｘ_o ）（−ｖ_max −ｖ_o ＋２ｖ_c ）ｔ_f ＋９（ｘ_p −ｘ_o ）² ＝０ （１４） （ｖ_c −ｖ_o ）² ｔ_f ² ＋５（ｘ_p −ｘ_o ）（ｖ_c −ｖ_o ）ｔ_f ＋６（ｘ_p −ｘ_o ）² ≦０ （１５） ２（ｖ_c −ｖ_o ）² ｔ_f ² ＋７（ｘ_p −ｘ_o ）（ｖ_c −ｖ_o ）ｔ_f ＋６（ｘ_p −ｘ_o ）² ≦０ （１６） 以上の手順により、捕獲時刻ｔｆを算出し、そのｔｆの
値により（１）式から目標軌道を計算することにより、
物体が初期速度のままに等速度運動している限りはその
軌道の最大速度が与えられた値になり、物体到達時の速
度が物体と等しくなる軌道が生成され、またその時間ｔ
ｆで物体に到達する軌道の中で加速度の二乗の総和の値
が最小の軌道となる。さらに目標軌道算出手段１０で算
出された目標軌道がロボット制御部１１に入力されるこ
とにより、ロボットは目標軌道に追従するように運動
し、結局、ロボットはその許容最大速度の値を越えるこ
となく移動物体に速度を等しくして到達することが可能
になる。

【００７２】また、仮に移動物体の速度が初期速度から
多少変化したり、物体位置検出センサで検出された物体
の初期位置や速度の値に多少の誤差が含まれている場合
でも、目標軌道算出手段１０では、各制御周期ごとに物
体位置検出センサから出力される物体位置および速度の
値を用いて軌道を生成しているために、物体の加速度運
動に対応した軌道を生成する。従って、ロボットは移動
物体にほぼ速度を等しくした状態で到達することが可能
となる。

【００７３】また、物体位置検出センサ９に含まれるノ
イズのために、目標軌道算出部１０にて算出されるロボ
ットの目標速度の値が位置的に許容最大速度の値を越え
ることも考えられるが、目標軌道算出部１０で目標速度
の値を計算した際に、許容最大速度の値と毎回比較し、
許容最大速度の値を越える値であったときは、許容最大
速度の値に設定することにより、物体位置検出センサ９
に含まれるノイズの影響を防ぐことができる。

【００７４】図８は上記のロボットの制御装置のシミュ
レーションを行った結果を示した図である。本シミュレ
ーションでの設定条件は第３表に示すとおりである。

【００７５】 第 ３ 表 ロボット初期位置 ０mm ロボット初期速度 ０mm／ｓ 物体初期位置 １０００mm 物体初期速度 −１００mm／ｓ 最大加速度 １００mm／ｓ 第４実施例 図９は本発明のロボットの制御装置を示すシステムのブ
ロック図である。ロボット１７はその先端で物体の捕捉
が可能となっている。また、ロボット１７には、ロボッ
ト位置検出センサ１４が取り付けられている。さらに、
ロボット１７には物体位置検出手段としての物体位置検
出センサ１８も取り付けられている。

【００７６】物体位置検出センサ１４の出力である物体
位置およびロボット位置検出センサ１８の出力および軌
道の最大加速度の値が目標軌道算出部１５に入力され、
物体が等速度運動している場合には軌道の最大加速度が
入力された値を越えずに物体捕捉時の物体とロボットと
の速度が等しくなるような軌道を生成する。

【００７７】目標軌道算出部１５の出力はロボット制御
部１６に入力され、ロボット制御部１６では、目標軌道
算出部１５の出力であるロボットの目標位置および目標
速度の値と、ロボット位置検出センサ１８の出力である
ロボットの現在位置および速度の値からロボットの駆動
信号を生成する。従って、ロボット１７は目標軌道算出
部１５で生成された目標軌道に追従するように動作す
る。以上の働きにより、ロボットは目標物体に対して滑
らかに接近し、捕捉時に目標物体の速度と等しい速度と
なり、確実な捕捉を可能とする。

【００７８】以上のような構成の本実施例において、目
標軌道算出部１５で算出されてロボット制御部１６に入
力されるロボットの目標軌道は第２の実施例と同様に、
まず最大加速度の値から捕獲時刻ｔｆの値を決定し、そ
のｔｆの値により（１）式を各制御周期ごとに計算する
ことにより算出する。ただし、（１）式における移動目
標の位置Ｘｔおよび速度Ｖｔを物体位置検出センサ９の
出力である物体位置およびその値を微分した値とする。

【００７９】捕獲時刻ｔｆは第２実施例と同様の手続き
で求められる。ただし、（１０）式および（１１）式が
それぞれ（１７）式および（１８）式になる。

【００８０】

【数６】 以上の手順により、捕獲時刻ｔｆを算出し、そのｔｆの
値により（１）式から目標軌道を計算することにより、
物体が初期速度のままに等速度運動している限りはその
軌道の最大加速度が与えられた値になり、物体到達時の
速度が物体と等しくなる軌道が生成され、またその時間
ｔｆで物体に到達する軌道の中で加速度の二乗の総和の
値が最小の軌道となる。さらに目標軌道算出部１５で算
出された目標軌道がロボット制御部１６に入力されるこ
とにより、ロボットは目標軌道に追従するように運動
し、結局、ロボットはその許容最大加速度の値を越える
ことなく移動物体に速度を等しくして到達することが可
能になる。

【００８１】また、仮に移動物体の速度が初期速度から
多少変化したり、物体位置検出センサ１４で検出された
物体の初期位置や速度の値に多少の誤差が含まれている
場合でも、目標軌道算出部１５では、各制御周期ごとに
物体位置検出センサ１４から出力される物体位置および
速度の値を用いて軌道を生成しているために、物体の加
速度運動に対応した軌道を生成する。従って、ロボット
は移動物体にほぼ速度を等しくした状態で到達すること
が可能となる。

【００８２】また、物体位置検出センサ１４に含まれる
ノイズのために、目標軌道算出部１５にて算出されるロ
ボットの目標加速度の値が一次的に許容最大加速度の値
を越えることも考えられるが、目標軌道算出部１５で目
標加速度の値を計算した際に、入力された最大加速度の
値と毎回比較し、最大加速度の値を越える値であったと
きは、最大加速度の値に設定することにより、物体位置
検出センサ１４に含まれるノイズの影響を防ぐことがで
きる。

【００８３】図１０は上記のロボットの制御装置のシミ
ュレーションを行った結果を示した図である。本シミュ
レーションでの設定条件は第４表に示すとおりである。

【００８４】 第 ４ 表 ロボット初期位置 ０mm ロボット初期速度 ０mm／ｓ 物体初期位置 １０００mm 物体初期速度 −１００mm／ｓ 最大加速度 １００mm／ｓ2

【００８５】

【発明の効果】以上のように本発明によれば、ロボット
の許容最大速度や最大加速度の値を越えずに、移動する
物体の捕捉やあらかじめ指定された目標到達位置に目標
速度で到達することのできるロボットの制御装置ができ
る。

【図面の簡単な説明】

【図１】本発明の第１実施例のロボットの制御装置のブ
ロック図。

【図２】最大速度の値から到達時刻を算出する手順を示
すフローチャート図。

【図３】本発明のロボット制御装置の軌道生成方法を用
いてロボット軌道のシミュレーションを行った結果を示
す時間軌道図。

【図４】本発明の第２実施例のロボットの制御装置のブ
ロック図。

【図５】最大加速度の値から到達時刻を算出する手順を
示すフローチャート図。

【図６】本発明のロボット制御装置の軌道生成方法を用
いてロボット軌道のシミュレーションを行った結果を示
す時間軌道図。

【図７】本発明の第３実施例のロボットの制御装置のブ
ロック図。

【図８】本発明のロボット制御装置の軌道生成方法を用
いてロボット軌道のシミュレーションを行った結果を示
す時間軌道図。

【図９】本発明の第４実施例のロボットの制御装置のブ
ロック図。

【図１０】本発明のロボット制御装置の軌道生成方法を
用いてロボット軌道のシミュレーションを行った結果を
示す時間軌道図。

【符号の説明】

１ 目標軌道算出部 ２ ロボット制御部 ３ ロボット ４ ロボット位置検出センサ ５ 目標軌道算出部 ６ ロボット制御部 ７ ロボット ８ ロボット位置検出センサ ９ 物体位置検出センサ １０ 目標軌道算出部 １１ ロボット制御部 １２ ロボット １３ ロボット位置検出センサ １４ 物体位置検出センサ １５ 目標軌道算出部 １６ ロボット制御部 １７ ロボット １８ ロボット位置検出センサ

Claims (1)

【特許請求の範囲】
1. 【請求項１】ロボットの位置を検出するためのロボット
   位置検出手段と、 前記ロボット位置検出手段により検出したロボットの位
   置、ロボットが到達目標とする目標到達位置、ロボット
   が前記目標到達位置へ到達する時に目標とする目標速
   度、およびロボットの制限最高速度または制限最高加速
   度の各値を用いて、ロボットが前記制限最高速度または
   前記制限最高加速度を越えない速度をとるとともにロボ
   ットの初期位置から前記目標位置へ到達するのに要する
   時間が等しい軌道の中ではロボットの加速度の二乗総和
   が最小となるように、ロボットの初期位置から前記目標
   到達位置へ至るロボットの目標軌道を算出する目標軌道
   算出手段と、 前記ロボット位置検出手段の出力信号と前記目標軌道算
   出手段の出力信号とが入力されてロボットを駆動する信
   号を出力するロボット制御手段と、を備えたことを特徴
   とするロボットの制御装置。