(57)【要約】【課題】割り込み命令が投入された際に適切な移動行動の変更や経路修正を行うことができる制御装置を提供する。【解決手段】新規投入命令を受領する命令受領部１１と、既投入命令列のうち現在実行中の命令を監視する命令監視部３１と、新規投入命令が現在実行中の命令に干渉するか判別する命令判別部１２と、現在実行中の命令に干渉すると判別された場合、新規投入命令に応じて移動体の現在行動を修正する修正部２２と、修正した行動後の移動体の現在位置座標、及び移動体の初期位置座標を取得する座標管理部３２と、新規投入命令が移動経路を指定しているか判別し、移動経路を指定していると判別された場合、現在位置座標から初期位置座標に移動する補完命令を生成し新規投入命令前に挿入する補完命令生成部２１と、補完命令及び新規投入命令を基本的命令列に分解する分解部２３と、基本的命令列を順次実行し移動体を制御する命令実行部５０を備える。【選択図】図１
2010102406.tif 000002
【特許請求の範囲】
【請求項１】
移動体を制御するための新規投入命令を受領する命令受領部と、
前記移動体を制御するための既投入命令列のうち現在実行中の命令を監視する命令監視部と、
前記新規投入命令が前記現在実行中の命令に干渉するか判別する命令判別部と、
前記現在実行中の命令に干渉すると判別された場合、前記新規投入命令に応じて前記移動体の現在行動を修正する修正部と、
前記修正した行動後の前記移動体の現在位置座標、及び前記現在実行中の命令に干渉すると判別された際の前記移動体の初期位置座標を取得する座標管理部と、
前記新規投入命令が移動経路を指定している場合、前記現在位置座標から前記初期位置座標に移動する補完命令を生成し、前記補完命令を前記新規投入命令前に挿入する補完命令生成部と、
前記補完命令及び前記新規投入命令を基本的命令列に分解する分解部と、
前記基本的命令列を順次実行し移動体を制御する命令実行部
とを備えることを特徴とする制御装置。
【請求項２】
前記補完命令生成部が、
前記新規投入命令の目標位置座標を算出し、
前記新規投入命令が移動経路を指定していない場合、前記新規投入命令を、前記移動体が前記現在位置座標から前記目標位置座標へ移動する命令に修正する
ことを特徴とする請求項１に記載の制御装置。
【請求項３】
前記命令判別部が、前記新規投入命令の緊急度に基づいて、前記移動体の現在行動を停止する必要の有無を判別し、
前記修正部が、前記停止する必要があると判別された場合、前記現在実行中の命令を破棄し、前記移動体の現在行動を停止させる命令を生成する
ことを特徴とする請求項１又は２に記載の制御装置。
【請求項４】
前記既投入命令列のうち前記新規投入命令が挿入されるタイミングを判別するタイミング判別部を更に備え、
前記補完命令生成部が、
前記タイミングに基づいて、前記新規投入命令と、前記既投入命令列のうちの新規投入命令が挿入される直前の既投入命令とが干渉するか判別し、
前記新規投入命令と前記直前の既投入命令と干渉すると判別された場合、前記直前の既投入命令及び前記新規投入命令を分解した基本的命令を修正する
ことを特徴とする請求項１〜３のいずれか１項に記載の制御装置。
【請求項５】
前記補完命令生成部が、
前記直前の既投入命令及び前記新規投入命令を分解した最初の基本的命令のそれぞれに並進方向移動成分があるか判別し、
前記直前の既投入命令が目標位置座標を指定しているか判別し、
前記直前の既投入命令及び前記最初の基本的命令のいずれにも並進方向移動成分があり、且つ前記直前の既投入命令が目標位置座標を指定していると判別された場合、前記直前の既投入命令及び前記最初の基本的命令のそれぞれを速度を合わせるように修正する
ことを特徴とする請求項４に記載の制御装置。
【請求項６】
前記補完命令生成部が、
前記直前の既投入命令及び前記最初の基本的命令のいずれか一方に並進方向移動成分があると判別された場合、前記直前の既投入命令の速度を０にする命令に修正し、
前記最初の基本的命令を加速する命令に修正する
ことを特徴とする請求項５に記載の制御装置。
【請求項７】
前記補完命令生成部が、
前記直前の既投入命令及び前記最初の基本的命令のいずれにも並進方向移動成分がないと判別された場合、前記直前の既投入命令及び前記最初の基本的命令の回転方向が同じか判別し、
前記回転方向が同じと判別された場合、前記最初の基本的命令の速度に応じて前記直前の既投入命令の速度を修正し、且つ前記直後の既投入命令の速度に応じて前記新規投入命令を分解した最後の基本的命令の速度を修正する
ことを特徴とする請求項５又は６に記載の制御装置。
【請求項８】
前記既投入命令列のうち前記新規投入命令が挿入されるタイミングを判別するタイミング判別部を更に備え、
前記補完命令生成部が、
前記タイミングに基づいて、前記新規投入命令が、前記新規投入命令以降の既投入命令に干渉するか判別し、
前記新規投入命令以降の既投入命令に干渉すると判別された場合、前記新規投入命令以降の既投入命令を削除する
ことを特徴とする請求項１〜３のいずれか１項に記載の制御装置。
【請求項９】
移動体を制御するための新規投入命令を受領するステップと、
前記移動体を制御するための既投入命令列のうち現在実行中の命令を監視するステップと、
前記新規投入命令が前記現在実行中の命令に干渉するか判別するステップと、
前記現在実行中の命令に干渉すると判別された場合、前記新規投入命令に応じて前記移動体の現在行動を修正するステップと、
前記修正した行動後の前記移動体の現在位置座標、及び前記現在実行中の命令に干渉すると判別された際の前記移動体の初期位置座標を取得するステップと、
前記新規投入命令が移動経路を指定している場合、前記現在位置座標から前記初期位置座標に移動する補完命令を生成し、前記補完命令を前記新規投入命令前に挿入するステップと、
前記補完命令及び前記新規投入命令を基本的命令列に分解するステップと、
前記基本的命令列を順次実行し移動体を制御するステップ
とを含むことを特徴とする制御方法。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、移動体を制御する制御装置及び制御方法に関するものである。
【背景技術】
【０００２】
障害物が存在する環境等において移動体が障害物を認識しつつ衝突しないよう移動するためには移動行動の変更や移動経路の修正等を適宜行う必要がある。また、移動経路の変更等のために割り込み命令が任意のタイミングで投入された場合も、移動行動の変更や移動経路の修正等を行う必要がある。
【０００３】
割り込み命令が投入されて移動行動を変更する際には、移動体自身の慣性力等を考慮し、状況に応じて速度修正と経路修正を行う必要がある。従来は、割り込み命令が投入される度にその投入前後で一旦停止をしたり、移動経路の変更の回数を減らすために障害物に対して大回りの軌道を与えていたため、適切な移動経路の変更ができなかった（例えば、特許文献１参照。）。
【特許文献１】特開平８−６６２３号公報
【発明の開示】
【発明が解決しようとする課題】
【０００４】
本発明の目的は、割り込み命令が投入された際に適切な移動行動の変更や経路修正を行うことができる制御装置及び制御方法を提供することである。
【課題を解決するための手段】
【０００５】
本願発明の一態様によれば、（イ）移動体を制御するための新規投入命令を受領する命令受領部と、（ロ）移動体を制御するための既投入命令列のうち現在実行中の命令を監視する命令監視部と、（ハ）新規投入命令が現在実行中の命令に干渉するか判別する命令判別部と、（ニ）現在実行中の命令に干渉すると判別された場合、新規投入命令に応じて移動体の現在行動を修正する修正部と、（ホ）修正した行動後の移動体の現在位置座標、及び現在実行中の命令に干渉すると判別された際の移動体の初期位置座標を取得する座標管理部と、（ヘ）新規投入命令が移動経路を指定している場合、現在位置座標から初期位置座標に移動する補完命令を生成し、補完命令を新規投入命令前に挿入する補完命令生成部と、（ト）補完命令及び新規投入命令を基本的命令列に分解する分解部と、（チ）基本的命令列を順次実行し移動体を制御する命令実行部とを備える制御装置が提供される。
【０００６】
本願発明の他の態様によれば、（イ）移動体を制御するための新規投入命令を受領するステップと、（ロ）移動体を制御するための既投入命令列のうち現在実行中の命令を監視するステップと、（ハ）新規投入命令が現在実行中の命令に干渉するか判別するステップと、（ニ）現在実行中の命令に干渉すると判別された場合、新規投入命令に応じて移動体の現在行動を修正するステップと、（ホ）修正した行動後の移動体の現在位置座標、及び現在実行中の命令に干渉すると判別された際の移動体の初期位置座標を取得するステップと、（ヘ）新規投入命令が移動経路を指定している場合、現在位置座標から初期位置座標に移動する補完命令を生成し、補完命令を新規投入命令前に挿入するステップと、（チ）補完命令及び新規投入命令を基本的命令列に分解するステップと、（リ）基本的命令列を順次実行し移動体を制御するステップとを含む制御方法が提供される。
【発明の効果】
【０００７】
本発明によれば、割り込み命令が投入された際に適切な移動行動の変更や経路修正を行うことができる制御装置及び制御方法を提供することができる。
【発明を実施するための最良の形態】
【０００８】
次に、図面を参照して、本発明の実施の形態を説明する。以下の図面の記載において、同一又は類似の部分には同一又は類似の符号を付している。ただし、図面は模式的なものである。
【０００９】
また、以下に示す実施の形態は、この発明の技術的思想を具体化するための装置や方法を例示するものであって、この発明の技術的思想を下記のものに特定するものでない。この発明の技術的思想は、特許請求の範囲において、種々の変更を加えることができる。
【００１０】
（制御装置）
本発明の実施の形態に係る制御装置は、図１に示すように、座標取得部３３、中央演算処理装置（ＣＰＵ）１、記憶装置２及び移動体制御部（アクチュエータ）６０を備える。座標取得部３３は、移動体の現在座標、併進方向速度、及び回転方向速度等を取得する。座標取得部３３としては、オドメトリデータを取得するセンサ、全地球測位システム（ＧＰＳ）又は環境カメラ等が使用可能である。アクチュエータ６０は、例えば移動体の車輪に取り付けられ、移動体の移動を制御する。
【００１１】
ＣＰＵ１は、状況把握部３０、命令受領判別部１０、経路生成部２０、命令列管理部４０及び命令実行部５０を備える。状況把握部３０は、命令実行部５０において現在実行中の命令等を監視する命令監視部３１と、座標取得部３３や命令実行部５０により取得された現在座標、併進方向速度及び回転方向速度等を管理する座標管理部３２を備える。
【００１２】
命令受領判別部１０は、命令受領部１１、命令判別部１２及びタイミング判別部１３を備える。命令受領部１１は、外部から移動体の目標位置座標の変更や移動経路の変更等を行う新規投入命令を任意のタイミングで受領する。新規投入命令は、図２及び図３にそれぞれ示すようなシステム命令又は行動命令を有する。新規投入命令には、割り込み命令である旨又は順次発動すべき行動である旨、割り込み命令であればその投入タイミングを指定した値、及び緊急度等の情報が付与されている。
【００１３】
命令判別部１２は、命令受領部１１から新規投入命令を受領すると、新規投入命令の情報、命令監視部３１から取得された現在実行中の命令、座標管理部３２から取得された現在座標、併進方向速度及び回転方向速度等に基づいて、新規投入命令の緊急度の度合い、新規投入命令が現在実行中の命令に対する割り込み命令であるか否か、新規投入命令が座標設定ならば現在座標と異なるか否か等を判別することにより、新規投入命令が現在実行中の命令や現在実行中の命令を分解した基本的命令に干渉するか判別する。なお、緊急度の度合いを判断するための閾値は、制御装置の性能や状況等により適宜設定され、記憶装置２等に予め記憶しておけば良い。
【００１４】
更に、命令判別部１２は、命令受領部１１から受領した新規投入命令の情報、命令監視部３１から取得された現在実行中の命令、及び座標管理部３２から取得された現在座標、併進方向速度及び回転方向速度等に基づいて、新規投入命令の緊急度や、新規投入命令が緊急停止する命令であるか否か、新規投入命令が停止命令であるか否か等を判別することにより、現在行動を緊急又は安全に停止する必要があるか判別する。
【００１５】
タイミング判別部１３は、命令受領部１１から新規投入命令を受領すると、その新規投入命令内の投入タイミングを指定した値と、命令列管理部４０により管理されている命令実行部５０に既に投入された既投入命令を比較し、既投入命令列のうち新規投入命令が挿入されるタイミングを判別する。
【００１６】
経路生成部２０は、補完命令生成部２１、修正部２２及び分解部２３を備える。分解部２３は、新規投入命令が挿入されるタイミングと、新規投入命令を受領すると、命令列管理部４０から取得された新規投入命令が挿入される直前の既投入命令の終了時の予定される座標姿勢と速度に基づいて、命令実行部５０において実行できる図４に示すような基本的命令の列（基本的命令列）に分解する。
【００１７】
図５は、既投入命令Ａ１，Ａ２を含む既投入命令の一例を示す。既投入命令Ａ１は、５ｋｍ／ｓの速度で２ｍの直線行動をする命令である。既投入命令Ａ２は、９０度その場回転移動をする命令である。図５に示した既投入命令Ａ１，Ａ２は、分解部２３により図６に示すように基本的命令Ｂ１，Ｂ２，Ｂ３の基本的命令列に分解され、命令実行部５０に投入されている。基本的命令Ｂ１は、５ｋｍ／ｓの速度で１ｍの直線移動をする命令である。基本的命令Ｂ２は、１ｍ直線移動をした後に停止する停止条件付き命令である。基本的命令Ｂ３は、９０度その場回転移動をする命令である。
【００１８】
図７は、１０ｋｍ／ｓの速度で相対座標移動を行う新規投入命令Ａ３を一例として示す。タイミング判別部１３は、図８に示すように、新規投入命令Ａ３が、既投入命令Ａ１と既投入命令Ａ２の間に挿入されると判別する。分解部２３は、新規投入命令Ａ３を、図９に示すように基本的命令Ｂ４，Ｂ５，Ｂ６の基本的命令列に分解する。基本的命令Ｂ４は、１０ｋｍ／ｓの速度で３ｍの直線移動をする命令である。基本的命令Ｂ５は、２ｍ、９０度の円弧軌道移動をする命令である。基本的命令Ｂ６は、１０ｋｍ／ｓの速度で３ｍの直線移動をする命令である。
【００１９】
補完命令生成部２１は、新規投入命令と新規投入命令が挿入される直前の既投入命令との干渉を解消する直前命令干渉解消部１００、新規投入命令と新規投入命令が挿入される直後の既投入命令との干渉を解消する直後命令干渉解消部１０１、及び新規投入命令と現在実行中の命令との干渉を解消する現在命令干渉解消部１０２を備える。
【００２０】
直前命令干渉解消部１００は、タイミング判別部１３により判別された新規投入命令が挿入されるタイミングと、新規投入命令の情報と、新規投入命令を分解した基本的命令列を受領すると、命令列管理部４０から取得された新規投入命令が挿入される直前の既投入命令とその直前の既投入命令を分解した基本的命令列に基づいて、新規投入命令が直前の既投入命令と干渉するか判別する。更に、直前命令干渉解消部１００は、新規投入命令が直前の既投入命令と干渉すると判別された場合、新規投入命令を分解した基本的命令列及び新規投入命令が挿入される直前の既投入命令を修正・補完する。
【００２１】
図９においては、新規投入命令Ａ３を分解した最初の基本的命令Ｂ４が直進行動をとる命令であり、その直前の基本的命令Ｂ２が停止命令付きの命令であるため互いに干渉する。そこで、直前命令干渉解消部１００は、直前の基本的命令Ｂ２とその直前の基本的命令Ｂ１とを、図１０に示すように５ｋｍ／ｓの速度で２ｍの直線行動をする基本的命令Ｂ７に修正する。更に、新規投入命令Ａ３を分解した最後の基本的命令Ｂ６が１０ｋｍ／ｓの時速で直線行動をする命令であり、その直後の基本的命令Ｂ３が９０度のその場回転移動の命令であるので、互いに干渉する。そこで、直前命令干渉解消部１００は、新規投入命令Ａ３を分解した最後の基本的命令Ｂ６を、図１１に示すように停止命令付きの基本的命令Ｂ８に修正する。
【００２２】
直後命令干渉解消部１０１は、タイミング判別部１３により判別された新規投入命令が挿入されるタイミングと、新規投入命令の情報と、新規投入命令を分解した基本的命令列を受領すると、新規投入命令が移動命令であるか否か、又は座標修正を伴う命令であるか否か等を判別することにより、新規投入命令と、新規投入命令が挿入される以降の既投入命令とが干渉するか判別する。更に、直後命令干渉解消部１０１は、新規投入命令と新規投入命令が挿入される以降の既投入命令とが干渉すると判別された場合、新規投入命令が挿入される以降の既投入命令を一度削除すると共に、新規投入命令を分解した最後の基本的命令に応じた命令を再度生成する。
【００２３】
現在命令干渉解消部１０２は、命令判別部１２により新規投入命令が現在実行中の命令に干渉すると判別された場合、必要に応じて新規投入命令を修正したり、補完命令を生成したりする。
【００２４】
修正部２２は、修正部２２は、現在行動を緊急又は安全に停止する必要があると判別された場合、現在実行中の命令を破棄し、移動体の現在行動を修正する命令を生成する。
【００２５】
命令列管理部４０は、経路生成部２０から受領した基本的命令列を管理し、基本的命令を命令実行部５０に逐次投入する。ここで、命令列管理部４０は、経路生成部２０から受領した基本的命令列の緊急度の度合いに応じて、緊急度が高い場合には即座に命令実行部５０に基本的命令を投入する。緊急度が低い場合には、命令列管理部４０は、命令実行部５０の状態と移動体の現在状態に応じて、投入する命令数を調整し、予め設定している命令数又は予め設定している命令終了にかかる時間がほぼ一定を保つように、命令実行部５０に基本的命令を投入する。なお、緊急度の度合いを判断するための閾値は、制御装置の性能や状況等により適宜設定され、記憶装置２等に予め記憶しておけば良い。
【００２６】
命令実行部５０は、命令列管理部４０から投入された基本的命令を挿入した既投入命令列を順次実行し、アクチュエータ６０を駆動し、移動体の移動を制御する。
【００２７】
記憶装置２は、命令列管理部４０により管理されている新規投入命令が分解された基本的命令列及び既投入命令が分解された基本的命令列や、座標管理部３２により管理されている現在座標等を記憶する。記憶装置２としては、例えばメモリ、磁気ディスク又は光ディスク等が採用可能である。メモリは、ＣＰＵ１において実行されるプログラムを格納しているプログラム記憶装置２等として機能させることが可能である（プログラムの詳細は後述する。）。メモリは、ＣＰＵ１におけるプログラム実行処理中に利用されるデータ等を一時的に格納したり、作業領域として利用される一時的なデータメモリ等として機能させることも可能である。
【００２８】
（制御方法）
次に、本発明の実施の形態に係る制御方法の一例を、図１２のフローチャートを参照しながら説明する。
【００２９】
（イ）ステップＳ１００において、命令受領部１１は、外部から新規投入命令を受領し、受領した新規投入命令を命令判別部１２に伝達する。ステップＳ１０１において、命令判別部１２は、命令受領部１１から新規投入命令を受領すると、命令監視部３１から取得された現在実行中の命令、座標管理部３２から取得された現在座標、併進方向速度及び回転方向速度等に基づいて、新規投入命令の緊急度の度合い、新規投入命令が現在実行中の命令に対する割り込み命令であるか否か、新規投入命令が座標設定ならば現在座標と異なるか否か等を判別することにより、新規投入命令が現在実行中の命令に干渉するか判別する。新規投入命令が現在実行中の命令に干渉しない場合、ステップＳ１０２に進む。
【００３０】
（ロ）ステップＳ１０２において、タイミング判別部１３が、命令判別部１２から新規投入命令を受領すると、新規投入命令内の投入タイミングを指定した値と、命令列管理部４０により管理されている既投入命令列を比較し、既投入命令列に新規投入命令が挿入されるタイミングを判別する。
【００３１】
（ハ）ステップＳ１０３において、分解部２３が、命令列管理部４０から取得された、新規投入命令が挿入されるタイミング直前の既投入命令の終了時の予定される座標姿勢と速度に基づいて、新規投入命令を命令実行部５０が実行できるように基本的命令列に分解する。
【００３２】
（ニ）ステップＳ１０４において、直前命令干渉解消部１００が、新規投入命令が、新規投入命令が挿入される直前の既投入命令に干渉するか判別する。新規投入命令が挿入される直前の既投入命令に干渉すると判別された場合、ステップＳ１１２に進み、直前命令干渉解消部１００が、新規投入命令が挿入される直前の既投入命令を分解した基本的命令を修正する。例えば、図９に示した新規投入命令を分解した最初の基本的命令Ｂ４が挿入される直前の既投入命令を分解した基本的命令Ｂ１，Ｂ２が、図１０に示すように基本的命令Ｂ７に修正される。
【００３３】
（ホ）Ｓ１０５において、直前命令干渉解消部１００が、新規投入命令が挿入される直前の既投入命令、及び新規投入命令が挿入される直後の既投入命令に応じて、新規投入命令を分解した基本的命令を修正する。例えば、図１０に示した新規投入命令が挿入される直後の既投入命令を分解した基本的命令Ｂ３に応じて、新規投入命令Ａ３を分解した最後の基本的命令Ｂ６を、図１１に示すように基本的命令Ｂ８に修正する。
【００３４】
（ヘ）ステップＳ１０６において、直後命令干渉解消部１０１が、新規投入命令が移動命令であるか、座標修正を伴うか等を判別することにより、新規投入命令が直後の既投入命令と干渉するか否か判別する。新規投入命令が直後の既投入命令と干渉する場合、ステップＳ１１３において、新規投入命令の挿入される以降の既投入命令を一度削除すると共に、新たな新規投入命令を順次再投入する。
【００３５】
（ト）ステップＳ１０７において、命令列管理部４０が、経路生成部２０から新規投入命令が分解され、必要に応じて修正、補完又は再投入された基本的命令列を受領すると、各基本的命令の終了時点における予定される速度と移動体の位置座標を算出し、基本的命令列とともに記憶装置２に記憶させる。
【００３６】
（チ）ステップＳ１０８において、命令列管理部４０が、新規投入命令が挿入されるタイミングに基づいて、経路生成部２０から受領した新規投入命令を分解した基本的命令列を、命令実行部５０に投入する。ここで、命令列管理部４０が、基本的命令の緊急度の度合いに応じ、緊急度が高い場合には即座に、緊急度が低い場合には、命令実行部５０の状態と現在状態に応じ、投入する命令数を調整し、予め設定している命令数又は予め設定している命令終了にかかる時間がほぼ一定を保つように、命令実行部５０に基本的命令を投入する。命令実行部５０は、命令列管理部４０から投入された新規投入命令を分解した基本的命令列含む既投入命令列を順次実行し、アクチュエータ６０を駆動することにより移動体を制御する。
【００３７】
（リ）一方、ステップＳ１０１において新規投入命令が現在実行中の命令に干渉すると判別された場合、ステップＳ１１１に進み、命令判別部１２は、命令監視部３１から取得された現在実行中の命令、座標管理部３２から取得された現在座標、併進方向速度及び回転方向速度等に基づいて、新規投入命令の緊急度が高いか、新規投入命令が現在実行中の命令に対する割り込み命令であるか否か、新規投入命令が座標設定ならば現在座標と異なるか否か等を判別することにより、新規投入命令が現在実行中の基本的な命令に干渉するか判別する。新規投入命令が現在実行中の基本的な命令に干渉しない場合、ステップＳ１１４に進み、分解部２３が、新規投入命令を命令実行部５０が実行できるように基本的命令列に分解し、ステップＳ１０７に進む。
【００３８】
次に、ステップＳ１１１において現在実行中の基本的命令に干渉すると判別された場合の対処方法を、図１３のフローチャートを参照しながら説明する。
【００３９】
（イ）ステップＳ２００において、座標管理部３２が、座標取得部３３により取得された初期位置座標Ａを、記憶装置２に記憶させる。
【００４０】
（ロ）ステップＳ２０１において、命令判別部１２が、新規投入命令の緊急度や、新規投入命令が緊急停止の命令であるか否か等を判別することにより、移動体の現在行動を緊急に停止する必要があるか判別する。移動体の現在行動を緊急に停止する必要があると判別された場合、ステップＳ２０２に進み、修正部２２が、現在実行中の命令を破棄し、最大減速で停止する命令を最大の緊急度を付与して、命令列管理部４０に投入し、命令実行部５０に停止命令を実行させる。一方、ステップＳ２０１において移動体の現在行動を緊急に停止させる必要がないと判断された場合、ステップＳ２１０に進む。
【００４１】
（ハ）ステップＳ２１０において、命令判別部１２が、新規投入命令の緊急度や、新規投入命令が停止命令であるか否か等を判別することにより、移動体の現在行動を安全に停止させる必要があるかを判別する。移動体の現在行動を安全に停止させる必要があると判断された場合、ステップＳ２１１に進み、修正部２２が、現在実行中の命令を破棄し、移動体の現在の速度に応じて安全に停止できる距離又は時間で速度を０にする命令に高い緊急度を付与して、命令列管理部４０に投入し、命令実行部５０に停止命令を実行させる。なお、安全に停止できる距離又は時間は制御装置の性能や状況等により適宜設定される。一方、ステップＳ２１０において移動体の現在行動を安全に停止させる必要がないと判別された場合、ステップＳ２０３に進む。
【００４２】
（ニ）ステップＳ２０３において、現在命令干渉解消部１０２が、座標管理部３２に管理されている初期位置座標Ａと新規投入命令から、新規投入命令の目標位置座標Ｂを算出する。
【００４３】
（ホ）ステップＳ２０４において、現在命令干渉解消部１０２が、新規投入命令が行動開始姿勢を指定する等、移動経路を指定しているか判別する。移動経路を指定していないと判別された場合、ステップＳ２０５に進み、現在命令干渉解消部１０２が、新規投入命令を、移動体の現在行動を修正後（停止後）の現在位置座標Ｃから目標位置座標Ｂに移動する命令に置き換える。分解部２３が、置き換えた新規投入命令を基本的命令に分解する。
【００４４】
（ヘ）一方、ステップＳ２０４において、移動経路を指定していると判別された場合、ステップＳ２１２に進み、現在命令干渉解消部１０２が、新規投入命令前に挿入する、現在位置座標Ｃから初期位置座標Ａへ移動する補完命令を生成する。Ｓ２１３において、現在命令干渉解消部１０２が、新規投入命令を、初期位置座標Ａから目標位置座標Ｂへ移動する命令に修正する。分解部２３が、現在位置座標Ｃから初期位置座標Ａへ移動する補完命令、及び初期位置座標Ａから目標位置座標Ｂへ移動する新規投入命令を基本的命令列に分解する。
【００４５】
（ト）ステップＳ２０６において、命令列管理部４０が、分解部２３により分解された基本的命令列から各命令列終了時点における予定される速度と移動体の位置座標を算出し、基本的命令列とともに記憶装置２に記憶させる。ステップＳ２０７において、命令列管理部４０が、基本的命令の緊急度に応じ、緊急度が高い場合には即座に、緊急度が低い場合には、命令実行部５０の状態と現在状態に応じ、投入する命令数を調整し、予め設定している命令数又は予め設定している命令終了にかかる時間がほぼ一定を保つように、命令実行部５０に基本的命令を投入する。
【００４６】
次に、ステップＳ１０４、Ｓ１１２、Ｓ１０５における新規投入命令が挿入される直前の既投入命令が干渉する場合の対処方法の一例を、図１４のフローチャートを参照しながら説明する。
【００４７】
（イ）ステップＳ３００において、直前命令干渉解消部１００が、新規投入命令が挿入されるタイミング、新規投入命令の情報、及び新規投入命令を分解した基本的命令列を受領すると、命令列管理部４０から取得された新規投入命令が挿入される直前の既投入命令とその直前の既投入命令を分解した基本的命令列に基づいて、新規投入命令が挿入される直前の既投入命令が停止命令付きでなく、新規投入命令が挿入される直前の既投入命令を分解した基本的命令列が停止で終了しているか判別する。新規投入命令が挿入される直前の既投入命令が停止命令付きであるか、新規投入命令が挿入される直前の既投入命令を分解した基本的命令列が停止で終了していると判別された場合、ステップＳ３１０に進み、新規投入命令を分解した最初の基本的命令に加速を付加する。一方、ステップＳ３００において新規投入命令が挿入される直前の既投入命令が停止命令付きでないか、又は新規投入命令が挿入される直前の既投入命令を分解した基本的命令列が停止で終了している場合、ステップＳ３０１に進む。
【００４８】
（ロ）ステップＳ３０１において、直前命令干渉解消部１００が、新規投入命令が挿入される直前の既投入命令を分解した最後の基本的命令に並進方向成分があるかどうか判別する。新規投入命令が挿入される直前の既投入命令を分解した最後の基本的命令に並進方向成分があると判別された場合、ステップＳ３２１に進み、新規投入命令が挿入される直前の既投入命令を分解した最後の基本的命令に並進方向成分がないと判別された場合、ステップＳ３０２に進む。ステップＳ３０２において、直前命令干渉解消部１００が、新規投入命令を分解した最初の基本的命令に並進方向成分があるか判別する。新規投入命令を分解した最初の基本的命令に並進方向成分があると判別された場合、ステップＳ３２２に進み、新規投入命令を分解した最初の基本的命令に並進方向成分がないと判別された場合、ステップＳ３０３に進む。ステップＳ３０３において、直前命令干渉解消部１００が、新規投入命令を分解した最初の基本的命令と、新規投入命令が挿入される直前の既投入命令を分解した最後の基本的命令の回転方向が同じか判別する。回転方向が同じではないと判別された場合、ステップＳ３２２に進み、回転方向が同じと判別された場合、ステップＳ３０４に進む。
【００４９】
（ハ）ステップＳ３０４において、直前命令干渉解消部１００が、新規投入命令が挿入される直前の既投入命令を分解した最後の基本的命令に対して、移動経路を保持しつつ、新規投入命令の希望速度に合わせるように加速又は減速を付加する。同時に、新規投入命令が挿入される直後の既投入命令の最初の基本的命令の希望速度に合わせるように、新規投入命令の最後の基本的命令に加速又は減速を付加する。
【００５０】
（ニ）ステップＳ３２１において、直前命令干渉解消部１００が、新規投入命令を分解した最初の基本的命令に並進方向成分があるか判別する。新規投入命令を分解した最初の基本的命令に並進方向成分があると判別された場合、ステップＳ３３０に進み、新規投入命令を分解した最初の基本的命令に並進方向成分がない場合、ステップＳ３２２に進む。
【００５１】
（ホ）ステップＳ３２２において、直前命令干渉解消部１００が、新規投入命令が挿入される直前の既投入命令の最後の基本的命令に対して、移動経路を保持しつつ、最終的に速度を０にするような命令に修正する。Ｓ３２３において、直前命令干渉解消部１００が、新規投入命令を分解した最初の基本的命令を加速する命令に修正する。
【００５２】
（ヘ）ステップＳ３３０において、直前命令干渉解消部１００が、新規投入命令が挿入される直前の既投入命令が目標位置座標（最終座標姿勢）を厳密に指定しているか判別する。新規投入命令が挿入される直前の既投入命令が目標位置座標を厳密に指定していない場合、ステップＳ３３１に進み、新規投入命令が挿入される直前の既投入命令が目標位置座標を厳密に指定している場合、ステップＳ３４０に進む。
【００５３】
（ト）ステップＳ３４０において、直前命令干渉解消部１００が、新規投入命令の基本的命令と新規投入命令が挿入される直前の既投入命令の最後の基本的命令に対し、新規投入命令を分解した最初の基本的命令の希望速度に合わせるように、それぞれの移動経路（軌道）は保持しつつ、加減速を付加する。
【００５４】
（ヌ）ステップＳ３３１において、直前命令干渉解消部１００が、新規投入命令を分解した最初の基本的命令と、新規投入命令が挿入される直前の既投入命令の最後の基本的命令の速度ベクトルより、それぞれの基本的命令の移動距離を短くして、曲線軌道や停止やその場回転や直進軌道で補完命令を生成し、移動経路を繋ぐ。Ｓ３３２において、直前命令干渉解消部１００が、曲線軌道の最小Ｒに応じて新規投入命令が挿入される直前の既投入命令の速度に制限を付加する。Ｓ３３３において、直前命令干渉解消部１００が、ステップＳ３３１で算出された曲線軌道、直線軌道、その場回転軌道、停止と命令列管理部４０から取得できる新規投入命令が挿入される直前の既投入命令の目標位置座標及び速度と、新規投入命令の希望速度に応じて、補完した経路の速度を決定する。
【００５５】
以上説明したように、本発明の実施の形態によれば、任意タイミングで挿入された新規投入命令に対し、移動体の状況、新規投入命令の緊急度、現在実行中の命令や新規投入命令の挿入される前後の既投入命令から想定される行動における座標や速度ベクトルに基づいて新規投入命令及び既投入命令を一部修正するとともに、補完命令を生成し一連の行動を修正する。これにより、新規投入命令が投入された際に適切な移動行動の変更や経路修正を行うことができる。
【００５６】
また、従来、障害物に対する反応速度を向上するために、障害物検知、詳細な経路生成と修正、現在地把握、車輪制御等の地図情報処理等の機能を分離せずに一つのモジュールで処理を行う必要があった。これに対して、本発明の実施の形態によれば、適切な移動行動の変更や経路修正を行うことができ、障害物に対する反応速度を向上することができるので、地図情報処理等の機能を他モジュールで共有すべき情報と分離することができ、モジュールの再利用性を高めることができる。
【００５７】
（プログラム）
又、図１２〜図１４に示した一連の手順は、図１２〜図１４と等価なアルゴリズムのプログラムにより、図１に示した制御装置を制御して実行出来る。このプログラムは、本発明の制御装置の図示を省略した記憶部に記憶させればよい。又、このプログラムは、コンピュータ読取り可能な記録媒体に保存し、この記録媒体を制御装置の記憶部に読み込ませることにより、本発明の実施の形態の一連の手順を実行することができる。ここで、「コンピュータ読取り可能な記録媒体」とは、例えばコンピュータの外部メモリ装置、半導体メモリ、磁気ディスク、光ディスク、光磁気ディスク、磁気テープ等のプログラムを記録することができるような媒体等を意味する。具体的には、フレキシブルディスク、ＣＤ−ＲＯＭ，ＭＯディスク等が「コンピュータ読取り可能な記録媒体」に含まれる。例えば、制御装置の本体は、フレキシブルディスク装置（フレキシブルディスクドライブ）及び光ディスク装置（光ディスクドライブ）を内蔵若しくは外部接続するように構成できる。
【００５８】
上記のように、本発明は実施の形態によって記載したが、この開示の一部をなす論述及び図面はこの発明を限定するものであると理解すべきではない。この開示から当業者には様々な代替実施の形態、実施例及び運用技術が明らかとなろう。本発明はここでは記載していない様々な実施の形態等を含むことは勿論である。したがって、本発明の技術的範囲は上記の説明から妥当な特許請求の範囲に係る発明特定事項によってのみ定められるものである。
【図面の簡単な説明】
【００５９】
【図１】本発明の実施の形態に係る制御装置の一例を示すブロック図である。
【図２】本発明の実施の形態に係るシステム関数のアプリケーション・プログラム・インターフェース（ＡＰＩ）の一例を示す表である。
【図３】本発明の実施の形態に係る行動関数のＡＰＩの一例を示す表である。
【図４】本発明の実施の形態に係る既投入命令の一例を示す概略図である。
【図５】本発明の実施の形態に係る新規投入命令の一例を示す概略図である。
【図６】本発明の実施の形態に係る新規投入命令が挿入されるタイミングを説明するための概略図である。
【図７】本発明の実施の形態に係る基本的命令の一例を示す表である。
【図８】本発明の実施の形態に係る既投入命令が分解された基本的命令列の一例を示す概略図である。
【図９】本発明の実施の形態に係る新規投入命令が分解された基本的命令列の一例を示す概略図である。
【図１０】本発明の実施の形態に係る新規投入命令が挿入される直前の既投入命令の修正方法の一例を示す概略図である。
【図１１】本発明の実施の形態に係る投入基本的命令の修正方法の一例を示す概略図である。
【図１２】本発明の実施の形態に係る制御方法の一例を説明するためのフローチャートである。
【図１３】本発明の実施の形態に係る新規投入命令が現在実行中の命令に干渉する際の対処フローチャートである。
【図１４】本発明の実施の形態に係る新規投入命令が挿入される直前の行動と干渉する際の対処フローチャートである。
【符号の説明】
【００６０】
１…中央演算処理装置（ＣＰＵ）
２…プログラム記憶装置
２…記憶装置
８…特開平
１０…命令受領判別部
１１…命令受領部
１２…命令判別部
１３…タイミング判別部
２０…経路生成部
２１…補完命令生成部
２２…修正部
２３…分解部
３０…状況把握部
３１…命令監視部
３２…座標管理部
３３…座標取得部
４０…命令列管理部
５０…命令実行部
６０…アクチュエータ
１００…直前命令干渉解消部
１０１…直後命令干渉解消部
１０２…現在命令干渉解消部
【図１】
2010102406.tif 000003
【図２】
2010102406.tif 000004
【図３】
2010102406.tif 000005
【図４】
2010102406.tif 000006
【図５】
2010102406.tif 000007
【図６】
2010102406.tif 000008
【図７】
2010102406.tif 000009
【図８】
2010102406.tif 000010
【図９】
2010102406.tif 000011
【図１０】
2010102406.tif 000012
【図１１】
2010102406.tif 000013
【図１２】
2010102406.tif 000014
【図１３】
2010102406.tif 000015
【図１４】
2010102406.tif 000016