JP7039514B2 - 自己位置推定装置 - Google Patents

Description

本発明の実施形態は、地図中における移動体の自己位置を推定する自己位置推定装置に関する。

自動運転や運転者支援において、地図中における移動体の自己位置を推定することにより、地図に紐付いた情報を活用した高度な判断や、移動体の運動推定の高精度化が期待されている。移動体の自己位置の推定は、移動体に搭載されたセンサにより取得された観測データを地図データに対応付けることによりおこなわれる。

特開２０１８－１１６０１４号公報

従来、移動体の自己位置の推定において、対応付けの対象の物体を移動体に搭載されたセンサが、地図データと同じように観測できるかは考慮されていない。観測する際の移動体の自己位置の微小な変化によって観測箇所が大きく変化する場合には、推定精度が低下してしまう。

本発明が解決しようとする課題は、移動体の自己位置の微小な変化に対して観測箇所が大きく変化する物体が存在する場合でも、移動体の自己位置をロバストに推定することができる自己位置推定装置を提供することである。

実施形態に係る自己位置推定装置は、地図データを取得する取得部と、移動体の周辺の観測データを取得する観測部と、前記地図データに設定した仮想観測地点を始点とする観察ベクトルの走査方向に対する前記仮想観測地点から障害物までの距離である奥行の変動量に基づいて、前記観測データと前記地図データの位置合わせにおける前記地図データの有効度を示す指標である重みを算出する算出部と、前記重みに基づいて、前記地図データと前記観測データを位置合わせすることにより、前記地図中における前記移動体の位置を推定する推定部とを備えている。前記算出部は、前記変動量が大きい程、有効でない地図データとみなすような重みを算出する。

図１は、第１の実施形態に係る自己位置推定装置の構成を示す図である。 図２は、第１の実施形態に係る自己位置推定装置の動作を示すフローチャートである。 図３Ａは、植物を対象とする観測例を模式的に示した図であり、植物の手前側で反射された光に基づいて観測された様子を示した図である。 図３Ｂは、植物を対象とする観測例を模式的に示した図であり、植物の奥側で反射された光に基づいて観測された様子を示した図である。 図４Ａは、水平面に対して斜めになっている土手を対象とする観測例を模式的に示した図であり、移動体が水平な状態において観測された様子を示した図である。 図４Ｂは、水平面に対して斜めになっている土手を対象とする観測例を模式的に示した図であり、移動体が傾いた状態において観測された様子を示した図である。 図４Ｃは、水平面に対して斜めになっている土手を対象とする観測例を模式的に示した図であり、図４Ｂに示された状態とは反対側に移動体が傾いた状態において観測された様子を示した図である。 図５は、観測の対象とする領域の３次元地図を示した図である。 図６は、観測の対象とする領域の水平面における２次元地図を示した図である。 図７Ａは、図６に示された左領域における回転方向に対する奥行の変動量を示した図である。 図７Ｂは、図６に示された右領域における回転方向に対する奥行の変動量を示した図である。 図８Ａは、手前側のみの障害物が観測される仮想観測地点を示した図である。 図８Ｂは、奥側のみの障害物が観測される仮想観測地点を示した図である。 図９Ａは、図８Ａと図８Ｂに示された仮想観測地点に対する左領域における奥行の変動量を示した図である。 図９Ｂは、図８Ａと図８Ｂに示された仮想観測地点に対する右領域における奥行の変動量を示した図である。 図１０は、移動体の周辺の障害物を観測点の集合として示した、観測部によって取得された観測データの図である。 図１１は、地図データに基づく地図と、観測データに含まれる複数の観測点を示した図である。 図１２は、図１１に示された複数の観測点が地図中の障害物に位置合わせされる様子を示した図である。 図１３は、図１２に示された位置合わせ後における地図中における移動体の自己位置を示した図である。 図１４は、移動体の構成例を示した図である。 図１５は、図１４に示された移動体のブロック図である。 図１６は、第１の実施形態における第１の変形例による重みの算出を説明するための図である。 図１７は、ガウス分布で近似された物体を説明するための図である。 図１８Ａは、第１の実施形態における第２の変形例による重みの算出を説明するための図であり、想定形状を示す図である。 図１８Ｂは、第１の実施形態における第２の変形例による重みの算出を説明するための図であり、想定形状上のサンプリングした点を示す図である。 図１８Ｃは、第１の実施形態における第２の変形例による重みの算出を説明するための図であり、想定形状上のサンプリングした点と障害物の実際形状を示す図である。 図１９は、第２の実施形態に係る自己位置推定装置の構成を示す図である。

［第１の実施形態］
〔自己位置推定装置１００の構成〕
図１は、第１の実施形態に係る自己位置推定装置１００の構成を示す図である。自己位置推定装置１００は、移動体２００に搭載される。移動体２００は、たとえば、車両（自動二輪車、自動四輪車、自転車）、台車、ロボットである。車両は、たとえば、人による運転操作を介して走行する車両や、人による運転操作を介さずに自動的に走行可能な車両である。自動的に走行可能な車両は、たとえば、自動運転車両である。ここでは、移動体２００が自動運転車両である例が説明される。

自己位置推定装置１００は、取得部１１０と、算出部１２０と、観測部１３０と、推定部１４０と、記憶部１５０を備えている。取得部１１０と算出部１２０と推定部１４０と記憶部１５０は、たとえば、専用または汎用コンピュータで構成される。取得部１１０と算出部１２０と推定部１４０の機能は、たとえば記憶部１５０に格納されたプログラムをＣＰＵなどのハードウェアプロセッサに実行させることにより実現される。

取得部１１０は、地図データを取得するように構成されている。取得部１１０はまた、取得した地図データＤ１を算出部１２０と推定部１４０に出力するように構成されている。

観測部１３０は、移動体２００の周辺の環境を観測して観測データＤ３を取得するように構成されている。観測部１３０はまた、取得した観測データＤ３を推定部１４０に出力するように構成されている。

観測部１３０は、たとえば、距離センサを有している。距離センサは、たとえば、ＬＩＤＡＲセンサで構成される。ＬＩＤＡＲセンサは、レーザ光を照射しながら走査方向に走査するとともに、物体で反射されたレーザ光を受光し、受光したレーザ光の照射方向と、レーザ光の照射から受光までの経過時間とから、その照射方向における物体までの距離を算出するように構成されている。

算出部１２０は、取得部１１０から入力された地図データＤ１から、地図中における移動体２００の位置を推定するための地図データＤ１と観測データＤ３の対応付けの重みを算出するように構成されている。詳しくは、算出部１２０は、地図データＤ１において、所定の方向における奥行の変動量に基づいて、対応付けの重みを算出するように構成されている。ここでは、奥行は、地図データＤ１において、仮想観測地点から物体たとえば障害物までの距離である。障害物は、移動体２００が衝突を避けるべき物体である。算出部１２０はまた、算出した対応付けの重みの情報すなわち重みデータＤ２を推定部１４０に出力するように構成されている。

推定部１４０は、算出部１２０から入力された対応付けの重みデータＤ２に基づいて、取得部１１０から入力された地図データＤ１と観測部１３０から入力された観測データＤ３とを対応付けて、地図中における移動体２００の自己位置を推定するように構成されている。移動体２００の自己位置は、地図中における移動体２００の位置の座標と、移動体２００の移動の方向とを含んでいる。推定部１４０はまた、推定した移動体２００の自己位置の情報すなわち自己位置データＤ４を出力するように構成されている。自己位置データＤ４は、地図データＤ１と、地図データＤ１に関連付けられた移動体２００の自己位置のデータとを含んでいる。

記憶部１５０は、各種データを記憶するように構成されている。記憶部１５０は、たとえば、地図データＤ１と重みデータＤ２と観測データＤ３と自己位置データＤ４のいずれかまたはすべてを記憶するように構成されている。記憶部１５０は、たとえば、ＲＡＭ、フラッシュメモリ等の半導体メモリ素子、ハードディスク、光ディスク等で構成されてよい。

〔自己位置推定装置１００の動作の概略〕
図２は、第１の実施形態に係る自己位置推定装置１００の動作を示すフローチャートである。以下、自己位置推定装置１００の動作について簡単に説明する。

まず、ステップＳ１０において、取得部１１０が、移動体２００の周辺の地図データＤ１を取得する。

次に、ステップＳ２０において、算出部１２０が、入力された地図データＤ１に基づく地図中に仮想観測地点を設定する。

さらに、ステップＳ３０において、算出部１２０は、観測ベクトルを所定の方向に走査し、観測ベクトルの各方向における奥行を取得する。ここで、観測ベクトルとは、仮想観測地点を始点とするベクトルである。奥行の取得は、仮想観測地点から、観測ベクトル上に位置する物体たとえば障害物までの距離を算出することによりおこなわれる。

さらに、ステップＳ４０において、算出部１２０は、所定の方向における奥行の変動量に基づいて、地図中の各領域における地図データＤ１と観測データＤ３の対応付けの重みを算出する。

また、ステップＳ５０において、観測部１３０が、移動体２００の周辺の環境を観測して観測データＤ３を取得する。

次に、ステップＳ６０において、推定部１４０が、地図データＤ１と重みデータＤ２と観測データＤ３を用いて、地図中における移動体２００の自己位置を推定する。具体的には、地図中の各領域の対応付けの重みデータＤ２に基づいて、各領域の地図データＤ１と観測データＤ３の一致度に従って、地図データＤ１に基づく物体と観測データＤ３に基づく物体を位置合わせすることにより、地図中における移動体２００の自己位置を推定する。

最後に、ステップＳ７０において、推定部１４０は、地図中における移動体２００の自己位置の情報を出力する。

〔移動体２００の自己位置の推定における課題〕
ここで、自己位置推定装置１００の動作について詳しく説明する前に、移動体２００の自己位置の推定における課題について説明する。

移動体２００の自己位置の推定においては、移動体２００の周辺に存在する物体は、移動体の自己位置の微小な変化に対して、観測箇所（たとえば水平方向の位置）がほとんど変化しないことが望ましい。地面に実質垂直な平面を有する物体、たとえば塀やビルの壁などは、そのような好ましい物体である。一方、移動体２００の周辺には、移動体２００の自己位置の微小な変化に対して観測箇所が大きく変化する物体が存在することもある。そのような物体としては、植え込みや生垣のような植物や、地面に対して傾斜している土手などがあげられる。このような物体の観測データは、移動体２００の自己位置を推定する際に、地図データとの対応付けには不向きである。

図３Ａ～図３Ｂは、植え込みや生垣のような植物３１０を対象とする観測例を模式的に示している。対象が植物３１０である場合、枝葉に隙間があるため、観測方向に応じて、異なる奥行が観測される。図３Ａは、植物３１０の手前側で反射された光に基づいて観測された様子を示している。また、図３Ｂは、植物３１０の奥側で反射された光に基づいて観測された様子を示している。このように、植物３１０に対して観測される奥行は、観測方向に応じて、植物３１０の表から裏までの空間的な広がり分のばらつきを有する。このため、植物３１０の観測データを地図データと対応付けて得られる移動体２００の自己位置は、精度が低いものとなる。

図４Ａ～図４Ｃは、水平面に対して斜めになっている土手３２０を対象とする、水平面を観測する距離センサを用いた観測例を模式的に示している。同じ地点から観測しても、移動体２００の姿勢変化たとえばローリング角の変化により観測平面の角度が変わると、これに伴い土手３２０の観測位置の高さが変わり、その結果、観測される奥行が変動する。たとえば、図４Ａに示されるように移動体２００が水平な状態において観測される奥行に比べて、図４Ｂに示されるように移動体２００が傾いた状態では、観測される奥行は長くなり、反対に、図４Ｃに示されるように移動体２００が傾いた状態では、観測される奥行は短くなる。このように、土手３２０に対して観測される奥行は、移動体２００のたとえばローリング角の変化に応じて変動する。このため、土手３２０の観測データを地図データと対応付けて得られる移動体２００の自己位置は、精度が低いものとなる。

〔自己位置推定装置１００の動作の詳細〕
以下、自己位置推定装置１００による移動体２００の自己位置の推定の動作の詳細について、図５と図６に示される地図の例に沿って説明する。図５は、観測の対象とする領域の３次元地図を示しており、図６は、観測の対象とする領域の水平面における２次元地図を示している。この地図の例では、移動体２００の走行領域（たとえば道路）に対して、右側に障害物４１０が存在し、左側に障害物４２０と障害物４３０が存在している。障害物４１０と障害物４２０は、たとえば、塀のように、水平面に対して実質垂直な平面を有している物体である。障害物４３０は、柵のように、水平面に対して実質垂直に延びた複数の細長い物体たとえば支柱を有し、これら複数の細長い物体は、移動体２００の走行方向に沿って間隔を置いて並んでいる。このような柵状の障害物４３０が、移動体２００の走行領域に対して、塀状の障害物４２０の手前に位置している。

〈地図データＤ１の取得（Ｓ１０）〉
まず、取得部１１０は、地図データＤ１を取得する。地図データＤ１は、図５と図６に示された地図を表現するものである。地図データＤ１は、図５に示された３次元地図を表現することなく、図６に示された２次元地図だけを表現するものであってもよい。地図データＤ１は、任意の既知の取得方法で取得してよい。たとえば、記憶部１５０が地図データをあらかじめ記憶しており、観測の対象とする領域の地図データを取得部１１０が記憶部１５０から必要に応じて読み出してよい。あるいは、ＧＰＳ等のセンサや直前の位置を利用して大まかな位置を用いて、観測の対象とする範囲の地図データを外部の記憶装置等から通信によって取得してもよい。

また、地図データＤ１は、観測の対象となる物体を表現するものであれば、上に示したものに限らない。たとえば、地図データＤ１は、点の集合であってもよいし、局所領域の表面形状を関数で表現したものでもよいし、いくつかの典型的な建物のポリゴンモデルを用いたものでもよい。地図データＤ１は、障害物に限らず、特定のマークを示すテクスチャやベクトルで表現されていてもよい。さらに、地図データＤ１は、水平面における２次元地図を表現するものである必要もなく、水平ではない平面に投影したものでもよい。地図データＤ１は、地図座標系と対応が付いている各観測位置でのカメラ画像や奥行距離センサのデータを保持していてもよい。

〈対応付けの重みの算出の概要〉
次に、算出部１２０は、地図データＤ１から、地図中における移動体２００の位置を推定するための対応付けの重みを算出する。本実施形態では、図６に示されるように、地図データＤ１に基づく地図を等間隔のグリッドに分割し、その各領域４５０における対応付けの重みを算出する。ここで、対応付けの重みは、後述する対応付けにおいて、各領域４５０が対応付けに有効か否かを示す指標であり、移動体２００の自己位置の微小な変化に対して観測箇所が大きく変化する物体であるか否かを示す。

領域４５０は、右領域４５２と、中央領域４５４と、左領域４５６を有している。右領域４５２内に障害物４１０が存在し、左領域４５６内に障害物４２０と障害物４３０が存在している。また、中央領域４５４内に、移動体２００が走行する領域が存在している。

ここでは、重みを算出する単位を、等間隔のグリッドとしたが、この限りではない。重みを算出する単位は、後述する対応付けにおいて地図データＤ１と観測データＤ３の比較をおこなう際に参照できることが好ましい。たとえば、特定の位置から距離と方位で表現された極座標表現でもよいし、等間隔でなくてもよい。また、対応付けの重みは、障害物や点ごとに算出してもよいし、地図データを構成する要素ごとに算出してもよい。後述する算出部１２０の変形例を用いて、複数の重みを同じ領域に対して算出してもよい。

〈仮想観測地点の設定（Ｓ２０）〉
算出部１２０は、まず、取得部１１０から入力された地図データＤ１に対して、図５と図６に示されるように、仮想観測地点４６０を設定する。仮想観測地点４６０の高さは、移動体２００の高さの範囲内に、好ましくは比較的低い位置に設定される。仮想観測地点４６０の設定方法としては、たとえば、等間隔に配置した複数の地点から選択する方法や、地図データ中に目標経路がある場合、その経路上の１地点を選択する方法、周辺に障害物が少ない開けた領域に設定するといった方法、地図データを生成する際のセンサ位置を利用する方法などがあげられる。地図データＤ１において、移動体２００が存在できる地点に設定することが好ましいが、この限りではない。

〈観測ベクトルの走査と奥行の取得（Ｓ３０）〉
算出部１２０は、次に、仮想観測地点４６０を始点とする観測ベクトル４６２を所定の方向に走査して、地図データＤ１中の障害物４１０，４２０，４３０までの奥行を取得する。

観測ベクトル４６２の走査は、たとえば、仮想観測地点４６０を中心として、観測ベクトル４６２を水平面内で回転させることによりおこなう。

また、障害物４１０，４２０，４３０までの奥行の取得は、次のようにおこなう。まず、地図データを参照して、観測ベクトル４６２の各回転方向θにおいて、仮想観測地点４６０から最も近くに存在する障害物４１０，４２０，４３０を特定する。次に、特定した障害物４１０，４２０，４３０と、観測ベクトル４６２を包含する直線との交点４１２，４２２，４３２を求める。続いて、仮想観測地点４６０と交点４１２，４２２，４３２との間の距離を算出する。最後に、算出した距離を、その観測ベクトル４６２の方向における奥行として取得する。

ここでは、観測ベクトル４６２を水平面内で回転させることにより、観測ベクトル４６２を走査する例を示したが、観測ベクトル４６２を走査する方向は、この限りではなく、水平から角度のある面に沿っていてもよい。さらに、複数の回転方向を組み合わせて、仮想観測地点４６０から見て正対する平面の領域に対して観測ベクトル４６２を走査してもよい。特に、観測ベクトル４６２の走査方向は、観測部１３０内の距離センサによって想定される走査方向に設定することが好ましい。また、観測ベクトル４６２の走査方向は、重みを算出する領域を網羅できるような走査方向に設定してもよい。たとえば、観測ベクトル４６２は、あらかじめ定めた角度範囲において走査してもよいし、地図データの領域ごとに走査してもよい。また、観測ベクトル４６２は、地図データＤ１中の障害物や３次元点の周辺毎に走査してもよい。

〈重みの算出（Ｓ４０）〉
算出部１２０は、続いて、観測ベクトル４６２を所定の方向に走査して取得した奥行から、所定の方向すなわち回転方向θにおける奥行の変動量を算出する。図７Ａは、障害物４２０と障害物４３０が存在する左領域４５６における、回転方向θに対する奥行ＤＰの変動量を示している。また、図７Ｂは、障害物４１０が存在する右領域４５２における、回転方向θに対する奥行ＤＰの変動量を示している。左領域４５６では、図７Ａに示されるように、奥行ＤＰの変動量は激しく変化している。一方、右領域４５２では、図７Ｂに示されるように、奥行ＤＰの変動量は一定量で変化している。

算出部１２０は、さらに、算出した奥行ＤＰの変動量に基づいて、地図中の各領域４５０における地図データＤ１と観測データＤ３の対応付けの重みを算出する。具体的には、算出部１２０は、奥行ＤＰの変動量が大きい左領域４５６については、観測データＤ３との対応付けに有効でない領域でないとする重みを算出し、奥行ＤＰの変動量が小さい右領域４５２については、観測データＤ３との対応付けに有効な領域であるとする重みを算出する。言い換えれば、算出部１２０は、奥行ＤＰの変動量が大きい程、有効でない地図データとみなすような重みを算出し、奥行ＤＰの変動量が小さい程、有効な地図データとみなすような重みを算出する。また、算出部１２０は、奥行ＤＰの変動量が一定である場合、有効な地図データとみなすような重みを算出する。

左領域４５６のように複数の奥行ＤＰが存在する場合、極端な場合には、仮想観測地点４６０として、図８Ａに示されるように、手前側のみの障害物４３０が観測される仮想観測地点４６０ａや、図８Ｂに示されるように、奥側のみの障害物４２０が観測される仮想観測地点４６０ｂが存在することがある。これにより、左領域４５６では、図９Ａに示されるように、奥行ＤＰの変動が激しい。一方、右領域４５２では、一定量の変化のため、図９Ｂに示されるように、仮想観測地点４６０の多少の変化では、奥行ＤＰに大きな変化が現れない。このように、複数の奥行ＤＰが存在する場合は、地図データ中の障害物４２０，４３０のいずれを観測しているか定めることが難しくなる場合がある。

前述したように、奥行の変動量に基づいて、対応付けの重みを算出することにより、地図データＤ１に基づく地図中の各領域が、対応付けに有効であるか否かを判定することができる。対応付けの重みの算出は、具体的には、回転方向θに対する奥行ＤＰの微分値や変動回数、関数あてはめの誤差等を用いればよい。重みの高低については、後述する対応付けにおいて、地図データＤ１と観測データＤ３の一致度と誤差のいずれを評価するかに対応させて、大小関係を適切に反転させる。

ここまで、ただ１つの仮想観測地点４６０について、一度の走査により対応付けの重みを算出する形態を示したが、複数の仮想観測地点を設定し、各仮想観測地点について算出した重みを統合する形態であってもよい。さらに、複数の走査方向に対する重みを算出し、それを統合する形態でもよい。統合の方法は、観測地点ごとでもよいし、地図データの特定の位置に対する重みを統合してもよい。さらに、走査する際の奥行を取得する間隔や計測範囲を複数変更して重みを算出する形態でもよい。地図データから取得した奥行に、移動体に搭載されるセンサの誤差を模擬して付与する形態でもよい。

ここまで、ただ１つの仮想観測地点４６０を設定し、その仮想観測地点４６０について、ただ１つの走査方向に対する重みを算出する形態を示したが、複数の仮想観測地点を設定し、各仮想観測地点について算出した重みを統合する形態でもよい。さらに、複数の走査方向に対する重みを算出し、それを統合する形態でもよい。統合の方法は、仮想観測地点ごとでもよいし、地図データの特定の位置に対する重みを統合してもよい。さらに、走査する際の奥行を取得する間隔や計測範囲を複数変更して重みを算出する形態でもよい。地図データから取得した奥行に、観測部１３０内の距離センサの誤差を模擬して付与する形態でもよい。

〈重みの具体例〉
ここで、奥行の変動量を走査する所定の方向と、算出する重みの具体例を述べる。

ポールや柵のように、一定の間隔で同じ形状と隙間が繰り返される対象の場合、繰り返し方向に対する移動の推定誤差が大きくなる。そこで、繰り返し方向における奥行の変動を走査することで、対応付けの重みに反映する。こうした形状の場合でも、奥行方向等、繰り返し方向と異なる方向においては位置推定に有用な場合がある。そこで、複数の方向に走査し、繰り返し形状か否かの重みを算出する構成も可能である。

対象が、植え込みや生垣のような植物の場合、枝葉に隙間があるため、植物を観測した範囲の各観測ベクトルで異なる奥行が観測されることにより、奥行方向に植え込み分の幅を持つ。そこで、観測方向に対して奥行の分散を重みとして用いる。これにより、複数の奥行が観測できる物体か否かに対応した重みとする。

後述する対応付けを評価する領域の大きさや、観測ベクトルの解像度によっては、ゴミや廃材のように異なる形状の物体が雑多にある環境や、ねじ等の細かな部品が山積みされている工場、複数のフェンスが重なって観測される環境のように、解像度よりも細かい変動が生じうる場合は奥行方向に分散を考慮する。

倉庫内を移動する場合、荷物が移動する場合や、袋や布のように荷物の形状自体が変化することで、棚に収納されている荷物により位置精度が悪化する場合がある。そこで、配置図や別の既知の処理で検出した棚の配置に基づいて、棚の整列方向に対して走査することで荷物に対する重みを算出する。荷物を収納できる領域全体に一括して重みを算出するよりも、荷物の配置ごとに重みを算出できる。また、形状については袋や布のような奥行に細かなうねりがある対象の重みを算出してもよく、箱や棚のような平面の多い対象に重みを算出してもよい。

水平面を観測する距離センサを用いて、水平面に対して斜めになっている土手や建物の壁面を観測する場合、同じ地点から観測していても、移動体のローリング角等の姿勢変化により観測面の角度が変わり、対象における観測する高さが変化すると奥行が変動する。そこで鉛直方向に対する奥行の変動量を用いる。これにより、水平面から傾きに対応した重みとする。同様に、特定の平面内での距離を観測する場合、観測平面に対する法線方向について対象が傾いているか否かを奥行の変動量から算出し、重みとする。

〈周辺の観測（Ｓ５０）〉
次に、観測部１３０が、移動体２００の周辺の環境を観測し、観測データＤ３を取得する。観測部１３０は、地図データＤ１が表現する障害物４１０，４２０，４３０の形状を観測できることが好ましい。たとえば、本実施形態では、距離センサを用いて、移動体２００の自己位置からの各方向における障害物４１０，４２０，４３０までの距離を観測し、図１０に示されるように、移動体２００の周辺の障害物４１０，４２０，４３０の形状を観測点５３０の集合として観測データＤ３を取得する。移動体２００の自己位置５１０は、移動体２００の位置座標５１２と、移動体２００の移動方向５１４を含んでいる。観測データＤ３の取得方法は、カメラ画像を用いて観測できる物体の属性を推定し、その属性の典型的な形状モデルを用いて観測データを取得する方法や、幾何的な拘束を用いて移動体からの距離を推定する方法があり、この限りではない。

〈自己位置の推定（Ｓ６０）〉
次に、推定部１４０が、地図データＤ１と観測データＤ３を重みデータＤ２に基づいて対応付けることにより、観測データＤ３を取得した移動体２００の地図中における自己位置を推定する。対応付けに用いる手法は、対応付けの重みを算出した領域４５０ごとに、地図データＤ１と観測データＤ３の一致度または誤差を評価できるような手法であることが好ましい。

たとえば、重みを３次元点毎に算出した場合はＩｔｅｒａｔｉｖｅ Ｃｌｏｓｅｓｔ Ｐｏｉｎｔ （ＩＣＰ）、重みをグリッドのセルごとに算出した場合はＮｏｒｍａｌ Ｄｉｓｔｒｉｂｕｔｉｏｎｓ Ｔｒａｎｓｆｏｒｍ（ＮＤＴ）による対応付けを用いることができる。対応付けに用いる単位が、重みを算出した領域と異なる場合でも、対応する領域を参照すればよく、必ずしも領域４５０が対応付けの手法と一致する必要はない。対応付けの重みは、対応付けを評価する単位ごとの相対的な重みとして用いてもよいし、適当な閾値を用いて対応付けを評価する単位の選択に用いてもよい。地図データＤ１と観測データＤ３が対応付けられると、観測データＤ３を尤もらしく再現する地図データＤ１に基づく地図における移動体２００の自己位置が推定される。

重みデータＤ２に基づく地図データＤ１と観測データＤ３の対応付けは、地図データＤ１に基づく物体と観測データＤ３に基づく物体を位置合わせすることによりおこなわれる。具体的には、観測データＤ３に含まれる観測点５３０の位置を、地図データＤ１中の障害物４１０，４２０，４３０の位置に合わせることによりおこなわれる。その際、複数の観測点５３０の位置合わせがおこなわれるが、対応付けの重みの高い領域が優先される。

図１１は、地図データＤ１に基づく地図と、観測データＤ３に含まれる複数の観測点５３０を示している。また、図１２は、複数の観測点５３０が地図中の障害物４１０，４２０，４３０に位置合わせされる様子を示している。図１２において、対応付けの重みの高さが矢印の幅で表現されている。位置合わせの結果、図１３に示されるように、地図データＤ１に基づく地図中における移動体２００の自己位置５１０すなわち移動体２００の位置座標５１２と移動体２００の移動方向５１４が変更される。推定部１４０は、変更された移動体２００の位置座標５１２と移動体２００の移動方向５１４を、移動体２００の自己位置５１０と推定する。

〈自己位置の出力（Ｓ７０）〉
続いて、推定部１４０は、推定した移動体２００の自己位置５１０の情報を、自己位置データＤ４として出力する。つまり、自己位置データＤ４は、地図データＤ１と、地図データＤ１に基づく地図に対する相対的な移動体２００の位置座標５１２と移動方向５１４の情報とを含んでいる。

〔自動運転〕
自己位置推定装置１００から出力される自己位置データＤ４は、移動体２００の自動運転に利用される。図１４は、移動体２００の構成例を示した図である。図１５は、移動体２００のブロック図である。たとえば、移動体２００は、走行方向（矢印Ｘ方向）に走行する。

移動体２００は、自己位置推定装置１００と、出力装置２１０と、入力装置２３０と、動力制御装置２４０と、動力装置２５０とを備えている。自己位置推定装置１００と出力装置２１０と入力装置２３０と動力制御装置２４０は、バスを介して接続されている。

出力装置２１０は、各種の出力情報を出力する。出力装置２１０は、たとえば、出力情報を送信する通信機能、出力情報を表示する表示機能、出力情報を示す音を出力する音出力機能を有している。たとえば、出力装置２１０は、通信装置２１２と、ディスプレイ２１４と、スピーカ２１６を含んでいる。

入力装置２３０は、ユーザからの各種指示や情報入力を受け付ける。入力装置２３０は、たとえば、マウスやトラックボール等のポインティングデバイス、あるいはキーボード等の入力デバイスである。

動力装置２５０は、移動体２００を駆動する装置である。動力装置２５０は、たとえば、エンジンやモータ、車輪、アクセル装置、ブレーキ装置、操舵装置等を有している。

動力制御装置２４０は、動力装置２５０を制御する。動力装置２５０は、動力制御装置２４０の制御によって駆動する。たとえば、動力制御装置２４０は、自己位置推定装置１００で生成された出力情報に基づいて、周辺の状況を判断し、アクセル量、ブレーキ量、操舵角などの制御をおこなう。たとえば、動力制御装置２４０は、障害物を避けて現在走行中の車線を保つように移動体２００の制御をおこなう。

〔効果〕
以上、本実施形態によれば、所定の方向における奥行の変動量に基づいて移動体の周辺の観測データと地図データの対応付けの重みを算出することによって、移動体の自己位置の微小な変化に対して観測箇所が大きく変化する物体が存在する場合であっても、移動体の自己位置をロバストに推定することができる。

［第１の変形例］
算出部１２０の第１の変形例を示す。本変形例では、算出部１２０は、仮想観測地点は設定せず、地図データＤ１中の物体の表面の凹凸の形状を利用して重みを算出する。

算出部１２０は、地図データＤ１における障害物の表面形状を算出し、表面形状の凹凸，つまり法線方向から観測した際の奥行の変動量に基づいて、地図データＤ１と観測データＤ３の対応付けの重みを算出する。ここで、表面形状は法線方向を取得できれば任意である。たとえば、ＮＤＴの場合、図１６に示すように、地図データＤ１に基づく地図６１０をグリッドに区切り、各セル６２０に含まれる障害物の３次元点群の分布をガウス分布で近似する。ガウス分布で近似された障害物６３０，６４０は、ガウス分布の共分散の固有値とその固有ベクトルにより、点の広がりの方向とその分布が表現される。

図１７は、ガウス分布で近似された物体６５０を説明するための図である。ガウス分布で近似された物体６５０は、最大固有値に対する固有ベクトル６５２と、最小固有値に対する固有ベクトル６５４を有している。

最大固有値と最小固有値の比を用いて、ガウス分布で定義される点の広がりが得られる。ここでは、ガウス分布の中心からの距離を奥行とする。これにより、法線方向には広がりが少ない平らな障害物６３０か、各方向に広がりがあり等方に点が分布している形状の障害物６４０か判定できる。これにより、植物のような枝葉に隙間がある障害物が地図データに含まれる場合でも、点が等方に広がっている形状を判定することにより，自己位置が微小に変化することで奥行の変動量が大きくなる対象として重みを算出することができる。

ここでは、セル６２０ごとに、ガウス分布により点の広がりを算出する方法について述べたが、障害物の凹凸形状が算出できれば、この限りではない。たとえば、障害物毎に仮想観測地点をその障害物の周りを移動させる構成でもよいし、平面当てはめにより法線と平面までの距離を求めてもよい。凹凸形状の判定方向も，法線方向に限ったことではない。

本変形例では、地図データＤ１における障害物の表面の法線方向に奥行を設定する。仮想観測地点を設定する第１の実施形態とは異なり、重みを算出するセル６２０ごとに適切な方向に点の広がりを評価するため、移動体２００の自己位置をロバストに推定することができる。

［第２の変形例］
算出部１２０の第２の変形例を示す。本変形例では、算出部１２０は、地図データＤ１に仮想モデルをあてはめ、仮想モデルを基準とした奥行の変動量を用いて重みを算出する。以下、図１８Ａ～図１８Ｃを参照しながら、本変形例による重みの算出について説明する。

まず、算出部１２０は、図１８Ａに示されるように、仮想観測地点４６０において想定形状７１０を設定する。想定形状７１０は、仮想観測地点４６０の周辺において、想定される障害物の形状である。ここでは、想定形状７１０は、仮想観測地点４６０が水平面７１２上にあり、周辺の障害物が水平面に垂直な鉛直面７１４，７１６を有しているものとして想定されている。

人工物が多い環境や、室内の地図データを用いる場合には、想定される障害物の形状を利用する。地図データに存在すると想定される形状を設定できれば、この限りではなく、高度が利用できる場合には、傾いた路面の形状を用いる構成や、典型的な障害物のモデルを配置する構成でもよい。

次に、算出部１２０は、図１８Ｂに示されるように、想定形状７１０上の点７２０をサンプリングする。これらの点７２０は、後述する奥行を取得する点に対応する。サンプリングは、適当な等間隔でおこなってもよいし、想定形状７１０の特徴的な領域たとえばエッジ部分などに応じて密度を変えてもよい。

続いて、算出部１２０は、図１８Ｃに示されるように、サンプリングした各点７２０毎に、移動体２００の周辺の地図データＤ１を参照して奥行を取得する。ここでは、奥行は、サンプリングした点７２０から、地図データＤ１における障害物の実際形状７５０までの距離である。実際形状７５０は、水平面７５２と鉛直面７５４と傾斜面７５６を有している。奥行の取得は、サンプリングした点７２０から実際形状７５０までの距離を算出することによりおこなわれる。奥行の方向は、これに限らないが、たとえば、鉛直面７１４，７１６の法線方向である。さらに、算出部１２０は、想定形状７１０の表面に沿ってサンプリングした点７２０を走査することにより、奥行の変動量を取得する。

次に、算出部１２０は、サンプリングした各点７２０における奥行の変動量に基づいて、対応付けの重みを算出する。重みの算出は、サンプリングした点７２０毎におこなってもよいし、地図データにおける領域毎におこなってもよい。

これにより、想定形状から外れた地図データの対応付けの重みを変更できるため、あらかじめ有効な形状を指定して、対応付けの重みを算出することにより、移動体２００の自己位置をロバストに推定することができる。

［第２の実施形態］
図１９は、第２の実施形態に係る自己位置推定装置１００Ａの構成を示す図である。図１９において、図１に示された要素と同一の参照符号が付された要素は同様の部材であり、その詳しい要素は省略する。以下、相違部分に重点をおいて説明する。つまり、以下の説明で触れない部分は、第１の実施形態と同様である。

第２の実施形態に係る自己位置推定装置１００Ａでは、取得部１１０と算出部１２０は移動体２００の外部に設けられている。たとえば、取得部１１０と算出部１２０は、クラウド上で処理を実行するクラウドサーバに搭載される。

自己位置推定装置１００Ａは、第１の実施形態に係る自己位置推定装置１００の要素に加えて、更新部１６０を備えている。更新部１６０は、観測部１３０から観測データＤ３を取得し、取得した観測データＤ３に基づいて地図データＤ１を更新するように構成されている。更新部１６０は、更新された地図データを更新地図データＤ５として取得部１１０に出力ように構成されている。

取得部１１０は、更新部１６０から更新地図データＤ５を取得し、これを新たな地図データＤ１として算出部１２０に出力する。算出部１２０は、入力された新たな地図データＤ１から、新たな地図データＤ１と観測データＤ３の対応付けの重みを算出する。対応付けの重みの算出の手法は、第１の実施形態と同様である。

更新部１６０は、たとえば、観測部１３０によって新たに観測された地図データＤ１の範囲外の観測データＤ３を取得し、これを地図データＤ１に統合することにより、地図データＤ１の範囲を拡大するように構成される。更新部１６０はまた、たとえば、地図データＤ１の一部の物体が消失したことが観測された場合に、その物体のデータを地図データＤ１から削除するように構成される。更新部１６０の構成は、地図データＤ１を変更する処理をおこなえれば、この限りではない。

さらに、更新部１６０が取得する観測データは、複数のセンサによって取得された観測データであってもよく、これにより、より効率良く地図データＤ１を更新することが可能である。また、地図データＤ１に基づく地図中における移動体２００の位置は、自己位置推定装置１００Ａの対応付けの重みを用いて推定してもよいし、他のセンサを用いて推定した結果を用いて推定してもよい。

本実施形態に係る自己位置推定装置１００Ａでは、取得部１１０は、更新された更新地図データＤ５が更新部１６０から入力されるたびに、入力された更新地図データＤ５を新たな地図データＤ１として算出部１２０に出力し、算出部１２０は、新たな地図データＤ１が入力されるたびに、地図データＤ１と観測データＤ３の対応付けの重みを算出する。

第１の実施形態では、あらかじめ定めた重みの算出法を用いるが、本実施形態では、更新に用いた観測データを利用して算出法を変更することができる。たとえば、更新に用いた観測データＤ３の観測位置を仮想観測位置に設定したり、観測データＤ３の解像度を奥行の取得の解像度に設定したりすることができる。

これにより、与えられた地図データＤ１を用いて対応付けの重みを算出する第１の実施形態に係る自己位置推定装置１００とは異なり、本実施形態に係る自己位置推定装置１００Ａは、随時更新される地図データＤ１を用いて対応付けの重みを算出するため、現在の環境に即した対応付けの重みを算出することができる。更新後の地図データにおける対応付けの重みを用いて、地図データＤ１と観測データＤ３の対応付けをおこなうため、地図データＤ１における移動体２００の自己位置をよりロバストに推定することができる。

本発明のいくつかの実施形態を説明したが、これらの実施形態は、例として提示したものであり、発明の範囲を限定することは意図していない。これら新規な実施形態は、その他の様々な形態で実施されることが可能であり、発明の要旨を逸脱しない範囲で、種々の省略、置き換え、変更を行うことができる。これら実施形態やその変形は、発明の範囲や要旨に含まれるとともに、特許請求の範囲に記載された発明とその均等の範囲に含まれる。

１００，１００Ａ…自己位置推定装置、１１０…取得部、１２０…算出部、１３０…観測部、１４０…推定部、１５０…記憶部、１６０…更新部、２００…移動体、２１０…出力装置、２１２…通信装置、２１４…ディスプレイ、２１６…スピーカ、２３０…入力装置、２４０…動力制御装置、２５０…動力装置、３１０…植物、３２０…土手、４１０，４２０，４３０…障害物、４１２，４２２，４３２…交点、４５０…領域、４５２…右領域、４５４…中央領域、４５６…左領域、４６０，４６０ａ，４６０ｂ…仮想観測地点、４６２…観測ベクトル、５１０…自己位置、５１２…位置座標、５１４…移動方向、５３０…観測点、６２０…セル、６３０，６４０…障害物、６５０…物体、６５２，６５４…固有ベクトル、７１０…想定形状、７１２…水平面、７１４，７１６…鉛直面、７２０…点、７５０…実際形状、７５２…水平面、７５４…鉛直面、７５６…傾斜面、Ｄ１…地図データ、Ｄ２…重みデータ、Ｄ３…観測データ、Ｄ４…自己位置データ、Ｄ５…更新地図データ。

Claims (9)

1. 地図中における移動体の位置を推定する自己位置推定装置であって、
   地図データを取得する取得部と、
   前記移動体の周辺の観測データを取得する観測部と、
   前記地図データに設定した仮想観測地点を始点とする観察ベクトルの走査方向に対する前記仮想観測地点から障害物までの距離である奥行の変動量に基づいて、前記観測データと前記地図データの位置合わせにおける前記地図データの有効度を示す指標である重みを算出する算出部と、
   前記重みに基づいて、前記地図データと前記観測データを位置合わせすることにより、前記地図中における前記移動体の位置を推定する推定部とを備え、
   前記算出部は、前記変動量が大きい程、有効でない地図データとみなすような重みを算出する、自己位置推定装置。
2. 地図中における移動体の位置を推定する自己位置推定装置であって、
   地図データを取得する取得部と、
   前記移動体の周辺の観測データを取得する観測部と、
   前記地図データにおける障害物の表面の凹凸による奥行の変動量に基づいて、前記観測データと前記地図データの位置合わせにおける前記地図データの有効度を示す指標である重みを算出する算出部と、
   前記重みに基づいて、前記地図データと前記観測データを位置合わせすることにより、前記地図中における前記移動体の位置を推定する推定部とを備え、
   前記算出部は、前記変動量が大きい程、有効でない地図データとみなすような重みを算出する、自己位置推定装置。
3. 前記地図データを更新する更新部をさらに備え、
   前記取得部は、更新された地図データを取得し、
   前記算出部は、前記更新された地図データを前記取得部が取得すると、前記重みを算出する、請求項１または請求項２に記載の自己位置推定装置。
4. 前記算出部は、前記変動量が小さい程、有効な地図データとみなすような重みを算出する、請求項１から請求項３までのいずれかひとつに記載の自己位置推定装置。
5. 前記算出部は、前記変動量が一定である場合、有効な地図データとみなすような重みを算出する、請求項１から請求項４までのいずれかひとつに記載の自己位置推定装置。
6. 前記算出部は、前記奥行の変動量に加えて、前記観測データに基づいて、前記重みを算出する、請求項１から請求項５までのいずれかひとつに記載の自己位置推定装置。
7. 地図中における移動体の位置を推定する自己位置推定方法であって、
   地図データを取得するステップと、
   前記移動体の周辺の観測データを取得するステップと、
   前記地図データに設定した仮想観測地点を始点とする観察ベクトルの走査方向に対する前記仮想観測地点から障害物までの距離である奥行の変動量に基づいて、前記観測データと前記地図データの位置合わせにおける前記地図データの有効度を示す指標である重みを算出するステップと、
   前記重みに基づいて、前記地図データと前記観測データを位置合わせすることにより、前記地図中における前記移動体の位置を推定するステップとを有し、
   前記重みを算出するステップは、前記変動量が大きい程、有効でない地図データとみなすような重みを算出する、自己位置推定方法。
8. 地図中における移動体の位置を推定する自己位置推定方法であって、
   地図データを取得するステップと、
   前記移動体の周辺の観測データを取得するステップと、
   前記地図データにおける障害物の表面の凹凸である奥行の変動量に基づいて、前記観測データと前記地図データの位置合わせにおける前記地図データの有効度を示す指標である重みを算出するステップと、
   前記重みに基づいて、前記地図データと前記観測データを位置合わせすることにより、前記地図中における前記移動体の位置を推定するステップとを有し、
   前記重みを算出するステップは、前記変動量が大きい程、有効でない地図データとみなすような重みを算出する、自己位置推定方法。
9. 請求項１から請求項６までのいずれかひとつに記載の自己位置推定装置の各要素の機能または請求項７または請求項８に記載の自己位置推定方法の各ステップの処理をコンピュータに実行させるプログラム。

Images