JP4315918B2 - Inclination angle measuring device - Google Patents

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. .

Therefore, conventionally, as shown in FIG. 5, for example, as shown in FIG. 5, the inclination angle measuring device is a low-pass filter that removes a high-frequency component including an error due to the influence of inertia from the output of the inclination angle detection means, and a high-frequency that compensates for the removal component. An integrator and a high-pass filter for obtaining a frequency component from the output of the angular velocity detection means, an adder for synthesizing a high-frequency component obtained from the output of the angular velocity detection means and a low-frequency component output from the low-pass filter, etc. With this configuration, there is one in which the tilt angle of the moving object is calculated with high accuracy with excellent responsiveness to the change (see, for example, Patent Documents 1 to 3).
Japanese Patent No. 2696427 Japanese Patent No. 2926140 Japanese Patent No. 3479366

  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.

  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.

In the invention according to claim 1 of the present invention,
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.

  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.

  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.

  Incidentally, the first gain G1 at this time is T1 ^ 2 / (T1-T2), and the second gain G2 is T2 ^ 2 / (T2-T1).

  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.

In the invention according to claim 2 of the present invention,
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.

  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.

  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.

  Incidentally, the first gain G1 at this time is T1 ^ 2 / (T1-T2), and the second gain G2 is T2 ^ 2 / (T2-T1).

  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.

  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 hydraulic lift cylinder 2 at the rear of the traveling machine body 1. The seedling planting device (an example of the moving body A) through the parallel link type link mechanism 3 and the connecting frame 5 detachably connected to the vertical link 4 provided at the rear end of the link mechanism 3 6 is connected so as to be driven up and down, and a fertilizer application device 7 is provided in a state extending from the rear part of the traveling machine body 1 to the seedling planting device 6.

  The traveling machine body 1 includes a hydrostatic continuously variable transmission 9 having a power transmission from an engine 8 mounted on the front thereof as a main transmission, and a gear-type auxiliary transmission (not shown) built in a transmission case 10. ), Etc., and a steering wheel 13 that steers the left and right front wheels 11 and a driver seat 14 are provided at the front and rear centers. A boarding operation unit 15 is formed.

  In the boarding operation unit 15, a main transmission lever 16 that enables forward / reverse switching operation and continuously variable transmission operation of the hydrostatic continuously variable transmission 9 is provided on the left side of the steering wheel 13. On the other hand, a sub-transmission lever 17 is provided that enables the sub-transmission device to perform a shifting operation. A first operation lever 18 that enables an operation switching operation is provided, and in the lower right part of the steering wheel 13, when the first operation lever 18 is operated to the “automatic” position, The 2nd operation lever 19 which enables the operation switching operation of the seedling planting apparatus 6 and the fertilizer application apparatus 7 is provided.

  As shown in FIGS. 1 to 3, the seedling planting device 6 is pivotally supported at the lower part of the connecting frame 5 so as to be able to roll around the axial center P <b> 1 in the front-rear direction. A plurality of leveling floats 20 arranged in a freely pitchable manner around the axial axis P2 facing the ground level the field mud surface as the aircraft travels, while a planting clutch (not shown) built in the mission case 10 is provided. The working power after shifting by the hydrostatic continuously variable transmission 9 or the like transmitted to the power distribution mechanism 21 is transmitted to the power distribution mechanism 21, and the distributed power from the power distribution mechanism 21 generates a predetermined number of mat-shaped seedlings. A plurality of rotary planting mechanisms 23 arranged side by side so as to reciprocate in the left-right direction with a predetermined stroke and to be placed at a predetermined interval in the left-right direction are placed from the corresponding mat-like seedlings. Cut out a certain amount of seedlings and level each A plurality of vertical feed belts 24 that are planted in the field mud after leveling by the funnel 20 and are arranged in parallel with each other at predetermined intervals in the left-right direction each time the seedling stage 22 reaches the left and right stroke ends. A corresponding mat-like seedling is vertically fed downward by a predetermined amount.

  A rotating body 27 linked to the left and right of the seedling stage 22 via the corresponding left and right operation wires 25, buffering springs 26, and the like are provided on the upper portion of the connecting frame 5, and the rotating body 27 is driven forward and backward. Then, by adjusting the winding amount of the left and right operation wires 25 by the rotator 27, an electric rolling motor 28 for rolling the seedling planting device 6 around the longitudinal axis P1 is equipped. Yes. The action of the left and right pulling springs 26 keeps the balance of the right and left balance of the seedling planting device 6 regardless of the reciprocating movement of the seedling mounting table 22 in the left and right direction.

  As shown in FIG. 1, the fertilizer application device 7 is a motive power after shifting by a hydrostatic continuously variable transmission 9 transmitted via a fertilization clutch (not shown), A plurality of feeding mechanisms 29 arranged in parallel in the direction feed out the fertilizer stored in the hopper 30 connected to the upper portion thereof by a predetermined amount, and the fed fertilizer is transported by the electric blower 31. In addition, a predetermined amount of fertilizer is supplied into the field mud adjacent to the seedling planting site by transporting from each feeding mechanism 29 to the grooving device 33 equipped in each leveling float 20 via the corresponding guide hose 32. It is configured.

  As shown in FIG. 4, the first operation lever 18 is a swing type that can swing to each of the “planting”, “down”, “neutral”, “up”, and “automatic” operation positions. Is detected by a first lever sensor 34 comprising a rotary potentiometer. The second operating lever 19 is a cross swing neutral return type that can be operated up and down and forward and backward from the neutral position, and each operation from the neutral position up and down and forward and backward is a second lever that includes a plurality of switches or multi-contact switches. Detected by sensor 35. The first lever sensor 34 and the second lever sensor 35 output their detection information to a control device 36 composed of a microcomputer mounted on the traveling machine body 1.

  In addition to the outputs of the first lever sensor 34 and the second lever sensor 35, the control device 36 includes a rotary potentiometer that detects the vertical swing angle of the link mechanism 3 as the height of the seedling planting device 6 with respect to the machine body. The outputs of the link sensor 37 and the float sensor 38 including a rotary potentiometer that detects the pitching angle of the leveling float 20 disposed at the center of the right and left as the ground contact height of the seedling planting device 6 are input, and the control device 36 Is an electric clutch motor that switches the operation state of the electromagnetic lift control valve 39 that switches the flow state of the hydraulic oil to the lift cylinder 3 or the transmission state of the planting clutch and the fertilizer clutch based on the input information. By controlling the operation of 40, the raising / lowering operation of the seedling planting device 6 and the operation switching operation of the seedling planting device 6 and the fertilizer application device 7 are performed.

  Specifically, when the first lever sensor 34 detects the operation of the first operating lever 18 to the “down” position, the control device 36 lowers the seedling planting device 6 while the detection continues. When the operation of the raising / lowering control valve 39 is controlled so that the operation is performed, and the operation of the first operating lever 18 to the “up” position is detected, the seedling planting device 6 is maintained while the detection is continued. When the operation of the first control lever 18 to the “neutral” position is detected by controlling the operation of the lifting control valve 39 so that the lifting operation is performed, seedling planting is performed while the detection is continued. The operation of the lifting control valve 39 is controlled so that the lifting operation of the device 6 is stopped.

  In the operation control of the lift control valve 39 based on the output of the first lever sensor 34, the output of the link sensor 37 is compared with an upper limit value and a lower limit value set in advance corresponding to the output, and the seedling planting is compared. When reaching the upper limit position or the lower limit position of the attaching device 6 with respect to the traveling machine body 1 and the output of the link sensor 37 reaches the upper limit value during the raising operation of the seedling planting device 6, the seedling planting device 6 It is determined that the upper limit position with respect to the traveling machine body 1 has been reached, and the operation of the lifting control valve 39 is controlled so that the raising operation of the seedling planting device 6 is stopped. When the output of the link sensor 37 reaches the lower limit value when the seedling planting device 6 is lowered, the seedling planting device 6 determines that the seedling planting device 6 has reached the lower limit position with respect to the traveling machine body 1. The operation of the elevating control valve 39 is controlled so that the lowering operation is stopped.

  When the operation of the first operation lever 18 to the “planting” position is detected by the first lever sensor 34, the seedling planting device 6 and the fertilizer application device 7 operate while the detection continues, and the first lever sensor 34 operates. When the operation to the “planting” position of the operation lever 18 is not detected, the clutch motor 40 is operated so that the seedling planting device 6 and the fertilizer application device 7 stop operating while the non-detection state is continued. Control the operation.

  When the first lever sensor 34 detects the operation of the first operating lever 18 to the “automatic” position, the lift control valve 39 is based on the output of the second lever sensor 35 while the detection continues. And the operation of the clutch motor 40 is controlled.

  If the second lever sensor 35 detects an upward operation of the second operation lever 19 in a state where the first operation lever 18 is operated to the “automatic” position, the seedling is continued until the output of the link sensor 37 reaches the upper limit value. The raising and lowering control valve 39 is controlled so that the raising operation of the seedling planting device 6 is stopped as the raising operation of the planting device 6 is performed and the output of the link sensor 37 reaches the upper limit value. .

  On the contrary, when the second lever sensor 35 detects the downward operation of the second operation lever 19, the seedling planting is performed until the output of the float sensor 38 reaches a preset target value corresponding to the output. The lowering operation of the attaching device 6 is performed, and the operation of the lifting control valve 39 is controlled so that the lowering operation of the seedling planting device 6 is stopped as the output of the float sensor 38 reaches the target value.

  When the operation of the second operation lever 19 downward is detected by the second lever sensor 35 in the ground contact state of the seedling planting device 6 where the output of the float sensor 38 has reached the target value, the seedling planting device 6 and the fertilizer application The operation of the clutch motor 40 is controlled so that the device 7 operates, and the operation of the elevating control valve 39 is controlled so that the output of the float sensor 38 is maintained at a target value.

  In the operating state of the seedling planting device 6 and the fertilizer application device 7, when the second lever sensor 35 detects an upward operation of the second operation lever 19, the seedling planting device 6 and the fertilizer application device 7 stop operating. The operation of the clutch motor 40 is controlled so that the raising operation of the seedling planting device 6 is performed until the output of the link sensor 37 reaches the upper limit value, and the output of the link sensor 37 reaches the upper limit value. Then, the operation of the lifting control valve 39 is controlled so that the raising operation of the seedling planting device 6 is stopped.

  Note that the target value of the float sensor 38 can be changed by operating the first setting device 41 including a rotary potentiometer provided in the boarding operation unit 15.

  In other words, the control device 36 allows the raising / lowering operation of the seedling planting device 6 to an arbitrary height position by the operation of the first operation lever 18 and a preset upper limit by the operation of the second operation lever 19. Manual raising / lowering control means 42 enabling the raising / lowering operation of the seedling planting device 6 to the position and the ground contact height position, so that the height position of the seedling planting device 6 is maintained at a preset ground height position. Automatic raising / lowering control means 43 for raising / lowering the seedling planting device 6 and operation switching for enabling the operation switching operation of the seedling planting device 6 and the fertilizer application device 7 by the operation of the first operation lever 18 or the second operation lever 19. Control means 44, etc. are provided.

  Then, during work travel, the height position of the seedling planting device 6 can be maintained at a preset ground contact height regardless of the pitching of the traveling machine body 1 by the control operation of the automatic lifting control means 43.

  As shown in FIGS. 3 and 4, the seedling planting device 6 includes an inclination angle sensor (an example of an inclination angle detection means) 45 that detects a rolling angle around the axis P1 in the front-rear direction, and a front-rear direction thereof. An angular velocity sensor (an example of an angular velocity detecting means) 46 that detects an angular velocity around the axis P1 is provided, and each of these sensors 45 and 46 outputs the detected value to the control device 36.

  In addition to the outputs of the sensors 45 and 46, the control device 36 includes an output of a second setter 47 including a rotary potentiometer provided in the boarding operation unit 15 for setting a target rolling angle of the seedling planting device 6. , Etc., and the calculation means 48 for calculating the rolling angle of the seedling planting device 6 based on the output of the tilt angle sensor 45 and the output of the angular velocity sensor 46, and the output (set value) of the second setter 47 ) And the output (computed value) of the computing means 48, the operation of the rolling motor 28 is controlled so that the output of the computing means 48 matches the output of the second setting device 47, so that the seedling planting device 6 The rolling control means 49 for rolling the seedling planting device 6 is provided so that the rolling angle is maintained at a preset target rolling angle. That is, the control device 36 has a function as an inclination angle measuring device.

  Although not shown in the drawings, the inclination angle sensor 45 determines the inclination angle of the container with respect to the liquid surface such as high viscosity silicone oil sealed in the container fixed to the seedling planting apparatus 6. A liquid level reference type that is electrically detected as an angle is employed. On the other hand, a vibration type gyro sensor is employed as the angular velocity sensor 46.

  The liquid level reference type tilt angle sensor 45 can detect the absolute angle in the rolling direction of the seedling planting device 6 due to its structure, but has poor response due to the use of a highly viscous liquid. When the attachment device 6 rolls violently, the liquid surface may roll from side to side due to the influence, and when this roll occurs, an error due to the influence is included in the high frequency component of the output. Become.

  On the other hand, the angular velocity sensor 46 detects the angular velocity when the seedling planting device 6 rolls around the axial center P1 in the front-rear direction. Therefore, if the output is integrated, the seedling planting device by rolling is used. 6 can be obtained with high responsiveness and high accuracy, but the absolute angle of the seedling planting device 6 in the rolling direction cannot be detected.

  Therefore, as shown in FIG. 5, the calculation unit 48 integrates the output of the low-pass filter 50 that removes the high-frequency component including the error due to the influence of the roll of the liquid surface from the output of the tilt angle sensor 45 and the output of the angular velocity sensor 46. The integrator 51 to be processed, the high-pass filter 52 that passes the high-frequency component that compensates for the component removed by the low-pass filter 50 from the output (integrated value) of the integrator 51, and the output of the low-pass filter 50 and the output of the high-pass filter 52 are combined. An adder 53 for calculating the rolling angle of the seedling planting device 6 with high responsiveness to the change with high accuracy based on the output of the inclination angle sensor 45 and the output of the angular velocity sensor 46. Can be considered.

  Incidentally, the tilt angle sensor 45 is provided with a low-pass filter (not shown) for noise countermeasures having a characteristic indicated by a transfer function of 1 / (T1s + 1).

  Therefore, when the calculation means 48 configured as described above performs a calculation with higher accuracy in consideration of the characteristics of the tilt angle sensor 45, the transfer function indicating the characteristics of the low-pass filter 50 is 1 / (T2s + 1) as shown in FIG. Then, in order to obtain from the output of the angular velocity sensor 46 a high frequency component necessary to compensate for the removal component due to these characteristics, transmission of 1-1 / (T1s + 1) × 1 / (T2s + 1) is performed as the high-pass filter 52. A secondary one having the characteristics indicated by the function is required, and the calculation amount in the calculation means 48 increases and the calculation speed decreases.

  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).

  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 tilt angle sensor 45, the tilt angle sensor 45 has a characteristic equivalent to the characteristics. A first low-pass filter 54 is provided, and the first low-pass filter 54 is inputted with a calculation result obtained by multiplying the output of the angular velocity sensor 46 by the first gain G1, and the second low-pass filter 50 is inputted with the angular velocity sensor 46. The calculation result obtained by combining the calculation result obtained by multiplying the output by the second gain G2 and the output of the tilt angle sensor 45 by the adder 55 is input, and the output of the first low-pass filter 54 and the second low-pass filter 50 are input. If the output signal is combined by the adder 53, the characteristics of the inclination angle sensor 45 and the second low-pass filter are adopted while the primary filters 50 and 54 are employed. A high frequency component necessary to compensate for the removal component due to the characteristic of 0 can be obtained from the output of the angular velocity sensor 46, and as a result, the inclination angle of the seedling planting device 6 can be calculated more accurately at a high processing speed. Become.

  Incidentally, the first gain G1 at this time is T1 ^ 2 / (T1-T2), and the second gain G2 is T2 ^ 2 / (T2-T1).

  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 angular velocity sensor 46 by the gain G2 and the output of the inclination angle sensor 45 by the adder 55 to the second low-pass filter 50 is obtained. It is possible to simplify the configuration to only input, thereby adopting the primary filter 50 and reducing the number of filters used, but also the characteristics of the tilt angle sensor 45 and the characteristics of the second low-pass filter 50. The high-frequency component necessary to compensate for the removal component due to can be obtained from the output of the angular velocity sensor 46. As a result, the inclination angle of the seedling planting device 6 can be accurately adjusted at a higher processing speed while reducing the cost. It can be calculated.

  Then, based on the output of the calculation means 48 and the output of the second setting device 47, the rolling control means 49 operates the rolling motor 28 so that the output of the calculation means 48 matches the output of the second setting device 47. Therefore, the rolling angle of the seedling planting device 6 is set to a preset target rolling angle regardless of the rolling of the traveling machine body 1 and the rolling of the liquid level in the tilt angle sensor 45 caused by the rolling. High accuracy and high responsiveness can be maintained.

  The angular velocity sensor 46 has a drawback that its zero point (reference voltage) drifts due to a change in temperature or the like. If this drift occurs, the detection accuracy of the angular velocity sensor 46 is lowered.

  Therefore, as shown in FIGS. 4 and 7, the control device 36 is provided with zero point correction means 56 for correcting the zero point of the angular velocity sensor 46 drifting due to the influence of temperature or the like.

  Since the fluctuation due to drift is very low frequency compared to the original output component of the angular velocity sensor 46, the zero point correction means 56 has a low-pass filter 57 having a large time constant and a low-frequency component that has passed through the low-pass filter 57. Is subtracted from the output of the angular velocity sensor 46.

  That is, the control device 36 includes the zero point correcting means 56 having the above-described configuration, thereby preventing a reduction in detection accuracy due to drift in the angular velocity sensor 46.

  However, when the low-pass filter 57 having a large time constant is employed in the zero point correction means 56, if a very large angular velocity is generated in the rolling direction due to a rapid change in the field cultivator or climbing on a stone or the like, the influence is zero-corrected. The means 56 is also received, which causes some errors in the subsequent calculation of the rolling angle by the calculating means 48, and the errors occur over a long time due to the large time constant of the low-pass filter 57. Become.

  Considering this point, the zero point correcting means 56 is based on the output of the angular velocity sensor 46, and the value is within a preset appropriate range (a range that can be a zero point in the specification of the angular velocity sensor 46, or the angular velocity sensor 46). The output of the angular velocity sensor 46 is passed through the low-pass filter 57 if the output is within the appropriate range. The zero value is updated by performing the zero correction process, and when the output is out of the proper range, the zero correction process is temporarily stopped by preventing the output of the angular velocity sensor 46 from passing through the low-pass filter 57. Even if a large angular velocity that affects the zero point correction means 56 occurs due to this, the error of the zero point is fixed. But it can reduce the occurrence of inconvenience that occurs over a long period of time in the calculation of the rolling angle by the calculating means 48.

  By the way, even if the above-mentioned drift does not occur in the angular velocity sensor 46, due to an error in converting the output of the angular velocity sensor 46 by an AD converter (not shown) included in the control device 36 and the straightness of the angular velocity sensor 46, The value after AD conversion includes an error equal to or less than a certain value. If zero point correction processing is performed based on the value, the zero point becomes inappropriate by the amount of the error.

  Therefore, in order to remove an error of a certain value or less that is included after AD conversion of the output of the angular velocity sensor 46, the control device 36 has a lower value (for example, the lower three values) after the AD conversion that includes the error. A dead zone processing means 59 for performing dead zone processing ignoring (min)) is provided, whereby AD conversion errors and sensor non-linearity errors at all angular velocities can be removed, and zero correction by the zero correction means 56 is performed. As a result, the calculation of the rolling angle by the calculation means 48 can be performed with higher accuracy.

  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.

  The angular velocity sensor 46 has other axis sensitivity that is sensitive not only to rolling of the seedling planting device 6 but also to yawing and pitching. In particular, the factor that makes this other axis sensitivity remarkable is the angular velocity in the yawing direction that occurs during turning. Therefore, at the time of turning, an error due to the influence of other axis sensitivity is included in the output of the angular velocity sensor 46.

  At the time of turning, the traveling machine body 1 turns (yaws) while pitching or rolling, and the angular velocity component in the yawing direction at that time is included in the output of the angular velocity sensor 46 as the angular velocity in the rolling direction. .

  That is, at the time of turning, the output of the angular velocity sensor 46 is offset by an error due to the influence of the angular velocity other than the rolling direction that occurs at that time.

  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 means 48 is lowered. Moreover, even after the turn is finished, the influence remains, and it becomes difficult to maintain the rolling angle of the seedling planting device 6 at a preset target rolling angle.

  Therefore, the control device 36 is provided with a discriminating means 60 for discriminating between the first traveling state in which the airframe moves straight or substantially straight and the second traveling state in which the airframe turns, and the zero point correcting means 56 is provided with the discriminating means 60. When it is determined that the vehicle is in the first running state, the zero correction process is performed to update the zero value, and when it is determined that the vehicle is in the second driving state, the zero correction process is temporarily stopped and the zero point correction is performed. It is configured to fix the value.

  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 point correction value 56 deviates from the true value can be effectively suppressed. As a result, the rolling angle of the calculation means 48 can be controlled regardless of whether the vehicle travels straight or turns. The calculation can be performed with high accuracy, and the rolling angle of the seedling planting device 6 can be maintained at a preset target rolling angle with high accuracy.

  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 steering wheel 13 to the left and right front wheels 11 as the turning angle of the front wheels 11. Based on the output of the sensor 61, a state in which a steering operation less than a predetermined angle (for example, 35 degrees) of the front wheel 11 is detected is determined as the first traveling state, and a predetermined angle (for example, 35 degrees) of the front wheel 11 is determined. The state in which the above steering operation is detected is determined to be the second traveling state.

  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 float sensor 38 and the output of the cutting angle sensor 61, and the grounding float 20 is grounded and the front wheel 11 is predetermined. A state in which a steering operation less than an angle (for example, 35 degrees) is detected is determined as the first traveling state, and other than that, for example, floating of the leveling float 20 and a predetermined angle (for example, 35 degrees) or more of the front wheel 11 A state in which a steering operation is detected, a state in which a steering operation at a predetermined angle (for example, 35 degrees) of the grounding float 20 and the front wheel 11 is detected, or the floating of the leveling float 20 and the front wheel 11 A state in which a steering operation less than a predetermined angle (for example, 35 degrees) is detected may be determined as the second traveling state.

[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 seedling planting device 6, or the traveling machine body 1 to which the ground work devices are connected, or a combine, etc. It may be a construction work vehicle such as a harvesting work vehicle or a backhoe.

[2] As the tilt angle detecting means 45, a pendulum reference tilt angle sensor, an acceleration sensor, or the like may be employed.

[3] As the angular velocity detection means 46, a mechanical gyro sensor or an optical gyro sensor may be employed.

[4] The calculating means 48 may calculate the pitching angle or yawing angle of the moving body 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-pass filter 50 employed in the calculation means 48. For example, as shown in FIG. In the case where the transfer function indicating these characteristics is the third order expressed by 1 / (T1s + 1) × (T2s + 1) × (T3s + 1), the high frequency components necessary to compensate for the removal components due to these characteristics Is obtained from the output of the angular velocity detection means 46 as 1 / s × {1-1 / [(T1s + 1) × (T2s + 1) × (T3s + 1)]}, and this transfer function is appropriately determined from its time constant. When the set gains G1, G2, and G3 are used, it can be expanded into an expression of G1 × 1 / (T1s + 1) + G2 × 1 / [(T2s + 1) × (T3s + 1)] + G3 × 1 / (T3s + 1) When the gains G1 to G3 are compared, the first gain G1 is found to be negligibly small compared to the second gain G2 and the third gain G3, thereby 1 / (T2s + 1). A calculation obtained by combining the result of multiplying the output of the angular velocity detection means 46 by the second gain G2 and the output of the tilt angle detection means 45 by the adder 55 to the low-pass filter 62 having the characteristic indicated by the transfer function The result is input, and the result obtained by multiplying the output of the low-pass filter 62 and the output of the angular velocity detection means 46 by the third gain G3 is synthesized by the adder 63, and then the characteristic indicated by the transfer function of 1 / (T3s + 1) If the filter is configured to be input to a low-pass filter 64 having a filter, the first-order filters 62 and 64 are employed, and the number of filters used is reduced. Therefore, it is possible to obtain a high-frequency component necessary to compensate for the removal component due to the characteristics of the tilt angle detection means 45 and the characteristics of the low-pass filter from the output of the angular velocity detection means, and as a result, calculate the tilt angle of the moving object A with high accuracy. become able to.

Overall side view of the riding type rice transplanter Overall view of the riding type rice transplanter Front view of seedling planting device Block diagram showing control configuration Block diagram showing schematic configuration of tilt angle measuring device The block diagram which shows schematic structure of the calculating means which considered the characteristic of the inclination angle detection means A block diagram showing a schematic configuration of computing means capable of speeding up while considering the characteristics of the tilt angle detecting means A block diagram showing the configuration of arithmetic means that can further increase the speed while taking into consideration the characteristics of the inclination angle detection means Block diagram showing the configuration of computing means corresponding to higher-order characteristics

Explanation of symbols

45 Inclination angle detection means 46 Angular velocity detection means 50 Low-pass filter 54 Low-pass filter 62 Low-pass filter 64 Low-pass filter A Moving object G1 gain G2 gain G3 gain

Claims (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. 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.

Images