JP4315917B2 - Inclination angle measuring device - Google Patents
Inclination angle measuring device Download PDFInfo
- Publication number
- JP4315917B2 JP4315917B2 JP2005033170A JP2005033170A JP4315917B2 JP 4315917 B2 JP4315917 B2 JP 4315917B2 JP 2005033170 A JP2005033170 A JP 2005033170A JP 2005033170 A JP2005033170 A JP 2005033170A JP 4315917 B2 JP4315917 B2 JP 4315917B2
- Authority
- JP
- Japan
- Prior art keywords
- output
- angular velocity
- calculation
- tilt angle
- detection means
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Expired - Fee Related
Links
Images
Landscapes
- Transplanting Machines (AREA)
- Lifting Devices For Agricultural Implements (AREA)
- Length Measuring Devices With Unspecified Measuring Means (AREA)
- Gyroscopes (AREA)
Description
本発明は、移動体の傾斜角度を検出する傾斜角度検出手段の出力と、その傾斜方向での前記移動体の角速度を検出する角速度検出手段の出力とに基づいて、前記移動体の傾斜角度を演算する演算手段を備えた傾斜角度計測装置に関する。 The present invention determines the inclination angle of the moving body based on the output of the inclination angle detecting means for detecting the inclination angle of the moving body and the output of the angular velocity detecting means for detecting the angular velocity of the moving body in the inclination direction. The present invention relates to an inclination angle measuring device provided with a calculating means for calculating.
上記のような傾斜角度検出手段としては、移動体に固定される容器に封入した粘度の高いシリコンオイルなどの液面に対する容器の傾斜角度を移動体の傾斜角度として電気的に検出する液面基準式の傾斜角センサや、移動体に固定される容器の内部に装備した振子に対する容器の傾斜角度を移動体の傾斜角度として電気的に検出する振子基準式の傾斜角センサなどがある。これらの傾斜角度検出手段は、その構造上、その出力から移動体の絶対角度を得ることができる反面、振動に起因したノイズの発生を抑制するために応答性が悪く、又、移動体が激しく傾動する場合には、そのときの慣性の影響による誤差を出力の高周波数成分に含むことが知られている。 As the tilt angle detecting means as described above, the liquid level reference for electrically detecting the tilt angle of the container with respect to the liquid level such as high viscosity silicone oil sealed in the container fixed to the movable body as the tilt angle of the movable body. There are a tilt angle sensor of the type, a pendulum reference type tilt angle sensor that electrically detects the tilt angle of the container with respect to the pendulum mounted inside the container fixed to the movable body as the tilt angle of the movable body, and the like. These tilt angle detecting means can obtain the absolute angle of the moving body from its output due to its structure, but have poor response to suppress the generation of noise due to vibration, and the moving body is intense. In the case of tilting, it is known that an error due to the influence of inertia at that time is included in the high frequency component of the output.
一方、上記のような角速度検出手段としては振動式や光学式のジャイロセンサなどがある。これらの角速度検出手段は、その出力を積分処理すれば、移動体の変化角度を高い応答性で精度良く得られる反面、その出力からは移動体の絶対角度が得られないことが知られている。 On the other hand, as the angular velocity detecting means as described above, there are a vibration type and an optical type gyro sensor. It is known that these angular velocity detection means can obtain the change angle of the moving body with high responsiveness and accuracy if the output is integrated, but the absolute angle of the moving body cannot be obtained from the output. .
そこで、従来では、傾斜角度計測装置に備える演算手段を、例えば、傾斜角度検出手段の出力から慣性の影響による誤差が含まれる高周波数成分を除去するローパスフィルタ、その除去成分を補う高周波数成分を角速度検出手段の出力から得るための積分器とハイパスフィルタ、及び、その角速度検出手段の出力から得た高周波数成分とローパスフィルタから出力される低周波数成分とを合成する加算器、などから構成することで、移動体の傾斜角度を、その変化に対して優れた応答性で精度良く演算するようにしたものがある(例えば特許文献1〜3参照)。
ところで、傾斜角度検出手段は、移動体が想定しうる傾斜角度以上に傾斜した場合に、その出力が飽和する(移動体の傾斜角度を正確に示さなくなる)ことがあり、この飽和状態においても、演算手段が傾斜角度検出手段の出力と角速度検出手段の出力とに基づく演算を継続すると、移動体の傾斜角度に対応しない傾斜角度検出手段の出力と、移動体の傾斜角速度に対応する角速度検出手段の出力とに基づいて、移動体の傾斜角度を演算することになるため、図6の(ロ)に示すように、傾斜角度検出手段の出力が飽和している間、演算手段が移動体の傾斜角度を正確に演算しなくなって、移動体の実際の傾斜角度〔図6の(ロ)において仮想線で示すもの〕と演算手段の演算で得られる傾斜角度〔図6の(ロ)において実線で示すもの〕との間にずれが生じるだけでなく、傾斜角度検出手段の飽和状態からの復帰後も、そのずれが定常偏差として長く残ることになって、演算手段の演算による傾斜角度が異常値を示すことになり、時には、演算手段の演算による傾斜角度が、実際の傾斜角度と反対方向の値を示すこともある。 By the way, when the moving body tilts more than the tilt angle that can be assumed, the output may be saturated (the tilt angle of the moving body cannot be accurately indicated). Even in this saturated state, When the calculation means continues the calculation based on the output of the tilt angle detection means and the output of the angular velocity detection means, the output of the tilt angle detection means not corresponding to the tilt angle of the moving body and the angular velocity detection means corresponding to the tilt angular speed of the moving body Since the inclination angle of the moving body is calculated based on the output of the moving body, as shown in (b) of FIG. 6, while the output of the inclination angle detecting means is saturated, the calculating means Since the tilt angle cannot be calculated accurately, the actual tilt angle of the moving object (shown by the phantom line in FIG. 6B) and the tilt angle obtained by the calculation of the calculation means [solid line in FIG. And what Not only will there be a gap between them, but even after the tilt angle detection means has returned from the saturated state, the deviation will remain as a steady deviation, and the tilt angle calculated by the calculation means will show an abnormal value. Sometimes, the tilt angle calculated by the calculation means may show a value in the direction opposite to the actual tilt angle.
そのため、例えば、このような演算を行う演算手段によって演算された移動体の傾斜角度に基づいて、移動体の傾斜角度を所定の傾斜角度に維持する制御を行うと、傾斜角度検出手段の出力が飽和した場合に、移動体の傾斜角度を所定の傾斜角度に維持することが難しくなり、特に、演算手段の演算による傾斜角度が、実際の傾斜角度と反対方向の値になると、その時の制御作動で、移動体が所定の傾斜角度から離れる方向に制御されることになる。 Therefore, for example, when control is performed to maintain the inclination angle of the moving body at a predetermined inclination angle based on the inclination angle of the moving body calculated by the calculating means that performs such calculation, the output of the inclination angle detecting means is When saturated, it becomes difficult to maintain the tilt angle of the moving body at a predetermined tilt angle. In particular, if the tilt angle calculated by the calculation means becomes a value opposite to the actual tilt angle, the control operation at that time Thus, the moving body is controlled in a direction away from the predetermined inclination angle.
尚、図6は、移動体の傾斜角度が±2deg以上で傾斜角度検出手段の出力が飽和する場合を示したものである。 FIG. 6 shows a case where the output of the tilt angle detecting means is saturated when the tilt angle of the moving body is ± 2 deg or more.
本発明の目的は、傾斜角度検出手段の出力が飽和することに起因して、その飽和状態からの復帰後も演算手段の演算による傾斜角度が異常値を示すことを回避することにある。 An object of the present invention is to avoid the fact that the inclination angle obtained by the calculation of the calculation means shows an abnormal value even after the return from the saturated state due to the saturation of the output of the inclination angle detection means.
本発明のうちの請求項1に記載の発明では、
移動体の傾斜角度を検出する傾斜角度検出手段の出力と、その傾斜方向での前記移動体の角速度を検出する角速度検出手段の出力とに基づいて、前記移動体の傾斜角度を演算する演算手段を備えた傾斜角度計測装置において、
前記演算手段が、前記傾斜角度検出手段の出力から高周波数成分を除去する演算処理と、前記角速度検出手段の出力に対して積分処理を行なった後、この積分処理により得た積分値から、前記傾斜角度検出手段の出力から除去した前記高周波数成分を補う高周波数成分を得る演算処理と、前記傾斜角度検出手段の出力に対する演算処理で得た演算値に、前記角速度検出手段の出力に対する演算処理で得た演算値を加える演算処理とを行うことにより、前記移動体の傾斜角度を演算するように構成し、
予め設定した閾値に基づいて前記傾斜角度検出手段が飽和状態か否かを判別するとともに、前記傾斜角度検出手段が飽和状態でない場合には、前記演算手段が、前記演算処理のうちの前記角速度検出手段の出力に対する前記演算処理を行なうことにより、前記角速度検出手段の出力に基づく演算値を更新し、前記傾斜角度検出手段が飽和状態である場合には、前記演算手段が、前記演算処理のうちの前記角速度検出手段の出力に対する前記演算処理を停止することにより、前記角速度検出手段の出力に基づく演算値を固定するように構成してある。
In the invention according to
Calculation means for calculating the inclination angle of the moving body based on the output of the inclination angle detecting means for detecting the inclination angle of the moving body and the output of the angular velocity detection means for detecting the angular velocity of the moving body in the inclination direction. In a tilt angle measuring device with
The calculation means performs calculation processing for removing a high frequency component from the output of the tilt angle detection means and integration processing for the output of the angular velocity detection means, and from the integration value obtained by this integration processing, Arithmetic processing for obtaining a high-frequency component that compensates for the high-frequency component removed from the output of the tilt angle detecting means, and an arithmetic processing for the output of the angular velocity detecting means to the arithmetic value obtained by the arithmetic processing for the output of the tilt angle detecting means And calculating the tilt angle of the moving body by performing the calculation process of adding the calculation value obtained in
Based on a preset threshold value, it is determined whether or not the tilt angle detection unit is in a saturated state, and when the tilt angle detection unit is not in a saturated state, the calculation unit detects the angular velocity detection in the calculation process. by performing the arithmetic process of the outputs of the means, updates the calculated value based on the output of said angular velocity detecting means, wherein when the inclination angle detection means is saturated, the calculation means, among the operation processing by stopping the operation processing to the output of said angular velocity detecting means, is arranged to secure the calculated value based on the output of said angular velocity detecting means.
本発明のうちの請求項2に記載の発明では、In the invention according to
移動体の傾斜角度を検出する傾斜角度検出手段の出力と、その傾斜方向での前記移動体の角速度を検出する角速度検出手段の出力とに基づいて、前記移動体の傾斜角度を演算する演算手段を備えた傾斜角度計測装置において、Calculation means for calculating the inclination angle of the moving body based on the output of the inclination angle detecting means for detecting the inclination angle of the moving body and the output of the angular velocity detection means for detecting the angular velocity of the moving body in the inclination direction. In a tilt angle measuring device with
前記演算手段が、前記傾斜角度検出手段の出力から高周波数成分を除去する演算処理と、前記角速度検出手段の出力に対して積分処理を行なった後、この積分処理により得た積分値から、前記傾斜角度検出手段の内部で除去される高周波数成分と、前記傾斜角度検出手段の出力から除去した前記高周波数成分とを補う高周波数成分を得る演算処理と、前記傾斜角度検出手段の出力に対する演算処理で得た演算値に、前記角速度検出手段の出力に対する演算処理で得た演算値を加える演算処理とを行うことにより、前記移動体の傾斜角度を演算するように構成し、The calculation means performs calculation processing for removing a high frequency component from the output of the tilt angle detection means and integration processing for the output of the angular velocity detection means, and from the integration value obtained by this integration processing, Arithmetic processing for obtaining a high-frequency component that compensates for the high-frequency component removed inside the tilt-angle detection means and the high-frequency component removed from the output of the tilt-angle detection means, and computation on the output of the tilt-angle detection means The calculation value obtained by the process is added to the calculation value obtained by the calculation process for the output of the angular velocity detection means, and the tilt angle of the moving body is calculated.
予め設定した閾値に基づいて前記傾斜角度検出手段が飽和状態か否かを判別するとともに、前記傾斜角度検出手段が飽和状態でない場合には、前記演算手段が、前記演算処理のうちの前記角速度検出手段の出力に対する前記演算処理を行なうことにより、前記角速度検出手段の出力に基づく演算値を更新し、前記傾斜角度検出手段が飽和状態である場合には、前記演算手段が、前記演算処理のうちの前記角速度検出手段の出力に対する前記演算処理を停止することにより、前記角速度検出手段の出力に基づく演算値を固定するように構成してある。Based on a preset threshold value, it is determined whether or not the tilt angle detection unit is in a saturated state, and when the tilt angle detection unit is not in a saturated state, the calculation unit detects the angular velocity detection in the calculation process. By performing the calculation process on the output of the means, the calculation value based on the output of the angular velocity detection means is updated, and when the tilt angle detection means is in a saturated state, the calculation means The calculation processing based on the output of the angular velocity detection means is fixed by stopping the calculation processing for the output of the angular velocity detection means.
請求項1又は2に記載の発明によると、傾斜角度検出手段の出力が飽和していない間、演算手段は、移動体の傾斜角速度に対応する角速度検出手段の出力に対する演算処理を行って、その演算で得られる演算値を更新し、その更新した演算値と、移動体の傾斜角度に対応する傾斜角度検出手段の出力とに基づいて移動体の傾斜角度を演算する。その結果、移動体の傾斜角度を、その変化に対して優れた応答性で精度良く演算できる。
According to the first or second aspect of the invention, while the output of the tilt angle detection unit is not saturated, the calculation unit performs a calculation process on the output of the angular velocity detection unit corresponding to the tilt angular velocity of the moving body, and The calculated value obtained by the calculation is updated, and the tilt angle of the moving body is calculated based on the updated calculated value and the output of the tilt angle detecting means corresponding to the tilt angle of the moving body. As a result, the inclination angle of the moving body can be calculated with high responsiveness with respect to the change.
そして、傾斜角度検出手段の出力が飽和すると、その飽和に伴って、演算手段が、角速度検出手段の出力に基づく演算値を固定し、傾斜角度検出手段の出力が飽和している間、その固定した演算値と飽和した傾斜角度検出手段の出力とに基づいて移動体の傾斜角度を演算し、傾斜角度検出手段が飽和状態から復帰すると、演算手段が、その復帰に伴って角速度検出手段の出力に対する演算を再開して、その演算値を更新し、その更新した演算値と飽和状態から復帰した傾斜角度検出手段の出力とに基づいて移動体の傾斜角度を演算する。 Then, when the output of the tilt angle detecting means is saturated, the calculation means fixes the calculated value based on the output of the angular velocity detecting means, and the output is fixed while the output of the tilt angle detecting means is saturated. When the inclination angle detecting means returns from the saturated state, the calculating means outputs the output of the angular velocity detecting means along with the return. The calculation is resumed, the calculated value is updated, and the tilt angle of the moving body is calculated based on the updated calculated value and the output of the tilt angle detecting means that has returned from the saturated state.
その結果、傾斜角度検出手段の出力が飽和している間は、移動体の実際の傾斜角度と演算手段の演算で得られる傾斜角度との間にずれが生じるものの、傾斜角度検出手段の出力が飽和している間も、その出力と角速度検出手段の出力とに基づく演算を継続する場合のように、傾斜角度検出手段の飽和状態からの復帰後も、そのずれが定常偏差として長く残ることはなく、傾斜角度検出手段の飽和状態からの復帰後には、移動体の傾斜角度を、その変化に対して優れた応答性で精度良く演算できることになる。 As a result, while the output of the tilt angle detecting means is saturated, there is a deviation between the actual tilt angle of the moving object and the tilt angle obtained by the calculation of the calculating means, but the output of the tilt angle detecting means is Even when saturated, the deviation remains long as a steady-state deviation even after the inclination angle detecting means returns from the saturated state, as in the case of continuing the calculation based on the output and the output of the angular velocity detecting means. In addition, after the inclination angle detecting means returns from the saturated state, the inclination angle of the moving body can be calculated with high responsiveness to the change with high accuracy.
従って、傾斜角度検出手段の出力が飽和している間は、演算手段における角速度検出手段の出力に基づく演算値を固定することで、傾斜角度検出手段の出力が飽和したことに起因して、その飽和状態からの復帰後も演算手段の演算による傾斜角度が異常値を示すことを回避でき、傾斜角度検出手段の飽和状態からの復帰後における傾斜角度演算精度の向上を図れる。 Therefore, while the output of the tilt angle detection means is saturated, the calculation value based on the output of the angular velocity detection means in the calculation means is fixed, so that the output of the tilt angle detection means is saturated. Even after returning from the saturated state, it is possible to avoid an abnormal value of the tilt angle calculated by the calculation means, and it is possible to improve the tilt angle calculation accuracy after the tilt angle detecting means returns from the saturated state.
本発明のうちの請求項3に記載の発明では、上記請求項1又は2に記載の発明において、前記傾斜角度検出手段が飽和状態でない場合には、前記演算手段に前記角速度検出手段の出力を入力し、前記傾斜角度検出手段が飽和状態である場合には、前記演算手段に前記角速度検出手段の出力の代わりに零を入力するように構成してある。
In the invention according to claim 3 of the present invention, in the invention according to
この構成によると、演算手段に対する入力の切り換えで、傾斜角度検出手段の出力が飽和するのに伴って、簡単かつ確実に、演算手段における角速度検出手段の出力に対する演算処理を停止させることができて、その演算値を固定することができ、又、傾斜角度検出手段の飽和状態からの復帰に伴って、簡単かつ確実に、演算手段における角速度検出手段の出力に対する演算処理を再開させることができて、その演算値を更新することができる。 According to this configuration, as the output of the tilt angle detecting means saturates when the input to the calculating means is switched, the arithmetic processing for the output of the angular velocity detecting means in the calculating means can be stopped easily and reliably. The calculation value can be fixed, and the calculation process for the output of the angular velocity detection means in the calculation means can be restarted easily and reliably with the return of the tilt angle detection means from the saturated state. The calculated value can be updated.
従って、構成の複雑化を招くことなく、演算手段における演算状態の切り換えを確実に行える。 Therefore, it is possible to reliably switch the calculation state in the calculation means without complicating the configuration.
図1には乗用形田植機の全体側面が、図2にはその全体平面が示されており、この田植機は、走行機体1の後部に、油圧式のリフトシリンダ2の作動で昇降揺動する平行リンク式のリンク機構3と、このリンク機構3の後端部に備えた縦リンク4に着脱可能に連結される連結フレーム5とを介して、苗植付装置(移動体Aの一例)6を駆動昇降可能に連結し、かつ、走行機体1の後部から苗植付装置6にわたる状態に施肥装置7を装備して構成されている。
FIG. 1 shows an overall side view of the riding type rice transplanter, and FIG. 2 shows an overall plan view thereof. This rice transplanter swings up and down by the operation of a
走行機体1は、その前部に搭載したエンジン8からの動力を、主変速装置として備えた静油圧式無段変速装置9や、ミッションケース10に内蔵したギヤ式の副変速装置(図示せず)などを介して、左右一対の前輪11及び後輪12に伝達する4輪駆動型に構成され、その前後中央に、左右の前輪11を操向するステアリングホイール13や運転座席14などを備えて搭乗運転部15が形成されている。
The
搭乗運転部15において、ステアリングホイール13の左方には、静油圧式無段変速装置9の前後進切り換え操作や無段変速操作を可能にする主変速レバー16が配備され、運転座席14の左方には、副変速装置の変速操作を可能にする副変速レバー17が配備され、運転座席14の右方には、苗植付装置6の昇降操作や苗植付装置6及び施肥装置7の作動切り換え操作を可能にする第1操作レバー18が配備され、ステアリングホイール13の右下方には、第1操作レバー18を「自動」位置に操作した状態において、苗植付装置6の昇降操作や苗植付装置6及び施肥装置7の作動切り換え操作を可能にする第2操作レバー19が配備されている。
In the
図1〜3に示すように、苗植付装置6は、連結フレーム5の下部に前後向きの軸心P1周りにローリング可能に軸支され、その下部に左右方向に所定間隔を隔てる状態で左右向きの軸心P2周りにピッチング自在に並設した複数の整地フロート20が、機体の走行に伴って圃場泥面を整地する一方で、ミッションケース10に内蔵した植付クラッチ(図示せず)を介して伝達される静油圧式無段変速装置9などによる変速後の作業用動力が動力分配機構21に伝達され、この動力分配機構21からの分配動力で、所定条数分のマット状苗を載置する苗載台22が所定ストロークで左右方向に往復移動し、又、左右方向に一定間隔を隔てる状態に並設した回転式の複数の植付機構23が、対応するマット状苗から所定量の苗を切り出して各整地フロート20による整地後の圃場泥土部に植え付け、更に、苗載台22が左右の各ストローク端に到達するごとに、左右方向に所定間隔を隔てる状態に並設した複数の縦送りベルト24が、対応するマット状苗を所定量だけ下方に向けて縦送りするように構成されている。
As shown in FIGS. 1 to 3, the
連結フレーム5の上部には、対応する左右の操作ワイヤ25や緩衝用の引きバネ26などを介して、苗載台22の左右に連係される回転体27と、この回転体27を正逆転駆動して、回転体27による左右の操作ワイヤ25の巻き取り量を調節することで、苗植付装置6を前後向きの軸心P1周りにローリング操作する電動式のローリングモータ28とが装備されている。そして、左右の引きバネ26の作用で、苗載台22の左右方向への往復移動にかかわらず、苗植付装置6の左右バランスの均衡を保つようになっている。
A rotating
図1に示すように、施肥装置7は、施肥クラッチ(図示せず)を介して伝達される静油圧式無段変速装置9などによる変速後の走行用動力で、走行機体1の後部に左右方向に並設した複数の繰出機構29が、それらの上部に連設したホッパー30に貯留した肥料を所定量ずつ繰り出し、その繰り出された肥料を、電動式の送風機31で生起される搬送風で、各繰出機構29から対応する案内ホース32を介して各整地フロート20に装備した作溝器33に搬送することで、苗植え付け箇所に隣接する圃場泥土内に所定量の肥料を供給するように構成されている。
As shown in FIG. 1, the
図4に示すように、第1操作レバー18は、「植付」「下降」「中立」「上昇」「自動」の各操作位置に揺動操作可能な揺動式で、それらの各操作位置への操作が、回転式のポテンショメータからなる第1レバーセンサ34によって検出される。第2操作レバー19は、中立位置から上下前後に操作可能な十字揺動式の中立復帰型で、その中立位置から上下前後への各操作が、複数のスイッチ又は多接点スイッチからなる第2レバーセンサ35によって検出される。第1レバーセンサ34及び第2レバーセンサ35は、それらの検出情報を、走行機体1に搭載されたマイクロコンピュータからなる制御装置36に出力する。
As shown in FIG. 4, the
制御装置36には、第1レバーセンサ34及び第2レバーセンサ35の出力以外に、リンク機構3の上下揺動角度を苗植付装置6の対機体高さとして検出する回転式のポテンショメータからなるリンクセンサ37や、左右中央に配置された整地フロート20のピッチング角度を苗植付装置6の接地高さとして検出する回転式のポテンショメータからなるフロートセンサ38、などの出力が入力され、制御装置36は、これらの入力情報に基づいて、リフトシリンダ3に対する作動油の流動状態を切り換える電磁式の昇降用制御弁39の作動、又は、植付クラッチ及び施肥クラッチの伝動状態を切り換える電動式のクラッチモータ40の作動を制御することで、苗植付装置6の昇降操作や苗植付装置6及び施肥装置7の作動切り換え操作を行う。
In addition to the outputs of the
詳述すると、制御装置36は、第1レバーセンサ34によって第1操作レバー18の「下降」位置への操作が検出されると、その検出が継続されている間は苗植付装置6の下降操作が行われるように昇降用制御弁39の作動を制御し、第1操作レバー18の「上昇」位置への操作が検出されると、その検出が継続されている間は苗植付装置6の上昇操作が行われるように昇降用制御弁39の作動を制御し、第1操作レバー18の「中立」位置への操作が検出されると、その検出が継続されている間は苗植付装置6の昇降操作が停止されるように昇降用制御弁39の作動を制御する。
Specifically, when the
この第1レバーセンサ34の出力に基づく昇降用制御弁39の作動制御においては、リンクセンサ37の出力と、その出力に対応させて予め設定した上限値及び下限値とを比較して、苗植付装置6の走行機体1に対する上限位置又は下限位置への到達を判別し、苗植付装置6の上昇操作時にリンクセンサ37の出力が上限値に達した場合には、苗植付装置6が走行機体1に対する上限位置に到達した判断して、苗植付装置6の上昇操作が停止されるように昇降用制御弁39の作動を制御する。又、苗植付装置6の下降操作時にリンクセンサ37の出力が下限値に達した場合には、苗植付装置6が走行機体1に対する下限位置に到達したと判断して、苗植付装置6の下降操作が停止されるように昇降用制御弁39の作動を制御する。
In the operation control of the
第1レバーセンサ34によって第1操作レバー18の「植付」位置への操作が検出されると、その検出が継続されている間は苗植付装置6及び施肥装置7が作動し、第1操作レバー18の「植付」位置への操作が検出されなくなると、その非検出状態が継続されている間は苗植付装置6及び施肥装置7が作動を停止するように、クラッチモータ40の作動を制御する。
When the operation of the
第1レバーセンサ34によって第1操作レバー18の「自動」位置への操作が検出されると、その検出が継続されている間は、第2レバーセンサ35の出力に基づいて昇降用制御弁39及びクラッチモータ40の作動を制御する。
When the
第1操作レバー18を「自動」位置に操作した状態において、第2レバーセンサ35によって第2操作レバー19の上方への操作が検出されると、リンクセンサ37の出力が上限値に達するまで苗植付装置6の上昇操作が行われ、リンクセンサ37の出力が上限値に達するのに伴って苗植付装置6の上昇操作が停止されるように、昇降用制御弁39の作動を制御する。
If the
逆に、第2レバーセンサ35によって第2操作レバー19の下方への操作が検出されると、フロートセンサ38の出力が、その出力に対応させて予め設定される目標値に達するまで、苗植付装置6の下降操作が行われ、フロートセンサ38の出力が目標値に達するのに伴って苗植付装置6の下降操作が停止されるように、昇降用制御弁39の作動を制御する。
On the contrary, when the
フロートセンサ38の出力が目標値に達した苗植付装置6の接地状態において、第2レバーセンサ35によって第2操作レバー19の下方への操作が検出されると、苗植付装置6及び施肥装置7が作動するようにクラッチモータ40の作動を制御し、又、フロートセンサ38の出力が目標値に維持されるように昇降用制御弁39の作動を制御する。
When the operation of the
この苗植付装置6及び施肥装置7の作動状態において、第2レバーセンサ35によって第2操作レバー19の上方への操作が検出されると、苗植付装置6及び施肥装置7が作動を停止するようにクラッチモータ40の作動を制御し、又、リンクセンサ37の出力が上限値に達するまで苗植付装置6の上昇操作が行われ、リンクセンサ37の出力が上限値に達するのに伴って苗植付装置6の上昇操作が停止されるように、昇降用制御弁39の作動を制御する。
In the operating state of the
尚、フロートセンサ38の目標値は、搭乗運転部15に備えた回転式のポテンショメータからなる第1設定器41の操作で変更できる。
Note that the target value of the
つまり、制御装置36には、第1操作レバー18の操作による任意の高さ位置への苗植付装置6の昇降操作を可能にし、かつ、第2操作レバー19の操作による予め設定された上限位置及び接地高さ位置への苗植付装置6の昇降操作を可能にする手動昇降制御手段42、苗植付装置6の高さ位置が予め設定された接地高さ位置に維持されるように苗植付装置6を昇降操作する自動昇降制御手段43、及び、第1操作レバー18又は第2操作レバー19の操作による苗植付装置6及び施肥装置7の作動切り換え操作を可能にする作動切換制御手段44、などが備えられている。
In other words, the
そして、作業走行時には、自動昇降制御手段43の制御作動によって、走行機体1のピッチングに関係なく、苗植付装置6の高さ位置を予め設定した接地高さに維持できる。
Then, during work travel, the height position of the
図3〜5に示すように、苗植付装置6には、その前後向きの軸心P1周りでのローリング角度を検出する傾斜角センサ(傾斜角度検出手段の一例)45と、その前後向きの軸心P1周りでの角速度を検出する角速度センサ(角速度検出手段の一例)46とが装備され、これらの各センサ45,46は、その検出値を制御装置36に出力する。
As shown in FIGS. 3 to 5, the
制御装置36には、それらの各センサ45,46の出力以外に、苗植付装置6の目標ローリング角度設定用として搭乗運転部15に備えた回転式のポテンショメータからなる第2設定器47の出力などが入力され、又、傾斜角センサ45の出力と角速度センサ46の出力とに基づいて苗植付装置6のローリング角度を演算する演算手段48、及び、第2設定器47の出力(設定値)と演算手段48の出力(演算値)とに基づいて、第2設定器47の出力に演算手段48の出力が一致するようにローリングモータ28の作動を制御することで、苗植付装置6のローリング角度が予め設定した目標ローリング角度に維持されるように苗植付装置6をローリング操作するローリング制御手段49が備えられている。つまり、制御装置36には傾斜角度計測装置としての機能が備えられている。
In addition to the outputs of the
尚、図示は省略するが、傾斜角センサ45には、苗植付装置6に固定される容器に封入した粘度の高いシリコンオイルなどの液面に対する容器の傾斜角度を苗植付装置6のローリング角度として電気的に検出する液面基準式のものが採用されている。一方、角速度センサ46には振動式のジャイロセンサが採用されている。
Although not shown in the drawings, the
液面基準式の傾斜角センサ45は、その構造上、苗植付装置6のローリング方向での絶対角度を検出できる反面、粘性の高い液体を使用するために応答性に劣り、又、苗植付装置6が激しくローリングする場合には、その影響を受けて液面が左右に横揺れすることがあり、この横揺れが生じると、その影響による誤差が出力の高周波数成分に含まれることになる。
The liquid level reference type
一方、角速度センサ46は、苗植付装置6が前後向きの軸心P1周りにローリングした際の角速度を検出するものであることから、その出力を積分処理すれば、そのローリングによる苗植付装置6のローリング角度の変化を高い応答性で精度良く得ることができる反面、苗植付装置6のローリング方向での絶対角度を検出することができない。
On the other hand, the
そこで、演算手段48には、傾斜角センサ45の出力から液面の横揺れの影響による誤差が含まれる高周波数成分を除去するローパスフィルタ50、角速度センサ46の出力を積分処理する積分器51、ローパスフィルタ50による除去成分を補う高周波数成分を積分器51の出力(積分値)から通過させるハイパスフィルタ52、及び、ローパスフィルタ50の出力とハイパスフィルタ52の出力とを合成する加算器53が備えられており、これによって、この演算手段48は、傾斜角センサ45の出力と角速度センサ46の出力とに基づいて、苗植付装置6のローリング角度を、その変化に対して優れた応答性で精度良く演算することができる。
Therefore, the calculation means 48 includes a low-
そして、この演算手段48の出力と第2設定器47の出力とに基づいて、ローリング制御手段49が、演算手段48の出力が第2設定器47の出力に一致するようにローリングモータ28の作動を制御することから、走行機体1のローリングや、そのローリングに起因した傾斜角センサ45における液面の横揺れにかかわらず、苗植付装置6のローリング角度を予め設定した目標ローリング角度に精度良く維持できる。
Then, based on the output of the calculation means 48 and the output of the
角速度センサ46には、その零点(基準電圧)が温度の変動などでドリフトする欠点があり、このドリフトが生じた場合には、角速度センサ46の検出精度が低下することになる。
The
そこで、図4及び図5に示すように、制御装置36には、温度などの影響でドリフトする角速度センサ46の零点を補正する零点補正手段54が備えられている。
Therefore, as shown in FIGS. 4 and 5, the
零点補正手段54は、ドリフトによる変動が角速度センサ46の本来の出力成分に比較して非常に低周波であることから、時定数の大きいローパスフィルタ55と、このローパスフィルタ55を通過した低周波数成分を角速度センサ46の出力から減算する減算器56とを備えて構成されている。
Since the fluctuation due to drift is very low frequency compared to the original output component of the
つまり、制御装置36に上記構成の零点補正手段54を備えることで、角速度センサ46におけるドリフトに起因した検出精度の低下を防止してある。
That is, by providing the
しかしながら、零点補正手段54において時定数の大きいローパスフィルタ55を採用すると、圃場耕盤の急激な変化や石などへの乗り上げなどによってローリング方向に非常に大きな角速度が生じた場合、その影響を零点補正手段54も受けることになって、その後の演算手段48によるローリング角度の演算に多少の誤差を生じさせることになり、その誤差は、ローパスフィルタ55の時定数が大きいために長時間にわたって生じることになる。
However, when the low-
その点を考慮して、零点補正手段54は、角速度センサ46の出力に基づいて、その値が予め設定した適正範囲内(角速度センサ46の仕様上において零点となり得る範囲内、又は、角速度センサ46の仕様上において零点となり得る範囲内で予め設定した範囲内)であるか否かを判別し、その出力が適正範囲内である場合には、角速度センサ46の出力をローパスフィルタ55に通過させて零点補正処理を行うことで零点の値を更新し、その出力が適正範囲外である場合には、角速度センサ46の出力をローパスフィルタ55に通過させないようにして零点補正処理を一時停止することで零点の値を固定するように構成されており、これによって、零点補正手段54に影響を与える大きな角速度が生じた場合であっても、その影響による誤差が、演算手段48によるローリング角度の演算において長時間にわたって生じる不都合の発生を軽減できる。
In consideration of this point, the zero
ところで、角速度センサ46において上述したドリフトが無かったとしても、角速度センサ46の出力を制御装置36に備えたAD変換器(図示せず)で変換する際の誤差や角速度センサ46の直進性により、AD変換後の値には一定値以下の誤差が含まれることになり、その値に基づいて零点補正処理を行うと、その誤差の分だけ零点が不適切になる。
By the way, even if the above-mentioned drift does not occur in the
そこで、制御装置36には、角速度センサ46の出力をAD変換した後に含まれる一定値以下の誤差を取り除くために、その誤差が含まれるAD変換後の値における下位の値(例えば下位の3つ分)を無視する不感帯処理を行う不感帯処理手段57が備えられており、これによって、全ての角速度におけるAD変換の誤差やセンサの非直線性誤差を取り除くことができ、零点補正手段54による零点補正をより適切に行えるようになり、結果、演算手段48によるローリング角度の演算をより精度良く行える。
Therefore, in order to remove an error of a certain value or less that is included after AD conversion of the output of the
尚、不感帯処理手段57で不感帯処理される下位の値は、AD変換器の補償などに応じて設定されるものであることから種々の変更が可能である。 Note that the lower value subjected to the dead band processing by the dead band processing means 57 is set according to compensation of the AD converter and the like, and thus various changes are possible.
角速度センサ46には、苗植付装置6のローリングだけでなくヨーイングやピッチングにも感応する他軸感度があり、特にこの他軸感度を顕著にする要因は旋回時に発生するヨーイング方向の角速度であることから、旋回時には、その他軸感度の影響による誤差が角速度センサ46の出力に含まれることになる。
The
又、その旋回時には、走行機体1がピッチングやローリングしながら旋回(ヨーイング)するため、そのときのヨーイング方向での角速度の分力がローリング方向の角速度として角速度センサ46の出力に含まれることになる。
At the time of turning, the traveling
つまり、旋回時には、そのときに発生するローリング方向以外での角速度の影響による誤差で角速度センサ46の出力がオフセットすることになる。
That is, at the time of turning, the output of the
そのため、旋回時に零点補正手段54が上述した零点補正処理(低周波数成分を零点とする処理)を行うと、零点補正手段54が真値からずれた値を零点とすることになり、結果、演算手段48によるローリング角度の演算精度が低下することになる。又、その旋回終了後にも、その影響が残ることで、苗植付装置6のローリング角度を予め設定した目標ローリング角度に維持することが難しくなる。
For this reason, when the zero point correction means 54 performs the above-described zero point correction process (a process in which the low frequency component is set to the zero point) during turning, the zero point correction means 54 sets a value that deviates from the true value as a zero point. The calculation accuracy of the rolling angle by the
そこで、制御装置36には、機体を直進又は略直進させる第1走行状態か機体を旋回させる第2走行状態かを判別する判別手段58が備えられており、零点補正手段54は、判別手段58によって第1走行状態であると判別された場合には零点補正処理を行って零点の値を更新し、第2走行状態であると判別された場合には零点補正処理を一時停止して零点の値を固定するように構成されている。
Therefore, the
つまり、零点補正手段54が、ローリング方向以外の角速度の影響が少ない第1走行状態においてのみ零点補正処理を行うことから、その影響が大きい第2走行状態においても零点補正処理を行うことに起因して発生する、零点補正手段54が真値からずれた値を零点とする不都合の発生を効果的に抑制することができ、その結果、直進時や旋回時にかかわらず、演算手段48によるローリング角度の演算を精度良く行えるとともに、苗植付装置6のローリング角度を予め設定した目標ローリング角度に精度良く維持することができる。
That is, since the zero point correction means 54 performs the zero point correction process only in the first traveling state where the influence of the angular velocity other than the rolling direction is small, the zero point correction process is also performed in the second traveling state where the influence is large. The zero point correction means 54 that occurs when the zero
尚、判別手段58は、ステアリングホイール13から左右の前輪11にわたるステアリング操作系におけるピットマンアーム(図示せず)の揺動操作量などを前輪11の切れ角として検出する回転式のポテンショメータからなる切角センサ59の出力に基づいて、前輪11の所定角度(例えば35度)未満の操向操作を検知している状態を第1走行状態と判別し、又、前輪11の所定角度(例えば35度)以上の操向操作を検知している状態を第2走行状態と判別するように構成されている。
The discriminating means 58 is a turning angle composed of a rotary potentiometer that detects the amount of swing operation of a pitman arm (not shown) in the steering operation system from the
又、第1走行状態か第2走行状態かの判別を、例えば、判別手段58が、フロートセンサ38の出力と切角センサ59の出力とに基づいて、整地フロート20の接地と前輪11の所定角度(例えば35度)未満の操向操作を検知している状態を第1走行状態と判別し、又、それ以外の例えば整地フロート20の浮上と前輪11の所定角度(例えば35度)以上の操向操作を検知している状態や、整地フロート20の接地と前輪11の所定角度(例えば35度)以上の操向操作を検知している状態、あるいは、整地フロート20の浮上と前輪11の所定角度(例えば35度)未満の操向操作を検知している状態、などを第2走行状態と判別するように構成してもよい。
Further, the determination means 58 determines whether the first traveling state or the second traveling state, for example, based on the output of the
傾斜角センサ45は、走行機体1の石への乗り上げなどに起因して苗植付装置6が想定しうる傾斜角度以上にローリングした場合に、その出力が飽和する(苗植付装置6のローリング角度を正確に検出しなくなる)ことがあり、この飽和状態においても、演算手段48が傾斜角センサ45の出力と角速度センサ46の出力とに基づいて苗植付装置6のローリング角度を演算する演算処理を継続すると、苗植付装置6のローリング角度の変化に対応しない傾斜角センサ45の出力と、苗植付装置6のローリング角度が変化する際の角速度に対応する角速度センサ46の出力とに基づいて、苗植付装置6のローリング角度を演算することになるために、図6の(ロ)に示すように、傾斜角センサ45の出力が飽和している間、演算手段48が苗植付装置6のローリング角度を正確に演算しなくなって、苗植付装置6の実際のローリング角度〔図6の(ロ)において仮想線で示すもの〕と演算手段48の演算で得られるローリング角度〔図6の(ロ)において実線で示すもの〕との間にずれが生じるだけでなく、傾斜角センサ45の飽和状態からの復帰後も、そのずれが定常偏差として長く残ることになって、演算手段48の演算で得られるローリング角度が異常値を示すことになり、時には、演算手段48の演算で得られるローリング角度が、実際のローリング角度と反対方向の値を示すこともある。
The
そのため、苗植付装置6のローリング角度を予め設定した目標ローリング角度に維持すること難しくなり、時には、予め設定した目標ローリング角度から離れる方向に苗植付装置6がローリング操作されることになる。
Therefore, it becomes difficult to maintain the rolling angle of the
尚、図6は、苗植付装置6のローリング角度が±2deg以上で傾斜角センサ45の出力が飽和する場合を示したものである。
FIG. 6 shows a case where the output of the
そこで、図4及び図5に示すように、制御装置36には、傾斜角センサ45の出力と、傾斜角センサ45の仕様に応じて予め設定した閾値(例えば±2deg)とに基づいて、傾斜角センサ45が飽和状態か否かを判別するとともに、傾斜角センサ45が飽和状態でない場合には、演算手段48に角速度センサ46の出力を入力し、傾斜角センサ45が飽和状態である場合には、演算手段48に角速度センサ46の出力の代わりに零を入力する入力切換手段60が備えられている。
Therefore, as shown in FIGS. 4 and 5, the
これによって、傾斜角センサ45の出力が飽和するまでの間(傾斜角センサ45の出力が閾値を超えていない間)は、演算手段48に、苗植付装置6のローリング角度に対応する傾斜角センサ45の出力と、ローリング方向での苗植付装置6の角速度に対応する角速度センサ46の出力とが入力され、演算手段48が、傾斜角センサ45の出力に対しては、ローパスフィルタ50によるフィルタ処理を行って、傾斜角センサ45の出力から液面の横揺れの影響による誤差が含まれる高周波数成分を除去する一方で、角速度センサ46の出力に対しては、積分器51による積分処理を行って、角速度センサ46の出力に基づく積分値(苗植付装置6のローリング角度変化)を更新し、その更新した積分値に対してハイパスフィルタ52によるフィルタ処理を行って、ローパスフィルタ50によるフィルタ処理で除去した高周波数成分を補う高周波数成分を通過させた後、ローパスフィルタ50の出力とハイパスフィルタ52の出力とに対して加算器53による加算処理を行って、それらの出力を合成するようになることから、図6の(イ)において仮想線で示すように、苗植付装置6のローリング角度を、その変化に対して優れた応答性で精度良く演算できるようになる。
Thus, until the output of the
つまり、傾斜角センサ45の出力が飽和するまでの間は、演算手段48によって優れた応答性で精度良く演算される苗植付装置6のローリング角度に基づくローリング制御手段49の制御作動で、苗植付装置6のローリング角度を予め設定した目標ローリング角度に精度良く維持できる。
That is, until the output of the
そして、傾斜角センサ45の出力が飽和すると(傾斜角センサ45の出力が閾値を超えると)、傾斜角センサ45の出力が飽和している間は、演算手段48に、その飽和によって一定値となって苗植付装置6のローリング角度に対応しなくなる傾斜角センサ45の出力と、角速度センサ46の出力の代わりに零が入力されることになり、演算手段48は、一定値となる傾斜角センサ45の出力に対してローパスフィルタ50によるフィルタ処理を行う。又、角速度センサ46の出力の代わりに入力される零に対して積分器51による積分処理を行うことから、角速度センサ46の出力に基づく積分値が、傾斜角センサ45の出力が飽和するとともに、飽和直前の積分処理で得た値に固定されることになる。そして、その固定状態の積分値に対してハイパスフィルタ52によるフィルタ処理を行った後、ローパスフィルタ50の出力とハイパスフィルタ52の出力とに対して加算器53による加算処理を行うことになる。そのため、図6の(イ)において実線で示すように、傾斜角センサ45の出力が飽和している間は、演算手段48で演算される苗植付装置6のローリング角度は、傾斜角センサ45の出力が飽和する閾値に応じた一定値となり、苗植付装置6の実際のローリング角度に対してずれが生じることになる。
When the output of the
つまり、傾斜角センサ45の出力が飽和している間は、苗植付装置6の実際のローリング角度と、演算手段48で演算される苗植付装置6のローリング角度との間でずれが生じるものの、演算手段48で演算される苗植付装置6のローリング角度が、傾斜角センサ45の出力が飽和する閾値に応じた一定値となることで、ローリング制御手段49は、その値から苗植付装置6のローリング操作方向を判別できることになり、結果、傾斜角センサ45の出力が飽和している間においても、演算手段48で演算される苗植付装置6のローリング角度に基づくローリング制御手段49の制御作動によって、苗植付装置6のローリング操作を適切に行える。
That is, while the output of the
その後、傾斜角センサ45が飽和状態から復帰すると、その復帰に伴って、演算手段48には、苗植付装置6のローリング角度に対応する傾斜角センサ45の出力と、ローリング方向での苗植付装置6の角速度に対応する角速度センサ46の出力とが入力されることになり、演算手段48が、その傾斜角センサ45の出力に対してローパスフィルタ50によるフィルタ処理を行う一方で、角速度センサ46の出力に対して積分器51による積分処理を行って、積分値を角速度センサ46の出力に基づいて更新し、その更新した積分値に対してハイパスフィルタ52によるフィルタ処理を行った後、ローパスフィルタ50の出力とハイパスフィルタ52の出力とに対して加算器53による加算処理を行うことから、図6の(イ)において仮想線で示すように、苗植付装置6のローリング角度を、その変化に対して優れた応答性で精度良く演算できるようになる。
Thereafter, when the
つまり、傾斜角センサ45の出力が飽和している間は、演算手段48に、角速度センサ46の出力の代わりに零を入力して積分器51からの積分値を固定し、傾斜角センサ45が飽和状態から復帰するのに伴って、演算手段48に、角速度センサ46の出力を入力して積分器51からの積分値を更新することで、傾斜角センサ45の出力が飽和している間に生じた、苗植付装置6の実際のローリング角度と、演算手段48で演算される苗植付装置6のローリング角度とのずれが、傾斜角センサ45の飽和状態からの復帰後も定常偏差として残ることを防止でき、傾斜角センサ45の飽和状態からの復帰後には、再び、苗植付装置6のローリング角度を、その変化に対して優れた応答性で精度良く演算できることから、傾斜角センサ45の飽和状態からの復帰後においても、演算手段48によって優れた応答性で精度良く演算される苗植付装置6のローリング角度に基づくローリング制御手段49の制御作動で、苗植付装置6のローリング角度を予め設定した目標ローリング角度に精度良く維持できる。
That is, while the output of the
〔別実施例〕
以下、本発明の別実施形態を列記する。
〔1〕移動体Aとしては、苗植付装置6以外の例えば耕耘装置などの対地作業装置であってもよく、又、それらの対地作業装置が連結される走行機体1、あるいは、コンバインなどの収穫作業車やバックホーなどの建設作業車、などであってもよい。
[Another Example]
Hereinafter, other embodiments of the present invention will be listed.
[1] The moving body A may be a ground work device such as a tillage device other than the
〔2〕角速度検出手段46として、機械式のジャイロセンサや光学式のジャイロセンサなどを採用してもよい。 [2] As the angular velocity detection means 46, a mechanical gyro sensor, an optical gyro sensor, or the like may be employed.
〔3〕傾斜角度検出手段45として、振子基準式の傾斜角センサや加速度センサなどを採用してもよい。
[3] As the tilt
〔4〕傾斜角度計測装置36としては、例えば、傾斜角度検出手段45の出力が飽和するのに伴って、演算手段48における角速度検出手段46の出力に対する積分処理を停止するとともに、その停止前に得た角速度検出手段46の出力に基づく積分値を記憶し、傾斜角度検出手段45の出力が飽和している間、その記憶した積分値と飽和している傾斜角度検出手段45の出力とに基づいて傾斜角度を演算し、傾斜角度検出手段45の飽和状態からの復帰に伴って、演算手段48における角速度検出手段46の出力に対する積分処理を再開して、傾斜角度検出手段45の出力と角速度検出手段46の出力とに基づく傾斜角度の演算を行うように構成されたものであってもよい。
[4] As the tilt
〔5〕演算手段48としては、移動体Aのピッチング角度やヨーイング角度を演算するものであってもよい。 [5] The calculating means 48 may calculate the pitching angle or yawing angle of the moving body A.
〔6〕演算手段48の構成としては種々の変更が可能であり、例えば、積分器51及びハイパスフィルタ52の代わりに、「積分器×ハイパスフィルタ」の伝達特性を有するローパスフィルタを備えるものであってもよく、又、角速度センサ46の零点補正処理などを行うように構成してもよい。
[6] Various changes can be made to the configuration of the calculation means 48. For example, instead of the
〔7〕演算手段48としては、図7に示すように、傾斜角度検出手段45の特性(時間遅れ要素)を考慮した演算を行うように構成しものであってもよく、又、図8に示すように、傾斜角度検出手段45の特性(時間遅れ要素)を考慮した演算を、フィルタ次数の低減による高速化を図りながら行うように構成しものであってもよく、更に、図9及び図10に示すように、傾斜角度検出手段45の特性(時間遅れ要素)を考慮した演算を、フィルタ次数の低減やフィルタ使用数の削減による高速化を図りながら行うように構成しものであってもよい。
[7] The calculation means 48 may be configured to perform calculation in consideration of the characteristics (time delay element) of the tilt angle detection means 45, as shown in FIG. As shown, the calculation considering the characteristic (time delay element) of the tilt angle detection means 45 may be performed while increasing the speed by reducing the filter order. As shown in FIG. 10, the calculation considering the characteristic (time delay element) of the tilt
ちなみに、図8に示す演算手段48の構成は、角速度検出手段46の出力から必要な高周波数成分を得るための伝達関数が、1/s×{1−1/〔(T1s+1)×(T2s+1)〕}である場合に、その時定数から適切に設定したゲインG1,G2を用いて、G1×1/(T1s+1)+G2×1/(T2s+1)という式に展開することで得た構成である。尚、このときの第1ゲインG1はT1^2/(T1−T2)、第2ゲインG2はT2^2/(T2−T1)である。 Incidentally, in the configuration of the calculation means 48 shown in FIG. 8, the transfer function for obtaining a necessary high frequency component from the output of the angular velocity detection means 46 is 1 / s × {1-1 / [(T1s + 1) × (T2s + 1). ], The gain G1 and G2 appropriately set from the time constants are used to develop the equation into G1 × 1 / (T1s + 1) + G2 × 1 / (T2s + 1). The first gain G1 at this time is T1 ^ 2 / (T1-T2), and the second gain G2 is T2 ^ 2 / (T2-T1).
又、図9に示す演算手段48の構成は、図8に示す演算手段48の構成において、第1ゲインG1と第2ゲインG2とを比較して、第1ゲインG1が第2ゲインG2に比べて無視できる程度のかなり小さい値であることが判明したことで得た構成である。 Further, the configuration of the calculation means 48 shown in FIG. 9 is the same as that of the calculation means 48 shown in FIG. 8, but the first gain G1 is compared with the second gain G2, and the first gain G1 is compared with the second gain G2. This is a configuration obtained by finding that the value is so small that it can be ignored.
更に、図10に示す演算手段48の構成は、傾斜角度検出手段の出力から高周波数成分を除去するローパスフィルタ50の特性が2次で、傾斜角度検出手段45の特性とローパスフィルタ50の特性を示す伝達関数が、1/(T1s+1)×(T2s+1)×(T3s+1)であり、角速度検出手段46の出力から必要な高周波数成分を得るための伝達関数が、1/s×{1−1/〔(T1s+1)×(T2s+1)×(T3s+1)〕}である場合に、その時定数から適切に設定したゲインG1,G2,G3を用いて、G1×1/(T1s+1)+G2×1/〔(T2s+1)×(T3s+1)〕+G3×1/(T3s+1)という式に展開し、又、各ゲインG1〜G3を比較して、第1ゲインG1が第2ゲインG2及び第3ゲインG3に比べて無視できる程度のかなり小さい値であることが判明したことで得た構成である。
Furthermore, configuration of the
A 移動体
45 傾斜角度検出手段
46 角速度検出手段
48 演算手段
A moving
Claims (3)
前記演算手段が、前記傾斜角度検出手段の出力から高周波数成分を除去する演算処理と、前記角速度検出手段の出力に対して積分処理を行なった後、この積分処理により得た積分値から、前記傾斜角度検出手段の出力から除去した前記高周波数成分を補う高周波数成分を得る演算処理と、前記傾斜角度検出手段の出力に対する演算処理で得た演算値に、前記角速度検出手段の出力に対する演算処理で得た演算値を加える演算処理とを行うことにより、前記移動体の傾斜角度を演算するように構成し、
予め設定した閾値に基づいて前記傾斜角度検出手段が飽和状態か否かを判別するとともに、前記傾斜角度検出手段が飽和状態でない場合には、前記演算手段が、前記演算処理のうちの前記角速度検出手段の出力に対する前記演算処理を行なうことにより、前記角速度検出手段の出力に基づく演算値を更新し、前記傾斜角度検出手段が飽和状態である場合には、前記演算手段が、前記演算処理のうちの前記角速度検出手段の出力に対する前記演算処理を停止することにより、前記角速度検出手段の出力に基づく演算値を固定するように構成してある傾斜角度計測装置。 Calculation means for calculating the inclination angle of the moving body based on the output of the inclination angle detecting means for detecting the inclination angle of the moving body and the output of the angular velocity detection means for detecting the angular velocity of the moving body in the inclination direction. An inclination angle measuring device comprising:
The calculation means performs calculation processing for removing a high frequency component from the output of the tilt angle detection means and integration processing for the output of the angular velocity detection means, and from the integration value obtained by this integration processing, Arithmetic processing for obtaining a high-frequency component that compensates for the high-frequency component removed from the output of the tilt angle detecting means, and an arithmetic processing for the output of the angular velocity detecting means to the arithmetic value obtained by the arithmetic processing for the output of the tilt angle detecting means And calculating the tilt angle of the moving body by performing the calculation process of adding the calculation value obtained in
Based on a preset threshold value, it is determined whether or not the tilt angle detection unit is in a saturated state, and when the tilt angle detection unit is not in a saturated state, the calculation unit detects the angular velocity detection in the calculation process. By performing the calculation process on the output of the means, the calculation value based on the output of the angular velocity detection means is updated, and when the tilt angle detection means is in a saturated state, the calculation means An inclination angle measuring device configured to fix the calculation value based on the output of the angular velocity detection means by stopping the calculation processing on the output of the angular velocity detection means.
前記演算手段が、前記傾斜角度検出手段の出力から高周波数成分を除去する演算処理と、前記角速度検出手段の出力に対して積分処理を行なった後、この積分処理により得た積分値から、前記傾斜角度検出手段の内部で除去される高周波数成分と、前記傾斜角度検出手段の出力から除去した前記高周波数成分とを補う高周波数成分を得る演算処理と、前記傾斜角度検出手段の出力に対する演算処理で得た演算値に、前記角速度検出手段の出力に対する演算処理で得た演算値を加える演算処理とを行うことにより、前記移動体の傾斜角度を演算するように構成し、
予め設定した閾値に基づいて前記傾斜角度検出手段が飽和状態か否かを判別するとともに、前記傾斜角度検出手段が飽和状態でない場合には、前記演算手段が、前記演算処理のうちの前記角速度検出手段の出力に対する前記演算処理を行なうことにより、前記角速度検出手段の出力に基づく演算値を更新し、前記傾斜角度検出手段が飽和状態である場合には、前記演算手段が、前記演算処理のうちの前記角速度検出手段の出力に対する前記演算処理を停止することにより、前記角速度検出手段の出力に基づく演算値を固定するように構成してある傾斜角度計測装置。 Calculation means for calculating the inclination angle of the moving body based on the output of the inclination angle detecting means for detecting the inclination angle of the moving body and the output of the angular velocity detection means for detecting the angular velocity of the moving body in the inclination direction. An inclination angle measuring device comprising:
The calculation means performs calculation processing for removing a high frequency component from the output of the tilt angle detection means and integration processing for the output of the angular velocity detection means, and from the integration value obtained by this integration processing, Arithmetic processing for obtaining a high-frequency component that compensates for the high-frequency component removed inside the tilt-angle detection means and the high-frequency component removed from the output of the tilt-angle detection means, and computation on the output of the tilt-angle detection means The calculation value obtained by the process is added to the calculation value obtained by the calculation process for the output of the angular velocity detection means, and the tilt angle of the moving body is calculated.
Based on a preset threshold value, it is determined whether or not the tilt angle detection unit is in a saturated state, and when the tilt angle detection unit is not in a saturated state, the calculation unit detects the angular velocity detection in the calculation process. By performing the calculation process on the output of the means, the calculation value based on the output of the angular velocity detection means is updated, and when the tilt angle detection means is in a saturated state, the calculation means An inclination angle measuring device configured to fix the calculation value based on the output of the angular velocity detection means by stopping the calculation processing on the output of the angular velocity detection means.
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
JP2005033170A JP4315917B2 (en) | 2005-02-09 | 2005-02-09 | Inclination angle measuring device |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
JP2005033170A JP4315917B2 (en) | 2005-02-09 | 2005-02-09 | Inclination angle measuring device |
Publications (2)
Publication Number | Publication Date |
---|---|
JP2006220489A JP2006220489A (en) | 2006-08-24 |
JP4315917B2 true JP4315917B2 (en) | 2009-08-19 |
Family
ID=36982918
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
JP2005033170A Expired - Fee Related JP4315917B2 (en) | 2005-02-09 | 2005-02-09 | Inclination angle measuring device |
Country Status (1)
Country | Link |
---|---|
JP (1) | JP4315917B2 (en) |
Cited By (1)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
JP2021106558A (en) * | 2019-12-27 | 2021-07-29 | 株式会社クボタ | Work vehicle and travel management system therefor |
Families Citing this family (3)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
KR100907826B1 (en) * | 2007-08-29 | 2009-07-14 | 한국전자통신연구원 | 3-D tilt angle calculation circuit |
CN113638735B (en) * | 2021-07-06 | 2023-03-28 | 中国地质大学(武汉) | Manual-automatic integrated non-electric connection drilling inclinometer and measuring method |
CN114402771B (en) * | 2022-01-07 | 2023-09-29 | 台州迪克机车工业有限公司 | Planting auxiliary equipment with planting pit detection function |
-
2005
- 2005-02-09 JP JP2005033170A patent/JP4315917B2/en not_active Expired - Fee Related
Cited By (2)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
JP2021106558A (en) * | 2019-12-27 | 2021-07-29 | 株式会社クボタ | Work vehicle and travel management system therefor |
JP7271416B2 (en) | 2019-12-27 | 2023-05-11 | 株式会社クボタ | Work vehicle and travel management system for work vehicle |
Also Published As
Publication number | Publication date |
---|---|
JP2006220489A (en) | 2006-08-24 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
US9624643B2 (en) | Blade tilt system and method for a work vehicle | |
JP6860387B2 (en) | Work platform | |
JP6420173B2 (en) | Automated traveling field work vehicle | |
JP4871002B2 (en) | Combine attitude control device | |
JP6371137B2 (en) | Planting field work machine | |
WO2017208306A1 (en) | Autonomous work vehicle | |
JP2007230387A (en) | Posture detector and posture control device of work vehicle | |
JP4315917B2 (en) | Inclination angle measuring device | |
JP2006220491A (en) | Apparatus for measuring slope angle | |
KR20120125964A (en) | Rice transplanter | |
WO2019030828A1 (en) | Work vehicle control system, method, and work vehicle | |
JP2018148858A (en) | Work vehicle | |
JP2019038535A (en) | Automatic travel field work vehicle | |
JP2019064542A (en) | Work vehicle | |
JP4315918B2 (en) | Inclination angle measuring device | |
KR101110939B1 (en) | Combine | |
JP2007238057A (en) | Attitude controller of working vehicle | |
JP4718256B2 (en) | Rolling control device for paddy field machine | |
JP5813365B2 (en) | Work vehicle | |
JP2018171073A (en) | Field work machine | |
JP3274352B2 (en) | Paddy field machine | |
JP5699036B2 (en) | Rice transplanter | |
JP2009148230A (en) | Working vehicle | |
JP3717411B2 (en) | Sensor zero correction device for work equipment | |
JP3838320B2 (en) | Horizontal control device |
Legal Events
Date | Code | Title | Description |
---|---|---|---|
A621 | Written request for application examination |
Free format text: JAPANESE INTERMEDIATE CODE: A621 Effective date: 20070328 |
|
A977 | Report on retrieval |
Free format text: JAPANESE INTERMEDIATE CODE: A971007 Effective date: 20080618 |
|
A131 | Notification of reasons for refusal |
Free format text: JAPANESE INTERMEDIATE CODE: A131 Effective date: 20080828 |
|
A521 | Written amendment |
Free format text: JAPANESE INTERMEDIATE CODE: A523 Effective date: 20081024 |
|
A131 | Notification of reasons for refusal |
Free format text: JAPANESE INTERMEDIATE CODE: A131 Effective date: 20090122 |
|
A521 | Written amendment |
Free format text: JAPANESE INTERMEDIATE CODE: A523 Effective date: 20090323 |
|
TRDD | Decision of grant or rejection written | ||
A01 | Written decision to grant a patent or to grant a registration (utility model) |
Free format text: JAPANESE INTERMEDIATE CODE: A01 Effective date: 20090507 |
|
A01 | Written decision to grant a patent or to grant a registration (utility model) |
Free format text: JAPANESE INTERMEDIATE CODE: A01 |
|
A61 | First payment of annual fees (during grant procedure) |
Free format text: JAPANESE INTERMEDIATE CODE: A61 Effective date: 20090519 |
|
R150 | Certificate of patent or registration of utility model |
Free format text: JAPANESE INTERMEDIATE CODE: R150 |
|
FPAY | Renewal fee payment (event date is renewal date of database) |
Free format text: PAYMENT UNTIL: 20120529 Year of fee payment: 3 |
|
LAPS | Cancellation because of no payment of annual fees |