JP6167770B2 - 多軸型ロボットの軌道生成方法及び多軸型ロボットの制御装置 - Google Patents

Description

本発明は、多軸型ロボットの動作速度を向上させる軌道生成方法及び制御装置に関する。

現在、生産設備等の現場において、多種多様な産業用ロボットが使用されている。この産業用ロボットとして、関節（軸）を複数有する所謂多軸型ロボットがある。この多軸型ロボットは、溶接、塗装、部品組立、部品搬送などの作業に使用され、それが担う作業の性質上、作業の始点から終点までの動作時間の短縮化、即ち、動作速度の高速化が求められている。

従来、多軸型ロボットにおける動作速度の高速化を図るために、動作の加減速を制御する方法が知られている。この方法には、例えば、基本的な動作速度パターンを台形にして、ロボットの姿勢に応じた最大加速度を計算し速度パターンを修正する方法や、バンバン（Bang-Bang）制御のように最大原理に基づく最大トルクパターンを用いて動作させる方法などがある。

しかし、前者については、台形状の動作速度パターンの上辺部分に相当する動作期間では制約で加速度がゼロになり、その動作速度パターンの肩部分（左右の両斜辺）に相当する加減速を行う期間において加減速を最大にできないという問題がある。この台形速度パターンでは、その肩の部分に相当する期間で加速度が一定になるが、実際のロボットの姿勢は時々刻々と変化しているので、最大加速度は非線形に変化している。すなわち、本来非線形で最大まで加速できるにもかかわらず線形で加速しているため、モータの性能を使い切れていないことになる。

また、後者については、実際には動作速度の上限（拘束条件）による制約があるにも関わらず、これを無視することになる。上述した加減速制御に関連する従来技術として、例えば特許文献１〜３に記載されたものが知られている。

特開２０００−９４３７１号公報 特開２００２−１３２３４９号公報 特開２００２−３２１１７８号公報

上述した特許文献１〜３に記載された制御法の概略及びその問題点を説明する。特許文献１には、台形速度パターンにおける加速度を、ワークの重量，イナーシャに応じて変化させる制御が示されている。しかしながら、この制御の場合、ワークの着脱時に計算した加速度で一定加速させるため、台形速度パターンの属性である上述した限界を超えることができない。
特許文献２に記載された制御も、台形速度パターンの加減速を行う期間について、実際の加速度曲線が予め設定しておいた制限加速度曲線に極力一致するように調整し、より大きな加速度を出力可能としたものである。

しかしながら、予め設定する制限加速度曲線をどのようにして求めるかが不明であり、さらにその曲線は軸毎、移動方向毎、及び加減速毎にパラメータとして設けられている。したがって、この手法では、（１）パラメータが複数できてしまい管理が煩雑となり、また（２）パラメータは離散値なので厳密に加速度曲線に沿うことはできない。さらに、（３）最大加速度はロボットの姿勢によって変化するが、この点が考慮されていない。よって、この制御法方法は汎用的とは言えない。

特許文献３には、動作の始点，加速度終了点，減速開始点，終点のそれぞれにおいて、ロボットの各軸に関するダイナミクスを考慮して動力学方程式を解くことで、加速時間，減速時間を最短にする制御法が示されている。しかしながら、この制御法の場合、上記の始点、終了点、開始点、及び終点以外の動作点については、加減速時間を最短にすることができない。

本発明は上記事情に鑑みてなされたもので、その目的は、軌道生成のための処理を簡単化でき、かつ、動作時間をより短縮化できる多軸型ロボットの軌道生成方法及び制御装置を提供することにある。

請求項１記載の多軸型ロボットの軌道生成方法によれば、先ず位置指定ステップにおいて、手先の始点及び終点の位置を指定すると、軌道生成ステップでは、始点から終点に向けて手先の移動を開始させるときに、第１のリンクに、第２のリンクの動作によって、第１のリンクを駆動する第１のジョイントが発生する加速力を増加させる反作用を生じさせる。また、手先を終点に停止させるときには、第２のリンクの動作によって、第１のリンクに、第１のジョイントが発生する減速力を増加させる反作用を生じさせるように、第１のジョイントの速度パターン及び第２のリンクを駆動する第２のジョイントの速度パターンを生成することで手先の軌道を生成する。

従来の多軸型ロボットの制御では、例えばピック・アンド・プレイスのような単純な作業を行わせる場合、始点、終点間で動作量に変化がない軸は動かない。この状態では、手先側アーム（第２のリンク）は基部側アーム（第１のリンク）の負荷となっているだけであり、基部側アームの負担が大きく、また、動作速度の上昇を図るためには障害となっている。

これに対して、上述したように軌道を生成すると、基部側アームの先端側で、手先側アームが基部側アームに対し反作用を生じるように動作する。したがって、手先の起動時に必要な加速力と、停止時に必要な減速力とをそれぞれ増加させて、手先を始点から終点まで移動させる速度を向上させ、移動時間を短縮することができる。ここで、
（最大関節加速度）＝（最大関節トルク＋干渉トルク＋重力トルク＋摩擦トルク）
／イナーシャ …（Ａ）’
（最大関節加速度）＝（最大関節トルク＋干渉トルク）／イナーシャ …（Ａ）
（最大関節速度）＝（モータ最大速度）／（減速比） …（Ｂ）
である。ただし、ここでは説明簡略化のため水平方向の動作のみを考えることとし、（Ａ）’式において重力トルク及び摩擦トルクを無視したものを（Ａ）式とする。例えばバンバン（Bang-Bang）制御のように、常に何れかの軸が最大トルクを出力することのみ考慮し、速度の上限を考慮しない場合は、（Ａ）式に基づきアームを折り畳みイナーシャを小さくすることで、加速度を向上させることによる動作短縮が期待できる。

しかしながら、現在多く普及しているロボットの大半は減速比が高く、（Ｂ）式より最大関節速度が低いため直ぐに上限に達してしまう。このため、動作時間を短縮するには加減速を急峻に立ち上げるしかない。加減速度を向上させるには、（Ａ）式よりイナーシャを小さくすること、干渉トルク項の符号を関節トルク符号と同じ（アシストする方向）にすることが有効であると判る。つまり動作としては、加減速時は手先側アームを基部側アームの動作方向と逆向きに振ることに対応する。

このとき、干渉される基部側アームの軸は、その軸を回転させるモータを前述した台形の速度パターンの上限を超えて加速できる。つまり、そのモータが有している性能をフルに発揮でき、従来以上の加速を出すことができるので、動作時間を短縮できる。逆に言えば、従来の手法と同じ移動時間で動作させるのであれば、従来と同じ加速度を出力するのに少ないモータトルクで済むため、モータ容量を下げることができ、ロボットの小型、低コスト化に貢献できる。そして、上述した軌道は、多軸型ロボットの動力学に基づいて計算された結果として生成されるので、ロボットの拘束条件を満たす範囲内で速度を最大化する軌道が求められる。したがって、現実的に使用できる軌道が得られる。

請求項２記載の多軸型ロボットの軌道生成方法によれば、軌道生成ステップは、第１及び第２のジョイントの駆動に関する拘束条件を加味して速度パターンを演算するステップを含む。これにより、ロボットの拘束条件を満たす範囲内で速度を最大化する軌道が求められるので、現実的に使用できる最適化された軌道が得られる。

請求項３記載の多軸型ロボットの軌道生成方法によれば、前記拘束条件は、第１及び第２のジョイントのそれぞれについて設定されている最大トルク及び最小トルクから成るトルク条件と、第１及び第２のジョイントのそれぞれについて設定されている最大速度及び最小速度から成る速度条件とを含む。したがって、第１及び第２のジョイントのそれぞれについてトルク条件及び速度条件を満たす範囲で軌道を生成できる。

請求項４記載の多軸型ロボットの軌道生成方法によれば、始点と終点との間に複数の初期経由点を自動的に設定するステップを有し、軌道生成ステップは、初期経由点の位置を更新しながら最短時間の速度パターンを生成するので、始点から終点への移動時間が最短となる速度パターンの軌道を生成できる。

請求項５記載の多軸型ロボットの軌道生成方法によれば、初期経由点を３点設定する。ここで、手先を始点から終点まで移動させることを想定すると、第１のジョイントにおいて、起動直後に加速度を付与する時点と、動作途中で加速度の方向が反転する時点と、停止直前に加速度をゼロにする時点とがある。そして、軌道の最適化を図る過程では、少なくとも上記３つの時点に対応する経由点を最適化することが重要になると考えられる。したがって、上記３つの時点に対応する経由点を設定すれば、演算量を抑制しつつ効率的に、移動時間を最短にする軌道を生成することが可能となる。これにより、従来の方法では、方法自体の限界によって引き出すことができなかったモータ，アクチュエータのポテンシャルを最大限引き出しつつ、現実的に使用できる最適化された軌道が得られるようになる。

請求項６記載の多軸型ロボットの軌道生成方法によれば、適用する多軸型ロボットの構成を以下のように限定する。すなわち、ボールねじ軸からなる直動軸（第１軸）と、この直動軸に沿って移動可能に配置される移動体と、この移動体に搭載される回転軸（第２軸）を基点として回動可能に配置される回動アームとを備える２軸ロボットとする。このような２軸ロボットを制御する際に設定される動作速度の上限値は、所謂台形パターンと称されるもので規定され、最大速度は移動体の位置に関わらず一定となっていた。

しかしながら、実際の２軸ロボットについて、上限速度，或いは限界速度，危険速度として評価される速度は、移動体の直動軸上の位置に応じて異なっている。つまり、従来のように速度を制限した状態では、ロボットの性能を十分に引き出せていなかった。そこで、請求項６の方法では、移動体の最大速度についての拘束条件を、少なくとも一部の範囲について、直動軸上における移動体の位置に応じて可変設定することで、前記位置によっては上限速度をより高く設定し、従来よりも移動時間を短縮させて作業効率を向上させることができる。

請求項７記載の多軸型ロボットの軌道生成方法によれば、移動体の位置に応じて求められるボールねじ軸の危険回転速度ＮｄとＤＮ値とを比較して、より小さい値を示す方を最大速度の拘束条件とする。すなわち、ボールねじ軸を回転させて移動体を直線移動させる構造については、ボールねじ軸の許容回転速度が上記のように規定される。そして、危険回転速度Ｎｄは、直動軸上における移動体の位置をパラメータとする演算で決まるので、その位置に応じて最大速度を設定できる。

各実施例において実行されるロボットの最短時間の動作軌跡を求めるための処理手順を概略的に示す図 各実施例に係る、ロボットのアームの移動を制御する方法の原理を従来例と比較して説明する図 第１の実施例にて示すロボットの全体構成を概略的に示す斜視図 第１の実施例にて示すロボットの制御系統を示す電気的な機能ブロック図 図１に示す処理に対応して制御部が実行する、速度パターンを作成するためのフローチャート 図５の処理の一部として実行される、拘束条件を加味したボブロー法による速度パターンの作成の詳細を示すフローチャート ロボットアームの右手系，左手系を説明する図 手先の座標位置を各軸の角度に変換して速度パターンの計算を行う理由を説明する図 加速度の拘束条件を説明する説明図 ＸＹ平面上で第１軸，第２軸及び手先の座標が変化する軌跡を示す図 （ａ）は図１０（ａ）〜（ｅ）に示す５つの手先位置を同時にプロットした図、（ｂ）は始点から終点まで移動する間の、第１軸，第２軸それぞれの角度，角速度，角加速度，トルクの変化を示す図 第２の実施例において制御部が実行する、速度パターンの準備処理説明するフローチャート 図１２に示す準備処理の一部として、拘束条件を加味して実行されるボブロー法に従う速度パターンの準備処理を詳細に説明するフローチャート 変形例に係る３軸のアームロボットを模式的に説明する図 ３軸のアームロボットに本願の軌道生成法を実施した時のシミュレーションの図 別の変形例に係る多軸ロボットに採用されるモータのＴＮ曲線を説明する図 別の変形例に係る２軸のＸＲロボットを概略的に示す平面図 ＸＲロボットにおける本願の軌道生成方法の効果を説明する模式図 第３の実施例として、ＸＲロボットの機構的数値パラメータを説明する図 シミュレーション結果を示す図 ＸＲロボットの各軸動作を同期させる場合を説明する図

以下、図面を参照して、本発明に係る多軸型ロボットの軌道生成方法及び多軸型ロボットの制御装置の様々な実施例を説明する。
［第１の実施例］
図１〜図１１を参照して、第１の実施例に係る多軸型ロボットの軌道生成方法及び多軸型ロボットの制御装置を説明する。先ず、本実施例の多軸型ロボットの制御法の概要を、図１，２を参照して説明する。最初に図２を参照して、本実施例に係る制御法を従来の制御法と比較しながら説明する。

図２（ａ）に示すように、第１及び第２アーム（リンク）ＡＭ１，ＡＭ２を２つのジョイントＪＴ１，ＪＴ２で可動可能に結合した簡単な２軸（Ｊ１，Ｊ２）ロボットを想定する。このロボットのアーム手先ＥＦを、Ｘ−Ｙ平面上において（Ｊ１，Ｊ２）＝（０°，０°）の角度位置から（Ｊ１，Ｊ２）＝（９０°，０°）の角度位置まで移動させることを想定する。

この場合の最も単純な手法は、図２（ｂ）に示すように、第２のアームＡＭ２（即ち第２軸Ｊ２）は動作させず、第１のアームＡＭ１（即ち第１軸Ｊ１）のみを９０度回転させるものである。この手法では、第１のアームＡＭ１のみを動作させるため、負荷が偏り、第１のアームＡＭ１の負担が大きく、また、アーム全体の動作半径が常に最大でイナーシャが大きくなる。このため、動作速度が遅くなるという問題がある。

図２（ｃ）は前述したバンバン（Bang-Bang）制御による動作である。この場合、第１のアームＡＭ１と共に第２のアームＡＭ２も動作させてアームを折り畳む。これにより、イナーシャが小さくなるため高速で動作させることができる。しかしながら、ロボットに課されている速度の制約等の拘束条件を全く考慮していないので、実際にこの様に動作させて動作時間の短縮を図ることは不可能である。

本実施例では、概略的に図２（ｄ）に示すような動作軌跡を実現させる。すなわち、手先ＥＦの移動を開始させると、最初に第１のアームＡＭ１（第１軸Ｊ１）を終点方向に回転させると共に（矢印Ａ参照）、第２のアームＡＭ２（第２軸Ｊ２）を終点方向と逆方向に回転させる（矢印Ａ´参照）。このとき、アームの形態は所謂左手系となる。
その後、第１のアームＡＭ１を回転させている途中で（矢印Ａ参照）、第１のアームＡＭ１を基準として見た場合に、手先位置（第２アームＡＭ２の先端位置）が第１のアームＡＭ１の先端位置よりも先行して終点側に近づくように、第２のアームＡＭ２をその回転方向に振り出させる（矢印Ｂ´参照）。このとき、アームの形態は所謂右手系となる。

すなわち、図２（ｂ）の動作の場合、第１のアームＡＭ１を回転中心とする半径上に常に第２のアームＡＭ２と手先ＥＦとが位置する。これに対して、図２（ｄ）の動作の場合、最初は第２のアームＡＭ２が始点方向の側に回転する（矢印Ａ´）ことで手先の回転が遅れる。しかし、その後、第２のアームＡＭ２が終点方向の側に、第１のアームＡＭ１よりも先行して回転する。つまり手先ＥＦが、第１のアームＡＭ１よりも先行した状態となる。
この様に、２つのアームＡＭ１，ＡＭ２（第１軸及び第２軸）の動きを協働させることで、図２（ｂ）の動作に比較して、２つのジョイントＪＴ１，ＪＴ２のモータに掛かる負荷を分散させることができる。また、アームが若干折り畳まれるのでイナーシャがより小さくなり、モータに掛かる負荷そのものが減少する。

加えて、第２アームＡＭ２の動作が第１アームＡＭ１、すなわち第１軸に反作用に伴う力を生じさせる。つまり、手先ＥＦが始点から移動するときには、第２のアームＡＭ２の一時的な逆方向への回転（矢印Ａ´）の反作用に伴い、矢印Ａ″で示す付勢力が第１のアームＡＭ１に加わる。この付勢力Ａ″は、第１のアームＡＭ１の終点側への回転を加勢する向きに加わる。反対に、手先ＦＥが終点に接近してくると、第２のアームＡＭ２のそれまでの回転方向への先行する運動（矢印Ｂ´）の反作用に伴い、矢印Ｂ″で示す抑制力が第１のアームＡＭ１に加わる。つまり、この抑制力Ｂ″は、第１のアームＡＭ１のそれまでの回転（矢印Ａ）を抑制しようとする向きに働く。

このように、手先ＥＦが回転を開始するとき及び回転を終了させるときのそれぞれにおいて、第２のアームＡＭ２の一時的な動作により生じる反作用の力、即ち付勢力及び抑制力が、第１のアームＡＭ１の回転の加速及び減速を積極的にアシストする。このため、手先ＥＦを（Ｊ１，Ｊ２）＝（０°，０°）の角度位置から（Ｊ１，Ｊ２）＝（９０°，０°）の角度位置まで移動させるのに要する時間は、上述したアシストが無い制御と比べて、格段に短縮される。

また、この様にアーム動作をさせるには、予めアーム、すなわち、各軸のモータを回転させるための速度パターンを生成しておく必要がある。この速度パターンを生成するに当たっては、後述する具体的な手法により、ロボットの拘束条件を満たすことを前提として、最も効率の良い動作軌道を選択する。

図１は、図２（ｄ）に示すような動作軌跡を実現させるための処理手順を概略的に示したものである。先ず、ロボットの手先を、始点から終点まで移動させる間に、複数の経由点を仮に設定する（図１（ａ））。そして、手先の軌道を計算するためにボブロー（Bobrow）法を用いることで、各経由点を通過しながら手先を移動させるのに最速となる速度パターンを演算する（同図（ｂ））。このボブロー法を用いた演算を行うに当たっては、実際のロボットの運動方程式が考慮されるため、各軸の動作速度やトルクの制限など、ロボットの拘束条件を満たした上で最速となる速度パターンが求められる。

速度パターンを求めると、その速度パターンに従ってロボットを所定の始点から終点まで動作させた場合の動作時間を算出する（同図（ｃ））。そして、経由点の位置を更新しながら（同図（ｄ））上述したプロセスを繰り返し実行し、動作時間が最短に収束したと判断した時点で処理を終了する。この一連の処理は、多軸ロボットの各軸に対して実行される。その結果として得られた軌道が、ロボットの拘束条件（速度＜速度制限値、且つ、トルク＜トルク制限値）を満たした上で、手先を始点から終点まで最短時間で移動できる軌道となる。このようにして求めた手先の経由点及び各軸の速度パターンの情報（軌道情報）は、ロボットコントローラに格納されて実際の多軸ロボットの制御に使用される。

次に、図３ないし図１１を参照して、上述した多軸ロボットの軌道情報を生成方法及びその軌道情報を使用した多軸ロボットの制御装置の一例を詳述する。図３（ａ）は、４軸ロボット１（多軸型ロボット）の全体構成を概略的に示している。直線移動レール２には、その下面側に移動体２ａ（基部側アーム）が第１のリンクとして、第１軸（Ｘ軸）であるＸ方向に沿って直線移動可能に支持されている。また、この移動レール２には、その移動体２ａを自在に移動させるための駆動機構が設けられている。詳しく図示はしないが、前記駆動機構は、直線移動レール２の一端部に配設された直線移動用モータ３や図示しないボールねじ機構などから構成されている。ここで、第１のジョイントは、直線移動レール２と移動体２ａとが連携する構成部分が対応する。

前記移動体２ａの下面部にはアーム支持ベース４が連結される。前記アーム支持ベース４の下面に水平方向に延びる水平旋回アーム５（手先側アーム）が第２のリンクとして、その一端部である基端側が第２軸（垂直軸，Ｒ軸，第２のジョイント）を中心にＲ方向に旋回可能に取付けられている。この水平旋回アーム５の前記垂直軸とは反対側の端部である先端部（先端面）に、昇降体６が第３のリンクとして上下動可能（第３軸，Ｚ軸）に取付けられている。この昇降体６の下面側に、手首部７が第４のリンクとして垂直軸（第４軸，Ｔ軸）を中心に同軸回転可能に取付けられている。また、手首部７には、ワークを把持するためのチャック（ハンド）等の作業用ツールが着脱可能に取付けられるようになっている。

この場合、前記水平旋回アーム５は、アーム支持ベース４内に設けられた駆動源であるアーム回転用モータ８（図４参照）及び回転伝達機構により水平方向に無限回転（旋回）可能であり、前記昇降体６は、水平旋回アーム４に内蔵されたモータ及びラック−ピニオン機構によって水平旋回アーム５の先端にて上下移動され、前記手首部７は、昇降体６に内蔵されたモータによって同軸回転される。

上記各ロボット１の動作（直線移動モータ３を含む各軸のモータ）は、それぞれ図４に示すように、ロボットコントローラＲＣの制御部９により作業工程プログラム等に基づいて制御されるようになっている。この制御部９（制御装置，制御手段）は、ＣＰＵ，ＲＯＭ，ＲＡＭなどを備えて構成され、ティーチングペンダント１０が接続されており、このティーチングペンダント１０には、スイッチ群１１が設けられていると共に、表示手段たる表示部１２が設けられている。

また、図３（ｂ）には、ロボット１の物理的パラメータ（動力学パラメータ）をＸＹ平面上に示している。尚、Ｙ軸は上記Ｘ軸に直交しており、図３（ａ）に示す正面側を座標の正側としている。図中に示すｍ１，ｍ２及びｍ３は、それぞれ第１のリンクの質量［ｋｇ］、第２のリンクの質量［ｋｇ］、及び「第３及び第４のリンク並びに負荷」の総質量［ｋｇ］である。ｌｇ２［ｍ］は、第１のリンクの軸位置からの第２のリンクの重心までの距離，ｌ２［ｍ］は第２のリンクの長さである。また、Ｉ２［ｋｇｍ^２］は第２のリンクのリンク重心回りのイナーシャ，Ｉ４［ｋｇｍ^２］は、第３のリンク及び第４のリンクの総重心回りのイナーシャである。また、θ１は第１軸の関節位置，θ２は第２軸の関節角度である。

上述した多軸ロボット１は４軸であり、本実施例に係る軌道生成方法及び制御法は、これらの４軸のうち、何れかの２軸及び３軸以上の複数軸について実施できる。しかしながら、以下の説明では、軌道生成及び制御の原理を判り易く説明するため、第１及び第２軸のみを計算対象として説明する。その説明を元に、３軸以上の軌道生成及び制御の例を後述する。

ここで、以下の説明で使用する物理量θ，λに関して、以下のような定義を与えておくことにする。
θ ： ロボットの位置を示す、各軸の関節角［ｒａｄ］である。
θ´： ロボットの速度を示す、各軸の関節角速度［ｒａｄ／ｓ］である。
θ″： ロボットの加速度を示す、各軸の関節角加速度［ｒａｄ／ｓ^２］である。
λ ： 軌道上の各位置を示す軌道パラメータである。
λ´： 軌道上の各位置における傾きであり、軌道の速度を示す。
λ″： 駆動上の各位置における曲率を示す、軌道の加速度を示す。

上述した多軸ロボット１の運動方程式の一般表記は（１）式となる。

但し、τはトルクベクトルである。そして、（１）式における慣性行列Ｈと、遠心力・コリオリ力を表す行列ｈとに、ロボット１の物理的パラメータが反映されている。ここで、

また、（１）式におけるＢは摩擦，ｇは重力であるが、以下ではこれらを無視して説明する。

尚、ロボットの運動方程式の詳細については、以下の文献（１）を参照できる。
・＜文献（１）＞Proceedings of the 2006 IEEE/RSJ International Conference on Intelligent Robots and Systems October 9-15,2006,Beijing China “Time-Optimal Trajectory Generation of Fast-Motion Planar Parallel Manipulator” by Yanjie Liu, Chenqi Wang, Juan Li, Lining Sun
図５は、図１に示す処理に対応して制御部９が実行するフローチャートである。先ず、制御部９は、例えばオペレータより与えられたロボット１の手先の始点，終点を読み込むと（ステップＳ１，位置指定ステップ，位置指定手段）、その始点，終点の間の３点を、初期経由点（補間点）として決定する（ステップＳ２，作成手段）。これらの初期経由点の決定には、例えば線形補間を用いる。例えば、始点座標が（０，０），終点座標が（３０，１００）であれば、初期経由点を（７．５，２５），（１５，５０），（２２．５，７５）とする。

次に、制御部９は、始点，終点及び上記３点の初期経由点の間を、例えばスプライン補間等により補間処理して（ステップＳ３）、補間後の軌道についてボブロー法を用いて移動時間が最短となる速度パターンを作成する（ステップＳ４）。尚、ボブロー法の詳細については、以下の，文献（２）（３）を参照。
・＜文献（２）＞The International Journal of Robotics Research,Vol.4, No.3,Fall 1985 p3-17“Time-Optimal Control of Robotic Manipulators Along Specified Paths” by J. E. Bobrow, S. Dubowsky, J. S. Gibson
・＜文献（３）＞IEEE JOURNAL OF ROBOTICS AND AUTOMATION,VOL.4,NO.4,AUGUST 1988 P443-450 “Optimal Robot Path Planning Using the Minimum-Time Criterion” by JAMES E. BOBROW

次いで、制御部９は、ロボット１の手先（今の例では第２軸、すなわち第２アームの先端）を、補間した軌道について作成した速度パターンに従い移動させた場合の動作時間を計算し（ステップＳ５）、その動作時間と前回に計算した動作時間との差を求める。そして、制御部９は、両者の差が所定の閾値（例えば０．００１ｓ）未満に収まっているか否かを判断し（ステップＳ６）、収まっていなければ（ＮＯ）ステップＳ２で求めた経由点を、例えば滑降シンプレックス法等を用いて更新する（ステップＳ７）。勿論ここでは他の手法，例えば勾配法や遺伝アルゴリズム等を用いても良い。

経由点を更新すると、制御部９の処理はステップＳ３に戻り、ステップＳ７までの処理を繰り返す。そして、ステップＳ６において、今回と前回の動作時間の差が所定の閾値未満に収まれば（ＹＥＳ）処理を終了する。尚、ステップＳ３〜Ｓ７が軌道生成ステップ（生成手段）に対応する。

次に、ステップＳ５で行うボブロー法を用いた計算処理について、図６を参照して説明する。先ず、ステップＳ３で決定した手先の軌道を示す（３）式
ｘ＝ｆ（λ） …（３）
より、軌道パラメータλを手先位置ｘ（ベクトル）に変換する（ステップＳ１１）。尚、式に付している番号は、文献（１）に記載されているものについては文献（１）の番号に合わせている。軌道ｆ（λ）は、例えば３次スプラインで補間した場合は次式となる。
ｆ（λ）＝ａλ^３＋ｂλ^２＋ｃλ＋ｄ …（３ａ）
但しλは、始点から終点までの座標の増分変数である。

続いて、逆運動学より、手先位置ｘを各軸（アーム）の角度ｑに変換する（ステップＳ１２，変換手段）。
ｑ＝ｐ^−１（ｆ（λ）） …（４）
但しｐは、順運動学を示す関数である。ここで手先位置ｘを角度ｑに変換するのは、手先の軌道を求める際に、図７に示すように、ロボット１のアームの形態である手系（図７（ａ）は左手系，図７（ｂ）は右手系）を限定することなく求められるからである。

すなわち、図８において説明するように、座標位置を用いて図６に示す計算を行うとした場合、軌道の途中でアームの形態を変化させることを想定すると（図８（ａ），（ｂ）参照）、形態が変化したところで拘束条件が不連続となり、計算結果が得られなくなってしまう。そのため、座標位置を用いて計算を行う場合には、アームの形態を変化させないで行う必要がある（因みに文献（２），（３）においても、ロボットのアーム形態を、最初に決定された形態から変更することなしに最速となる速度パターンを求めている）。

これに対して、本実施例のように、予め手先の座標位置を各軸角度に変換してから計算を行うようにすれば、軌道の途中で形態が変化しても拘束条件が不連続とならず、計算結果を得ることができる。したがって、アーム形態に制約されることなく最速となる速度パターンが求められる。
そして、（１）式に（４）式を代入すると、（５）式が得られる（ステップＳ１３）。

尚、Ｊはヤコビアンである。また、（５）式において摩擦Ｂ，重力ｇを夫々含むパラメータａ３，ａ４の項は「ゼロ」となる。

次に、ステップＳ１４において、制御部９は、（５）式より求められる最大加速度λ″max，最大減速度λ″minを計算する（これらについては、文献（１）の（８）〜（１０）式を参照）。尚、「λ」に付した「²」の記号は文献（１）において加速度を示すためのダブルドットに替わるものである。ここで求められる最大加速度λ″max，最大減速度λ″minが、ロボット１の動作の加速度に関する拘束条件となる。

次いで、制御部９は、軌道上の次の位置λ及び速度λ´を、最大加速度λ″max(又は最大減速度λ″min)を用いて（１１）式，（１２）式により計算する（ステップＳ１５）。ここで、時間ｔはサンプリングタイム（例えば１ｍｓ）であり、λ０，λ´０は現在の位置及び速度である。また「λ」に付した「´」は一階微分を表し、文献（１）において速度を示すためのドットに替わるものである。
λ＝０．５λ″maxｔ^２＋λ´ｔ …（１１）
λ´=λ″maxｔ …（１２）

次に、制御部９は、軌道パラメータλの速度λ´と加速度（減速度）λ″がそれぞれの拘束条件を満足するか否かを判定する（ステップＳ１６）。つまり、制御部９は、
λ´min≦λ´≦λ´max …（１３）
λ″min(λ，λ´)≦λ″≦λ″max(λ，λ´) …（１４）
の両条件を同時に満足するか否かを判定する。なお、加速時か減速時かに応じて、λ´min又はλ´max、及び、λ″min又はλ″maxが使用される。

図９は、上記文献（２）のＦｉｇ．２を基に模式的に表すもので、２軸ロボットについて、軌道パラメータλの加速度(減速度)λ″に関する拘束条件を満たす範囲を示している。すなわち、各軸(各関節に配置されるモータ)について規定されている最大加速度〜最大減速度の範囲でＡＮＤ条件を採った許容範囲λ″min〜λ″maxが設定されている。

また、軌道パラメータλの速度λ´に関する拘束条件の限界値λ´min、λ´maxは、
λ´min＝max(ｑ´min／ｆ´(λ)) …（１５）
λ´max＝min(ｑ´max／ｆ´(λ)) …（１６）
である。ここで、ｆ（λ）は軌道を示し、ｑ´min、ｑ´maxはロボット１の仕様より決まる最小及び最大の関節速度である。これらの速度条件及び加速度（トルク）条件は、拘束条件として必要な最小限の条件であり、状況に応じてこれ以外の条件を追加してもよい。

上述のステップＳ１６において、軌道パラメータλの速度λ´と加速度（減速度）λ″が式（１３）、（１４）の拘束条件を共に満足していれば（ステップＳ１６、ＹＥＳ）、制御部９は、位置λをステップＳ１５で計算した値に更新する（ステップＳ１７）。次いで、制御部９は、位置λが終点λendに到達したか否かを判断し（ステップＳ１８）、到達していなければ（ＮＯ）、処理をステップＳ１４に戻し、到達していれば（ＹＥＳ）処理を終了する。

一方、ステップＳ１６において、速度λ´と加速度（減速度）λ″が式（１３），（１４）の少なくも一方の拘束条件を満足していない場合（ステップＳ１６，ＮＯ）、制御部９は、ステップＳ１９の処理を行う。すなわち、制御部９は、加速時であれば最大加速度λ″maxより任意の定数ａを減じたものを新たな最大加速度λ″maxとし、減速時であれば最大減速度λ″minより任意の定数ａを減じたものを新たな最大減速度λ″minとする。その後、ステップＳ１５に戻り、軌道パラメータである位置λ及び速度λ´を再計算する。

以上の図６に示す処理を完了すると、図５に示すステップＳ３で補間されて求められた軌道上を、ロボット１の手先が速度及び加速度（＝トルク）についての拘束条件を満たしつつ移動した場合に、移動速度が最も速くなる速度パターンが得られる。すなわち、ボブロー法を用いた計算処理はロボット１の運動力学に基づいて行われているので、ロボット１についての拘束条件を満たすことができる。

なお、特に図示していないが、実際には、ステップＳ４及びＳ５に係る動作時間は、始点から終点までの往復の速度パターンに基づいて演算される。つまり、ステップＳ４にて説明したアルゴリズムに基づいて、最初に始点から終点への行きの速度パターンが演算される。次いで、終点から始点までの帰りの速度パターンが演算される。それから、行き速度パターンと帰り速度パターンとが合成され、その合成された速度パターンについて、ステップＳ５にて動作時間が演算される。

次に、図１０及び図１１は、以上のようにしてロボット１の手先の移動速度が最短となる軌道を求めた一例を示す。図１０（ａ）は、ＸＹ平面上における第１軸，第２軸及び手先の座標が変化する軌跡を実線で示すもので、始点座標は（−０．３，０．３）であり、終点座標は（０．３，０．３）である。図１１（ｂ）には、始点から終点まで移動する間の、第１軸，第２軸それぞれの角度，角速度，角加速度，トルクの変化を示している。

第１軸は、Ｘ軸上を−０．３［ｍ］から（図１０（ａ）参照）０．３［ｍ］へ（図１０（ｅ）参照）直線的に移動する。すなわち、移動体２ａが直線移動レール２に沿って移動することに対応する。また、第２軸の回転角θ２（水平旋回アーム５の回転角）は、始点及び終点，並びにＸ軸上の中間点で夫々０［ｒａｄ］であるが、図１０（ｂ）では、終点方向とは逆方向に０．６［ｒａｄ］だけ回転した状態になる。この場合、手先位置のＸ軸座標は、移動体２ａの座標よりも負側にあり、ロボット１のアーム形態としては右手系となる。図１０（ｄ）では逆に、−０．６［ｒａｄ］（始点方向）に回転した状態になる。この場合、手先位置のＸ軸座標は、移動体２ａの座標よりも正側にあり、ロボット１のアーム形態としては左手系となる。

上記の動きに応じて、第２軸の角速度は始点（Ｖ１）から移動した直後に正側のピーク（１０［ｒａｄ／ｓ］，ピーク点Ｖ２）に達し、そこから中間点における負側のピーク（−１０［ｒａｄ／ｓ］，ピーク点Ｖ３）に向けて直線的に変化する。また、その中間点から終点の直前における正側のピーク（１０［ｒａｄ／ｓ］，ピークＶ４）に向けて直線的に変化する。すなわち、図１１（ｂ）に示すように、中間点を中心として対称な速度パターンとなる。

また、上記の角速度パターンに対応する角加速度は、始点Ｖ１において０から正側のピーク４００［ｒａｄ／ｓ^２］に達し、その後、ピーク点Ｖ２で負の値約−５０［ｒａｄ／ｓ^２］に切り替わって、中間点のピーク点Ｖ３までその負の値約−５０［ｒａｄ／ｓ^２］をキープする。さらに、中間点のピーク点Ｖ３で正の値５０［ｒａｄ／ｓ^２］に切り替わると、ピーク点Ｖ４までその値をキープする。ピーク点Ｖ４で負側のピークである−４００［ｒａｄ／ｓ^２］に達し終点Ｖ５で０に戻る、というパターンとなる。

第２軸が上述のように動作することで、以下のような作用が発生する。第２軸は、始点より移動を開始してからＸ軸上の中間点に達するまでの間に、０［ｒａｄ］→０．６［ｒａｄ］→０［ｒａｄ］と角度θ２を変化させる。これにより、ロボット１の手先が一旦、正回転方向に振られた状態から負回転方向に振り戻されるように動く。これに応答して、その動きに伴う反作用が移動体２ａに生じて、移動を開始した際の第１アームの運動を加速する加速力が発生し、この付勢力が第１、第２のアームの終点方向の運動をアシスト（付勢）する。

一方、Ｘ軸上の中間点から終点に達するまでの間に、第２軸は０［ｒａｄ］→−０．６［ｒａｄ］→０［ｒａｄ］と角度θ２を変化させるが、これにより、ロボット１の手先が一旦、負回転方向に振られた状態から正回転方向に振り戻されるように動く。この場合も、その動きに伴う反作用が移動体２ａ側に生じて減速力として作用する。この減速力は、終点に到達して停止するまでの減速運動をアシスト（抑制）する。

以上のように第２軸が動作する結果、ロボット１の手先が始点から終点まで移動する時間は、従来のように単純な動作をさせた場合に比較して短縮される。図１０には、第２軸を全く回転させずに第１軸のみを動作させた場合の軌道を破線で示すが、この軌道についてシミュレートした移動時間は５３６［ｍｓ］であるのに対し、実線で示す軌道についてシミュレートした移動時間は５１４［ｍｓ］であり、４％強の短縮効果があった。したがって、移動時間を従来と同じ時間にすれば、ロボット１をより少ないエネルギーで動作させることが可能になり、省エネルギー化にも貢献できる。更に、例えば出力を低下させることでモータのワット数を低下させることも可能になり、ロボットの小型化，低コスト化を図ることもできる。

また、図１１（ａ）は、図１０（ａ）〜（ｅ）に示す５つの手先位置を同時にプロットしたものである。また、これらの５点は、図１１（ｂ）に示す第２軸の角速度パターン上にプロットした５点（Ｖ１〜Ｖ５），すなわち角速度又は角加速度の変極点に対応している。図５に示すステップＳ２において、始点と終点との間に３点の初期経由点を設定したが、初期経由点を３点にすることは、図１１に示す角速度又は角加速度の変極点に対応するものを選択するのが、計算効率を向上させるために有利であるとの判断に基づいている。

例えば、文献（３）のＦｉｇ．４に示されていると共に関連する記載からは、ボブロー法では、軌道を探索するための経由点数を増やすほど動作時間を短縮する効果がある。しかしながら、経由点数を増やせば、それは直接的に計算時間を増大させることに繋がるため、無制限に増やすのは実用的ではない。

そこで、本実施例では、動作時間の短縮を図ることと計算時間の増大を抑制することとのバランスを考慮して、初期経由点を３点にしている。初期経由点の選択は厳密に行う必要はなく、前述したように始点と終点との間の直線距離を単純に４等分するなどして決定すれば良い。図１又は図５に示す処理を繰り返し実行する過程で、経由点が変更されつつ新たな軌道が求められ、最適化される。

その結果、ピック・アンド・プレイスのような動作パターンにおいて動作時間が最短になる軌道を選択することは、３点の経由点が、図１１（ａ）に示すような第２軸の角速度又は角加速度の３つの変極点として、どの座標位置となるかにより決まると言える。したがって、初期経由点を３点とすれば、動作時間が最短となる軌道を、計算時間の増大を抑制しつつ求めることが可能となるのである。

以上のように本実施例によれば、基部側に位置する移動体２ａと、その手先側に連結されている水平旋回アーム５とを有する多軸型のロボット１を対象とする。そして、制御部９は、手先を始点から移動させる際には、水平旋回アーム５の動作によって移動体２ａ側に生じる反作用が、移動体２ａの基軸である直線移動レール２に沿って動作させて発生する加速力を増加させるように軌道を生成し、手先を終点に停止させる際には、水平旋回アーム５の動作によって移動体２ａに生じる反作用が、移動体２ａを動作させて発生する減速力を増加させるように軌道を生成する。

より具体的には、始点から終点に向けて手先の移動を開始させるときに、移動体２ａに、水平旋回アーム５の動作によって、移動体２ａが発生する加速力を増加させる反作用を生じさせ、手先を終点に停止させるときには、水平旋回アーム５の動作によって、移動体２ａが発生する減速力を増加させる反作用を生じさせるように、移動体２ａ及び第２軸の
速度パターンを生成することで手先の軌道を生成する。

従来の多軸型ロボットの制御では、例えばピック・アンド・プレイスのような単純な作業を行わせる場合、始点、終点間で動作量に変化がない軸は動かない。すなわち、図１０に破線で示した動作パターンとなる。このようにアームが伸びきった状態では、第１軸（走行軸）の高加速による慣性トルクで手先側アームが動かないように第２軸は無駄な保持トルクを出力し続けており、基部側アームの負荷となっているだけである。

これに対して、上述したように軌道を生成すると、水平旋回アーム５が、移動体２ａに対し反作用を生じるように動作するようになり、手先の起動時に必要な加速力と、停止時に必要な減速力とをそれぞれ増加させ、手先を始点から終点まで移動させる速度を向上させて移動時間を短縮できる。すなわち、手先を移動させる途中で水平旋回アーム５を若干回転させることで、移動体２ａが直線移動する際のイナーシャが小さくなり、手先をより少ないエネルギーで移動させることができる。よって、従来の手法で決定された軌道と同じ移動時間で動作させるのであれば、ロボット１を従来よりも少ない消費エネルギーで動作させることができる。

また、上述した軌道は、多軸型ロボット１の動力学に基づいて計算された結果として生成されるので、ロボットの拘束条件を満たす範囲内で速度を最大化する軌道が求められる。したがって、現実的に使用できる最適化された軌道が得られる。加えて、ボブロー法を用いて速度パターンを演算する際に、軌道上の位置を角度に変換して演算を行うようにしたので、ロボット１のアーム形態を左手系，右手系の何れか一方に限定することなく、左右両方の手系を用いて最速となる速度パターンを求めることができる。

また、手先を始点から移動させる際には、移動体２ａを終点方向に移動させると共に、水平旋回アーム５を、その先端が終点方向とは逆方向に移動するように回転させる。その後、水平旋回アーム５の先端を終点方向に回転させる。この時、水平旋回アーム５の動作によって生じる反作用が起動時の加速力をアシストするようになる。

また、手先を終点に停止させる際には、水平旋回アーム５を、その先端である手先が終点を超えている位置から、先端を始点方向に移動させるように軌道を生成する。すなわち、水平旋回アーム５の先端，手先を始点方向に振り返すように軌道を生成して停止させる。この時、水平旋回アーム５の動作によって生じる反作用が停止時の減速力をアシストするようになる。すなわち、この様に軌道を生成することで、水平旋回アーム５が、移動体２ａに対し反作用を生じるように動作させることができる。

そして、始点と終点との間に３点の経由点を設定し、３点の位置を変更しながら各点に対応した移動時間を演算するようにした。すなわち、手先の移動時間を最短にする軌道を生成するには、軌道を最適化する必要がある。そして、最適化を行うには、始点と終点との間に１点以上の経由点を設定し、その経由点の位置を変更した場合に求められる移動速度を比較する必要がある。

ここで、上述したように水平旋回アーム５を動作させることを想定すると、水平旋回アーム５の基部側軸である移動体２ａにおいて、起動直後に加速度を付与する時点と、動作途中で加速度の方向が反転する時点と、停止直前に加速度をゼロにする時点とがある。そして、軌道の最適化を図る過程では、少なくとも上記３つの時点に対応する経由点を最適化することが重要になると考えられる（図１（ａ）参照）。したがって、上記３つの時点に対応する経由点を設定すれば、演算量を抑制しつつ効率的に、移動時間を最短にする軌道を生成することが可能となる。これにより、従来の方法では、方法自体の限界によって引き出すことができなかったモータ，アクチュエータのポテンシャルを最大限引き出しつつ、現実的に使用できる最適化された軌道が得られるようになる。

［第２の実施例］
図１２及び図１３は、多軸型ロボットの軌道生成方法及び多軸型ロボットの制御装置に係る第２の実施例を示す。なお、第２の実施例及びそれ以降の実施例において、前述した第１の実施例で説明した構成要素と同一又は同等のものには、同一の符号を付してその説明を省略又は簡略化する。

第２の実施例に係る多軸型ロボットの軌道生成方法及び多軸型ロボットの制御装置では、手先の位置ｘを各軸の角度ｑへ変換するステップの実行タイミングが変更されている。つまり、前述した第１の実施例では、逆運動学を解いて手先の位置ｘを各軸の角度ｑへ変換していた（ステップＳ１２参照）。これに対し、第２の実施例では、最短の速度パターンの作成前の段階、すなわち経由点の補間処理の段階で、その角度ｑへの変換処理を行うようにした。これにより、逆運動学の演算を回避することができ、演算負荷が格段に減少する。

図１２及び図１３に、制御部９が実行する、速度パターンを作成する処理を説明する。制御部９は、例えばオペレータによって与えられた開始点及び終点を示す情報を読み出し（図１２：ステップＳ１）、３つの初期経由点を生成する（ステップＳ２）。次いで、制御部９は、手先空間におけるそれらの開始点、終点及び初期経由点を、関節空間における相当する角度に変換する（ステップＳ２Ａ）。この変換の後、制御部９はステップＳ３に移行し、それらの開始点、終点及び３つの初期経由点を補間して軌道ｘ＝ｆ（λ）を生成する。次いで、ステップＳ４Ａでは、図１３に示すように、ステップＳ１３Ａ、Ｓ１４〜Ｓ１９の処理が行われる。このうち、ステップＳ１３Ａでは、角度ｑをロボットの運動方程式に代入する。

このとき、補間された角度ｑは、
ｑ＝ｆ（λ），ｑ´=ｆ´λ´，ｑ″=ｆ´λ″＋ｆ″λ´^２ …（１７）
で表されるので、ロボットの運動方程式は関節空間において

と表される。上記以外の処理は前述した第１の実施例のものと同様である。

したがって、この第２の実施例によれば、軌道ｘ＝ｆ（λ）を補間する前に、軌道ｘを関節角度ｑに変換する。これにより、前述した逆運動学の方程式を解く処理、即ち、前述した図６のステップＳ１１，Ｓ１２の処理が不要になる。つまり、関節角度空間においてそのままダイナミクス（順運動学）を解けばよく、その分だけ演算負荷が軽くなる。その他の作用効果は、前述した第１の実施例と同様のものが得られる。

［変形例］
本発明は上記した、または図面に記載した実施例にのみ限定されるものではなく、以下のような変形又は拡張が可能である。
初期経由点を４点以上設定しても良い。
多軸型ロボットの動力学に基づいて速度を最大化する軌跡を求める計算手法は、ボブロー法以外の手法を用いても良い。
基部側アームと手先側アームとは、各アームの相対的な位置関係で決まる。例えば第２アームの先に第３アームが連結されている場合、第１アームを「基部側アーム」とすれば第２及び第３アームが「手先側アーム」となり、第２アームを「基部側アーム」とすれば第３アームが「手先側アーム」となる。

この一例として、図１４に３軸のロボットアームを示す。この３軸アームは、ジョイントＪＴ１，ＪＴ２，ＪＴ３にそれぞれアームＡＭ１，ＡＭ２，ＡＭ３が接続されている。この３軸アームの手先ＥＦを所望の始点から所望の終点まで移動させるときも同様に、前述した第１又は第２の実施例に記載の軌道生成方法及び制御方法を適用できる。この場合、ジョイントＪＴ１，ＪＴ２，ＪＴ３の関節角をθ１，θ２，θ３とすると、式（１）において角度ｑを
ｑ＝［θ１，θ２，θ３］ …（１９）
の３次元ベクトルとして処理すればよい。

この結果、この３軸のロボットアームを、一例として図１５に示したように動作させた。これによれば、３軸（Ｊ１，Ｊ２，Ｊ３）の位置を始点（０，０，０）度から終点（９０，４５，９０）度まで移動させる際に、手先側アームの反作用による加速力及び減速力の助長効果が得られことが確認できた。つまり、従来の軌道軌跡（図１５中の二点鎖線）に比較して、動作時間の短縮を確認できた。

また、走行軸と回転軸との組み合わせから成るロボットに限らず、他の軸数が多いロボットに適用しても良い。
さらに、前述した実施例では拘束条件の速度の上限値λ´min，λ´maxを一定値として扱っていたが、これを可変値として扱うこともできる。この例を２つ挙げる。

第１の例は、本願のロボットのモータの速度とトルクの関係に着目した、トルク、すなわち、拘束条件の一つとしての加速度の上限値を可変させる機能に関する。図１６に、一般的に知られるモータのＴＮ曲線を示す。モータは高速になるほど、出力する最大トルクが減少することが知られている。そこでこの例では、モータのトルクに関する拘束条件を前述した関係から、
λ″min(λ，ｑ´／ｆ)≦λ″（λ，ｑ´／ｆ）≦λ″max(λ，ｑ´／ｆ)
…（２０）
とおく。
図１６に示すＴＮ曲線を使って現在の速度ｑ´に対応する最大トルクを演算して式（２０）に基づく判定を行う。これにより、速度に応じて最大トルクを可変できる。

第２の例は、ボールねじ等の走行軸を持つＸＲロボットにおける拘束条件の一つとして、速度の上限速度の可変機能に関する。図１７に示すＸＲロボットは、ボールねじ軸２０を備え且つ両端が固定手段２１により固定ベース２２に固定された直動軸Ｊ１（Ｘ軸，第１軸）と、この直動軸Ｊ１にナット部材３０を介して移動可能に保持された移動ステージ２３と、この移動ステージ２３の内部に配置されているジョイント２４（関節，モータを含む）を介して基部側が回転可能に保持されたアーム２５とを備える。このアーム２５の先端側が手先となる。ジョイント２４が回転軸（Ｒ軸，第２軸）Ｊ２を成す。これにより、２軸のＸＲロボットが構成されている。これらの２軸Ｊ１，Ｊ２の軌道生成に、本願の加速度及び減速度を助長する構成が実施されている。

ボールねじなどの単軸は、その上限速度（限界速度、危険速度と呼ばれる）が定められているが、この速度の値はその軸の固定端からの位置によって変化する。従来の台形速度パターンで駆動させる場合、台形を成す上辺（上限速度）は一定値であり、安全率を考慮した値に設定していた。

そこでこの例では、本願の軌道生成手法をＸＲロボットに適用することで、その上限速度を直動軸Ｊ１に沿った移動ステージ２３の位置Ｐに応じて変化させることができる。前述した式（１３）から判るように、速度の拘束条件λ´min≦λ´≦λ´maxは位置λ（すなわち角度）と最大速度ｑ´によって決まるので、Ｊ１軸上の位置Ｐに応じて限界速度を可変にできる。これにより、図１８においてハッチングした領域で模式的に示すように、従来では使いきれていない速度領域を有効に使用できる。つまり、より最短の軌道で手先を移動させることができると共に、省エネルギーにもなる。

［第３の実施例］
以下、変形例２を第３の実施例としてより詳細に説明する。ボールねじ軸２０は、図１７中右端側に配置されている、モータを含む駆動手段２６により回転駆動される。前記モータの許容回転数Ｎｐは、ねじ軸の危険速度ＮとＤＮ値（回転限界値）との小さい方で決まる（例えば、特開２００６−１９８７１６号公報参照）。すなわち、
許容回転数Ｎｐ＝Ｍｉｎ（Ｎｄ，ＤＮ値） …（２１）
尚、ＤＮ値は、ねじの型式による値（メーカ値）Ｋと、ボール中心径Ｄより以下で計算される。
ＤＮ値＝Ｋ／Ｄ …（２２）
許容回転数について、ロボットの位置に依存するのは危険速度Ｎｄであり、以下のように計算される。図１９に示すように、ロボット走行軸（直動軸）の位置をＪ１［ｍｍ］（上記位置Ｐに相当），メカストローク長をＳｔ［ｍｍ］，可動範囲をＳｆ［ｍｍ］とすると、固定端からの位置Ｌａ［ｍｍ］は、
Ｌａ＝Ｊ１＋（Ｓｔ−Ｓｆ）／２ …（２３）
となる。取り付け間距離Ｌｂ［ｍｍ］は、両端から遠い方を用いて次式とする。
Ｌｂ＝Ｍａｘ（Ｌａ，Ｓｔ−Ｌａ） …（２４）
よって、ボールねじ軸の取り付け方法（固定／支持／自由）による係数ｋ，ねじ軸谷径ｄ１［ｍｍ］とすると、危険速度Ｎｄは次式となる。
Ｎｄ＝ｋ×ｄ１×１０^７／Ｌｂ …（２５）
上記の（２１）式に基づいて規定された上限速度が図１８に示すものであり、上限速度がフラットになっている部分（３４００ｒｐｍ程度）は、ＤＮ値によって規定されている。

また、図１８に示す上限速度に従い、ＸＲロボットの動作をシミュレーションした結果を図２０（ａ），（ｃ）に示す。図２０（ｂ）は図１８相当図であり、（ｃ）は（ａ）を拡大して示したものである。図２０（ｃ）には、４つの結果が示されている。
第１（一点鎖線）は、ＸＲロボットの移動ステージ２３（移動体）を移動（０［ｍｍ］〜１２００［ｍｍ］）させる際に、従来と同様にボールねじ軸２０のみを回転駆動させた場合で（上限速度は固定）、移動時間は約９０７ｍｓである。第２（二点鎖線）は、上述した第１又は第２実施例のように、ボブロー法を用いて計算を行い、移動の際に回転軸Ｊ２を回転させることでアーム２５（回動アーム）が生じる反力によるアシストを加えると移動時間は約８９４ｍｓに短縮される。

第３（破線）は、移動ステージ２３の移動はボールねじ軸２０の回転駆動だけで行うが、（２１）式に基づいて上限速度を可変させると、移動時間は約５９６ｍｓまで大幅に短縮される。更に第４（実線）として、移動の際にアーム２５が生じる反力によるアシストを加えると移動時間は約５７４ｍｓに短縮される。これは、第１の結果からの比較では、移動時間を約３７％短縮したことになる。

また、（２１）式に基づいて上限速度を可変させる制御と、ボブロー法とを組み合わせることで、以下のような作用効果も得られる。例えば図２１（ａ）に示すように、図１７に示すＸＲロボットについて、第１軸を０［ｍｍ］から１０００［ｍｍ］に移動させると共に、第２軸を１２０°から９０°まで−３０°移動させることを想定する。このような多軸ロボットを協調動作させる場合、位置決めは全ての軸を同時発着させることが前提となる。図２１（ｂ）に示すように、上限速度が位置に応じて変化する第１軸の速度パターン（１点鎖線）を、実線で示すように、図２１（ｃ）に示す第２軸に合わせて修正変更することは、一般に困難である。
しかし、本実施例のように、ボブロー法を用いれば、軌道空間において速度パターンを決定することになり、その結果として、各軸の発着は自動的に同期するようになる。したがって、ユーザは、各軸の動作を同期させることを特段意識する必要がなくなる。

以上のように第３実施例によれば、ボールねじ軸２０からなる直動軸と、この直動軸に沿って移動可能に配置される移動ステージ２３と、移動ステージ２３に搭載される回転軸を基点として回動可能に配置されるアーム２５とを備える２軸ロボットについて、移動ステージ２３の最大速度についての拘束条件を、少なくとも一部の範囲について、直動軸上における移動ステージ２３の位置に応じて可変設定するように、具体的には（２１）式に基づいて設定するようにした。これにより、前記位置によっては上限速度を従来よりも高く設定し、移動時間を短縮させて作業効率を向上させることができる。

１はロボット（多軸型ロボット）、２ａは移動体（基部側アーム，第１のリンク）、５は水平旋回アーム（手先側アーム，第２のリンク）、９は制御部（制御装置，位置指定手段，作成手段，変換手段，生成手段，制御手段）を示す。

Claims (8)

1. 複数のリンクと複数のジョイントとを有してなる多軸型ロボットについて、前記複数のリンクの先端に位置する手先を、始点から終点まで移動させるための当該手先の軌道を生成する方法において、
   前記始点及び前記終点の位置を指定する位置指定ステップと、
   前記始点から前記終点に向けて前記手先の移動を開始させるときに、前記複数のリンクのうちの相対的に基部側に位置している第１のリンクに、前記複数のリンクのうちの前記第１のリンクよりも手先側に位置している第２のリンクの動作によって、前記第１のリンクを駆動する第１のジョイントが発生する加速力を増加させる反作用を生じさせると共に、
   前記手先を前記終点に停止させるときには、前記第２のリンクの動作によって、前記第１のリンクに、前記第１のジョイントが発生する減速力を増加させる反作用を生じさせるように、前記第１のジョイントの速度パターン及び前記第２のリンクを駆動する第２のジョイントの速度パターンを生成することで前記軌道を生成する軌道生成ステップとを有することを特徴とする多軸型ロボットの軌道生成方法。
2. 前記軌道生成ステップは、前記第１及び第２のジョイントの駆動に関する拘束条件を加味して前記速度パターンを演算するステップを含むことを特徴とする請求項１記載の多軸型ロボットの軌道生成方法。
3. 前記拘束条件は、前記第１及び第２のジョイントのそれぞれについて設定されている最大トルク及び最少トルクから成る加速度条件と、前記第１及び第２のジョイントのそれぞれについて設定されている最大速度及び最小速度から成る速度条件とを含むことを特徴とする請求項２記載の多軸型ロボットの軌道生成方法。
4. 前記始点と前記終点との間に複数の初期経由点を自動的に設定するステップを有し、
   前記軌道生成ステップは、前記初期経由点の位置を更新しながら最短時間の前記速度パターンを生成することを特徴とする請求項１から３の何れか一項に記載の多軸型ロボットの軌道生成方法。
5. 前記初期経由点を、３点設定することを特徴とする請求項４記載の多軸型ロボットの軌道生成方法。
6. 前記多軸型ロボットが、
   両端が固定されたボールねじ軸からなる直動軸と、
   前記ボールねじ軸を回転駆動する駆動手段と、
   前記ボールねじ軸が回転することで、前記直動軸に沿って移動可能に配置される移動体と、
   この移動体に搭載される回転軸，及び前記回転軸を基点として回動可能に配置される回動アームとを備えて構成されており、
   前記移動体の最大速度についての拘束条件を、少なくとも一部の範囲について、前記直動軸上における前記移動体の位置に応じて可変設定することを特徴とする請求項３又は請求項３を引用する請求項４若しくは５の何れか一項に記載の多軸型ロボットの軌道生成方法。
7. 前記移動体の位置に応じて求められる前記ボールねじ軸の危険回転速度ＮｄとＤＮ値とを比較して、より小さい値を示す方を前記最大速度の拘束条件とすることを特徴とする請求項６記載の多軸型ロボットの軌道生成方法。
8. 複数のリンクと複数のジョイントとを有してなる多軸型ロボットを制御する装置において、
   前記複数のリンクの先端にある手先について、始点及び終点の位置を指定する位置指定手段と、
   前記始点から前記終点に向けて前記手先の移動を開始させるときに、前記複数のリンクのうちの相対的に基部側に位置している第１のリンクに、前記複数のリンクのうちの前記第１のリンクよりも手先側に位置している第２のリンクの動作によって、前記第１のリンクを駆動する第１のジョイントが発生する加速力を増加させる反作用を生じさせると共に、
   前記手先を前記終点に停止させるときには、前記第２のリンクの動作によって、前記第１のリンクに、前記第１のジョイントが発生する減速力を増加させる反作用を生じさせるように、前記第１のジョイントの速度パターン及び前記第２のリンクを駆動する第２のジョイントの速度パターンを生成する速度パターン生成手段と、
   前記速度パターンに基づいて前記第１及び第２のジョイントの駆動を制御する制御手段と、を備えたことを特徴とする制御装置。

Images