JP6759266B2 - 作業車両の傾斜角検出装置 - Google Patents

Description

本発明は、ホイールローダ等の作業車両に用いられる、作業車両の傾斜角検出装置に関する。

本技術分野の背景技術として、例えば特許文献１には、車体に搭載された加速度センサと速度センサを備え、加速度センサの観測誤差と加速度センサにより検出した加速度と、速度センサにより検出した速度とに基づき、公知のカルマンフィルタ処理を実行することによって、車体の傾斜角（姿勢角ともいう）と加速度センサのオフセットを更新する制御技術が開示されている。

特開２０１１−１０２７８４号公報

しかしながら、特許文献１に記載された従来技術では、車体の傾斜する傾斜角をある程度精度良く検出し補正することができるが、角速度を考慮していない分、さらに傾斜角を精度良く検出する余地が残されている。また、特許文献１に記載された従来技術を、ホイールローダのような不整地の走行が主となる作業車両に適用しようとすると、振動の影響を受けてしまい、車体の傾斜角が精度良く得られない可能性がある。

本発明の目的は、車体の振動が発生しやすい不整地を走行する際にも、傾斜角を算出する精度を向上させることができる作業車両の傾斜角検出装置を提供することにある。

上記の目的を達成するために、代表的な本発明は、車体に搭載され、前記車体の前後方向の加速度を検出すると共に検出された加速度を加速度信号として出力する加速度センサと、前記車体に搭載され、前記車体のピッチ方向の角速度を検出すると共に検出された角速度を角速度信号として出力する角速度センサと、前記車体に搭載され、前記車体の速度を検出すると共に検出された速度を速度信号として出力する速度センサと、前記加速度センサ、前記角速度センサ、及び前記速度センサからの各信号を入力し、前記車体のピッチ方向における傾斜角を演算するコントローラと、を備えた作業車両において、前記コントローラは、前記速度センサにて検出された前記速度信号を微分して加速度成分を求め、当該加速度成分を前記加速度センサにて検出された前記加速度信号から除去して前記傾斜角を算出し、前記速度センサにて検出された前記速度信号に応じて、前記車体の速度が大きいほど前記観測誤差を大きく、前記速度が小さいほど前記観測誤差を小さくなるように前記観測誤差を設定し、算出された前記傾斜角と、設定された前記観測誤差と、前記角速度センサにて検出された前記角速度信号とに基づいて、カルマンフィルタ処理を実行して前記車体の傾斜角と前記角速度センサのオフセットとを更新する、ことを特徴とする。

本発明に係る作業車両の傾斜角検出装置によれば、車体の振動が発生しやすい不整地を走行する際にも、傾斜角を算出する精度を向上させることができる。なお、前述した以外の課題、構成、及び効果は、以下の実施形態の説明により明らかにされる。

本発明の実施形態に係るホイールローダの側面図である。 コントローラのハードウェア構成を模式的に示すブロック図である。 コントローラの機能構成を示すブロック図である。 コントローラによる演算に用いられる座標系を示す図である。 傾斜角の推定手順を示すフローチャートである。 カルマンフィルタ処理の手順を示すフローチャートである。 本実施形態の効果を説明するための図である。 変形例の効果を説明するための図である。

以下、本発明に係る作業車両の傾斜角検出装置が適用されるホイールローダを例に挙げ、その実施形態の図面を参照しつつ説明する。

図１は本発明の実施形態に係るホイールローダ１の側面図である。図１に示すように、ホイールローダ１は、一対のブーム２、バケット３、一対の前輪４等を有する前フレーム（車体）５と、運転室６、エンジン室７、一対の後輪８等を有する後フレーム（車体）９とで構成されている。エンジン室７にはエンジン２５が搭載されており、後フレーム９の後方にはカウンタウェイト１０が取り付けられている。

一対のブーム２は一対のブームシリンダ１１の駆動により上下方向に回動（俯仰動）し、バケット３はバケットシリンダ１２の駆動により上下方向に回動（クラウドまたはダンプ）する。バケットシリンダ１２とバケット３の間にはベルクランク１３を含むリンク機構が介設されており、このリンク機構を介してバケットシリンダ１２はバケット３を回動させる。なお、これら一対のブーム２、バケット３、一対のブームシリンダ１１、バケットシリンダ１２、ベルクランク１３等によって作業機１４が構成されている。

前フレーム５と後フレーム９とはセンタピン１５により互いに回動自在に連結され、ステアリングシリンダ（不図示）の伸縮により後フレーム９に対し前フレーム５が左右に屈折する。後フレーム９の前部に搭載された運転室６には、オペレータが座る運転席、ホイールローダ１の操舵角を制御するステアリングホイールと、ホイールローダ１を始動・停止させるキースイッチ、オペレータへの情報を提示する表示装置（いずれも図示せず）等が設置されている。

また、ホイールローダ１には、ホイールローダ１の移動速度に対応した速度（走行速度）を検出するための速度センサ３１、ホイールローダ１の前後方向に対応した加速度を検出するための加速度センサ３２、ホイールローダ１のピッチ方向に対応した角速度を検出するための角速度センサ３３が設けられている。これら各センサ３１，３２，３３からの各信号は、コントローラ５０に入力される。コントローラ５０は、入力された各信号に基づいて後述する車体の傾斜角θ（図４（ｂ）参照）の演算を行う。なお、加速度センサ３２、角速度センサ３３の代わりに、車体加速度及び車体角速度を検出するＩＭＵ（Ｉｎｅｒｔｉａｌ Ｍｅａｓｕｒｅｍｅｎｔ Ｕｎｉｔ／慣性計測装置）を使用しても良い。

図２はコントローラ５０のハードウェア構成を模式的に示すブロック図である。図２に示すように、コントローラ５０は、車体の動作全体を制御するための各種演算を行うＣＰＵ（Ｃｅｎｔｒａｌ Ｐｒｏｃｅｓｓｉｎｇ Ｕｎｉｔ）５０Ａと、ＣＰＵ５０Ａによる演算を実行するためのプログラムを格納するＲＯＭ（Ｒｅａｄ Ｏｎｌｙ Ｍｅｍｏｒｙ）５０Ｂの記憶装置と、ＣＰＵ５０Ａがプログラムを実行する際の作業領域となるＲＡＭ（Ｒａｎｄｏｍ Ａｃｃｅｓｓ Ｍｅｍｏｒｙ）５０Ｃと、外部の装置との間で各種の情報や信号の入出力を行う入出力インターフェース５０Ｄとを含むハードウェアから構成されている。

このようなハードウェア構成において、ＲＯＭ５０Ｂに格納されたプログラムがＲＡＭ５０Ｃに読み出され、ＣＰＵ５０Ａの制御に従って動作することによりプログラム（ソフトウェア）とハードウェアとが協働して、コントローラ５０の機能を実現する機能ブロックが構成される。

図３はコントローラ５０の機能構成を示すブロック図である。図３に示すように、コントローラ５０には、速度センサ３１から出力された速度信号Ｖ、加速度センサ３２から出力された加速度信号ａ_ｘ、角速度センサ３３から出力された角速度信号θドットが入力され、以下に述べる演算により、最終的に車体の傾斜角θが出力される。コントローラ５０は、並進加速度除去部５１、傾斜角演算部５２、観測誤差設定部５３、カルマンフィルタから成る状態推定部６０を含む。状態推定部６０は、推定値演算部６１、観測残差演算部６２、カルマンゲイン演算部６３、状態変数更新部６４、状態変数記憶部６５を含む。

以下、各部の機能の詳細について説明する。なお、以下の説明は図４に示す座標系に従うものとする。

推定値演算部６１は、角速度センサ３３からの角速度信号θドットを入力する。ここでは、時刻ｋでの角速度信号をθ_ｋドットと示し、時刻ｋでの状態変数ｘ_ｋを定義する。状態変数ｘ_ｋは傾斜角θと角速度のオフセットθ_ｂドットを含み、以下の数式１で表される。

推定値演算部６１は、入力ｕ_ｋ及び１回前の状態変数ｘ_ｋ−１に基づき数式２に示した状態方程式により、時刻ｋでの推定値ｘ_ｋハットを演算する。ここで、数式２において、入力ｕ_ｋはθ_ｋドットとする。なお、状態変数ｘ_ｋ−１は、状態変数記憶部６５から入力される。

Δｔをサンプリング間隔とすると、数式２におけるシステム行列Ａは数式３、入力行列Ｂは数式４のように示すことができる。

観測残差演算部６２は、傾斜角演算部５２から傾斜角θの観測値ｚ_ｋと推定値演算部６１から推定値ｘ_ｋハットとを入力し、数式５により観測値ｚ_ｋと推定値ｘ_ｋハットとの観測残差ｙ_ｋを演算する。

ここで、出力行列Ｈは数式６のように示される。

また、観測値Ｚ_ｋは数式７により演算できる。ここで、数式７において、ａ_xｋは加速度センサの加速度信号、Ｖ_ｋ−１，Ｖ_ｋは速度センサの速度信号、ｇは重力加速度である。

並進加速度除去部５１は、速度センサ３１からの速度信号Ｖと加速度センサ３２からの加速度信号ａ_xとを入力し、加速度信号ａ_xから、速度信号Ｖを微分して求めた加速度成分を除去する。傾斜角演算部５２は、並進加速度除去部５１によって加速度成分が除去された加速度信号に基づいて、上記した数式７により観測値Ｚ_ｋを演算する。

観測誤差設定部５３は、速度センサ３１から速度信号Ｖを入力し、時刻ｋにおける観測値Ｚ_ｋの観測誤差δθ_ｋを設定する。観測値Ｚ_ｋの観測誤差δθ_ｋは車両の移動時や停車時など場面によって異なることから、観測誤差設定部５３は、速度信号Ｖ（車速）を構成要素として規定した数式８により観測誤差δθ_ｋを演算する。この数式８は、以下に示すように、速度信号Ｖが小さくなると小さくなり、速度信号が大きくなると大きくなるように規定されている。ここで、数式８において、α、βは係数である。

なお、観測誤差δθ_ｋを上記した数式８も用いて演算する以外にも、例えば、速度信号Ｖ_ｋと観測値Ｚ_ｋの観測誤差δθ_ｋとの対応関係が予め規定されたテーブルをＲＯＭ５０Ｂ等に記憶しておき、観測誤差設定部５３は、このテーブルを参照して観測誤差δθ_ｋを決定しても良い。

カルマンゲイン演算部６３は、推定値演算部６１からの推定値ｘ_ｋハットと、観測誤差設定部５３からの観測誤差δθ_ｋとを入力し、システム誤差Ｑ、共分散行列Ｒ_ｋ等に基づいて、カルマンフィルタ６０のカルマンゲインＫ_ｋを演算する。観測値Ｚ_ｋの観測誤差δθ_ｋが標準偏差を表すものとして、共分散行列Ｒ_ｋを数式９のように規定する。

なお、共分散行列Ｒ_ｋ等からカルマンゲインＫ_ｋを算出する方法は、公知であるため、ここでの説明は省略する。

状態変数更新部６４は、観測残差演算部６２から観測残差ｙ_ｋを入力し、推定値演算部６１から推定値ｘ_ｋハットを入力し、カルマンゲイン演算部６３からカルマンゲインＫ_ｋを入力する。状態変数更新部６４は、推定値ｘ_ｋハット、ゲインＫ_ｋ、及び観測残差ｙ_ｋに基づいて、数式１０を演算することによって、状態変数ｘ_ｋを更新する。即ち、状態変数更新部６４は、ホイールローダ１の傾斜角θと角速度センサ３３のオフセットを更新する。

そして、状態変数更新部６４は、更新した状態変数ｘ_ｋの傾斜角θを出力する。また、更新した状態変数ｘ_ｋは状態変数記憶部６５に記憶され、次の離散時間後の推定値ｘ_ｋ＋１ハットに使用される。

次に、コントローラ５０による傾斜角θの演算処理の手順について説明する。図５は傾斜角θの推定手順を示すフローチャートである。図５に示すように、コントローラ５０は、速度センサ３１から速度信号Ｖを取得し（Ｓ１０）、加速度センサ３２から加速度信号ａ_xを取得し（Ｓ１２）、角速度センサ３３から角速度信号θドットを取得する（Ｓ１３）。コントローラ５０は、速度信号Ｖを微分して加速度成分を除去し（Ｓ４）、加速度信号ａ_xから速度信号Ｖを微分して得られた加速度成分が除去された加速度信号に基づき傾斜角を演算する（Ｓ５）。次いで、コントローラ５０は観測誤差δθ_ｋを設定し（Ｓ６）、カルマンフィルタ処理を実行し（Ｓ７）、傾斜角θを出力する（Ｓ８）。処理が継続されるのであれば（Ｓ９／Ｙｅｓ）、Ｓ１に戻る。処理が継続されないのであれば（Ｓ９／Ｎｏ）、処理は終了する。

次に、Ｓ７において実行されるカルマンフィルタ処理の詳細について説明する。図６はカルマンフィルタ処理の手順を示すフローチャートである。図６に示すように、推定値演算部６１は、前回の状態変数ｘ_ｋを取得し（Ｓ１１）、推定値ｘ_ｋハットを演算する（Ｓ１２）次いで、観測残差演算部６２は、観測残差ｙ_ｋを演算する（Ｓ１３）。次いで、カルマンゲイン演算部６３は、カルマンゲインＫ_ｋを演算する（Ｓ１４）。次いで、状態変数更新部６４は、状態変数を更新し（Ｓ１５）、状態変数記憶部６５は、更新された状態変数を記憶する（Ｓ１６）。

（効果）
本実施形態によれば、カルマンゲインＫ_ｋが小さければ、推定値ｘ_ｋハットの重みが大きく、カルマンゲインＫ_ｋが大きければ、観測残差ｙ_ｋの重みが大きいということになる。カルマンゲインＫ_ｋの大きさは、観測誤差設定部５３による観測誤差δθ_ｋの設定値に左右され、観測誤差δθ_ｋの設定値が小さければカルマンゲインＫ_ｋが大きくなり、観測誤差δθ_ｋの設定値が大きければカルマンゲインＫ_ｋが小さくなる。本実施形態では、速度信号（車速）Ｖが低速度領域にある場合に比べて速度信号（車速）Ｖが高速度領域にある場合の方が、観測誤差δθ_ｋが大きい値に設定される。すなわち、速度信号（車速）Ｖが高速度領域にあると推定値ｘ_ｋハットの重みが大きくなる。

加速度センサ３２による傾斜角算出はホイールローダ１が停車しているときは検出精度が高いが、走行中は並進運動や振動の影響を受け、検出精度が低下する（図７ｂの角速度角度変換）。一方、角速度センサ３３はホイールローダ１が走行中においても並進運動の影響を受けずに検出できるが、センサ出力値にオフセットを含んでいるため、傾斜角算出のために積分すると誤差が累積する（図７ｂの角速度積分）。本実施形態では、速度信号（車速）Ｖを基に観測誤差を設定し、観測誤差を基に傾斜角θと角速度センサ３３のオフセットとを更新するので、速度信号（車速）によって変化する加速度センサ３２の検出精度の変化を反映させた傾斜角θと角速度センサ３３のオフセットを得ることができる。

これについて、図７を使用して説明を行う。観測誤差を低い値の一定値とした場合（図７ｃのＲ固定１）、車速が低い間（図７ａの車速の１〜２秒）は、良好に傾斜角度推定ができている（図７ｄのＲ固定１の１〜２秒）が、車速が上昇すると（図７ａの２〜１０秒）、加速度センサ３２による傾斜角算出の精度が低下（図７ｂの２〜１０秒）し、傾斜角θも影響を受けて精度が悪化する（図７ｄの角度推定（Ｒ固定１）の２〜１０秒）。一方、観測誤差を高い値の一定値とした場合（図７ｃのＲ固定２）、角速度センサ３３が持つオフセットの推定精度が悪化し、傾斜角推定の誤差が時間の経過と共に累積している（図７ｄの角度推定（Ｒ固定２）１〜２秒）。観測誤差を車速に応じて変化させると（図７ｃのＲ可変）、車速によって変化する加速度センサ３２の精度変化（図７ｂの加速度角度変換）を傾斜角θと角速度センサ３３のオフセットに反映させることができ、良好に傾斜角推定ができていることがわかる（図７ｄの角度推定（Ｒ可変））。従って、速度信号（車速）を基に観測誤差を設定することにより、車体の振動が発生し易い不整地を走行する際においても、ホイールローダ１の傾斜角θを高精度で検出することができる（図７参照）。

以上より、本実施形態で得られた傾斜角θに基づいて、例えばトランスミッションの変速スケジュールを変更する構成にすると、登坂路走行の場面において最適な変速を実施することができ、変速頻度の低減が可能となる。

（変形例）
本実施形態において、観測誤差δθ_ｋの設定値に対してフィルタ処理を行って、観測誤差δθ_ｋの設定値の立ち上がり時は速く、立下り時は遅くなるようにすることもできる（図８ｃのＲ可変）。これにより、車両移動時から停車直後においても、観測誤差δθ_ｋが直ちに小さくなるのを防ぐことができる。車両の移動時から停車直後は停止時に伴う車体の振動があり、加速度センサ３２の検出精度が低い状態が継続している。すなわち、振動中は観測誤差が大きい状態である。変形例のような構成とすることで、観測誤差を適切に設定することが可能になり、傾斜角推定の精度の悪化を防ぐことができる（図８参照）。

なお、上記した実施形態は、本発明の説明のための例示であり、本発明の範囲をそれらの実施形態にのみ限定する趣旨ではない。当業者は、本発明の要旨を逸脱することなしに、他の様々な態様で本発明を実施することができる。

１ ホイールローダ（作業車両）
５ 前フレーム（車体）
９ 後フレーム（車体）
３１ 速度センサ
３２ 加速度センサ
３３ 角速度センサ
５０ コントローラ

Claims (2)

1. 車体に搭載され、前記車体の前後方向の加速度を検出すると共に検出された前記加速度を加速度信号として出力する加速度センサと、
   前記車体に搭載され、前記車体のピッチ方向の角速度を検出すると共に検出された前記角速度を角速度信号として出力する角速度センサと、
   前記車体に搭載され、前記車体の速度を検出すると共に検出された前記速度を速度信号として出力する速度センサと、
   前記加速度センサ、前記角速度センサ、及び前記速度センサからの各信号を入力し、前記車体のピッチ方向における傾斜角を演算するコントローラと、を備えた作業車両の傾斜角検出装置において、
   前記コントローラは、
   前記速度センサにて検出された前記速度信号を微分して加速度成分を求め、当該加速度成分を前記加速度センサにて検出された前記加速度信号から除去して前記傾斜角を演算し、
   前記速度センサにて検出された前記速度信号に応じて、前記車体の速度が大きいほど観測誤差を大きく、前記速度が小さいほど前記観測誤差を小さくなるように前記観測誤差を設定し、
   演算された前記傾斜角と、設定された前記観測誤差と、前記角速度センサにて検出された前記角速度信号とに基づいて、カルマンフィルタ処理を実行して前記車体の傾斜角と前記角速度センサのオフセットとを更新する、ことを特徴とする作業車両の傾斜角検出装置。
2. 請求項１に記載の作業車両の傾斜角検出装置において、
   前記コントローラは、
   前記観測誤差に対してフィルタ処理を実行して、
   前記観測誤差の上昇時には速く、前記観測誤差の下降時には遅くして異ならせるように前記観測誤差を設定することを特徴とする作業車両の傾斜角検出装置。
   

Images