JP2014161917A

JP2014161917A - ロボット制御システム、ロボット、ロボット制御方法及びプログラム

Info

Publication number: JP2014161917A
Application number: JP2013031790A
Authority: JP
Inventors: Nobuhiro Karido; 信宏狩戸
Original assignee: Seiko Epson Corp
Current assignee: Seiko Epson Corp
Priority date: 2013-02-21
Filing date: 2013-02-21
Publication date: 2014-09-08

Abstract

【課題】ロボットが通過する経由点を複数指定した場合に、当該経由点に基づいたなめらかな軌道を生成するロボット制御システム、ロボット、ロボット制御方法及びプログラム等を提供すること。
【解決手段】ロボット制御システムは、軌道計画部１０３と、ロボット制御部１０５を含み、軌道計画部１０３は、２つの経由点の間の処理区間の時間長を設定し、前記処理区間と、第１〜第Ｍの経由点での関節角の値とに基づいて、第１の補間処理を行い、第１の補間処理の結果から特定される処理区間での関節角速度と、最大関節角速度に基づいて、前記時間長の更新処理を行い、更新処理後の処理区間と、第１〜第Ｍの経由点での関節角の値とに基づいて、第２の補間処理を行う処理を、軌道計画処理として実行する。
【選択図】図１

Description

本発明は、ロボット制御システム、ロボット、ロボット制御方法及びプログラム等に関する。

近年、産業用ロボットをはじめとして種々の分野でロボットが利用されている。その場合、ロボットはユーザーにより逐次操作されるのではなく、ある程度自動制御を行うことで作業の効率化等が図られることになる。具体的には、ロボットの現在位置と目標位置とが設定された場合に、その間の経路（軌道）を自動生成し、生成した軌道に沿ってモーター等の制御を行うことが考えられる。

特許文献１には、ロボットの現在位置から目標位置まで移動する間に、中間目標位置をサンプリング周期毎に変更し、そのサンプリング周期毎に中間目標位置までの経路に対して、スプライン補間で軌道を計算し、ロボットを制御する手法が開示されている。

特開２００１−６０１１２号公報

ロボットの軌道を生成する軌道計画処理においては、ロボットによる作業の高速化を図るために、ロボットをできるだけ高速で動作させるという点が重要となる。特許文献１では、中間目標位置を周期毎に変更してスプライン補間を行うことで、高速性を実現している。

しかし特許文献１では、ロボットの機械的な限界を考慮していない。例えば、各関節角をアクチュエーター（狭義にはモーター）により制御する多関節ロボットの場合であれば、モーターの性能に起因して関節角速度の上限、或いは関節角加速度の上限等が決まるが、特許文献１の手法ではそれらの上限を考慮していない。よって、ロボットの現在位置と目標位置が近い場合等には、モーター性能からすればとることができない速度や加速度で関節角を制御する軌道を生成する可能性があり好ましくない。

また、ロボットの軌道計画では、開始位置と目標位置だけでなく、経由点についても重要となる場合がある。例えば作業空間に他のロボット等が配置される場合には、それらと干渉しない軌道を生成するために、所与の経由点を通過する必要が生じる。しかし、特許文献１の中間目標位置はシステムが動的に生成するものであり、通過すべき経由点を表すものではない。つまり特許文献１の手法は、目標位置の他、複数の経由点が入力され、それらをできるだけ通過するように軌道を生成する場合には利用することができない。

本発明の幾つかの態様によれば、ロボットが通過する経由点を複数指定した場合に、当該経由点に基づいたなめらかな軌道を生成するロボット制御システム、ロボット、ロボット制御方法及びプログラム等を提供することができる。

本発明の一態様は、第１〜第Ｍ（Ｍは２以上の整数）の経由点を表す経由点情報に基づいて、第１〜第Ｎ（Ｎは２以上の整数）の関節を備えたロボットに対して、前記第１〜第Ｍの経由点に基づく経路を移動させるための軌道を生成する軌道計画処理を行う軌道計画部と、前記軌道計画処理の結果に基づいて、前記ロボットを制御するロボット制御部と、を含み、前記軌道計画部は、２つの経由点の間に処理区間を設定するとともに、前記処理区間の時間長を設定し、前記時間長が設定された前記処理区間と、前記第１〜第Ｍの経由点での関節角の値とに基づいて、各処理区間での前記第１〜第Ｎの関節の前記関節角の値を決定する第１の補間処理を行い、前記第１の補間処理の結果から特定される前記処理区間での関節角速度と、最大速度設定処理により設定された最大関節角速度に基づいて、前記処理区間の前記時間長の更新処理を行い、前記更新処理後の前記処理区間と、前記第１〜第Ｍの経由点での前記関節角の値とに基づいて、各処理区間での前記第１〜第Ｎの関節の前記関節角の値を決定する第２の補間処理を行う処理を、前記軌道計画処理として行うロボット制御システムに関係する。

本発明の一態様では、各処理区間に設定した時間長を用いて第１の補間処理を行い、その結果と最大関節角速度を用いて時間長の更新処理を行い、更新後の時間長を用いて第２の補間処理を行う。これにより、最大関節角速度を基準として時間長が更新されるため、軌道計画処理の出力である軌道に従えば、各関節の関節角速度を最大関節角速度に基づき設定する（例えば最大値を最大関節角速度以下にする等）処理等が可能になる。

また、本発明の一態様では、前記軌道計画部は、前記ロボットの移動開始時刻から所与の加速期間内に含まれる前記経由点を削除する処理と、前記ロボットの移動終了時刻から所与の減速期間内に含まれる前記経由点を削除する処理の少なくとも一方の処理を、削除処理として行い、前記第１〜第Ｍの経由点のうち、前記削除処理により削除されなかった前記経由点に基づいて、前記第２の補間処理を行ってもよい。

これにより、加速区間、減速区間を考慮した軌道計画処理を行うこと等が可能になる。

また、本発明の一態様では、前記軌道計画部は、前記更新処理後の前記処理区間の前記時間長に基づいて、前記ロボットの前記移動開始時刻から、前記第１〜第Ｍの経由点の各経由点に到達するまでの時間である第１の時間情報と、前記第１〜第Ｍの経由点の各経由点への到達後、前記ロボットの移動終了時刻までの時間である第２の時間情報の少なくとも一方を求め、前記第１の時間情報と前記加速期間の比較処理と、前記第２の時間情報と前記減速期間の比較処理の少なくとも一方に基づいて、前記第１〜第Ｍの経由点の各経由点が前記加速期間又は前記減速期間に含まれるかの判定を行って前記削除処理を行ってもよい。

これにより、処理区間の時間長に基づいて、経由点の削除処理を行うこと等が可能になる。

また、本発明の一態様では、前記軌道計画部は、前記第１〜第Ｎの関節の第ｉ（ｉは１≦ｉ≦Ｎを満たす整数）の関節を駆動する第ｉのアクチュエーターの性能に基づいて、前記第ｉの関節の前記最大関節角速度を設定する処理を、前記最大速度設定処理として行ってもよい。

これにより、アクチュエーターの性能に基づいて最大関節角速度を設定すること等が可能になる。

また、本発明の一態様では、前記軌道計画部は、前記第ｉのアクチュエーターの性能に基づいて決定される機械的な最大速度に基づいて、前記第ｉの関節の前記最大関節角速度を設定する処理を、前記最大速度設定処理として行ってもよい。

これにより、機械的な最大速度に基づいて最大関節角速度を設定すること等が可能になる。

また、本発明の一態様では、前記軌道計画部は、前記第ｉのアクチュエーターの性能に基づいて決定される機械的な最大加速度と、前記経路の始点と終点の間の前記第ｉの関節の前記関節角の変位である総移動距離とに基づいて、前記総移動距離を移動する間に到達できる到達可能速度を求め、求めた到達可能速度に基づいて、前記第ｉの関節の前記最大関節角速度を設定する処理を、前記最大速度設定処理として行ってもよい。

これにより、加速度や総移動距離から決定される到達可能速度に基づいて、最大関節角速度を設定すること等が可能になる。

また、本発明の一態様では、前記軌道計画部は、設定された前記最大関節角速度に対する、前記第１の補間処理の結果から特定される前記処理区間での前記関節角速度の比を求め、求めた比を前記処理区間の前記時間長に乗ずることで、前記処理区間の前記時間長の前記更新処理を行ってもよい。

これにより、補間処理の結果得られる関節角速度と、最大関節角速度の比から、容易に時間長の更新処理を行うこと等が可能になる。

また、本発明の一態様では、前記軌道計画部は、所与の前記処理区間において、前記第１〜第Ｎの関節に対応する第１〜第Ｎの補正係数を求め、求めた前記第１〜第Ｎの補正係数のうち最大となる値を、前記処理区間の前記時間長に乗ずることで、前記処理区間の前記時間長の前記更新処理を行ってもよい。

これにより、複数の関節を考慮して時間長の更新処理を行うこと等が可能になる。

また、本発明の一態様では、前記軌道計画部は、第ｉ（ｉは１≦ｉ≦Ｎを満たす整数）の関節の前記最大関節角速度に対する、前記第１の補間処理の結果から特定される前記処理区間での前記第ｉの関節の前記関節角速度の比を、第ｉの補正係数として求めてもよい。

また、本発明の一態様では、前記軌道計画部は、前記第１〜第Ｎの関節のいずれかの関節において、第ｊ（ｊは１≦ｊ≦Ｍを満たす整数）の経由点で移動の折り返し点が検出された場合に、前記第１〜第Ｎの関節について、前記経由点情報を、前記第ｊの経由点の前方の第１の部分経由点情報と、前記第ｊの経由点の後方の第２の部分経由点情報とに分割し、前記第１の部分経由点情報と、前記第２の部分経由点情報のそれぞれに対して、前記軌道計画処理を行ってもよい。

これにより、折り返しが発生した場合にも、適切な軌道計画処理を行うこと等が可能になる。

本発明の他の態様は、第１〜第Ｍ（Ｍは２以上の整数）の経由点を表す経由点情報に基づいて、第１〜第Ｎ（Ｎは２以上の整数）の関節を備えたロボットに対して、前記第１〜第Ｍの経由点に基づく経路を移動させるための軌道を生成する軌道計画処理を行う軌道計画部と、前記軌道計画処理の結果に基づいて、前記ロボットを制御するロボット制御部と、を含み、前記軌道計画部は、各処理区間が前記第１〜第Ｍの経由点のうちの２つの経由点の間に設定される、複数の処理区間を設定し、設定した複数の前記処理区間の各処理区間において、前記第１〜第Ｎの関節のうち少なくとも１つの関節の関節角速度が、最大速度設定処理により設定された最大関節角速度となる前記軌道計画処理を行うロボット制御システムに関係する。

これにより、高速でロボットを移動させる軌道を生成する軌道計画処理を行うこと等が可能になる。

本発明の他の態様は、上記のロボット制御システムを含むロボットに関係する。

本発明の他の態様は、第１〜第Ｎ（Ｎは２以上の整数）の関節を備えたロボットであって、第１〜第Ｍ（Ｍは２以上の整数）の経由点を表す経由点情報を取得し、２つの経由点の間に処理区間を設定するとともに、前記処理区間の時間長を設定し、前記時間長が設定された前記処理区間と、前記第１〜第Ｍの経由点での関節角の値とに基づいて、各処理区間での前記第１〜第Ｎの関節の前記関節角の値を決定する第１の補間処理を行い、前記第１の補間処理の結果から特定される前記処理区間での関節角速度と、最大速度設定処理により設定された最大関節角速度に基づいて、前記処理区間の前記時間長の更新処理を行い、前記更新処理後の前記処理区間と、前記第１〜第Ｍの経由点での前記関節角の値とに基づいて、各処理区間での前記第１〜第Ｎの関節の前記関節角の値を決定する第２の補間処理を行い、前記第２の補間処理の結果に基づいて、前記第１〜第Ｍの経由点に基づく経路を移動するロボットに関係する。

本発明の他の態様は、第１〜第Ｍ（Ｍは２以上の整数）の経由点を表す経由点情報に基づいて、第１〜第Ｎ（Ｎは２以上の整数）の関節を備えたロボットに対して、前記第１〜第Ｍの経由点に基づく経路を移動させるための軌道を生成する軌道計画処理を行うロボット制御方法であって、前記軌道計画処理は、２つの経由点の間に処理区間を設定するとともに、前記処理区間の時間長を設定し、前記時間長が設定された前記処理区間と、前記第１〜第Ｍの経由点での関節角の値とに基づいて、各処理区間での前記第１〜第Ｎの関節の前記関節角の値を決定する第１の補間処理を行い、前記第１の補間処理の結果から特定される前記処理区間での関節角速度と、最大速度設定処理により設定された最大関節角速度に基づいて、前記処理区間の前記時間長の更新処理を行い、前記更新処理後の前記処理区間と、前記第１〜第Ｍの経由点での前記関節角の値とに基づいて、各処理区間での前記第１〜第Ｎの関節の前記関節角の値を決定する第２の補間処理を行う処理であるロボット制御方法に関係する。

本発明の他の態様は、上記の各部としてコンピューターを機能させるプログラムに関係する。

ロボット制御システムを含むロボットのシステム構成例。ロボット及びその周辺配置図。本実施形態に係るロボットの例。経由点情報の具体例を説明する図。本実施形態のロボット制御の流れを説明するフローチャート。第１の実施形態の軌道計画処理を説明するフローチャート。図７（Ａ）〜図７（Ｃ）は、第１の補間処理及びその結果から決定される各関節の関節角速度の説明図。加速度及び総移動距離により特定される関節角速度の時間変化の例。第１の実施形態の変形例の軌道計画処理を説明するフローチャート。図１０（Ａ）、図１０（Ｂ）は経由点の削除処理の説明図。加速度及び総移動距離により特定される関節角速度の時間変化の他の例。第２の実施形態の軌道計画処理を説明するフローチャート。折り返しが検出された場合の処理を説明する図。折り返しが複数検出された場合の処理を説明する図。

以下、本実施形態について説明する。なお、以下に説明する本実施形態は、特許請求の範囲に記載された本発明の内容を不当に限定するものではない。また本実施形態で説明される構成の全てが、本発明の必須構成要件であるとは限らない。

１．第１の実施形態
まず第１の実施形態について説明する。具体的には、ロボットのシステム構成例を説明し、次に経由点情報の具体例及び軌道計画処理の概要について述べる。その後、経由点情報に基づく軌道計画処理の詳細について説明する。また、経由点を一部削除することでなめらかな軌道生成を行う変形例についても説明する。

１．１システム構成例
図１に、第１の実施形態に係るロボット制御システムを含むロボットのブロック図を示す。ロボットは制御部（ロボット制御システム）１、ロボットアーム２によって構成されている。

制御部１は、ロボットアーム２を制御するために必要な中央制御部１０１、メモリー１０２、アームの軌道を計画する軌道計画部１０３、アームの各関節の角度を読み取るエンコーダー読み取り部１０４、アームの各関節の角度を制御するロボット制御部（アーム制御部）１０５を備えている。

中央制御部１０１は、制御部１の各部を制御するものであり、例えばＣＰＵ等により実現されてもよい。メモリー１０２は、制御部１の各部のワーク領域となるもので、その機能はＲＡＭ等のメモリーやＨＤＤ（ハードディスクドライブ）などにより実現できる。メモリー１０２には、エンコーダー読み取り部１０４が読み取ったロボットアーム２の関節角等の情報や、軌道計画部１０３で生成された軌道情報や、ロボット制御部１０５からロボットアーム２へ出力される制御情報等、種々の情報が記憶される。またメモリー１０２は、ロボットアーム２の特性（関節角の数、可動範囲、アクチュエーター性能、連結部の長さ、作業空間における設置位置等）に関する情報を記憶しておく。

軌道計画部１０３は、複数の経由点を表す経由点情報を取得した場合に、当該経由点情報に基づいて軌道計画処理を行う。軌道計画部１０３での処理の詳細については後述する。

エンコーダー読み取り部１０４は、ロボットアーム２のアクチュエーターに設けられたエンコーダーから、ロボットアーム２の状態等の情報を読み取る。エンコーダーの値からはロボットの各関節の角度等を特定できるため、エンコーダー読み取り部１０４は、読み取り時におけるロボットの状態（位置等）を表す情報を取得することができる。

ロボット制御部１０５は、軌道計画部１０３で生成された軌道に従うように、ロボットアーム２の制御を行う。この際、フィードバック制御を行うのであれば、エンコーダー読み取り部１０４で読み取られたロボットの現在の情報を用いて、軌道に基づいて決定される目標値と、現在値との差分等からロボット制御を行うことになる。

次に、ロボットアーム２と、その周辺配置図を図２に示す。ロボットアーム２は、例えば図１に示したように３つのアクチュエーター２１１，２２１，２３１を備えている。各アームは３つのジョイント(関節)と４つのリンク(腕)から構成されており、アクチュエーターが各ジョイントを駆動することで、アームが動作する。具体的には、ロボットアーム２の接地面に連結部（リンク）２４が接続され、連結部２４と連結部２５の間に関節（ジョイント）２１が設けられる。同様に、連結部２５と連結部２６の間に関節２２が設けられ、連結部２６と連結部２７の間に関節２３が設けられる。図２の例では、アクチュエーター２１１，２２１，２３１は、それぞれ関節２１，２２，２３を駆動する。

また、アーム２にはハンド部が接続されている。具体的には、連結部２７にハンド部２８が接続されることになる。また、アクチュエーターには、現在のアクチュエーターの回転角度を正確に把握することができる、エンコーダーが搭載されている。上述のエンコーダー読み取り部１０４は、このエンコーダーの値を読み取るものである。

なお、本実施形態のロボットは図２に示した構成に限定されず、関節の数や連結部の長さ等が異なるものであってもよいし、単腕ではなく双腕ロボットであってもよい。また、図２ではロボット（ロボットアーム２）とロボット制御システム（制御部１）とが別体として構成される例を示したがこれに限定されない。例えば、図３に示すように、ロボットアーム２と制御部１とが一体として構成されてもよい。

また、図２では作業空間にカメラ３や、作業台４を配置するものとしたが、これらの配置についても種々の変形実施が可能である。

１．２経由点情報の具体例及び軌道計画処理の概要
次に、経由点情報の具体例について説明する。１つの経由点は、ロボットの１つの状態に対応する情報であり、具体的にはロボットの位置等を表す情報である。さらに具体的には、複数の関節を有するロボットにおいては各関節角の値の集合であってもよい。

具体例を図４に示す。多関節ロボットにおいては、各関節の角度が決定されれば、フォワードキネマティクスにより手先位置（例えばハンド部２８の位置）が決定できる。つまり、第１〜第Ｎ（Ｎは２以上の整数）の関節を有するロボットにおいては、各関節の角度θ_１〜θ_Ｎの組により、１つの経由点が表現されることになる。

そして、経由点がＭ個（Ｍは２以上の整数）ある場合には、図４に示したようにθ_１〜θ_Ｎの組をＭ組、すなわちθ_１１〜θ_１Ｎ、θ_２１〜θ_２Ｎ、・・・θ_Ｍ１〜θ_ＭＮを、経由点情報とすればよい。

本実施形態における軌道計画処理とは、上記の経由点情報が入力された場合に、第１〜第Ｍの経由点を可能な限り通過し、且つ目標位置（例えば最後の経由点である第Ｍの経由点を目標位置としてもよい）まで短い時間で（速い関節角速度で）移動する軌道を生成する処理となる。

ここで図４を見ればわかるように、各経由点はその関節角が決定されているものであるが、時間的な制約は付されていない。つまり本実施形態での軌道計画処理においては、経由点と経由点の間でどのような関節角の制御を行うか（経由点間をどのように補間するか）という軌道計画に加え、各経由点をどのようなタイミングで通過するか、という軌道計画も行うことになる。

そこで本実施形態では、各経由点の間に処理区間を設定し、当該処理区間の時間長をまずは暫定的な値に設定する。そして、その暫定的な値を用いて、各経由点の間を埋める第１の補間処理を実行する。各処理区間に暫定的にでも時間長が設定されれば、各経由点を横軸ｔ（時刻）、縦軸θ（関節角度）の平面にプロットすることができるため、平面上の複数の点の間を補間する処理は実行可能である。

しかし、第１の補間処理後の軌道は、なめらかな軌道になっていない可能性が高い。例えば、ある処理区間で、所与の関節角が大きく変動している場合には、上記暫定的な時間長でそれだけの変動を実現しようとすると、非常に大きな関節角速度をとる必要が生じうる。しかし、関節角速度とは対応する関節のアクチュエーターの性能等により上限が決まっているものであり、当該上限を超えるような軌道に従った制御は実現が困難である。

逆に、全ての関節において所与の処理区間での関節角の変位が小さい場合、当該処理区間では全ての関節の関節角速度が非常に小さいものとなる。この場合、当該処理区間の移動にはさほど時間をかけなくてもよいのに、上記暫定的な時間長を設定したことで、無為に移動時間を要している、すなわち機械的な速度性能を十分発揮していないということになる。

よってここでは、第１の補間処理の結果をそのまま用いるのではなく、第１の補間処理の結果から取得される各処理区間での各関節の関節角速度と、最大速度設定処理により設定された（例えば機械的な特性により決定される）各関節の最大関節角速度とを比較し、各処理区間の時間長を更新する更新処理を行う。上述したように、暫定的な時間長が相対的に短いことで、機械的な限界を超える軌道計画がなされている処理区間であれば、時間長を長くする更新処理を行って、当該処理区間での関節角速度を抑える処理を行う。逆に、暫定的な時間長が相対的に長いことで、機械的な限界性能を十分発揮できない軌道計画がなされている処理区間であれば、時間長を短くする更新処理を行って、当該処理区間での関節角速度を増大させる処理を行う。

処理区間の時間長の更新処理が行われた場合、横軸ｔ、縦軸θの平面における各経由点のプロットされる位置が変化することになる。よって、更新処理後の時間長を用いて、再度補間処理（第２の補間処理）を行い、第２の補間処理の結果を軌道計画処理の出力とする。

１．３軌道計画処理の詳細
本実施形態では、各関節のジョイント角度が単調に増加または単調に減少することで目標位置に到達可能な経路に対して軌道を計画し、ロボット（ロボットアーム２）を制御する例を示す。

図５、図６に本実施形態の処理を説明するフローチャートを示す。図５がロボット制御全体の処理フローである。この処理を開始すると、まず各処理モジュールおよびアームの初期化を行う。この初期化の際、各関節の現在の角度を読み取る（ステップＳ１０１）。この現在の角度が生成する経路のスタート位置に対応する。

次に、目標位置に到達可能な、経路情報を入力とした、軌道計画処理を実行する（ステップＳ１０２）。ステップＳ１０２の軌道計画処理の流れを図６に示す。

軌道計画処理では、まず経由点情報を読み込む（ステップＳ２０１）。この情報は、図４に示したように、各関節の角度がそれぞれアームの移動に伴って通過する順番に記録されているデータである。例えば図７（Ａ）に示したように、３関節ロボットであれば関節１〜３について、それぞれ時系列的に経由点の情報が取得される。次に、各経由点に対して、暫定の時刻を設定する（ステップＳ２０２）。例えば、１つ目の経由点には１ｓ後に到達、２つ目の経由点は２ｓ後に到達するという、仮の時刻データを設定する。これは各処理区間の時間長を暫定的に決定することと同義である。

次に、第１のスプライン補間処理により、各経由点を速度連続の条件でつなぐ、３次式を求める。（ステップＳ２０３）。ここで、移動開始時と、移動終了時の速度が０であるという条件を設定することで、すべての区間に対して、３次の補間式を決定することができる。この結果の例を示したものが図７（Ｂ）である。

なおスプライン補間については広く知られた補間手法であるため、詳細な説明は省略する。また、本実施形態の補間処理はスプライン補間処理に限定されるものではなく、他の補間処理を適用してもよい。

次に、ステップＳ２０３で求めたスプライン係数から、各処理区間、各関節に対して取りうる最大速度（以下、区間最大関節角速度）を計算し、区間の時間を調整する（ステップＳ２０４）。第１のスプライン補間処理を行うことで、各経由点の間は３次曲線により結ばれることになる。そして、対象としている座標系はθ―ｔ平面であるから、当該曲線の接線の傾きが関節角速度を表すことになる。よって例えば、所与の処理区間での経路を表す曲線の接線の傾きを求め、当該処理区間のなかでの接線の傾きの最大値を、当該軌道で取る区間最大関節角速度とすればよい。なお、３次曲線により補間をする場合には、極値を持つ場合には当該極値で傾きが最大になり、極値を持たない場合には処理区間の始点、又は終点に対応する経由点において傾きが最大となる。よって、補間の手法、補間に用いる曲線の特性によっては、処理区間全体での接線の傾きをもとめることなく、傾きの最大値を求めることも可能である。例えば、図７（Ｂ）に基づいて、各関節の関節角速度を求めた結果が図７（Ｃ）である。

第１のスプライン補間処理の結果から求められる区間最大関節角速度が、機械的に決定される最大関節角速度を超過している、或いは全ての関節で最大関節角速度に到達していない場合には、上述したように適切な経路を生成したとは言えない。よってここでは、求めた区間最大速度が、各関節に対してあらかじめ設定された最大関節角速度に対して、何倍超過しているかの比率を求め、それらの比率が最大となる関節の値を用いて、ステップＳ２０２で設定した区間の時間長を補正する。具体的には、先の比率を時間長に掛けた値を、新しい区間の時間長とする。この操作により、すべての区間で、いずれの関節も、あらかじめ決められた最大関節角速度を超える可能性を抑止した軌道を生成することができる。

比率の最大値が１未満の場合は、当該比率を時間長に掛けることで、時間長を短縮する更新処理を行うことになる。これはつまり、機械的な性能を十分発揮できていなかった処理区間について、区間最大関節角速度を上げるような軌道生成を行う処理に相当する。

なお、ステップＳ２０３〜ステップＳ２０４を複数回繰り返すことで、速度の変動が少ないなめらかな軌道を生成する効果が得られるため、複数回繰り返してもよい。

そして最後に、時間長の更新処理後の処理区間を用いて補間処理（第２のスプライン補間処理）を行う（ステップＳ２０５）。ステップＳ２０４の終了時には、各処理区間の時間長は、全関節について最大関節角速度を超えず、且つ少なくとも１つの関節について最大関節角速度（或いはそれに近い速度）をとるという条件を満たすことが期待される値となっている。よって、その結果を用いて補間処理を行えば、速度条件を満たしつつ各経由点を通過する経路を設定することができる。

以上の処理で求めたスプライン補間の係数に従って、逐次の軌道生成処理を行い（ステップＳ１０３）、アームへの位置指令を実施する（ステップＳ１０４）。このステップＳ１０３，ステップＳ１０４の処理を、累計の移動時間が、計画した移動時間を超過するまで繰り返す（ステップＳ１０６）。なお、ステップＳ１０５の待機時間は制御部１からロボットアーム２への制御信号の出力周期等により決定されるものである。以上で、ロボットの移動制御が終了する。

１．４変形例
上述の手法で生成されたスプライン関数による軌道は、経由点同士が十分に離れており、加速および減速時間が十分確保されている場合には、なめらかな制御が可能であるが、経由点同士が距離的に近い場合には、移動開始直後から最大速度を目指して加速を始めるため、機械的な加速度の限界を超えてしまう。

関節を駆動するアクチュエーターには、速度の制限だけでなく加速度の制限も存在する。そのため、関節を動かし始めた直後に最大関節角速度に到達させることはできない。通常、アクチュエーターの機械的な性能をフルに発揮した場合、関節角速度の時間変化は、図８に示すように台形形状の軌道を描く。図８は縦軸が関節角速度ｖ、横軸が時間ｔである。つまり、スタート時とゴール時には速度が０になるという制約の下、ロボットを制御する場合には、図８のＡ１に示した加速区間、Ａ２に示した等速区間、Ａ３に示した減速区間の３つのフェーズを移行することになる。

Ａ１の加速区間は、アクチュエーターの機械的な性能により決定される最大関節角加速度をもって、最大関節角速度に向かって加速している状態である。Ａ２の等速区間は、最大関節角速度に到達し、当該速度を維持している状態である。Ａ３の減速区間は、最大関節角速度から停止状態へ向かって、最大関節角減速度で減速している状態である。

以上のことからわかるように、加速区間Ａ１と減速区間Ａ３においては、関節角速度は最大関節角速度をとることができない。しかしながら、図６のフローチャート等に示した上述の処理においては、仮に加速区間又は減速区間に経由点があったとしても、最大関節角速度をとることができるものとして軌道計画処理を行っている。そのため、現在位置と目標位置が近い場合や、経由点が密に設定されている場合等、加速区間又は減速区間に経由点が存在する場合に、当該経由点周辺での処理区間では、現実には取り得ない速度でロボットを制御しようとするおそれがある。

そこで本変形例では、移動開始時刻から所定の時間（Ｔａ）以内に通過する経由点がある場合、当該経由点は加速区間に存在するものとして、それらの経由点をすべて削除する処理を行う。また、移動終了時刻から所定の時間（Ｔｄ）以内に通過する経由点がある場合、当該経由点は減速区間に存在するものとして、それらの経由点をすべて削除する処理を行う。

Ｔａ，Ｔｄの値としては、図６のステップＳ２０４での時間調整後の経路の総移動時間（始点から終点までの移動に要する時間）が、所定の値（例えば１．０ｓ）を超える場合は、あらかじめ決められた時間（Ｔａ，Ｔｄ）＝（０．３ｓ，０．３ｓ）などに設定する。このあらかじめ決められた時間とは、図８のｔ１，ｔ２に示したように最大関節角速度と、最大関節角加速度の情報に基づいて、事前に計算したものであってもよい。或いは、最大関節角速度と、最大関節角加速度の値によらず、所与の標準的な値を設定してもよい。また、総移動時間が所定の値未満の場合は、（Ｔａ，Ｔｄ）＝（総移動時間の３０％，総移動時間の３０％）のように設定してもよく、Ｔａ，Ｔｄの値設定については種々の変形実施が可能である。

本変形例の処理について図９のフローチャートを用いて説明する。なお、ロボット制御全体の処理フローは、図５と同様である。ステップＳ３０１〜ステップＳ３０４については、図６のステップＳ２０１〜ステップＳ２０４と同様であるため説明は省略する。ステップＳ３０４の時間長の更新処理により、各経由点には当該経由点に到達するまでの時間を表す情報が対応づけられている。よって、その時間情報と、上述のＴａとを比較し、各経由点が加速区間に含まれるかを判定し、含まれる場合には当該経由点を削除する（ステップＳ３０５）。同様に、各経由点の時間情報と、上述のＴｄとを比較し、各経由点が減速区間に含まれるかを判定し、含まれる場合には当該経由点を削除する（ステップＳ３０６）。

そして、最後に、ステップＳ３０５，ステップＳ３０６で経由点を削除した経路に対して、スプライン補間（第２のスプライン補間処理）を行う。（ステップＳ３０７）。このスプライン補間では、ステップＳ３０４で調整した時間が維持されているため、総移動時間には変化がないが、加速区間、および減速区間がそれぞれ１つの３次式で表現されるようになるため、より滑らかな加速と減速とを実現することができる。

具体例を図１０（Ａ）、図１０（Ｂ）に示す。図１０（Ａ）に示しように、加速区間に含まれる経由点（図１０（Ａ）で言えばＢ１〜Ｂ３）を通過する経路を設定した場合、当該経由点を通過するものとしたために、機械的な性能をフルに発揮したとしても取り得ない速度を設定している可能性がある。図１０（Ａ）では現在位置（始点）〜Ｂ１の処理区間で速度超過であり、最大関節角加速度で加速したとしても到達できない関節角速度になっている。

その点、ステップＳ３０５の処理により加速区間に含まれる経由点Ｂ１〜Ｂ３を削除しておけば、現在位置と、加速区間外の経由点（図１０（Ｂ）で言えばＢ４）を補間処理によりなめらかにつなげばよい。よって、図１０（Ａ）に比べて急激な加速を行う箇所はなくなり、結果としてアクチュエーターの機械的な性能を超えるような軌道生成を行うおそれを抑止できる。これは減速区間についても同様である。

以上の処理により、先行例では実現できなかった、複数の細かい経由点を指定した場合でも、経路の途中の区間では最大速度での移動を実行しつつ、加速区間および減速区間では、なめらかな加減速を実現することができる。なお、本変形例では削除された経由点については、当該点をロボットが通過しない可能性が出てくるが、機械的に無理な制御を行わないことを重視する観点から、当該可能性を許容するものである。

以上の本実施形態では、ロボット制御システムは図１（図１では制御部１がロボット制御システムに対応）に示したように、第１〜第Ｍ（Ｍは２以上の整数）の経由点を表す経由点情報に基づいて、第１〜第Ｎ（Ｎは２以上の整数）の関節を備えたロボットに対して、第１〜第Ｍの経由点に基づく経路を移動させるための軌道を生成する軌道計画処理を行う軌道計画部１０３と、軌道計画処理の結果に基づいて、ロボット（狭義には図１に示したようにロボットアーム２）を制御するロボット制御部１０５を含む。そして、軌道計画部１０３は、２つの経由点の間に処理区間を設定するとともに、処理区間の時間長を設定し、時間長が設定された処理区間と、第１〜第Ｍの経由点での関節角の値とに基づいて、各処理区間での第１〜第Ｎの関節の関節角の値を決定する第１の補間処理を行い、第１の補間処理の結果から特定される処理区間での関節角速度と、最大速度設定処理により設定された最大関節角速度に基づいて、処理区間の時間長の更新処理を行い、更新処理後の処理区間と、第１〜第Ｍの経由点での関節角の値とに基づいて、各処理区間での第１〜第Ｎの関節の関節角の値を決定する第２の補間処理を行う処理を、軌道計画処理として行う。

ここで、経由点情報とは例えば図４に示したように、Ｎ関節ロボットであれば各関節の関節角の値の集合として表現される情報である。Ｎ関節ロボット（Ｎ自由度ロボット）であれば、各関節の関節角を決定すれば、手先位置やロボットの姿勢を一意に決定することが可能となる。つまり、各関節に対応するＮ個の角度値の組により、１つの経由点が決定されることになる。また、経由点は複数あることが想定されるため、Ｎ個の角度値の組をＭ組用いることで、Ｍ個の経由点を含む経由点情報が表現されることになる。なお、本明細書においては、「第ｉの関節についての経由点情報」といったように、経由点情報を関節毎に分けて考えるケースも想定している。これは、第１，第２の補間処理が関節毎に行われるためである。つまり、上述したように本来であれば経由点はＮ個の角度値の組を用いて表現するものではあるが、そのうちの所与の関節の関節角の値だけを取り出した情報についても、本実施形態の経由点情報に含まれるものとする。

また、「第１の補間処理の結果から特定される処理区間での関節角速度」とは、図７（Ｃ）に示した関節角速度に対応する。処理区間の時間長を設定することで、関節毎に各経由点をθ―ｔ平面上にプロットすることができ、そのプロットされた点の間を埋める処理が第１の補間処理である。つまり、第１の補間処理の結果として、図７（Ｂ）に示したように関節角の時間変化グラフを取得することができるのであるから、それを微分すれば図７（Ｃ）に示したように関節角速度の時間変化グラフを取得することができる。ここでは処理区間毎に最大関節角速度との比較処理を行う関係上、図７（Ｃ）で得られる関節角速度のうち、対象としている処理区間での最大値を「第１の補間処理の結果から特定される処理区間での関節角速度」として用いることが想定される。この点は、第２の補間処理についても同様である。なお、関節角の時間変化から、関節角速度を求める（微分を行う）手法は種々考えられ、例えば対象とする点とその近傍の点の差分を用いてもよい。或いは、スプライン補間を用いる場合、多項式（狭義には３次式）を用いて処理区間を補間するため、当該多項式を直接微分することも容易である。

また、最大関節角速度とは、最大速度設定処理により設定される関節角速度であり、本実施形態の軌道計画処理では、各関節の関節角速度が、当該関節についての最大関節角速度を超える可能性を抑止することが可能になる。つまり、ロボットの軌道計画において、これ以上の速度を出すことが好ましくないという上限がユーザーの入力等により事前にわかっている場合には、当該速度の上限を最大関節角速度として設定する。よって、本実施形態においては、関節を駆動するアクチュエーターの性能から決定される機械的な限界速度を最大関節角速度として設定していたが、これには限定されない。本実施形態での最大関節角速度は、本実施形態の手法を用いた結果、全ての関節の関節角速度の最大値が、当該最大関節角速度以下となることが期待されるという意味で「最大」と表現しているに過ぎず、ロボットの性能（アクチュエーター性能等）から決定される限界速度と必ずしも一致するものではないということである。例えば、アクチュエーター等の部品の消耗や、他のロボット動作とのタイミング合わせ、ユーザーと衝突した場合の危険性軽減、省電力化等の観点から、アクチュエーター等から決定される「機械的な最大関節角速度」に比べて小さい値を、前記最大関節角速度として設定することは充分考えられる。

ここで、本実施形態のアクチュエーターはモーターであってもよいし、圧電アクチュエーター（ピエゾアクチュエーター）であってもよいし、他のアクチュエーターであってもよく、種々の変形実施が可能である。

これにより、処理区間の時間長を、補間処理に基づいて得られた関節角速度と、設定された最大関節角速度とに基づいて更新することで、各処理区間、各関節の関節角速度が最大関節角速度を超過する可能性を抑止し、所望の速度上限により安定的にロボットを動作させることが可能になる。また、全関節について補間処理に基づいて得られた関節角速度が最大関節角速度に満たない場合には、関節角速度を増加させることもできるため、最大関節角速度を超過しないことを前提としつつ、高速でロボットを移動させる軌道を生成することも可能である。

また、軌道計画部１０３は、ロボットの移動開始時刻から所与の加速期間内に含まれる経由点を削除する処理と、ロボットの移動終了時刻から所与の減速期間内に含まれる経由点を削除する処理の少なくとも一方の処理を、削除処理として行い、第１〜第Ｍの経由点のうち、削除処理により削除されなかった経由点に基づいて、第２の補間処理を行ってもよい。

これにより、本実施形態の変形例において図１０（Ａ）、図１０（Ｂ）を用いて説明したように、よりなめらかな（関節角速度が上記最大関節角速度を超過する可能性をより抑止した）軌道計画処理を行うことが可能になる。本実施形態では、スタートとゴールではロボットの速度が０になることを前提としている（例えば上述の説明では、この制約条件によりスプライン補間を可能にしている）。加速減速にも限界がある以上、スタート直後及びゴール直前には、上述した最大関節角速度をとることができない期間が存在するため、これらを考慮することで、軌道をよりなめらかにすることができる。

また、軌道計画部１０３は、更新処理後の処理区間の時間長に基づいて、ロボットの移動開始時刻から、第１〜第Ｍの経由点の各経由点に到達するまでの時間である第１の時間情報と、第１〜第Ｍの経由点の各経由点への到達後、ロボットの移動終了時刻までの時間である第２の時間情報の少なくとも一方を求め、第１の時間情報と加速期間の比較処理と、第２の時間情報と減速期間の比較処理の少なくとも一方に基づいて、第１〜第Ｍの経由点の各経由点が加速期間又は減速期間に含まれるかの判定を行って削除処理を行ってもよい。

これにより、経由点の適切な削除処理が可能になる。第１の補間処理を行った段階では、処理区間の時間長は暫定的に設定されたものであり、この時間長を用いて各経由点への到達時間を算出したとしても、それは最大関節角速度を一切考慮したものではなく意義の高いものとは言えない。それに対して、時間長の更新処理後は、加速区間・減速区間での例外を除けば、関節角速度の最大関節角速度の超過を抑止する時間長が得られている。つまり、適切な到達時間の算出には更新処理後の時間長を用いることが必要となる。なお、移動開始から、所与の経由点までの到達時間は、当該経由点に至るまでの処理区間の時間長の総和等から求めることができる。また、移動開始時から移動終了時までに要する時間も全処理区間の時間長の総和等からも止めることができるため、第１，第２の時間情報は算出可能である。

また、軌道計画部１０３は、第１〜第Ｎの関節の第ｉ（ｉは１≦ｉ≦Ｎを満たす整数）の関節を駆動する第ｉのアクチュエーターの性能に基づいて、第ｉの関節の最大関節角速度を設定する処理を、最大速度設定処理として行ってもよい。

これにより、機械的な限界を考慮して最大関節角速度を設定することが可能になる。機械的な限界値をそのまま用いれば、現実に不可能な挙動となる軌道生成を行う可能性を抑止し、且つ実現可能な最速の軌道を生成することが可能になる。また、部品の消耗や、部品毎の性能のブレ等を考慮して、機械的な限界値に対して、定数α（＜１）を乗じて得られる値を最大関節角速度とする等の処理も可能である。

また、軌道計画部１０３は、第ｉのアクチュエーターの性能に基づいて決定される機械的な最大速度に基づいて、第ｉの関節の最大関節角速度を設定する処理を、最大速度設定処理として行ってもよい。

これにより、速度的な限界に基づいて最大関節角速度を設定することが可能になる。例えば、本実施形態において上述したように、機械的な限界速度をそのまま最大関節角速度としてもよい。

また、軌道計画部１０３は、設定された最大関節角速度に対する、第１の補間処理の結果から特定される処理区間での関節角速度の比を求め、求めた比を処理区間の時間長に乗ずることで、処理区間の時間長の更新処理を行ってもよい。

これにより、補間処理から得られる関節角速度（図７（Ｃ）の縦軸の値に相当）と、設定された最大関節角速度との比から、時間長の更新処理を行うことが可能になる。暫定的な時間長において、関節角速度が最大関節角速度に対してｘ倍となっていたのであれば、時間長をｘ倍すれば、当該処理区間での関節角速度は更新処理前の１／ｘ倍程度になることが期待され、最大関節角速度を超過しない可能性が高くなる。さらに、最大関節角速度を考慮して倍率を決定するため、全ての関節について、関節角速度が最大関節角速度に対して著しく小さいという可能性も抑止できる。ただし、本実施形態の補間処理は直線（１次式）を用いる手法を除外するものではないが、なめらかな軌道生成の観点から多項式等が用いられると考えられる。よって時間長（横軸の長さ）をｘ倍にしたからといって、当該処理区間での関節角速度（傾き）が１／ｘ倍になることは確定しておらず、場合によっては最大関節角速度を超過する可能性があることに留意すべきである。そのため、上述したように時間長の更新処理と第２の補間処理を繰り返すことで精度向上を図ってもよい。

また、軌道計画部１０３は、所与の処理区間において、第１〜第Ｎの関節に対応する第１〜第Ｎの補正係数を求め、求めた第１〜第Ｎの補正係数のうち最大となる値を、処理区間の時間長に乗ずることで、処理区間の時間長の更新処理を行ってもよい。

これにより、複数の関節について考慮したうえで時間長の更新処理を行うことが可能になる。Ｎ関節ロボットにおいては、Ｎ個の関節のうち少なくとも１つでも、当該関節の最大関節角速度を超過しているのであれば、それは所望の条件にそぐわない不自然な軌道を生成していることになる。つまりロボット全体を見たときに適切な軌道計画処理を行うためには、全関節のうち最も条件を満たさない危険性が高い関節、すなわち補正係数が最大となる関節を用いて時間長の更新処理を行うとよい。

また、軌道計画部１０３は、第ｉ（ｉは１≦ｉ≦Ｎを満たす整数）の関節の最大関節角速度に対する、第１の補間処理の結果から特定される処理区間での第ｉの関節角速度の比を、第ｉの補正係数として求めてもよい。

これにより、所与の処理区間に着目した場合、関節毎に補正係数を求め、それを用いて時間長の更新処理を行うことが可能になる。各関節の最大関節角速度は、同一の値を用いることや、結果として同一の値となることを妨げないが、関節毎に設定することが想定される。また、処理区間での関節角速度は図７（Ｃ）からも明らかなように関節毎に求められる。つまり、処理区間での関節角速度と最大関節角速度との比を補正係数として用いる場合には、関節毎に当該比の値を求める必要がある。

また以上の本実施形態では、ロボット制御システムは、第１〜第Ｍ（Ｍは２以上の整数）の経由点を表す経由点情報に基づいて、第１〜第Ｎ（Ｎは２以上の整数）の関節を備えたロボットに対して、前記第１〜第Ｍの経由点に基づく経路を移動させるための軌道を生成する軌道計画処理を行う軌道計画部１０３と、軌道計画処理の結果に基づいて、ロボットを制御するロボット制御部１０５を含み、軌道計画部は、各処理区間が第１〜第Ｍの経由点のうちの２つの経由点の間に設定される、複数の処理区間を設定し、設定した複数の処理区間の各処理区間において、第１〜第Ｎの関節のうち少なくとも１つの関節の関節角速度が、最大速度設定処理により設定された最大関節角速度となる軌道計画処理を行う。

これにより、各関節について最大関節角速度を超過する可能性を抑止しつつも、少なくとも１つの関節については、自身の最大関節角速度で動作することが可能になる。つまり、なめらかな動作を行いつつ、できるだけ高速で移動可能な軌道を生成することが可能になる。

また以上の本実施形態は、上記のロボット制御システムを含むロボットに適用できる。

これにより、上述の軌道計画処理の結果に従って動作するロボットを実現することが可能になる。ここでのロボットは、図２に示したようにロボット制御システム（制御部１）と、制御対象（ロボットアーム２等）が別体として設けられるものであってもよいし、図３に示したように一体として設けられるものであってもよい。

なお、本実施形態のロボット制御システム等は、その処理の一部または大部分をプログラムにより実現してもよい。この場合には、ＣＰＵ等のプロセッサーがプログラムを実行することで、本実施形態のロボット制御システム等が実現される。具体的には、非一時的な情報記憶媒体に記憶されたプログラムが読み出され、読み出されたプログラムをＣＰＵ等のプロセッサーが実行する。ここで、情報記憶媒体（コンピューターにより読み取り可能な媒体）は、プログラムやデータなどを格納するものであり、その機能は、光ディスク（ＤＶＤ、ＣＤ等）、ＨＤＤ（ハードディスクドライブ）、或いはメモリー（カード型メモリー、ＲＯＭ等）などにより実現できる。そして、ＣＰＵ等のプロセッサーは、情報記憶媒体に格納されるプログラム（データ）に基づいて本実施形態の種々の処理を行う。即ち、情報記憶媒体には、本実施形態の各部としてコンピューター（操作部、処理部、記憶部、出力部を備える装置）を機能させるためのプログラム（各部の処理をコンピューターに実行させるためのプログラム）が記憶される。

２．第２の実施形態
第２の実施形態のロボット制御システム等の構成例は、第１の実施形態で説明した図１、図２と同様であるため詳細な説明は省略する。

第１の実施形態では、各関節の移動距離が十分長いことを仮定していた。そのため、速度変化のグラフも図８に示したように台形軌道を描くものとした。しかし、経路の総移動距離が短い場合は、加速区間、および減速区間の時間を確保したとしても、加速度の制限により、到達できる速度は、あらかじめ設定された最大速度よりも小さくなる。

具体例を図１１に示す。経路の総移動距離はｖ−ｔ平面のグラフでは、速度変化を表す線とｔ軸とで囲まれる図形の面積に相当し、これはロボットの現在位置と目標位置が決定されれば、そこから決定されるものである。よって総移動距離が決定された場合には、図形の面積が当該総移動距離の値となり、且つ最大関節角加速度、最大関節角減速度をもって関節角速度が変化するようにｖ−ｔグラフを描くことで、アクチュエーターの機械的な性能をフルに発揮した場合の速度変化グラフを取得することができる。

その際、図形の面積が小さい場合には、図１１に示したように、加速区間Ｃ１で最大関節角速度に到達せず、等速区間を経ずに減速区間Ｃ２に移行するケースが出てくる。この場合、三角形の頂点に対応する速度ｖ_ｍｉは、機械的に決定される最大関節角速度に比べて小さい値となる。

そこで、本実施形態では、最大関節角速度を考慮した時間調整（時間長の更新処理）の際に、各関節が移動する総距離Ｄ_ｉと、各関節の限界加速度Ａ_ｉに応じて、各関節が到達可能な最大速度ｖ_ｍｉを求め、このｖ_ｍｉを用いて時間調整を行う。

仮に移動時間をＴとすると、最大加速度Ａ_ｉで加減速した場合、移動距離Ｄ_ｉは最大関節角速度で移動時間を移動した場合の半分となるため、下式（１）が成り立ち、また、最大関節角加速度で移動時間の半分を加速すると、最大速度ｖ_ｍｉとなるため、下式（２）が成り立つ。

Ｄ_ｉ＝Ｔ＊Ｖ_ｍｉ／２・・・・・（１）
Ａ_ｉ＊Ｔ／２＝Ｖ_ｍｉ・・・・・（２）
よって、上式（１）、（２）の２式から、下式（３）によりｖ_ｍｉが求められる。

ｖ_ｍｉ＝ｓｑｒｔ（Ａ_ｉＤ_ｉ）・・・・・（３）
本実施形態では、第１の実施形態の最大関節角速度（機械的に決定される最大関節角速度）に変えて、上式（３）で求めたｖ_ｍｉを用いて同様の処理を行う。

図１２に本実施形態の軌道計画処理を説明するフローチャートを示す。図１２のステップＳ４０１〜ステップＳ４０３については図６のステップＳ２０１〜ステップＳ２０３と同様である。

そして、本実施形態では上式（３）によって、第ｉの関節の加速度制限（Ａ_ｉ）と総移動距離（Ｄ_ｉ）に応じた最大関節角速度（到達可能速度ｖ_ｍｉ）を求め（ステップＳ４０４）、求めた最大関節角速度を用いて、区間の時間調整を行う（ステップＳ４０５）。ステップＳ４０５では、スプライン補間の補間係数から求めた区間最大速度が、ステップＳ４０４で求めた最大関節角速度ｖ_ｍｉに対して何倍超過しているかの比率を各関節について求め、それらの比率が最大となる関節の値を用いて、ステップＳ４０２で設定した処理区間の時間長を更新する。

ステップＳ４０６〜ステップＳ４０８については、図９のステップＳ３０５〜ステップＳ３０７と同様である。なお、本実施形態では（Ｔａ，Ｔｄ）＝（総移動時間の３０％，総移動時間の３０％）のように設定することが想定されるが、上述したようにＴａ，Ｔｄの設定手法は種々の変形実施が可能である。

以上の変更を行うことで、総移動距離が短い場合でも、加速度の限界を考慮した、なめらかな加減速を伴った、高速な移動を実現することができる。

以上の本実施形態では、ロボット制御システムの軌道計画部１０３は、第ｉのアクチュエーターの性能に基づいて決定される機械的な最大加速度と、経路の始点と終点の間の第ｉの関節の関節角の変位である総移動距離とに基づいて、総移動距離を移動する間に到達できる到達可能速度を求め、求めた到達可能速度に基づいて、第ｉの関節の最大関節角速度を設定する処理を、最大速度設定処理として行う。

これにより、総移動距離（関節角の変位）が短い場合に、より適切な最大関節角速度を設定し、なめらかな軌道を生成することが可能になる。総移動距離が短い場合とは、図１１に示したように機械的な最大加速度、最大減速度を用いたとしても、機械的な最大関節角速度に到達することができない。この場合に、機械的な最大関節角速度等を用いる第１の実施形態の処理を行ってしまうと、例えば機械的に実現不可能な軌道を生成する危険性が高い。よってここでは、図１１に示した到達可能速度ｖ_ｍｉを最大関節角速度として用いることで、よりなめらかな軌道生成を実現する。

３．第３の実施形態
第１，第２の実施形態では、全ての関節についてその関節角の値は単調増加、或いは単調減少することを仮定していた。しかし、経路によっては１又は複数の関節で折り返し動作が行われることも考えられる。そこで、本実施形態では、経路内の折り返しも考慮し、各関節が同一方向に移動する区間ごとに、到達できる最大速度を求め、それらの最大速度に応じて各区間の時間調整を行う。

ある関節において折り返し動作が発生した場合、その折り返し点においては関節角速度は０となる。第１，第２の実施形態では、始点及び終点において関節角速度が０になるという条件で処理を行ったが、本実施形態では当該折り返し点においても関節角速度が０となるように軌道計画処理を行う必要が生じる。

そこで本実施形態では、経由点情報を取得した場合に、いずれかの関節において折り返し点が発生しているか否かの判定を行い、折り返し点があった場合には、入力された経由点情報を、図１３の関節１に示したように当該折り返し点より前方の第１の部分経由点情報と、当該折り返し点より後方の第２の部分経由点情報とに分割する。このようにすれば、第１の部分経由点情報では始点（ロボットの現在位置）及び終点（折り返し点）での速度が０となり、第２の部分経由点情報においても始点（折り返し点）及び終点（ロボットの目標位置）での速度が０となる。つまり、部分経由点情報のそれぞれを対象とすれば、上述した第１の実施形態或いは第２の実施形態の手法をそのまま適用することが可能である。

なお本実施形態では、所与の関節で折り返し点が発生している場合、当該点においては、他の関節の関節角速度も０になっている（折り返しているわけではないが、一時的に停止する）ことを仮定し、処理を簡略化する。このようにすれば、図１３に示したように、関節１で折り返し点が発生している場合、折り返し点が発生していない関節２についても、第１の関節と同様に部分経由点情報に分割することができ、第１，第２の実施形態の手法をそのまま適用可能である。

複数の関節で折り返し点が発生している場合、或いは１つの関節で複数の折り返し点が発生している場合にも同様に考えればよい。つまり、全ての折り返し点で経由点情報を分割する、そして所与の関節での折り返し点は他の関節での部分経由点情報の設定に影響する、という観点で処理を行えばよく、例えば図１４に示したように３つ以上の部分経由点情報に分割する処理が行われることになる。

なお、各部分経由点情報に対しては、上述したように第１の実施形態の手法を適用してもよいし、第２の実施形態の手法を適用してもよい。ただし、部分経由点情報に分割されることで、各部分経由点情報に対応する経路での総移動距離が短くなる傾向にあり、速度変化が図１１に示した状態となる可能性が高まる。その点を考慮すれば、各部分経由点情報に対しては第２の実施形態の手法を適用することを積極的に検討してもよい。

以上の本実施形態では、ロボット制御システムの軌道計画部１０３は、第１〜第Ｎの関節のいずれかの関節において、第ｊ（ｊは１≦ｊ≦Ｍを満たす整数）の経由点で移動の折り返し点が検出された場合に、第１〜第Ｎの関節について、経由点情報を、第ｊの経由点の前方の第１の部分経由点情報と、第ｊの経由点の後方の第２の部分経由点情報とに分割し、第１の部分経由点情報と、第２の部分経由点情報のそれぞれに対して、軌道計画処理を行う。

これにより、折り返しが発生した場合にも適切に軌道計画処理を行うことが可能になる。折り返しが発生した場合、折り返し点では関節角速度が０になるという新たな制約が加わることになる。つまり、折り返し点を考慮せずに第１，第２の実施形態を実行したとしても、折り返し点での速度を０にすることができず、適切な軌道計画処理を行うことはできない。ここで第１，第２の実施形態の手法を考えると、ロボットの現在位置及び目標位置（移動の開始及び終了に対応）では、全関節の関節角速度が０になり、その点は考慮された処理となっている。つまり、経由点情報の両端点については、速度が０であってもよいということである。よってここでは、折り返し点が端点となるように経由点情報を複数の部分経由点情報に分割する。全ての折り返し点を考慮して部分経由点情報を設定すれば、各部分経由点情報は端点での速度が０になり、端点以外では速度が０にならないため、部分経由点情報を処理対象とすれば、第１，第２の実施形態の手法そのままで、軌道計画処理を行うことが可能になる。

以上、本発明を適用した３つの実施の形態１〜３およびその変形例について説明したが、本発明は、各実施の形態１〜３やその変形例そのままに限定されるものではなく、実施段階では、発明の要旨を逸脱しない範囲内で構成要素を変形して具体化することができる。また、上記した各実施の形態１〜３や変形例に開示されている複数の構成要素を適宜組み合わせることによって、種々の発明を形成することができる。例えば、各実施の形態１〜３や変形例に記載した全構成要素からいくつかの構成要素を削除してもよい。さらに、異なる実施の形態や変形例で説明した構成要素を適宜組み合わせてもよい。また、明細書又は図面において、少なくとも一度、より広義または同義な異なる用語と共に記載された用語は、明細書又は図面のいかなる箇所においても、その異なる用語に置き換えることができる。このように、発明の主旨を逸脱しない範囲内において種々の変形や応用が可能である。

１制御部、２ロボットアーム、３カメラ、４作業台、２１，２２，２３関節、
２４，２５，２６，２７連結部、２８ハンド部、
１０１中央制御部、１０２メモリー、１０３軌道計画部、
１０４エンコーダー読み取り部、１０５ロボット制御部

Claims

第１〜第Ｍ（Ｍは２以上の整数）の経由点を表す経由点情報に基づいて、第１〜第Ｎ（Ｎは２以上の整数）の関節を備えたロボットに対して、前記第１〜第Ｍの経由点に基づく経路を移動させるための軌道を生成する軌道計画処理を行う軌道計画部と、
前記軌道計画処理の結果に基づいて、前記ロボットを制御するロボット制御部と、
を含み、
前記軌道計画部は、
２つの経由点の間に処理区間を設定するとともに、前記処理区間の時間長を設定し、
前記時間長が設定された前記処理区間と、前記第１〜第Ｍの経由点での関節角の値とに基づいて、各処理区間での前記第１〜第Ｎの関節の前記関節角の値を決定する第１の補間処理を行い、
前記第１の補間処理の結果から特定される前記処理区間での関節角速度と、最大速度設定処理により設定された最大関節角速度に基づいて、前記処理区間の前記時間長の更新処理を行い、
前記更新処理後の前記処理区間と、前記第１〜第Ｍの経由点での前記関節角の値とに基づいて、各処理区間での前記第１〜第Ｎの関節の前記関節角の値を決定する第２の補間処理を行う処理を、
前記軌道計画処理として行うことを特徴とするロボット制御システム。
請求項１において、
前記軌道計画部は、
前記ロボットの移動開始時刻から所与の加速期間内に含まれる前記経由点を削除する処理と、前記ロボットの移動終了時刻から所与の減速期間内に含まれる前記経由点を削除する処理の少なくとも一方の処理を、削除処理として行い、
前記第１〜第Ｍの経由点のうち、前記削除処理により削除されなかった前記経由点に基づいて、前記第２の補間処理を行うことを特徴とするロボット制御システム。
請求項２において、
前記軌道計画部は、
前記更新処理後の前記処理区間の前記時間長に基づいて、前記ロボットの前記移動開始時刻から、前記第１〜第Ｍの経由点の各経由点に到達するまでの時間である第１の時間情報と、前記第１〜第Ｍの経由点の各経由点への到達後、前記ロボットの移動終了時刻までの時間である第２の時間情報の少なくとも一方を求め、
前記第１の時間情報と前記加速期間の比較処理と、前記第２の時間情報と前記減速期間の比較処理の少なくとも一方に基づいて、前記第１〜第Ｍの経由点の各経由点が前記加速期間又は前記減速期間に含まれるかの判定を行って前記削除処理を行うことを特徴とするロボット制御システム。
請求項１乃至３のいずれかにおいて、
前記軌道計画部は、
前記第１〜第Ｎの関節の第ｉ（ｉは１≦ｉ≦Ｎを満たす整数）の関節を駆動する第ｉのアクチュエーターの性能に基づいて、前記第ｉの関節の前記最大関節角速度を設定する処理を、前記最大速度設定処理として行うことを特徴とするロボット制御システム。
請求項４において、
前記軌道計画部は、
前記第ｉのアクチュエーターの性能に基づいて決定される機械的な最大速度に基づいて、前記第ｉの関節の前記最大関節角速度を設定する処理を、前記最大速度設定処理として行うことを特徴とするロボット制御システム。
請求項４において、
前記軌道計画部は、
前記第ｉのアクチュエーターの性能に基づいて決定される機械的な最大加速度と、前記経路の始点と終点の間の前記第ｉの関節の前記関節角の変位である総移動距離とに基づいて、前記総移動距離を移動する間に到達できる到達可能速度を求め、
求めた到達可能速度に基づいて、前記第ｉの関節の前記最大関節角速度を設定する処理を、前記最大速度設定処理として行うことを特徴とするロボット制御システム。
請求項１乃至６のいずれかにおいて、
前記軌道計画部は、
設定された前記最大関節角速度に対する、前記第１の補間処理の結果から特定される前記処理区間での前記関節角速度の比を求め、求めた比を前記処理区間の前記時間長に乗ずることで、前記処理区間の前記時間長の前記更新処理を行うことを特徴とするロボット制御システム。
請求項７において、
前記軌道計画部は、
所与の前記処理区間において、前記第１〜第Ｎの関節に対応する第１〜第Ｎの補正係数を求め、
求めた前記第１〜第Ｎの補正係数のうち最大となる値を、前記処理区間の前記時間長に乗ずることで、前記処理区間の前記時間長の前記更新処理を行うことを特徴とするロボット制御システム。
請求項８において、
前記軌道計画部は、
第ｉ（ｉは１≦ｉ≦Ｎを満たす整数）の関節の前記最大関節角速度に対する、前記第１の補間処理の結果から特定される前記処理区間での前記第ｉの関節の前記関節角速度の比を、第ｉの補正係数として求めることを特徴とするロボット制御システム。
請求項１乃至９のいずれかにおいて、
前記軌道計画部は、
前記第１〜第Ｎの関節のいずれかの関節において、第ｊ（ｊは１≦ｊ≦Ｍを満たす整数）の経由点で移動の折り返し点が検出された場合に、
前記第１〜第Ｎの関節について、前記経由点情報を、前記第ｊの経由点の前方の第１の部分経由点情報と、前記第ｊの経由点の後方の第２の部分経由点情報とに分割し、
前記第１の部分経由点情報と、前記第２の部分経由点情報のそれぞれに対して、前記軌道計画処理を行うことを特徴とするロボット制御システム。
第１〜第Ｍ（Ｍは２以上の整数）の経由点を表す経由点情報に基づいて、第１〜第Ｎ（Ｎは２以上の整数）の関節を備えたロボットに対して、前記第１〜第Ｍの経由点に基づく経路を移動させるための軌道を生成する軌道計画処理を行う軌道計画部と、
前記軌道計画処理の結果に基づいて、前記ロボットを制御するロボット制御部と、
を含み、
前記軌道計画部は、
各処理区間が前記第１〜第Ｍの経由点のうちの２つの経由点の間に設定される、複数の処理区間を設定し、
設定した複数の前記処理区間の各処理区間において、前記第１〜第Ｎの関節のうち少なくとも１つの関節の関節角速度が、最大速度設定処理により設定された最大関節角速度となる前記軌道計画処理を行うことを特徴とするロボット制御システム。
請求項１乃至１１のいずれかに記載のロボット制御システムを含むことを特徴とするロボット。
第１〜第Ｎ（Ｎは２以上の整数）の関節を備えたロボットであって、
第１〜第Ｍ（Ｍは２以上の整数）の経由点を表す経由点情報を取得し、
２つの経由点の間に処理区間を設定するとともに、前記処理区間の時間長を設定し、
前記時間長が設定された前記処理区間と、前記第１〜第Ｍの経由点での関節角の値とに基づいて、各処理区間での前記第１〜第Ｎの関節の前記関節角の値を決定する第１の補間処理を行い、
前記第１の補間処理の結果から特定される前記処理区間での関節角速度と、最大速度設定処理により設定された最大関節角速度に基づいて、前記処理区間の前記時間長の更新処理を行い、
前記更新処理後の前記処理区間と、前記第１〜第Ｍの経由点での前記関節角の値とに基づいて、各処理区間での前記第１〜第Ｎの関節の前記関節角の値を決定する第２の補間処理を行い、
前記第２の補間処理の結果に基づいて、前記第１〜第Ｍの経由点に基づく経路を移動することを特徴とするロボット。
第１〜第Ｍ（Ｍは２以上の整数）の経由点を表す経由点情報に基づいて、第１〜第Ｎ（Ｎは２以上の整数）の関節を備えたロボットに対して、前記第１〜第Ｍの経由点に基づく経路を移動させるための軌道を生成する軌道計画処理を行うロボット制御方法であって、
２つの経由点の間に処理区間を設定するとともに、前記処理区間の時間長を設定し、
前記時間長が設定された前記処理区間と、前記第１〜第Ｍの経由点での関節角の値とに基づいて、各処理区間での前記第１〜第Ｎの関節の前記関節角の値を決定する第１の補間処理を行い、
前記第１の補間処理の結果から特定される前記処理区間での関節角速度と、最大速度設定処理により設定された最大関節角速度に基づいて、前記処理区間の前記時間長の更新処理を行い、
前記更新処理後の前記処理区間と、前記第１〜第Ｍの経由点での前記関節角の値とに基づいて、各処理区間での前記第１〜第Ｎの関節の前記関節角の値を決定する第２の補間処理を行うことを特徴とするロボット制御方法。
第１〜第Ｍ（Ｍは２以上の整数）の経由点を表す経由点情報に基づいて、第１〜第Ｎ（Ｎは２以上の整数）の関節を備えたロボットに対して、前記第１〜第Ｍの経由点に基づく経路を移動させるための軌道を生成する軌道計画処理を行う軌道計画部と、
前記軌道計画処理の結果に基づいて、前記ロボットを制御するロボット制御部として、
コンピューターを機能させ、
前記軌道計画部は、
２つの経由点の間に処理区間を設定するとともに、前記処理区間の時間長を設定し、
前記時間長が設定された前記処理区間と、前記第１〜第Ｍの経由点での関節角の値とに基づいて、各処理区間での前記第１〜第Ｎの関節の前記関節角の値を決定する第１の補間処理を行い、
前記第１の補間処理の結果から特定される前記処理区間での関節角速度と、最大速度設定処理により設定された最大関節角速度に基づいて、前記処理区間の前記時間長の更新処理を行い、
前記更新処理後の前記処理区間と、前記第１〜第Ｍの経由点での前記関節角の値とに基づいて、各処理区間での前記第１〜第Ｎの関節の前記関節角の値を決定する第２の補間処理を行う処理を、
前記軌道計画処理として行うことを特徴とするプログラム。