JP4373315B2 - 作業車輌における傾斜自動制御装置 - Google Patents

Description

本発明は、走行機体または作業部の刻々の傾斜角を傾斜センサと角速度センサで検知して作業部の左右傾斜角を自動制御する作業車輌における傾斜自動制御装置に関する。

トラクタ等の走行機体にロータリ耕耘機等の作業部を連結して整地、耕耘等を行う作業車輌では、走行機体の傾きとは無関係に、作業部を水平に維持したり、地面の傾きと一致した左右傾斜角を作業部に設定したりすることが望まれる。このような要求に応えて、走行機体に傾斜センサと角速度センサを配置して走行機体の刻々の左右傾斜角を演算し、当該傾斜角に基づいて作業部の左右傾斜角を自動調整するようにした作業車輌が実用化されている。

ここでは、走行機体と作業部とが左右傾斜角度を変更可能な形式で連結され、連結部に油圧シリンダ等を配置して作業部の左右傾斜角を任意に設定できるようにし、傾斜センサと角速度センサの出力に基づいて演算された刻々の傾斜角が油圧シリンダ等の駆動にフィードバックされる。

このような作業車輌における傾斜自動制御装置では、絶対傾斜角度を検出できる傾斜センサの出力と応答性の高い角速度センサの出力とを組み合わせて傾斜角が演算される。角速度センサは走行機体が傾斜する際の角速度に応じた出力を出力するから、所定時間積分すれば当該時間における走行機体の傾斜角度変化量が演算される。

ここで、例えば、０〜５Ｖの範囲で±最大検出角速度の出力を行うような角速度センサを使用する場合、角速度０に対応する基準値（例えば２．５Ｖ）を定めて、（センサ出力−基準値）で右回転／左回転の角速度を判別する必要がある。

しかし、この基準値は、角速度センサの温度変化、計時変化、振動や運転状態等によって変化する可能性がある。基準値が変化すると、左回転の角速度を右回転の角速度と誤認したり、演算された角速度の大きさが変化したりする。そして、このような角速度の誤差が積分によって累積されると実際の傾斜角とはかけ離れた傾斜角が演算されてしまう。

そこで、所定のタイミングで角速度センサの出力に基づく傾斜角を傾斜センサの出力に基づく傾斜角に置き換えたり、角速度センサの基準値を随時更新したりして、制御に用いられる傾斜角の誤差を小さくする技術が提案されている。

特許文献１には、傾き角度を検知する傾斜センサとローリングの角速度を検知する角速度センサとを走行機体に取り付けて作業部の左右傾斜角度を計測するようにした作業車輌が示される。ここでは、角速度非検出時には傾斜センサの出力に基づいて傾斜角を演算するが、角速度検出時には角速度センサの出力を積分して傾斜角を演算している。また、最後の角速度検出から一定時間が経過した後に、傾斜センサの出力に基づく傾斜角の演算に復帰させている。

特許文献２には、傾斜センサの出力状態と角速度センサの出力状態との組み合わせから走行機体の走行状態をきめ細かく判別して傾斜角度の演算方法を切り替えるようにした傾斜自動制御装置が示される。また、角速度センサの出力に基づく演算から傾斜センサの出力に基づく演算へ切り替える際に、演算された傾斜角の格差を次第に減少させて傾斜角の急変を回避している。

特許文献３には、角速度センサの出力の移動平均値で角速度演算の基準値を刻々と更新する傾斜自動制御装置が示される。ここでは、角速度センサの安定出力における上のピーク値の平均に対して不感帯の上限を設定し、下のピーク値の平均に対して不感帯の下限を設定し、両者の間の不感帯内に角速度センサの出力があれば傾斜センサの出力に基づく傾斜角度制御を行う一方、不感帯を超えたときに角速度センサの出力に基づく傾斜角度制御へ移行させている。

特許文献４には、角速度センサの出力に基づく傾斜角度の演算値に傾斜センサの出力をフィードバックして実際の傾斜角度に演算値を近付ける傾斜自動制御装置が示される。ここでは、角速度センサの出力値の平均値と基準値を重み付け加算平均して基準値を刻々更新している。

特開平８−８９０１１号公報 特開平１１−２８９８０８号公報 特開２０００−３５４４０２号公報 特開２００２−２５３００５号公報

特許文献１、２に示される傾斜自動制御装置では、角速度センサの出力に基づく傾斜角と傾斜センサの出力に基づく傾斜角を置き換えるにとどまり、角速度センサの出力から演算された角速度の誤差は基準値次第である。従って、基準値がずれると角速度の誤差が累積した傾斜角が演算される可能性がある。

特許文献３に示される傾斜自動制御装置では、角速度センサの出力の移動平均を求めて基準値を刻々と更新するから、角速度センサの基準値がずれた際には速やかに正しい基準値に誘導されて角速度の誤差は維持される。しかし、角速度センサの出力のピークを検出するために高速でサンプリングを行う必要があり、過去の角速度センサの出力を多数記憶しておく必要もある。従って、演算負荷や記憶容量が膨大になって演算装置の大型化と消費電力増を招く。

特許文献４に示される傾斜自動制御装置では、角速度センサの出力に基づく傾斜角度の演算値に傾斜センサの出力をフィードバックして演算値を刻々補正するから、いつまでも演算値が実際の傾斜角に追従する。しかし、加算とローパスフィルタ演算を正確に行うには、ｋＨｚレベルの高速サンプリングが必要であり、演算負荷や記憶容量が膨大になって演算装置の大型化と消費電力増を招く。

本発明は、角速度センサの基準値を刻々と補正する際の演算負荷や必要なメモリ容量が小さく済み、しかも、角速度センサ出力の読み込み頻度が少なくても補正の精度が必要十分に確保され、従って、低速でメモリ容量の少ないマイコン素子等を採用して小型安価低消費電力を実現可能な作業車輌における傾斜自動制御装置を提供することを目的としている。

請求項１の作業車輌における傾斜自動制御装置は、走行機体（１０）に作業部（３０）を左右傾斜自在に支持し、前記走行機体（１０）又は作業部（３０）の左右傾斜角を検出して、前記走行機体（１０）の左右傾斜に拘らず、前記作業部（３０）が予め設定された姿勢に保持されるように制御する傾斜自動制御装置（４０）を備えた、作業車輌において、前記左右傾斜角を検出する手段として傾斜センサ（１３）と角速度センサ（１４）とを備え、前記角速度センサ（１４）の出力の右回りと左回りを判別する原点値としての基準値を、「今までの基準値＋（角速度センサの検出値−今までの基準値）／２^ｎ」により算出し、上記指数ｎを、傾斜自動制御の作動中に変更可能な整数としたものである。

請求項２の作業車輌における傾斜自動制御装置は、請求項１の発明における前記指数ｎを、前記（角速度センサの検出値−今までの基準値）の差分が所定の不感帯の範囲内である状態が続くと次第に増加させ、上記差分が前記所定の不感帯の範囲を越える状態が続くと次第に減少させるように変更するものである。

請求項３の作業車輌における傾斜自動制御装置は、請求項１の発明において、（前記角速度センサの検出値−前記基準値）の差分データが、不感帯を越えることに基づき、該差分データの積分値による傾斜角により傾斜自動制御し、前記差分データが前記不感帯を越えないことに基づき、前記傾斜センサによる傾斜角により傾斜自動制御し、
かつ前記不感帯が、車速が速くなるほど大きくなるように変化するものである。

なお、上述した括弧内の符号は図面を参照するために示すものであって、本発明を何ら限定するものではない。

請求項１に係る本発明によると、移動平均値による基準値の設定のように、多くのデータを蓄積する必要がなく、バッファやメモリを多用することなく、演算を高速化できると共に、安価なマイクロコンピュータを用いることが可能となる。

請求項２に係る本発明によると、（角速度センサの検出値−今までの基準値）の差分が小さい場合は、指数を大きくして基準値の安定化を図り、上記差分が大きい場合、指数を小さくして基準値を角速度の検出値に速く近づけるようにして、角速度センサによる傾斜自動制御を高い応答性能でかつ安定して行うことができる。

請求項３に係る本発明によると、上記基準値に基づく角速度センサの差分データの不感帯が、車速により変化するので、車速が高い場合、不感帯を広くして安定した制御を行うことができると共に、車速が低い場合は不感帯を狭くして、高い応答性能により正確に制御することができる。

傾斜センサ１３と角速度センサ１４の出力に基づいてトラクタ１０の傾斜角を刻々と演算し、演算結果に基づいてリフトロッドシリンダ２５を駆動することにより、作業機３０の左右傾斜角を自動調整する。

図１は実施例１の傾斜自動制御装置を搭載したトラクタの側面図、図２は傾斜自動制御装置の接続状態の説明図、図３は実施例１の傾斜自動制御装置における制御全体のフローチャート、図４は初期設定のフローチャート、図５は、平滑初期設定と基準初期設定のフローチャート、図６は接続判断のフローチャート、図７は接続判断におけるタイマ割込処理のフローチャート、図８は平滑処理のフローチャート、図９は演算不感帯設定のフローチャート、図１０は切替制御のフローチャート、図１１は切替制御における部分的なフローチャート、図１２は角度演算のフローチャート、図１３は傾斜制御のフローチャート、図１４は基準データ処理のフローチャート、図１５は基準データ処理における部分的なフローチャート、図１６はヒステリシス設定の線図、図１７は傾斜制御における不感帯設定の線図、図１８は基準データ処理における不感帯設定の線図である。

図１に示すように、走行機体（トラクタ１０）の後部には昇降リンク機構２０が接続され、昇降リンク機構２０の後部には、回転刃３１を有する作業機（ロータリ耕耘ユニット）３０が連結されている。昇降リンク機構２０は、１個のアッパリンク２１と左右一対のロアリンク２２を備える三点式のもので、作業機３０以外にも種々の作業機を選択して装着可能である。

昇降リンク機構２０のロアリンク２２は、リフトロッド２４を介してリフトアーム２３に吊り下げ支持され、リフトアーム２３は、その根元側に配置したリフトシリンダ２７の伸長／収縮動作によって上下に回動される。したがって、リフトシリンダ２７を油圧駆動して作業機３０を上昇／下降動作させたり、任意の耕耘深さを設定したりすることができる。

また、トラクタ１０の進行方向左側のリフトロッド２４は、リフトロッドシリンダ２５を介してリフトアーム２３に吊り下げ支持されており、リフトロッドシリンダ２５を油圧駆動して左右のリフトロッド２４の長さを異ならせることによって、作業機３０の左右傾き角度を任意に設定可能である。

トラクタ１０の運転席１５に隣接させて操作パネル１６が配置され、操作パネル１６の下方に傾斜自動制御装置４０が配置されている。操作パネル１６には、リフトアーム２３を手動で昇降動作させるポジションレバー１１と、作業に関する各種設定や切替えを行うためのスイッチやボリュームが多数配置されている。

図２に示すように、傾斜自動制御装置４０は、マイクロコンピュータ回路（ＭＰＵ、ＲＡＭ、ＲＯＭ、ＩＯ回路等を含む）を用いて構成される電子回路であって、ポジションレバー１１による設定高さを目標として作業機３０を自動的に昇降制御するポジション制御、図示しない耕深設定ボリュームの設定耕深を目標として作業機３０を自動的に昇降制御する耕深自動制御、傾斜設定ボリューム４３の設定傾斜角度を目標として作業機３０を自動的に傾斜制御する傾斜自動制御等の制御を実行する。

傾斜自動制御装置４０には、自動切替ボリューム４１、作業機切替ボリューム４２、傾き設定ボリューム４３、操作スイッチ４４、エンジン回転センサ４５、車軸回転センサ４６、リモコンスイッチ４７、リフトアームセンサ２６、リフトロッドセンサ２８、ポジションレバーセンサ２９、傾斜センサ１３、角速度センサ１４、リレーユニット５７、ソレノイドバルブ５１、５２等が接続されている。

自動切替ボリューム４１は、操作パネル１６に配置された自動切替ツマミに連動させた可変抵抗器であって、傾斜自動制御のＯＮ／ＯＦＦ等に対応させた複数の固定ポジションを判別させる。自動切替スイッチでＯＦＦが設定されると作業機３０の左右傾斜の自動調整は実行されないが、ＯＮが設定されるとスピーカー５４を通じて短い切替音が出力されて自動調整が開始され、自動調整状態を通知するランプ５５が点灯される。

作業機切替ボリューム４２は、操作パネル１６に配置された作業機切替ツマミに連動させた可変抵抗器であって、トラクタ１０に連結する作業機の種類に対応させた複数の固定ポジションを判別させる。

傾き設定ボリューム４３は、操作パネル１６に配置された傾斜角度設定ツマミに連動させた可変抵抗器であって、作業者が傾斜角度設定ツマミを回転させて設定した作業機３０の傾斜角度に対応するアナログ電圧を出力する。操作スイッチ群４４は、傾斜角度調整の作動感度を設定する等、傾斜自動制御の付属機能を設定するものである。

エンジン回転センサ４５は、トラクタ１０のエンジン出力軸に取り付けられたパルスエンコーダであって、エンジン回転速度に応じた周波数のパルス電圧を出力する。車軸回転センサ４６は、トラクタ１０の後輪軸に取り付けられたパルスエンコーダであって、トラクタ１０の対地速度（走行速度）に応じた周波数のパルス電圧を出力する。リモコンスイッチ４７は、リフトロッドシリンダ２５を運転席１５の外から手動で動作させるスイッチである。

リフトアームセンサ２６は、図１にも示すように、リフトアーム２３の根元に配置された可変抵抗器であって、リフトアーム２３の回動角に応じた電圧信号を出力する。リフトロッドセンサ２８は、リフトロッドシリンダ２５に隣接配置された測長センサであって、リフトロッドシリンダ２５の引出し長さに応じた電圧信号を出力する。ポジションレバーセンサ２９は、ポジションレバー１１の根元に配置された可変抵抗器であって、ポジションレバー１１の回動角に対応した電圧信号を出力する。

傾斜センサ１３は、トラクタ１０の後輪軸の近傍に配置された重力式の傾斜角度センサであって、車体のローリング傾斜角度に対応した電圧信号を出力する。角速度センサ１４は、傾斜自動制御装置４０の箱内に格納された角速度センサであって、車体のローリング角速度に応じた電圧信号を出力する。

ステアリング切れ角検出スイッチ５６は、トラクタ１０のステアリング１２が回転されて前輪切れ角が左右一定角度以上になるとＯＮするように配置されたリミットスイッチである。リレーユニット５７は、ステアリング１２が操作されてステアリング切れ角検出スイッチ５６のいずれかがＯＮしている間、一定の電圧信号を出力する。

ソレノイドバルブ５１は、リフトロッドシリンダ２５の伸長方向にオイルを流し、ソレノイドバルブ５２は、リフトロッドシリンダ２５の短縮方向にオイルを流す。プライオリティバルブ５３は、ソレノイドバルブ５１、５２へのオイル供給を許可（または停止）する。

以上のように構成された実施例１の傾斜自動制御装置４０では、自動切替ボリューム４１がＯＮに設定されて左右傾斜角度の自動調整が選択され、作業機切替ボリューム４２はロータリ式の作業機３０に設定されている。このとき、傾斜自動制御装置４０は、傾斜センサ１３および角速度センサ１４の出力に基づいてトラクタ１０の刻々の傾斜角を演算し、リフトロッドセンサ２８の出力を参照しつつ傾き設定ボリューム４３に設定された傾斜角度を目標にして刻々の制御量を演算する。そして、傾斜自動制御装置４０は、この制御量に基づいてソレノイドバルブ５１、５２を作動させることにより、リフトロッドシリンダ２５を伸長／短縮して作業機３０の傾斜角を制御する。

図３に示すように、傾斜自動制御装置４０は、ステップ２１０〜３７０で構成されるメインフロー２００に従って傾斜角自動制御を実行する。

ステップ２１０では、図４、図５に示すように各種演算処理の初期設定を行う。続くステップ２２０では、図６、図７に示すように、角速度センサの接続判断を行う。続くステップ２５０では、図８に示すように、傾斜センサ１３の出力を平均して基準データを作成する平滑処理を行う。続くステップ２８０では、図９に示すように、角速度センサ１４の出力データを積算する際の判断に使用される演算不感帯が設定される。

続くステップ３１０では、図１０、図１１に示すように、トラクタ１０の状況に応じて角速度センサ１４に基づく制御と傾斜センサ１３に基づく制御とを選択する切替制御を行う。続くステップ３６０では、図１２に示すように、角速度センサ１４の出力を積算する角度演算を行う。続くステップ３７０では、図１３に示すように、演算された傾斜角度に基づいて出力を制御する傾斜制御を行う。

図４に示すように、初期設定フロー（２１０）では、ステップ２１１で初期設定が必要か否かを判定し、自動切替スイッチがＯＮされる等して必要と判断されればステップ２１２〜２１６に示されるように、各種演算処理の初期設定を行う。ステップ２１２では演算開始フラグと積算フラグがリセットされる。

図４のステップ２１３では、図５に示すように、平滑初期設定フロー（２４０）が実行される。すなわち、ステップ２４１では、平滑データとして傾斜センサ１３の基準値が採用され、ステップ２４２では、平滑演算データの設定が開始され、ステップ２４４では平滑フラグがクリアされる。

図４のステップ２１４では、図５に示すように、基準初期設定フロー（３９０）が実行される。すなわち、ステップ３９１では基準シフトの設定が開始され、ステップ３９２ではベースの設定が開始され、ステップ３９３では４つの制御カウンタがクリアされる。

図４に戻って、ステップ２１５では、更新確認タイマ、タイムアウト、開始遅延タイマの各設定が開始される。ステップ２１６では、演算不感帯の設定が開始される。

図６に示すように、接続判断フロー（２２０）では、ステップ２２１で角速度センサ１４の出力値が正常範囲か否かが判定される。正常範囲は、図１６に示すように、角速度センサの出力０〜５Ｖの範囲中０．１〜４．９Ｖの範囲に定められ、正常範囲の上下０．２Ｖの範囲にヒステリシス範囲を設けている。ここで、正常範囲外と判断されればステップ２２４へ進んで範囲外フラグがＯＮされ、さらに、ステップ２２５へ進んでセンサ外れタイマが限界時間を越えたか否かが判定される。センサ外れタイマは、図７に示すように、タイマ割込み処理として範囲外フラグのＯＮ経過時間を積算しており、ＯＮ状態が限界時間に達するとステップ２２６へ進んでセンサ外れフラグがＯＮされる。

一方、角速度センサ１４の出力値が正常範囲内であれば、ステップ２２２へ進んでヒステリシス範囲か否かが判定される。そして、ヒステリシス範囲でなければ、ステップ２２７へ進んで範囲外フラグがＯＦＦされ、さらに、ステップ２２８へ進んでセンサ接続タイマが限界時間を越えたか否かが判定される。センサ接続タイマは、図７に示すように、タイマ割込み処理として範囲外フラグのＯＦＦ経過時間を積算しており、ＯＦＦ状態が限界時間を越えていればステップ２２９へ進んでセンサ外れフラグがＯＮか否かが判定され、センサ外れフラグがＯＮであれば、ステップ２３１へ進んでセンサ外れフラグがＯＦＦされる。続いて、ステップ２３２ではリセット処理が実行され、ステップ２３３ではベース初期設定が実行される。

また、ステップ２２２でヒステリシス範囲の場合、ステップ２２３へ進んで範囲外フラグのＯＮ／ＯＦＦが判定され、ＯＦＦの場合はステップ２２７へ進むが、ＯＮの場合（正常出力範囲外からヒステリシス範囲へ入った場合に該当）はステップ２２４へ進んで範囲外に対応する処理を継続する。そして、ステップ２２５でセンサ外れタイマ未了、ステップ２２８でセンサ接続タイマ未了、ステップ２２９でセンサ外れフラグＯＦＦのそれぞれの場合は、現状の各フラグ状態が継続される。

このようにして、角速度センサ１４の出力を判定し、角速度センサ１４の接続と機能の正常が確認されるとセンサ外れフラグはＯＦＦとなる一方、確認されないとセンサ外れフラグはＯＮとなる。

図８に示すように、平滑制御フロー（２５０）では、傾斜センサ１３の出力の平均値を演算するためのサンプリング周期を設定する。ステップ２５１では、平滑フラグが平滑初期設定フロー（２４０）でクリアされたまま、すなわち初期状態のままか否かが判定される。そして、初期状態であればステップ２５３へ進んで通常の２００ｍｓｅｃのサンプリング周期が設定される。

一方、初期状態でなければ、ステップ２５２へ進んで作業開始フラグがセットからリセットへ反転したか否かが確認される。作業開始フラグは、図１０、図１１に示す切替制御フロー（３１０）で説明するように、作業機３０が下降されて所定時間が経過するまでセットされ続け、経過後にリセットされるので、作業開始フラグがセットからリセットへ反転すると、作業開始と判定してステップ２５４へ進み、短い１００ｍｓｅｃのサンプリング周期を設定する。作業開始に該当しなければ通常の２００ｍｓｅｃのサンプリング周期が設定される。

このようにして、作業機３０を下降させて傾斜制御を再開する際には、傾斜センサ１３の出力値も変動し易いため、サンプリング周期を短くして少しでも正確な傾斜角を演算しようとしている。

傾斜センサ１３の出力値の平滑処理は、図５の平滑初期設定（２４０）で設定された条件に基づいて他の制御に割り込んで定期的に実行されるタイマ割り込み処理フロー（２６０）で実行される。ステップ２６１でサンプリング時刻と判定されると、ステップ２６２へ進んで傾斜センサ１３出力値が加算される。続くステップ２６３で所定回数（５回）の加算が確認されると、ステップ２６４へ進んで平滑フラグがセットされる。平滑フラグは、傾斜角の自動制御において、傾斜センサ１３の出力の平滑データが利用可能であることを示している。

このようにして、作業開始時以外は、ほぼ２００ｍｓｅｃ間隔で、最新の５個のサンプリング出力値が加算平均される。

図９に示すように、不感帯設定フロー（２８０）では、トラクタ１０の対地速度に応じた不感帯を設定する。不感帯は、角速度センサ１４の小さな出力変化を傾斜角の自動制御に反映させないために設定される出力範囲である。ステップ２８０では、車軸回転センサ４６の出力が参照され、対地速度２．５ｋｍ／ｈ未満か否かが判定される。そして、対地速度２．５ｋｍ／ｈ以下に該当すれば、線図２９０に示すように、ステップ２８３へ進んで不感帯を１．５度に設定するが、該当しなければ、ステップ２８２へ進んで対地速度６．０ｋｍ／ｈを越えるか否かが判定される。そして、対地速度が６．０ｋｍ／ｈを越えていればステップ２８４へ進んで不感帯を２．２５度に設定するが、対地速度２．５〜６．０ｋｍ／ｈの範囲であれば、線図２９０に示すように、１．５度から２．２５度の範囲の連続した値を設定する。

このようにして、トラクタ１０の対地速度が増すと同じ耕作面でも角速度センサ１４の出力値の変化が大きくなるが、対地速度に対応させて不感帯を大きく設定することによって、制御出力の回数が異常に増えたり、制御の反転を繰り返して不安定を引き起こしたりする事態を回避している。

図１０に示すように、切替制御フロー（３１０）では、角速度センサ１４の出力が不感帯を越えた場合にだけ角速度センサ１４の出力に基づく制御を適用している。ステップ３１１ではリレーボックス５７の出力が参照され、ステアリング１２が所定角度を越えて操作されたか否かが判定される。そして、操作されていなければステップ３１２へ進んでポジションレバーセンサ２９の出力が参照され、ポジションレバー１１が上昇状態に操作されているか否かが判定される。そして、上昇状態に操作されていれば、ステップ３１３へ進んでリフトアームセンサ２６の出力が参照され、リフトアーム２３が上昇状態か否かが判定される。

そして、ステアリング１２が所定角度を越えて操作されたか、ポジションレバーセンサ２９またはリフトアームセンサ２６の出力によって作業機３０が上昇状態と判定される場合には、ステップ１５４へ進んで開始遅延タイマのカウントを開始させるが、いずれにも該当しなければ、ステップ３２０へ進んで開始遅延タイマが限界時間ＴＡを越えたか否かを判定する。

開始遅延タイマは、ステップ３２１でカウントアップされてステップ３１１〜３１３をＮＯで通過し続ける時間（最後にステップ１５４を実行してからの経過時間、言い換えればステアリング終了または作業機３０の下降完了からの経過時間）を計測しており、開始遅延タイマが限界時間ＴＡを越えるとステップ３２０からステップ３３１へ移行して、作業開始フラグをリセットするが、限界時間ＴＡに達するまでは、ステップ３２１で開始遅延タイマのカウントを継続し、ステップ３２２で積算フラグをリセットし、ステップ３２３で傾斜基準データに平滑データを取り込み、ステップ３２４で角速度センサ１４の出力の積算データをクリアし、ステップ３２５で作業開始フラグをセットし、ステップ３２６でタイムアウトのカウントダウンを開始させ続ける。

ステップ３３１に続くステップ３３２ではタイムアウトをカウントダウンし、ステップ３３３では、角速度センサ１４の出力値から基準データを差し引いた絶対値から、不感帯の角速度を差し引いた差分データが演算される。続くステップ３３４では、差分データが判定され、差分データが０より大、すなわち角速度センサ１４の出力が不感帯を越える場合にはステップ３３５へ進んでタイムアウトに達したか否かが判定される。ステップ３３５でタイムアウト未了の場合はステップ３３６へ進んで演算開始フラグがセットされ、ステップ３３７で更新確認タイマがクリアされ、ステップ３３８で積算フラグがセットされる。

なお、このときの不感帯は、先に図９の不感帯設定フロー２８０で対地速度に応じて１．５〜２．２５度の範囲に定めたものであって、図１７に示されるように、実際には境界での不安定を回避すべく、ヒステリシスを加味して不感帯の内外を判定される。

ここで、タイムアウトは角速度センサ１４の出力が長時間に渡って不感帯を越えている場合にその積算誤差を除去するために設定されるタイマである。タイムアウトは、作業機３０の下降後限界時間ＴＡが経過する直前にステップ３２６を通過した時点、または角速度センサ１４の出力が最後に不感帯にあったステップ３５６でカウントダウン開始されている。ステップ３３５でタイムアウト完了の場合は、ステップ３４１へ進んで積算フラグがリセットされ、ステップ３４２で傾斜基準データに平滑データが取り込まれ、ステップ３４３で積算データがクリアされる。

一方、ステップ３３４で差分データが０以下、すなわち角速度センサ１４の出力が不感帯の内側と判定された場合、図１１の部分的なフロー３３０へ移行する。図１１に示すように、ステップ３５１では演算開始フラグがリセットされ、ステップ３５２では更新確認タイマのカウント完了か否かが確認される。

ここで、更新確認タイマは、角速度センサ１４の出力が最後に不感帯外から不感帯内へ変化して以降の経過時間を計測し、限界時間ＴＢの経過を検知する。そして、角速度センサ１４の出力が不感帯内へ変化しても角速度センサ１４の出力に基づく傾斜角の演算を限界時間ＴＢだけ継続させ、その後に傾斜センサ１３の平滑データで傾斜角を演算開始させることにより、傾斜センサ１３の出力が傾斜に追従する時間を確保して、安定した切替えが実現される。

ステップ３５２で更新確認タイマ完了と判定された場合、ステップ３５５へ進んで積算フラグがリセットされ、続くステップ３５６でタイムアウトが設定され、続くステップ３５７で傾斜基準データとして平滑データが取り込まれ、続くステップ３５８で積算データがクリアされる。一方、ステップ３５２で更新確認タイマ未了と判定された場合、ステップ３５３へ進んで更新確認タイマのカウントが継続され、ステップ３５４で積算フラグがセットされる。

図１２に示すように、角度演算フロー（３６０）では、傾斜基準データを初期値にして角速度センサ１４の出力を積分することにより、トラクタ１０の刻々の傾斜角を演算する。

ステップ３６１では、一定周期でサンプリングされる角速度センサ１４の差分データが加算されて毎秒積算回数で除算された後に、傾斜データと角速度データの定数比が乗算され、これに傾斜センサ１３の出力の平滑データが加算される。

ここで、角速度センサ１４の差分データを所定時間分積算して毎秒積算回数で除算すると、当該所定時間の傾斜角変化（積分値）が演算される。例えば、２００ｍｓｅｃごとにサンプリングされる角速度センサ１４の出力値から基準データを差し引いた差分データ１度／秒が５回分加算されて毎秒積算回数５で除算されると、１秒間に追加された傾斜角１度が演算される。

また、傾斜データと角速度データの定数比とは、出力レベルの異なる角速度センサの出力を傾斜センサの出力に適合させて加算減算を可能にするための定数であって、例えば、傾斜センサ１３が１０度の傾斜に対して１Ｖを出力し、角速度センサ１４が同じ１０度の傾斜に対して２Ｖを出力するとすれば、角速度センサの積分値を傾斜センサ１３のレベルに合わせるために１／２とするものである。

実際の積算処理は、タイマ割込処理３６５によって実行される。ステップ３６６ではサンプリング時刻か否かが判定される。サンプリング時刻に合致すればステップ３６７へ進んで積算フラグのＯＮが確認され、積算フラグがＯＮであればステップ３６８へ進んで角速度データが加算されるが、積算フラグがＯＦＦの場合はステップ３６９へ進んで積算データがクリアされる。なお、積算フラグは、図１０のステップ３３８において、角速度センサ１４の出力（差分データ）が不感帯の外にある限りセットされ続ける。

このようにして、トラクタ１０が左右に傾斜して角速度センサ１４の出力が不感帯から飛び出すと、次回に不感帯へ復帰するまで角速度センサ１４の出力変化を積分した傾斜角が演算され続ける。

図１３に示すように、傾斜制御フロー（３７０）では、演算された傾斜角に基づいてソレノイドバルブ５１、５２を作動させて作業機３０の傾斜角制御を行う。ステップ３７１ではセンサ接続が判定され、図６に示す接続判断フロー２２０でセットされたセンサ接続フラグがＯＮの場合は、ステップ３７２へ進んで積算フラグがセットされているか否かが確認される。そして、図１０、図１１に示す切替制御フロー（３１０、３３０）で積算フラグがセットされていれば、ステップ３７３へ進んで、図１２に示す角度演算フロー（３６０）で演算された角速度積算データを傾斜データとして採用する。そして、続くステップ３７４では、図９に示す不感帯設定フロー（２８０）で設定した１．５〜２．２５度の狭い不感帯を採用する。

一方、ステップ３７１でセンサ外れフラグがＯＮの場合は、ステップ３７７へ進んで無事な傾斜センサデータ１３だけを採用し、続くステップ３７８では、ステップ３７４の場合よりも広い不感帯を設定する。また、ステップ３７２で積算フラグがリセット状態の場合は、ステップ３７５へ進んで傾斜センサの平滑データを採用し、ステップ３７６でステップ３７４の場合よりも広い不感帯を設定する。

そして、いずれの場合もステップ３７９へ進み、演算された傾斜データに基づいてソレノイドバルブ５１、５２を作動させて作業機３０の傾斜角制御を行う。

さて、実施例１の傾斜角自動制御装置では、角速度センサ１４の出力の基準値を２００ｍｓｅｃおきに定期的に更新する基準データ処理を実行する。基準値とは、角速度センサ１４の出力の右回りと左回りを判別する原点値、すなわち角速度０に対応する角速度センサ１４の出力値である。また、角速度センサ１４は、図１６を参照して説明したように、右回り最大角速度で０Ｖ、左回り最大角速度で５Ｖを出力し、両者の間の角速度に対して０〜５Ｖのアナログ電圧を出力する。

そして、このアナログ電圧は、図１７に示すように、８ビット２５６段階の出力データとして傾斜制御装置４０に読み取られ、出力データから基準値（以下基準データ）を差し引いて角速度（差分）が演算される。その後、差分を積算して毎秒積算回数で除することによって傾斜角が演算されることは、図１２に示す角度演算フロー（３６０）について説明したとおりである。

そして、基準データの更新は、
新しい基準データ＝現在の基準データ＋（出力データ−現在の基準データ）／２のｎ乗
という演算によって実行され、乗数ｎは、角速度レベルが大きいと次第に減らされるが、角速度レベルが小さいと次第に増加される。

図１４に示すように、ステップ４２１では、基準データ処理のタイミングに合致するか否かが判定され、合致していればステップ４２２へ進んでリレーボックス５７の出力が参照されて、ステアリング切れ角スイッチ５６がＯＮする旋回中に合致するか否かが判定される。そして、この旋回中に合致しなければ、ステップ４２３以下の基準データ処理が実行される。旋回中は地軸感度の影響で角速度センサ１４の出力値が変化するので、基準データ処理は行わない。

ステップ４２３では、角速度センサ１４の出力データから現在の基準値（以下基準データ）が差し引かれて差分が演算される。この差分は、現在の基準データに基づく現在の角速度に対応している。続くステップ４２４では、差分が０以上か否かが判定される。０以上の場合は、ステップ４２５以下のフローへ進むが、０未満の場合は図１５に示すステップ４４５以下のフローへ進む。

ステップ４２５では、以下のフローでは使用されない減算アップカウンタおよび減算ダウンカウンタがクリアされ、続くステップ４２６では差分が不感帯の範囲か否かが判定される。ここで、不感帯は、図９に示す不感帯設定フロー（２８０）で設定した不感帯とは異なる、さらに小さな範囲の所定の不感帯であって、図１８に示すように現在の基準データを中心とする±１６の範囲に定められている。ステップ４２６で差分が不感帯内と判定されるとステップ４２７へ進んで加算アップカウンタがクリアされて、加算ダウンカウンタがインクリメントされる。

加算アップカウンタ、加算ダウンカウンタは、同じ乗数ｎによる基準データ処理の繰り返し回数を設定するもので、例えば２と設定していれば、２回おきに乗数ｎが１つ増加または１つ減少される。

ステップ４２８では、加算ダウンカウンタが設定回数に達したか否かが判定され、達していなければ、ステップ４３１へ進んで同じ乗数ｎで、
差分シフト処理データ＝（出力データ−現在の基準データ）／２のｎ乗
の演算を実行する。続くステップ４３２では、
新しい基準データ＝現在の基準データ＋差分シフト処理データ
の演算を実行する。

一方、ステップ４２８で加算ダウンカウンタが設定回数に達していると、ステップ４２９へ進んで加算ダウンカウンタがクリアされ、続くステップ４３０では乗数ｎ（基準シフト）が１つ加算される。

ここで、図１８に示すように、角速度センサ１４の出力データが１３０のとき、基準データが１２５とすると、差分は１３０−１２５＝５となって、ステップ４２４からステップ４２５へ進む。その後、ステップ４２６では差分５が不感帯±１６の範囲と判断されてステップ４２７へ進み、加算ダウンカウンタが設定値に達するまで、２００ｍｓｅｃごとに、例えば乗数８で上記演算を繰り返す。

そして、加算ダウンカウンタが設定値に達すると、ステップ４２９からステップ４３０へ進んで乗数ｎが１つ増やされ、次に加算ダウンカウンタが設定値に達するまで乗数９による上記演算が繰り返される。これにより、基準データは、最初は大きく次第にゆっくりと正しい基準データ１３０へ向かってシフトされ、なめらかに正しい基準データ１３０へ収束する。

ところで、ステップ４２６で差分が不感帯外と判定されるとステップ４３６へ進んで差分が範囲外か否かが判定される。この範囲外の領域は、図１８に示すように、不感帯の外側に間隔を置いて上下に配置されている。この範囲外の領域に達するほど角速度レベルが大きい場合には、ステップ４３７以下のフローへ進んで乗数ｎを次第に減らす基準データ処理を実行するが、範囲外の領域ほど大きくない場合には、ステップ４３５へ進んで加算アップカウンタおよび加算ダウンカウンタをクリアし、現在の乗数ｎを継続した基準データ処理を実行する。

従って、トラクタ１０に範囲外の領域に達するほどの大きな揺れが発生している場合には１回あたりの補正量を大きくし、正しい基準データへの収束が遅れないようにしている。このようにして、乗数ｎ（基準シフト）は、図５に示す基準初期設定フロー（３９０）のステップ３９１で８と初期設定された後は揺れのレベルに応じて２〜１６の範囲でインクリメント／デクリメントされ、これに対応して差分シフト処理データは、差分データ／４から差分データ／（２の１６乗）の範囲で変化する。

ステップ４３７では、加算アップカウンタをインクリメントして加算ダウンカウンタをクリアする。続くステップ４３８では加算アップカウンタが設定回数に達したか否かが判定され、達していればステップ４３９へ進んで加算アップカウンタをクリアし、続くステップ４４０で乗数ｎ（基準シフト）がデクリメントされる。達していなければ、ステップ４３１へ進んで、加算アップカウンタが設定回数に達するまで同じ乗数ｎの演算を繰り返す。

一方、ステップ４２４で差分が０未満と判定された場合、すなわち差分が０以上と判定された場合とは逆方向に角速度を検知しているような場合、図１５に示すように、ステップ４４５以下のフローを実行して、基準データを引き下げる方向で、ステップ４２５以下とほぼ同様な基準データ処理を実行する。

ステップ４４５では、以下のフローで使用しない加算アップカウンタおよび加算ダウンカウンタをクリアする。続くステップ４４６では差分が不感帯の範囲か否かが判定され、不感帯内と判定されるとステップ４４７へ進んで減算アップカウンタがクリアされて、減算ダウンカウンタがインクリメントされる。減算アップカウンタ、減算ダウンカウンタもまた、同じ乗数ｎによる基準データ処理の繰り返し回数を設定するものである。

続くステップ４４８では、減算ダウンカウンタが設定回数に達したか否かが判定され、達していなければ、ステップ４５１へ進んで同じ乗数ｎで、
差分シフト処理データ＝（出力データ−現在の基準データ）／２のｎ乗
の演算を実行する。続くステップ４５２では、
新しい基準データ＝現在の基準データ＋差分シフト処理データ
の演算を実行する。

一方、ステップ４４８で減算ダウンカウンタが設定回数に達していると、ステップ４４９へ進んで減算ダウンカウンタがクリアされ、続くステップ４５０では乗数ｎ（基準シフト）が１つ加算される。

ところで、ステップ４４６で差分が不感帯外と判定されるとステップ４５６へ進んで差分が範囲外か否かが判定される。そして、範囲外に該当すればステップ４５７以下のフローへ進んで乗数ｎを次第に減らす基準データ処理を実行するが、範囲外に該当しなければステップ４５５へ進んで減算アップカウンタおよび減算ダウンカウンタをクリアし、現在の乗数ｎを継続した基準データ処理を実行する。

ステップ４５７では、減算アップカウンタをインクリメントして減算ダウンカウンタをクリアする。続くステップ４５８では減算アップカウンタが設定回数に達したか否かが判定され、達していればステップ４５９へ進んで減算アップカウンタをクリアし、続くステップ４６０で乗数ｎ（基準シフト）がデクリメントされる。達していなければ、ステップ４５１へ進んで、減算アップカウンタが設定回数に達するまで同じ乗数ｎの演算を繰り返す。

以上のように構成された実施例の傾斜角自動制御装置では、角速度センサの出力信号を出力データに変換し、角速度０に対応する基準データを出力データから差し引いた差分データに基づいて傾斜角を演算する傾斜自動制御装置において、基準データを（新しい基準データ←古い基準データ＋差分データ×比例定数）という演算式によって刻々と修正するから、図１８に示すように、正しい基準データに向かって現在の基準データが次第に収束する。

そして、基準データ、出力データ、比例定数という３つの数値データだけで修正演算が完了するから、出力データの移動平均値で基準データを更新する場合に比較して過去の出力データが不要な分、演算負荷が軽減される。そして、出力データの移動平均値を求める場合に比較して、ピーク検出の必要が無い分、出力データのサンプリング間隔を拡大しても修正精度がさほど低下しないから、サンプリング回数を減らして演算負荷を大幅に削減できる。

従って、角速度センサの基準値を刻々と補正する際の演算負荷や必要なメモリ容量が小さく済み、しかも、角速度センサ出力の読み込み頻度が少なくても補正の精度が必要十分に確保される。これにより、低速でメモリ容量の少ないマイコン素子等を採用して小型安価低消費電力を実現でき、例えば、メモリやＡＤコンバータをＣＰＵとともに１チップ化した８ビットマイコン素子でも、基準データの修正を含む傾斜角自動制御全体の演算処理を賄うことができる。

また、その比例定数は、差分データの絶対値が小さい領域（不感帯）では、次第に小さく置き換えられ、一方、差分データの絶対値が大きい領域（範囲外）では、次第に大きく置き換えられるから、角度センサの基準値が大きく変化した場合でも、基準データが速やかに正しい基準値に相当する基準データへ収束する。

また、比例定数は、１／（２のｎ乗）と定めて、乗数ｎを段階的に増減することにより比例定数を変化させるから、比例定数の演算と、差分データとの乗算とをデジタル回路で簡単に演算でき、演算負荷はさらに軽減される。

また、角速度センサの出力データに不感帯を設定し、差分データが不感帯内であれば傾斜センサの出力データの移動平均値を傾斜角データとし、差分データが不感帯の外側であれば角速度センサの出力データから基準データを差し引いた差分データを積算して傾斜角データを演算する傾斜自動制御装置において、不感帯を走行機体の対地速度に応じて変化させて対地速度が高い場合は低い場合よりも広い不感帯を設定し、かつ、基準データを（新しい基準データ←古い基準データ＋差分データ×比例定数）という演算式によって刻々と修正する構成を採用したので、不感帯の外側では、常に基準データが更新された高精度の差分データを積算して正確な傾斜角の演算ができるとともに、基準データの更新を含む角速度演算に使用する合計のデータ数が少なくて済み、演算もデジタル処理が容易な数式によってなされ、しかも、演算頻度を下げても傾斜角制御の制度が十分確保できるようになった。従って、低速でメモリ容量の少ないマイコン素子等を採用して小型安価低消費電力を実現でき、例えば、メモリやＡＤコンバータをＣＰＵとともに１チップ化した８ビットマイコン素子でも、基準データの修正を含む傾斜角自動制御全体の演算処理を賄うことができる。

本発明は、角速度センサの基準値を刻々と修正する際に用いられる演算式に特徴を有するから、ロータリ式耕耘ユニットとトラクタの組み合わせに限定されず、走行機体に作業部を連結して作業部の左右傾斜角度を自動調整する作業車輌に広く利用可能である。

実施例１の傾斜自動制御装置を搭載したトラクタの側面図である。 傾斜自動制御装置の接続状態の説明図である。 実施例１の傾斜自動制御装置における制御全体のフローチャートである。 初期設定のフローチャートである。 平滑初期設定と基準初期設定のフローチャートである。 接続判断のフローチャートである。 接続判断におけるタイマ割込処理のフローチャートである。 平滑処理のフローチャートである。 演算不感帯設定のフローチャートである。 切替制御のフローチャートである。 切替制御における部分的なフローチャートである。 角度演算のフローチャートである。 傾斜制御のフローチャートである。 基準データ処理のフローチャートである。 基準データ処理における部分的なフローチャートである。 ヒステリシス設定の線図である。 傾斜制御における不感帯設定の線図である。 基準データ処理における不感帯設定の線図である。

符号の説明

１０ トラクタ（走行機体）
１１ ポジションレバー
１２ ステアリング
１３ 傾斜センサ
１４ 角速度センサ
１５ 運転席
１６ 操作パネル
２０ 昇降リンク機構
２１ アッパリンク
２２ ロアリンク
２３ リフトアーム
２４ リフトロッド
２５ リフトロッドシリンダ
２６ リフトアームセンサ
２７ リフトシリンダ
２８ リフトロッドセンサ
２９ ポジションレバーセンサ
３０ 作業機
３１ 回転刃
４０ 傾斜自動制御装置
４１ 自動切替ボリューム
４２ 作業機切替ボリューム
４３ 傾き設定ボリューム
４４ 操作スイッチ群
４５ エンジン回転センサ
４６ 車軸回転センサ
４７ リモコンスイッチ
５１、５２ ソレノイドバルブ
５４ スピーカー
５５ ランプ
５６ ステアリング切れ角検出スイッチ

Claims (3)

1. 走行機体に作業部を左右傾斜自在に支持し、前記走行機体又は作業部の左右傾斜角を検出して、前記走行機体の左右傾斜に拘らず、前記作業部が予め設定された姿勢に保持されるように制御する傾斜自動制御装置を備えた、作業車輌において、
   前記左右傾斜角を検出する手段として傾斜センサと角速度センサとを備え、
   前記角速度センサの出力の右回りと左回りを判別する原点値としての基準値を、
   今までの基準値＋（角速度センサの検出値−今までの基準値）／２^ｎ
   により算出し、
   上記指数ｎは、傾斜自動制御の作動中に変更可能な整数である、
   ことを特徴とする作業車輌における傾斜自動制御装置。
2. 上記指数ｎを、上記（角速度センサの検出値−今までの基準値）の差分が所定の不感帯の範囲内である状態が続くと次第に増加させ、上記差分が前記所定の不感帯の範囲を越える状態が続くと次第に減少させるように変更する、
   ことを特徴とする請求項１記載の作業車輌における傾斜自動制御装置。
3. （前記角速度センサの検出値−前記基準値）の差分データが、不感帯を越えることに基づき、該差分データの積分値による傾斜角により傾斜自動制御し、前記差分データが前記不感帯を越えないことに基づき、前記傾斜センサによる傾斜角により傾斜自動制御し、
   かつ前記不感帯が、車速が速くなるほど大きくなるように変化する、
   ことを特徴とする請求項１記載の作業車輌における傾斜自動制御装置。

Images