JPH1083207A

JPH1083207A - 被搬送物体への接近及び／又は追従に係る装置及びロボット装置

Info

Publication number: JPH1083207A
Application number: JP25742096A
Authority: JP
Inventors: Ietoshi Itou; 家年伊藤
Original assignee: Sony Corp
Current assignee: Sony Corp
Priority date: 1996-09-06
Filing date: 1996-09-06
Publication date: 1998-03-31

Abstract

(57)【要約】【課題】制御遅れや構成の複雑化を伴うなく、所定の
搬送経路に沿って移動される対象物への接近及び追跡制
御を実現する。【解決手段】搬送手段ｃによって搬送されてくる被搬
送物体ｗに対して制御対象ＳＴの作用端Ｈを接近させた
りあるいは被搬送物体に追従させるための装置１におい
て、搬送基準経路Ｃ１に対して直交する方向における被
搬送物体の位置の変位量を検出する位置変位量検出手段
２と、被搬送物体がその搬送経路Ｃ２における基準位置
を通過したことを検出する基準位置通過検出手段３と、
被搬送物体が上記基準位置を通過した時点からその搬送
経路に沿って搬送手段ｃによってどれだけ搬送されたか
を検出する搬送量検出手段４とを設ける。そして、位置
変位量検出手段２、基準位置通過検出手段３、搬送量検
出手段４による検出結果を受けて被搬送物体の現時点で
の位置を現在位置計算手段６が計算し、その計算結果を
動作制御手段７が受けて制御対象の作用端を被搬送物体
に接近させ及び／又は追従させるための制御信号を生成
して制御対象に送出する。

Description

【発明の詳細な説明】

【０００１】

【発明の属する技術分野】本発明は、所定の搬送手段に
よって運ばれてくる被搬送物体への接近や追従のための
制御を可能とする装置及びロボット装置に関する。

【０００２】

【従来の技術】移動物体を掴んでこれを別の場所に移送
したり、あるいは、移動物体に追従してこれに付された
識別情報を読み取る等の制御では、移動物体への接近制
御や、その後の移動物体への追従制御が基本となる。例
えば、ロボット制御では、コンベア等によって搬送され
てくるワークをロボットの作用端で捕捉したり、あるい
はロボットの作用端をワークに追従させて移動させるよ
うな例が挙げられる。

【０００３】ところで、移動体への接近及び追従のため
には、移動物体の現在位置を知る必要がある。例えば、
撮像手段を用いて移動物体を撮影し、撮影画像から移動
体の位置を検出する方法を挙げることができ、ロボット
制御では、ロボットやコンベアの脇にカメラを付設して
コンベア上を搬送されてくるワークの撮影画像からワー
クの搬送方向における位置を検出することができる。

【０００４】

【発明が解決しようとする課題】しかしながら、上記の
方法は、時々刻々と位置が変化する移動物体の撮影画像
を常に得る必要があり、また、画像から移動物体の位置
を検出するために必要な画像処理にかかる時間がある程
度長い場合には制御の遅れが問題となる。

【０００５】例えば、多数のワークが所定の搬送経路を
次々に運ばれて来る場合であって搬送速度がある程度速
い場合には、各ワークについての現在位置をその都度撮
影画像から解析していたのでは時間がかかり過ぎ、ワー
クを捕捉し損なう確率が高くなってしまう。

【０００６】また、複数のワークについてその搬送方向
における位置を検出するためには撮像手段の画角を拡げ
るか、あるいは複数の撮像手段を設置する必要があり、
前者の場合には位置検出の精度不足が問題となり、ま
た、後者の場合には構成の複雑化やコスト上昇等の問題
がある。

【０００７】本発明は、制御遅れ及び構成の複雑化を伴
うことなく、所定の搬送経路に沿って移動される対象物
への接近及び追跡の制御を実現することを課題とする。

【０００８】

【課題を解決するための手段】本発明は上記した課題を
解決するために、搬送基準経路に対して直交する方向に
おいて被搬送物体が置かれた位置の変位量を検出する位
置変位量検出手段と、被搬送物体がその搬送経路におけ
る基準位置を通過したことを検出する基準位置通過検出
手段と、被搬送物体が上記基準位置を通過した時点から
その搬送経路に沿って搬送手段によりどれだけ搬送され
たかを検出する搬送量検出手段とを設けるとともに、こ
れらの検出手段による検出結果を受けて被搬送物体の現
時点での位置を計算する現在位置計算手段と、現在位置
計算手段からの計算結果を受けて制御対象の作用端を被
搬送物体へ接近させ及び／又は追従させるための制御信
号を生成して制御対象に送出する動作制御手段とを設け
たものである。

【０００９】従って、本発明によれば、搬送手段に係る
搬送基準経路については予め明らかであるか若しくは教
示等によって知られているので、搬送基準経路に対して
被搬送物体が置かれた位置の変位量と、被搬送物体が基
準位置を通過した以後の搬送量を検出することによって
被搬送物体の現在位置を逐次に計算して求め、これに対
して制御対象の作用端を接近させたり、あるいはこれに
追従させることができる。

【００１０】

【発明の実施の形態】以下に、本発明被搬送物体への接
近及び／又は追従に係る装置について説明する。

【００１１】先ず、本発明に係る「被搬送物体」とは、
搬送手段によって所定の搬送基準経路（搬送方向に沿っ
て延びる基準曲線）に沿うようにして移動される物体を
いい、搬送基準経路に対して直交する方向における物体
の個々の相対位置はランダムであっても構わないが、そ
の相対位置が所定範囲に亘って変化しないことを前提と
する。即ち、図１に示すように、被搬送物体ｗが搬送手
段ｃによってその搬送基準経路Ｃ１（例えば、搬送路の
中心線等を基準とする。）に沿って運ばれる場合に、搬
送基準経路Ｃ１に対して直交するＲ軸上における被搬送
物体ｗの位置（以下、Ｃ１とＲ軸との交点Ｓを基準とす
る変位量（オフセット）を「Δ」で示す。）が、範囲Ａ
内に進入してから範囲外に出ていくまでの間変化しない
ということである。

【００１２】尚、被搬送物体の姿勢については、球のよ
うな回転対称性を有する場合には問題とならないが、一
般には形状に方向性をもつものについてはその姿勢（配
位）についても範囲Ａ内で変化しないことが必要とな
る。

【００１３】また、搬送基準経路Ｃ１については搬送手
段ｃがコンベアの場合、直線又は円弧状である場合が多
いが、一般には任意の曲線である。

【００１４】そして、図１において制御対象ＳＴ（ロボ
ット等の機械的構造物）の作用端Ｈを被搬送物体ｗに接
近させたり、作用端Ｈで被搬送物体ｗを追いかける動作
を以下では、「トラッキング動作」と呼ぶことにし、該
トラッキング動作は「接近動作」や「追従動作」を含む
ものとする。

【００１５】即ち、「接近動作」とは、被搬送物体ｗに
対して所定範囲内の近さをもって作用端Ｈを近づける動
作であり、両者が位置的に近いだけでなく速度や加速度
についても近いこという。つまり、この場合には被搬送
物体ｗや作用端Ｈについての位置、速度、加速度のベク
トル差の成分を求め、それぞれの値と判定精度を示すベ
クトルの成分との大小関係を比較することによって被搬
送物体ｗと作用端Ｈとの間の運動状態の近さを判断する
ことができる。尚、判定精度を示すベクトルは位置、速
度、加速度毎にそれぞれ指定される。

【００１６】また、「追従動作」とは、接近動作後にお
いてこの状態から所定の精度を保ったままの状態で搬送
物体を追いかける動作であり、追従動作から逸脱した場
合を、「トラッキングアウト」ということにする。

【００１７】そして、このトラッキングアウトと対をな
す「トラッキングイン」とは、トラッキング動作の開始
後に接近動作が完了した状態をいい、トラッキング動作
の開始時点からトラッキングインまでに要する時間を
「トラッキングイン時間」といい、「Ｔ_tr」で表すこと
にする。

【００１８】以上で用語の定義が明らかとなったので、
本発明に係る被搬送物体への接近及び／又は追従に係る
装置の基本的構成について説明する。

【００１９】被搬送物体への接近及び／又は追従に係る
装置１は、図２に示すように、被搬送物体ｗに係る位置
変位量検出手段２及び基準位置通過検出手段３、搬送手
段ｃに係る搬送量検出手段４及び搬送経路教示手段５、
トラッキング動作の制御目標位置を計算する現在位置計
算手段６、制御対象ＳＴの作用端Ｈについての動作制御
を行う動作制御手段７とを備えている。

【００２０】位置変位量検出手段２は、搬送基準経路Ｃ
１に直交する方向における被搬送物体ｗの変位量Δを検
出するために設けられ、その検出結果は現在位置計算手
段６に送出される。この位置変位量検出手段２には、例
えば撮影手段を用いることができるが、これに限らず非
接触式の各種のセンサーを用いることができる。尚、接
触式センサーの使用が許されないのは、検出時の接触後
に被搬送物体の位置や姿勢が変化してしまう虞があるた
めである。

【００２１】基準位置通過検出手段３は、被搬送物体ｗ
が予め決められている基準位置を通過したことを検出す
るものであり、その検出結果は現在位置計算手段６に送
出される。

【００２２】尚、被搬送物体ｗの搬送中の姿勢を検出す
る必要がある場合には搬送姿勢検出手段８を設けて、そ
の検出結果を現在位置計算手段６に送出すれば良い。搬
送中の被搬送物体の姿勢が問題になるのは、被搬送物体
をハンド等の効果器によって所定の状態で確実に捕捉す
る等の必要がある場合である。また、搬送姿勢検出手段
８として撮像手段を用いる場合には、これを上記の位置
変位量検出手段２の撮像手段と同じものとすることによ
って両者を兼用することで構成の簡単化を図ることがで
きる。

【００２３】搬送量検出手段４は、搬送手段ｃによる被
搬送物体の搬送量、つまり、被搬送物体が上記基準位置
を通過してからどのくらい搬送されたかを示す量を検出
するものであり、その検出結果は現在位置計算手段６に
送出される。尚、「搬送量」とは、被搬送物体の搬送経
路が直線状である場合には距離の単位で表されるが、搬
送経路が円若しくは円弧の場合には角度で表すことがで
きるので、必ずしも距離の次元をもった量であるとは限
らない。また、搬送量検出手段４が、搬送姿勢検出手段
８及び位置変位量検出手段２と同一の撮像手段を有し、
該撮像手段による撮影画像に基づいて搬送速度を算出し
てこれに搬送時間を掛けたり、あるいは搬送速度の積分
によって搬送量を算出する場合には、さらに構成が簡単
になる。例えば、撮像手段によって単位時間当たりの被
搬送物体の位置変化を求めたり、搬送経路上の特定距離
を示す基準物を所定の時間間隔でもって撮影することに
よって搬送速度を算出し、これに対する積分処理によっ
て搬送量を求めることができる。また、搬送手段に対す
る速度指令値を得ることができる場合にはこの値に基づ
いて搬送量を求めることができる。

【００２４】搬送経路教示手段５は、搬送基準経路Ｃ１
を前もって装置に教示するものであり、搬送基準経路Ｃ
１が常に固定されている場合には、このような手段は不
要であるが、搬送基準経路Ｃ１が作業によって変更され
たり、あるいは、装置が別の搬送手段に対して設置され
た場合等においては新たな搬送基準経路を装置に予め教
え込んでおく必要がある。尚、搬送経路教示手段５によ
る搬送基準経路についての教示結果は現在位置計算手段
６に送出される。

【００２５】現在位置計算手段６は、被搬送物体ｗが制
御対象ＳＴの動作範囲を示す範囲Ａ内に入ってから制御
対象ＳＴの作用端Ｈを被搬送物体に接近させたりこれに
追従させるための制御についての目標位置を計算して、
これを動作制御手段７に送出する。つまり、位置変位量
検出手段２によって変位量Δが分かっており、搬送基準
経路Ｃ１が搬送経路教示手段５によって教示されている
か又はこれが最初から固定した経路とされているので、
被搬送物体ｗの搬送経路Ｃ２（図１、２に２点鎖線で示
す。）は搬送基準経路Ｃ１に対してほぼ相似的な関係を
有する（平行移動によりＣ１とＣ２とを常に重ね合せる
ことができないことは、例えば、円弧状の経路における
内側回りの軌跡と外側回りの軌跡との比較から明らかで
ある。）。よって、被搬送物体ｗが上記基準位置を通過
してからの搬送量（これを「ｌ」と記す。）が搬送量検
出手段４により時々刻々と検出されているので、搬送経
路Ｃ２上における被搬送物体ｗの現在位置を求めること
ができる。

【００２６】また、現在位置計算手段６は制御対象ＳＴ
の作用端Ｈの現在位置を常に把握しており、この位置か
らトラッキング動作の制御を開始した場合に、被搬送物
体が範囲Ａ内に入っている間に当該制御を完遂すること
ができるか否かを判断し、トラッキング動作が不可能で
ある場合には被搬送物体の深追い等、制御対象に対して
無駄な動作をさせないように制御する。

【００２７】動作制御手段７は、制御対象ＳＴの作用端
Ｈについて被搬送物体ｗに対するトラッキング動作の制
御を行うものであればどのような構成であっても構わな
いが、トラッキング動作制御に係る加減速曲線（あるい
は加減速指令曲線）の生成のためのアルゴリズムが制御
対象の特定の構造に依拠しないことが望ましく、また、
作用端Ｈを静止状態の対象物に接近させる移動制御と、
作用端の運動物体へのトラッキング動作を行う制御とを
統一的に取り扱うことのできるアルゴリズムを採用する
ことが望ましい。というのは両者を完全に分離すること
は動作モードを分けたりモード毎に加減速曲線の基本形
状が異なる等、制御が複雑化することは勿論、両動作間
の遷移を実現することが容易でないためである。

【００２８】尚、その方法、構成については本願出願人
が特願平７−２９７３０６号や特願平７−３０６４４３
号で提案した方法等を用いることができ、以下、その内
容についてやや詳細に説明する。

【００２９】図３に示すように、動作制御手段７は、指
令手段９、トラッキング制御手段１０（制御指令値計算
手段１１、Ｔ_P（ピーク速度到達時間）値決定手段１
２、α（時間軸伸縮用パラメータ）値決定手段１３、捕
捉／接近判定手段１４からなる。）、加減速パターン生
成手段１５、切換手段１６（図ではスイッチの記号で示
す。）から構成されている。

【００３０】尚、制御対象１７については、被搬送物体
への接近動作や追従動作が可能な作用端を有するものに
限られるが、該作用端によって必ずしも被搬送物体を捕
まえたり、被搬送物体に対して何等かの影響を及ぼす必
要はない。被搬送物体への接近動作や追従動作は、作用
端を被搬送物体に対して何ら接触させることなく行い得
る動作だからである。尚、図３では、制御対象を「サー
ボ系」に特定しているが、これは駆動源及び／又は駆動
機構の他、サーボ回路等を含む広い系を意味しており、
単一の部品だけから成る系や、これに付随する制御部品
を含む系を指す。

【００３１】指令手段９は、被搬送物体へのトラッキン
グ動作の指令を発するものであり、該指令は制御指令値
計算手段１１に送られる。尚、指令手段９が発する指令
には、制御対象についての動作を所定の言語に従って記
述したプログラムの解釈結果に基づく指令が含まれる。

【００３２】加減速パターン生成手段１５は、何等かの
制御指令に応じて加減速制御に係る制御パターン（加減
速パターン）を生成するものである。尚、ここにいう
「加減速パターン」とは、加減速曲線によって直接に記
述される状態量（速度や角速度）は勿論、該状態量の時
間微分や積分値の変化等を含む広義の概念である。つま
り、位置や位相角を示す関数の時間による１階微分で定
義される速度関数あるいは角速度関数は、その制御にお
いて加速期間や減速期間（時には定速期間）を含んでお
り、これをグラフ化したものが加減速曲線に他ならない
が、「加減速パターン」という時には、速度又は角速度
の他、速度又は角速度に関連して時間とともに変化する
量（位置や加速度等）を含むものとする。

【００３３】加減速パターンの生成法には、例えば、下
記に示す２つの方法を挙げることができる。（１）関数計算による方法（図４参照。）現実の制御対象の応答特性が、理論的に明確に解析され
ている場合に、制御変数を関数式として表現することが
できれば、加減速パターンの生成対象となる量を予め数
式によって用意することができる。

【００３４】速度を例にすると、図４に示す速度の基準
関数Ω_N（ｔ）に対して「スケーリング操作」を施すこ
とによって加減速曲線を生成することができる。尚、
「スケーリング操作」とは関数値そのものの変倍操作と
関数の時間軸方向の伸縮操作を含む操作である。即ち、
基準関数Ω_N（ｔ）（そのピーク値を仮に１とし、速度
がピーク値に達するまでに要するピーク速度到達時間
（これを「Ｔ_Pn」とする。）に対して制御対象について
の総移動量Δθ（尚、この場合の移動の意味には回転も
含まれており、例えば、モータを例にするとΔθは位相
角の総量に相当する。）を掛けることによって図にΩv
（ｔ）で示すように山の高さを変化させたり、あるい
は、ピーク速度到達時間Ｔ_Pnの基準関数Ω_N（ｔ）に対
してＴ_Pnをα倍することによって図にΩt（ｔ）で示す
ように時間軸方向の伸縮の度合を変化させる操作を基本
として、両操作を組み合わせることにより図にΩv*t
（ｔ）に示すような曲線を得ることができる。尚、時間
軸伸縮用パラメータαの値についてはΔθとの関係にお
いて決定する必要があるが、その説明については後述す
る。

【００３５】このように、制御変数に係る基準関数（又
は関数群）を予め決定しておけば、これらにスケーリン
グ操作を施すことによって加減速パターンに係る関数を
容易に得ることができ、その際の計算については基準関
数の関数値の計算とこれに四則演算を組みあわせた計算
レベルで済むことになる。尚、加減速曲線を構成する加
速部分と減速部分とが時間軸について対称性（線対称性
及び／又は回転対称性）を有する場合には、加速部分又
は減速部分に係る基準関数だけを用意すれば良い。何故
なら、どちらか一方の部分に係る基準関数に対して時間
軸に係る対称化操作を施せば他方の部分に係る関数を容
易に求めることができるからである。（２）仮想サーボ系を用いる方法（図５参照。）ここにいう「仮想サーボ系」とは、現実のサーボ系に対
応した構成をソフトウェア処理によって模擬的に構築し
て制御装置内に用意される系のことであり、これを計算
手段として用いることによって加減速パターンの生成や
各種の推定量の算出に利用することができる。

【００３６】例えば、モータの制御回路には、モータの
制御に直接に関係する回路部分や制御の安定化のための
動的補償に係る部分等がサーボ系内に設けられるが、こ
れらに対応した構成を有する仮想サーボ系（以下、これ
を「仮想モータ系」という。）を、ソフトウェア処理に
よる内部モデルとして制御装置内に用意すれば、仮想モ
ータ系に対して指令を与えた時の応答出力を得て、これ
をそのまま現実のサーボ系の制御に用いることができ
る。

【００３７】また、サーボ系の構成が比較的簡単な場
合、即ち、サーボ系の応答特性が解析式によって表現す
ることができ、計算が簡単な場合には上記（１）のよう
に関数計算による方法が有効であるが、一般には、サー
ボ系の応答特性が常に解析式によって容易に表わされる
とは限らないので、現実のサーボ系を摸した仮想サーボ
系を導入することは、実際的な見地から有用である。

【００３８】図５は、制御対象を、モータ及びその制御
系を含む系（以下、「モータ系」という。）に特定した
場合の現実のモータ系と仮想モータ系の構成の一例を示
すものである。

【００３９】図５（ａ）は現実のモータ系の一例を簡略
化してシグナルフロー線図で示したものであり、図中の
１点鎖線の左側がソフトウェア処理又は回路によって構
築される部分であり、１点鎖線の右側がモータや機械系
の部分である。また、図５（ｂ）は図５（ａ）に対応し
た仮想モータ系を示すシグナルフロー線図であり、全て
ソフトウェア処理により構築したものである。

【００４０】尚、図中の記号の意味を下表１にまとめて
示す。

【００４１】

【表１】

【００４２】ところで、上記仮想モータ系について所定
のアナログ近似（つまり、１−ｚ^-1≒Ｔ_s・ｓ、（１＋
ｚ^-1）／（２・（１−ｚ^-1））≒１／（Ｔ_s・ｓ））を
施した場合に、オープン特性に係る伝達関数が求まり、
この式から位置指令入力Θ_IN（ｓ）に対する位置出力Θ
_OUT（ｓ）の比としての閉ループ伝達関数を得ることが
できる。

【００４３】例えば、図５の例では仮想モータ系が３次
のロ−パスフィルタと等価な構成を有している。尚、仮
想サーボ系は一般にはｎ（ｎは自然数。）次のフィルタ
と等価な構成を有し、閉ループ伝達関数はｎ次多項式を
分母とする有理式で表される。

【００４４】よって、分母式の値をゼロとすることで極
が求まり、例えば、次数３の場合には、ループの時定数
に相当する量を「Ｔ_i（ｉ＝１、２、３）」とすると、
「−１／Ｔ_i」がループの極を示す。そして、図１０
（ｂ）の例では、ｓについての３次の分母式を因数分解
したものが閉ループ伝達関数の分母式に等しいという関
係に基づいて両者を比較することによってＴ_iとサーボ
パラメータ、モータ系のアクチュエータ部のゲインとの
間には下式のような関係があることを示すことができ
る。

【００４５】

【数１】

【００４６】このような仮想モータ系に各種の位置入力
を与えた時の出力によって加減速パターンを得ることが
でき、ステップ信号やランプ信号等を入力信号として用
いることができる。

【００４７】例えば、ステップ信号入力の場合には、加
減速曲線における加速部をｔ＝Ｔ_P（ピーク速度到達時
間）において時間的に折り返すことによって減速部に重
ね合わせることができるようなパターンが得られ、ま
た、ランプ信号入力の場合には、加減速曲線における加
速部に対して１８０゜の回転を施したものｔ＝Ｔ_Pにお
いて時間的に折り返す（あるいは加速部を時間軸に関し
て反転（鏡像変換）させた後に平行移動させる。）こと
によって減速部に重ね合わせることができるようなパタ
ーンが得られる。

【００４８】尚、この場合、ピーク速度到達時間Ｔ
_Pは、加速度がゼロになる時点として求まり、これはΔ
θやＴ_fに無関係な量（Ｔ_i（ｉ＝１、２、３）のみに依
存する。）となる。

【００４９】また、ステップ信号やランプ信号を仮想モ
ータ系に入力した時の出力についての計算式を求めるに
は、ステップ信号やランプ信号についてラプラス変換を
施したものに、仮想モータ系の閉ループ伝達関数を掛け
てから逆ラプラス変換を行えば良いが、それらの計算過
程や結果については省略する。

【００５０】ところで、時間の次元をもつ量（Ｔ_i、
Ｔ_f）をすべてα倍する操作を考えたときに、仮想モー
タ系の出力についての関係式が同型に保たれるという性
質があり、新たなピーク速度到達時間がα・Ｔ_Pとなる
ことが示される。つまり、Ｔ_iをα倍することは、例え
ば、［数１］式におけるサーボパラメータを下式［数
２］のように変換することと等価であり（変換後のパラ
メータは「′」を付して示す。但し、ｇｍ、Ｔ_sについ
ては固定。）、これは仮想モータ系の周波数特性の形を
変えずにループの極配置を変更することである。以下で
は、この変換のことを「α変換」と呼ぶことにする。

【００５１】

【数２】

【００５２】尚、α変換によって、位置、速度、加速度
がどのように変換するかを知ることは実際に計算すれば
容易に分かることであるが、その変換則は一般には入力
信号の如何に依存する。

【００５３】α値を何等かの方法によって決めれば、α
変換によって仮想モータ系の周波数特性に係る極配置を
制御することができることが明らかとなったが、α値が
比の値であることから分かるようにその基準が必要とな
る。

【００５４】そこで、仮想モータ系の基準系を、そのピ
ーク速度到達時間値「Ｔ_PMIN」を用いて定義すると、該
基準系は、図６に示すように、サーボパラメータ
Ｋ_iMIN、Ｋ_PMIN、Ｋ_MINとＴ_PMINとの組み[Ｋ_iMIN，Ｋ
_PMIN，Ｋ_MIN，Ｔ_PMIN]によってその特性を記述すること
ができる。尚、基準系のサーボパラメータＫ_iMIN、Ｋ
_PMIN、Ｋ_MINは、現実のモータ系に係る時定数又はサー
ボパラメータ値に合せて決められる。また、添え字「
_MIN」が各々の値の最小値を意味する場合もあるが、そ
の意味はあくまで基準値であり、当該添え字が必ずしも
最小値を意味しないことに注意を要する。

【００５５】この基準系の導入によって、αは下式のよ
うに定義される。

【００５６】

【数３】

【００５７】つまり、何等かの方法で決定されるＴ_P値
からαを求め、図６に示すように、基準系に対してα変
換を施すことによって仮想モータ系（これは[Ｋ_i＝Ｋ
_iMIN／α²，Ｋ_P＝α・Ｋ_PMIN，Ｋ＝α・Ｋ_MIN，Ｔ_P＝α
・Ｔ_PMIN]によりその特性が特定される。）が得られ、
これに位置指令θ_REFを入力すれば、その出力θ_Oとして
ピーク速度到達時間Ｔ_Pの加減速パターンを得ることが
できる。尚、ロボットのように複数の駆動軸によって構
成される系では、一般には各駆動軸に係るサーボ系に対
応した複数の仮想サーボ系が用いられる。

【００５８】さて、α値の決定のためにはＴ_P値を決定
する必要があり、このためには制御対象についての総移
動量（これを「Δθ」と記す。）とＴ_P値とを関係付け
るために下記に示す２段階を経る。（１）被搬送物体の現在位置と制御対象の作用端の現在
位置から制御対象に係る総移動量Δθを計算するつまり、現在位置計算手段６により被搬送物体の現在位
置が常時算出されるので、制御対象の作用端をその現在
位置からどれくらい移動させれば、作用端の位置を被搬
送物体の位置に一致させることができるかを計算するこ
とができる。

【００５９】図３の制御指令値計算手段１１は、現在位
置計算手段６からの位置情報と、制御対象の作用端の位
置情報に基づいてΔθ等を計算するものである。

【００６０】その計算の具体例について説明するため
に、制御対象を２軸の水平多関節型の構成を有するロボ
ットに特定して、ロボットを平面で見たときの構成を略
線的に表現したものが図７である。

【００６１】ロボットＲＢは第１アームａ１と第２アー
ムａ２とを有しており、図では第１アームａ１の回動中
心Ｏ₁を原点とする２次元の直交位置座標系（Ｘ，Ｙ）
が設定されており、点Ｅ（Ｘ，Ｙ）がロボットＲＢの手
先位置を示している。

【００６２】また、点Ｏ₂は第２アームａ２の回動中心
であり、線分Ｏ₁Ｏ₂の長さ（これを「Ｌ₁」とする。）
が第１アームａ１の長さを示し、線分Ｏ₂Ｅの長さ（こ
れを「Ｌ₂」とする。）が第２アームａ２の長さを示し
ている。

【００６３】そして、「Θ_i（ｉ＝１、２）」は、各ア
ームの駆動軸に係る関節角を表しており、角度「Θ₁」
は線分Ｏ₁Ｏ₂がＸ軸に対してなす角度、角度「Θ₂」は
線分Ｏ ₂Ｅが線分Ｏ₁Ｏ₂の延長線に対してなす角度をそ
れぞれ示している。

【００６４】点Ｍ（ｘ，ｙ）は被搬送物体の位置を示し
ており、ｔｇは被搬送物体の搬送経路（つまり、Ｃ２）
を示している。

【００６５】今、図７に２点鎖線で示すように、仮想ロ
ボットＶＲ、つまり上記第１アームａ１及び第２アーム
ａ２と同等の仮想アームａ１′、ａ２′からなるロボッ
トを想定し、その手先位置が点Ｍにあるとすると、仮想
アームに係る関節角θ_i（ｉ＝１、２）を求めることが
できる。

【００６６】つまり、角度「θ₁」は仮想第１アームａ
１′がＸ軸に対してなす角度、角度「θ₂」は仮想第２
アームａ２′が仮想第１アームａ１′の延長線に対して
なす角度を示しており、これらの角度を手先位置の座標
値（ｘ，ｙ）から計算する問題は逆運動学問題として知
られている。

【００６７】ロボットＲＢについても同様にその手先位
置の座標値（Ｘ，Ｙ）から関節角Θ_i（ｉ＝１、２）を
求めることができるので、関節角の差、「Δθ_i＝Θ_i−
θ_i」（ｉ＝１、２）を求めることができる（尚、その
具体的な数式については省略する。）。

【００６８】尚、制御対象の運動が１次元の場合には、
Δθは被搬送物体の位置と制御対象の作用端の位置との
差から容易に求められるが、制御対象が多自由度となる
ように複数の駆動系から構成されている場合には、各駆
動軸に係るΔθを上記のように計算する必要がある。（２）（１）で得られたΔθからＴ_Pを求める。

【００６９】Ｔ_P値決定手段１２は、制御指令値計算手
段１１によって得られたΔθとＴ_P値との関係を規定す
るものであり、Δθに対応したＴ_P値を決定する。

【００７０】例えば、図８に示すように、横軸にΔθを
とり、縦軸にＴ_Pをとると、Ｔ_P値の下限値及び上限値に
それぞれ対応した点ＭＩＮ（Δθ_MIN，Ｔ_PMIN）、点Ｍ
ＡＸ（Δθ_MAX，Ｔ_PMAX）が制御対象に固有の点として
決まる。つまり、下限値Ｔ_PMINは、制御対象に係るサー
ボ系の周波数特性によって決まる値であり、例えば、上
記［数３］式の基準値Ｔ_PMINをこの下限値に一致させる
ことができる。また、上限値Ｔ_PMAXは制御対象の最大速
度によって規定される。尚、ピーク速度到達時間Ｔ_Pの
下限や上限を考慮して加減速曲線を生成する理由は、制
御対象の能力を越える動作を禁止することによって過負
荷や故障等が起きるのを防止し、制御対象に対して負担
がかからない加減速制御を行うためである。

【００７１】上記点ＭＩＮと点ＭＡＸとの間をどのよう
な形状の線で結ぶかについては、何を評価関数にするか
によって異なるが、その具体的な説明については本発明
の要旨に直接関係がないので省略する。尚、図では、点
ＭＩＮと点ＭＡＸとの間を直線で結んだ特性となってお
り、０≦Δθ≦Δθ_MINの範囲ではＴ_P＝Ｔ_PMIN、Δθ
_MIN≦Δθ≦Δθ_MAXの範囲では直線の傾斜に応じたＴ_P
値（但し、Ｔ_PMIN≦Ｔ_P≦Ｔ_PMAX）、Δθ≧Δθ_MAXの範
囲ではＴ_P＝Ｔ_PMAXがそれぞれ得られることになる。

【００７２】また、この例では一つのΔθに対して一つ
Ｔ_P値が決まるが、制御対象が複数の駆動系から構成さ
れている場合には、各駆動軸に係るΔθ_i（ｉ＝１、
２、・・・）のそれぞれに対応するＴ_Pi（ｉ＝１、２、
・・・）が決まるので、場合によってはそれらの中から
適当なものを選択する操作が必要となる（例えば、最大
のものを選ぶ等。）。

【００７３】以上のようにＴ_P値が求まると、α値決定
手段１３によりαの値を決定することができる（［数
３］式参照。）。尚、α値の決定については、これをト
ラッキング動作の開始時点で決定してそれ以後のα値を
固定する方法と、被搬送物体と制御対象の作用端との間
の関節角差Δθ_iに応じてα値を時々刻々と変化させて
いく方法とを挙げることができるが、本発明の趣旨には
直接関係がないのでその説明を省略する。

【００７４】トラッキングインにおける処理は、制御対
象の作用端による被搬送物体の捕捉や作用端の被搬送物
体への接近を捕捉／接近判定手段１４が判定する（図３
参照。）。即ち、捕捉／接近判定手段１４は被搬送物体
に係る位置、速度、加速度の情報と、制御対象の作用端
に係る位置、速度、加速度の情報とに基づいて、作用端
と被搬送物体との運動状態として近さを判断する。その
際、被搬送物体に係る位置等の情報（速度や加速度は、
位置変化、速度変化から求まる。）は現在位置計算手段
６から得られ、また、制御対象の作用端に係る位置等の
情報は、制御対象の作用端が制御指令値通りに運動して
いる場合には制御指令値から明らかであり、また制御対
象の作用端の運動状態を検出する検出手段が制御対象内
若しくは制御対象とは別個に設けられている場合にはこ
れらによって制御対象の実際の運動を監視することによ
って得ることができる。

【００７５】捕捉／接近判定手段１４による判定出力
は、図３に示すように、加減速パターン生成手段１５と
制御対象との間に設けられた切換手段１６の制御信号と
して用いられる。即ち、切換手段１６は、捕捉／近接判
定手段８の信号を受けて、制御対象の作用端が被搬送物
体を捕捉し又は作用端が被搬送物体に近接したことが判
定されるまでの間は、制御指令値計算手段１１による制
御指令値が加減速パターン生成手段１５を介して制御対
象に送出されるようにし、また、制御対象の作用端が被
搬送物体を捕捉し又は作用端が被搬送物体に近接したこ
とが判定された場合には、制御指令値計算手段１１によ
る制御指令値がそのまま制御対象に送出されるように切
り換える。つまり、トラッキングイン後に被搬送物体の
位置等の情報を制御対象への直接の制御目標値として送
出することによって追従動作の精度を向上させることが
でき、制御の遅れの影響等を排除することができる。

【００７６】追従動作中は、捕捉／接近判定手段１４に
よる判定処理が常時行われ、制御対象の作用端が所定の
精度でもって被搬送物体に追従しているか否かがチェッ
クされる。

【００７７】しかして、図３の動作制御手段７において
は、指令手段９が発するトラッキング動作の開始指令
が、制御指令値計算手段１１に送られると、該制御指令
値計算手段１１は被搬送物体の位置情報や制御対象の作
用端の位置情報に基づいて、制御対象の作用端の位置を
被搬送物体の位置に一致させるために制御対象の作用端
をどれくらい動かせば良いかを計算する。そして、制御
指令値計算手段１１の出力は加減速パターン生成手段１
５やＴ_P値決定手段１２に送出され、Ｔ_P値決定手段１２
は、図９に示したようなΔθ−Ｔ_P特性に基づいてＴ_P値
を求めてこれをα値決定手段１３に送出し、また、α値
決定手段１３はＴ_P値をその基準値で割ることによって
α値を決定してこれを加減速パターン生成手段１５に送
出する。

【００７８】加減速パターン生成手段１５は、制御指令
値計算手段１１やα値決定手段１３からの情報に基づい
て加減速パターンを生成して、該加減速パターンに従う
制御信号を切換手段１６を介して制御対象に送出する。

【００７９】捕捉／接近判定手段１４は、現在位置計算
手段６からの情報や制御対象の作用端の情報に基づいて
トラッキングインについての判定を行い、判定結果に応
じて切換手段１６を切り換えることによって、制御指令
を直接制御対象に送出するか又は加減速パターン生成手
段１５を介して制御対象に送出するかを制御する。ま
た、捕捉／接近判定手段１４は、制御対象の作用端が被
搬送物体に追従しているか否かを監視することによって
トラッキングアウトの判定を行う。

【００８０】以上で、動作制御手段７の基本的な構成が
明らかになったので、以下ではランプ信号入力を用いる
ことによって、上記のようなトラッキング動作と、制御
対象の作用端の静止物への接近／捕捉動作とが制御上如
何にして統一化されるかについてやや詳しく説明する。

【００８１】トラッキング動作に係る加減速パターンの
生成について説明するに先だって、制御対象の作用端を
静止物体又は定位置へと移動させる動作（以下、「定点
間移動」という。）について説明する。その理由はこの
動作が最終的にトラッキング動作の中に吸収されるから
であり、また、被搬送物体への接近動作に比べて簡単で
あるからである。尚、定点間移動の例としては、ロボッ
ト制御において始点と終点の途中経路を指定されずに２
点間でロボットの作用端を移動させるＰＴＰ（Ｐｏｉｎ
ｔＴｏＰｏｉｎｔ）動作等を挙げることができる。

【００８２】図９は定点間移動に係る加減速パターンの
生成の仕方を概念的に示すものであり、上記した仮想モ
ータ系を用いた例を示している。

【００８３】図中の、「θ_REF」はランプ指令信号であ
り、これが仮想モータ系の位置指令に係るノードに与え
られた結果、加減速パターン、つまり、速度出力θ_O ⁽¹⁾
や位置出力θ_Oが得られることになる。

【００８４】ランプ指令信号θ_REFは、その折れ時間Ｔ_f
に達するまでの期間（０≦ｔ≦Ｔ_f）において傾きが一
定の傾斜部を有し、ｔ＞Ｔ_fにおいて一定値Δθとな
り、これが速度θ_O ⁽¹⁾に係るピーク速度到達時間Ｔ_Pだ
け持続する信号である。よって、傾斜部の傾きはΔθ／
Ｔ_fに等しい。

【００８５】また、速度θ_O ⁽¹⁾は、その加速期間の長さ
と減速期間の長さがいずれもピーク速度到達時間Ｔ_Pに
等しくなっており、減速部の形状は加速部の形状に対し
て対称性を有しており、両者は時間軸についての線対称
化操作及び点対称化（回転）操作によってお互いに一致
させることができる。尚、このような時間軸に係る対称
性は、制御対象の加速度−速度線図上の特性についての
対称性に起因しており、よって、一般には加速期間の長
さと減速期間の長さとが一致するとは限らないことに注
意を要する。

【００８６】上記したように加減速パターンの生成にあ
たって、α値の決定のためにはＴ_P値を決定する必要が
あり、図８で説明した内容を定点間移動においても用い
ることができる。

【００８７】Ｔ_P値が求まると、［数３］式に示すよう
に、Ｔ_P値をその基準値Ｔ_PMINで割ることによってα値
を決定することができる。

【００８８】ランプ信号の折れ時間Ｔ_fの決定について
は、ランプ信号の傾斜部の傾きΩが制御対象に係る最高
速（これを「Ω_MAX」とする。）未満の場合にはＴ_f＝Ｔ
_Pとし、Ω＝Ω_MAXの場合には、「Δθ／Ｔ_f＝Ω_MAX」か
ら決まるＴ_f値とする。これを数式で表すと、下式のよ
うになる。

【００８９】

【数４】

【００９０】従って、Ｔ_f値やＴ_P値の決定にあたって
は、図１０（ａ）に示すようにθ−ｔ線図とΔθ−Ｔ_P
線図とを重ね合せたグラフ図を用いると便利である。即
ち、横軸に時間ｔをとり縦軸にθをとってランプ信号を
示すグラフ図と、横軸にＴ_Pをとり縦軸にΔθをとって
両者の関係を規定する特性線を示すグラフ図とを併せた
ものが図１０（ａ）である。

【００９１】図中に太線で示す右上りの半直線Ｌは、Ω
＝Ω_MAX、つまり制御対象に許される最大速度を示す特
性線であり、ランプの傾斜部（傾斜角φ）は半直線Ｌに
重なるか又はその下側に位置するように規制される。

【００９２】図では移動量がΔθ_a、Δθ_b、Δθ_c（Δ
θ_a＜Δθ_b＜Δθ_c）の３つの値をとる場合について示
しており、Δθ_aの場合には、これに対応するＴ_P値がＴ
_PMINであり、ランプ信号の傾斜部の傾きが半直線Ｌの傾
きより小さいことから明らかなようにＴ_f＝Ｔ_PMINであ
る。また、Δθ_bの場合には、これに対応するＴ_P値がＴ
_P＝Ｔ_P（Δθ_b）＝Ｔ_Pbであり、ランプ信号の傾斜部の
傾きが半直線Ｌの傾きより小さいことから明らかなよう
にＴ_f＝Ｔ_Pbである。そして、Δθ_cの場合には、これに
対応するＴ_P値がＴ_P＝Ｔ_PMAXであり、ランプ信号の傾斜
部の傾きが半直線Ｌの傾きに等しいので、Ｔ_f＝Δθ_c／
Ω_MAXとなり、この場合にはＴ_f≠Ｔ_Pである。よっ
て、Δθ＝Δθ_aの場合のランプ信号は、図１０（ａ）
で点Ｏ（０，０）−点Ｐ_a（Ｔ_PMIN，Δθ_a）−点Ｐ_a′
（２・Ｔ_PMIN，Δθ_a）に示すようになり、また、Δθ
＝Δθ_bの場合のランプ信号は、図１０（ａ）で点Ｏ
（０，０）−点Ｐ_b（Ｔ_Pb，Δθ_b）−点Ｐ_b′（・
Ｔ_Pb，Δθ_b）に示すようになり、そして、Δθ＝Δθ_C
の場合のランプ信号は、図１０（ａ）で点Ｏ（０，０）
−点Ｐ_C（Ｔ_f（＝Δθ_c／Ω_MAX），Δθ_C）−点Ｐ_C′
（Ｔ_f（＝Δθ_c／Ω_MAX）＋Ｔ_PMAX，Δθ_C）に示すよう
になる。また、Δθ_a、Δθ_b、Δθ_cのそれぞれの場合
の加減速曲線を示すと図１０（ｂ）のようになり、Δθ
_aやΔθ_bの場合には、加減速曲線が加速部と減速部とか
らなり、Δθ_cの場合には、加減速曲線が加速部とΩ＝
Ω_MAXの定速部と減速部とからなる。図示するようにこ
れらの加減速曲線は減速期間においてゆっくりと停止に
近づいていき、この事は定点間移動における滑らかな停
止制御を意味し、また、これがトラッキング動作におい
ては、制御対象の作用端を被搬送物体に対して忍び寄る
ように接近させる制御を意味することが後に明らかとな
る。

【００９３】尚、上記の説明は制御対象が一の駆動軸を
有する構成であるとしたが、制御系が複数の駆動系から
構成されている場合に、上記したＴ_P値やＴ_f値の決定方
法を拡張することは容易である。

【００９４】例えば、制御対象が２軸の構成となってい
る場合のＴ_P値やＴ_f値の決定法を示すと図１１のように
なる。尚、図１１は図１０と同様に、横軸に時間ｔをと
り縦軸にθをとってランプ信号を示すグラフ図と、横軸
にＴ_Pをとり縦軸にΔθをとって両者の関係を規定する
特性線を示すグラフ図とを併せて示したものであり、図
中に太線で示す右上りの半直線Ｌ１は、第１軸に係る最
大速度（これを「Ω_1MAX」とする。）を示す特性線であ
り、また、これより傾きの大きな半直線Ｌ２は、第２軸
に係る最大速度（これを「Ω_2MAX」とする。）を示す特
性線である。

【００９５】Δθ−Ｔ_P特性は軸毎に異なっており、図
１１に示す折れ線Ｎ１（点ＭＩＮ１（Ｔ_PMIN1，Δθ
_MIN1）、点ＭＡＸ１（Ｔ_PMAX1，Δθ_MAX1）は上下限を
それぞれ示す。）が第１軸に係る特性を示し、折れ線Ｎ
２（点ＭＩＮ２（Ｔ_PMIN2，Δθ_MIN ₂）、点ＭＡＸ２
（Ｔ_PMAX2，Δθ_MAX2）は上下限をそれぞれ示すが第２
軸に係る特性を示している。尚、この例では、Δθ_MIN2
＝Δθ_MIN1、Δθ_MAX2＜Δθ_MAX1、Ｔ_PMIN2＜Ｔ_PMIN1＜
Ｔ_PMAX2＜Ｔ_PMAX1である。

【００９６】第１軸に関する移動量を「Δθ₁」とし、
第２軸に関する移動量を「Δθ₂」とすると、Ｔ_P値につ
いては、Δθ₁に対応するＴ_P値（これを「Ｔ_P1」とす
る。）と、Δθ₂に対応するＴ_P＝Ｔ_PMAX2とを比較する
ことによって後者が選ばれ、また、Ｔ_f値については、
第１軸に係るＴ_f1がＴ_P1に等しく、第２軸に係るＴ_f2が
Δθ₂／Ω_MAX2となり、両者の比較からＴ_f2が選ばれる
ことになり、下式のようになる。

【００９７】

【数５】

【００９８】よって、第１軸に係るランプ信号は図１１
の線分Ｏ−Ｐ１−Ｐ１′に示すようになり、また、第２
軸に係るランプ信号は図１１の線分Ｏ−Ｐ２−Ｐ２′に
示すようになる。尚、点Ｏは原点（０，０）であり、点
Ｐ１（Ｔ_f2，Δθ₁）、点Ｐ１′（Ｔ_f2＋Ｔ_PMAX2，Δθ
₁）、点Ｐ２（Ｔ_f2，Δθ₂）、点Ｐ２′（Ｔ_f2＋Ｔ_PMA
_X2，Δθ₂）である。

【００９９】このようにＴ_P値やＴ_f値の決定法を多数の
駆動軸からなる制御対象に関して一般化することが容易
である。

【０１００】以上に説明した定点間移動に係る制御の手
順をまとめると以下のようになる。（１）制御対象の作用端を目的位置へと移動させるため
の制御指令値の算出つまり、目標位置に係る情報と、制御対象の作用端の位
置情報とに基づいて、両者間の差をゼロにするための制
御指令値（Δθ）を計算する。

【０１０１】（２）加減速曲線に係るピーク速度到達時
間（Ｔ_P）の決定上記（１）の制御指令値と加減速曲線のピーク速度到達
時間との関係を予め規定しておき、制御指令値に対応す
るピーク速度到達時間を決定する。

【０１０２】（３）ランプ信号の折れ時間（Ｔ_f）の決
定即ち、ランプ信号に係る速度が制御対象に係る最大速度
未満の場合にはランプ信号の折れ時間をピーク速度到達
時間と同じ値とし、また、ランプ信号に係る速度が制御
対象に係る最大速度の場合には、（１）の制御指令値を
最大速度で割った時間をランプ信号の折れ時間とする。

【０１０３】（４）時間軸伸縮用パラメータ値の決定ピーク速度到達時間をその基準値で割ることによって無
次元量として定義される時間軸伸縮用パラメータαの値
を決定する。

【０１０４】（５）ランプ信号の生成即ち、（１）の制御指令値や（３）の折れ時間に基づい
てランプ信号を生成する。尚、速度や加速度についてこ
れらの最大値による制限を考慮してランプ信号の傾斜を
補正する必要が生じる場合がある。

【０１０５】（６）加減速パターンの生成つまり、（５）のランプ信号と、（４）の時間軸伸縮用
パラメータに基づいて加減速曲線の山の高さや時間軸方
向の幅を制御することにより加減速曲線を生成して、該
加減速曲線に係る制御信号を制御対象に送出する。

【０１０６】尚、上記（４）の手順は（２）の手順の直
後に位置させても良い。

【０１０７】次に、トラッキング動作の説明に移り、ト
ラッキング動作からみた場合の上記定点間移動が停止物
体へのトラッキング動作と等価であることについて説明
する。

【０１０８】今、制御対象が一の駆動軸を有する場合を
考え、トラッキング動作の開始時点における制御対象の
作用端と被搬送物体との間の距離差を「Δθ_S」とし、
被搬送物体の位置を時間ｔの関数として「θ_T（ｔ）」
とし、また、Δθ_S／Ｔ_fの傾斜を有するランプ信号の傾
斜部を「θ_rmp（ｔ）」、Δθ_Sを基準とした被搬送物体
の位置の変化分（つまり、「θ_T（ｔ）−Δθ_S」）を
「θ^〜（ｔ）」、これをランプ信号に重畳した信号（つ
まり、「θ_rmp（ｔ）＋θ^〜（ｔ）」であり、以下、こ
れを「ランプ変調信号」という。）」を「θ〜
_rmp（ｔ）」と定義する。尚、これらを表にまとめたも
のが下表２である。

【０１０９】

【表２】

【０１１０】図１２は上記に定義した諸量の関係を図示
したものであり、図中の点Ｑにおいて被搬送物体の位置
と制御対象の作用端の位置が一致し、その時点がｔ＝Ｔ
_fである。

【０１１１】ここで、新たに導入されたランプ変調信号
θ^〜 _rmp（ｔ）が上記した定点間移動に係るランプ信号
θ_rmp（ｔ）に対してどのような関係を有するかを理解
するには、被搬送物体が静止している場合を想定すれば
良く、この場合には「θ_T（ｔ）＝Δθ_S」から分かるよ
うに「θ^〜（ｔ）＝０」、つまり、「θ^〜 _rmp（ｔ）＝
θ_rmp（ｔ）」となってランプ信号そのものとなる。即
ち、ランプ変調信号θ^〜 _rmpは被搬送物体の位置変化を
取り入れることによってランプ信号θ_rmpを拡張した信
号であることが分かる。尚、ランプ変調信号θ^〜 _rmpに
ついて注意すべき点は期間ｔ≧Ｔ_fにおいてθ
^〜 _rmp（ｔ）＝θ_T（ｔ）とされる点である。

【０１１２】ランプ変調信号θ^〜 _rmpに関する諸関係を
数式で表すと下式のようになる。

【０１１３】

【数６】

【０１１４】上記のようにランプ変調信号θ^〜 _rmpはラ
ンプ信号θ_rmpを含んでおり、ランプ信号を拡張したも
のとなっていることから、上記定点間移動に係るＴ_fと
トラッキング動作に係るＴ_fとが実は同じであることを
示すことは容易である。

【０１１５】即ち、定点間移動ではランプ信号θ_rmpが
下式のように表されるので、制御対象の作用端の位置が
Δθ_Sに示す位置に一致する時間は明らかにｔ＝Ｔ_fであ
る（［数７］式の第１式の右辺と第２式の右辺とを等し
いとおけば良い。）。

【０１１６】

【数７】

【０１１７】他方、トラッキング動作の場合には、ラン
プ変調信号θ^〜 _rmpに関する［数６］式において第１式
の右辺がθ_Tに等しいとおいた下式からやはりｔ＝Ｔ_fが
得られる。

【０１１８】

【数８】

【０１１９】また、ｔ＝Ｔ_fの時点において、制御対象
の作用端と被搬送物体との間の相対速度（これを「θ
_REL ⁽¹⁾」とする。）を求めてみると、定点間移動の場合
には被搬送物体が静止していると考えれば良いので、下
式のようにθ_REL ⁽¹⁾はランプ信号θ_rmpの傾きに等し
い。

【０１２０】

【数９】

【０１２１】他方、トラッキング動作の場合には、下式
のように計算することができ、やはりθ_REL ⁽¹⁾はランプ
信号θ_rmpの傾きに等しい。尚、制御対象の運動が１次
元であるため、相対速度の向きは正負によって表される
ことに注意すると、［数９］式と［数１０］式とは相対
速度の大きさ及び符号を含めて一致している。

【０１２２】

【数１０】

【０１２３】定点間移動とトラッキング動作との比較か
らＴ_fや相対速度の一致が偶然ではないことは、ランプ
変調信号θ^〜 _rmpがランプ信号θ_rmpを含んでこれを拡張
したものであること、換言すれば、定点間移動とは被搬
送物体が静止している場合にその静止位置に制御対象の
作用端を移動させるトラッキング動作に他ならないこと
から理解することができる。

【０１２４】よって、上記した定点間移動に係る制御に
おいてランプ信号をランプ変調信号に置換すれば、定点
間移動に係る加減速曲線の制御性がトラッキング動作に
おいても同様に実現されることが分かる。例えば、図１
０（ｂ）に示すような定点間移動における滑らかな停止
制御は、トラッキング動作において、制御対象の作用端
と被搬送物体との間の相対速度を徐々にゼロに近づけて
いく制御（つまり、被搬送物体から見た場合に被搬送物
体への接近時に、作用端がゆっくりと被搬送物体に近づ
いてくる。）に相当する。

【０１２５】尚、上記の説明では制御対象を一の駆動軸
のみの系に特定したが、制御対象が複数の駆動軸を有す
る場合にも上記の議論をそのまま一般化することができ
る。何故なら、各駆動軸についてランプ変調信号をそれ
ぞれに独立に定義することができ、それらについて定点
間移動とトラッキング動作との間でＴ_f値や相対速度の
一致を示すことができるからである。

【０１２６】しかして、トラッキング動作に係る制御
は、上記した定点間移動に係る制御を含むものとなる
が、ランプ変調信号のうちランプ信号成分の生成に係る
アルゴリズムについては、定点間移動の場合のアルゴリ
ズムを踏襲することができる。

【０１２７】例えば、Δθ_SからＴ_P値を決定するには、
Δθ_S−Ｔ_P特性を予め規定しておき、Δθ_Sと対応する
ピーク速度到達時間Ｔ_Pを決定すれば良い。但し、その
際、ピーク速度到達時間Ｔ_Pが制御対象の性能を越える
加減速曲線が生成されないように制御対象の能力に基づ
いてピーク速度到達時間Ｔ_Pに係る上限値（制御対象の
最高速度により規定される。）や下限値（制御対象の周
波数特性によって規定される。）を規定する必要がある
ことは勿論である。

【０１２８】また、Ｔ_f値についても定点間移動に係る
制御の場合と同じアルゴリズムに従ってＴ_f値を決定す
ることができる。即ち、ランプ信号に係る速度が制御対
象に係る最大速度未満か否かに応じて、Ｔ_f＝Ｔ_Pと同じ
値とするか、または、Ｔ_f＝Δθ_S／Ω_MAXとするかを決
定する。例えば、図１０（ａ）において決定されるＴ_f
値に基づいて生成されるランプ信号がランプ変調信号θ
^〜 _rmpのランプ信号成分θ_rmpに係る折れ時間であると考
えれば良く、あるいは、図１０（ａ）のランプ信号が静
止物体へのトラッキング動作に係るランプ変調信号であ
ると考えれば良い。

【０１２９】ランプ変調信号θ^〜 _rmpは、ランプ信号θ
_rmpに被搬送物体に係るθ^〜（ｔ）を重畳することで得
られる信号である。尚、θ_T（ｔ）は被搬送物体の位置
情報に基づいてその位置に制御対象の作用端を移動させ
るのに必要な制御対象の構成軸に係る移動量から計算す
ることができる。

【０１３０】上記したようにＴ_P値が求まると、これを
その基準値Ｔ_PMINで割ることでα値を決定することがで
きる。

【０１３１】図１３はランプ信号を用いて加減速パター
ンを生成する場合の動作制御手段７の構成例を示すもの
であり、指令手段９、トラッキング制御手段１０（制御
指令値計算手段１１、Ｔ_P（ピーク速度到達時間）値決
定手段１２、Ｔ_f（ランプ信号の折れ時間）値決定手段
１８、変調信号生成手段１９、α（時間軸伸縮用パラメ
ータ）値決定手段１３、捕捉／接近判定手段１４からな
る。）、加減速パターン生成手段１５、切換手段１６
（図ではスイッチの記号で示す。）から構成されてい
る。

【０１３２】即ち、指令手段９が発するトラッキング動
作の開始指令が、制御指令値計算手段１１に送られる
と、該制御指令値計算手段１１は被搬送物体の位置情報
及び制御対象の作用端の位置情報に基づいて、制御対象
１７の作用端の位置を被搬送物体の位置に一致させるた
めには制御対象の作用端をどれくらい動かせばよいかに
ついての制御値（Δθやθ_i等）や被搬送物体の位置に
対応するθ_Tを計算する。そして、制御指令値計算手段
１１の出力はＴ_P値決定手段１２及びＴ_f値決定手段１８
に送出される。Ｔ_P値決定手段１２は、予め規定されて
いるΔθ−Ｔ_P特性に基づいてＴ_P値を求めてこれをα値
決定手段１３に送出する。そして、α値決定手段１３は
Ｔ_P値をその基準値で割ることによってα値を決定して
これを加減速パターン生成手段１５に送出する。

【０１３３】また、Ｔ_f値決定手段１８は、ランプ信号
に係る速度が制御対象に係る最大速度未満か否かに応じ
て、Ｔ_f＝Ｔ_P又はＴ_f＝Δθ／Ω_MAXによりＴ_f値を決定
して、これを変調信号生成手段１９に送出する。する
と、変調信号生成手段１９は、Ｔ_f値決定手段１８から
のＴ_fや制御指令値計算手段１１からのΔθ、θ_Tに基づ
いて上記したランプ変調信号θ^〜 _rmpを生成してこれを
加減速パターン生成手段１５や切換手段１６に送出す
る。つまり、トラッキング動作の開始時点を起点として
０≦ｔ≦Ｔ_fの期間中は信号θ^〜 _rmp＝θ_rmp＋θ_Tが加減
速パターン生成手段１５に送出され、Ｔ_f≦ｔ≦Ｔ_trの
期間中は信号θ^〜 _rmp＝θ_Tが加減速パターン生成手段１
５に送出され、ｔ＞Ｔ_trでは信号θ_Tが切換手段１６に
送出される。

【０１３４】加減速パターン生成手段１５は、変調信号
生成手段１９やα値決定手段９からの情報に基づいて加
減速パターンを生成して、該加減速パターンに沿う制御
信号を切換手段１６を介して制御対象に送出する。

【０１３５】捕捉／接近判定手段１４は、現在位置計算
手段６からの情報や制御対象の作用端の情報に基づいて
トラッキングインについての判定を行い、判定結果に応
じて切換手段１６を切り換えることによって、ランプ変
調信号θ^〜 _rmpを直接制御対象に送出するか又は加減速
パターン生成手段１５を介して制御対象１３に送出する
かの選択を行う。また、捕捉／接近判定手段１４は追従
動作中において許容範囲内の状態で制御対象の作用端に
より被搬送物体への追従が行われているか否かを監視
し、トラッキングアウトの判定を下す。

【０１３６】尚、図１３ではθ_Tが変調信号生成手段１
９から切換手段１６に送出されるようになっているが、
θ_Tが元来制御指令値計算手段１１において計算される
ものであるから該制御指令値計算手段１１からθ_Tを切
換手段１６に送出するようにしても良い。

【０１３７】以上に説明した動作制御のアルゴリズムは
制御対象の機械的な構造が如何なるものであっても適用
が可能であるという点で汎用性が高いものであり、制御
の流れが制御対象に係る特定の構造に依存しない。

【０１３８】次に、制御対象の動作指令を記述するプロ
グラムの言語仕様について簡単に説明する。

【０１３９】トラッキング動作に関して追加するコマン
ドの数についてはこれをできるだけ少なくすることが好
ましく、必要最小限のコマンドとして下記に示すものを
用いる。

【０１４０】ｉ）トラッキング動作の宣言文ｉｉ）トラッキングインの可・不可等を示す予約語ｉｉｉ）トラッキング動作の開始や終了を示す指示文。

【０１４１】先ず、ｉ）はトラッキング動作を開始する
場合の被搬送物体の状況が被搬送物体毎に異なっている
ので、その旨を被搬送物体の各々について宣言するもの
である。例えば、上記位置変位量検出手段２や搬送姿勢
検出手段８が撮像手段を有する場合には、撮影画像の画
像処理に要する時間を無視する訳にはいかず、被搬送物
体の位置変位量や姿勢をリアルタイムで計測することが
できない。つまり、撮像手段による計測データが利用可
能な状態となるのはある時間を経過した後となり、従っ
て被搬送物体の位置変位量Δや姿勢についても同様であ
る。この場合には、運ばれて来るそれぞれの被搬送物体
についてトラッキング動作で使用する変数を設定して内
部データの計算をさせるために宣言文をプログラム中に
記述する。尚、この宣言文はプログラム中で一回だけ記
述される通常の宣言文とは異なり、被搬送物体毎に宣言
するものであり、変数が動的変化するように繰り返し文
中に挿入する必要がある。

【０１４２】このようにトラッキング動作時における被
搬送物体についての状況や環境を宣言文を使って被搬送
物体毎にプログラム中で宣言する方式は、ユーザーから
の各種の要望に対応し易いという利点がある。例えば、
円形状の搬送経路を有する搬送手段を使いたい場合や、
ターゲットである被搬送物体の形状や面積等を処理の対
象にしたい場合等において、宣言文について規約の変更
等を含む簡易な修正を行うだけで仕様の変更に対処する
ことができる。

【０１４３】次のｉｉ）については、被搬送物体が制御
対象の動作範囲内に進入してくる場合に、被搬送物体に
対するトラッキング動作を今から開始させたとして当該
動作が果たして間に合うか否かをプログラム中で判断す
るのに使用される。例えば、予約語についてのエラーコ
ード値を決めておき、特定のコード値によりトラッキン
グ動作の可・不可等を表せば良い。尚、その判断につい
てはトラッキング動作の開始時点でトラッキングイン時
間を知り、この時間に、作用端が被搬送物体に対して働
きかけるのに要する時間や制御の遅れ時間等を加味した
時間と、被搬送物体の作業範囲内の通過時間とを比較す
ることによって、トラッキングインについての可又は不
可の判定を下すことができる。

【０１４４】ｉｉｉ）については、ある一つのトラッキ
ング動作についてその開始を示す指示文や終了を示する
指示文をプログラム中に明示的に記述することで、どの
範囲がトラッキング動作に関するのかが明瞭となり、こ
れによってプログラムの可読性やデバッグ作業の効率化
を図ることができる。

【０１４５】

【実施例】以下に、本発明をロボット装置に適用した実
施の一例を図１４乃至図２８に従って説明する。

【０１４６】図１４はロボット装置２０のハードウェア
構成の要部を概略的に示すものであり、被搬送物体であ
るワーク２１、２１、・・・が、直線状の搬送経路を有
するコンベア２２によって次々と運ばれて来る（コンベ
ア２２の搬送方向を矢印Ｃで示す。）。尚、本実施例で
はワークをロボットハンドで掴むときのワークの姿勢も
問題とするのでワークの形状を２等辺３角形で簡略化し
て示し、その２つの等辺で挟まれた頂角の向く方向によ
ってワークの姿勢を表すことにする。

【０１４７】撮像部２３は、上記した位置変位量検出手
段２及び搬送姿勢検出手段８に相当し、図１５に示すよ
うに、コンベア２２の搬送基準経路を示す中心線Ｃ１を
基準としてワーク２１が搬送方向に対して直交する方向
にどれだけ離れて位置するかを示す変位量Δと、ワーク
２１の方位角（中心線Ｃ１に平行な直線に対してワーク
２１の向きを示す直線Ｄがなす角度であり、これを「Δ
Φ」と記す。）を検出するものである。該撮像部２３
は、カメラ２４と、そのコントローラ２５、光センサー
２６から構成され、カメラ２４によるワーク２１の撮影
画像はコントローラ２５を介してロボット制御装置２７
に送出される。また、光センサー２６はワーク２１がカ
メラ２４の撮影範囲内においてカメラ２４の手前に来た
ことを知らせるトリガー信号を発するために設けられて
おり、該信号はコントローラ２５に送出される。

【０１４８】尚、図１４ではカメラ２４がコンベア２２
の中心線Ｃ１の真上であってコンベアの搬送面から所定
の距離をもって離間したところに設置されているが、要
はワーク２１の変位量Δや方位角ΔΦを検出することが
できれば良く、また、撮像部２３の形態についてもカメ
ラにコントローラが内蔵されたのものやカメラとコント
ローラとが分離されたものを用いることができる。

【０１４９】ロータリーエンコーダ２８は上記搬送量検
出手段４に相当し、例えば、コンベア２２の駆動モータ
の回転角を計測するものであり、計測結果はロボット制
御装置２７に送出されてカウントされる。

【０１５０】また、光センサー２９は上記基準位置通過
検出手段３に相当し、ロボット３０の手前に位置されて
おり、その信号は上記ロータリーエンコーダ２８による
カウント動作のトリガー信号として用いられ、ワーク位
置の原点を示すものである。ロボット３０としては、例
えば、多関節型ロボットを用いることができるが、その
構成の如何は問わない。尚、図に２点鎖線で示す範囲３
１はロボット３０の作業範囲を示しており、当該範囲３
１内においてロボットハンド３２のワーク２１に対する
トラッキング動作が行われる。

【０１５１】ロボット制御装置２７は、上記の撮像部２
３やロータリーエンコーダ２８、光センサー２９からの
情報に基づいて、ロボット３０の動作制御を行うもので
あり、ロボット動作についてはトラッキング動作の他、
ＰＴＰ動作、ＣＰ（ＣｏｎｔｉｎｕｏｕｓＰａｔｈの
略であり、ロボットハンドを指定した軌跡に沿って移動
させることをいう。）動作等が含まれる。そして、ロボ
ット制御装置２７内には、ＣＰＵ（中央処理装置）ボー
ド３３、トラッキング制御用ボード３４、サーボコント
ロール用ボード３５が設けられており、撮像部２３から
の情報がＲＳ−２３２Ｃ等のシリアルインターフェース
を介してＣＰＵボード３３に送られ、また、ロータリー
エンコーダ２８や光センサー２９の出力がトラッキング
制御用ボード３４に送られる。

【０１５２】トラッキング制御用ボード３４は、ロータ
リーエンコーダ２８や光センサー２９への電源供給を行
ったり、光センサー２９からの信号を原点信号としてロ
ータリーエンコーダ２８の出力をカウントする役割を受
け持っている。尚、このボードは、ロボット３０を構成
するサーボ系の制御を担当するサーボコントロール用ボ
ード３５と同様にＣＰＵボード３３内のＣＰＵに対して
所定時間毎の割込み信号を送出するようになっており、
サーボコントロール用ボード３５と対等な関係とされ、
ＣＰＵボード３３内のＣＰＵから見るとあたかも２台の
ロボットを制御しているかのような構成になっている
が、トラッキング動作に係る制御部はＣＰＵから見て一
つのデバイスとして扱うことができるようにすることが
コスト等の観点から好ましいので、そのような構成を採
用することも勿論可能である。

【０１５３】これらのＣＰＵボード３３、トラッキング
制御用ボード３４、サーボコントロール用ボード３５は
いずれもバス３６に接続されており、該バス３６には、
この他、ロボット３０の動作指示を行ったり、ユーザー
からの指示を受けるための教示／入力部３７（ティーチ
ングペンダント等）を接続するためのインターフェース
部３８や、ロボットの動作手順の記述に係るロボット
（動作）プログラムや、ロボットシステムの記述（例え
ば、ロボットの各軸の動作を決定する座標変換等の記
述）に係るシステムプログラム等を記憶するためのメモ
リを有するメモリボード３９（これはＣＰＵボード３３
内に設けることもできる。）、Ｉ／Ｏポート等が接続さ
れる。

【０１５４】上記した現在位置計算手段６や、動作制御
手段７等は、各ボードのもつ機能の結合によって実現さ
れるため、これらの手段にそれぞれ各別に対応するボー
ド等が存在する訳ではなく、全てソフトウェア処理によ
って実現される。

【０１５５】図１６はソフトウェア処理の構成を視覚化
して示すものであり、ＣＰＵボード３３では、ホストコ
ンピュータとしてのシステムの基本的機能を実現するた
めのソフトウェア４０によりユーザーの作成したロボッ
トプログラム４１が読み込まれ、コマンド等の解釈や実
行等が行われる。

【０１５６】ロボットプログラム４１は、ロボットの作
業内容に応じた動作に係るコマンド等を、所定のロボッ
ト言語に従って記述したものであり、本実施例における
ロボット言語の仕様には、前記したｉ）の宣言文として
「ＴＲＡＣＫ」、ｉｉ）の予約語として「ＴＲＣＫＣＨ
ＥＲＲ」、ｉｉｉ）の指示文として「ＤＯＴＲＡＣＫ
ＯＮ／ＴＲＡＣＫＯＦＦ」が用意されている。

【０１５７】宣言文「ＴＲＡＣＫ」は、下表３に示すコ
マンド仕様となっている。

【０１５８】

【表３】

【０１５９】尚、上表中の各パラメーターの意味を図１
７を用いて説明すると（図のｘ軸、ｙ軸は２次元直交座
標軸を示す。）、「ＰＯＮ１」、「ＰＯＮ２」は、ロボ
ット３０の作業範囲３１を示す境界線とコンベア２２の
中心線との交点Ｐ１、Ｐ２を教示したときのポイントの
データであり、「ＰＯＮ１」がワーク２１の作業範囲３
１への進入点Ｐ１に係る位置データ、「ＰＯＮ２」がワ
ーク２１の作業範囲３１からの脱出点Ｐ２に係る位置デ
ータをそれぞれ示している。また、「ＰＯＮ３」はワー
ク２１を把持する時のロボットハンド３２の姿勢を教示
したポイントＰ３の位置データである。

【０１６０】「Ｌｓ」は光センサー２９により検出され
る基準位置、つまり原点Ｐ０からＰ１までの間における
コンベア２２の中心線の長さである。

【０１６１】「Ｖ」はコンベア２２の送りの速さをコン
ベアの駆動モータに送出する１パルス当たりの移動距離
で表したものであり、また、「Ａｃｙ」はトラッキング
インの判定に係る精度を示しており、Ａｃｙ＞Ｖとされ
る。

【０１６２】尚、ワーク数の上限である「Ｔｎ＿ｍａ
ｘ」については、線分Ｐ１Ｐ２の長さを「Ｌｃ」とした
とき、これを搬送方向におけるワーク２１の長さ「ｌ
ｔ」若しくは最短長等で割った数程度とする。

【０１６３】ところで、ワーク２１のコンベア２２上の
位置は変位量Δによって表され、これがゼロであればワ
ークがコンベアの中心線Ｃ１に沿って運ばれて行くが、
一般にはΔがゼロでないので、ワーク２１は中心線Ｃ１
に対して平行であってこれからΔの距離だけ離れた直線
経路Ｃ１′に沿って運ばれて行くことになる。

【０１６４】つまり、コンベア上での各ワークの位置は
常に不特定であって、どの位置で運ばれて来るかは撮像
部２３からの情報から変位量Δを知ったときに初めて経
路Ｃ１′をワーク毎に予測することができる。そのため
に、直線経路Ｃ１′と作業範囲３１の境界線との交点を
それぞれ「Ｐ１′」（入り口側）、「Ｐ２′」（出口
側）とし、直線経路Ｃ１′において点Ｐ３に対応する点
（つまり、点Ｐ３を通り中心線Ｃ１に対して直交しかつ
コンベア２２の幅方向に延びる直線とＣ１′との交
点。）を「Ｐ３′」、直線経路Ｃ１′において原点Ｐ０
に対応する点（つまり、点Ｐ０を通り中心線Ｃ１に対し
て直交しかつコンベア２２の幅方向に延びる直線とＣ
１′との交点。）を「Ｐ０′」とし、線分Ｐ０′Ｐ１′
の長さを「Ｌｓ′」とした場合に、Δの値に応じてこれ
らの位置や長さをワーク毎に特定するための計算が必要
である。

【０１６５】このような計算が可能であることを理解す
るのは容易であり、例えば、点Ｐ０′や点Ｐ３′につい
ては、点Ｐ０や点Ｐ３（これらやＬｓは次の教示が行わ
れるまでの間固定された点である。）を、変位量Δに係
るベクトル（つまり、大きさがΔであってコンベア２２
の幅方向を向いたベクトル。）を移動ベクトルとして平
行移動させれば良い。また、点Ｐ１′や点Ｐ２′等につ
いては、ロボットの作業範囲の境界線の形状が予め分か
っているので、経路Ｃ１′と境界線との交点を求めれば
良く、例えば、境界線が円形状をしている場合には、経
路Ｃ１′を表す直線の方程式と、円を示す方程式とを連
立させて交点を求める問題を解くことに他ならない（具
体的な計算には興味がないのでこれを省略する。）。
尚、作業範囲の境界線が複雑な形状をしている場合には
交点の算出が厄介になるだけであって、本質的な困難を
伴うものではない。

【０１６６】このようにして、Ｐ１′、Ｐ２′、Ｌｓ′
等を知ることができることを明らかにし、その旨を所定
の命令解釈手段に伝えるために宣言文ＴＲＡＣＫがプロ
グラム中に記述される訳である。

【０１６７】尚、本実施例では、点Ｐ１や点Ｐ２をコン
ベア２２の中心線Ｃ１とロボット３０の作業範囲３１と
の交点に指定したが、必ずしもこのように限定された交
点を選ぶ必要はなく、例えば、コンベアの中心線上であ
って作業範囲の任意の２点（例えば、作業範囲内に属す
る円や四角形と中心線との交点等）を選ぶことはいっこ
うに構わない。

【０１６８】ワーク２１の現在位置、つまり、経路Ｃ
１′上の点Ｐ′の位置は、図１８に示すように、線分Ｐ
０′Ｐ′の長さを「ｌ′」とし、線分Ｐ１′Ｐ２′の長
さを「Ｌｃ′」としたとき、下式のベクトル方程式に従
って計算することができる。

【０１６９】

【数１１】

【０１７０】尚、上式では、点Ｐ、Ｐ１、Ｐ２、ｌ、Ｌ
ｓ、Ｌｃ等に付すべき「′」を省略することによって記
法を簡略化しており、この式は点Ｐ１′や点Ｐ２′の位
置やＬｓ′、Ｌｃ′、ｌ′（ロータリーエンコーダ２８
及び光センサー２９により計測される搬送距離。）に基
づくワーク位置の方程式である。

【０１７１】また、「ｌ′」の基準点を点Ｐ１′に選ぶ
ことも可能であり、この場合には、［数１１］式でＬｓ
＝０とすれば良い。

【０１７２】しかして、以上の事項を概念的にまとめる
と、図１９に示すように、変位量Δから経路Ｃ１′やこ
れに係るパラメータが決まり、「ＴＲＡＣＫ」による各
ワークの状況の宣言及び命令解釈手段４２によるその解
釈、搬送距離ｌ′の計測によってワークの現在位置がワ
ーク毎に各別に計算されることになる。

【０１７３】次に、予約語「ＴＲＣＫＣＨＥＲＲ」を、
図２０を参照しながら説明すると、そのコード値として
は、Ｌｓ′やＬｓ′＋Ｌｃ′とｌとの比較結果に応じ
て、例えば、下式のような仕様とする。

【０１７４】

【数１２】

【０１７５】尚、ここで、「追尾不能」とは、ワークが
ロボットの作業範囲内に存在しないことを示し、「追尾
可能」とは、ロボットの作業範囲内においてワークの追
尾が可能であることを示している。

【０１７６】また、「追尾不可」とはワークに対するト
ラッキング動作の開始を禁止することを意味し、この中
には最初からトラッキング動作が不可能である場合と、
一旦ＴＲＣＫＣＨＥＲＲ＝０とされた後、作業範囲内で
のワークに追いつけないと判断された場合との両方を含
む。前者の例としては、図２０に示すように、ワーク位
置の変位量Δが大きいためにワーク２１の搬送経路Ｃ
１′′がロボットの作業範囲の周辺部に位置し、ワーク
が作業範囲内を短時間で通過してしまうような場合が含
まれ、また、後者の例としては、ロボットハンドがワー
クから離れたところにあるためにワークへの追尾が間に
合わない等の場合である。

【０１７７】尚、この「ＴＲＣＫＣＨＥＲＲ」について
は必ずしも予約語にする必要はなく（通常の変数として
も良い。）、また、［数１２］式のようなコード値の場
合分けではなく搬送距離ｌ′を直接的に示す予約語等
（例えば、実変数の予約語「ＴＲＧＤＩＳＴ」に搬送距
離ｌ′をセットする等。）を用いることができる。

【０１７８】図２１はロボットプログラム内において
「ＴＲＣＫＣＨＥＲＲ」を条件文「ＩＦ」とともに用い
た例を示している。

【０１７９】この場合、ロボットプログラムにはＴＲＣ
ＫＣＨＥＲＲ＝０、つまり追尾可能である場合に、プロ
グラム中の（Ａ）部の内容（トラッキング動作を含
む。）を実行するように記述されているので、命令解釈
手段４２は、ＴＲＣＫＣＨＥＲＲ＝０の条件が満たされ
る場合に（Ａ）部の命令内容を解釈してこれを実行に移
すが、ＴＲＣＫＣＨＥＲＲ＝０の条件が満たされない場
合には（Ａ）部の処理を飛び越えて次の処理に進む。
尚、ＴＲＣＫＣＨＥＲＲが０でない場合には、［数１
２］式から分かるようにＴＲＣＫＣＨＥＲＲ＝３、つま
り追尾不可の場合があるので、その数をカウントする変
数を設定すると、追尾し損なったワークの総数を知るこ
とができる。

【０１８０】ｉｉｉ）の指示文については、「ＤＯＴ
ＲＡＣＫＯＮ」がプログラム中におけるトラッキング動
作の開始を明示的に記述するものであり、例えば、それ
までＰＴＰ動作を行っていたとしても、この指示文以降
の部分はトラッキング動作に係るものと解釈される。
尚、このことは、ＣＰＵボード３３からトラッキング制
御用ボード３４に通知される。

【０１８１】プログラム中に「ＤＯＴＲＡＣＫＯＮ」
を発見した場合には、上記した予約語ＴＲＣＫＣＨＥＲ
Ｒにコード値がセットされるので、これ以降は上記した
ようにＴＲＣＫＣＨＥＲＲを含む条件文を使った処理が
中心的になる。

【０１８２】また、「ＤＯＴＲＡＣＫＯＦＦ」は、プ
ログラム中におけるトラッキング動作の終了を明示的に
記述するものであり、例えば、それまで行っていたトラ
ッキング動作が、この指示文以降の部分はその他の動作
（ＰＴＰ動作、ＣＰ動作等）に係るものとして解釈され
る。尚、このことは、やはり、ＣＰＵボード３３からト
ラッキング制御用ボード３４に通知される。

【０１８３】図２２はロボットプログラムにおいて「Ｄ
ＯＴＲＡＣＫＯＮ」と「ＤＯＴＲＡＣＫＯＦＦ」と
の間に（Ｂ）部が記述されている例を示している。

【０１８４】命令解釈手段４２は「ＤＯＴＲＡＣＫＯ
Ｎ」を見つけてから「ＤＯＴＲＡＣＫＯＦＦ」を見つ
けるまでの部分をトラッキング動作に係る命令であると
解釈するので、（Ｂ）部についてトラッキング動作を行
うが、「ＤＯＴＲＡＣＫＯＦＦ」の以降に記述された
部分については次の「ＤＯＴＲＡＣＫＯＮ」を見つけ
るまでの間、トラッキング動作以外の動作（ＰＴＰ動
作、ＣＰ動作等）に係る命令であると解釈される。尚、
これらの指示文をプログラムに記述するに先だって上記
宣言文「ＴＲＡＣＫ」が記述されていなければならない
ことは言うまでもない。

【０１８５】以上はＣＰＵボード３３に係るソフトウェ
ア処理であるが、図１６においてサーボコントロール用
ボード３５やトラッキング制御用ボード３４に対してそ
れぞれのソフトウェア４３、４４が設けられることは勿
論である。

【０１８６】例えば、サーボ制御に係るソフトウェア４
３については、上記したトラッキング動作とＰＴＰ動作
とを統一化するアルゴリズムに従ってフローチャートを
作成して所定のプログラム言語を使ってプログラムを書
き、これをコンパイルして実行可能な形式にしたものの
集合である。この他、座標変換処理に関するソフトウェ
アも含まれる。これは、ワーク位置にロボットハンドを
移動させるためには、ワークの位置座標系とロボット座
標系との間の座標変換処理が必要であり、例えば、目標
物であるワークの位置にロボットハンドを移動させるた
めにはどれだけの関節角でもってロボットアームを動か
す必要があるかを計算する問題（逆運動学問題）を解く
必要があり、また、逆にロボットアームの関節角が与え
られた場合に、ロボットの手先位置（ハンドの先端位
置）を計算する問題（所謂、順運動学問題）について同
次変換行列を使った座標変換の計算が必要となる。

【０１８７】トラッキング制御用ボード３４について
は、図２３に概略的に示すように、ＣＰＵ４５、ロータ
リーエンコーダ２８の出力をカウントするカウンタ４
６、デュアルポートＲＡＭ４７が設けられており、上記
したＣＰＵボード３３内のＣＰＵ３３ａはＣＰＵ４５に
対してはホストとなる。

【０１８８】ＣＰＵ４５にはインターバルタイマー割込
みがかけられる（図には時計の図形で示す。）ととも
に、ワーク２１が光センサー２９を横切ったときに光セ
ンサー２９からの信号に基づく割込み信号が入って来る
ようになっており、後者の割込みのタイミングでもって
カウンタ４６の値を読み出してこれをリングバッファに
格納する。

【０１８９】尚、ここで「リングバッファ」とは、図２
４に示すように、データをポインタによって管理するＦ
ＩＦＯ（ＦｉｒｓｔＩｎＦｉｒｓｔＯｕｔ）バッ
ファであり、先頭番地（図では０番地）と最終番地（図
では２５５（ＦＦｈ）番地）とが環状に繋がっており、
「ヘッダーポインタ」（これを「Ｈｐ」と記す。）及び
「カレントポインタ」（これを「Ｃｐ」と記す。）とい
う２つのポインタを使ってデータを格納したり取り出し
たりすることができる構造を有している。つまり、ヘッ
ダーポインタＨｐは次にデータを格納する番地を常に差
し示しており、新データの格納指示が来るとヘッダーポ
インタＨｐの指す番地に新データが格納され、ヘッダー
ポインタ自身は次の格納番地を指すようにインクリメン
ト（格納番地について１単位のメモリ分だけ番地をデー
タ未格納の番地方向にずらすこと。）される。一方、カ
レントポインタＣｐは処理対象となるデータの格納番地
を指しており、「ＤＯＴＲＡＣＫＯＦＦ」によりイン
クリメントされる。よって、リングバッファにおけるデ
ータの有無はヘッダーポインタとカレントポインタとが
一致するか否かによっ判断することができ、不一致の場
合にデータが有り、一致する場合にはデータが無いこと
になる。また、ヘッダーポインタのインクリメント後に
これがカレントポインタに一致したらオーバーフローで
ある。

【０１９０】リングバッファへのカウント値の格納操作
は、ワーク２１が光センサー２９を横切る度に行われ
る。そして、インターバルタイマー割込みによってカウ
ンタ４６の値を読み、この値からリングバッファにおい
てカレントポインタＣｐの指す値を引くと、ワーク２１
が光センサー２９を横切ってからどれだけ搬送経路上を
運ばれたか（つまり、搬送距離ｌ′）が求まることにな
る。この引き算値をデュアルポートＲＡＭ４７に書き込
むことによって、ＣＰＵ３３ａはこれを読み出すことが
できる。

【０１９１】あるワークへのトラッキング動作が終了す
ると、その旨がＣＰＵ３３ａからデュアルポートＲＡＭ
４７を介してＣＰＵ４５に知らされるので、その時点で
リングバッファのカレントポインタＣｐをインクリメン
トして次のワークに対して上記した操作が繰り返され
る。

【０１９２】図２５はトラッキング制御用ボード３４に
おけるソフトウェア処理について説明するものであり、
２つのフローチャート図とリングバッファを示す図とを
併せて示すものである。

【０１９３】図２５の左側のフローチャート図はインタ
ーバルタイマー割込みにおける処理の主要部を示してお
り、ステップＳ１でカウンタ４６の値を読み出した後、
ステップＳ２においてデュアルポートＲＡＭ４７に書き
込まれたＴＲＡＣＫＯＮ／ＴＲＡＣＫＯＦＦ（つまり、
トラッキング動作の開始／終了）を示すのフラグ若しく
は所定ビット値を読み出して、次ステップＳ３ではこれ
がＴＲＡＣＫＯＦＦを示すか否かを判断し、そうである
場合にはステップＳ４に進んでリングバッファについて
のカレントポインタＣｐのインクリメントを行って次の
目標であるワークについてのトラッキング動作に移行
し、またＴＲＡＣＫＯＮの場合にはステップＳ５に進
む。

【０１９４】ステップＳ５ではカウント値から、カレン
トポインタＣｐが差し示すリングバッファの格納値を引
くことによって搬送距離ｌ′を求めた後、この値を次ス
テップＳ６でデュアルポートＲＡＭ４７に書き込んだ後
リターンする。

【０１９５】図２５の右側のフローチャート図は光セン
サー２９による割込み処理を示しており、ステップＳＳ
１でカウンタ４６の値を読み、次ステップＳＳ２でこの
値をリングバッファに格納し、ＳＳ３でリングバッファ
のヘッダーポインタＨｐをインクリメントした後リター
ンする。

【０１９６】図２６及び図２７は上記の処理を図示によ
り分かり易くするためのものであり、図２６の上段のグ
ラフ図は横軸に時間ｔ、縦軸に搬送距離ｌ′をとり、各
ワークｗ１、ｗ２、・・・の搬送の様子を示し、下段に
は上段のグラフに対するカウント値（これを「ＣＮＴ」
と記す。）の時間的変化を示している。

【０１９７】この例では、各ワークｗ１、ｗ２、・・・
のそれぞれの位置変化を示す傾斜直線ｇ１、ｇ２、・・
・が時間軸ｔに対してなす角度φが一定しており、これ
がコンベア２２の搬送速度Ｖに相当する。そして、傾斜
直線ｇ１、ｇ２、・・・と時間軸とのそれぞれの交点Ｑ
１、Ｑ２、・・・は、各ワークが光センサー２９を過る
位置を示しており、上述したようにこれらのタイミング
でＣＰＵ４５への割込みが発生して、当該時点のカウン
ト値が読み出されてリングバッファに格納される。尚、
カウンタ４６のカウント最大長Ｎは、光センサー２９の
位置と作業範囲３１からワークが脱出する点Ｐ２の位置
との間の距離を、上記精度Ａｓｙで割った値以上の値に
設定される。また、図中の「ｄ」はコンベア２２の搬送
方向におけるワークｗ３とｗ４との間隔を示している。

【０１９８】図２７は２つの光センサー及び２つのリン
グバッファを用いてワークの搬送距離ｌ′を算出する様
子を概念的に示すものであり、最上部にインターバルタ
イマー割込みの周期を示すサンプリング信号「Ｔｓ」、
その下に２つの光センサーのそれぞれの出力信号「ＯＬ
１」、「ＯＬ１′」（信号の立ち上がりエッジによりワ
ークを検知する。）、さらにその下にカウント値ＣＮＴ
の時間的変化を示している。

【０１９９】２つのリングバッファＬＢ１、ＬＢ２のう
ちの一方ＬＢ１には、光センサー出力信号ＯＬ１の立ち
上がりエッジのタイミングでＣＰＵ４５に割込みがかか
り、その割込みサービスによりカウント値（ＣＮＴ１、
ＣＮＴ２参照。）が格納される。同様に他方のリングバ
ッファＬＢ２には、光センサー出力信号ＯＬ１′の立ち
上がりエッジのタイミングでＣＰＵ４５に割込みがかか
り、その割込みサービスによりカウント値（ＣＮＴ
１′、ＣＮＴ２′参照。）が格納される。

【０２００】カウント値ＣＮＴやリングバッファの格納
値の読み出しは、信号Ｔｓによる割込みによって行わ
れ、カウント値ＣＮＴからリングバッファのカレントポ
インタＣｐの指す番地の格納値を引いた値（図２７では
差動器ＤＩＦ１、ＤＩＦ２により概念的に示す。）が求
める搬送距離ｌ′である。

【０２０１】尚、リングバッファに係るオーバーフロー
処理については格納されたデータの一部が捨てられるた
めに、本来あるべきワークがコンベア上に存在しないこ
とになってしまうので、その数を所定のカウンタで計数
しておくことが好ましい。

【０２０２】また、リングバッファについてはワークの
位置変位量Δや方位角ΔΦについても用いることがで
き、当該リングバッファの操作については上記カウント
値の格納用リングバッファとの間で完全な同期をとる必
要があることは勿論である。

【０２０３】尚、リングバッファを用いる理由は、ワー
クとその情報（ロータリーエンコーダ出力のカウント値
や変位量Δ、方位角ΔΦ）との対応関係や順番を間違え
ることなく正確に管理するのに簡便な方法であることに
依る。よって、リングバッファを使う方法以外の方法
（例えば、カウント値や変位量Δ、方位角ΔΦを一組み
のデータとしてメモリ上に記憶する方法等）を用いる場
合には、これらのデータとワークとを関係を正確に関連
付ける必要がある。

【０２０４】しかして、上記のロボット装置２０におけ
るロボットプログラムの一例を示すと下記のようなイメ
ージになる。 −−ロボットプログラム−−−−−−−−−−−−−−−−−−−−−−−− １：ＩＮＴＰａｓｓ＿ｎｏ；追尾失敗若しくは不可能なワーク；数のカウント変数宣言 … ２：ＴＲＡＣＫ＿ＳＴＡＲＴ：；開始位置を示すラベル … ３：ＴＲＡＣＫ（．．．）；宣言文４：ＴＲＡＣＫ＿ＭＡＩＮ：；ラベル５：Ｐａｓｓ＿ｎｏ＝Ｐａｓｓ＿ｎｏ＋１；最初は追尾失敗と仮定６：ＤＯＴＲＡＣＫＯＮ；トラッキング動作開始７：ＤＯＰＯＮ３（Ｚ＝０）；ロボットハンドを上げて；ワークを追尾８：ＩＦＴＲＣＫＣＨＥＲＲ＜＞０；追尾不可又は不能ならＧＯＴＲＡＣＫ＿ＥＮＤ；何もせずに；ＴＲＡＣＫ＿ＥＮＤへ９：ＤＯＰＯＮ３；ロボットハンドを上げる１０：ＩＦＴＲＣＫＣＨＥＲＲ＜＞０；追尾不可又は不能ならＧＯＴＲＡＣＫ＿ＥＮＤ；何もせずに；ＴＲＡＣＫ＿ＥＮＤへ１１：ＤＯＬ１（ＯＮ）；ロボットハンドを閉じる１２：ＤＯＰＯＮ３（Ｚ＝０）；追尾しながらロボット；ハンドを上げる１３：Ｐａｓｓ＿ｎｏ＝Ｐａｓｓ＿ｎｏ−１；追尾成功によるカウント；減数１４：ＴＲＡＣＫ＿ＥＮＤ：；終了位置を示すラベル１５：ＤＯＴＲＡＣＫＯＦＦ；トラッキング動作終了；ＴＲＣＫＣＨＥＲＲ；＝１をセット１６：ＤＯＰＡ；掴んだワークを所定；位置に移動１７：ＤＯＬ１（ＯＦＦ）；ロボットハンドを開く … −−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−− 上記プログラム中において左端の数字は説明の便宜上付
した行番号であり、また、「；」の右側が注釈部分であ
り、その内容から明らかなようにロボットハンドによっ
てワークへの接近／追従を行った後当該ワークを掴ん
で、これを別の場所に移送してからをワークを離す作業
を行う例を示している。この場合には、行番号７乃至１
４までの間の命令等がトラッキング動作に係るものとさ
れ、行番号１６ではその他の動作（ＰＴＰ動作等）とさ
れる。尚、「…」はプログラムの記述についての部分的
な省略を示す。

【０２０５】また、追尾失敗若しくは不可能なワーク数
をカウントした結果（つまり、Ｐａｓｓ＿ｎｏの値）
は、例えば、ロボットの能力評価に用いたり、あるい
は、ロボットに何等かの問題が起きた場合の原因の究明
等に使うことができる。また、この情報については、複
数台のロボット装置を生産ライン上に配置して使用して
いる場合には最も下流に位置するロボットについてのカ
ウント数を特に当該生産ラインにおける生産管理用の情
報として利用することができ、例えば、ロボットが問題
なく動作しているにもかかわらずカウント数が大きい場
合には、ワーク数に比してロボットの台数が少ないこと
を示す情報となる。

【０２０６】図２８はトラッキング動作についてのベク
トルを用いた状態図であり、点Ｐφがロボットハンドの
現在位置（時間にとともに変化する。）を示しており、
点Ｐ（ｔ＝０）がトラッキング動作開始時におけるワー
クの位置（時間ｔの起点はワークが光センサー２９を過
った時点である。）、点Ｐ（ｔ）がｔ秒後のワーク位置
をそれぞれ示している。尚、点Ｐ１′、Ｐ２′、Ｐ３′
等については既述の通りであり、点Ｏは３次元直交座標
系（ｘ，ｙ，ｚ）の原点を示している。

【０２０７】点Ｐφから点Ｐ（ｔ）に向かうベクトルＷ
Ｔは時間ｔに応じて変化し、これが加減速パターン、つ
まり、ロボットハンドによりワークを追尾する際の時間
の関数としての位置ベクトルを示しており、その生成法
については上述した通りである。

【０２０８】また、点Ｐ（ｔ＝０）から点Ｐ（ｔ）に向
かうベクトルＴＥは、コンベア２２の進行を示してお
り、その大きさ（これが搬送距離ｌ′に相当する。）が
時間の経過とともに大きくなっていく。

【０２０９】そして、点Ｏから点Ｐ（ｔ）に向かうベク
トルＲＰはロボットのサーボ系を構成するモータへの指
令値を示しており、点Ｏから点Ｐφへ向かうベクトルを
ＳＴＤとするとき、ベクトルＲＰは、ベクトルＳＴＤ、
ＷＴ、ＴＥの和である。

【０２１０】尚、ＰＴＰ動作においてはワークが静止し
ているのでベクトルＴＥがゼロベクトルであり、よって
ベクトルＲＰは、ベクトルＳＴＤとＷＴとの和となる。

【０２１１】また、以上のベクトルは３次元位置座標値
ｘ、ｙ、ｚの他、ロボットハンドの所定軸回りの回転角
を加えた４次元ベクトルであり、ロボットハンドによる
ワークの把持にあたってはワークの姿勢を示すΔΦが撮
像部２３によって検出されているので、これに基づくロ
ボットハンドの姿勢制御によりワークが如何なる姿勢で
あっても確実にワークを把持することができる。

【０２１２】以上の説明では便宜上一台のロボットを使
った装置について説明したが、図１４に示すロボット装
置を複数段に亘って直列に配置することができることは
勿論である。また、その際、コンベア２２の共通化が可
能であることから撮像部２３（カメラ２４、光センサー
２６等）やロータリーエンコーダ２８の共通化を図るこ
とがコスト面で有利であるが、この場合には、上流に位
置するロボットがとりこぼしたワークを下流に位置する
ロボットに対して通信により伝達する必要があり、ロボ
ットプログラムにおいてリングバッファ等の同期をとる
必要が生じる。また、上記の例ではワークがロボットの
作業範囲に入って来るタイミングが不規則であるため、
図２５に示すような割込み処理を用いたが、これに限ら
ず、例えば、光センサー信号による割込み処理だけを行
って、リングバッファについてのカレントポインタのイ
ンクリメント等をロボットプログラム中で記述するよう
にしても良い。尚、この場合には、例えば、ＴＲＣＫＣ
ＨＥＲＲ＝２の場合において、ダミーの指示文「ＤＯ
ＴＲＡＣＫＯＮ／ＴＲＡＣＫＯＦＦ」をプログラム中に
記述してワークへの追尾をわざと失敗させてから、カレ
ントポインタＣｐのインクリメントを行うようにすれば
良い（つまり、この場合にはプログラム中で「ＤＯＴ
ＲＡＣＫＯＦＦ」を見つけた場合に、カレントポインタ
Ｃｐのインクリメントが行われないので、インクリメン
ト処理をプログラム中に記述しておく必要がある。）。

【０２１３】

【発明の効果】以上に記載したところから明らかなよう
に、請求項１に係る発明によれば、搬送手段に係る搬送
基準経路に対して被搬送物体が置かれた位置の変位量
と、被搬送物体が基準位置を通過した以後の搬送量とを
検出することによって被搬送物体の現在位置を逐次に計
算して求めることができるので、制御の遅れや構成の複
雑化を伴うことがない。

【０２１４】請求項２に係る発明によれば、制御指令値
に基づいて加減速曲線の山の高さを制御したり、制御指
令値から決定される時間軸伸縮用パラメータ値に基づい
て加減速曲線の時間軸方向の幅を制御することによっ
て、制御対象の様々な構成に柔軟に対応することのでき
る加減速曲線を生成して、被搬送物体への接近動作や追
従動作を実現することができる。

【０２１５】請求項３に係る発明によれば、制御対象の
作用端についての位置指令としてランプ信号に係る変調
信号を用いることによって、被搬送物体への接近動作及
び追従動作に係る制御と、制御対象の作用端を静止物体
又は定位置へと移動させる制御との間で統一化された加
減速曲線を生成して両制御間の遷移を容易に実現するこ
とができる。

【０２１６】請求項４乃至請求項６に係る発明によれ
ば、被搬送物体の位置変位量と搬送姿勢とを、搬送姿勢
検出手段と位置変位量検出手段との間で兼用された撮像
手段により得られる撮影画像から算出することで構成の
簡単化を図ることができる。

【０２１７】請求項７乃至請求項９に係る発明によれ
ば、搬送姿勢検出手段及び／又は位置変位量検出手段の
撮像手段と兼用された撮影手段によって得られる撮影画
像から搬送量を算出することで構成の簡単化を図ること
ができる。

【０２１８】請求項１０乃至請求項１２に係る発明によ
れば、被搬送物体への接近動作や追従動作が間に合わな
い場合には、制御対象の作用端に対して無駄な動作をさ
せないようにすることで動作制御の効率化を図ることが
できる。

【０２１９】請求項１３に係る発明によれば、コンベア
の搬送基準経路に対して被搬送物体が置かれた位置の変
位量と、被搬送物体が基準位置を通過した以後の搬送量
とを検出することによって被搬送物体の現在位置を逐次
に計算して求めることができるので、ロボットの動作制
御に著しい遅れが生じたり、被搬送物体の現在位置を監
視するために複雑な構成を用いる必要がない。

【０２２０】請求項１４に係る発明によれば、コンベア
の送り量を一時的に格納した後で取り出すためのリング
バッファを設けることによって、被搬送物体とその搬送
量との対応関係や順番を正確かつ簡易に管理することが
できる。

【０２２１】請求項１５や請求項１８に係る発明によれ
ば、ロボットプログラム中に接近動作や追従動作の宣言
文を記述することができるようにし、被搬送物体毎の搬
送状況若しくは環境を示すパラメータに基づいて被搬送
物体毎にその現在位置を計算することができる。

【０２２２】請求項１６や請求項１９に係る発明によれ
ば、ロボットハンドの現在位置に基づいてこれを被搬送
物体に接近させ又は追従させることがロボットの作業範
囲内において可能であるか否かを示す予約語若しくは変
数をロボットプログラム中に記述することができるよう
にし、その値に応じてロボットプログラム中の命令解釈
に係る進行の流れを制御することができる。

【０２２３】請求項１７や請求項２０に係る発明によれ
ば、ロボットプログラム中に接近動作や追従動作の開始
や終了を明示する命令を記述することができるように
し、当該命令で挟まれた範囲について接近動作や追従動
作を指定することができる。

【０２２４】請求項２１に係る発明によれば、ロボット
プログラム中に接近動作や追従動作の終了を明示する命
令を見つけた場合に、リングバッファの格納値を次の被
搬送物体に係る格納値へと進めることで、そのための特
別な命令等をロボットプログラム中に記述する必要がな
くなる。

【図面の簡単な説明】

【図１】搬送基準経路と被搬送物体の位置変位量につい
ての説明図である。

【図２】本発明に係る装置の基本構成を示す図である。

【図３】動作制御手段の基本構成を示す図である。

【図４】関数計算による加減速曲線の生成について説明
するためのグラフ図である。

【図５】仮想サーボ系について説明するためのものであ
り、（ａ）は現実のモータ系の一例を示すシグナルフロ
ー線図、（ｂ）は（ａ）のモータ系に対する仮想モータ
系の構成を示すシグナルフロー線図である。

【図６】仮想モータ系に係る基準系と、該基準系に対す
るα変換によって得られる仮想モータ系について説明す
るための図である。

【図７】制御対象を２軸構成のロボットに特定した場合
に、ロボットや被搬送物体に関する諸量の関係を示す略
線図である。

【図８】Δθ−Ｔ_P特性の一例を示すグラフ図である。

【図９】定点間移動に係る加減速パターンの生成につい
て説明するための図である。

【図１０】Ｔ_P値及びＴ_f値の決定の仕方について説明す
るための図であり、（ａ）はｔ−θ図とＴ_P−Δθ図と
を併記したグラフ図、（ｂ）は（ａ）に対応するｔ−Ω
図である。

【図１１】制御対象が２軸構成の場合のＴ_P値及びＴ_f値
の決定について説明するための図であり、ｔ−θ図とＴ
_P−Δθ図とを併記して示すグラフ図である。

【図１２】ランプ変調信号等の定義量の関係を示すｔ−
θ図である。

【図１３】ランプ信号を用いた場合の動作制御手段の構
成を示す図である。

【図１４】図１５乃至図２８とともに本発明をロボット
装置に適用した実施例を示すものであり、本図は全体の
構成を示すブロック図である。

【図１５】ワークについての位置変位量及び姿勢を示す
方位角についての説明図である。

【図１６】ソフトウェア構成を概念的に示す図である。

【図１７】宣言文「ＴＲＡＣＫ」に係るパラメータにつ
いて説明するための図である。

【図１８】搬送距離に係るベクトル方程式についての説
明図である。

【図１９】宣言文「ＴＲＡＣＫ」についての概念的な説
明図である。

【図２０】予約語「ＴＲＣＫＣＨＥＲＲ」について説明
するための図である。

【図２１】予約語「ＴＲＣＫＣＨＥＲＲ」ついての概念
的な説明図である。

【図２２】指示文「ＤＯＴＲＡＣＫＯＮ／ＴＲＡＣＫ
ＯＦＦ」についての概念的な説明図である。

【図２３】トラッキング制御用ボードの構成例を示すブ
ロック図である。

【図２４】リングバッファについて説明するための図で
ある。

【図２５】タイマー割込みと光センサー信号割込みにつ
いてのフローチャート図とリングバッファとを併せて示
す図である。

【図２６】搬送距離とカウント値の時間的変化を示す図
である。

【図２７】２つの光センサー及び２つのリングバッファ
を用いてワークの搬送距離を算出する様子を概念的に示
す図である。

【図２８】トラッキング動作についてベクトルを用いて
説明するための状態図である。

【符号の説明】

１…被搬送物体への接近及び／又は追従に係る装置、２
…位置変位量検出手段、３…基準位置通過検出手段、ｃ
…搬送手段、ｗ…被搬送物体、ＳＴ…制御対象、４…搬
送量検出手段、６…現在位置計算手段、７…動作制御手
段、９…指令手段、１１…制御指令値計算手段、１２…
ピーク速度到達時間値決定手段、１３…時間軸伸縮用パ
ラメータ値決定手段、１５…加減速パターン生成手段、
１７…制御対象、１８…ランプ折れ時間値決定手段、１
９…変調信号生成手段、２０…ロボット装置、２２…コ
ンベア、２７…ロボット制御装置、３０…ロボット、３
２…ロボットハンド、４２…命令解釈手段

Claims

【特許請求の範囲】

【請求項１】搬送手段によって搬送されてくる被搬送
物体に対して制御対象の作用端を接近させたりあるいは
被搬送物体に追従させるための被搬送物体への接近及び
／又は追従に係る装置において、搬送手段の搬送基準経路に対して直交する方向における
被搬送物体の位置の変位量を検出する位置変位量検出手
段と、被搬送物体がその搬送経路における基準位置を通過した
ことを検出する基準位置通過検出手段と、被搬送物体が上記基準位置を通過した時点からその搬送
経路に沿って搬送手段によりどれだけ搬送されたかを検
出する搬送量検出手段と、位置変位量検出手段、基準位置通過検出手段、搬送量検
出手段による検出結果を受けて被搬送物体の現時点での
位置を計算する現在位置計算手段と、現在位置計算手段からの計算結果を受けて制御対象の作
用端を被搬送物体に接近させ及び／又は追従させるため
の制御信号を生成して制御対象に送出する動作制御手段
とを備えたことを特徴とする被搬送物体への接近及び／
又は追従に係る装置。
【請求項２】請求項１に記載の被搬送物体への接近及
び／又は追従に係る装置において、動作制御手段が、被搬送物体への接近動作や追従動作に係る制御の開始指
令を発する指令手段と、指令手段からの上記開始指令を受けた場合に、現在位置
計算手段からの被搬送物体の位置情報と、制御対象の作
用端の位置情報とに基づいて、両者間の差をゼロにする
ために制御対象の作用端への制御指令値を計算する制御
指令値計算手段と、制御指令値計算手段からの制御指令値と、制御対象に係
る加減速曲線において速度がピーク値に達するまでの時
間（以下、「ピーク速度到達時間」という。）との関係
を規定するとともに、制御指令値からピーク速度到達時
間の値を決定するピーク速度到達時間値決定手段と、上記ピーク速度到達時間をその基準値で割ることによっ
て無次元量として定義される時間軸伸縮用パラメータの
値を決定する時間軸伸縮用パラメータ値決定手段と、制御指令値計算手段からの制御指令値及び時間軸伸縮用
パラメータ値に基づいて加減速曲線の山の高さや時間軸
方向の幅を変化させることにより加減速曲線を生成し
て、該加減速曲線に係る制御信号を制御対象に送出する
加減速パターン生成手段とを備えたことを特徴とする被
搬送物体への接近及び／又は追従に係る装置。
【請求項３】請求項１に記載の被搬送物体への接近及
び／又は追従に係る装置において、動作制御手段が、被搬送物体への接近動作や追従動作に係る制御の開始指
令を発する指令手段と、指令手段からの上記開始指令を受けた場合に、現在位置
計算手段からの被搬送物体の位置情報と、制御対象の作
用端の位置情報とに基づいて、両者間の差をゼロにする
ために制御対象の作用端への制御指令値を計算する制御
指令値計算手段と、制御指令値計算手段からの制御指令値と、制御対象に係
る加減速曲線のピーク速度到達時間との関係を規定する
とともに、制御指令値からピーク速度到達時間の値を決
定するピーク速度到達時間値決定手段と、上記ピーク速度到達時間をその基準値で割ることによっ
て無次元量として定義される時間軸伸縮用パラメータの
値を決定する時間軸伸縮用パラメータ値決定手段と、制御指令値計算手段からの制御指令値に基づいて位置指
令に係るランプ信号の折れ時間を求める際に、ランプ信
号の傾斜つまり速度が制御対象に係る最大速度未満の場
合にはランプ信号の折れ時間をピーク速度到達時間と同
じ値とし、また、ランプ信号に係る速度が制御対象に係
る最大速度である場合にはランプ信号の折れ時間を、制
御指令値計算手段からの位置に係る制御指令値を制御対
象に係る最大速度で割った時間として求めるランプ折れ
時間値決定手段と、ランプ折れ時間値決定手段からの折れ時間に基づいてラ
ンプ信号を生成するとともに、被搬送物体への接近動作
の開始時点での制御対象の作用端と被搬送物体との間の
距離を基準とした被搬送物体の位置変化量をランプ信号
に重畳することによって変調信号を生成する変調信号生
成手段と、変調信号生成手段からの変調信号及び時間軸伸縮用パラ
メータ値決定手段からの時間軸伸縮用パラメータ値に基
づいて加減速曲線の山の高さや時間軸方向の幅を変化さ
せることにより加減速曲線を生成して、該加減速曲線に
係る制御信号を制御対象に送出する加減速パターン生成
手段とを備えていることを特徴とする被搬送物体への接
近及び／又は追従に係る装置。
【請求項４】請求項１に記載の被搬送物体への接近及
び／又は追従に係る装置において、被搬送物体の搬送中の姿勢を検出するための搬送姿勢検
出手段を設けるとともに、該搬送姿勢検出手段と位置変
位量検出手段とが同一の撮像手段を有し、被搬送物体の
位置変位量及び姿勢を撮像手段による撮影画像から算出
してこれらを現在位置計算手段に送出することを特徴と
する被搬送物体への接近及び／又は追従に係る装置。
【請求項５】請求項２に記載の被搬送物体への接近及
び／又は追従に係る装置において、被搬送物体の搬送中の姿勢を検出するための搬送姿勢検
出手段を設けるとともに、該搬送姿勢検出手段と位置変
位量検出手段とが同一の撮像手段を有し、被搬送物体の
位置変位量及び姿勢を撮像手段による撮影画像から算出
してこれらを現在位置計算手段に送出することを特徴と
する被搬送物体への接近及び／又は追従に係る装置。
【請求項６】請求項３に記載の被搬送物体への接近及
び／又は追従に係る装置において、被搬送物体の搬送中の姿勢を検出するための搬送姿勢検
出手段を設けるとともに、該搬送姿勢検出手段と位置変
位量検出手段とが同一の撮像手段を有し、被搬送物体の
位置変位量及び姿勢を撮像手段による撮影画像から算出
してこれらを現在位置計算手段に送出することを特徴と
する被搬送物体への接近及び／又は追従に係る装置。
【請求項７】請求項４に記載の被搬送物体への接近及
び／又は追従に係る装置において、搬送量検出手段が、搬送姿勢検出手段及び／又は位置変
位量検出手段と同一の撮像手段を有し、該撮像手段によ
る撮影画像に基づいて搬送速度を算出してこれに搬送時
間を掛け若しくは搬送速度の積分によって搬送量を算出
することを特徴とする被搬送物体への接近及び／又は追
従に係る装置。
【請求項８】請求項５に記載の被搬送物体への接近及
び／又は追従に係る装置において、搬送量検出手段が、搬送姿勢検出手段及び／又は位置変
位量検出手段と同一の撮像手段を有し、該撮像手段によ
る撮影画像に基づいて搬送速度を算出してこれに搬送時
間を掛け若しくは搬送速度の積分によって搬送量を算出
することを特徴とする被搬送物体への接近及び／又は追
従に係る装置。
【請求項９】請求項６に記載の被搬送物体への接近及
び／又は追従に係る装置において、搬送量検出手段が、搬送姿勢検出手段及び／又は位置変
位量検出手段と同一の撮像手段を有し、該撮像手段によ
る撮影画像に基づいて搬送速度を算出してこれに搬送時
間を掛け若しくは搬送速度の積分によって搬送量を算出
することを特徴とする被搬送物体への接近及び／又は追
従に係る装置。
【請求項１０】請求項１に記載の被搬送物体への接近
及び／又は追従に係る装置において、現在位置計算手段が、制御対象の作用端の現在位置に基
づいてこれを被搬送物体に接近させ又は追従させること
が制御対象の動作範囲内において可能であるか否かを判
断して、これが可能である場合にのみ動作制御手段を介
して制御対象を動作させるようにしたことを特徴とする
被搬送物体への接近及び／又は追従に係る装置。
【請求項１１】請求項２に記載の被搬送物体への接近
及び／又は追従に係る装置において、現在位置計算手段が、制御対象の作用端の現在位置に基
づいてこれを被搬送物体に接近させ又は追従させること
が制御対象の動作範囲内において可能であるか否かを判
断して、これが可能である場合にのみ動作制御手段を介
して制御対象を動作させるようにしたことを特徴とする
被搬送物体への接近及び／又は追従に係る装置。
【請求項１２】請求項３に記載の被搬送物体への接近
及び／又は追従に係る装置において、現在位置計算手段が、制御対象の作用端の現在位置に基
づいてこれを被搬送物体に接近させ又は追従させること
が制御対象の動作範囲内において可能であるか否かを判
断して、これが可能である場合にのみ動作制御手段を介
して制御対象を動作させるようにしたことを特徴とする
被搬送物体への接近及び／又は追従に係る装置。
【請求項１３】複数のアーム及び／又はツール搭載部
と、これらの駆動源及びその駆動回路を含む複数のサー
ボ系とから構成されるロボットと、ロボットハンドを、
コンベアにより運ばれて来る被搬送物体に対して接近さ
せたりあるいは被搬送物体に追従させるための動作制御
を行うロボット制御装置とを備えたロボット装置におい
て、コンベアの搬送基準経路に対して直交する方向における
被搬送物体が置かれた位置の変位量を検出する位置変位
量検出手段と、被搬送物体がその搬送経路における基準位置を通過した
ことを検出する基準位置通過検出手段と、被搬送物体が上記基準位置を通過した時点からその搬送
経路に沿ってコンベアによってどれだけ搬送されたかを
検出する搬送量検出手段と、位置変位量検出手段、基準位置通過検出手段、搬送量検
出手段による検出結果を受けて被搬送物体の現時点での
位置を計算する現在位置計算手段と、現在位置計算手段からの計算結果を受けてロボットハン
ドを被搬送物体に接近させ及び／又は追従させるための
制御信号を生成してロボットに送出する動作制御手段と
を備えたことを特徴とするロボット装置。
【請求項１４】請求項１３に記載のロボット装置にお
いて、搬送量検出手段がコンベアの送り量を検出する手段を有
するとともに、該送り量を一時的に格納した後で取り出
すためのリングバッファを現在位置計算手段が有してお
り、被搬送物体が基準位置を通過した時のコンベアの搬送量
をリングバッファに順次に格納し、コンベアの現在の送
り量とリングバッファの格納値との間の差を求めること
によって被搬送物体のコンベア上の搬送位置を計算する
ことを特徴とするロボット装置。
【請求項１５】請求項１３に記載のロボット装置にお
いて、ロボットの動作を記述するロボットプログラムに係る命
令を解釈する命令解釈手段を設けるとともに、該命令解
釈手段がロボットプログラム中に接近動作や追従動作の
宣言文を見つけた場合に、当該宣言文に係る被搬送物体
毎の搬送状況若しくは環境を示すパラメータに基づいて
現在位置計算手段が被搬送物体の現時点の位置を計算す
ることを特徴とするロボット装置。
【請求項１６】請求項１３に記載のロボット装置にお
いて、ロボットの動作を記述するロボットプログラムに係る命
令を解釈する命令解釈手段を設けるとともに、ロボットハンドの現在位置に基づいてこれを被搬送物体
に接近させ又は追従させることがロボットの作業範囲内
において可能であるか否かを判断して、判断結果を示す
特定値をロボットプログラムに係る所定の予約語若しく
は変数に設定し、命令解釈手段がロボットプログラム中
の条件命令文中に上記の予約語若しくは変数を見つけた
場合に、その値に応じてロボットプログラムの進行の流
れを制御することを特徴とするロボット装置。
【請求項１７】請求項１３に記載のロボット装置にお
いて、ロボットの動作を記述するロボットプログラムに係る命
令を解釈する命令解釈手段を設けるとともに、該命令解
釈手段がロボットプログラム中に接近動作や追従動作の
開始や終了を明示する命令を見つけた場合に、当該命令
で挟まれた範囲の動作命令が接近動作や追従動作に係る
命令であると解釈して、動作制御手段を介してロボット
に動作指令を発することを特徴とするロボット装置。
【請求項１８】請求項１４に記載のロボット装置にお
いて、ロボットの動作を記述するロボットプログラムに係る命
令を解釈する命令解釈手段を設けるとともに、該命令解
釈手段がロボットプログラム中に接近動作や追従動作の
宣言文を見つけた場合に、当該宣言文に係る被搬送物体
毎の搬送状況若しくは環境を示すパラメータに基づいて
現在位置計算手段が被搬送物体の現時点の位置を計算す
ることを特徴とするロボット装置。
【請求項１９】請求項１４に記載のロボット装置にお
いて、ロボットの動作を記述するロボットプログラムに係る命
令を解釈する命令解釈手段を設けるとともに、ロボットハンドの現在位置に基づいてこれを被搬送物体
に接近させ又は追従させることがロボットの作業範囲内
において可能であるか否かを判断して、判断結果を示す
特定値をロボットプログラムに係る所定の予約語若しく
は変数に設定し、命令解釈手段がロボットプログラム中
の条件命令文中に上記の予約語若しくは変数を見つけた
場合に、その値に応じてロボットプログラムの進行の流
れを制御することを特徴とするロボット装置。
【請求項２０】請求項１４に記載のロボット装置にお
いて、ロボットの動作を記述するロボットプログラムに係る命
令を解釈する命令解釈手段を設けるとともに、該命令解
釈手段がロボットプログラム中に接近動作や追従動作の
開始や終了を明示する命令を見つけた場合に、当該命令
で挟まれた範囲の動作命令が接近動作や追従動作に係る
命令であると解釈して、動作制御手段を介してロボット
に動作指令を発することを特徴とするロボット装置。
【請求項２１】請求項２０に記載のロボット装置にお
いて、命令解釈手段がロボットプログラム中に接近動作や追従
動作の終了を明示する命令を見つけた場合に、リングバ
ッファの格納値を次の被搬送物体に係る格納値へと進め
るようにしたことを特徴とするロボット装置。