JP4315918B2 - Inclination angle measuring device - Google Patents
Inclination angle measuring device Download PDFInfo
- Publication number
- JP4315918B2 JP4315918B2 JP2005033171A JP2005033171A JP4315918B2 JP 4315918 B2 JP4315918 B2 JP 4315918B2 JP 2005033171 A JP2005033171 A JP 2005033171A JP 2005033171 A JP2005033171 A JP 2005033171A JP 4315918 B2 JP4315918 B2 JP 4315918B2
- Authority
- JP
- Japan
- Prior art keywords
- output
- pass filter
- low
- tilt angle
- angular velocity
- 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
- Lifting Devices For Agricultural Implements (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 apparatus configured to perform calculation.
上記のような傾斜角度検出手段としては、移動体に固定される容器に封入した粘度の高いシリコンオイルなどの液面に対する容器の傾斜角度を移動体の傾斜角度として電気的に検出する液面基準式の傾斜角センサや、移動体に固定される容器の内部に装備した振子に対する容器の傾斜角度を移動体の傾斜角度として電気的に検出する振子基準式の傾斜角センサなどがある。これらの傾斜角度検出手段は、その構造上、その出力から移動体の絶対角度を得ることができる反面、振動に起因したノイズの発生を抑制するために応答性が悪く、又、移動体が激しく傾動する場合には、そのときの慣性の影響による誤差を出力の高周波数成分に含むことが知られている。 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. .
そこで、従来では、傾斜角度計測装置を、例えば、図5に示すように、傾斜角度検出手段の出力から慣性の影響による誤差が含まれる高周波数成分を除去するローパスフィルタ、その除去成分を補う高周波数成分を角速度検出手段の出力から得るための積分器とハイパスフィルタ、及び、その角速度検出手段の出力から得た高周波数成分とローパスフィルタから出力される低周波数成分とを合成する加算器、などから構成することで、移動体の傾斜角度を、その変化に対して優れた応答性で精度良く演算するようにしたものがある(例えば特許文献1〜3参照)。
ところで、傾斜角度検出手段である液面基準式の傾斜角センサや振子基準式の傾斜角センサなどは、本来よりノイズ対策用のローパスフィルタなどを備える構造上、その特性として時間遅れ要素を有するのであるが、上記の従来技術に基づく傾斜角度計測装置では、その傾斜角度検出手段の特性を考慮した演算を行うようには構成されていないために、移動体の傾斜角度が急激に変化する場合には移動体の傾斜角度を正しく演算することができない、などの不都合が生じるようになっていた。 By the way, the liquid level reference type tilt angle sensor and the pendulum reference type tilt angle sensor, which are the tilt angle detecting means, have a time delay element as a characteristic thereof because they have a low-pass filter for noise suppression. However, since the tilt angle measuring device based on the above-described prior art is not configured to perform calculation in consideration of the characteristics of the tilt angle detecting means, the tilt angle of the moving body changes rapidly. Inconveniences such as being unable to correctly calculate the tilt angle of the moving body have occurred.
そこで、上記従来構成の傾斜角度計測装置において、傾斜角度検出手段の特性を考慮した演算を行うように構成することが考えられるが、単純に傾斜角度検出手段の特性を考慮した演算を行うようにすると、例えば、図6に示すように、傾斜角度検出手段の特性を示す伝達関数が1/(T1s+1)であり、又、傾斜角度検出手段の出力から高周波数成分を除去するローパスフィルタの特性を示す伝達関数が1/(T2s+1)である場合、これらの特性による除去成分を補うのに必要な高周波数成分を角速度検出手段の出力から得るためには、1/sの伝達関数で示される特性を備えた積分器と、1−1/(T1s+1)×1/(T2s+1)の伝達関数で示される特性を備えた2次のハイパスフィルタを要することになる。 In view of the above, it is conceivable that the conventional tilt angle measuring apparatus is configured to perform calculations in consideration of the characteristics of the tilt angle detection means, but simply performs calculations in consideration of the characteristics of the tilt angle detection means. Then, for example, as shown in FIG. 6, the transfer function indicating the characteristics of the tilt angle detecting means is 1 / (T1s + 1), and the characteristics of the low-pass filter for removing the high frequency component from the output of the tilt angle detecting means are as follows. When the transfer function shown is 1 / (T2s + 1), in order to obtain a high-frequency component necessary to compensate for the removal component due to these characteristics from the output of the angular velocity detection means, the characteristic indicated by the 1 / s transfer function And a second-order high-pass filter having characteristics represented by a transfer function of 1-1 / (T1s + 1) × 1 / (T2s + 1).
つまり、上記従来構成の傾斜角度計測装置において、単純に傾斜角度検出手段の特性を考慮した演算を行うようにすると、ハイパスフィルタの次数が高くなるために、傾斜角度計測装置での演算量が増大して演算速度が低下することになる。 In other words, in the tilt angle measuring apparatus having the above-described conventional configuration, if the calculation is performed simply taking into account the characteristics of the tilt angle detecting means, the order of the high-pass filter increases, and the amount of calculation in the tilt angle measuring apparatus increases. As a result, the calculation speed decreases.
本発明の目的は、次数の高いフィルタを採用することによる演算速度の低下を招くことなく、傾斜角度検出手段の特性を考慮した精度の高い移動体の傾斜角度の演算を行えるようにすることにある。 An object of the present invention is to enable calculation of the tilt angle of a mobile object with high accuracy in consideration of the characteristics of the tilt angle detection means without causing a decrease in calculation speed due to the use of a high-order filter. is there.
本発明のうちの請求項1に記載の発明では、
移動体の傾斜角度を検出する傾斜角度検出手段の出力と、その傾斜方向での前記移動体の角速度を検出する角速度検出手段の出力とに基づいて、前記移動体の傾斜角度を演算するように構成した傾斜角度計測装置において、
前記傾斜角度検出手段の特性が、時定数をT1とする1次遅れ要素の伝達関数で表され、
前記傾斜角度検出手段の1次遅れ要素の伝達関数と等しい特性を有する第1ローパスフィルタと、前記傾斜角度検出手段の出力から高周波数成分を除去する第2ローパスフィルタとを備え、
前記第2ローパスフィルタの特性が、時定数をT2とする1次遅れ要素の伝達関数で表され、
前記第1ローパスフィルタの時定数T1と前記第2ローパスフィルタの時定数T2とから導き出した所定の第1関係式で算出される第1ゲインと、前記第1ローパスフィルタの時定数T1と前記第2ローパスフィルタの時定数T2とから導き出した所定の第2関係式で算出される第2ゲインとを設定し、
前記第1ローパスフィルタには、前記角速度検出手段の出力に前記第1ゲインを乗算した演算結果を入力し、
前記第2ローパスフィルタには、前記角速度検出手段の出力に前記第2ゲインを乗算した演算結果と前記傾斜角度検出手段の出力とを合成して得た演算結果を入力し、
前記第1ローパスフィルタの出力と前記第2ローパスフィルタの出力とを合成して、前記移動体の傾斜角度を演算するように構成してある。
In the invention according to
The tilt angle of the moving body is calculated based on the output of the tilt angle detecting means for detecting the tilt 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 tilt direction. In the configured tilt angle measuring device,
The characteristic of the tilt angle detecting means is represented by a transfer function of a first-order lag element having a time constant T1.
A first low-pass filter having a characteristic equal to a transfer function of a first-order lag element of the tilt angle detection means, and a second low-pass filter that removes a high-frequency component from the output of the tilt angle detection means,
The characteristic of the second low-pass filter is represented by a transfer function of a first-order lag element having a time constant T2.
A first gain calculated from a predetermined first relational expression derived from a time constant T1 of the first low-pass filter and a time constant T2 of the second low- pass filter; a time constant T1 of the first low-pass filter; A second gain calculated from a predetermined second relational expression derived from the time constant T2 of the two low-pass filter ,
The first low-pass filter receives a calculation result obtained by multiplying the output of the angular velocity detection means by the first gain,
The second low-pass filter receives a calculation result obtained by combining the calculation result obtained by multiplying the output of the angular velocity detection means by the second gain and the output of the tilt angle detection means,
The tilt angle of the movable body is calculated by synthesizing the output of the first low-pass filter and the output of the second low-pass filter.
単純に、傾斜角度検出手段の特性を考慮した演算を行う場合、例えば、その特性を示す伝達関数が1/(T1s+1)であり、又、傾斜角度検出手段の出力から高周波数成分を除去するローパスフィルタ(第2ローパスフィルタ)の特性を示す伝達関数が1/(T2s+1)であると、これらの特性による除去成分を補うのに必要な高周波数成分を角速度検出手段の出力から得るためには、1/sの伝達関数で示される特性を備えた積分器と、1−1/(T1s+1)×1/(T2s+1)の伝達関数で示される特性を備えた2次のハイパスフィルタとを要することになる。 When a calculation that simply considers the characteristics of the tilt angle detecting means is performed, for example, the transfer function indicating the characteristics is 1 / (T1s + 1), and a low-pass component that removes high frequency components from the output of the tilt angle detecting means. If the transfer function indicating the characteristics of the filter (second low-pass filter) is 1 / (T2s + 1), in order to obtain a high frequency component necessary to compensate for the removal component due to these characteristics from the output of the angular velocity detection means, It requires an integrator having a characteristic indicated by a transfer function of 1 / s and a second-order high-pass filter having a characteristic indicated by a transfer function of 1-1 / (T1s + 1) × 1 / (T2s + 1). Become.
つまり、角速度検出手段の出力から必要な高周波数成分を得るための伝達関数は、1/s×{1−1/〔(T1s+1)×(T2s+1)〕}となる。これは、その時定数から適切に設定したゲインG1,G2を用いると、G1×1/(T1s+1)+G2×1/(T2s+1)という式に展開することができる。 That is, the transfer function for obtaining a necessary high frequency component from the output of the angular velocity detection means is 1 / s × {1-1 / [(T1s + 1) × (T2s + 1)]}. This can be developed into an equation of G1 × 1 / (T1s + 1) + G2 × 1 / (T2s + 1) when gains G1 and G2 appropriately set from the time constant are used.
ちなみに、このときの第1ゲインG1はT1^2/(T1−T2)、第2ゲインG2はT2^2/(T2−T1)である。 Incidentally, the first gain G1 at this time is T1 ^ 2 / (T1-T2), and the second gain G2 is T2 ^ 2 / (T2-T1).
要するに、傾斜角度検出手段の出力から高周波数成分を除去するローパスフィルタ(第2ローパスフィルタ)とは別に、傾斜角度検出手段の特性と等価な特性を有するローパスフィルタ(第1ローパスフィルタ)を備え、この第1ローパスフィルタには、角速度検出手段の出力に第1ゲインG1を乗算した演算結果を入力し、第2ローパスフィルタには、角速度検出手段の出力に第2ゲインG2を乗算した演算結果と傾斜角度検出手段の出力とを合成して得た演算結果を入力し、第1ローパスフィルタの出力と第2ローパスフィルタの出力とを合成するようにすれば、次数の低いフィルタを採用しながらも、傾斜角度検出手段の特性と第2ローパスフィルタの特性による除去成分を補うのに必要な高周波数成分を角速度検出手段の出力から得ることができ、もって、移動体の傾斜角度を速い処理速度で精度良く演算できる。 In short, in addition to the low-pass filter (second low-pass filter) that removes high-frequency components from the output of the tilt angle detection means, a low-pass filter (first low-pass filter) having characteristics equivalent to the characteristics of the tilt angle detection means is provided. A calculation result obtained by multiplying the output of the angular velocity detection means by the first gain G1 is input to the first low-pass filter, and a calculation result obtained by multiplying the output of the angular velocity detection means by the second gain G2 is input to the second low-pass filter. By inputting the calculation result obtained by combining the output of the tilt angle detection means and combining the output of the first low-pass filter and the output of the second low-pass filter, while adopting a low-order filter The high frequency component necessary to compensate for the removal component due to the characteristics of the tilt angle detecting means and the characteristics of the second low-pass filter is obtained from the output of the angular velocity detecting means. It can be, it has been, can be accurately calculating the inclination angle of the moving object at a high processing speed.
従って、次数の低減が図られたフィルタ構成を採用して演算速度の低下を回避しながら、傾斜角度検出手段の特性を考慮した精度の高い移動体の傾斜角度の演算を行える。 Therefore, it is possible to calculate the tilt angle of the moving body with high accuracy in consideration of the characteristics of the tilt angle detecting means while adopting the filter configuration in which the order is reduced and avoiding a decrease in the calculation speed.
本発明のうちの請求項2に記載の発明では、
移動体の傾斜角度を検出する傾斜角度検出手段の出力と、その傾斜方向での前記移動体の角速度を検出する角速度検出手段の出力とに基づいて、前記移動体の傾斜角度を演算するように構成した傾斜角度計測装置において、
前記傾斜角度検出手段の特性が、時定数をT1とする1次遅れ要素の伝達関数で表され、
前記傾斜角度検出手段の出力から高周波数成分を除去するローパスフィルタを備え、
前記ローパスフィルタの特性が、時定数をT2とする1次遅れ要素の伝達関数で表され、
前記傾斜角度検出手段の時定数T1と前記ローパスフィルタの時定数T2とから導き出した所定の関係式で算出されるゲインを設定し、
前記ローパスフィルタに、前記角速度検出手段の出力にゲインを乗算した演算結果と前記傾斜角度検出手段の出力とを合成して得た演算結果を入力して、前記移動体の傾斜角度を演算するように構成してある。
In the invention according to
The tilt angle of the moving body is calculated based on the output of the tilt angle detecting means for detecting the tilt 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 tilt direction. In the configured tilt angle measuring device,
The characteristic of the tilt angle detecting means is represented by a transfer function of a first-order lag element having a time constant T1.
A low-pass filter for removing high frequency components from the output of the tilt angle detection means;
The characteristic of the low-pass filter is represented by a transfer function of a first-order lag element with a time constant T2.
Setting a gain calculated by a predetermined relational expression derived from the time constant T1 of the tilt angle detecting means and the time constant T2 of the low-pass filter;
The calculation result obtained by combining the calculation result obtained by multiplying the output of the angular velocity detection means by the gain and the output of the inclination angle detection means is input to the low-pass filter, and the inclination angle of the moving body is calculated. It is configured.
単純に、傾斜角度検出手段の特性を考慮した演算を行う場合、例えば、その特性を示す伝達関数が1/(T1s+1)であり、又、傾斜角度検出手段の出力から高周波数成分を除去するローパスフィルタ(第2ローパスフィルタ)の特性を示す伝達関数が1/(T2s+1)であると、これらの特性による除去成分を補うのに必要な高周波数成分を角速度検出手段の出力から得るためには、1/sの伝達関数で示される特性を備えた積分器と、1−1/(T1s+1)×1/(T2s+1)の伝達関数で示される特性を備えた2次のハイパスフィルタとを要することになる。 When a calculation that simply considers the characteristics of the tilt angle detecting means is performed, for example, the transfer function indicating the characteristics is 1 / (T1s + 1), and a low-pass component that removes high frequency components from the output of the tilt angle detecting means. If the transfer function indicating the characteristics of the filter (second low-pass filter) is 1 / (T2s + 1), in order to obtain a high frequency component necessary to compensate for the removal component due to these characteristics from the output of the angular velocity detection means, It requires an integrator having a characteristic indicated by a transfer function of 1 / s and a second-order high-pass filter having a characteristic indicated by a transfer function of 1-1 / (T1s + 1) × 1 / (T2s + 1). Become.
つまり、角速度検出手段の出力から必要な高周波数成分を得るための伝達関数は、1/s×{1−1/〔(T1s+1)×(T2s+1)〕}となる。これは、その時定数から適切に設定したゲインG1,G2を用いると、G1×1/(T1s+1)+G2×1/(T2s+1)という式に展開することができる。 That is, the transfer function for obtaining a necessary high frequency component from the output of the angular velocity detection means is 1 / s × {1-1 / [(T1s + 1) × (T2s + 1)]}. This can be developed into an equation of G1 × 1 / (T1s + 1) + G2 × 1 / (T2s + 1) when gains G1 and G2 appropriately set from the time constant are used.
ちなみに、このときの第1ゲインG1はT1^2/(T1−T2)、第2ゲインG2はT2^2/(T2−T1)である。 Incidentally, the first gain G1 at this time is T1 ^ 2 / (T1-T2), and the second gain G2 is T2 ^ 2 / (T2-T1).
そして、このときの第1ゲインG1と第2ゲインG2とを比較すると、第1ゲインG1は第2ゲインG2に比べて無視できる程度のかなり小さい値であることが判明する。その結果、ローパスフィルタに、角速度検出手段の出力にゲインG2を乗算した演算結果と傾斜角度検出手段の出力とを構成して得た演算結果を入力するようにすれば、次数の低いフィルタを採用し、かつ、フィルタの使用数を削減したフィルタ構成でありながら、傾斜角度検出手段の特性と、傾斜角度検出手段の出力から高周波数成分を除去するローパスフィルタの特性による除去成分を補うのに必要な高周波数成分を角速度検出手段の出力から得ることができ、もって、移動体の傾斜角度をより速い処理速度で精度良く演算できる。 Then, comparing the first gain G1 and the second gain G2 at this time, it is found that the first gain G1 is a considerably small value that is negligible compared to the second gain G2. As a result, a low-order filter can be adopted if the low-pass filter receives the calculation result obtained by multiplying the output of the angular velocity detection means by the gain G2 and the output of the tilt angle detection means. In addition, it is necessary to supplement the removal component due to the characteristics of the tilt angle detection means and the characteristics of the low-pass filter that removes high frequency components from the output of the tilt angle detection means, while the filter configuration reduces the number of filters used. A high frequency component can be obtained from the output of the angular velocity detection means, so that the tilt angle of the moving body can be calculated with high accuracy at a higher processing speed.
従って、次数の低減及び使用数の削減が図られたフィルタ構成を採用して演算速度の低下をより効果的に回避しながら、傾斜角度検出手段の特性を考慮した精度の高い移動体の傾斜角度の演算を行える。 Therefore, a highly accurate tilt angle of the moving body considering the characteristics of the tilt angle detection means while more effectively avoiding a decrease in calculation speed by adopting a filter configuration in which the order and the number of uses are reduced. Can be operated.
図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 traveling
搭乗運転部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 lift control valve 39 based on the output 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及び図4に示すように、苗植付装置6には、その前後向きの軸心P1周りでのローリング角度を検出する傾斜角センサ(傾斜角度検出手段の一例)45と、その前後向きの軸心P1周りでの角速度を検出する角速度センサ(角速度検出手段の一例)46とが装備され、これらの各センサ45,46は、その検出値を制御装置36に出力する。
As shown in FIGS. 3 and 4, 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
そこで、図5に示すように、演算手段48に、傾斜角センサ45の出力から液面の横揺れの影響による誤差が含まれる高周波数成分を除去するローパスフィルタ50、角速度センサ46の出力を積分処理する積分器51、ローパスフィルタ50による除去成分を補う高周波数成分を積分器51の出力(積分値)から通過させるハイパスフィルタ52、及び、ローパスフィルタ50の出力とハイパスフィルタ52の出力とを合成する加算器53を備えて、傾斜角センサ45の出力と角速度センサ46の出力とに基づいて、苗植付装置6のローリング角度を、その変化に対して優れた応答性で精度良く演算することが考えられる。
Therefore, as shown in FIG. 5, the
ところで、傾斜角センサ45には、1/(T1s+1)の伝達関数で示される特性を備えたノイズ対策用のローパスフィルタ(図示せず)が備えられている。
Incidentally, the
そのため、上記構成の演算手段48において、傾斜角センサ45の特性を考慮したより精度の高い演算を行う場合、図6に示すように、ローパスフィルタ50の特性を示す伝達関数を1/(T2s+1)とすると、これらの特性による除去成分を補うのに必要な高周波数成分を角速度センサ46の出力から得るためには、ハイパスフィルタ52として、1−1/(T1s+1)×1/(T2s+1)の伝達関数で示す特性を備えた2次のものを要することになり、演算手段48での演算量が増大して演算速度が低下することになる。
Therefore, when the calculation means 48 configured as described above performs a calculation with higher accuracy in consideration of the characteristics of the
ここで、除去成分を補う高周波数成分を得るための伝達関数について考察すると、その伝達関数は、1/s×{1−1/〔(T1s+1)×(T2s+1)〕}となるが、これは、その時定数から適切に設定したゲインG1,G2を用いると、G1×1/(T1s+1)+G2×1/(T2s+1)という式に展開することができる。 Here, considering a transfer function for obtaining a high-frequency component that compensates for the removed component, the transfer function is 1 / s × {1-1 / [(T1s + 1) × (T2s + 1)]}. If gains G1 and G2 appropriately set based on the time constant are used, the gain can be expanded to an expression of G1 × 1 / (T1s + 1) + G2 × 1 / (T2s + 1).
つまり、図7に示すように、傾斜角センサ45の出力から高周波数成分を除去する第2ローパスフィルタ(前出のローパスフィルタ)50とは別に、傾斜角センサ45の特性と等価な特性を有する第1ローパスフィルタ54を備え、この第1ローパスフィルタ54には、角速度センサ46の出力に第1ゲインG1を乗算した演算結果を入力し、又、第2ローパスフィルタ50には、角速度センサ46の出力に第2ゲインG2を乗算した演算結果と傾斜角センサ45の出力とを加算器55で合成して得た演算結果を入力し、そして、第1ローパスフィルタ54の出力と第2ローパスフィルタ50の出力とを加算器53で合成するように構成すれば、1次のフィルタ50,54を採用しながらも、傾斜角センサ45の特性と第2ローパスフィルタ50の特性による除去成分を補うのに必要な高周波数成分を角速度センサ46の出力から得ることができ、結果、苗植付装置6の傾斜角度を、速い処理速度でより精度良く演算できるようになる。
That is, as shown in FIG. 7, in addition to the second low-pass filter (previously described low-pass filter) 50 that removes a high-frequency component from the output of the
ちなみに、このときの第1ゲインG1はT1^2/(T1−T2)、第2ゲインG2はT2^2/(T2−T1)である。 Incidentally, the first gain G1 at this time is T1 ^ 2 / (T1-T2), and the second gain G2 is T2 ^ 2 / (T2-T1).
ところで、このときの第1ゲインG1と第2ゲインG2とを比較すると、第1ゲインG1は第2ゲインG2に比べて無視できる程度のかなり小さい値であることが判明する。つまり、図8に示すように、第2ローパスフィルタ50に、角速度センサ46の出力にゲインG2を乗算した演算結果と傾斜角センサ45の出力とを加算器55で合成して得た演算結果を入力するだけの構成に簡略化することができ、これによって、1次のフィルタ50を採用する上にフィルタの使用数を削減しながらも、傾斜角センサ45の特性と第2ローパスフィルタ50の特性による除去成分を補うのに必要な高周波数成分を角速度センサ46の出力から得ることができ、結果、コストの削減を図りながら、苗植付装置6の傾斜角度を、より速い処理速度で精度良く演算できることになる。
By the way, when the first gain G1 and the second gain G2 at this time are compared, it is found that the first gain G1 is a considerably small value that can be ignored compared to the second gain G2. That is, as shown in FIG. 8, the calculation result obtained by synthesizing the calculation result obtained by multiplying the output of the
そして、この演算手段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及び図7に示すように、制御装置36には、温度などの影響でドリフトする角速度センサ46の零点を補正する零点補正手段56が備えられている。
Therefore, as shown in FIGS. 4 and 7, the
零点補正手段56は、ドリフトによる変動が角速度センサ46の本来の出力成分に比較して非常に低周波であることから、時定数の大きいローパスフィルタ57と、このローパスフィルタ57を通過した低周波数成分を角速度センサ46の出力から減算する減算器58とを備えて構成されている。
Since the fluctuation due to drift is very low frequency compared to the original output component of the
つまり、制御装置36に上記構成の零点補正手段56を備えることで、角速度センサ46におけるドリフトに起因した検出精度の低下を防止してある。
That is, the
しかしながら、零点補正手段56において時定数の大きいローパスフィルタ57を採用すると、圃場耕盤の急激な変化や石などへの乗り上げなどによってローリング方向に非常に大きな角速度が生じた場合、その影響を零点補正手段56も受けることになって、その後の演算手段48によるローリング角度の演算に多少の誤差を生じさせることになり、その誤差は、ローパスフィルタ57の時定数が大きいために長時間にわたって生じることになる。
However, when the low-
その点を考慮して、零点補正手段56は、角速度センサ46の出力に基づいて、その値が予め設定した適正範囲内(角速度センサ46の仕様上において零点となり得る範囲内、又は、角速度センサ46の仕様上において零点となり得る範囲内で予め設定した範囲内)であるか否かを判別し、その出力が適正範囲内である場合には、角速度センサ46の出力をローパスフィルタ57に通過させて零点補正処理を行うことで零点の値を更新し、その出力が適正範囲外である場合には、角速度センサ46の出力をローパスフィルタ57に通過させないようにして零点補正処理を一時停止することで零点の値を固定するように構成されており、これによって、零点補正手段56に影響を与える大きな角速度が生じた場合であっても、その影響による誤差が、演算手段48によるローリング角度の演算において長時間にわたって生じる不都合の発生を軽減できる。
Considering 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つ分)を無視する不感帯処理を行う不感帯処理手段59が備えられており、これによって、全ての角速度におけるAD変換の誤差やセンサの非直線性誤差を取り除くことができ、零点補正手段56による零点補正をより適切に行えるようになり、結果、演算手段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
尚、不感帯処理手段59で不感帯処理される下位の値は、AD変換器の補償などに応じて設定されるものであることから種々の変更が可能である。 The lower value subjected to the dead zone processing by the dead zone processing means 59 is set in accordance with the compensation of the AD converter, and thus can be variously changed.
角速度センサ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
そのため、旋回時に零点補正手段56が上述した零点補正処理(低周波数成分を零点とする処理)を行うと、零点補正手段56が真値からずれた値を零点とすることになり、結果、演算手段48によるローリング角度の演算精度が低下することになる。又、その旋回終了後にも、その影響が残ることで、苗植付装置6のローリング角度を予め設定した目標ローリング角度に維持することが難しくなる。
For this reason, when the zero point correction means 56 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 56 sets the value deviated from the true value as a zero point. The calculation accuracy of the rolling angle by the
そこで、制御装置36には、機体を直進又は略直進させる第1走行状態か機体を旋回させる第2走行状態かを判別する判別手段60が備えられており、零点補正手段56は、判別手段60によって第1走行状態であると判別された場合には零点補正処理を行って零点の値を更新し、第2走行状態であると判別された場合には零点補正処理を一時停止して零点の値を固定するように構成されている。
Therefore, the
つまり、零点補正手段56が、ローリング方向以外の角速度の影響が少ない第1走行状態においてのみ零点補正処理を行うことから、その影響が大きい第2走行状態においても零点補正処理を行うことに起因して発生する、零点補正手段56が真値からずれた値を零点とする不都合の発生を効果的に抑制することができ、その結果、直進時や旋回時にかかわらず、演算手段48によるローリング角度の演算を精度良く行えるとともに、苗植付装置6のローリング角度を予め設定した目標ローリング角度に精度良く維持することができる。
That is, since the zero point correction means 56 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 56 that occurs when the zero
尚、判別手段60は、ステアリングホイール13から左右の前輪11にわたるステアリング操作系におけるピットマンアーム(図示せず)の揺動操作量などを前輪11の切れ角として検出する回転式のポテンショメータからなる切角センサ61の出力に基づいて、前輪11の所定角度(例えば35度)未満の操向操作を検知している状態を第1走行状態と判別し、又、前輪11の所定角度(例えば35度)以上の操向操作を検知している状態を第2走行状態と判別するように構成されている。
The discriminating means 60 is a turning angle comprising 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走行状態かの判別を、例えば、判別手段60が、フロートセンサ38の出力と切角センサ61の出力とに基づいて、整地フロート20の接地と前輪11の所定角度(例えば35度)未満の操向操作を検知している状態を第1走行状態と判別し、又、それ以外の例えば整地フロート20の浮上と前輪11の所定角度(例えば35度)以上の操向操作を検知している状態や、整地フロート20の接地と前輪11の所定角度(例えば35度)以上の操向操作を検知している状態、あるいは、整地フロート20の浮上と前輪11の所定角度(例えば35度)未満の操向操作を検知している状態、などを第2走行状態と判別するように構成してもよい。
Further, for example, the determination means 60 determines whether the first traveling state or the second traveling state is based on the output of the
〔別実施形態〕
以下、本発明の別実施形態を列記する。
〔1〕移動体Aとしては、苗植付装置6以外の例えば耕耘装置などの対地作業装置であってもよく、又、それらの対地作業装置が連結される走行機体1、あるいは、コンバインなどの収穫作業車やバックホーなどの建設作業車、などであってもよい。
[Another embodiment]
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〕傾斜角度検出手段45として、振子基準式の傾斜角センサや加速度センサなどを採用してもよい。
[2] As the tilt
〔3〕角速度検出手段46として、機械式のジャイロセンサや光学式のジャイロセンサなどを採用してもよい。 [3] As the angular velocity detection means 46, a mechanical gyro sensor or an optical gyro sensor may be employed.
〔4〕演算手段48としては、移動体Aのピッチング角度やヨーイング角度を演算するものであってもよい。 [4] The calculating means 48 may calculate the pitching angle or yawing angle of the moving body A.
〔5〕演算手段48の構成としては、傾斜角度検出手段45の特性や、演算手段48で採用するローパスフィルタ50の特性に応じて種々の変更が可能であり、例えば、図9に示すように、それらの特性を示す伝達関数が1/(T1s+1)×(T2s+1)×(T3s+1)で示される3次のものとなる場合には、これらの特性による除去成分を補うのに必要な高周波数成分を角速度検出手段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に比べて無視できる程度のかなり小さい値であることが判明し、これによって、1/(T2s+1)の伝達関数で示される特性を有するローパスフィルタ62に、角速度検出手段46の出力に第2ゲインG2を乗算した演算結果と傾斜角度検出手段45の出力とを加算器55で合成して得た演算結果を入力し、そのローパスフィルタ62の出力に、角速度検出手段46の出力に第3ゲインG3を乗算した演算結果を加算器63で合成した後、1/(T3s+1)の伝達関数で示される特性を有するローパスフィルタ64に入力するように構成すれば、1次のフィルタ62,64を採用し、かつ、フィルタの使用数を削減したフィルタ構成でありながら、傾斜角度検出手段45の特性とローパスフィルタの特性による除去成分を補うのに必要な高周波数成分を角速度検出手段の出力から得ることができ、結果、移動体Aの傾斜角度を精度良く演算できるようになる。
[5] The configuration of the calculation means 48 can be variously changed according to the characteristics of the inclination angle detection means 45 and the characteristics of the low-
45 傾斜角度検出手段
46 角速度検出手段
50 ローパスフィルタ
54 ローパスフィルタ
62 ローパスフィルタ
64 ローパスフィルタ
A 移動体
G1 ゲイン
G2 ゲイン
G3 ゲイン
45 Inclination angle detection means 46 Angular velocity detection means 50 Low-
Claims (2)
前記傾斜角度検出手段の特性が、時定数をT1とする1次遅れ要素の伝達関数で表され、
前記傾斜角度検出手段の1次遅れ要素の伝達関数と等しい特性を有する第1ローパスフィルタと、前記傾斜角度検出手段の出力から高周波数成分を除去する第2ローパスフィルタとを備え、
前記第2ローパスフィルタの特性が、時定数をT2とする1次遅れ要素の伝達関数で表され、
前記第1ローパスフィルタの時定数T1と前記第2ローパスフィルタの時定数T2とから導き出した所定の第1関係式で算出される第1ゲインと、前記第1ローパスフィルタの時定数T1と前記第2ローパスフィルタの時定数T2とから導き出した所定の第2関係式で算出される第2ゲインとを設定し、
前記第1ローパスフィルタには、前記角速度検出手段の出力に前記第1ゲインを乗算した演算結果を入力し、
前記第2ローパスフィルタには、前記角速度検出手段の出力に前記第2ゲインを乗算した演算結果と前記傾斜角度検出手段の出力とを合成して得た演算結果を入力し、
前記第1ローパスフィルタの出力と前記第2ローパスフィルタの出力とを合成して、前記移動体の傾斜角度を演算するように構成してある傾斜角度計測装置。 The tilt angle of the moving body is calculated based on the output of the tilt angle detecting means for detecting the tilt 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 tilt direction. A tilt angle measuring device configured,
The characteristic of the tilt angle detecting means is represented by a transfer function of a first-order lag element having a time constant T1.
A first low-pass filter having a characteristic equal to a transfer function of a first-order lag element of the tilt angle detection means, and a second low-pass filter that removes a high-frequency component from the output of the tilt angle detection means,
The characteristic of the second low-pass filter is represented by a transfer function of a first-order lag element having a time constant T2.
A first gain calculated from a predetermined first relational expression derived from a time constant T1 of the first low-pass filter and a time constant T2 of the second low-pass filter; a time constant T1 of the first low-pass filter; A second gain calculated from a predetermined second relational expression derived from the time constant T2 of the two low-pass filter,
The first low-pass filter receives a calculation result obtained by multiplying the output of the angular velocity detection means by the first gain,
The second low-pass filter receives a calculation result obtained by combining the calculation result obtained by multiplying the output of the angular velocity detection means by the second gain and the output of the tilt angle detection means,
An inclination angle measuring device configured to calculate the inclination angle of the moving body by combining the output of the first low-pass filter and the output of the second low-pass filter.
前記傾斜角度検出手段の特性が、時定数をT1とする1次遅れ要素の伝達関数で表され、
前記傾斜角度検出手段の出力から高周波数成分を除去するローパスフィルタを備え、
前記ローパスフィルタの特性が、時定数をT2とする1次遅れ要素の伝達関数で表され、
前記傾斜角度検出手段の時定数T1と前記ローパスフィルタの時定数T2とから導き出した所定の関係式で算出されるゲインを設定し、
前記ローパスフィルタに、前記角速度検出手段の出力にゲインを乗算した演算結果と前記傾斜角度検出手段の出力とを合成して得た演算結果を入力して、前記移動体の傾斜角度を演算するように構成してある傾斜角度計測装置。 The tilt angle of the moving body is calculated based on the output of the tilt angle detecting means for detecting the tilt 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 tilt direction. A tilt angle measuring device configured,
The characteristic of the tilt angle detecting means is represented by a transfer function of a first-order lag element having a time constant T1.
A low-pass filter for removing high frequency components from the output of the tilt angle detection means;
The characteristic of the low-pass filter is represented by a transfer function of a first-order lag element with a time constant T2.
Setting a gain calculated by a predetermined relational expression derived from the time constant T1 of the tilt angle detecting means and the time constant T2 of the low-pass filter;
The calculation result obtained by combining the calculation result obtained by multiplying the output of the angular velocity detection means by the gain and the output of the inclination angle detection means is input to the low-pass filter, and the inclination angle of the moving body is calculated. An inclination angle measuring device configured as described above.
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
JP2005033171A JP4315918B2 (en) | 2005-02-09 | 2005-02-09 | Inclination angle measuring device |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
JP2005033171A JP4315918B2 (en) | 2005-02-09 | 2005-02-09 | Inclination angle measuring device |
Publications (2)
Publication Number | Publication Date |
---|---|
JP2006220490A JP2006220490A (en) | 2006-08-24 |
JP4315918B2 true JP4315918B2 (en) | 2009-08-19 |
Family
ID=36982919
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
JP2005033171A Expired - Fee Related JP4315918B2 (en) | 2005-02-09 | 2005-02-09 | Inclination angle measuring device |
Country Status (1)
Country | Link |
---|---|
JP (1) | JP4315918B2 (en) |
Cited By (1)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
KR101364183B1 (en) * | 2012-08-01 | 2014-02-20 | 한국과학기술원 | Slope-angle sensor with accuracy in steady state and transient state |
Families Citing this family (2)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
JP4281777B2 (en) | 2006-10-05 | 2009-06-17 | トヨタ自動車株式会社 | Mobile object with tilt angle estimation mechanism |
JP5976841B2 (en) * | 2012-12-12 | 2016-08-24 | 富士機械製造株式会社 | Optical communication device and electronic component mounting device |
-
2005
- 2005-02-09 JP JP2005033171A patent/JP4315918B2/en not_active Expired - Fee Related
Cited By (1)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
KR101364183B1 (en) * | 2012-08-01 | 2014-02-20 | 한국과학기술원 | Slope-angle sensor with accuracy in steady state and transient state |
Also Published As
Publication number | Publication date |
---|---|
JP2006220490A (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 | |
JP4871002B2 (en) | Combine attitude control device | |
JP2007230387A (en) | Posture detector and posture control device of work vehicle | |
JP2016029913A (en) | Work vehicle | |
JP6811655B2 (en) | Work platform | |
JP2006220491A (en) | Apparatus for measuring slope angle | |
KR20120125964A (en) | Rice transplanter | |
WO2019030828A1 (en) | Work vehicle control system, method, and work vehicle | |
JP4315917B2 (en) | Inclination angle measuring device | |
JP4315918B2 (en) | Inclination angle measuring device | |
JP2019064542A (en) | Work vehicle | |
JP4718256B2 (en) | Rolling control device for paddy field machine | |
JP5813365B2 (en) | Work vehicle | |
JP2007238057A (en) | Attitude controller of working vehicle | |
JP6247145B2 (en) | Passenger rice transplanter | |
JP3274352B2 (en) | Paddy field machine | |
JP2011167202A (en) | Riding rice transplanter | |
JP5699036B2 (en) | Rice transplanter | |
JP2009148230A (en) | Working vehicle | |
JP6270677B2 (en) | Paddy field machine | |
JP3835079B2 (en) | Sensor zero error correction method | |
JP4718393B2 (en) | Agricultural machine rolling control device | |
JP4241738B2 (en) | Agricultural machine | |
JP4901667B2 (en) | Work vehicle attitude 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 |
|
FPAY | Renewal fee payment (event date is renewal date of database) |
Free format text: PAYMENT UNTIL: 20120529 Year of fee payment: 3 |
|
FPAY | Renewal fee payment (event date is renewal date of database) |
Free format text: PAYMENT UNTIL: 20130529 Year of fee payment: 4 |
|
FPAY | Renewal fee payment (event date is renewal date of database) |
Free format text: PAYMENT UNTIL: 20130529 Year of fee payment: 4 |
|
FPAY | Renewal fee payment (event date is renewal date of database) |
Free format text: PAYMENT UNTIL: 20140529 Year of fee payment: 5 |
|
LAPS | Cancellation because of no payment of annual fees |