(57)【要約】【課題】傾斜角を算出する精度を向上させることができる作業車両の傾斜角検出装置を提供する。【解決手段】加速度センサ（３２）、角速度センサ（３３）、及び速度センサ（３１）からの各信号を入力し、車体のピッチ方向における傾斜角を演算するコントローラ（５０）を備えた作業車両において、コントローラは、速度センサにて検出された速度信号を微分して加速度成分を求め（５１）、当該加速度成分を加速度センサにて検出された加速度信号から除去して傾斜角を演算し（５２）、車体の速度が大きいほど観測誤差を大きく、速度が小さいほど観測誤差を小さくなるように観測誤差を設定し（５３）、演算された傾斜角と、設定された観測誤差と、角速度信号とに基づいて、カルマンフィルタ処理を実行して車体の傾斜角と角速度センサのオフセットとを更新する（６０）、ことを特徴とする。【選択図】図３
2019174300.tif 000002
【特許請求の範囲】
【請求項１】
車体に搭載され、前記車体の前後方向の加速度を検出すると共に検出された前記加速度を加速度信号として出力する加速度センサと、
前記車体に搭載され、前記車体のピッチ方向の角速度を検出すると共に検出された前記角速度を角速度信号として出力する角速度センサと、
前記車体に搭載され、前記車体の速度を検出すると共に検出された前記速度を速度信号として出力する速度センサと、
前記加速度センサ、前記角速度センサ、及び前記速度センサからの各信号を入力し、前記車体のピッチ方向における傾斜角を演算するコントローラと、を備えた作業車両の傾斜角検出装置において、
前記コントローラは、
前記速度センサにて検出された前記速度信号を微分して加速度成分を求め、当該加速度成分を前記加速度センサにて検出された前記加速度信号から除去して前記傾斜角を演算し、
前記速度センサにて検出された前記速度信号に応じて、前記車体の速度が大きいほど観測誤差を大きく、前記速度が小さいほど前記観測誤差を小さくなるように前記観測誤差を設定し、
演算された前記傾斜角と、設定された前記観測誤差と、前記角速度センサにて検出された前記角速度信号とに基づいて、カルマンフィルタ処理を実行して前記車体の傾斜角と前記角速度センサのオフセットとを更新する、ことを特徴とする作業車両の傾斜角検出装置。
【請求項２】
請求項１に記載の作業車両の傾斜角検出装置において、
前記コントローラは、
前記観測誤差に対してフィルタ処理を実行して、
前記観測誤差の上昇時には速く、前記観測誤差の下降時には遅くして異ならせるように前記観測誤差を設定することを特徴とする作業車両の傾斜角検出装置。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、ホイールローダ等の作業車両に用いられる、作業車両の傾斜角検出装置に関する。
【背景技術】
【０００２】
本技術分野の背景技術として、例えば特許文献１には、車体に搭載された加速度センサと速度センサを備え、加速度センサの観測誤差と加速度センサにより検出した加速度と、速度センサにより検出した速度とに基づき、公知のカルマンフィルタ処理を実行することによって、車体の傾斜角（姿勢角ともいう）と加速度センサのオフセットを更新する制御技術が開示されている。
【先行技術文献】
【特許文献】
【０００３】
【特許文献１】特開２０１１−１０２７８４号公報
【発明の概要】
【発明が解決しようとする課題】
【０００４】
しかしながら、特許文献１に記載された従来技術では、車体の傾斜する傾斜角をある程度精度良く検出し補正することができるが、角速度を考慮していない分、さらに傾斜角を精度良く検出する余地が残されている。また、特許文献１に記載された従来技術を、ホイールローダのような不整地の走行が主となる作業車両に適用しようとすると、振動の影響を受けてしまい、車体の傾斜角が精度良く得られない可能性がある。
【０００５】
本発明の目的は、車体の振動が発生しやすい不整地を走行する際にも、傾斜角を算出する精度を向上させることができる作業車両の傾斜角検出装置を提供することにある。
【課題を解決するための手段】
【０００６】
上記の目的を達成するために、代表的な本発明は、車体に搭載され、前記車体の前後方向の加速度を検出すると共に検出された加速度を加速度信号として出力する加速度センサと、前記車体に搭載され、前記車体のピッチ方向の角速度を検出すると共に検出された角速度を角速度信号として出力する角速度センサと、前記車体に搭載され、前記車体の速度を検出すると共に検出された速度を速度信号として出力する速度センサと、前記加速度センサ、前記角速度センサ、及び前記速度センサからの各信号を入力し、前記車体のピッチ方向における傾斜角を演算するコントローラと、を備えた作業車両において、前記コントローラは、前記速度センサにて検出された前記速度信号を微分して加速度成分を求め、当該加速度成分を前記加速度センサにて検出された前記加速度信号から除去して前記傾斜角を算出し、前記速度センサにて検出された前記速度信号に応じて、前記車体の速度が大きいほど前記観測誤差を大きく、前記速度が小さいほど前記観測誤差を小さくなるように前記観測誤差を設定し、算出された前記傾斜角と、設定された前記観測誤差と、前記角速度センサにて検出された前記角速度信号とに基づいて、カルマンフィルタ処理を実行して前記車体の傾斜角と前記角速度センサのオフセットとを更新する、ことを特徴とする。
【発明の効果】
【０００７】
本発明に係る作業車両の傾斜角検出装置によれば、車体の振動が発生しやすい不整地を走行する際にも、傾斜角を算出する精度を向上させることができる。なお、前述した以外の課題、構成、及び効果は、以下の実施形態の説明により明らかにされる。
【図面の簡単な説明】
【０００８】
【図１】本発明の実施形態に係るホイールローダの側面図である。
【図２】コントローラのハードウェア構成を模式的に示すブロック図である。
【図３】コントローラの機能構成を示すブロック図である。
【図４】コントローラによる演算に用いられる座標系を示す図である。
【図５】傾斜角の推定手順を示すフローチャートである。
【図６】カルマンフィルタ処理の手順を示すフローチャートである。
【図７】本実施形態の効果を説明するための図である。
【図８】変形例の効果を説明するための図である。
【発明を実施するための形態】
【０００９】
以下、本発明に係る作業車両の傾斜角検出装置が適用されるホイールローダを例に挙げ、その実施形態の図面を参照しつつ説明する。
【００１０】
図１は本発明の実施形態に係るホイールローダ１の側面図である。図１に示すように、ホイールローダ１は、一対のブーム２、バケット３、一対の前輪４等を有する前フレーム（車体）５と、運転室６、エンジン室７、一対の後輪８等を有する後フレーム（車体）９とで構成されている。エンジン室７にはエンジン２５が搭載されており、後フレーム９の後方にはカウンタウェイト１０が取り付けられている。
【００１１】
一対のブーム２は一対のブームシリンダ１１の駆動により上下方向に回動（俯仰動）し、バケット３はバケットシリンダ１２の駆動により上下方向に回動（クラウドまたはダンプ）する。バケットシリンダ１２とバケット３の間にはベルクランク１３を含むリンク機構が介設されており、このリンク機構を介してバケットシリンダ１２はバケット３を回動させる。なお、これら一対のブーム２、バケット３、一対のブームシリンダ１１、バケットシリンダ１２、ベルクランク１３等によって作業機１４が構成されている。
【００１２】
前フレーム５と後フレーム９とはセンタピン１５により互いに回動自在に連結され、ステアリングシリンダ（不図示）の伸縮により後フレーム９に対し前フレーム５が左右に屈折する。後フレーム９の前部に搭載された運転室６には、オペレータが座る運転席、ホイールローダ１の操舵角を制御するステアリングホイールと、ホイールローダ１を始動・停止させるキースイッチ、オペレータへの情報を提示する表示装置（いずれも図示せず）等が設置されている。
【００１３】
また、ホイールローダ１には、ホイールローダ１の移動速度に対応した速度（走行速度）を検出するための速度センサ３１、ホイールローダ１の前後方向に対応した加速度を検出するための加速度センサ３２、ホイールローダ１のピッチ方向に対応した角速度を検出するための角速度センサ３３が設けられている。これら各センサ３１，３２，３３からの各信号は、コントローラ５０に入力される。コントローラ５０は、入力された各信号に基づいて後述する車体の傾斜角θ（図４（ｂ）参照）の演算を行う。なお、加速度センサ３２、角速度センサ３３の代わりに、車体加速度及び車体角速度を検出するＩＭＵ（Ｉｎｅｒｔｉａｌ  Ｍｅａｓｕｒｅｍｅｎｔ  Ｕｎｉｔ／慣性計測装置）を使用しても良い。
【００１４】
図２はコントローラ５０のハードウェア構成を模式的に示すブロック図である。図２に示すように、コントローラ５０は、車体の動作全体を制御するための各種演算を行うＣＰＵ（Ｃｅｎｔｒａｌ  Ｐｒｏｃｅｓｓｉｎｇ  Ｕｎｉｔ）５０Ａと、ＣＰＵ５０Ａによる演算を実行するためのプログラムを格納するＲＯＭ（Ｒｅａｄ  Ｏｎｌｙ  Ｍｅｍｏｒｙ）５０Ｂの記憶装置と、ＣＰＵ５０Ａがプログラムを実行する際の作業領域となるＲＡＭ（Ｒａｎｄｏｍ  Ａｃｃｅｓｓ  Ｍｅｍｏｒｙ）５０Ｃと、外部の装置との間で各種の情報や信号の入出力を行う入出力インターフェース５０Ｄとを含むハードウェアから構成されている。
【００１５】
このようなハードウェア構成において、ＲＯＭ５０Ｂに格納されたプログラムがＲＡＭ５０Ｃに読み出され、ＣＰＵ５０Ａの制御に従って動作することによりプログラム（ソフトウェア）とハードウェアとが協働して、コントローラ５０の機能を実現する機能ブロックが構成される。
【００１６】
図３はコントローラ５０の機能構成を示すブロック図である。図３に示すように、コントローラ５０には、速度センサ３１から出力された速度信号Ｖ、加速度センサ３２から出力された加速度信号ａｘ、角速度センサ３３から出力された角速度信号θドットが入力され、以下に述べる演算により、最終的に車体の傾斜角θが出力される。コントローラ５０は、並進加速度除去部５１、傾斜角演算部５２、観測誤差設定部５３、カルマンフィルタから成る状態推定部６０を含む。状態推定部６０は、推定値演算部６１、観測残差演算部６２、カルマンゲイン演算部６３、状態変数更新部６４、状態変数記憶部６５を含む。
【００１７】
以下、各部の機能の詳細について説明する。なお、以下の説明は図４に示す座標系に従うものとする。
【００１８】
推定値演算部６１は、角速度センサ３３からの角速度信号θドットを入力する。ここでは、時刻ｋでの角速度信号をθｋドットと示し、時刻ｋでの状態変数ｘｋを定義する。状態変数ｘｋは傾斜角θと角速度のオフセットθｂドットを含み、以下の数式１で表される。
【数１】
2019174300.tif 000003
【００１９】
推定値演算部６１は、入力ｕｋ及び１回前の状態変数ｘｋ−１に基づき数式２に示した状態方程式により、時刻ｋでの推定値ｘｋハットを演算する。ここで、数式２において、入力ｕｋはθｋドットとする。なお、状態変数ｘｋ−１は、状態変数記憶部６５から入力される。
【数２】
2019174300.tif 000004
【００２０】
Δｔをサンプリング間隔とすると、数式２におけるシステム行列Ａは数式３、入力行列Ｂは数式４のように示すことができる。
【数３】
2019174300.tif 000005
【数４】
2019174300.tif 000006
【００２１】
観測残差演算部６２は、傾斜角演算部５２から傾斜角θの観測値ｚｋと推定値演算部６１から推定値ｘｋハットとを入力し、数式５により観測値ｚｋと推定値ｘｋハットとの観測残差ｙｋを演算する。
【数５】
2019174300.tif 000007
【００２２】
ここで、出力行列Ｈは数式６のように示される。
【数６】
2019174300.tif 000008
【００２３】
また、観測値Ｚｋは数式７により演算できる。ここで、数式７において、ａxｋは加速度センサの加速度信号、Ｖｋ−１，Ｖｋは速度センサの速度信号、ｇは重力加速度である。
【数７】
2019174300.tif 000009
【００２４】
並進加速度除去部５１は、速度センサ３１からの速度信号Ｖと加速度センサ３２からの加速度信号ａxとを入力し、加速度信号ａxから、速度信号Ｖを微分して求めた加速度成分を除去する。傾斜角演算部５２は、並進加速度除去部５１によって加速度成分が除去された加速度信号に基づいて、上記した数式７により観測値Ｚｋを演算する。
【００２５】
観測誤差設定部５３は、速度センサ３１から速度信号Ｖを入力し、時刻ｋにおける観測値Ｚｋの観測誤差δθｋを設定する。観測値Ｚｋの観測誤差δθｋは車両の移動時や停車時など場面によって異なることから、観測誤差設定部５３は、速度信号Ｖ（車速）を構成要素として規定した数式８により観測誤差δθｋを演算する。この数式８は、以下に示すように、速度信号Ｖが小さくなると小さくなり、速度信号が大きくなると大きくなるように規定されている。ここで、数式８において、α、βは係数である。
【数８】
2019174300.tif 000010
【００２６】
なお、観測誤差δθｋを上記した数式８も用いて演算する以外にも、例えば、速度信号Ｖｋと観測値Ｚｋの観測誤差δθｋとの対応関係が予め規定されたテーブルをＲＯＭ５０Ｂ等に記憶しておき、観測誤差設定部５３は、このテーブルを参照して観測誤差δθｋを決定しても良い。
【００２７】
カルマンゲイン演算部６３は、推定値演算部６１からの推定値ｘｋハットと、観測誤差設定部５３からの観測誤差δθｋとを入力し、システム誤差Ｑ、共分散行列Ｒｋ等に基づいて、カルマンフィルタ６０のカルマンゲインＫｋを演算する。観測値Ｚｋの観測誤差δθｋが標準偏差を表すものとして、共分散行列Ｒｋを数式９のように規定する。
【数９】
2019174300.tif 000011
なお、共分散行列Ｒｋ等からカルマンゲインＫｋを算出する方法は、公知であるため、ここでの説明は省略する。
【００２８】
状態変数更新部６４は、観測残差演算部６２から観測残差ｙｋを入力し、推定値演算部６１から推定値ｘｋハットを入力し、カルマンゲイン演算部６３からカルマンゲインＫｋを入力する。状態変数更新部６４は、推定値ｘｋハット、ゲインＫｋ、及び観測残差ｙｋに基づいて、数式１０を演算することによって、状態変数ｘｋを更新する。即ち、状態変数更新部６４は、ホイールローダ１の傾斜角θと角速度センサ３３のオフセットを更新する。
【数１０】
2019174300.tif 000012
【００２９】
そして、状態変数更新部６４は、更新した状態変数ｘｋの傾斜角θを出力する。また、更新した状態変数ｘｋは状態変数記憶部６５に記憶され、次の離散時間後の推定値ｘｋ＋１ハットに使用される。
【００３０】
次に、コントローラ５０による傾斜角θの演算処理の手順について説明する。図５は傾斜角θの推定手順を示すフローチャートである。図５に示すように、コントローラ５０は、速度センサ３１から速度信号Ｖを取得し（Ｓ１０）、加速度センサ３２から加速度信号ａxを取得し（Ｓ１２）、角速度センサ３３から角速度信号θドットを取得する（Ｓ１３）。コントローラ５０は、速度信号Ｖを微分して加速度成分を除去し（Ｓ４）、加速度信号ａxから速度信号Ｖを微分して得られた加速度成分が除去された加速度信号に基づき傾斜角を演算する（Ｓ５）。次いで、コントローラ５０は観測誤差δθｋを設定し（Ｓ６）、カルマンフィルタ処理を実行し（Ｓ７）、傾斜角θを出力する（Ｓ８）。処理が継続されるのであれば（Ｓ９／Ｙｅｓ）、Ｓ１に戻る。処理が継続されないのであれば（Ｓ９／Ｎｏ）、処理は終了する。
【００３１】
次に、Ｓ７において実行されるカルマンフィルタ処理の詳細について説明する。図６はカルマンフィルタ処理の手順を示すフローチャートである。図６に示すように、推定値演算部６１は、前回の状態変数ｘｋを取得し（Ｓ１１）、推定値ｘｋハットを演算する（Ｓ１２）次いで、観測残差演算部６２は、観測残差ｙｋを演算する（Ｓ１３）。次いで、カルマンゲイン演算部６３は、カルマンゲインＫｋを演算する（Ｓ１４）。次いで、状態変数更新部６４は、状態変数を更新し（Ｓ１５）、状態変数記憶部６５は、更新された状態変数を記憶する（Ｓ１６）。
【００３２】
（効果）
本実施形態によれば、カルマンゲインＫｋが小さければ、推定値ｘｋハットの重みが大きく、カルマンゲインＫｋが大きければ、観測残差ｙｋの重みが大きいということになる。カルマンゲインＫｋの大きさは、観測誤差設定部５３による観測誤差δθｋの設定値に左右され、観測誤差δθｋの設定値が小さければカルマンゲインＫｋが大きくなり、観測誤差δθｋの設定値が大きければカルマンゲインＫｋが小さくなる。本実施形態では、速度信号（車速）Ｖが低速度領域にある場合に比べて速度信号（車速）Ｖが高速度領域にある場合の方が、観測誤差δθｋが大きい値に設定される。すなわち、速度信号（車速）Ｖが高速度領域にあると推定値ｘｋハットの重みが大きくなる。
【００３３】
加速度センサ３２による傾斜角算出はホイールローダ１が停車しているときは検出精度が高いが、走行中は並進運動や振動の影響を受け、検出精度が低下する（図７ｂの角速度角度変換）。一方、角速度センサ３３はホイールローダ１が走行中においても並進運動の影響を受けずに検出できるが、センサ出力値にオフセットを含んでいるため、傾斜角算出のために積分すると誤差が累積する（図７ｂの角速度積分）。本実施形態では、速度信号（車速）Ｖを基に観測誤差を設定し、観測誤差を基に傾斜角θと角速度センサ３３のオフセットとを更新するので、速度信号（車速）によって変化する加速度センサ３２の検出精度の変化を反映させた傾斜角θと角速度センサ３３のオフセットを得ることができる。
【００３４】
これについて、図７を使用して説明を行う。観測誤差を低い値の一定値とした場合（図７ｃのＲ固定１）、車速が低い間（図７ａの車速の１〜２秒）は、良好に傾斜角度推定ができている（図７ｄのＲ固定１の１〜２秒）が、車速が上昇すると（図７ａの２〜１０秒）、加速度センサ３２による傾斜角算出の精度が低下（図７ｂの２〜１０秒）し、傾斜角θも影響を受けて精度が悪化する（図７ｄの角度推定（Ｒ固定１）の２〜１０秒）。一方、観測誤差を高い値の一定値とした場合（図７ｃのＲ固定２）、角速度センサ３３が持つオフセットの推定精度が悪化し、傾斜角推定の誤差が時間の経過と共に累積している（図７ｄの角度推定（Ｒ固定２）１〜２秒）。観測誤差を車速に応じて変化させると（図７ｃのＲ可変）、車速によって変化する加速度センサ３２の精度変化（図７ｂの加速度角度変換）を傾斜角θと角速度センサ３３のオフセットに反映させることができ、良好に傾斜角推定ができていることがわかる（図７ｄの角度推定（Ｒ可変））。従って、速度信号（車速）を基に観測誤差を設定することにより、車体の振動が発生し易い不整地を走行する際においても、ホイールローダ１の傾斜角θを高精度で検出することができる（図７参照）。
【００３５】
以上より、本実施形態で得られた傾斜角θに基づいて、例えばトランスミッションの変速スケジュールを変更する構成にすると、登坂路走行の場面において最適な変速を実施することができ、変速頻度の低減が可能となる。
【００３６】
（変形例）
本実施形態において、観測誤差δθｋの設定値に対してフィルタ処理を行って、観測誤差δθｋの設定値の立ち上がり時は速く、立下り時は遅くなるようにすることもできる（図８ｃのＲ可変）。これにより、車両移動時から停車直後においても、観測誤差δθｋが直ちに小さくなるのを防ぐことができる。車両の移動時から停車直後は停止時に伴う車体の振動があり、加速度センサ３２の検出精度が低い状態が継続している。すなわち、振動中は観測誤差が大きい状態である。変形例のような構成とすることで、観測誤差を適切に設定することが可能になり、傾斜角推定の精度の悪化を防ぐことができる（図８参照）。
【００３７】
なお、上記した実施形態は、本発明の説明のための例示であり、本発明の範囲をそれらの実施形態にのみ限定する趣旨ではない。当業者は、本発明の要旨を逸脱することなしに、他の様々な態様で本発明を実施することができる。
【符号の説明】
【００３８】
１  ホイールローダ（作業車両）
５  前フレーム（車体）
９  後フレーム（車体）
３１  速度センサ
３２  加速度センサ
３３  角速度センサ
５０  コントローラ
【図１】
2019174300.tif 000013
【図２】
2019174300.tif 000014
【図３】
2019174300.tif 000015
【図４】
2019174300.tif 000016
【図５】
2019174300.tif 000017
【図６】
2019174300.tif 000018
【図７】
2019174300.tif 000019
【図８】
2019174300.tif 000020