JP2005032196A - 移動ロボット用経路計画システム - Google Patents

Abstract

【課題】衝突の危険性が少ない安全な経路設定可能なシステム
【解決手段】幾何マップからグリッドマップを生成してから（Ｓ１０２）、グリッドマップから重み付きボロノイグラフを生成する（Ｓ１０４）。重み付きボロノイグラフは、物体の位置姿勢の不確実性に応じた重みの付いた距離を用いている。指定された目標物体を１つ取り出し（Ｓ１０６）、その目標物体のボロノイ領域の境界線上にあるボロノイノードからなる境界サブグラフを抽出する（Ｓ１０８）。現在の目標物体の境界サブグラフ内において、目標物体を観測できる最適な観測経路を求め、観測経路間の移動経路を求める（Ｓ１１２）。目標物体の境界サブグラフの各ノードから全体経路の終点までの移動経路を求めてから（Ｓ１１６）、それまで求めた移動経路と観測経路を交互に連結した複合グラフで経路探索を行う。最終的に、得られた経路を最適化する。
【選択図】図１

Description

本発明は、移動ロボット用経路計画システムに関するものである。

移動ロボットが目的地まで移動する経路を計画する方法として、地図上で走行可能な領域を求め、その領域を通過する線分をアークとしたグラフを生成し、そのグラフ上での経路探索問題に帰着させる方法が一般的である。このグラフとして、ボロノイグラフがよく用いられる（例えば、非特許文献１参照）。ボロノイグラフは、周囲の物体から等距離にある点をつないだ線をアークとし、アークの分岐点をノードとしたグラフである。周囲の物体から等距離にあれば、それらの物体に衝突する確率が最も小さいと期待されるため、安全な経路を求めるのに適している。
グラフ上の経路探索の方法としてはダイクストラ法がよく用いられる。これは、各アークにコストを設定し、始点から目的地までの経路のうち、その構成アークのコストの総和が最小になる経路を求める方法である。コストとしては、目的地までの距離や危険度などが用いられる。この他、Ａスター・アルゴリズムや最良優先探索などの手法もよく用いられるが、コストを最小にする点は同じである。
上記のように求めた経路は、いったん環境をグラフで離散化しているため、経路長や滑らかさの点で、必ずしも最適ではない。そこで、求めた経路を最適化することが行われる。具体的には、安全性や経路長、滑らかさをすべて考慮した評価関数を定義して、グラフから求めた経路を、評価関数が最小になるように変形する。
O. Takahashi and R. J. Schillinger: "Motion Planning in a Plane using Generalized Voronoi Diagrams," IEEE Trans. Robotics Automation, Vol.5, No. 2, pp. 143-150,1989

技術的背景で述べた方法には、以下の２つの問題がある。
（１） 物体の位置姿勢の不確実性に応じた経路生成ができない。
物体の位置姿勢が正確にはわからない場合、従来の方法では、必ずしも最も安全な経路が得られるとは限らない。ボロノイグラフでは、周囲の物体から等距離にある点をつないだ線をアークとしているが、ある物体の位置姿勢の不確実性が大きい場合、安全のためには、その物体から離れた点を通る方がよい。しかし、従来の方法では、どの物体からも等距離の点を通るようにするため、特定の物体だけ遠ざけるような経路をとることができない。
（２） 目標物体を見つけるための経路を生成できない。
従来の方法では、目的地に到達する経路は求めることができるが、ロボットが把持や運搬の対象とする目標物体を見つけて、その後、目的地へと移動する経路を計画することは、一般にできない。なぜなら、目標物体を見つけるには、目的地からいったん遠ざかるような経路をとって、回りこんで目標物体を探さなければならないことがありうる。ところが、ダイクストラ法ではアークのコストは非負でなければならないが、目標物体が見えないことをコストとすると、迂回経路をとらない方がコストの総和は必ず小さくなるため、目標物体を見つける経路が選ばれることはない。

本発明は、移動ロボットが出発点から目的地に向かう経路を求める移動ロボット用経路計画システムであって、環境内の物体の幾何マップを用いて、位置姿勢を確率的に表した物体が占有する空間の分布を表したグリッドマップを生成するグリッドマップ生成手段と、前記生成したグリッドマップから、物体の位置姿勢の不確実性に応じた距離を用いてボロノイグラフを生成するボロノイグラフ生成手段と、前記生成したボロノイグラフの上で、物体との衝突可能性と経路長をコストとして、与えられた前記出発点から目的地へ向かう経路の探索を行う経路探索手段とを備えることを特徴とする。
さらに、前記経路を、制御点を含む線分で分割し、該経路の出発点から目的地までの隣接する複数本の線分を組にして順次取り出し、それぞれの線分組に対して、最初の線分の始点と最後の線分の終点は固定し、前記制御点の位置を変化させることにより、経路長、経路の滑らかさ、物体との衝突可能性で定義される評価関数を用いて最適又は準最適にすることで最適化する最適化手段を備えることもできる。

又、１つまたは複数の目標物体が与えられており、前記経路探索手段は、全目標物体の観測経路を、ロボットの位置姿勢と目標物体の位置姿勢に基づいて、前記目標物体が前記ロボットのセンサにより観測できるかどうかの度合を前記コストに含めて探索することを含むこともできる。
このとき、さらに、前記経路を、制御点を含む線分で分割し、該経路の出発点から目的地までの隣接する複数本の線分を組にして順次取り出し、それぞれの線分組に対して、最初の線分の始点と最後の線分の終点は固定し、前記制御点の位置を変化させることにより、経路長、経路の滑らかさ、物体との衝突可能性および観測経路の場合は目標物体の可視度で定義される評価関数を用いて最適又は準最適にすることで最適化する最適化手段を備えることもできる。
上述の各移動ロボット用経路計画システムをコンピュータ・システムに構築させるプログラムも本発明である。

本発明の移動ロボット用経路計画システムでは、物体の位置姿勢の不確実性に応じた距離を用いてボロノイグラフを生成して、生成した重み付きボロノイグラフの上で、コスト計算を行い経路の探索を行っているので、従来のボロノイグラフよりも安全な経路を生成できる。
また、ロボットが最終目的点に行く途中で目標物体を見つける作業を行う場合に、目標物体を発見する確率が高く、しかも、安全性が高く、走行距離も短い経路を生成できるという効果がある。
さらに、得られた経路を最適化することにより、目標物体を発見し、かつ、安全性が高いという条件を保ちながら、経路長や滑らかさを改善させることができる。

本発明の実施形態の移動ロボット用経路計画システムでは、ロボットは環境の地図（マップと呼ぶ）を持っているが、環境内の物体の位置姿勢が大まかにしかわからないという条件下で経路計画を立てる状況を想定する。このような状況は、マップの誤差、環境の変化、あるいは、ロボット自身の位置姿勢の誤差により、現実的に頻繁に生じる。ロボットは、現在位置を始点とし、指定された目的地まで移動する経路を計画する。
また、単に目的地に移動する問題の他に、目標物体を見つけてから目的地に行く問題も考える。ロボットは、カメラなどの外界センサにより環境中の物体を認識し、物体の位置姿勢を推定する。目標物体はロボット自身が認識してもよいし、遠隔にいる操作者がカメラ画像を見て認識してもよい。ここで問題となるのは、物体を認識するための特徴は物体の特定の面に集中していることがあるため、適切な方向からカメラで物体を撮影する必要があるということである。このため、ロボットは単に目標物体の近くを通るだけでなく、目標物体の特徴を示す面がよく見える地点を通る経路を計画する必要がある。しかも、目標物体の位置姿勢が大まかにしかわからない場合にも対処しなければならない。なお、目標物体は複数個あってよいが、その順序は与えられているとする。

本発明の実施形態では、このような経路を計画するために、上述の２つの問題に次のように対処する。
（１）の問題については、周囲の物体までのユークリッド距離ではなく、物体の位置姿勢の不確実性に応じた距離を用いた重み付きボロノイグラフを用いることで、より安全な経路を計画できるようにする。これにより、位置姿勢の不確実性が大きい物体に対しては距離が大きくなるため、位置が正確にわからない物体や動きやすい物体は遠巻きにする経路を生成することができる。
（２）の問題については、目標物体を見つけるために遠回りの経路を生成できるように、全体の経路を移動経路と観測経路という２種類の部分経路に分けて構成する。移動経路は、全体経路の始点から最初の目標物体の近傍までの経路、ある目標物体の近傍から次の目標物体の近傍までの経路、最後の目標物体の近傍から全体経路の終点までの経路、のいずれかである。また、観測経路は、目標物体の周辺を走行して目標物体をよく観測するための経路である。そして、全体経路の計画は、まず各移動経路と各観測経路ごとに最適な経路を求め、次にそれらの移動経路と観測経路の最適な組合せを求める、という二段階で行う。

移動経路のコストは従来の技術と同じく、経路長と物体との衝突可能性を用いる。また、観測経路のコストは、これに加えて、ロボットと目標物体の位置姿勢に応じた目標物体の観測確率も考慮する。各部分経路には迂回路は必要ないので、ダイクストラ法を用いることができる。また、部分経路の最適な組合せは、各部分経路の始点と終点をノードとし、部分経路そのものをアークとした上位レベルのグラフ（複合グラフと呼ぶ）を用いて求める。この複合グラフのアークのコストは、対応する部分経路のコストであるが、必ず非負になるようにできるので、複合グラフにもダイクストラ法が適用できる。この方法を用いれば、目標物体を見つけるために遠回りの経路が必要になる場合の計画を、二段階のダイクストラ法で求めることができる。
なお、本発明の実施形態である移動ロボット用経路計画システムは、移動ロボット自体にあるコンピュータ・システムに組み込んでもよいし、移動ロボットとは別のコンピュータ・システムに実装し、その処理結果の経路のみを移動ロボットに渡してもよい。

本実施形態の処理の流れを、図１の流れ図をもとに説明する。経路計画システムへの入力は、環境を表す幾何マップ、始点と終点、および、走行中に見つけるべき目標物体である。これらの入力されたものは、システム中に記憶されており、また、処理過程で生成されるもの（たとえば、グリッドマップやボロノイグラフ等）もシステム中に記憶される。
まず、ステップＳ１０２において、与えられた幾何マップからグリッドマップを生成する。幾何マップは、環境内の物体を図形で表し、その位置姿勢を記述した地図である。グリッドマップは、床面を小さな格子で区切り、各格子が物体で占有されているかどうかを示した地図である。これは、ロボットが物体と衝突しない経路を計画するために用いられる。幾何マップ、グリッドマップとも、物体の位置姿勢に不確実性を許す。幾何マップの構成とグリッドマップの生成処理の詳細は後述する。
ステップＳ１０４では、グリッドマップから重み付きボロノイグラフを生成する。重み付きボロノイグラフは、物体の位置姿勢の不確実性に応じた重みの付いた距離を用いて、物体から等距離にある点を結んだ線をアークとし、アークの分岐点をノードとしたボロノイグラフである。ボロノイグラフのノードとアークを、それぞれ、ボロノイノード、ボロノイアークと呼ぶ。また、物体の回りのボロノイアークに囲まれた最小の領域をボロノイ領域と呼ぶ。重み付きボロノイグラフの生成方法の詳細は後述する。
ステップＳ１０６において、指定された目標物体を１つ取り出し、ステップＳ１０８で、その目標物体のボロノイ領域の境界線上にあるボロノイノードからなるサブグラフを抽出する。このサブグラフを境界サブグラフと呼ぶ。目標物体のボロノイ領域の境界線上にあるボロノイノードを求めるには、ステップＳ１０４で得た各ボロノイノードのうち、その隣接するボロノイ領域内に目標物体があるものを選べばよい。境界線上のボロノイノードを抽出したら、それらを連結するボロノイアークを抽出することで、求める境界サブグラフを得ることができる。
次に、ステップＳ１１０で、現在の目標物体の境界サブグラフと１つ前の目標物体の境界サブグラフをつなぐ移動経路を求める。具体的には、現在の目標物体の境界サブグラフのあるノードを始点とし、１つ前の目標物体の境界サブグラフのノードを終点として、経路長が短く、衝突可能性が小さい経路を選ぶ。この経路の良さは、後述する評価関数を用いて評価する。この処理を、現在の目標物体の境界サブグラフのノードと１つ前の目標物体の境界サブグラフのノードのすべての組合せについて行う。なお、最初の目標物体の境界サブグラフに対しては、１つ前の目標物体はないので、全体経路の始点から該境界サブグラフの各ノードへの移動経路を求める。
ステップＳ１１２で、現在の目標物体の境界サブグラフ内において、目標物体を観測できる最適な経路を求める。具体的には、まず、該境界サブグラフのボロノイノード集合から始点と終点を選び、この始点と終点を結ぶ経路のうち、目標物体が観測できて、経路長が短く、衝突可能性が小さい経路を選ぶ。この経路の良さは、後述する評価関数を用いて評価する。
この処理を、境界サブグラフ内のボロノイノードのすべての組合せに対して行う。始点と終点が同じになってもよい。ステップＳ１１４で、すべての目標物体に対して処理を行ったか調べる。未処理の目標物体があればステップＳ１０６に戻り、すべて処理したらステップＳ１１６に移る。
ステップＳ１１６で、最後の目標物体の境界サブグラフの各ノードから全体経路の終点までの移動経路を求める。これは、ステップＳ１１０の移動経路と同様に求める。ステップＳ１１８では、それまで求めた移動経路と観測経路を交互に連結した複合グラフを生成する。詳細は後述する。
ステップＳ１２０で、複合グラフ上でダイクストラ法などの従来の方法を用いて、全体経路の始点から終点までの最適な経路を求める。最後に、ステップＳ１２２で、得られた経路を最適化する。重み付きボロノイグラフでは、離散化されたアークで経路を構成するため、経路長や滑らかさの点で最適とはいえない。そこで、衝突可能性や目標物体の観測確率を良好に保ったまま、経路長を短くし、しかも、滑らかにする。この経路の評価関数と最適方法については後述する。
なお、ステップＳ１０２，Ｓ１０４，Ｓ１１０のみの処理では、目標物体がない場合なので、移動経路は経路の始点から終点までの１本となる。

以下に、上述の各ステップにおける処理について詳細に説明する。
（幾何マップの構成）
本発明の実施形態では、幾何マップとグリッドマップの２種類のマップを用いる。ここで用いるマップは、対象とする環境を、ある２次元平面に投影した地図である。多くの場合、この２次元平面として床面を設定する。
幾何マップは、図形で表した物体モデル（単に物体と呼ぶ）の集合で構成する。幾何マップはあらかじめ作成されて、ロボットに与えられるとする。その物体情報の構成例を図２に示す。
図２に示すように、物体情報２１０は、物体名２１２、形状２１４、位置姿勢代表値２１６、位置姿勢の確率分布２１８からなる。物体名２１２は、物体をマップ内で一意に表す名前である。形状２１４は、物体を床面に投影したときの２次元形状である。その表現方法は任意であり、多角形、円、自由曲線などで表す。図２の例では、長方形という図形種類と、横Ｗ，縦Ｈの寸法を与えている。物体の位置姿勢は（ｘ，ｙ，θ）^Ｔの３次元で表す。ここで、ｘ，ｙは床面上の位置，θは物体の向きである。位置姿勢の不確実性は確率分布で表すが、代表値となる位置姿勢２１６を便宜上もっておく。この位置姿勢代表値２１６は必須ではないが、幾何マップを人間が見るときに便利である。位置姿勢の確率分布２１８は、Ｎ個のランダムサンプルの集合｛ｑ_ｉ，ｋ｝，ｋ＝１，…，Ｎで表す。ただし、ｑ_ｉ，ｋ＝（ｘ_ｉ，ｋ，ｙ_ｉ，ｋ，θ_ｉ，ｋ）^Ｔである。なお、もし確率分布が正規分布などのパラメータで規定できる単純なものであれば、ランダムサンプルではなく、その分布型とパラメータ値をマップに登録してもよい。

（グリッドマップの生成）
グリッドマップは、床面を格子状に分割し、各格子に囲まれた微小領域（セルと呼ぶ）が物体に占有されているかどうかを表したマップであり、ロボットが物体に衝突せずに走行するための経路を計画するのに用いる。グリッドマップは、幾何マップをもとに生成する。グリッドマップは、セルの集合で表される。セル情報２２０の構成例を図３に示す。
図３において、セル位置２２２は、セルの床面上の位置を表すもので、たとえば、セルの中心点の位置で代表する。占有度２２４は、セルが物体によって占有されている度合を表すものである。その計算方法は後述する。最大占有物体２２６は、そのセルを占有する物体のうち、最も高い占有確率をもつ物体である。これは、後述する重み付きボロノイグラフを求めるときに用いられる。

さて、図１のステップＳ１０４でグリッドマップを生成するには、以下のように、各セルの占有度を幾何マップでの物体の位置姿勢の確率に基づいて計算する。
まず、セルＰ＝（ｘ，ｙ）が物体ｅ_ｉに占有されているかどうかを確率変数Ｃ_Ｐ，ｅｉで表す。Ｃ_Ｐ，ｅｉは２値で、セルが一部でも占有されているとき１、占有されていないとき０をとる。セルＰが物体ｅ_ｉに占有されている確率は次のように計算される。
ここで、Ｐ（Ｃ_Ｐ，ｅｉ｜ｑ_ｉ）は、ｅ_ｉの形状が既知ならば、ｑ_ｉだけに依存して０か１に確定する。
ランダムサンプルで位置姿勢の確率分布を表現する場合は、式（１）は、次のように離散型で計算する。
Ｐ（Ｃ_Ｐ，ｅｉ｜ｑ_ｉ，ｋ）はｑ_ｉ，ｋに依存して０か１に確定するので、
はセルＰを占有するランダムサンプルの個数になる。

物体の位置姿勢だけでなく、ロボットの位置姿勢も不確実性をもつ。ロボットの位置姿勢は、物体と同様に、ｒ＝（ｘ，ｙ，θ）^Ｔの３次元で表す。ロボットの位置姿勢は、オドメトリなどを用いて車輪の回転数から推定するが、走行につれて誤差が累積する。また、移動前の初期位置姿勢にも誤差がある可能性がある。そこで、これらの要因からなるロボット位置姿勢の誤差をグリッドマップに組み込んでおく。具体的には、ロボットの形状をその外接円で近似し、さらに、その外接円の半径Ｒに、ロボット位置姿勢の誤差の期待値Ｄを加える。そして、各物体の形状をＲ＋Ｄだけ等方的に膨張させる。つまり、式（１）のＰ（Ｃ_Ｐ，ｅｉ）をＲ＋Ｄだけ膨張させる。このようなマップには、ロボットの大きさが組み込まれているので、経路計画の際には、ロボットは点として扱って良い。このように、物体をロボットの外接円で膨張させることは、移動ロボットの経路計画ではよく行われる手法である。
ロボット位置姿勢の誤差の期待値Ｄは、対象とする領域での走行距離からおおまかに設定すればよい。安全のためには大きめに設定する。オドメトリの誤差が小さく、走行距離も小さい場合は無視してもよい。また、走行距離が長く、誤差が大きくなる場合には、外界センサを用いてロボットの位置姿勢の修正を適宜行うことで、誤差を大きくしすぎないようにする。この位置姿勢の修正は、移動ロボットで標準的に用いられる手法で行えばよい。
セルの占有度は、たとえば、各物体の占有確率の最大値として定義する。
ただし、Ｃ’_Ｐ，ｅｉは、ロボットの外接円半径と位置誤差Ｒ＋Ｄの分だけ膨張させた場合の物体ｅ_ｉによるセル占有を表す。

図４（ａ）にグリッドマップの例を示す。このマップは部屋を４ｃｍのセルで分割して表したものであり、黒い長方形は机４１１〜４１８を、灰色の領域は椅子４２１〜４２８および目標物体４３０を表す。机４１１〜４１８の姿勢は不確実性が小さいが、椅子４２１〜４２８の姿勢は不確実性を大きく設定した。目標物体４３０は一時的に置かれたものとし、マップ上の位置は不正確である。色が濃いほど、物体の位置姿勢の不確実性が小さくセルの占有度が高い。白い領域が、ロボットが走行できる領域である。図４（ｂ）は、図４（ａ）のマップを、ロボットの外接円と位置誤差で物体の占有領域を膨張させたマップである。

（重み付きボロノイグラフの生成）
重み付きボロノイグラフは、膨張させたグリッドマップを用いて求める。前述のように、各セルＰには、最大占有物体が登録されているので、床面は最大占有物体が占めるボロノイ領域に分割できる。すなわち、グリッドマップは最大占有物体の勢力圏を表したマップとみなすことができる。そこで、この物体勢力圏（ボロノイ領域）の境界線をアークとし、境界線の分岐点をノードとして、重み付きボロノイグラフを生成する。式（３）の定義に従えば、重み付きボロノイグラフのアーク上においてセルの占有度は極小になる。したがって、このアークで走行経路を構成すれば最も安全な経路になると期待される。
物体勢力圏の境界線を検出するには、グリッドマップの各セルＰ_ｉについて、その隣接する４近傍あるいは８近傍のセルの最大占有物体を調べ、２つ以上の最大占有物体が見つかった場合に、そのセルＰ_ｉを、境界線を構成する点とすればよい。この方法では、境界線が太くなることがあるため、画像処理で行われる細線化処理を行って、太さが１（セル）になるようにする。また、物体の存在しない領域が広い場合、コンピュータの計算精度では占有度が０になるセルが連続することがある。このような占有度０のセルの最大占有物体は、次のように定義する。まず、占有度０のセルＰから、ユークリッド距離の意味で最も近い位置にある、占有度０でないセルを探す。そして、このセルの最大占有物体をセルＰの最大占有物体とする。

重み付きボロノイグラフは、ボロノイノードの集合とボロノイアークの集合から構成される。図５（ａ）にボロノイノード情報５１０の構成例を示す。ボロノイノード情報５１０は、ノードＩＤ５１２、位置５１４、接続アーク集合５１６からなる。ノードＩＤ５１２は、そのボロノイグラフで一意な名前である。位置５１４は、グリッドマップ上の位置である。接続アーク集合５１６は、そのノードに接続するボロノイアークの集合である。
図５（ｂ）にボロノイアーク情報５２０の構成例を示す。ボロノイアーク情報は、アークＩＤ５２１，始点ノード５２２，終点ノード５２３，構成点列５２４，コスト５２５からなる。アークＩＤ５２１は、そのボロノイグラフで一意な名前である。始点ノード５２２，終点ノード５２３は、そのアークの始点および終点となるノードを示す。構成点列５２４はアークを構成する点（セル）の列である。アークは一般に直線にならないので、前述の境界線検出で得られた各セルをそのままもっておく。コスト５２５については後で説明する。

図６に、図４（ｂ）に示したグリッドマップ上に生成された重み付きボロノイグラフ６００の例を示す。
探索移動経路では目標物体を観測する必要がないので、重み付きボロノイグラフの上でダイクストラ法などの従来方法を用いて最適な経路を求めることができる。具体的には、経路長とセルの占有度をコストとして、経路ηの評価関数を以下のように定める。
ａ_ｉ（１≦ｉ≦Ｎ）は、ηを構成するボロノイアークである。｜ａ_ｉ｜はａ_ｉの長さである。Ａは非負の適当な重みであり、経験的に定める。なお、各ボロノイアークのコスト５２５は、
で計算する。

（目標物体の観測確率）
目標物体を見つけるための経路計画を考える。目標物体を見つけるには、経路を走るロボットの視野が目標物体の存在可能範囲を網羅しなければならない。目標物体の観測確率は、前述の幾何マップの物体の位置姿勢の確率から計算する。いま、ロボットから物体ｅ_ｉが見える事象をＬ_ｉで表す。ｅ_ｉが見えれば１、見えなければ０である。ｅ_ｉの位置姿勢をｑ_ｉとし、ロボットが動作列ａ^ｋによって経路ηを走行するとすると、ロボットがａ^ｋだけ移動した後ｅ_ｉを観測できる確率Ｐ（Ｌ_ｉ｜ｑ_ｉ，ａ^ｋ）は、以下のように計算される。
ここで、Ｐ（Ｌ_ｉ｜ｑ_ｉ，ｒ_ｋ）は、ｅ_ｉの位置姿勢がｑ_ｉで、ロボットの位置姿勢がｒ_ｋのとき、ロボットからｅ_ｉが見える確率である。これは、経路ηによらず、ｑ_ｉとｒ_ｋだけで決まる。式（６）の計算量は多いが、次節で実装時の対処を述べる。また、Ｐ（ｒ_ｋ｜ａ^ｋ）はロボットが動作列ａ^ｋによってｒ_ｋという位置姿勢に到達する確率であり、オドメトリの測定誤差の分布に相当する。Ｐ（ｒ_ｋ｜ｑ_ｉ，ａ^ｋ）のｑ_ｉを省略したのは、この測定誤差は物体の位置姿勢には影響されないと仮定したことによる。
次に、ｅ_ｉの位置姿勢がｑ_ｉのとき、経路ηを走行するロボットからｅ_ｉが見える確率Ｐ（Ｌ_ｉ｜ｑ_ｉ，η）は、次のように計算される。
Ｐ（Ｌ_ｉ｜ｑ_ｉ，η）は、ｅ_ｉがｑ_ｉの位置姿勢にあるとき、η上の少なくとも１点から見える確率である。なお、各Ｌ_ｉは互いに独立であると仮定している。
ロボットがｅ_ｉを観測できる確率は、Ｐ（Ｌ_ｉ｜ｑ_ｉ，η）を物体の位置姿勢の確率Ｐ（ｑ_ｉ）で平均した値として計算する。
（物体の可視度）
式（８）は二重積分をもつため計算量が多い。この計算量を減らすために、式（６）のＰ（Ｌ_ｉ｜ｑ_ｉ，ａ^ｋ）を近似計算する。具体的には、Ｐ（Ｌ_ｉ｜ｑ_ｉ，ｒ_ｋ）をロボットの位置姿勢誤差の期待値で膨張させることで近似する。外界センサとしてカメラを使うとすれば、カメラパラメータが既知ならば、Ｐ（Ｌ_ｉ｜ｑ_ｉ，ｒ_ｋ）は目標物体の形状や属性に依存する。そのため、Ｐ（Ｌ_ｉ｜ｑ_ｉ，ｒ_ｋ）は物体ごとに定義する必要がある。Ｐ（Ｌ_ｉ｜ｑ_ｉ，ｒ_ｋ）を厳密に定義することは困難であるが、経路計画に用いる場合は、近似値でも十分に役に立つ。
Ｐ（Ｌ_ｉ｜ｑ_ｉ，ｒ_ｋ）は、物体とカメラの距離や、カメラ視線と物体とのなす角を用いて定義する。この定義は物体ごとに行うものであるが、ここでは、一例として、物体の正面に認識特徴が集中している場合を説明する。ｖをカメラ中心から物体正面を結ぶベクトルとする。Ｄをベクトルｖの長さ、φを物体座標系でのベクトルｖの方向角、ψはロボット座標系でのベクトルｖの方向角とする（カメラのパン角に相当する）。物体を認識するには、Ｄ，φ，ψが適切な範囲に入っている必要がある。物体までの距離Ｄは、物体が適切な大きさで画像に写る範囲に制限される。認識特徴は物体の正面に集中しているので、物体が認識できる方向はφのある範囲内に限られる。また、ψはカメラのパンニングの可動範囲で制限される。そこで、次式のように、このような範囲内にＤ，φ，ψがあるとき、ロボットから物体を見える確率を１とする。
パラメータ値の境界付近の確率は連続な方が一般に挙動は安定するので、適当な関数を用いて平滑化してもよい。

図７に例を示す。目標物体７１０の正面はｘ軸方向を向いているとする。図７の明るい部分はＰ（Ｌ_ｉ｜ｑ_ｉ，ｒ_ｋ）が大きい領域である。ここでは、φの上下限を−６０［度］〜６０［度］、ψの上下限を−９０［度］〜９０［度］としている。この図７の例では、ロボット７２０と物体７１０のなす角が９０［度］の場合であり、物体を観測できる範囲は目標物体に向かって左側の領域に制限されている。
φの範囲は物体の認識特徴の分布に応じて設定する。どの方向からも認識特徴が見えるならば、この範囲は無視してよい。また、全方位カメラを用いれば、ψの範囲は無視できる。なお、物体の高さによって認識の可否が変わる場合は、カメラのチルト角の制限範囲も上式に加えればよい。

（観測経路の生成）
観測経路は目標物体の境界サブグラフ上で求める。図８に境界サブグラフの例を示す。境界サブグラフ７３０のボロノイノード集合７３１〜７３５から始点Ｓと終点Ｇを選び、この始点Ｓと終点Ｇを結ぶ経路を生成し、後述の方法でそのコストを計算する。境界サブグラフ７３０は円環状になるので、始点Ｓと終点Ｇを結ぶ経路は、図８（ａ）のように反時計回りと、図８（ｂ）のように時計回りの２通りがあるが、このうちコストが小さい方を選ぶ。この処理を、始点と終点に境界サブグラフの各ボロノイノードを割り当てた組に
対して行う。たとえば、図８の境界サブグラフのボロノイノードは、７３１〜７３５の５個あるが、始点Ｓと終点Ｇに各ノードを割り当てた２５通りの組合せについて、前記処理を行う。図８は、始点Ｓに７３１、終点Ｇに７３４を割り当てた場合の例である。。境界サブグラフのボロノイノード数はあまり多くないので、このようにすべての経路を生成しても処理量はさほど大きくならない。なお、始点と終点が同じノードになってもよい。この場合の経路は、境界サブグラフを一周するようにする。
観測経路のコストは、次の評価関数を用いて計算する。これは、Ｈ_１の経路長と衝突可能性に、観測確率を加えたものである。
ここで、Ｂは非負の適当な重みであり、経験的に定める。

（全体経路の探索）
全体経路は、移動経路と観測経路を交互に連結して構成する。２つの経路の連結点は、前の経路の終点であり、後の経路の始点となる。この点を中間目標点と呼ぶ。全体経路を最適にするには、適切な中間目標点を選ぶ必要がある。このために、求めた移動経路と観測経路を単位とする複合グラフを考える。複合グラフは、ノード集合とアーク集合からなる。
複合グラフのノードは、重み付きボロノイグラフのノードのうち、経路の始点、経路の終点、および、中間目標点に対応する。図９（ａ）に、複合グラフのノード情報８１０の構成例を示す。ノードＩＤ８１２は、複合グラフ内での一意な名前である。対応ボロノイノード８１４は、このノードに対応する重み付きボロノイノードを指す。接続アーク集合８１６は、このノードに接続する複合グラフのアークの集合である。
複合グラフのアークは、１つの移動経路あるいは観測経路に対応する。図９（ｂ）に、複合グラフのアーク情報８２０の構成例を示す。アークＩＤ８２１は、複合グラフ内での一意な名前である。始点ノード８２２，終点ノード８２３は、それぞれ、このアークの始点、終点となる複合グラフのノードを指す。構成ボロノイアーク８２４は、始点ノードから終点ノードに到達する最適な移動経路あるいは観測経路を構成するボロノイアークの列である。この最適経路は、前述の方法により求める。コスト８２５は、構成ボロノイアークのコストの総和である。各アークのコストは、移動経路の場合は式（４）の評価関数、観測経路の場合は式（９）の評価関数を用いて計算する。

図１０に複合グラフの概念的な構成を示す。この図で、Ｓは全体経路の始点、Ｇは全体経路の終点、ｖ_ｉ，ｊは重み付きボロノイグラフのノードである。複合グラフの概念的な構成を明らかにするため、この図１０では、複合グラフのノードではなく、対応するボロノイノードを用いて表現している。ノードｖ_ｉ，ｊはｉ番目の目標物体を観測する経路の中間目標点であり、その目標物体の境界サブグラフのボロノイノードである。ｖ_ｉ，ｊ１からｖ_ｉ，ｊ２へのアークは、ｖ_ｉ，ｊ１を始点、ｖ_ｉ，ｊ２を終点とした観測経路である。観測経路なのでボロノイグラフの上では始点と終点が同じになりうるが、複合グラフの上では異なるノードである。たとえば、図１０の左側のｖ_１，１と右側のｖ_１，１には、異なる複合グラフのノードを割り当てる。一方、Ｓからｖ_１，ｊ１，ｖ_{ｉ１，ｊ１}からｖ_{ｉ２，ｊ２}，および、ｖ_ｋ，ｊ２からＧへのアークは、移動経路に対応する。移動経路では、始点と終点が同じになることはない。
複合グラフでは、コストはすべて非負であり、しかも、始点と終点が同じノードになることはない。そこで、ダイクストラ法を用いてＳからＧまでの最適な経路を求めることができる。このようにして求めた経路は複合グラフのアーク列となるので、各アークの構成ボロノイアーク情報を用いて重み付きボロノイグラフのアーク列に変換し、最終的な経路とする。

（経路の最適化）
最後に、重み付きボロノイグラフで求めた経路を、衝突可能性と観測確率に配慮しながら、経路長と滑らかさについて最適化する。経路はボロノイアークの列から成る。ここでは、ボロノイアークを経路線分と呼ぶことにする。
経路の最適化の処理手順を図１１の流れ図を用いて説明する。まず、ステップＳ９０２で、Ｍを経路線分の個数とする。またｉを経路線分の番号とし、その初期値をｉ＝１とする。
ステップＳ９０４では、ｉ番目の経路線分とｉ＋１番目の経路線分を取り出す。
そして、ステップＳ９０６で、これらの経路線分を制御点で分割する。ｉ＞１のときは、ｉ＋１番目の経路線分だけ分割し、ｉ番目の経路線分の制御点は、１つ前のステップで分割したものを用いればよい。
次に、ステップＳ９０８で、該経路線分の評価関数が最適になる制御点の位置を求める。ただし、ｉ番目の経路線分の始点とｉ＋１番目の経路線分の終点は固定し、その間にある制御点の位置だけを変化させる。この詳細は後述する。
ステップＳ９１０で、ｉ番目の経路線分を更新経路に加える。なお、ｉ＋１番目の経路線分は、次の繰り返しで、もう一度最適化処理が施される。ステップＳ９１２でｉを１増やし、ステップＳ９１４でｉ＝Ｍになったら終了し、ｉ＜ＭならステップＳ９０４に戻る。

（最適化の詳細）
経路線分の評価関数として、移動経路の場合は、たとえば、次のものを用いる。
ただし、ｐｓは制御点の集合である。式（１０）では、ｐｓ内の各制御点を微小線分列ｇ_１，…，ｇ_Ｋで連結したと考えている。‖ｇ_ｉ‖は、線分ｇ_ｉの長さを表す。ａｎｇ（ｇ_ｉ）はｇ_ｉの方向角を表す。式（１０）右辺の第２項は、進行方向の急激な変化に対するペナルティであり、経路の滑らかさの評価値として用いている。Ａ’とＢ’は非負の適当な重みであり、経験的に求める。Ｐは数センチのオーダでサンプルする。
観測経路の場合は、Ｅ_１に物体の可視度を加えて、評価関数を以下のように定義する。
ここで、Ｃは非負の適当な重みであり、経験的に求める。Ｅ_１（ｐｓ）やＥ_２（ｐｓ）の値が小さいほど、制御点ｐｓの位置が良いことになる。
Ｅ_１（ｐｓ）やＥ_２（ｐｓ）を最小にする制御点の位置は、式（１０）や式（１１）に対して非線形最小化の手法を適用すれば求めることができる。ここでは、パーティクルフィルタを用いた例を説明する。パーティクルフィルタは逐次型のモンテカルロ法であり、確率分布をパーティクルと呼ばれるサンプルの集合で表す。各パーティクルは、ｐｓの全制御点の位置を変数として持つ。
パーティクルフィルタを用いて、次のように、準最適解を求める。まず、各パーティクルごとに、制御点の位置をランダムに変化させる。ただし、ｐｓの始点と終点は固定しておく。そして、その制御点の位置に対して、評価関数ｅ^{−Ｅ１（ｐｓ）}またはｅ^{−Ｅ２（ｐｓ）}の値を計算して、その値をパーティクルの重みとする。この重みが、各パーティクルが表す経路線分の良さを表す。そして、パーティクルの重みに比例した確率で、パーティクルをリサンプルする。このリサンプリング処理により、大きな重みをもつパーティクルがより多くの子孫を残すことができ、その結果、良い経路線分に対応するパーティクルが増えていく。この処理を適当な回数だけ繰り返し、重みが最大のパーティクルを取り出し、その制御点の位置を解とする。このような解は最適とは限らないが、評価関数の最小値にかなり近い準最適解になる。
一般に、変数の個数が増えると、パーティクルフィルタの性能が落ちる。これを防ぐために、本発明の実施形態では、全経路線分ではなく、２本の経路線分ごとに最適化することで、変数の個数を少なく抑えている。また、少数の経路線分ごとの最適化は、ロボットが走行時に外界センサで周囲を計測しながら、局所的に経路を修正するのにも適している。

経路線分を１本ごとでなく、２本を組にして最適化する理由は次のようである。経路を構成する経路線分の連結性を保証するには、経路線分の端点の位置は固定する必要がある。しかし、１本の経路線分ごとに最適化すると、端点が常に固定されるため、滑らかな経路が生成できない。そこで、２本の経路線分を組にして、その連結点の位置を変化できるようにし、経路線分を１本ずつずらしながらこの処理を行うことで、全体経路の始点と終点を除く各経路線分の端点の位置も最適化できるようにしている。なお、経路線分の組は、２本とは限らず、少数であれば何本でもよい。その際も、最初の経路線分の始点と最後の経路線分の終点だけを固定すればよい。

（適用結果）
上述で説明した経路計画の手法を適用した結果の例を以下に説明する。
図４および図６に示したグリッドマップや重み付きボロノイグラフの場合で、ロボットは点Ｓを出発して目標物体４３０を見つけて、それを把持して点Ｇまで運ぶという作業を想定する。そのためには、目標物体４３０を見つける観測経路を求めることが必要である。目標物体の認識特徴は物体正面に集中し、その法線ベクトルの±６０［度］以内にカメラ視線があれば、物体を認識できると仮定した。また、ロボットが持つパンチルトカメラのパン角の可動範囲は−９０〜９０［度］とした。
図１２に経路計画を適用した結果を示す。図１２（ａ）は、目標物体４３０の正面が上を向いている（矢印）場合の経路である。目標物体の位置は不確実だが、その向きはだいだいわかっていると仮定した。この場合は、目標物体が見やすいので、Ｓ→Ａ→Ｇという簡単な経路が生成された。
図１２（ｂ）は、目標物体４３０の正面が左を向いている（矢印）場合の経路である。この場合は、ロボットは、目標物体の正面を見るために、Ｓ→Ａ→Ｂ→Ａ→Ｇという回りこんだ経路を生成した。なお、図１２で、目標物体の領域４３０内の黒い長方形は、姿勢の期待値を示しており、実際の位置姿勢は、その周辺に雲状に分布している。
図１３には、目標物体が２つある例に経路計画を適用した場合を示す。矢印で示した方向が正面である。図１３は経路計画の結果であり、Ｓ→Ａ→Ｂ→Ａ→Ｃ→Ｄ→Ｃ→Ｇという経路が生成されている。目標物体４３２，４３３を見るためにＡ→ＢとＣ→Ｄのように、回りこむ経路を生成していることがわかる。
図１２（ｂ）の経路について、最適化を行った場合の結果を図１４に示す。図１２（ｂ）の経路が滑らかになっているのが分かる。しかも、経路の下の部分は、途中で引き返しても目標物体を観測できるので、経路長も短くなっている。

本発明の実施形態の処理手順を示す流れ図である。 幾何マップの物体情報の構成例を示す図である。 グリッドマップのセル情報の構成例を示す図である。 グリッドマップの一例を示す図である。 重み付きボロノイグラフのノード情報およびアーク情報の構成例を示す図である。 重み付きボロノイグラフの一例を示す図である。 ロボットから見た物体の可視領域の一例を示す図である。 観測経路のパターンを示す図である。 複合グラフのノード情報およびアーク情報の構成例を示す図である。 複合グラフの概念的な構成を示す図である。 本発明の実施形態の経路最適化手順を示す流れ図である。 図４や図６で、経路計画を適用した結果を示す図である。 目標物体が２つある例に経路計画を適用した場合を示す図である。 図１２（ｂ）の経路について、最適化を行った場合の結果を示す図である。

Claims (5)

1. 移動ロボットが出発点から目的地に向かう経路を求める移動ロボット用経路計画システムであって、
   環境内の物体の幾何マップを用いて、位置姿勢を確率的に表した物体が占有する空間の分布を表したグリッドマップを生成するグリッドマップ生成手段と、
   前記生成したグリッドマップから、物体の位置姿勢の不確実性に応じた距離を用いてボロノイグラフを生成するボロノイグラフ生成手段と、
   前記生成したボロノイグラフの上で、物体との衝突可能性と経路長をコストとして、与えられた前記出発点から目的地へ向かう経路の探索を行う経路探索手段と
   を備えることを特徴とする移動ロボット用経路計画システム。
2. 請求項１記載の移動ロボット用経路計画システムにおいて、
   さらに、前記経路を、制御点を含む線分で分割し、該経路の出発点から目的地までの隣接する複数本の線分を組にして順次取り出し、それぞれの線分組に対して、最初の線分の始点と最後の線分の終点は固定し、前記制御点の位置を変化させることにより、経路長、経路の滑らかさ、物体との衝突可能性で定義される評価関数を用いて最適又は準最適にすることで最適化する最適化手段
   を備えることを特徴とする移動ロボット用経路計画システム。
3. 請求項１記載の移動ロボット用経路計画システムにおいて、
   さらに、1 つまたは複数の目標物体が与えられており、
   前記経路探索手段は、全目標物体の観測経路を、ロボットの位置姿勢と目標物体の位置姿勢に基づいて、前記目標物体が前記ロボットのセンサにより観測できるかどうかの度合を前記コストに含めて探索することを含む
   ことを特徴とする移動ロボット用経路計画システム。
4. 請求項３記載の移動ロボット用経路計画システムにおいて、
   さらに、前記経路を、制御点を含む線分で分割し、該経路の出発点から目的地までの隣接する複数本の線分を組にして順次取り出し、それぞれの線分組に対して、最初の線分の始点と最後の線分の終点は固定し、前記制御点の位置を変化させることにより、経路長、経路の滑らかさ、物体との衝突可能性および観測経路の場合は目標物体の可視度で定義される評価関数を用いて最適又は準最適にすることで最適化する最適化手段
   を備えることを特徴とする移動ロボット用経路計画システム。
5. 請求項１〜４のいずれかに記載の移動ロボット用経路計画システムをコンピュータ・システムに構築させるプログラム。