JP2020168903A - 座標データ補正装置およびこれを用いた移動体追従装置並びに移動体追従方法 - Google Patents

Abstract

【課題】移動体の動きに対し、正確な追従を行うことができる座標データ補正装置を得る。
【解決手段】座標データ入力部３に自車を基準にした移動体の相対位置が入力され、フィルタ処理部４により、平滑化処理される一方で、自車状態量検出部３により車速およびヨーレートを含む自車状態量が検出され、自車移動量演算部１４により、自車の移動量を演算して、これを基にして、自車座標系移動体履歴記憶部１５が、移動体の相対位置を、自車基準座標系に変換して、記憶し、次いで、自車座標系移動体履歴曲率演算部１６により、移動体履歴の曲率を演算して、この曲率に基づき、自車座標系移動体履歴補正部１７により、移動体履歴を、平滑化処理される前の曲率に近づくように補正して、自車の操舵を制御する制御部７へ出力する。
【選択図】図１

Description

本願は、移動体の追従に用いられる座標データ補正装置およびこれを用いた移動体追従装置並びに移動体追従方法に関するものである。

従来の移動体追従装置は、例えば、自動車等の車両の運転支援制御技術に属する。従来の移動体追従装置では、車両に搭載されたレーダーセンサ、ビデオカメラ、ＧＰＳ（Ｇｌｏｂａｌ Ｐｏｓｉｔｉｏｎｉｎｇ Ｓｙｓｔｅｍ）装置などが検出する先行車（車両の前方を走行する車両）の位置、速度または走行軌跡に基づいて、先行車に追従して車両の走行を制御する（先行車追従制御）。
従来の移動体追従装置としては、例えば、特許文献１がある。

特許文献１に記載された従来の移動体追従装置では、センサによる検出信号に基づき自車両前方の追従対象の自車両に対する相対位置を対象位置として算出するとともに、算出した対象位置を時間軸上で複数回取得し、取得した対象位置をバッファメモリにバッファリングする。そして、バッファリングした複数の対象位置に基づき、追従対象の移動軌跡を推定する。
この移動軌跡の推定で、現在の自車両位置よりも後方に位置していた追従対象について算出された対象位置が、バッファリングされている状態で、移動軌跡が推定されるように、自車両と追従対象との離間距離と自車速とに基づいて、対象位置の取得周期を調整し、移動体の履歴を保存するようにしている。

特開２０１７―６５４７３号公報（第４〜７頁、第１図）

特許文献１に記載された従来の移動体追従装置では、移動体の履歴を等間隔で取得するように取得周期を調整することのみが記載されている。
このため、取得された移動体履歴情報に対する一般的な平滑化処理（ローパスフィルタまたは移動平均処理）を行った履歴に基づいて、移動体に追従制御しようとする場合、カーブ走行時は、移動体が通過した履歴と異なるものとなってしまい、移動体が実際に通過した経路を通れない場面が出てくるという問題があった。

本願は、上記のような課題を解決するための技術を開示するものであり、移動体の動きに対し、正確な追従を行うことができる座標データ補正装置およびこれを用いた移動体追従装置並びに移動体追従方法を提供することを目的とする。

本願に開示される座標データ補正装置は、ある点を基点とする座標系における、追従対象の移動体の座標データが入力される座標データ入力部、この座標データ入力部により入力された座標データを平滑化処理するフィルタ処理部、このフィルタ処理部により平滑化処理された座標データを、移動体の履歴として３点以上記憶する座標データ記憶部、この座標データ記憶部に記憶された平滑化処理された３点以上の座標データから移動体の履歴の曲率を演算し、実際の移動体の履歴の曲率に近づけるように、移動体の履歴を補正する補正部を備えたものである。

本願に開示される座標データ補正装置によれば、移動体の動きに対し、正確な追従を行うことができる。

実施の形態１による座標データ補正装置の概略構成を示すブロック図である。 実施の形態１による座標データ補正装置の動作を示すフローチャートである。 実施の形態１による座標データ補正装置の自車移動量の演算処理を示すフローチャートである。 実施の形態１による座標データ補正装置の移動体の過去履歴の自車座標系への変換処理を示すフローチャートである。 実施の形態１による座標データ補正装置の移動体履歴の生成過程を示す図である。 実施の形態１による座標データ補正装置のフィルタ処理の効果を示す説明図である。 実施の形態１による座標データ補正装置の移動体履歴の補正の効果を示す説明図である。 実施の形態１による座標データ補正装置のハードウェア構成を示す図である。 実施の形態２による座標データ補正装置の概略構成を示すブロック図である。 実施の形態２による座標データ補正装置の動作を示すフローチャートである。

実施の形態１．
以下、実施の形態１による座標データ補正装置について、図を用いて説明する。
図１は、実施の形態１による座標データ補正装置の概略構成を示すブロック図である。
図１において、座標データ補正装置１は、それぞれ後述する自車状態量検出部２と、座標データ入力部３と、フィルタ処理部４と、座標データ記憶部５と、補正部６とを有し、補正部６で補正された追従対象の移動体の座標データに基づく制御量を制御部７に出力する。
なお、以下の説明において、「自車」とは、座標データ補正装置を搭載した車両を指している。

自車状態量検出部２は、自車の走行状態を示す各種の自車状態量を取得し、取得した自車状態量を座標データ記憶部５に伝送する。実施の形態１では、自車状態量検出部２は、後述する自車速検出部１１およびヨーレート検出部１２を有する。
自車速検出部１１は、自車の走行速度を検出し、検出した走行速度に応じた信号を座標データ記憶部５へ出力する。
ヨーレート検出部１２は、自車の旋回方向への回転角の変化速度であるヨーレートを検出し、検出したヨーレートに応じた信号を座標データ記憶部５へ出力する。
なお、座標データ記憶部５に入力される自車状態量は、自車の走行速度、ヨーレートがあれば、他の信号があってもかまわない。
また、本実施の形態１においては、座標データ記憶部５にて制御周期間で得られたヨーレートをヨー角変化量に演算するため、ヨーレートの変わりに、ＧＰＳなど、自車のヨー角変化量が取得できるものであればよい。

座標データ入力部３は、移動体相対位置検出部１３を有し、この移動体相対位置検出部１３で検出された信号をフィルタ処理部４へ出力する機能を有している。
移動体相対位置検出部１３は、自車を基準とした場合の移動体の相対縦位置、相対横位置を検出し、その位置に応じた信号をフィルタ処理部４へ出力する。なお、フィルタ処理部４を必ず通過する必要はないので、直接、座標データ記憶部５に出力するようにしてもよい。

移動体相対位置検出部１３は、例えば、自車に搭載されたカメラにより撮影された自車前方の画像から、自車を基準とした、対象とする移動体の相対位置を検出する。カメラの他にも、ミリ波レーダあるいはレーザレーダ等、自車から見た移動体の相対位置が取得できるものでもよいし、ＧＰＳなどを自車基準座標系に変換して相対位置として取得するものでもよい。
移動体相対位置検出部１３により検出された移動体の相対位置は、自車の現在位置を基準とする固定座標系（自車固定座標系）で表されるものとする。
ここで、自車の進行方向をＸ方向（前方を正、後方を負とする）、横方向をＹ方向（右方を正、左方を負とする）と定義する。また、ヨーレートなど、自車の旋回の向きは、時計回りを正、反時計回りを負と定義する。なお、自車固定座標系では、自車座標は常に原点（０，０）となる。

フィルタ処理部４は、座標データ入力部３から出力される信号に対し、フィルタ処理（平滑化処理）を行なう。フィルタ処理部４は、各種フィルタ処理を行う機能を持ち、座標データ記憶部５に対して、フィルタ処理した信号を出力する機能を持つ。
フィルタ機能は、必要に応じてバイパス可能であるため、フィルタ処理後の信号およびフィルタ未処理の信号を同時に出力することができる。
具体的なフィルタ機能として、一般的に用いられるハイパス・ローパスフィルタ、移動平均などのフィルタなどを使用することができる。本実施の形態１では、使用するフィルタの仕様は、既知である必要がある。

座標データ記憶部５は、後述する自車移動量演算部１４、自車座標系移動体履歴記憶部１５を有し、自車基準座標系の移動体履歴情報を補正部６に出力する機能を有している。座標データ記憶部５には、３点以上の自車基準座標系（自車を基準とする座標系）の移動体の座標データが、移動体履歴情報として記憶されている。

自車移動量演算部１４は、前回演算周期から今回演算周期までの時間（以後、「演算周期」という。実施の形態１では、１００ｍｓｅｃ）における自車移動量を演算する。
自車座標系移動体履歴記憶部１５は、自車移動量演算部１４で演算された自車基準座標系での所定の範囲の移動体履歴データを保持し、次回制御周期（今回の例では、１００ｍｓｅｃ後）に引き渡す機能を持つ。自車座標系移動体履歴記憶部１５は、移動体履歴データ（座標データ）を、自車基準座標系での移動体履歴データに変換して保持する。

補正部６は、後述する自車座標系移動体履歴曲率演算部１６および自車座標系移動体履歴補正部１７を有し、補正処理された移動体履歴データを制御部７に出力する機能を有している。
自車座標系移動体履歴曲率演算部１６は、移動体履歴データがどの程度の曲率になっているかを演算する機能を持つ。

自車座標系移動体履歴補正部１７は、フィルタ処理部４の処理が実行され、自車座標系移動体履歴記憶部１５で保持されている移動体履歴を補正する機能を持つ。また、バイパス機能も合わせて有しており、移動体履歴の曲率が閾値未満の場合、補正を実施しなくてもよい。

制御部７は、補正部６で演算された移動体履歴に基づく目標値を演算し、例えば操舵用の動力を発生させ、自車を目標値に追従するように操舵制御を行う。
操舵制御の手法として、例えば、電動式パワーステアリングの電動モータ（種類は特に限定されず、直流、交流を問わない）、または油圧式パワーステアリングの油圧ポンプを用いることができる。
本実施の形態１では、制御部７として、電動パワーステアリング装置をベースとしているが、操舵を制御できるものであればよく、例えば、ハンドルと転舵輪との間の機械的リンクを廃したステアリングバイワイヤと称される操舵装置であってもよい。

図５は、実施の形態１による座標データ補正装置の移動体履歴の生成過程を示す図であり、自車固定座標系から見た、移動体の相対位置を記憶しながら、演算周期毎に自車基準座標系へ変換する様子を示している。順次、アフィン変換（座標軸の回転＋移動）を行いながら相対位置を記憶する。
図５において、ｘ_１，ｙ_１軸〜ｘ_ｎ，ｙ_ｎ軸は、演算周期毎の自車基準座標系を示している。記号○（ｘ’_１，ｙ’_１）〜（ｘ’_ｎ，ｙ’_ｎ）は、演算周期毎の自車基準座標系から見た移動体の相対位置を示している。
例えば、ｘ_ｎ，ｙ_ｎ軸における演算周期で検出された移動体相対位置は（ｘ’_ｎ，ｙ’_ｎ）となり、ｘ_ｎ−１，ｙ_ｎ−１軸における演算周期で検出された移動体相対位置は（ｘ’_ｎ−１，ｙ’_ｎ−１）となる。

ｘ_ｎ，ｙ_ｎ軸をｘ_ｎ−１，ｙ_ｎ−１軸に変換する際、後述する式（１）の座標変換を行うことで、ｘ_ｎ，ｙ_ｎ軸で検出された移動体履歴についても、ｘ_ｎ−１，ｙ_ｎ−１軸から見た場合の相対位置として記憶されることとなる。

この手順を繰り返すことで、最終的には今回演算周期の自車固定座標系であるｘ_１，ｙ_１軸から見た移動体の過去履歴（ｘ’_１，ｙ’_１）〜（ｘ’_ｎ，ｙ’_ｎ）として記憶することができる。メモリは有限であるため、記憶する履歴数も有限とし、その値を超えた場合は古い値を削除し、新しい値を入れ込むＦＩＦＯ（Ｆｉｒｓｔ−Ｉｎ Ｆｉｒｓｔ−Ｏｕｔ）などの考え方を採用すればよい。

図６は、実施の形態１による座標データ補正装置のフィルタ処理の効果を示す説明図である。
図６において、自車３０と、先行車である移動体４０が示されている。
図中の記号〇は、フィルタ処理無しの移動体履歴の各座標データを示し、移動体履歴は、センサ取得値の曲線半径Ｒ１を有する。記号□は、フィルタ処理にローパスフィルタを用いた場合の移動体履歴の各座標データを示し、移動体履歴は、ローパスフィルタ処理後の曲線半径Ｒ２を有する。記号△は、フィルタ処理に移動平均を用いた場合の移動体履歴の各座標データを示し、移動体履歴は、移動平均処理後の曲線半径Ｒ３を有する。

図７は、実施の形態１による座標データ補正装置の移動体履歴の補正の効果を示す説明図である。
図７において、符号３０および記号〇、△並びにＲ１、Ｒ３は図６におけるものと同一のものである。図７中、補正量ΔＲが示されている。

次に、動作について説明する。
まず、自車移動量演算部１４の動作について説明する。
自車移動量演算部１４の具体的な演算として、自車状態量検出部２で検出される、自車速に基づく自車移動量と、ヨーレートに基づく演算周期間の自車旋回角度の変化量（ヨー角変化量）に基づき、式（１）に記載された座標変換を行う。
式（１）において、ｓｘ_ｎ、ｓｙ_ｎは、演算周期間の縦方向、横方向の自車移動量をそれぞれ表す。
［ｘ_ｎｙ_ｎ］^Ｔは、前回制御周期の自車固定座標系から見た、移動体相対位置検出部１３で検出された移動体の相対位置であり、［ｘ_ｎ−１ｙ_ｎ−１］^Ｔは、今回制御周期の自車固定座標系から見た、［ｘ_ｎｙ_ｎ］^Ｔとなる。θは今回演算周期におけるヨーレートを積分した値であり、自車固定座標系から見た自車のヨー角を表す。

演算周期毎にこの座標変換を行うことで、常に自車座標を（０，０）とした移動体の過去履歴を表現できる。

式（１）の、ｓｘ_ｎ、ｓｙ_ｎは、各演算周期において自車が停車している、または自車状態量検出部２が検知できないほど小さい変動だった場合、ともに０となる。ヨー角変化量θについても同様となる。

次に、自車座標系移動体履歴記憶部１５が、自車移動量演算部１４により演算された自車基準座標系での所定の範囲の移動体履歴データを保持し、次回制御周期（今回の例では、１００ｍｓｅｃ後）に引き渡す。

ここで、自車座標系移動体履歴記憶部１５に保持されている所定範囲の移動体履歴データについて説明する。
自車座標系移動体履歴記憶部１５には、フィルタ処理部４でフィルタ処理された移動体履歴データと、フィルタ処理されていない移動体履歴データが保持されている。
フィルタ処理を行う移動体履歴データ側にフィルタ処理を行わなかった場合、２つの移動体履歴データは同一となる。

次に、フィルタ処理を行う場合と、行わない場合の違いについて、図６を用いて説明する。
まず、フィルタ処理無しの移動体履歴の状態について説明する。
移動体は、車速４０ｋｍ／ｈで、曲線半径Ｒ１＝１２５．９７［ｍ］を走行している状態（横加速度０．９８ｍ／ｓ^２相当）である。この状態で得られる、図６の記号〇が移動体の走行履歴を表している。

次に、記号△で表される移動平均は、１０点分の処理を行っている。この点数については限定するものではなく、必要に応じて変更してよい。
移動平均そのものについても、（移動体履歴の総和（縦方向、横方向））／（移動平均数）で表わされる一般的なものである。

同様に、記号□で表されるローパスフィルタは、カットオフ周波数４Ｈｚ，サンプリング時間１秒として処理を行っている。移動平均と同様、特段に限定しているものではなく、ハイパスフィルタなど、仕様がわかるものであれば何でもよい。

今回の事例について総合的な誤差を計算すると、フィルタ処理無しの経路に対し、移動平均処理を行った場合、約０．５５ｍの誤差（曲線半径としては約４ｃｍ内側の経路）が発生し、ローパスフィルタの処理を行った場合、約０．５０ｍの誤差（曲線半径としては約４ｃｍ外側の経路）となる。
このように、フィルタ処理の内容に応じて、本来移動体が通過した経路と異なる経路になることがわかる。

本実施の形態１では、車速、横加速度の一例を示したが、もちろん、車速・横加速度・過去履歴データから得られた曲率に対して可変である。

次に、移動体履歴に対する補正部６による補正処理について説明する。
ここまでの手順を行うことで、現在演算周期における自車基準座標系での移動体履歴を得られたので、ここから実際の移動体履歴に近づけるための、フィルタ処理後の移動体履歴の補正処理について説明する。

補正部６の自車座標系移動体履歴曲率演算部１６は、移動体履歴データがどの程度の曲率になっているかを演算する。この曲率を求めるためには、曲率項を持つ二次または三次などの関数で近似する必要があるが、曲率が分かれば、必ずしも関数近似をする必要はない。
曲率項を求めるための関数近似に関しては、一般的な近似手法として、よく知られている最小二乗法などを用いればよい。最小二乗法により移動体履歴データを二次または三次式で表すことができる。
二次式で近似した場合は「ｙ（ｘ）＝ａｘ^２＋ｂｘ＋ｃ」として表され、ａが曲率項に該当する。また、三次式で近似した場合は「ｙ（ｘ）＝ａｘ^３＋ｂｘ^２＋ｃｘ＋ｄ」としてあらわされ、ｂが曲率項に該当する。

補正部６の自車座標系移動体履歴補正部１７は、フィルタ処理部４の処理が実行され、自車座標系移動体履歴記憶部１５で保持されている移動体履歴を補正する。自車座標系移動体履歴補正部１７は、バイパス機能も合わせて有しており、移動体履歴の曲率が閾値未満の場合、補正を実施しなくてもよい。

次に、移動体履歴の補正を実施する手段として、自車座標系移動体履歴記憶部１５で保持されている移動体履歴を直接補正する方法と、移動体履歴の曲率に相当する値を補正する方法について説明する。

まず、移動体履歴を直接補正する場合について説明する。
自車座標系移動体履歴記憶部１５で保持されている移動体履歴には、フィルタ処理を行っている移動体履歴と、フィルタ処理を行っていない移動体履歴との２つが常に存在することとなる。（フィルタ処理部４の処理をバイパスした場合、２つのフィルタ処理を行っていない移動体履歴が、自車座標系移動体履歴記憶部１５で保持されている状態となる。）

自車座標系移動体履歴曲率演算部１６で、フィルタ処理を行った移動体履歴の曲率が演算され、フィルタ処理仕様および自車速情報から曲率項の補正量が求まる。
その結果から、フィルタ処理を行った移動体履歴の曲線半径と、その曲線半径から移動体履歴の各点が垂直になるような線を求め、その関係を維持したまま曲線半径の補正量分だけ移動させることで、フィルタ処理を行う前の移動体履歴と同じ曲率情報を持った、フィルタ処理後の移動体履歴を得ることができる。図７に示すような結果になる。
すなわち、移動体履歴の座標データの各点を直接補正する方法である。

次に、求められた曲率項に対して補正を行う方法について述べる。
移動体履歴そのものを補正する方法については、上述のとおりであるが、曲率項に対して補正を行う場合は、フィルタ処理を行った移動体履歴の曲率と、曲率の補正量はすでに求められているので、上述の法線の関係などを用いずに、直接、曲率に対して補正値を加算すればよい。
具体的には、図７に記載しているとおり、移動平均後の曲線半径Ｒ３が求まっており、移動平均の補正量も求まっているので、図７に示す補正量ΔＲを加算する。この方法により、本来得られる正確な曲率項を得ることができる。

次に、図２のフローチャートに基づき、実施の形態１による座標データ補正装置の動作について説明する。

座標データ補正装置は、まず、移動体相対位置検出部１３により、今回制御周期における、移動体相対位置を取得する（ステップＳ１１１、第一のステップ）。

次に、取得した相対位置に対し、必要に応じて、フィルタ処理部４によるフィルタ処理を行う（ステップＳ１１２、第二のステップ）。

次に、後述する図３のステップＳ１２２で演算された自車移動量に基づき、現在記憶されている移動体の過去履歴を今回制御周期における、自車基準座標系に変換して、記憶する（ステップＳ１１３、第三のステップ）。

次に、今回制御周期における、自車基準座標系に変換された移動体の過去履歴に基づき、自車座標系移動体履歴曲率演算部１６による曲率演算を行う（ステップＳ１１４）。
この演算手法としては、一般的によく知られている最小法二乗法などを実施すればよい。また、曲率を得るためには二次、または三次式による近似を行えばよい。

次に、ステップＳ１１４で演算された曲率に該当する項が閾値以上であれば、自車座標系移動体履歴補正部１７による移動体履歴の補正を行うステップＳ１１６へ進み、閾値未満であれば、そのまま今回制御周期における処理は終了となる（ステップＳ１１５）。

次に、移動体履歴の補正が必要と判断された場合、自車座標系移動体履歴補正部１７により、後述する図４のステップＳ１３１で実施されたフィルタ処理が行われていない移動体履歴の曲率に近づくように、移動体履歴の各点を直接補正（座標データ補正）、または曲率項に該当する数値に対して、補正を行う処理（座標データ曲率補正）が実施される（ステップＳ１１６、第四のステップ）。

ステップＳ１１６で最終的に演算された結果を制御部７に出力し、今回制御周期の処理を終了する。制御部７は、ステップＳ１１６で出力された演算結果に基づき、移動体を追従するための自車の操舵を制御する（第五のステップ）。

次に、図３のフローチャートに基づき、自車移動量演算部１４による自車移動量の演算処理について説明する。
まず、自車状態量検出部２により、今回制御周期における、自車の状態量を取得する（ステップＳ１２１）。

次に、取得された自車状態量に基づく、前回制御周期から今回制御周期までの自車移動量を、自車移動量演算部１４により、演算する（ステップＳ１２２）。

ステップＳ１２２で演算された結果を、図２のステップＳ１１３および後述する図４のステップＳ１３１に渡し、今回制御周期における処理を終了する。

最後に、図４のフローチャートに基づき、自車座標系移動体履歴記憶部１５による、移動体の過去履歴の自車基準座標系への変換処理について説明する。

まず、移動体相対位置検出部１３により、今回制御周期における、移動体相対位置を取得する（ステップＳ１１１）。

次に、上述した図３のステップＳ１２２で演算された自車移動量に基づき、現在記憶されている、フィルタ処理が行われていない移動体の過去履歴を、今回制御周期における自車基準座標系に変換する（ステップＳ１３１）。
自車基準座標系に変換された今回制御周期における移動体の過去履歴を、図２のステップＳ１１６に渡し、今回制御周期における処理を終了する。

次に、座標データ補正装置のハードウェア構成について説明する。
図８は、処理回路１０１がプロセッサを用いて構成されている場合における座標データ補正装置１のハードウェア構成を示している。この場合、座標データ補正装置１の各要素の機能は、ソフトウェア等（ソフトウェア、ファームウェア、またはソフトウェアとファームウェア）との組み合わせにより実現される。
ソフトウェア等はプログラムとして記述され、メモリ１０３に格納される。処理回路１０１としてのプロセッサ１０２は、メモリ１０３に記憶されたプログラムを読み出して実行することにより、各部の機能を実現する。
すなわち、座標データ補正装置１は、処理回路１０１により実行されるときに、座標データ補正装置１を構成する各要素の機能を実行するためのプログラムを格納するメモリ１０３を有している。
換言すれば、このプログラムは、座標データ補正装置１の各要素の動作の手順、方法をコンピュータに実行させるものであるともいえる。

ここで、メモリ１０３は、例えば、ＲＡＭ（Ｒａｎｄｏｍ Ａｃｃｅｓｓ Ｍｅｍｏｒｙ）、ＲＯＭ（Ｒｅａｄ Ｏｎｌｙ Ｍｅｍｏｒｙ）、フラッシュメモリー、ＥＰＲＯＭ（Ｅｒａｓａｂｌｅ Ｐｒｏｇｒａｍｍａｂｌｅ Ｒｅａｄ Ｏｎｌｙ Ｍｅｍｏｒｙ）、ＥＥＰＲＯＭ（Ｅｌｅｃｔｒｉｃａｌｌｙ Ｅｒａｓａｂｌｅ Ｐｒｏｇｒａｍｍａｂｌｅ Ｒｅａｄ Ｏｎｌｙ Ｍｅｍｏｒｙ）などの、不揮発性または揮発性の半導体メモリ、ＨＤＤ（Ｈａｒｄ Ｄｉｓｋ Ｄｒｉｖｅ）、磁気ディスク、フレキシブルディスク、光ディスク、コンパクトディスク、ミニディスク、ＤＶＤ（Ｄｉｇｉｔａｌ Ｖｅｒｓａｔｉｌｅ Ｄｉｓｃ）およびそのドライブ装置等、または、今後使用されるあらゆる記憶媒体であってもよい。

以上、座標データ補正装置１の各要素の機能が、ハードウェアおよびソフトウェア等のいずれか一方で実現される構成について説明した。
しかし、これに限ったものではなく、座標データ補正装置１の一部の要素を専用のハードウェアで実現し、別の一部の要素をソフトウェア等で実現する構成であってもよい。
例えば、一部の要素については専用のハードウェアとしての処理回路１０１でその機能を実現し、他の一部の要素については、プロセッサ１０２としての処理回路がメモリ１０３に格納されたプログラムを読み出して実行することによって、その機能を実現することが可能である。

以上のように、座標データ補正装置１は、ハードウェア、ソフトウェア等、またはこれらの組み合わせによって、上述の各機能を実現することができる。

実施の形態１によれば、従来の移動体追従装置と比べて、特に車両のカーブ走行時における移動体履歴誤差を低減することができ、縁石への衝突または路肩への乗揚げを防止し、移動体追従時の安全性を向上させることができる。
また、フィルタ処理による移動体履歴の補正を行い、移動体の動きに対し正確な追従を行うことができる。

実施の形態２．
図９は、実施の形態２による座標データ補正装置の概略構成を示すブロック図である。
図９において、符号１、３〜７は図１におけるものと同一のものである。
図９では、座標データ入力部３は、移動体の緯度経度を検出する緯度経度情報検出部１８を有する。座標データ記憶部５は、緯度経度座標を記憶する緯度経度座標系緯度経度座標記憶部１９を有する。補正部６は、移動体の緯度経度の曲率を演算する緯度経度座標系緯度経度曲率演算部２０と、緯度経度座標系緯度経度曲率演算部２０により演算された移動体の緯度経度の曲率を補正する緯度経度座標系緯度経度曲率補正部２１とを有する。

実施の形態２は、実施の形態１で述べたような自車基準座標系ではなく、移動体の緯度経度を用いて、緯度経度座標の補正を行なうようにしたものである。すなわち、座標系を、緯度経度を基準とする座標系としている。
緯度経度情報検出部１８により、直接、移動体の緯度経度を検出し、その緯度経度の検出情報に基づいて、フィルタ処理を行い、フィルタ処理後の値を記憶する。
そして、記憶したフィルタ処理後の値に対して、曲率を演算し、曲率または緯度経度座標に対し、実施の形態１と同様の補正を行うようにした。

次に、図１０のフローチャートを用いて、実施の形態２の移動体の座標データの補正について説明する。
まず、緯度経度情報検出部１８により、今回制御周期における、移動体の緯度経度情報を取得する（ステップＳ２１１）。

次に、取得した移動体の緯度経度情報に対し、必要に応じて、フィルタ処理部４によるフィルタ処理を行う。（行う場合、ステップＳ２１２を実行）。

次に、緯度経度座標系緯度経度座標記憶部１９により、移動体の緯度経度情報を、記憶する（ステップＳ２１３）。

次に、今回制御周期における、移動体の過去の緯度経度情報に基づき、緯度経度座標系緯度経度曲率演算部２０による曲率の演算を行う（ステップＳ２１４）。

次に、ステップＳ２１４で演算された曲率に該当する項が閾値以上であれば、緯度経度座標系緯度経度曲率補正部２１により補正を行なうステップＳ２１６へ進み、閾値未満であれば、そのまま今回制御周期における処理を終了する（ステップＳ２１５）。

次に、曲率の補正が必要と判断された場合、緯度経度座標系緯度経度曲率補正部２１により、緯度経度情報（座標データ）に基づく補正、または曲率項に該当する数値に対して、直接、補正を行う処理が実施される（ステップＳ２１６）。

ステップＳ２１６で最終的に演算された結果を制御部７に出力し、今回制御周期の処理を終了する。

実施の形態２によれば、移動体について、適切な曲率の緯度経度情報を得ることができる。

本開示は、様々な例示的な実施の形態及び実施例が記載されているが、１つ、または複数の実施の形態に記載された様々な特徴、態様、及び機能は特定の実施の形態の適用に限られるのではなく、単独で、または様々な組み合わせで実施の形態に適用可能である。
従って、例示されていない無数の変形例が、本願明細書に開示される技術の範囲内において想定される。例えば、少なくとも１つの構成要素を変形する場合、追加する場合または省略する場合、さらには、少なくとも１つの構成要素を抽出し、他の実施の形態の構成要素と組み合わせる場合が含まれるものとする。

１ 座標データ補正装置、２ 自車状態量検出部、３ 座標データ入力部、
４ フィルタ処理部、５ 座標データ記憶部、６ 補正部、７ 制御部、
１１ 自車速検出部、１２ ヨーレート検出部、１３ 移動体相対位置検出部、
１４ 自車移動量演算部、１５ 自車座標系移動体履歴記憶部、
１６ 自車座標系移動体履歴曲率演算部、１７ 自車座標系移動体履歴補正部、
１８ 緯度経度情報検出部、１９ 緯度経度座標系緯度経度座標記憶部、
２０ 緯度経度座標系緯度経度曲率演算部、２１ 緯度経度座標系緯度経度曲率補正部、
３０ 自車、４０ 移動体、１０１ 処理回路、１０２ プロセッサ、１０３ メモリ

本願に開示される座標データ補正装置は、自車に搭載された座標データ補正装置であって、ある点を基点とする座標系における、追従対象の移動体の座標データが入力される座標データ入力部、この座標データ入力部により入力された座標データを平滑化処理するフィルタ処理部、このフィルタ処理部により平滑化処理された座標データを、移動体の履歴として３点以憶する座標データ記憶部、この座標データ記憶部に記憶された平滑化処理された３点以上の座標データから移動体の履歴の曲率を演算し、フィルタ処理部におけるフィルタ処理仕様から求まる曲率の補正量を用いて、実際の移動体の履歴の曲率に近づけるように、移動体の履歴を補正する補正部を備えたものである。

Claims (17)

1. ある点を基点とする座標系における、追従対象の移動体の座標データが入力される座標データ入力部、
   この座標データ入力部により入力された上記座標データを平滑化処理するフィルタ処理部、
   このフィルタ処理部により平滑化処理された上記座標データを、上記移動体の履歴として３点以上記憶する座標データ記憶部、
   この座標データ記憶部に記憶された上記平滑化処理された３点以上の上記座標データから上記移動体の履歴の曲率を演算し、実際の上記移動体の履歴の曲率に近づけるように、上記移動体の履歴を補正する補正部を備えたことを特徴とする座標データ補正装置。
2. 上記座標データ記憶部は、上記平滑化処理された上記座標データに加えて、上記平滑化処理される前の上記座標データを記憶することを特徴とする請求項１に記載の座標データ補正装置。
3. 上記補正部による上記移動体の履歴の補正は、上記座標データの各点の補正により行なうことを特徴とする請求項１に記載の座標データ補正装置。
4. 上記補正部による上記移動体の履歴の補正は、上記曲率の補正により行なうことを特徴とする請求項１に記載の座標データ補正装置。
5. 上記補正部による上記移動体の履歴の補正は、当該移動体の履歴の曲率が、予め定めた閾値以上の場合に行なうことを特徴とする請求項１から請求項４のいずれか一項に記載の座標データ補正装置。
6. 上記座標系は、上記座標データ補正装置が搭載された自車を基準とする座標系であることを特徴とする請求項１から請求項５のいずれか一項に記載の座標データ補正装置。
7. 上記座標系に基づき、自車の車速およびヨーレートを含む自車状態量を検出する自車状態量検出部を備えたことを特徴とする請求項６に記載の座標データ補正装置。
8. 上記座標データ記憶部は、上記自車状態量から演算された自車移動量に基づいて、上記移動体の座標データを、上記自車を基準とする座標系に変換して記憶することを特徴とする請求項７に記載の座標データ補正装置。
9. 上記座標系は、緯度経度を基準とする座標系であることを特徴とする請求項１から請求項５のいずれか一項に記載の座標データ補正装置。
10. 請求項１から請求項９のいずれか一項に記載の座標データ補正装置、
    この座標データ補正装置によって補正された上記移動体の履歴に基づき、上記移動体を追従するように、上記座標データ補正装置が搭載された自車の操舵を制御する制御部を備えたことを特徴とする移動体追従装置。
11. 座標データ入力部に、ある点を基点とする座標系における、追従対象の移動体の座標データが入力される第一のステップ、
    この第一のステップにより入力された上記座標データを、フィルタ処理部が平滑化処理する第二のステップ、
    この第二のステップにより平滑化処理された上記座標データを、座標データ記憶部が上記移動体の履歴として３点以上記憶する第三のステップ、
    補正部が、上記第三のステップで記憶された上記平滑化処理された３点以上の上記座標データから上記移動体の履歴の曲率を演算し、実際の上記移動体の履歴の曲率に近づけるように、上記移動体の履歴を補正する第四のステップ、
    この第四のステップにより補正された上記移動体の履歴に基づき、制御部が、上記移動体を追従するように、自車の操舵を制御する第五のステップを含むことを特徴とする移動体追従方法。
12. 上記第三のステップは、上記第二のステップで平滑化処理された上記座標データに加えて、上記平滑化処理される前の上記座標データを記憶することを特徴とする請求項１１に記載の移動体追従方法。
13. 上記第四のステップでの上記移動体の履歴の補正は、上記座標データの各点の補正により行なうことを特徴とする請求項１１に記載の移動体追従方法。
14. 上記第四のステップでの上記移動体の履歴の補正は、上記曲率の補正により行なうことを特徴とする請求項１１に記載の移動体追従方法。
15. 上記第四のステップでの上記移動体の履歴の補正は、当該移動体の履歴の曲率が、予め定めた閾値以上の場合に行なうことを特徴とする請求項１１から請求項１４のいずれか一項に記載の移動体追従方法。
16. 上記第一のステップにおける座標系は、自車を基準とする座標系であることを特徴とする請求項１１から請求項１５のいずれか一項に記載の移動体追従方法。
17. 上記第一のステップにおける座標系は、緯度経度を基準とする座標系であることを特徴とする請求項１１から請求項１５のいずれか一項に記載の移動体追従方法。

Images