JP3932606B2

JP3932606B2 - Guidance device

Info

Publication number: JP3932606B2
Application number: JP17791097A
Authority: JP
Inventors: 宗訓大島; 直道藤永
Original assignee: 神鋼電機株式会社
Priority date: 1997-07-03
Filing date: 1997-07-03
Publication date: 2007-06-20
Anticipated expiration: 2017-07-03
Also published as: JPH1144550A

Abstract

PROBLEM TO BE SOLVED: To provide a guide unit for estimating the position and the attitude of a vehicle through a simple processing using minimum number of measuring means without being restricted by the traveling route. SOLUTION: A vehicle is controlled through a control section 6 to travel basically along a traveling route based on position/attitude values estimated at a position/attitude estimating section 4 using outputs from a steering angle sensor 1 and a moving speed sensor 2. Every time when a lateral shift from a magnetic mark being put on the traveling route at an arbitrary interval is detected by a magnetic sensor 3, a correcting section 5 corrects the position/ attitude values based on the position of the magnetic mark stored at a map information memory section 7, a lateral shift obtained from the magnetic sensor 3, and the position/attitude values.

Description

【０００１】
【発明の属する技術分野】
本発明は，車両に取り付けられた各種測定手段から得られた情報に基づいて車両の現在位置及び姿勢を推定し，車両を所定の走行経路に沿って走行させるための誘導装置に関するものである。
【０００２】
【従来の技術】
車両に取り付けられた各種測定手段から得られた情報に基づいて車両の現在位置及び姿勢を推定し，その推定値を利用して車両を所定の走行経路に従って誘導する推測航法として，従来より，車両の進行方向と移動距離を計測し積分することによって車両の推定位置を推定する航法と，ＧＰＳやビーコンなどを用いた電波航法とを組み合わせてそれぞれの欠点を補うことにより上記推定値の精度を高めるハイブリッド航法が用いられている。その一例として，例えば特開平０６−３４３７９号公報に提案されている方法がある。
この方法は，各測定手段（地磁気センサ，角速度センサ，距離センサ，位置センサ）のそれぞれの誤差を補正するため，１つの測定手段の出力誤差を他の測定手段の出力を用いて補正し，それら補正された出力値に基づいて車両の位置及び姿勢を推定するものである。
【０００３】
【発明が解決しようとする課題】
ところが，上述のような従来のハイブリッド航法では，車両位置の推定に必要な各測定値に対して，それぞれ複数の異なる測定手段を取り付けなければならないため，コスト高になるという問題点があった。
また，複数の測定手段からの出力を評価するための処理が複雑であるため処理時間がかかり，制御遅れ等の問題もあった。
また，特に電波航法による位置計測においては，一定時間毎，又は一定距離毎の計測が必要であり，走行経路の制約を受けるという問題点もあった。
本発明は上記事情に鑑みてなされたものであり，その目的とするところは，最小限の測定手段を用いた簡単な処理により車両の位置及び姿勢を高精度で推定することができ，走行経路の制約を受けることのない誘導装置を提供することである。
【０００４】
【課題を解決するための手段】
上記目的を達成するために本発明は，車両に設置され，該車両の相対位置及び姿勢を検出する位置・姿勢検出手段と，車両に設置され，該車両の走行経路上に任意の間隔で設けられた認識マークを検出することにより，該認識マークに対する上記車両の横ズレ量を検出する１つのセンサにより構成されるズレ検出手段と，上記認識マークの位置を予め記憶する認識マーク位置記憶手段と，上記位置・姿勢検出手段からの出力に基づいて上記車両の相対位置及び姿勢を推定する位置・姿勢推定手段と，上記認識マーク位置記憶手段に記憶された認識マーク位置と，上記ズレ検出手段より得られた上記横ズレ量と，上記位置・姿勢推定手段より得られた車両の位置・姿勢推定値と車両の走行距離と，に基づいて上記車両の位置・姿勢推定値を補正する補正手段と，上記補正手段より得られた補正後の車両の位置・姿勢推定値に基づいて，車両を上記走行経路に沿って走行させるように制御する制御手段とを具備してなることを特徴とする誘導装置として構成されている。
また，上記補正手段は，例えば，上記認識マーク位置記憶手段に記憶された認識マーク位置と上記ズレ検出手段より得られた上記横ズレ量と上記位置・姿勢推定手段より得られた車両の姿勢推定値とから求めた車両の位置計算値と，上記位置・姿勢推定手段より得られた車両の位置推定値と，上記位置・姿勢推定手段より得られた車両の走行距離とに基づいて車両の姿勢補正量を求め，該姿勢補正量により，上記位置・姿勢推定手段より得られた車両の姿勢推定値を補正し，該補正された姿勢推定値と，上記認識マーク位置記憶手段に記憶された認識マーク位置と，上記ズレ検出手段より得られた上記横ズレ量とに基づいて，上記位置・姿勢推定手段より得られた車両の位置推定値を補正するように構成することができる。
【０００５】
或いは，上記補正手段は，上記ズレ検出手段により２つの上記認識マークが連続して検出された時，上記認識マーク位置記憶手段に記憶された認識マーク位置と上記ズレ検出手段より得られた上記横ズレ量と上記位置・姿勢推定手段より得られた車両の姿勢推定値とから上記２つの認識マーク検出時における車両の位置計算値をそれぞれ求め，それら２つの位置計算値より求めた車両の姿勢計算値と，上記２つの認識マーク検出時において上記位置・姿勢推定手段より得られた２つの位置推定値より求めた車両の姿勢推定値とに基づいて車両の姿勢補正量を求め，該姿勢補正量により，上記位置・姿勢推定手段より得られた車両の姿勢推定値を補正し，該補正された姿勢推定値と，上記認識マーク位置記憶手段に記憶された認識マーク位置と，上記ズレ検出手段より得られる上記横ズレ量とに基づいて，上記位置・姿勢推定手段より得られた車両の位置推定値を補正するように構成することもできる。
上記位置・姿勢検出手段としては，例えば，車両のステア角を検出するステア角検出センサと，車両の移動速度を検出する移動速度検出センサとを具備する構成が考えられる。
また，上記ズレ検出手段としては，例えば，磁気マークで構成される上記認識マークからの車両の横ズレ量を検出する磁気センサによる構成が考えられる。
【０００６】
【作用】
本発明に係る誘導装置では，位置・姿勢検出手段からの出力を用いて位置・姿勢推定手段により推定された位置・姿勢推定値に基づいて，制御手段により車両を走行経路に沿って走行させることを基本とする。但し，走行経路上に任意の間隔で設けられた磁気マークなどの認識マークが磁気センサなどのズレ検出手段により検出された時には，補正手段により，上記位置・姿勢推定手段により推定された位置・姿勢推定値が，認識マーク位置記憶手段に記憶された上記認識マーク位置と，上記ズレ検出手段より得られた上記横ズレ量と，上記位置・姿勢推定手段より得られた車両の位置・姿勢推定値とに基づいて補正される。補正の態様としては，請求項２又は３に示すような方法が挙げられる。このように，測定手段として，位置・姿勢検出手段，及びズレ検出手段のみを用いた構成により，複数の異なる測定手段を重複して取り付けなければならなかった従来のハイブリッド航法と比べてコストが低く抑えられ，また，測定手段からの出力を評価するための処理が簡単なため高速処理が行える。また，上記走行経路上に設けられた認識マークは任意の間隔で設けられ，またどのような状況でも確実に検出できるため，一定時間毎，又は一定距離毎の計測が必要な電波航法による位置計測のように走行経路の制約を受けることもない。
【０００７】
【発明の実施の形態】
以下添付図面を参照して，本発明の実施の形態及び実施例につき説明し，本発明の理解に供する。尚，以下の実施の形態及び実施例は本発明を具体化した一例であって，本発明の技術的範囲を限定する性格のものではない。
ここに，図１は本発明の実施の形態に係る誘導装置Ａ１の概略構成を示す模式図，図２は上記誘導装置Ａ１の車両誘導処理手順の一例を示すフローチャート，図３は上記誘導装置Ａ１の車両誘導処理手順の説明図，図４は上記誘導装置Ａ１を適用する車両１０の構成の一例を示す模式図，図５は実施例に係る車両誘導処理手順を示すフローチャート，図６は上記実施例に係る車両誘導処理手順の説明図である。
本実施の形態に係る誘導装置Ａ１は，図１に示すように，車両のステア角を検出するポテンショメータ等のステア角検出センサ１と，車両の駆動輪に取付けられ車両の速度を検出するエンコーダ等の移動速度検出センサ２と，車両の走行経路上に任意の間隔で設けられた磁気マークに対する横ズレ量を検出する磁気センサ３と，上記ステア角検出センサ１及び上記移動速度検出センサ２からの検出値に基づいて車両の位置及び姿勢を推定する位置・姿勢推定部４と，上記走行経路の情報及び上記磁気マークの位置（座標）を予め記憶する地図情報記憶部７（認識マーク位置記憶手段に相当）と，上記地図情報記憶部７に記憶された上記磁気マークの位置と，上記磁気センサ３により検出された磁気マークからの横ズレ量と，上記位置・姿勢推定部４により得られた位置・姿勢推定値とに基づいて上記位置・姿勢推定値を補正する補正部５と，上記位置・姿勢推定部４により得られた位置・姿勢推定値若しくは上記補正部５により得られた補正後の位置・姿勢推定値に基づいて車両を走行経路に沿って走行させるように制御する制御部６とで構成されている。尚，上記ステア角検出センサ１と移動速度検出センサ２とで位置・姿勢検出手段を構成している。
以上のような構成を有する誘導装置Ａ１は，上記ステア角検出センサ１と移動速度検出センサ２からの検出値に基づいて上記位置・姿勢推定部４において車両の位置・姿勢推定値を求め，該位置・姿勢推定値と上記地図情報記憶部７に記憶された走行経路の情報とに基づいて制御部６により走行制御を行う，いわゆる慣性航法を基本とし，上記磁気センサ３により上記磁気マークが検出される度に上記補正部５により上記位置・姿勢推定部４で求めた車両の位置・姿勢推定値を補正するものである。これは，上記ステア角検出センサ１と移動速度検出センサ２により得られる相対位置を，上記磁気センサ３により得られる絶対位置で補正していると言うこともできる。
【０００８】
以下，図２及び図３を用いて，上記誘導装置Ａ１による車両の誘導手順について説明する。尚，以下の説明に用いる車両１０は，図４に示すように，ホイールベース（＝Ｗ）の中心を車体中心位置とし，車輪１１から距離Ｌだけ離れた車体最前部に磁気センサ３が取り付けられている。また，車両１０の走行経路Ｒ上には任意の間隔で磁気マークＭ（Ｍ１，Ｍ２，…Ｍｎ）が設置されており，上記磁気センサ３は上記磁気マークＭと車両１０の中心線との横ズレ量Ｄを検出する。
スタート地点ＳＰ（図３）から車両１０が走行を始めると，ステア角検出センサ１及び移動速度検出センサ２によりそれぞれステア角と移動速度の検出が連続的に行われ（ステップＳ１），それら検出値に基づいて位置・姿勢推定部４において車両１０の位置及び姿勢の推定値が求められる（ステップＳ２）。車両１０の最前部に取り付けられた磁気センサ３により，走行経路Ｒ上の磁気マークＭ１を検出するまでは，上記位置・姿勢推定部４において求められた位置・姿勢推定値が地図情報記憶部７に記憶された走行経路Ｒに沿うように制御部１０により車体１０の走行が制御される（ステップＳ３→Ｓ７→Ｓ８→Ｓ１→…）。
磁気センサ３により磁気マークＭ１が検出されると（ステップＳ３），上記ステップＳ７を実行する前に，以下に説明するステップＳ４〜Ｓ６の処理が行われる。
【０００９】
図３は磁気センサ３により磁気マークＭ１が検出された時点での車両１０の実際の位置・姿勢１０ａと，上記位置・姿勢推定部４において推定された位置・姿勢１０ａ′との関係を示している。図中，実線が実際の車両１０の位置・姿勢１０ａ，破線が位置・姿勢推定部４において推定された位置・姿勢１０ａ′である。即ち，磁気センサ３により磁気マークＭ１が検出されるまでは，制御部６では１０ａ′を走行経路Ｒにのせるような走行制御が行われていたものとする。
ここで，上記走行経路Ｒの方向をＸ軸，それと直角の方向をＹ軸，車両の走行経路Ｒに対する姿勢角をθとし，実際の車両の中心位置Ｐｒの座標及び姿勢を（Ｘｒ，Ｙｒ，θｒ），上記位置・姿勢推定部４において推定された車両位置・姿勢の中心位置Ｐｇの座標及び姿勢を（Ｘｇ，Ｙｇ，θｇ），実際の磁気センサ３の位置Ｐｓの座標を（Ｘｓ，Ｙｓ），磁気マークＭ１の座標を（Ｘｍ，Ｙｍ），上記磁気センサ３による検出値（磁気マークＭ１からの横ズレ量）をＤとする。以上の各値のうち，既知の値は，地図情報記憶部７に記憶された磁気マークＭ１の座標（Ｘｍ，Ｙｍ），上記位置・姿勢推定部４において推定された車両位置・姿勢の中心位置Ｐｇの座標及び姿勢（Ｘｇ，Ｙｇ，θｇ），及び上記磁気センサ３による検出値Ｄである。
磁気センサ３により磁気マークＭ１が検出されると，補正部５により以下のような処理（ステップＳ４〜Ｓ６）が行われる。
【００１０】
まず，実際の磁気センサ３の位置Ｐｓの座標（Ｘｓ，Ｙｓ）は次式のようにおくことができる。
Ｘｓ＝Ｘｍ−Ｄ・ｓｉｎ（θｒ） …（１）
Ｙｓ＝Ｙｍ−Ｄ・ｃｏｓ（θｒ） …（２）
ところが，上記（１），（２）式中，θｒは未知である。そこで，θｒに代えて，既知の値であるθｇを代入すると，次式のようになる。
Ｘｓ＝Ｘｍ−Ｄ・ｓｉｎ（θｇ） …（１′）
Ｙｓ＝Ｙｍ−Ｄ・ｃｏｓ（θｇ） …（２′）
ここで，上記（１′），（２′）式は，θｇ＝θｒの場合には上記（１），（２）式と等価となるため正しい関係を示すが，θｇ≠θｒの場合には当然にその角度差の分だけ誤差を含む，誤った関係式となる。
上記（１′），（２′）式を用いて，実際の車両の中心位置Ｐｒの座標（Ｘｒ，Ｙｒ）を表すと次式のようになる。
Ｘｒ＝Ｘｓ−（Ｌ＋Ｗ／２）・ｃｏｓ（θｇ） …（３）
Ｙｒ＝Ｙｓ−（Ｌ＋Ｗ／２）・ｓｉｎ（θｇ） …（４）
ここでも，上記（１′），（２′）式と同様，未知の値θｒに代えて既知の値θｇを用いる。
ここで，磁気マークＭ１を検出したときの移動距離（θｇ≒０であるのでＹ座標は無視でき，磁気マーク検出１回目ではスタート地点ＳＰからの移動距離，即ち上記推定のＸ座標の値Ｘｇとし，磁気マーク検出２回目以降は前回の磁気マーク検出時のＸ座標と今回の磁気マーク検出時のＸ座標との差とする）をＳとすると，車両姿勢角の補正量θ_offsetは次式のようになる。
θ_offset＝Ｋｖａ・ａｔａｎ（（Ｙｒ−Ｙｇ）／Ｓ） …（５）
但し，Ｋｖａは比例ゲインであり，ここでは１とする。
ここで，θｇ＝θｒの場合，即ち上記（１′），（２′）式，及び上記（３），（４）式が正しい関係を示す場合には，上記（５）式中の（Ｙｒ−Ｙｇ）が０になり，θ_offsetは０になる。ところが，θｇ≠θｓの場合，即ち上記（１′），（２′）式，及び上記（３），（４）式が誤っている場合には，それらの式に含まれる誤差，即ちθｇとθｒとの差に応じてθ_offsetの値が大きく現れる。従って，実際の車両姿勢角θｒを次式により求める。
θｒ＝θｇ＋θ_offset …（６）
以上，実際の車両姿勢角θｒを求めるまでがステップＳ４にあたる。
【００１１】
続いて，上記θｒを用いて実際の磁気センサの座標（Ｘｓ，Ｙｓ），及び実際の車両の中心位置の座標（Ｘｒ，Ｙｒ）を算出する（ステップＳ５）。
Ｘｓ＝Ｘｍ−Ｄ・ｓｉｎ（θｒ） …（７）
Ｙｓ＝Ｙｍ−Ｄ・ｃｏｓ（θｒ） …（８）
Ｘｒ＝Ｘｓ−（Ｌ＋Ｗ／２）・ｃｏｓ（θｒ） …（９）
Ｙｒ＝Ｙｓ−（Ｌ＋Ｗ／２）・ｓｉｎ（θｒ） …（１０）
求めた車両中心Ｐｒの座標及び姿勢（Ｘｒ，Ｙｒ，θｒ）を，推定した車両中心Ｐｇの位置及び姿勢（Ｘｇ，Ｙｇ，θｇ）と置き換える（ステップＳ６）。
以上のステップＳ４〜Ｓ６の処理により，磁気マークＭを検出する度に位置・姿勢推定部４において推定された車両の位置・姿勢が補正され，その補正後の推定位置・姿勢を用いて制御部１０により車体１０の走行が制御される（ステップＳ７）ため，常に高精度の制御が行える。
以上のような処理を，目標位置に到着するまで（ステップＳ８），繰り返し行う。
【００１２】
以上説明したように，本実施の形態に係る誘導装置Ａ１は，測定手段として，慣性航法に必要なステア角検出センサ１と移動速度検出センサ２，及び走行経路上に任意の間隔で設けられた磁気マークＭに対する車両の横ズレ量を検出する磁気センサ３のみを用いた構成により，複数の異なる測定手段を重複して取り付けなければならなかった従来のハイブリッド航法と比べてコストを低く抑えることができ，また，測定手段からの出力を評価するための処理が簡単であるため制御遅れ等の心配もない。また，上記走行経路上に設けられた磁気マークＭは任意の間隔で設けることができ，またどのような状況でも確実に検出できるため，一定時間毎，又は一定距離毎の計測が必要な電波航法による位置計測のように走行経路の制約を受けることもない。このように，本実施の形態に係る誘導装置Ａ１は，最小限の測定手段を用いた簡単な処理により車両の位置及び姿勢を高精度で推定することができ，走行経路の制約を受けることがない。
【００１３】
【実施例】
上記実施の形態におけるステップＳ４〜Ｓ６においては，前回の磁気マーク検出時（若しくはスタート地点）から今回の磁気マーク検出時までの走行距離Ｓを用いて車体姿勢角の補正量θ_offsetを求めた（（５）式）が，補正量θ_offsetの求め方はこれに限られるものではない。例えば，連続する２つの磁気マークＭ１，Ｍ２検出時のそれぞれの車体位置（実際の位置，及び推定の位置）より車体姿勢角の補正量θ_offsetを求めてもよい。以下図５，図６を用いて，上記実施の形態におけるステップＳ４〜Ｓ６に対応するステップＳ１１〜Ｓ１８について説明する。尚，ステップＳ１〜Ｓ３，及びステップＳ７，Ｓ８の処理については上記実施の形態と同様である。
図６は磁気センサ３により磁気マークＭ１，Ｍ２が検出された時点での車両１０の実際の位置・姿勢１０ａ１，１０ａ２と，上記位置・姿勢推定部４において推定された位置・姿勢１０ａ１′，１０ａ２′との関係を示している。図中，実線が実際の車両１０の位置・姿勢１０ａ１，１０ａ２，破線が位置・姿勢推定部４において推定された位置・姿勢１０ａ１′，１０ａ２′である。
ここで，上記走行経路Ｒの方向をＸ軸，それと直角の方向をＹ軸，車両の走行経路Ｒに対する姿勢角をθとし，磁気マークＭ２が検出された時点での実際の車両中心Ｐｒ２の座標及び姿勢を（Ｘｒ２，Ｙｒ２，θｒ２），上記位置・姿勢推定部４において推定された車両中心Ｐｇ２の座標及び姿勢を（Ｘｇ２，Ｙｇ２，θｇ２），実際の磁気センサ３の位置Ｐｓ２の座標を（Ｘｓ２，Ｙｓ２），磁気マークＭ２の座標を（Ｘｍ２，Ｙｍ２），上記磁気センサ３による検出値（磁気マークＭ２からの横ズレ量）をＤ２とする。以上の各値のうち，既知の値は，地図情報記憶部７に記憶された磁気マークＭ２の座標（Ｘｍ２，Ｙｍ２），上記位置・姿勢推定部４において推定された車両中心Ｐｇ２の座標及び姿勢（Ｘｇ２，Ｙｇ２，θｇ２），及び上記磁気センサ３による検出値Ｄ２である。
磁気センサ３により磁気マークＭが検出されると，補正部５により以下のような処理（ステップＳ１１〜Ｓ１８）が行われる。
検出された磁気マークがＭ１，即ち最初の磁気マークである場合には，位置・姿勢推定部４により推定された推定位置・姿勢Ｐｇ（Ｘｇ１，Ｙｇ１，θｇ１）と，磁気センサ３の検出値Ｄ１を記憶した後（ステップＳ１１→Ｓ１８），通常どおり上記推定位置・姿勢Ｐｇ及び地図情報記憶部７に記憶された走行経路Ｒに関する情報に基づいて制御部６により走行制御が行われる（ステップＳ７）。
検出された磁気マークがＭ２以降，即ち２つ目以降の磁気マークである場合には（ステップＳ１１），以下に示すステップＳ１２〜Ｓ１７の処理が行われる。
【００１４】
まず，実際の磁気センサ３の位置Ｐｓ２の座標（Ｘｓ２，Ｙｓ２）は次式のようにおくことができる。
Ｘｓ２＝Ｘｍ２−Ｄ２・ｓｉｎ（θｒ２） …（１１）
Ｙｓ２＝Ｙｍ２−Ｄ２・ｃｏｓ（θｒ２） …（１２）
ところが，上記（１１），（１２）式中，θｒ２は未知である。そこで，θｒ２に代えて，既知の値であるθｇ２を代入すると，次式のようになる。
Ｘｓ２＝Ｘｍ２−Ｄ２・ｓｉｎ（θｇ２） …（１１′）
Ｙｓ２＝Ｙｍ２−Ｄ２・ｃｏｓ（θｇ２） …（１２′）
ここで，上記（１１′），（１２′）式は，θｇ２＝θｒ２の場合には上記（１１），（１２）式と等価となるため正しい関係を示すが，θｇ２≠θｒ２の場合には当然にその角度差の分だけ誤差を含む，誤った関係式となる。
上記（１１），（１２）式，及び同様の方法で既に求められ記憶されている磁気マークＭ１検出時点での実際の磁気センサ３の位置Ｐｓ１の座標（Ｘｓ１，Ｙｓ１）を用いて，磁気センサ位置Ｐｓ１，Ｐｓ２を通る直線と走行経路Ｒとのなす角度θｓは次式のように求められる（ステップＳ１２，Ｓ１３）。
θｓ＝ａｔａｎ（（Ｙｓ２−Ｙｓ１）／（Ｘｓ２−Ｘｓ１）） …（１３）
また，磁気マークＭ１を検出した時点での推定車体中心位置Ｐｇ１（Ｘｇ１，Ｙｇ１）と磁気マークＭ２を検出した時点での推定車体中心位置Ｐｇ２（Ｘｇ２，Ｙｇ２）とを通る直線と，走行経路Ｒとのなす角度θｇは次式のように求められる（ステップＳ１４）。
θｇ＝ａｔａｎ（（Ｙｇ２−Ｙｇ１）／（Ｘｇ２−Ｘｇ１）） …（１４）
上記（１３），（１４）式より，車体姿勢角の補正量θ_offsetは次式のようになる。
θ_offset＝Ｋｖａ・（θｓ−θｇ） …（１５）
但し，Ｋｖａは比例ゲインであり，ここでは１とする。
ここで，θｓ＝θｇの場合，即ち上記（１１′），（１２′）式が正しい関係を示す場合には，θ_offsetは０になる。ところが，θｓ≠θｇの場合，即ち上記（１１′），（１２′）式が誤っている場合には，それらの式に含まれる誤差，即ちθｇ２とθｒ２との差に応じてθ_offsetの値が大きく現れる。従って，実際の車両姿勢角θｒ２を次式により求める。
θｒ２＝θｇ２＋θ_offset …（１６）
【００１５】
続いて，上記θｒ２を用いて実際の磁気センサの座標（Ｘｓ２，Ｙｓ２），及び実際の車両の中心位置の座標（Ｘｒ２，Ｙｒ２）を算出する（ステップＳ１６）。
Ｘｓ２＝Ｘｍ２−Ｄ２・ｓｉｎ（θｒ２） …（１７）
Ｙｓ２＝Ｙｍ２−Ｄ２・ｃｏｓ（θｒ２） …（１８）
Ｘｒ２＝Ｘｓ２−（Ｌ＋Ｗ／２）・ｃｏｓ（θｒ２） …（１９）
Ｙｒ２＝Ｙｓ２−（Ｌ＋Ｗ／２）・ｓｉｎ（θｒ２） …（２０）
求めた車両中心Ｐｒ２の座標及び姿勢（Ｘｒ２，Ｙｒ２，θｒ２）を，推定した車両中心Ｐｇ２の座標及び姿勢（Ｘｇ２，Ｙｇ２，θｇ２）と置き換える（ステップＳ１７）と共に，次の磁気マークＭ２検出時の処理のために上記車両中心Ｐｇ２の座標及び姿勢（Ｘｇ２，Ｙｇ２，θｇ２）と磁気センサ３の検出値Ｄ２を記憶する（ステップＳ１８）。
上記ステップＳ１１〜Ｓ１８の処理により，磁気マークＭを検出する度に位置・姿勢推定部４において推定された車両の位置・姿勢が補正され，その補正後の推定位置・姿勢を用いて制御部１０により車体１０の走行が制御される（ステップＳ７）ため，常に高精度の制御が行える。
以上説明したように，本実施例に係る補正方法（ステップＳ１１〜Ｓ１８）を用いても上記実施の形態と同様の効果を得ることができる。
【００１６】
【発明の効果】
以上説明したように，本発明に係る誘導装置は，車両に設置され，該車両の相対位置及び姿勢を検出する位置・姿勢検出手段と，車両に設置され，該車両の走行経路上に任意の間隔で設けられた認識マークに対する上記車両の横ズレ量を検出するズレ検出手段と，上記認識マークの位置を予め記憶する認識マーク位置記憶手段と，上記位置・姿勢検出手段からの出力に基づいて上記車両の相対位置及び姿勢を推定する位置・姿勢推定手段と，上記認識マーク位置記憶手段に記憶された認識マーク位置と，上記ズレ検出手段より得られた上記横ズレ量と，上記位置・姿勢推定手段より得られた車両の位置・姿勢推定値とに基づいて上記車両の位置・姿勢推定値を補正する補正手段と，上記補正手段より得られた補正後の車両の位置・姿勢推定値に基づいて，車両を上記走行経路に沿って走行させるように制御する制御手段とを具備してなることを特徴とする誘導装置として構成されているため，複数の異なる測定手段を重複して取り付けなければならなかった従来のハイブリッド航法と比べてコストを低く抑えることができ，また，測定手段からの出力を評価するための処理が簡単であるため制御遅れ等の心配もない。
また，上記走行経路上に設けられた認識マークは任意の間隔で設けることができ，またどのような状況でも確実に検出できるため，一定時間毎，又は一定距離毎の計測が必要な電波航法による位置計測のように走行経路の制約を受けることもない。
このように，本発明に係る誘導装置は，最小限の測定手段を用いた簡単な処理により車両の位置及び姿勢を高精度で推定することができ，走行経路の制約を受けることがない。
【図面の簡単な説明】
【図１】本発明の実施の形態に係る誘導装置Ａ１の概略構成を示す模式図。
【図２】上記誘導装置Ａ１の車両誘導処理手順の一例を示すフローチャート。
【図３】上記誘導装置Ａ１の車両誘導処理手順の説明図。
【図４】上記誘導装置Ａ１を適用する車両１０の構成の一例を示す模式図。
【図５】実施例に係る車両誘導処理手順を示すフローチャート。
【図６】上記実施例に係る車両誘導処理手順の説明図。
【符号の説明】
１…ステア角検出センサ
２…移動速度検出センサ
３…磁気センサ
４…位置・姿勢推定部
５…補正部
６…制御部
７…地図情報記憶部
１０…車両
Ｒ…走行経路
Ｍ…磁気マーク[0001]
BACKGROUND OF THE INVENTION
The present invention relates to a guidance device for estimating a current position and posture of a vehicle based on information obtained from various measuring means attached to the vehicle and causing the vehicle to travel along a predetermined travel route.
[0002]
[Prior art]
As a dead-reckoning navigation method in which the current position and orientation of a vehicle are estimated based on information obtained from various measuring means attached to the vehicle, and the vehicle is guided according to a predetermined travel route using the estimated value, The accuracy of the estimated value is improved by combining navigation that estimates the vehicle's estimated position by measuring and integrating the travel direction and travel distance of the vehicle, and radio navigation using GPS, beacons, etc. Hybrid navigation is used. As an example, there is a method proposed in Japanese Patent Laid-Open No. 06-34379, for example.
In this method, in order to correct each error of each measuring means (geomagnetic sensor, angular velocity sensor, distance sensor, position sensor), the output error of one measuring means is corrected using the output of another measuring means, The position and orientation of the vehicle are estimated based on the corrected output value.
[0003]
[Problems to be solved by the invention]
However, the conventional hybrid navigation as described above has a problem in that the cost increases because a plurality of different measurement means must be attached to each measurement value necessary for estimating the vehicle position.
In addition, the processing for evaluating the outputs from a plurality of measuring means is complicated, so that processing time is required and there are problems such as control delay.
In particular, position measurement by radio navigation requires measurement at regular time intervals or constant distances, and there is a problem that the travel route is restricted.
The present invention has been made in view of the above circumstances, and an object of the present invention is to be able to estimate the position and posture of a vehicle with high accuracy by simple processing using a minimum measuring means. It is providing the guidance device which does not receive restrictions of.
[0004]
[Means for Solving the Problems]
In order to achieve the above object, the present invention is a position / posture detecting means installed in a vehicle and detecting the relative position and posture of the vehicle, and installed in the vehicle at an arbitrary interval on the travel route of the vehicle. A displacement detection means comprising one sensor for detecting the amount of lateral displacement of the vehicle relative to the recognition mark by detecting the recognized recognition mark ; and a recognition mark position storage means for previously storing the position of the recognition mark; A position / posture estimation means for estimating the relative position and posture of the vehicle based on an output from the position / posture detection means, a recognition mark position stored in the recognition mark position storage means, and a deviation detection means. and the lateral deviation amount obtained, the position and orientation and distance traveled position and orientation estimate and the vehicle of the vehicle obtained from estimating means corrects the position and orientation estimate of the vehicle based on the And a control means for controlling the vehicle to travel along the travel route based on the corrected position / posture estimated value of the vehicle obtained by the correction means. It is configured as a featured guidance device.
The correction means may be, for example, a recognition mark position stored in the recognition mark position storage means, a lateral deviation amount obtained from the deviation detection means, and a vehicle posture estimation obtained from the position / posture estimation means. Vehicle position based on the calculated position of the vehicle, the estimated position of the vehicle obtained from the position / posture estimation means, and the travel distance of the vehicle obtained from the position / posture estimation means. A correction amount is obtained, the posture estimation value of the vehicle obtained from the position / posture estimation means is corrected by the posture correction amount, and the corrected posture estimation value and the recognition mark stored in the recognition mark position storage means are corrected. The vehicle position estimation value obtained from the position / posture estimation means can be corrected based on the mark position and the lateral deviation amount obtained from the deviation detection means.
[0005]
Alternatively, the correction means may detect the recognition mark position stored in the recognition mark position storage means and the horizontal position obtained from the deviation detection means when the two detection marks are successively detected by the deviation detection means. The calculated position of the vehicle at the time of detection of the two recognition marks is obtained from the amount of deviation and the estimated position of the vehicle obtained from the position / posture estimation means, and the calculated posture of the vehicle is obtained from the two calculated positions. A posture correction amount of the vehicle is obtained based on the value and a vehicle posture estimation value obtained from the two position estimation values obtained by the position / posture estimation means when the two recognition marks are detected, and the posture correction amount To correct the estimated posture value of the vehicle obtained from the position / posture estimation means, and the corrected estimated posture value and the recognition mark position stored in the recognition mark position storage means. Based on the above lateral deviation amount obtained from the deviation detection means may be configured to correct the position estimate of the vehicle obtained from the position and posture estimation unit.
As the position / posture detection means, for example, a configuration including a steering angle detection sensor for detecting the steering angle of the vehicle and a movement speed detection sensor for detecting the movement speed of the vehicle can be considered.
Further, as the deviation detecting means, for example, a configuration using a magnetic sensor for detecting the lateral deviation amount of the vehicle from the recognition mark constituted by a magnetic mark can be considered.
[0006]
[Action]
In the guidance device according to the present invention, the control unit causes the vehicle to travel along the travel route based on the position / posture estimation value estimated by the position / posture estimation unit using the output from the position / posture detection unit. Based on. However, when recognition marks such as magnetic marks provided at arbitrary intervals on the travel route are detected by a deviation detection means such as a magnetic sensor, the position / posture estimated by the position / posture estimation means by the correction means. The estimated value is the recognition mark position stored in the recognition mark position storage means, the lateral deviation amount obtained from the deviation detection means, and the vehicle position / posture estimation value obtained from the position / posture estimation means. It is corrected based on. As a mode of correction, a method as shown in claim 2 or 3 can be cited. As described above, the configuration using only the position / posture detection means and the deviation detection means as the measurement means is low in cost compared with the conventional hybrid navigation in which a plurality of different measurement means had to be mounted in duplicate. In addition, the processing for evaluating the output from the measuring means is simple and high-speed processing can be performed. In addition, the recognition marks provided on the travel route are provided at arbitrary intervals and can be reliably detected in any situation. Therefore, position measurement by radio navigation that requires measurement at regular intervals or at regular intervals. Unlike the above, there is no restriction on the travel route.
[0007]
DETAILED DESCRIPTION OF THE INVENTION
DESCRIPTION OF THE PREFERRED EMBODIMENTS Embodiments and examples of the present invention will be described below with reference to the accompanying drawings for understanding of the present invention. It should be noted that the following embodiments and examples are examples embodying the present invention and do not limit the technical scope of the present invention.
FIG. 1 is a schematic diagram showing a schematic configuration of the guidance device A1 according to the embodiment of the present invention, FIG. 2 is a flowchart showing an example of a vehicle guidance process procedure of the guidance device A1, and FIG. 3 is the guidance device A1. FIG. 4 is a schematic diagram showing an example of the configuration of the vehicle 10 to which the guidance device A1 is applied, FIG. 5 is a flowchart showing the vehicle guidance processing procedure according to the embodiment, and FIG. It is explanatory drawing of the vehicle guidance process procedure which concerns on an example.
As shown in FIG. 1, the guidance device A1 according to the present embodiment includes a steer angle detection sensor 1 such as a potentiometer that detects the steer angle of the vehicle, an encoder that is attached to a drive wheel of the vehicle, and detects the speed of the vehicle. From the moving speed detection sensor 2, the magnetic sensor 3 that detects the amount of lateral deviation with respect to the magnetic marks provided at arbitrary intervals on the travel path of the vehicle, the steer angle detection sensor 1, and the movement speed detection sensor 2. A position / posture estimator 4 that estimates the position and posture of the vehicle based on the detected value, and a map information storage 7 (recognition mark position storage means) that stores in advance the information on the travel route and the position (coordinates) of the magnetic mark. ), The position of the magnetic mark stored in the map information storage unit 7, the amount of lateral deviation from the magnetic mark detected by the magnetic sensor 3, and the position / posture estimation. The correction unit 5 that corrects the position / posture estimation value based on the position / posture estimation value obtained by the unit 4, and the position / posture estimation value obtained by the position / posture estimation unit 4 or the correction unit 5 And a control unit 6 that controls the vehicle to travel along the travel route based on the corrected position / posture estimated value obtained by the above. The steer angle detection sensor 1 and the movement speed detection sensor 2 constitute position / posture detection means.
The guidance device A1 having the above configuration obtains the estimated position / posture value of the vehicle in the position / posture estimation unit 4 based on the detection values from the steering angle detection sensor 1 and the moving speed detection sensor 2, and Based on the position / orientation estimation value and the travel route information stored in the map information storage unit 7, the control unit 6 performs traveling control, which is based on so-called inertial navigation, and the magnetic sensor 3 detects the magnetic mark. The estimated position / posture value of the vehicle obtained by the position / posture estimation unit 4 is corrected by the correction unit 5 each time. It can also be said that the relative position obtained by the steering angle detection sensor 1 and the moving speed detection sensor 2 is corrected by the absolute position obtained by the magnetic sensor 3.
[0008]
Hereinafter, a vehicle guidance procedure by the guidance device A1 will be described with reference to FIGS. In the vehicle 10 used in the following description, as shown in FIG. 4, the center of the wheel base (= W) is the vehicle body center position, and the magnetic sensor 3 is attached to the forefront of the vehicle body at a distance L from the wheel 11. ing. Further, magnetic marks M (M1, M2,... Mn) are installed on the travel route R of the vehicle 10 at arbitrary intervals, and the magnetic sensor 3 is located between the magnetic mark M and the center line of the vehicle 10. A deviation amount D is detected.
When the vehicle 10 starts traveling from the start point SP (FIG. 3), the steer angle detection sensor 1 and the movement speed detection sensor 2 continuously detect the steering angle and the movement speed, respectively (step S1), and the detected values. Based on the above, the position / posture estimation unit 4 obtains estimated values of the position and posture of the vehicle 10 (step S2). Until the magnetic mark M1 on the travel route R is detected by the magnetic sensor 3 attached to the frontmost part of the vehicle 10, the position / posture estimation value obtained by the position / posture estimation unit 4 is the map information storage unit 7 The travel of the vehicle body 10 is controlled by the control unit 10 along the travel route R stored in (Step S3 → S7 → S8 → S1 →...).
When the magnetic mark M1 is detected by the magnetic sensor 3 (step S3), steps S4 to S6 described below are performed before executing step S7.
[0009]
FIG. 3 shows the relationship between the actual position / posture 10a of the vehicle 10 when the magnetic mark M1 is detected by the magnetic sensor 3 and the position / posture 10a 'estimated by the position / posture estimation unit 4. Yes. In the figure, the solid line is the actual position / posture 10a of the vehicle 10, and the broken line is the position / posture 10a 'estimated by the position / posture estimation unit 4. In other words, it is assumed that the travel control that puts 10a ′ on the travel route R is performed in the control unit 6 until the magnetic mark M1 is detected by the magnetic sensor 3.
Here, the direction of the travel route R is the X axis, the direction perpendicular thereto is the Y axis, the posture angle of the vehicle with respect to the travel route R is θ, and the coordinates and posture of the actual vehicle center position Pr are (Xr, Yr, θr), the coordinates and posture of the vehicle position / posture center position Pg estimated by the position / posture estimation unit 4 are (Xg, Yg, θg), and the actual position Ps coordinates of the magnetic sensor 3 are (Xs, Ys). ), The coordinates of the magnetic mark M1 are (Xm, Ym), and the value detected by the magnetic sensor 3 (the amount of lateral deviation from the magnetic mark M1) is D. Among the above values, the known values are the coordinates (Xm, Ym) of the magnetic mark M1 stored in the map information storage unit 7 and the center position of the vehicle position / posture estimated by the position / posture estimation unit 4. The coordinates and orientation (Xg, Yg, θg) of Pg and the detection value D by the magnetic sensor 3 are shown.
When the magnetic mark M1 is detected by the magnetic sensor 3, the following processing (steps S4 to S6) is performed by the correction unit 5.
[0010]
First, the coordinates (Xs, Ys) of the actual position Ps of the magnetic sensor 3 can be set as follows.
Xs = Xm−D · sin (θr) (1)
Ys = Ym−D · cos (θr) (2)
However, in the above equations (1) and (2), θr is unknown. Therefore, if θg which is a known value is substituted for θr, the following equation is obtained.
Xs = Xm−D · sin (θg) (1 ′)
Ys = Ym−D · cos (θg) (2 ′)
Here, the above equations (1 ′) and (2 ′) are equivalent to the above equations (1) and (2) when θg = θr, and thus show a correct relationship. However, when θg ≠ θr, Naturally, it becomes an incorrect relational expression including an error corresponding to the angle difference.
Using the above equations (1 ′) and (2 ′), the coordinates (Xr, Yr) of the actual vehicle center position Pr are expressed as follows.
Xr = Xs− (L + W / 2) · cos (θg) (3)
Yr = Ys− (L + W / 2) · sin (θg) (4)
In this case as well, the known value θg is used instead of the unknown value θr, as in the above formulas (1 ′) and (2 ′).
Here, since the movement distance when the magnetic mark M1 is detected (θg≈0, the Y coordinate can be ignored, and the first movement of the magnetic mark detection is the movement distance from the start point SP, that is, the estimated X coordinate value Xg. , Where S is the difference between the X coordinate at the time of the previous magnetic mark detection and the X coordinate at the time of the current magnetic mark detection), the vehicle attitude angle correction amount θ _offset is given by It becomes like this.
θ _offset = Kva · atan ((Yr−Yg) / S) (5)
However, Kva is a proportional gain, and is 1 here.
Here, when θg = θr, that is, when the above equations (1 ′) and (2 ′) and the above equations (3) and (4) show the correct relationship, (Yr in the above equation (5) −Yg) becomes 0 and θ _offset becomes 0. However, when θg ≠ θs, that is, when the above equations (1 ′) and (2 ′) and the above equations (3) and (4) are incorrect, errors included in these equations, ie, θg and A large value of θ _offset appears depending on the difference from θr. Therefore, the actual vehicle attitude angle θr is obtained by the following equation.
θr = θg + θ _offset (6)
As described above, the process until the actual vehicle attitude angle θr is obtained corresponds to step S4.
[0011]
Subsequently, the actual magnetic sensor coordinates (Xs, Ys) and the actual vehicle center position coordinates (Xr, Yr) are calculated using the θr (step S5).
Xs = Xm−D · sin (θr) (7)
Ys = Ym−D · cos (θr) (8)
Xr = Xs− (L + W / 2) · cos (θr) (9)
Yr = Ys− (L + W / 2) · sin (θr) (10)
The obtained coordinates and orientation (Xr, Yr, θr) of the vehicle center Pr are replaced with the estimated position and orientation (Xg, Yg, θg) of the vehicle center Pg (step S6).
Through the processing in steps S4 to S6, the position / posture of the vehicle estimated by the position / posture estimation unit 4 is corrected each time the magnetic mark M is detected, and the controller uses the estimated position / posture after the correction. 10, the traveling of the vehicle body 10 is controlled (step S7), so that highly accurate control can always be performed.
The above processing is repeated until the target position is reached (step S8).
[0012]
As described above, the guidance device A1 according to the present embodiment is provided as a measuring unit with the steering angle detection sensor 1 and the moving speed detection sensor 2, which are necessary for inertial navigation, and on the travel route at arbitrary intervals. By using only the magnetic sensor 3 for detecting the lateral displacement of the vehicle with respect to the magnetic mark M, the cost can be kept low compared to the conventional hybrid navigation in which a plurality of different measuring means have to be mounted redundantly. Moreover, since the process for evaluating the output from the measuring means is simple, there is no concern about control delay or the like. In addition, the magnetic marks M provided on the travel route can be provided at arbitrary intervals and can be reliably detected in any situation. Therefore, radio navigation that requires measurement at regular intervals or at regular intervals. There is no restriction on the travel route as in the case of position measurement by. As described above, the guidance device A1 according to the present embodiment can estimate the position and posture of the vehicle with high accuracy by a simple process using the minimum measurement means, and is subject to restrictions on the travel route. Absent.
[0013]
【Example】
In steps S4 to S6 in the above-described embodiment, the correction amount θ _offset of the vehicle body posture angle is obtained using the travel distance S from the previous magnetic mark detection (or start point) to the current magnetic mark detection ( However, the method of _obtaining the correction amount θ _offset is not limited to (5). For example, the correction amount θ _offset of the vehicle body posture angle may be obtained from the respective vehicle body positions (actual position and estimated position) when two consecutive magnetic marks M1 and M2 are detected. Hereinafter, steps S11 to S18 corresponding to steps S4 to S6 in the above embodiment will be described with reference to FIGS. In addition, about the process of step S1-S3 and step S7, S8, it is the same as that of the said embodiment.
6 shows the actual position / posture 10a1, 10a2 of the vehicle 10 at the time when the magnetic marks M1, M2 are detected by the magnetic sensor 3, and the positions / postures 10a1 ', 10a2 estimated by the position / posture estimation unit 4. The relationship with ′ is shown. In the figure, solid lines indicate actual positions / postures 10a1 and 10a2 of the vehicle 10, and broken lines indicate positions / postures 10a1 'and 10a2' estimated by the position / posture estimation unit 4.
Here, the direction of the travel route R is the X axis, the direction perpendicular thereto is the Y axis, the attitude angle of the vehicle with respect to the travel route R is θ, and the coordinates of the actual vehicle center Pr2 when the magnetic mark M2 is detected. And the attitude (Xr2, Yr2, θr2), the coordinates and attitude of the vehicle center Pg2 estimated by the position / orientation estimation unit 4 (Xg2, Yg2, θg2), and the coordinates of the actual magnetic sensor 3 position Ps2 ( Xs2, Ys2), the coordinates of the magnetic mark M2 are (Xm2, Ym2), and the detection value (lateral displacement from the magnetic mark M2) by the magnetic sensor 3 is D2. Among these values, the known values are the coordinates (Xm2, Ym2) of the magnetic mark M2 stored in the map information storage unit 7, the coordinates and posture of the vehicle center Pg2 estimated by the position / posture estimation unit 4 (Xg2, Yg2, θg2), and a detection value D2 by the magnetic sensor 3.
When the magnetic mark M is detected by the magnetic sensor 3, the following processing (steps S11 to S18) is performed by the correction unit 5.
When the detected magnetic mark is M1, that is, the first magnetic mark, the estimated position / posture Pg (Xg1, Yg1, θg1) estimated by the position / posture estimation unit 4 and the detection value D1 of the magnetic sensor 3 are detected. Is stored (step S11 → S18), and the control is performed by the control unit 6 based on the information on the estimated position / posture Pg and the travel route R stored in the map information storage unit 7 as usual (step S7). .
If the detected magnetic mark is M2 or later, that is, the second or later magnetic mark (step S11), the following steps S12 to S17 are performed.
[0014]
First, the coordinates (Xs2, Ys2) of the actual position Ps2 of the magnetic sensor 3 can be set as follows.
Xs2 = Xm2-D2 · sin (θr2) (11)
Ys2 = Ym2-D2 · cos (θr2) (12)
However, in the above equations (11) and (12), θr2 is unknown. Therefore, if θg2 which is a known value is substituted for θr2, the following equation is obtained.
Xs2 = Xm2−D2 · sin (θg2) (11 ′)
Ys2 = Ym2−D2 · cos (θg2) (12 ′)
Here, the above equations (11 ′) and (12 ′) are equivalent to the above equations (11) and (12) when θg2 = θr2, and thus show a correct relationship. However, when θg2 ≠ θr2, Naturally, it becomes an incorrect relational expression including an error corresponding to the angle difference.
Using the above equations (11) and (12) and the coordinates (Xs1, Ys1) of the actual position Ps1 of the magnetic sensor 3 at the time of detection of the magnetic mark M1 already obtained and stored by the same method, the magnetic sensor An angle θs formed by a straight line passing through the positions Ps1 and Ps2 and the travel route R is obtained as follows (steps S12 and S13).
θs = atan ((Ys2−Ys1) / (Xs2−Xs1)) (13)
In addition, a straight line passing through the estimated vehicle body center position Pg1 (Xg1, Yg1) when the magnetic mark M1 is detected and the estimated vehicle body center position Pg2 (Xg2, Yg2) when the magnetic mark M2 is detected, and the travel route R Is obtained as shown in the following equation (step S14).
θg = atan ((Yg2-Yg1) / (Xg2-Xg1)) (14)
From the above equations (13) and (14), the correction amount θ _offset of the vehicle body posture angle is as follows.
θ _offset = Kva · (θs−θg) (15)
However, Kva is a proportional gain, and is 1 here.
Here, when θs = θg, that is, when the above equations (11 ′) and (12 ′) indicate a correct relationship, θ _offset is zero. However, when θs ≠ θg, that is, when the above equations (11 ′) and (12 ′) are incorrect, the value of θ _offset depends on the error included in those equations, that is, the difference between θg2 and θr2. Appears greatly. Therefore, the actual vehicle attitude angle θr2 is obtained by the following equation.
_{θr2 = θg2 + θ offset ... (} 16)
[0015]
Subsequently, the actual magnetic sensor coordinates (Xs2, Ys2) and the actual vehicle center position coordinates (Xr2, Yr2) are calculated using the θr2 (step S16).
Xs2 = Xm2-D2 · sin (θr2) (17)
Ys2 = Ym2-D2 · cos (θr2) (18)
Xr2 = Xs2- (L + W / 2) .cos (θr2) (19)
Yr2 = Ys2- (L + W / 2) · sin (θr2) (20)
The obtained coordinates and orientation (Xr2, Yr2, θr2) of the vehicle center Pr2 are replaced with the estimated coordinates and orientation (Xg2, Yg2, θg2) of the vehicle center Pg2 (step S17), and at the time of detecting the next magnetic mark M2. For processing, the coordinates and orientation (Xg2, Yg2, θg2) of the vehicle center Pg2 and the detected value D2 of the magnetic sensor 3 are stored (step S18).
The position / posture of the vehicle estimated by the position / posture estimator 4 is corrected each time the magnetic mark M is detected by the processing in steps S11 to S18, and the controller 10 uses the estimated position / posture after the correction. Thus, the travel of the vehicle body 10 is controlled (step S7), so that highly accurate control can always be performed.
As described above, even if the correction method (steps S11 to S18) according to the present embodiment is used, the same effect as in the above embodiment can be obtained.
[0016]
【The invention's effect】
As described above, the guidance device according to the present invention is installed in a vehicle, position / posture detection means for detecting the relative position and posture of the vehicle, and installed in the vehicle, on the travel route of the vehicle, Based on output from the displacement detection means for detecting the lateral displacement amount of the vehicle with respect to the recognition marks provided at intervals, the recognition mark position storage means for storing the position of the recognition mark in advance, and the position / posture detection means Position / posture estimation means for estimating the relative position and posture of the vehicle, recognition mark position stored in the recognition mark position storage means, lateral displacement amount obtained from the deviation detection means, and position / posture Correcting means for correcting the estimated position / posture value of the vehicle based on the estimated position / posture value of the vehicle obtained from the estimating means, and the corrected estimated position / posture value of the vehicle obtained from the correcting means And a control device for controlling the vehicle to travel along the travel route. Therefore, a plurality of different measurement devices must be attached in duplicate. Compared with the conventional hybrid navigation that had to be performed, the cost can be kept low, and the process for evaluating the output from the measuring means is simple, so there is no concern about control delay or the like.
In addition, the recognition marks provided on the travel route can be provided at arbitrary intervals and can be reliably detected in any situation, so that radio wave navigation that requires measurement at regular intervals or regular intervals is used. The travel route is not restricted like the position measurement.
As described above, the guidance device according to the present invention can estimate the position and posture of the vehicle with high accuracy by a simple process using the minimum measurement means, and is not restricted by the travel route.
[Brief description of the drawings]
FIG. 1 is a schematic diagram showing a schematic configuration of a guidance device A1 according to an embodiment of the present invention.
FIG. 2 is a flowchart showing an example of a vehicle guidance process procedure of the guidance device A1.
FIG. 3 is an explanatory diagram of a vehicle guidance process procedure of the guidance device A1.
FIG. 4 is a schematic diagram showing an example of a configuration of a vehicle 10 to which the guidance device A1 is applied.
FIG. 5 is a flowchart showing a vehicle guidance process procedure according to the embodiment.
FIG. 6 is an explanatory diagram of a vehicle guidance process procedure according to the embodiment.
[Explanation of symbols]
DESCRIPTION OF SYMBOLS 1 ... Steer angle detection sensor 2 ... Movement speed detection sensor 3 ... Magnetic sensor 4 ... Position and attitude | position estimation part 5 ... Correction | amendment part 6 ... Control part 7 ... Map information storage part 10 ... Vehicle R ... Traveling route M ... Magnetic mark

Claims

A position / posture detection means installed in the vehicle for detecting the relative position and posture of the vehicle;
A deviation detection means comprising a single sensor that is installed in a vehicle and detects a lateral deviation amount of the vehicle relative to the recognition mark by detecting recognition marks provided at arbitrary intervals on the travel route of the vehicle. When,
Recognition mark position storage means for storing the position of the recognition mark in advance;
Position / posture estimation means for estimating the relative position and posture of the vehicle based on the output from the position / posture detection means;
The recognition mark position stored in the recognition mark position storage means, the lateral deviation amount obtained from the deviation detection means, the vehicle position / posture estimation value obtained from the position / posture estimation means, and the vehicle running Correction means for correcting the estimated position / posture value of the vehicle based on the distance ;
And a control unit that controls the vehicle to travel along the travel route based on the corrected position / posture estimated value of the vehicle obtained from the correction unit. .

The correction means is
Vehicle position calculation obtained from the recognition mark position stored in the recognition mark position storage means, the lateral deviation amount obtained from the deviation detection means, and the estimated posture of the vehicle obtained from the position / posture estimation means. Vehicle posture correction amount is obtained based on the value, the vehicle position estimation value obtained from the position / posture estimation means, and the vehicle travel distance obtained from the position / posture estimation means,
The vehicle posture estimation value obtained from the position / posture estimation means is corrected by the posture correction amount,
Based on the corrected posture estimation value, the recognition mark position stored in the recognition mark position storage unit, and the lateral shift amount obtained from the shift detection unit, the position / posture estimation unit obtains The guidance device according to claim 1, wherein the estimated position value of the vehicle is corrected.

The correction means is
When the two detection marks are detected in succession by the displacement detection means, the recognition mark position stored in the recognition mark position storage means, the lateral displacement amount obtained from the displacement detection means, and the position / posture From the estimated vehicle posture value obtained by the estimation means, the calculated position of the vehicle when the two recognition marks are detected is obtained.
Based on the calculated vehicle posture calculated from the two calculated positions and the estimated vehicle posture calculated from the two estimated positions obtained by the position / posture estimation means when the two recognition marks are detected. To obtain the vehicle attitude correction amount,
The vehicle posture estimation value obtained from the position / posture estimation means is corrected by the posture correction amount,
Based on the corrected posture estimation value, the recognition mark position stored in the recognition mark position storage unit, and the lateral shift amount obtained from the shift detection unit, the position / posture estimation unit obtained The guidance device according to claim 1, wherein the estimated position value of the vehicle is corrected.

The position / posture detecting means is
A steering angle detection sensor for detecting the steering angle of the vehicle;
The guidance device according to claim 1, further comprising a movement speed detection sensor that detects a movement speed of the vehicle.

The guidance device according to any one of claims 1 to 4, wherein the deviation detecting means is constituted by a magnetic sensor for detecting a lateral deviation amount of the vehicle from the recognition mark constituted by a magnetic mark.