JP4315917B2 - 傾斜角度計測装置 - Google Patents

Description

本発明は、移動体の傾斜角度を検出する傾斜角度検出手段の出力と、その傾斜方向での前記移動体の角速度を検出する角速度検出手段の出力とに基づいて、前記移動体の傾斜角度を演算する演算手段を備えた傾斜角度計測装置に関する。

上記のような傾斜角度検出手段としては、移動体に固定される容器に封入した粘度の高いシリコンオイルなどの液面に対する容器の傾斜角度を移動体の傾斜角度として電気的に検出する液面基準式の傾斜角センサや、移動体に固定される容器の内部に装備した振子に対する容器の傾斜角度を移動体の傾斜角度として電気的に検出する振子基準式の傾斜角センサなどがある。これらの傾斜角度検出手段は、その構造上、その出力から移動体の絶対角度を得ることができる反面、振動に起因したノイズの発生を抑制するために応答性が悪く、又、移動体が激しく傾動する場合には、そのときの慣性の影響による誤差を出力の高周波数成分に含むことが知られている。

一方、上記のような角速度検出手段としては振動式や光学式のジャイロセンサなどがある。これらの角速度検出手段は、その出力を積分処理すれば、移動体の変化角度を高い応答性で精度良く得られる反面、その出力からは移動体の絶対角度が得られないことが知られている。

そこで、従来では、傾斜角度計測装置に備える演算手段を、例えば、傾斜角度検出手段の出力から慣性の影響による誤差が含まれる高周波数成分を除去するローパスフィルタ、その除去成分を補う高周波数成分を角速度検出手段の出力から得るための積分器とハイパスフィルタ、及び、その角速度検出手段の出力から得た高周波数成分とローパスフィルタから出力される低周波数成分とを合成する加算器、などから構成することで、移動体の傾斜角度を、その変化に対して優れた応答性で精度良く演算するようにしたものがある（例えば特許文献１〜３参照）。
特許２６９６４２７号公報 特許２９２６１４０号公報 特許３４７９３６６号公報

ところで、傾斜角度検出手段は、移動体が想定しうる傾斜角度以上に傾斜した場合に、その出力が飽和する（移動体の傾斜角度を正確に示さなくなる）ことがあり、この飽和状態においても、演算手段が傾斜角度検出手段の出力と角速度検出手段の出力とに基づく演算を継続すると、移動体の傾斜角度に対応しない傾斜角度検出手段の出力と、移動体の傾斜角速度に対応する角速度検出手段の出力とに基づいて、移動体の傾斜角度を演算することになるため、図６の（ロ）に示すように、傾斜角度検出手段の出力が飽和している間、演算手段が移動体の傾斜角度を正確に演算しなくなって、移動体の実際の傾斜角度〔図６の（ロ）において仮想線で示すもの〕と演算手段の演算で得られる傾斜角度〔図６の（ロ）において実線で示すもの〕との間にずれが生じるだけでなく、傾斜角度検出手段の飽和状態からの復帰後も、そのずれが定常偏差として長く残ることになって、演算手段の演算による傾斜角度が異常値を示すことになり、時には、演算手段の演算による傾斜角度が、実際の傾斜角度と反対方向の値を示すこともある。

そのため、例えば、このような演算を行う演算手段によって演算された移動体の傾斜角度に基づいて、移動体の傾斜角度を所定の傾斜角度に維持する制御を行うと、傾斜角度検出手段の出力が飽和した場合に、移動体の傾斜角度を所定の傾斜角度に維持することが難しくなり、特に、演算手段の演算による傾斜角度が、実際の傾斜角度と反対方向の値になると、その時の制御作動で、移動体が所定の傾斜角度から離れる方向に制御されることになる。

尚、図６は、移動体の傾斜角度が±２ｄｅｇ以上で傾斜角度検出手段の出力が飽和する場合を示したものである。

本発明の目的は、傾斜角度検出手段の出力が飽和することに起因して、その飽和状態からの復帰後も演算手段の演算による傾斜角度が異常値を示すことを回避することにある。

本発明のうちの請求項１に記載の発明では、
移動体の傾斜角度を検出する傾斜角度検出手段の出力と、その傾斜方向での前記移動体の角速度を検出する角速度検出手段の出力とに基づいて、前記移動体の傾斜角度を演算する演算手段を備えた傾斜角度計測装置において、
前記演算手段が、前記傾斜角度検出手段の出力から高周波数成分を除去する演算処理と、前記角速度検出手段の出力に対して積分処理を行なった後、この積分処理により得た積分値から、前記傾斜角度検出手段の出力から除去した前記高周波数成分を補う高周波数成分を得る演算処理と、前記傾斜角度検出手段の出力に対する演算処理で得た演算値に、前記角速度検出手段の出力に対する演算処理で得た演算値を加える演算処理とを行うことにより、前記移動体の傾斜角度を演算するように構成し、
予め設定した閾値に基づいて前記傾斜角度検出手段が飽和状態か否かを判別するとともに、前記傾斜角度検出手段が飽和状態でない場合には、前記演算手段が、前記演算処理のうちの前記角速度検出手段の出力に対する前記演算処理を行なうことにより、前記角速度検出手段の出力に基づく演算値を更新し、前記傾斜角度検出手段が飽和状態である場合には、前記演算手段が、前記演算処理のうちの前記角速度検出手段の出力に対する前記演算処理を停止することにより、前記角速度検出手段の出力に基づく演算値を固定するように構成してある。

本発明のうちの請求項２に記載の発明では、
移動体の傾斜角度を検出する傾斜角度検出手段の出力と、その傾斜方向での前記移動体の角速度を検出する角速度検出手段の出力とに基づいて、前記移動体の傾斜角度を演算する演算手段を備えた傾斜角度計測装置において、
前記演算手段が、前記傾斜角度検出手段の出力から高周波数成分を除去する演算処理と、前記角速度検出手段の出力に対して積分処理を行なった後、この積分処理により得た積分値から、前記傾斜角度検出手段の内部で除去される高周波数成分と、前記傾斜角度検出手段の出力から除去した前記高周波数成分とを補う高周波数成分を得る演算処理と、前記傾斜角度検出手段の出力に対する演算処理で得た演算値に、前記角速度検出手段の出力に対する演算処理で得た演算値を加える演算処理とを行うことにより、前記移動体の傾斜角度を演算するように構成し、
予め設定した閾値に基づいて前記傾斜角度検出手段が飽和状態か否かを判別するとともに、前記傾斜角度検出手段が飽和状態でない場合には、前記演算手段が、前記演算処理のうちの前記角速度検出手段の出力に対する前記演算処理を行なうことにより、前記角速度検出手段の出力に基づく演算値を更新し、前記傾斜角度検出手段が飽和状態である場合には、前記演算手段が、前記演算処理のうちの前記角速度検出手段の出力に対する前記演算処理を停止することにより、前記角速度検出手段の出力に基づく演算値を固定するように構成してある。

請求項１又は２に記載の発明によると、傾斜角度検出手段の出力が飽和していない間、演算手段は、移動体の傾斜角速度に対応する角速度検出手段の出力に対する演算処理を行って、その演算で得られる演算値を更新し、その更新した演算値と、移動体の傾斜角度に対応する傾斜角度検出手段の出力とに基づいて移動体の傾斜角度を演算する。その結果、移動体の傾斜角度を、その変化に対して優れた応答性で精度良く演算できる。

そして、傾斜角度検出手段の出力が飽和すると、その飽和に伴って、演算手段が、角速度検出手段の出力に基づく演算値を固定し、傾斜角度検出手段の出力が飽和している間、その固定した演算値と飽和した傾斜角度検出手段の出力とに基づいて移動体の傾斜角度を演算し、傾斜角度検出手段が飽和状態から復帰すると、演算手段が、その復帰に伴って角速度検出手段の出力に対する演算を再開して、その演算値を更新し、その更新した演算値と飽和状態から復帰した傾斜角度検出手段の出力とに基づいて移動体の傾斜角度を演算する。

その結果、傾斜角度検出手段の出力が飽和している間は、移動体の実際の傾斜角度と演算手段の演算で得られる傾斜角度との間にずれが生じるものの、傾斜角度検出手段の出力が飽和している間も、その出力と角速度検出手段の出力とに基づく演算を継続する場合のように、傾斜角度検出手段の飽和状態からの復帰後も、そのずれが定常偏差として長く残ることはなく、傾斜角度検出手段の飽和状態からの復帰後には、移動体の傾斜角度を、その変化に対して優れた応答性で精度良く演算できることになる。

従って、傾斜角度検出手段の出力が飽和している間は、演算手段における角速度検出手段の出力に基づく演算値を固定することで、傾斜角度検出手段の出力が飽和したことに起因して、その飽和状態からの復帰後も演算手段の演算による傾斜角度が異常値を示すことを回避でき、傾斜角度検出手段の飽和状態からの復帰後における傾斜角度演算精度の向上を図れる。

本発明のうちの請求項３に記載の発明では、上記請求項１又は２に記載の発明において、前記傾斜角度検出手段が飽和状態でない場合には、前記演算手段に前記角速度検出手段の出力を入力し、前記傾斜角度検出手段が飽和状態である場合には、前記演算手段に前記角速度検出手段の出力の代わりに零を入力するように構成してある。

この構成によると、演算手段に対する入力の切り換えで、傾斜角度検出手段の出力が飽和するのに伴って、簡単かつ確実に、演算手段における角速度検出手段の出力に対する演算処理を停止させることができて、その演算値を固定することができ、又、傾斜角度検出手段の飽和状態からの復帰に伴って、簡単かつ確実に、演算手段における角速度検出手段の出力に対する演算処理を再開させることができて、その演算値を更新することができる。

従って、構成の複雑化を招くことなく、演算手段における演算状態の切り換えを確実に行える。

図１には乗用形田植機の全体側面が、図２にはその全体平面が示されており、この田植機は、走行機体１の後部に、油圧式のリフトシリンダ２の作動で昇降揺動する平行リンク式のリンク機構３と、このリンク機構３の後端部に備えた縦リンク４に着脱可能に連結される連結フレーム５とを介して、苗植付装置（移動体Ａの一例）６を駆動昇降可能に連結し、かつ、走行機体１の後部から苗植付装置６にわたる状態に施肥装置７を装備して構成されている。

走行機体１は、その前部に搭載したエンジン８からの動力を、主変速装置として備えた静油圧式無段変速装置９や、ミッションケース１０に内蔵したギヤ式の副変速装置（図示せず）などを介して、左右一対の前輪１１及び後輪１２に伝達する４輪駆動型に構成され、その前後中央に、左右の前輪１１を操向するステアリングホイール１３や運転座席１４などを備えて搭乗運転部１５が形成されている。

搭乗運転部１５において、ステアリングホイール１３の左方には、静油圧式無段変速装置９の前後進切り換え操作や無段変速操作を可能にする主変速レバー１６が配備され、運転座席１４の左方には、副変速装置の変速操作を可能にする副変速レバー１７が配備され、運転座席１４の右方には、苗植付装置６の昇降操作や苗植付装置６及び施肥装置７の作動切り換え操作を可能にする第１操作レバー１８が配備され、ステアリングホイール１３の右下方には、第１操作レバー１８を「自動」位置に操作した状態において、苗植付装置６の昇降操作や苗植付装置６及び施肥装置７の作動切り換え操作を可能にする第２操作レバー１９が配備されている。

図１〜３に示すように、苗植付装置６は、連結フレーム５の下部に前後向きの軸心Ｐ１周りにローリング可能に軸支され、その下部に左右方向に所定間隔を隔てる状態で左右向きの軸心Ｐ２周りにピッチング自在に並設した複数の整地フロート２０が、機体の走行に伴って圃場泥面を整地する一方で、ミッションケース１０に内蔵した植付クラッチ（図示せず）を介して伝達される静油圧式無段変速装置９などによる変速後の作業用動力が動力分配機構２１に伝達され、この動力分配機構２１からの分配動力で、所定条数分のマット状苗を載置する苗載台２２が所定ストロークで左右方向に往復移動し、又、左右方向に一定間隔を隔てる状態に並設した回転式の複数の植付機構２３が、対応するマット状苗から所定量の苗を切り出して各整地フロート２０による整地後の圃場泥土部に植え付け、更に、苗載台２２が左右の各ストローク端に到達するごとに、左右方向に所定間隔を隔てる状態に並設した複数の縦送りベルト２４が、対応するマット状苗を所定量だけ下方に向けて縦送りするように構成されている。

連結フレーム５の上部には、対応する左右の操作ワイヤ２５や緩衝用の引きバネ２６などを介して、苗載台２２の左右に連係される回転体２７と、この回転体２７を正逆転駆動して、回転体２７による左右の操作ワイヤ２５の巻き取り量を調節することで、苗植付装置６を前後向きの軸心Ｐ１周りにローリング操作する電動式のローリングモータ２８とが装備されている。そして、左右の引きバネ２６の作用で、苗載台２２の左右方向への往復移動にかかわらず、苗植付装置６の左右バランスの均衡を保つようになっている。

図１に示すように、施肥装置７は、施肥クラッチ（図示せず）を介して伝達される静油圧式無段変速装置９などによる変速後の走行用動力で、走行機体１の後部に左右方向に並設した複数の繰出機構２９が、それらの上部に連設したホッパー３０に貯留した肥料を所定量ずつ繰り出し、その繰り出された肥料を、電動式の送風機３１で生起される搬送風で、各繰出機構２９から対応する案内ホース３２を介して各整地フロート２０に装備した作溝器３３に搬送することで、苗植え付け箇所に隣接する圃場泥土内に所定量の肥料を供給するように構成されている。

図４に示すように、第１操作レバー１８は、「植付」「下降」「中立」「上昇」「自動」の各操作位置に揺動操作可能な揺動式で、それらの各操作位置への操作が、回転式のポテンショメータからなる第１レバーセンサ３４によって検出される。第２操作レバー１９は、中立位置から上下前後に操作可能な十字揺動式の中立復帰型で、その中立位置から上下前後への各操作が、複数のスイッチ又は多接点スイッチからなる第２レバーセンサ３５によって検出される。第１レバーセンサ３４及び第２レバーセンサ３５は、それらの検出情報を、走行機体１に搭載されたマイクロコンピュータからなる制御装置３６に出力する。

制御装置３６には、第１レバーセンサ３４及び第２レバーセンサ３５の出力以外に、リンク機構３の上下揺動角度を苗植付装置６の対機体高さとして検出する回転式のポテンショメータからなるリンクセンサ３７や、左右中央に配置された整地フロート２０のピッチング角度を苗植付装置６の接地高さとして検出する回転式のポテンショメータからなるフロートセンサ３８、などの出力が入力され、制御装置３６は、これらの入力情報に基づいて、リフトシリンダ３に対する作動油の流動状態を切り換える電磁式の昇降用制御弁３９の作動、又は、植付クラッチ及び施肥クラッチの伝動状態を切り換える電動式のクラッチモータ４０の作動を制御することで、苗植付装置６の昇降操作や苗植付装置６及び施肥装置７の作動切り換え操作を行う。

詳述すると、制御装置３６は、第１レバーセンサ３４によって第１操作レバー１８の「下降」位置への操作が検出されると、その検出が継続されている間は苗植付装置６の下降操作が行われるように昇降用制御弁３９の作動を制御し、第１操作レバー１８の「上昇」位置への操作が検出されると、その検出が継続されている間は苗植付装置６の上昇操作が行われるように昇降用制御弁３９の作動を制御し、第１操作レバー１８の「中立」位置への操作が検出されると、その検出が継続されている間は苗植付装置６の昇降操作が停止されるように昇降用制御弁３９の作動を制御する。

この第１レバーセンサ３４の出力に基づく昇降用制御弁３９の作動制御においては、リンクセンサ３７の出力と、その出力に対応させて予め設定した上限値及び下限値とを比較して、苗植付装置６の走行機体１に対する上限位置又は下限位置への到達を判別し、苗植付装置６の上昇操作時にリンクセンサ３７の出力が上限値に達した場合には、苗植付装置６が走行機体１に対する上限位置に到達した判断して、苗植付装置６の上昇操作が停止されるように昇降用制御弁３９の作動を制御する。又、苗植付装置６の下降操作時にリンクセンサ３７の出力が下限値に達した場合には、苗植付装置６が走行機体１に対する下限位置に到達したと判断して、苗植付装置６の下降操作が停止されるように昇降用制御弁３９の作動を制御する。

第１レバーセンサ３４によって第１操作レバー１８の「植付」位置への操作が検出されると、その検出が継続されている間は苗植付装置６及び施肥装置７が作動し、第１操作レバー１８の「植付」位置への操作が検出されなくなると、その非検出状態が継続されている間は苗植付装置６及び施肥装置７が作動を停止するように、クラッチモータ４０の作動を制御する。

第１レバーセンサ３４によって第１操作レバー１８の「自動」位置への操作が検出されると、その検出が継続されている間は、第２レバーセンサ３５の出力に基づいて昇降用制御弁３９及びクラッチモータ４０の作動を制御する。

第１操作レバー１８を「自動」位置に操作した状態において、第２レバーセンサ３５によって第２操作レバー１９の上方への操作が検出されると、リンクセンサ３７の出力が上限値に達するまで苗植付装置６の上昇操作が行われ、リンクセンサ３７の出力が上限値に達するのに伴って苗植付装置６の上昇操作が停止されるように、昇降用制御弁３９の作動を制御する。

逆に、第２レバーセンサ３５によって第２操作レバー１９の下方への操作が検出されると、フロートセンサ３８の出力が、その出力に対応させて予め設定される目標値に達するまで、苗植付装置６の下降操作が行われ、フロートセンサ３８の出力が目標値に達するのに伴って苗植付装置６の下降操作が停止されるように、昇降用制御弁３９の作動を制御する。

フロートセンサ３８の出力が目標値に達した苗植付装置６の接地状態において、第２レバーセンサ３５によって第２操作レバー１９の下方への操作が検出されると、苗植付装置６及び施肥装置７が作動するようにクラッチモータ４０の作動を制御し、又、フロートセンサ３８の出力が目標値に維持されるように昇降用制御弁３９の作動を制御する。

この苗植付装置６及び施肥装置７の作動状態において、第２レバーセンサ３５によって第２操作レバー１９の上方への操作が検出されると、苗植付装置６及び施肥装置７が作動を停止するようにクラッチモータ４０の作動を制御し、又、リンクセンサ３７の出力が上限値に達するまで苗植付装置６の上昇操作が行われ、リンクセンサ３７の出力が上限値に達するのに伴って苗植付装置６の上昇操作が停止されるように、昇降用制御弁３９の作動を制御する。

尚、フロートセンサ３８の目標値は、搭乗運転部１５に備えた回転式のポテンショメータからなる第１設定器４１の操作で変更できる。

つまり、制御装置３６には、第１操作レバー１８の操作による任意の高さ位置への苗植付装置６の昇降操作を可能にし、かつ、第２操作レバー１９の操作による予め設定された上限位置及び接地高さ位置への苗植付装置６の昇降操作を可能にする手動昇降制御手段４２、苗植付装置６の高さ位置が予め設定された接地高さ位置に維持されるように苗植付装置６を昇降操作する自動昇降制御手段４３、及び、第１操作レバー１８又は第２操作レバー１９の操作による苗植付装置６及び施肥装置７の作動切り換え操作を可能にする作動切換制御手段４４、などが備えられている。

そして、作業走行時には、自動昇降制御手段４３の制御作動によって、走行機体１のピッチングに関係なく、苗植付装置６の高さ位置を予め設定した接地高さに維持できる。

図３〜５に示すように、苗植付装置６には、その前後向きの軸心Ｐ１周りでのローリング角度を検出する傾斜角センサ（傾斜角度検出手段の一例）４５と、その前後向きの軸心Ｐ１周りでの角速度を検出する角速度センサ（角速度検出手段の一例）４６とが装備され、これらの各センサ４５，４６は、その検出値を制御装置３６に出力する。

制御装置３６には、それらの各センサ４５，４６の出力以外に、苗植付装置６の目標ローリング角度設定用として搭乗運転部１５に備えた回転式のポテンショメータからなる第２設定器４７の出力などが入力され、又、傾斜角センサ４５の出力と角速度センサ４６の出力とに基づいて苗植付装置６のローリング角度を演算する演算手段４８、及び、第２設定器４７の出力（設定値）と演算手段４８の出力（演算値）とに基づいて、第２設定器４７の出力に演算手段４８の出力が一致するようにローリングモータ２８の作動を制御することで、苗植付装置６のローリング角度が予め設定した目標ローリング角度に維持されるように苗植付装置６をローリング操作するローリング制御手段４９が備えられている。つまり、制御装置３６には傾斜角度計測装置としての機能が備えられている。

尚、図示は省略するが、傾斜角センサ４５には、苗植付装置６に固定される容器に封入した粘度の高いシリコンオイルなどの液面に対する容器の傾斜角度を苗植付装置６のローリング角度として電気的に検出する液面基準式のものが採用されている。一方、角速度センサ４６には振動式のジャイロセンサが採用されている。

液面基準式の傾斜角センサ４５は、その構造上、苗植付装置６のローリング方向での絶対角度を検出できる反面、粘性の高い液体を使用するために応答性に劣り、又、苗植付装置６が激しくローリングする場合には、その影響を受けて液面が左右に横揺れすることがあり、この横揺れが生じると、その影響による誤差が出力の高周波数成分に含まれることになる。

一方、角速度センサ４６は、苗植付装置６が前後向きの軸心Ｐ１周りにローリングした際の角速度を検出するものであることから、その出力を積分処理すれば、そのローリングによる苗植付装置６のローリング角度の変化を高い応答性で精度良く得ることができる反面、苗植付装置６のローリング方向での絶対角度を検出することができない。

そこで、演算手段４８には、傾斜角センサ４５の出力から液面の横揺れの影響による誤差が含まれる高周波数成分を除去するローパスフィルタ５０、角速度センサ４６の出力を積分処理する積分器５１、ローパスフィルタ５０による除去成分を補う高周波数成分を積分器５１の出力（積分値）から通過させるハイパスフィルタ５２、及び、ローパスフィルタ５０の出力とハイパスフィルタ５２の出力とを合成する加算器５３が備えられており、これによって、この演算手段４８は、傾斜角センサ４５の出力と角速度センサ４６の出力とに基づいて、苗植付装置６のローリング角度を、その変化に対して優れた応答性で精度良く演算することができる。

そして、この演算手段４８の出力と第２設定器４７の出力とに基づいて、ローリング制御手段４９が、演算手段４８の出力が第２設定器４７の出力に一致するようにローリングモータ２８の作動を制御することから、走行機体１のローリングや、そのローリングに起因した傾斜角センサ４５における液面の横揺れにかかわらず、苗植付装置６のローリング角度を予め設定した目標ローリング角度に精度良く維持できる。

角速度センサ４６には、その零点（基準電圧）が温度の変動などでドリフトする欠点があり、このドリフトが生じた場合には、角速度センサ４６の検出精度が低下することになる。

そこで、図４及び図５に示すように、制御装置３６には、温度などの影響でドリフトする角速度センサ４６の零点を補正する零点補正手段５４が備えられている。

零点補正手段５４は、ドリフトによる変動が角速度センサ４６の本来の出力成分に比較して非常に低周波であることから、時定数の大きいローパスフィルタ５５と、このローパスフィルタ５５を通過した低周波数成分を角速度センサ４６の出力から減算する減算器５６とを備えて構成されている。

つまり、制御装置３６に上記構成の零点補正手段５４を備えることで、角速度センサ４６におけるドリフトに起因した検出精度の低下を防止してある。

しかしながら、零点補正手段５４において時定数の大きいローパスフィルタ５５を採用すると、圃場耕盤の急激な変化や石などへの乗り上げなどによってローリング方向に非常に大きな角速度が生じた場合、その影響を零点補正手段５４も受けることになって、その後の演算手段４８によるローリング角度の演算に多少の誤差を生じさせることになり、その誤差は、ローパスフィルタ５５の時定数が大きいために長時間にわたって生じることになる。

その点を考慮して、零点補正手段５４は、角速度センサ４６の出力に基づいて、その値が予め設定した適正範囲内（角速度センサ４６の仕様上において零点となり得る範囲内、又は、角速度センサ４６の仕様上において零点となり得る範囲内で予め設定した範囲内）であるか否かを判別し、その出力が適正範囲内である場合には、角速度センサ４６の出力をローパスフィルタ５５に通過させて零点補正処理を行うことで零点の値を更新し、その出力が適正範囲外である場合には、角速度センサ４６の出力をローパスフィルタ５５に通過させないようにして零点補正処理を一時停止することで零点の値を固定するように構成されており、これによって、零点補正手段５４に影響を与える大きな角速度が生じた場合であっても、その影響による誤差が、演算手段４８によるローリング角度の演算において長時間にわたって生じる不都合の発生を軽減できる。

ところで、角速度センサ４６において上述したドリフトが無かったとしても、角速度センサ４６の出力を制御装置３６に備えたＡＤ変換器（図示せず）で変換する際の誤差や角速度センサ４６の直進性により、ＡＤ変換後の値には一定値以下の誤差が含まれることになり、その値に基づいて零点補正処理を行うと、その誤差の分だけ零点が不適切になる。

そこで、制御装置３６には、角速度センサ４６の出力をＡＤ変換した後に含まれる一定値以下の誤差を取り除くために、その誤差が含まれるＡＤ変換後の値における下位の値（例えば下位の３つ分）を無視する不感帯処理を行う不感帯処理手段５７が備えられており、これによって、全ての角速度におけるＡＤ変換の誤差やセンサの非直線性誤差を取り除くことができ、零点補正手段５４による零点補正をより適切に行えるようになり、結果、演算手段４８によるローリング角度の演算をより精度良く行える。

尚、不感帯処理手段５７で不感帯処理される下位の値は、ＡＤ変換器の補償などに応じて設定されるものであることから種々の変更が可能である。

角速度センサ４６には、苗植付装置６のローリングだけでなくヨーイングやピッチングにも感応する他軸感度があり、特にこの他軸感度を顕著にする要因は旋回時に発生するヨーイング方向の角速度であることから、旋回時には、その他軸感度の影響による誤差が角速度センサ４６の出力に含まれることになる。

又、その旋回時には、走行機体１がピッチングやローリングしながら旋回（ヨーイング）するため、そのときのヨーイング方向での角速度の分力がローリング方向の角速度として角速度センサ４６の出力に含まれることになる。

つまり、旋回時には、そのときに発生するローリング方向以外での角速度の影響による誤差で角速度センサ４６の出力がオフセットすることになる。

そのため、旋回時に零点補正手段５４が上述した零点補正処理（低周波数成分を零点とする処理）を行うと、零点補正手段５４が真値からずれた値を零点とすることになり、結果、演算手段４８によるローリング角度の演算精度が低下することになる。又、その旋回終了後にも、その影響が残ることで、苗植付装置６のローリング角度を予め設定した目標ローリング角度に維持することが難しくなる。

そこで、制御装置３６には、機体を直進又は略直進させる第１走行状態か機体を旋回させる第２走行状態かを判別する判別手段５８が備えられており、零点補正手段５４は、判別手段５８によって第１走行状態であると判別された場合には零点補正処理を行って零点の値を更新し、第２走行状態であると判別された場合には零点補正処理を一時停止して零点の値を固定するように構成されている。

つまり、零点補正手段５４が、ローリング方向以外の角速度の影響が少ない第１走行状態においてのみ零点補正処理を行うことから、その影響が大きい第２走行状態においても零点補正処理を行うことに起因して発生する、零点補正手段５４が真値からずれた値を零点とする不都合の発生を効果的に抑制することができ、その結果、直進時や旋回時にかかわらず、演算手段４８によるローリング角度の演算を精度良く行えるとともに、苗植付装置６のローリング角度を予め設定した目標ローリング角度に精度良く維持することができる。

尚、判別手段５８は、ステアリングホイール１３から左右の前輪１１にわたるステアリング操作系におけるピットマンアーム（図示せず）の揺動操作量などを前輪１１の切れ角として検出する回転式のポテンショメータからなる切角センサ５９の出力に基づいて、前輪１１の所定角度（例えば３５度）未満の操向操作を検知している状態を第１走行状態と判別し、又、前輪１１の所定角度（例えば３５度）以上の操向操作を検知している状態を第２走行状態と判別するように構成されている。

又、第１走行状態か第２走行状態かの判別を、例えば、判別手段５８が、フロートセンサ３８の出力と切角センサ５９の出力とに基づいて、整地フロート２０の接地と前輪１１の所定角度（例えば３５度）未満の操向操作を検知している状態を第１走行状態と判別し、又、それ以外の例えば整地フロート２０の浮上と前輪１１の所定角度（例えば３５度）以上の操向操作を検知している状態や、整地フロート２０の接地と前輪１１の所定角度（例えば３５度）以上の操向操作を検知している状態、あるいは、整地フロート２０の浮上と前輪１１の所定角度（例えば３５度）未満の操向操作を検知している状態、などを第２走行状態と判別するように構成してもよい。

傾斜角センサ４５は、走行機体１の石への乗り上げなどに起因して苗植付装置６が想定しうる傾斜角度以上にローリングした場合に、その出力が飽和する（苗植付装置６のローリング角度を正確に検出しなくなる）ことがあり、この飽和状態においても、演算手段４８が傾斜角センサ４５の出力と角速度センサ４６の出力とに基づいて苗植付装置６のローリング角度を演算する演算処理を継続すると、苗植付装置６のローリング角度の変化に対応しない傾斜角センサ４５の出力と、苗植付装置６のローリング角度が変化する際の角速度に対応する角速度センサ４６の出力とに基づいて、苗植付装置６のローリング角度を演算することになるために、図６の（ロ）に示すように、傾斜角センサ４５の出力が飽和している間、演算手段４８が苗植付装置６のローリング角度を正確に演算しなくなって、苗植付装置６の実際のローリング角度〔図６の（ロ）において仮想線で示すもの〕と演算手段４８の演算で得られるローリング角度〔図６の（ロ）において実線で示すもの〕との間にずれが生じるだけでなく、傾斜角センサ４５の飽和状態からの復帰後も、そのずれが定常偏差として長く残ることになって、演算手段４８の演算で得られるローリング角度が異常値を示すことになり、時には、演算手段４８の演算で得られるローリング角度が、実際のローリング角度と反対方向の値を示すこともある。

そのため、苗植付装置６のローリング角度を予め設定した目標ローリング角度に維持すること難しくなり、時には、予め設定した目標ローリング角度から離れる方向に苗植付装置６がローリング操作されることになる。

尚、図６は、苗植付装置６のローリング角度が±２ｄｅｇ以上で傾斜角センサ４５の出力が飽和する場合を示したものである。

そこで、図４及び図５に示すように、制御装置３６には、傾斜角センサ４５の出力と、傾斜角センサ４５の仕様に応じて予め設定した閾値（例えば±２ｄｅｇ）とに基づいて、傾斜角センサ４５が飽和状態か否かを判別するとともに、傾斜角センサ４５が飽和状態でない場合には、演算手段４８に角速度センサ４６の出力を入力し、傾斜角センサ４５が飽和状態である場合には、演算手段４８に角速度センサ４６の出力の代わりに零を入力する入力切換手段６０が備えられている。

これによって、傾斜角センサ４５の出力が飽和するまでの間（傾斜角センサ４５の出力が閾値を超えていない間）は、演算手段４８に、苗植付装置６のローリング角度に対応する傾斜角センサ４５の出力と、ローリング方向での苗植付装置６の角速度に対応する角速度センサ４６の出力とが入力され、演算手段４８が、傾斜角センサ４５の出力に対しては、ローパスフィルタ５０によるフィルタ処理を行って、傾斜角センサ４５の出力から液面の横揺れの影響による誤差が含まれる高周波数成分を除去する一方で、角速度センサ４６の出力に対しては、積分器５１による積分処理を行って、角速度センサ４６の出力に基づく積分値（苗植付装置６のローリング角度変化）を更新し、その更新した積分値に対してハイパスフィルタ５２によるフィルタ処理を行って、ローパスフィルタ５０によるフィルタ処理で除去した高周波数成分を補う高周波数成分を通過させた後、ローパスフィルタ５０の出力とハイパスフィルタ５２の出力とに対して加算器５３による加算処理を行って、それらの出力を合成するようになることから、図６の（イ）において仮想線で示すように、苗植付装置６のローリング角度を、その変化に対して優れた応答性で精度良く演算できるようになる。

つまり、傾斜角センサ４５の出力が飽和するまでの間は、演算手段４８によって優れた応答性で精度良く演算される苗植付装置６のローリング角度に基づくローリング制御手段４９の制御作動で、苗植付装置６のローリング角度を予め設定した目標ローリング角度に精度良く維持できる。

そして、傾斜角センサ４５の出力が飽和すると（傾斜角センサ４５の出力が閾値を超えると）、傾斜角センサ４５の出力が飽和している間は、演算手段４８に、その飽和によって一定値となって苗植付装置６のローリング角度に対応しなくなる傾斜角センサ４５の出力と、角速度センサ４６の出力の代わりに零が入力されることになり、演算手段４８は、一定値となる傾斜角センサ４５の出力に対してローパスフィルタ５０によるフィルタ処理を行う。又、角速度センサ４６の出力の代わりに入力される零に対して積分器５１による積分処理を行うことから、角速度センサ４６の出力に基づく積分値が、傾斜角センサ４５の出力が飽和するとともに、飽和直前の積分処理で得た値に固定されることになる。そして、その固定状態の積分値に対してハイパスフィルタ５２によるフィルタ処理を行った後、ローパスフィルタ５０の出力とハイパスフィルタ５２の出力とに対して加算器５３による加算処理を行うことになる。そのため、図６の（イ）において実線で示すように、傾斜角センサ４５の出力が飽和している間は、演算手段４８で演算される苗植付装置６のローリング角度は、傾斜角センサ４５の出力が飽和する閾値に応じた一定値となり、苗植付装置６の実際のローリング角度に対してずれが生じることになる。

つまり、傾斜角センサ４５の出力が飽和している間は、苗植付装置６の実際のローリング角度と、演算手段４８で演算される苗植付装置６のローリング角度との間でずれが生じるものの、演算手段４８で演算される苗植付装置６のローリング角度が、傾斜角センサ４５の出力が飽和する閾値に応じた一定値となることで、ローリング制御手段４９は、その値から苗植付装置６のローリング操作方向を判別できることになり、結果、傾斜角センサ４５の出力が飽和している間においても、演算手段４８で演算される苗植付装置６のローリング角度に基づくローリング制御手段４９の制御作動によって、苗植付装置６のローリング操作を適切に行える。

その後、傾斜角センサ４５が飽和状態から復帰すると、その復帰に伴って、演算手段４８には、苗植付装置６のローリング角度に対応する傾斜角センサ４５の出力と、ローリング方向での苗植付装置６の角速度に対応する角速度センサ４６の出力とが入力されることになり、演算手段４８が、その傾斜角センサ４５の出力に対してローパスフィルタ５０によるフィルタ処理を行う一方で、角速度センサ４６の出力に対して積分器５１による積分処理を行って、積分値を角速度センサ４６の出力に基づいて更新し、その更新した積分値に対してハイパスフィルタ５２によるフィルタ処理を行った後、ローパスフィルタ５０の出力とハイパスフィルタ５２の出力とに対して加算器５３による加算処理を行うことから、図６の（イ）において仮想線で示すように、苗植付装置６のローリング角度を、その変化に対して優れた応答性で精度良く演算できるようになる。

つまり、傾斜角センサ４５の出力が飽和している間は、演算手段４８に、角速度センサ４６の出力の代わりに零を入力して積分器５１からの積分値を固定し、傾斜角センサ４５が飽和状態から復帰するのに伴って、演算手段４８に、角速度センサ４６の出力を入力して積分器５１からの積分値を更新することで、傾斜角センサ４５の出力が飽和している間に生じた、苗植付装置６の実際のローリング角度と、演算手段４８で演算される苗植付装置６のローリング角度とのずれが、傾斜角センサ４５の飽和状態からの復帰後も定常偏差として残ることを防止でき、傾斜角センサ４５の飽和状態からの復帰後には、再び、苗植付装置６のローリング角度を、その変化に対して優れた応答性で精度良く演算できることから、傾斜角センサ４５の飽和状態からの復帰後においても、演算手段４８によって優れた応答性で精度良く演算される苗植付装置６のローリング角度に基づくローリング制御手段４９の制御作動で、苗植付装置６のローリング角度を予め設定した目標ローリング角度に精度良く維持できる。

〔別実施例〕
以下、本発明の別実施形態を列記する。
〔１〕移動体Ａとしては、苗植付装置６以外の例えば耕耘装置などの対地作業装置であってもよく、又、それらの対地作業装置が連結される走行機体１、あるいは、コンバインなどの収穫作業車やバックホーなどの建設作業車、などであってもよい。

〔２〕角速度検出手段４６として、機械式のジャイロセンサや光学式のジャイロセンサなどを採用してもよい。

〔３〕傾斜角度検出手段４５として、振子基準式の傾斜角センサや加速度センサなどを採用してもよい。

〔４〕傾斜角度計測装置３６としては、例えば、傾斜角度検出手段４５の出力が飽和するのに伴って、演算手段４８における角速度検出手段４６の出力に対する積分処理を停止するとともに、その停止前に得た角速度検出手段４６の出力に基づく積分値を記憶し、傾斜角度検出手段４５の出力が飽和している間、その記憶した積分値と飽和している傾斜角度検出手段４５の出力とに基づいて傾斜角度を演算し、傾斜角度検出手段４５の飽和状態からの復帰に伴って、演算手段４８における角速度検出手段４６の出力に対する積分処理を再開して、傾斜角度検出手段４５の出力と角速度検出手段４６の出力とに基づく傾斜角度の演算を行うように構成されたものであってもよい。

〔５〕演算手段４８としては、移動体Ａのピッチング角度やヨーイング角度を演算するものであってもよい。

〔６〕演算手段４８の構成としては種々の変更が可能であり、例えば、積分器５１及びハイパスフィルタ５２の代わりに、「積分器×ハイパスフィルタ」の伝達特性を有するローパスフィルタを備えるものであってもよく、又、角速度センサ４６の零点補正処理などを行うように構成してもよい。

〔７〕演算手段４８としては、図７に示すように、傾斜角度検出手段４５の特性（時間遅れ要素）を考慮した演算を行うように構成しものであってもよく、又、図８に示すように、傾斜角度検出手段４５の特性（時間遅れ要素）を考慮した演算を、フィルタ次数の低減による高速化を図りながら行うように構成しものであってもよく、更に、図９及び図１０に示すように、傾斜角度検出手段４５の特性（時間遅れ要素）を考慮した演算を、フィルタ次数の低減やフィルタ使用数の削減による高速化を図りながら行うように構成しものであってもよい。

ちなみに、図８に示す演算手段４８の構成は、角速度検出手段４６の出力から必要な高周波数成分を得るための伝達関数が、１／ｓ×｛１−１／〔（Ｔ１ｓ＋１）×（Ｔ２ｓ＋１）〕｝である場合に、その時定数から適切に設定したゲインＧ１，Ｇ２を用いて、Ｇ１×１／（Ｔ１ｓ＋１）＋Ｇ２×１／（Ｔ２ｓ＋１）という式に展開することで得た構成である。尚、このときの第１ゲインＧ１はＴ１^２／（Ｔ１−Ｔ２）、第２ゲインＧ２はＴ２^２／（Ｔ２−Ｔ１）である。

又、図９に示す演算手段４８の構成は、図８に示す演算手段４８の構成において、第１ゲインＧ１と第２ゲインＧ２とを比較して、第１ゲインＧ１が第２ゲインＧ２に比べて無視できる程度のかなり小さい値であることが判明したことで得た構成である。

更に、図１０に示す演算手段４８の構成は、傾斜角度検出手段の出力から高周波数成分を除去するローパスフィルタ５０の特性が２次で、傾斜角度検出手段４５の特性とローパスフィルタ５０の特性を示す伝達関数が、１／（Ｔ１ｓ＋１）×（Ｔ２ｓ＋１）×（Ｔ３ｓ＋１）であり、角速度検出手段４６の出力から必要な高周波数成分を得るための伝達関数が、１／ｓ×｛１−１／〔（Ｔ１ｓ＋１）×（Ｔ２ｓ＋１）×（Ｔ３ｓ＋１）〕｝である場合に、その時定数から適切に設定したゲインＧ１，Ｇ２，Ｇ３を用いて、Ｇ１×１／（Ｔ１ｓ＋１）＋Ｇ２×１／〔（Ｔ２ｓ＋１）×（Ｔ３ｓ＋１）〕＋Ｇ３×１／（Ｔ３ｓ＋１）という式に展開し、又、各ゲインＧ１〜Ｇ３を比較して、第１ゲインＧ１が第２ゲインＧ２及び第３ゲインＧ３に比べて無視できる程度のかなり小さい値であることが判明したことで得た構成である。

乗用形田植機の全体側面図 乗用形田植機の全体平面図 苗植付装置の正面図 制御構成を示すブロック図 傾斜角度計測装置の構成を示すブロック図 （イ）傾斜角センサが飽和した状態での傾斜角度演算値を示す図 （ロ）従来の傾斜角センサが飽和した状態での傾斜角度演算値を示す図 傾斜角度検出手段の特性を考慮した演算手段の構成を示すブロック図 傾斜角度検出手段の特性を考慮しながら高速化を図れる演算手段の構成を示すブロック図 傾斜角度検出手段の特性を考慮しながら高速化を更に図れる演算手段の構成を示すブロック図 高次の特性に対応する演算手段の構成を示すブロック図

符号の説明

Ａ 移動体
４５ 傾斜角度検出手段
４６ 角速度検出手段
４８ 演算手段

Claims (3)

1. 移動体の傾斜角度を検出する傾斜角度検出手段の出力と、その傾斜方向での前記移動体の角速度を検出する角速度検出手段の出力とに基づいて、前記移動体の傾斜角度を演算する演算手段を備えた傾斜角度計測装置であって、
   前記演算手段が、前記傾斜角度検出手段の出力から高周波数成分を除去する演算処理と、前記角速度検出手段の出力に対して積分処理を行なった後、この積分処理により得た積分値から、前記傾斜角度検出手段の出力から除去した前記高周波数成分を補う高周波数成分を得る演算処理と、前記傾斜角度検出手段の出力に対する演算処理で得た演算値に、前記角速度検出手段の出力に対する演算処理で得た演算値を加える演算処理とを行うことにより、前記移動体の傾斜角度を演算するように構成し、
   予め設定した閾値に基づいて前記傾斜角度検出手段が飽和状態か否かを判別するとともに、前記傾斜角度検出手段が飽和状態でない場合には、前記演算手段が、前記演算処理のうちの前記角速度検出手段の出力に対する前記演算処理を行なうことにより、前記角速度検出手段の出力に基づく演算値を更新し、前記傾斜角度検出手段が飽和状態である場合には、前記演算手段が、前記演算処理のうちの前記角速度検出手段の出力に対する前記演算処理を停止することにより、前記角速度検出手段の出力に基づく演算値を固定するように構成してある傾斜角度計測装置。
2. 移動体の傾斜角度を検出する傾斜角度検出手段の出力と、その傾斜方向での前記移動体の角速度を検出する角速度検出手段の出力とに基づいて、前記移動体の傾斜角度を演算する演算手段を備えた傾斜角度計測装置であって、
   前記演算手段が、前記傾斜角度検出手段の出力から高周波数成分を除去する演算処理と、前記角速度検出手段の出力に対して積分処理を行なった後、この積分処理により得た積分値から、前記傾斜角度検出手段の内部で除去される高周波数成分と、前記傾斜角度検出手段の出力から除去した前記高周波数成分とを補う高周波数成分を得る演算処理と、前記傾斜角度検出手段の出力に対する演算処理で得た演算値に、前記角速度検出手段の出力に対する演算処理で得た演算値を加える演算処理とを行うことにより、前記移動体の傾斜角度を演算するように構成し、
   予め設定した閾値に基づいて前記傾斜角度検出手段が飽和状態か否かを判別するとともに、前記傾斜角度検出手段が飽和状態でない場合には、前記演算手段が、前記演算処理のうちの前記角速度検出手段の出力に対する前記演算処理を行なうことにより、前記角速度検出手段の出力に基づく演算値を更新し、前記傾斜角度検出手段が飽和状態である場合には、前記演算手段が、前記演算処理のうちの前記角速度検出手段の出力に対する前記演算処理を停止することにより、前記角速度検出手段の出力に基づく演算値を固定するように構成してある傾斜角度計測装置。
3. 前記傾斜角度検出手段が飽和状態でない場合には、前記演算手段に前記角速度検出手段の出力を入力し、前記傾斜角度検出手段が飽和状態である場合には、前記演算手段に前記角速度検出手段の出力の代わりに零を入力するように構成してある請求項１又は２に記載の傾斜角度計測装置。

Images