JP6955315B2 - 自動運転における慣性航法 - Google Patents

Description

本発明は、自動運転を行う際に、カメラ等の視覚センサ方式、GPS方式、磁気マーカ方式と併用して、これらの方式を補う冗長構造を構成する慣性航法に関する。

自動運転車両が、自車位置を認識して進路をとる方法として、車両重心に生じる公転運動と自転運動から軌道計算をして自車位置の座標と方位を認識する方法（以下、慣性航法と称す）、カメラ等の視覚センサによる方法、GPSによる方法、更に道路に埋設した磁石の位置検出による方法がある。

GPSも磁石も慣性航法も経路上の障害認識のため視覚センサ情報が必要である。一方、GPS座標も、磁石マーカも、視覚マーカ(目印)も、いわば「道標」であり、現在位置の認識から次の「道標」への移動（道標から道標までの移動）のため慣性航法が必要になる。

GPSは、トンネル、陸橋、ネット、樹木の下での電波の受信不能乃至強度低下、ビルの谷間や崖横などでの反射による乱れへの対処が必要になる。その際は慣性航法と視覚センサによって継続航行する信頼性設計が期待される。

道路への磁石マーカ埋設には限りがある。磁石からGPSへ或いは慣性航法への切り替え機能、或いは並列機能する信頼性設計が期待される。

慣性航法は、移動体の重心に作用する上下（ｚ軸）前後（ｘ軸）左右（ｙ軸）三軸方向の力と三軸まわりのモーメントからその重心の（目標となる位置までの）移動と姿勢方位の変化を算出して航行する方法である。

陸上移動体の一つである自動車には、空力六分力の揚力、抗力、横力と偏揺モーメント(Mz、ヨーイングモーメント)、横揺れモーメント（Mx、ローリングモーメント）、縦揺れモーメント（My、ピッチングモーメント）及び、路面荷重が作用する。

路面荷重には、車両が発する駆動・制動・旋回にかかわる出力に対する反力、重力加速度の路面傾斜角分力によって作用する路面反力、及び路面不整（路面係数）のために生じる振動荷重がある。

慣性航法を実施するには、空力と路面荷重とが作用して重心点に生じる慣性力と慣性モーメントをジャイロと加速度計を装備して検出して、ノイズ成分を除去して、真に軌跡算出にかかわる成分を取り出して先ず、自己位置を算出する。

算出した自己位置から、目標経路に流入する流入経路曲率を算出して、その経路曲率に相応する操舵を加える。

道路には、排水のためのと、旋回遠心力に見合った求心力を生じさせるための横断勾配（カント）がつけられている。そのために、そこを走る車両は“あて舵”をして車両姿勢を“カント山側”に向けて走行する。カント山側に向く姿勢をとるための“あて舵”が左カーブと右カーブで符号の単純反転にはならない。そのためカント路上ではハンドル中立が変動するので、これに対応する制御が必要になる。

ハンドルに加えられた操舵は、操舵系と懸架系を連結するリンケージに介在する軌跡の不整（リンク干渉と称す）、ヒステリシスによる誤差を含み操舵輪に至る。自己位置検出の誤差、操舵誤差を少なくして目標軌跡を辿る技術が求められる。

特開2016-17796 号公報

籾山冨士男ほか；自動運転大型トラックのための横運動モデルの積載状態推定、自動車技術会論文集,Vol.44,No.6,November 2013.,No.20134847,p.1377-1382. 籾山冨士男ほか；自動運転トラックのカント路車線維持解析とモデリング、自動車技術会論文集、Vol.45,No.6,November 2014.,No.20144800,p.1027-1034.

慣性航法は、車両重心まわりに生じる回転遍揺角（ヨー角）と車体横すべり角の和の余弦成分（COS）を積分して得られる前後移動距離と、正弦成分（SIN）を積分して得られる左右移動距離の比から方位角、その合成値から移動距離を求めることによって刻々と変わる自己位置を算出する。その位置から次の目標に至る操舵角を求めて操舵を加えて生じた回転遍揺角（ヨー角）と車体横すべり角からその結果としての自己位置を計算し更新しつつ進む方法である。

ヨー角は、ヨーレイトセンサによって検出されるヨーレイトを積分して得られる。横すべり角は、横加速度を積分して横速度を得て前後加速度を積分して前後速度を得て、得られた横速度を得られた前後速度で除算して得る。

しかしながら、慣性航法には次の課題がある。
（１）前後移動距離は車輪パルスによって車輪1回転当たりの移動距離を積算し、それを時間で除算して速度を求めるのであるが、例えば、トラック・バス用タイヤ245/70Rの例での1パルス当たりの移動距離は2.5m〜0.4m（JISD5601-1992に準拠してパルス数が４〜25の場合）になるから、このパルスを数えて、定位置に停車する制御の精度保証は粗すぎて困難になる。定位置に停車する制御にはパルス数が多いABSブレーキ用パルス数を利用するなどの策が必要である。また、一回転当たりの移動距離の誤差が積算されて、移動距離誤差、速度誤差を生じる。GPSの距離と比較して、誤差補正する手段を備える必要がある。

（２）車両運動には車体横すべり角を伴うので、車体前後軸（x軸）方向の前後速度（車輪速）と車体横（ｙ軸）方向の横速度が生じるので、上記（１）の補正に加えて、この前後速度（車輪速）と横速度を合成した車速と前後速度の違いを考慮する必要がある。この合成車速がGPSによる車速に対応し、慣性航法計算はこの合成車速が用いられる。車速と車輪速との違いに配慮してGPS車速を正にしての誤差補正が必要である。

（３）ヨーレイトセンサと加速度センサは、重心位置に装備しなければならないが、積載量・乗客数によって、車両総重量（自重）及びその重心位置が変化するので、自重を検出する方法、センサ出力値を重心位置に換算して用いる方法、及び、軌跡計算に用いる車両モデル諸元へ反映する方法を備える必要がある。この備えの必要は、GPSについても同様である。

（４）道路の前後勾配が変化すると、ヨーレイトセンサ、加速度センサの出力に勾配変化成分がのるのでセンサ出力を補正して制御に用いることが必要になり、車両には前後軸重移動が生じ、それによる操舵応答性が変化するので、勾配を検出する方法と勾配に応じて前後荷重配分を補正する制御モデルが必要になる。

（５）操舵を加えて生じた回転遍揺角（ヨー角）と車体横すべり角からその結果としての自己位置を計算し更新しつつ進むについて、当該車両が2軸車、3軸車、4軸車と仕様が異なる場合そのそれぞれの車両仕様違いに対応しての操舵が加えられねばならない。車両仕様違い及び稼働条件違いに対応する制御モデルが必要である。

（６）車体横すべり角は、車両重心点における横速度を前後速度で除した値である。原理的には、横加速度と前後加速度をそれぞれ積分して横速度と前後速度を出して、それらの比をとることで求められるものであるが、前後勾配・横勾配がある場所では重力加速成分が前後速度・横速度に重畳して誤差を生じる。路面不整による振動（ノイズ）が重畳して誤差を生じる。横加速度は、そもそも値が小さい、ゼロ点補正変動などの影響が出やすい。横すべり角を安定的に捉える手段の備えが必要である。

（７）実車の操舵系には、ガタ（ヒステリシス）があり、実路には、カント（横断勾配）があるので、実車はカント山側に「あて舵」し、山側に「車両姿勢角」をとり直進するので、実車とセンサとモデルのゼロ点を整合させる方法及びガタ（ヒステリシス）に対処する方法が必要である。

（８）上述の視覚センサ方式、磁気マーカ方式、GPS方式との並列冗長ないし待機冗長で機能するについて各方式間の誤差に対処して円滑に切替機能する方法が必要である。

しかしながら、特許文献１は、MEMS（Micro Electro-Mechanical System）慣性センサ、GPSセンサ、タイヤ速度センサ、ステアリング角センサ、カルマンフィルタを備えて、各出力の誤差を推定して補正するとするも、誤差の捉え方、補正の仕方、及びカルマンフィルタによる実車仕様との整合には言及していない。またGPSを含む構成としているため、GPS受信強度低下状態場面での対応に供するものではない。

非特許文献1は、トラックのバウンシングとピッチングの固有振動数を検出して積載量と重心位置の変化を推定する方法を紹介している。実用面で車載コンピュータの能力にかかわる。

非特許文献2は、路面カントの検出法と、それに対応する車両モデルについて紹介している。慣性航法センサ出力に含まれる路面カントの影響分離に応用できるが、上記（１）〜（８）の問題を解決するものではない。

上記の課題を解決するため、本願の第１発明は、車両の進行要素（前後運動要素）と、車両の方向要素（横運動要素）で構成され、視覚センサ、磁気センサまたはGPSと冗長構造を構成する慣性航法であって、前記進行要素として前後車速或いは車輪速を検出し、前記方向要素として車体横すべり角を検出し、前記前後車速或いは車輪速を前記車体横すべり角の余弦で除して車速を算出し、算出した車速とハンドル角と道路横断勾配（カント）を状態方程式に入力して車体遍揺角速度（ヨーレイト）を算出し、前記車速と目標経路の曲率と道路の横断勾配（カント）を代数式に入力してハンドル角を算出し、算出したハンドル角を前記状態方程式に代入して自車の位置座標と進路方位を算出し、更に、前記前後車速或いは車輪速は車速パルス（JIS5601規格）または車輪速パルスを検出することで行い、低速域においては、パルス間隔が狭いパルスに切り替えることで低速走行に必要な車速精度を得るようにした。

また、本願の第２発明は、車両の進行要素（前後運動要素）と、車両の方向要素（横運動要素）で構成され、視覚センサ、磁気センサまたはGPSと冗長構造を構成する慣性航法であって、前記進行要素として前後車速或いは車輪速を検出し、前記方向要素として車体横すべり角を検出し、前記前後車速或いは車輪速を前記車体横すべり角の余弦で除して車速を算出し、算出した車速とハンドル角と道路横断勾配（カント）を状態方程式に入力して車体遍揺角速度（ヨーレイト）を算出し、前記車速と目標経路の曲率と道路の横断勾配（カント）を代数式に入力してハンドル角を算出し、算出したハンドル角を前記状態方程式に代入して自車の位置座標と進路方位を算出し、更に、積載量・乗客数によって変化する車両総重量（自重）及びその重心位置の変化を、後述する式（１０）により自重を検出し、エアサスペンションのエア圧により重心位置を検出して慣性センサ位置、GPS位置にたいする重心位置の差異を車両軌跡計算に反映するようにした。

また、本願の第３発明は、車両の進行要素（前後運動要素）と、車両の方向要素（横運動要素）で構成され、視覚センサ、磁気センサまたはGPSと冗長構造を構成する慣性航法であって、前記進行要素として前後車速或いは車輪速を検出し、前記方向要素として車体横すべり角を検出し、前記前後車速或いは車輪速を前記車体横すべり角の余弦で除して車速を算出し、算出した車速とハンドル角と道路横断勾配（カント）を状態方程式に入力して車体遍揺角速度（ヨーレイト）を算出し、前記車速と目標経路の曲率と道路の横断勾配（カント）を代数式に入力してハンドル角を算出し、算出したハンドル角を前記状態方程式に代入して自車の位置座標と進路方位を算出し、更に、道路の勾配変化に対応してヨーレイトセンサ、加速度センサの出力補正、車両の軌跡計算のための軸荷重補正をするようにした。

また、本願の第４発明は、車両の進行要素（前後運動要素）と、車両の方向要素（横運動要素）で構成され、視覚センサ、磁気センサまたはGPSと冗長構造を構成する慣性航法であって、前記進行要素として前後車速或いは車輪速を検出し、前記方向要素として車体横すべり角を検出し、前記前後車速或いは車輪速を前記車体横すべり角の余弦で除して車速を算出し、算出した車速とハンドル角と道路横断勾配（カント）を状態方程式に入力して車体遍揺角速度（ヨーレイト）を算出し、前記車速と目標経路の曲率と道路の横断勾配（カント）を代数式に入力してハンドル角を算出し、算出したハンドル角を前記状態方程式に代入して自車の位置座標と進路方位を算出し、更に、前記状態方程式は、前２軸後２軸車、前２後１軸車、前１軸後２軸車、前１軸後１軸車に対応する可変仕様とした。

また、本願の第５発明は、車両の進行要素（前後運動要素）と、車両の方向要素（横運動要素）で構成され、視覚センサ、磁気センサまたはGPSと冗長構造を構成する慣性航法であって、前記進行要素として前後車速或いは車輪速を検出し、前記方向要素として車体横すべり角を検出し、前記前後車速或いは車輪速を前記車体横すべり角の余弦で除して車速を算出し、算出した車速とハンドル角と道路横断勾配（カント）を状態方程式に入力して車体遍揺角速度（ヨーレイト）を算出し、前記車速と目標経路の曲率と道路の横断勾配（カント）を代数式に入力してハンドル角を算出し、算出したハンドル角を前記状態方程式に代入して自車の位置座標と進路方位を算出し、更に、前記横すべり角の精度を確保すべく、IMU（慣性計測装置inertial measurement unit）による横加速度と前後加速度の比、車両CAN（相対カント）による前後速度とGPS車速の比、GPS速度の方位角からヨーレイトの積分値（車体遍揺角、ヨー角）を差引いた値及び車両モデル出力値の４通りを用いる構成とした。

本願の第１発明〜第５発明によれば、カメラ等の視覚センサ方式、GPS方式、磁気マーカ方式に対し、慣性航法を併用する際に、個々に生じる問題点を解消することができる。
特に第１発明の場合、1パルス当たりの移動距離を短くして移動距離誤差および速度誤差を小さくでき、特に低速域での自動運転に好適する

また第２発明の構成によれば、車体横滑り角を考慮した精度の高い慣性航法を実施することができる。

また第３発明の構成によれば、道路の勾配変化に対応してヨーレイトセンサ、加速度センサの出力補正、車両の軌跡計算のための軸荷重補正を行うため、精度の高い慣性航法を実施することができる。

また第４発明の構成によれば、車両に生じる前後軸重移動とそれによる操舵応答性が変化を考慮した慣性航法を実施することができる。

また第５発明の構成によれば、車両が2軸車、3軸車、4軸車と仕様が異なる場合でも、それぞれの車両仕様違いに対応することが可能となる。

更に本願発明によれば、路面不整による振動（ノイズ）が重畳した誤差、実車の操舵系のガタ（ヒステリシス）、実路のカント（横断勾配）にも対応した慣性航法を実施することができる。

慣性航法、視覚センサ、磁気マーカ、GPSの各方式の説明図 軌跡計算にかかわる遍揺角と車体横すべり角と座標系の説明図 自動車用スピードメータ（JIS5061）による車速を慣性航法に用いることの課題の説明図 車速センサと車輪速センサの説明図 道路勾配の求め方の説明図 必要加速度の算出法（アクセル％から自重推定し車速を決める方法）の説明図 必要減速度の算出法（惰行・リターダ・主ブレーキに至る減速制御）の説明図 慣性センサ位置に対する重心位置変動への対応の説明図 道路勾配による軸重変化を把握して車両軌跡計算に反映する方法の説明図 道路の横断勾配によって車両に作用する横加速度と車両姿勢の変化の説明図 道路の横断勾配によって生じるハンドル中立変化の説明図 ハンドル中立位置づれの検出の説明図 車両仕様違いに適応する可変仕様モデルの説明図 可変仕様車両モデルの可変の実施の説明図 車両仕様違いに適応する状態方程式の構造の説明図 GPSと磁気マーカと慣性計測および車両モデルとの整合をとり機能する冗長システムの説明図 目標経路から障害回避する進路変更式（経路イベントモジュール）の説明図

図１に示す「慣性航法」「視覚センサ」「磁気マーカ」「GPS」の各方式を順に説明して、各方式の相互関係を理解する。
先ず、「慣性航法」につき説明する。大文字のXとYで示す地球座標の原点P0から座標「X1、Y1」P1へは、「横すべり角βとヨー角（φ）との和の正弦」に車速を乗じ積分してX1点に至り、「横すべり角βとヨー角（φ）との和の余弦」に車速を乗じ積分してY1点に至る軌跡になる。この軌跡を計算するシムリンクを図の左下に示す。
自己位置認識の要は、車速と横すべり角とヨーレイトの精度である。その結果として、座標、と方位が算出される。

次に「視覚センサ」につき説明する。自車位置P0から、P1点にクロソイド曲線を描き向かう。P１点の座標（Ｘpos,Ｙpos)と方位角（φxy）を視覚センサによって得る。クロソイド曲線を描く軌跡の曲率式を以下の式（１）〜（４）に示す。

更に、「磁気マーカ」につき説明する。床下に磁気センサS1,S2を備え、磁気マーカ位置e21,e22を捉えて、重心Gcの座標を算出して自己位置P0を認識する。そこから合流目標位置P1で磁気マーカ経路にクロソイド曲線を描き合流する。ここは、上述の「視覚センサ」の場合と同じである。

更に、「VRS-GPS」につき説明する。屋根上にGPSアンテナを備え、自車位置P0の座標を捉えて、そこから合流目標位置P1でGPS経路にクロソイド曲線を描き合流する。ここは、上述の「視覚センサ」及び「磁気マーカ」の場合と同じである。

視覚センサ方式、磁気マーカ方式、GPS方式の基盤に慣性航法がある。逆に言えば、慣性航法の道標として視覚センサ方式、磁気マーカ方式、GPS方式がある。道標から次の道標へ移動する軌跡計算に慣性航法を用いる。

図２に示す軌跡計算にかかわる遍揺角と車体横すべり角と座標系について説明する。座標系には大文字のX,Y,Zで示す地球座標系（固定座標）と小文字のx,y,zで示す車体座標系（移動座標系）がある。
固定座標の原点を例えば、車庫等のしかるべき起点に置き、移動座標の原点を車両重心点に置いて、「重心点座標（X_G,Y_G,Z_G）と車両方位角（λ）によって、軌跡とその方位」を捉える。車両方位角が積分され軌跡になり速度が積分されて軌跡の長さ（距離）になる。車体座標は、x,y,zの3軸方向の直線運動と、その3軸まわりの回転運動の自由度を持つため固定座標上でのX_G,Y_G座標変化にZ_G座標変化及びx,y軸まわりの回転変化が、「重心点座標（X_G,Y_G,Z_G）と車両方位角（λ）によって、「軌跡とその方位」に重畳し外乱要因になるので、その影響を抑えて「重心点座標（X_G,Y_G,Z_G）と車両方位角（λ）によって、軌跡とその方位」を捉えることが肝要になる。

図３にスピードメータによる車速とGPSによる車速を重ね描きして慣性航法に用いることの課題について述べる。スピードメータ表示の車速は、変速機出力軸の回転パルスから算出される。この回転パルスからの出力線を点線で示し、GPSからの出力線を実線で示す。点線と実線は、50〜60秒の間、及び100秒付近で差異が大きくなっている。50〜60秒の間の差異は曲線走行に入り横すべり角が増加する場所であり、100秒付近は車速パルスが次のパルスに至る途中で車両停止するためである。即ち、制御に用いる車速は、横すべり角の補正を必要とし、粗いパルス間隔を狭いパルス間隔にする必要（課題）がある。

図４に、粗いパルス間隔を狭いパルス間隔にする対応、即ち、車速センサの粗い間隔から車輪パルスの狭い間隔に切替る対応について説明する。車輪1回転当たりのパルス数は、車速センサの場合は、１〜６パルスで、1パルス当たりの移動距離は2.5ｍ〜0.4mであり、車輪速センサの場合は100パルス以上で、1パルス当たりの移動距離は0.025m以下である。図の左側に、変速機の出力軸の回転をパルスで検出する「車速センサ」（1）を示す。車速センサが検出したパルスは推進軸、デファレンシャルギヤ比（2.5〜５程度）を介し車輪にいたるためパルス間隔が粗くなる。一方、車輪パルスは、車輪に装備された車速センサーリングのパルスの山谷を直接検出するため狭いパルス間隔が得られる。車速センサのパルス間隔速度が低くなる車速20km/h以下では、車輪パルスに切り替えることによって停止位置精度の必要を満たす。

図５に示す道路勾配の求め方について説明する。道路の前後勾配が変化すると、速度制御に影響し、且つヨーレイトセンサ、加速度センサの出力に勾配変化成分がのり、車両には前後軸重移動が生じ、それによる操舵応答性が変化するので、勾配を検出し車両モデルに反映させる制御が必要になる。

ここでは勾配検出の方法を述べる。車両に装備される加速度計と車輪速によって勾配を推定する。勾配θの坂に静止している車両の加速度は、重力加速度（9.81）に坂道勾配の正弦を乗じた値（式５）になる。走行時のG計よみ値は、車輪速度成分が重畳した値（式６）になる。勾配θは、式６に式５を代入整理すると式７になる。

ここに、ｒはタイヤ半径、ωは車輪回転加速度である。
この方法による出力と、GPSによるピッチ角出力との並列冗長構造を考慮する。

アクセル100%の加速度は、車両総重量に反比例するので、同じアクセル％で発生する加速度の逆比（Y_L/Y₀）を空車重量に乗じることで現在の車両重量を知ることが出来る。図６の(3)に任意のアクセルS％の加速度の式(8)、この式の関係図をその右に示す。図６に必要加速度の算出法について説明する。アクセル％から自重を推定し車速を決める方法を説明する。
図６の（1）は、所謂、走行性能線図の縦軸の駆動力を加速度に置き換えて、横軸に車速をとり、縦軸に加速度をとった走行性能線図である。加速実験を実施して、実発生加速度に惰行減速度を加えた全加速度で表現するとY=A/Xの関係の双曲線になる。加速度Yの単位はG、即ち1/9.81(m/s²)で、横軸は車速（m/s）でAは双曲線定数である。縦軸は加速度であるから、車両の自重変化にY値が反比例するから、或る車速におけるY値から自重変化を検出することができる。

図６の(2)に述べる様に、アクセル％の遊び代Ss％とフル％S_F％の間の直線関係からアクセルS％に対する自重YLを知ることができる。

図６の(４)にて、（３）で述べた任意のアクセル％における加速度の式（８）を書き改めて、アクセルS%、自重M_Lに対する余裕加速度（発生可能加速度）の式（９）にする。式（９）からアクセルS%と車速Xm/sその時の加速度A_L/Xから求める自重MLは、式（１０）になる。また、その時の車速Xm/s及び自重M_Lから加速度AL/Xを生じさせるアクセルS％は、式（１１）になる。

図７に必要減速度の算出法、即ち、アクセルを放しての惰行減速からリターダを１段作動、更に２段作動させての減速、その上でEBSを作動させてより高い減速度へ至る減速制御について述べる。図の左に惰行減速とリターダ減速を示し、図の右に惰行減速、リターダ減速にEBS主ブレーキを加えた減速度を示す。惰行減速度は、自重比（M₀/M_L）がかかる式（１２）になる。a_bは、空気抵抗の抗力係数である。b_rは、ころがり抵抗で、自重に比例する。式（１２）の状態でリターダを１段作動させると式（１３）になり、リターダを２段作動させると式（１４）になる。式（１３）式（１４）におけるb_R1、b_R2はリターダが発する減速度であり、それに変速機のギヤ比iGRが乗ぜられ、自重比（M₀/M_L）がかかって車両減速度になる。図の右のEBS減速度は、EBS＿ECUによる自重変化の学習がされるので、自重比（M₀/M_L）はかからず作動する。

図８に示す積載量変化、重心位置変化を検出して、車両軌跡計算に反映する方法について説明する。この図は、敢えて、重心点と慣性センサの位置がずれている状態を示している。前後速度v_x、横速度v_y、の合成速度vで横すべり角βの方向に、遍揺角速度（ヨーレイト）ｒを伴い走行しているのであるが、慣性センサによって捉えられる横速度は、式（15）になる。

そのため、慣性センサ位置での横すべり角は、誤差を含む式（16）になる。

尚、重心位置Ｌｒは、エアサスペンションのエアバッグのエア圧から、前軸と後軸の軸荷重を検出して、その比をホィールベースLに乗じて算出する方法、或いは、後軸エアバッグのエア圧から検出した後軸荷重を、先の図６のアクセル開度から求めた車両総重量で除し、その比をホィールベースLに乗じて算出する方法等の方法がとられる。この方法は、同様にGPSに対しても適用できる。

図９に示す勾配変化成分を補正し、軸重移動を補正して車両軌跡計算に反映する方法について説明する。図の左に下り勾配、右に上り勾配の図を示す。重心高さH_GをホィールベースLで除した値の荷重移動が生じて、下り勾配では前軸荷重が増し後軸荷重は減少して、上り勾配では前軸荷重が減少し後軸荷重は増す。その分タイヤのコーナリングパワーが変化して、舵の効きが下り坂では強くなり、上り坂では弱くなる。また、前後移動距離が水平路換算でcosθ倍短縮される。故に、制御モデルのコーナリングパワーを軸荷重に対応して変化させての制御が必要になる。

図１０に、道路の横断勾配（カント）によって車両に作用する横加速度と車両姿勢の変化について説明する。この図は、カント角ξの「直線路を走行」する状態を示している。
カント角に伴って車体ロール角Φが生じるので、「カント角＋ロール角」相応の横加速度が作用するのであるが、エアサスペンションのオートレベリング機構が作用して定常状態では、車体ロール角Φはゼロになる。この状況下で直進するためには、「作用する横加速度×車両質量」につり合うタイヤ横力が発揮されねばならないので、カント山側に回頭した車両横すべり角βを生じさせる必要があり、その横すべり角βを生じさせるために前輪実舵角δが必要になる。これを「あて舵」と称し、操舵中立舵角としての制御が必要になる。

カント角ξの曲線路を走行する場合は、上述の「直線を走行」に求心加速度分が加算され式（21）になる。

ここに、G_yはセンサによって検出される加速度（ｇ）、ξはカント角（rad）、φは車体ロール角(rad)、ρは曲線路の曲率（無次元）、ｒはヨーレイト（rad/s）、vは車速(m/s)である。カント曲線路での横加速度の影響をカント相当にし、相対カント（ξ…Relative Cantと称す）を定義すると式２３になる。

予め、カント角が既知の直線路で計測される横加速度、操舵角を中立補正値として把握してその両振幅を操舵ヒステリシスとして押さえ、直進及び左曲線路ではξ相応の右あて舵、右曲線路ではマイナスξ相応の左あて舵を加えて操舵角制御を行う。

図１１に道路の横断勾配によって生じるハンドル中立変化について説明する。（A）の様に、水平台上で油を敷いたテフロン（登録商標）プレートに前輪を載せて、パワ−ステアリングをOFFにして操舵トルクを計測する。
片輪にテフロン（登録商標）シート、他輪を地面から浮かして操舵を加えるとテフロン（登録商標）シート側リフトトルクを計測することができる。リフトトルクとは、地球の重力加速度効果によって車輪がキングピン傾き角のまわりに回転しようとするところを抑止するトルクのことである。

左右の車輪が互いにトーイン方向に回転しようとしてタイロッドを介して押し合う所謂「予圧トルク」になる。片方の車輪を地面から浮かせ、他方の車輪を「油を敷いたテフロン（登録商標）シート」に載せることでタイヤ摩擦の影響を極小にしてリフトトルクを計測する。左カントの場合のA（１）、水平の場合のA（２）、右カントの場合のA（３）を計測すると、道路の横断勾配（カント）によって、ハンドル中立位置が変化する様子が把握できる。

それの一例の左カント２°の実験結果を（B）に示す。左輪によるリフトトルクをLH、右輪によるリフトトルクをRH、左輪と右輪がタイロッドを介して互いに引き合う関係の予圧トルクをLH＋RHで示す。LH+RHの線の中立が右（マイナス）に１５．３°ずれることが分かる。この関係は三角関数の式で表現できるので、図13、図14、図15の数学モデルにこの結果を反映する。

図１２にハンドル中立位置ずれの検出について説明する。自動運転のための操舵モータの中立位置は、ハンドルの中立位置と一致していなければならない。
ハンドル中立位置を目分量で決めて、その状態で操舵モータの電源を入れ、その電源投入点をゼロ点（中立）とするのでは、目分量の「あいまいさ」がそのまま、制御誤差になってしまう。ハンドル中立位置は水平路での左右操舵反力の谷間の中央位置であるので、操舵モータの電流とモータ角の関係を把握することで「操舵反力の谷間」を検出することができる。この図は左カント１°の直線路で定振幅モータ角を加えて直進した時のモータ角とモータ電流の定振幅中央値をプロットしたものである。モータ角が左に1.5°ずれている。右カント１°では右に1.5°ずれる。カント路の代わりに、左段差路及び右段差路でも同様なデータを取得することができる。この様に取得した左右ズレの中央位置をハンドル中央位置と認識して後、制御に入ることによってハンドル中立位置ずれによる制御誤差の発生を除去する。

図１３に車両仕様違いに適応する可変仕様モデルについて説明する。前1軸・後1軸の2軸車、前1軸・後2軸の3軸車、前2軸・後1軸の3軸車及び前2軸・後2軸の4軸車が存在するので、4軸車モデルを用意して不要な項をゼロと置くことで、4軸から3軸へ更に2軸への適応を可能にしている。図の左側に、操舵系、右側に車体系（シャシ）系を示す。

右側の車体系では、前第1軸、前第2軸、後第1軸、後第2軸を示す。各軸とも左右二輪を一本の車輪にまとめて、車輪の車体支持が柔であるために生じるコンプライアンスステアをコンプライアンスステア中心（P_Cf1,P_Cf2, P_Cr1, P_Cr2）及び横剛性(k_f1, k_f2, kr₁, kr₂)と減衰係数（C_f1,C_f2,Cr₁,Cr₂)で表現し、タイヤコーナリングフォースをCF₁,CF₂,CR₁,CR₂、道路横断勾配によって作用するカント横力をSF₁,SF₂,SR₁,SR₂で表現し考慮して、各軸毎にコンプライアンスステア角（δ_Cf1,δ_Cf2,δ_Cr1,δ_Cr2）角、タイヤ横すべり角（β_f1,β_f2,β_r1,β_r2）を考慮し、前第1軸、第2軸には、実舵角（δ_f1,δ_f2）を考慮する。重心位置に車両質量ｍ、慣性モーメントＩにコーナリングフォースCF₁,CF₂,CR₁,CR₂の合力とカント横力SF₁,SF₂,SR₁,SR₂の合力、及びそれぞれのモーメントが作用して車両横すべり角βと、ヨーレイトγが生じる。ヨーレイトγを積分して得られるヨー角φと横すべり角βの和の余弦と車速の積の積分が車体前後移動距離になり、その和の正弦と車速の積の積分が車体横移動距離になり車両の移動軌跡が描かれる。

左側の操舵系では、キングピン傾角（σ_1,σ₂）とキャスタ角（τ_1、τ₂）の合成角とキングピンオフセット(r_k1, r_k2)によって生じる「左右キングピンのリフトトルクによる予圧トルクに対して路面側から作用するコーナリングフォースCF₁,CF₂とカント横力SF₁,SF₂、及びハンドル側からサーボ機構であるパワーステアリングを介して作用するハンドル角（モータ角）、ハンドルトルク（モータトルク）とのつり合い関係から前第１軸と前第２軸の実舵角（δ_f1,δ_f2）が所謂“あて舵”分を含み決まる。尚、操舵系と車体系は、連立方程式として解かれる。

図１４に可変仕様車両モデルの可変の実際について説明する。１に操舵系モデル、２に車両系モデルを示す。前2軸操舵モデルの第2軸を外せば、前1軸操舵モデルになる。“前2軸後2軸”の後第2軸を外せば、“前2軸後1軸車”になり、前第2軸を外せば“前1軸後2軸”になる。更に、“前1軸後2軸”から後第2軸を外せば“前1軸後１軸”になる。この図にて、軸を外すことによって軸数違いの車両モデルになるのと同様な感覚で、数学モデル表現での可変仕様車両モデルへの変更が容易にできる。

図１５に車両仕様違いに適応する状態方程式の構造について説明する。状態方程式（１7）は、入力（コントロールベクトルと言う）を（7）のカント（ξ）と（8）のハンドルトルク（Ｔ_H）に対する車両応答を出力する状態方程式である。一行目が車体の横力のつり合い式(1)、二行目が車体の回転のつり合い式(2)、三行目を飛ばして、四行目がキングピン軸まわりのリフトトルクのつり合い式(4)、更に五行目を飛ばして、六行目がハンドル軸まわりのトルクのつり合い式(6)である。例えば、式(1)は次の様に書ける。尚、三行目、五行目は二階微分を一階微分ずつ演算の必要から設けられている。

ここに、A10,A20,A30…A50の各エレメントは、それぞれ式31、式24、式32…式34である。エレメント中のk_f1、CS_f1・・・等は図11にて説明の通りである。車両の軸構成の変化には、このk_f1、CS_f1・・・等の軸に対応するサフィックス…ｆ_1、…ｆ_2、…ｒ_1、…ｒ₂によって識別して不要な項目を外すことによって対応できる。式（2）以下も同様である。

状態方程式（24）の点線枠の部分を抜き出すと、（9）のハンドル角（δ_H）と(7)のカント（ξ）を入力とする車両応答の状態方程式（25）になる。自動運転車両は、(9)の（δ_H）をモータ角とし、(7)の（ξ）の相対カントを検出して走行する。準定常状態での操舵角は車両状態変化と釣合状態にあるとして加速度項、速度項を無視し(13)の式を式（26）にし式（27）のδの式にする。
そのδを、式(10)から式（28）とし、式（11）から式（29）し、式（28）と式（29）のδの項に式（27）のδを代入して連立方程式としその解をしてのハンドル角δ_Hの式（30）を得る。この式は（7）の相対カント（ξ）と（14）のヨーレイト（γ）を引数としており、ヨーレイトは車速と旋回曲率の積、即ちγ＝ρｖ であるから、道路カント、曲率からハンドル角δ_Hを与える制御式として使用する。

図１６にGPSと磁気マーカと慣性計測および車両モデルとの整合をとり機能する並列冗長システムについて説明する。図の上段太線の上側に前後運動を示し、太線の下側に横運動を示す。
先ず、前後運動の部分について説明する。上段中央に「加速度線図・減速度線図」の太枠を示す。ここに、運行経路の「停止位置」「車速」「道路勾配」の計画が経路IDに対応して用意され枠の左から入力される。その計画に対する、現実の車速、前後Gから実勾配が検出され、現実アクセル％から自重推定がされて、現実の運行事情による修正も加えられて上段中央太枠に入力されて、アクセル％、ギヤ選択、リターダ選択、ブレーキ％が出力され、その出力が上段中央右の実車枠の入力になり、車速になる。実車の車速はJIS5601規格による車速を用いるが低速域（例えば、20km/h以下）では、車輪速に切り替えることによって、低速位置制御の必要に備える。
ところで、この車速は「前後速度」であって、車体すべり角のために生じる横速度を含む実の車速（前後と横の合成速度）ではないので、上段太線の下側の横運動から求められる車体横すべり角βを取り込んで、前後速度をcosβで除算して実の車速とする。このβは、慣性航法の車速精度を満たすため重要であるが、精度が得にくいので下段の横運動のところでは、横加速度の積分値を前後加速度の積分値で除算する方法、車輪速をGPS速度で除算する方法、GPS方位角から遍揺角（ヨー角）を差引く方法、及び、車両モデルからの算出値を検討選択して使用される。

次に、横運動部分について説明する。左端の太枠に示すGPSベースでの目標経路座標を備えて現在位置から目標経路へ流入するクロソイド曲線式を作成してその曲率計算する。並行して、視覚センサによる目標点（P1）に至る経路曲率を計算もして備えて待機する。視覚センサが障害検出すると、その障害を回避するための前後距離と横距離をパラメータとする進路変更式に検出した前後距離と横距離を代入して進路変更の曲率を算出して、その曲率を計画経路曲率へ加えて合成曲率を得てハンドル角の式に代入すると共に、「その合成曲率と車速から求められる横加速度」と「車輪速と横加速度とヨーレイトによって検出される道路カント(図10)」との和になる「相対カント角（ξ）」をハンドル角の式へ代入する。ハンドル角の式には、積載と走行環境により変化する軸重が代入され車両或いは車両モデルへの入力舵角を算出する。その入力操舵は操舵モータによって行われる。その際に、操舵モータのゼロ点位置と実車ハンドル中立位置との偏差が車両制御量に入力され、それを受けて車両運動が生じ、生じた横すべり角（β）、ヨー角（φ）から移動座標とその方位角がGPS,磁気センサ、及びIMUから出力される。並行してハンドル角に操舵ヒステリシス（図12）が加えられたハンドル角が車両モデルに入力され、横すべり角（β）、ヨー角（φ）から移動座標とその方位角の計算値が出力される。

図の中央部の縦長枠に実車、その下の太線の縦長枠に車両モデルを示す。実車に搭載されるIMU（慣性計測装置inertial measurement unit）によって横加速度Gy_imu、前後加速度Gx_imu,ヨーレイトγ_imuが検出され、GPSからはX座標X_{_GPS}、Y座標Y_{_GPS}、方位角λ_{_GPS}が検出され、磁気センサからはX座標Xmk、Y座標Ymk、方位角λmkが検出される。
Gy_imuとGx_imu,からIMU検出横すべり角βimuが計算され、GPS検出の車速V_GPSと車両CAN検出の前後車速Vcanから横すべり角β_GPS１及びGPS方位角と車両遍揺角（ヨー角）との差から得る横すべり角βG_PS2更に車両モデルから算出されるβcalの四つのβが、システム診断状況によって待機冗長構造を構成する。
このβとIMU検出のヨーレイトγ_imuから、IMUによるX座標Ximu、Y座標Yimu、方位角λimuが算出され、GPS検出のX座標X_GPS、Y座標Y_GPS、方位角λ_GPS及び磁気センサ検出のX座標Xmk、Y座標Ymk、方位角λmk、更に車両モデルから計算出力されるX座標Xcal、Y座標Ycal、方位角λcal、の四つが、システム診断状況によって待機冗長構造を構成する。
待機冗長構造からシステム診断状況により出力された自己位置（X,Y）と進行方向（λ）の現在位置をフィードフォワード項である目標経路座標に照らし、現在位置から目標経路へ流入するクロソイド曲線式を作成し、その曲率から舵角決める。GPSが使用できない環境下では、視覚センサによって目標経路を定め、舵角を決める。この様に、GPSと磁気マーカと慣性計測および車両モデルとの整合をとり機能する冗長システムを構成する。

図１７に目標経路から障害回避する進路変更式（経路イベントモジュール）について説明する。図の左上にモジュール曲線を示す。距離Lm走行する間に横へDm移動する“進路変更の要素曲線”である。横移動に伴う経路進路角を式（47）とする。

ここにTは進路変更の所要時間である。車速v、時間Tで前後Lm、横Dmの移動する単位要素即ちモジュールである。予定された“或る経路”を走行中に障害を回避する必要などのイベントが生じる。或る経路の経路角をφrouteとしモジュールの経路角をφpartとすると回避経路の進路角は“φroute+φpart”になる。その経路計算の方法を図の中央に示す。
このように障害回避などのイベントに対応する経路の経路角は“φroute+φpart”になり、この経路を通過する際の横加速度は、式（48）になるので、横加速度を許容する車速で走行する制御が自在になる。

Claims (14)

1. 車両の進行要素（前後運動要素）と、車両の方向要素（横運動要素）で構成され、視覚センサ、磁気センサまたはＧＰＳと冗長構造を構成する慣性航法であって、前記進行要素として前後車速或いは車輪速を検出し、前記方向要素として車体横すべり角を検出し、前記前後車速或いは車輪速を前記車体横すべり角の余弦で除して車速を算出し、算出した車速とハンドル角と道路横断勾配（カント）を状態方程式に入力して車体遍揺角速度（ヨーレイト）を算出し、前記車速と目標経路の曲率と道路の横断勾配（カント）を代数式に入力してハンドル角を算出し、算出したハンドル角を前記状態方程式に代入して自車の位置座標と進路方位を算出し、更に、前記前後車速或いは車輪速は車輪１回転当たりのパルス数が少ない車速パルス（JIS5601規格）または車輪１回転当たりのパルス数が多い車輪速パルスを検出することで行い、低速域においては、パルス間隔が狭い車輪速パルスに切り替えることで低速走行に必要な車速精度を得ることを特徴とする自動運転における慣性航法。
2. 車両の進行要素（前後運動要素）と、車両の方向要素（横運動要素）で構成され、視覚センサ、磁気センサまたはＧＰＳと冗長構造を構成する慣性航法であって、前記進行要素として前後車速或いは車輪速を検出し、前記方向要素として車体横すべり角を検出し、前記前後車速或いは車輪速を前記車体横すべり角の余弦で除して車速を算出し、算出した車速とハンドル角と道路横断勾配（カント）を状態方程式に入力して車体遍揺角速度（ヨーレイト）を算出し、前記車速と目標経路の曲率と道路の横断勾配（カント）を代数式に入力してハンドル角を算出し、算出したハンドル角を前記状態方程式に代入して自車の位置座標と進路方位を算出し、更に、積載量・乗客数によって変化する車両総重量（自重？１）を式（１０）により検出し、重心位置によって変化するエアサスペンションのエア圧により重心位置を検出して、慣性センサ位置、ＧＰＳ位置に対する重心位置の差異を車両軌跡計算に反映することを特徴とする自動運転における慣性航法。
   

   
3. 車両の進行要素（前後運動要素）と、車両の方向要素（横運動要素）で構成され、視覚センサ、磁気センサまたはＧＰＳと冗長構造を構成する慣性航法であって、前記進行要素として前後車速或いは車輪速を検出し、前記方向要素として車体横すべり角を検出し、前記前後車速或いは車輪速を前記車体横すべり角の余弦で除して車速を算出し、算出した車速とハンドル角と道路横断勾配（カント）を状態方程式に入力して車体遍揺角速度（ヨーレイト）を算出し、前記車速と目標経路の曲率と道路の横断勾配（カント）を代数式に入力してハンドル角を算出し、算出したハンドル角を前記状態方程式に代入して自車の位置座標と進路方位を算出し、更に、道路の勾配変化に対応してヨーレイトセンサ、加速度センサの出力補正、車両の軌跡計算のための軸荷重補正をすることを特徴とする自動運転における慣性航法。
4. 車両の進行要素（前後運動要素）と、車両の方向要素（横運動要素）で構成され、視覚センサ、磁気センサまたはＧＰＳと冗長構造を構成する慣性航法であって、前記進行要素として前後車速或いは車輪速を検出し、前記方向要素として車体横すべり角を検出し、前記前後車速或いは車輪速を前記車体横すべり角の余弦で除して車速を算出し、算出した車速とハンドル角と道路横断勾配（カント）を状態方程式に入力して車体遍揺角速度（ヨーレイト）を算出し、前記車速と目標経路の曲率と道路の横断勾配（カント）を代数式に入力してハンドル角を算出し、算出したハンドル角を前記状態方程式に代入して自車の位置座標と進路方位を算出し、更に、前記状態方程式は、前２軸後２軸車、前２後１軸車、前１軸後２軸車、前１軸後１軸車に対応する可変仕様であることを特徴とする慣性航法。
5. 車両の進行要素（前後運動要素）と、車両の方向要素（横運動要素）で構成され、視覚センサ、磁気センサまたはＧＰＳと冗長構造を構成する慣性航法であって、前記進行要素として前後車速或いは車輪速を検出し、前記方向要素として車体横すべり角を検出し、前記前後車速或いは車輪速を前記車体横すべり角の余弦で除して車速を算出し、算出した車速とハンドル角と道路横断勾配（カント）を状態方程式に入力して車体遍揺角速度（ヨーレイト）を算出し、前記車速と目標経路の曲率と道路の横断勾配（カント）を代数式に入力してハンドル角を算出し、算出したハンドル角を前記状態方程式に代入して自車の位置座標と進路方位を算出し、更に、ＩＭＵ（慣性計測装置inertial measurement unit）によって横加速度Gy#imu、前後加速度Gx#imu、ヨーレイトγ#imuを検出し、ＧＰＳからはX座標X_#GPS、Y座標Y_#GPS、方位角λ_#GPSを検出し、磁気センサからはX座標Xmk、Y座標Ymk、方位角λmkを検出し、前記横加速度Gy#imu、前後加速度Gx#imuからＩＭＵ検出横すべり角βimuを計算し、ＧＰＳ検出の車速V_GPSと車両CAN検出の前後車速Vcanから横すべり角β_GPS1及びＧＰＳ方位角と車両遍揺角（ヨー角）との差から得る横すべり角βG_PS2、更に車両モデルから算出される横すべり角βcalを算出し、これら四つの横すべり角βimu、β_GPS1、角βG_PS2、βcalがシステム診断状況によって待機冗長構造を構成することを特徴とする自動運転における慣性航法。
6. 請求項１乃至５の何れかに記載の自動運転における慣性航法において、前記進行要素としての前後車速或いは車輪速は、加速度線図及び減速度線図に運行計画、計画に対応する道路勾配、自重変化および実運行事情を入力し、その結果得られる実車のアクセル％、ギヤ選択、リターダ選択およびアクセル％制御から検出することを特徴とする自動運転における慣性航法。
7. 請求項１乃至５の何れかに記載の自動運転における慣性航法において、実車のハンドル中立位置と操舵モータのゼロ点との舵角誤差を検出補正することを特徴とする自動運転における慣性航法。
8. 請求項１乃至５の何れかに記載の自動運転における慣性航法において、実車のハンドル中立ヒステリシスを把握して車両モデルの操舵角補正を行うことを特徴とする自動運転における慣性航法。
9. 請求項１乃至５の何れかに記載の自動運転における慣性航法において、目標経路上の障害を回避する進路変更式を備えて障害回避することを特徴とする自動運転における慣性航法。
10. 請求項１乃至５の何れかに記載の自動運転における慣性航法において、実車の速度制御のための、必要アクセル％、必要ブレーキ％の検出方法を備えることを特徴とする自動運転における慣性航法。
11. 請求項１乃至５の何れかに記載の自動運転における慣性航法において、視覚センサを備えて目標経路を捉えそこへ至る曲率とカントを代数式に入力してハンドル角を算出し、それを状態方程式に代入して自車の位置座標と進路方位を算出して、目標経路を辿ることを特徴とする自動運転における慣性航法。
12. 請求項１乃至５の何れかに記載の自動運転における慣性航法において、磁気センサを備えて目標経路に埋設された磁石位置を検出して、自己位置と方位を認識して次の経路上の磁石へ至る曲率とカントを代数式に入力してハンドル角を算出し、それを状態方程式に代入して自車の位置座標と進路方位を算出して、目標経路を辿ることを特徴とする自動運転における慣性航法装置。
13. 請求項１乃至５の何れかに記載の自動運転における慣性航法において、ＧＰＳを備えてＧＰＳ座標と方位による目標経路の曲率とカント角を代数式に代入してハンドル角を算出し、それを状態方程式に代入して自車の位置座標と進路方位を算出して、目標経路を辿ることを特徴とする自動運転における慣性航法。
14. 請求項１乃至５の何れかに記載の自動運転における慣性航法において、視覚センサと磁気センサとＧＰＳの各方式を並行に機能させてシステム計画と診断に応じる待機冗長システムとして機能することを特徴とする自動運転における慣性航法。
    
    
    
    

Images