JP7274366B2 - 自己位置推定方法及び自己位置推定装置 - Google Patents

自己位置推定方法及び自己位置推定装置 Download PDF

Info

Publication number
JP7274366B2
JP7274366B2 JP2019121087A JP2019121087A JP7274366B2 JP 7274366 B2 JP7274366 B2 JP 7274366B2 JP 2019121087 A JP2019121087 A JP 2019121087A JP 2019121087 A JP2019121087 A JP 2019121087A JP 7274366 B2 JP7274366 B2 JP 7274366B2
Authority
JP
Japan
Prior art keywords
self
vehicle
value
relative position
observed
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.)
Active
Application number
JP2019121087A
Other languages
English (en)
Other versions
JP2021006797A (ja
Inventor
千加夫 土谷
祐一 武田
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Renault SAS
Original Assignee
Renault SAS
Priority date (The priority date 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 date listed.)
Filing date
Publication date
Application filed by Renault SAS filed Critical Renault SAS
Priority to JP2019121087A priority Critical patent/JP7274366B2/ja
Publication of JP2021006797A publication Critical patent/JP2021006797A/ja
Application granted granted Critical
Publication of JP7274366B2 publication Critical patent/JP7274366B2/ja
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Landscapes

  • Instructional Devices (AREA)
  • Navigation (AREA)

Description

本発明は、自己位置推定方法及び自己位置推定装置に関する。
従来、自車両に対する自車両周囲の物標の相対位置に基づいて、自車両の自己位置の推定値を校正する技術が知られている。物標に基づいて自己位置を校正するタイミングは、物標を検出した直後であるため、誤差補正量が多い場合には校正直後に自己位置が突然大きく補正される現象、いわゆる位置飛びの問題があった。
このため下記特許文献1の自車位置校正装置は、自車の進行先の基準地物の設置地点の所定距離前の補正開始地点から当該設置地点に対応する補正終了地点までの区間を複数の補正区間に分割する。複数の補正区間毎に異なる誤差補正カーブに従って補正終了地点において誤差補正量に到達するよう段階的に自車位置校正処理を実行する。
特開2014-066636号公報
しかしながら、特許文献1の自車位置校正装置においては、進路変更等により補正区間の途中で自車両が予定経路から外れると、新たに基準地物を検出するまで自己位置の推定ができなくなるという問題がある。
本発明は、周囲の物標の相対位置に基づいて自車両の自己位置の推定値を校正する際の推定値の急変を抑制すると共に、自車両の進路に関わらずに自己位置の推定を継続することを目的とする。
本発明の一態様によれば、自車両に対する自車両の周囲の物標の相対位置を検出し、予め記憶した物標の地図上の位置と相対位置とに基づき自車両の自己位置を推定する自己位置推定方法が与えられる。自己位置推定方法では、自車両の移動量に基づいて自車両の自己位置の予測値を算出し、物標の相対位置の検出結果に基づく観測値を生成し、相対位置の検出タイミングにおいて、観測値に応じて予測値を補正して自車両の自己位置を推定し、検出タイミングからの自車両の移動量に基づいて観測値を更新した更新観測値を算出し、相対位置の検出タイミングの合間において、更新観測値に応じて予測値を補正して自車両の自己位置を推定する。
本発明によれば、周囲の物標の相対位置に基づいて自車両の自己位置の推定値を校正する際の推定値の急変を抑制すると共に、自車両の進路に関わらずに自己位置の推定を継続できる。
第1実施形態の自己位置推定装置の一例の概略構成図である。 自己位置推定における位置飛びの説明図である。 第1実施形態の自己位置推定方法の説明図である。 第1実施形態の自己位置推定方法の一例のフローチャートである。 第2実施形態の自己位置推定装置の一例の概略構成図である。
以下、図面を参照して、本発明の実施形態を説明する。図面の記載において、同一又は類似の部分には同一又は類似の符号を付し、重複する説明を省略する。各図面は模式的なものであり、現実のものとは異なる場合が含まれる。以下に示す実施形態は、本発明の技術的思想を具体化するための装置や方法を例示するものであって、本発明の技術的思想は、下記の実施形態に例示した装置や方法に特定するものでない。本発明の技術的思想は、特許請求の範囲に記載された技術的範囲内において、種々の変更を加えることができる。
(第1実施形態)
(構成)
図1を参照する。以下、移動体の一例として車両の現在位置を推定する場合について説明するが、本発明は車両に限らず様々な移動体の現在位置の推定に広く適用することができる。
自己位置推定装置1は車両(以下「自車両」と表記する)に搭載され、自車両の現在位置を推定する。自己位置推定装置1は、物標検出部10、地図データベース11、移動量検出部12、コントローラ13を備える。なお、添付する図面において地図データベースを「地図DB」と表記する。
物標検出部10は、カメラ、LIDAR(Light Detection and Ranging)、レーダ等のセンサを備え、パターン認識処理によって自車両の周囲の物標であるランドマークを検出する。
ランドマークは、例えば車線区分線(白線)、停止線、道路標示、道路標識、信号機、その他の道路周辺の地物である。
物標検出部10は、検出したランドマークの自車両に対する相対位置を表す位置情報を生成する。この位置情報は、例えば、自車両の位置を中心とする車両座標系上のランドマークの座標Pc(xc、yc)を含む。物標検出部10は、ランドマークの位置情報をコントローラ13へ出力する。
地図データベース11は、フラッシュメモリ等の記憶に記憶されたデータベースであり、自車両の自己位置推定に用いられる地図情報を含む。地図情報には例えば、道路形状やランドマークの位置や種類などの情報が含まれている。
移動量検出部12は、自車両の相対的な移動量ΔPを検出する。移動量検出部12は、例えばヨーレイトセンサと車輪速センサを備えてよい。移動量検出部12は、ヨーレイトセンサから読み出されるヨーレイトと、車輪速センサの検出結果から算出した車速に基づいて、自車両の相対的な移動量ΔPを検出する。移動量検出部12は、検出した移動量ΔPをコントローラ13へ出力する。
コントローラ13は、自車両の自己位置を推定する電子制御ユニット(ECU:Electronic Control Unit)である。
コントローラ13は、プロセッサと記憶装置等の周辺部品とを含んだコンピュータであってよい。プロセッサは、例えばCPU(Central Processing Unit)、やMPU(Micro-Processing Unit)であってよい。
記憶装置は、半導体記憶装置、磁気記憶装置及び光学記憶装置を備えてよい。記憶装置は、レジスタ、キャッシュメモリ、主記憶装置として使用されるROM(Read Only Memory)及びRAM(Random Access Memory)等のメモリを含んでよい。
なお、汎用の半導体集積回路中に設定される機能的な論理回路でコントローラ13を実現してもよい。例えば、コントローラ13はフィールド・プログラマブル・ゲート・アレイ(FPGA:Field-Programmable Gate Array)等のプログラマブル・ロジック・デバイス(PLD:Programmable Logic Device)等を有していてもよい。
コントローラ13は、移動量検出部12が検出した自車両の相対的な移動量ΔPに基づいて、デッドレコニング技術やオドメトリを用いて自車両の自己位置を推定する。
また、物標検出部10がランドマークを検出すると、コントローラ13は自車両に対するランドマークの相対位置Pcと、地図データベース11に記憶されたランドマークの地図上の位置である地図座標Pm(xm,ym)とに基づいて、自車両の自己位置を校正する。
上述の通り、ランドマークの検出位置に基づいて自車両の自己位置の推定値を校正すると、推定位置の急変(いわゆる位置飛び)が発生することがある。
図2Aを参照して位置飛びの問題を説明する。横軸は自車両の移動時間を表し、縦軸は自車両の自己位置の推定値を表す。ここでは、理解の容易のため2次元の自己位置推定ではなく1次元の自己位置推定の例を示す。
丸印は、自車両の移動量に基づいてデッドレコニング技術等で推定した自己位置を表し、星印は、ランドマーク(物標)の検出位置により校正(検出)した自己位置を表す。
図2Aに示すとおり、移動量に基づく自己位置(丸印)は比較的滑らかに推移するのに対し、ランドマークで校正した自己位置(星印)は直前の自己位置(丸印)から急変し、位置飛びが発生している。
このため、実施形態の自己位置推定方法では、ランドマークの検出タイミングだけでなく、ランドマークの検出タイミングの合間においても、直前のランドマークの検出結果を利用して自己位置の推定結果を補正する。
具体的には、実施形態の自己位置推定方法では、例えば自車両の走行開始地点やランドマークの相対位置Pcの検出結果に基づく観測値の初回値等の任意の基準位置からの自車両の移動量ΔPに基づいて自車両の自己位置の予測値Ppを算出する。すなわち自車両の自己位置の予測値Ppは任意の時点における自車両の位置を基準位置とし、基準位置からの自車両の移動量を移動量ΔPに基づいて算出して求められた自己位置であり、基準位置を除いて自車両の移動量のみから予測された自己位置である。また、ランドマークの相対位置Pcの検出結果に基づく観測値(自己位置)を生成する。
相対位置Pcの検出タイミングでは、観測値に応じて予測値Ppを補正して自車両の自己位置Peを推定する。
相対位置Pcの検出タイミングの合間(相対位置Pcが検出されてから次に相対位置Pcが検出されるまでの間)では、相対位置Pcの検出タイミングからの自車両の移動量を移動量ΔPに基づいて算出し、相対位置Pcと自車両の移動量に基づいて観測値を更新した更新観測値を算出し、更新観測値に応じて予測値Ppを補正して自車両の自己位置Peを推定する。
図2Bを参照して第1実施形態の自己位置推定方法を説明する。
星印は、ランドマークの相対位置Pcの検出結果に基づく観測値として算出された自車両の自己位置を示す。自己位置の観測値は、自車両に対するランドマークの相対位置Pcとランドマークの地図上の位置Pm(xm,ym)とに基づいて決定できる。
三角印は、ランドマークの相対位置Pcの検出結果に基づく観測値に対して、ランドマークの相対位置Pcを検出した時点からの自車両の移動量に基づいて観測値(星印)を更新した更新観測値を示す。
丸印は、基準位置からの自車両の移動量に基づいて予測した自己位置の予測値Ppを、観測値(星印)又は更新観測値(三角印)に応じて補正した自己位置の推定値Peである。
相対位置Pcの検出タイミングでは観測値(星印)に応じて予測値Ppを補正し、検出タイミングの合間では更新観測値(三角印)に応じて予測値Ppを補正する。
このように第1実施形態の自己位置推定方法では、相対位置Pcの検出タイミングだけでなく、検出タイミングの合間においても、相対位置Pcの検出結果に応じて予測値Ppを補正する。
これにより、相対位置Pcの検出結果に基づいて補正するタイミングが分散されるので1回ごとの補正量を低減できる。このため、図2Bに示すように予測値Ppを補正した推定値Peの急な変動が抑制され、位置飛びを軽減できる。
さらに、更新観測値は自車両の移動量に基づいて算出されるので、自車両の進路に関わらずに更新観測値を算出できる。このため、自車両の進路に関わらずに継続して予測値Ppを補正して、自己位置の推定値Peを算出することができる。
なお、図2Bの例では、ランドマークの相対位置Pcの検出結果に基づく観測値として自車両の自己位置の観測値を生成した。しかしながら、観測値は必ずしも自己位置の観測値でなくともよい。
観測値は、自車両周囲のランドマークの相対位置Pcの検出結果に基づいて自車両の予測値Ppを補正するために利用可能な測定値であれば足りる。例えば以下の説明では、ランドマークの相対位置Pcそれ自身を観測値として使用する。
以下、コントローラ13の機能について詳述する。図1を参照する。コントローラ13は、予測部20と、更新観測値生成部21と、スイッチ(SW)22と、座標変換部23と、マッチング部24と、減算器25と、ゲイン設定部26と、乗算器27と、更新部28を備える。
予測部20、更新観測値生成部21、スイッチ22、座標変換部23、マッチング部24、減算器25、ゲイン設定部26、乗算器27及び更新部28の機能は、例えば、コントローラ13が備えるプロセッサが記憶装置に格納されたコンピュータプログラムを実行することによって実現されてもよく、FPGA等のPLDなどのロジックで実現されてもよい。
予測部20は、移動量検出部12が検出した自車両の移動量ΔPに基づいて自車両の自己位置の予測値Ppを算出する。
具体的には、予測部20は、前回の自己位置の推定値Peを推定した時点から現在までの自車両の相対的な移動量ΔPを、移動量検出部12から取得する。予測部20は、前回の自己位置の推定値Peに移動量ΔPを加えて現在の自己位置の予測値Pp(xp,yp)及び方位角θを算出する処理を繰り返す。この工程は、カルマンフィルタの予測ステップの処理に相当する。
更新観測値生成部21は、ランドマークの相対位置Pcの検出タイミングにおいて、相対位置Pcの検出結果を観測値として取得する。更新観測値生成部21は、相対位置Pcの検出タイミングからの自車両の相対的な移動量を移動量検出部12から取得する。
更新観測値生成部21は、相対位置Pcの検出タイミングの合間に予測部20が予測値Ppを算出するタイミングにおいて、自車両の移動量に基づいて観測値Pcを更新して更新観測値Pd(xd,yd)を算出する。
具体的には、ランドマークの相対位置Pcの検出タイミングからの自車両の相対的な移動量を、ランドマークの相対位置(観測値)Pcから減じた差分を、更新観測値Pdとして算出する。
スイッチ22は、ランドマークの相対位置(観測値)Pc及び更新観測値Pdを切り替えて、座標変換部23に入力する。
具体的には、相対位置Pcの検出タイミングにおいて観測値Pcを座標変換部23に入力する。また、相対位置Pcの検出タイミングの合間に予測部20が予測値Ppを算出するタイミングでは、更新観測値Pdを座標変換部23に入力する。
座標変換部23は、自車両に対するランドマークの相対位置(車両座標系で表された位置)である観測値Pc及び更新観測値Pdの座標を、地図データベース11の地図座標系上の座標へ変換する。地図座標系上の座標に変換された観測値Pc及び更新観測値Pdを「地図座標系観測値Pm’」と表記する。
具体的には、予測値Pp(xp,yp)及び方位角θに基づいて次式により観測値Pc(xc,yc)を地図座標系観測値Pm’(xm’,ym’)へ変換する。
xm’=cosθ×xc-sinθ×yc+xp
ym’=sinθ×xc+cosθ×yc+yp
なお、地図座標系上の座標に変換された観測値Pcは、特許請求の範囲に記載の「物標の地図座標系上の第1位置」の一例である。
また、次式により更新観測値Pd(xd、yd)を地図座標系観測値Pm’(xm’,ym’)へ変換する。
xm’=cosθ×xd-sinθ×yd+xp
ym’=sinθ×xd+cosθ×yd+yp
地図座標系上の座標に変換された更新観測値Pdは、特許請求の範囲に記載の「物標の地図座標系上の第2位置」の一例である。
座標変換部23は、地図座標系観測値Pm’をマッチング部24と減算器25に出力する。
マッチング部24は、地図座標系観測値Pm’と、地図データベース11に記憶された同じ種類のランドマークの地図座標とを比較し、最も近いランドマーク同士をマッチングさせる。このとき、最も近いランドマークであっても所定閾値以上離れたランドマークはマッチングしない。例えば、所定閾値は1mであってよい。マッチング部24は、地図座標系観測値Pm’とマッチングしたランドマークの地図座標Pm(xm,ym)を減算器25に出力する。
減算器25は、地図座標系観測値Pm’と地図座標Pmとの差(xm’-xm,ym’-ym)である予測誤差eを算出し、乗算器27に出力する。
乗算器27は、ゲイン設定部26で設定されたゲインgを予測誤差eに乗じ、その結果得られる積(g×e)を更新部28へ出力する。
ゲイン設定部26で設定されたゲインgは、カルマンフィルタに相当し、観測値Pcと更新観測値Pdの観測誤差に応じて設定される。例えば、更新観測値Pdの観測誤差は観測値Pcの観測誤差よりも大きいので、更新観測値Pdの予測誤差eに乗じるゲインgを、観測値Pcの予測誤差eに乗じるゲインgよりも小さな値に設定してよい。
更新部28は、予測誤差eとゲインgの積(g×e)に応じて、予測部20が算出した自車両の自己位置の予測値Ppを補正して、自己位置の推定値Peを推定する。例えば、更新部28は、積(g×e)によりカルマンフィルタの観測更新式に従って予測値Ppを更新して推定値Peを推定する。
ここで、ゲインgを十分小さくすれば、更新部28における予測値Ppの一回の補正量を制限できる。これにより、予測誤差eが徐々に小さくなるように(すなわち、地図座標系観測値Pm’が地図座標Pmに徐々に近づくように)推定値Peを推定することができる。このため、観測値Pcによる校正の際の推定値Peの急変を抑制できる。
(動作)
次に、図3を参照して第1実施形態の自己位置推定方法の一例を説明する。
ステップS1において移動量検出部12は、前回の自己位置の推定値Peを推定した時点から現在までの自車両の相対的な移動量ΔPを検出する。
ステップS2において予測部20は、前回の自己位置の推定値Peに移動量ΔPを加えて現在の自己位置の予測値Ppを算出する。
ステップS3において、現時刻が移動量検出部12による自車両周囲のランドマーク()の相対位置Pcの検出タイミングであるか否かを判定する。検出タイミングである場合(ステップS3:Y)には処理はステップS4に進む。検出タイミングの合間のタイミングである場合(ステップS3:N)には処理はステップS5へ進む。
ステップS4において移動量検出部12は、自車両に対するランドマークの相対位置を検出し、相対位置の観測値Pcを生成する。その後に処理はステップS6へ進む。
一方でステップS5において更新観測値生成部21は、相対位置の前回の検出タイミングからの自車両の相対的な移動量を観測値Pcから減じて、更新観測値Pdとして算出する。その後に処理はステップS6へ進む。
ステップS6において座標変換部23は、観測値Pc及び更新観測値Pdを地図座標系観測値Pm’へ変換する。
ステップS7においてマッチング部24は、地図データベース11から、地図座標系観測値Pm’とマッチングするランドマークの地図座標Pmを読み出す。
ステップS8において更新部28は、地図座標系観測値Pm’と地図座標Pmとの間の予測誤差eに応じて、予測部20が算出した自車両の自己位置の予測値Ppを補正して、自己位置の推定値Peを推定する。
(第1実施形態の効果)
(1)自己位置推定装置1は、自車両に対する自車両の周囲の物標の相対位置を検出し、予め記憶した物標の地図上の位置と相対位置とに基づき自車両の自己位置を推定する。予測部20は、自車両の移動量ΔPに基づいて自車両の自己位置の予測値Ppを算出する。物標検出部10は、物標の相対位置の検出結果に基づく観測値を生成する。更新部28は、相対位置の検出タイミングにおいて、観測値Pcに応じて予測値Ppを補正して自車両の自己位置Peを推定する。
更新観測値生成部21は、相対位置の検出タイミングからの自車両の移動量に基づいて観測値Pcを更新した更新観測値Pdを算出する。更新部28は、相対位置の検出タイミングの合間において、更新観測値Pdに応じて予測値Ppを補正して自車両の自己位置Peを推定する。
このように相対位置の検出タイミングで予測値Ppを補正するだけでなく、その合間のタイミングでも、観測値Pcから算出した更新観測値Pdに基づいて補正する。これにより、観測値Pcの検出結果に基づく補正タイミングを分散できるので、1回の補正量を低減できる。このため、物標の相対位置に基づいて自車両の自己位置の推定値を校正する際の推定値の急変を抑制できる。また、自車両の移動量に基づいて更新観測値Pdを算出するので、自車両の進路に関わらずに更新観測値を算出して自己位置の推定を継続できる。
(2)物標検出部10は、観測値として物標の相対位置の観測値を生成する。座標変換部23は、観測値Pcと予測値Ppとに基づき物標の地図座標系上の第1位置Pm’を予測し、更新観測値Pdと予測値Ppとに基づき物標の地図座標系上の第2位置Pm’を予測する。
更新部28は、相対位置の検出タイミングにおいて第1位置Pm’が予め記憶した物標の地図上の位置Pmに近づくように予測値Ppを補正し、相対位置の検出タイミングの合間において第2位置Pm’が予め記憶した物標の地図上の位置Pmに近づくように予測値を補正する。
このように、物標検出部10が検出した物標の地図座標系上の位置Pm’が、予め記憶した地図上の位置Pmに近づくように、予測値Ppを補正して自己位置Peの推定することにより、推定値の急変を抑制できる。
(変形例)
上述の通り、物標の相対位置の検出結果に基づく観測値として、自車両の自己位置の観測値を生成してもよい。自車両に対するランドマークの相対位置とランドマークの地図上の位置とに基づいて自己位置の観測値を算出してよい。
この場合、更新部28は、相対位置の検出タイミングにおいて観測値に近づくように予測値Ppを補正し、相対位置の検出タイミングの合間において更新観測値に近づくように予測値Ppを補正する。
このように自車両の自己位置の観測値を使用しても、第1実施形態の上記効果と同様の効果を得ることができる。
(第2実施形態)
続いて、図4を参照して第2実施形態の自己位置推定装置1を説明する。第1実施形態の構成要素と同様の構成要素には同じ参照符号を付し、同様の機能の説明を省略する。
第2実施形態の自己位置推定装置1は、複数の物標検出部10a、10b及び10cを備える。
例えば物標検出部10aは、カメラを備え、パターン認識処理によって自車両の周囲のランドマークである信号機を検出する。物標検出部10aは、自車両に対する信号機の相対位置の観測値Pcaを生成する。
例えば物標検出部10bは、カメラを備え、パターン認識処理によって自車両の周囲のランドマークである標識を検出する。物標検出部10bは、自車両に対する標識の相対位置の観測値Pcbを生成する。
例えば物標検出部10cは、LIDARを備え、パターン認識処理によって自車両の周囲の地物を検出する。物標検出部10cは、自車両に対する地物表面の各点の相対位置を表す点群データ、すなわち検出した地物形状のポイントクラウドを観測値Pccとして生成する。
さらに、第2実施形態の自己位置推定装置1は、複数の物標検出部10a~10cの検出結果に基づく複数の観測値Pca~Pccの更新観測値Pda、Pdb及びPdcをそれぞれ算出する複数の更新観測値生成部21a、21b及び21cを備える。
更新観測値生成部21aは、物標検出部10aの検出タイミングから自車両の移動量に基づいて観測値Pcaを更新した更新観測値Pdaを算出する。
更新観測値生成部21bは、物標検出部10bの検出タイミングから自車両の移動量に基づいて観測値Pcbを更新した更新観測値Pdbを算出する。
更新観測値生成部21cは、物標検出部10cの検出タイミングから自車両の移動量に基づいて観測値Pccを更新した更新観測値Pdcを算出する。
このように生成された観測値Pca~Pcc及び更新観測値Pda~Pdcは、順次、座標変換部23に入力されて、それぞれ地図座標系観測値Pm’に変換される。
マッチング部24は、地図座標系観測値Pm’の各々とマッチングするランドマークの地図座標Pmを地図データベース11から読み出し、減算器25は、地図座標系観測値Pm’と地図座標Pmとの間の予測誤差eが算出する。更新部28は、予測誤差eとゲインgの積(g×e)に応じて予測値Ppを補正して、自己位置の推定値Peを推定する。
以下の説明において、物標検出部10a~10cを総称して「物標検出部10」と表記することがある。更新観測値生成部21a~21cを総称して「更新観測値生成部21」と表記することがある。
また、観測値Pca~Pccを総称して「観測値Pc」と表記することがある。更新観測値Pda~Pdcを総称して「更新観測値Pd」と表記することがある。
物標検出部10a~10cによる相対位置の検出周期は異なっていてもよく、同じであってもよい。
物標検出部10a~10cの検出周期が異なる場合、観測値Pca~Pccの発生頻度に差が生じることになる。
このように発生頻度の異なる観測値Pca~Pccに基づいて自己位置の推定値Peを推定すると、発生頻度の高い観測値Pcに応じて予測値Ppを補正する頻度が高くなるため、発生頻度の低い観測値Pcが自己位置の推定値Peに反映されにくくなる。すなわち、検出周期が比較的長い物標検出部10の検出結果が推定値Peに反映されにくくなる。
そこで発生頻度の低い観測値Pcを重み付けして推定値Peに反映され易くなるようにすると、今度は発生頻度の低い観測値Pcによる補正で推定値Peが大きく変動し、位置飛びを生じさせることになる。
このため、第2実施形態の自己位置推定装置1は、検出周期がより長い物標検出部10の観測値Pcについてはより多くの更新観測値Pdを発生させてよい。これにより、検出周期の異なる物標検出部10a~10cの検出結果が、推定値Peに与える影響の違いを低減する。
具体的には、比較的長い検出周期の物標検出部10による観測値Pcの更新観測値Pdを算出する更新観測値生成部21は、比較的短い検出周期の物標検出部10による観測値Pcの更新観測値Pdを算出する他の更新観測値生成部21よりも、より高い頻度で更新観測値を算出してよい。
すなわち、比較的長い検出周期の物標検出部10による観測値Pcの更新観測値Pdを算出する更新観測値生成部21は、比較的短い検出周期の物標検出部10による観測値Pcの更新観測値Pdを算出する他の更新観測値生成部21よりも、より多くの更新観測値を算出してよい。
これにより、例えば、単位時間あたりの観測値Pca~Pccの生成回数Nca、Ncb、Nccの比(Nca:Ncb:Ncc)よりも、単位時間あたり観測値Pca~Pccと更新観測値Pda~Pdcの生成回数の和の比(Nca+Nda:Ncb+Ndb:Ncc+Ndc)を1に近付けることができる。
この結果、検出周期の異なる物標検出部10a~10cの検出結果が、推定値Peに与える影響の違いを低減できる。
また、物標検出部10a~10cの観測値Pca~Pccや、その更新観測値Pda~Pdcの信頼度(すなわち測定誤差)は、物標検出部10a~10cの特性や、作動状況、検出状態に応じてそれぞれ異なる。
このため、ゲイン設定部26は、観測値Pca~Pccと更新観測値Pda~Pdcの信頼度を設定する。例えば、物標検出部10a~10cの特性や、作動状況、検出状態に応じてこれらの信頼度を設定してよい。
ゲイン設定部26は、観測値Pca~Pcc及び更新観測値Pda~Pdcに乗じるゲインgを、観測値Pca~Pcc及び更新観測値Pda~Pdcの信頼度に応じてそれぞれ設定する。例えば、ゲイン設定部26は、より信頼度が高い場合により大きなゲインgを設定する。
例えば、ゲイン設定部26は、これらの信頼度をカルマンフィルタにおける観測の共分散行列として設定する。具体的には信頼度の高い観測値や更新観測値に対しては小さな共分散を、信頼度の低い観測値や更新観測値に対しては大きな共分散を設定する。
これにより、信頼度に応じて重み付けされた複数の観測値Pca~Pccに応じて予測値Ppを補正して自己位置Peを推定することができる。
また、信頼度に応じて重み付けされた複数の更新観測値Pda~Pdcに応じて予測値Ppを補正して自己位置Peを推定することができる。
(第2実施形態の効果)
(1)自己位置推定装置1は、ランドマークの相対位置をそれぞれ検出する複数の物標検出部10a~10cの検出結果に基づいて、複数の観測値Pca~Pccをそれぞれ生成する。更新観測値生成部21a~21cは、複数の物標検出部10a~10cの検出タイミングからの自車両の移動量に基づいて、複数の観測値Pca~Pccをそれぞれ更新した複数の更新観測値Pda~Pdcを算出する。これにより、複数の物標検出部10a~10cの検出結果に基づいて自車両の自己位置Peを推定できる。
(2)ゲイン設定部26は、複数の観測値Pca~Pcc毎に信頼度を定める。更新部28は、相対位置の検出タイミングにおいて、信頼度に応じて重み付けされた複数の観測値Pca~Pccに応じて予測値Ppを補正して自車両の自己位置Peを推定する。これにより、観測値Pca~Pccの信頼度に応じた補正量で予測値Ppを補正して自己位置Peを推定できる。
(3)ゲイン設定部26は、複数の更新観測値Pda~Pdc毎に信頼度を定める。更新部28は、相対位置の検出タイミングの合間において、信頼度に応じて重み付けされた複数の更新観測値Pda~Pdcに応じて予測値Ppを補正して自車両の自己位置Peを推定する。これにより、更新観測値Pda~Pdcの信頼度に応じた補正量で予測値Ppを補正して自己位置Peを推定できる。
(4)複数の物標検出部10a~10cは、第1周期でランドマークの相対位置を検出する第1の物標検出部と、第1周期よりも長い第2周期でランドマークの相対位置を検出する第2の物標検出部とを含む。
更新観測値生成部21a~21cのうち第1の更新観測値生成部は、第1の物標検出部の検出タイミングからの自車両の移動量に基づいて、第1の物標検出部の検出結果に基づく観測値を更新した第1更新観測値を算出する。
更新観測値生成部21a~21cのうち第2の更新観測値生成部は、第1更新観測値の算出頻度よりも高い算出頻度で、第2の物標検出部の検出タイミングからの自車両の移動量に基づいて、第2の物標検出部の検出結果に基づく観測値を更新した第2更新観測値を算出する。
これにより、検出周期の異なる物標検出部10a~10cの検出結果が、自己位置Peの推定結果に与える影響の違いを低減できる。
1…自己位置推定装置、10、10a~10c…物標検出部、11…地図データベース、12…移動量検出部、13…コントローラ、20…予測部、21、21a~21c…更新観測値生成部、22…スイッチ、23…座標変換部、24…マッチング部、25…減算器、26…ゲイン設定部、27…乗算器、28…更新部

Claims (8)

  1. 自車両に対する前記自車両の周囲の物標の相対位置を検出し、予め記憶した前記物標の地図上の位置と前記相対位置とに基づき前記自車両の自己位置を推定する自己位置推定方法であって、
    前記自車両の移動量に基づいて前記自車両の自己位置の予測値を算出し、
    前記物標の前記相対位置の検出結果に基づく観測値を生成し、
    前記相対位置の検出タイミングにおいて、前記観測値に応じて前記予測値を補正して前記自車両の自己位置を推定し、
    前記相対位置の検出タイミングの合間において、前記検出タイミングからの前記自車両の移動量に基づいて前記観測値を更新した更新観測値を算出するとともに前記更新観測値に応じて前記予測値を補正して前記自車両の自己位置を推定する、
    ことを特徴とする自己位置推定方法。
  2. 前記観測値として前記物標の前記相対位置の観測値を生成し、
    前記観測値と前記予測値とに基づき前記物標の地図座標系上の第1位置を予測し、
    前記更新観測値と前記予測値とに基づき前記物標の地図座標系上の第2位置を予測し、
    前記相対位置の検出タイミングにおいて前記第1位置が前記予め記憶した前記物標の地図上の位置に近づくように前記予測値を補正し、前記相対位置の検出タイミングの合間において前記第2位置が前記予め記憶した前記物標の地図上の位置に近づくように前記予測値を補正する、
    ことを特徴とする請求項1に記載の自己位置推定方法。
  3. 前記観測値として前記自車両の自己位置の観測値を生成し、
    前記相対位置の検出タイミングにおいて観測値に近づくように前記予測値を補正し、前記相対位置の検出タイミングの合間において前記更新観測値に近づくように前記予測値を補正する、
    ことを特徴とする請求項1に記載の自己位置推定方法。
  4. 前記物標の前記相対位置をそれぞれ検出する複数のセンサの検出結果に基づいて、複数の前記観測値をそれぞれ生成し、
    前記複数のセンサのそれぞれの検出タイミングからの前記自車両の移動量に基づいて、前記複数のセンサの検出結果に基づく前記複数の観測値をそれぞれ更新した複数の前記更新観測値を算出する、
    ことを特徴とする請求項1~3の何れか一項に記載の自己位置推定方法。
  5. 前記複数の観測値毎に信頼度を定め、
    前記相対位置の検出タイミングにおいて、前記信頼度に応じて重み付けされた前記複数の観測値に応じて前記予測値を補正して前記自車両の自己位置を推定する、
    ことを特徴とする請求項4に記載の自己位置推定方法。
  6. 前記複数の更新観測値毎に信頼度を定め、
    前記相対位置の検出タイミングの合間において、前記信頼度に応じて重み付けされた前記複数の更新観測値に応じて前記予測値を補正して前記自車両の自己位置を推定する、
    ことを特徴とする請求項4に記載の自己位置推定方法。
  7. 前記複数のセンサは、第1周期で前記相対位置を検出する第1センサと、前記第1周期よりも長い第2周期で前記相対位置を検出する第2センサと、を含み、
    前記第1センサの検出タイミングからの前記自車両の移動量に基づいて、前記第1センサの検出結果に基づく前記観測値を更新した第1更新観測値を算出し、
    前記第1更新観測値の算出頻度よりも高い算出頻度で、前記第2センサの検出タイミングからの前記自車両の移動量に基づいて、前記第2センサの検出結果に基づく前記観測値を更新した第2更新観測値を算出する、
    ことを特徴とする請求項4~6のいずれか一項に記載の自己位置推定方法。
  8. 自車両に対する前記自車両の周囲の物標の相対位置を検出し、予め記憶した前記物標の地図上の位置と前記相対位置とに基づき前記自車両の自己位置を推定する自己位置推定装置であって、
    前記物標の前記相対位置を検出するセンサと、
    前記自車両の移動量を検出する移動量検出器と、
    前記移動量に基づいて前記自車両の自己位置の予測値を算出し、前記相対位置の検出タイミングにおいて、前記物標の前記相対位置の検出結果に基づく観測値に応じて前記予測値を補正して前記自車両の自己位置を推定し、前記相対位置の検出タイミングの合間において、前記検出タイミングからの前記自車両の移動量に基づいて前記観測値を更新した更新観測値を算出するとともに前記更新観測値に応じて前記予測値を補正して前記自車両の自己位置を推定するコントローラと、
    を備えることを特徴とする自己位置推定装置。
JP2019121087A 2019-06-28 2019-06-28 自己位置推定方法及び自己位置推定装置 Active JP7274366B2 (ja)

Priority Applications (1)

Application Number Priority Date Filing Date Title
JP2019121087A JP7274366B2 (ja) 2019-06-28 2019-06-28 自己位置推定方法及び自己位置推定装置

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
JP2019121087A JP7274366B2 (ja) 2019-06-28 2019-06-28 自己位置推定方法及び自己位置推定装置

Publications (2)

Publication Number Publication Date
JP2021006797A JP2021006797A (ja) 2021-01-21
JP7274366B2 true JP7274366B2 (ja) 2023-05-16

Family

ID=74174511

Family Applications (1)

Application Number Title Priority Date Filing Date
JP2019121087A Active JP7274366B2 (ja) 2019-06-28 2019-06-28 自己位置推定方法及び自己位置推定装置

Country Status (1)

Country Link
JP (1) JP7274366B2 (ja)

Citations (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JP2007303842A (ja) 2006-05-08 2007-11-22 Toyota Central Res & Dev Lab Inc 車両位置推定装置及び地図情報作成装置
CN105628026A (zh) 2016-03-04 2016-06-01 深圳大学 一种移动物体的定位定姿方法和系统
WO2017009934A1 (ja) 2015-07-13 2017-01-19 日産自動車株式会社 信号機認識装置及び信号機認識方法
WO2018020588A1 (ja) 2016-07-26 2018-02-01 日産自動車株式会社 自己位置推定方法及び自己位置推定装置

Patent Citations (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JP2007303842A (ja) 2006-05-08 2007-11-22 Toyota Central Res & Dev Lab Inc 車両位置推定装置及び地図情報作成装置
WO2017009934A1 (ja) 2015-07-13 2017-01-19 日産自動車株式会社 信号機認識装置及び信号機認識方法
CN105628026A (zh) 2016-03-04 2016-06-01 深圳大学 一种移动物体的定位定姿方法和系统
WO2018020588A1 (ja) 2016-07-26 2018-02-01 日産自動車株式会社 自己位置推定方法及び自己位置推定装置

Also Published As

Publication number Publication date
JP2021006797A (ja) 2021-01-21

Similar Documents

Publication Publication Date Title
US11740352B2 (en) Obstacle recognition device, vehicle system including the same, and method thereof
US9114810B2 (en) Preceding vehicle selection apparatus
US11460568B2 (en) Estimating in-plane velocity from an arbitrary radar return
US20140297093A1 (en) Autonomous vehicle and method of estimating self position of autonomous vehicle
CN108419441B (zh) 路面形状测定装置、测定方法及计算机可读存储介质
WO2014068924A1 (ja) 音波センサ、補正値設定装置、および距離検出装置
JP7155284B2 (ja) 計測精度算出装置、自己位置推定装置、制御方法、プログラム及び記憶媒体
US20190114490A1 (en) Information processing device, learned model, information processing method, and computer program product
JP7193414B2 (ja) 軸ずれ推定装置
US11993289B2 (en) Vehicle control system and vehicle control method
KR20200028648A (ko) 센서들에 대한 정렬 모델 조정 방법 및 그 방법을 수행하는 전자 장치
JP2019152575A (ja) 物体追跡装置、物体追跡方法及び物体追跡用コンピュータプログラム
JP2017144888A (ja) 推定装置
JP6333437B1 (ja) 物体認識処理装置、物体認識処理方法および車両制御システム
CN114026456A (zh) 追踪装置
JP7408236B2 (ja) 位置推定方法及び位置推定装置
JP7274366B2 (ja) 自己位置推定方法及び自己位置推定装置
US11841419B2 (en) Stationary and moving object recognition apparatus
US11754403B2 (en) Self-position correction method and self-position correction device
JP2022042771A (ja) 測距システム
US20220075053A1 (en) Method and apparatus for sensor data fusion for a vehicle
JP7412254B2 (ja) 物体認識装置および物体認識方法
CN118050707A (zh) 激光雷达的标定方法及装置、存储介质、终端设备
US20220227395A1 (en) Vehicle Control System and Vehicle Control Method
Richter et al. Advanced occupancy grid techniques for lidar based object detection and tracking

Legal Events

Date Code Title Description
A621 Written request for application examination

Free format text: JAPANESE INTERMEDIATE CODE: A621

Effective date: 20220204

A977 Report on retrieval

Free format text: JAPANESE INTERMEDIATE CODE: A971007

Effective date: 20221214

A131 Notification of reasons for refusal

Free format text: JAPANESE INTERMEDIATE CODE: A131

Effective date: 20230104

A521 Request for written amendment filed

Free format text: JAPANESE INTERMEDIATE CODE: A523

Effective date: 20230125

TRDD Decision of grant or rejection written
A01 Written decision to grant a patent or to grant a registration (utility model)

Free format text: JAPANESE INTERMEDIATE CODE: A01

Effective date: 20230404

A61 First payment of annual fees (during grant procedure)

Free format text: JAPANESE INTERMEDIATE CODE: A61

Effective date: 20230501

R150 Certificate of patent or registration of utility model

Ref document number: 7274366

Country of ref document: JP

Free format text: JAPANESE INTERMEDIATE CODE: R150