JP2024161105A - 自己位置推定装置、制御方法、プログラム及び記憶媒体 - Google Patents
自己位置推定装置、制御方法、プログラム及び記憶媒体 Download PDFInfo
- Publication number
- JP2024161105A JP2024161105A JP2024144149A JP2024144149A JP2024161105A JP 2024161105 A JP2024161105 A JP 2024161105A JP 2024144149 A JP2024144149 A JP 2024144149A JP 2024144149 A JP2024144149 A JP 2024144149A JP 2024161105 A JP2024161105 A JP 2024161105A
- Authority
- JP
- Japan
- Prior art keywords
- feature
- information
- vehicle position
- vehicle
- self
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Pending
Links
- 238000000034 method Methods 0.000 title description 17
- 238000005259 measurement Methods 0.000 claims abstract description 84
- 238000012937 correction Methods 0.000 claims description 34
- 230000006866 deterioration Effects 0.000 abstract description 2
- 239000013598 vector Substances 0.000 description 13
- 238000010586 diagram Methods 0.000 description 10
- 230000007423 decrease Effects 0.000 description 8
- 239000011159 matrix material Substances 0.000 description 7
- 230000008569 process Effects 0.000 description 6
- 230000008859 change Effects 0.000 description 4
- 238000001514 detection method Methods 0.000 description 3
- 238000000605 extraction Methods 0.000 description 3
- 230000005484 gravity Effects 0.000 description 3
- 230000006870 function Effects 0.000 description 2
- 238000012545 processing Methods 0.000 description 2
- NAWXUBYGYWOOIX-SFHVURJKSA-N (2s)-2-[[4-[2-(2,4-diaminoquinazolin-6-yl)ethyl]benzoyl]amino]-4-methylidenepentanedioic acid Chemical compound C1=CC2=NC(N)=NC(N)=C2C=C1CCC1=CC=C(C(=O)N[C@@H](CC(=C)C(O)=O)C(O)=O)C=C1 NAWXUBYGYWOOIX-SFHVURJKSA-N 0.000 description 1
- 238000004891 communication Methods 0.000 description 1
- 238000005516 engineering process Methods 0.000 description 1
- 238000009434 installation Methods 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 239000002245 particle Substances 0.000 description 1
- 230000004044 response Effects 0.000 description 1
Abstract
【課題】地物の計測誤差の可能性があった場合でも,自己位置推定精度の劣化を好適に抑制することが可能な自己位置推定装置を提供する
【解決手段】車載機1の自車位置推定部17は、予測自車位置X-(t)を示す情報を生成すると共に、ライダ2により計測対象となる地物の法線情報とサイズ情報とを地図DB10から取得する。そして、自車位置推定部17は、車両の進行方向の情報と地物の法線情報から算出される角度差Δθと、地物のサイズ情報が示すサイズSとに基づき、予測自車位置X-(t)を補正する。
【選択図】図7
【解決手段】車載機1の自車位置推定部17は、予測自車位置X-(t)を示す情報を生成すると共に、ライダ2により計測対象となる地物の法線情報とサイズ情報とを地図DB10から取得する。そして、自車位置推定部17は、車両の進行方向の情報と地物の法線情報から算出される角度差Δθと、地物のサイズ情報が示すサイズSとに基づき、予測自車位置X-(t)を補正する。
【選択図】図7
Description
本発明は、自己位置推定技術に関する。
従来から、車両の進行先に設置される地物をレーダやカメラを用いて検出し、その検出結果に基づいて自車位置を校正する技術が知られている。例えば、特許文献1には、計測センサの出力と、予め地図上に登録された地物の位置情報とを照合させることで自己位置を推定する技術が開示されている。また、特許文献2には、カルマンフィルタを用いた自車位置推定技術が開示されている。
自己位置推定処理において計測対象となる道路標識や方面看板などの地物は,運転手が認識しやすいように作製及び設置されているが、全てのサイズや向きが一定となっているわけではなく、表示内容に応じて大きさが異なり、また設置位置に応じて向きが異なる。その結果、自己位置推定を行うシステムが計測対象とする地物が計測しにくいサイズや向きである場合も存在し,そのような場合には計測対象の地物の計測値に誤差が大きく含まれる可能性もある。このように誤差を含んだ計測値を用いて自己位置推定を行うと、自己位置推定精度が低下してしまう。
本発明は、上記のような課題を解決するためになされたものであり、地物の計測誤差の可能性があった場合でも,自己位置推定精度の劣化を好適に抑制することが可能な自己位置推定装置を提供することを主な目的とする。
請求項1に記載の発明は、自己位置推定装置であって、予測された自己位置を示す予測位置情報を取得する第1取得部と、移動体の進行方向の情報を取得する第2取得部と、前記移動体と共に移動する計測部により計測対象となる地物の法線情報とサイズ情報とを取得する第3取得部と、前記進行方向の情報と、前記法線情報と、前記サイズ情報とに基づき、前記予測された自己位置を補正する補正部と、を備える。
請求項8に記載の発明は、自己位置推定装置が実行する制御方法であって、予測された自己位置を示す予測位置情報を取得する第1取得工程と、移動体の進行方向の情報を取得する第2取得工程と、前記移動体と共に移動する計測部により計測対象となる地物の法線情報とサイズ情報とを取得する第3取得工程と、前記進行方向の情報と、前記法線情報と、前記サイズ情報とに基づき、前記予測された自己位置を補正する補正工程と、を有する。
請求項9に記載の発明は、コンピュータが実行するプログラムであって、予測された自己位置を示す予測位置情報を取得する第1取得部と、移動体の進行方向の情報を取得する第2取得部と、前記移動体と共に移動する計測部により計測対象となる地物の法線情報とサイズ情報とを取得する第3取得部と、前記進行方向の情報と、前記法線情報と、前記サイズ情報とに基づき、前記予測された自己位置を補正する補正部として前記コンピュータを機能させる。
本発明の好適な実施形態によれば、自己位置推定装置は、予測された自己位置を示す予測位置情報を取得する第1取得部と、移動体の進行方向の情報を取得する第2取得部と、前記移動体と共に移動する計測部により計測対象となる地物の法線情報とサイズ情報とを取得する第3取得部と、前記進行方向の情報と、前記法線情報と、前記サイズ情報とに基づき、前記予測された自己位置を補正する補正部と、を備える。この態様により、自己位置推定装置は、予測した自己位置を、移動体の進行方向に対する地物の向きやサイズに基づき好適に補正することができる。
上記自己位置推定装置の一態様では、自己位置推定装置は、前記移動体から前記地物までの前記計測部による計測距離を示す第1距離情報と、前記地物の位置情報に基づき予測された前記移動体から前記地物までの距離を示す第2距離情報とを取得する第4取得部をさらに備え、前記補正部は、前記予測された自己位置を前記第1距離情報及び前記第2距離情報が示す距離の差分値により補正する度合いを、前記進行方向の情報と、前記法線情報と、前記サイズ情報とに基づき定める。この態様により、自己位置推定装置は、第1距離情報及び第2距離情報が示す距離の差分値により予測自己位置を補正する度合いを、移動体の進行方向に対する地物の向きやサイズに基づき好適に調整することができる。
上記自己位置推定装置の他の一態様では、前記補正部は、前記差分値に所定の利得を乗じた値により、前記予測された自己位置を補正し、前記補正部は、前記進行方向の情報と、前記法線情報と、前記サイズ情報とに基づいて、前記利得に対する補正係数を決定する。この態様により、自己位置推定装置は、第1距離情報及び第2距離情報が示す距離の差分値により予測自己位置を補正する度合いを定める利得に対する補正係数を、移動体の進行方向に対する地物の向きやサイズに基づき定めることができる。好適には、前記利得は、カルマンゲインである。
上記自己位置推定装置の他の一態様では、前記補正部は、前記進行方向と前記法線情報が示す前記地物の法線方向との角度差が大きいほど、前記進行方向において前記予測された自己位置を補正する度合いを低くし、前記角度差が小さいほど、前記移動体の横方向において前記予測された自己位置を補正する度合いを低くする。このようにすることで、自己位置推定装置は、地物の位置計測に誤差が生じやすい方向に対する自己位置の補正の度合いを低くし、位置推定精度を好適に向上させることができる。
上記自己位置推定装置の他の一態様では、前記補正部は、前記サイズ情報が示す前記地物のサイズが小さいほど、前記予測された自己位置を補正する度合いを小さくする。一般に、計測する地物のサイズが小さいほど、当該地物に対する計測点が少なくなり、当該地物の位置計測精度が低下する。よって、自己位置推定装置は、この態様により、位置推定精度の低下を好適に抑制することができる。
上記自己位置推定装置の他の一態様では、前記補正部は、前記サイズ情報が示す前記地物のサイズよりも、前記計測部が計測した前記地物のサイズが小さいほど、前記予測された自己位置を補正する度合いを小さくする。一般に、オクルージョン等に起因して地物の一部分しか計測できない場合には、当該地物の位置計測精度は低くなる。よって、自己位置推定装置は、この態様により、オクルージョン等に起因して地物の一部分しか計測できない場合に予測自己位置の補正の度合いを低下させて自己位置推定精度の低下を好適に抑制することができる。
本発明の他の好適な実施形態によれば、自己位置推定装置が実行する制御方法であって、予測された自己位置を示す予測位置情報を取得する第1取得工程と、移動体の進行方向の情報を取得する第2取得工程と、前記移動体と共に移動する計測部により計測対象となる地物の法線情報とサイズ情報とを取得する第3取得工程と、前記進行方向の情報と、前記法線情報と、前記サイズ情報とに基づき、前記予測された自己位置を補正する補正工程と、を有する。自己位置推定装置は、この制御方法を実行することで、予測した自己位置を、移動体の進行方向に対する地物の向きやサイズに基づき好適に補正することができる。
本発明の他の好適な実施形態によれば、コンピュータが実行するプログラムであって、予測された自己位置を示す予測位置情報を取得する第1取得部と、移動体の進行方向の情報を取得する第2取得部と、前記移動体と共に移動する計測部により計測対象となる地物の法線情報とサイズ情報とを取得する第3取得部と、前記進行方向の情報と、前記法線情報と、前記サイズ情報とに基づき、前記予測された自己位置を補正する補正部として前記コンピュータを機能させる。コンピュータは、このプログラムを実行することで、予測した自己位置を、移動体の進行方向に対する地物の向きやサイズに基づき好適に補正することができる。好適には、上記プログラムは、記憶媒体に記憶される。
以下、図面を参照して本発明の好適な実施例について説明する。なお、任意の記号の上に「^」または「-」が付された文字を、本明細書では便宜上、「A^」または「A-」(「A」は任意の文字)と表す。
[概略構成]
図1は、本実施例に係る運転支援システムの概略構成図である。図1に示す運転支援システムは、車両に搭載され、車両の運転支援に関する制御を行う車載機1と、ライダ(Lidar:Light Detection and Ranging、または、Laser Illuminated Detection And Ranging)2と、ジャイロセンサ3と、車速センサ4と、GPS受信機5とを有する。
図1は、本実施例に係る運転支援システムの概略構成図である。図1に示す運転支援システムは、車両に搭載され、車両の運転支援に関する制御を行う車載機1と、ライダ(Lidar:Light Detection and Ranging、または、Laser Illuminated Detection And Ranging)2と、ジャイロセンサ3と、車速センサ4と、GPS受信機5とを有する。
車載機1は、ライダ2、ジャイロセンサ3、車速センサ4、及びGPS受信機5と電気的に接続し、これらの出力に基づき、車載機1が搭載される車両の位置(「自車位置」とも呼ぶ。)の推定を行う。そして、車載機1は、自車位置の推定結果に基づき、設定された目的地への経路に沿って走行するように、車両の自動運転制御などを行う。車載機1は、道路データ及び道路付近に設けられた目印となる地物に関する情報である地物情報を記憶した地図データベース(DB:DataBase)10を記憶する。上述の目印となる地物は、例えば、道路脇に周期的に並んでいるキロポスト、100mポスト、デリニエータ、交通インフラ設備(例えば標識、方面看板、信号)、電柱、街灯などの地物である。そして、車載機1は、この地物情報に基づき、ライダ2等の出力と照合させて自車位置の推定を行う。車載機1は、本発明における「自己位置推定装置」の一例である。
ライダ2は、水平方向および垂直方向の所定の角度範囲に対してパルスレーザを出射することで、外界に存在する物体までの距離を離散的に測定し、当該物体の位置を示す3次元の点群情報を生成する。この場合、ライダ2は、照射方向を変えながらレーザ光を照射する照射部と、照射したレーザ光の反射光(散乱光)を受光する受光部と、受光部が出力する受光信号に基づくスキャンデータを出力する出力部とを有する。スキャンデータは、受光部が受光したレーザ光に対応する照射方向と、上述の受光信号に基づき特定される当該レーザ光の応答遅延時間とに基づき生成される。一般的に、対象物までの距離が近いほどライダの距離計測値の精度は高く、距離が遠いほど精度は低い。ライダ2、ジャイロセンサ3、車速センサ4、GPS受信機5は、それぞれ、出力データを車載機1へ供給する。ライダ2は、本発明における「計測部」の一例である。
図2は、車載機1の機能的構成を示すブロック図である。車載機1は、主に、インターフェース11と、記憶部12と、入力部14と、制御部15と、情報出力部16と、を有する。これらの各要素は、バスラインを介して相互に接続されている。
インターフェース11は、ライダ2、ジャイロセンサ3、車速センサ4、及びGPS受信機5などのセンサから出力データを取得し、制御部15へ供給する。
記憶部12は、制御部15が実行するプログラムや、制御部15が所定の処理を実行するのに必要な情報を記憶する。本実施例では、記憶部12は、地物情報を含む地図DB10を記憶する。図3は、地図DB10のデータ構造の一例を示す。図3に示すように、地図DB10は、施設情報、道路データ、及び地物情報を含む。
地物情報は、地物ごとに当該地物に関する情報が関連付けられた情報であり、ここでは、地物のインデックスに相当する地物IDと、位置情報と、法線情報と、サイズ情報とを含む。位置情報は、緯度及び経度(及び標高)等により表わされた地物の絶対的な位置を示す。法線情報及びサイズ情報は、例えば、看板などの平面的な形状を有している地物に対して設けられる情報である。法線情報は、地物の向きを表す情報であり、例えば地物に形成された面に対する法線ベクトル等を示す。サイズ情報は、地物の大きさを表す情報であり、例えば、地物に形成された面の面積を示す情報であってもよく、地物に形成された面の縦及び横の幅の情報を示すものであってもよい。
なお、地図DB10は、定期的に更新されてもよい。この場合、例えば、制御部15は、図示しない通信部を介し、地図情報を管理するサーバ装置から、自車位置が属するエリアに関する部分地図情報を受信し、地図DB10に反映させる。
入力部14は、ユーザが操作するためのボタン、タッチパネル、リモートコントローラ、音声入力装置等である。情報出力部16は、例えば、制御部15の制御に基づき出力を行うディスプレイやスピーカ等である。
制御部15は、プログラムを実行するCPUなどを含み、車載機1の全体を制御する。本実施例では、制御部15は、自車位置推定部17を有する。制御部15は、本発明における「第1取得部」、「第2取得部」、「第3取得部」、「第4取得部」、「補正部」、及びプログラムを実行する「コンピュータ」の一例である。
自車位置推定部17は、地物に対するライダ2による距離及び角度の計測値と、地図DB10から抽出した地物の位置情報とに基づき、ジャイロセンサ3、車速センサ4、及び/又はGPS受信機5の出力データから推定した自車位置を補正する。本実施例では、一例として、自車位置推定部17は、ベイズ推定に基づく状態推定手法に基づき、ジャイロセンサ3、車速センサ4等の出力データから自車位置を推定する予測ステップと、直前の予測ステップで算出した自車位置の推定値を補正する計測更新ステップとを交互に実行する。これらのステップで用いる状態推定フィルタは、ベイズ推定を行うように開発された様々のフィルタが利用可能であり、例えば、拡張カルマンフィルタ、アンセンテッドカルマンフィルタ、パーティクルフィルタなどが該当する。このように、ベイズ推定に基づく位置推定は、種々の方法が提案されている。以下では、一例として拡張カルマンフィルタを用いた自車位置推定について簡略的に説明する。
図4は、状態変数ベクトルxを2次元直交座標で表した図である。図4に示すように、xyの2次元直交座標上で定義された平面での自車位置は、座標「(x、y)」、自車の方位「Ψ」により表される。ここでは、方位Ψは、車の進行方向とx軸とのなす角として定義されている。座標(x、y)は、例えば緯度及び経度の組合せに相当する絶対位置を示す。
図5は、予測ステップと計測更新ステップとの概略的な関係を示す図である。また、図6は、自車位置推定部17の機能ブロックの一例を示す。図5に示すように、予測ステップと計測更新ステップとを繰り返すことで、自車位置を示す状態変数ベクトル「X」の推定値の算出及び更新を逐次的に実行する。また、図6に示すように、自車位置推定部17は、予測ステップを実行する位置予測部21と、計測更新ステップを実行する位置推定部22とを有する。位置予測部21は、デッドレコニングブロック23及び位置予測ブロック24を含み、位置推定部22は、地物探索・抽出ブロック25及び位置補正ブロック26を含む。なお、図5では、計算対象となる基準時刻(即ち現在時刻)「t」の状態変数ベクトルを、「X-(t)」または「X^(t)」と表記している(「状態変数ベクトルX(t)=(x(t)、y(t)、Ψ(t))T」と表記する)。ここで、予測ステップで推定された暫定的な推定値(予測値)には当該予測値を表す文字の上に「-」を付し、計測更新ステップで更新された,より精度の高い推定値には当該値を表す文字の上に「^」を付す。
予測ステップでは、自車位置推定部17のデッドレコニングブロック23は、車両の移動速度「v」と角速度「ω」(これらをまとめて「制御値u(t)=(v(t)、ω(t))T」と表記する。)を用い、前回時刻からの移動距離と方位変化を求める。自車位置推定部17の位置予測ブロック24は、直前の計測更新ステップで算出された時刻t-1の状態変数ベクトルX^(t-1)に対し、求めた移動距離と方位変化を加えて、時刻tの自車位置の予測値(「予測自車位置」とも呼ぶ。)X-(t)を算出する。また、これと同時に、予測自車位置X-(t)の誤差分布に相当する共分散行列「P-(t)」を、直前の計測更新ステップで算出された時刻t-1での共分散行列「P^(t-1)」から算出する。
計測更新ステップでは、自車位置推定部17の地物探索・抽出ブロック25は、地図DB10に登録された地物の位置ベクトルとライダ2のスキャンデータとの対応付けを行う。そして、自車位置推定部17の地物探索・抽出ブロック25は、この対応付けができた場合に、対応付けができた地物のライダ2による計測値(「地物計測値」と呼ぶ。)「Z(t)」と、予測自車位置X-(t)及び地図DB10に登録された地物の位置ベクトルを用いてライダ2による計測処理をモデル化して求めた地物の計測推定値(「地物予測値」と呼ぶ。)「Z-(t)」とをそれぞれ取得する。地物計測値Z(t)は、時刻tにライダ2が計測した地物の距離及びスキャン角度から、車両の進行方向と横方向を軸とした成分に変換した車両のボディ座標系における2次元ベクトルである。そして、自車位置推定部17の位置補正ブロック26は、以下の式(1)に示すように、地物計測値Z(t)と地物予測値Z-(t)との差分値を算出する。
なお、自車位置推定部17は、複数の地物に対し、地図DB10に登録された地物の位置ベクトルとライダ2のスキャンデータとの対応付けができた場合、選定した任意の一個の地物計測値等に基づき計測更新ステップを行ってもよく、対応付けができた全ての地物計測値等に基づき計測更新ステップを複数回行ってもよい。なお、複数の地物計測値等を用いる場合には、自車位置推定部17は、ライダ2から遠い地物ほどライダ計測精度が悪化することを勘案し、ライダ2と地物との距離が長いほど、当該地物に関する重み付けを小さくするとよい。
このように、予測ステップと計測更新ステップが繰り返し実施され、予測自車位置X-(t)と推定自車位置X^(t)が逐次的に計算されることにより、もっとも確からしい自車位置が計算される。
なお、上記の説明において、地物計測値Z(t)は本発明の「第1距離情報」の一例であり、地物予測値Z-(t)は本発明の「第2距離情報」の一例である。
[自車位置推定の詳細]
次に、本実施例における自車位置推定の詳細について説明する。概略的には、自車位置推定部17は、ライダ2による計測対象の地物の地物情報に含まれる法線情報及びサイズ情報に基づき、カルマンゲインを補正する。これにより、自車位置推定部17は、計測対象の地物の向き及びサイズに応じて車両の進行方向及び横方向での地物計測値の精度を推定し、当該精度に応じて各方向における予測自車位置X-(t)の補正量を的確に定める。
次に、本実施例における自車位置推定の詳細について説明する。概略的には、自車位置推定部17は、ライダ2による計測対象の地物の地物情報に含まれる法線情報及びサイズ情報に基づき、カルマンゲインを補正する。これにより、自車位置推定部17は、計測対象の地物の向き及びサイズに応じて車両の進行方向及び横方向での地物計測値の精度を推定し、当該精度に応じて各方向における予測自車位置X-(t)の補正量を的確に定める。
図7は、ライダ2の計測対象となる地物50と車両との位置関係の一例を表す図である。図7において、矢印40は、車両の進行方向を示し、矢印41は、地物50の法線方向を示す。また、計測点「P1」~「P7」は、ライダ2が出力した点群データの各点の位置を示す。さらに、「Δθ」は、矢印40が示す車両の進行方向と矢印41が示す地物50の法線方向との角度差を示す。なお、角度差Δθは、0~90度の範囲であるものとする。
図7の例では、自車位置推定部17は、ライダ2から計測点P1~P7を示す点群データを取得し、取得した点群データからx方向及びy方向のそれぞれの地物計測値Z(t)を算出する。この場合、自車位置推定部17は、地物50に対する計測点P1~P7が示すボディ座標系のx座標の重心座標(即ち各計測点が示す座標の平均値)を、x方向の地物計測値Z(t)として算出し、地物50に対する計測点P1~P7が示すボディ座標系のy座標の重心座標を、y方向の地物計測値Z(t)として算出する。
このとき、角度差Δθが小さい(即ち平行に近い)ほど、地物計測値の進行方向成分の精度が高まり、横方向成分の精度が低くなる。言い換えると、角度差Δθが大きい(即ち垂直に近い)ほど、地物計測値の横方向成分の精度が高まり、進行方向成分の精度が低くなる。また、地物50のサイズが大きいほど、当該地物50に対するライダ2の計測点が多くなるため、地物計測値の精度が高まる。
以上を勘案し、自車位置推定部17は、角度差Δθと、計測対象の地物のサイズ情報とに基づき、カルマンゲインを小さくするための係数値「aX(t)」、「aY(t)」を算出し、以下の式(3)に示すように、これらをカルマンゲインK(t)に乗じることで、補正後のカルマンゲイン「K(t)´」を算出する。
この場合、係数値aX(t)、aY(t)は、0から1までの値となり、かつ、計測対象の地物のサイズが大きいほど大きい値となるように定められる。さらに、係数aX(t)は、角度差Δθが大きいほど小さい値となり、aY(t)は、角度差Δθが大きいほど大きい値となるように定められる。言い換えると、係数aX(t)は、角度差Δθが小さいほど大きい値となり、aY(t)は、角度差Δθが小さいほど小さい値となるように定められる。係数値aX(t)、aY(t)は、式又はテーブルを用いて設定され、例えば、計測対象の地物のサイズ(例えば面積)を「S」とすると、以下の式(4)に基づき設定される。
次に、図8を参照して係数値aX(t)、aY(t)の設定例について説明する。
図8(A)は、ライダ2によるスキャンを実行中の車両の俯瞰図を示す。図8(A)では、ライダ2の計測範囲内に法線方向が異なる地物51、52が存在している。図8(B)は、ライダ2の計測点「P11」~「P15」を明示した地物51の拡大図である。また、図8(C)は、ライダ2の計測点「P16」~「P19」を明示した地物52の拡大図である。図8(B)、(C)では、それぞれ、各照射点の位置座標から算出した車両の横方向及び進行方向の重心座標が破線により示されている。なお、図8(A)~(C)では、説明便宜上、ライダ2が所定の走査面に沿ってレーザ光を出射した場合を例示しているが、高さの異なる複数の走査面に沿ってレーザ光を出射してもよい。
図8(A)及び図8(B)に示すように、地物51の法線方向は、車両の横方向と略一致している。従って、照射点P11~P15は、車両の進行方向に沿って並んでおり、進行方向におけるばらつきが大きく、横方向のばらつきが小さい。従って、地物51を用いた位置推定では、照射点P11~P15のばらつきが小さい横方向成分の地物計測値の精度が高くなることが予測される。そして、この場合、角度差Δθは、ほぼ90度であり、「cosΔθ≒0」かつ「sinΔθ≒1」となるため、式(4)に基づく係数値aX(t)は0に近い値となり、係数値aY(t)はサイズSに応じた値となる。よって、この場合、地物計測値の精度が低くなるx方向(即ち進行方向)に対する予測自車位置X-(t)への補正量を相対的に小さくすることができる。
一方、図8(A)及び図8(C)に示すように、地物52の法線方向は、車両の進行方向と略一致している。従って、照射点P16~P19は、車両の横方向に沿って並んでおり、横方向におけるばらつきが大きく、進行方向のばらつきが小さい。従って、地物52を用いた位置推定では、照射点P16~P19のばらつきが小さい進行方向成分の地物計測値の精度が高くなることが予測される。そして、この場合、角度差Δθは、ほぼ0度であり、「cosΔθ≒1」かつ「sinΔθ≒0」となるため、式(4)に基づく係数値aX(t)はサイズSに応じた値となり、係数値aY(t)は0に近い値となる。よって、この場合、地物計測値の精度が低くなるy方向(即ち横方向)に対する予測自車位置X-(t)への補正量を相対的に小さくすることができる。なお、係数値aX(t)とaY(t)は、下限値を設けてもよい。その場合、式(4)で計算される結果が所定の下限値以下となる場合は,その下限値が設定される。これにより、Δθによっては補正されない方向が継続してしまう状況を回避できる。
図9は、車載機1の自車位置推定部17により行われる自車位置推定処理のフローチャートである。車載機1は、図9のフローチャートの処理を繰り返し実行する。
まず、自車位置推定部17は、GPS受信機5等の出力に基づき、自車位置の初期値を設定する(ステップS101)。次に、自車位置推定部17は、車速センサ4から車体速度を取得すると共に、ジャイロセンサ3からヨー方向の角速度を取得する(ステップS102)。そして、自車位置推定部17は、ステップS102の取得結果に基づき、車両の移動距離と車両の方位変化を計算する(ステップS103)。
その後、自車位置推定部17は、1時刻前の推定自車位置X^(t-1)に、ステップS103で計算した移動距離と方位変化を加算し、予測自車位置X-(t)を算出する(ステップS104)。さらに、自車位置推定部17は、予測自車位置X-(t)に基づき、地図DB10の地物情報を参照し、ライダ2の計測範囲となる地物を探索する(ステップS105)。
そして、自車位置推定部17は、予測自車位置X-(t)及びステップS105で探索した地物の地物情報が示す位置座標から、地物予測値Z-(t)を算出する(ステップS106)。さらに、ステップS106では、自車位置推定部17は、ステップS105で探索した地物に対するライダ2の計測データから地物計測値Z(t)を算出する。
そして、自車位置推定部17は、ステップS105で探索した地物の地物情報のサイズ情報及び法線情報を参照し、当該地物の被照射面のサイズSと、車両の進行方向と対象の地物の法線ベクトルとの角度差Δθを算出する(ステップS107)。この場合、自車位置推定部17は、角度差Δθを、予測自車位置X-(t)が示す自車の方位Ψと、地物情報に含まれる法線情報が示す地物の法線方向とに基づき算出する。
次に、自車位置推定部17は、サイズSと角度差Δθに基づき、例えば式(4)を用いることで、係数値aX(t)、aY(t)を算出する(ステップS108)。そして、自車位置推定部17は、式(3)に基づき、カルマンゲインK(t)に係数値aX(t)、aY(t)を乗じることで、カルマンゲインK(t)´を生成する(ステップS109)。その後、自車位置推定部17は、式(2)に基づき、生成したカルマンゲインK(t)´をK(t)の代わりに用いて予測自車位置X-(t)を補正し、推定自車位置X^(t)を算出する(ステップS110)。
以上説明したように、本実施例に係る車載機1の自車位置推定部17は、予測自車位置X-(t)を示す情報を生成すると共に、ライダ2により計測対象となる地物の法線情報とサイズ情報とを地図DB10から取得する。そして、自車位置推定部17は、車両の進行方向の情報及び地物の法線情報から算出される角度差Δθと、地物のサイズ情報が示すサイズSとに基づき、予測自車位置X-(t)を補正する。これにより、自車位置推定部17は、計測対象の地物の向き及びサイズに応じて車両の進行方向及び横方向での地物計測値の精度を推定し、当該精度に応じて各方向における予測自車位置の補正量を的確に定めることができる。
[変形例]
以下、実施例に好適な変形例について説明する。以下の変形例は、組み合わせて実施例に適用してもよい。
以下、実施例に好適な変形例について説明する。以下の変形例は、組み合わせて実施例に適用してもよい。
(変形例1)
予測自車位置X-(t)の補正量を調整する方法は、係数値aX(t)、aY(t)をカルマンゲインK(t)に乗じることに限定されない。
予測自車位置X-(t)の補正量を調整する方法は、係数値aX(t)、aY(t)をカルマンゲインK(t)に乗じることに限定されない。
これに代えて、第1の例では、自車位置推定部17は、式(1)に示される差分値dx、dyに対して係数値aX(t)、aY(t)を乗じてもよい。即ち、この場合、自車位置推定部17は、以下の式(5)に基づき、推定自車位置を算出する。
(変形例2)
自車位置推定部17は、地図DB10に登録されたサイズ情報から特定される計測対象の地物のサイズと、ライダ2の計測データから特定されるサイズとの比を勘案してカルマンゲインを補正してもよい。
自車位置推定部17は、地図DB10に登録されたサイズ情報から特定される計測対象の地物のサイズと、ライダ2の計測データから特定されるサイズとの比を勘案してカルマンゲインを補正してもよい。
図10は、ライダ2によるレーザ光が照射される地物55の被照射面を示した図である。図10の例では、地物55の被照射面上での計測点が明示されている。なお、地物55の左側の一部は、障害物によるオクルージョン等に起因してライダ2のレーザ光が照射されていない。
この場合、自車位置推定部17は、地物55に対応する地物情報に含まれるサイズ情報に基づき、地物55の高さ方向の幅「HM」と、横方向の幅「WM」とをそれぞれ特定する。また、自車位置推定部17は、ライダ2が出力する点群データに基づき、地物55に対する計測点が分布する範囲(計測範囲)の高さ方向の幅「HL」と、横方向の幅「WL」とをそれぞれ特定する。
そして、自車位置推定部17は、地物55に対する距離計測の正確さを示す信頼度情報「a(t)」を、以下の式(10)を用いて生成する。
そして、自車位置推定部17は、例えば、算出した信頼度情報a(t)を、係数値aX(t)、aY(t)と共にカルマンゲインに乗じることで、地物に対する距離計測の正確さを示す信頼度に応じた予測自車位置の補正量を定めることができる。
(変形例3)
図1に示す運転支援システムの構成は一例であり、本発明が適用可能な運転支援システムの構成は図1に示す構成に限定されない。例えば、運転支援システムは、車載機1を有する代わりに、車両の電子制御装置が車載機1の自車位置推定部17の処理を実行してもよい。この場合、地図DB10は、例えば車両内の記憶部に記憶され、車両の電子制御装置は、地図DB10の更新情報を図示しないサーバ装置から受信してもよい。
図1に示す運転支援システムの構成は一例であり、本発明が適用可能な運転支援システムの構成は図1に示す構成に限定されない。例えば、運転支援システムは、車載機1を有する代わりに、車両の電子制御装置が車載機1の自車位置推定部17の処理を実行してもよい。この場合、地図DB10は、例えば車両内の記憶部に記憶され、車両の電子制御装置は、地図DB10の更新情報を図示しないサーバ装置から受信してもよい。
1 車載機
2 ライダ
3 ジャイロセンサ
4 車速センサ
5 GPS受信機
10 地図DB
2 ライダ
3 ジャイロセンサ
4 車速センサ
5 GPS受信機
10 地図DB
Claims (1)
- 予測された自己位置を示す予測位置情報を取得する第1取得部と、
移動体の進行方向の情報を取得する第2取得部と、
前記移動体と共に移動する計測部により計測対象となる地物の法線情報とサイズ情報とを取得する第3取得部と、
前記進行方向の情報と、前記法線情報と、前記サイズ情報とに基づき、前記予測された自己位置を補正する補正部と、
を備える自己位置推定装置。
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
JP2017100217 | 2017-05-19 |
Related Parent Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
JP2023092428A Division JP2023101820A (ja) | 2017-05-19 | 2023-06-05 | 自己位置推定装置、制御方法、プログラム及び記憶媒体 |
Publications (1)
Publication Number | Publication Date |
---|---|
JP2024161105A true JP2024161105A (ja) | 2024-11-15 |
Family
ID=
Similar Documents
Publication | Publication Date | Title |
---|---|---|
JP7155284B2 (ja) | 計測精度算出装置、自己位置推定装置、制御方法、プログラム及び記憶媒体 | |
JP2024050990A (ja) | 判定装置 | |
JP6980010B2 (ja) | 自己位置推定装置、制御方法、プログラム及び記憶媒体 | |
JP6806891B2 (ja) | 情報処理装置、制御方法、プログラム及び記憶媒体 | |
JP2022176322A (ja) | 自己位置推定装置、制御方法、プログラム及び記憶媒体 | |
JP2023164553A (ja) | 位置推定装置、推定装置、制御方法、プログラム及び記憶媒体 | |
JP2023075184A (ja) | 出力装置、制御方法、プログラム及び記憶媒体 | |
JP2023118751A (ja) | 自己位置推定装置 | |
JP2023054314A (ja) | 情報処理装置、制御方法、プログラム及び記憶媒体 | |
WO2018212302A1 (ja) | 自己位置推定装置、制御方法、プログラム及び記憶媒体 | |
JP2019174191A (ja) | データ構造、情報送信装置、制御方法、プログラム及び記憶媒体 | |
WO2019188886A1 (ja) | 端末装置、情報処理方法、プログラム、及び、記憶媒体 | |
JP2024161105A (ja) | 自己位置推定装置、制御方法、プログラム及び記憶媒体 | |
WO2019188874A1 (ja) | データ構造、情報処理装置、及び地図データ生成装置 | |
WO2018212290A1 (ja) | 情報処理装置、制御方法、プログラム及び記憶媒体 | |
JP2024161130A (ja) | 自己位置推定装置、制御方法、プログラム及び記憶媒体 | |
US12140697B2 (en) | Self-position estimation device, self-position estimation method, program, and recording medium | |
US20240053440A1 (en) | Self-position estimation device, self-position estimation method, program, and recording medium | |
JP2019174675A (ja) | データ構造、地図データ生成装置、制御方法、プログラム及び記憶媒体 | |
JP2024161585A (ja) | 自己位置推定装置 |