JP7067289B2 - 動作計画装置及び動作計画方法 - Google Patents

Description

本発明は、ロボットの動作計画装置及び動作計画方法に関する。

特許文献１は、ロボットの動作経路を計画する動作経路計画方法を開示している。具体的には、始点と終点との間でロボットが障害物と干渉しない経路となるように複数のノードを設定し、隣接するノード間に主要点をそれぞれ設定し、隣接した２つの該主要点の区間と所定関数との合成積を計算することで、該区間に対する平滑化関数の経路を算出している。

特開２０１８－６９４２８号公報

特許文献１では、ロボットを動作させるに際し、最大許容速度や最大許容加速度に関する条件をどのように満足させるか具体的に開示していない。

本発明の目的は、障害物と干渉することがなく、最大許容速度や最大許容加速度に関する条件を満足したロボットの動作を計画する技術を提供することにある。

本願発明の第１の観点によれば、ロボットの動作計画装置であって、予め設定された始点と終点との間で前記ロボットが障害物と干渉しない非干渉経路を生成する非干渉経路生成部と、前記非干渉経路を空間的に平滑化した空間平滑化経路を生成する空間平滑化経路生成部と、前記空間平滑化経路に含まれる複数の主要点を通過する時間を決定することで、時空間における経路としての時空間経路を生成する時間決定部と、を備え、前記複数の主要点は、前記空間平滑化経路の始点と、前記空間平滑化経路の終点と、前記空間平滑化経路の始点と前記空間平滑化経路の終点の間における中間点と、を含み、前記時間決定部は、前記複数の主要点間で動作速度が最大許容速度を越えないように、各主要点毎に、前記始点から各主要点に至るまでに経過する時間である仮時間を設定する仮時間設定部と、前記主要点を含む第１の平滑化範囲において前記空間平滑化経路を軟化関数を用いて空間的に平滑化し、前記第１の平滑化範囲において前記動作速度が最大許容加速度を越えていたら、前記第１の平滑化範囲において前記動作速度が前記最大許容加速度を越えないように前記主要点以降の前記複数の主要点の前記仮時間を引き延ばす仮時間調整部と、を有する、動作計画装置が提供される。以上の構成によれば、簡単な演算で、障害物と干渉することがなく、最大許容速度や最大許容加速度に関する条件を満足したロボットの動作計画が実現される。
好ましくは、前記仮時間調整部は、前記第１の平滑化範囲において前記空間平滑化経路を前記軟化関数を用いて空間的に平滑化した結果、前記空間平滑化経路が前記障害物と干渉していた場合、前記主要点を含み前記第１の平滑化範囲よりも狭い範囲である第２の平滑化範囲において前記空間平滑化経路を前記軟化関数を用いて空間的に平滑化する。以上の構成によれば、前記空間平滑化経路と前記障害物との干渉を解消することができる。
好ましくは、前記仮時間調整部は、前記第２の平滑化範囲において前記空間平滑化経路を前記軟化関数を用いて空間的に平滑化した結果、前記第２の平滑化範囲において前記動作速度が最大許容加速度を越えていたら、前記第２の平滑化範囲において前記動作速度が前記最大許容加速度を越えないように前記主要点以降の前記複数の主要点の前記仮時間を再び引き延ばす。以上の構成によれば、簡単な演算で、かつ、確実に、障害物と干渉することがなく、最大許容速度や最大許容加速度に関する条件を満足したロボットの動作計画が実現される。
好ましくは、前記中間点は、前記空間平滑化経路上の通過点のうち、前記空間平滑化経路の距離に対する傾きが前記通過点の前後で所定量以上変化した通過点である。
好ましくは、前記ロボットは複数の関節を有する多関節ロボットであり、前記複数の関節間で前記仮時間は同期している。
本願発明の第２の観点によれば、ロボットの動作計画方法であって、予め設定された始点と終点との間で前記ロボットが障害物と干渉しない非干渉経路を生成する非干渉経路生成ステップと、前記非干渉経路を空間的に平滑化した空間平滑化経路を生成する空間平滑化経路生成ステップと、前記空間平滑化経路に含まれる複数の主要点を通過する時間を決定することで、時空間における経路としての時空間経路を生成する時間決定ステップと、を備え、前記複数の主要点は、前記空間平滑化経路の始点と、前記空間平滑化経路の終点と、前記空間平滑化経路の始点と前記空間平滑化経路の終点の間における中間点と、を含み、前記時間決定ステップは、前記複数の主要点間で動作速度が最大許容速度を越えないように、各主要点毎に、前記始点から各主要点に至るまでに経過する時間である仮時間を設定する仮時間設定ステップと、前記主要点を含む第１の平滑化範囲において前記空間平滑化経路を軟化関数を用いて空間的に平滑化し、前記第１の平滑化範囲において前記動作速度が最大許容加速度を越えていたら、前記第１の平滑化範囲において前記動作速度が前記最大許容加速度を越えないように前記主要点以降の前記複数の主要点の前記仮時間を引き延ばす仮時間調整ステップと、を有する、動作計画方法が提供される。以上の方法によれば、簡単な演算で、障害物と干渉することがなく、最大許容速度や最大許容加速度に関する条件を満足したロボットの動作計画が実現される。

本発明によれば、簡単な演算で、障害物と干渉することがなく、最大許容速度や最大許容加速度に関する条件を満足したロボットの動作計画が実現される。

本発明の一実施形態に係るロボットの概略的構成を示す図である。 本発明の一実施形態に係る動作計画装置の概略的なシステム構成を示すブロック図である。 本発明の一実施形態に係る時間決定部の概略的なシステム構成を示すブロック図である。 ロボットの関節部の関節角度と関節ベクトルの距離とのグラフを示す図である。 時間調整部による処理を示す説明図である。 時間調整部による処理を示す説明図である。 時間調整部による処理を示す説明図である。 時間調整部による処理を示す説明図である。 時間調整部による処理を示す説明図である。 動作計画装置の制御フローである。 時間決定部の制御フローである。 計画軌道が途中で変更軌道に変更された場合の時空間経路の接続方法を例示する図である。 計画軌道が途中で変更軌道に変更された場合の時空間経路の接続方法を例示する図である。 計画軌道が途中で変更軌道に変更された場合の時空間経路の接続方法を例示する図である。

以下、図面を参照して本発明の実施形態について説明する。

本発明の一実施形態に係る動作計画装置は、マニピュレータを備えた移動体に対して、移動軌道やマニピュレータの軌道であって、空間的に滑らかであり、かつ、速度や加速度、ジャーク等の制約条件を満足することで時間的にも滑らかであり、もって動作効率のよい時空間における軌道を低い演算コストで生成するものである。このような時空間における軌道に従って動作するロボットは、ロボットの周囲にいる人に対して親和性を期待でき、また、振動が少なくなるので例えばコップに入った液体をこぼしにくい、という効果を期待できる。

図１は、本発明の一実施形態に係るロボット１００の概略的構成を示す図である。ロボット１００は、例えば、人型のロボットであり、頭部１０１と、胴体部１０２と、一対のアーム部１０３（マニピュレータ）と、移動機構１０４と、制御装置１０５と、を有している。

頭部１０１には、カメラ及びデプスセンサが搭載されている。従って、ロボット１００は、ロボット１００の周囲にある障害物を検出することができる。

胴体部１０２には、複数の関節部Ｃ１～Ｃ５が設けられている。アーム部１０３は、複数の関節部Ｒ１～Ｒ４、Ｌ１～Ｌ４と、ハンドＲ５、Ｌ５と、を有している。各関節部Ｒ１～Ｒ４、Ｌ１～Ｌ４には、各関節部Ｒ１～Ｒ４、Ｌ１～Ｌ４を回転あるいはスライドさせるサーボモータなどのアクチュエータが設けらている。各関節部Ｒ１～Ｒ４、Ｌ１～Ｌ４には、各関節部Ｒ１～Ｒ４、Ｌ１～Ｌ４の回転あるいはスライドを検出するセンサが設けられている。

移動機構１０４は、車輪あるいは脚部を有し、これらを駆動してロボット１００を移動させる。

制御装置１０５は、各関節部Ｒ１～Ｒ４、Ｌ１～Ｌ４のアクチュエータ及び移動機構１０４を制御する。制御装置１０５は、図２に示す動作計画装置１を含む。

図２に示す動作計画装置１は、ロボット１００の各関節部Ｒ１～Ｒ４、Ｌ１～Ｌ４の位置を示す各関節部Ｒ１～Ｒ４、Ｌ１～Ｌ４の関節ベクトルの時空間における動作経路を計画する。

図２は、本実施形態に係る動作計画装置１の概略的なシステム構成を示すブロック図である。本実施形態に係る動作計画装置１は、外部情報取得部２と、非干渉経路生成部３と、空間的平滑化経路生成部４と、時間決定部５と、出力部６と、を備えている。

動作計画装置１は、例えば、CPU、RAM、ROMを備えている。CPUがROMに記録された制御プログラムを読み込み、CPU上で実行することで、制御プログラムは、CPU等のハードウェアを、外部情報取得部２と、非干渉経路生成部３と、空間的平滑化経路生成部４と、時間決定部５と、出力部６と、して機能させる。これに代えて、外部情報取得部２と、非干渉経路生成部３と、空間的平滑化経路生成部４と、時間決定部５と、出力部６と、をハードウェアで実現してもよい。

外部情報取得部２は、頭部１０１のカメラ及びデプスセンサを用いて、環境内の障害物の位置や形状などの外部情報を取得する。外部情報取得部２により取得された外部情報は、環境地図情報としてメモリなどに記憶される。

非干渉経路生成部３は、例えば、予め設定されたスタート位置（始点）及びゴール位置（終点）と、メモリに記憶されている環境地図情報と、に基づいて、ロボット１００と環境地図情報の障害物とが干渉しない、ロボット本体や関節部Ｒ１～Ｒ４、Ｌ１～Ｌ４の空間的な非干渉動作経路を生成する。非干渉経路生成部３は、例えば、障害物が込み入った環境であっても動作経路の探索が可能な確率的な探索手法を用いて、動作経路を生成する。非干渉経路生成部３が生成した非干渉動作経路は、ジグザグの動作経路である。

なお、この確率的な探索手法の詳細は、例えば、非特許文献：LaValle Steven M, Kuffner Jr. James J.(2001). "Randomized kinodynamic planning". The International Journal of Robotics Research (IJRR)20(5).に開示されており、これを援用できるものとする。また、上述した動作経路の生成方法は一例であり、これに限定されない。

空間的平滑化経路生成部４は、非干渉経路生成部３により生成されたジグザグの非干渉動作経路を空間的に平滑化して空間平滑化経路する。ここで、平滑化とは、上述したように設定された非干渉動作経路を空間的に滑らかな曲線にすることを指す。平滑化に際して空間平滑化経路が障害物と干渉するか検証し、干渉する場合はその干渉が回避されるまで平滑化が再履行される。

空間的平滑化経路生成部４は、例えば、非干渉経路生成部３により生成されたジグザグの非干渉動作経路と、多項式の軟化関数（所定関数）と、の合成積を行うことにより、その非干渉動作経路を平滑化する。

上記の軟化関数は、例えば、テナリー多項式（Ternary Polynomial）関数である。テナリー多項式関数は、関数値の増加領域、一定領域、減少領域の三領域を有し、一定領域を挟んで増加領域と減少領域が対称に表される多項式関数である。なお、テナリー多項式の詳細については、本出願人が既に提出した特開２００９－０５３９２６号公報に開示されており、これを援用できるものする。

この軟化関数Ｔ（ｘ）は、例えば下記式（１）により表される。

上記式（１）において、次数０のＴ_０（ｘ）は、Ｔ_０（ｘ）＝Ｃ（－ａ_０／２＜ｘ＜ａ_０／２）、Ｔ_０（ｘ）＝０（ｘ≦－ａ_０／２、ｘ≧ａ_０／２）である。Ｃ、ａ_０は定数である。ｉは０又は正の整数である。

次に、非干渉動作経路と多項式の軟化関数との合成積の演算方法について詳細に説明する。空間的平滑化経路生成部４は、非干渉経路生成部３により生成された非干渉動作経路ｆ（ｙ）と、上述した多項式の軟化関数Ｔ_ｎ（ｘ）と、に基づいて、下記式（２）を用いて合成積ｆ_ｄ（ｘ）を行うことにより、空間平滑化経路を生成する。

軟化関数Ｔ_ｎ（ｘ）は、ｎ－１回微分可能な関数であり、パラメータａ_０、ａ_１、・・・・、ａ_ｎ、及びＣを有する。ｄ＝（ａ_０、ａ_１、・・・・、ａ_ｎ、Ｃ）と定義する。Ｃは、軟化関数の積分値が１となるように調整されている。ｆ_ｄ（ｘ）はｎ－１回微分可能な関数で、ｆ_ｄ→ｆに一様に収束する。

時間決定部５は、空間平滑化経路に含まれる複数の主要点を通過する時間を決定することで、時空間における経路としての時空間経路を生成する。

出力部６は、時間決定部５によって生成された時空間経路を出力する。例えば、出力部６は、ロボット１００の関節ベクトルの成分毎に、各関節の時間を同期させて、関節角度、関節角速度、関節ジャークと時間との関係を示すグラフデータを出力する。

次に、図３を参照して、時間決定部５について説明する。

図３に示すように、時間決定部５は、主要点抽出部７と、仮時間設定部８と、仮時間調整部９と、を有する。主要点抽出部７及び仮時間設定部８、仮時間調整部９は、前述したようにソフトウェアで実現してもよいし、ハードウェアで実現してもよい。

主要点抽出部７は、空間的平滑化経路生成部４から空間平滑化経路を取得し、空間平滑化経路を構成する複数の関節ベクトル列から主要関節ベクトル列（主要点）を抽出する。空間平滑化経路は、始点からの距離毎に定められた複数の関節ベクトル列によって構成されている。

ここで、複数の関節ベクトル列は、図４に示すように、始点における関節ベクトルである関節ベクトル１、終点における関節ベクトルである関節ベクトル８を含む。また、複数の関節ベクトル列は、関節ベクトル１と関節ベクトル８の間における複数の関節ベクトル２、３、４、・・・、７、８を含む。空間平滑化経路に含まれる関節ベクトルの個数は８に限定されない。複数の関節ベクトル列は、例えば以下のようなデータ構造を有する。ここで、Ｍは、関節の総数を意味している。

関節ベクトル１＝（関節１角度１、関節２角度１、・・・、関節Ｍ角度１）
関節ベクトル２＝（関節１角度２、関節２角度２、・・・、関節Ｍ角度２）
関節ベクトル３＝（関節１角度３、関節２角度３、・・・、関節Ｍ角度３）
・・・
関節ベクトル８＝（関節１角度８、関節２角度８、・・・、関節Ｍ角度８）
ここで、「関節ａ角度ｂ」は、距離軸上でｂ番目の関節ベクトルにおけるａ番目の関節の角度を意味している。

また、主要点抽出部７は、外部から、始点条件と終点条件を取得する。始点条件は、下記の関節ベクトル１と関節速度ベクトル１により構成されている。終点条件は、下記の関節ベクトル８と関節速度ベクトル８により構成されている。
関節ベクトル１＝（関節１角度１、関節２角度１、・・・、関節M角度１）
関節速度ベクトル１＝（関節１角速度１、関節２角速度１、・・・、関節M角速度１）
関節ベクトル８＝（関節１角度８、関節２角度８、・・・、関節M角度８）
関節速度ベクトル８＝（関節１角速度８、関節２角速度８、・・・、関節M角速度８）

次に、図４を参照して、主要関節ベクトル列について説明する。図４には、空間平滑化経路を構成する複数の関節ベクトル１、２、・・・、８を示している。横軸は、距離であり、縦軸は各関節における角度である。図４に示すように、各関節の角度は移動した距離に応じて増減している。図４には、１番目の関節の角度の遷移と、Ｍ番目の関節の角度の遷移を例示している。図４に示すように、空間平滑化経路においても個々の関節の角度の遷移が完全に平滑化されているわけではない。即ち、空間平滑化経路における個々の関節の角度は、ジグザグに遷移している。

以下、図４に示すように、関節ベクトルｃにおけるｄ番目の関節の角度を角度θ_ｄ（ｃ）と称する。

図４によると、１番目の関節の角度は、関節ベクトル４の前後において折り目状に変化している。換言すれば、角度θ_１（３）と角度θ_１（４）を結ぶ線分の傾きと、角度θ_１（４）と角度θ_１（５）を結ぶ線分の傾きは、大きく異なっている。このように、ある関節ベクトルの前後において関節の角度の距離に対する傾きが所定値以上に変化している場合、その関節ベクトルを主要関節ベクトルと称する。そして、そのような関節ベクトルを複数含むものを主要関節ベクトル列と定義する。更に、主要関節ベクトルに含まれる空間平滑化経路のプロットを主要点と称する。従って、角度θ_１（４）で示すプロットは主要点である。同様に、角度θ_１（５）で示すプロットは主要点である。角度θ_M（４）で示すプロットは、前後で傾きが所定値以上変化しているので主要点である。角度θ_M（５）で示すプロットは、距離軸上で対応する角度θ_１（５）が主要点であるので、同様に主要点となる。また、始点（関節ベクトル１）及び終点（関節ベクトル８）におけるプロットは主要点とみなす。従って、角度θ_１（１）及び角度θ_M（１）、角度θ_１（８）及び角度θ_M（８）は主要点である。

関節ベクトル１、関節ベクトル４、関節ベクトル５、関節ベクトル８は、主要関節ベクトルである。従って、以降、関節ベクトル１を主要関節ベクトル１、関節ベクトル４を主要関節ベクトル２、関節ベクトル５を主要関節ベクトル３、関節ベクトル８を主要関節ベクトル４と称する。また、主要関節ベクトルｅにおけるｆ番目の関節の角度を角度θ_MAINｆ（ｅ）と称する。以降の処理は、主要関節ベクトル１－４に含まれる主要点のみに対して行われる。

仮時間設定部８は、複数の主要点間で動作速度が最大許容速度を越えないように、各主要点毎に、始点から各主要点に至るまでに経過する時間である仮時間P(N)を設定する。具体的には、仮時間Ｐ（N）は、以下の式（３）で求められる。

ここで、Mは関節の総数であり、Nは主要関節ベクトルの番号、ｖｍａｘ_ｍは関節ｍにおける最大許容速度である。

従って、例えば、主要関節ベクトル２の仮時間P(2)は、以下の式（４）で求められる。

P(3)及びP(4)についても同様に求められる。こうして、各主要点N（各主要関節ベクトルN）毎に仮時間Ｐ（N）が求められる。なお、上記式（３）は、主要点間の角度差を最大許容速度で割っているので、主要点間を許容される範囲で最も速く移動させた場合にかかる時間である。また、各関節において、最も時間のかかる関節の仮時間を採用することとし、他の関節はその関節に同期させている。

仮時間調整部９は、空間平滑化経路に沿って移動した場合に最大許容加速度及び最大ジャークを満足するように、空間平滑化経路の時間軸を調整する。即ち、仮時間調整部９は、空間平滑化経路に沿って移動した場合に最大許容加速度及び最大ジャークを満足するように、各主要点の仮時間を調整する。仮時間調整部９は、併せて、障害物との干渉チェックを行い、干渉していた場合は各主要点の仮時間を再度、調整する。

図５には、仮時間設定部８が設定した各主要点における仮時間P(N)に基づいて作成した時空間経路を示すグラフであって、ある関節の角度と仮時間との関係を示している。横軸は仮時間であり、縦軸は角度である。また、当該関節における主要点を時間の経過する方向に沿って順に、主要点Ｑ１、主要点Ｑ２、主要点Ｑ３、主要点Ｑ４と称する。図５に示すグラフは、前述した仮時間P(N)の設定により、既に最大許容速度に関する条件を満足している。例えば、主要点Ｑ１と主要点Ｑ２との間の線分の傾きの絶対値は最大許容速度以下となっている。

次に、図６に示すように、仮時間調整部９は、主要点Ｑ２における最大加速度条件を満足すべく、主要点Ｑ２－Ｑ４の仮時間を引き延ばす。具体的には、まず、平滑化範囲Ｒ２において時空間経路を軟化関数を用いて空間的に平滑化する。

ここで、平滑化範囲Ｒ２は、仮時間軸上での範囲であって主要点Ｑ２を含む範囲である。具体的には、まず、主要点Ｑ１と主要点Ｑ２の仮時間軸上での差分Ｌ１２と、主要点Ｑ２と主要点Ｑ３の仮時間軸上での差分Ｌ２３を比較する。そして、差分Ｌ１２と差分Ｌ２３のうち小さい方である差分を決定する。図６の例では、差分Ｌ２３の方が差分Ｌ１２より小さいので、平滑化範囲Ｒ２を決定する基準を差分Ｌ２３とする。そして、平滑化範囲Ｒ２は、仮時間軸上で、主要点Ｑ２を基準とし主要点Ｑ３側へＬ２３／２ずれた時点から主要点Ｑ１側へＬ２３／２ずれた時点までの範囲とする。

また、軟化関数は、前述したテナリー多項式関数を用いることができる。

主要点Ｑ２において空間的に平滑化された曲線経路を以下、曲線経路Ｂ２と称する。

次に、仮時間設定部８は、曲線経路Ｂ２に沿って移動した際に最大許容加速度を越えているか判定し、判定結果がYESの場合は、曲線経路Ｂ２に沿って移動した際に最大許容加速度を越えないように、主要点Ｑ２以降のすべての主要点の仮時間を十分に引き延ばす。具体的には、図６の下図に示すように、主要点Ｑ２－Ｑ４の仮時間を延ばす。これにより、曲線経路Ｂ２は仮時間軸に沿って引き伸ばされ、もって、曲線経路Ｂ２に沿って移動した際に動作速度が最大許容加速度内に収まる。

なお、曲線経路Ｂ２における加速度は曲線経路Ｂ２を時間で微分すれば容易に求めることができる。また、曲線経路Ｂ２におけるジャークは曲線経路Ｂ２を時間で二階微分すれば容易に求めることができる。従って、曲線経路Ｂ２に沿って移動した際にジャークを越えているか判定し、判定結果がYESの場合は、曲線経路Ｂ２に沿って移動した際にジャークを越えないように、主要点Ｑ２以降のすべての主要点の仮時間を十分に引き延ばしてもよい。

図７には、主要点Ｑ２において最大許容加速度に関する条件を満足した時空間経路を示している。主要点Ｑ２においては、軟化関数を用いて空間的に平滑化したので、曲線経路Ｂ２に沿って移動すると障害物と干渉する可能性が新たに生じる。従って、仮時間調整部９は、曲線経路Ｂ２と障害物との干渉の有無を判定し、判定結果がYESの場合は、曲線経路Ｂ２を破棄し、曲線経路Ｂ２を生成した際に使用した平滑化範囲Ｒ２の範囲を狭めて再度、主要点Ｑ２における平滑化を実行する。平滑化範囲Ｒ２の範囲の狭め方としては、例えば、新たな平滑化範囲Ｒ２を、仮時間軸上で、主要点Ｑ２を基準とし主要点Ｑ３側へＬ２３／４ずれた時点から主要点Ｑ１側へＬ２３／４ずれた時点までの範囲とすることが挙げられるがこれに限定されない。このように平滑化範囲Ｒ２を狭めることで、新たな曲線経路Ｂ２は主要点Ｑ２のより近くを通過することになり、もって、上記の干渉を解消することができる。

ただし、平滑化範囲Ｒ２を狭めて生成した曲線経路Ｂ２は鋭利な曲線となっているので最大許容加速度を越えてしまうことが考えられる。従って、仮時間調整部９は、新たな平滑化範囲Ｒ２において時空間経路を軟化関数を用いて空間的に平滑化した結果、新たな平滑化範囲Ｒ２において動作速度が最大許容加速度を越えていたら、新たな平滑化範囲Ｒ２において動作速度が最大許容加速度を越えないように主要点Ｑ２以降の主要点の仮時間を再び引き延ばす。一度干渉が解消されたことが確認されているので、この引き伸ばしによって新たに干渉の有無を判定する必要性は実質的にないと言える。

次に、仮時間調整部９は、図８に示すように、主要点Ｑ３における最大加速度条件を満足すべく、主要点Ｑ３－Ｑ４の仮時間を引き延ばす。具体的には、まず、平滑化範囲Ｒ３において時空間経路を軟化関数を用いて空間的に平滑化する。

ここで、平滑化範囲Ｒ３は、仮時間軸上での範囲であって主要点Ｑ３を含む範囲である。平滑化範囲Ｒ３の決定方法は、平滑化範囲Ｒ２におけるものを援用することができる。ただし、主要点Ｑ２における最大加速度条件を満足させる際に差分Ｌ２３は増加する方向に変化しているが、差分Ｌ２３と差分Ｌ３４を比較するに際し、増加後の差分Ｌ２３を用いてもよいし、増加前の差分Ｌ２３を用いてもよい。計算の簡略化にとっては後者である増加前の差分Ｌ２３を用いることが好ましい。

主要点Ｑ３において空間的に平滑化された曲線経路を以下、曲線経路Ｂ３と称する。

次に、仮時間設定部８は、曲線経路Ｂ３に沿って移動した際に最大許容加速度を越えているか判定し、判定結果がYESの場合は、曲線経路Ｂ３に沿って移動した際に最大許容加速度を越えないように、主要点Ｑ３以降のすべての主要点の仮時間を十分に引き延ばす。具体的には、図８の下図に示すように、主要点Ｑ３及びＱ４の仮時間を延ばす。これにより、曲線経路Ｂ３は仮時間軸に沿って引き伸ばされ、もって、曲線経路Ｂ３に沿って移動した際に動作速度が最大許容加速度内に収まる。

また、上記同様に、曲線経路Ｂ３に沿って移動した際にジャークを越えているか判定し、判定結果がYESの場合は、曲線経路Ｂ３に沿って移動した際にジャークを越えないように、主要点Ｑ３以降のすべての主要点の仮時間を十分に引き延ばしてもよい。

図９には、主要点Ｑ３において最大許容加速度に関する条件を満足した時空間経路を示している。上記同様に、仮時間調整部９は、曲線経路Ｂ３と障害物との干渉の有無を判定し、判定結果がYESの場合は、曲線経路Ｂ３を破棄し、曲線経路Ｂ３を生成した際に使用した平滑化範囲Ｒ３の範囲を狭めて再度、主要点Ｑ３における平滑化を実行する。また、仮時間調整部９は、新たな平滑化範囲Ｒ３において時空間経路を軟化関数を用いて空間的に平滑化した結果、新たな平滑化範囲Ｒ３において動作速度が最大許容加速度を越えていたら、新たな平滑化範囲Ｒ３において動作速度が最大許容加速度を越えないように主要点Ｑ３以降の主要点の仮時間を再び引き延ばす。一度干渉が解消されたことが確認されているので、この引き伸ばしによって新たに干渉の有無を判定する必要性は実質的にないと言える。

このように、仮時間設定部８が仮時間を設定することで、空間平滑化経路について最大許容速度に関する条件が満足され、仮時間調整部９が仮時間を調整することで、時空間経路について最大許容加速度及び干渉に関する条件が満足される。こうして、時間決定部５は、空間平滑化経路の時間軸上でのスピードプロファイルを決定することで、障害物と干渉することがなく、最大許容速度や最大許容加速度に関する条件を満足したロボットの時空間経路を生成する。

次に、図１０及び図１１を参照して、動作計画装置１の制御フローを説明する。

まず、外部情報取得部２は、外部情報を取得する（S100）。

次に、非干渉経路生成部３は、予め設定された始点と終点との間でロボット１００が障害物と干渉しない非干渉経路を生成する（S110）。

次に、空間的平滑化経路生成部４は、非干渉経路を空間的に平滑化した空間平滑化経路を生成する（S120）。

次に、時間決定部５は、空間平滑化経路に含まれる複数の主要点を通過する時間を決定することで、時空間における経路としての時空間経路を生成する（S130）。

次に、出力部６は、時間決定部５が生成した時空間経路を出力する（S140）。

図１１に示すように、時間決定ステップ（S130）においては、更に、以下のフローを実行する。

まず、主要点抽出部７が、空間平滑化経路から主要点を抽出する（S150）。

次に、仮時間設定部８が、複数の主要点間で動作速度が最大許容速度を越えないように、各主要点毎に、始点から各主要点に至るまでに経過する時間である仮時間を設定する（S160）。

次に、仮時間調整部９が、主要点を含む平滑化範囲Ｒ２（第１の平滑化範囲）において空間平滑化経路を軟化関数を用いて空間的に平滑化し、平滑化範囲Ｒ２において動作速度が最大許容加速度を越えていたら、平滑化範囲Ｒ２において動作速度が最大許容加速度を越えないように主要点以降の複数の主要点の仮時間を引き延ばす（S170）。

以上に、本願発明の好適な実施形態を説明したが、上記実施形態は、以下の特徴を有する。

即ち、ロボット１００の動作計画装置１は、予め設定された始点と終点との間でロボットが障害物と干渉しない非干渉経路を生成する非干渉経路生成部３と、非干渉経路を空間的に平滑化した空間平滑化経路を生成する空間的平滑化経路生成部４と、空間平滑化経路に含まれる複数の主要点を通過する時間を決定することで、時空間における経路としての時空間経路を生成する時間決定部５と、を備える。複数の主要点は、空間平滑化経路の始点と、空間平滑化経路の終点と、空間平滑化経路の始点と空間平滑化経路の終点の間における中間点と、を含む。時間決定部５は、複数の主要点間で動作速度が最大許容速度を越えないように、各主要点毎に、始点から各主要点に至るまでに経過する時間である仮時間を設定する仮時間設定部８と、主要点を含む第１の平滑化範囲（例えば、平滑化範囲Ｒ２）において空間平滑化経路を軟化関数を用いて空間的に平滑化し、第１の平滑化範囲において動作速度が最大許容加速度を越えていたら、第１の平滑化範囲において動作速度が最大許容加速度を越えないように主要点以降の複数の主要点の仮時間を引き延ばす仮時間調整部９と、を有する。以上の構成によれば、簡単な演算で、障害物と干渉することがなく、最大許容速度や最大許容加速度に関する条件を満足したロボットの動作計画が実現される。

仮時間調整部９は、平滑化範囲Ｒ２において空間平滑化経路を軟化関数を用いて空間的に平滑化した結果、空間平滑化経路が障害物と干渉していた場合、主要点を含み平滑化範囲Ｒ２よりも狭い範囲である新たな平滑化範囲Ｒ２（第２の平滑化範囲）において空間平滑化経路を軟化関数を用いて空間的に平滑化する。以上の構成によれば、空間平滑化経路と障害物との干渉を解消することができる。

また、仮時間調整部９は、新たな平滑化範囲Ｒ２において空間平滑化経路を軟化関数を用いて空間的に平滑化した結果、新たな平滑化範囲Ｒ２において動作速度が最大許容加速度を越えていたら、新たな平滑化範囲Ｒ２において動作速度が最大許容加速度を越えないように主要点以降の複数の主要点の仮時間を再び引き延ばす。以上の構成によれば、簡単な演算で、かつ、確実に、障害物と干渉することがなく、最大許容速度や最大許容加速度に関する条件を満足したロボットの動作計画が実現される。

なお、主要点となり得る中間点は、図４に示すように、空間平滑化経路上の通過点のうち、空間平滑化経路の距離に対する傾きが通過点の前後で所定量以上変化した通過点とする。

また、ロボット１００は複数の関節を有する多関節ロボットである。複数の関節間で仮時間は同期している。

上記実施形態において、ロボット１００は、例えば、１つのアーム部１０３を有する片腕ロボットとして構成されてもよく、アーム部１０３のみを有するアーム型ロボットであってもよい。さらに、ロボット１００は、自律型の車両などの移動体であってもよい。

次に、図１２に示すように、ロボットが動作中に動作軌道を計画軌道から変更軌道へと変更する場合であって、始点や終点での速度がゼロでない場合を考える。以下、変更軌道の始点における速度がゼロでない場合における、変更軌道の始点を含めた空間平滑化、及び、時間平滑化の方法を説明する。

まず、図１２に示す計画軌道の終点と変更軌道の始点を接続するには、図１３において二点鎖線で示すように、計画軌道の終点と変更軌道の始点を最大許容速度を越えない範囲で線形補間する。

次に、図１３に示すように、線形補完した線分と計画軌道との接続箇所において最大加速度を越えないように軌道を平滑化する。同様に、線形補間した線分と変更軌道との接続箇所において最大加速度を越えないように軌道を平滑化する。以上の方法により、２つの曲線軌道が生成され、もって、計画軌道と変更軌道を最大許容速度及び最大許容加速度に関する条件を満足しつつ空間的に滑らかに接続することが可能となる。

１ 動作計画装置
２ 外部情報取得部
３ 非干渉経路生成部
４ 空間的平滑化経路生成部
５ 時間決定部
６ 出力部
７ 主要点抽出部
８ 仮時間設定部
９ 仮時間調整部

Claims (6)

1. ロボットの動作計画装置であって、
   予め設定された始点と終点との間で前記ロボットが障害物と干渉しない非干渉経路を生成する非干渉経路生成部と、
   前記非干渉経路を空間的に平滑化した空間平滑化経路を生成する空間平滑化経路生成部と、
   前記空間平滑化経路に含まれる複数の主要点を通過する時間を決定することで、時空間における経路としての時空間経路を生成する時間決定部と、
   を備え、
   前記複数の主要点は、前記空間平滑化経路の始点と、前記空間平滑化経路の終点と、前記空間平滑化経路の始点と前記空間平滑化経路の終点の間における中間点と、を含み、
   前記時間決定部は、
   前記複数の主要点間で動作速度が最大許容速度を越えないように、各主要点毎に、前記始点から各主要点に至るまでに経過する時間である仮時間を設定する仮時間設定部と、
   前記複数の主要点のうちの主要点を含む第１の平滑化範囲において前記空間平滑化経路を軟化関数を用いて空間的に平滑化し、前記第１の平滑化範囲において動作加速度が最大許容加速度を越えていたら、前記第１の平滑化範囲において前記動作加速度が前記最大許容加速度を越えないように前記主要点以降の複数の主要点のみの前記仮時間を引き延ばす仮時間調整部と、
   を有する、
   動作計画装置。
2. 請求項１に記載の動作計画装置であって、
   前記仮時間調整部は、前記第１の平滑化範囲において前記空間平滑化経路を前記軟化関数を用いて空間的に平滑化した結果、前記空間平滑化経路が前記障害物と干渉していた場合、前記主要点を含み前記第１の平滑化範囲よりも狭い範囲である第２の平滑化範囲において前記空間平滑化経路を前記軟化関数を用いて空間的に平滑化する、
   動作計画装置。
3. 請求項２に記載の動作計画装置であって、
   前記仮時間調整部は、前記第２の平滑化範囲において前記空間平滑化経路を前記軟化関数を用いて空間的に平滑化した結果、前記第２の平滑化範囲において前記動作加速度が前記最大許容加速度を越えていたら、前記第２の平滑化範囲において前記動作加速度が前記最大許容加速度を越えないように前記主要点以降の前記複数の主要点の前記仮時間を再び引き延ばす、
   動作計画装置。
4. 請求項１から３までの何れか１項に記載の動作計画装置であって、
   前記中間点は、前記空間平滑化経路上の通過点のうち、前記空間平滑化経路の距離に対する傾きが前記通過点の前後で所定量以上変化した通過点である、
   動作計画装置。
5. 請求項１から４までの何れか１項に記載の動作計画装置であって、
   前記ロボットは複数の関節を有する多関節ロボットであり、
   前記複数の関節間で前記仮時間は同期している、
   動作計画装置。
6. ロボットの動作計画方法であって、
   予め設定された始点と終点との間で前記ロボットが障害物と干渉しない非干渉経路を生成する非干渉経路生成ステップと、
   前記非干渉経路を空間的に平滑化した空間平滑化経路を生成する空間平滑化経路生成ステップと、
   前記空間平滑化経路に含まれる複数の主要点を通過する時間を決定することで、時空間における経路としての時空間経路を生成する時間決定ステップと、
   を備え、
   前記複数の主要点は、前記空間平滑化経路の始点と、前記空間平滑化経路の終点と、前記空間平滑化経路の始点と前記空間平滑化経路の終点の間における中間点と、を含み、
   前記時間決定ステップは、
   前記複数の主要点間で動作速度が最大許容速度を越えないように、各主要点毎に、前記始点から各主要点に至るまでに経過する時間である仮時間を設定する仮時間設定ステップと、
   前記複数の主要点のうちの主要点を含む第１の平滑化範囲において前記空間平滑化経路を軟化関数を用いて空間的に平滑化し、前記第１の平滑化範囲において動作加速度が最大許容加速度を越えていたら、前記第１の平滑化範囲において前記動作加速度が前記最大許容加速度を越えないように前記主要点以降の複数の主要点のみの前記仮時間を引き延ばす仮時間調整ステップと、
   を有する、
   動作計画方法。

Images