JP6773471B2

JP6773471B2 - 自律移動体と環境地図更新装置

Info

Publication number: JP6773471B2
Application number: JP2016146615A
Authority: JP
Inventors: 亮暢藤井; 英典藪下
Original assignee: Toyota Motor Corp; Toyota Central R&D Labs Inc
Current assignee: Toyota Motor Corp; Toyota Central R&D Labs Inc
Priority date: 2016-07-26
Filing date: 2016-07-26
Publication date: 2020-10-21
Anticipated expiration: 2036-07-26
Also published as: JP2018017826A

Description

本明細書に開示する技術は自律移動体に関する。詳しくは、自律移動体が記憶している環境地図を更新する技術に関する。

移動体が移動する領域内に存在する障害物の位置を示す環境地図を記憶しており、移動体から障害物までの距離と方位を観測する観測装置を備えており、環境地図と観測装置による観測結果を使って移動体の位置を計算し、その位置から障害物を避けて目標位置に至る進路を探索して移動する移動体（以下では、自律移動体という。）が開発されている。

障害物には様々な種類が存在し、固定的に存在する障害物もあれば、移動する障害物も存在する。移動する障害物が環境地図に反映されておらず、環境地図を修正ないし更新する必要がある場合がある。

特許文献１に、領域内を移動しながら環境を観測し、移動体の移動量と距離センサによる観測データを用いて、環境地図を生成する技術が開示されている。特許文献１の技術では、環境の変化を認識すると、当該部分の撮像画像をオペレータに提示する。オペレータは、当該部分の環境地図を更新する必要があるか否かを判断し、この判断に基づいて最新の環境地図に更新する。

特許文献２の技術では、オペレータが、環境地図を用いて、環境が不変である領域と、環境が可変である領域とを設定する。環境が不変である領域では、更新を禁止することによって、誤差が蓄積することを抑制する。

特開２００９−１６９８４５号公報特開２０１４−８９７４０号公報

特許文献１の技術では、環境地図を更新するか否かについて、オペレータの判断を必要とする。すなわち、自律移動体自体によって自律的に環境地図を更新することができない。また、特許文献２の技術では、オペレータが事前に、環境が不変である領域と可変である領域を設定する必要がある。移動領域が変わる度に、オペレータの処理を必要とする。いずれの技術においても、自律移動体のみでは環境地図を更新するか否かの判断を行うことができない。本明細書は、オペレータによる処理を要することなく、移動体自体が自律的に環境地図を更新する必要があるか否かを判断し、更新が必要なときに環境地図を更新する技術を開示する。

環境地図が利用可能であれば、自律移動体の位置を仮定することによって、仮定した位置から障害物までの距離と方位を環境地図から計算することができる。以下では仮定した位置と環境地図から計算した障害物までの距離と方位の情報を計算結果という。
自律移動体から障害物までの距離と方位を観測する観測装置が用意されていれば、その観測結果から、自律移動体から障害物までの距離と方位の情報を特定することができる。以下では、観測装置で観測した障害物までの距離と方位の情報を観測結果という。
複数の位置を仮定し、仮定した位置の各々で計算結果を計算することが可能である。この場合、観測結果に最もよく一致する計算結果をもたらす仮定位置に自律移動体が存在しているとすることができる。

しかしながら、前記した移動する障害物の存在等によって、環境地図が常に正しいとは限らない。環境地図の更新が必要な状況では、環境地図から計算した障害物までの距離と方位の情報が誤っており、観測装置で観測した障害物までの距離と方位の情報に一致しない。環境地図の更新が必要な状況では、自律移動体の正確な位置が特定できないという問題が生じる。すなわち、環境地図の更新が必要な状況では、観測装置によって自律移動体から障害物までの距離と方位が観測されていても、自律移動体の正確な位置が特定できないために、環境地図を正しく更新することができない。環境地図を使って位置を特定する技術と、環境地図を更新する技術は、本質的に両立しない要因を持っている。本明細書では、両者を両立させる技術を開示する。

複数の位置を仮定し、環境地図を利用し、仮定した位置の各々において障害物までの距離と方位を計算し（計算結果を求め）、観測装置で観測した障害物までの距離と方位（観測結果）と計算結果とを対比して自律移動体の位置を推定する技術によると、位置を推定できるだけでなく、推定した位置の信憑度を計算することができる。例えば、特定の仮定位置でのみ計算結果と観測結果の一致度が高く、その他の仮定位置の全部で計算結果と観測結果の一致度が低い場合、推定した位置の信憑度が高いということができる。それに対して、複数個の仮定位置の全部について、計算結果と観測結果の一致度がほぼ一様であれば、複数個の仮定位置の近傍に移動体が位置していることが推定できるものの、それ以上には明確な位置を推定できず、推定した位置の信憑度が低いということができる。

本明細書では、信憑度を数学的に計算し、信憑度が低下したときに環境地図を更新するというアルゴリズムを採用した技術を開示する。

本明細書が開示する自律移動体は、環境地図記憶手段と、観測装置と、地図依存式の推定位置計算手段と、信憑度計算手段と、環境地図更新手段を備えている。環境地図記憶手段は、自律移動体が移動する領域内に存在する障害物の位置を示す環境地図を記憶している。観測装置は、自律移動体から障害物までの距離と方位を観測する。地図依存式の推定位置計算手段は、自律移動体が環境地図上に位置すると仮定した仮定位置と環境地図から計算される障害物までの距離と方位の情報と、観測装置で観測した障害物までの距離と方位の情報から、自律移動体の位置を推定する。信憑度計算手段は、推定した位置の信憑度を計算する。環境地図更新手段は、信憑度が所定値より低下したときに、環境地図記憶手段が記憶している環境地図を、観測装置が観測した障害物までの距離と方位の情報に基づいて更新する。

上記の自律移動体では、信憑度が所定値より低下したときに、観測装置が観測した障害物までの距離と方位の情報に基づいて環境地図を更新する。すなわち、地図依存式の推定位置計算手段で計算した推定位置に許容範囲以上の誤差が生じる確率が所定値より上昇したときに、環境地図を更新する。このため、オペレータによる処理を要さず、自律移動体自体によって、環境地図の更新の要否を判断することができる。

本明細書が開示する技術の詳細、及び、さらなる改良は、発明を実施するための形態及び実施例にて詳しく説明する。

移動体のシステム構成を示す。移動体が環境地図更新処理等を実施する際のシステム構成を示す。移動体が学習データ生成処理を実施する際のシステム構成を示す。移動体が学習時に移動する移動領域の一例を示す。学習データの一例を示す。移動体が信憑度計算式生成処理を実施する際のシステム構成を示す。信憑度計算式のグラフを示す。信憑度に依存する重み係数のグラフを示す。移動体が実施する環境地図更新処理のフローチャートを示す。移動体が実施する環境地図更新処理内容を視覚的に表した図を示す。

以下に説明する実施例の主要な特徴を列記しておく。なお以下に記載する技術要素は、それぞれが独立した技術要素であって、単独であるいは各種の組合せによって技術的有用性を発揮するものであり、出願時の請求項に記載の組合せに限定されるものではない。

（特徴１）自律移動体に環境地図が誤っている領域を移動させ、確率分布と信憑度の間に成立する式を求めておく。
（特徴２）観測装置は、自律移動体に対する方位角を変化させながら、方位角に対応づけて障害物までの距離を観測する。
（特徴３）観測装置は、自律移動体に搭載されている。
（特徴４）環境地図から、障害物の輪郭上に位置している点群の位置を導出可能である。
（特徴５）観測装置から、障害物の輪郭上に位置している点群の位置を導出可能である。
（特徴６）環境地図更新手段は、同一方位角において、観測装置で観測した距離が環境地図から計算した距離よりも短い場合に、環境地図が記憶している点を観測装置が観測した点に置き換える。

実施例の自律移動体（以下では、移動体という。）について説明する。移動体は、左右に車輪を有する車輪駆動型の移動体であり、所定の領域内を自律的に走行する。左右の車輪は独立して駆動され、左右の車輪の回転速度を変えることで、移動体は進行方向を変えることができる。移動体は、後記するように、移動体が存在している位置を推定する処理を繰り返して実行する。各回の処理をステップという。後述する「現在のステップ」は、移動体が実施中のステップを意味する。また、「１つ前のステップ」は「現在のステップから１つ前のステップ」をいい、「１つ後の（次の）ステップ」は「現在のステップから１つ後の（次の）ステップ」を意味する。移動体は、ステップごとに計算される推定位置に基づいて、スタート地点から目標位置まで走行する。なお移動体は、自身の位置と自身が向いている方位を推定するが、以下では、移動体の位置と方位をまとめて「位置」と称する。

図１に示すように、移動体１０は、エンコーダ１２と、レーザレンジファインダ１４（以下、ＬＲＦ１４と称する）と、三角測量センサ１６と、演算装置１８を備える。

移動体１０の左輪を駆動するモータと右輪を駆動するモータの各々にエンコーダ１２が設けられ、各エンコーダ１２は左輪回転角度と右輪回転角度を独立に検出する。一対のエンコーダ１２で検出された左輪回転角度と右輪回転角度が、演算装置１８に入力される。

ＬＲＦ１４は、レーザ光を射出し、射出したレーザ光が物体で反射して返ってくるまでの時間を観測する。ＬＲＦ１４で観測される時間から、ＬＲＦ１４（即ち、移動体１０）から物体までの距離が観測される。またＬＲＦ１４からレーザ光を射出する方位角（移動体に固定された軸からの回転角）は時間的に変化し、所定の方位角度範囲内を走査する。ＬＲＦ１４は、移動体１０に対する方位角に対応づけて障害物までの距離を観測することができる。以下、移動体１０から障害物までの距離と方位を「観測結果」と称する。ＬＲＦ１４で取得した観測結果は、演算装置１８に入力される。ＬＲＦ１４は、移動体１０に固定された方向に対して所定の角度範囲内（例えば、左右方向にそれぞれ６０°の角度範囲内）でレーザ光の射出方向を走査する。
三角測量センサ１６については、後記する。

演算装置１８は、演算処理を行うＣＰＵ２０、演算処理のデータが一時的に記憶されるＲＡＭ２２、及びＣＰＵによって実行される演算プログラム等が記憶されたＲＯＭ２４を備えている。ＲＯＭ２４の一部はＥＥＰＲＯＭであり、書き換え可能となっている。
演算装置１８は、後述する様々な処理を実行する。ＣＰＵ２０がＲＯＭ２４に記憶された演算プログラムを実行することで、ＣＰＵ２０は、図２に示す地図依存式推定位置計算部２８、地図不依存式推定位置計算部２９、図３に示す位置誤差計算部３４等として機能する。なお、演算装置１８による移動体１０の走行制御については、公知の方法で行うことができるため、ここでは、演算装置１８による推定位置計算処理と環境地図更新処理等について詳細に説明する。

（地図依存式の推定位置の計算処理）
図２に示すように、演算装置１８は、環境地図記憶部２６と、地図依存式推定位置計算部（第１計算部と略称する）２８を備える。第１計算部２８は、時間間隔をおいて繰り返して移動体１０の推定位置を計算する。

環境地図記憶部２６は、ＲＯＭ２４のＥＥＰＲＯＭに記憶されており、移動領域内に存在する物体の輪郭の位置（ｘ，ｙ座標）と高さ（ｚ座標）を示す情報が記録された環境地図を記憶している。環境地図に記憶される物体としては、移動の障害となる壁、家具等が含まれる。後記するように、環境地図は更新される。環境地図を用意した時点では存在していなかった障害物が移動して移動領域内に存在するような場合には、環境地図に障害物の存在を追加する更新がされる。あるいは、環境地図を用意した時点では移動領域内に存在していた障害物が無くなったなった場合には、環境地図から障害物の存在を削除する更新がされる。

第１計算部２８は、時間間隔をおいて繰り返してモンテカルロ位置同定法（ＭｏｎｔｅＣａｒｌｏＬｏｃａｌｉｚａｔｉｏｎ（以下、ＭＣＬと称する））を実行し、移動体１０の推定位置を計算する。
ＭＣＬによって地図に依存する推定位置を計算する処理では、
（１）１つ前のステップにおいて計算された統合推定位置を用いる。現在のステップが、移動体１０がスタート地点から移動を開始してからの最初のステップであるときは、上記「１つ前のステップにおいて計算された統合推定位置」はスタート地点の位置を表す。
（２）１つ前のステップにおいて計算された統合推定位置を、移動体１０の運動モデルに従って移動させ、現在のステップの実行時における推定位置を計算する。この段階では、運動モデルに従って推定位置を計算する。
（３）前記（２）の推定位置を中心にし、その周囲に分布しているパーティクル集合を生成する。ここでいうパーティクルとは、その位置にあると仮定して計算を進める仮定位置のことをいい、移動体１０のｘ座標、ｙ座標、及びヨー角を備える。
（４）次に、環境地図記憶部２６から取得した環境地図と、パーティクルの位置とヨー角から、パーティクル毎に、方位角に対する障害物までの距離を計算する。すなわち、計算結果を求める。
（５）ＬＲＦ１４も、方位角に対する障害物までの距離を観測している。すなわち、観測結果が得られている。
（６）そこで、前記（４）で計算した方位角に対する障害物までの距離の分布（計算結果）と、前記（５）で観測した方位角に対する障害物までの距離の分布（観測結果）を対比する。
（７）環境地図が正確なものであれば、特定の１個のパーティクルにおいて（４）の計算結果と（５）の観察結果が一致し、その他のパーティクルの全部で（４）と（５）が不一致となる。この場合、移動体１０がその特定のパーティクルに位置している確率が高く、他のパーティクルに位置している確率が低いといえる。すなわち、（４）の計算結果と（５）の観察結果の一致度から、移動体１０が各パーティクルに位置している確率を計算することができる。
（８）パーティクル毎に確率を計算すれば、確率の分布を得ることができる。その確率分布がピークとなる位置に、移動体１０が位置しているとすることができる。
（９）パーティクルの選択が不適切であり、正しい推定位置が計算できない場合がある。その場合は、確率分布において、高い確率が分布している領域ではパーティクル密度を上げ、低い確率が分布している領域ではパーティクル密度を下げる処理をして、（４）以降の処理を繰り返す。これによって、パーティクルの分布が適正化され、正しい推定位置が計算可能となる。

前記では、（２）によってパーティクルの中心位置を定めている。それに対して、パーティクルの中心位置を定める基準が得られない場合には、移動領域の全域に一様に広がるパーティクル群を仮定して計算を始めてもよい。前記したパーティクル群の適正化処理によって、パーティクルの分布が適正化され、正しい推定位置が計算可能となる。
前記（２）に代えて、１つ前のステップにおいて計算された統合推定位置に、最新１ステップ分の移動ベクトル（エンコーダ１２から得られる）を加算した推定位置を、パーティクル群の中央位置としてもよい。この場合も、前記（８）で計算される推定位置は、地図に依存して推定した位置となる。

第１計算部２８は、パーティクルの集合中に移動体１０の実際位置（以下では、真値とも称する）が含まれ、いずれかのパーティクルにおける計算結果が観測結果に極めて近くなるまで、上記の処理を繰り返す。１回のステップ中で、パーティクル集合の分布が複数回更新されることがある。これにより、第１計算部２８において、現在のステップにおける移動体１０の推定位置が計算される。この推定位置は、環境地図から推定されたものである。

（学習データ生成処理）
移動体１０は、自律走行を行う前に、学習走行と呼ばれる走行を行う。以下では、移動体１０の学習走行、及び学習走行時に得られるデータを元に学習データを生成する処理について説明する。なお、学習データとは、後述する信憑度計算式を生成する処理に用いられるデータである。

図３に示すように、演算装置１８は、自律走行に使用する各部の他に、真値計算用地図記憶部３０と、真値計算部３２と、位置誤差計算部３４と、学習データ生成部３６と、学習データ記憶部３８を備える。環境地図記憶部２６には、移動体１０が学習走行を行う領域の環境地図が記憶されている。移動体１０が学習走行を行う領域は、自律走行を行う領域と異なっていてもよい。学習走行時と自律走行時とで移動領域が異なる場合は、環境地図記憶部２６は、両方の移動領域の環境地図を記憶している。以下では、学習走行時の移動領域と自律走行時の移動領域とを区別するために、前者を「第１領域」と称し、後者を「第２領域」と称する。また、第１領域の環境地図を「第１環境地図」と称し、自律走行領域の環境地図を「第２環境地図」と称する。

図４は、移動体１０が学習走行を行う第１領域４０の一例を示す。図４に示すように、第１領域４０の一部には、障害物である塀４２，４３が配置されている。塀４２，４３の対向面（内側の面）にはリフレクタ４４が等間隔で固定されている。学習走行時の移動体１０の走行は、リモートコントローラ（リモコン）で制御される。破線は、学習走行時の移動体１０の走行ルートを示す。破線で示すように、移動体１０は、塀４２，４３が配置されていない領域、及び対向する塀４２，４３の間の領域の双方を走行する。

環境地図記憶部２６に記憶されている第１環境地図は完全なものでなく、例えば塀４２は記憶されているものの、塀４３は記憶されていない。学習走行と学習データ生成処理は不完全な環境地図のもとで実施する。

真値計算用地図記憶部３０は、ＲＯＭ２４に設けられており、塀４２，４３とリフレクタ４４の位置が記録された地図を記憶している。真値計算用地図記憶部３０に記憶されている環境地図は正確であって、更新を要しない。

三角測量センサ１６は、ほぼ全方位（３６０°）にレーザ光を射出し、リフレクタ４４で反射して返ってくるレーザ光を検知することで、リフレクタ４４までの距離と移動体１０に対するリフレクタ４４の方位角を観測し、これらの情報を演算装置１８の真値計算部３２に出力する。三角測量センサ１６は、移動体１０に搭載されている。

真値計算部３２は、三角測量センサ１６から取得した距離と方位角の情報から、リフレクタ４４に対する移動体１０の相対位置を特定する。真値計算用地図記憶部３０から取得した地図にはリフレクタ４４の位置が記録されているため、当該地図に移動体１０の前記相対位置をマッピングすることで、当該地図上における移動体１０の位置を特定できる。このようにして、真値計算部３２は、移動体１０の実際の位置（真値）を計算する。この処理はＣＰＵ２０で行われる。移動体１０の真値は、位置誤差計算部３４に出力される。

一方、第１計算部２８は、不完全な第１環境地図を用いて移動体１０の推定位置を計算し、その結果を位置誤差計算部３４に出力する。推定位置計算処理と真値計算処理は同じタイミングで行われる。また、第１計算部２８は、最新のパーティクル集合（即ち、推定位置を計算したときのパーティクル集合）の分布から、ｘ方向の標準偏差σ_ｘ、ｙ方向の標準偏差σ_ｙ、及びパーティクル毎に計算される確率の最大値ｗ_ｍａｘを計算し、これらの値（標準偏差σ_ｘ、標準偏差σ_ｙ、最大確率ｗ_ｍａｘ）を学習データ生成部３６に出力する。

位置誤差計算部３４は、真値計算部３２から取得した真値と、第１計算部２８から取得した推定位置との差（位置誤差）を計算し、学習データ生成部３６に出力する。この処理はＣＰＵ２０で行われる。

学習データ生成部３６は、位置誤差計算部３４から取得した位置誤差が１５ｃｍ以下の場合は、第１計算部２８が計算した推定位置が真値から許容範囲内である（すなわち位置の推定に成功した）とみなし、推定結果を「ＯＫ」と分類する。一方、位置誤差が１５ｃｍを超える場合は、第１計算部２８が計算した推定位置が真値から許容範囲以上ずれた（すなわち位置の推定に失敗した）とみなし、推定結果を「ＮＧ」と分類する。学習走行では、位置誤差が「ＮＧ」と分類される場合を意図的に作り出している。即ち、実際の第１領域４０には、第１環境地図に記録されていない塀４３が配置されている。このため、移動体１０が塀４２，４３の間を走行する際は、ＬＲＦ１４により取得される観測結果と、パーティクル毎に計算する計算結果とのマッチング度が低くなり、適合パーティクルの発見が困難になる。これは、仮にパーティクル集合の中に真値が含まれていたとしても、移動体１０は真値と一致するパーティクルを適合パーティクルとして発見できないことを意味する。このため、移動体１０が塀４２，４３の間を走行する場合は、真値とは異なるパーティクルを適合パーティクルとみなし、そのパーティクルを推定位置と計算する可能性が高くなる。この結果、推定位置と真値との位置誤差が大きくなり、その位置誤差が１５ｃｍを超える場合は「ＮＧ」と分類されることになる。

このように、学習データ生成部３６では、位置誤差を基準とし、即ち、「真値」に対する移動体１０の推定位置のずれ量を基準として、地図に依存する推定結果の分類が行われる。本実施例では、位置の推定結果がＯＫとなる許容範囲の基準値を１５ｃｍに設定したが、基準値はこれに限られない。許容範囲の基準値を低くするほど、信憑度計算式による予測精度を高くすることができる。

図５は、学習データ生成部３６により生成される学習データの例を示す。図５に示すように、学習データ生成部３６は、地図に依存する推定位置の計算時刻と、位置誤差計算部３４から取得した位置誤差と、推定位置の計算結果を分類した結果と、第１計算部２８から取得した３つの変数（標準偏差σ_ｘ、標準偏差σ_ｙ、最大確率ｗ_ｍａｘ）とを関連付けたデータセットを時系列に並べたデータセット群（学習データ）を生成する（なおσ_ｘ、σ_ｙ、ｗ_ｍａｘの欄に示したアルファベットは、実際には数値である）。学習データを参照することで、各時刻における位置誤差、推定位置の計算結果の成功／失敗、及び推定位置が計算されたときの変数を知ることができる。例えば、時刻ｔ＝２ｓでは、推定位置と真値との位置誤差は７ｃｍであるため、推定位置計算結果は「ＯＫ」と分類されており、そのときの変数σ_ｘ、σ_ｙ、ｗ_ｍａｘは、それぞれｂ、ｇ、ｌであることがわかる。また、時刻ｔ＝１０ｓでは、推定位置と真値との位置誤差は２０ｃｍであるため、推定位置計算結果は「ＮＧ」と分類されており、そのときの変数σ_ｘ、σ_ｙ、ｗ_ｍａｘは、それぞれｃ、ｈ、ｍであることがわかる。学習データ生成処理はＣＰＵ２０で行われる。

学習データ記憶部３８は、ＲＯＭ２４に設けられており、学習データ生成部３６で生成された学習データを記憶する。学習データ記憶部３８に記憶された学習データは、後述する信憑度計算式生成部４６に出力される。このように、移動体１０自身が学習データを記憶しておくことにより、移動体１０に不具合が生じた場合等に、学習データのログを分析してエラーがないかどうか検証することが可能となる。このため、移動体１０に不具合が生じた場合にその原因を発見し易くなり、移動体１０の信頼性を向上させることができる。なお、本実施例では、データセットに推定位置の計算時刻が含まれているが、この構成に限られない。例えば、データセットは時刻を含んでいなくてもよく、学習データは、データセットが時刻とは無関係に並べられたデータ群により構成されていてもよい。

（信憑度計算式生成処理）
移動体１０が信憑度計算式を生成する処理について説明する。図６に示すように、演算装置１８は、上述した処理で使用される各部の他に、信憑度計算式生成部４６と、信憑度計算式記憶部４８を備える。

信憑度計算式生成部４６は、学習データ記憶部３８に記憶されている学習データを取得し、学習データをロジスティック回帰分析して、信憑度計算式を生成する。ロジスティック回帰分析は多変量解析の一手法であり、結果が２値の場合に、その結果の起きる確率を予測できる統計的な回帰モデルを生成する手法である。具体的には、次の数式で表されるロジスティックモデル；

（Ｚ：目的変数、Ｘ_１〜Ｘ_３：説明変数、ｂ_０：定数、ｂ_１〜ｂ_３：回帰係数）に学習データを適用して、定数ｂ_０及び回帰係数ｂ_１〜ｂ_３の値を求める。学習データを適用する際は、説明変数Ｘ_１、Ｘ_２、Ｘ_３に学習データの標準偏差σ_ｘ、標準偏差σ_ｙ、最大確率ｗ_ｍａｘのそれぞれ用いる。これにより、次の数式で表される信憑度計算式；

が生成される（図７参照）。ｐの値は、第１計算部２８により計算される推定位置がＮＧとなる確率である。ｐはまた、計算された推定位置の真値からのずれ量の大きさを示す尺度でもあり、ずれ量が大きいほど大きくなる。本明細書ではｐを不信度という。不信度ｐが所定値よりも大きくなったことは、計算した推定位置の信憑度が所定値より低下したことを意味する。
説明変数Ｘ_１〜Ｘ_３に用いる変数（σ_ｘ、σ_ｙ、ｗ_ｍａｘ）は互いに独立しているため、回帰係数ｂ_１〜ｂ_３は、項目間の影響が排除された係数となる。従って、不信度を算出する際に、より正確に項目ごとの影響度を量ることができる。信憑度計算式生成処理はＣＰＵ２０で行われる。
信憑度計算式記憶部４８は、ＲＯＭ２４に設けられており、信憑度計算式生成部４６で生成された信憑度計算式を記憶する。

（信憑度計算処理）
移動体１０が不信度ｐを計算する処理について説明する。演算装置１８は、上述した処理で使用される各部の他に、信憑度計算部５０を備える（図２参照）。

移動体１０が自律走行領域において自律走行を行う際は、第１計算部２８が、自律走行時用環境地図（第２環境地図）を用いて推定位置を計算する。第１計算部２８で現在の推定位置が計算されたときの３つの変数σ_ｘ、σ_ｙ、ｗ_ｍａｘが、信憑度計算部５０に出力される。

信憑度計算部５０は、第１計算部２８から取得した３つの変数σ_ｘ、σ_ｙ、ｗ_ｍａｘを、信憑度計算式記憶部４８から取得した信憑度計算式のＸ_１、Ｘ_２、Ｘ_３にそれぞれ入力して、現在のステップにおける不信度ｐを求める。不信度ｐは、ステップごとに計算される。この処理はＣＰＵ２０で行われる。信憑度計算部５０で算出された不信度ｐは、地図更新判断部５８（後述）に出力される。なお、移動体１０がスタート地点から移動を開始してからの最初のステップでは、判定の基準となる不信度ｐがまだ算出されていない。不信度ｐの計算は、２回目以降のステップから実施される。
不信度ｐは、重み係数計算部５２にも伝えられる。これについては後記する。

（地図に依存しない推定位置の計算処理）
図２に示すように、演算装置１８は、地図に依存する推定位置を計算する第１計算部２８の他に、地図に依存しない推定位置を計算する地図不依存式推定位置計算部（第２計算部と略称する）２９と、統合推定位置計算部５４と、統合推定位置記憶部５６を備える。統合推定位置記憶部５６は、ＲＡＭ２２に設けられており、統合推定位置計算部５４によって推定された統合推定位置（厳密には、環境地図上の座標）をステップごとに記憶している。第２計算部２９は、統合推定位置記憶部５６に記憶されている統合推定位置のうち、１つ前のステップにおける統合推定位置に、移動体１０の１ステップ分の移動量ベクトルを加算することにより算出される値を、現在のステップにおける移動体１０の位置と推定する。移動量ベクトルはエンコーダ１２から入手可能である。上記の説明から明らかなように、第２計算部２９は、直接的には環境地図を用いずに推定位置を計算する。なお、第２計算部２９が計算した推定位置に、環境地図に依存した要素が含まれる場合がある。１つ前のステップにおける統合推定位置に、地図に依存する要素が含まれているからである。

なお、上記の「１つ前のステップにおける統合推定位置」は、別言すれば、統合推定位置記憶部５６に記憶されている統合推定位置の中で最も新しい統合推定位置である。なお現在のステップが、移動体１０がスタート地点から移動を開始してから最初のステップであるときは、「１つ前のステップにおける統合推定位置」とは、スタート地点の位置を意味する。

（統合推定位置計算処理）
移動体１０は、第２領域において自律走行を行う前に、上述した学習データ生成処理及び信憑度計算式生成処理を実施する。そして、移動体１０が自律走行を行う際は、信憑度計算式から算出した信憑度に基づいて重み係数を計算し、その重み係数を用いて統合推定位置を計算する。以下では、移動体１０の自律走行時における統合推定位置計算処理について説明する。図２に示すように、演算装置１８は、上述した処理で使用される各部の他に、重み係数計算式記憶部５１と、重み係数計算部５２と、統合推定位置計算部５４を備える。

重み係数計算式記憶部５１は、ＲＯＭ２４に設けられており、予め設定された重み係数計算式を記憶している。重み係数計算式は、次のシグモイド関数；

（ｐ：不信度、α：重み係数）で表される（図８参照（後述））。
重み係数計算部５２は、重み係数計算式記憶部５１から取得した重み係数計算式に、信憑度計算部５０から取得した現在のステップにおける不信度ｐを入力して重み係数αを求める。この処理はＣＰＵ２０で行われる。重み係数計算部５２で算出された重み係数αは、統合推定位置計算部５４に出力される。

統合推定位置計算部５４は、重み係数計算部５２から取得した重み係数αを用いて、第１計算部２８から取得した推定位置（地図に依存する推定位置）と第２計算部２９から取得した推定位置（地図に依存しない推定位置）の重み付き平均値を算出し、その値を統合推定位置とする。具体的には、統合推定位置計算部５４は、次の数式；

（ｒ_ｆ：統合推定位置、ｒ_１：地図に依存して推定した位置、ｒ_２：地図に依存しないで推定した位置）に従って計算を行い、移動体１０の現在のステップにおける統合推定位置を算出する。重み係数αは「地図に依存する推定位置」の重み係数として使用され、１から重み係数αを減じた係数１−αは「地図に依存しない推定位置」の重み係数として使用される。この処理は、ＣＰＵ２０で行われる。統合推定位置計算部５４で算出された統合推定位置は、統合推定位置記憶部５６と観測点群生成部６０（後述）に出力される。統合推定位置記憶部５６に出力されて記憶される統合推定位置は、１つ後のステップにおける地図に依存しない推定位置の計算処理に用いられる。

ここで、上記の関数（数３）及び図８を参照して重み係数計算式について説明する。重み係数計算式により算出される重み係数αは、不信度ｐの関数となっている。地図に依存する推定位置の重み係数である重み係数αは、不信度ｐが大きくなるにつれて（即ち、地図に依存する推定位置の推定精度が低下するにつれて）減少している。具体的には、重み係数αの値は、不信度ｐが０＜ｐ＜０．１を満たすときは１に近似しており、ｐ＝０．２のときは０．５であり、０．３＜ｐを満たすときは０に近似している。このため、統合推定位置は、不信度ｐが１０％未満のときは「地図に依存する推定位置」とほぼ一致し、不信度ｐが２０％であるときは「地図に依存する推定位置」と「地図に依存しない推定位置」の中間地点となり、不信度が３０％超のときは「地図に依存しない推定位置」とほぼ一致する。なお、重み係数計算式が用いする定数の値は、上記の値（５０、０．２）に限られない。また、重み係数計算式はシグモイド関数に限られない。重み係数αの値を不信度ｐの値に対してどのように対応付けるかによって、種々の関数を用いることができる。

（地図更新判断処理）
次に、地図更新判断処理について説明する。移動体１０が、自律走行領域において自律走行用環境地図（第２環境地図）には記録されていない障害物の付近を走行すると、ＬＲＦ１４で取得される観測結果と環境地図から計算される計算結果とのマッチング度が低くなるため、「地図に依存する推定位置」の推定精度が低下する。このため、不信度ｐが高くなる。即ち、たとえパーティクル集合の中に真値が含まれていたとしても、移動体１０が真値に相当するパーティクルを適合パーティクルと見なすことができず、別のパーティクルを適合パーティクルと見なすことになる。実施例の移動体１０では、第１計算部２８は、ＭＣＬの手法に則り、地図に依存する推定位置を計算する。このため、移動体１０が第２環境地図に記録されていない障害物の付近を走行する場合は、第１計算部２８により計算される地図に依存する推定位置が真値から離れていくことになり、最終的にはパーティクル集合に真値が含まれなくなる。一旦パーティクル集合に真値が含まれなくなると、この後でたとえ移動体１０が自律走行領域（第２領域）において正しい環境地図が記憶されている領域を走行するようになったとしても（即ち、有用な観測結果を取得できるようになったとしても）、ＭＣＬの手法では、真値が含まれる範囲にパーティクル集合を生成することが不可能になることがある。即ち、第１計算部２８で、精度良く地図に依存する推定位置を計算することが不可能になることがある。地図更新判断部５８は、このような事態を避けるために、不信度ｐが高くなっていき、不信度ｐが２０％を超えると、環境地図の更新指令を観測点群生成部６０に出力する。すなわち、地図更新判断部５８は、不信度ｐが０＜ｐ≦０．２を満たすとき（地図に依存する推定位置の信憑度が高いとき）は、環境地図の更新指令を出力せず、０．２＜ｐを満たすとき（地図に依存する推定位置の信憑度が低いとき）は、観測点群生成部６０に環境地図の更新指令を出力する。この処理はＣＰＵ２０で行われる。なお、本実施例では、第１計算部２８の重みと第２計算部２９の重みが５０：５０となる値（すなわち、ｐ＝０．２）を環境地図の更新指令の閾値としているが、環境地図の更新を実行するか否かを判定する基準となる不信度ｐは、上記に限られない。例えば、２０％未満であってもよいし、２０％超であってもよい。

（環境地図更新処理）
次に、演算装置１８が環境地図を更新する処理の一例について図９、１０を参照して説明する。まず、地図点群生成部６２が、移動体１０が記憶している環境地図（図１０中のＡ参照）をｘｙ座標値で表される地図点群（図１０中のＢ参照）に変換する（Ｓ１０）。次に、観測点群生成部６０が、ＬＲＦ１４による現在の観測結果をｘｙ座標値で表される観測点群（図１０中のＣ参照）に変換する（Ｓ１２）。観測点群生成部６０には、統合推定位置計算部５４から、現在のステップにおける統合推定位置が入力されている。観測点群生成部６０は、この統合推定位置に基づいてＬＲＦ１４による観測結果を環境地図座標系（ｘｙ座標系）へ変換することにより、観測点群を生成する。ステップＳ１２、Ｓ１４で変換された地図点群及び観測点群は、点群合わせ込み部６４にそれぞれ出力される。なお、ステップＳ１２とステップＳ１４の処理は、いずれを先に実行するように演算装置１８を構成してもよい。

次に、点群合わせ込み部６４は、地図点群と観測点群を合わせ込む（Ｓ１６）。点群の合わせ込みには、ＩＣＰ（ＩｔｅｒａｔｉｖｅＣｌｏｓｅｓｔＰｏｉｎｔ）アルゴリズムを用いることができる。ＩＣＰは、２つの異なる点群の距離の二乗和を最小化することによりマッチングを行う技術である。なお、ＩＣＰは公知の技術であるため、詳細な説明は省略する。ＩＣＰによりマッチングされた点群（図１０中のＤ参照）は、環境地図更新部６６に出力される。

観測点群と地図点群をマッチングさせると、環境の変化分を表す領域が現れる。実際には、環境地図には記録されていても観測されなかった障害物と、環境地図には記憶されていなくても観測された障害物があり得る。環境地図更新部６６は、それぞれの変化分を移動体１０が記憶している環境地図に反映させることによって、最新環境地図を生成する。具体的には、まず、環境地図更新部６６は、移動体１０が記憶する現在の環境地図から除去された物体をグリッドマップに反映させるために、ｆｒｅｅグリッド（図１０中のＥ参照）を生成する（Ｓ１６）。ｆｒｅｅグリッドとは、障害物が存在しないことを表したグリッドマップ上の格子のことであり、現在の位置（すなわち、現在のステップの統合推定位置）とＬＲＦ１４により観測された観測点群との間の空間がｆｒｅｅグリッド化される。すなわち、ＩＣＰにより、地図点群にマッチングした観測点群をグリッド化し、移動体１０の位置（位置）から観測点群までには障害物がないもの（空白領域）としてｆｒｅｅグリッドを生成する。

次に、環境地図更新部６６は、移動体１０が記憶する現在の環境地図に追加した障害体をグリッドマップに反映するために、ｏｃｃｕｐｉｅｄグリッド（図１０中のＦ参照）を生成する（Ｓ１８）。ｏｃｃｕｐｉｅｄグリッドとは、物体が存在することを表したグリッドマップ上の格子のことであり、地図点群に存在しないが観測点群に存在する変化分の点群がｏｃｃｕｐｉｅｄグリッド化される。すなわち、ＩＣＰによるマッチングで、地図点群に無くて観測点群に存在する点群（差分点群）を抽出し、抽出された点群をグリッド化することによってｏｃｃｕｐｉｅｄグリッドを生成する。

次に、環境地図更新部６６は、生成したｆｒｅｅグリッド及びｏｃｃｕｐｉｅｄグリッドを移動体１０が記憶する環境地図に重畳することにより、最新の環境地図（図１０中のＧ参照）を生成する（Ｓ２０）。以上の処理を実行することにより、最新環境地図が生成される。環境地図更新部６６は、移動体１０が現在記憶している環境地図を最新環境地図へ更新することにより（Ｓ２２）、処理を終了する。

更新された環境地図（最新環境地図）は、第１計算部２８にて地図に依存する推定位置を推定する際に再び用いられる。このとき、ＬＲＦ１４による観測結果と最新環境地図とのマッチングが正確に行われ、第１計算部２８の不信度は低下する（信憑度が向上する）。そのため、地図更新判断部５８による地図更新指令は出力されず、統合推定位置計算部５４からは地図に依存する推定位置を重視した（すなわち、本実施例では不信度ｐが２０％未満）となる統合推定位置が推定される。なお、環境地図の更新処理の態様は、上記のＩＣＰに限られない。信憑度が所定値より低下した場合に、環境地図を更新する処理を開始する形態であれば、様々な公知の手法を用いることができる。

実施例の移動体１０の作用効果を説明する。この移動体１０では、地図に依存する推定位置の信憑度が所定値より低下したとき（不信度ｐが所定値より上昇したとき）に、障害物までの距離と方位角の観測結果に基づいて最新の環境地図を生成する。すなわち、第１計算部２８の推定位置の信憑度が低下したときに、環境地図を更新する処理を行う。したがって、オペレータの操作を要さず、移動体１０自体によって環境地図の更新の可否を判断することができる。

また、観測結果と環境地図の合わせ込みの際に、グリッドマップではなく、座標系に変換した点群を用いるため、合わせ込みによる誤差を低減することができる。また、２種類の推定位置（地図に依存する推定位置と地図に依存しない推定位置）の混成結果を用いて統合推定位置を推定することにより、観測点群を精度良く生成することができる。このため、観測点群と地図点群との乖離が小さくなり、ＩＣＰによる点群マッチングの精度を向上させることができる。

また、統合推定位置計算部５４で移動体１０の統合推定位置を推定する際に用いられる重み係数αが、不信度ｐの関数となっている。不信度ｐを算出する信憑度計算式は、移動体１０が学習走行することで取得される学習データを、機械学習の一手法であるロジスティック回帰分析の手法を用いて解析することにより得られる式である。このため、重み係数αは、信憑度計算式以外の要因によって影響を受けることがない。従って、例えば移動領域が変更したり、移動体１０の管理者が変更したとしても、重み係数αの値を決定する基準が変動することがない。結果として、移動体１０の統合推定位置の推定精度を安定化することができる。

また、地図に依存しない推定位置は、１つ前のステップにおける統合推定位置に、エンコーダ１２から取得される左右の車輪の回転角度を元に算出された１ステップ分の移動ベクトルを加算することにより求められる。エンコーダ１２が検出する情報には、各車輪の滑り等により誤差が含まれる場合があるが、エンコーダ１２による検出距離が比較的に短い場合は、上記の誤差はほとんど無視できる。１ステップ分の移動ベクトルは極めて短いため、１ステップ分の移動ベクトルの精度は高い。また、地図に依らない推定位置は、環境地図を用いずに推定される。このため、移動体１０が、環境地図に記録されていない障害物の付近を自律走行する場合であっても、このことに起因して地図に依らない推定位置の推定精度が低下することはない。１ステップ前の統合推定位置の推定精度が高ければ、現在のステップにおける地図に依らない推定位置は高い推定精度を維持できる。

本実施例の移動体１０では、不信度ｐが増加するにつれて地図に依存する推定位置の重み係数αが減少するとともに、地図に依存しない推定位置の重み係数１−αが増加する。この構成によると、不信度ｐが高いとき（即ち、地図に依存する推定位置の推定精度が低いとき）は、統合推定位置に占める地図に依存する推定位置の割合を減らすとともに、地図に依存しない推定位置の割合を増やすことができる。上述したように、地図に依存しない推定位置の推定精度は高い。このため、この構成によると、地図に依存する推定位置の推定精度が低下しても、統合推定位置の推定精度が低下することを抑制できる。また、重み係数計算式は連続した滑らかな関数であるため、重み係数αは不信度ｐの推移に応じて滑らかに変動する。このため、統合推定位置は、地図に依存する推定位置と地図に依存しない推定位置の間を連続的に推移可能な値となる。従って、統合推定位置の推定精度がさらに向上する。

本実施例では、上述したように、地図更新判断部が環境地図の更新指令を出力する際の統合推定位置は、地図に依存しない推定位置を計算する第２計算部２９の推定結果が有利となっている。このため、精度良く最新環境地図を生成することができる。

また、本実施例では、学習データの元になる情報（即ち、位置推定結果や標準偏差σ_ｘ等の変数）は、移動体１０に実際に走行させることで取得する。このため、学習データには、移動体１０の地図に依存する推定位置の計算性能が直接的に反映される。このように、実際に走行させて得られる情報を元に学習データを生成することにより、より精度の高い信憑度計算式を生成できる。

以上、本明細書が開示する技術の実施例について詳細に説明したが、これらは例示にすぎず、本明細書が開示する移動体は、上記の実施例を様々に変形、変更したものが含まれる。

例えば、上述した実施例では、第１計算部２８と第２計算部２９により位置を推定したが、これら以外の推定方法を用いて位置を推定してもよい。例えば、カメラやＧＰＳを用いることによっても位置を推定することができる。

また上述した実施例では、地図に依存する推定位置と地図に依存しない推定位置の平均値を統合推定位置とし、統合推定位置に基づいて環境地図を更新する処理を実行したが、これに限られない。例えば、地図に依存する推定位置を計算する第１計算部２８の信憑度が所定値より低下した場合に、信憑度が所定値以上であった最後の時点における地図に依存する推定位置（すなわち、信憑度が所定値未満となる直前の地図に依存する推定位置）に、１ステップ分の移動ベクトルを加算した位置を現時点での推定位置とし、その現時点での推定位置と観測結果とに基づいて、最新の環境地図を生成する処理を行ってもよい。また、第１計算部２８による推定位置の信憑度が所定値より低下した場合に、信憑度が所定値以上であった時点における地図に依存する推定位置と、その時点で観測した観測結果とに基づいて、最新の環境地図を生成してもよい。このような構成であっても、実施例の移動体１０と同様の作用効果を奏することができる。後者の方式の場合、地図に依存しない推定位置を計算する必要がなく、地図に依存する推定位置のみを計算する実施例とすることができる。この場合、地図に依存する推定位置を統合推定位置とすればよい。

また、第１計算部２８では、ＭＣＬの代わりに拡張カルマンフィルタを用いて、地図に依存する推定位置を計算してもよい。この場合、推定誤差共分散行列から得られる値（例えば、分散値、誤差共分散行列の各要素の値、誤差楕円の長軸及び短軸等）を説明変数に用いて信憑度計算式を生成することができる。また、自律走行時に得られるこれらの変数を信憑度計算式に入力することで、地図に依存する推定位置推定結果を精度良く判定することができる。

また、上記の実施例では、モータに設けられたエンコーダ１２を用いて移動情報を取得したが、移動情報を取得する手法はこれに限られない。例えば、カメラを用いたビジュアルオドメトリ又はレーザセンサを用いたレーザオドメトリを用いて移動情報を取得してもよい。

また、移動体１０は学習データ生成部３６を有していなくてもよく、学習データは移動体１０の外部で生成されてもよい。また、移動体１０は、学習データ記憶部３８を有していなくてもよい。即ち、学習データを元に信憑度計算式が生成されたら、学習データを消去する構成であってもよい。また、移動体１０は、信憑度計算式生成部４６を有していなくてもよく、信憑度計算式は移動体１０の外部で生成されてもよい。

また、信憑度計算式生成部４６では、多変量解析の代わりにサポートベクターマシーンを用いてもよい。サポートベクターマシーンは、パターン認識のモデリングにおいて公知の手法であり、クラスの帰属が未知の特徴ベクトルを入力すると、当該特徴ベクトルが帰属するクラスを２値で出力する識別関数を構成する手法である。識別関数は、クラスの帰属が既知の訓練用のサンプル集合から構成される。このため、実施例の学習データを訓練用のサンプル集合に用いて学習データをサポートベクターマシーンで学習することで、識別関数（信憑度計算式）を構成することができる。そして、当該識別関数に第１計算部２８から取得される３つの変数σ_ｘ、σ_ｙ、ｗ_ｍａｘを入力することで、地図に依存する推定位置推定結果を判定することができる。

また、信憑度計算式生成部４６では、多変量解析の代わりにニューラルネットワークを用いてもよい。ニューラルネットワークは、パターン認識のモデリングにおいて公知の手法であり、クラスの帰属が未知の変数を入力すると、当該変数が帰属するクラスを２値で出力する識別関数を構成する手法である。この手法では、教師あり学習と教師なし学習の２通りで識別関数を構成できる。教師あり学習では、学習例と、当該学習例に対する目標出力が与えられ、初期状態の関数に学習例を入力して得られる出力が目標出力と一致するように初期状態の関数の重みを調整することで、識別関数が構成される。このため、実施例の学習データの３つの変数σ_ｘ、σ_ｙ、ｗ_ｍａｘを学習例とし、そのときの分類結果（ＯＫ／ＮＧ）を目標出力として、学習データをニューラルネットワークで学習することで、識別関数（信憑度計算式）を構成することができる。そして、当該識別関数に第１計算部２８から取得される３つの変数σ_ｘ、σ_ｙ、ｗ_ｍａｘを入力することで、地図に依存する推定位置推定結果を判定することができる。

また、学習データを解析する際は、ロジスティック回帰分析の代わりに判別分析を用いてもよい。

また、学習走行は、実際に移動体に特定の領域を走行させる代わりに、シミュレーションにより行ってもよい。

また、移動体１０は車輪駆動型でなくてもよく、例えば、低速で移動する小型モビリティや脚式型（二足歩行等）、飛行体であってもよい。また、移動体１０はＬＲＦ１４を複数個有していてもよい。

また、真値計算部３２では、三角測量センサ１６及びリフレクタ４４を用いて移動体１０の真値を計算したが、真値を計算する方法はこれに限られない。例えば、モーションキャプチャや磁気レールの手法を用いて真値を計算してもよい。

また、本実施例では位置推定等に２次元の観測結果を用いたが、３次元距離センサ等により得られる３次元の観測結果を用いてもよい。

また、移動体１０が記憶する環境地図は、グリッドマップではなく、座標値で表される点群として記憶してもよい。この構成によると、図２における地図点群生成部６２を備えなくとも、最新の環境地図を生成することができる。

本明細書で開示する技術では、地図依存式の推定位置計算手段が、仮定位置（パーティクル）毎に、自律移動体がその仮定位置に存在する確率を計算し、その確率分布から位置を推定する。信憑度計算手段が、前記確率分布から、信憑度を計算する。ここでいう信憑度は、推定位置が真値から許容範囲内に留まる確率である。

本明細書に開示する自律移動体は、移動距離と移動方向から自律移動体の位置を推定する地図不依存式の位置推定手段をさらに備えている。この場合、信憑度が低下して環境地図を更新するときには、信憑度が所定値以上であった期間のなかで最後に計算された推定位置（この推定位置は、地図に依る推定位置でもよいし、地図に依る推定位置と地図に依らない推定位置を統合した推定位置であってもよい。）に、その後の移動量と移動方向を加算した最新位置を求め、その最新位置と観測装置で最後に観測した障害物までの距離と方位の情報に基づいて環境地図を更新する。それに代えて、信憑度が所定値より低下したときには、信憑度が所定値以上であった期間内で最後に計算推定位置と、その時点で観測装置が観測した障害物までの距離と方位の情報から、環境地図を更新するようにしてもよい。

本明細書に開示する自律移動体では、更新が必要なときに環境地図を更新することから、地図に依存する推定位置の信憑度が高い。地図に依存する推定位置の計算部だけで足り、地図に依存しない推定位置の計算部は省略可能である。
本実施例の自律移動体は、地図依存式の推定位置計算部と、地図不依存式の推定位置計算部を合わせ持っており、両者を統合して統合推定位置を計算する。その際に、地図依存式の推定位置計算部の信憑度に基づいて、いずれの推定位置を重視するかを決定する。地図依存式の推定位置計算部の信憑度が高ければ地図に依存する推定位置を重視したものを統合推定位置とし、地図依存式の推定位置計算部の信憑度が低ければ、地図に依存しない推定位置を重視したものを統合推定位置とする。統合推定位置の信憑度が高く、環境地図の更新時に用いる自律移動体の位置の信憑度が高い。

環境地図更新装置を独立させることもでき、環境地図更新装置自体を製造・販売・使用することもできる。

実施例の構成要素と請求項の構成要素との関係について説明する。実施例の環境地図記憶部２６は、請求項の環境地図記憶手段の一例である。実施例のＬＲＦ１４は、請求項の観測装置の一例である。実施例の第１計算部２８は、請求項の地図依存式の推定位置計算手段の一例である。実施例の第２計算部２９は、請求項の地図不依存式の推定位置計算手段の一例である。実施例の信憑度計算部５０は、請求項の信憑度計算手段の一例である。実施例の観測点群生成部６０、地図点群生成部６２、点群合わせ込み部６４及び環境地図更新部６６は、請求項の環境地図更新手段の一例である。

以上、本発明の具体例を詳細に説明したが、これらは例示にすぎず、特許請求の範囲を限定するものではない。特許請求の範囲に記載の技術には、以上に例示した具体例を様々に変形、変更したものが含まれる。また、本明細書または図面に説明した技術要素は、単独であるいは各種の組合せによって技術的有用性を発揮するものであり、出願時請求項記載の組合せに限定されるものではない。また、本明細書または図面に例示した技術は複数目的を同時に達成するものであり、そのうちの一つの目的を達成すること自体で技術的有用性を持つものである。

１０：移動体
１２：エンコーダ
１４：レーザレンジファインダ（ＬＲＦ）
２６：環境地図記憶部
２８：第１計算部（地図依存式推定位置計算部）
２９：第２計算部（地図不依存式推定位置計算部）
４６：信憑度計算式生成部
４８：信憑度計算式記憶部
５０：信憑度計算部
５１：重み係数計算式記憶部
５２：重み係数計算部
５４：統合推定位置計算部
５６：統合推定位置記憶部
５８：地図更新判断部
６０：観測点群生成部
６２：地図点群生成部
６４：点群合わせ込み部
６６：環境地図更新部

Claims

自律移動体であり、
前記自律移動体が移動する領域内に存在する障害物の位置を示す環境地図を記憶している環境地図記憶手段と、
前記自律移動体から障害物までの距離と方位を観測する観測装置と、
前記自律移動体が前記環境地図上に位置すると仮定した仮定位置と前記環境地図から計算される障害物までの距離と方位の情報と、前記観測装置で観測した障害物までの距離と方位の情報から、前記自律移動体の位置を推定する地図依存式の推定位置計算手段と、
前記地図依存式の位置推定手段による推定位置の信憑度を計算する信憑度計算手段と、
前記信憑度が所定値より低下したときに、前記観測装置が観測した障害物までの距離と方位の情報に基づいて、前記環境地図記憶手段が記憶している前記環境地図を更新する環境地図更新手段、
を備えている自律移動体。
前記地図依存式の推定位置計算手段が、前記仮定位置毎に前記自律移動体がその仮定位置に存在する確率を計算し、前記確率の分布から位置を推定することを特徴とする請求項１に記載の自律移動体。
前記信憑度計算手段が、前記確率の分布から、前記信憑度を計算することを特徴とする請求項２に記載の自律移動体。
前記信憑度計算手段が、前記確率の分布から、前記推定位置が真値から許容範囲内に留まる信憑度を計算することを特徴とする請求項３に記載の自律移動体。
前記自律移動体が、移動距離と移動方向から前記自律移動体の位置を推定する地図不依存式の位置推定手段をさらに備えており、
前記信憑度が前記所定値より低下したときに、前記信憑度が前記所定値以上であった最後の推定位置にその後の移動量と移動方向を加算した最新位置を求め、前記最新位置と前記観測装置で観測した障害物までの距離と方位の情報から、前記環境地図を更新することを特徴とする請求項１〜４のいずれかの１項に記載の自律移動体。
前記信憑度が前記所定値より低下したときに、前記信憑度が前記所定値以上であった最後の推定位置と、前記最後の推定位置において前記観測装置で観測した障害物までの距離と方位の情報から、前記環境地図を更新することを特徴とする請求項１〜４のいずれかの１項に記載の自律移動体。
前記自律移動体が、
移動距離と移動方向から前記自律移動体の位置を推定する地図不依存式の推定位置計算手段と、
前記地図依存式の推定位置計算手段による推定位置と、前記地図不依存式の推定位置計算手段による推定位置を重み平均して統合推定位置を計算する装置を備えており、
前記重み平均に用いる重みを前記信憑度に基づいて決定することを特徴とする請求項１〜６のいずれかの１項に記載の自律移動体。
前記地図依存式の推定位置計算手段が、モンテカルロ位置同定法（ＭｏｎｔｅＣａｒｌｏＬｏｃａｌｉｚａｔｉｏｎ、ＭＣＬ）または拡張カルマンフィルタのいずれかの手法を用いて位置を推定する、請求項１〜７のいずれかの１項に記載の自律移動体。
障害物までの距離と方位を観測する観測装置を備え、移動領域内に存在する障害物の位置を示す環境地図を記憶している自律移動体のための環境地図更新装置であり、
前記自律移動体が前記環境地図上に位置すると仮定した仮定位置と前記環境地図から計算される障害物までの距離と方位の情報と、前記観測装置で観測した障害物までの距離と方位の情報から、前記自律移動体の位置を推定する地図依存式の推定位置計算手段と、
前記地図依存式の推定位置計算手段による推定位置の信憑度を計算する信憑度計算手段と、
前記信憑度が所定値より低下した場合に、前記観測装置が観測した前記障害物までの距離と方位の情報に基づいて、前記環境地図を更新する環境地図更新手段、
を備えている環境地図更新装置。