(57)【要約】【課題】  簡単な操作で以って移動経路地図を作成することができ、さらに走行領域内で移動体が常に最短経路を選択することができる合理的な移動経路地図を作成する方法を提供する。【解決手段】  移動体の走行可能な経路を表す移動経路地図の作成方法において、前記移動体の手動走行による走行軌跡上の複数の節点と、隣接する節点同士を連結したパス経路とからなる初期経路を登録する第１のステップ（Ｓ１）と、任意の節点に近接する節点を削除して節点の減数化を行なう第２のステップ（Ｓ２）と、前記減数化を行なった初期経路に対して、曲折部を判別して端部を生成する曲折部処理（Ｓ３）と、前記パス経路の近傍に位置する節点をパス経路上に投影する節点投影処理（Ｓ４）と、交差するパス経路を判別して交点を生成する交点生成処理（Ｓ５）と、から少なくとも一の処理を行なう第３のステップと、を備える。【選択図】      図１
2005316760.tif 000002
【特許請求の範囲】
【請求項１】
移動体の走行可能な経路を表す移動経路地図の作成方法において、
前記移動体の手動走行による走行軌跡上の複数の節点と、隣接する節点同士を連結したパス経路とからなる初期経路を登録する第１のステップと、
任意の節点に近接する節点を削除して節点の減数化を行なう第２のステップと、
前記減数化を行なった初期経路に対して、曲折部を判別して端部を生成する曲折部処理と、前記パス経路の近傍に位置する節点をパス経路上に投影する節点投影処理と、交差するパス経路を判別して交点を生成する交点生成処理と、から少なくとも一の処理を行なう第３のステップと、を備えることを特徴とする移動経路地図作成方法。
【請求項２】
前記第１のステップにて、前記移動体の姿勢方向変化量に対する閾値と、前記移動体の姿勢方向を基準軸とし、該基準軸からのずれ量に対する閾値とを設定しておき、前記移動体の手動走行に伴い前記姿勢方向変化量及び前記ずれ量のうち少なくとも何れか一方が前記閾値を超えた場合に、その位置を節点として自動登録することを特徴とする請求項１記載の移動経路地図作成方法。
【請求項３】
前記第１のステップにて、前記節点を登録する際に、対象節点の付随情報を該対象節点の位置情報に関連付けて登録することを特徴とする請求項１記載の移動経路地図作成方法。
【請求項４】
前記第２のステップにおける曲折部処理は、対象節点を挟む２のパス経路を表すベクトルを正規化し、これらのベクトルの内積が閾値以内である場合に曲折部と判断し、
前記対象節点の前後に位置する節点のうち、該対象節点からの距離が短い方の節点を、他の節点と前記対象節点間のパス経路上に投影し、前記対象節点と投影節点間のパス経路を削除して、端部を生成することを特徴とする請求項１記載の移動経路地図作成方法。
【請求項５】
前記第２のステップにおける節点投影処理は、任意の節点とこれに最も近接するパス経路との距離が閾値以下で、かつ前記節点における前記移動体の姿勢方向と、前記近接するパス経路との傾きの差が一定の閾値以下である場合に、前記節点を処理対象と判断し、
対象節点を通り前記近接するパス経路に直交する直線と、該近接するパス経路との交点に、前記対象節点を投影することを特徴とする請求項１記載の移動経路地図作成方法。
【請求項６】
請求項５記載の節点投影処理にて、前記対象節点に隣接する節点が、前記近接するパス経路若しくはこれに連結する他のパス経路上に投影されている場合には、前記対象節点の投影後に、該対象節点と前記隣接節点間のパス経路を削除し、
前記隣接節点が、前記近接するパス経路若しくはこれに連結する他のパス経路上に投影されていない場合には、前記対象節点と前記隣接節点間のパス経路を残存させることを特徴とする移動経路地図作成方法。
【請求項７】
前記第２のステップにおける交点生成処理は、２のパス経路が交差する位置に交点となる節点を生成した後に、該交点と、前記２のパス経路の両端に位置する各既設節点との距離を夫々導出し、
該距離が閾値以下である既設節点が存在する場合には、前記生成した交点を削除するとともに前記既設節点を交点と見なし、該交点と見なした既設節点を含む第１のパス経路を残存し、これに交差する第２のパス経路を削除した後、
前記交点と見なした既設節点を一端とし、前記第２のパス経路を代替する第３のパス経路を生成することを特徴とする請求項１記載の移動経路地図作成方法。
【請求項８】
前記作成した移動経路地図に対して、前記節点及び前記パス経路の削除、追加処理を含む編集処理を行なうことを特徴とする請求項１記載の移動経路地図作成方法。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、自律走行する移動体に対して、走行領域内の移動可能経路が示された移動経路地図を作成する移動経路地図作成方法に関する。
【背景技術】
【０００２】
自律的に行動する移動体に関する技術は種々提案されており、またこのような移動体は各種工業にて広く利用されている。さらに近年においては、一般家庭や各種施設等においても移動体の導入が要望されており、あらゆる動作環境に対応できる移動体が必要とされている。
壁や家具等の障害物が存在する環境下にて、移動体を円滑に自律走行させるには、到達目的地に辿り着くまでの移動経路を予め移動体に教示する必要がある。
従来の一般的な移動経路の教示方法としては、例えば図１５に示されるような移動体の屋内走行環境における間取り図５０から、ユーザが絶対座標系（グローバル座標系）を基準として、任意の位置におけるXY座標を測定して、これを節点（ノード）５２ａ、５２ｂ、…、５２ｈ、５２ｉ、…というように手入力で登録し、さらに、目的地５１に辿り着くまでの移動経路をユーザが計画して、該計画した移動経路上に節点を追加しながら、実際の走行経路として繋ぐという方法が用いられている。
【０００３】
しかし、上記した方法では移動経路の事前設定に非常に手間がかかるため、移動経路地図を簡単に設定する方法が提案されている。
特許文献１（特開平１１−２５９６５９号公報）には、移動体に搭載した撮像手段を使って、移動方向の上下左右の画像を取得してリアルタイムで環境地図を作成する方法が開示されている。
また、特許文献２（特開平７−１１０７０９号公報）では、手動走行モードでジョイスティックを使って走行経路を自動生成させる方法を提案している。
さらに、特許文献３（特開２０００−２５４８７９号公報）では、ロボット先端がティーチングポイントを通過するように制御するため、一旦教示した各ティーチングポイントについて、経路の中点に動作ポイントの候補点を設定してロボットの軌道が必ず当該候補点を通過するように制御する方法が示されている。
【０００４】
【特許文献１】特開平１１−２５９６５９号公報
【特許文献２】特開平７−１１０７０９号公報
【特許文献３】特開２０００−２５４８７９号公報
【発明の開示】
【発明が解決しようとする課題】
【０００５】
しかしながら、特許文献１では、周囲の照明状態などによって画像の認識精度が悪くなる。また画像処理を用いるため、ＣＰＵへの計算負荷も高く、多数の画像を用いた場合に経路作成に時間が掛かりリアルタイムの操作が行えないという問題がある。
さらに、特許文献２を含め上記従来の方法では、教示した経路は節点と経路を単純に繋いだものであり、教示した経路や節点でほぼ同一と思われる節点や経路をマージ（融合）したり、経路同士の交点を判断して新たに節点を追加し、経路の再統合を行なうといった処理が行われていない。これでは移動体が目的地に移動する場合、最短となる経路を選択することができないため、移動体における地図作成方法として不十分なものであった。
従って、本発明は上記従来技術の問題点に鑑み、計算負荷が小さく、簡単な操作で以って移動経路地図を作成することができ、さらには走行領域内で移動体が常に最短経路を選択することができる合理的な移動経路地図を作成する方法を提供することを目的とする。
【課題を解決するための手段】
【０００６】
そこで、本発明はかかる課題を解決するために、
移動体の走行可能な経路を表す移動経路地図の作成方法において、
前記移動体の手動走行による走行軌跡上の複数の節点と、隣接する節点同士を連結したパス経路とからなる初期経路を登録する第１のステップと、
任意の節点に近接する節点を削除して節点の減数化を行なう第２のステップと、
前記減数化を行なった初期経路に対して、曲折部を判別して端部を生成する曲折部処理と、前記パス経路の近傍に位置する節点をパス経路上に投影する節点投影処理と、交差するパス経路を判別して交点を生成する交点生成処理と、から少なくとも一の処理を行なう第３のステップと、を備えることを特徴とする。
【０００７】
このように、本発明では手動走行により作成した初期経路を元に移動経路地図を作成しているため、計算負荷が小さく、簡単な操作で以って移動経路地図を作成することができる。
また、減数化処理、折曲部処理、節点投影処理、交点処理等を行なうことにより、前記初期経路が合理化され、移動体が自律走行する際の経路選択自由度が広がり、目的地まで移動する最短経路を選択することができるようになる。
さらに、前記移動経路地図は、手動走行により作成した初期経路から、節点数、パス経路数が大幅に減少しているため、使用するメモリ量が少なく、メモリを節約できる。
【０００８】
また、前記第１のステップにて、前記移動体の姿勢方向変化量に対する閾値と、前記移動体の姿勢方向を基準軸とし、該基準軸からのずれ量に対する閾値とを設定しておき、前記移動体の手動走行に伴い前記姿勢方向変化量及び前記ずれ量のうち少なくとも何れか一方が前記閾値を超えた場合に、その位置を節点として自動登録することを特徴とする。
本発明によれば、節点生成の閾値を変化させることで適当な間隔で節点を生成できるようになり、適用範囲が広がる。また、姿勢方向変化に対する閾値を設けることで、例えば、廊下から側面ドア入口への侵入等の折れ曲がり部のように、急激な姿勢変化が生じる箇所で必ず節点が生成できるようになる。
【０００９】
さらに、前記第１のステップにて、前記節点を登録する際に、対象節点の付随情報を該節点の位置情報に関連付けて登録することが好適である。
これは、前記節点に対して、例えば節点の名称を位置情報に関連付けて登録することにより、利用者が名称を指定するだけで移動体が該名称に対応する位置を認識するようになる。このように、前記付随情報を関連付けて登録することにより、移動体がより一層知能的に振舞えるようになる。
【００１０】
また、前記第２のステップにおける夫々の処理に対して、以下のような処理を行なうことにより、冗長な初期経路を、合理化された移動経路に簡単に変換することができる。
前記第２のステップにおける曲折部処理は、対象節点を挟む２のパス経路を表すベクトルを正規化し、これらのベクトルの内積が閾値以内である場合に曲折部と判断し、
前記対象節点の前後に位置する節点のうち、該対象節点からの距離が短い方の節点を、他の節点と前記対象節点間のパス経路上に投影し、前記対象節点と投影節点間のパス経路を削除して、端部を生成することを特徴とする。
【００１１】
また、前記第２のステップにおける節点投影処理は、任意の節点とこれに最も近接するパス経路との距離が閾値以下で、かつ前記節点における前記移動体の姿勢方向と、前記近接するパス経路との傾きの差が一定の閾値以下である場合に、前記節点を処理対象と判断し、
対象節点を通り前記近接するパス経路に直交する直線と、該近接するパス経路との交点に、前記対象節点を投影することを特徴とする。
さらに、前記節点投影処理にて、前記対象節点に隣接する節点が、前記近接するパス経路若しくはこれに連結する他のパス経路上に投影されている場合には、前記対象節点の投影後に、該対象節点と前記隣接節点間のパス経路を削除し、
前記隣接節点が、前記近接するパス経路若しくはこれに連結する他のパス経路上に投影されていない場合には、前記対象節点と前記隣接節点間のパス経路を残存させることが好ましい。
【００１２】
また、前記第２のステップにおける交点生成処理は、２のパス経路が交差する位置に交点となる節点を生成した後に、該交点と、前記２のパス経路の両端に位置する各既設節点との距離を夫々導出し、
該距離が閾値以下である既設節点が存在する場合には、前記生成した交点を削除するとともに前記既設節点を交点と見なし、該交点と見なした既設節点を含む第１のパス経路を残存し、これに交差する第２のパス経路を削除した後、
前記交点と見なした既設節点を一端とし、前記第２のパス経路を代替する第３のパス経路を生成することを特徴とする。
【００１３】
さらにまた、前記作成した移動経路地図に対して、前記節点及び前記パス経路の削除、追加処理を含む編集処理を行なうことが好適である。
本発明は、例えば前記節点、前記パス経路の削除、追加処理、または別個に作成した移動経路地図の融合等の編集機能を持たせることにより、個々の利用者が移動経路地図をカスタマイズでき、利用者の要望に沿った移動経路地図を提供することができる。
【発明の効果】
【００１４】
以上記載のごとく本発明によれば、手動走行により作成した初期経路を元に移動経路地図を作成するため、計算負荷が小さく、簡単な操作で以って移動経路地図を作成することができる。
また、折曲部処理、節点投影処理、交点処理等を行なうことにより、前記初期経路が合理化され、移動体が自律走行する際の経路選択自由度が広がり、目的地まで移動する最短経路を選択することができるようになる。
【発明を実施するための最良の形態】
【００１５】
以下、図面を参照して本発明の好適な実施例を例示的に詳しく説明する。但しこの実施例に記載されている構成部品の寸法、材質、形状、その相対的配置等は特に特定的な記載がない限りは、この発明の範囲をそれに限定する趣旨ではなく、単なる説明例に過ぎない。
本実施例に示す移動体の走行空間としては、一般家庭、各種施設、工場内等が挙げられるが、特に前記移動体は一般家庭内にてユーザの生活を補助、支援、介護するロボットであることが好適である。
【実施例１】
【００１６】
図１は本発明の実施例に係る移動経路地図作成アルゴリズムを示すフロー、図２は本発明の実施例に係る移動体を示す概略構成図である。
まず、図２により本実施例の移動体の概略構成を説明する。図２に示すように本実施例に係る移動体１０は、本体１１と、該本体１１からの各種指令に基づき図示しない駆動装置により走行を補助する左右一対の駆動輪１２と、前記移動体１０に指令を入力するマイク、キーボード、リモコン等の入力手段１３と、該移動体１０からの音声出力、画面出力等を行なうスピーカー、モニター等の出力手段１４と、移動経路地図を作成するための各種プログラムが格納された制御部２０と、を具備している。
【００１７】
前記制御部２０は、前記駆動輪１２の回転を検出するエンコーダにより移動体１０の移動距離、姿勢方位変化量を検出するオドメトリデータ検出部２１、グローバル座標系を基準として節点、パス経路を生成して初期経路を作成する節点，パス経路作成部２２、該生成した節点の減数化を行なう減数化処理部２３、前記初期経路の曲折部を抽出して端部を生成する曲折部処理部２４、交差する２のパス経路を判別して交点を生成する交点生成処理部２５、パス経路に近接する節点を該パス経路に投影する節点投影処理部２６、前記初期経路、グローバル座標系、作成した移動経路地図等を格納する記憶部２７、前記移動経路地図に融合、修正等の編集処理を行う編集機能２８、を有している。
【００１８】
次に、図１のフローを参照して移動経路地図作成アルゴリズムにつき説明する。
図１に示されるように、まず走行可能領域にて網目状に前記移動体１０を手動走行させ、移動経路を手押し教示する（Ｓ１）。これは、走行軌跡上にて任意の通過点を節点とし、２の節点間を繋ぐ線分をパス経路として前記記憶部２７に登録し、複数の節点とパス経路からなる初期経路が移動可能経路であることを移動体１０に教示するものである。手押し教示により生成した初期経路を表す移動経路地図を図５に示した。
さらに、前記初期経路上に存在する節点の減数化を行う（Ｓ２）。初期経路上には連続した節点が密集して存在する場合がある。従って、任意の節点を基準とし、予め設定した範囲内に複数の節点が存在する場合には、前記任意の設定以外の他の節点を削除し、パス経路を再生成するものである。これにより、冗長な節点とパス経路を削減でき、前記記憶部２７の記憶容量を節約することができる。節点の減数化後の初期経路を表す移動経路地図を図６に示した。
【００１９】
そして、前記節点を減数化した初期経路に対して、曲折部処理を行なう（Ｓ３）。該曲折部処理は、前記初期経路から曲折部を判別し、該曲折部の折れ曲がり点に位置する節点に対して前後の節点の何れか一方を、他の節点と前記折れ曲がり点の節点で形成されるパス経路上に投影するものである。これにより、前記曲折部に端部が生成され、端部へ向かうパス経路が一本に統合される。
前記曲折部処理を行なった後に、節点投影処理を行なう（Ｓ４）。これは、前記パス経路の近傍に位置する対象節点を該パス経路上に投影する処理である。さらに、節点投影処理後に、前記対象節点に隣接する節点が投影処理を施されているか否かを判別し、投影されていない場合には、前記対象節点の前後のパス経路を残存させ、投影されている場合には隣接節点と対象節点間のパス経路を削除する経路統合化処理を行なう（Ｓ４）。かかる節点投影処理後の移動経路地図を図７に示した。
【００２０】
前記節点投影処理を行なった後に、交点生成処理を行なう（Ｓ５）。これは、交差する２のパス経路を判別し、交差する点に交点となる節点を新たに生成する処理である。
さらに、前記交点生成処理した後に、前記２のパス経路の両端に位置する各既設節点と、前記生成した交点との間の距離を夫々導出し、該距離が一定の閾値以下である近接した既設節点が存在する場合には、前記生成した交点を削除するとともに前記近接した既設節点を交点と見なし、該交点と見なした既設節点を含む第１のパス経路を残存し、これに交差する第２のパス経路を削除する。そして、前記交点と見なした既設節点を一端とし、前記第２のパス経路を代替する第３のパス経路を生成する、交点追設、経路再結合処理を行なう（Ｓ６）。かかる交点処理後の移動経路地図を図８に示した。
このように、曲折部処理、節点投影処理、交点作成、交点追設、経路再結合処理等を行なうことにより、移動体の経路選択自由度が広がり、移動体が自律走行する際に自在に動き回ることができ、目的地までの最短距離を選択可能な合理的な移動経路地図が作成できる。
【００２１】
ここで、図１に示した各処理について詳しく説明する。
まず、前記手押し教示の処理において、節点とパス経路をオドメトリから自動登録する方法として図４に示す方法を採用することができる。図４において、３０は任意の節点、３１は新しく生成された節点で、３２は前記任意の節点３０における移動体１０の姿勢方向を示す基準軸、３３は前記基準軸３２を中心として、左右に一定幅の閾値Ｗだけ離間した許容限界線、３４は移動体の手動走行軌跡、３５は節点同士を連結するパス経路である。
かかる処理では、前記基準軸３２からのずれ量に対する閾値Ｗと、移動体１０の姿勢方向変化量に対する閾値Ｄを予め設定しておき、前記任意の節点３０から手動走行させながら、前記ずれ量と姿勢方位変化量を測定し、該ずれ量及び姿勢方向変化量のうち少なくとも何れか一方が閾値Ｗ、Ｄを越えたら、その位置を新規な節点３１として登録する。そして、前記任意の節点３０と新規な節点３１とを繋いだ線分をパス経路３５として登録する。前記基準軸３２及び許容限界線３３は、新規な節点３１が生成されると矢印Ａ方向に再作成される。
このように、節点生成の際に閾値を変化させることで、適当な間隔で節点を生成できるようになる。また、姿勢方位変化に対する閾値を設けることで、急激な姿勢方位変化が生じる箇所、例えば廊下から側面ドア入り口への侵入などの折れ曲がり部で、必ず節点が生成できるようになる。
【００２２】
また、前記手押し教示の処理において、節点を登録する際に、対象節点に付随する情報を同時に登録するようにしても良い。例えば、図３のフローに示すように、前記移動体１０を手押し移動し（Ｓ１１）、節点として指定したい目的地に到達したら（Ｓ１２）、目的地の座標値を登録するとともに、目的地の名称を音声教示する（Ｓ１３）。さらに、目的地の名称と前記座標値とを関連付けて登録する（Ｓ１４）。そして、経路教示が終了したかを判別し（Ｓ１５）、終了していない場合には、次の目的地まで再度手押し移動させる。これを経路教示が終了するまで繰り返し行なう。
このように、必要に応じて前記節点に付随情報を関連付けて登録することにより、移動体がより一層知能的に振舞えるようになる。例えば、一の節点に対してキッチンという名称を関連付けると、移動体１０に走行指令を出す際に、利用者が「キッチン」と言葉を発することにより、キッチンに対応する節点まで移動体が自動で走行するようになる。
【００２３】
図９に前記曲折部処理の一例を示す。図９（ａ）は曲折部処理前の移動経路を示す図、（ｂ）は曲折部処理後の移動経路を示す図である。
図９において、グローバル座標系Σ0を基準とし、対象節点をRP[i]、この前に位置する隣接節点をRP[i-1]、後に位置する隣接節点をRP[i+1]、前記対象節点RP[i]と隣接節点RP[i-1]の間のパス経路をPT[j]、前記対象節点RP[i]と隣接節点RP[i+1]の間のパス経路をPT[k]、前記隣接節点RP[i+1]を一端とするパス経路をPT[k+1]とする。
まず、曲折部の判別を行なう。前記対象節点RP[i]を挟む位置に存在する前記パス経路PT[j]と前記パス経路PT[k]を表すベクトルを夫々正規化して生成し、両ベクトルの内積を求め、該内積が予め設定した閾値以下となった場合に曲折部と判定する。即ち、前記パス経路PT[j]と前記パス経路PT[k]の為す角が、所定の角度以下となったときに曲折部と判定する。
【００２４】
曲折部と判定されたら、前記対象節点RP[i]からの距離が短い方の隣接節点、即ち本実施例では隣接節点RP[i+1]を、他の隣接節点RP[i-1]と前記対象節点RP[i]との間のパス経路PT[j]上に投影する。投影処理については後述する。
投影処理後、前記パス経路PT[j]と前記パス経路PT[k]を消去する。
前記投影した隣接節点RP[i+1]と、他の隣接節点RP[i-1]との間に、新しいパス経路PT[max+1]を追加する。
前記投影した隣接節点RP[i+1]と、前記対象節点RP[i]との間に、新しいパス経路PT[max+2]を追加する。
前記パス経路PT[k+1]の始点を前記投影した隣接節点RP[i+1]に変更する。
以上により曲折部処理が終了し、前記対象節点RP[i]を終端とする端部が生成される。
【００２５】
図１０乃至図１２に節点投影処理を示す。
節点投影処理では、対象節点と該対象節点に最も近接するパス経路との距離を算出し、該距離が予め設定した閾値以下であり、かつ前記対象節点における移動体の姿勢方向と、前記パス経路の傾きの差が一定の閾値以下である場合に、対象節点の投影処理を行うようにする。
また、投影処理を行なう際には、前後の隣接節点のどちらも投影されていない場合（ケース１）と、前後の隣接節点のうち何れか一方が投影されている場合（ケース２）とに場合分けして行なうものとする。
【００２６】
図１０は、隣接節点が投影されていない場合（ケース１）の節点投影処理の一例を示す図であり、（ａ）は投影処理前の移動経路を示す図、（ｂ）は投影処理後の移動経路を示す図である。
図１０によれば、グローバル座標系Σ0を基準とし、対象節点をRP[i]、この前に位置する隣接節点をRP[i-1]、後に位置する隣接節点をRP[i+1]、前記隣接節点RP[i-1]と前記対象節点RP[i]との間のパス経路をPT[k-1]、前記対象節点RP[i]と前記隣接節点RP[i+1]との間のパス経路をPT[k]、投影するパス経路の両端に位置する節点をRP[j]、RP[j+1]、前記節点RP[j]と前記節点RP[j+1]の間のパス経路をPT[j]とする。
投影処理を行う前の経路は、（１）→RP[j]→RP[j+1]→、（２）→RP[i-1]→RP[i]→RP[i+1]→、の２の移動経路が存在している。前記対象節点RP[i]を前記パス経路PT[j]に投影した後、前記パス経路PT[k-1]と前記パス経路PT[k]とを消去し、新たに前記投影した対象節点を一端としたパス経路PT[max+1]とパス経路PT[max+2]を生成する。
従って、投影処理後の経路は、（１）→RP[j]→RP[i]→RP[j+1]→、（２）→RP[i-1]→RP[i]→RP[i+1]→の２の移動経路が存在することとなる。
【００２７】
図１１は、隣接節点の何れか一が投影されている場合（ケース２）の節点投影処理の一例を示す図であり、（ａ）は投影処理前の移動経路を示す図、（ｂ）は投影処理後の移動経路を示す図である。
図１１では、図１０に示した投影処理と同様に、対象節点RP[i]をパス経路PT[j]に投影し、前記パス経路PT[k-1]と前記パス経路PT[k]を消去した後、新たにパス経路PT[max+1]、パス経路ＰT[max+2]を生成する。
さらに本実施例では、既に前記隣接節点RP[i-1]が投影されているため、該隣接節点RP[i-1]と対象節点RP[i]との間に新たに生成したパス経路PT[max+1]を消去する。このように、前記パス経路PT[max+1]を消去することにより重複した経路を省き、移動経路地図を簡略化することができる。
【００２８】
また、図１２に前記節点投影処理の具体的な演算方法を示す。図１２では、グローバル座標系Σ0：（Ｘ，Ｙ，ＴＨ）（ここで、THは姿勢方向）、対象節点RP[j]を原点とした並進座標系Σ1：（ｘ’，ｙ’，ｔｈ’）、対象節点RP[j]を中心にｘ”軸が経路と一致するように回転角αだけ回転させた回転座標系Σ2：（ｘ”，ｙ”，ｔｈ”）、の３の座標系を利用している。
まず、グローバル座標系Σ0→並進座標系Σ1、並進座標系Σ1→回転座標系Σ2への座標変換を夫々行なう。
次に、回転座標系Σ2における対象節点のｘ”座標を計算する。
さらに、前記回転座標系Σ2における対象節点RP[i]のｙ”座標が、予め設定された閾値Ｌより小さい場合には、パス経路PT[j]上に前記対象節点RP[i]を投影する。
投影した対象節点RP[i]の回転座標系Σ2における座標は、（RP[i].ｘ”，0，90）となる。
さらに、前記座標について、回転座標系Σ2→グローバル座標系Σ0に座標変換して戻すと、グローバル座標系における投影座標が得られる。
【００２９】
図１３に前記交点処理の一例を示す。図１３（ａ）は交点処理前の移動経路を示す図、（ｂ）は交点処理後の移動経路を示す図である。
図１３において、グローバル座標系Σ0を基準とし、交差する２のパス経路を、夫々パス経路PT[i]、パス経路PT[j]、前記２のパス経路の交点をRP[k]とする。このとき、前記パス経路PT[i]、PT[j]、交点RP[k]は以下のように定義される。
PT[i]：対象パス経路（0≦i≦Data-2）
始点の座標値：（PT[i].xb，PT[i].yb）
終点の座標値：（PT[i].xe，PT[i].ye）
PT[j]：対象パス経路（i+1≦j≦Data-1）
始点の座標値：（PT[j].xb，PT[j].yb）
終点の座標値：（PT[j].xe，PT[j].ye）
RP[k]：交点（Data≦k）
交点の座標値：（Ｘ座標，Ｙ座標，姿勢th）=（RP[k].x，RP[k].y，RP[k].th）
【００３０】
また、前記パス経路PT[i]、前記パス経路PT[j]の関数は以下のように定義される。
経路PT[i]：Y=f1(X)=A1・X+B1
[min(PT[i].xb，PT[i].xe)≦X≦max(PT[i].xb，PT[i].xe)]
経路PT[j]：Y=f2(X)=A2・X+B2
[min(PT[j].xb，PT[j].xe)≦X≦max(PT[j].xb，PT[j].xe)]
このとき、A1=(PT[i].ye-PT[i].yb)/(PT[i].xe-PT[i].xb)
B1=(PT[i].xe×PT[i].yb-PT[i].xb×PT[i].ye)/(PT[i].xe-PT[i].xb)
A2=(PT[j].ye-PT[j].yb)/(PT[j].xe-PT[j].xb)
B2=(PT[j].xe×PT[j].yb-PT[j].xb×PT[j].ye)/(PT[j].xe-PT[j].xb)
【００３１】
このように、前記パス経路PT[i]、前記パス経路PT[j]が定義できるため、これらのパス経路が交差するか否かを判別する際には、何れか一方のパス経路に注目し、他のパス経路の始点と終点が前記注目したパス経路を挟んで位置するか否かを求める。前記始点と終点とが前記注目したパス経路を挟んで位置する場合には、前記２のパス経路が交差していると判定し、同じ側に位置する場合には交差していないものと判定する。
前記２のパス経路が交差していると判定された場合には、前記定義したように交点を導出し、ここに新たに節点を生成する。
【００３２】
さらに、前記交点生成処理を行なった後に、生成した交点RP[k]と、前記２のパス経路PT[i]、PT[j]の両端に位置する節点との距離を算出し、該距離が予め設定した閾値以下である場合には、図１４に示されるように交点追設、経路再結合処理を行なう。図１４は交点追設、経路再結合処理の一例を説明する図で、（ａ）は処理前の移動経路を示す図、（ｂ）は処理後の移動経路を示す図である。
図１４において、交差するパス経路を夫々パス経路PT[i]、パス経路PT[j]、前記パス経路PT[i]の両端の既設節点をRP[i]、RP[ii]、前記パス経路PT[j]の両端の既設節点をRP[j]、RP[jj]、生成した交点をCS[k]とする。
【００３３】
前記生成した交点CS[k]に最も近接する節点が前記既設節点RP[j]である場合、これらの距離を算出し、該距離が閾値以下である場合には、前記既設節点RP[j]を交点と見なす。
前記既設節点RP[j]を含むパス経路PT[j]を残し、これに交差するパス経路PT[i]を削除する。
前記新たな交点RP[j]と前記既設節点RP[i]の間にパス経路PT[max+1]を生成する。同様に、前記新たな交点RP[j]と前記既設節点RP[ii]の間にパス経路PT[max+2]を生成する。
このようにして、交点が既設節点に近い場合には交点を追設し、経路を再結合する。このとき、他の既設節点RP[i]、RP[ii]、RP[jj]が生成した交点CS[k]と近接する場合にも同様の処理を行う。
【００３４】
また、前記各種処理を施して作成した移動経路地図に対して、例えば前記節点、前記パス経路の削除、追加処理、または別個に作成した移動経路地図の融合等の編集機能を持たせることが好適である。
上記した図９乃至図１４に示す折曲部処理、節点投影処理、交点処理等は一例であるが、このように節点、パス経路の処理を行うことにより、手動走行により作成された初期経路が合理化され、移動体が自律走行する際の経路選択自由度が広く、目的地まで移動する最短経路を選択することができる合理的な移動経路地図を作成することができる。
【図面の簡単な説明】
【００３５】
【図１】本発明の実施例に係る移動経路地図作成アルゴリズムを示すフローである。
【図２】本発明の実施例に係る移動体を示す概略構成図である。
【図３】対象節点への付随情報関連付け処理を示すフローである。
【図４】本実施例における節点生成方法を説明する図である。
【図５】手押し教示直後の初期経路を表す移動経路地図である。
【図６】節点減数化直後の初期経路を表す移動経路地図である。
【図７】節点投影処理後の移動経路地図である。
【図８】交点処理後の移動経路地図である。
【図９】曲折部処理の一例を説明する図で、（ａ）は曲折部処理前の移動経路を示す図、（ｂ）は曲折部処理後の移動経路を示す図である。
【図１０】本実施例における節点投影処理（ケース１）の一例を説明する図で、（ａ）は投影処理前の移動経路を示す図、（ｂ）は投影処理後の移動経路を示す図である。
【図１１】本実施例における節点投影処理（ケース２）の一例を説明する図で、（ａ）は投影処理前の移動経路を示す図、（ｂ）は投影処理後の移動経路を示す図である。
【図１２】図１０及び図１１に示した節点投影処理における座標変換を説明する図である。
【図１３】本実施例における交点生成処理の一例を説明する図である。
【図１４】本実施例における交点追設、経路再結合処理の一例を説明する図で、（ａ）は処理前の移動経路を示す図、（ｂ）は処理後の移動経路を示す図である。
【図１５】本発明の実施例を適用する一般家庭屋内の間取り図である。
【符号の説明】
【００３６】
１０    移動体
１２    駆動輪
１３    入力手段
１４    出力手段
２０    制御部
２１    オドメトリデータ検出部
２２    節点，パス経路生成部
２３    減数化処理部
２４    曲折部処理部
２５    交点生成処理部
２６    節点投影処理部
２７    記憶部
２８    編集機能
【図１】
2005316760.tif 000003
【図２】
2005316760.tif 000004
【図３】
2005316760.tif 000005
【図４】
2005316760.tif 000006
【図５】
2005316760.tif 000007
【図６】
2005316760.tif 000008
【図７】
2005316760.tif 000009
【図８】
2005316760.tif 000010
【図９】
2005316760.tif 000011
【図１０】
2005316760.tif 000012
【図１１】
2005316760.tif 000013
【図１２】
2005316760.tif 000014
【図１３】
2005316760.tif 000015
【図１４】
2005316760.tif 000016
【図１５】
2005316760.tif 000017