JP6123595B2

JP6123595B2 - ２軸ロボットの速度制御方法

Info

Publication number: JP6123595B2
Application number: JP2013187341A
Authority: JP
Inventors: 吉朗岩崎
Original assignee: Denso Wave Inc
Current assignee: Denso Wave Inc
Priority date: 2013-09-10
Filing date: 2013-09-10
Publication date: 2017-05-10
Anticipated expiration: 2033-09-10
Also published as: JP2015054360A

Description

本発明は、両端が固定されたボールねじ軸からなる直動軸を備えてなる２軸ロボットを速度制御する方法に関する。

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

一般に、ロボットの動作速度を制御する際に基本的な動作速度パターンとして、台形状の速度パターンが用いられることが多い（例えば、特許文献１，２参照）。台形状の動作速度パターンではその上辺部分が一定の上限速度となり、上辺部分に相当する動作期間では加速度がゼロとなる。

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

従来の台形状の動作速度パターンで規定される上限速度については、安全を考慮して最悪値を用いることが多い。しかしながら、その上限速度が必ずしも、ロボットが採用している構成に基づいた適切な限界値に近接した値として設定されておらず、本来のロボットの性能を十分に発揮させることができない場合があった。

本発明は上記事情に鑑みてなされたもので、その目的は、ボールねじ軸からなる直動軸を備えてなる２軸ロボットについて、従来よりも高い性能を発揮させることができる２軸ロボットの速度制御方法を提供することにある。

請求項１記載の２軸ロボットの速度制御方法によれば、２軸ロボットは、両端が固定されたボールねじ軸からなる直動軸を備えるもので、ボールねじ軸が第１駆動手段により回転されると移動体が直動軸に沿って移動する。その移動体には、第２駆動手段と、第２駆動手段により回転駆動され、回転軸を基点として回動可能に配置される回動アームとが搭載されている。

そして、前記２軸ロボットを動作させ、回動アームの先端に位置する手先を、始点から終点まで移動させるための当該手先の軌道を生成する際に、始点及び終点の位置を指定すると、始点から終点に向けて手先の移動を開始させるときに、回動アームの動作によって、第１駆動手段の作用により移動体が発生する加速力を増加させる反作用を生じさせる。また、手先を終点に停止させるときには、回動アームの動作によって、第１駆動手段の作用により移動体が発生する減速力を増加させる反作用を生じさせるように、移動体及び回動アームの速度パターンを生成することで前記軌道を生成する。

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

これに対して、上述したように軌道を生成すると、回動アームが移動体の動きに対し反作用を生じるように動作する。したがって、手先の起動時に必要な加速力と、停止時に必要な減速力とをそれぞれ増加させて、手先を始点から終点まで移動させる速度を向上させ、移動時間を短縮することができる。ここで、
（最大関節加速度）＝（最大関節トルク＋干渉トルク＋重力トルク＋摩擦トルク）
／イナーシャ …（Ａ）’
（最大関節速度）＝（モータ最大速度）／（減速比） …（Ｂ）
である。ただし、ここでは説明簡略化のため水平方向の動作のみを考えることとし、（Ａ）’式において重力トルク及び摩擦トルクを無視したものを（Ａ）式とする。例えばバンバン（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とする。その後、ステップＳ１５に戻り、軌道パラメータである位置λ及び速度λ´を再計算する。

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

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

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

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

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

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

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

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

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

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

例えば、文献（３）のＦｉｇ．４に示されていると共に関連する記載からは、ボブロー法では、軌道を探索するための経由点数を増やすほど動作時間を短縮する効果がある。しかしながら、経由点数を増やせば、それは直接的に計算時間の増大させることに繋がるため、無制限に増やすのは実用的ではない。
そこで、第２実施形態では、動作時間の短縮を図ることと計算時間の増大を抑制することとのバランスを考慮して、初期経由点を３点にしている。初期経由点の選択は厳密に行う必要はなく、前述したように始点と終点との間の直線距離を単純に４等分するなどして決定すれば良い。図６又は図９に示す処理を繰り返し実行する過程で、経由点が変更されつつ新たな軌道が求められ、最適化される。

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

図１６は、第１実施形態で示した図１に、第２実施形態の制御を加えた実行結果を併せて示している。二点鎖線は、第１実施形態の制御を行わず、第２実施形態のようにボブロー法を用いて計算を行い、移動の際に回転軸Ｊ２を回転させることでアーム７が生じる反力によるアシストを加えたもので、移動時間は約８９４ｍｓに短縮されている。また、実線は、第１実施形態の制御と第２実施形態の制御とを同時に行ったもので、移動時間は約５７４ｍｓに短縮されている。これは、一点鎖線で示す結果からの比較では、移動時間を約３７％短縮したことになる。

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

以上のように第２実施形態によれば、制御部１１は、手先を始点から移動させる際には、アーム７の動作によって移動ステージ５側に生じる反作用が、移動ステージ５の基軸であるボールねじ軸１に沿って動作させて発生する加速力を増加させるように軌道を生成し、手先を終点に停止させる際には、アーム７の動作によって移動ステージ５に生じる反作用が、移動ステージ５を動作させて発生する減速力を増加させるように軌道を生成する。

より具体的には、始点から終点に向けて手先の移動を開始させるときに、移動ステージ５に、アーム７の動作によって、ボールねじ軸１及び移動ステージ５が発生する加速力を増加させる反作用を生じさせ、手先を終点に停止させるときには、アーム７の動作によって、移動ステージ５が発生する減速力を増加させる反作用を生じさせるように、移動ステージ５及びジョイント６の速度パターンを生成することで手先の軌道を生成する。

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

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

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

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

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

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

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

（第３実施形態）
図１８及び図１９は第３実施形態であり、図９及び図１０相当図である。第３実施形態では、手先の位置ｘを各軸の角度ｑへ変換するステップの実行タイミングが変更されている。つまり、第２実施形態では、逆運動学を解いて手先の位置ｘを各軸の角度ｑへ変換していた（ステップＳ１２参照）。これに対し、第３実施形態では、最短の速度パターンの作成前の段階、すなわち経由点の補間処理の段階で、その角度ｑへの変換処理を行うようにした。これにより逆運動学の演算を回避することができ、演算負荷が格段に減少する。

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

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

と表される。上記以外の処理は第２実施形態と同様である。

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

本発明は上記した、または図面に記載した実施例にのみ限定されるものではなく、以下のような変形又は拡張が可能である。
初期経由点を４点以上設定しても良い。
多軸型ロボットの動力学に基づいて速度を最大化する軌跡を求める計算手法は、ボブロー法以外の手法を用いても良い。

１はボールねじ軸、５は移動ステージ（移動体）、６はジョイント（第２駆動手段）、７はアーム、８は駆動手段（第１駆動手段）、９ＸＲロボット（２軸ロボット）、１１は制御部（制御装置，位置指定手段，作成手段，変換手段，生成手段，制御手段）を示す。

Claims

両端が固定されたボールねじ軸からなる直動軸と、
前記ボールねじ軸を回転駆動する第１駆動手段と、
前記ボールねじ軸が回転することで、前記直動軸に沿って移動可能に配置される移動体と、
この移動体に搭載されるもので、第２駆動手段，及び前記第２駆動手段により回転駆動され、回転軸を基点として回動可能に配置される回動アームとを備えて構成される２軸ロボットを動作させ、前記回動アームの先端に位置する手先を、始点から終点まで移動させるための当該手先の軌道を生成する際に、
前記始点及び前記終点の位置を指定し、
前記始点から前記終点に向けて前記手先の移動を開始させるときに、前記回動アームの動作によって、前記第１駆動手段の作用により前記移動体が発生する加速力を増加させる反作用を生じさせると共に、
前記手先を前記終点に停止させるときには、前記回動アームの動作によって、前記第１駆動手段の作用により前記移動体が発生する減速力を増加させる反作用を生じさせるように、前記移動体及び前記回動アームの速度パターンを生成することで前記軌道を生成することを特徴とする２軸ロボットの速度制御方法。
前記始点と前記終点との間に複数の初期経由点を自動的に設定し、
前記初期経由点の位置を更新しながら最短時間となる前記速度パターンを生成することを特徴とする請求項１記載の２軸ロボットの速度制御方法。
前記初期経由点を、３点設定することを特徴とする請求項２記載の２軸ロボットの速度制御方法。