JP6217089B2

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

Info

Publication number: JP6217089B2
Application number: JP2013031791A
Authority: JP
Inventors: 信宏狩戸
Original assignee: Seiko Epson Corp
Current assignee: Seiko Epson Corp
Priority date: 2013-02-21
Filing date: 2013-02-21
Publication date: 2017-10-25
Anticipated expiration: 2033-02-21
Also published as: JP2014161918A

Description

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

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

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

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

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

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

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

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

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

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

また、本発明の一態様では、前記軌道計画部は、前記第１〜第Ｎの関節の第ｉ（ｉは１≦ｉ≦Ｎを満たす整数）の関節を駆動する第ｉのアクチュエーターの性能に基づいて決定される機械的な最大加速度と、前記経路の始点と終点の間の前記第ｉの関節の前記関節角の変位である総移動距離とを取得し、前記第ｉの関節について、前記第１〜第Ｐの処理区間のそれぞれでの前記到達可能関節角速度であるｖ_１ｉ〜ｖ_Ｐｉを求め、前記到達可能関節角速度ｖ_１ｉ〜ｖ_Ｐｉに基づいて、前記第１〜第Ｐの処理区間の前記時間長の前記更新処理を行ってもよい。

これにより、所与の関節について、処理区間毎に到達可能関節角速度を求めることができるため、よりなめらかな軌道を生成する軌道計画処理を行うこと等が可能になる。

また、本発明の一態様では、前記軌道計画部は、前記第ｉの関節の前記最大加速度と、前記到達可能関節角速度ｖ_１ｉ〜ｖ_Ｐｉに基づいて、前記第１〜第Ｍの経由点のそれぞれの経由点に到達するために必要な最短時間Ｔｓ_１〜Ｔｓ_Ｍを求め、求めた前記最短時間Ｔｓ_１〜Ｔｓ_Ｍに基づいて、前記第１〜第Ｐの処理区間の各処理区間の移動に要する区間最短時間Ｔｍｉｎ_１〜Ｔｍｉｎ_Ｐを求め、求めた前記区間最短時間Ｔｍｉｎ_１〜Ｔｍｉｎ_Ｐに基づいて、前記第１〜第Ｐの処理区間の前記時間長の前記更新処理を行ってもよい。

これにより、区間最短時間を用いて処理区間の時間長の更新処理を行うこと等が可能になる。

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

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

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

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

また、本発明の一態様では、前記軌道計画部は、前記第１〜第Ｎの関節の第ｉ（ｉは１≦ｉ≦Ｎを満たす整数）の関節において、第ｍ（ｍは１≦ｍ≦Ｍを満たす整数）の経由点で移動の折り返し点が検出された場合に、前記第ｉの関節について、前記折り返し点の情報に基づいて、前記第１の補間処理、前記時間長の前記更新処理、及び前記第２の補間処理を行ってもよい。

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

また、本発明の一態様では、前記軌道計画部は、前記第ｉの関節について、前記経由点情報を、前記第ｍの経由点の前方の第１の部分経由点情報と、前記第ｍの経由点の後方の第２の部分経由点情報とに分割し、前記第１の部分経由点情報と、前記第２の部分経由点情報のそれぞれに対して、前記第１の補間処理、前記時間長の前記更新処理、及び前記第２の補間処理を行ってもよい。

これにより、折り返しが発生した場合にも、経由点情報を複数の部分経由点情報に分割することで、適切な軌道計画処理を行うこと等が可能になる。

また、本発明の一態様では、前記軌道計画部は、前記第１〜第Ｎの関節のうち前記第ｉの関節以外の関節について、前記第ｉの関節の前記折り返し点に対応する対応点での各関節の前記関節角速度を、前記第２の補間処理の結果から決定し、決定した前記対応点での前記第ｉの関節以外の関節の前記関節角速度に基づいて、前記到達可能関節角速度の速度更新処理を行ってもよい。

これにより、所与の関節での折り返し点の情報を、他の関節での処理に反映させることで、より適切な軌道計画処理を行うこと等が可能になる。

また、本発明の一態様では、前記軌道計画部は、前記速度更新処理後の前記第１〜第Ｎの関節の前記到達可能関節角速度と、前記第２の補間処理の結果から特定される前記各処理区間での前記第１〜第Ｎの関節の前記関節角速度とに基づいて、前記各処理区間の前記時間長の第２の更新処理を行い、前記第２の更新処理後の前記処理区間と、前記第１〜第Ｍの経由点での前記関節角の値とに基づいて、各処理区間での前記第１〜第Ｎの関節の前記関節角の値を決定する第３の補間処理を行ってもよい。

本発明の他の態様は、第１〜第Ｍ（Ｍは２以上の整数）の経由点を表す経由点情報に基づいて、第１〜第Ｎ（Ｎは２以上の整数）の関節を備えたロボットに対して、前記第１〜第Ｍの経由点を含む経路を移動させるための軌道を生成する軌道計画処理を行う軌道計画部と、前記軌道計画処理の結果に基づいて、前記ロボットを制御するロボット制御部と、を含み、前記軌道計画部は、前記第１〜第Ｎの関節の第ｉ（ｉは１≦ｉ≦Ｎを満たす整数）の関節において、第ｍ（ｍは１≦ｍ≦Ｍを満たす整数）の経由点で移動の折り返し点が検出された場合に、前記第１〜第Ｎの関節のうち前記第ｉの関節以外の関節について、前記折り返し点に対応する点の直前では関節角速度を減速させ、前記折り返し点に対応する点の直後では前記関節角速度を加速させる前記軌道計画処理を行うロボット制御システムに関係する。

これにより、所与の関節の折り返しによる他の関節での対応点での関節角速度の変動を考慮して、適切な軌道計画処理を行うこと等が可能になる。

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

次に、各処理区間について、アクチュエーターの機械的な性能をフルに発揮した場合に当該処理区間で到達しうる最大の関節角速度を、到達可能関節角速度として求める（Ｓ２０３）。通常、アクチュエーターの機械的な性能をフルに発揮した場合、関節角速度の時間変化は、図８に示すように台形形状の軌道を描く。図８は縦軸が関節角速度ｖ、横軸が時間ｔである。つまり、スタート時とゴール時には速度が０になるという制約の下、ロボットを制御する場合には、図８のＡ１に示した加速区間、Ａ２に示した等速区間、Ａ３に示した減速区間の３つのフェーズを移行することになる。つまり各処理区間の到達可能関節角速度は、各処理区間内の点（狭義には端点である経由点を考えればよい）が、図８のグラフ上のどこに対応するかで決定される。

具体的な処理の例を示す。図８のグラフ（実線で示した台形軌道）は、軌道の起点（ロボットの現在位置）を基準として関節角加速度の限界により決定される第１の直線Ａ４と、軌道の終点（ロボットの目標位置）を基準として関節角減速度の限界により決定される第２の直線Ａ５と、アクチュエーター性能から機械的に決定される最大関節角速度を表す第３の直線Ａ６のうち、最も小さいものの集合と捉えることができる。

ここで第１の直線は、ロボットの現在位置からの移動距離（関節角の変位）をｄｉｓｔ、機械的に決定される最大関節角加速度をＡｍａｘとすれば、下式（１）のように表すことができる。

Ｖ＝ｓｑｒｔ（２＊ｄｉｓｔ＊Ａｍａｘ）・・・・・（１）
また、第２の直線についても、ロボットの目標位置を基準に過去方向について考えることで上式（１）と同様に考えることができる。例えば、第２の直線は所与の点からロボットの目標位置までの移動距離をｄｉｓｔ２とした場合に、当該所与の点での到達可能関節角速度（厳密にはその候補）を、ｄｉｓｔ２の関数として表現することも可能である。第３の直線についてはＶ＝Ｖｍａｘという、ｔ軸に平行な直線として定義できる。

よって、図４に示したように所与の経由点に関する情報が与えられた場合、現在位置と目標位置についても既知であることが想定されるため、上式（１）のｄｉｓｔや、第２の直線をｄｉｓｔ２の関数とした場合の当該ｄｉｓｔ２の値を決定することができる。その場合、当該経由点の到達可能速度とは、上式（１）に当該ｄｉｓｔを代入することで得られる第１の関節速度値と、第２の直線についてｄｉｓｔ２を代入することで得られる第２の関節速度値と、第３の直線により決定される第３の関節速度値（＝Ｖｍａｘ）のうち、最小の値を採用すればよい。

ここでは、Ｓ２０３の処理は関節毎、及び処理区間毎に行うことを想定している。つまり、第１〜第Ｎ（Ｎは２以上の整数）の関節、第１〜第Ｍ（Ｍは２以上の整数）の処理区間を対象とする場合には、Ｎ×Ｍ個の到達可能関節角速度を求めることになる。

なお、経路の総移動距離が短い場合等には、図９に示したように、加速区間Ｃ１で最大関節角速度に到達せず、等速区間を経ずに減速区間Ｃ２に移行するケースが出てくる。この場合、三角形の頂点に対応する速度ｖ_ｍｉは、機械的に決定される最大関節角速度Ｖｍａｘに比べて小さい値となる。しかしこの場合でも、到達可能速度の処理は上述のものを行えば十分である。この場合、全区間において第１の直線又は第２の直線による値が最小値となり、第３の直線による値が最小値となり得ない、というだけのことであり、上式（１）等による算出処理及び値の比較処理等を変更する必要はないためである。

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

なお、スプライン補間については広く知られた補間手法であるため、詳細な説明は省略する。また、本実施形態の補間処理はスプライン補間処理に限定されるものではなく、他の補間処理を適用してもよい。また、Ｓ２０３とＳ２０４の処理はどちらを先に行ってもよく、図６に示した順序で行われるものに限定されない。

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

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

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

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

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

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

１．４変形例
上述の手法では到達可能関節角速度を用いて、Ｓ２０５において処理区間の時間長の更新処理を行うものとした。本変形例では、到達可能関節角速度に加えて、区間最短時間を用いて前記更新処理を行う手法について説明する。

所与の処理区間（狭義には端点の経由点）において、上述の到達可能関節角速度がＶとして求められた場合、下式（２）により到達最短時間Ｔｓを求めることができる。

Ｔｓ＝Ｖ／Ａｍａｘ・・・・・（２）
これは、対象としている経由点では到達可能関節角速度がＶとなる以上、機械的に決定される最大関節角加速度で加速をしたとしても、関節角速度がＶとなるにはＴｓ秒を要することを意味する。すなわち、移動の開始点（ロボットの現在位置）から対象としている経由点に到達するためには、最短でもＴｓ秒を要するのであり、それより短い時間で到達する経路を計画していたとしたら、それはアクチュエーターの機械的な性能を超えるもので不適切となる。

このＴｓは全ての経由点で求めることができるため、隣接する経由点でのＴｓの差分をとることで、隣接経由点間の処理区間を移動するために最低でも要する時間である区間最短時間Ｔｍｉｎを、処理区間毎、関節毎に求めることができる。

本変形例ではこの区間最短時間Ｔｍｉｎを用い、各処理区間の時間長が、Ｔｍｉｎ（関節がＮ個あるのであればＮ個のＴｍｉｎのうちの最大値）よりも大きい値となるように、Ｓ２０５での時間長の更新処理を行う。

具体的な処理の流れを図１０のフローチャートに示す。Ｓ３０１〜Ｓ３０３は図６のＳ２０１〜Ｓ２０３と同様である。本変形例では、Ｓ３０３の到達可能関節角速度の算出処理に加えて、上式（２）等を用いて関節毎、処理区間毎に区間最短時間Ｔｍｉｎを求める（Ｓ３０４）。Ｓ３０５の第１のスプライン補間処理については、Ｓ２０４と同様である。

そして、第１のスプライン補間処理後に、Ｓ２０５と同様に到達可能関節角速度を用いて、処理区間の時間長の更新処理を行う（Ｓ３０６）。その処理結果である更新処理後の時間長と、当該処理区間の区間最短時間Ｔｍｉｎ（関節がＮ個あるのであればＮ個のＴｍｉｎのうちの最大値）を比較し、Ｓ３０６の出力である時間長がＴｍｉｎよりも小さい場合には、当該処理区間の時間長をＴｍｉｎとするクリッピング処理を行う（Ｓ３０７）。

到達可能関節角速度、及び区間最短時間の両方による更新処理後の情報を用いて、Ｓ２０６と同様に第２のスプライン補間処理を行う（Ｓ３０８）。

以上の処理により、先行例では実現できなかった、複数の細かい経由点を指定した場合でも、経路の途中の区間では最大速度での移動を実行しつつ、加速区間および減速区間では、なめらかな加減速を実現することができる。

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

ここで、到達可能関節角速度とは、各関節が対象としている処理区間において取り得る最大の関節角速度を表すものであり、例えば図８に示した手法により求めることができる。

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

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

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

また、軌道計画部１０３は、第１〜第Ｎの関節の第ｉ（ｉは１≦ｉ≦Ｎを満たす整数）の関節を駆動する第ｉのアクチュエーターの性能に基づいて決定される機械的な最大加速度と、経路の始点と終点の間の第ｉの関節の関節角の変位である総移動距離とを取得し、第ｉの関節について、第１〜第Ｐの処理区間のそれぞれでの到達可能関節角速度であるｖ_１ｉ〜ｖ_Ｐｉを求め、到達可能関節角速度ｖ_１ｉ〜ｖ_Ｐｉに基づいて、第１〜第Ｐの処理区間の時間長の更新処理を行ってもよい。

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

これにより、所与の関節に着目した場合に、複数の処理区間の各処理区間において到達可能関節角速度を求め、その情報を用いて時間長の更新処理を行うことが可能になる。図８の台形軌道や図９に示したように、１つの関節について考えた場合でも、処理区間（経由点）毎に機械的性能をフルに発揮した場合に取り得る関節角速度は異なる。本実施形態は関節毎、処理区間毎に到達可能関節角速度を求めるため、各処理区間について適切な時間長の更新処理を行うことが可能になる。

また、軌道計画部１０３は、第ｉの関節の最大加速度と、到達可能関節角速度ｖ_１ｉ〜ｖ_Ｐｉに基づいて、第１〜第Ｍの経由点のそれぞれの経由点に到達するために必要な最短時間Ｔｓ_１〜Ｔｓ_Ｍを求め、求めた最短時間Ｔｓ_１〜Ｔｓ_Ｍに基づいて、第１〜第Ｐの処理区間の各処理区間の移動に要する区間最短時間Ｔｍｉｎ_１〜Ｔｍｉｎ_Ｐを求め、求めた区間最短時間Ｔｍｉｎ_１〜Ｔｍｉｎ_Ｐに基づいて、第１〜第Ｐの処理区間の時間長の更新処理を行ってもよい。

これにより、本実施形態の変形例で上述したように区間最短時間まで考慮した時間長の更新処理を行うことが可能になる。区間最短時間についても、到達可能関節角速度と同様に、機械的性能をフルに発揮した前提で求めればよく、区間最短時間よりも短い処理区間とは、機械的に実現不可能な軌道となるため、このような軌道生成を抑止することで生成する軌道をなめらかなものにできる。

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

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

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

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

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

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

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

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

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

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

ある関節について、複数の折り返し点が検出された場合には、当該関節についての経由点情報を３つ以上の部分経由点情報に分割し、それぞれについて処理を行えばよい。そして、折り返しが発生していない関節については、部分経由点情報に分割することなく第１の実施形態と同様の処理を行う。例えば、図１１に示したように、関節１での折り返し点は、この時点の処理においては関節２では考慮せず、関節２では部分経由点情報の設定等は行わない。これにより、折り返し点が発生した場合にも、軌道計画処理を行うことが可能になる。

ただし、ここまでの制御ではある関節で発生した折り返し動作が、他の関節に及ぼす影響を考慮していない。所与の関節で折り返し動作が発生した場合、折り返し点直前の処理区間においては、当該所与の関節は速度を０にするための減速動作が必要となり、また折り返し点直後の処理区間においては、速度０から移動を開始するための加速動作が必要となる。それらの加減速動作は、一般的に大きい（例えば一定方向へ移動している場合の加速度、減速度に比べて大きい）ため、当該加減速動作を無理なく行うためには、ある程度長い時間を要する。つまり、第１の実施形態と同様の処理を行うことで、処理区間の時間長は長くなることが想定される。そのため、折り返し動作を行っていない他の関節に着目した場合、関節角の変位がそれほど大きくないのにもかかわらず、処理区間の時間長が長く更新された結果、周辺の処理区間に比べて速度が落ち込む可能性がある。

これを図示したものが図１２である。図１２の関節１では、Ｄ１及びＤ２の２カ所で折り返し（図１２の縦軸は関節角速度であるため、横軸の時間軸との交点）が発生している。そのため、それぞれの直前直後の処理区間の時間長が長く調整され、結果としてほぼ等速で移動していても差し支えない関節２の速度が、Ｄ１，Ｄ２の折り返し点に対応する点Ｄ３，Ｄ４において落ち込む結果となる。

図１２の関節２に実線で示したように、Ｄ３，Ｄ４のような速度の落ち込み点では、その落ち込み度合いが急激なものになる可能性があり、そのためその前後での関節角加速度が非常に大きくなってしまい、アクチュエーターの機械的な性能を超えてしまうおそれもある。

そこで本実施形態では、第２のスプライン補間処理で軌道計画処理を終了させるのではなく、所与の関節での折り返しを考慮して、他の関節の到達可能関節角速度を更新する速度更新処理を行い、速度更新処理後の値を用いて時間長の更新処理（第２の更新処理）及び、再度のスプライン補間処理（第３のスプライン補間処理）を行うものとする。

まず、速度更新処理について説明する。上述したように、他の関節の折り返し点に対応する点（Ｄ３，Ｄ４）は、その点では折り返しを行っていない関節にとっても特異な点となる。そこでここでは、そのような点を端点として、上述したように部分経由点情報を生成し、部分経由点情報毎に処理（具体的には、到達可能関節角速度を求め、時間長の第２の更新処理を行い、第３のスプライン補間処理を行う）を行うものとする。

ただし、上述の図１１の関節１の例では、部分経由点情報の端点では関節角速度が０となるものとしていたが、折り返しが発生していない関節については、関節角速度は落ち込むもののＶｉｎｉｔという０でない値をとる。

よって、部分経由点情報のうち、上記Ｄ３，Ｄ４のような点を端点として到達可能関節角速度（場合によっては区間最短時間Ｔｍｉｎを併用してもよい）を求める場合には、初期速度Ｖｉｎｉｔを考慮しなくてはならない。具体的には、折り返し点に対応する点を基準として到達可能関節角速度（正確にはその候補）を求める際には、下式（３）〜（６）を用いればよい。

Tc1=(sqrt(2*dist*Amax+Vinit*Vinit)-Vinit)/Amax ・・・・・（３）
Tc2=-(sqrt(2*dist*Amax+Vinit*Vinit)+Vinit)/Amax ・・・・・（４）
Tc=Max(Tc1,Tc2) ・・・・・（５）
V=Vinit+Tc*Amax ・・・・・（６）
部分経由点情報の両端が折り返し点に対応する点（Ｄ３，Ｄ４のように、速度は０でないが周辺に比べて落ち込む点であり、当該関節は折り返していないが他の関節が折り返している点）の場合には、図８の第１の直線Ａ４で決定する値及び第２の直線Ａ５で決定する値の両方を、上式（３）〜（６）により代用すればよい。

また、部分経由点情報の一端が折り返し点に対応する点であり、他端が当該関節の折り返し点又はロボットの現在位置若しくは目標位置である場合には、折り返し点に対応する点については上式（３）〜（６）で代用し、そうでない側については図８の第１の直線Ａ４又は第２の直線Ａ５をそのまま用いればよい。

また、両端が当該関節の折り返し点又はロボットの現在位置若しくは目標位置である場合には、第１の実施形態と同様に処理を行うことができる。

以上の処理により、他の関節の折り返し点を考慮して到達可能関節角速度を求めることができる。後は当該到達可能関節角速度（必要に応じて到達可能関節角速度から求められる区間最短時間Ｔｍｉｎ）を用いて、処理区間の時間長の第２の更新処理を行う。そして、第２の更新処理の結果に対して、第３のスプライン補間処理を行って、本実施形態の軌道計画処理の出力とすればよい。

本実施形態の処理の流れを図１３のフローチャートに示す。Ｓ４０１，Ｓ４０２については図６のＳ２０１，Ｓ２０２と同様である。次に、Ｓ４０１で取得した経由点情報を用いて、関節毎に折り返し点の検出処理を行う（Ｓ４０３）。そして、Ｓ４０３の検出処理の結果を用いて、関節毎、処理区間毎に到達可能関節角速度を算出する（Ｓ４０４）。具体的には上述したように、折り返し点がある関節については、当該折り返し点により経由点情報を複数の部分経由点情報に分割し、それぞれの部分経由点情報に対して第１の実施形態と同様の処理を行えばよい。また、折り返し点がない関節については、経由点情報を分割することなく、第１の実施形態と同様の処理を行えばよい。Ｓ４０４の処理では、所与の関節の処理において、他の関節の折り返し点を考慮する必要はない。

Ｓ４０５〜Ｓ４０７の処理については、図６のＳ２０４〜Ｓ２０６と同様である。次に、他の関節の折り返し点の情報を用いて、到達可能関節角速度の更新処理（速度更新処理）を行う（Ｓ４０８）。具体的には、第１〜第Ｎの関節のうち、第ｉの関節の速度更新処理を行う場合には、第１〜第ｉ−１、第ｉ＋１〜第Ｎの関節での折り返し点に対応する第ｉの関節の経由点（図１２のＤ３，Ｄ４のような速度が落ち込むことが想定される点）を基準として、その前後で到達可能関節角速度の再計算を行う。算出処理は上式（３）〜（６）等を用いればよい。

具体例を図１４に示す。図１４では関節１ではＥ１とＥ３で折り返しが発生し、関節２ではＥ２で折り返しが発生している。この場合、Ｓ４０４では関節１については自身の折り返し点であるＥ１とＥ３を基準点（端点）として到達可能関節角速度の算出処理を行い、関節２については、Ｅ２を基準点として到達可能関節角速度の算出処理を行う。それに対して、Ｓ４０８では関節１については、関節２の折り返し点であるＥ２に対応する点を基準点として到達可能関節角速度の速度更新処理を行い、関節２については関節１の折り返し点であるＥ１とＥ３に対応する点を基準点として速度更新処理を行うことになる。なお、図１４については、Ｓ４０６による時間長の更新処理後の情報を表すため、本来は各処理区間によって時間長が異なり、且つ他の関節の折り返し点に対応する点での速度の落ち込みがあるはずだが、グラフの簡略化のためそれらは省略してある。

速度更新処理が行われたら、当該速度更新処理後の到達可能関節角速度と、Ｓ４０７の結果から取得される処理区間での関節角速度を用いて、Ｓ４０６と同様に処理区間の時間長の更新処理（Ｓ４０６と区別するために第２の更新処理としている）を行う（Ｓ４０９）。そして、第２の更新処理の結果を用いて再度スプライン補間（第３のスプライン補間処理）を行い（Ｓ４１０）、処理を終了する。

なお、図１３では、Ｓ４０８〜Ｓ４１０を１回のみ実行する例を説明したが、これらの処理を複数回実行し、生成された軌道の時間に変化がなくなるまで繰り返すようにしてもよい。そうすることで、全体の移動時間が短い、より好ましい軌道を得ることができる。

以上の本実施形態では、ロボット制御システムの軌道計画部１０３は、第１〜第Ｎの関節の第ｉ（ｉは１≦ｉ≦Ｎを満たす整数）の関節において、第ｍ（ｍは１≦ｍ≦Ｍを満たす整数）の経由点で移動の折り返し点が検出された場合に、第ｉの関節について、折り返し点の情報に基づいて、第１の補間処理、時間長の前記更新処理、及び第２の補間処理を行う。

これにより、折り返しが発生した場合にも適切に軌道計画処理を行うことが可能になる。ある関節で折り返しが発生した場合、当該関節の折り返し点では関節角速度が０になるという新たな制約が加わることになる。つまり、折り返し点を考慮せずに第１の実施形態を実行したとしても、折り返し点での速度を０にすることができず、適切な軌道計画処理を行うことはできない。よってここでは、折り返し点を考慮することで、当該折り返し点での速度が０となる条件を満たす軌道計画処理を行う。

また、軌道計画部１０３は、第ｉの関節について、経由点情報を、第ｍの経由点の前方の第１の部分経由点情報と、第ｍの経由点の後方の第２の部分経由点情報とに分割し、第１の部分経由点情報と、第２の部分経由点情報のそれぞれに対して、第１の補間処理、時間長の更新処理、及び第２の補間処理を行ってもよい。

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

また、軌道計画部１０３は、第１〜第Ｎの関節のうち第ｉの関節以外の関節について、第ｉの関節の折り返し点に対応する対応点での各関節の関節角速度を、第２の補間処理の結果から決定し、決定した対応点での第ｉの関節以外の関節の関節角速度に基づいて、到達可能関節角速度の速度更新処理を行ってもよい。

これにより、所与の関節で折り返し点が検出された場合に、当該折り返し点の他の関節での対応点を考慮して、当該他の関節の到達可能関節角速度の速度更新処理を行うことが可能になる。上述したように、折り返し点近傍では時間長を長くする方向での更新処理が行われやすく、結果として図１２のＤ３，Ｄ４に示したように、他の関節では対応点において速度の落ち込み等が生じうる。この対応点は、速度は０とは限らないものの経由点情報の端点のように特異な点である以上、当該対応点での速度を基準として到達可能関節角速度を再計算するとよい。これは、到達可能関節角速度での特異な速度を軌道に反映するという新たな制約が加わったと考えることもでき、速度更新処理により当該制約を満足する軌道を生成することが可能になる。

また、軌道計画部１０３は、速度更新処理後の第１〜第Ｎの関節の到達可能関節角速度と、第２の補間処理の結果から特定される各処理区間での第１〜第Ｎの関節の関節角速度とに基づいて、各処理区間の時間長の第２の更新処理を行い、第２の更新処理後の処理区間と、第１〜第Ｍの経由点での関節角の値とに基づいて、各処理区間での第１〜第Ｎの関節の関節角の値を決定する第３の補間処理を行ってもよい。

これにより、速度更新処理後の到達可能関節角速度を用いて、時間長の更新処理（第２の更新処理）及び補間処理（第３の補間処理）を行うことが可能になる。到達可能関節角速度が更新されたということは、時間長の更新処理の基準が変更されたということであるため、図１３のＳ４０６に示した更新処理と同様の処理を再度行う必要がある。また、時間長が更新されれば、軌道生成のためには更新後の時間長を用いた補間処理が必要となるため、Ｓ４０７と同様の処理を再度行う必要がある。よってここでは、図１３のＳ４０９，Ｓ４１０に示したように、時間長を更新する第２の更新処理と、第３の補間処理を行うものとしている。なお、Ｓ４０５〜Ｓ４０６の処理と、ここで述べたＳ４０９〜Ｓ４１０の処理が同様であるなら、これらを１つにまとめる、すなわちＳ４０４の到達可能関節角速度の算出時点で他の関節の折り返し点まで考慮しておくことはできるのではないかとも考えられる。しかし、速度更新処理においては対応点での速度が基準となる以上、その値にある程度の信頼性が必要なところ、Ｓ４０４の段階では時間長は暫定的に設定されたものであり、対応点での関節角速度は機械的な限界を一切考慮していない。つまり、Ｓ４０４での対応点の関節角速度は、望ましい経路に従った場合の値との乖離が大きく、そのような値を用いたのでは到達可能関節角速度を精度よく設定することが難しい。よって、到達可能関節角速度はＳ４０６〜Ｓ４０７の処理前に一旦求めておき、Ｓ４０７の処理後に更新する（図１３でいえばＳ４０８の処理を行う）ことが望ましいと言える。

また以上の本実施形態では、ロボット制御システムは、第１〜第Ｍ（Ｍは２以上の整数）の経由点を表す経由点情報に基づいて、第１〜第Ｎ（Ｎは２以上の整数）の関節を備えたロボットに対して、第１〜第Ｍの経由点を含む経路を移動させるための軌道を生成する軌道計画処理を行う軌道計画部１０３と、軌道計画処理の結果に基づいて、ロボットを制御するロボット制御部１０５を含み、軌道計画部１０３は、第１〜第Ｎの関節の第ｉ（ｉは１≦ｉ≦Ｎを満たす整数）の関節において、第ｍ（ｍは１≦ｍ≦Ｍを満たす整数）の経由点で移動の折り返し点が検出された場合に、第１〜第Ｎの関節のうち第ｉの関節以外の関節について、折り返し点に対応する点の直前では関節角速度を減速させ、折り返し点に対応する点の直後では関節角速度を加速させる軌道計画処理を行う。

これにより、所与の関節で折り返し点が発生した場合に、当該折り返し点に対応する他の関節の対応点の前後において、適切な関節角速度を設定する軌道生成を行うことが可能になる。具体的には、対応点では図１２のＤ３，Ｄ４に示したように速度が落ち込むことが想定されるため、当該落ち込みを実現し（前記所与の関節での折り返し点での制約を満たすために必要）、且つ対応点での落ち込み方、落ち込みからの復帰の仕方をできるだけなめらかにすることで、全体としての経路をなめらかにするものである。

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

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

Claims

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