ホーム > 特許ランキング > 先進モビリティ株式会社
(19)【発行国】日本国特許庁(JP)
(12)【公報種別】公開特許公報(A)
(11)【公開番号】P2022009595
(43)【公開日】2022-01-14
(54)【発明の名称】自動運行方法
(51)【国際特許分類】
B60W 30/10 20060101AFI20220106BHJP
B60W 40/103 20120101ALI20220106BHJP
B60W 40/105 20120101ALI20220106BHJP
B60W 40/114 20120101ALI20220106BHJP
G01C 21/28 20060101ALI20220106BHJP
【ＦＩ】
B60W30/10
B60W40/103
B60W40/105
B60W40/114
G01C21/28
【審査請求】有
【請求項の数】3
【出願形態】ＯＬ
(21)【出願番号】P 2021175672
(22)【出願日】2021-10-27
(62)【分割の表示】P 2018161185の分割
【原出願日】2018-08-30
(71)【出願人】
【識別番号】515213711
【氏名又は名称】先進モビリティ株式会社
(74)【代理人】
【識別番号】100085257
【弁理士】
【氏名又は名称】小山 有
(72)【発明者】
【氏名】江尻 賢治
【テーマコード（参考）】
2F129
3D241
【Ｆターム（参考）】
2F129AA03
2F129BB03
2F129BB22
2F129BB23
2F129BB26
2F129BB33
2F129BB38
2F129BB39
2F129BB40
2F129BB46
2F129BB47
2F129BB62
2F129BB64
2F129BB66
2F129FF02
2F129FF62
3D241BA11
3D241BA64
3D241BB72
3D241BB80
3D241CE01
3D241CE02
3D241CE04
3D241CE05
3D241DB01Z
3D241DB02Z
3D241DB08Z
3D241DB12Z
3D241DB18Z
(57)【要約】
【課題】ＧＰＳ信号による検出位置精度が低下した場合でも、そのまま自動運行を継続することができ自動運行方法を提供する。
【解決手段】
走行中にＧＰＳ信号を受信してリアルタイムで自車両の位置を取得し、取得した自車両の位置を予め定めた目標軌跡に一致するように修正して走行するにあたり、前記ＧＰＳ信号によって検出される自車両の位置精度の信頼性が低下した場合に、ジャイロと加速度計によって構成される慣性計測装置（ＩＭＵ：Inertial Measurement Unit）による走行に切り換えて走行するようにした。
【選択図】図５
【解決手段】
走行中にＧＰＳ信号を受信してリアルタイムで自車両の位置を取得し、取得した自車両の位置を予め定めた目標軌跡に一致するように修正して走行するにあたり、前記ＧＰＳ信号によって検出される自車両の位置精度の信頼性が低下した場合に、ジャイロと加速度計によって構成される慣性計測装置（ＩＭＵ：Inertial Measurement Unit）による走行に切り換えて走行するようにした。
【選択図】図５
【特許請求の範囲】
【請求項1】
走行中にＧＰＳ信号を受信してリアルタイムで自車両の位置を取得し、取得した自車両の位置を予め定めた目標軌跡に乗るように修正して走行し、前記ＧＰＳ信号によって検出される自車両の位置精度の信頼性が低下したことを感知した場合、慣性計測装置（ＩＭＵ）による走行に切り替え、この慣性計測装置（ＩＭＵ）による座標及び方位角を整合させて走行するにあたり、前記ＧＰＳ信号によって検出される自車両の位置精度の信頼性をＮＭＥＡ（National Marine Electronic Association）０１８３に示される位置特定品質が３以上になっているか否かを走行中は常時判断し、走行中に３以上になっていないと判断した場合には速度を維持して慣性計測装置（ＩＭＵ）による走行に切り替え、この慣性計測装置（ＩＭＵ）による走行が所定距離を超えたと判断した場合には慣性計測装置（ＩＭＵ）による走行速度を減速し、走行速度を減速した後に、更に前記位置特定品質が３以上になっていないと判断した場合には停車し、前記位置特定品質が３以上に回復するのを待ってＧＰＳ走行を開始する自動運行方法であって、
前記慣性計測装置（ＩＭＵ）による座標及び方位角を得るために必要な車体横滑り角を、車両モデルから導出される式またはモデル同定実験式を用いて算出することを特徴とする自動運行方法。
前記慣性計測装置（ＩＭＵ）による座標及び方位角を得るために必要な車体横滑り角を、車両モデルから導出される式またはモデル同定実験式を用いて算出することを特徴とする自動運行方法。
【請求項2】
走行中にＧＰＳ信号を受信してリアルタイムで自車両の位置を取得し、取得した自車両の位置を予め定めた目標軌跡に乗るように修正して走行し、前記ＧＰＳ信号によって検出される自車両の位置精度の信頼性が低下したことを感知した場合、慣性計測装置（ＩＭＵ）による走行に切り替え、この慣性計測装置（ＩＭＵ）による座標及び方位角を整合させて走行するにあたり、前記ＧＰＳ信号によって検出される自車両の位置精度の信頼性をＮＭＥＡ（National Marine Electronic Association）０１８３に示される位置特定品質が３以上になっているか否かを走行中は常時判断し、走行中に３以上になっていないと判断した場合には速度を維持して慣性計測装置（ＩＭＵ）による走行に切り替え、この慣性計測装置（ＩＭＵ）による走行が所定距離を超えたと判断した場合には慣性計測装置（ＩＭＵ）による走行速度を減速し、走行速度を減速した後に、更に前記位置特定品質が３以上になっていないと判断した場合には停車し、前記位置特定品質が３以上に回復するのを待ってＧＰＳ走行を開始する自動運行方法であって、
前記慣性計測装置（ＩＭＵ）に切り替えた走行では、慣性計測装置（ＩＭＵ）にて取得した自車両の位置と目標軌跡上の点とをクロソイド曲線などで曲線補間して走行することを特徴とする自動運行方法。
前記慣性計測装置（ＩＭＵ）に切り替えた走行では、慣性計測装置（ＩＭＵ）にて取得した自車両の位置と目標軌跡上の点とをクロソイド曲線などで曲線補間して走行することを特徴とする自動運行方法。
【請求項3】
請求項１又は２に記載の自動運行方法において、前記ＧＰＳ走行と慣性計測装置（ＩＭＵ）による走行の切替時には、実運行に入る試運行の段階でＧＰＳの測位精度が正しく受信できる運行経路上の場所を把握し、その場所におけるＧＰＳによる重心座標（Ｘn,Ｙn)と慣性計測装置（ＩＭＵ）による重心座標（Ｘs,Ｙs）の差分及び姿勢角φ0とφsの差分を経路ＩＤに対応づけて把握し、その差分を調整項として予めＩＭＵデータ値に加え、この調整値を用いて切替えることを特徴とする自動運行方法。
【発明の詳細な説明】
【技術分野】
【0001】
本発明はＧＰＳ信号を利用した自動運行方法に関する。
【背景技術】
【0002】
バスなどの決められた経路を自動運行する方法として、予め目標軌跡を作成し、この目標軌跡に沿って車両を走行させるために、ＧＰＳ信号を受信して自車両の位置をリアルタイムで取得し、この取得した自車両の位置が目標軌跡上にあるか否かを判断し、目標軌跡を辿るように位置修正して走行することが行われている。
【0003】
特許文献１には、自動運転ではなくドライバーによる運転中に、ＧＰＳ信号に基づく位置情報と慣性計測装置（ＩＭＵ：Inertial Measurement Unit）に基づく位置情報とを比較し、その差が大きくなった場合に、ＧＰＳ信号の受信状態が劣化する遮蔽領域（トンネルなど）を走行していると判断し、遮蔽領域に入ったことをドライバーに早期に知らせることが記載されている。
【0004】
特許文献２には、ＭＥＭＳ慣性センサからの３軸角速度信号及び３軸加速度信号を慣性航法計算部で受け、この慣性航法計算部からの各出力とＧＰＳセンサとタイヤ速度センサとステアリング角センサからの各出力とを減算して誤差を推定して補正することが記載されている。
【0005】
特許文献３には、車載カメラが取得した画像データから路面画像を生成し、また道路情報から疑似路面画像を生成し、これら路面画像と疑似路面画像との相関値が一定以上になった場合（ズレが大きくなったと判断した場合）には、ＧＰＳ信号に基づく自動運転からドライバーによるマニュアル運転に切り替えることが記載されている。
同じく特許文献４にもＧＰＳ信号に基づく自動運転とドライバーによるマニュアル運転との切替えについて記載されている。
同じく特許文献４にもＧＰＳ信号に基づく自動運転とドライバーによるマニュアル運転との切替えについて記載されている。
【0006】
非特許文献１及び非特許文献２には、車両がＲＴＫ（Real Time Kinematics）－ＧＰＳで走行する内容の説明がされている。
【先行技術文献】
【特許文献】
【0007】
【特許文献1】特開２０１６－０３８３７９号公報
【特許文献2】特開２０１６－０１７７９６号公報
【特許文献3】特開２０１８－０７７７７１号公報
【特許文献4】特開２０１６－１２４５４２号公報
【非特許文献】
【0008】
【非特許文献1】「ＲＴＫ－ＧＰＳの原理と応用」浪江宏宗http://www.nda.ac.jp/~nami/research/pdf/CGSIC2001.pdf
【非特許文献2】「ＲＴＫ－ＧＰＳのデータ交換に関する研究」佐田達典ほか 土木情報システム論文集 Vol.8 1999.p57-64
【発明の概要】
【発明が解決しようとする課題】
【0009】
ＧＰＳ信号によって特定される位置（絶対位置）は、現在では誤差が数ｃｍまで向上している。しかしながら、高精度に位置が特定されるのはＧＰＳ信号を高感度で受信できることが前提条件である。
【0010】
ＧＰＳシステムには障害物（建物、樹木、電線など）の影響でマルチパスと呼ばれるエラーが生じる。このマルチパスが発生すると、位置を高精度に特定できない。 人間が運転している場合には、自車両の位置情報に誤差が生じても運転自体には特に支障がない。このため特許文献１及び２ではその旨をドライバーに知らせるに止まる。
【0011】
しかしながらＧＰＳ信号に基づいて自動運転している場合は、ＧＰＳ信号によって特定される位置誤差が直接運転に反映されてしまう。誤差を認識しない状態で運転を継続することは事故につながる。
【0012】
また、従来技術においても慣性センサ（ＩＭＵ）に基づく位置情報との比較によってＧＰＳ信号に基づく位置情報の信頼性が分かるので、信頼性が低下した時点で自動運転を停止し、信号強度が回復するまで待つことも考えられる。しかしながら、これでは自動運転車両が信号や渋滞以外の理由で頻繁に停止し、周囲の車両との運転の調和がとれず、却って渋滞や事故の原因にもつながる。
【0013】
特許文献３には自動運転からマニュアル運転に切り替えることが記載されているので停止による交通の混乱は避けられるが、常にドライバーが同乗することが要求されるので完全な自動運転にはならない。
【0014】
非特許文献１、２には、ネットワーク型(RTK: Real Time Kinematics)ＧＰＳについての記載があるが、自動運転中にＧＰＳ信号の感度が低下した場合の対処については何ら示唆していない。
【課題を解決するための手段】
【0015】
上記の課題を解決するため、本発明に係る自動運行方法は、走行中にＧＰＳ信号を受信してリアルタイムで自車両の位置を取得し、取得した自車両の位置を予め定めた目標軌跡に沿って走行するにあたり、前記ＧＰＳ信号によって検出される自車両の位置精度の信頼性が低下したことを感知した場合に、ジャイロと加速度計によって構成される慣性計測装置（ＩＭＵ：Inertial Measurement Unit）による走行に切り換え、この慣性計測装置（ＩＭＵ）による走行では、ＧＰＳシステムによる座標及び方位角と慣性計測装置（ＩＭＵ）による座標及び方位角とを整合させて走行するようにした。
前記慣性計測装置（ＩＭＵ）による座標及び方位角を得るために必要な車体横滑り角を、車両モデルから導出される式またはモデル同定実験式を用いて算出する。
前記慣性計測装置（ＩＭＵ）による座標及び方位角を得るために必要な車体横滑り角を、車両モデルから導出される式またはモデル同定実験式を用いて算出する。
【0016】
前記慣性計測装置（ＩＭＵ）に切り替えた走行では、慣性計測装置（ＩＭＵ）にて取得した自車両の位置と目標軌跡上の点とをクロソイド曲線などで曲線補間し、この補間した曲線に沿って走行し目標軌跡に乗る。
【0017】
慣性計測装置に切り換えた後の走行速度はＧＰＳ走行と同じでもよいが、一般的に慣性計測装置による位置精度はＧＰＳによる位置精度よりも低いため、慣性計測装置に切り替えた走行では、安全走行が確認できる速度まで速度を落として走行するのが好ましい。
また、ＧＰＳ信号によって検出される位置精度に信頼性があるか否かの判断は、例えば、NMEA(National Marine Electronic Association)0183の位置特定品質を基準として判断する。
また、ＧＰＳ信号によって検出される位置精度に信頼性があるか否かの判断は、例えば、NMEA(National Marine Electronic Association)0183の位置特定品質を基準として判断する。
【0018】
目標軌跡から外れた自車位置から目標軌跡に戻るには、慣性計測装置（ＩＭＵ）によって検出される偏揺角速度（ヨーレイト）、車体横すべり角、及び車速を計算式に代入して取得される。
即ち、自車位置から目標軌跡に向かい目標軌跡を辿るところで、自車の位置および姿勢変化をリアルタイムで把握し予測する計算をＩＭＵが行う。即ち、前記ヨーレイトと車体横すべり角及び車速を、車両モデル式ないしスタビリティファクタを用いる式に代入して行われる。ここで、スタビリティファクタとは、車両モデルから算出ないし実験によって得られ、経路の曲率に対して必要な操舵角を得ることができる数値である。
即ち、自車位置から目標軌跡に向かい目標軌跡を辿るところで、自車の位置および姿勢変化をリアルタイムで把握し予測する計算をＩＭＵが行う。即ち、前記ヨーレイトと車体横すべり角及び車速を、車両モデル式ないしスタビリティファクタを用いる式に代入して行われる。ここで、スタビリティファクタとは、車両モデルから算出ないし実験によって得られ、経路の曲率に対して必要な操舵角を得ることができる数値である。
【発明の効果】
【0019】
本発明に係る自動運行方法によれば、ＧＰＳ信号による検出位置精度が低下した場合でも、そのまま自動運行を継続することができ、ドライバーの同乗を必要としない完全な自動運行が可能となる。
【0020】
また、本発明に係る自動運行方法は、ＧＰＳ信号による検出位置精度が低下した場合でも、基本的には車両を停止しないため、周囲の車両との調和がとれ、渋滞や事故の原因を誘発しない。
【図面の簡単な説明】
【0021】
【図1】ＧＰＳデータから作成した目標軌跡の説明図
【図2】ＶＲＳ－ＧＰＳの概念の説明図
【図3】ＧＰＳデータ（NMEA0183sentences）の構成の説明図
【図4】目標軌跡の曲率から実舵角を求める方法を説明した
【図5】制御フローチャート
【図6】ＧＰＳ走行と慣性計測装置（ＩＭＵ）走行の切替説明図
【図7】表示器の説明図
【発明を実施するための形態】
【0022】
先ず、本発明においては予め目標軌跡を作成しておく。目標軌跡の作成は図１に示すように、運行を予定する経路を手動運転で走行してＧＰＳデータを取得する。取得したデータに基づき、経路始点から終点までの複数のポイント（Ｎ点）の緯度と経度と方位角を取得し、各ポイントにＩＤ番号をつけて経路軌跡を作成し、これを目標軌跡とし目標速度で走行する。
【0023】
上記の目標軌跡に沿って走行が開始されると、ＶＲＳ（Virtual Reference Station）－ＧＰＳによって、リアルタイムで自車両の位置を測定する。
ここで、ＶＲＳ－ＧＰＳは図２に示すように、予め設置した基準点（基準局）で観測を行い、ＶＲＳセンターに観測データを送る。観測データが送られてきたＶＲＳセンターでは基準局で囲まれた△ａｂｃ内での補正データを作成する。一方、車両（移動局）は搭載されたＲＴＫ－ＧＰＳからＶＲＳセンターを呼び出して現在位置を単独測位でＶＲＳセンターに教える。ＶＲＳセンターは単独測位した位置が基準局となるように補正データを車両に送り返す。これで単独測位した位置が既知局（仮想基準位置）となり、車両はこれを基準にＲＴＫ－ＧＰＳで走行する。
ここで、ＶＲＳ－ＧＰＳは図２に示すように、予め設置した基準点（基準局）で観測を行い、ＶＲＳセンターに観測データを送る。観測データが送られてきたＶＲＳセンターでは基準局で囲まれた△ａｂｃ内での補正データを作成する。一方、車両（移動局）は搭載されたＲＴＫ－ＧＰＳからＶＲＳセンターを呼び出して現在位置を単独測位でＶＲＳセンターに教える。ＶＲＳセンターは単独測位した位置が基準局となるように補正データを車両に送り返す。これで単独測位した位置が既知局（仮想基準位置）となり、車両はこれを基準にＲＴＫ－ＧＰＳで走行する。
【0024】
本実施例の場合、車両が取得するＧＰＳデータはNMEA0183sentencesフォーマットで取得する。NMEA0183には、もともと音波探知機やソナー、風速計、ジャイロコンパスなどの電子装置をつなぐためのインターフェイスとして規格化されたもので、位置情報のみでなく誤差に関する情報も含まれている。
【0025】
図３に示すように、ＧＰＳデータ（NMEA0183 sentences）は２５種のセンテンス（図３の左側）で構成されている。各センテンスは、「＄」で始まり「＊」で終わる。「＊」の後の１６進数はエラーの有無を確認するものである。
【0026】
２５種のセンテンスのうち「ＣＧＡ」に、時間、位置及び位置決定関連データが入っている。協定世界時での時刻、経度、北緯、経度、東経と続きその次に「数字：が並ぶ。この数字は位置特定品質が悪い順に、０、１、２、３、４である。
また「ＧＳＴ」に位置誤差統計データがあり、誤差楕円長軸、短軸、緯度、経度、高度の誤差が１シグマ（メートル単位）で表示される。即ち、「ＧＧＡ」又は「ＧＳＴ」から誤差の大きさを知ることができる。
本実施例は、「ＧＧＡ」の位置データを目標軌跡生成に使用し、「ＧＧＡ」の誤差数字が閾値よりも小さく（３未満）になった場合に、ＧＰＳ走行からＩＭＵ走行に切り換え、同時にＩＭＵ走行でも支障がない速度まで速度を下げる。「ＧＧＡ」の誤差数値に代えて「ＧＳＴ」を用いてもよい。
また「ＧＳＴ」に位置誤差統計データがあり、誤差楕円長軸、短軸、緯度、経度、高度の誤差が１シグマ（メートル単位）で表示される。即ち、「ＧＧＡ」又は「ＧＳＴ」から誤差の大きさを知ることができる。
本実施例は、「ＧＧＡ」の位置データを目標軌跡生成に使用し、「ＧＧＡ」の誤差数字が閾値よりも小さく（３未満）になった場合に、ＧＰＳ走行からＩＭＵ走行に切り換え、同時にＩＭＵ走行でも支障がない速度まで速度を下げる。「ＧＧＡ」の誤差数値に代えて「ＧＳＴ」を用いてもよい。
【0027】
外れた位置から目標軌跡に合流して目標軌跡に沿って走行するには、目標軌跡の曲率から実舵角を求める。図４の左部分（定常円旋回）に示すように、旋回する円の中心まわりの公転運動と車両重心点まわりの自転運動をする。公転速度と自転速度の和が偏揺角速度（ヨーレイト、γ）になる。車両速度の二乗及び車両質量に比例する遠心力が生じ、その遠心力につり合う求心力をタイヤが発生して旋回する。
【0028】
タイヤが発生する求心力はタイヤ横すべり角に比例する摩擦力（コーナリングフォース）による。タイヤ横すべり角（β）は前輪実舵角によって制御されるが、後輪タイヤには操舵機構の備えがないので車体が横すべりすることによって後輪横すべり角を生じさせる。この状態でハンドルを固定して車速を徐々に上昇させていくと回転半径が増加し、車体横すべり角がマイナス側からプラス側へ変化する。その関係のグラフを図の中央に示す。
【0029】
車速の二乗に対する回転半径の変化勾配をスタビリティファクタ（ＫＳＦ）と称し、車体横すべり角の変化を横すべり係数（ＫβＯ）と称する。車両の自車位置変化、速度変化はＧＰＳで捉えることができ、車載するＩＭＵのジャイロ及び加速度計の出力値から自車位置、車両横すべり角(β)が計算把握される。
【0030】
ＧＰＳによって得られる車両速度は、図に示す絶対速度（ｖ）が求められるのに対してＩＭＵから求められる速度は、前後加速度の積分値としての前後速度（ｖｘ）と横加速度の積分値としての横速度（ｖｙ）になる。前後速度（ｖｙ）と横速度（ｖｘ）を合成して絶対速度（ｖ）を得、ｖｙ/ｖｘの比から車体横すべり角（β）を得る。
【0031】
ＩＭＵの場合は計算によって絶対速度（ｖ）を求めるため精度はＧＰＳに劣るが、ＧＰＳは車体横すべり角を求めることができないので両者長短がある。ＧＰＳとＩＭＵを組み合わせることで長短補い合って精度が向上する。図の右側に極低速時の回転半径（ＲО）、横すべり角（βО）と実舵角δの関係を示す。
【0032】
図５は本発明に係る自動運行方法のフローチャートである。先ずステップ１で制御が開始され、ステップ２で慣性計測装置（ＩＭＵ）のキャリブレーションを実施して、ＧＰＳとＩＭＵとの整合のための調整項を決めてステップ３へ進む。
【0033】
ステップ３ではＧＰＳデータ（NMEA0183 sentences）のクオリティを確認し位置特定品質が“３以上”であれば、ステップ４へ進みＧＰＳで既定の速度で走行し、ステップ５からステップ６の停止まで走行する。
【0034】
一方、位置特定品質が“３未満”の場合には、ステップ７に進みＧＰＳ走行からＩＭＵ走行へ切替えて既定目標車速（減速）を維持してステップ８へ進む。ステップ８でクオリティが“３以上”に回復していれば、ステップ９にてＩＭＵ走行からＧＰＳ走行に切替えて前記ステップ４へ戻る。
【0035】
ステップ８でクオリティが“３以上”に回復していない場合には、ステップ１０へ進みＩＭＵ走行の距離が所定距離を超えているかを確認して、超えていない場合にはであれば車速を低くしてＩＭＵ走行を継続し、更にステップ１２でクオリティを確認し“３以上”に回復していればステップ４へ戻る。
【0036】
クオリティを確認し“３以上”に回復していなければ、ステップ１３にて停車してステップ３へ戻りＧＰＳクオリティの回復を待ち、回復したら、上述の工程を繰り返す。
【0037】
図６は、ＧＰＳ走行と慣性計測装置（ＩＭＵ）による走行の切替説明図である。図の上側にＧＰＳによる走行を示し、下側に慣性計測装置（ＩＭＵ）による走行を示す。 （１）のＧＰＳ目標軌跡（目標軌跡）のところでは、予め手動運転を実施して経路ＧＰＳデータを取得してオフラインで目標軌跡（Xo、Yo、φo）を作成する。（２）のＧＰＳ制御データのところで、ＧＰＳデータを測位精度と共に取得して、リアルタイムで自車位置（Xｎ、Yｎ、φｎ）を確認しつつ目標軌跡を辿る。
【0038】
目標軌跡追跡の仕方を（３）に示す。自車両は、Ｐn点に居て、目標軌跡上のＰv点に乗ろうとしている。Ｐn点における姿勢角φnが分かり、Ｐv点における姿勢角φ0が分かっているのでＰn－Ｐv間の曲線補間が出来る。曲線補間はクロソイド曲線が好ましいが、限定はしない。
【0039】
ＧＰＳの測位精度が低下するとＰn点の座標(Ｘn,Ｙn)が分からなくなり、Ｐn点での方位角、即ち速度ベクトル方向が分からなくなるので、慣性計測装置（ＩＭＵ）によるＰn点の座標（Ｘs,Ｙs）及び姿勢角φsを用いて走行する。
【0040】
図６の（１０）にて座標(Ｘn,Ｙn)と（Ｘs,Ｙs）の切替、姿勢角φ0とφsの切替を行うが、その切替は不用意には行えない。実運行に入る試運行の段階でＧＰＳの測位精度が正しく受信できる運行経路上の場所を把握して、その場所における座標(Ｘn,Ｙn)と（Ｘs,Ｙs）及び姿勢角φ0とφsとの差分を経路ＩＤに対応づけて把握して、その差分を調整項として、ＩＭＵデータ値に加えた調整値を用いて切替える。
【0041】
図６の（４）にてＧＰＳの測位精度、（５）にてＧＰＳによる車速が取り込まれる。この車速は（４）の測位精度と共に車速調整マップに入り測位精度に順じて調整される。
【0042】
図５で述べた様に測位精度が「３」に満たない場合は、ＧＰＳからＩＭＵに切替えて所定距離継続走行する。その間に測位精度が回復しない場合は減速する。
また（６）の慣性計測装置（ＩＭＵ）は前記したように加速度計と車輪速とジャイロによるヨーレイトによって自車位置（ＸＳ、ＹＳ、φＳ）を算出する。
また（６）の慣性計測装置（ＩＭＵ）は前記したように加速度計と車輪速とジャイロによるヨーレイトによって自車位置（ＸＳ、ＹＳ、φＳ）を算出する。
【0043】
測位精度が低下している条件下では、ＧＰＳによる絶対車速ｖは得られないので、車輪速から得られる前後速度ｖｘに車体横すべり角βの余弦cosβでで除算して絶対車速ｖを求める。そのβは、（８）による横加速度を前後加速度で除算から得る。又は、（９）の近似式によるβも求めて備えとする。（８）によるβを用いるか（９）によるβ（式１）を用いるかは試運行を繰返し判断して定める。
【0044】
【数1】
【0045】
（１１）にβの切替スイッチを備えて、（７）にてＧＰＳによる絶対車速ｖをモデル式に代入して求めるβ（式２）と、加速度計または近似式（式１）から求めるβとを、ＧＰＳ測位精度にもとづき切替える。
【0046】
【数2】
【0047】
（１２）のジャイロによって検出されるヨーレイトｒを積分してヨー角ψを求め、（１３）にて（１１）のβと合算するとその合算値は方位角φｓになる。φｓの余弦に車速を乗じて（１４）の慣性計測装置（ＩＭＵ）による現在位置ＸＳ、φＳの正弦に車速を乗じて（１５）の慣性計測装置（ＩＭＵ）による現在位置ＹＳを得る。ＧＰＳによるＸｎ、Ｙｎ、φｎと慣性計測装置（ＩＭＵ）によるＸＳ、ＹＳ、φＳの整合が理想であるが、現実には上述の調整項が必要になる。その調整項として前述の（１０）を設ける。（３）においてＸＳ、ＹＳ、φＳによる制御がＸｎ、Ｙｎ、φｎに行われる。
【0048】
【図1】
【図2】
【図3】
【図4】
【図5】
【図6】
【図7】