JPH06259131A - 移動ロボットの誘導制御装置 - Google Patents

Abstract

(57)【要約】 （修正有） 【目的】 移動ロボットの位置や姿勢の推定値が、実際
値から、かけ離れた場合に、環境内経路を再設計して、
障害物を多く含むような複雑な環境下でも、安定でかつ
高速な移動ロボットの誘導を実現する。 【構成】 推定計画部３がその位置・姿勢の推定のため
に環境の一部または環境に含まれる道標を観察する方法
を計画した後、位置推定部７が地点Ｃで環境観察と位置
推定のための処理を開始するとともに、移動制御部４は
地点Ｂに向かってロボットの移動を制御し、位置推定部
７が地点Ｃにおけるロボットの位置・姿勢の推定が終了
した地点Ｄにおいて、移動制御部４が算出した地点Ｃと
地点Ｄの事前推定値と位置推定部７が推定した地点Ｃの
新たな位置姿勢推定値とを利用して位置更新部８が地点
Ｄでのロボットの位置と姿勢の推定値を更新し、その更
新された位置から目的地Ｂまでの経路を経路計画部１が
再計画する。

Description

【発明の詳細な説明】

【０００１】

【産業上の利用分野】本発明は移動ロボットの誘導制御
装置に関する。

【０００２】

【従来の技術】以下の文献は移動ロボットの誘導制御装
置に関する技術を開示している。

【０００３】文献１：A.Kosaka and A.C.Kak,"Fast vis
ion-guided mobile robotnavigation using model-base
d reasoning and prediction of uncertainties,"Compu
ter Vision,graphics and Image Processig--Image Und
erstanding,Vol.56,No..3,November 1992,pp271-329. 文献２：O.I.Camps,L.G.Shapiro,and R.M.Haralick,"Ob
ject Recognitionusing Prediction and Probabilistic
Matching,"Proceedings of the 1992IEEE Internation
al Conference on Intelligent Robots and Systems,Ra
leigh,NC,1992,pp.1044-1052. 病院や工場さらには原子力発電所の内部など、物を自動
的に運搬したり、人間が入れないような危険で遠隔な場
所で、自律移動の利点を生かした移動ロボットが応用さ
れている。一般的に、移動ロボットには走行距離計な
ど、移動用モーターに装着された内部センサがあるが、
走行距離計から読み取れる移動ロボットの位置や姿勢の
値は、実際の位置と姿勢からかけ離れたものとなる。こ
うした違いは、移動ロボットの車輪と床面との間の滑べ
りや、移動ロボットの車輪間の半径の違い、移動ロボッ
トの左右重量のバランスの違いなどによるものであるこ
とが、文献１に説明されている。しかも、ロボットが長
い距離を移動すれば移動するほど、その違いは大きくな
ることが予想される。

【０００４】こうした問題を解決するためには、ロボッ
トに与えられた環境の地図とロボットが環境を観察した
結果をロボット自身が参照して、環境に対するロボット
の相対位置と姿勢を推定することが重要となる。こうし
た方法としては、文献１に表されたような方法がある。
文献１の方法では、ロボットの環境をモデル化してお
き、ロボットの位置と姿勢の不確定度が大きくなったと
き、ロボットが自分の位置と姿勢を推定するように制御
する。推定すべきときには、まずロボットは完全に立ち
止まる。そして、その位置でどちらの方向を向いて環境
を観察したらよいかを計画し、その方向でロボットが見
るべき予測図を作成する。その予測図に現れる予測道標
（ランドマーク）を選択し、その予測道標に対応する画
像道標を実際にロボットが撮影した画像の中から抽出
し、予測図内の予測道標の環境内での位置と画像内の画
像道標の画像内に位置を比較することにより、環境に対
するロボットの相対的位置を計算するわけである。その
計算後、ロボットは環境内の経路を再計画して、ロボッ
トは目標地に向けて再出発することになる。

【０００５】この方法では、予測画に現れる顕著な予測
道標に対応する画像道標だけを画像内から抽出しようと
試みるので障害物を多く含むような複雑な環境下でもロ
ボットを誘導することができたが、画像内からの画像道
標抽出ならびに予測道標と画像道標の比較に時間がかか
るので、位置姿勢推定のためのロボットが立ち止まって
いる時間が多大となるという問題点があった。

【０００６】別の従来例としては、特開昭６０−２１７
４１２号公報をあげることができる。この従来例では、
ロボットはある一定時間間隔で環境の観察を行い、自分
の位置を推定するように制御されている。実際には、ロ
ボットは自分の位置の事前推定量Ｘ0 を用意しておき、
その事前推定量をロボットの観察の視点と仮定して環境
モデルから予測画像を生成し、実際に観測された観測画
像と予測画像とを比較する。この比較によって得られた
画像誤差Ｅf をロボットの位置誤差Ｅx に変換し、この
位置誤差Ｅx をリアルタイムでロボットにフィードバッ
クすることでロボットが次に進むべき方向や位置を設定
するという方法を提案している。

【０００７】この方法を用いると、単純な環境では画像
認識による高速な誘導は実現できるが、ロボットの前方
に障害物があるような複雑な環境においては、ロボット
の誘導が困難になるという問題点があった。たとえば、
ある一定時間ロボットの前方に障害物などがあり、予測
画像にない物体の特徴が道標として抽出されたとき、そ
の期間中ロボットが進むべき方向が定義されなかった
り、本来進むべきでない方向に移動したりするので、安
定した誘導が困難となる点があげられる。

【０００８】さらに、複数個の画像特徴と予測特徴を比
較する際、複数の特徴の中で最も良好に認識された特徴
を誘導の指標とする点に別の問題点がある。ロボットの
位置姿勢を平面上で規定するには少なくとも３個のパラ
メータ、すなわち、平面上の位置座標（ｐx ，ｐy ）と
ロボットの角度方向φを決定しなければならない。従来
例で説明されているように１本の予測直線成分と１本の
観測直線成分でロボットの位置誤差を計算する場合を考
える。直線成分の端点は、３次元空間内にある他の物体
による遮蔽、照明条件、あるいは測定誤差の問題で、必
ずしも正確に検出できるとは限らない。そこで端点の情
報を使用しないで直線として照合しようとすると、照合
によって得られるパラメータの制約の数は２にすぎな
い。

【０００９】従って、推定する３次元パラメータを制約
数２のパラメータによって推定することとなり、安定し
た解は必ずしも求められないことが示される。さらに、
障害物などがロボットの観測画像内に新たな観測パター
ンを形成するときには、照合の正確さの点で、１組の特
徴間の照合だけによる位置の測定には問題がある。

【００１０】

【発明が解決しようとする課題】こうした問題を解決す
るには、複数の特徴パターンと複数の観測パターンの照
合によって得られる制約条件を融合して解を求めればよ
いが、従来例ではこの点に関して解説するに及んでいな
い。一方、このように複数の予測パターンと複数の観測
パターンとの照合を考えると、第１の従来例のように処
理に要する時間がかかることが予測され、従来例で示さ
れているようなリアルタイムによる位置情報のフィード
バックが困難であるという問題点があった。

【００１１】本発明の移動ロボットの誘導制御装置はこ
のような課題に着目してなされたものであり、その目的
とするところは、障害物を多く含むような複雑な環境下
でも、安定でかつ高速な移動ロボットの誘導を実現する
ことにある。

【００１２】

【課題を解決するための手段及び作用】上記の目的を達
成するために、本発明の移動ロボットの誘導制御装置
は、環境の地図を記憶する手段と、その地図を利用しロ
ボットの経路を計画する経路計画手段と、その経路情報
に基づいてロボットの移動を制御しロボットの位置姿勢
の事前推定値を算出する移動制御手段と、前記地図を利
用し環境の一部または環境に含まれる道標を観察してロ
ボットの位置・姿勢を推定する位置推定手段と、ロボッ
トの位置・姿勢の推定方法を計画する推定計画手段と、
ロボット位置・姿勢の推定値の更新を行う位置更新手段
とを具備し、ある地点Ａから別の地点Ｂまで移動するこ
とを命令された移動ロボットの誘導制御装置において、
経路計画手段が作成した経路に基づいて移動制御手段が
制御するロボットの進行に応じて、推定計画手段がその
位置・姿勢の推定のために環境の一部または環境に含ま
れる道標を観察する方法を計画した後、位置推定手段が
地点Ｃで環境観察と位置推定のための処理を開始すると
ともに、移動制御手段は地点Ｂに向かってロボットの移
動を制御し、位置推定手段が地点Ｃにおけるロボットの
位置・姿勢の推定が終了した地点Ｄにおいて、移動制御
手段が算出した地点Ｃと地点Ｄの事前推定値と位置推定
手段が推定した地点Ｃの新たな位置姿勢推定値とを利用
して位置更新手段が地点Ｄでのロボットの位置と姿勢の
推定値を更新し、その更新された位置から目的地Ｂまで
の経路を経路計画手段が再計画し、その経路にもとづい
て移動制御手段がロボットの移動を制御することを基本
サイクルとし、以上の基本サイクルをロボットが地点Ｂ
付近に到達するまで繰り返す。

【００１３】

【実施例】以下、図面を参照して本発明の実施例を説明
する。

【００１４】まず、第１実施例について説明する。本実
施例で説明される移動ロボットは、あらかじめ地図が与
えられた環境の中を移動することを前提としている。そ
の環境の地図は、環境の特徴となる対象物や道標（ラン
ドマーク）を含んだものである。しかしながら、その環
境に障害物が存在しても構わない。また、環境の地図は
必ずしも正確でなくても構わないとする。

【００１５】ロボットが環境を移動することを明確にす
るためには、こうした環境とロボットとの位置関係を規
定する必要がある。その例として、ロボットが部分的に
平面で近似できる環境内を移動すると考える。するとロ
ボットの位置と姿勢を規定するためには、３個のパラメ
ータ、すなわち各部分平面上でのロボットの基準点の位
置（ｐｘ，ｐｙ）と各部分平面上でのロボットの向き
（方角、方向）φを利用すればよい。

【００１６】より一般的には、ロボットが３次元空間内
を移動すると仮定すると６個のパラメータ、すなわちロ
ボット内の基準点の位置（ｐｘ，ｐｙ，ｐｚ）とロボッ
トの３次元方向（方角）（φｘ，φｙ，φｚ）によって
規定することができる。本実施例では、どちらの方法で
位置関係を規定しても構わないが、説明を簡単にするた
めに、以下の実施例ではロボットは部分的に平面で記述
できる環境を移動すると仮定する。

【００１７】図２は、この仮定のもとで３個のパラメー
タを説明したものである。図中、斜線で表された５角形
はロボットを表し、ハッチングされた領域はロボットが
移動できる通路を表している。

【００１８】環境を記述するために、環境内にある原点
Ｏｗを設定し、この原点に基づいて世界座標系（ｘｗ，
ｙｗ，ｚｗ）を定義する。さらにロボットに関連するロ
ボット座標系（ｘｒ，ｙｒ，ｚｒ）（原点をＯｒとす
る）を定義する。ロボットは２次元空間内を移動するの
で、ロボット座標系のｚｒ軸は、世界座標系のｚｗ軸と
平行で、ロボット座標系の原点Ｏｒと世界座標系の原点
Ｏｗは同一平面上にあると設定してもかまわない。この
条件のもとで、世界座標系とロボット座標系を関連づけ
るためには、３個のパラメータｐ＝（ｐｘ，ｐｙ，φ）
が必要となる。ここに、（ｐｘ，ｐｙ）はロボット座標
系の原点Ｏｒの、世界座標系におけるｘｙ座標であり、
φはｘｒのｘｗに対する回転角度である。

【００１９】ロボットの移動にともない、ロボット座標
系の位置と方向は順次変化することとなるが、その位置
と方向はｐ＝（ｐｘ，ｐｙ，φ）によって規定すること
ができる。本実施例では、このベクトルｐ＝（ｐｘ，ｐ
ｙ，φ）をロボットの位置と姿勢を表すパラメータと呼
ぶことにする。もし、３次元的な移動を考える際には、
ロボットの位置と姿勢を表すパラメータとして、ｐ＝
（ｐｘ，ｐｙ，ｐｚ、φｘ，φｙ，φｚ）を利用すれば
よい。

【００２０】文献１で説明されているように、ロボット
の位置と姿勢は、ロボットが移動するにつれて不確定と
なる。例えばロボットが車輪駆動の場合には、車輪間の
半径の違い、床面でのすべり、また移動量を計測するセ
ンサでの読み取り誤差などが原因となることが知られて
いる。従って、ロボットはその位置と姿勢を表すパラメ
ータを移動中に推定することが重要となる。本実施例が
提案しようとするのは、ロボットの移動に際しロボット
の位置と姿勢の推定処理を効率的に制御する方法であ
る。

【００２１】図１は、本実施例の基本構成を表したもの
である。移動ロボットの誘導制御部分は、ロボットが移
動する環境の地図やその環境に存在する道標を記憶する
地図記憶モジュール２と、その地図情報１０１と操作者
からの移動命令１００に基づいて出発地から目的地まで
の経路を計画する経路計画モジュール１と、その経路情
報１０４をロボットが移動できる形の基本コマンドに分
解し実際にロボットの移動機構がその移動を実行してい
るかを管理・制御し、ロボットの位置と姿勢を表すパラ
メータを事前推定値として随時算出する移動制御モジュ
ール４と、ロボットの位置と姿勢を環境を認識すること
によって推定する方法を計画する推定計画モジュール３
と、その推定計画に基づいて実際に位置姿勢推定を行う
位置姿勢推定モジュール７と、その位置推定に必要な画
像を撮像する画像入力モジュール９と撮影された画像信
号を記憶する画像記憶モジュール１０と、位置姿勢推定
モジュール７が新しいロボットの位置姿勢推定値１０９
を計算したとき、移動制御モジュール４が算出した事前
位置姿勢値１０５を新しい推定値を用いて更新する位置
更新モジュール８と、移動ロボットの前方にある障害物
を検知し、障害物がある領域の情報を移動制御モジュー
ル４に告知する障害物検知モジュール６からなる。

【００２２】以下、各モジュールの機能に関して説明す
る。

【００２３】まず、地図記憶モジュール２について説明
する。地図記憶モジュール２は、環境に関するモデルを
含んだものである。環境内にある経路を図２にあるよう
に世界座標系ｘｙ平面上に投影して、２次元的な地図と
して保有している。具体的に述べれば、ロボットが移動
できる経路の境界線を基本要素として地図を作成する。

【００２４】図３は、図２に表された環境について、こ
れら基本要素としての境界線を表したものである。図３
において、直線成分Ｌ1 ，…，Ｌ20は境界線を表す。こ
の例の場合は直線成分を境界線の基本要素として表した
が、基本要素としてパラメータ化ができる曲線成分など
で表しても構わない。こうした基本要素は世界座標系で
のｘｙ座標を用いてパラメータ化される。例えば、図３
のＬ1 の場合には、Ｌ1 が直線成分があることから２端
点のｘｙ座標（ｘ11，ｙ11），（ｘ12，ｙ12）を用いて
表されることとなる。

【００２５】境界を表す各基本成分には、経路を特徴づ
けるような道標が割り付けられている。ここで道標とし
ては、経路内にあるドア、掲示版、窓、床と壁を分ける
側線、あるいは特殊なマークなどであってもよい。図４
は、こうした道標の例を表したものである。図中、道標
としては、ドア３０１、掲示版３０２、側線３０３、３
０４が示されている。こうした道標にはそれぞれＩＤ番
号が与えられている。さらに、道標の位置を表すパラメ
ータは３次元の世界座標系の中での３次元ベクトルを基
準として記述される。例えば、図４のドア３０１の場合
には、そのドアを構成する４本の直線成分を詳細特徴要
素として所有し、以下のような、データ構造で表現され
る： ＩＤ：３０１ 種類：ドア 詳細特徴要素個数：４ 詳細特徴要素： １：ＩＤ：３０１１ 種類：直線成分 端点：（ｘ1 ，ｙ1 ，ｚ1 ），（ｘ2 ，ｙ2 ，ｚ1 ） ２：ＩＤ：３０１２ 種類：直線成分 端点：（ｘ1 ，ｙ1 ，ｚ2 ），（ｘ1 ，ｙ1 ，ｚ2 ） ３：ＩＤ：３０１３ 種類：直線成分 端点：（ｘ1 ，ｙ1 ，ｚ2 ），（ｘ2 ，ｙ2 ，ｚ2 ） ４：ＩＤ：３０１４ 種類：直線成分 端点：（ｘ2 ，ｙ2 ，ｚ2 ），（ｘ2 ，ｙ2 ，ｚ1 ） ここで、（ｘ1 ，ｙ1 ，ｚ1 ），（ｘ2 ，ｙ2 ，ｚ2 ）
などは直線成分の端点のｘｙｚ座標を表す。道標はこの
ように、この詳細特徴要素が世界座標系での３次元座標
をパラメータとして表現されている。

【００２６】次に、経路計画モジュール１について説明
する。

【００２７】図１において、移動ロボットには操作者な
どから移動命令１００が与えられる。その移動命令は、
出発地Ａから目的地Ｂへ移動せよ、という形でのコマン
ドである。移動コマンドの地点Ａ、Ｂは世界座標系での
ｘｙ座標とロボットの方向で与えられる。すなわち、
Ａ：（ｘａ，ｙａ，φａ），Ｂ：（ｘｂ，ｙｂ，φｂ）
の形で与えられる。ここに、（ｘａ，ｙａ）は出発地Ａ
の世界座標系でのｘｙ座標であり、φａは出発地Ａにお
けるロボットの方向を表したものである。一方、（ｘ
ｂ，ｙｂ）は目的地Ｂの世界座標系でのｘｙ座標であ
り、φｂの目的地Ｂにおけるロボットの方向を表したも
のである。ここで出発地Ａを表す値は、必ずしも正確な
値である必要はなく、その推定値であってもよい。

【００２８】このように、ロボットに移動の出発地Ａと
目的地Ｂが与えられると、その命令は経路計画モジュー
ル１に送られる。経路計画モジュール１では、地図記憶
モジュール２に格納された地図情報に基づいて出発地Ａ
から目的地Ｂまでの経路が計画される。その経路情報は
移動制御モジュール４に送られる。その経路が如何にし
て作成されるかについては、例えば文献１などが説明し
ている。これには多くの方法が考えられるが、経路を計
画する方法が本実施例の主となる点ではないので、ここ
では詳しい説明を省略する。

【００２９】図５は、経路計画モジュールが作成した経
路を表したものである。これは、図３の出発地Ａから目
的地Ｂまでの経路を計画した結果である。図３におい
て、矢印で表示されているのは、地点Ａ、Ｂでのロボッ
トの方向を表したものである。図５の場合は、出発地Ａ
から目的地Ｂまでの３本の直線成分ｓ1 ，ｓ2 ，ｓ3 で
近似される。この経路情報は、その直線成分の端点の世
界座標の形、すなわち、点Ａ，Ｐ1 ，Ｐ2 ，Ｂという点
列の２次元世界座標の形で、移動制御モジュール４に送
られる。

【００３０】次に、障害物検知モジュール６について説
明する。

【００３１】障害物検知モジュール６は、移動ロボット
の前方に存在する障害物を検知するモジュールである。
本実施例の場合、このモジュールには、超音波センサア
レイが装備されている。超音波センサアレイは、ロボッ
トの前方に超音波を発し、そのエコーが帰ってくるまで
の時間を計測することにより、障害物の存在する相対的
な方向と距離とその障害物の大きさを検出するものであ
る。この結果により、もしロボットの経路を侵害するよ
うな障害物が検知されたときには、その障害物の存在す
る相対的な方向と距離とその障害物の大きさの情報１１
５を移動制御モジュール４に送る。

【００３２】次に、移動制御モジュール４について説明
する。

【００３３】経路計画モジュール１から与えられる経路
情報１０４は点列Ａ，Ｐ1 ，Ｐ2 ，…，Ｐn ，Ｂで表現
され、その点間はほぼ直線成分で近似できるものから成
り立つ。移動制御モジュール４では、上記直線成分をロ
ボットの移動軌道とするように、ロボットに装着された
車輪や足を制御する機能を果たす。

【００３４】実際、移動ロボットには、図１には図示さ
れない車輪や足などを備えた移動機構がある。この移動
機構は、この移動制御モジュール４によって制御され
る。移動制御モジュール４は、前述の直線成分で近似さ
れた軌道を直進や回転などという移動機構が認識できる
基本コマンドに変形し、そのコマンドに対応する移動が
移動機構で実行されたか否かをモニタしながら、移動を
制御することになる。実際には移動機構に装着された移
動量を計測するセンサの出力とコマンドで示された移動
量との差をモニタしながら移動を制御する。

【００３５】さらに、移動制御モジュール４は、ロボッ
トの移動量に基づいて、環境に対する移動ロボットの位
置と姿勢ｐ＝（ｐｘ，ｐｙ，φ）を各時間ごとにモニタ
し、目的地Ｂに到着したか否かも常時チェックする。こ
うしてモニタされたロボットの位置姿勢データは、経路
計画モジュール１、推定計画モジュール３、位置姿勢推
定モジュール７、位置更新モジュール８などで、事前推
定値として利用される。

【００３６】一方、もし障害物検知モジュール６が障害
物をロボットの前方に検知したときには、移動制御モジ
ュール４が局所的にその障害物を退避するような局所的
な経路を生成し、その新しい経路に従って障害物を退避
する。この際、ロボットはあらかじめ作成された経路
Ａ，Ｐ1 ，Ｐ2 ，…，Ｐn ，Ｂから離れてしまうので、
障害物退避後、移動制御モジュール４はもとの経路に復
帰するように、ロボットの経路を局所的に計画し、その
移動を移動機構に命令する。

【００３７】次に、推定計画モジュール３について説明
する。

【００３８】ロボットが自分の位置と姿勢を推定する必
要が発生したとき、推定計画モジュール３は、環境のど
の位置で、どの方向に対して、どのような手段で環境を
観察したらよいかを計画する。具体的には、現在位置す
る場所付近で観察に都合のよい位置と姿勢、あるいはカ
メラの視線方向を設定するものである。このような計画
を作成するために、推定計画モジュール３は、移動制御
モジュール４が送出する現在地の事前位置姿勢推定値１
０５と、地図記憶モジュール２が送出する道標情報１０
２とを利用する。

【００３９】図６は、推定計画モジュール３の処理を説
明するためのものである。図６において、ロボットは現
時点でＰで示される位置で矢印で示された方向を向いて
いると仮定する。その時、ロボットに装着されたカメラ
を含む画像入力モジュール９で撮影されると予測される
ものは、図６の黒色で示された環境の領域であることが
わかる。そこで、推定計画モジュール３は、この黒色で
示された領域に存在する道標群を選択する。これを実行
するには、黒色で示される領域と、地図記憶モジュール
２に格納されている境界線を示す基本要素との交わりを
求めればよい。図６では、直線成分Ｌ2 の一部である線
分Ｑ1 Ｑ2 、直線成分Ｌ3 の一部である線分Ｑ2 Ｑ3 、
そして直線成分Ｌ14の一部である線分Ｑ4 Ｑ5 が、これ
に相当する。そして、こうした線分領域Ｑ1 Ｑ2 ，Ｑ2
Ｑ3 ，Ｑ4 Ｑ5 内に含まれる道標を地図記憶モジュール
２の道標情報から抽出する。推定計画モジュール３で
は、現在地の推定値からロボットの位置と姿勢を推定す
るのに充分な数の道標が得られたときには、現在地の位
置と姿勢で、ロボットの位置と姿勢を推定することとし
て、現在地から可視な道標の情報を位置姿勢推定モジュ
ール７に送出する。

【００４０】もし現在地の事前推定値では、ロボットの
位置姿勢を推定するのに充分な道標が得られないと判断
できるときには、まず画像入力モジュール９内のカメラ
の位置や姿勢を変更して充分な可視な道標が得られるか
否かをチェックする。具体的には、現在のカメラの方向
から、４５°、−４５°、９０°、−９０°、１３５
°、−１３５°、１８０°などの角度分だけカメラを回
転したと仮定したときに、その新しい方向での道標の数
をチェックすることによって回転方向を決定する。

【００４１】もし、移動ロボットのカメラが固定されて
いて、回転できないときには、ロボット自体を上記のよ
うな角度分回転すると仮定したときの道標の数と内容を
チェックする。もしある方向が決定したら、その回転角
度分の移動を移動制御モジュール４に命令し、ロボット
を回転させた後、可視の道標の情報を位置姿勢推定モジ
ュール７に送出する。

【００４２】もし、現時点の回転だけでは、充分な可視
な道標が得られないときには、移動ロボットを進行方向
に移動させた時に充分な道標が得られる場所を計算す
る。この計算は、現時点ＰからＢまでの経路を適当な間
隔で離散化することによって実現する。その点をＰに近
い方から、Ｒ1 ，Ｒ2 ，…，Ｒｍ，Ｂとする。そして、
Ｒ1 ，Ｒ2 ，…，Ｒｍ，Ｂの順で、その点での可視な道
標の数をチェックすればよい。

【００４３】もし、道標の数が点Ｒｉで充分であること
がわかったならば、ＰからＲｉまで、ロボットを移動さ
せることを移動制御モジュール４に命令する。その移動
が確認された後、ロボットは地点Ｒｉでの可視な道標の
情報を位置姿勢推定モジュール７に送出する。もちろ
ん、送出される道標の情報は、前述した道標のデータ構
造に従ったものであるが、道標としては、その全貌が可
視でなくても構わない。実際には、道標に含まれる詳細
特徴要素がいくつか可視であるかが、推定計画モジュー
ル３が推定を実行すべき位置と姿勢を決定する条件とな
る。

【００４４】こうしてロボットの局所的な移動が必要と
なったときには、推定計画モジュール３は、移動制御モ
ジュール４と連携して、ロボットの位置姿勢を観察に必
要な位置まで移動させた後、ロボット位置姿勢推定に必
要な観察の情報を位置姿勢推定モジュール７に送出す
る。

【００４５】次に、画像入力モジュール９について説明
する。

【００４６】画像入力モジュール９は、画像を入力する
ＴＶカメラなどを含む。このＴＶカメラは、その視線方
向が回転によって変更できるものでもよいし、ロボット
本体に固定されているものでも構わない。ＴＶカメラの
向きを変更できるものに関しては、先に述べたように、
推定計画モジュール３のカメラ移動命令によってカメラ
の向きの方向が決定される。カメラで撮影された画像信
号１１４は、画像記憶モジュール１０へ送られる。ここ
で、画像入力モジュール９内に格納されているカメラ
は、較正されていると仮定する。すなわち、３次元空間
内の点Ｐを既知のロボットの位置姿勢で撮像したときに
は、点Ｐの像Ｐ′の位置は計算できると仮定する。

【００４７】次に、画像記憶モジュール１０について説
明する。

【００４８】画像入力モジュール９で撮像された画像信
号１１４が、画像記憶モジュール１０で受けとられる
と、その信号はまずディジタル信号に変換され、本モジ
ュール内にある画像メモリに格納される。位置姿勢推定
モジュール７で、ロボットの位置と姿勢の推定の処理が
必要になったときには、ディジタル化されたカメラ画像
データ１１３が位置姿勢推定モジュール７に送出され
る。

【００４９】次に、位置姿勢推定モジュール７について
説明する。

【００５０】位置姿勢推定モジュール７は、推定計画モ
ジュール３で可視であると予測された道標１０７を画像
記憶モジュール１０に格納されたカメラ画像１１３の中
から抽出し、道標のカメラ画像内での位置と世界座標系
での道標の位置と関係を利用して、ロボットの世界座標
系に対する位置と姿勢を推定するモジュールである。推
定計画モジュール３によって与えられる可視な道標の情
報は、その詳細特徴要素の３次元パラメータとして与え
られる。移動制御モジュール４によって与えられる位置
姿勢推定値１０５により、推定すべきロボットの位置と
姿勢から、どのような詳細特徴要素がカメラ画像のどこ
に位置するかを予測することができる。たとえば、いま
画像入力モジュール９のカメラの較正行列をＴとし、ロ
ボットの位置姿勢を表すパラメータを前述したようにｐ
＝（ｐｘ，ｐｙ，φ）とする。このとき、世界座標系の
３次元点Ｐ（ｘ，ｙ，ｚ）をカメラによって撮影したと
き、その画像内の像の位置Ｐ′（ｒ，ｃ）は、遠近効果
を表すパラメータｗを用いて、

【数１】 と表すことができる。ここにＨは世界座標系からロボッ
ト座標系への変換を表す変換行列であり、

【数２】 となる。実際、式（１）から画像点Ｐ′（ｒ，ｃ）は、
ｒ＝ｒｗ／ｗ、ｃ＝ｃｗ／ｗという計算によって求める
ことができる。上記の計算によって、可視な道標の詳細
特徴要素の画像内の位置の予測を立てることができる。
例えば、図６のロボットの位置から予測される詳細特徴
要素群は、図７のようになる。図７では、８個の詳細特
徴要素Ｓ1 ，Ｓ2 ，Ｓ3 ，…，Ｓ8 が予測される。本実
施例では、こうした図のことを予測図と呼ぶことにし、
詳細特徴要素で予測図の中に現れたものをモデル特徴と
呼ぶことにする。

【００５１】こうしたモデル特徴は、たとえば直線成分
の端点など、特徴点の集合として表現することが可能と
なる。たとえば、予測画内のモデル特徴Ｓ1 は、２個の
端点Ｑ1 ′とＱ2 ′によって表現できる。これら予測画
内の特徴点に対応する詳細特徴要素内の３次元点をＱ1
，Ｑ2 とすると、位置姿勢推定モジュール７では、こ
の両者の座標を利用する。

【００５２】さて位置姿勢推定モジュール７では、続い
てモデル特徴をカメラ画像内から抽出することを行う。
これに際して図８に示されたように、以下のステップを
とる。

【００５３】ステップＳ１：モデル特徴のインデックス
ｋを定義し、その初期値を１とする。またモデル特徴の
総数をＮとする。

【００５４】ステップＳ２：各モデル特徴を画像内に探
索する領域Ｗ(k) を設定する。これは、予測図内のモデ
ル特徴の位置のまわりに適当なウィンドウを設定するこ
とにより実現する。このウィンドウは、ある一定の大き
さであってもよいし、ロボットが移動し距離や時間に応
じて可変であってもよい。図９は、直線成分のモデル特
徴（ｋ）に対する探索領域Ｗ(k) を表したものである。

【００５５】ステップＳ３：ステップＳ２で設定された
ウィンドウ内Ｗ(k) で、モデル特徴（ｋ）に類似した画
像特徴を抽出する。この際、ウィンドウ内に類似した複
数の画像特徴が抽出される場合があり得る。そうした場
合には、その全てを候補としてＣ(k) に格納する。

【００５６】例えばモデル特徴が図７に示されたような
直線成分のときには、原画像に微分処理を施すことによ
ってエッジ画像を得た後、そのモデル直線特徴と傾きが
ある差θ(k) 以内の画像直線をハフ変換などを利用して
抽出することができる。こうして画像内で得られた直線
成分は、以下のようなデータ構造によって表現される： ＩＤ： １８７ 端点： （ｒ1 ，ｃ1 ），（ｒ2 ，ｃ2
） ハフ変換パラメータ： （ρ，γ） ここで、ＩＤは画像直線に固有の番号を表し、端点（ｒ
1 ，ｃ1 ），（ｒ2 ，ｃ2 ）は、画像特徴の端点の画像
内座標を表す。ハフ変換パラメータ（ρ，γ）は、図１
０に示すような直線を記述するためのパラメータであ
り、ρは画像原点から画像直線までの距離、γは直線の
法線が画像座標軸ｒとなす角度を表す。

【００５７】ステップＳ４：次のモデル特徴を画像内か
ら探索するように、インデックスｋの値を１増やす。

【００５８】ステップＳ５：インデックスｋとモデル特
徴の総数Ｎとが比較される。もし、ｋがＮを超えた場合
には、全てのモデル特徴に対して画像特徴が求まったこ
とになり、ステップＳ６に進む。そうでなければ、ステ
ップ２に戻る。

【００５９】ステップＳ６：このステップでは、予測画
内のモデル特徴とステップＳ３で抽出された画像特徴と
の間の最適な対応を探索し、その対応に基づいて、ロボ
ットの位置姿勢を推定する。

【００６０】基本的なステップは以上のようである。以
下では、ステップＳ６で行われる処理について、詳しく
説明する。

【００６１】いまモデル特徴として直線成分を考える
と、その直線成分を３次元空間内で記述するのには、そ
の直線成分上の１点（ｘ1 ，ｙ1 ，ｚ1 ）と直線の方向
余弦（ａ，ｂ，ｃ）を用いて表現することができる。す
なわち、直線上の各点（ｘ，ｙ，ｚ）は、

【数３】 ここで、ｕは直線の各点への媒介変数である。同様に、
抽出された画像特徴は、ハフ変換パラメータ（ρ，γ）
を用いて、

【数４】 と書くことができる。ここで、ｓは直線の各点への媒介
変数である。モデル特徴点（ｘ，ｙ，ｚ）と画像特徴点
（ｒ，ｃ）が対応したと仮定すると、式（１）（３）
（４）を利用して、ロボットの位置姿勢を表す未知の３
個のパラメータｐｘ，ｐｙ，φの間の制約方程式を得る
ことができる。実際測定するパラメータは、ρ，γの２
個であるから、

【数５】 の２個の制約方程式が得られる。（具体的な制約方程式
の形については、文献１を参照。）もし、モデル特徴が
点（ｘ，ｙ，ｚ）であり、その画像特徴が点（ｒ，ｃ）
であるときにも、測定するパラメータの数が２であるか
ら、式（５）と同様に、２個の制約方程式を求めること
ができる。同様に、任意のモデル特徴に関して、独立に
ｍ個のパラメータを測定したとすれば、ｍ個の制約方程
式を求めることができる。

【００６２】いまモデル特徴がｎ個あるとし、もし各モ
デル特徴ｓ_k （ｋ＝１，２，…，ｎ）に正しく対応する
画像特徴をｂ_k と書くことにする。すると正しい対応の
組み合わせ（ｓ_k ，ｂ_k ）（ｋ＝１，２，…，ｎ）は、
以下のような制約方程式を与える。

【００６３】

【数６】 ここに、ｆ1 ，…，ｆＭは独立な方程式であると仮定す
る。Ｍは整数で制約条件の数を表わす。

【００６４】実際に、画像特徴を抽出する際には、雑音
（ノイズ）が混入してしまうので、上記の制約方程式
は、必ずしも正確に満たされるわけではない。実際的に
は、ある許容誤差εを認めて、正しい対応の組み合わせ
は、次のような不等式を満たすと考える。

【００６５】

【数７】 ここで、もう一度、モデル特徴と画像特徴の対応問題を
考える。各モデル特徴ｓ_k に関して、探索領域Ｗ(k) 内
で抽出された画像特徴候補をｃ_k1，ｃ_k2，…，ｃ_k,mkと
する。もちろん、この探索領域Ｗ(k) 内にモデル特徴ｋ
に対応するものがない可能性もある。例えば、障害物が
そのモデル特徴を隠してしまった場合には、対応する画
像特徴はｃ_k1，ｃ_k2，…，ｃ_k,mkの中にない。

【００６６】このとき、モデル特徴と画像特徴の間の正
しい対応を探すということは、各モデル特徴にｓ_k に対
して、上記の不等式（７）を満たすような画像特徴ｂ_k
をｃ_k1，ｃ_k2，…，ｃ_k,mkの中から捜すことと考えるこ
とができる。

【００６７】我々に与えられた最終問題は、ロボットの
位置姿勢パラメータ（ｐｘ，ｐｙ，φ）を推定すること
にあるが、このパラメータ数（制約数）は３である。一
方、モデル特徴ｓ_k と画像特徴ｂ_k の対応関係が作り出
す制約方程式の制約数はＭとなる。いまＭ＞３であると
仮定する。こう仮定できるのは、推定計画モジュール３
において、ある一定以上の数のモデル特徴が得られるよ
うに、環境の観察の方法を選択しているからである。

【００６８】この仮定のもとで、式（７）を満たすモデ
ル特徴と画像特徴の対応の正しい組み合わせを如何にし
て求めるかを説明する。本実施例では、モデル特徴と画
像特徴の対応の組み合わせの中で、少なくともＮ（Ｎ＞
３）個の制約数を持つ独立な制約不等式を満たすもの
は、正しいモデル特徴と画像特徴の組み合わせであると
仮定する。例えば、ｈ個のモデル特徴ｓ₁ ，ｓ₂ ，…，
ｓ_h と画像特徴ｃ₁ ，ｃ₂ ，…，ｃ_h による対応の組み
合わせ（ｓ₁ ，ｃ₁ ），（ｓ₂ ，ｃ₂ ），…（ｓ_h ，ｃ
_h ）から得られるパラメータ（ｐｘ，ｐｙ，φ）が制約
不等式

【数８】 を満たし、Ｈ≧Ｎを満たすならば、（ｓ₁ ，ｃ₁ ），
（ｓ₂ ，ｃ₂ ），…（ｓh，ｃh ）を正しい組み合わせ
であり、（ｐｘ，ｐｙ，φ）を求めるべきパラメータの
推定値である。ここで、組み合わせ（ｓ₁ ，ｃ₁ ），
（ｓ₂ ，ｃ₂ ），…（ｓ_h ，ｃ_h ）から得られるパラメ
ータ（ｐｘ，ｐｙ，φ）は、制約方程式

【数９】 なる非線形方程式を適当な方法で解くことによって得ら
れる。ただし、Ｈはある整数で制約条件の数（制約数）
である。この方程式では、解（ｐｘ，ｐｙ，φ）の初期
推定値が、移動制御モジュール４が与える事前推定値１
０５として、与えられているので、各種の非線形方程式
の解法を利用することができる。例えば、Newton法、最
急勾配法、共役勾配法などを利用すればよい。また、制
約数Ｈが３より大きいときには、統計的な処理によって
最も誤差を小さくする解を求める方法なども多く提案さ
れているので、そうした方法を利用すればよい。

【００６９】以下、モデル特徴と画像特徴の組み合わせ
問題を如何に探索するかを説明する。 まず、全てのモ
デル特徴に関して、モデル特徴と画像特徴候補の組Ｑj
をリストとして作成する（ｊ＝１，２，…，Ｌ）：

【数１０】 図１１は、こうした組み合わせを表現するために、作成
されたものである。図には、予測画に現れた４本のモデ
ル特徴Ｓ1 ，Ｓ2 ，Ｓ3 ，Ｓ4 が存在し、それらは破線
で表示されている。一方、各モデル特徴に対して画像内
から抽出された画像特徴候補も、この図の中に実線で表
示されている。特徴Ｓ1 では画像特徴候補ａ，ｂ、モデ
ル特徴Ｓ2 では画像特徴候補ｃ，ｄ、モデル特徴Ｓ3 で
は画像特徴候補ｅ、モデル特徴Ｓ4 では画像特徴候補
ｆ，ｇが抽出されたとする。すると、

【数１１】 となるモデル特徴と画像特徴候補の対応が作られる。こ
こで、Ｑ1 ，Ｑ2 ，Ｑ3，Ｑ4 ，Ｑ5 ，Ｑ6 ，Ｑ7 が作
る独立な制約方程式（６）の制約数は、それぞれ２，
２，１，１，２，１，１である。

【００７０】次に、以下の方法をとる： （１）ＬＩＳＴを空集合に設定する。

【００７１】（２）インデックスｉを１からＬまで変え
て、処理（ａ）または（ｂ）をする。

【００７２】(a) もしＱi が制約数３以上の制約方程式
を作り、その方程式（９）の解（ｐｘ，ｐｙ，φが制約
不等式（８）を満たすならば、ＬＩＳＴに｛Ｑi ｝を加
える。

【００７３】(b) もしＱi が制約数３以上の制約方程式
を作らなければ、(b.1) インデックスｊを１からＬまで
変えて、処理（b.1.1)または（b.1.2)を行なう。

【００７４】(b.1.1) もし｛Ｑi ｝と｛Ｑj ｝がＬＩ
ＳＴの要素でなく、次数３以上の制約方程式を作り、そ
の方程式（９）の解（ｐｘ，ｐｙ，φ）が、制約不等式
（８）を満たすならば、ＬＩＳＴに｛Ｑi ，Ｑj ｝を加
える。

【００７５】(b.1.2) もしＱi とＱj が制約数３以上
の制約方程式を作らなければ、(b.1.2.1) インデックス
ｋを１からＬまで変えて、以下の処理をする。

【００７６】(b.1.2.1.1) もし｛Ｑi ｝，｛Ｑj ｝，
｛Ｑk ｝，｛Ｑi ，Ｑj｝，｛Ｑi ，Ｑk ｝，｛Ｑj ，
Ｑk ｝がＬＩＳＴの要素でなく、制約数３以上の制約式
を作り、その方程式（９）の解（ｐｘ，ｐｙ，φ）が、
制約不等式（８）を満たすならば、ＬＩＳＴに｛Ｑi ，
Ｑj ，Ｑk ｝を加える。

【００７７】（３）ＬＩＳＴをその要素が作る制約方程
式の制約数の高い順に整列する。その要素をＲ1 ，Ｒ2
，…，ＲＭとする。

【００７８】（４）次の操作をＬＩＳＴ内の要素Ｒi に
ついてＲ1 ，Ｒ2 ，…の順に、解が発見できるまで行
う。Ｒi に含まれていないモデル特徴ｓに対して、その
モデル特徴ｓと画像特徴の組が作る制約不等式（８）
を、Ｒi が作る解（ｐｘ，ｐｙ，φ）が満たすようなモ
デル特徴ｓが少なくともＮ−３個存在するなら、Ｒi を
正しい対応とし、（ｐｘ，ｐｙ，φ）をその推定値とす
る。そうでなければ、次のＲi+1 について調べる。

【００７９】図１１の例では、ＬＩＳＴは、

【数１２】 が構成される。ここで、全てのＲi （ｉ＝１，２，…１
５）は、制約数３の制約方程式を形成する。

【００８０】本例でＮ＝５が解が満足する最小制約数と
すると、Ｒ1 から順に上記（４）の処理を行い、結局、
｛Ｑ1 ，Ｑ4 ，Ｑ5 ，Ｑ7 ｝が、制約数５の制約方程式
の解を与える組み合せとして求まる。

【００８１】この方法では、あかじめ可能性のある解の
候補をリストアップしておき、その中で解として有望で
あるものから順に、その妥当性を検証してゆく方法をと
っている。言い替えれば、ＬＩＳＴの可能性のある解の
候補をリストアップしておき、それを制約次数の高い順
に整列させることで、有望な解の候補を先に検証できる
ようにし、有望な解から順にその妥当性を制約方程式の
制約数によって検証しているわけである。

【００８２】この方法では、制約数Ｈ≧Ｎを満たす解が
存在しない場合ももちろんありうる。これは、おもに障
害物などがモデル特徴を遮蔽してしまっている場合であ
る。この場合の対処の方法は、後述する第２実施例で説
明する。

【００８３】以上のような方法によって、モデル特徴と
画像特徴との対応と新しいロボットの位置姿勢推定値を
求めることができる。ここでは、モデル特徴と画像特徴
の対応とロボットの位置姿勢推定を同時に求める一例を
示したが、上記文献２に示す従来の方法のようにモデル
特徴と画像特徴の最適な対応を適当な評価関数を利用し
て求めた後、その対応関係が作り出す制約方程式から新
しいロボットの位置を推定してもよい。

【００８４】次に、位置更新モジュール８について説明
する。

【００８５】位置姿勢推定モジュール７で環境が観察さ
れた地点のロボットの位置姿勢が推定されながら、ロボ
ットは移動を続ける。推定の処理が終った時刻Ｎでのロ
ボットの位置姿勢は、環境が観察された場所とは異なっ
ているので、時刻Ｎでの場所がどこにあたるのかを新し
い推定値の基づいて更新する必要がある。いま移動制御
モジュール４が算出したロボットの事前位置姿勢推定値
をｐ(O) ＝（ｐｘ(O)，Ｐｙ(O) ，φ(O))，ｐ(N) ＝
（ｐｘ(N) ，ｐｙ(N) ，φ(N))として、位置姿勢推定モ
ジュール７で求められた新しい位置姿勢推定値をｐ^new
(O) ＝（ｐｘ^new(O），ｐｙ^new (O) ，φ^new (O))とす
ると、時刻Ｎでの新しいロボットの位置姿勢推定値は以
下のように計算することができる。いまθを

【数１３】 と定義したとき、

【数１４】 によって与えられる。こうして、この位置更新モジュー
ル８では時刻Ｎでの新しいロボットの位置姿勢推定値を
計算する。

【００８６】以上、各モジュールの機能について説明し
てきた。次に各モジュールがどのような時間的スケジュ
ールのもとで、機能を果たすかを図１２のタイムチャー
トを利用して説明する。図において、各矢印は、その矢
印に対応する処理があるモジュールでなされることを示
し、上記三角マークは画像が画像入力モジュール９で入
力された瞬間を表す。

【００８７】図１２において、時刻ｔ0 において、経路
計画モジュール１は現在地点Ａから目的地Ｂまでの経路
を計画を開始する。その計画作成が時刻ｔ1 に終了する
と、移動制御モジュール４はその経路に従って、ロボッ
トの移動を制御する。ある一定の距離を進んだところ
（時刻ｔ2 ）で、移動制御モジュール４は、いったんロ
ボットの移動を停止し、推定計画モジュール３にその地
点での位置姿勢の推定値１０５を送る。ロボットの現在
地の位置姿勢推定値１０５を受け取った推定計画モジュ
ール３は、どのような方法でロボットの位置と姿勢を推
定したらよいかを計画する（もし必要ならば、カメラの
方向を変えたり、ロボットを移動させたりする。この場
合、その計画が終了すると、経路計画モジュール１が再
び観察位置から目的地までの経路を再計画する。これ
は、推定計画モジュール３でロボットが移動した時のみ
必要な作業である。）こうして、ロボットの位置姿勢の
推定方法（環境の観察方法）が決定した時刻ｔ3 から、
ロボットの位置姿勢推定が、位置姿勢推定モジュール７
で開始される。位置姿勢推定のために画像入力モジュー
ル９で環境が撮像され、その画像が画像記憶モジュール
１０に格納された後、その撮像された際のロボットの位
置と姿勢（地点Ｃとする）がまずｐ(O) として、移動制
御モジュール４から各モジュールに送られる。続いて、
ロボットの移動は再開される。ロボットの移動が再開さ
れると、移動制御モジュール４は各時刻のロボットの位
置姿勢の推定値１０５を算出する。

【００８８】位置姿勢モジュール７が、地点Ｃの事前推
定値ｐ(O) に対応する移動ロボットの位置と姿勢の新し
い推定値ｐ^new (O) を算出したとき、移動制御モジュー
ル４がロボットの移動をいったん停止し、停止した地点
Ｄでのロボットの事前位置姿勢履歴ｐ(N) が移動制御モ
ジュール４で算出される。時刻ｔ4 において位置更新モ
ジュール８は、地点Ｃでのロボットの新しい位置姿勢推
定値ｐ^new (O) の値を利用して、移動制御モジュール４
で算出された地点Ｄでの事前位置姿勢推定値ｐ(N) を更
新して、地点Ｄの新しい推定値ｐ^new (N) を算出する。

【００８９】こうして更新が終了したのち、時刻ｔ5 に
おいて現時点Ｄでのロボットの新しい位置姿勢推定値ｐ
^new (N) を使用して、経路計画モジュール１がｐ
^new (N) から目的地までの経路を計画する。

【００９０】以上の時刻ｔ0 から時刻ｔ5 までを１サイ
クルとして、移動ロボットは目的地付近の到達するま
で、このサイクルを繰り返す。そして、移動制御モジュ
ール４は、ロボットの移動の際、目的地と現在位置（ｐ
ｘ，ｐｙ，φ）の推定値とを比較し、その差がある閾値
より小さくなるまでロボットの移動を制御する。一方、
その差がある閾値より小さくなったときは、目的地に到
達したことを、経路計画モジュールに報告するととも
に、移動を終了する。

【００９１】以上、第１の実施例について説明した。文
献１に述べられているような従来の方法では、位置姿勢
推定モジュール７がロボットの位置と姿勢を推定してい
る間、すなわち時刻ｔ3 からｔ4 までの間、ロボットの
移動を完全に停止させていなければならなかった。本発
明の方法によれば、ロボットが位置推定を行っている間
も、ロボットを移動させることができるので、より高速
なロボットの誘導を実現することができる。

【００９２】以上では、ロボットの位置姿勢推定をする
時期を、それまでロボットが進んだ距離によって制御し
た例を説明したが、そのかわりにロボットが移動した時
間などによって制御してもかまわない。また、後述する
第６の実施例で説明するように、ロボットの位置姿勢に
含まれる不確定度をモデル化して、その不確定度によっ
て制御してもかまわない。

【００９３】以下に第２実施例について説明する。

【００９４】環境には、未知の障害物が含まれている場
合がある。こうした場合は、観察する画像内に障害物が
現れ、その障害物が本来見えるべきモデル特徴を遮蔽し
てしまうような場合である。第２実施例は、障害物が多
くある環境でも、安定でかつ高速なロボットの誘導が実
現可能であることを表す実施例である。

【００９５】実際に障害物がロボットの環境観察を妨害
する場合は、モデル特徴が障害物によって遮蔽されてし
まう場合がほとんどである。こうした場合には、推定計
画モジュール３が選択したモデル特徴が、画像特徴とし
て抽出されないことを意味する。従って、位置姿勢推定
モジュール７でモデル特徴と画像特徴の正しい対応を探
そうとしても充分な数の対応が得られないこととなる。

【００９６】この場合に対処するために、位置姿勢推定
モジュール７は、第１実施例で表された方法をさらに拡
張することによって、環境の観察が成功したか否か、あ
るいは算出された解が本当に正しい解とみなし得るのか
を、自動的に判断する。

【００９７】具体的には、第１の実施例で説明した制約
不等式に関して、ある制約数以上の制約不等式を満たす
組み合わせが求められないときには、観察が失敗したと
判定する。もし、失敗したと判定した場合には、移動ロ
ボットは、もう一度環境の観察をして、位置姿勢を推定
しなおすわけである。それを実現するために、位置姿勢
推定モジュール７が、移動制御モジュール４と推定計画
モジュール３に観察の失敗したことを告げると、移動制
御モジュール４は、ロボットの移動を一時停止し、推定
計画モジュール３は、再度観察をする計画するわけであ
る。

【００９８】以下、図１３を使用して説明する。図１３
において、時刻ｔ0 において、経路計画モジュール１は
現在地点Ａから目的地Ｂまでの経路を計画を開始する。
その計画作成が時刻ｔ1 に終了すると、移動制御モジュ
ール４はその経路に基づいて、ロボットの移動を制御す
る。ある一定の距離を進んだところ（時刻ｔ2 ）で、移
動制御モジュール４は、いったんロボットの移動を停止
し、推定計画モジュール３にその地点での位置姿勢の推
定値（１０５）を送る。ロボットの現在地の位置姿勢推
定値１０５を受け取った推定計画モジュール３は、どの
ような方法でロボットの位置と姿勢を推定したらよいか
を計画する（もし必要ならば、カメラの方向を変えた
り、ロボットを移動させたりする。この場合、その計画
が終了すると、経路計画モジュール１が再び観察位置か
ら目的地までの経路を再計画する。これは、推定計画モ
ジュール３でロボットが移動した時のみに必要な作業で
ある。）。

【００９９】こうして、ロボットの位置姿勢の推定方法
（環境の観察方法）が決定した時刻ｔ3 から、ロボット
の位置姿勢推定が、位置姿勢推定モジュール７で開始さ
れる。画像入力モジュール９で環境が撮像され、その画
像が画像記憶モジュール１０に格納された後、その撮像
された際のロボットの位置と姿勢（地点Ｃとする）が事
前推定値ｐ(O) として、移動制御モジュール４から各モ
ジュールに送られる。続いてロボットの移動は再開され
る。ロボットの移動が再開されると、移動制御モジュー
ル４は各時刻のロボットの位置姿勢の事前推定値１０５
を算出する。

【０１００】位置姿勢推定モジュール７において、ある
制約数Ｎ以上の制約不等式を満たす解が求められなかっ
たときには、モジュール７は観察が失敗したと判断し、
移動制御モジュール４にロボットの移動と一時停止する
ように命令する。移動制御モジュール４は、その地点の
ロボットの事前推定値１０５を推定計画モジュール３に
送る。推定計画モジュール３は、その事前推定値１０５
に基づいて、観察方法の計画を行う。こうして、観察方
法が決定した時刻ｔ5 から、位置姿勢推定モジュール７
で、ロボットの位置姿勢推定が開始する。環境観察のた
め、画像入力モジュール９で環境が撮像され、その画像
が画像記憶モジュール１０に格納された後、その撮像さ
れた際のロボットの位置と姿勢（その地点をＣ′とす
る）の推定値ｐ(O) が事前推定値として、移動制御モジ
ュール４から各モジュールに送られる。続いて、ロボッ
トの移動が、移動成業モジュール４の制御のもとに開始
される。ロボットの移動が再開されると、移動制御モジ
ュール４は、各時刻のロボットの位置姿勢の事前推定値
１０５を算出する。

【０１０１】位置姿勢推定モジュール７は地点Ｃ′の位
置推定の処理をおこなう。もし、その結果が成功の場合
には、地点Ｃ′の推定結果ｐ^new (O) を位置更新モジュ
ール８に送出し、移動制御モジュール４にロボットの移
動を一時停止させる。移動制御モジュール４は、停止し
た後の地点Ｄでのロボットの位置姿勢の事前推定値１０
５をｐ(N) として、位置更新モジュール８に送出する。
位置更新モジュール８は、地点Ｃ′の事前推定値ｐ(O)
とその新しい推定値ｐ^new (O) と地点Ｄにおける事前推
定値ｐ(N) を利用して、地点Ｄでの位置姿勢推定値を更
新し、ｐ^new (N) を算出する。その値は、経路計画モジ
ュール１に送られ、地点Ｄから目的地Ｂまでの経路が再
計画され、移動制御モジュール４はその経路に基づいて
目的地Ｂへの移動を制御する。

【０１０２】もし地点Ｃ′での観察が再度失敗したこと
がわかったときには、上記の観察をくりかえしてロボッ
トの位置姿勢を推定する。

【０１０３】以上の時刻ｔ0 から時刻ｔ7 までを１サイ
クルとして、移動ロボットは目的地付近の到達するま
で、このようなサイクルを繰り返す。そして、移動制御
モジュール４は、ロボットの移動の際、目的地と現在位
置（ｐｘ，ｐｙ，φ）の推定値とを比較し、その差があ
る閾値より小さくなるまでロボットの移動を制御する。
一方、その差がある閾値より小さくなったときには、目
的地に到達したことを、経路計画モジュール１に報告す
るとともに、移動を終了する。

【０１０４】以上、第２実施例について説明した。第２
の実施例によれば、障害物などの影響などで環境の観察
が安定的になされない場合には、その観察の良否を自動
的に判断するようにし、一度の観察が失敗したときには
観察をくりかえすことによって、ロボットの位置姿勢を
安定的に推定することができる。

【０１０５】以下に第３実施例について説明する。

【０１０６】図１４は、第３の実施例を説明するための
タイムチャートである。第３実施例で使用されるモジュ
ールは、第１実施例で使用されたモジュールと基本的に
同じ機能を持つ。しかしながら、その時間的スケジュー
ルが異なり、図１４はその時間的スケジュールを表した
ものである。

【０１０７】第１、２の実施例では、移動ロボットがあ
る一定距離進んだところで、ロボット位置姿勢を推定し
た。本実施例では、移動ロボットが位置姿勢推定の処理
が終了した時点で次の位置姿勢推定の処理を実行する点
が異なる。

【０１０８】図１４において、時刻ｔ0 において、ロボ
ットは一時停止しており、推定計画モジュール３がロボ
ットの位置姿勢推定のための環境観察の計画を立てる
（必要ならば、カメラの方向を変えたり、ロボットの位
置や姿勢を変えたりして、観察の位置に到達した後、環
境の観察が開始される。）。その後、経路計画モジュー
ル１は時刻ｔ１において環境観察をする位置から目的地
までの経路の作成を開始される。

【０１０９】また推定計画が終了した時刻ｔ1 には、位
置姿勢推定モジュール７は、推定計画モジュール３が立
てた計画に基づいて、環境の観察を開始する。この際、
画像入力モジュール９が環境の画像を撮り、その画像信
号１１４が画像記憶モジュール１０に格納される。この
観察した位置（地点Ｃ）でのロボットの位置と姿勢は、
移動制御モジュール４にて推定され、その地点Ｃでの推
定値はｐ(O) とされる。

【０１１０】経路計画が終了した時刻ｔ2 において、移
動制御モジュール４の制御のもとに、経路計画モジュー
ル１が計画した経路に基づいてロボットは移動を開始す
る。この移動の間、移動制御モジュール４が各時刻ｋの
事前位置姿勢推定値ｐ(K) を算出する。

【０１１１】時刻ｔ3 において、観察位置Ｃの事前推定
値ｐ(O) に対する新しい推定値ｐ^new (0) が、位置姿勢
推定モジュール７で得られたとき、移動制御モジュール
４はロボットの移動を一時停止し、停止した時点Ｄでの
ロボットの位置と姿勢の事前推定値ｐ(N) が移動制御モ
ジュール４で算出される。その後、ロボットの位置更新
モジュール８が移動制御モジュール４で算出された時点
Ｄでのロボットの位置姿勢の事前推定値ｐ(N) を更新
し、ｐ^new (N) を作成する。

【０１１２】更新が終った時点ｔ4 において、推定計画
モジュール３が現在位置Ｄの推定値ｐ^new (N) を利用し
て、次の環境観察の方法を計画する。

【０１１３】以上述べてきた時刻ｔ0 から時刻ｔ4 まで
の処理を１サイクルとして、このサイクルを繰り返しな
がら、移動ロボットは目的地付近に到達する。この間、
移動制御モジュール４は、現在の位置姿勢と目的地の位
置と姿勢の差を常時モニタしておき、その差がある閾値
以下になったときには、その移動を終了する。

【０１１４】第３実施例を利用すると、全体の処理の基
本サイクルが短くなる。従って、ロボットの位置と姿勢
の推定値は、第１実施例の場合より真値に近いことが統
計的に予想されるため、位置姿勢推定モジュール７がカ
メラ画像内でモデル特徴をさらに簡単に探索することが
できる。従って、位置姿勢推定モジュール７で処理に必
要な時間が短縮されるという利点がある。また、推定計
画モジュール３での処理を経路計画モジュール１での処
理に先行させることにより、位置姿勢推定モジュール７
での処理と経路計画モジュール１での処理を平行して行
うことができるようになる点と、推定計画モジュール３
で観察のためにロボットが移動する必要ができたときに
も、その後の経路再計画が経路計画モジュール１で直ち
に処理できるようになる点で、処理の効率化を図ること
もできる。

【０１１５】図１４で示されるように、ロボットの移動
とロボットの位置姿勢推定が平行して処理されるため、
ロボットは位置姿勢推定処理の間、その移動を停止する
必要はなく、ロボットの誘導に要する時間が短くなる。
こうして、文献１に示されるような方法に較べて、高速
な処理が実現出来る。また、環境を観察するのに適した
方向など位置姿勢推定に適した方法を計画してから環境
の観察を行うので、特開昭６０−２１７４１２号公報に
述べられている方法よりも安定した位置姿勢推定を実行
することができる。

【０１１６】以下に、第４実施例を説明する。

【０１１７】第４実施例でも、移動ロボットを誘導する
のに必要なモジュール群は第１の実施例の場合と同様で
ある。しかし、それらのモジュールの処理を時間的にど
のようにスケジューリングするかが異なる。

【０１１８】以下、図１５のタイムチャートを使用しな
がら、第４実施例を説明する。現時点ｔ0 において、ロ
ボットは停止している。移動制御モジュール４によって
与えられる現時点でのロボットの事前位置姿勢推定値１
０５を利用して、経路計画モジュール１が現在地の推定
値から目的地Ｂまでの経路を計画する。その経路に基づ
いて、時刻ｔ1 から移動制御モジュール４はロボットを
目的地に向けて移動させる。その移動が開始してからあ
る一定距離を進行したことを移動モジュール４が認識し
た時刻ｔ2 において、推定計画モジュール３は、画像入
力モジュール９と画像記憶モジュール１０に命令して、
ある時刻ｓ1 での環境を撮影し、その画像を画像記憶モ
ジュール１０に格納する。同時にその時刻ｓ1 でのロボ
ットの事前位置姿勢推定値１０５を、推定計画モジュー
ル３が移動制御モジュール４から入手して、時刻ｓ1 で
観察した位置と方向が適切であったか否かを判定する。
この判定は、予測図内のモデル特徴の数などによってな
される。これは、第１実施例で説明した通りである。

【０１１９】もし時刻ｓ1 で観察した位置と方向が不適
切であったときには、別の時刻ｓ2で画像を撮り、時刻
ｓ2 で観察した位置と方向が適切であるか否かを推定計
画モジュール３が判断する。このような操作をロボット
が移動している間、適切な観察画像が得られるまで繰り
返す。図１５に示すように、適切な観察画像が得られた
地点をＣ、その時刻をｓn とし、その観測画像が適切で
あったことの判断が終了した時刻をｔ3 とする。この
間、もちろん移動制御モジュール４は、時刻ｓ1からロ
ボットの事前位置姿勢推定値を算出する。

【０１２０】時刻ｔ3 において、位置推定モジュール７
は、時刻ｔ3 において撮影された画像を利用して、地点
Ｃでのロボットの位置姿勢の推定を開始する。

【０１２１】その推定が終了した時刻ｔ4 において、移
動制御モジュール４はロボットに移動を一時停止し、移
動が停止した時点Ｄでの事前位置姿勢推定値ｐ(N) を、
位置更新モジュール８に送出する。新しく得られた地点
Ｃでの位置姿勢推定値ｐ^new(O) を利用して、位置更新
モジュール８が地点Ｄでの事前推定値ｐ(N) の値を更新
して、その結果ｐ^new (N) は経路計画モジュール１に送
られ、新しい経路が再計画される。

【０１２２】第４実施例の方法では、推定計画モジュー
ルで計画される観察の方法がロボットを一時停止しなく
ても実行できるので、ロボットを停止させない時間分移
動ロボットの誘導を高速化することができる。

【０１２３】以下に、第５実施例を説明する。

【０１２４】第５実施例は、環境内に障害物があまり存
在しないとき、特に有効な移動ロボットの誘導を表した
ものである。本実施例を実現するための機能モジュール
は、第１実施例と同様であるが、それらの機能モジュー
ルでの処理をどのようにスケジューリングするかが、本
実施例と第１、２、３、４の実施例と異なる点である。
具体的には、推定計画モジュール３が、ロボットの移動
に際して、次にロボットが位置姿勢の推定をすべき地点
をあらかじめ決定しておき、ロボットがこの地点付近に
到達した時に、位置姿勢推定の処理を行うものである。
もちろん、この処理が実現できるのは、ロボットの移動
がほぼ予測通りにいく場合、すなわち、障害物が環境内
にあまり存在しない場合である。

【０１２５】以下、図１６のタイムチャートを使用しな
がら、第５実施例を説明する。現時点ｔ0 において、ロ
ボットは停止している。移動制御モジュール４によって
与えられる現時点Ａでのロボットの事前位置姿勢推定値
１０５を利用して、経路計画モジュール１が現在地の推
定値から目的地までの経路を計画する。その経路に基づ
いて、時刻ｔ1 から移動制御モジュール４はロボットを
目的地Ｂに向けて移動させる。移動が開始された後、推
定計画モジュール３は、その計画された経路のどの位置
で、位置姿勢推定のための環境観察を行ったらよいかを
計画する。

【０１２６】図１７にその計画方法を示す。図におい
て、計画された経路上にある一定間隔ごとに代表点Ｐ1
，Ｐ2 ，Ｐ3 ，…ＰＭをとり、出発地点Ａからある距
離以上離れ、かつある一定距離以内の代表点Ｐi に関し
て、その代表点での観察が有効であるかを判定する。図
１７においては、Ｐ3 ，Ｐ4 ，Ｐ5 ，Ｐ6 ，Ｐ7 がその
範囲にある代表点となる。有効性の判定には、その位置
姿勢で予測画内に現れるモデル特徴の数を利用する。も
しモデル特徴の数がある値を越えた時には、有効である
とする。もしこの代表点が複数個存在するときには、現
在地から最遠の代表点を観察点とする。このようにして
決定された観察点のある地点をＣとする。

【０１２７】ロボットの移動は、移動制御モジュール４
によって制御されるが、移動制御モジュール４は、この
観察すべき地点Ｃの付近にロボットが到達したか否か
を、ロボットの位置と姿勢の事前推定値１０５と地点Ｃ
との差を利用して判定する。ロボットが地点Ｃ付近に到
達されたと判定できたとき（時刻ｔ3 ）、移動制御モジ
ュール４がその時刻ｔ3 での事前位置姿勢推定値ｐ(O)
を再度推定計画モジュール３に送るとともに、推定計画
モジュール３が先に計画した観察方法に従って、画像入
力モジュール９が時刻ｔ3 での画像を撮るとともに、そ
の画像信号を画像記憶モジュール１０が記憶する。

【０１２８】時刻ｔ3 での事前推定値１０５を利用し
て、推定計画モジュール３は、再度予測図を作成し、そ
の予測図に含まれるモデル特徴の情報を位置姿勢推定モ
ジュール７に送る。ここで、推定計画モジュール３が再
度予測図を作成する理由は、時刻ｔ3 でのロボットの位
置が必ずしも、推定計画モジュール３が先に観察地とし
て決定しておいた場所と一致しないためである。

【０１２９】位置姿勢推定モジュール７にモデル特徴の
情報が送られた後、時刻ｔ4 において位置姿勢推定モジ
ュール７は、ロボットの位置姿勢推定処理を開始する。
位置姿勢推定の処理が位置姿勢推定モジュール７で終了
したとき、移動制御モジュール４は、ロボットの移動を
一時停止し（その時点をＤとする）、その時点ｔ5 での
ロボットの事前位置姿勢推定値ｐ(N) を位置更新モジュ
ール８に送信する。位置更新モジュール８は、位置姿勢
推定モジュール７で計算された位置姿勢推定値ｐ
^new (O) とその事前推定値ｐ(O) を利用して、ｐ(N) を
更新してｐ^new (N) を算出する。

【０１３０】この処理が終了した時刻ｔ6 において、経
路計画モジュール１は地点Ｄに関する更新された位置姿
勢推定値ｐ(N) を利用して、地点Ｄから目的地Ｂまでの
経路を再計画する。

【０１３１】以上、時刻ｔ1 から時刻ｔ6 までの処理
を、ロボットが目的地Ｂ付近に到達するまで、実行す
る。目的地Ｂに到達したか否かは、移動制御モジュール
４が、事前推定値１０５と目的地Ｂの差をモニタしなが
ら判定する。

【０１３２】第５実施例の方法では、推定計画モジュー
ルで計画される観察の方法がロボットを一時停止しなく
ても実行できるので、ロボットを停止させない時間分移
動ロボットの誘導を高速化することができる。

【０１３３】以下に、第６実施例を説明する。

【０１３４】第６実施例は、移動制御モジュール４で算
出される事前位置姿勢推定値１０５の形式を変え、新し
い形式のもとでロボットの誘導を制御しようとするもの
である。この方法では、移動制御モジュール４、位置姿
勢推定モジュール７、位置更新モジュール８での処理が
異なるとともに、事前位置姿勢推定値の履歴をロボット
の移動にともなって記憶する履歴記憶モジュール５が加
わってくるので、以下に説明する。

【０１３５】図１８は、第６実施例の基本構成を示す図
である。移動ロボットの誘導制御部分は、ロボットが移
動する環境の地図やその環境に存在する道標を記憶する
地図記憶モジュール２と、その地図情報１０１と操作者
からの移動命令１００に基づいて出発地から目的地まで
の経路を計画する経路計画モジュール１と、その経路情
報１０４をロボットが移動できる形の基本コマンドに分
解し実際にロボットの移動機構がその実行しているかを
管理・制御し、その移動の軌跡からロボットの位置と姿
勢を表すパラメータの平均値と共分散行列を組としたロ
ボットの位置と姿勢の事前推定値を算出する移動制御モ
ジュール４と、ロボットの位置と姿勢を環境を観察する
ことによって推定する方法を計画する推定計画モジュー
ル３と、その推定計画に基づいて実際に位置姿勢推定を
行う位置姿勢推定モジュール７と、その位置推定に必要
な画像を撮影する画像入力モジュール９と撮影された画
像信号を記憶する画像記憶モジュール１０と、移動制御
モジュール４が算出する事前位置姿勢推定値をロボット
の移動の間、順次記憶する履歴記憶モジュール５と、位
置姿勢推定モジュール７が新しいロボットの位置姿勢推
定値を計算したとき、履歴記憶モジュール５に記憶され
た推定値の履歴を参考にして事前推定値を更新する位置
更新モジュール８と、移動ロボットに前方にある障害物
を検知し、障害物がある領域の情報を移動制御モジュー
ル４に告知する障害物検知モジュール６からなる。

【０１３６】以下、第１実施例と機能の異なるモジュー
ルに関して、その機能の説明をする。

【０１３７】まず、移動制御モジュール４について説明
する。

【０１３８】第１実施例では、ロボットの位置と姿勢の
事前推定値が、（ｐｘ，Ｐｙ，φ）の形で算出された。
文献１で詳しく述べられているように、ロボットの走行
状態によって起こり得るロボットの位置の不確定度は、
２次統計量によって近似することができる。すなわち、
ロボットの位置と姿勢のパラメータｐ＝（ｐｘ，ｐｙ，
φ）をその平均値ｐバーと共分散行列Σ_p によって表現
することができる。

【０１３９】具体的には、ロボットの移動を基本直線移
動と基本回転移動で近似し、各基本移動によって、ロボ
ットの位置姿勢推定値がどのように変化するかを計算す
るわけである。図１９は、この方法を説明するためのも
のである。図１９（ａ）は、移動制御モジュール４によ
って、計測されたＡＢ間の経路である。図１９の点Ａ，
Ｂで示されている矢印は、Ａ，Ｂにおけるロボットの方
向を示す。この曲線経路ＡＢを、その下図（ｂ）で示さ
れるように、線分によって近似し、その端点をＰ1 ，Ｐ
2 ，…，ＰＭとする。直線部分にマークされたＬ0 ，Ｌ
1 ，…，ＬＭは、基本直線移動を表し、その直線成分と
矢印とがなす角度部分にマークされたＲ1 ，Ｒ2 ，…Ｒ
Ｍ，ＲＢは、その角度分の基本回転移動を表す。する
と、ＡからＢまで進行する間に、以下のような基本移動
Ｒ0 ，Ｌ0 ，Ｒ1 ，Ｌ1 ，Ｒ2 ，Ｌ2 ，…ＲＭ-1，ＬＭ
-1，ＲＭ，ＬＭ，ＲＢの組み合せによって、ＡからＢま
でのロボットの移動を近似することができる。

【０１４０】

【数１５】 いま基本移動Ｓ（Ｒi またはＬi ）の前の位置姿勢のパ
ラメータの平均値をｐバー、共分散行列をΣ_p とし、基
本移動Ｓの後の位置姿勢のパラメータの平均値ｐ′バー
とその共分散行列をΣ_{p ′}とすると、文献１に述べられ
ているように、

【数１６】 一方、移動制御モジュール４は、こうした位置姿勢パラ
メータの変化をモニタする。一般的に、推定値の共分散
行列内の分散が大きくなったときには、そのパラメータ
の不確定度が大きくなったということができる。移動制
御モジュール４では、共分散行列Σ_p のｐｘ，ｐｙに関
する分散とφに関する分散とを別々にモニタしながら、
不確定度の増加を管理する。後述するように、そのいず
れかの分散がある閾値を越えたときには、不確定度が十
分大きくなったと判断し、ロボットの移動を一時停止す
ることができる機能を持つ。

【０１４１】次に、履歴記憶モジュール５について説明
する。

【０１４２】履歴記憶モジュール５は、移動制御モジュ
ール４で算出されるロボット位置・姿勢のパラメータｐ
の事前推定値（ｐバー，Σ_p ）を順次記憶するモジュー
ルである。具体的には、時刻ｋの推定値ｐ(K) を

【数１７】 の点列の形で記憶することになる。これらの事前推定値
は、位置更新モジュール８に送られる。

【０１４３】次に、位置姿勢推定モジュール７について
説明する。

【０１４４】位置姿勢推定モジュール７での処理は、文
献１に述べられている方法と同様である。推定計画モジ
ュール３が、地点Ｃでのロボットの位置姿勢推定値を利
用して、予測図を作成し、その予測図に現れるモデル特
徴群を、位置姿勢推定モジュール７は受け取る。こうし
て受け取った特徴群が画像内と特徴空間内（ハフ空間
内）で存在し得る領域を限定する。この限定を実行する
に際しては、移動制御モジュール４が算出した事前位置
姿勢推定量を画像空間と特徴空間に伝播することによっ
て行う。

【０１４５】こうしてモデル特徴の存在する空間を限定
したのち、その限定空間を探索領域として、ハフ変換な
どを利用してモデル特徴に対応する画像特徴候補を抽出
する。もちろん画像特徴は一意に抽出できるとは限らな
い。そこで、カルマン・フィルタを利用して、モデル特
徴に対する最適な画像特徴の対応関係を求め、その対応
関係から地点Ｃにおけるロボットの位置姿勢推定値を求
めるわけである。こうして求められた推定値は、

【数１８】 の形で表現され、位置更新モジュール８に送られる。

【０１４６】次に、位置更新モジュール８について説明
する。

【０１４７】

【数１９】 によって時刻ｋでの平均値が与えられる。この後、これ
ら平均値の点列が作る経路を、移動制御モジュール４の
項で説明したように、基本移動に分解して、各基本移動
ごと、位置推定値の共分散行列を求めていけばよい。式
（１８）、（１９）において、地点Ａの推定値ｐ
^new (O) として、地点Ｂの推定値ｐ^new (N) とするわけ
である。こうして、このモジュール８では時刻Ｎでの新
しいロボットの位置姿勢推定値ｐ^new (N) を計算する。

【０１４８】次に、その他のモジュールについて説明す
る。

【０１４９】その他のモジュールでの処理は、基本的に
第１実施例と同じである。ただし、本実施例では、事前
位置推定値が（ｐバー，Σ_P ）の形で表現される。その
他のモジュールでは、事前位置推定値に含まれる平均値
ｐバーを第１実施例の場合の事前位置推定値として使用
する。

【０１５０】以上、各モジュールの機能について説明し
てきた。次に各モジュールがどのような時間的スケジュ
ールのもとで機能を果たすかを図２０を利用して説明す
る。図２０において、時刻ｔ0 において、経路計画モジ
ュール１は現在地点Ａから目的地Ｂまでの経路を計画を
開始する。その計画作成が時刻ｔ1 に終了すると、移動
制御モジュール４はその経路に従って、ロボットの移動
を制御する。この間、移動制御モジュール４は、ロボッ
トの位置姿勢推定値を各時刻ごとに算出する。そして、
その推定値の共分散行列のｐｘ，ｐｙの分散がある閾値
を越えるか、またはφの分散がある閾値を越えたときに
は、ロボットの位置姿勢パラメータの不確定度が大きい
と判断し、移動制御モジュール４は、いったんロボット
の移動を停止し、推定計画モジュール３にその地点での
事前位置姿勢の推定値（１０５）を送る。

【０１５１】ロボットの現在地の事前位置姿勢推定値１
０５を受け取った推定計画モジュール３は、どのような
方法でロボットの位置と姿勢を推定したらよいかを計画
する（もし必要ならば、カメラの方向を変えたり、ロボ
ットを移動させたりする。この場合、その計画が終了す
ると、経路計画モジュール１が再び観察位置から目的地
までの経路を再計画する。これは、推定計画モジュール
３でロボットが移動した時のみ必要な作業である。）。

【０１５２】こうして、ロボットの位置姿勢の推定方法
（環境の観察方法）が決定した時刻ｔ3 から、ロボット
の位置姿勢推定が、位置姿勢推定モジュール７で開始さ
れる。位置姿勢推定のために画像入力モジュール９で環
境が撮像され、その画像が画像記憶モジユール１０に格
納された後、その撮像された際のロボットの位置と姿勢
（地点Ｃとする）がまずｐ(O) として、移動制御モジュ
ール４から履歴記憶モジュール５に送られて記憶され
る。続いて、ロボットの移動は再開される。ロボットの
移動が再開されると、移動制御モジュール４は各時刻ｋ
のロボットの位置姿勢の推定値ｐ(k) を算出し、履歴記
憶モジュール５に送る。これを受け取った履歴記憶モジ
ュール５は、その推定値ｐ(k) を記憶する。

【０１５３】位置姿勢モジュール７が、地点Ｃの事前推
定値ｐ(O) に対応する移動ロボットの位置と姿勢の新し
い推定値ｐ^new (O) を算出したとき、移動制御モジュー
ル４がロボットの移動をいったん停止し、停止した地点
Ｄでのロボットの事前位置姿勢履歴ｐ(N) が移動制御モ
ジュール４で算出される。その事前位置姿勢推定値ｐ
(N) は、履歴記憶モジュール５に記憶される。時刻ｔ4
において位置更新モジュール８は、地点Ｃでのロボット
の新しい位置姿勢推定値ｐ^new (O) の値を利用して、履
歴記憶モジュール５に記憶された位置姿勢の履歴｛ｐ
(k) ｝を順次更新し、最後に地点Ｄでの事前位置姿勢推
定値ｐ(N) を更新して、地点Ｄの新しい推定値ｐ
^new (N) を算出する。

【０１５４】こうして更新が終了した時刻ｔ5 におい
て、線路計画モジュール１は現時点Ｄでのロボットの新
しい位置姿勢推定値ｐ^new (N) を使用して、ｐ^new (N)
から目的地Ｂまでの経路を計画する。

【０１５５】以上の時刻ｔ0 から時刻ｔ5 までを１サイ
クルとして、移動ロボットは目的地付近の到達するま
で、このサイクルを繰り返す。そして、移動制御モジュ
ール４は、ロボットの移動の際、目的地と現在位置の平
均推定値とを比較し、その差がある閾値より小さくなる
までロボットの移動を制御する。一方、その差がある閾
値より小さくなったときには、目的地に到達したことと
判断し、その旨を経路計画モジュール１に報告するとと
もに、移動を終了する。

【０１５６】以上、第６実施例について説明した。本実
施例では、文献１に述べられているのと同様な方法で、
ロボットの位置と姿勢の不確定度をモニタし、その大き
さに応じて、ロボットの移動を一時停止して位置姿勢の
推定を実行する方法を採用した。この結果、第１から第
５までの実施例と異なり、ロボットが移動した経路の複
雑さに応じてロボットの位置推定の間隔を制御すること
ができるため、より安定したロボットの誘導が実現でき
る。例えば、ロボットが障害物を退避して、回転や直進
を繰り返すと位置の不確定度が急激に増大するので、ロ
ボットの位置推定を短い時間間隔で行わなければならな
いが、第６実施例では、こうした制御が可能となる。

【０１５７】しかしながら、文献１に述べられているよ
うな従来の方法では、位置姿勢推定モジュール７がロボ
ットの位置と姿勢を推定している間、すなわち時刻ｔ3
からｔ4 までの間、ロボットの移動を完全に停止させて
いなければならなかった。本実施例によれば、ロボット
が位置推定を行っている間も、ロボットを移動させるこ
とができるので、より高速なロボットの誘導を実現する
ことができる。第６実施例では、ロボットの位置姿勢を
表すパラメータを不確定度（平均値と共分散行列）を付
加して表現した。この方法を用いて、第２、３、４、５
実施例に表されているスケジューリングのもとでロボッ
トを誘導することが出来ることを記しておく。

【０１５８】以上、６個の実施例では、ロボットの位置
姿勢を表すパラメータと３次元ベクトル（ｐｘ，ｐｙ，
φ）として、それを推定する方法を中心に述べてきた。
本発明で提案されている方法は、ロボットの位置姿勢
は、３次元ベクトルである必要はない。より一般的に、
ロボットの位置姿勢を表すパラメータとして、６次元ベ
クトル（ｐｘ，ｐｙ，ｐｚ，φｘ，φｙ，φｚ）を用い
たとしても、基本的な動作は変わらない。

【０１５９】また、道標を構成するモデル特徴として、
直線特徴を例として説明してきたが、これもパラメータ
できるより一般的な特徴を利用することが可能であるこ
とも記しておく。

【０１６０】さらに、第２実施例では、観察が失敗した
ときに直ちに再観察を行う方法を述べた。この再観察の
方法は、第３、４、５、６実施例でも組み合わせて使用
することができる。

【０１６１】

【発明の効果】以上詳述したように、本発明の移動ロボ
ットの誘導制御装置によれば、ロボットが位置姿勢の推
定を行っている間も、できる限りロボットを移動させる
ことにより、高速な移動ロボットの誘導を実現すること
ができる。

【図面の簡単な説明】

【図１】第１実施例の基本構成を示す図である。

【図２】部分的に平面で記述できる環境を移動する場合
のロボットの動作を説明するための図である。

【図３】図２に表された環境について、これら基本要素
としての境界線を表した図である。

【図４】道標の例を表した図である。

【図５】経路計画モジュールが作成した経路を表した図
である。

【図６】推定計画モジュールの処理を説明するための図
である。

【図７】図６のロボットの位置から予測される詳細特徴
要素群を示す図である。

【図８】モデル特徴をカメラ画像内から抽出する工程を
示すフローチャートである。

【図９】直線成分のモデル特徴（ｋ）に対する探索領域
ｗ(k) を表した図である。

【図１０】ハフ変換パラメータによって記述された直線
を示す図である。

【図１１】モデル特徴と画像特徴の組み合わせを表現す
るために作成された図である。

【図１２】各モジュールがどのような時間的スケジュー
ルのもとで、機能を果たすかを説明するための図であ
る。

【図１３】第２実施例を説明するためのタイムチャート
である。

【図１４】第３実施例を説明するためのタイムチャート
である。

【図１５】第４実施例を説明するためのタイムチャート
である。

【図１６】第５実施例を説明するためのタイムチャート
である。

【図１７】位置姿勢推定のための環境観察を計画された
どの位置で行ったらよいかを説明するための図である。

【図１８】第６実施例の基本構成を示す図である。

【図１９】ロボットの位置姿勢推定値がどのように変化
するかを計算するための図である。

【図２０】各モジュールがどのような時間的スケジュー
ルのもとで機能を果たすかを説明するための図である。

【符号の説明】

１…経路計画モジュール、２…地図記憶モジュール、３
…推定計画モジュール、４…移動制御モジュール、６…
障害物検知モジュール、７…位置姿勢推定モジュール、
８…位置行進モジュール、９…画像入力モジュール、１
０…画像記憶モジュール。

Claims (1)

【特許請求の範囲】
1. 【請求項１】 環境の地図を記憶する手段と、 その地図を利用しロボットの経路を計画する経路計画手
   段と、 その経路情報に基づいてロボットの移動を制御しロボッ
   トの位置・姿勢の事前推定値を算出する移動制御手段
   と、 前記地図を利用し環境の一部または環境に含まれる道標
   を観察してロボットの位置・姿勢を推定する位置推定手
   段と、 ロボットの位置・姿勢の推定方法を計画する推定計画手
   段と、 ロボットの位置・姿勢の推定値の更新を行う位置更新手
   段とを具備し、 ある地点Ａから別の地点Ｂまで移動することを命令され
   た移動ロボットの誘導制御装置において、 経路計画手段が作成した経路に基づいて移動制御手段が
   制御するロボットの進行に応じて、推定計画手段がその
   位置・姿勢の推定のために環境の一部または環境に含ま
   れる道標を観察する方法を計画した後、位置推定手段が
   地点Ｃで環境観察と位置推定のための処理を開始すると
   ともに、移動制御手段は地点Ｂに向かってロボットの移
   動を制御し、位置推定手段が地点Ｃにおけるロボットの
   位置・姿勢の推定が終了した地点Ｄにおいて、移動制御
   手段が算出した地点Ｃと地点Ｄの事前推定値と位置推定
   手段が推定した地点Ｃの新たな位置姿勢推定値とを利用
   して位置更新手段が地点Ｄでのロボットの位置と姿勢の
   推定値を更新し、その更新された位置から目的地Ｂまで
   の経路を経路計画手段が再計画し、その経路にもとづい
   て移動制御手段がロボットの移動を制御することを基本
   サイクルとし、以上の基本サイクルをロボットが地点Ｂ
   付近に到達するまで繰り返すことを特徴とする移動ロボ
   ットの誘導制御装置。