(57)【要約】【課題】計算負荷を軽くしつつ、障害物を干渉することなく滑らかに動作できる経路を計画すること。【解決手段】動作経路計画方法は、始端点と終端点との間でロボットが障害物と干渉しない経路となるように複数のノードを設定するステップと、隣接するノード間に中間点をそれぞれ設定し、隣接した２つの該中間点の区間と所定関数との合成積を計算することで、該区間に対する平滑化関数の経路を算出するステップと、該算出した平滑化関数の経路が障害物と干渉しない場合、該平滑化関数の経路を新たな経路として生成し、該算出した平滑化関数の経路が障害物と干渉する場合、中間点とノードとの間を所定比率で分割する分割点を再設定し、該再設定した分割点と中間点との間の平滑化関数の経路を算出し、該平滑化関数の経路が障害物と干渉するか否かを判定して、新たな経路を生成するステップと、を含む。【選択図】図９
2018069428.tif 000002
【特許請求の範囲】
【請求項１】
ロボットの動作経路を計画する動作経路計画方法であって、
前記ロボットの動作環境内の障害物の位置及び形状を含む環境地図情報に基づいて、予め設定された始端点と終端点との間で前記ロボットが前記障害物と干渉しない経路となるように複数のノードを設定するステップと、
隣接するノード間に中間点をそれぞれ設定し、隣接した２つの該中間点の区間と所定関数との合成積を計算することで、該区間に対する平滑化関数の経路を算出するステップと、
該算出した平滑化関数の経路が前記障害物と干渉しない場合、該平滑化関数の経路を新たな経路として生成し、該算出した平滑化関数の経路が前記障害物と干渉する場合、前記中間点と前記ノードとの間を所定比率で分割する分割点を再設定し、該再設定した分割点と前記中間点との間の前記平滑化関数の経路を算出し、該平滑化関数の経路が前記障害物と干渉するか否かを判定して、前記新たな経路を生成するステップと、
を含む、
ことを特徴する動作経路計画方法。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、ロボットの動作経路を計画する動作経路計画方法に関する。
【背景技術】
【０００２】
ロボットの動作経路のスタート位置とゴール位置との間には、障害部を回避するように複数の中継点が設定され、隣接する中継点間に中間点が設定され、隣接する中間点を結ぶ経路を平滑化する動作経路計画方法が知られている（特許文献１参照）。例えば、１つ目の中継点と２つ目の中継点との間に位置する中間点と、２つ目の中継点と３つ目の中継点との間に位置する中間点と、を結んだ線を所定関数を用いて曲線にすることで平滑化を行っている。
【先行技術文献】
【特許文献】
【０００３】
【特許文献１】特開２００５−３０９９９０号公報
【発明の概要】
【発明が解決しようとする課題】
【０００４】
上記動作経路計画方法において、例えば、中継点間で中間点をずらしつつ、その中間点を結び平滑化した経路が障害物に干渉しないように経路を計画する。このため、滑らかで障害物に干渉しない最適な中間点を見つけ出すのに、その計算負荷が高くなる虞がある。
【０００５】
本発明は、かかる課題を解決するためになされたものであり、計算負荷を軽くしつつ、障害物を干渉することなく滑らかに動作できる経路を計画する動作経路計画方法を提供することを主たる目的とする。
【課題を解決するための手段】
【０００６】
上記目的を達成するための本発明の一態様は、
ロボットの動作経路を計画する動作経路計画方法であって、
前記ロボットの動作環境内の障害物の位置及び形状を含む環境地図情報に基づいて、予め設定された始端点と終端点との間に前記ロボットが前記障害物と干渉しない経路となるように複数のノードを設定するステップと、
隣接するノード間に中間点をそれぞれ設定し、隣接した２つの該中間点の区間と所定関数との合成積を計算することで、該区間に対する平滑化関数の経路を算出するステップと、
該算出した平滑化関数の経路が前記障害物と干渉しない場合、該平滑化関数の経路を新たな経路として生成し、該算出した平滑化関数の経路が前記障害物と干渉する場合、前記中間点と前記ノードとの間を所定比率で分割する分割点を再設定し、該再設定した分割点と前記中間点との間の前記平滑化関数の経路を算出し、該平滑化関数の経路が障害物と干渉するか否かを判定して、前記新たな経路を生成するステップと、
を含む、
ことを特徴する動作経路計画方法
である。
【発明の効果】
【０００７】
本発明によれば、計算負荷を軽くしつつ、障害物を干渉することなく滑らかに動作できる経路を計画する動作経路計画方法を提供することができる。
【図面の簡単な説明】
【０００８】
【図１】本発明の一実施形態に係るロボットの概略的構成を示す図である。
【図２】本発明の一実施形態に係る動作経路計画装置の概略的なシステム構成を示すブロック図である。
【図３】ロボットの関節部の関節角度と関節ベクトルの距離とのグラフを示す図である。
【図４】平坦化の一例を説明するための図である。
【図５】平滑化を説明するための図である。
【図６】軟化関数の区間を示す図である。
【図７】軟化関数Ｔ（ｘ）の一例をグラフ表示した図である。
【図８】図８に示す軟化関数の生成方法を説明するための図である。
【図９】経路平滑化部による平滑化処理のフローを示すフローチャートである。
【図１０】１つのアーム部を有するロボットの概略的構成を示す図である。
【発明を実施するための形態】
【０００９】
以下、図面を参照して本発明の実施形態について説明する。
本発明の一実施形態に係る動作経路計画装置は、ロボットの動作経路を計画する。
図１は、本発明の一実施形態に係るロボットの概略的構成を示す図である。ロボット１００は、例えば、人型のロボットであり、頭部１０１と、胴体部１０２と、一対のアーム部１０３と、移動機構１０４と、制御装置１０５と、を有している。
【００１０】
頭部１０１にはカメラなどの距離センサが設けられている。胴体部１０２には、複数の関節部Ｃ１〜Ｃ５が設けられている。アーム部１０３は、複数の関節部Ｒ１〜Ｒ４、Ｌ１〜Ｌ４と、ハンドＲ５、Ｌ５と、を有している。各関節部Ｒ１〜Ｒ４、Ｌ１〜Ｌ４には、各関節部Ｒ１〜Ｒ４、Ｌ１〜Ｌ４を回転あるいはスライドさせるサーボモータなどのアクチュエータが設けらている。各関節部Ｒ１〜Ｒ４、Ｌ１〜Ｌ４には、各関節部Ｒ１〜Ｒ４、Ｌ１〜Ｌ４の回転あるいはスライドを検出するセンサが設けられている。
【００１１】
移動機構１０４は、車輪あるいは脚部を有し、これらを駆動してロボット１００を移動させる。制御装置１０５は、各関節部Ｒ１〜Ｒ４、Ｌ１〜Ｌ４のアクチュエータ及び移動機構１０４を制御して、ロボット１００を移動させる。
【００１２】
制御装置１０５は、例えば、環境地図の位置を示す環境地図ベクトル（Ｘ、Ｙ）とロボット１００の位置（ロボット１００の重心位置や中心位置など）を示すロボット地図ベクトル（ｘ、ｙ）とを対応付けて、ロボット１００の移動を制御する。なお、上記ロボット１００の構成は一例であり、これに限定されない。
【００１３】
本実施形態に係る動作経路計画装置１は、ロボット１００の各関節部Ｒ１〜Ｒ４、Ｌ１〜Ｌ４の位置を示す各関節部Ｒ１〜Ｒ４、Ｌ１〜Ｌ４の関節ベクトル、及び、ロボット１００の位置（ｘ、ｙ）を示すロボット地図ベクトル、のうちの少なくとも一方の動作経路（以下、動作経路）を計画する。
【００１４】
図２は、本実施形態に係る動作経路計画装置の概略的なシステム構成を示すブロック図である。本実施形態に係る動作経路計画装置１は、外部情報取得部２と、経路生成部３と、経路平坦化部４と、経路平滑化部５と、経路出力部６と、を備えている。
【００１５】
動作経路計画装置１は、例えば、制御処理、演算処理等と行うＣＰＵ（Central Processing Unit）、ＣＰＵによって実行される演算プログラム、制御プログラム等が記憶されたＲＯＭ（Read Only Memory）やＲＡＭ（Random Access Memory）からなるメモリ、外部と信号の入出力を行うインターフェイス部（Ｉ／Ｆ）、などからなるマイクロコンピュータを中心にして、それぞれ、ハードウェア構成されている。ＣＰＵ、メモリ、及びインターフェイス部は、データバスなどを介して相互に接続されている。
【００１６】
外部情報取得部２は、頭部のカメラなどの距離センサを用いて、外部環境内の障害物の位置や形状などの外部情報を取得する。外部情報取得部２により取得された外部情報は、環境地図情報としてメモリなどに記憶する。
【００１７】
経路生成部３は、例えば、予め設定されたスタート位置（始端点）及びゴール位置（終端点）と、メモリに記憶されている環境地図情報と、に基づいて、ロボット１００と環境地図情報の障害物とが干渉しない、ロボット本体や関節部Ｒ１〜Ｒ４、Ｌ１〜Ｌ４の動作経路を生成する。経路生成部３は、例えば、障害物が込み入った環境であっても動作経路の探索が可能な確率的な探索手法を用いて、動作経路を生成する。
【００１８】
なお、この確率的な探索手法の詳細は、例えば、非特許文献：LaValle Steven K, Kuffner Jr. James J.(2001). “Randomized kinodynamic planning”. The International Journal of Robotics Research (IJRR)20(5).に開示されており、これを援用できるものとする。また、上述した動作経路の生成方法は一例であり、これに限定されない。
【００１９】
経路生成部３は、例えば、図３に示す如く、ロボット１００の関節部Ｒ１〜Ｒ４、Ｌ１〜Ｌ４の関節ベクトルの動作経路を生成する。図３において、縦軸は関節部Ｒ１〜Ｒ４、Ｌ１〜Ｌ４の角度θｉ（θｉ（０）、θｉ（１）、θｉ（２）、・・・、θｉ（ｋ））であり、横軸は、関節ベクトルの距離Ｓである。関節ベクトルは、例えば、以下のように表現される。
θ＝（θ１、θ２、θ３、・・・、θｎ）
［θｉ（０）＝（θ１（０）、θ２（０）、θ３（０）、・・・、θｎ（０））、θｉ（１）＝（θ１（１）、θ２（１）、θ３（１）、・・・、θｎ（１））、・・・、θｉ（ｋ）＝（θ１（ｋ）、θ２（ｋ）、θ３（ｋ）、・・・、θｎ（ｋ））］
【００２０】
関節ベクトルの距離Ｓは、各関節ベクトル間の距離である。なお、経路生成部３は、図３と同様に、縦軸をｙ座標、横軸をｘ座標としたロボット地図ベクトル（ｘ、ｙ）の動作経路を生成してもよい。
【００２１】
上記生成方法によって生成された動作経路のスタート位置とゴール位置との間には、ロボット１００が障害物と干渉しない経路となるように複数の中継点が設定される。経路生成部３は、スタート位置とゴール位置の間のこれら中継点を結んだジグザグの動作経路を生成する。
【００２２】
経路平坦化部４は、経路生成部３により生成された動作経路を平坦化する。ここで、平坦化とは、上述したように設定された中継点の中から省略可能な中継点を抽出し、抽出された中継点を削除することである。平坦化することで、動作経路が単純化されることになり、ロボット１００の無駄な動作が低減（あるいは解消）される。
【００２３】
図４は、上記平坦化の一例を説明するための図である。例えば、図４において、ｐ１はスタート点、ｐ２〜ｐ５は中継点、ｐ６はゴール点とする。まず、経路平坦化部４は、スタート点ｐ１から何点先（２点以上先から端点までの間）までの平坦化を検討するか指定する。この指定は、ユーザによって経路平坦化部４に入力されても良いし、経路平坦化部４が予め設定されたデフォルト値を用いるようにしても良い。ここでは、３点先（ｐ４）までと指定された場合を例にして説明する。このとき、経路平坦化部４は、ｐ１とｐ４とを線分で結び、メモリの環境地図情報に基づいて、この線分上のある一定間隔毎の点が障害物と干渉するか否かを検証する。経路平坦化部４は、検証の結果、障害物との干渉がない場合は、ｐ１とｐ４とを結んだ経路を新経路とする。
【００２４】
一方、経路平坦化部４は、検証の結果、障害物と干渉する場合は、ｐ１とｐ３とを結んだ経路について同様の検証を行い障害物との干渉がない場合、ｐ１とｐ３とを結んだ経路を新経路とする。ｐ１とｐ３とを結んだ経路が障害物と干渉する場合、経路平坦化部４は、この区間での平坦化を行わない。そして、経路平坦化部４は、平坦化後の終点から再度次の平坦化を行う。例えば、経路平坦化部４は、ｐ１とｐ４とを結んだ経路を新経路とした場合、ｐ４をスタート点として次の平坦化処理を実行する。このようにして、経路平坦化部４は、順次、経路の平坦化を行い、動作経路のスタート位置からゴール位置までの全てに関して平坦化を行う。なお、上述した平坦化の方法は、一例でありこれに限定されない。
【００２５】
経路平滑化部５は、経路平坦化部４により平坦化されたジグザグの動作経路を平滑化する。ここで、平滑化とは、上述したように設定された経路を滑らかな曲線にすることを指す。平坦化が終了した時点での動作経路は、各中継点において急激に方向が変わる、所謂ジグザグの経路となっている。これを滑らかな曲線とすることで、ロボット１００の動作が滑らかとなる。ここでは、平滑化後の経路が無限回微分可能な関数によって近似されるため、経路自体の滑らかさに加えて、その経路上の移動速度及び加速度の滑らかさも保証される。平滑化に際して、滑らかにした動作経路が障害物と干渉するか検証し、干渉する場合はその干渉が回避されるまで平滑化が再履行される。
【００２６】
ところで、従来の動作経路計画装置において、例えば、中継点間で中間点をずらしつつ、その中間点を結び平滑化した経路が障害物に干渉しないように経路を計画する。このため、滑らかで障害物に干渉しない最適な中間点を見つけ出すのに、その計算負荷が高くなる虞がある。
【００２７】
これに対し、本実施形態に係る動作経路計画装置において、経路平滑化部５は、隣接するノード間に中間点をそれぞれ設定し、隣接した２つの該中間点の区間と軟化関数との合成積を計算することで、該区間に対する平滑化関数の経路を算出する。そして、経路平滑化部５は、該算出した平滑化関数の経路が障害物と干渉しない場合、該平滑化関数の経路を新たな経路として生成、該算出した平滑化関数の経路が障害物と干渉する場合、中間点とノードとの間を所定比率で分割する分割点を再設定し、該再設定した分割点と中間点との間の平滑化関数の経路を算出し、該平滑化関数の経路が障害物と干渉するか否かを判定して、新たな経路を生成する。
【００２８】
これにより、各ノード間に中間点及び分割点を自動的に設定し、設定した中間点及び分割点に対して平滑化関数を算出して障害物の干渉を判定できるため、滑らかで障害物に干渉しない最適な中間点及び分割点を自動的に設定できる。したがって、計算負荷を軽くしつつ、障害物を干渉することなく滑らかに動作できる経路を計画できる。
【００２９】
経路平滑化部５は、例えば、経路平坦化部４により平坦化された動作経路と、多項式の軟化関数（所定関数）と、の合成積を行うことにより、その動作経路を平滑化する。
【００３０】
図５は、上記平滑化を説明するための図である。図５において、ｎ０〜ｎ４は、上述のように設定された動作経路の中継点であり、ノードである。ｓｎ_０〜ｓｎ４は、ノードｎ０〜ｎ４に対応する関節ベクトルの距離である。ｍ１〜ｍ４は、動作経路の隣接するノード間の中間点である。ｓ_１〜ｓ_４は、中間点ｍ１〜ｍ４に対応する関節ベクトルの距離である。ｍ１１〜ｍ３１は、動作経路の中間点と隣接するノード間を所定比率（例えば、１：１、や２：３など）で分割する分割点である。所定比率は、例えば、ロボット１００の体格や環境状態（障害物の数、大きさ、など）を考慮して実験的に求めた最適値がメモリなどに予め設定されている。ｓｍ_１１〜ｓｍ_３１は、分割点ｍ１１〜ｍ３１に対応する関節ベクトルの距離である。
【００３１】
経路平滑化部５は、例えば、図５に示す如く、まずノードｎ１近傍での平滑化を行う。
経路平滑化部５は、動作経路（実線（１））の中間点ｍ１、ｍ２の区間[ｓ_１、ｓ_２]と、図６に示す軟化関数の区間[−ａｎ／２、ａｎ／２]と、を合せる。例えば、経路平滑化部５は、中間点ｍ１とノードｎ１との区間［ｓ_１、ｓｎ_１］の距離と、ノードｎ１と中間点ｍ２の区間［ｓｎ_１、ｓ_２］の距離と、のうち短い方（この場合、区間［ｓ_１、ｓｎ_１］）を選択する。経路平滑化部５は、選択した区間［ｓ_１、ｓｎ_１］を２倍したものと、区間［−ａｎ／２、ａｎ／２］の間隔が等しくなるように、ａｎを設定することで、区間[ｓ_１、ｓ_２]と、区間[−ａｎ／２、ａｎ／２]と、を合せる（ｓ_１＝−ａｎ／２、ｓ_２＝ａｎ／２）。そして、経路平滑化部５は、区間[ｓ_１、ｓ_２]と、区間[−ａｎ／２、ａｎ／２]と、の合成績を計算し、平滑化関数（点線（２））を生成する。
【００３２】
経路平滑化部５は、メモリの環境地図情報に基づいて、生成した平滑化関数の経路が障害物と干渉するか否かを確認する。この場合、平滑化関数の経路が障害物と干渉するので、経路平滑化部５は、生成した平滑化関数を破棄する。
【００３３】
ここで、分割点ｍ１１とノードｎ１間の距離が所定距離εよりも大きい場合、経路平滑化部５は、次の平滑化処理を継続する。所定距離εは、例えば、ロボット１００の体格や環境状態（障害物の数、大きさ、など）を考慮して実験的に求めた最適値がメモリなどに予め設定されている。経路平滑化部５は、動作経路の分割点ｍ１１、ｍ２の区間[ｓ_１１、ｓ_２]と、軟化関数の区間[−ａｎ／２、ａｎ／２]と、を合せて（ｓ_１１＝−ａｎ／２、ｓ_２＝ａｎ／２）、合成績を計算し、平滑化関数（実線（３））を生成する。この場合、平滑化関数が障害物と干渉しないので、経路平滑化部５は、その平滑化関数を新たな動作経路とする。一方で、分割点ｍ１１とノードｎ１間の距離が所定距離ε以下の場合、経路平滑化部５は、平滑化処理を中止し、動作経路の中間点ｍ１、ｍ２の区間[ｓ_１、ｓ_２]を、そのまま、ジグザグ状態で残す。この場合、動作経路はｍ１、ｎ１、ｍ２となる。
【００３４】
同様にして、経路平滑化部５は、動作経路の区間[ｓ_２、ｓ_３]、及び[ｓ_３、ｓ_４]についても、平滑化を行い、新たな動作経路を生成する。
【００３５】
ここで、上述した軟化関数について、説明する。
図７（ａ）〜（ｄ）は、軟化関数Ｔ（ｘ）の一例をグラフ表示した図である。
軟化関数は、例えば、テナリー多項式（Ternary  Polynomial）関数である。テナリー多項式関数は、は、関数値の増加領域、一定領域、減少領域の三領域を有し、一定領域を挟んで増加領域と減少領域が対称に表される多項式関数である。なお、テナリー多項式の詳細については、本出願人が既に提出した特開２００９−０５３９２６号公報に開示されており、これを援用できるものする。
【００３６】
この軟化関数Ｔ（ｘ）は、例えば次の式（１）により表される。
【数１】
2018069428.tif 000003
式（１）
【００３７】
この式（１）において、次数０のＴ０（ｘ）は、Ｔ０（ｘ）＝Ｃ（−ａ０／２＜ｘ＜ａ０／２）、Ｔ０（ｘ）＝０（ｘ≦−ａ０／２、ｘ≧ａ０／２）である。Ｃ、ａ０は定数である。ｉは０又は正の整数である。
【００３８】
図７（ａ）は、軟化関数の次数０のＴ０（ｘ）を示したものであり、矩形波状のプロファイルとなっている。図７（ｂ）は、軟化関数の次数１のＴ１（ｘ）を示したものであり、台形状のプロファイルとなっている。図７（ｃ）は、軟化関数の次数２のＴ２（ｘ）を示したものであり、滑らかに変化する台形状のプロファイルとなっている。図７（ｄ）は、軟化関数の次数３のＴ３（ｘ）を示したものであり、滑らかに変化する台形状のプロファイルとなっている。
【００３９】
図８に示すように、軟化関数Ｔｉ（ｘ）は、Ｔｉ（ｘ）−ｘの座標系においてＴｉ（ｘ）の波形を原点を中心に点対称に振り分けてＴ′ｉ＋１（ｘ）を生成し、このＴ′ｉ＋１（ｘ）を積分することによって、一つ次数の高いＴｉ＋１（ｘ）を生成することができる。
【００４０】
例えば、Ｔ０（ｘ）からＴ２（ｘ）を生成する場合、Ｔ０（ｘ）を対称分割（対称振分け）し積分してＴ１（ｘ）を生成し、Ｔ１（ｘ）を対称分割し積分してＴ２（ｘ）を生成してもよいし、Ｔ０（ｘ）を対称分割した後、さらに対称分割してＴ″２（ｘ）を生成し、Ｔ″２（ｘ）を二回積分してＴ２（ｘ）を生成してもよい。
【００４１】
軟化関数Ｔ（ｘ）におけるＣ、ａ０などの定数は、ロボット１００の移動部分における動作の制限値（上限値、下限値）に応じて設定すればよい。例えば、軟化関数Ｔ（ｘ）を用いて移動部分の速度プロファイルを生成する場合、図８において、Ｔ２（ｘ）を速度プロファイルとして設定すると、Ｔ２′（ｘ）は加速度状態を示し、Ｔ２″（ｘ）がジャーク状態を示すこととなる。
【００４２】
このため、Ｔ２″（ｘ）の制限値Ｃ、−Ｃとして移動部分のジャーク上限値、下限値を設定し、Ｔ２′（ｘ）の制限値Ｃ・ａ０、−Ｃ・ａ０として移動部分の加速度上限値、下限値を設定し、Ｔ２（ｘ）の制限値Ｃ・（ａ１−ａ０）・ａ０として移動部分の速度制限値を設定することにより、一義的に定数Ｃ、ａ０、ａ１の値が決まる。
【００４３】
このような軟化関数Ｔ（ｘ）は、速度、加速度、ジャークなどの微分係数の制限を満たす演算が迅速に行える。このため、軟化関数Ｔ（ｘ）を用いて動作経路の演算を行うことにより、速度などの所定の制限を満たす移動経路を迅速に演算することができる。
【００４４】
次に、動作経路と多項式の軟化関数との合成積の演算方法について詳細に説明する。
経路平滑化部５は、経路生成部３により生成された動作経路ｆ（ｙ）と、上述した多項式の軟化関数Ｔｎ（ｘ）と、に基づいて、下記式（２）を用いて合成積ｆｄ（ｘ）を行う。
【数２】
2018069428.tif 000004
式（２）
【００４５】
軟化関数Ｔｎ（ｘ）は、ｎ−１回微分可能な関数であり、パラメータａ０、ａ１、・・・・、ａｎ、及びＣを有する。ｄ＝（ａ０、ａ１、・・・・、ａｎ、Ｃ）と定義する。Ｃは、軟化関数の積分値が１となるように調整されている。ｆｄ（ｘ）はｎ−１回微分可能な関数で、ｆｄ→ｆに一様に収束する。
【００４６】
経路出力部６は、経路平滑化部５により平滑化した動作経路を出力する。例えば、経路出力部６は、図５に示すような関節角度と関節ベクトルの距離との関係を示すグラフデータを平滑化した動作経路として、ロボット１００の関節ベクトルの成分毎に出力する。また、経路出力部６は、ロボット地図ベクトル（ｘ、ｙ）の軌道を、平滑化した動作経路として出力する。経路出力部６は、平滑化した動作経路を、ロボット１００を制御する制御装置１０５や上記グラフデータを表示する表示部に出力する。
【００４７】
次に、本実施形態に係る動作経路計画方法について、詳細に説明する。図９は、動作経路計画方法のフローを示すフローチャートである。経路生成部３は、メモリの環境地図情報に基づいて、予め設定された始端点と終端点との間にロボット１００が障害物と干渉しない動作経路となるようにノードｎｉを設定する。経路平坦化部４は、経路生成部３により設定された動作経路のノードｎｉの平坦化を行う。
【００４８】
続いて、経路平滑化部５は、経路平坦化部４により平坦化された動作経路のノードｎｉの平滑化を行う。経路平滑化部５は、動作経路のノードｎｉの平滑化を行う場合、中間点ｍｉとノードｎｉ間の距離および中間点ｍｉ＋１とノードｎｉ間の距離が所定距離εより大きいか否かを判定する（ステップＳ１０１）。ｉは自然数とする。
【００４９】
経路平滑化部５は、中間点ｍｉとノードｎｉ間の距離および中間点ｍｉ＋１とノードｎｉ間の距離のうち一方が所定距離ε以下であると判定すると（ステップＳ１０１のＮＯ）、次のノードｎｉ＋１の平滑化を行う。この場合、動作経路は、中間点ｍｉ、ノードｎｉ、及び中間点ｍｉ＋１を結んだジグザグの経路となる。
【００５０】
一方、経路平滑化部５は、中間点ｍｉとノードｎｉ間の距離および中間点ｍｉ＋１とノードｎｉ間の距離が所定距離εより大きいと判定すると（ステップＳ１０１のＹＥＳ）、中間点ｍｉと中間点ｍｉ＋１との間の区間と、対応する軟化関数Ｔｎと、を合せて、合成績ｆｄ（ｘ）を計算し、平滑化関数を生成する（ステップＳ１０２）。そして、経路平滑化部５は、生成した平滑化関数の経路が障害物に干渉するか否かを判定する（ステップＳ１０３）。
【００５１】
経路平滑化部５は、生成した平滑化関数の経路が障害物に干渉しないと判定したとき（ステップＳ１０３のＮＯ）、その平滑化関数の経路を新たな経路とし、次のノードｎｉ＋１の平滑化を行う。一方で、経路平滑化部５は、生成した平滑化関数の経路が障害物に干渉すると判定したとき（ステップＳ１０３のＹＥＳ）、ｐ＝１とし、ノードｎｉと中間点ｍｉとの間の分割点ｍｉｐと、ノードｎｉと、の間の距離が所定距離εより大きいか否かを判定する（ステップＳ１０４）。
【００５２】
経路平滑化部５は、分割点ｍｉｐとノードｎｉとの間の距離が所定距離ε以下であると判定すると（ステップＳ１０４のＮＯ）、元のジグザグの経路ｍ１、ｎ１、ｍ２を動作経路とし次のノードｎｉ＋１の平滑化を行う。一方、経路平滑化部５は、分割点ｍｉｐとノードｎｉ間の距離が所定距離εより大きいと判定すると（ステップＳ１０４のＹＥＳ）、分割点ｍｉｐと中間点ｍｉ＋１との区間と、対応する軟化関数Ｔｎと、を合せて、合成績ｆｄ（ｘ）を計算し、平滑化関数を生成する（ステップＳ１０５）。経路平滑化部５は、生成した平滑化関数の経路が障害物に干渉するか否かを判定する（ステップＳ１０６）。
【００５３】
経路平滑化部５は、生成した平滑化関数の経路が障害物に干渉しないと判定したとき（ステップＳ１０６のＮＯ）、その平滑化関数の経路を新たな経路とし、次のノードｎｉ＋１の平滑化を行う。一方で、経路平滑化部５は、生成した平滑化関数の経路が障害物に干渉すると判定したとき（ステップＳ１０６のＹＥＳ）、ｐ＝１、ｑ＝１とし、ノードｎｉと中間点ｍ２との間の分割点ｍｉ＋１ｑと、ノードｎｉと、の間の距離が所定距離εより大きいか否かを判定する（ステップＳ１０７）。
【００５４】
経路平滑化部５は、分割点ｍｉ＋１ｑとノードｎｉとの間の距離が所定距離ε以下であると判定すると（ステップＳ１０７のＮＯ）、元のジグザグの経路ｍ１、ｎ１、ｍ２を動作経路とし、次のノードｎｉ＋１の平滑化を行う。一方、経路平滑化部５は、分割点ｍｉ＋１ｑとノードｎｉとの間の距離が所定距離εより大きいと判定すると（ステップＳ１０７のＹＥＳ）、分割点ｍｉｐと分割点ｍｉ＋１ｑとの区間と、対応する軟化関数Ｔｎと、を合せて、合成績ｆｄ（ｘ）を計算し、平滑化関数を生成する（ステップＳ１０８）。そして、経路平滑化部５は、生成した平滑化関数の経路が障害物に干渉するか否かを判定する（ステップＳ１０９）
【００５５】
経路平滑化部５は、生成した平滑化関数の経路が障害物に干渉しないと判定したとき（ステップＳ１０９のＮＯ）、その平滑化関数の経路を新たな経路とし、次のノードｎｉ＋１の平滑化を行う。一方で、経路平滑化部５は、生成した平滑化関数の経路が障害物に干渉すると判定したとき（ステップＳ１０９のＹＥＳ）、分割点ｍｉｐとノードｎｉとの間を所定比率で分割した分割点とノードｎｉの間の距離が所定距離εより大きいか否かを判定する（ステップＳ１１０）。
【００５６】
経路平滑化部５は、分割点とノードｎｉとの間の距離が所定距離ε以下であると判定すると（ステップＳ１１０のＮＯ）、元のジグザグの経路ｍ１、ｎ１、ｍ２を動作経路とし、次のノードｎｉ＋１の平滑化を行う。一方、経路平滑化部５は、分割点とノードｎｉとの間の距離が所定距離εより大きいと判定すると（ステップＳ１１０のＹＥＳ）、ｐ＝ｐ＋１、ｑ＝ｑ＋１（ステップＳ１１１）とし、上記（ステップＳ１０８）に戻る。
【００５７】
以上、本実施形態において、経路生成部３は、ロボット１００の動作環境内の障害物の位置及び形状を含む環境地図情報に基づいて、予め設定された始端点と終端点との間でロボット１００が障害物と干渉しない経路となるように複数のノードを設定する。経路平滑化部５は、隣接するノード間に中間点をそれぞれ設定し、隣接した２つの該中間点の区間と軟化関数との合成積を計算することで、該区間に対する平滑化関数の経路を算出する。さらに、経路平滑化部５は、該算出した平滑化関数の経路が障害物と干渉しない場合、該平滑化関数の経路を新たな経路として生成、該算出した平滑化関数の経路が障害物と干渉する場合、中間点とノードとの間を所定比率で分割する分割点を再設定し、該再設定した分割点と中間点との間の平滑化関数の経路を算出し、該平滑化関数の経路が障害物と干渉するか否かを判定して、新たな経路を生成する。
これにより、各ノード間に中間点及び分割点を自動的に設定し、設定した中間点及び分割点に対して平滑化関数を算出して障害物の干渉を判定することができ、滑らかで障害物に干渉しない最適な中間点及び分割点を自動的に設定できる。したがって、計算負荷を軽くしつつ、障害物を干渉することなく滑らかに動作できる経路を計画できる。
【００５８】
なお、本発明は上記実施の形態に限られたものではなく、趣旨を逸脱しない範囲で適宜変更することが可能である。
上記実施形態において、動作経路計画装置１は、経路平坦化部４を備えない構成であってもよい。この場合、経路平滑化部５は、経路生成部３により生成された動作経路を平滑化する。
上記実施形態において、ロボット１００は、例えば、１つのアーム部１０３を有する片腕ロボットとして構成されてもよく（図１０）、アーム部１０３のみを有するアーム型ロボットであってもよい。さらに、ロボット１００は、自律型の車両などの移動体であってもよい。
【００５９】
本発明は、例えば、図９に示す処理を、ＣＰＵにコンピュータプログラムを実行させることにより実現することも可能である。
【００６０】
プログラムは、様々なタイプの非一時的なコンピュータ可読媒体（non-transitory computer readable medium）を用いて格納され、コンピュータに供給することができる。非一時的なコンピュータ可読媒体は、様々なタイプの実体のある記録媒体（tangible storage medium）を含む。非一時的なコンピュータ可読媒体の例は、磁気記録媒体（例えばフレキシブルディスク、磁気テープ、ハードディスクドライブ）、光磁気記録媒体（例えば光磁気ディスク）、ＣＤ−ＲＯＭ（Read Only Memory）、ＣＤ−Ｒ、ＣＤ−Ｒ／Ｗ、半導体メモリ（例えば、マスクＲＯＭ、ＰＲＯＭ（Programmable ROM）、ＥＰＲＯＭ（Erasable PROM）、フラッシュＲＯＭ、ＲＡＭ（random access memory））を含む。
【００６１】
プログラムは、様々なタイプの一時的なコンピュータ可読媒体（transitory computer readable medium）によってコンピュータに供給されてもよい。一時的なコンピュータ可読媒体の例は、電気信号、光信号、及び電磁波を含む。一時的なコンピュータ可読媒体は、電線及び光ファイバ等の有線通信路、又は無線通信路を介して、プログラムをコンピュータに供給できる。
【符号の説明】
【００６２】
１  動作経路計画装置、２  外部情報取得部、３  経路生成部、４  経路平坦化部、５経路平滑化部、６  経路出力部
【図１】
2018069428.tif 000005
【図２】
2018069428.tif 000006
【図３】
2018069428.tif 000007
【図４】
2018069428.tif 000008
【図５】
2018069428.tif 000009
【図６】
2018069428.tif 000010
【図７】
2018069428.tif 000011
【図８】
2018069428.tif 000012
【図９】
2018069428.tif 000013
【図１０】
2018069428.tif 000014