JP6523196B2

JP6523196B2 - 推定装置、方法及びプログラム

Info

Publication number: JP6523196B2
Application number: JP2016054473A
Authority: JP
Inventors: 豪田崎; 雄土山地; 学西山
Original assignee: Toshiba Corp
Current assignee: Toshiba Corp
Priority date: 2016-03-17
Filing date: 2016-03-17
Publication date: 2019-05-29
Anticipated expiration: 2036-03-17
Also published as: US10275900B2; JP2017167974A; EP3223035B1; US20170270682A1; EP3223035A1

Description

本発明の実施形態は、推定装置、方法及びプログラムに関する。

従来から、第１移動体（例えば、自車両）の周囲に存在する第２移動体（例えば、他車両）の位置及び姿勢を推定する技術が知られている。例えば、自車両の周囲を３次元計測し、この３次元計測で得られる計測点の時系列の対応を取ることで求められる他車両の移動ベクトルと、車車間通信により他車両から取得した他車両の移動ベクトルと、の一致度を利用することで、他車両の位置及び姿勢を推定する技術が知られている。

特許第４９１８８０７号公報

しかしながら、上述したような従来技術では、第１移動体からの第２移動体の距離が遠くなるほど計測点の計測精度が悪くなる。このため、第１移動体からの第２移動体の距離が遠くなるほど、計測に基づいて求められる第２移動体の移動ベクトルの精度も悪くなり、第２移動体の位置及び姿勢の推定精度も悪くなる。

本発明が解決しようとする課題は、第１移動体の周囲に存在する第２移動体の位置及び姿勢の推定精度を向上させることができる推定装置、方法及びプログラムを提供することである。

実施形態の推定装置は、第１取得部と、第１推定部と、分類部と、第２取得部と、第２推定部と、を備える。第１取得部は、第１移動体の周囲を計測した第１計測点群を定期的に取得する。第１推定部は、前記第１移動体の位置及び姿勢を推定する。分類部は、前記位置及び姿勢と、最新の第１計測点群よりも過去に取得された過去の第１計測点群と、に基づいて、前記最新の第１計測点群のうち前記第１移動体以外の第２移動体上の計測点の候補となる複数の第１計測点を、複数の候補点に分類する。第２取得部は、前記第２移動体から、前記第２移動体の位置と、前記第２移動体の車幅及び車長の少なくともいずれかである辺長と、を含む第２移動体情報を取得する。第２推定部は、前記第２移動体情報から特定される領域の向きと前記複数の候補点との位置関係で定まる第１尤度を用いて評価値を算出し、当該評価値に基づいて、前記第２移動体の位置及び姿勢を推定する。

本実施形態の推定装置の構成例を示す図。本実施形態の距離センサのレーザ照射態様例を示す図。本実施形態の距離センサのレーザ照射態様例を示す図。本実施形態の第１計測点群例を示す図。本実施形態の分類手法例を示す説明図。本実施形態の第１尤度算出例を示す説明図。本実施形態の第１尤度算出例を示す説明図。本実施形態の第１尤度算出例を示す説明図。本実施形態の第１尤度算出例を示す説明図。本実施形態の処理例を示すフローチャート。変形例１の第１計測点群及び第２計測点群例を示す図。変形例２の推定装置の構成例を示す図。変形例２の第２尤度算出例を示す説明図。実施形態及び各変形例の推定装置のハードウェア構成例を示す図。

以下、添付図面を参照しながら、実施形態を詳細に説明する。

図１は、本実施形態の推定装置１０の構成の一例を示す図である。図１に示すように、推定装置１０は、距離センサ２を備える移動体１（第１移動体の一例）に搭載され、第１取得部１１と、第１推定部１３と、分類部１５と、第２取得部１７と、第２推定部１９と、出力部２１と、を備える。

本実施形態では、移動体が、自動車などの車両である場合を例に取り説明するが、これに限定されるものではない。移動体は、例えば、船舶や移動ロボットなど移動可能な物体であればどのようなものであってもよい。

距離センサ２としては、例えば、レーザセンサ及び画像センサ（カメラ）の少なくともいずれかなどが挙げられる。距離センサ２は、センシングした情報を推定装置１０に出力する。

推定装置１０は、移動体１の周囲に存在する当該移動体１以外の移動体（第２移動体の一例）の位置及び姿勢を推定するものであり、例えば、コンピュータなどが挙げられる。なお、移動体１としては、自移動体（自車両）が挙げられ、移動体１以外の移動体としては、他移動体（他車両）が挙げられる。

第１取得部１１、第１推定部１３、分類部１５、第２取得部１７、第２推定部１９、及び出力部２１は、例えば、ＣＰＵ（Central Processing Unit）などの処理装置にプログラムを実行させること、即ち、ソフトウェアにより実現してもよいし、ＩＣ（Integrated Circuit）などのハードウェアにより実現してもよいし、ソフトウェア及びハードウェアを併用して実現してもよい。

第１取得部１１は、移動体１の周囲を計測した第１計測点群を定期的に取得する。具体的には、第１取得部１１は、距離センサ２によりセンシングが行われる毎に、センシングされた情報に基づいて、移動体１の周囲を３次元計測し、３次元計測した計測点の集合である第１計測点群を得る。本実施形態では、第１計測点群は、移動体１の周囲に存在する障害物を表す。障害物としては、他移動体などの動的障害物や構造物などの静的障害物が挙げられる。

なお、第１取得部１１は、公知の３次元点計測手法を用いて、３次元計測を行えばよい。

例えば、距離センサ２が画像センサである場合、距離センサ２は、移動体１の周囲を時系列で順次撮像し、第１取得部１１は、距離センサ２により時系列で撮像された複数の画像それぞれから特徴点を検出し、検出した画像上の特徴点を時系列で追跡することで、画像上の特徴点に対応する実空間上の計測点を３次元計測し、第１計測点群を得る。

また例えば、距離センサ２がレーザセンサである場合、距離センサ２は、移動体１の周囲にレーザを照射し、当該レーザの照射光と反射光との位相差や反射までの反射時間をセンシングし、第１取得部１１は、レーザセンサによりセンシングされた位相差や反射時間を用いて、レーザが照射された計測点を３次元計測し、第１計測点群を得る。

なお、距離センサ２がレーザセンサである場合、距離センサ２は、図２に示すように、２次元でレーザを照射する（高さ固定でレーザを照射する）レーザセンサであってもよいし、図３に示すように、３次元でレーザを照射する（高さ非固定でレーザを照射する）レーザセンサであってもよい。

距離センサ２が２次元でレーザを照射する場合、第１計測点群は、２次元（レーザを照射した高さを基準とした平面上）の第１計測点の集合となり、距離センサ２が３次元でレーザを照射する場合、第１計測点群は、３次元の第１計測点の集合となる。

以下では、説明の便宜上、第１計測点群が、図４に示すような、２次元の平面上における第１計測点の集合である場合を例に取り説明する。なお、距離センサ２が画像センサである場合であっても、特定の高さの第１計測点の集合を第１計測点群とすれば、図４に示すような第１計測点群が得られる。

図４に示す例では、移動体１の周囲に、動的障害物である他移動体３１、３２、及び静的障害物である構造物４１、４２、４３が存在しており、第１計測点群として、計測点５１〜５６及び計測点６１〜６８が得られている。なお、第１取得部１１により第１計測点群が取得された段階では、推定装置１０において、取得された第１計測点群を構成する各計測点がいずれの障害物を表す計測点であるか未知である。

第１推定部１３は、移動体１の位置及び姿勢を推定する。なお、第１推定部１３は、公知の姿勢算出手法を用いて、移動体１の位置及び姿勢を推定すればよく、例えば、距離センサ２によりセンシングが行われる毎に、センシングされた情報に基づいて、移動体１の位置及び姿勢を推定する。

例えば、距離センサ２が画像センサである場合、第１推定部１３は、距離センサ２により時系列で撮像された複数の画像それぞれから第１取得部１１により検出された画像上の特徴点を時系列で追跡することで、過去の移動体１の位置及び姿勢から見た最新（現時点）の移動体１の位置及び姿勢を推定する。なお、第１推定部１３自身で、距離センサ２により時系列で撮像された複数の画像それぞれから特徴点を検出するようにしてもよい。

また例えば、距離センサ２がレーザセンサである場合、第１推定部１３は、第１取得部１１により得られた第１計測点を時系列で対応付けることで、過去の移動体１の位置及び姿勢から見た最新の移動体１の位置及び姿勢を推定する。

なお、第１推定部１３は、距離センサ２によりセンシングされた情報ではなく、ＧＰＳ（Global Positioning System）などの位置センサや、ＩＭＵ（Inertial Measurement Unit）などの姿勢センサによりセンシングされた情報を用いて、最新の移動体１の位置及び姿勢を直接推定するようにしてもよい。

分類部１５は、第１推定部１３により推定された移動体１の位置及び姿勢と、第１取得部１１により取得された最新の第１計測点群よりも過去に取得された過去の第１計測点群と、に基づいて、当該最新の第１計測点群のうち移動体１以外の他移動体上の計測点の候補となる複数の第１計測点を、複数の候補点に分類する。なお分類部１５は、最新の第１計測点群のうち他移動体上の計測点の候補となる複数の第１計測点以外の第１計測点については、静止点に分類する。

具体的には、分類部１５は、最新の第１計測点群を取得した際の移動体１の位置及び姿勢から見た座標系に、最新の第１計測点群及び過去の第１計測点群を投影する。そして分類部１５は、最新の第１計測点群と過去の第１計測点群とから、対応する最新の第１計測点と過去の第１計測点とのペアを抽出し、両第１計測点間の距離が分類用の閾値距離内であるか否かを判定し、分類用の閾値距離内であれば、当該最新の第１計測点を静止点に分類し、分類用の閾値距離内でなければ、当該最新の第１計測点を候補点に分類する。

図５は、本実施形態の分類手法の一例を示す説明図である。図５に示す例では、最新の第１計測点群を取得した際の移動体１の位置及び姿勢から見た座標系に、最新の第１計測点群を構成する最新の計測点５１〜５６及び最新の計測点６１〜６８、及び過去の第１計測点群を構成する過去の計測点７１〜７６及び最新の計測点８１〜８８が投影されている。なお、最新の計測点５１〜５６及び最新の計測点６１〜６８は、それぞれ、過去の計測点７１〜７６及び最新の計測点８１〜８８に対応するものとする。なお、計算上は、最新の計測点と最も近い位置にある投影された過去の計測点とを対応付けている。

この場合、最新の計測点５１については、対応する過去の計測点７１との距離が分類用の閾値距離内でないため、候補点に分類される。説明は省略するが、最新の計測点５２〜５６についても同様に候補点に分類される。なお、図４で図示した通り、計測点５１〜５６は、他移動体３１、３２を表す計測点であり、時系列の変化に伴い移動するため、対応する過去の計測点との距離が分類用の閾値距離内とならない。

一方、最新の計測点６１については、対応する過去の計測点８１との距離が分類用の閾値距離内であるため、静止点に分類される。説明は省略するが、最新の計測点６２〜６８についても静止点に分類される。なお、図４で図示した通り、計測点６１〜６８は、構造物４１、４２、４３を表す計測点であり、時系列の変化に伴い移動しないため、対応する過去の計測点との距離が分類用の閾値距離内となる。

第２取得部１７は、移動体１以外の他移動体から、当該他移動体の位置と、当該他移動体の車幅及び車長の少なくともいずれかである辺長と、を含む第２移動体情報を取得する。但し、第２移動体情報に含まれる情報は、これらに限定されず、他移動体の姿勢なども含まれていてもよい。なお本実施形態では、第２移動体情報に含まれる情報が、他移動体の位置と、当該他移動体の車幅である場合を例に取り説明する。

具体的には、第２取得部１７は、車車間通信などにより、移動体１から所定範囲内に位置する他移動体と無線通信を行い、当該他移動体から第２移動体情報を取得する。なお第１実施形態では、所定範囲が、移動体１の現在の制動距離を入力とした単調増加関数で求まる距離の範囲であるものとする。例えば、制動距離がｋ［ｍ］である場合、所定範囲は、定数ａ、ｂを用いて、ａｋ＋ｂ［ｍ］の範囲とすればよい。つまり第１実施形態では、所定範囲は、移動体１の制動距離が長くなるほど広くなる。なお、単調増加関数に上限下限を設定し、単調増加関数で求まる距離が上限を上回る場合は上限値に、下限を下回る場合には下限値に、単調増加関数で求まる距離を丸め込むようにしてもよい。但し、所定範囲はこれに限定されず、予め定めた範囲内としてもよい。

第２推定部１９は、第２取得部１７により取得された第２移動体情報から特定される領域の向きと、分類部１５により分類される複数の候補点と、の位置関係で定まる第１尤度を用いて評価値を算出し、当該評価値に基づいて、他移動体の位置及び姿勢を推定する。具体的には、第２推定部１９は、評価値が最も高くなる向きの領域を、他移動体の位置及び姿勢に推定する。

例えば、第２推定部１９は、第２取得部１７により取得された第２移動体情報に含まれる他移動体の位置から領域設定用の閾値範囲内において、車幅を第２移動体情報に含まれる他移動体の車幅に固定、車長及び向きを様々に変化させた複数の領域である他移動体の複数の位置姿勢候補を設定する。そして第２推定部１９は、設定した位置姿勢候補毎に、当該位置姿勢候補と分類部１５により分類される複数の候補点との位置関係で定まる第１尤度を算出する。

第１尤度としては、領域（位置姿勢候補）の外形から第１閾値距離内に含まれる候補点の数が多くなるほど値が高くなる尤度（外形尤度）が挙げられる。例えば、他移動体の領域設定用の閾値範囲に候補点となる計測点５１〜５４が含まれるとする。この場合、図６に示す位置姿勢候補９１の外形上には、計測点５１〜５４が存在し、外形から第１閾値距離内に含まれる候補点の数が４となる。一方、図７に示す位置姿勢候補９２の外形上には、計測点５２、５４が存在し、外形から第１閾値距離内に含まれる候補点の数が２となる。このため、位置姿勢候補９１と位置姿勢候補９２を比較した場合、位置姿勢候補９１の方が位置姿勢候補９２よりも第１尤度が高くなり、第１尤度をそのまま評価値とした場合、位置姿勢候補９１が、他車両の位置及び姿勢に推定される。

また、第１尤度としては、移動体１（詳細には、距離センサ２）と領域（位置姿勢候補）との間に位置する静止点の数が少なくなるほど値が高くなる尤度（遮蔽尤度）が挙げられる。例えば、他移動体の領域設定用の閾値範囲に候補点となる計測点５１〜５４及び静止点となる計測点９５が含まれるとする。この場合、図８に示すように、移動体１と位置姿勢候補９１（詳細には、位置姿勢候補９１の外形）との間には、静止点は含まれておらず、移動体１と位置姿勢候補９１との間に位置する静止点の数が０となる。一方、図９に示すように、移動体１と位置姿勢候補９６（詳細には、位置姿勢候補９６の外形）との間には、静止点９５が含まれており、移動体１と位置姿勢候補９６との間に位置する静止点の数が１となる。このため、位置姿勢候補９１と位置姿勢候補９６を比較した場合、位置姿勢候補９１の方が位置姿勢候補９６よりも第１尤度が高くなり、第１尤度をそのまま評価値とした場合、位置姿勢候補９１が、他車両の位置及び姿勢に推定される。なお、遮蔽尤度の具体的な値としては、候補点数−移動体１と位置姿勢候補との間に位置する静止点数などとすることもできる。

なお上述のように、第１尤度をそのまま評価値としてもよいし、複数の第１尤度（例えば、外形尤度と遮蔽尤度）を統合した値を評価値としてもよい。この場合、第２推定部１９は、各第１尤度を正規化し、和積演算を行うことで評価値とすればよい。和積演算は、正規化した各尤度を単純に足し合わせるだけであっても掛け合わせるだけであってもよい。

また、第２移動体情報に他移動体の車幅ではなく車長が含まれる場合には、第２推定部１９は、車長を第２移動体情報に含まれる他移動体の車長に固定、車幅及び向きを様々に変化させた複数の領域である他移動体の複数の位置姿勢候補を設定すればよい。

また、第２移動体情報に他移動体の車幅だけでなく車長も含まれる場合には、第２推定部１９は、車幅を第２移動体情報に含まれる他移動体の車幅に固定、車長を第２移動体情報に含まれる他移動体の車長に固定、向きを様々に変化させた複数の領域である他移動体の複数の位置姿勢候補を設定すればよい。

また、第２移動体情報に他移動体の姿勢も含まれる場合には、第２推定部１９は、向きについては、第２移動体情報に含まれる姿勢を向き用の閾値範囲で変化させて、複数の領域である他移動体の複数の位置姿勢候補を設定すればよい。

出力部２１は、第２推定部１９により推定された他移動体の位置及び姿勢を出力する。例えば、出力部２１は、第２推定部１９により推定された他移動体の位置及び姿勢を、移動体１の制御系に出力し、移動体１の各種制御に用いられるようにしてもよい。

図１０は、本実施形態の処理の手順の流れの一例を示すフローチャートである。なお、図１０に示す処理は、定期的に行われる。

まず、第１取得部１１は、移動体１（第１移動体）の周囲を計測した第１計測点群を取得する（ステップＳ１０１）。

続いて、第１推定部１３は、移動体１（第１移動体）の位置及び姿勢を推定する（ステップＳ１０３）。

続いて、分類部１５は、第１推定部１３により推定された移動体１（第１移動体）の位置及び姿勢と、第１取得部１１により取得された最新の第１計測点群よりも過去に取得された過去の第１計測点群と、に基づいて、当該最新の第１計測点群のうち他移動体（第２移動体）上の計測点の候補となる複数の第１計測点を候補点に分類し、当該候補となる複数の第１計測点以外の第１計測点を静止点に分類する（ステップＳ１０５）。

続いて、第２取得部１７は、他移動体（第２移動体）から、当該他移動体の位置と、当該他移動体の車幅及び車長の少なくともいずれかである辺長と、を含む第２移動体情報を取得する（ステップＳ１０７）。

続いて、第２推定部１９は、第２取得部１７により取得された第２移動体情報に含まれる他移動体（第２移動体）の位置から領域設定用の閾値範囲内において、車幅を第２移動体情報に含まれる他移動体の車幅に固定、車長及び向きを様々に変化させた複数の領域である他移動体の複数の位置姿勢候補を設定する（ステップＳ１０９）。

続いて、第２推定部１９は、分類部１５により分類された複数の候補点に基づいて、他移動体（第２移動体）の各位置姿勢候補の第１尤度を算出することで、各位置姿勢候補の評価値を算出する（ステップＳ１１１）。

続いて、第２推定部１９は、評価値が最も高くなる位置姿勢候補を、他移動体の位置及び姿勢に推定する（ステップＳ１１３）。

続いて、出力部２１は、第２推定部１９により推定された他移動体の位置及び姿勢を出力する（ステップＳ１１５）。

以上のように本実施形態によれば、移動体１の周囲に存在する他移動体の距離が遠くなり（例えば、５０ｍ以上離れていても）、他移動体を表す計測点の密度が疎である場合であっても、他移動体の位置及び姿勢の推定精度を向上させることができる。

（変形例１）
上記実施形態において、他移動体から取得した計測点を更に用いるようにしてもよい。変形例１については、図１１を参照しながら説明する。

この場合、第２取得部１７は、他移動体３１（第３移動体の一例）から、当該第３移動体の周囲を計測した第２計測点群を更に取得する。

また第１推定部１３は、第２取得部１７により取得された第２計測点群を構成する第２計測点のうち最新の第１計測点群を構成する第１計測点と第２閾値距離内に位置する第２計測点に更に基づいて、移動体１の位置及び姿勢を推定する。

図１１に示す例では、移動体１の周囲に、動的障害物である他移動体３１、３２、及び静的障害物である構造物４１、４２、４３、１４４が存在しており、第１計測点群として、計測点５１〜５６及び計測点６１〜６８が得られ、他移動体３１から得られた第２計測点群として、計測点１５５、１５６、１６４、１６５、１６７、及び１８１〜１８３が得られている。

なお、第１推定部１３は、第２取得部１７により取得された第２計測点が他移動体上で定義される他移動体座標系である場合は、他移動体位置及び姿勢と、自移動体の位置及び姿勢から、自移動体座標系に投影すればよい。また、第１推定部１３は、第２取得部１７により取得された第２計測点が世界座標系である場合は、自移動体の位置及び姿勢から、自移動体座標系に投影すればよい。

ここで、計測点１５５、１５６、１６４、１６５、１６７は、それぞれ、計測点５５、５６、６４、６５、６７に対応するものとする。また、計測点１５５、１５６、１６４、１６５、１６７については、それぞれ、対応する計測点５５、５６、６４、６５、６７との距離が第２閾値距離内であるものとする。また、計測点１８１〜１８３については、対応する計測点が存在せず、対応する計測点との距離が第２閾値距離内でないものとする。

この場合、第１推定部１３は、第１計測点群を構成する計測点５１〜５６及び計測点６１〜６８だけでなく、計測点１５５、１５６、１６４、１６５、１６７も用いて、移動体１の位置及び姿勢を推定する。

また分類部１５は、前記複数の第１計測点、及び第１計測点と第２閾値距離内に位置する複数の第２計測点を、複数の候補点に分類する。つまり分類部１５は、複数の第１計測点だけでなく、自移動体の位置及び姿勢に用いられた第２計測点についても、上記実施形態で説明した手法で候補点又は静止点に分類する。

（変形例２）
上記実施形態において、第１尤度に加え第２尤度を用いて評価値を求めてもよい。

図１２は、変形例２の推定装置１１０の構成の一例を示す図である。図１２に示すように、推定装置１１０は、第２推定部１１９及び第３取得部１２３が上記実施形態と相違する。

第３取得部１２３は、移動体１０１（第１移動体）の周囲を撮像した画像を更に取得する。

第２推定部１１９は、第３取得部１２３により取得された画像から他移動体が位置する画像領域を抽出するとともに、複数の候補点を当該画像上に投影し、当該画像領域に含まれる候補点の数で定まる第２尤度を更に算出し、第１尤度及び第２尤度を用いて評価値を算出する。なお、第２尤度は、画像領域に含まれる候補点の数が多くなるほど値が大きくなる。

例えば、第２推定部１１９は、図１３に示すように、第３取得部１２３により取得された画像２２１から他移動体が位置する画像領域２３０を抽出するとともに、分類部１５により候補点に分類された計測点２０１、２０２を、それぞれ、投影点２４１、２４２として画像２２１上に投影する。そして第２推定部１１９は、画像領域２３０に含まれる候補点（詳細には、投影点）の数から、第２尤度を更に算出する。なお、第１尤度の算出手法は、第１実施形態と同様であり、第１尤度及び第２尤度を統合した評価値の求め方については、第１実施形態の手法を用いればよい。

（ハードウェア構成）
図１４は、実施形態及び変形例の推定装置のハードウェア構成の一例を示す図である。図１４に示すように、実施形態及び変形例の推定装置は、ＣＰＵなどの制御装置９０１と、ＲＯＭやＲＡＭなどの主記憶装置９０２と、ＨＤＤやＳＳＤなどの補助記憶装置９０３と、ディスプレイなどの表示装置９０４と、キーボードやマウスなどの入力装置９０５と、通信インタフェースなどの通信装置９０６と、を備えており、通常のコンピュータを利用したハードウェア構成となっている。

実施形態及び変形例の推定装置で実行されるプログラムは、インストール可能な形式又は実行可能な形式のファイルでＣＤ−ＲＯＭ、ＣＤ−Ｒ、メモリカード、ＤＶＤ（Digital Versatile Disk）、フレキシブルディスク（ＦＤ）等のコンピュータで読み取り可能な記憶媒体に記憶されて提供される。

また、実施形態及び変形例の推定装置で実行されるプログラムを、インターネット等のネットワークに接続されたコンピュータ上に格納し、ネットワーク経由でダウンロードさせることにより提供するようにしてもよい。また、実施形態及び変形例の推定装置で実行されるプログラムを、インターネット等のネットワーク経由で提供または配布するようにしてもよい。また、実施形態及び変形例の推定装置で実行されるプログラムを、ＲＯＭ等に予め組み込んで提供するようにしてもよい。

実施形態及び変形例の推定装置で実行されるプログラムは、上述した各部をコンピュータ上で実現させるためのモジュール構成となっている。実際のハードウェアとしては、ＣＰＵがＲＯＭやＨＤＤなどからプログラムをＲＡＭ上に読み出して実行することにより、上記各部がコンピュータ上で実現されるようになっている。

本発明は、上記実施形態及び変形例そのままに限定されるものではなく、実施段階ではその要旨を逸脱しない範囲で構成要素を変形して具体化することができる。また、上記実施形態及び変形例に開示されている複数の構成要素の適宜な組み合わせにより、種々の発明を形成することができる。例えば、上記実施形態及び変形例に示される全構成要素からいくつかの構成要素を削除してもよい。さらに、異なる実施形態及び変形例にわたる構成要素を適宜組み合わせても良い。

例えば、実施形態のフローチャートにおける各ステップを、その性質に反しない限り、実行順序を変更し、複数同時に実施し、あるいは実施毎に異なった順序で実施してもよい。

以上のように、上記実施形態及び変形例によれば、第１移動体の周囲に存在する第２移動体の位置及び姿勢の推定精度を向上させることができる。

１、１０１移動体
２距離センサ
１０、１１０推定装置
１１第１取得部
１３第１推定部
１５分類部
１７第２取得部
１９、１１９第２推定部
２１出力部
１２３第３取得部
９０１制御装置
９０２主記憶装置
９０３補助記憶装置
９０４表示装置
９０５入力装置
９０６通信装置

Claims

第１移動体の周囲を計測した第１計測点群を定期的に取得する第１取得部と、
前記第１移動体の位置及び姿勢を推定する第１推定部と、
前記位置及び姿勢と、最新の第１計測点群よりも過去に取得された過去の第１計測点群と、に基づいて、前記最新の第１計測点群のうち前記第１移動体以外の第２移動体上の計測点の候補となる複数の第１計測点を、複数の候補点に分類する分類部と、
前記第２移動体から、前記第２移動体の位置と、前記第２移動体の車幅及び車長の少なくともいずれかである辺長と、を含む第２移動体情報を取得する第２取得部と、
前記第２移動体情報に基づいて設定される、前記第２移動体の位置及び姿勢の候補を示す複数の位置姿勢候補それぞれと、前記複数の候補点と、の位置関係で定まる第１尤度を用いて、複数の前記位置姿勢候補それぞれの評価値を算出し、当該評価値に基づいて、前記第２移動体の位置及び姿勢を推定する第２推定部と、
を備える推定装置。
前記第２推定部は、前記評価値が最も高くなる前記位置姿勢候補が示す位置及び姿勢を、前記第２移動体の位置及び姿勢に推定する請求項１に記載の推定装置。
前記第１尤度は、前記位置姿勢候補の外形から第１閾値距離内に含まれる候補点の数が多くなるほど値が高くなる尤度である請求項１に記載の推定装置。
前記分類部は、前記最新の第１計測点群のうち前記複数の第１計測点以外の第１計測点を静止点に分類し、
前記第１尤度は、前記第１移動体と前記位置姿勢候補との間に位置する前記静止点の数が少なくなるほど値が高くなる尤度である請求項１に記載の推定装置。
前記第２移動体は、前記第１移動体から所定範囲内に位置し、
前記所定範囲は、前記第１移動体の制動距離が長くなるほど広くなる請求項１に記載の推定装置。
前記第２取得部は、第３移動体から、当該第３移動体の周囲を計測した第２計測点群を更に取得し、
前記第１推定部は、前記第２計測点群を構成する第２計測点のうち前記最新の第１計測点群を構成する第１計測点と第２閾値距離内に位置する第２計測点に更に基づいて、前記第１移動体の位置及び姿勢を推定し、
前記分類部は、前記複数の第１計測点、及び前記第１計測点と第２閾値距離内に位置する複数の第２計測点を、前記複数の候補点に分類する請求項１に記載の推定装置。
前記第１移動体の周囲を撮像した画像を更に取得する第３取得部を更に備え、
前記第２推定部は、前記画像から前記第２移動体が位置する画像領域を抽出するとともに、前記複数の候補点を前記画像上に投影し、前記画像領域に含まれる前記候補点の数で定まる第２尤度を更に算出し、前記第１尤度及び前記第２尤度を用いて前記評価値を算出する請求項１に記載の推定装置。
コンピュータが、第１移動体の周囲を計測した第１計測点群を定期的に取得する第１取得ステップと、
前記コンピュータが、前記第１移動体の位置及び姿勢を推定する第１推定ステップと、
前記コンピュータが、前記位置及び姿勢と、最新の第１計測点群よりも過去に取得された過去の第１計測点群と、に基づいて、前記最新の第１計測点群のうち前記第１移動体以外の第２移動体上の計測点の候補となる複数の第１計測点を、複数の候補点に分類する分類ステップと、
前記コンピュータが、前記第２移動体から、前記第２移動体の位置と、前記第２移動体の車幅及び車長の少なくともいずれかである辺長と、を含む第２移動体情報を取得する第２取得ステップと、
前記コンピュータが、前記第２移動体情報に基づいて設定される、前記第２移動体の位置及び姿勢の候補を示す複数の位置姿勢候補それぞれと、前記複数の候補点と、の位置関係で定まる第１尤度を用いて、複数の前記位置姿勢候補それぞれの評価値を算出し、当該評価値に基づいて、前記第２移動体の位置及び姿勢を推定する第２推定ステップと、
を含む推定方法。
第１移動体の周囲を計測した第１計測点群を定期的に取得する第１取得ステップと、
前記第１移動体の位置及び姿勢を推定する第１推定ステップと、
前記位置及び姿勢と、最新の第１計測点群よりも過去に取得された過去の第１計測点群と、に基づいて、前記最新の第１計測点群のうち前記第１移動体以外の第２移動体上の計測点の候補となる複数の第１計測点を、複数の候補点に分類する分類ステップと、
前記第２移動体から、前記第２移動体の位置と、前記第２移動体の車幅及び車長の少なくともいずれかである辺長と、を含む第２移動体情報を取得する第２取得ステップと、
前記第２移動体情報に基づいて設定される、前記第２移動体の位置及び姿勢の候補を示す複数の位置姿勢候補それぞれと、前記複数の候補点と、の位置関係で定まる第１尤度を用いて、複数の前記位置姿勢候補それぞれの評価値を算出し、当該評価値に基づいて、前記第２移動体の位置及び姿勢を推定する第２推定ステップと、
をコンピュータに実行させるためのプログラム。