JP2020080743A

JP2020080743A - 車両の姿勢推定装置

Info

Publication number: JP2020080743A
Application number: JP2018220940A
Authority: JP
Inventors: 俊信新帶; Toshinobu Shinobi
Original assignee: Yanmar Co Ltd
Current assignee: Yanmar Co Ltd
Priority date: 2018-11-27
Filing date: 2018-11-27
Publication date: 2020-06-04
Anticipated expiration: 2038-11-27
Also published as: JP7012001B2

Abstract

【課題】車両の姿勢を精度良く推定する車両の姿勢推定装置を提供する。【解決手段】測位情報検出部１５にて検出した測位情報、角速度情報検出部１６にて検出した角速度情報、及び、加速度情報検出部１７にて検出した加速度情報の入力を受けて、カルマンフィルタを用いて、車両の姿勢状態を示す状態推定値を求めるカルマンフィルタ演算部１０９と、操舵角検出部２０にて検出した操舵角に基づいて車両が直進であるか旋回であるかを判定する判定部１０７と、直進と判定した場合に、直進用演算処理にて車両における現在ヨー角を求め、旋回と判定した場合に、旋回用演算処理にて車両における現在ヨー角を求める現在ヨー角演算部１０６と、カルマンフィルタ演算部１０９にて求めた状態推定値におけるヨー角について、現在ヨー角演算部１０６にて求めた現在ヨー角を用いて補正するヨー角補正部１１０とが備えられている。【選択図】図２

Description

本発明は、車両の姿勢を推定する車両の姿勢推定装置に関する。

上記の姿勢推定装置として、カルマンフィルタを用いて二輪車両の姿勢を推定するものが知られている（例えば、特許文献１参照。）。この特許文献１に記載の装置では、車両におけるロール角速度を検出するロール角速度センサ、車両におけるヨー角速度を検出するヨー角速度センサ、車両における上下加速度を検出する上下加速度センサ、車両における前後加速度を検出する前後加速度センサ、車両における左右加速度を検出する左右加速度センサ、後輪速度を検出する後輪速度センサ等のセンサ群が二輪車両に備えられている。

特許文献１に記載の装置では、カルマンフィルタを有する演算部に対して、ロール角速度、ロール角加速度、ヨー角速度、ヨー角加速度、上下加速度、前後加速度、左右加速度、後輪速度に関する検出値が入力されている。演算部は、カルマンフィルタを用いて、ロール角、前後方向車両速度、ロール角加速度センサオフセット、ヨー角速度センサオフセット、左右加速度センサオフセット、上下加速度センサオフセットを推定して出力し、二輪車両の姿勢を推定している。

再公表特許ＷＯ２０１７／１７５８４４

特許文献１に記載の装置では、カルマンフィルタを用いて、ロール角、前後方向車両速度、ロール角加速度センサオフセット、ヨー角速度センサオフセット、左右加速度センサオフセット、上下加速度センサオフセット等を推定しているものの、推定値に誤差が生じる可能性があり、車両の姿勢を更に精度良く推定することが求められている。

この実情に鑑み、本発明の主たる課題は、車両の姿勢を精度良く推定することができる車両の姿勢推定装置を提供する点にある。

本発明の第１特徴構成は、前輪が操舵自在な四輪の車両における姿勢を推定する車両の姿勢推定装置において、
衛星測位システムを用いて前記車両の測位情報を検出する測位情報検出部と、
前記車両におけるロール角速度、ピッチ角速度及びヨー角速度を含む前記車両の角速度情報を検出する角速度情報検出部と、
前記車両におけるロール軸方向の加速度、ピッチ軸方向の加速度及びヨー軸方向の加速度を含む前記車両の加速度情報を検出する加速度情報検出部と、
前記測位情報検出部にて検出した測位情報、前記角速度情報検出部にて検出した角速度情報、及び、前記加速度情報検出部にて検出した加速度情報の入力を受けて、カルマンフィルタを用いて、ロール角、ピッチ角及びヨー角を含む前記車両の姿勢状態を示す状態推定値を求めるカルマンフィルタ演算部と、
前記車両における操舵角を検出する操舵角検出部と、
前記操舵角検出部にて検出した操舵角に基づいて前記車両が直進であるか旋回であるかを判定する判定部と、
前記判定部にて直進と判定した場合に、直進用演算処理にて前記車両における現在ヨー角を求め、前記判定部にて旋回と判定した場合に、旋回用演算処理にて前記車両における現在ヨー角を求める現在ヨー角演算部と、
前記カルマンフィルタ演算部にて求めた状態推定値におけるヨー角について、前記現在ヨー角演算部にて求めた現在ヨー角を用いて補正するヨー角補正部とが備えられている点にある。

本構成によれば、カルマンフィルタ演算部が、ロール角、ピッチ角及びヨー角を含む車両の姿勢状態を示す状態推定値を求めるのに加えて、ヨー角補正部が、カルマンフィルタ演算部にて求めた状態推定値におけるヨー角について、現在ヨー角演算部にて求めた現在ヨー角を用いて補正しているので、ヨー角を適切に補正しながら、精度よく状態推定値を求めることができる。しかも、現在ヨー角演算部が現在ヨー角を求めるに当たり、判定部にて車両における操舵角に基づいて車両が直進であるか旋回であるかを判定して、車両が直進であるときも、旋回であるときも、その状況に対応した演算処理にて現在ヨー角を求めている。よって、現在ヨー角を精度良く求めることができるので、その正確な現在ヨー角を用いて状態推定値を補正することで、状態推定値の精度をより一層向上することができる。

本発明の第２特徴構成は、前記直進用演算処理では、前記現在ヨー角演算部が、前記測位情報検出部にて検出した測位情報から求めた第１演算ヨー角と、前記角速度情報検出部にて検出した角速度情報から求めた第２演算ヨー角とを用いて、現在ヨー角を求め、
前記旋回用演算処理では、前記現在ヨー角演算部が、前記角速度情報検出部にて検出した角速度情報から現在ヨー角を求めている点にある。

本構成によれば、現在ヨー角を求める場合に、車両が直進であれば、現在ヨー角演算部が、測位情報検出部にて検出した測位情報から求めた第１演算ヨー角と、角速度情報検出部にて検出した角速度情報から求めた第２演算ヨー角とを用いて、現在ヨー角を求めるので、直進であるときの現在ヨー角を正確に求めることができる。また、車両が旋回であるときには、現在ヨー角演算部が、角速度情報検出部にて検出した角速度情報から現在ヨー角を求めているので、旋回であるときの現在ヨー角を正確に求めることができる。これにより、車両が直進であるときも旋回であるときも、現在ヨー角を正確に求めることができ、状態推定値の精度向上を図ることができる。

本発明の第３特徴構成は、前記操舵角検出部にて検出した操舵角に基づいて前記車両が直進であると判定した場合に、前記測位情報検出部にて検出した測位情報から初期ヨー角を求める初期ヨー角演算部が備えられ、
前記カルマンフィルタ演算部は、前記初期ヨー角演算部にて求めた初期ヨー角を用いて状態推定値を求める初期演算を行い、その後、前回求めた状態推定値を用いて状態推定値を求める通常演算を繰り返し行う点にある。

本構成によれば、初期ヨー角演算部が、車両が直進であるときの初期ヨー角を求め、カルマンフィルタ演算部が、初期ヨー角演算部にて求めた初期ヨー角を用いて状態推定値を求める初期演算を行うので、カルマンフィルタ演算部にて演算を行う当初から状態推定値を精度良く求めることができる。また、カルマンフィルタ演算部は、通常演算にて、前回求めた状態推定値を用いて状態推定値を求めているので、通常演算においても、前回求めた状態推定値を反映させながら、正確な状態推定値を求めることができる。

田植機の側面図姿勢推定装置の概略構成を示すブロック図姿勢推定装置の演算部における動作を示すフローチャート現在ヨー角演算部における動作を示すフローチャート

本発明に係る姿勢推定装置の実施形態を図面に基づいて説明する。
この姿勢推定装置は、図１に示すように、車両としての田植機１に備えられているが、姿勢推定装置を備える車両は、前輪が操舵自在な四輪の車両であればよく、例えば、トラクタやコンバイン等の作業車両だけでなく、その他、各種の車両を適用することができる。

田植機１は、走行機体２の後部に昇降リンク機構３を介して植付部４が備えられている。走行機体２は、前後方向に延びる車体フレーム５が備えられ、車体フレーム５に各種の機器が支持されている。車体フレーム５には、その前方側に駆動源となるエンジン６が支持され、エンジン６を覆うボンネット７が備えられている。

車体フレーム５の前部には、フロントアクスルケース８が支持されており、フロントアクスルケース８の左右両側に前輪９が取り付けられている。車体フレーム５の後部には、リアアクスルケース１０が支持されており、リアアクスルケース１０の左右両側に後輪１１が取り付けられている。

ボンネット７の上方には、アーチ状の取付フレーム１２が備えられている。取付フレーム１２の左右両側に予備苗載台１３が備えられ、取付フレーム１２の上部中央にセンサユニット１４が備えられている。センサユニット１４には、衛星測位システム（ＮＳＳ：Navigation Satellite System）の一例であるＧＮＳＳ（Global Navigation Satellite System）等を利用して田植機１の現在位置等の位置情報（測位情報）を測定する測位センサ１５（測位情報検出部に相当する）、田植機１におけるロール角速度、ピッチ角速度及びヨー角速度に関する角速度情報を測定する角速度センサ１６（角速度情報検出部に相当する）、田植機１におけるロール軸方向の加速度、ピッチ軸方向の加速度及びヨー軸方向の加速度に関する加速度情報を測定する加速度センサ１７（加速度情報検出部に相当する）が備えられている。

角速度センサ１６及び加速度センサ１７については、角速度センサ１６と加速度センサ１７とがユニット化された慣性計測装置（ＩＭＵ：Inertial Measurement Unit）として備えられている。ちなみに、田植機１（車両）の前後方向をロール軸方向として、田植機１の前方側をロール軸方向のプラス側とし、田植機１（車両）の左右方向をピッチ軸方向として、田植機１の左側をピッチ軸方向のプラス側とし、田植機１（車両）の上下方向をヨー軸方向として、田植機１の上方側をヨー軸方向のプラス側としている。

ボンネット７の後部には、前輪９を操舵自在なステアリング１８が備えられ、ステアリング１８の後部に運転座席１９が備えられている。走行機体２には、田植機１の状態を検出する各種のセンサが備えられている。センサとして、例えば、図２に示すように、センサステアリング角度又は前輪９の切れ角度に関する田植機１の操舵角を検出する操舵角センサ２０（操舵角検出部に相当する）、主変速操作レバーが前進、中立、後進のどの位置に操作されているかを検出するレバー位置センサ２１等が備えられている。

姿勢推定装置は、図２に示すように、各種のセンサの検出値等の入力を受けて、ロール角、ピッチ角及びヨー角を含む田植機１の姿勢状態を示す姿勢状態推定値と田植機１の位置を示す位置状態推定値とを含む状態推定値を求める演算部１００が備えられている。演算部１００は、センサの検出値として、例えば、測位センサ１５の検出値、角速度センサ１６の検出値、加速度センサ１７の検出値、操舵角センサ２０の検出値、レバー位置センサ２１の検出値を入力としている。

演算部１００は、座標変換部１０１、状態遷移判定部１０３、初期ヨー角演算部１０４、現在ヨー角演算部１０６、判定部１０７、カルマンフィルタ演算部１０９、ヨー角補正部１１０等が備えられている。

座標変換部１０１は、測位センサ１５にて検出される田植機１の測位情報、加速度センサ１７の検出値、角速度センサ１６の検出値における座標系を変換している。座標変換部１０１には、測位センサ１５にて検出される田植機１の緯度経度をＮＥＤ座標系に変換した情報が入力されている。座標変換部１０１は、測位センサ１５の検出値に基づくＮＥＤ座標系における田植機１の位置を、ＩＳＯ座標系における位置に変換している。この変換では、例えば、北をＸ軸とし、西をＹ軸とし、上空をＺ軸とし、Ｙ軸とＺ軸の符号を反転させている。また、角速度センサ１６の検出値、及び、加速度センサ１７の検出値は、ＮＥＤ座標系の値であるので、座標変換部１０１は、ＮＥＤ座標系の値を地上座標系に変換し出力している。

状態遷移判定部１０３は、測位センサ１５にて検出される田植機１の測位情報に基づいて、田植機１が走行中であるか否かを判定している。状態遷移判定部１０３には、測位センサ１５にて検出される田植機１の緯度経度をＮＥＤ座標系に変換した情報が入力されている。状態遷移判定部１０３は、測位センサ１５にて検出される田植機１の緯度経度をＮＥＤ座標系に変換したＮ軸の値とＥ軸の値とを微分して速度ベクトルＶｘ，Ｖｙを求めている。状態遷移判定部１０３は、求めた速度ベクトルＶｘ，Ｖｙを用いて、下記の（式１）にてＮＥＤ座標系におけるＸＹ平面上における速度ベクトルの絶対値Ｖｎｏｒｍを求めている。

（式１）
Ｖｎｏｒｍ＝√（Ｖｘ^２＋Ｖｙ^２）

状態遷移判定部１０３は、求めた速度ベクトルの絶対値と閾値とを比較して、求めた速度ベクトルの絶対値Ｖｎｏｒｍが閾値よりも小さい場合に、田植機１が停止中と判定し、求めた速度ベクトルの絶対値Ｖｎｏｒｍが閾値以上の場合に、田植機１が走行中と判定している。

初期ヨー角演算部１０４は、田植機１の初期ヨー角を求めている。初期ヨー角演算部１０４には、測位センサ１５にて検出される田植機１の緯度経度をＮＥＤ座標系に変換した情報に加えて、操舵角センサ２０の検出値、及び、レバー位置センサ２１の検出値が入力されている。初期ヨー角演算部１０４は、操舵角センサ２０の検出値、及び、レバー位置センサ２１の検出値に基づいて、田植機１が進行方向に真っ直ぐ向いているか否かを判定し、田植機１が進行方向に真っ直ぐ向いている場合に、そのときのヨー角を初期ヨー角として求めている。

初期ヨー角演算部１０４は、田植機１が進行方向に真っ直ぐ向いていると判定した場合に、測位センサ１５にて検出される田植機１の緯度経度をＮＥＤ座標系に変換したＮ軸の値とＥ軸の値とを微分して速度ベクトルＶｘ，Ｖｙを求めている。初期ヨー角演算部１０４は、求めた速度ベクトルＶｘ，Ｖｙを用いて、下記の（式２）にて初期ヨー角θを求めている。

（式２）
θ＝ｔａｎ^−１（Ｖｙ／Ｖｘ）

初期ヨー角演算部１０４は、求めた初期ヨー角θを用いて、その初期ヨー角θを向いた状態を示す姿勢の初期姿勢状態値を求めている。初期ヨー角演算部１０４は、初期姿勢状態値に加えて、田植機１の初期位置を示す任意の初期位置状態値を設定して、初期姿勢状態値と初期位置状態値とを含む初期状態値を出力している。

現在ヨー角演算部１０６は、田植機１の現在ヨー角を求めている。現在ヨー角演算部１０６には、座標変換部１０１にて座標変換された測位センサ１５による位置情報、初期ヨー角演算部１０４からの初期状態値、座標変換部１０１にて座標変換された角速度センサ１６による角速度情報、状態遷移判定部１０３の判定結果、操舵角センサ２０の検出値、レバー位置センサ２１の検出値等が入力されている。

現在ヨー角演算部１０６は、最初、初期ヨー角演算部１０４からの初期状態値における初期ヨー角を現在ヨー角として求めている。その後、現在ヨー角演算部１０６は、カルマンフィルタ演算部１０９が演算処理を実行中でなければ、前回求めた現在ヨー角をそのまま今回の現在ヨー角として求めている。

カルマンフィルタ演算部１０９が演算処理を実行中である場合には、田植機１が停止中であると、現在ヨー角演算部１０６が、前回求めた現在ヨー角をそのまま今回の現在ヨー角として求めている。カルマンフィルタ演算部１０９が演算処理を実行中である場合に、田植機１が走行中であると、判定部１０７が、操舵角センサ２０の検出値、及び、レバー位置センサ２１の検出値に基づいて、田植機１が直進か旋回かを判定している。現在ヨー角演算部１０６は、判定部１０７にて直進と判定した場合に、直進用演算処理にて田植機１の現在ヨー角を求め、判定部１０７にて旋回と判定した場合に、旋回用演算処理にて田植機１の現在ヨー角を求めている。

直進用演算処理では、現在ヨー角演算部１０６が、測位センサ１５にて検出した測位情報から求めた第１演算ヨー角θ１と、角速度センサ１６にて検出した角速度情報から求めた第２演算ヨー角θ２とを用いて、現在ヨー角を求めている。また、旋回用演算処理では、現在ヨー角演算部１０６が、角速度センサ１６にて検出した角速度情報から現在ヨー角を求めている。

カルマンフィルタ演算部１０９は、各種の入力情報に基づいて、田植機１の位置を示す位置状態推定値（例えば、田植機１（測位センサ１５）の位置、田植機１（測位センサ１５）の速度、田植機１（加速度センサ１７）の加速度、田植機１の３軸の角速度、田植機１の３軸の角加速度）と田植機１の姿勢を示す姿勢状態推定値とを含む状態推定値を求めている。カルマンフィルタ演算部１０９には、座標変換部１０１にて座標変換された測位センサ１５による位置情報、初期ヨー角演算部１０４からの初期状態値、座標変換部１０１にて座標変換された角速度センサ１６による角速度情報及び加速度センサ１７による加速度情報、カルマンフィルタ演算部１０９にて前回求めた状態推定値等が入力されている。

カルマンフィルタ演算部１０９は、下記の（１）〜（６）を状態変数として、状態推定値を求めている。また、カルマンフィルタ演算部１０９には、後輪車軸中心から測位センサ１５までの３軸の位置ベクトル（規定値）、後輪車軸中心から角速度センサ１６及び加速度センサ１７までの３軸の位置ベクトル（規定値）が入力されている。

（１）地上座標系から見た測位センサ１５の位置
（２）地上座標系から見た測位センサ１５の速度
（３）地上座標系から見た角速度センサ１６及び加速度センサ１７の加速度
（４）地上座標系から見た田植機１の姿勢
（５）機体座標系から見た田植機１の角速度
（６）機体座標系から見た田植機１の角加速度

ちなみに、地上座標系は、地上のある一点を原点に設定して、そのときの北をＸ軸、西をＹ軸、上空をＺ軸とした座標系である。機体座標系は、田植機１の後輪車軸中心を原点として、田植機１の前方側をＸ軸、田植機１の左側をＹ軸、田植機１の上方側をＺ軸とした座標系である。

ヨー角補正部１１０は、現在ヨー角演算部１０６にて求めた現在ヨー角を用いて、カルマンフィルタ演算部１０９にて求められた状態推定値に含まれるヨー角を補正している。ヨー角補正部１１０には、現在ヨー角演算部１０６からの現在姿勢情報、カルマンフィルタ演算部１０９にて求められた状態推定値等が入力されている。ヨー角補正部１１０は、現在ヨー角演算部１０６にて求めた現在ヨー角と、カルマンフィルタ演算部１０９にて求められた状態推定値に含まれるヨー角との差分を真のヨー角（補正後のヨー角）として求めている。ヨー角補正部１１０は、カルマンフィルタ演算部１０９にて求められた状態推定値に含まれるヨー角を、求めた真のヨー角を用いて補正したヨー角補正状態推定値を出力している。

以下、図２のブロック図、及び、図３のフローチャートに基づいて、姿勢推定装置の演算部１００における動作について説明する。
まず、演算部１００は、初期ヨー角演算を行う必要があるか否かを判定しており、初期ヨー角演算を行う必要がある場合に、初期ヨー角を演算している（ステップ＃１のＹｅｓの場合、ステップ＃２）。初期ヨー角演算部１０４が初期ヨー角を演算すると、現在ヨー角演算部１０６が現在ヨー角を演算している（ステップ＃３）。現在ヨー角演算部１０６にて現在ヨー角を演算するときの動作については後述する。

また、例えば、既に初期ヨー角演算部１０４にて初期ヨー角を演算している場合等、初期ヨー角演算を行う必要がない場合には、初期ヨー角の演算を行うことなく、現在ヨー角演算部１０６が現在ヨー角を演算している（ステップ＃１のＮｏの場合、ステップ＃３）。

カルマンフィルタ演算部１０９は、各種の情報の入力を受けると、カルマンフィルタを用いて、位置状態推定値と姿勢状態推定値とを含む状態推定値を演算している（ステップ＃４）。ちなみに、カルマンフィルタ演算部１０９は、各種の情報の入力を受けると演算処理を行っており、現在ヨー角演算部１０６にて現在ヨー角が演算されているか否かにかかわらず、カルマンフィルタ演算部１０９での演算処理が行われる。

カルマンフィルタ演算部１０９にて状態推定値を求めると、ヨー角補正部１１０は、現在ヨー角演算部１０６にて求めた現在ヨー角を用いて、カルマンフィルタ演算部１０９にて求めた状態推定値に含まれるヨー角を補正している（ステップ＃５）。

以上の如く、姿勢推定装置の演算部１００における動作について説明したが、姿勢推定装置の演算部１００にて出力される補正後の状態推定値は、例えば、田植機１の現在の姿勢を把握するためや、田植機１の姿勢を所望の姿勢に維持する制御を行うために用いられている。

演算部１００は、図３にて示す動作を所定周期（例えば、測位センサ１５にて田植機１の位置情報を検出する周期）で繰り返し行っている。これにより、田植機１の姿勢を所定周期で繰り返し推定することができる。演算部１００が繰り返し演算処理を行うのに当たり、カルマンフィルタ演算部１０９は、最初、初期ヨー角演算部１０４にて求めた初期ヨー角の入力を受けるので、初期ヨー角演算部１０４にて求めた初期ヨー角を用いて状態推定値を求める初期演算を行っている。その後、カルマンフィルタ演算部１０９は、前回求めた状態推定値の入力を受けるので、前回求めた状態推定値を用いて状態推定値を求める通常演算を繰り返し行っている。

図４に示すフローチャートに基づいて、現在ヨー角演算部１０６にて現在ヨー角を演算する際の動作について説明する。
まず、現在ヨー角演算部１０６は、状態遷移判定部１０３の判定結果の入力に基づいて、カルマンフィルタ演算部１０９にて演算処理を実行中であるか否かを判定している（ステップ＃１１）。カルマンフィルタ演算部１０９にて演算処理を実行中でない場合には、現在ヨー角演算部１０６が、前回求めた現在ヨー角を今回の現在ヨー角として求め、前回値を保持している（ステップ＃１１のＮｏの場合、ステップ＃１２）。

カルマンフィルタ演算部１０９にて演算処理を実行中である場合には、現在ヨー角演算部１０６が、状態遷移判定部１０３の判定結果の入力に基づいて、田植機１が走行中であるか否かを判定している（ステップ＃１１のＹｅｓの場合、ステップ＃１３）。田植機１が走行中でない場合には、現在ヨー角演算部１０６が、前回求めた現在ヨー角を今回の現在ヨー角として求め、前回値を保持している（ステップ＃１３のＮｏの場合、ステップ＃１２）。

田植機１が走行中である場合には、判定部１０７が、操舵角センサ２０の検出値、及び、レバー位置センサ２１の検出値に基づいて、田植機１が直進か旋回かを判定している（ステップ＃１１のＹｅｓの場合、ステップ＃１４）。田植機１が旋回であると、現在ヨー角演算部１０６が、角速度センサ１６にて検出した角速度情報から現在ヨー角を求める旋回用演算処理を行うことで、現在ヨー角を求めている（ステップ＃１４の旋回の場合、ステップ＃１５）。

田植機１が直進であると、現在ヨー角演算部１０６が、測位センサ１５にて検出した測位情報から求めた第１演算ヨー角θ１と、角速度センサ１６にて検出した角速度情報から求めた第２演算ヨー角θ２とを用いて、現在ヨー角を求める直進用演算処理を行うことで、現在ヨー角を求めている（ステップ＃１４の直進の場合、ステップ＃１６）。

このようにして、現在ヨー角演算部１０６は、カルマンフィルタ演算部１０９にて演算処理を実行中である否か、田植機１が走行中であるか否か、田植機１が走行中であれば直進か旋回かの夫々の状況に応じて求め方を変更しながら、現在ヨー角を求めており、各状況に適した正確な現在ヨー角を求めることができる。よって、ヨー角補正部１１０では、現在ヨー角演算部１０６にて求めた現在ヨー角を用いて、カルマンフィルタ演算部１０９にて求めた状態推定値におけるヨー角を補正するので、正確な状態推定値を出力することができる。

〔別実施形態〕
本発明の他の実施形態について説明する。
尚、以下に説明する各実施形態の構成は、夫々単独で適用することに限らず、他の実施形態の構成と組み合わせて適用することも可能である。

（１）上記実施形態において、カルマンフィルタ演算部１０９にて求めた状態推定値に対して、更なる補正を加えることもできる。例えば、姿勢角の変化に伴う過渡応答を改善するための補正を加えることができる。

（２）上記実施形態では、現在ヨー角演算部１０６が、直進用演算処理において、測位センサ１５にて検出した測位情報から求めた第１演算ヨー角と、角速度センサ１６にて検出した角速度情報から求めた第２演算ヨー角とを用いて、現在ヨー角を求めているが、直進用演算処理として、この演算処理に限らず、他の演算処理を用いることもできる。
また、現在ヨー角演算部が、旋回用演算処理において、角速度センサ１６にて検出した角速度情報から現在ヨー角を求めているが、旋回用演算処理として、この演算処理に限らず、他の演算処理を用いることもできる。

１田植機（車両）
１５測位センサ（測位情報検出部）
１６角速度センサ（角速度情報検出部）
１７加速度センサ（加速度情報検出部）
２０操舵角センサ（操舵角検出部）
１０４初期ヨー角演算部
１０６現在ヨー角演算部
１０７判定部
１０９カルマンフィルタ演算部
１１０ヨー角補正部

Claims

前輪が操舵自在な四輪の車両における姿勢を推定する車両の姿勢推定装置において、
衛星測位システムを用いて前記車両の測位情報を検出する測位情報検出部と、
前記車両におけるロール角速度、ピッチ角速度及びヨー角速度を含む前記車両の角速度情報を検出する角速度情報検出部と、
前記車両におけるロール軸方向の加速度、ピッチ軸方向の加速度及びヨー軸方向の加速度を含む前記車両の加速度情報を検出する加速度情報検出部と、
前記測位情報検出部にて検出した測位情報、前記角速度情報検出部にて検出した角速度情報、及び、前記加速度情報検出部にて検出した加速度情報の入力を受けて、カルマンフィルタを用いて、ロール角、ピッチ角及びヨー角を含む前記車両の姿勢状態を示す状態推定値を求めるカルマンフィルタ演算部と、
前記車両における操舵角を検出する操舵角検出部と、
前記操舵角検出部にて検出した操舵角に基づいて前記車両が直進であるか旋回であるかを判定する判定部と、
前記判定部にて直進と判定した場合に、直進用演算処理にて前記車両における現在ヨー角を求め、前記判定部にて旋回と判定した場合に、旋回用演算処理にて前記車両における現在ヨー角を求める現在ヨー角演算部と、
前記カルマンフィルタ演算部にて求めた状態推定値におけるヨー角について、前記現在ヨー角演算部にて求めた現在ヨー角を用いて補正するヨー角補正部とが備えられている車両の姿勢推定装置。
前記直進用演算処理では、前記現在ヨー角演算部が、前記測位情報検出部にて検出した測位情報から求めた第１演算ヨー角と、前記角速度情報検出部にて検出した角速度情報から求めた第２演算ヨー角とを用いて、現在ヨー角を求め、
前記旋回用演算処理では、前記現在ヨー角演算部が、前記角速度情報検出部にて検出した角速度情報から現在ヨー角を求めている請求項１に記載の車両の姿勢推定装置。
前記操舵角検出部にて検出した操舵角に基づいて前記車両が直進であると判定した場合に、前記測位情報検出部にて検出した測位情報から初期ヨー角を求める初期ヨー角演算部が備えられ、
前記カルマンフィルタ演算部は、前記初期ヨー角演算部にて求めた初期ヨー角を用いて状態推定値を求める初期演算を行い、その後、前回求めた状態推定値を用いて状態推定値を求める通常演算を繰り返し行う請求項１又は２に記載の車両の姿勢推定装置。