(57)【要約】【課題】ロボットと障害物との干渉判定を最適な頻度で行ってロボットの軌道計画を行うこと。【解決手段】軌道計画装置は、ロボットが始点から終点まで移動するための軌道を計画する。軌道計画装置は、計画された軌道上におけるロボットと障害物との距離を算出する距離算出手段と、距離算出手段により算出された距離に基づいて、始点から終点までの軌道間を離散化する離散化幅を設定する離散化設定手段と、離散化設定手段により設定された離散化幅で離散化したときの軌道上の各位置で、ロボットと障害物とが干渉しているか否かを判定する干渉判定手段と、干渉判定手段による干渉判定結果に基づいて、ロボットの軌道を計画する軌道計画手段と、を備える。【選択図】図１
2013246553.tif 000002
【特許請求の範囲】
【請求項１】
ロボットが始点から終点まで移動するための軌道を計画する軌道計画装置であって、
前記計画された軌道上における前記ロボットと障害物との距離を算出する距離算出手段と、
前記距離算出手段により算出された距離に基づいて、前記始点から終点までの軌道間を離散化する離散化幅を設定する離散化設定手段と、
前記離散化設定手段により設定された離散化幅で離散化したときの軌道上の各位置で、前記ロボットと障害物とが干渉しているか否かを判定する干渉判定手段と、
前記干渉判定手段による干渉判定結果に基づいて、前記ロボットの軌道を計画する軌道計画手段と、
を備える、ことを特徴とする軌道計画装置。
【請求項２】
請求項１記載の軌道計画装置であって、
前記離散化設定手段は、前記距離算出手段により算出された距離が増加するに従って前記離散化隔を増加させる、ことを特徴とする軌道計画装置。
【請求項３】
請求項１又は２記載の軌道計画装置であって、
前記離散化設定手段は、
前記距離算出手段により算出された距離と、前記軌道計画手段により算出された前記ロボットの関節角度と、に基づいて、前記ロボットのヤコビアンを算出するヤコビアン計算部と、
前記ヤコビアン計算部により算出されたロボットのヤコビアンと、前記距離算出手段により算出された距離と、に基づいて前記離散化隔を算出するパラメータ計算部と、を有する、ことを特徴とする軌道計画装置。
【請求項４】
請求項１乃至３のうちいずれか１項記載の軌道計画装置であって、
前記距離算出手段は、前記計画された軌道上におけるロボットと障害物との最短距離を算出する、ことを特徴とする軌道計画装置。
【請求項５】
請求項１乃至３のうちいずれか１項記載の軌道計画装置であって、
前記ロボットは、アームロボットであり、
前記距離算出手段は、前記障害物から所定番目に近い前記アームロボットの関節部と障害物との距離を算出する、ことを特徴とする軌道計画装置。
【請求項６】
請求項１乃至３のうちいずれか１項記載の軌道計画装置であって、
前記距離算出手段は、前記障害物とロボットの特徴部位との距離を算出する、ことを特徴とする軌道計画装置。
【請求項７】
請求項３記載の軌道計画装置であって、
前記パラメータ計算部は、前記ヤコビアン計算部により算出されたロボットのヤコビ逆行列と、前記距離算出手段により算出された最短距離と、所定のマージン定数と、を乗算することで、前記離散化隔を算出する、ことを特徴とする軌道計画装置。
【請求項８】
請求項７記載の軌道計画装置であって、
前記所定のマージン定数は、前記ロボットの自己干渉の場合大きく設定され、他の物体との干渉の場合小さく設定される、ことを特徴とする軌道計画装置。
【請求項９】
請求項６記載の軌道計画装置であって、
前記ロボットの特徴部位は、手先、指先、複雑な形状部位、及び、薄い形状部位のうち少なくとも１つを含む、ことを特徴とする軌道計画装置。
【請求項１０】
ロボットが始点から終点まで移動するための軌道を計画する軌道計画方法であって、
前記計画された軌道上における前記ロボットと障害物との距離を算出するステップと、
前記算出された距離に基づいて、前記始点から終点までの軌道間を離散化する離散化幅を設定するステップと、
前記設定された離散化幅で離散化したときの軌道上の各位置で、前記ロボットと障害物とが干渉しているか否かを判定するステップと、
前記干渉判定結果に基づいて、前記ロボットの軌道を計画するステップと、
を含む、ことを特徴とする軌道計画方法。
【請求項１１】
ロボットが始点から終点まで移動するための軌道を計画する軌道計画プログラムであって、
前記計画された軌道上における前記ロボットと障害物との距離を算出する処理と、
前記算出された距離に基づいて、前記始点から終点までの軌道間を離散化する離散化幅を設定する処理と、
前記設定された離散化幅で離散化したときの軌道上の各位置で、前記ロボットと障害物とが干渉しているか否かを判定する処理と、
前記干渉判定結果に基づいて、前記ロボットの軌道を計画する処理と、
をコンピュータに実行させる、ことを特徴とする軌道計画プログラム。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、ロボットと障害物との干渉を判定しつつ、ロボットの軌道を計画できる軌道計画装置、軌道計画方法及び軌道計画プログラムに関するものである。
【背景技術】
【０００２】
近年、ロボットなどの移動体が始点から終点までを移動するための軌道を計画する様々な軌道計画装置が知られている。例えば、始点から終点までの軌道間を離散化して離散化した各位置にサブゴールを夫々設け、各サブゴールでロボットと障害物との干渉を判定して、ロボットの軌道を計画する動作経路計画方法が知られている（特許文献１参照）。
【先行技術文献】
【特許文献】
【０００３】
【特許文献１】特開２００２−０７３１３０号公報
【発明の概要】
【発明が解決しようとする課題】
【０００４】
しかしながら、上記特許文献１に示す動作経路計画方法においては、例えば、始点から終点までの軌道間の離散化幅を大きく設定し、離散化した各位置で干渉判定を行った場合、ロボットが障害物をすり抜ける可能性が生じ、その干渉判定精度が低下する問題が生じる。一方、始点から終点までの軌道間の離散化幅を小さく設定し、離散化した各位置で干渉判定を行った場合、ロボットと障害物との干渉判定頻度が増加しその計算時間が増加する問題が生じる。
【０００５】
本発明は、このような問題点を解決するためになされたものであり、ロボットと障害物との干渉判定を最適な頻度で行ってロボットの軌道計画を行うことができる軌道計画装置、軌道計画方法及び軌道計画プログラムを提供することを主たる目的とする。
【課題を解決するための手段】
【０００６】
上記目的を達成するための本発明の一態様は、ロボットが始点から終点まで移動するための軌道を計画する軌道計画装置であって、前記計画された軌道上における前記ロボットと障害物との距離を算出する距離算出手段と、前記距離算出手段により算出された距離に基づいて、前記始点から終点までの軌道間を離散化する離散化幅を設定する離散化設定手段と、前記離散化設定手段により設定された離散化幅で離散化したときの軌道上の各位置で、前記ロボットと障害物とが干渉しているか否かを判定する干渉判定手段と、前記干渉判定手段による干渉判定結果に基づいて、前記ロボットの軌道を計画する軌道計画手段と、を備える、ことを特徴とする軌道計画装置である。
この一態様において、前記離散化設定手段は、前記距離算出手段により算出された距離が増加するに従って前記離散化隔を増加させてもよい。
この一態様において、前記離散化設定手段は、前記距離算出手段により算出された距離と、前記軌道計画手段により算出された前記ロボットの関節角度と、に基づいて、前記ロボットのヤコビアンを算出するヤコビアン計算部と、前記ヤコビアン計算部により算出されたロボットのヤコビアンと、前記距離算出手段により算出された距離と、に基づいて前記離散化隔を算出するパラメータ計算部と、を有していてもよい。
この一態様において、前記距離算出手段は、前記計画された軌道上におけるロボットと障害物との最短距離を算出してもよい。
この一態様において、前記ロボットは、アームロボットであり、前記距離算出手段は、前記障害物から所定番目に近い前記アームロボットの関節部と障害物との距離を算出してもよい。
この一態様において、前記距離算出手段は、前記障害物とロボットの特徴部位との距離を算出してもよい。
この一態様において、前記パラメータ計算部は、前記ヤコビアン計算部により算出されたロボットのヤコビ逆行列と、前記距離算出手段により算出された最短距離と、所定のマージン定数と、を乗算することで、前記離散化隔を算出してもよい。
この一態様において、前記所定のマージン定数は、前記ロボットの自己干渉の場合大きく設定され、他の物体との干渉の場合小さく設定されてもよい。
この一態様において、前記ロボットの特徴部位は、手先、指先、複雑な形状部位、及び、薄い形状部位のうち少なくとも１つを含んでいてもよい。
他方、上記目的を達成するための本発明の一態様は、ロボットが始点から終点まで移動するための軌道を計画する軌道計画方法であって、前記計画された軌道上における前記ロボットと障害物との距離を算出するステップと、前記算出された距離に基づいて、前記始点から終点までの軌道間を離散化する離散化幅を設定するステップと、前記設定された離散化幅で離散化したときの軌道上の各位置で、前記ロボットと障害物とが干渉しているか否かを判定するステップと、前記干渉判定結果に基づいて、前記ロボットの軌道を計画するステップと、を含む、ことを特徴とする軌道計画方法であってもよい。
また、上記目的を達成するための本発明の一態様は、ロボットが始点から終点まで移動するための軌道を計画する軌道計画プログラムであって、前記計画された軌道上における前記ロボットと障害物との距離を算出する処理と、前記算出された距離に基づいて、前記始点から終点までの軌道間を離散化する離散化幅を設定する処理と、前記設定された離散化幅で離散化したときの軌道上の各位置で、前記ロボットと障害物とが干渉しているか否かを判定する処理と、前記干渉判定結果に基づいて、前記ロボットの軌道を計画する処理と、をコンピュータに実行させる、ことを特徴とする軌道計画プログラムであってもよい。
【発明の効果】
【０００７】
本発明によれば、ロボットと障害物との干渉判定を最適な頻度で行ってロボットの軌道計画を行うことができる軌道計画装置、軌道計画方法及び軌道計画プログラムを提供することができる。
【図面の簡単な説明】
【０００８】
【図１】本発明の一実施の形態に係る軌道計画装置の概略的なシステム構成を示すブロック図である。
【図２】アームロボット及び最短距離を説明するための図である。
【図３】アームロボットと障害物との最短距離に応じた離散化幅を説明するための図である。
【図４】アームロボットと障害物との最短距離に応じた干渉判定頻度を説明するための図である。
【図５】本発明の一実施の形態に係る軌道計画装置による軌道計画処理の一例を示すフローチャートである。
【発明を実施するための形態】
【０００９】
以下、図面を参照して本発明の実施の形態について説明する。本発明の一実施の形態に係る軌道計画装置１は、例えば、複数のリンク１０１と各リンク１０１を回転可能に連結する複数の関節部１０２とを有するアームロボット１００が始点（スタート点）から終点（ゴール点）まで移動するための軌道を最適に計画するものである（図２）。図１は、本発明の一実施の形態に係る軌道計画装置の概略的なシステム構成を示すブロック図である。
【００１０】
軌道計画装置１は、環境復元部２と、距離計算部３と、ヤコビアン計算部４と、パラメータ計算部５と、干渉判定部６と、軌道計画部７と、を備えている。
【００１１】
環境復元部２には、例えば、障害物Ｘの位置情報、形状情報、マップ情報などの環境情報が入力される。環境情報は、例えば、磁気ディスク装置、光ディスク装置、ＲＯＭ、ＲＡＭなどの記憶装置８に予め記憶されており、この記憶装置８から出力されてもよい。なお、環境復元部２は、アームロボット１００に搭載された検出センサ９（超音波センサ、ミリ波センサ、カメラなど）を用いて、アームロボット１００周囲の環境情報を算出してもよい。
【００１２】
環境復元部２は、算出あるいは入力された環境情報と、軌道計画部７により算出されるアームロボット１００の各関節部１０２の関節角度（時系列データ等）と、に基づいて、アームロボット１００に対する障害物Ｘの相対位置及びアームロボット１００の姿勢を算出する。環境復元部２は、算出した障害物Ｘの相対位置及びアームロボット１００の姿勢を距離計算部３及び干渉判定部６に対して出力する。
【００１３】
距離計算部３は、距離算出手段の一具体例であり、環境復元部２により算出された障害物Ｘの相対位置及びアームロボット１００の姿勢に基づいて、アームロボット１００と障害物Ｘとの最短距離Ｌminを算出する。距離計算部３は、例えば、ＧＪＫアルゴリズム（Gillbert-Johnson-Keerthi Algorithm）などを用いて、アームロボット１００と障害物Ｘとの最短距離Ｌminを算出しているが、任意のアルゴリズムを用いて最短距離Ｌminを算出してもよい。距離計算部３は、算出したアームロボット１００と障害物Ｘとの最短距離Ｌminをヤコビアン計算部４及びパラメータ計算部５に対して出力する。
【００１４】
なお、距離計算部３は、環境復元部２により算出された障害物Ｘの相対位置及びアームロボット１００の姿勢に基づいて、上記最短距離の代わりに、障害物Ｘから所定番目（例えば、２番目又は３番目）に近い関節部１０２と、その障害物Ｘと、の距離を算出してもよい。これにより、アームロボット１００と障害物Ｘとの接触をより確実に防止できる。
【００１５】
また、距離計算部３は、環境復元部２により算出された障害物Ｘの相対位置及びアームロボット１００の姿勢に基づいて、上記最短距離の代わりに、アームロボット１００の特徴部位と障害物Ｘとの距離を算出してもよい。なお、アームロボット１００の特徴部位として、例えば、手先、指先、複雑な形状部位（肘関節部など）、すり抜けが生じ易い薄い形状部位などが含まれる。これにより、アームロボット１００の特徴部位の特性に応じて、最適に干渉判定を行うことができる。
【００１６】
ヤコビアン計算部４は、距離計算部３により算出されたアームロボット１００と障害物Ｘとの最短距離Ｌminと、軌道計画部７により算出されるアームロボット１００の各関節部１０２の関節角度と、に基づいてアームロボット１００のヤコビアンJ（θ）を算出する。ヤコビアン計算部４は、算出したアームロボット１００のヤコビアンJ（θ）をパラメータ計算部５に対して出力する。
【００１７】
ところで、従来、例えば、始点から終点までの軌道間の離散化幅を大きく設定し、離散化した各位置で干渉判定を行った場合、ロボットが障害物をすり抜ける可能性が生じ、その干渉判定精度が低下する問題が生じる。一方で、始点から終点までの軌道間の離散化幅を小さく設定し、離散化した各位置で干渉判定を行った場合、ロボットと障害物との干渉判定頻度が増加しその計算時間が増加する問題が生じる。
【００１８】
そこで、本実施の形態に係る軌道計画装置１において、パラメータ計算部５は、距離計算部３により算出されたアームロボット１００と障害物Ｘとの最短距離Ｌminが増加し、アームロボット１００と障害物Ｘが離間するに従って離散化幅Δθを増加させる（図３）。一方、パラメータ計算部５は、距離計算部３により算出されたアームロボット１００と障害物Ｘとの最短距離Ｌminが減少し、アームロボット１００と障害物Ｘが接近するに従って離散化幅Δθを増加させる。
【００１９】
これにより、環境中でアームロボット１００と障害物Ｘとの距離が大きいとき（接触の可能性が低いとき）、そのアームロボット１００と障害物Ｘとの干渉を低頻度で判定を行う（図４）。一方、アームロボット１００と障害物Ｘとの距離が小さいとき（接触の可能性が高いとき）、そのアームロボット１００と障害物Ｘとの干渉を高頻度で判定を行う。したがって、アームロボット１００と障害物Ｘとの干渉の可能性に応じて、アームロボット１００と障害物Ｘとの干渉判定を最適な頻度で行ってアームロボット１００の軌道を計画できる。
【００２０】
例えば、パラメータ計算部５は、距離計算部３により算出されたアームロボット１００と障害物Ｘとの最短距離Ｌminと、ヤコビアン計算部４により算出されたアームロボット１００のヤコビアンJ（θ）と、に基づいて、下記（１）式を用いて離散化幅Δθを算出する。パラメータ計算部５は、算出した離散化幅Δθを干渉判定部６に対して出力する。
Δθ＝J（θ）＃δＬmin      （１）式
ここで、Ｊ（θ）＃は、ヤコビアンＪ（θ）の疑似逆行列であり、δは最短距離Ｌminに乗算するマージン定数である。
【００２１】
例えば、マージン定数δは、アームロボット１００の自己干渉が想定される場合、その干渉がある程度予測可能であることから大きく設定される（１に近い値を設定）。一方、マージン定数δは、他の物体との干渉が想定される場合、その干渉が予測不可能であることから、小さく設定される。このように、干渉の予測可能性に応じて、マージン定数δを設定しているが、これに限らず、任意の設定方法を適用できる。また、上記（１）式によって得られる離散化幅Δθは、例えば、障害物Ｘとアームロボット１００上の最短点との幅のδ倍となる１次近似値となっているが、より高精度にｎ次近似値としてもよい。
【００２２】
干渉判定部６は、干渉判定手段の一具体例であり、環境復元部２により算出された障害物Ｘの相対位置及びアームロボット１００の姿勢と、パラメータ計算部５により算出された離散化幅Δθと、に基づいて、アームロボット１００と障害物Ｘとが干渉しているか否かを判定する。例えば、干渉判定部６は、パラメータ計算部５により算出された離散化幅Δθで離散化された軌道上の各位置で、アームロボット１００と障害物Ｘとが干渉しているか否かを判定する。なお、本実施の形態において、アームロボット１００と障害物Ｘとの干渉には、アームロボット１００の自己干渉も含むものとする。干渉判定部６は、アームロボット１００と障害物Ｘとの干渉判定結果を軌道計画部７に対して出力する。
【００２３】
軌道計画部７は、軌道計画手段の一具体例であり、干渉判定部６による干渉判定結果に基づいて、アームロボット１００の軌道を計画する。例えば、軌道計画部７は、アームロボット１００と障害物Ｘとが干渉しないような軌道を計画する。
【００２４】
軌道計画装置１は、例えば、演算処理等と行うＣＰＵ（Central Processing Unit）、ＣＰＵによって実行される演算プログラム等が記憶されたＲＯＭ（Read Only Memory）、処理データ等を一時的に記憶するＲＡＭ（Random Access Memory）等からなるマイクロコンピュータを、中心にしてハードウェア構成されている。ＣＰＵ、ＲＯＭ、及びＲＡＭは、データバスなどを介して相互に接続されている。
【００２５】
次に、本実施の形態に係る軌道計画装置による軌道計画方法の一例について詳細に説明する。図５は、本実施の形態に係る軌道計画装置による軌道計画処理の一例を示すフローチャートである。なお、図５に示す軌道計画処理は、例えば、所定時間毎に繰り返し実行される。
【００２６】
環境復元部２は、入力された環境情報と、軌道計画部７により算出されるアームロボット１００の各関節部１０２の関節角度と、に基づいて、アームロボット１００に対する障害物Ｘの相対位置及びアームロボット１００の姿勢を算出する（ステップＳ１０１）。
【００２７】
距離計算部３は、環境復元部２により算出された障害物Ｘの相対位置及びアームロボット１００の姿勢に基づいて、アームロボット１００と障害物Ｘとの最短距離Ｌminを算出する（ステップＳ１０２）。
【００２８】
ヤコビアン計算部４は、距離計算部３により算出されたアームロボット１００と障害物Ｘとの最短距離Ｌminと、軌道計画部７により算出されるアームロボット１００の関節角度と、に基づいてアームロボット１００のヤコビアンJ（θ）を算出する（ステップＳ１０３）。
【００２９】
パラメータ計算部５は、距離計算部３により算出されたアームロボット１００と障害物Ｘとの最短距離Ｌminと、ヤコビアン計算部４により算出されたアームロボット１００のヤコビアンJ（θ）と、に基づいて、上記（１）式を用いて離散化幅Δθを算出する（ステップＳ１０４）。
【００３０】
干渉判定部６は、環境復元部２により算出された障害物Ｘの相対位置及びアームロボット１００の姿勢と、パラメータ計算部５により算出された離散化幅Δθと、に基づいて、アームロボット１００と障害物Ｘとが干渉しているか否かを判定する（ステップＳ１０５）。
【００３１】
軌道計画部７は、干渉判定部６による干渉判定結果に基づいて、アームロボット１００と障害物Ｘとが干渉しないような軌道を計画する（ステップＳ１０６）。
【００３２】
なお、（ステップＳ１０５）において、干渉判定部６は、環境復元部２により算出された障害物Ｘの相対位置及びアームロボット１００の姿勢に基づいて、アームロボット１００と障害物Ｘとが干渉しているか否かを判定してもよい。この場合、（ステップＳ１０６）において、軌道計画部７は、干渉判定部６による干渉判定結果を、パラメータ計算部５により算出された離散化幅Δθで判断し、アームロボット１００と障害物Ｘとが干渉しないような軌道を計画する。
【００３３】
以上、本実施の形態に係る軌道計画装置１おいて、アームロボットと障害物との最短距離に応じて離散化幅を設定する。これにより、アームロボットと障害物との距離が大きく干渉の可能性が低いとき、アームロボットと障害物との干渉を低頻度で判定を行う。一方、アームロボットと障害物との距離が小さく干渉の可能性が高いとき、アームロボットと障害物との干渉を高頻度で判定を行う。したがって、アームロボットと障害物との干渉の可能性に応じて、アームロボットと障害物との干渉判定を最適な頻度で行うことができる。すなわち、アームロボットと障害物との干渉判定を最適な頻度で行ってロボットの軌道計画を行うことができる
【００３４】
なお、本発明は上記実施の形態に限られたものではなく、趣旨を逸脱しない範囲で適宜変更することが可能である。例えば、上記一実施の形態において、ロボットがアームロボットである場合について説明したが、これに限らず、二足歩行ロボットなどの任意のロボット、更には、任意の移動体に適用可能である。
また、本発明は、例えば、図５に示す処理を、ＣＰＵにコンピュータプログラムを実行させることにより実現することも可能である。
【００３５】
プログラムは、様々なタイプの非一時的なコンピュータ可読媒体（non-transitory computer readable medium）を用いて格納され、コンピュータに供給することができる。非一時的なコンピュータ可読媒体は、様々なタイプの実体のある記録媒体（tangible storage medium）を含む。非一時的なコンピュータ可読媒体の例は、磁気記録媒体（例えばフレキシブルディスク、磁気テープ、ハードディスクドライブ）、光磁気記録媒体（例えば光磁気ディスク）、ＣＤ−ＲＯＭ（Read Only Memory）、ＣＤ−Ｒ、ＣＤ−Ｒ／Ｗ、半導体メモリ（例えば、マスクＲＯＭ、ＰＲＯＭ（Programmable ROM）、ＥＰＲＯＭ（Erasable PROM）、フラッシュＲＯＭ、ＲＡＭ（random access memory））を含む。
【００３６】
また、プログラムは、様々なタイプの一時的なコンピュータ可読媒体（transitory computer readable medium）によってコンピュータに供給されてもよい。一時的なコンピュータ可読媒体の例は、電気信号、光信号、及び電磁波を含む。一時的なコンピュータ可読媒体は、電線及び光ファイバ等の有線通信路、又は無線通信路を介して、プログラムをコンピュータに供給できる。
【産業上の利用可能性】
【００３７】
本発明は、例えば、アームロボット、二足歩行ロボット、ヒューマノイドロボット、などの移動体を移動させる際の軌道を計画する軌道計画装置に利用可能である。
【符号の説明】
【００３８】
１    軌道計画装置１
２    環境復元部
３    距離計算部
４    ヤコビアン計算部
５    パラメータ計算部
６    干渉判定部
７    軌道計画部７
８    記憶装置
９    検出センサ
【図１】
2013246553.tif 000003
【図２】
2013246553.tif 000004
【図３】
2013246553.tif 000005
【図４】
2013246553.tif 000006
【図５】
2013246553.tif 000007