(57)【特許請求の範囲】
【請求項１】
車両に設置され，該車両の相対位置及び姿勢を検出する位置・姿勢検出手段と，
車両に設置され，該車両の走行経路上に任意の間隔で設けられた認識マークを検出することにより，該認識マークに対する上記車両の横ズレ量を検出する１つのセンサにより構成されるズレ検出手段と，
上記認識マークの位置を予め記憶する認識マーク位置記憶手段と，
上記位置・姿勢検出手段からの出力に基づいて上記車両の相対位置及び姿勢を推定する位置・姿勢推定手段と，
上記認識マーク位置記憶手段に記憶された認識マーク位置と，上記ズレ検出手段より得られた上記横ズレ量と，上記位置・姿勢推定手段より得られた車両の位置・姿勢推定値と車両の走行距離と，に基づいて上記車両の位置・姿勢推定値を補正する補正手段と，
上記補正手段より得られた補正後の車両の位置・姿勢推定値に基づいて，車両を上記走行経路に沿って走行させるように制御する制御手段とを具備してなることを特徴とする誘導装置。
【請求項２】
上記補正手段が，
上記認識マーク位置記憶手段に記憶された認識マーク位置と上記ズレ検出手段より得られた上記横ズレ量と上記位置・姿勢推定手段より得られた車両の姿勢推定値とから求めた車両の位置計算値と，上記位置・姿勢推定手段より得られた車両の位置推定値と，上記位置・姿勢推定手段より得られた車両の走行距離とに基づいて車両の姿勢補正量を求め，
該姿勢補正量により，上記位置・姿勢推定手段より得られた車両の姿勢推定値を補正し，
該補正された姿勢推定値と，上記認識マーク位置記憶手段に記憶された認識マーク位置と，上記ズレ検出手段より得られた上記横ズレ量とに基づいて，上記位置・姿勢推定手段より得られた車両の位置推定値を補正する請求項１記載の誘導装置。
【請求項３】
上記補正手段が，
上記ズレ検出手段により２つの上記認識マークが連続して検出された時，上記認識マーク位置記憶手段に記憶された認識マーク位置と上記ズレ検出手段より得られた上記横ズレ量と上記位置・姿勢推定手段より得られた車両の姿勢推定値とから上記２つの認識マーク検出時における車両の位置計算値をそれぞれ求め，
それら２つの位置計算値より求めた車両の姿勢計算値と，上記２つの認識マーク検出時において上記位置・姿勢推定手段より得られた２つの位置推定値より求めた車両の姿勢推定値とに基づいて車両の姿勢補正量を求め，
該姿勢補正量により，上記位置・姿勢推定手段より得られた車両の姿勢推定値を補正し，
該補正された姿勢推定値と，上記認識マーク位置記憶手段に記憶された認識マーク位置と，上記ズレ検出手段より得られる上記横ズレ量とに基づいて，上記位置・姿勢推定手段より得られた車両の位置推定値を補正する請求項１記載の誘導装置。
【請求項４】
上記位置・姿勢検出手段が，
車両のステア角を検出するステア角検出センサと，
車両の移動速度を検出する移動速度検出センサとを具備して構成される請求項１〜３のいずれかに記載の誘導装置。
【請求項５】
上記ズレ検出手段が，磁気マークで構成される上記認識マークからの車両の横ズレ量を検出する磁気センサにより構成される請求項１〜４のいずれかに記載の誘導装置。
【発明の詳細な説明】
【０００１】
【発明の属する技術分野】
本発明は，車両に取り付けられた各種測定手段から得られた情報に基づいて車両の現在位置及び姿勢を推定し，車両を所定の走行経路に沿って走行させるための誘導装置に関するものである。
【０００２】
【従来の技術】
車両に取り付けられた各種測定手段から得られた情報に基づいて車両の現在位置及び姿勢を推定し，その推定値を利用して車両を所定の走行経路に従って誘導する推測航法として，従来より，車両の進行方向と移動距離を計測し積分することによって車両の推定位置を推定する航法と，ＧＰＳやビーコンなどを用いた電波航法とを組み合わせてそれぞれの欠点を補うことにより上記推定値の精度を高めるハイブリッド航法が用いられている。その一例として，例えば特開平０６−３４３７９号公報に提案されている方法がある。
この方法は，各測定手段（地磁気センサ，角速度センサ，距離センサ，位置センサ）のそれぞれの誤差を補正するため，１つの測定手段の出力誤差を他の測定手段の出力を用いて補正し，それら補正された出力値に基づいて車両の位置及び姿勢を推定するものである。
【０００３】
【発明が解決しようとする課題】
ところが，上述のような従来のハイブリッド航法では，車両位置の推定に必要な各測定値に対して，それぞれ複数の異なる測定手段を取り付けなければならないため，コスト高になるという問題点があった。
また，複数の測定手段からの出力を評価するための処理が複雑であるため処理時間がかかり，制御遅れ等の問題もあった。
また，特に電波航法による位置計測においては，一定時間毎，又は一定距離毎の計測が必要であり，走行経路の制約を受けるという問題点もあった。
本発明は上記事情に鑑みてなされたものであり，その目的とするところは，最小限の測定手段を用いた簡単な処理により車両の位置及び姿勢を高精度で推定することができ，走行経路の制約を受けることのない誘導装置を提供することである。
【０００４】
【課題を解決するための手段】
上記目的を達成するために本発明は，車両に設置され，該車両の相対位置及び姿勢を検出する位置・姿勢検出手段と，車両に設置され，該車両の走行経路上に任意の間隔で設けられた認識マークを検出することにより，該認識マークに対する上記車両の横ズレ量を検出する１つのセンサにより構成されるズレ検出手段と，上記認識マークの位置を予め記憶する認識マーク位置記憶手段と，上記位置・姿勢検出手段からの出力に基づいて上記車両の相対位置及び姿勢を推定する位置・姿勢推定手段と，上記認識マーク位置記憶手段に記憶された認識マーク位置と，上記ズレ検出手段より得られた上記横ズレ量と，上記位置・姿勢推定手段より得られた車両の位置・姿勢推定値と車両の走行距離と，に基づいて上記車両の位置・姿勢推定値を補正する補正手段と，上記補正手段より得られた補正後の車両の位置・姿勢推定値に基づいて，車両を上記走行経路に沿って走行させるように制御する制御手段とを具備してなることを特徴とする誘導装置として構成されている。
また，上記補正手段は，例えば，上記認識マーク位置記憶手段に記憶された認識マーク位置と上記ズレ検出手段より得られた上記横ズレ量と上記位置・姿勢推定手段より得られた車両の姿勢推定値とから求めた車両の位置計算値と，上記位置・姿勢推定手段より得られた車両の位置推定値と，上記位置・姿勢推定手段より得られた車両の走行距離とに基づいて車両の姿勢補正量を求め，該姿勢補正量により，上記位置・姿勢推定手段より得られた車両の姿勢推定値を補正し，該補正された姿勢推定値と，上記認識マーク位置記憶手段に記憶された認識マーク位置と，上記ズレ検出手段より得られた上記横ズレ量とに基づいて，上記位置・姿勢推定手段より得られた車両の位置推定値を補正するように構成することができる。
【０００５】
或いは，上記補正手段は，上記ズレ検出手段により２つの上記認識マークが連続して検出された時，上記認識マーク位置記憶手段に記憶された認識マーク位置と上記ズレ検出手段より得られた上記横ズレ量と上記位置・姿勢推定手段より得られた車両の姿勢推定値とから上記２つの認識マーク検出時における車両の位置計算値をそれぞれ求め，それら２つの位置計算値より求めた車両の姿勢計算値と，上記２つの認識マーク検出時において上記位置・姿勢推定手段より得られた２つの位置推定値より求めた車両の姿勢推定値とに基づいて車両の姿勢補正量を求め，該姿勢補正量により，上記位置・姿勢推定手段より得られた車両の姿勢推定値を補正し，該補正された姿勢推定値と，上記認識マーク位置記憶手段に記憶された認識マーク位置と，上記ズレ検出手段より得られる上記横ズレ量とに基づいて，上記位置・姿勢推定手段より得られた車両の位置推定値を補正するように構成することもできる。
上記位置・姿勢検出手段としては，例えば，車両のステア角を検出するステア角検出センサと，車両の移動速度を検出する移動速度検出センサとを具備する構成が考えられる。
また，上記ズレ検出手段としては，例えば，磁気マークで構成される上記認識マークからの車両の横ズレ量を検出する磁気センサによる構成が考えられる。
【０００６】
【作用】
本発明に係る誘導装置では，位置・姿勢検出手段からの出力を用いて位置・姿勢推定手段により推定された位置・姿勢推定値に基づいて，制御手段により車両を走行経路に沿って走行させることを基本とする。但し，走行経路上に任意の間隔で設けられた磁気マークなどの認識マークが磁気センサなどのズレ検出手段により検出された時には，補正手段により，上記位置・姿勢推定手段により推定された位置・姿勢推定値が，認識マーク位置記憶手段に記憶された上記認識マーク位置と，上記ズレ検出手段より得られた上記横ズレ量と，上記位置・姿勢推定手段より得られた車両の位置・姿勢推定値とに基づいて補正される。補正の態様としては，請求項２又は３に示すような方法が挙げられる。このように，測定手段として，位置・姿勢検出手段，及びズレ検出手段のみを用いた構成により，複数の異なる測定手段を重複して取り付けなければならなかった従来のハイブリッド航法と比べてコストが低く抑えられ，また，測定手段からの出力を評価するための処理が簡単なため高速処理が行える。また，上記走行経路上に設けられた認識マークは任意の間隔で設けられ，またどのような状況でも確実に検出できるため，一定時間毎，又は一定距離毎の計測が必要な電波航法による位置計測のように走行経路の制約を受けることもない。
【０００７】
【発明の実施の形態】
以下添付図面を参照して，本発明の実施の形態及び実施例につき説明し，本発明の理解に供する。尚，以下の実施の形態及び実施例は本発明を具体化した一例であって，本発明の技術的範囲を限定する性格のものではない。
ここに，図１は本発明の実施の形態に係る誘導装置Ａ１の概略構成を示す模式図，図２は上記誘導装置Ａ１の車両誘導処理手順の一例を示すフローチャート，図３は上記誘導装置Ａ１の車両誘導処理手順の説明図，図４は上記誘導装置Ａ１を適用する車両１０の構成の一例を示す模式図，図５は実施例に係る車両誘導処理手順を示すフローチャート，図６は上記実施例に係る車両誘導処理手順の説明図である。
本実施の形態に係る誘導装置Ａ１は，図１に示すように，車両のステア角を検出するポテンショメータ等のステア角検出センサ１と，車両の駆動輪に取付けられ車両の速度を検出するエンコーダ等の移動速度検出センサ２と，車両の走行経路上に任意の間隔で設けられた磁気マークに対する横ズレ量を検出する磁気センサ３と，上記ステア角検出センサ１及び上記移動速度検出センサ２からの検出値に基づいて車両の位置及び姿勢を推定する位置・姿勢推定部４と，上記走行経路の情報及び上記磁気マークの位置（座標）を予め記憶する地図情報記憶部７（認識マーク位置記憶手段に相当）と，上記地図情報記憶部７に記憶された上記磁気マークの位置と，上記磁気センサ３により検出された磁気マークからの横ズレ量と，上記位置・姿勢推定部４により得られた位置・姿勢推定値とに基づいて上記位置・姿勢推定値を補正する補正部５と，上記位置・姿勢推定部４により得られた位置・姿勢推定値若しくは上記補正部５により得られた補正後の位置・姿勢推定値に基づいて車両を走行経路に沿って走行させるように制御する制御部６とで構成されている。尚，上記ステア角検出センサ１と移動速度検出センサ２とで位置・姿勢検出手段を構成している。
以上のような構成を有する誘導装置Ａ１は，上記ステア角検出センサ１と移動速度検出センサ２からの検出値に基づいて上記位置・姿勢推定部４において車両の位置・姿勢推定値を求め，該位置・姿勢推定値と上記地図情報記憶部７に記憶された走行経路の情報とに基づいて制御部６により走行制御を行う，いわゆる慣性航法を基本とし，上記磁気センサ３により上記磁気マークが検出される度に上記補正部５により上記位置・姿勢推定部４で求めた車両の位置・姿勢推定値を補正するものである。これは，上記ステア角検出センサ１と移動速度検出センサ２により得られる相対位置を，上記磁気センサ３により得られる絶対位置で補正していると言うこともできる。
【０００８】
以下，図２及び図３を用いて，上記誘導装置Ａ１による車両の誘導手順について説明する。尚，以下の説明に用いる車両１０は，図４に示すように，ホイールベース（＝Ｗ）の中心を車体中心位置とし，車輪１１から距離Ｌだけ離れた車体最前部に磁気センサ３が取り付けられている。また，車両１０の走行経路Ｒ上には任意の間隔で磁気マークＭ（Ｍ１，Ｍ２，…Ｍｎ）が設置されており，上記磁気センサ３は上記磁気マークＭと車両１０の中心線との横ズレ量Ｄを検出する。
スタート地点ＳＰ（図３）から車両１０が走行を始めると，ステア角検出センサ１及び移動速度検出センサ２によりそれぞれステア角と移動速度の検出が連続的に行われ（ステップＳ１），それら検出値に基づいて位置・姿勢推定部４において車両１０の位置及び姿勢の推定値が求められる（ステップＳ２）。車両１０の最前部に取り付けられた磁気センサ３により，走行経路Ｒ上の磁気マークＭ１を検出するまでは，上記位置・姿勢推定部４において求められた位置・姿勢推定値が地図情報記憶部７に記憶された走行経路Ｒに沿うように制御部１０により車体１０の走行が制御される（ステップＳ３→Ｓ７→Ｓ８→Ｓ１→…）。
磁気センサ３により磁気マークＭ１が検出されると（ステップＳ３），上記ステップＳ７を実行する前に，以下に説明するステップＳ４〜Ｓ６の処理が行われる。
【０００９】
図３は磁気センサ３により磁気マークＭ１が検出された時点での車両１０の実際の位置・姿勢１０ａと，上記位置・姿勢推定部４において推定された位置・姿勢１０ａ′との関係を示している。図中，実線が実際の車両１０の位置・姿勢１０ａ，破線が位置・姿勢推定部４において推定された位置・姿勢１０ａ′である。即ち，磁気センサ３により磁気マークＭ１が検出されるまでは，制御部６では１０ａ′を走行経路Ｒにのせるような走行制御が行われていたものとする。
ここで，上記走行経路Ｒの方向をＸ軸，それと直角の方向をＹ軸，車両の走行経路Ｒに対する姿勢角をθとし，実際の車両の中心位置Ｐｒの座標及び姿勢を（Ｘｒ，Ｙｒ，θｒ），上記位置・姿勢推定部４において推定された車両位置・姿勢の中心位置Ｐｇの座標及び姿勢を（Ｘｇ，Ｙｇ，θｇ），実際の磁気センサ３の位置Ｐｓの座標を（Ｘｓ，Ｙｓ），磁気マークＭ１の座標を（Ｘｍ，Ｙｍ），上記磁気センサ３による検出値（磁気マークＭ１からの横ズレ量）をＤとする。以上の各値のうち，既知の値は，地図情報記憶部７に記憶された磁気マークＭ１の座標（Ｘｍ，Ｙｍ），上記位置・姿勢推定部４において推定された車両位置・姿勢の中心位置Ｐｇの座標及び姿勢（Ｘｇ，Ｙｇ，θｇ），及び上記磁気センサ３による検出値Ｄである。
磁気センサ３により磁気マークＭ１が検出されると，補正部５により以下のような処理（ステップＳ４〜Ｓ６）が行われる。
【００１０】
まず，実際の磁気センサ３の位置Ｐｓの座標（Ｘｓ，Ｙｓ）は次式のようにおくことができる。
Ｘｓ＝Ｘｍ−Ｄ・ｓｉｎ（θｒ）                          …（１）
Ｙｓ＝Ｙｍ−Ｄ・ｃｏｓ（θｒ）                          …（２）
ところが，上記（１），（２）式中，θｒは未知である。そこで，θｒに代えて，既知の値であるθｇを代入すると，次式のようになる。
Ｘｓ＝Ｘｍ−Ｄ・ｓｉｎ（θｇ）                          …（１′）
Ｙｓ＝Ｙｍ−Ｄ・ｃｏｓ（θｇ）                          …（２′）
ここで，上記（１′），（２′）式は，θｇ＝θｒの場合には上記（１），（２）式と等価となるため正しい関係を示すが，θｇ≠θｒの場合には当然にその角度差の分だけ誤差を含む，誤った関係式となる。
上記（１′），（２′）式を用いて，実際の車両の中心位置Ｐｒの座標（Ｘｒ，Ｙｒ）を表すと次式のようになる。
Ｘｒ＝Ｘｓ−（Ｌ＋Ｗ／２）・ｃｏｓ（θｇ）              …（３）
Ｙｒ＝Ｙｓ−（Ｌ＋Ｗ／２）・ｓｉｎ（θｇ）              …（４）
ここでも，上記（１′），（２′）式と同様，未知の値θｒに代えて既知の値θｇを用いる。
ここで，磁気マークＭ１を検出したときの移動距離（θｇ≒０であるのでＹ座標は無視でき，磁気マーク検出１回目ではスタート地点ＳＰからの移動距離，即ち上記推定のＸ座標の値Ｘｇとし，磁気マーク検出２回目以降は前回の磁気マーク検出時のＸ座標と今回の磁気マーク検出時のＸ座標との差とする）をＳとすると，車両姿勢角の補正量θoffsetは次式のようになる。
θoffset＝Ｋｖａ・ａｔａｎ（（Ｙｒ−Ｙｇ）／Ｓ）        …（５）
但し，Ｋｖａは比例ゲインであり，ここでは１とする。
ここで，θｇ＝θｒの場合，即ち上記（１′），（２′）式，及び上記（３），（４）式が正しい関係を示す場合には，上記（５）式中の（Ｙｒ−Ｙｇ）が０になり，θoffsetは０になる。ところが，θｇ≠θｓの場合，即ち上記（１′），（２′）式，及び上記（３），（４）式が誤っている場合には，それらの式に含まれる誤差，即ちθｇとθｒとの差に応じてθoffsetの値が大きく現れる。従って，実際の車両姿勢角θｒを次式により求める。
θｒ＝θｇ＋θoffset                                    …（６）
以上，実際の車両姿勢角θｒを求めるまでがステップＳ４にあたる。
【００１１】
続いて，上記θｒを用いて実際の磁気センサの座標（Ｘｓ，Ｙｓ），及び実際の車両の中心位置の座標（Ｘｒ，Ｙｒ）を算出する（ステップＳ５）。
Ｘｓ＝Ｘｍ−Ｄ・ｓｉｎ（θｒ）                          …（７）
Ｙｓ＝Ｙｍ−Ｄ・ｃｏｓ（θｒ）                          …（８）
Ｘｒ＝Ｘｓ−（Ｌ＋Ｗ／２）・ｃｏｓ（θｒ）              …（９）
Ｙｒ＝Ｙｓ−（Ｌ＋Ｗ／２）・ｓｉｎ（θｒ）              …（１０）
求めた車両中心Ｐｒの座標及び姿勢（Ｘｒ，Ｙｒ，θｒ）を，推定した車両中心Ｐｇの位置及び姿勢（Ｘｇ，Ｙｇ，θｇ）と置き換える（ステップＳ６）。
以上のステップＳ４〜Ｓ６の処理により，磁気マークＭを検出する度に位置・姿勢推定部４において推定された車両の位置・姿勢が補正され，その補正後の推定位置・姿勢を用いて制御部１０により車体１０の走行が制御される（ステップＳ７）ため，常に高精度の制御が行える。
以上のような処理を，目標位置に到着するまで（ステップＳ８），繰り返し行う。
【００１２】
以上説明したように，本実施の形態に係る誘導装置Ａ１は，測定手段として，慣性航法に必要なステア角検出センサ１と移動速度検出センサ２，及び走行経路上に任意の間隔で設けられた磁気マークＭに対する車両の横ズレ量を検出する磁気センサ３のみを用いた構成により，複数の異なる測定手段を重複して取り付けなければならなかった従来のハイブリッド航法と比べてコストを低く抑えることができ，また，測定手段からの出力を評価するための処理が簡単であるため制御遅れ等の心配もない。また，上記走行経路上に設けられた磁気マークＭは任意の間隔で設けることができ，またどのような状況でも確実に検出できるため，一定時間毎，又は一定距離毎の計測が必要な電波航法による位置計測のように走行経路の制約を受けることもない。このように，本実施の形態に係る誘導装置Ａ１は，最小限の測定手段を用いた簡単な処理により車両の位置及び姿勢を高精度で推定することができ，走行経路の制約を受けることがない。
【００１３】
【実施例】
上記実施の形態におけるステップＳ４〜Ｓ６においては，前回の磁気マーク検出時（若しくはスタート地点）から今回の磁気マーク検出時までの走行距離Ｓを用いて車体姿勢角の補正量θoffsetを求めた（（５）式）が，補正量θoffsetの求め方はこれに限られるものではない。例えば，連続する２つの磁気マークＭ１，Ｍ２検出時のそれぞれの車体位置（実際の位置，及び推定の位置）より車体姿勢角の補正量θoffsetを求めてもよい。以下図５，図６を用いて，上記実施の形態におけるステップＳ４〜Ｓ６に対応するステップＳ１１〜Ｓ１８について説明する。尚，ステップＳ１〜Ｓ３，及びステップＳ７，Ｓ８の処理については上記実施の形態と同様である。
図６は磁気センサ３により磁気マークＭ１，Ｍ２が検出された時点での車両１０の実際の位置・姿勢１０ａ１，１０ａ２と，上記位置・姿勢推定部４において推定された位置・姿勢１０ａ１′，１０ａ２′との関係を示している。図中，実線が実際の車両１０の位置・姿勢１０ａ１，１０ａ２，破線が位置・姿勢推定部４において推定された位置・姿勢１０ａ１′，１０ａ２′である。
ここで，上記走行経路Ｒの方向をＸ軸，それと直角の方向をＹ軸，車両の走行経路Ｒに対する姿勢角をθとし，磁気マークＭ２が検出された時点での実際の車両中心Ｐｒ２の座標及び姿勢を（Ｘｒ２，Ｙｒ２，θｒ２），上記位置・姿勢推定部４において推定された車両中心Ｐｇ２の座標及び姿勢を（Ｘｇ２，Ｙｇ２，θｇ２），実際の磁気センサ３の位置Ｐｓ２の座標を（Ｘｓ２，Ｙｓ２），磁気マークＭ２の座標を（Ｘｍ２，Ｙｍ２），上記磁気センサ３による検出値（磁気マークＭ２からの横ズレ量）をＤ２とする。以上の各値のうち，既知の値は，地図情報記憶部７に記憶された磁気マークＭ２の座標（Ｘｍ２，Ｙｍ２），上記位置・姿勢推定部４において推定された車両中心Ｐｇ２の座標及び姿勢（Ｘｇ２，Ｙｇ２，θｇ２），及び上記磁気センサ３による検出値Ｄ２である。
磁気センサ３により磁気マークＭが検出されると，補正部５により以下のような処理（ステップＳ１１〜Ｓ１８）が行われる。
検出された磁気マークがＭ１，即ち最初の磁気マークである場合には，位置・姿勢推定部４により推定された推定位置・姿勢Ｐｇ（Ｘｇ１，Ｙｇ１，θｇ１）と，磁気センサ３の検出値Ｄ１を記憶した後（ステップＳ１１→Ｓ１８），通常どおり上記推定位置・姿勢Ｐｇ及び地図情報記憶部７に記憶された走行経路Ｒに関する情報に基づいて制御部６により走行制御が行われる（ステップＳ７）。
検出された磁気マークがＭ２以降，即ち２つ目以降の磁気マークである場合には（ステップＳ１１），以下に示すステップＳ１２〜Ｓ１７の処理が行われる。
【００１４】
まず，実際の磁気センサ３の位置Ｐｓ２の座標（Ｘｓ２，Ｙｓ２）は次式のようにおくことができる。
Ｘｓ２＝Ｘｍ２−Ｄ２・ｓｉｎ（θｒ２）                    …（１１）
Ｙｓ２＝Ｙｍ２−Ｄ２・ｃｏｓ（θｒ２）                    …（１２）
ところが，上記（１１），（１２）式中，θｒ２は未知である。そこで，θｒ２に代えて，既知の値であるθｇ２を代入すると，次式のようになる。
Ｘｓ２＝Ｘｍ２−Ｄ２・ｓｉｎ（θｇ２）                  …（１１′）
Ｙｓ２＝Ｙｍ２−Ｄ２・ｃｏｓ（θｇ２）                  …（１２′）
ここで，上記（１１′），（１２′）式は，θｇ２＝θｒ２の場合には上記（１１），（１２）式と等価となるため正しい関係を示すが，θｇ２≠θｒ２の場合には当然にその角度差の分だけ誤差を含む，誤った関係式となる。
上記（１１），（１２）式，及び同様の方法で既に求められ記憶されている磁気マークＭ１検出時点での実際の磁気センサ３の位置Ｐｓ１の座標（Ｘｓ１，Ｙｓ１）を用いて，磁気センサ位置Ｐｓ１，Ｐｓ２を通る直線と走行経路Ｒとのなす角度θｓは次式のように求められる（ステップＳ１２，Ｓ１３）。
θｓ＝ａｔａｎ（（Ｙｓ２−Ｙｓ１）／（Ｘｓ２−Ｘｓ１））  …（１３）
また，磁気マークＭ１を検出した時点での推定車体中心位置Ｐｇ１（Ｘｇ１，Ｙｇ１）と磁気マークＭ２を検出した時点での推定車体中心位置Ｐｇ２（Ｘｇ２，Ｙｇ２）とを通る直線と，走行経路Ｒとのなす角度θｇは次式のように求められる（ステップＳ１４）。
θｇ＝ａｔａｎ（（Ｙｇ２−Ｙｇ１）／（Ｘｇ２−Ｘｇ１））  …（１４）
上記（１３），（１４）式より，車体姿勢角の補正量θoffsetは次式のようになる。
θoffset＝Ｋｖａ・（θｓ−θｇ）                          …（１５）
但し，Ｋｖａは比例ゲインであり，ここでは１とする。
ここで，θｓ＝θｇの場合，即ち上記（１１′），（１２′）式が正しい関係を示す場合には，θoffsetは０になる。ところが，θｓ≠θｇの場合，即ち上記（１１′），（１２′）式が誤っている場合には，それらの式に含まれる誤差，即ちθｇ２とθｒ２との差に応じてθoffsetの値が大きく現れる。従って，実際の車両姿勢角θｒ２を次式により求める。
θｒ２＝θｇ２＋θoffset                                  …（１６）
【００１５】
続いて，上記θｒ２を用いて実際の磁気センサの座標（Ｘｓ２，Ｙｓ２），及び実際の車両の中心位置の座標（Ｘｒ２，Ｙｒ２）を算出する（ステップＳ１６）。
Ｘｓ２＝Ｘｍ２−Ｄ２・ｓｉｎ（θｒ２）                    …（１７）
Ｙｓ２＝Ｙｍ２−Ｄ２・ｃｏｓ（θｒ２）                    …（１８）
Ｘｒ２＝Ｘｓ２−（Ｌ＋Ｗ／２）・ｃｏｓ（θｒ２）          …（１９）
Ｙｒ２＝Ｙｓ２−（Ｌ＋Ｗ／２）・ｓｉｎ（θｒ２）          …（２０）
求めた車両中心Ｐｒ２の座標及び姿勢（Ｘｒ２，Ｙｒ２，θｒ２）を，推定した車両中心Ｐｇ２の座標及び姿勢（Ｘｇ２，Ｙｇ２，θｇ２）と置き換える（ステップＳ１７）と共に，次の磁気マークＭ２検出時の処理のために上記車両中心Ｐｇ２の座標及び姿勢（Ｘｇ２，Ｙｇ２，θｇ２）と磁気センサ３の検出値Ｄ２を記憶する（ステップＳ１８）。
上記ステップＳ１１〜Ｓ１８の処理により，磁気マークＭを検出する度に位置・姿勢推定部４において推定された車両の位置・姿勢が補正され，その補正後の推定位置・姿勢を用いて制御部１０により車体１０の走行が制御される（ステップＳ７）ため，常に高精度の制御が行える。
以上説明したように，本実施例に係る補正方法（ステップＳ１１〜Ｓ１８）を用いても上記実施の形態と同様の効果を得ることができる。
【００１６】
【発明の効果】
以上説明したように，本発明に係る誘導装置は，車両に設置され，該車両の相対位置及び姿勢を検出する位置・姿勢検出手段と，車両に設置され，該車両の走行経路上に任意の間隔で設けられた認識マークに対する上記車両の横ズレ量を検出するズレ検出手段と，上記認識マークの位置を予め記憶する認識マーク位置記憶手段と，上記位置・姿勢検出手段からの出力に基づいて上記車両の相対位置及び姿勢を推定する位置・姿勢推定手段と，上記認識マーク位置記憶手段に記憶された認識マーク位置と，上記ズレ検出手段より得られた上記横ズレ量と，上記位置・姿勢推定手段より得られた車両の位置・姿勢推定値とに基づいて上記車両の位置・姿勢推定値を補正する補正手段と，上記補正手段より得られた補正後の車両の位置・姿勢推定値に基づいて，車両を上記走行経路に沿って走行させるように制御する制御手段とを具備してなることを特徴とする誘導装置として構成されているため，複数の異なる測定手段を重複して取り付けなければならなかった従来のハイブリッド航法と比べてコストを低く抑えることができ，また，測定手段からの出力を評価するための処理が簡単であるため制御遅れ等の心配もない。
また，上記走行経路上に設けられた認識マークは任意の間隔で設けることができ，またどのような状況でも確実に検出できるため，一定時間毎，又は一定距離毎の計測が必要な電波航法による位置計測のように走行経路の制約を受けることもない。
このように，本発明に係る誘導装置は，最小限の測定手段を用いた簡単な処理により車両の位置及び姿勢を高精度で推定することができ，走行経路の制約を受けることがない。
【図面の簡単な説明】
【図１】  本発明の実施の形態に係る誘導装置Ａ１の概略構成を示す模式図。
【図２】  上記誘導装置Ａ１の車両誘導処理手順の一例を示すフローチャート。
【図３】  上記誘導装置Ａ１の車両誘導処理手順の説明図。
【図４】  上記誘導装置Ａ１を適用する車両１０の構成の一例を示す模式図。
【図５】  実施例に係る車両誘導処理手順を示すフローチャート。
【図６】  上記実施例に係る車両誘導処理手順の説明図。
【符号の説明】
１…ステア角検出センサ
２…移動速度検出センサ
３…磁気センサ
４…位置・姿勢推定部
５…補正部
６…制御部
７…地図情報記憶部
１０…車両
Ｒ…走行経路
Ｍ…磁気マーク
【図１】
0003932606.tif 000002
【図２】
0003932606.tif 000003
【図３】
0003932606.tif 000004
【図４】
0003932606.tif 000005
【図５】
0003932606.tif 000006
【図６】
0003932606.tif 000007