JP7123430B2 - 地図生成システムおよび移動体 - Google Patents

Description

特許法第３０条第２項適用 平成３０年３月１４日に第２３回ロボテックスシンポジウムにて発表

本発明は、地図生成システムおよび移動体に関する。

従来、自律走行可能な移動体が自律走行するための技術として、ＳＬＡＭ(Simultaneous Localization and Mapping) と称される自己位置推定と環境地図作成の技術が用いられている。環境地図としては、移動体の移動範囲における物体の有無に基づいて作成された占有格子地図が用いられることがある（例えば、特許文献１参照）。この占有格子地図は、移動範囲の平面や空間を複数の区画（セル）に分割して記憶するとともに、分割した各区画に対して物体の有無に応じたセル値が付与されている。また、占有格子地図では、区画ごとに複数のスキャンに基づいて物体の存在する確率（以下では、存在確率や占有確率と称することがある）が算出され、この確率に基づいて各区画のセル値が管理されている。

特開２０１７－１５７０８７号公報

ところで、占有格子地図の作成（および更新）は、所定時間ごとや所定スキャン回数ごとに実行され、そのスキャン数が多いほど情報量が増えて地図の精度を高めることができるものの、スキャンデータ（以下では、検出情報と称することがある）の増大に伴いメモリを圧迫するとともに演算処理の負荷も増加してしまう。また、１つの区画に対して複数のスキャンから存在確率およびセル値を算出する場合、前後に実行されたスキャンの間で情報が重複し、後のスキャンによってセル値が上書きされることが起きる。このため古いスキャンデータから順次削除していき、新しいスキャンデータだけを使用してセル値を算出する手法も考えられるが、そうすると有効な情報を含んだ古いスキャンデータをも削除してしまう可能性があり、地図の精度が低下してしまうことがある。

本発明の目的は、占有格子地図の地図精度を確保しつつメモリ容量や演算処理の負荷を軽減させることができる地図生成システムおよび移動体を提供することである。

本発明の地図生成システムは、移動体の移動範囲における周囲の物体までの距離と方向を検出手段によって検出し、物体の有無に基づいて占有格子地図を生成する地図生成システムであって、前記検出手段による検出単位ごとの検出値を検出情報として時系列に記憶する記憶手段と、前記記憶手段に記憶された検出情報に基づいて占有格子地図を作成して前記記憶手段に記憶させる地図作成手段と、前記記憶手段に記憶された検出情報ごとに地図作成に寄与する度合いを示す影響度を、前記占有格子地図の区画の値に関与する検出情報の集合を区画ごとに求め、該関与検出情報集合に含まれる検出情報を新しい順に並べた順列において、該順列の先頭から所定個数番目までの検出情報に対しては該区画を有効とし、該所定個数番目の次から最後までの検出情報に対しては該区画を無効とすることによって、検出情報ごとの各区画の有効または無効を決定するとともに、有効な区画の割合に応じて高くなるように該検出情報の影響度を算出する影響度算出手段と、を備え、前記地図作成手段は、前記影響度算出手段にて算出された影響度が所定の閾値よりも小さい検出情報を用いないように選別して占有格子地図を作成することを特徴とする。

このような本発明によれば、地図作成手段は、影響度算出手段にて算出された影響度に基づいて、検出情報を選別して占有格子地図を作成するので、効率よく占有格子地図を作成または更新することができる。すなわち、地図作成手段は、占有格子地図に対する影響度が小さい検出情報を用いないように選別することで、演算に利用する検出情報の数を減らすことができ、メモリ容量が少なくて済むとともに演算処理の負荷を軽減させることができる。一方、占有格子地図に対する影響度が大きい検出情報を選別して用いることで、古い検出情報であっても有効なものが残されるため、作成する占有格子地図の地図精度を確保することができる。

本発明では、前記記憶手段に記憶された検出情報を占有格子地図の区画ごとに、該検出情報の新しい順に計数して観測度数として算出する観測度数算出手段をさらに備え、
前記影響度算出手段は、新しい検出情報から入力して、前記観測度数算出手段にて算出された観測度数が所定の閾値より大きい区画を該検出情報に対して有効とし、前記観測度数が前記閾値より大きい区画を該検出情報に対して無効とすることによって、検出情報ごとの各区画の有効または無効を決定するとともに、有効な区画の割合に応じて高くなるように該検出情報の影響度を算出することが好ましい。

このような構成によれば、観測度数算出手段は、記憶手段に記憶された検出情報を占有格子地図の区画ごとに計数して観測度数として算出し、この観測度数に基づいて有効とした区画の割合に応じて検出情報の影響度を高くすることで、効率よく検出情報を選別することができる。すなわち、ある区画に対して古いものから新しいものまで複数の検出情報（検出値）が存在する場合に、その個数（検出の回数）を観測度数として記録し、新しいものから所定数を有効とし、それよりも古いものを無効とする。そして、有効な区画が多く存在する検出情報の影響度を高く評価する。このようにすることで観測度数が高い（検出の回数が多い）区画に対しては、環境変化が反映されやすい新しい検出情報の影響度が高くなり、占有格子地図の地図精度を高めることができる。一方、観測度数が低い（検出の回数が少ない）区画に対しては、比較的古い検出情報まで影響度が高く評価され、古くても有効な情報を含んだ検出情報を適切に残して占有格子地図を作成することができる。

本発明では、前記地図作成手段は、前記観測度数算出手段にて算出された観測度数が所定の閾値よりも小さい区画に対して区画値を更新して占有格子地図を作成することが好ましい。

このような構成によれば、地図作成手段は、観測度数が所定の閾値よりも小さい区画に対して区画値を更新して占有格子地図を作成するので、相対的に新しい検出情報が選別されることになり、環境変化を反映させた占有格子地図を作成することができる。

本発明では、前記地図作成手段は、前記観測度数算出手段にて算出された観測度数に関わらずに、各区画に対する区画値を更新して占有格子地図を作成することが好ましい。

このような構成によれば、地図作成手段は、観測度数に関わらずに、各区画に対する区画値を更新して占有格子地図を作成するので、占有格子地図の安定性を向上させることができる。

本発明では、前記地図作成手段は、前記影響度算出手段にて算出された影響度が所定の閾値よりも小さい検出情報を前記記憶手段から消去することが好ましい。

このような構成によれば、地図作成手段は、影響度が所定の閾値よりも小さい検出情報を記憶手段から消去することで、占有格子地図の作成にかかる演算負荷と時間を削減することができるとともに、記憶手段の容量を節約することができる。

本発明では、前記地図作成手段は、検出情報を前記記憶手段から消去する前に、前記記憶手段に記憶された全ての検出情報を使用して占有格子地図を作成することが好ましい。

このような構成によれば、地図作成手段は、検出情報を記憶手段から消去する前に、記憶手段に記憶された全ての検出情報を使用して占有格子地図を作成するので、占有格子地図の安定性を向上させることができる。

本発明では、前記記憶手段に記憶された全ての検出情報の影響度の合計と、所定の数の検出情報の集合に属する検出情報の影響度の合計と、の比を網羅率として算出する網羅率算出手段をさらに備え、前記地図作成手段は、前記網羅率算出手段にて算出された網羅率に基づいて、前記影響度の閾値を設定することが好ましい。

このような構成によれば、地図作成手段は、網羅率算出手段にて算出された網羅率に基づいて、影響度の閾値を設定するので、効率よく検出情報を選別して占有格子地図を作成することができる。また、網羅率に基づいて影響度の閾値が自動的に設定されるので、演算処理を効率化することができる。

本発明の移動体は、移動手段、検出手段および制御手段を備えた移動体であって、前記いずれかの地図生成システムが前記制御手段によって構成されることを特徴とする。

このような本発明によれば、移動体は、移動手段、検出手段および制御手段を備え、制御手段によって前記地図生成システムが構成されることで、前述したのと同様に効率よく占有格子地図を作成または更新することができ、自律走行する移動体において、占有格子地図の地図精度を確保しつつメモリ容量や演算処理の負荷を軽減させることができる。

本発明の一実施形態に係る移動体のブロック図 前記移動体の移動範囲を示す占有格子地図の真値の概念図 前記占有格子地図の作成経過を示す概念図 前記移動体の動作を示すフローチャート 前記占有格子地図の作成方法を示すフローチャート 図５と異なる前記占有格子地図の作成方法を示すフローチャート 前記占有格子地図の作成例を示す図 前記作成例におけるスキャン数の削減率を示すグラフ 前記作成例における占有格子地図の品質を示す図 （Ａ）～（Ｃ）は、前記作成例における入力スキャン数と削減後のスキャン数、ノード数、アーク数の関係を示すグラフ 前記移動体の動作の変形例を示すフローチャート

以下、本発明の一実施形態を図１～図９に基づいて説明する。
本実施形態の移動体は、図１に示すように、移動体本体１と、移動体本体１を駆動制御する制御手段２と、移動体本体１を移動させる移動手段としての走行手段３と、移動体本体１の周囲の物体を検出するための検出手段４と、を備えている。この移動体は、ＳＬＡＭ技術によって自己位置推定と環境地図作成を行い、制御手段２に記憶された対象空間（移動範囲）ごとのマップや走行スケジュールに基づいて、移動体本体１が走行手段３によって自律走行するものである。このマップとしては、後述する占有格子地図が利用される。

移動体本体１は、図示しないボディおよびシャーシを備え、走行手段３は、例えば、左右一対の車輪と、一対の車輪を各々独立して回転駆動するモータと、を備えている。

検出手段４は、例えば、移動体本体１の前部に設けられた前方センサと、移動体本体１の上部に設けられた周囲センサと、移動体本体１の後部に設けられた後方センサと、を備えている。周囲センサは、赤外線レーザー等のレーザー光を周囲に照射して物体までの距離を測定するレーザースキャナ（ＬＩＤＡＲ（Light Detection and RangingまたはLaser Imaging Detection and Ranging））である。この周囲センサは、ＳＬＡＭ技術によって取得した自己位置であるセンサ中心と方位を基準位置（ｘ，ｙ，θ）^Ｔとし、センサ中心からの距離と、センサ中心回りの角度（方向）と、に基づいて物体の有無や位置を検出する。この物体検出（以下では、単にスキャンと称することがある）では、周囲センサの１周期（例えば、１回転、３６０°）分の検出において、所定の解像度（例えば、１°）で照射されるレーザービームに基づき、１周期分の検出値を１単位（１スキャン）とし、この１スキャンの検出値が検出情報として時系列で記憶部２８に記憶される。

制御手段２は、ＣＰＵ等の演算手段やＲＯＭ、ＲＡＭ等の記憶手段を備えて移動体本体１の動作を制御するものであって、走行手段３を制御する走行制御部２１と、検出手段４を制御する検出制御部２２と、を備える。さらに、制御手段２は、後述するような占有格子地図の作成に係る地図作成部（地図作成手段）２３、影響度算出部（影響度算出手段）２４、観測度数算出部（観測度数算出手段）２５および網羅率算出部（網羅率算出手段）２６と、作成した占有格子地図や各種のデータを記憶する記憶部（記憶手段）２７と、を備えて構成されている。

地図作成部２３が占有格子地図を作成するタイミングとしては、例えば、新たな対象空間の占有格子地図を新規に生成する地図生成モードの場合、すでに占有格子地図を生成した既知の対象空間に対して地図を更新する地図更新モードの場合、既知の対象空間に対して通常の作業を実施する自律走行モードの場合などである。また、地図更新モードで地図を更新する期間（更新スパン）としては、その対象空間にて移動体本体１が走行した累計走行時間（例えば、数時間から数十時間ごと）に基づいてもよいし、その対象空間にて検出手段４が検出を実行した累計スキャン回数（例えば、数千から数万回ごと）に基づいてもよいし、使用者から設定入力された期間や手動での指令に基づいてもよい。

次に、図２、図３を参照して占有格子地図の作成に係る基本的な概念を説明する。図２は、移動体の移動範囲を示す占有格子地図Ｍの真値の概念図であり、図３は、占有格子地図Ｍの作成経過を示す概念図である。占有格子地図Ｍは、対象空間を二次元平面に規定し、この平面を所定サイズ（例えば、５ｃｍ角）の格子に分割し、分割した区画であるセルＣごとにセル値（区画値）を設定する。この占有格子地図Ｍの作成方法としては、例えば、レイキャスティングが標準的であり、以下ではレイキャスティングを用いた占有格子地図Ｍの作成手順を説明する。

レイキャスティングでは、センサ中心からレーザービームを模擬した直線を伸ばし、ビームが通過したセルＣの値を更新する。図３に示すように、レーザースキャナ（周囲センサ）の計測範囲が円内であるとすると，ビームが物体に当たった位置にあるセルＣは占有（例えば、セル値＝１）、ビームが通過したセルＣは自由（例えば、セル値＝－１）となる。計測範囲内に物体がない場合はビームが返ってこないので、未観測となり、通常はセル値を変えない。占有格子地図Ｍのセル値は対数オッズで管理され、複数のスキャンによるセル値を合算して対数オッズを計算する。例えば、あるセルＣの占有が３回、自由が２回あれば、対数オッズは３＋（－２）＝１となる。この対数オッズから占有確率が計算される。ただし、図３では、簡単のため、セル値を単純に上塗りしている。

次に、図２、図３を用いて本発明の占有格子地図Ｍの作成方法の考え方を説明する。図２は、占有格子地図Ｍの真値であり、実環境には壁Ｗと物体Ｏがあるとする。図３（Ａ）は、検出手段４により制御手段２が１回目のスキャン（３６０°のスキャン）を取得したときの占有格子地図Ｍのセル値である。ここでは、１回目のスキャンによる自由セルＣ_f1を白で表す。同様に、図３（Ｂ），（Ｃ）は、２回目のスキャン、３回目のスキャンによるセル値であり、それらの自由セルＣ_f2，Ｃ_f3をそれぞれ中間のグレーおよび濃いグレーで表す。図３において、占有セルＣ_ｏはいずれも黒で表し、未観測のセルＣ_ｎは薄いグレーで表す。各スキャンにおいて、複数本（例えば、１°ずつ３６０本）のレーザービームが照射され、１本ごとのレーザービームが通過したセルに対してセル値を設定する。

図３を参照すると、１回目のスキャンが書き込んだセル値の多くは、その後のスキャンによって上塗りされている。１回目のスキャンの占有セルＣ_ｏは、２回目、３回目のスキャンの円内に含まれるので、すべて上塗りされる。１回目のスキャンの自由セルＣ_f1はわずかしか残っておらず、１回目のスキャンが占有格子地図Ｍの生成にあまり寄与していないことが分かる。この例では、セル値を単純に上塗りしているが、多くのスキャンから占有確率を計算する場合でも、スキャンが重複するにつれて同様の現象が生じることが分かっている。

本発明の占有格子地図Ｍの作成方法は、各スキャンが占有格子地図Ｍの生成に寄与したセル数からそのスキャンの影響度を計算し、影響度の小さいスキャンを削減する。図２、図３からわかるように、スキャンの影響度は、時間の進行とともに変化し、一般的には古いスキャンほど影響度は小さくなる。ただし、時間的には古くても、その領域で新しいスキャンが取得されなければ、古いスキャンは生き残る。このように、本発明の作成方法は、相対的に新しいスキャンを残す性質をもつので、スキャンデータの削減と同時に、占有格子地図Ｍの更新も行いやすい手法となっている。

また、占有格子地図Ｍの作成において、各セルＣに対数オッズを持たせて占有確率を計算することが一般的に行われている。しかし、対数オッズだけでは、そのセルＣを何回スキャンしたかの情報は残らない。例えば、あるセルＣの対数オッズが－３となるには、セルＣをビームが３回通過して自由値（－１）が３回加算されてもよいし、あるいは、セルＣをビームが６回通過、３回到達して自由値（－１）が６回と占有値（１）が３回加算されてもよいが、対数オッズだけでは、ビームの通過回数（スキャン回数）の区別はつかない。

占有格子地図Ｍを作成する際、ノイズや誤差があっても安定した結果となるためには、複数回のスキャンによって、各セルＣに多くのビームが通って、統計的に処理することが望ましい。しかし、スキャンは多いほどよいわけではなく、古いスキャンは新しいスキャンによって塗り替えられて必要なくなることも多い。例えば、物体の位置が変わると、移動元のセルＣは自由状態となり、移動先のセルＣは占有状態となり、移動後の位置が確定すれば、古いスキャンは必要なくなる。これらのことから、スキャンした回数を明示的に管理し、適切な数のスキャンでセル値を計算すれば、無駄な計算を減らせることになる。

本発明の占有格子地図Ｍの作成方法では、適切な数のスキャンでセル値を計算するために観測度数を導入する。各セルＣは観測度数をもち、そのセルＣをスキャンした回数を観測度数に記録し、記録した観測度数に基づいて最新のＮ個のスキャンだけを残し、古いスキャンを捨てることに特徴がある。ここで、Ｎは対数オッズから占有確率を計算するのに十分な観測数であればよく、経験的には、１～２秒分のスキャンでよいと考えられる。例えば、周囲センサのフレームレートが１０ｆｐｓならば、Ｎは１０～２０で十分実用的な占有格子地図Ｍが作成できることが分かった。

また、占有格子地図Ｍは、環境の変化を反映して随時更新する必要があり、その更新頻度としては、実時間で更新する場合や毎日定期的に更新する場合など、種々の形態が考えられる。また、地図形状の歪みを減らして一貫性を保つには、ループ閉じ込みを行う必要がある。ここで、ループとは移動体が同じ場所を通る周回経路であり、ループを検出して地図の歪みを解消する処理をループ閉じ込みと呼ぶ。そして、移動体は同じ場所を何度も通るので、ループ閉じ込みが頻発する。ループ閉じ込みをすると地図を作り直すが、これには過去のデータをすべて用いることが一般的であったため、地図を長期間にわたって更新し続ける際の問題として、処理時間とメモリ使用量の増加がある。

本発明の占有格子地図Ｍの作成方法では、前述のようなスキャンデータの削減に合せて、ループ閉じ込みで用いる内部データであるポーズグラフを縮約する。ポーズグラフを縮約することにより、ループ閉じ込みとその後の地図再構築に要する処理時間とメモリ使用量が削減される。

具体的には、移動体の走行軌跡のループを検出し、ポーズ調整によりループ閉じ込みを行う。ポーズ調整は、移動体の位置の拘束関係を表すポーズグラフを用いて行う技術であり、ポーズグラフは、移動体の位置を表すノードと、ノード間の相対位置を表すアークと、によって構成され、移動体の推定位置と計測値とのずれを表すことができる。ポーズ調整は、このずれを最小化することで、地図の歪みを修正する。ポーズ調整の処理時間は、ポーズグラフのノード数にほぼ比例し、ループの数にも依存する。ＳＬＡＭ技術では、移動体の位置はスキャンごとに推定されるため、移動体の位置およびノードの個数はスキャン数に比例する。また、環境が小規模ならば同じ場所を何度も通ると考えられるので、走行軌跡が長くなると、ループの数も増えると想定される。したがって、スキャン数が増えればポーズ調整の処理時間も増えることから、前述のように、使用するスキャン数を削減することで、ループ閉じ込み処理に伴うポーズグラフのサイズを小さくして、ポーズ調整の処理時間が短縮でき、メモリ使用量も削減することが可能になる。

次に、図４～図６も参照して移動体の動作および占有格子地図Ｍの作成（更新）方法について詳しく説明する。図４は、移動体の動作を示すフローチャートである。図５は、占有格子地図Ｍの作成方法を示すフローチャートであり、図６は、図５と異なる占有格子地図Ｍの作成方法を示すフローチャートである。なお、ここでは、移動体の走行に係る動作については説明を省略し、走行に伴って占有格子地図Ｍを作成、更新する方法に着目して説明する。

制御手段２の走行制御部２１によって移動体本体１の走行が開始されると、検出制御部２２によって周囲センサが駆動制御されて周囲の物体の検出が開始される（検出情報取得工程：ステップＳＴ１）。制御手段２は、ＳＬＡＭ技術によって自己位置を推定しつつ、自己位置と関連付けて周囲センサによって検出した検出値を検出情報として記憶部２７に記憶させる（検出情報記憶工程：ステップＳＴ２）。検出情報の検出は、周囲センサによる１周期分の検出値を１スキャンとし、この１スキャンを１つの検出情報として記憶部２７に記憶する。次に、制御手段２は、記憶部２７に記憶した検出情報に基づいて、地図作成部２３に占有格子地図Ｍを作成させ（地図作成工程：ステップＳＴ３）、作成した占有格子地図Ｍを記憶部２７に記憶させる。次に、制御手段２は、検出情報を削減するか否かを判断する（削減判断工程：ステップＳＴ４）。ここでは、検出回数（スキャン回数）が所定回数に達したか否かを判断し、所定回数に達していれば（ステップＳＴ４でＹＥＳ）、次のステップＳＴ５に進み、所定回数に達していなければ（ステップＳＴ３でＮＯ）、再び検出情報取得工程（ステップＳＴ１）に戻って、新たな検出情報を取得する。

削減判断工程（ステップＳＴ４）で検出情報を削減すると判断した場合、制御手段２の影響度算出部２４は、記憶部２７に記憶された検出情報ごとに占有格子地図Ｍに対する影響度を算出する（影響度算出工程：ステップＳＴ５）。次に、制御手段２の地図作成部２３は、影響度算出部２４にて算出された影響度に基づいて、検出情報を選別して記憶部２７から検出情報を削減する（検出情報削減工程：ステップＳＴ６）。これらの影響度算出工程（ステップＳＴ５）および検出情報削減工程（ステップＳＴ６）における演算手順は後述する。次に、制御手段２の地図作成部２３は、削減された検出情報に基づいて占有格子地図Ｍを再度作成（更新）し（地図再作成工程：ステップＳＴ７）、更新した占有格子地図Ｍを記憶部２７に記憶させる。

以上のようにして占有格子地図Ｍが作成および更新されたら、制御手段２は、ステップＳＴ８にて移動体本体１の走行を継続するか否かを判断する。ステップＳＴ８で走行を継続する場合、再び検出情報取得工程（ステップＳＴ１）に戻り、走行制御部２１は、新たに作成した占有格子地図Ｍに基づいて移動体本体１の走行を制御するとともに、検出制御部２２は、周囲センサによる周囲の物体の検出を継続し、制御手段２は上述した各工程を繰り返す。ステップＳＴ８で走行を継続しない場合、制御手段２は、移動体本体１の走行を停止させるとともに、各部の動作を停止させる。

次に、影響度算出工程（ステップＳＴ５）で算出する影響度は、占有格子地図Ｍに対するスキャン（検出情報）ごとに影響度算出部２４が算出するものであって、前述したように観測度数算出部２５が算出した観測度数が用いられる。ここでは、あるスキャンｓ_ｋに対して、その中に含まれる有効自由セル数をｆ_ｋ、有効占有セル数をｏ_ｋとする。ここで有効とは、観測度数の閾値ｂ_threよりも古くないことを意味する。この閾値ｂ_threは、前述したように、残すべき最新のスキャン数Ｎに相当する。また、ｉ番目のセルの対数オッズをｖ_ｉ、観測度数をｂ_ｉとする。スキャンｓ_ｋのセンサ中心位置は、ＳＬＡＭ技術によって得られているものとする。

スキャンの影響度ｅｋは、式（１）で表す。
ｅ_ｋ＝ｆ_ｋ＋Ａｏ_ｋ …（１）
ここで、Ａは、有効占有セル数ｏ_ｋに重みをつけるための定数である。占有格子地図Ｍでは、一般に、占有セルは、自由セルに比べて数は少ないが、障害物を示すので重要性が高い。このため、Ａによって占有セル数に重みを持たせ、有効占有セルをもつスキャンを優先する仕組みを入れておく。なお、以下の演算では、定数Ａは１とする。

影響度ｅ_ｋは、レイキャスティングの最中に計算する。影響度ｅ_ｋの計算を含めたレイキャスティングの手順を図５のフローチャートを参照して説明する。

まず、ステップＳＴ１０において、全セルについて、対数オッズｖ_ｉおよび観測度数ｂ_ｉを初期化する（ｖ_ｉ＝０、ｂ_ｉ＝０）。次に、ステップＳＴ１１において、スキャンｓ_ｋを新しいものから順に選択する。すなわち、ｋ値が大きい新しいスキャンｓ_ｋから降順に選択する。そして、ステップＳＴ１２以降において、スキャンｓ_ｋの各セルＣ_ｉに対して、以下の処理を繰り返す。すなわち、スキャンｓ_ｋを得たときのセンサ中心から、レーザービームを模した直線を計測距離だけ（物体に到達するまで）伸ばし、直線上の各セルＣ_ｉについて以下の処理を繰り返す。ステップＳＴ１３において、直線が通過した自由セルＣ_ｉ（自由空間）である場合、ステップＳＴ１４～ＳＴ１７を実行し、直線が到達した占有セルＣ_ｉ（物体）である場合、ステップＳＴ１８～ＳＴ２１を実行する。なお、レーザービームの反射が観測されない場合は、そのレーザービームが通過したセルは未観測セルとして演算を行わない。

自由セルＣ_ｉの場合、ステップＳＴ１４において、観測度数ｂ_ｉが閾値ｂ_threよりも小さければ（ステップＳＴ１４でＹＥＳ）、ステップＳＴ１５にて有効自由セル数ｆ_ｋに１を加え（ｆ_ｋ＝ｆ_ｋ＋１）、ステップＳＴ１６にて対数オッズｖ_ｉから１を減じ（ｖ_ｉ＝ｖ_ｉ－１）、ステップＳＴ１７にて観測度数ｂ_ｉに１を加える（ｂ_ｉ＝ｂ_ｉ＋１）。ステップＳＴ１４において、観測度数ｂ_ｉが閾値ｂ_threよりも大きければ（ステップＳＴ１４でＮＯ）、ステップＳＴ２２に進む。

占有セルＣ_ｉの場合、ステップＳＴ１８において、観測度数ｂ_ｉが閾値ｂ_threよりも小さければ（ステップＳＴ１８でＹＥＳ）、ステップＳＴ１９にて有効占有セル数ｏ_ｋに１を加え（ｏ_ｋ＝ｏ_ｋ＋１）、ステップＳＴ２０にて対数オッズｖ_ｉに１を加え（ｖ_ｉ＝ｖ_ｉ＋１）、ステップＳＴ２１にて観測度数ｂ_ｉに１を加える（ｂ_ｉ＝ｂ_ｉ＋１）。ステップＳＴ１８において、観測度数ｂ_ｉが閾値ｂ_threよりも大きければ（ステップＳＴ１８でＮＯ）、次のステップＳＴ２２に進む。

ステップＳＴ２２において、あるスキャンｓ_ｋに含まれる全てのセルＣ_ｉに対しての処理が終了したか否かを判断し、処理が終了していなければ（ステップＳＴ２２でＮＯ）、ステップＳＴ１２に戻って次のセルＣ_ｉ（ｉ＝ｉ＋１）に対して同様の処理を実行する。一方、あるスキャンｓ_ｋに含まれる全てのセルＣ_ｉに対して処理が終了すれば（ステップＳＴ２２でＹＥＳ）、次のステップＳＴ２３に進み、当該スキャンｓ_ｋの影響度ｅ_ｋを算出する。ステップＳＴ２３において、スキャンｓ_ｋの影響度ｅ_ｋは、前式（１）によって算出する。次に、ステップＳＴ２４において、全てのスキャンｓ_ｋに対しての処理が終了したか否かを判断し、処理が終了していなければ（ステップＳＴ２４でＮＯ）、ステップＳＴ１１に戻って１つ古いスキャンｓ_ｋ（ｋ＝ｋ－１）に対して同様の処理を実行する。一方、全てのスキャンｓ_ｋに対して処理が終了すれば（ステップＳＴ２４でＹＥＳ）、影響度算出工程（ステップＳＴ５）を終了する。

以上の処理は次のことを意味する。スキャンｓ_ｋのビームによってセル値が更新されるかどうかは観測度数ｂ_ｉで判定され、新しいスキャンｓ_ｋから処理するので、セルＣ_ｉの観測度数ｂ_ｉが閾値ｂ_threを超えれば、その先のスキャンｓ_ｎ（１≦ｎ≦ｋ－１）はセル値に影響を与えない。このスキャンｓ_ｎは，セル値に対しては古い観測であり、セル値はスキャンｓ_ｎの影響度に加算されない。そして、大部分が古い観測であると判定されたスキャンｓ_ｋは、その影響度が小さくなり、占有格子地図Ｍを更新する際に必要ないと判定されて削除される（後述する検出情報削減工程：ステップＳＴ６）。

以上の処理の利点としては、レイキャスティングの過程で影響度が計算できるので、計算コストが小さくできるとともに、古くても観測が少ない領域のスキャンｓ_ｋを残すことができる。すなわち、新しい観測がない領域では古いスキャンｓ_ｋしか残っていないが、観測度数ｂ_ｉが閾値ｂ_threを超えなければ、そのスキャンｓ_ｋは影響度が大きくなり、削除されず、最後に観測した状態が保たれて占有格子地図Ｍを安定させることができる。

なお、影響度算出工程（ステップＳＴ５）は、以上の処理手順によるものに限らず、図６に示す手順で実行されてもよい。図６に示す処理手順は、ステップＳＴ１４～ＳＴ１７およびステップＳＴ１８～ＳＴ２１が図５のものと相違する。具体的には、ステップＳＴ１４よりもステップＳＴ１６を先に実行し、ステップＳＴ１９よりも先にステップＳＴ２０を実行する。すなわち、観測度数ｂ_ｉと閾値ｂ_threとの比較に関わらず、自由セルＣ_ｉの場合にはステップＳＴ１６にて対数オッズｖ_ｉから１を減じ（ｖ_ｉ＝ｖ_ｉ－１）、占有セルＣ_ｉの場合にはステップＳＴ２０にて対数オッズｖ_ｉに１を加える（ｖ_ｉ＝ｖ_ｉ＋１）。このような処理手順によることで、セル値の更新にｂ_threを過ぎた古いスキャンｓ_ｋも使われるようになる。したがって、環境変化に対するセル値の更新が遅くなる半面、セル値の安定性は増すことになる。

以上の影響度算出工程（ステップＳＴ５）によって、スキャン（検出情報）ごとに影響度ｅ_ｋを算出したら、図４に示すように、検出情報削減工程（ステップＳＴ６）において、網羅率算出部２７が網羅率を算出し、この網羅率に基づいて検出情報を選別して記憶部２７から検出情報を削減する。

算出した影響度ｅ_ｋに基づいてスキャンを選別する方法は、影響度の閾値ｅ_threを用いて、ｅ_ｋ≦ｅ_threとなるスキャンを削除する。ここで、ｅ_threは環境によって変わることから、その設定には、スキャン集合Ｓの網羅率を用いる。スキャン集合Ｓの網羅率とは、次式（２）のように、現時点で残っている全スキャン集合Ｓ_０の影響度の合計に対するスキャン集合Ｓによる影響度ｅ_ｋの合計の比である。

この網羅率が大きいほど、スキャン集合Ｓによる占有格子地図Ｍは全スキャン集合Ｓ_０による占有格子地図に近づく。このように、網羅率は占有格子地図Ｍの完成度合いを表すことができ、直感的に指定しやすい指標であると考えられる。スキャンを削減しつつ網羅率を高く保つには、影響度ｅ_ｋが大きいスキャンを優先してスキャン集合Ｓに入れるとよい。したがって、網羅率を用いてスキャン選別の閾値ｅ_threを決める手順は以下のようになる。

まず、全スキャンの影響度の合計totalＥを求めてから、全スキャンを影響度ｅ_ｋの順にソートする。次に、各スキャンｓ_ｋについて、影響度ｅ_ｋの大きい方から以下の式（３）、式（４）を繰り返し計算し、式（４）を満足しなくなった（ｓｕｍ／totalＥ≧coverageとなった）ときの影響度ｅ_ｋを閾値ｅ_threとする。ここで、coverageはユーザが指定した網羅率であり、例えば、０．９５（９５％）などが用いられる。
ｓｕｍ＝ｓｕｍ＋ｅ_ｋ …（３）
ｓｕｍ／totalＥ＜coverage …（４）

地図再作成工程（ステップＳＴ７）において、地図作成部２３は、以上の各手順によって選別されて残ったスキャンｓ_ｋごとに、演算によって得られたセル値である対数オッズｖ_ｉおよび観測度数ｂ_ｉを記憶部２７に記憶させ、これにより占有格子地図Ｍが更新される。

以下、図７～図１０を参照して、占有格子地図Ｍを作成した例について説明する。図７は、占有格子地図の作成例を示す図である。図８は、作成例におけるスキャン数の削減率を示すグラフであり、網羅率の設定によるスキャン数の削減率を示す。図９は、作成例における占有格子地図の品質を示す図である。図１０（Ａ）～（Ｃ）は、作成例における入力スキャン数と削減後のスキャン数、ノード数、アーク数の関係を示すグラフである。

図７（Ａ）には、従来例としてスキャンを削減せずに作成した占有格子地図Ｍが示され、図７（Ｂ）には、本発明の前述した作成方法によりスキャンを削減してから作成した占有格子地図Ｍが示されている。本発明の作成方法において、網羅率には０．９５を適用した。図７では，占有格子地図Ｍの上に、地図生成に用いたスキャンに対応する移動体の移動軌跡を表示している。図７（Ａ）に示すように、移動体は、Ａ、Ｂ、…Ｆの順に走行した。図７（Ｂ）では、スキャンが削減されているので、移動体の位置も断片的になっている。なお、ここでは，見やすさのために単純な短い経路をとっており削減率は高くない。

図７（Ｂ）に示すように、本発明の作成方法によって古いスキャンが削除されているのがわかる。たとえば、区間Ａ－Ｂの前半部は削除されている。その部分で取得したスキャン群は主に部屋の上部のセルの占有確率を決めるが、後に走行した区間Ｅ－Ｆの後半部によってセル値が置換えられ、区間Ａ－Ｂの影響度が減ったためである。また、区間Ｃ－Ｄも削除されているが、これは区間Ｄ－Ｅの残っている部分で置換されたためである。なお、図７（Ｂ）において、区間Ｃ－Ｄの右上にセル値が未定の領域があるが、これは網羅率が０．９５であるために生じた欠落である。

また、図７（Ａ）では、壁に対する占有セル（黒）が欠落なく出ているが、図７（Ｂ）では、欠けている部分がある。これは、スキャンの歪みや誤差のため、最近の少数のスキャンだけでは、セル値が不安定になるためと考えられる。これは、セル値計算で多くのスキャンを使用すれば解決する。すなわち、観測度数ｂ_ｉはスキャン選択だけに用いて、図６に示すように、対数オッズｖ_ｉの計算を観測度数ｂ_ｉによらずに行えばよい。ただし、図６に示す処理手順によると、環境変化を迅速に反映することはできなくなる。このように、セル値の安定性と環境変化への対応は相反する面がある。この特性は占有格子地図Ｍに本質的なものと考えられ、対象とする空間や環境に応じて処理手順を適宜に使い分ければよい。

図８、図９には、網羅率の設定によってスキャン数の削減率および占有格子地図Ｍの品質がどのように変化したかが示されている。図８のグラフは、スキャン数を３０００個、６０００個、９０００個とした場合の網羅率に対する削減率を示す。図９（Ａ），（Ｂ）は、それぞれ網羅率が０．６および０．９の場合に作成された占有格子地図Ｍを示す。ここでは、削減率（reduction ratio）を、削減後のスキャン数を削減前のスキャン数で割った値で定義する。この定義では、削減後のスキャンが少ないほど削減率は小さくなる。

図８に示すように、網羅率（coverage）を高くするほど削減率は大きくなっている。また、スキャン数が多いほど削減率は小さくなっている。これはスキャン数が多いほど、重複して計測する面積が増えるためである。また、対象空間の面積について、同じスキャン数ならば、面積が小さいほど計測の重複度が大きくなる。つまり、何度も同じ場所を計測していることになり、スキャンの削減率が小さくなる。

図９に示すように、生成された占有格子地図Ｍの品質は、網羅率によって変化する。これは、占有格子地図Ｍを網羅率が０．６の場合と０，９の場合とで比較した例である。図９（Ａ）に示す網羅率が０．６の場合では欠けた部分Ｎ（図中、グレーで示す部分）があるが、図９（Ｂ）に示す網羅率が０．９の場合だと欠けた部分はほぼ埋まっている。なお、対象空間の広さや長さなどによっては、網羅率が０．９であっても欠けた領域が見られることがある。この要因は、スキャンの重複が小さく、影響度の大きいスキャンが多数存在することと、長い廊下などでレーザースキャナの最大計測距離が不足することが考えられる。長い廊下の長手方向では計測不可となって領域が未観測になり、廊下の中間部分のスキャンは有効セル数が減って、影響度が小さくなる。このような要因が重なることで、欠落部分が残ることが予想される。したがって、網羅率を高くすれば欠落部分は解消するが、削減率は低くなるため、対象空間や環境に応じて網羅率を適宜に設定することが有効である。

図１０（Ａ）には、作成例における入力スキャン数と削減後のスキャン数との関係が示され、（Ｂ）には、入力スキャン数と削減後のノード数との関係が示され、（Ｃ）には、入力スキャン数と削減後のアーク数との関係が示されている。なお、図１０（Ｂ），（Ｃ）は、前述したループ閉じ込みに用いるポーズグラフを縮約した結果、ノード数およびアーク数が削減されたことを示している。ＳＬＡＭ技術では、各スキャンに対して移動体位置を求めるので、ノードとスキャンは１対１に対応する。ただし、ポーズ調整をキーフレームで行う場合は、キーフレーム単位で移動体位置を抽出してノードとする。ここでは、キーフレーム単位でノードを生成した。ここで、キーフレームとは、一定間隔で間引いたスキャンのことであり、並進距離および回転角が閾値（例えば、それぞれ０．４ｍと６０°）を超えるたびに移動体位置を抽出した。また、ノードに対応するキーフレーム間のスキャン群の中で、影響度ｅ_ｋが閾値ｅ_threを超えるスキャンが１つでもあれば、そのノードを採用し、採用されないノードは削除した。

図１０（Ａ）に示すように、入力スキャン数が増えても、削減後のスキャン数はそれほど増えず、一定あるいは微増であることがわかる。これは、走行につれてスキャン数が増えると同時に、重複部分も増えるためである。図１０（Ｂ）の破線はポーズグラフのキーフレーム間引きだけ行った場合のノード数であるが、キーフレーム間引きだけだと、ノード数はスキャン数に比例して増加する。一方、本発明の処理を行うことで、実線のように、ノード数はさらに削減されるとともに、スキャン数に対する増加率も抑えられていることがわかる。図１０（Ｃ）の破線はキーフレーム間引きだけ行った場合のアーク数であり、アークもノードと同様の傾向があるが、アークはループの個数にも影響されるので、増減の挙動はノードよりも複雑である。

このような本実施形態によれば、以下の作用・効果を奏することができる。
（１）移動体は、走行手段３、検出手段４および制御手段２を備え、制御手段２の地図作成部２３によって占有格子地図Ｍを生成し、更新することで、占有格子地図Ｍの地図精度を確保しつつメモリ容量や演算処理の負荷を軽減させることができる。

（２）制御手段２の地図作成部２３は、影響度算出部２４にて算出された影響度ｅ_ｋに基づいて、スキャンｓ_ｋ（検出情報）を選別してから占有格子地図Ｍを作成するので、効率よく占有格子地図Ｍを作成または更新することができる。すなわち、地図作成部２３は、占有格子地図Ｍに対する影響度ｅ_ｋが小さいスキャンｓ_ｋを用いないように選別することで、演算に利用するスキャン数を減らすことができ、メモリ容量が少なくて済むとともに演算処理の負荷を軽減させることができる。一方、占有格子地図Ｍに対する影響度が大きいスキャンｓ_ｋを選別して用いることで、古いスキャンｓ_ｋであっても有効なものが残されるため、作成する占有格子地図Ｍの地図精度を確保することができる。

（３）観測度数算出部２５は、記憶部２７に記憶されたスキャンｓ_ｋを占有格子地図ＭのセルＣ_ｉごとに計数して観測度数ｂ_ｉとして算出し、この観測度数ｂ_ｉに基づいて有効としたセルＣ_ｉの割合に応じてスキャンｓ_ｋの影響度ｅ_ｋを高くすることで、効率よくスキャンｓ_ｋを選別することができる。すなわち、あるセルＣ_ｉに対して古いものから新しいものまで複数のスキャンｓ_ｋが存在する場合に、その数を観測度数ｂ_ｉとして記録し、新しいものから所定数を有効とし、それよりも古いものを無効とする。そして、有効なセルＣ_ｉが多く存在するスキャンｓ_ｋの影響度ｅ_ｋを高く評価する。このようにすることで観測度数ｂ_ｉが高いセルＣ_ｉに対しては、環境変化が反映されやすい新しいスキャンｓ_ｋの影響度ｅ_ｋが高くなり、占有格子地図Ｍの地図精度を高めることができる。一方、観測度数ｂ_ｉが低いセルＣ_ｉに対しては、比較的古いスキャンｓ_ｋまで影響度ｅ_ｋが高く評価され、古くても有効な情報を含んだスキャンｓ_ｋを適切に残して占有格子地図Ｍを作成することができる。

（４）地図作成部２３は、影響度ｅ_ｋが所定の閾値ｅ_threよりも小さいスキャンｓ_ｋを記憶部２７から消去し、削減したスキャンｓ_ｋに基づいて占有格子地図Ｍを作成することで、占有格子地図Ｍの作成にかかる演算負荷と時間を削減することができるとともに、記憶部２７の容量を節約することができる。さらに、影響度ｅ_ｋの高いスキャンｓ_ｋを使用して占有格子地図Ｍを作成するので、効率よく占有格子地図Ｍを作成することができる。

（５）地図作成部２３は、網羅率算出部２６によって算出された網羅率に基づいて、影響度ｅ_ｋの閾値ｅ_threを設定するので、効率よくスキャンｓ_ｋを選別して占有格子地図Ｍを作成することができる。また、網羅率に基づいて影響度ｅ_ｋの閾値ｅ_threが自動的に設定されるので、演算処理を効率化することができる。

（６）図６に示すような処理手順とすれば、セル値の更新にｂ_threを過ぎた古いスキャンｓ_ｋも使って対数オッズｖ_ｉが更新されるので、環境変化に対するセル値の更新が遅くなる半面、セル値の安定性は増すことになり、占有格子地図Ｍの安定性を向上させることができる。

〔実施形態の変形〕
なお、本発明は、前記実施形態に限定されるものではなく、本発明の目的を達成できる範囲での変形、改良等は本発明に含まれるものである。
例えば、前記実施形態では、移動体として具体的なものを例示しなかったが、移動体としては、サービスロボット、ホームロボットなどの移動ロボットであって、より具体的には、掃除ロボットや警備ロボット、運搬ロボット、案内ロボットなどが例示できる。さらに、移動体としては、自動運転自動車や作業車などであってもよい。さらに、移動体の移動範囲は、二次元平面空間に限らず、三次元空間であってもよく、その場合には移動体がドローンなどの飛行体であってもよい。すなわち、前記実施形態では、ＳＬＡＭ技術として、２Ｄ－ＳＬＡＭを用いた例を示したが、本発明の地図生成システムは、三次元空間を対象にした３Ｄ－ＳＬＡＭにも応用可能である。

前記実施形態では、移動体の移動体本体１が制御手段２を備え、この制御手段２に地図作成部２３や記憶部２７が設けられていたが、占有格子地図Ｍを作成する地図作成手段や、占有格子地図Ｍを記憶する記憶手段などは、移動体本体１ではなく、移動体本体と通信可能な他の機器に設けられ、他の機器によって作成された占有格子地図Ｍに基づいて移動体本体１が自律走行するように構成されていてもよい。また、移動体としては、移動手段（走行手段）を備えていなくてもよく、検出手段が設置された台車等を使用者が手動で移動させつつ周囲を検出するような構成であってもよい。

前記実施形態の占有格子地図Ｍの作成方法では、図４に示すように、検出情報削減工程（ステップＳＴ６）にて検出情報を選別して記憶部２７から削減してから、削減された検出情報に基づいて地図再作成工程（ステップＳＴ７）にて占有格子地図Ｍを更新したが、このような処理手順に限られず、図１１に示すような処理手順を採用してもよい。図１１は、移動体の動作の変形例を示すフローチャートである。図１１に示す手順では、検出情報削減工程（ステップＳＴ６）よりも先に地図再作成工程（ステップＳＴ７）を実行し、削減される前の検出情報に基づいて占有格子地図Ｍを更新し、その後に選別された検出情報を記憶部２７から削減する。この手順によれば、地図作成部２３は、検出情報を記憶部２７から削除する前に、記憶部２７に記憶された全ての検出情報を使用して占有格子地図Ｍを作成するので、占有格子地図Ｍの安定性を向上させることができる。

以上のように、本発明は、占有格子地図を用いて自律移動するロボットなどの各種の移動体において、占有格子地図の作成などに要するメモリ容量が少なくて済むとともに演算処理の負荷を軽減させることによって、占有格子地図の作成の効率化を図ることができる地図生成システムに好適に利用できる。

１ 移動体本体
２ 制御手段
３ 走行手段（移動手段）
４ 検出手段
２３ 地図作成部（地図作成手段）
２４ 影響度算出部（影響度算出手段）
２５ 観測度数算出部（観測度数算出手段）
２６ 網羅率算出部（網羅率算出手段）
２７ 記憶部（記憶手段）
Ｃ セル（区画）
Ｍ 占有格子地図
Ｓ スキャン（検出情報）

Claims (8)

1. 移動体の移動範囲における周囲の物体までの距離と方向を検出手段によって検出し、物体の有無に基づいて占有格子地図を生成する地図生成システムであって、
   前記検出手段による検出単位ごとの検出値を検出情報として時系列に記憶する記憶手段と、
   前記記憶手段に記憶された検出情報に基づいて占有格子地図を作成して前記記憶手段に記憶させる地図作成手段と、
   前記記憶手段に記憶された検出情報ごとに地図作成に寄与する度合いを示す影響度を、前記占有格子地図の区画の値に関与する検出情報の集合を区画ごとに求め、該関与検出情報集合に含まれる検出情報を新しい順に並べた順列において、該順列の先頭から所定個数番目までの検出情報に対しては該区画を有効とし、該所定個数番目の次から最後までの検出情報に対しては該区画を無効とすることによって、検出情報ごとの各区画の有効または無効を決定するとともに、有効な区画の割合に応じて高くなるように該検出情報の影響度を算出する影響度算出手段と、を備え、
   前記地図作成手段は、前記影響度算出手段にて算出された影響度が所定の閾値よりも小さい検出情報を用いないように選別して占有格子地図を作成する
   ことを特徴とする地図生成システム。
2. 請求項１に記載された地図生成システムにおいて、
   前記記憶手段に記憶された検出情報を占有格子地図の区画ごとに、該検出情報の新しい順に計数して観測度数として算出する観測度数算出手段をさらに備え、
   前記影響度算出手段は、新しい検出情報から入力して、前記観測度数算出手段にて算出された観測度数が所定の閾値より小さい区画を該検出情報に対して有効とし、前記観測度数が前記閾値より大きい区画を該検出情報に対して無効とすることによって、検出情報ごとの各区画の有効または無効を決定するとともに、有効な区画の割合に応じて高くなるように該検出情報の影響度を算出する
   ことを特徴とする地図生成システム。
3. 請求項２に記載された地図生成システムにおいて、
   前記地図作成手段は、前記観測度数算出手段にて算出された観測度数が所定の閾値よりも小さい区画に対して区画値を更新して占有格子地図を作成する
   ことを特徴とする地図生成システム。
4. 請求項２に記載された地図生成システムにおいて、
   前記地図作成手段は、前記観測度数算出手段にて算出された観測度数に関わらずに、各区画に対する区画値を更新して占有格子地図を作成する
   ことを特徴とする地図生成システム。
5. 請求項１から請求項４のいずれかに記載された地図生成システムにおいて、
   前記地図作成手段は、前記影響度算出手段にて算出された影響度が所定の閾値よりも小さい検出情報を前記記憶手段から消去する
   ことを特徴とする地図生成システム。
6. 請求項５に記載された地図生成システムにおいて、
   前記地図作成手段は、検出情報を前記記憶手段から消去する前に、前記記憶手段に記憶された全ての検出情報を使用して占有格子地図を作成する
   ことを特徴とする地図生成システム。
7. 請求項５または請求項６に記載された地図生成システムにおいて、
   前記記憶手段に記憶された全ての検出情報の影響度の合計と、所定の数の検出情報の集合に属する検出情報の影響度の合計と、の比を網羅率として算出する網羅率算出手段をさらに備え、
   前記地図作成手段は、前記網羅率算出手段にて算出された網羅率に基づいて、前記影響度の閾値を設定する
   ことを特徴とする地図生成システム。
8. 移動手段、検出手段および制御手段を備えた移動体であって、
   請求項１から請求項７のいずれかに記載された地図生成システムが前記制御手段によって構成される
   ことを特徴とする移動体。

Images