技術論文 GPS と慣性センサを利用した車両姿勢角推定 * 平野 麻衣子 ) 天野 真輝 ) 服部 義和 小野 英一 ) 福田 光雄 ) 廣瀬 太郎 Estimation of Vehicle Attitude Integrating Inertial Sensors

Transcription

1 技術論文 * 平野 麻衣子 ) 天野 真輝 ) 服部 義和 小野 英一 ) 福田 光雄 ) 廣瀬 太郎 timation o Vehicle Attitude Interatin Inertial Senor and S Maiko Hirano Maateru Amano Yohikazu Hattori iichi Ono Mituo Fukuda Taro Hiroe S inal i ued or etimatin it in inertial enor. The it in enor could be etimated accurately without updatin S inal contantly becaue the it rate i lower than vehicle dynamic rate. In thi paper, we propoe the oberver or etimatin vehicle roll and pitch anle accurately with compenatin or it inertial enor. KY WORDS: vehicle dynamic, motion control, electronic tability control, S, Drit (B). ま え が き 車両の姿勢角 ( 鉛直軸に対するロール角, ピッチ角 ) は, 車体スリップ角の推定精度に影響を与えるため, 車両の安定化制御において重要な状態量である. これらの状態量を推定する手法として, 慣性センサのみを利用する方法 や, 慣性センサに加え, からの情報を利用する手法が提案されている. 姿勢角の推定精度は, センサのゼロ点ドリフトに依存する. しかし, ゼロ点ドリフトが小さい高精度なセンサや, 内部でドリフト補償可能なセンサを利用することはコスト面が課題となる. そこで, 小野ら はセンサのゼロ点ドリフトをオンラインで補償しつつ, 姿勢角を推定する手法を提案している. 文献 のドリフト推定手法では, オブザーバ出力はセンサ信号の積分に比べ, センサ誤差の影響を受けにくいことに着目し, センサ信号の積分とオブザーバ出力を比較することでセンサ誤差を推定する手法である. ここでは, 横加速度とヨー角速度のゼロ点ドリフトの推定に対し, 車速変化を利用することを特徴としている. 本稿では, 文献 のセンサ情報に新たに 信号を追加し, センサゼロ点ドリフトの推定に対し, 信号から推定される車両のヨー角を利用する. これにより, 車速変化を必要とせず, ドリフトを補償することが可能となる. また, を利用する際, 衛星を常時捕捉できないことがしばしば問題となるが, センサゼロ点ドリフトは車両運動に比べ, 十分低い周波数で推移するため, ある程度の時間内では一定と考えられる. したがって, 必ずしも 信号の常時捕捉を必要としない., 章では, 文献 の姿勢角推定手法について述べる. 章に, 信号を利用するセンサゼロ点ドリフト推定手法を 年 月 日受理, 年 月 日自動車技術会春季学術講演会において発表. ) 株 豊田中央研究所 愛知県長久手市横道 ) トヨタ自動車 株 静岡県裾野市御宿 提案し, 章に実験データを利用した推定結果を示す.. 車両の運動方程式 剛体の運動方程式 剛体に固定された 軸加速度 ( 前後加速度, 横加速度, 上 下加速度 ), 軸角速度 ( ロール角速度, ピッチ角速度, ヨー角速度 ) と運動状態量 ( 前後速度, 横速度, 上下速度, ロール角, ピッチ角 ) の関係を表す運動方程式を以下に示す. (5) ここで,U: 前後速度 V: 横速度 W: 上下速度 : 前後加速度 y : 横加速度 z : 上下加速度 : ロール角速度 Q: ピッチ角速度 R: ヨー角速度 : ロール角 : ピッチ角 : 重力加速度とする ( 図 ). また, 座標系は図のように車体上向きを 軸の正方向とする右手座標系とし, 角度はオイラー角を表す. V Z ai R W Z U Y ai X ai V O Y X Q Fi. Vehicle Coordinate ピッチ角速度の推定文献 (4) で提案されている姿勢角推定手法では, ヨー角速度と前後, 横加速度を計測する横滑り防止制御用センサを二つ利 () (2) (3) (4) Vol.46,No.2,March

2 用する. すなわち, 軸加速度 ( 前後, 横, 上下 ) と 軸角速 度 ( ロール, ヨー ) をセンサ信号として計測する. ピッチ角 速度 Q は文献 で提案されているように, 式の関係から推 定する. 自動車の運動では上下速度 W の絶対値が小さく, 路 面外乱の高周波成分を除けば, 上下速度 W の微分値の平均 ( 低周波成分 ) は 付近の値となる という特徴を利用する と, 以下のような推定式が得られる. ここで, は推定値を表すとする. (6) 姿勢角推定に必要なセンサ信号は前述の信号の他に, 車輪 速センサを利用も利用する. 次節にその詳細を述べる. 自動車運動固有の特徴. 姿勢角推定 節で示した運動方程式を利用し, オブザーバを構成する. オブザーバでは, 積分演算による状態量の発散を抑制するため, 観測できる物理量をフィードバックする. 文献 の提案手法では, フィードバックする物理量として, 自動車運動固有の特徴を利用する. すなわち, 車体前後速度微分値( 低周波成分 ) は車輪速から推定ができる, 車体横速度微分値の平均値 ( 低周波成分 ) は 付近の値となる ( スリップ角は長時間増大し続けない ), 地面固定座標系の鉛直軸方向の加速度は, 重力加速度にほぼ一致する という三つの特性をフィードバックに反映する. また, 上下速度は小さいとし, 以下ではW と仮定する.より, 車輪速から推定された前後速度を V とすると, 前後速度微分値 U は, U と表せることから, 以下の条件を利用する. V (7) これは,V から演算される加速度と前後加速度の差と, ピッチ角の関係を表している. 次により, 低周波成分に対してV が成り立つことから, yd T ~ y ~ ~ ~ () とする. また, センサ信号から演算される車両出力を y yd T (2) とする. ここで,, yd は, センサ信号および横速度推定値 V ~ から得られる 式の左辺を, 車両出力 ~, ~ yd, ~ は, 加速度センサ信号および姿勢角推定値から得られる 式の右辺をそれぞれローパスフィルタ処理した値とし, 以下の式で定義する. (3) (4) (5) (6) (7) ただし, はそれぞれローパスフィルタの時定数を表, y, している. すなわち, 式はいずれも長い時間の中での変化を考慮したときに満足する条件 に由来する関係式であるため, オブザーバの測定量のフィードバックには, 両辺をローパスフィルタ処理した値を利用する. フィルタ時定数は, 遅れによる誤差を伴わない値に調整する. オブザーバ出力と車両出力に対し, 適切なオブザーバゲイン K を設定することによってロール角, ピッチ角を推定するオブザーバが構成できる. すなわち, ロール角, ピッチ角を推定する非線形オブザーバは, 以下の状態方程式で記述される. (8) という条件を利用する.は, 座標系 上の重力加速度 と座標系 上の V y z との関係が, 次式によって記述できることを意味する. (9) 姿勢角推定オブザーバ 前節に示した自動車運動固有の特徴を利用し, 姿勢角推定のためのオブザーバを構成する. オブザーバの状態量を K フィードバックに用いるオブザーバ出力を () (8) 48 自動車技術会論文集

3 オブザーバゲインの一例として, オブザーバの安定性を確保するため, 線形化を行ったときの対角成分が負の係数を持つように, 次式のように表す. K k ~, u K K K, K K K y y K K y (9) K K は適切な正の定数とす ただし, K y,, K, K, K y, る. 式のパラメータを調整することによって, オブザーバの安定性と収束性は確保される. しかしながら, 収束性の向上のためには, 各パラメータを可変にする等の方策が必要となる. 式の安定性と収束性を保証する K の一般的な設計方法については今後の課題である. 姿勢角推定, 車体速度推定およびドリフト推定の関係 車体速度についても姿勢角の推定と同様に, オブザーバを構成することができ, これまでにも様々な手法が提案されている. 車体速度推定オブザーバから, スリップ角推定値 ~ は ~ V ~ U ~ ~ によって求められる. この車体速度推定値 U, V ~ は姿勢角の影響を受ける. 一方, 姿勢角推定オブザーバの 式は, 横速度推定値 V ~ を利用するため, 車体速度の影響を受ける. また, 姿勢角 車体速度の推定値は共にセンサドリフトの影響を受ける. そこで, 本稿で提案する手法は, 各オブザーバから得られる誤差を含む推定値を用い, センサゼロ点ドリフトを同時に推定することによって, 相互に影響する誤差をオンラインで徐々に低減させていく手法である. 以下では, 車体速度に関してもオブザーバによって推定されていることを前提に, ドリフト推定を検討する.. 信号を利用するセンサゼロ点ドリフト推定文献 で提案されているセンサゼロ点ドリフト推定の手法は, オブザーバ出力としての姿勢角推定値はセンサ信号の積分によって演算される姿勢角と比べ, センサ誤差の影響を受けにくいことに着目し, 式の運動方程式の単純積分とオブザーバ出力を比較することでセンサ誤差を推定する手法である. この手法では, ゼロ点ドリフトを推定するセンサ信号の中で, 横加速度とヨー角速度について, 車両の横方向に関する 式の関係を基に, 一つの基礎式から二つのドリフト量を推定する必要がある. そこで, 文献 では, ヨー角速度センサドリフトの係数が車速となることに着目し, 車速変化を利用することで, 二つのドリフト量を推定した. 本稿では, 信号から得られる車両ヨー角をヨー角速度の積分と比較することで, 二つの基礎式から横加速度とヨー角速度のドリフト量を推定することを考える. 信号から推定される車体ヨー角 衛星から得られる信号は, 搬送波と搬送波上に,で付与されるコードで構成されており, 搬送波のドップラー効果によって生じる周波数変化分を計測したものを ドップラーという. この ドップラーと衛星速度から車体の地球中心座標系に対する速度を推定する手法は様々なものが提案されており, これらの手法により, 精度の高い車体速度ベクトルが得られる. ドップラーによる車体の速度ベクトルは, 図 のような地面固定座標系 O-XYZ) に対する速度として 求めることができる. したがって, 座標系 と ドップラ ーによる車体速度ベクトルとのなす角が 得られる. Z X Z O S S X V Y U β Fi.2 Vehicle Yaw Anle and Velocity Vector XY 平面での運動のみを考えた場合, S と車体スリップ角 および車体ヨー角の関係は, (2) と表される. 本稿では, 車体のロール角, ピッチ角を考慮した車両運動を扱うため, 式の関係を三次元での車両運動を考慮した形に拡張する. 車体姿勢角は,Z 軸 Y 軸 X 軸の順番で回転 させたオイラー角である. したがって, 車体座標系 V を X 軸 Y 軸の順に戻した座標系 O-X Y Z と, 地面固定座標系との Z 軸まわりの回転角が車体ヨー角である. このとき, 車体の速度ベクトルを O-X Y Z で表した場合の X 方向成分と Y 方 X 向成分から作られるベクトルを Y V Z とすると,X 軸と XY Z V とがなす角は Y (2) と表される. すなわち, 車両の三次元運動を考慮した場合, 式は次式のように修正される. (22) 上式により, 信号から得られる車体速度ベクトルと車体ヨー角の関係が得られた. ドリフト推定に利用する基礎式 式において, 上下速度を無視するとともに, ピッチ Vol.46,No.2,March

4 角速度に 式の推定値を代入した以下の方程式をオブザーバで利用する状態方程式とする. (23) (24) (25) (26) 前節で示した手法によって, 車体ヨー角の情報が得られる. したがって, 本手法では文献 でも用いた 式に加え, 車両のヨー方向に関する次式も状態方程式として利用する. (27) D D 2 D 3 (33) ただし, D D D 2 3 co co in co in in in co co co y z (34) とする. これらの式をセンサドリフト誤差について整理すると, 以下のように記述できる. dd d d 2 R d 3 d 4 T d 5 d T 6 (35) センサ信号, y, z,, R に一定のドリフト誤差,,,, R が重畳し, 式の左辺はオブザーバによっ て真値が既知と仮定すると, 以下の関係が成立する. (28) (29) (3) (3) (32) ただし, du, dv, d, d は, 式の右辺に相当するオブザーバの内部演算値である. なお, 車両上下方向については, 鉛直方向の加速度は重力加速度 という条件から以下の関係も利用できる. in in co co co y z (36) 4 自動車技術会論文集

5 する. しかしながら, 以上での精度は高く, 計測器から得られる車体ヨー角との誤差は小さい. 図 の結果から, 信号を受信可能な環境では, 精度良く車体ヨー角が推定できることが分かる. (37) ここで,t mp はオブザーバの演算に用いるサンプリング時間とする. 式における各信号 ( センサ, 推定値 ) は, ノイズ除去のため, 一定時間区間 ( ドリフト推定のサンプリング時間 t ) の平均値を利用する. すなわち, センサのゼロ点ドリフトの時間変化は小さいことを考慮し, サンプル時間 t ごとに演算の平滑化を行う. したがって, センサゼロ点ドリフトは, 式の両辺を t 秒間積分した以下の式から推定する. D 2 R T T 6 (38) 式において, 行列 D は 6 行 5 列の行列である. したがって, ドリフト誤差を求める方程式は擬似逆行列 D + を用いて次式で表される. D R T 5 T 6 (39) 上式の演算によってセンサのゼロ点ドリフトを求めることもできるが, ここではゼロ点ドリフトの時間変化は小さいことを考慮し, さらにサンプル時間毎の演算の平滑化等を行う. 信号を利用するセンサドリフト推定の効果確認 本稿で提案した 信号を利用するセンサドリフト推定手法の効果をオフライン演算によって確認する. 図 に山岳路走行時のデータを示す. データ計測には, 軸加速度, 軸角速度を高精度に計測できる慣性センサ ( 社 ) と, 信号を受信するアンテナおよび受信機 ( 社製 ) を利用した. は, とは別に システムを搭載しており, 車体ヨー角も高精度に計測できる. 図のデータは, 車速 から まで, 横加速度, 減速度は の範囲で変化する通常走行である. 図 は, ドップラーから推定した車体ヨー角と, 信号の合計受信時間の推移である. のサンプリング時間は である. 図 の走行条件下では, 衛星を常時捕捉できず, 信号の連続受信時間は最大 程度となる. また, 全走行区間での受信時間の合計は 程度となる. ドップラーから推定する車体ヨー角は, 車両速度ベクトルの方向を基に推定するため, 低速時にはノイズが多く重畳し, 精度は劣化 Fi.3 Acceleration and Anular Velocity o the Vehicle Meaured S doppler Fi.4 Yaw Anle and Receivin nvironment o S 図 のデータに のヨー角速度ドリフトを印加した場合について, ドリフト推定を行わない場合 ( 点線 ), ドリフト推定を行った場合 ( 実線 ), およびドリフトを印加しない状態での推定値 ( 灰色 ) を図, 図 に示す. また, の横加速度ドリフトを印加した場合についても同様に図, 図 に示す. 図 では, 付近でヨー角速度のドリフト推定値が真値に収束し, 姿勢角推定誤差は低減していることが分かる. このとき, 図 から 信号は合計で 程度受信している. 一方, 横加速度にドリフトを印加した場合, ヨー角速度にドリフトを印加した場合よりも収束は遅いものの, ドリフト推定値は真値に収束していくことが分かる. なお, 提案手法のロバスト性は様々な環境下および試験条件での検証が必要であり, 今後の課題である. Vol.46,No.2,March 25. 4

6 Without it With it etimation(olid) Without it etimation(dahed) With it etimation(olid) Without it Without it etimation(dahed) Fi.5 timated Attitude (Drit in yaw anular velocity) Fi.7 timated Attitude (Drit in lateral acceleration) Fi.6 timated Zero Drit (Drit in yaw anular velocity) ま と め センサゼロ点ドリフトの推定に対し, 信号から得られる 車体ヨー角の情報を利用する手法を提案した. このドリフト推定手法と姿勢角推定オブザーバを組み合わせたアルゴリズムについて, 走行データを用いた検証を行ったところ, の利用によって, 精度良くゼロ点ドリフトと車両のロール角, ピッチ角を推定できることが分かった. 参 考 文 献 Fi.8 timated Zero Drit (Drit in lateral acceleration) 小野英一, 三浦有美子, 安富大祐, 山口克之 軸加速度 軸角速度センサを利用した姿勢角推定 自動車技術会論文集 42 自動車技術会論文集