JP5380672B2 - モーションプランナ、制御システム、および多軸サーボシステム - Google Patents

Description

本発明は、多軸のモーションコントローラおよびモーションコントローラへの制御命令を計算する計算機（モーションプランナ）およびサーボアンプに関するものであり、特に最短時間制御が可能なシステムに関するものである。

多軸のモーションコントローラは、産業界においてロボットや製造装置などの制御に数多く用いられている。その機能は、上位コントローラからの指令、または予め蓄積しておいた指令に基づいて、各軸のモータコントローラへの位置目標値を出力するものである。この機能を用いて、予め各軸の最高速度や最高加速度などの値を設定しておき目標位置を逐次与えて直線運動させたり、円弧状に移動するコマンドを与えて曲線運動をさせたりする。各軸のモータコントローラへの指令としては、位置目標値の変化(増減)をパルス信号で与えるタイプのものと、一定時間ごとに位置目標値を指定するタイプのものがある。

一方、製造装置においては与えられた仕事をいかに短い時間で終了して生産性を上げたいといった要求は多い。そのためには、モーションに要する時間を短縮することが要求される。特に搬送ロボットにおいては、その軌道は初期条件や終了条件、禁止領域を始めとするいくつかの制約はあるが、その制約内において軌道を自由に設定することができる。しかし、従来のモーションコントローラは位置のみを指定するPTP（ポイント・ツー・ポイント）制御が基本であるために、最短時間制御を実現するようなモーションを指定することが困難であった。図２に従来のPTP制御に基づく指定軌道の例を示す。いくつかの直線と円弧により軌道を指定している。特許文献１においては、多関節ロボットに対する最短時間制御のアプローチが提案されているが、禁止領域などの条件を考慮することが難しい。

また、最短時間制御を実現する軌道（以下、最適軌道という）の計算に関しては最大原理などの手法を用いることができる。しかし、実際に扱わなければならない問題には軌道の制約などがあり、最適軌道の厳密な計算は容易ではない。

直交軸ロボットの場合は、各軸の制約が独立しているので、その点では最適軌道の計算が相対的に容易になるが、多関節ロボットなどの場合においては、各関節に掛かるトルクに対する制約などがあり、各軸の制約が独立にはなっておらず、最適軌道の厳密な計算はさらに困難となる。

最短時間制御を実現するには、指定した軌道を精度良く再現するためには各軸において追従性の良いサーボアンプを用いる必要がある。今まで、指定した軌道を精度良く再現させたい場合は、指定する軌道をゆっくりとしたものにしていたが、最短時間制御を実現させる上においては、軌道をゆっくりとしたものにすることが許されない。したがって、追従性の良いサーボアンプの実現が必要になる。
特開平７−３２５６０７号公報

解決しようとする問題点は、モーションコントローラに予め計算しておいた最適軌道を実現させるための補間機能を持たせることである。

また、そのようなモーションコントローラに対する最適軌道の計算を実現させるための手段を用意することである。

さらに、最適軌道に追従させるための追従性能の良いサーボアンプを用意することである。

モーションコントローラにおいて、モーションを指定するのに一連の位置を指定するのではなく、一連の位置とその位置における速度も指定できるようにする。各位置における速度も指定できることより、モーションにおける速度も細かく指定することができ、予め計算しておいた最短時間モーションを実現させることが容易になる。また、各位置における速度を指定することにより、一連の位置と速度からモーション全体に要する時間が算出できるので、一連の位置と速度を最適化することにより最短時間モーションを算出することができる。このとき、厳密な意味での最短時間モーションを求めることはできないが、モーションの分割数を十分に確保することにより、要する時間が最短に近いモーションを求めることができる。

さらに、各位置における速度を指定することによってモーションの全ての時刻における位置および速度が決まるので、関節タイプのロボットに適用した場合、全ての時刻における関節トルクを計算することができる。そのため、最短時間制御において、各関節トルクの最大値に対しても制約を設けても、最短時間モーションを求めることが容易にできる。

本発明の第１の局面においては、１軸もしくは２軸以上の軸を持つモーションコントローラに対して、いくつかの通過点情報が与えられると前記通過点を結ぶ軌跡に掛かる信号を出力するようにし、前記通過点情報は通過点の座標と、前記通過点を通過する速度と、連続する前記通過点を通過する通過時間の組み合わせからなるようにすることによって、比較的少ない通過点の指定により最短時間軌道に近い軌道の指定を可能にする。

また、本発明第２の局面において、連続する二つの前記通過点の間の軌跡における各軸の速度を、連続する二つの前記通過点を通過するのに要する時間が前記通過時間と等しくなるように時刻に対して２次関数となるように補間することによって、各軸に対して２次の係数を調整することにより指定した時間で指定した位置に指定した初期速度と最終速度でモーションを実現することを可能にする。

さらに、本発明第３の局面において、２軸以上の軸を持つモーションコントローラに対して、いくつかの通過点情報が与えられると前記通過点を結ぶ通過軌跡に掛かる信号を出力し、前記通過点情報は通過点の座標と、前記通過点を通過する速度の組み合わせとし、連続する二つの前記通過点の間を通過する通過時間は前記連続する二つの前記通過点における前記通過点情報より算出することにより、位置と速度の組み合わせによる限られた数の通過点の情報により最適軌道の情報を持たせることができる。

本発明第４の局面においては、上記のモーションコントローラにおいて、前記通過軌跡に掛かる信号は、各軸に対するパルス列とすることにより、従来のサーボアンプを用いて最適軌道を実現するモーションコントロールシステムを実現できる。

本発明第５の局面においては、上記のモーションコントローラにおいて、前記通過軌跡に掛かる信号は、一定時間間隔ごとの各軸の座標とすることにより、従来の一定時間ごとの移動量を入力とするサーボアンプを用いて最適軌道を実現するモーションコントロールシステムを実現できる。

本発明第６の局面においては、上記のモーションコントローラにおいて、前記通過軌跡に掛かる信号は、一定時間間隔ごとの各軸の座標に速度を加えることにより、サーボアンプの目標軌道追従性能を上げ、最適軌道に対する条件を緩和する。

本発明第７の局面においては、本発明第１の局面から第６の局面に記載のモーションコントローラと前記モーションコントローラからの指令を受けるサーボアンプを組み合わせることにより多軸サーボシステムを構成する。

本発明第８の局面においては、最適軌道を設定するアプローチに関するものであり、エンドエフェクタを持つ多軸サーボシステムに対して、前記エンドエフェクタの移動範囲である作業領域と、前記作業領域内において前記エンドエフェクタが侵入することが許されないゼロまたは一つまたは複数の禁止領域を設定し、さらに各軸に対して許される速度および加速度の組み合わせの範囲を設定し、エンドエフェクタの初期位置および初期速度および最終位置および最終速度を与えると各軸の通過軌道を算出する機能をモーションプランナに持たせることにより最適軌道の計算を可能にする。

また、本発明第９の局面において、上記のモーションプランナにおいて、算出される前記軌道はエンドポイントが通過する複数の通過点の座標および前記通過点を通過する速度および連続する二つの前記通過点を通過する通過時間とすることにより、上記のモーションコントローラとの組み合わせを可能にする。

本発明第１０の局面においては、本発明第８の局面または第９の局面に記載のモーションプランナにより各軸の前記通過軌跡を算出し、前記算出された前記通過軌跡をエンドポイントの目標軌道とすることにより多軸サーボシステムに対するモーションプランナを実現する。

本発明第１１の局面においては、さらに多軸サーボシステムにおいて本発明第１の局面から第６の局面に記載のモーションコントローラと上記モーションプランナを組み合わせることにより、最適軌道を実現する多軸サーボシステムを実現する。

本発明第１２の局面においては、直交軸ロボットに対して本発明第８の局面または第９の局面に記載のモーションプランナを用いることにより、直交軸ロボットの最適軌道生成を可能にする。

本発明第１３の局面においては、直交軸ロボットに対して本発明第１０の局面または第１１の局面に記載の多軸サーボシステムを用いることにより、直交軸ロボットにおける最適軌道制御を可能にする。

本発明第１４の局面においては、非直交軸ロボットに対して本発明第８の局面または第９の局面に記載のモーションプランナを用いることにより、関節ロボットや関節と並進を併せ持つロボットに対する最適軌道生成を可能にする。

本発明第１５の局面においては、非直交軸ロボットに対して本発明第１０の局面または第１１の局面に記載の多軸サーボシステムを用いることにより、関節ロボットや関節と並進を併せ持つロボットに対する最適軌道制御を可能にする。

本発明第１６の局面においては、サーボアンプにおいて、一定時間間隔ごとに位置目標値と速度目標値を入力し、被制御量を前記位置目標値に近づけ、前記被制御量の時間微分の値を前記速度目標値に近づける動作を同時に行うことにより、サーボアンプの目標軌道追従性能を向上させ、精度の良い最適軌道実現を可能にする。

さらに、本発明第１７の局面においては、サーボアンプにおいて、一定時間間隔ごとに位置目標値を入力し、速度目標値を前記位置目標値の差分とし、被制御量を前記位置目標値に近づけ、被制御量の時間微分の値を前記速度目標値に近づける動作を同時に行うことにより、目標値信号として速度目標値を含んでいなくてもサーボアンプの目標軌道追従性能を向上させ、精度の良い最適軌道実現を可能にする。

本発明のモーションコントローラは、PTP制御において位置だけではなく速度も指定するようにするので、速度を含めたモーションを細かく指定することが可能になり、最適軌道を容易に指定することができるという利点がある。

また、モーション全体を有限個の位置と速度で記述するため、最短時間制御問題が有限個の変数の最適化問題となるので、最適軌道を比較的容易に算出することが可能となる。

さらに、サーボアンプにおいて目標速度を考慮することにより、サーボアンプの目標軌道追従性能を向上させ、精度の良い最適軌道実現を可能にする。

（第１の実施の形態）
本発明第１の実施の形態として、直交軸ロボットに対するモーションコントロールシステムの実現例を示す。ここでは２軸の場合を考える。すなわち、２次元座標系において、ロボットの軌道を生成する場合を扱う。

図５にモーション制御系の構成を示す。モーションコントローラに予め決められた一連の通過点(x_k,y_k)およびその通過点における速度(v_k,w_k)が目標点信号Ｓ１として順次入力される。モーションコントローラ２は各軸のモーション補間を行い、1msごとに各軸の目標速度と目標速度が算出され、各軸のサーボアンプ３ａ，３ｂに指令値として出力する。サーボアンプ３ａ，３ｂにおいては、モーションコントローラ２から受け取った位置と速度の指令値に対して実際のロボットの位置が追従するようにフィードバック制御を行う。

図１および図３に生成されるモーションの経路の例を示す。図１と図３は基本的には同じ図であるが、各軸の速度の記号が異なる。図１は直観的に判りやすい記号を用いており、図３は本発明第１の実施の形態の説明において使用した記号を用いている。作業領域ＡＷは２次元の矩形領域であり、その中に禁止領域（図３においてはＡＰ１からＡＰ４）がいくつか設定されている。禁止領域は、この領域の中に入るとロボットが装置などに衝突するといった領域であり、モーションの経路がこの禁止領域に入ることは許されない。ロボットのモーションは始点および終点を含むN+1個の点における位置と速度によって決定される。これらの位置や速度の決定については後述する。

ロボットのモーションは有限個の点で表現されているため、サーボアンプへの指令値を算出するには補間を行なう必要がある。補間は２点間で行い、２点の位置と速度をもとに各時刻における位置と速度を決定する。その際、x軸で補間を行なって決定されるx軸の移動時間とy軸で補間を行なって決定されるy軸の移動時間は等しい必要がある。そこで、補間は次のように行なう。２点の位置を(x_k,y_k)および(x_k+1,y_k+1)とする。また、それらの点における速度を(v_k,w_k) および (v_k+1,w_k+1)とする。そして、x軸およびy軸において、速度を時刻に対して１次関数で補間する場合を考える。その際、各軸により移動時間が異なる。そこで、各軸において速度を時刻に対して一次補間を行なったときの移動時間T^x _k+1およびT^y _k+1を求める。

しかし、このように移動時間を決めると、移動時間がとても長くなったり場合によっては求めた移動時間が負になってしまうこともある。そこで、ある程度長い時間T_mの値を設定し、各軸の移動時間がT_mより長かったり負である場合は次のように修正し、修正移動時間S^x _k+1およびS^y _k+1を求める。

そしてx軸の修正移動時間とy軸の修正移動時間の長い方を移動時間τ_kとする。すなわち、

とする。ここで、点(x_k,y_k)を通過する時刻をt_k、点(x_k+1,y_k+1)を通過する時刻をt_k+1とすると、

となる。そして、速度を時刻に対して１次関数で補間されなかった軸に対しては、移動時間が指定した移動時間と等しくなるように速度を時刻に対して２次関数により補間する。数式で表すと次のようになる。

このとき、２次の係数a_k+1またはb_k+1のどちらも値が０とならない場合もあれば、どちらかの値が０となる場合もある。２次の係数が０であるとき、１次補間を行なうことになる。この様子を図４に示す。

モーションはN+1個の点の座標と速度によって決まるため、全部で4(N+1)個の自由度を有する。このうち、始点と終点の位置と速度は予め決められてしまうので、残る4(N-1)個のパラメータを調整して最短時間モーションを求めることになる。すなわち、

の値を最小にするようにする。その際、いくつかの拘束条件を考慮する必要がある。まず各軸における最大加速度に制限が存在する。x軸の最大加速度の絶対値をm_x、y軸の加速度の最大値をm_yとし、時刻tにおけるx軸の速度をv(t)、y軸の速度をw(t)とする。このとき、加速度に対する条件は、

に対して、p_xおよびp_yが正とならないことである。ここではm_xおよびm_yを一定としたが、これらは速度の関数であっても良い。

さらに、x軸およびy軸に対して最大速度に対しても制約を受ける。すなわち、x軸の最大速度の絶対値をq_x、y軸の速度の最大値をq_yとしたとき、

に対して、p_vおよびp_wが正とならない必要がある。

モーションの経路は禁止領域（図３においてはＡＰ１からＡＰ４）の中に入ることは許されない。禁止領域は平行四辺形もしくは楕円により定義される。その場合、k番目の禁止領域は

といった形で記述することができる。ここで、o_kは１ノルム（禁止領域が平行四辺形の場合）または２ノルム（禁止領域が楕円の場合）を表し、Ｑ_kは座標変換を示す２行２列の行列である。禁止領域の数をMとすると、どの禁止領域に入らないための条件は、全てのk (k=1,2,…,M)に対して

の値が正とならないことである。ただし、x(t)およびy(t)はそれぞれ、時刻tにおけるx軸およびy軸における軌道の座標である。禁止領域が平行四辺形の場合、Ｑ_kの値を適当に設定することにより、任意の形状で任意の回転角度を持った平行四辺形を指定することができる。禁止領域が楕円の場合、Ｑ_kの値を適当に設定することにより、任意の長径および短径を持ち任意の回転角度を持った楕円を指定することができる。

さらに、モーションの経路が作業領域ＡＷ内である必要がある。このための条件としては、

に対してr₀が正とならないことでるというように表すことができる。ただし、作業領域は平行四辺形とした。ここでもＱ_kは座標変換を示す２行２列の行列であり、作業域の平行四辺形の形状を指定するものである。

ここで最短時間制御問題を考える。すなわち、最短時間モーションを実現する経路を計算することを考える。算出するのは通過点(x_k,y_k) (k=1,…,N-1)および通過速度(v_k,w_k) (k=1,…,N-1)である。通過時間τ_k(k=1,…,N-1)については、それらの情報から前述の方法により一意に決定される。最適化問題は、数１２から数１５および数１７，数１８が０という拘束条件の下で数１１の値を最小化することになる。これは拘束条件付最適化問題であり、いくつかの解法がある。ここでは次の方法を用いる。

評価関数を

とし、この値を最小化する。操作変数は通過点(x_k,y_k) (k=1,…,N-1)および通過速度(v_k,w_k) (k=1,…,N-1)である。ただし、λ_kやη_x,η_y,η_v,η_wは十分に大きい正の定数とする。この最小化問題は直接探索法により解くことができる。しかしその場合、拘束条件がわずかに満たされない解が算出されてしまう。そこで、拘束条件を本来よりも少し厳しい条件にして再度数１９を最小化する解を求めることにより、本来の拘束条件を満たす最適解を算出することができる。

以上のように、初期点および初期速度、最終点および最終速度、作業領域および禁止領域、速度制約、加速度制約を設定し、最適化問題を解くことにより最適軌道を指定することができるわけだが、得られた最適軌道に対する通過点(x_k,y_k) (k=1,…,N-1)、通過速度(v_k,w_k) (k=1,…,N-1)および通過時間τ_k(k=1,…,N-1)は、あらかじめ記憶装置に通過点情報１として記憶させておく（図５）。ここまでの最適軌道の算出作業は、オフラインにより行われる。最適軌道の算出作業における禁止領域の設定、速度や加速度等における制約条件等は、オペレータとインタラクティブに行ってもよいし、自動的に行ってもよい。

モーションにおける実時間処理としては、モーションコントローラ２による補間作業およびサーボアンプ３ａ，３ｂによるモータ制御がある（図５）。上述のようにモーションコントローラ２は、予め決められた一連の通過点(x_k,y_k)およびその通過点における速度(v_k,w_k)および前の通過点からの通過時間τ_kを目標点信号Ｓ１として順次入力し、モーションコントローラ２は各軸のモーション補間を行う。各軸のモーション補間は数７から数１０に示す式に従い行う。そして1msごとに各軸の目標位置と目標速度を算出し、各軸のサーボアンプ３ａ，３ｂに指令値として出力する。

各軸のサーボアンプ３ａ，３ｂの校正例を図７に示す。位置目標値だけではなく、位置目標値に加えて速度目標値を同時に加えていることが従来のサーボアンプと異なっている。速度目標値信号に関しては、フィードバック系に対してフィードフォワードの形で入っているので、従来のものに比して目標値追従精度の向上が期待できる。また、このような目標値追従性能の向上は最適軌道を精度良く実現するためにも有用である。最適軌道にたいする追従誤差を少なくすることにより、禁止領域として設定する領域のマージンを減らすことができ、移動時間の短縮に寄与することになる。

本実施の形態においては、モーションコントローラ２は一連の通過点(x_k,y_k)およびその通過点における速度(v_k,w_k)および前の通過点からの通過時間τ_kを目標点信号Ｓ１として順次入力していたが、通過時間については一連の通過点(x_k,y_k)およびその通過点における速度(v_k,w_k)の情報から算出することができるので、目標点信号Ｓ１として通過時間τ_kの情報を入力せずに通過時間τ_kをモーションコントローラ２の内部で算出するようにしてもよい。そのときは、数１から数５に示す式を用いて通過時間τ_kを計算することができる。

本実施の形態においては、モーションコントローラ２は一定時間間隔ごとに各軸の目標位置と目標速度を出力していたが、一定時間間隔ごとの目標位置またはその変化分のみを出力するようにしてもよい。目標位置の時間変化から目標速度を推定することが可能であるからである。しかし、そのようにした場合、サーボアンプ３ａ，３ｂが認識する速度目標値に対して最適軌道との差が発生するので、目標位置を出力する時間間隔を十分に小さくする必要がある。

本実施の形態においては、サーボアンプ３は位置目標値と速度目標値を一定時間間隔ごとに入力していたが、位置目標値のみを一定時間間隔ごとに入力し、速度目標値を一連の位置目標値から推定し、推定した速度目標値を図７におけるサーボアンプ３の速度目標値としてもよい。速度目標値は、位置目標値の差分を用いてもよいし、位置目標値を２次補間して求めてもよい。

本実施の形態においては、モーションコントローラ２は一定時間間隔ごとに各軸の目標位置と目標速度を出力していたが、モーションコントローラ２は各軸のサーボアンプ３ａ，３ｂに対して、その軸の目標位置が一定量変化するごとにパルス信号（CWおよびCCW）を出力するようにしてもよい（図６）。そのようにすることにより、現在多く用いられているサーボアンプをそのまま用いることができる。

本実施の形態においては、軸数が２であったが、同様のことが３軸以上の場合についても行なうことができる。

（第２の実施の形態）
本発明第２の実施の形態は関節数に冗長性がない多関節ロボットに対する最短時間制御を実現するものである。すなわち、対象とするロボットのエンドエフェクタの自由度とロボットの軸数が等しい。直交軸ロボットにおいては、各軸における座標とエンドエフェクタの座標との関係が１次関数になっていたために、各軸の座標のみを考慮すればよかったが、関節ロボットにおいては、各関節軸の座標とエンドエフェクタの座標の関係が複雑な関数となる。そのため、禁止領域の扱いや、加速度に対する高速条件などの扱いが複雑になる。

作業領域および禁止領域はエンドエフェクタの位置（実空間）において設定する。関節数に冗長性がないので、本発明第１の実施の形態と同じように、経路の有限個の通過点と通過速度を設定する。実空間における通過点をx_k (k=1,…,N-1)、速度をu_k (k=1,…,N-1)とする。ただし、x_k、u_kともにベクトルとする。また、始点および終点に関するx₀、v₀、x_N、v_Nはあらかじめ条件として与えられる。すると、逆キネマティックスから、各関節の角度および角速度が角点において決定するので、それらをそれぞれθ_kおよびv _k (k=0,…,N)（ともにベクトルであり、始点および終点を含む）とする。通過点間の補間は本発明第１の実施例と同じ方法を用いて時刻に対する１次または２次補間を行う。すると、キネマティックスにより、エンドエフェクタの実空間における軌道を算出することができるの。その軌道をx(t)（ベクトル）とする。また、任意の時刻における関節に働くトルクT(t)（ベクトル）を求めることができる。各関節においても、許容される最大トルクが存在するので、最適軌道を算出する上において、最大トルクの条件も考慮する必要がある。

最適軌道を算出する上での拘束条件は次のようになる。まず、禁止領域についてはM個の禁止領域が存在するものとする。これらのどの禁止領域の中にエンドエフェクタが入らないための条件は全てのk (k=1,2,…,M)に対して

の値が正とならないことである。ただし、c_kは禁止領域の中心座標であり、o_kは１ノルム（禁止領域が菱面体等の場合）または２ノルム（禁止領域が楕円体等の場合）を表し、Ｑ_kは座標変換を示す行列である。

さらに、モーションの経路が作業領域内である必要がある。このための条件としては、

に対してr₀が正とならないことでるというように表すことができる。ただし、作業領域は直方体または超直方体とした。ここでもＱ_kは座標変換を示す行列であり、作業域の形状を指定するものである。

関節の最大速度に対しても制約を受ける。すなわち、各軸の最大速度を対角にならべた行列をAとしたとき、

の値が正とならない必要がある。

また、関節の最大トルクに対しても制約を受ける。すなわち、各軸の最大トルクを対角にならべた行列をBとしたとき、

の値が正とならない必要がある。

以上の制約の中で最短時間移動を実現する軌道を算出するために、評価関数を

とし、この値を最小化する。操作変数は通過点x_k (k=1,…,N-1)および通過速度u_k (k=1,…,N-1)である。ただし、λ_kやη_T,η_vは十分に大きい正の定数とする。この最小化問題は直接探索法により解くことができる。しかしその場合、拘束条件がわずかに満たされない解が算出されてしまう。そこで、拘束条件を本来よりも少し厳しい条件にして再度数１９を最小化する解を求めることにより、本来の拘束条件を満たす最適解を算出することができる。

以上のように、初期点および初期速度、最終点および最終速度、作業領域および禁止領域、速度制約、加速度制約を設定し、最適化問題を解くことにより最適軌道を指定することができるわけだが、得られた最適軌道に対する軸空間における通過点θ_k (k=1,…,N-1)、通過速度v_k(k=1,…,N-1)および通過時間τ_k(k=1,…,N-1)は、あらかじめ記憶装置に通過点情報１として記憶させておく（図５）。ここまでの最適軌道の算出作業は、オフラインにより行われる。最適軌道の算出作業における禁止領域の設定、速度や加速度等における制約条件等は、オペレータとインタラクティブに行ってもよいし、自動的に行ってもよい。

そして、モーションの実行時においては本発明第１の実施の形態と同様の動作を行い、所望の関節ロボットに所望のモーションを行わせる。

部品の移送や工作などを行なうロボットや工作機において、作業時間を短くするモーションを行なわせることが可能となる。

提案手法の概念図。 従来のモーション指定方法 提案手法のモーション指定方法。図１と同じであるが、記号が一部異なる。 速度補間の説明図。 モーション制御システムの第１の構成。 モーション制御システムの第２の構成。 サーボアンプの構成図。

符号の説明

ＡＷ・・・作業領域
ＡＰ１〜ＡＰ４・・・禁止領域
１・・・通過点情報
２・・・モーションコントローラ
３，３ａ，３ｂ，３ｃ，３ｄ・・・サーボアンプ
４，４ａ，４ｂ，４ｃ，４ｄ・・・モータ
５，５ａ，５ｂ，５ｃ，５ｄ・・・ロータリエンコーダ
３１・・・位置制御器
３２・・・速度制御器
３３・・・電流制御器
３４・・・微分器
３５・・・インバータ
３６・・・カウンタ
３７・・・電流検出器
Ｓ１・・・通過点信号
Ｓ２ａ，Ｓ２ｂ・・・移動指令信号（位置および速度指令信号）
Ｓ２ｃ，Ｓ２ｄ・・・移動指令パルス
Ｓ４ａ，Ｓ４ｂ，Ｓ４ｃ，Ｓ４ｄ・・・モータ駆動電圧
Ｓ５ａ，Ｓ５ｂ，Ｓ５ｃ，Ｓ５ｄ・・・エンコーダ出力

Claims (13)

1. エンドエフェクタを持つ多軸サーボシステムに対して前記エンドエフェクタの軌道を算出するものであって、
   前記エンドエフェクタの移動範囲である作業領域と、
   前記作業領域内において前記エンドエフェクタが侵入することが許されないゼロまたは一つまたは複数の禁止領域を設定し、
   さらに各軸に対して許される速度および加速度の組み合わせの範囲を制約条件として設定し、
   前記エンドエフェクタの初期位置および初期速度および最終位置および最終速度を与えると各軸の通過軌道により構成される前記軌道を算出する機能を有し、
   算出される前記軌道は、前記エンドエフェクタが通過する複数の通過点の座標および前記通過点を通過する各軸の速度および連続する二つの前記通過点を通過する通過時間から構成され、
   前記軌道を算出する手段は、
   前記座標および前記各軸の速度を、最適化の対象となる変数とし、
   前記変数の値に基づき前記軌道における前記通過点を含む全体の軌跡を前記通過点の間を前記通過点の座標および通過する速度により決まる態様に応じて補間することにより算出し、
   前記全体の軌跡に対して見積もった所要時間に関する項と、
   前記作業領域および前記禁止領域に係る拘束条件並びに前記制約条件を、
   満たす場合のみゼロとなり、満たさない場合正の値をとる関数を設定し、
   前記関数に正の係数を乗じた項と、
   を含んだ評価関数を設定し、
   前記評価関数を最小化する前記変数の組を直接探索法を用いて算出することであることを特徴とするモーションプランナ。
2. 請求項１のモーションプランナと、当該モーションプランナにより算出された前記軌道を
   入力情報とする２軸以上のモーションコントローラと、を備えた制御システムであって、
   当該モーションコントローラは、前記入力情報の内容であるいくつかの前記通過点に係る
   情報は、通過点の座標と、前記通過点を通過する各軸の速度と、連続する前記通過点を通
   過する通過時間の組み合わせからなり、前記入力情報を元に前記通過点を結ぶ軌道に係る
   信号を算出し出力することを特徴とする、制御システム。
3. 前記モーションコントローラにおいて、連続する二つの前記通過点の間の軌跡における各
   軸の速度は、連続する二つの前記通過点を通過するのに要する時間が前記通過時間と等し
   くなるように時刻に対して２次関数となるように補間することを特徴とする、請求項２に
   記載の制御システム。
4. 前記モーションコントローラは２軸以上の軸を持ち、いくつかの通過点情報が与えられる
   と前記通過点を結ぶ通過軌跡に掛かる信号を出力するものであり、前記通過点情報は通過
   点の座標と、前記通過点を通過する各軸の速度の組み合わせからなり、連続する二つの前
   記通過点の間を通過する通過時間は前記連続する二つの前記通過点における前記通過点情
   報より算出されることを特徴とする、請求項２または請求項３に記載の制御システム。
5. 前記モーションコントローラにおいて、前記通過軌跡に掛る信号は、各軸に対するパルス
   列であることを特徴とする、請求項２〜４のいずれかに記載の制御システム。
6. 前記モーションコントローラにおいて、前記通過軌跡に掛る信号は、一定時間間隔ごとの
   各軸の座標であることを特徴とする、請求項２〜４のいずれかに記載の制御システム。
7. 前記モーションコントローラにおいて、前記通過軌跡に掛る信号は、一定時間間隔ごとの
   各軸の座標および各軸の速度であることを特徴とする、請求項２〜４のいずれかに記載の
   制御システム。
8. 請求項１に記載のモーションプランナにより各軸の前記通過軌跡を算出し、前記算出された前記通過軌跡をエンドエフェクタの目標軌道とすることを特徴とする多軸サーボシステム。
9. 請求項２〜７のいずれかに記載の制御システムにおけるモーションコントローラを含むことを特徴とする請求項８に記載の多軸サーボシステム。
10. 直交軸ロボットを対象とすることを特徴とする請求項１に記載のモーションプランナ。
11. 直交軸ロボットを対象とすることを特徴とする請求項８〜９のいずれかに記載の多軸サ
    ―ボシステム。
12. 非直交軸ロボットを対象とすることを特徴とする請求項１に記載のモーションプランナ。
13. 非直交軸ロボットを対象とすることを特徴とする請求項８〜９のいずれかに記載の多軸サーボシステム。