JP2023110557A

Movatterモバイル変換

Info

Publication number: JP2023110557A
Application number: JP2022012083A
Authority: JP
Inventors: 将太郎加藤; Shotaro Kato; 淳一林田; Junichi Hayashida
Original assignee: Mitsubishi Mahindra Agricultural Machinery Co Ltd
Current assignee: Mitsubishi Mahindra Agricultural Machinery Co Ltd
Priority date: 2022-01-28
Filing date: 2022-01-28
Publication date: 2023-08-09

Abstract

【課題】自動操舵システムにおいて、前輪の舵角を機械的に検出する前輪舵角検出機構を不要とする。【解決手段】測位システムが計測した走行機体１の位置及び方位に基づいて操舵ユニットを制御する操舵ユニットＥＣＵを備える自動操舵システムであって、操舵ユニットＥＣＵは、測位システムが計測した走行機体の位置及び方位の変化量に基づいて走行機体の車速Ｖ及び角速度ωを算出し、且つ算出した車速Ｖ及び角速度ωに基づいて走行機体の推定舵角φを算出する推定舵角算出手段を備え、推定舵角算出手段が算出した推定舵角φを使用して操舵ユニットをフィードバック制御する。【選択図】図４An automatic steering system eliminates the need for a front wheel steering angle detection mechanism for mechanically detecting the steering angle of the front wheels. An automatic steering system comprising a steering unit ECU for controlling a steering unit based on the position and orientation of a traveling body 1 measured by a positioning system, wherein the steering unit ECU controls the position of the traveling body measured by the positioning system. and an estimated steering angle calculation means for calculating a vehicle speed V and an angular velocity ω of the traveling machine body based on the amount of change in direction, and calculating an estimated steering angle φ of the traveling machine body based on the calculated vehicle speed V and angular velocity ω; The steering unit is feedback-controlled using the estimated steering angle φ calculated by the steering angle calculation means. [Selection drawing] Fig. 4

Description

Translated fromJapanese

本発明は、作業車両などに搭載される自動操舵システムに関する。 The present invention relates to an automatic steering system mounted on a work vehicle or the like.

アクチュエータの動力で車両を操舵する操舵ユニットと、車両の位置及び方位を計測する位置・方位計測手段と、位置・方位計測手段が計測した車両の位置及び方位に基づいて操舵ユニットを制御する制御部と、を備える自動操舵システムが知られている。例えば、特許文献１には、前輪の舵角を機械的に検出し、検出した前輪の舵角を使用して操舵ユニットをフィードバック制御する自動操舵システムが記載されている。 A steering unit that steers the vehicle by power of an actuator, a position/orientation measuring means that measures the position and orientation of the vehicle, and a control section that controls the steering unit based on the position and orientation of the vehicle measured by the position/orientation measuring means. and are known. For example,Patent Document 1 describes an automatic steering system that mechanically detects the steering angle of the front wheels and performs feedback control of the steering unit using the detected steering angle of the front wheels.

特開２０１８－８６５０号公報JP-A-2018-8650

しかしながら、特許文献１にように、前輪の舵角を機械的に検出する場合、機種毎に専用の前輪舵角検出機構が必要になるため、コストが上昇するだけでなく、適応機種が制限されるという課題があった。 However, when the steering angle of the front wheels is mechanically detected as inPatent Document 1, a dedicated front wheel steering angle detection mechanism is required for each model, which not only increases the cost but also limits the applicable models. There was a problem that

本発明は、上記の如き実情に鑑みこれらの課題を解決することを目的として創作されたものであって、請求項１の発明は、アクチュエータの動力で車両を操舵する操舵ユニットと、前記車両の位置及び方位を計測する位置・方位計測手段と、前記位置・方位計測手段が計測した前記車両の位置及び方位に基づいて前記操舵ユニットを制御する制御部と、を備える自動操舵システムであって、前記制御部は、前記位置・方位計測手段が計測した前記車両の位置及び方位の変化量に基づいて前記車両の車速及び角速度を算出し、且つ算出した前記車速及び前記角速度に基づいて前記車両の推定舵角を算出する推定舵角算出手段を備え、前記推定舵角算出手段が算出した前記推定舵角を使用して前記操舵ユニットをフィードバック制御することを特徴とする。
また、請求項２の発明は、請求項１に記載の自動操舵システムであって、前記車両の機体姿勢を検出する機体姿勢検出手段を更に備え、前記推定舵角算出手段は、前記機体姿勢検出手段の検出値に基づいて前記推定舵角を補正することを特徴とする。
また、請求項３の発明は、請求項１又は２に記載の自動操舵システムであって、前記車両を操舵するステアリングハンドルの操作角度を検出するハンドル角度検出手段を更に備え、前記制御部は、前記車速が所定速度よりも速いとき、前記推定舵角算出手段が算出した前記推定舵角を使用して前記操舵ユニットをフィードバック制御し、前記車速が所定速度よりも遅いとき、前記ハンドル角度検出手段が検出した前記ステアリングハンドルの前記操作角度を使用して前記操舵ユニットをフィードバック制御することを特徴とする。SUMMARY OF THE INVENTION The present invention was created with the object of solving these problems in view of the actual situation as described above. An automatic steering system comprising: position/orientation measuring means for measuring a position and orientation; and a control section for controlling the steering unit based on the position and orientation of the vehicle measured by the position/orientation measuring means, The control unit calculates a vehicle speed and an angular velocity of the vehicle based on the amount of change in the position and orientation of the vehicle measured by the position/orientation measuring means, and calculates the vehicle speed and the angular velocity based on the calculated vehicle speed and the angular velocity. Estimated steering angle calculation means for calculating an estimated steering angle is provided, and feedback control of the steering unit is performed using the estimated steering angle calculated by the estimated steering angle calculation means.
According to a second aspect of the invention, there is provided the automatic steering system according to the first aspect, further comprising aircraft attitude detection means for detecting an attitude of the vehicle, wherein the estimated steering angle calculation means detects the attitude of the vehicle. The estimated steering angle is corrected based on the detected value of means.
Further, the invention ofclaim 3 is the automatic steering system according toclaim 1 or 2, further comprising steering wheel angle detection means for detecting an operation angle of a steering wheel for steering the vehicle, wherein the control unit comprises: feedback control of the steering unit using the estimated steering angle calculated by the estimated steering angle calculating means when the vehicle speed is faster than a predetermined speed, and the steering wheel angle detecting means when the vehicle speed is slower than the predetermined speed; feedback-controlling the steering unit using the operation angle of the steering wheel detected by.

請求項１の発明によれば、前輪の舵角を機械的に検出する前輪舵角検出機構が不要になるため、部品点数や組付工数を削減して大幅なコストの削減を実現できるだけでなく、多様な機種に適応可能な汎用性の高い自動操舵システムを提供することが可能になる。
また、請求項２の発明によれば、機体姿勢検出手段の検出値に基づいて推定舵角を補正するので、推定舵角の算出精度を高め、操舵ユニットの制御精度を向上させることができる。
また、請求項３の発明によれば、車速が所定速度よりも速いとき、推定舵角算出手段が算出した推定舵角を使用して操舵ユニットをフィードバック制御し、車速が所定速度よりも遅いとき、ハンドル角度検出手段が検出したステアリングハンドルの操作角度を使用して操舵ユニットをフィードバック制御するので、推定舵角の算出精度が低下しやすい低速走行時でも操舵ユニットを適切にフィードバック制御することができる。According to the first aspect of the present invention, a front wheel steering angle detection mechanism for mechanically detecting the steering angle of the front wheels is not required. , it becomes possible to provide a highly versatile automatic steering system that can be applied to various models.
Further, according to the second aspect of the present invention, the estimated steering angle is corrected based on the detected value of the aircraft attitude detecting means, so that the accuracy of calculating the estimated steering angle can be increased, and the control accuracy of the steering unit can be improved.
According to the invention ofclaim 3, when the vehicle speed is higher than the predetermined speed, feedback control is performed on the steering unit using the estimated steering angle calculated by the estimated steering angle calculating means, and when the vehicle speed is lower than the predetermined speed. Since the steering unit is feedback-controlled using the operation angle of the steering wheel detected by the steering wheel angle detection means, the steering unit can be appropriately feedback-controlled even during low-speed running where the accuracy of calculating the estimated steering angle tends to be reduced. .

トラクタの側面図である。1 is a side view of a tractor; FIG.ＲＴＫ－ＧＮＳＳ受信装置を示すトラクタの要部斜視図である。1 is a perspective view of a tractor showing an RTK-GNSS receiver; FIG.自動操舵システムの構成を示すブロック図である。1 is a block diagram showing the configuration of an automatic steering system; FIG.推定舵角算出手段の構成を示すブロック図である。3 is a block diagram showing the configuration of estimated steering angle calculation means; FIG.自動操舵制御の処理手順を示すフローチャートである。4 is a flowchart showing a processing procedure of automatic steering control;

以下、本発明の実施の形態について、図面に基づいて説明する。図１において、１はトラクタ（車両）の走行機体であって、該走行機体１は、エンジン（図示せず）が搭載されるエンジン搭載部２と、エンジン動力を変速し、走行動力及び作業動力として出力するミッションケース３と、ミッションケース３が出力する走行動力で駆動され、かつ、ステアリングハンドル４の操作に応じて操舵される前輪５と、ミッションケース３が出力する走行動力で駆動される後輪６と、作業者が乗車する操縦部７と、各種の作業機（図示せず）を昇降自在に連結可能な作業機連結部８と、を備える。 BEST MODE FOR CARRYING OUT THE INVENTION Embodiments of the present invention will be described below with reference to the drawings. In FIG. 1,reference numeral 1 denotes a traveling body of a tractor (vehicle). , thefront wheels 5 driven by the driving power output by themission case 3 and steered according to the operation of thesteering handle 4, and the driving power output by themission case 3. It comprises awheel 6, acontrol section 7 on which a worker rides, and a workmachine connection section 8 capable of connecting various work machines (not shown) so that they can move up and down.

操縦部７は、作業者が座る運転席９を備え、該運転席９の周辺には、前述したステアリングハンドル４を含む各種の操作具が配置されている。ステアリングハンドル４には、自動操舵システムＳ（図３参照）の構成要素である操舵ユニット１０が連結されている。 Thecontrol unit 7 has a driver'sseat 9 on which an operator sits, and various operating tools including thesteering handle 4 are arranged around the driver'sseat 9 . Asteering unit 10 that is a component of an automatic steering system S (see FIG. 3) is connected to thesteering handle 4 .

操舵ユニット１０は、ステアリングハンドル４の手動操作に代えて、ステアリングハンドル４をモータ動力で回転操作する自動操作ユニットであり、図３に示すように、伝動部１１を介してステアリングハンドル４に連結され、モータ動力でステアリングハンドル４を回転操作するステッピングモータ１２（アクチュエータ）と、ステッピングモータ１２を制御する操舵ユニットＥＣＵ１３（制御部）と、ステアリングハンドル４の操作角度を検出する操舵角センサ１４（ハンドル角度検出手段）と、操舵ユニット１０の各種操作を行う操作スイッチ１５と、操舵ユニット１０の各種状態を表示するＬＥＤランプ１６と、を備える。 Thesteering unit 10 is an automatic operation unit that rotates thesteering handle 4 by motor power instead of manually operating thesteering handle 4. As shown in FIG. , a stepping motor 12 (actuator) that rotates thesteering wheel 4 by motor power, a steering unit ECU 13 (control section) that controls thestepping motor 12, and a steering angle sensor 14 (steering wheel angle detection means), anoperation switch 15 for performing various operations of thesteering unit 10, and anLED lamp 16 for displaying various states of thesteering unit 10.

図１及び図２に示すように、操縦部７の屋根部２０には、走行機体１の位置及び方位を計測する測位システム２１（位置・方位計測手段）の構成要素であるＲＴＫ－ＧＮＳＳ受信装置２２が取付けられている。本実施形態の測位システム２１としては、数ｃｍの誤差で高精度な測位が可能なＲＴＫ－ＧＮＳＳ測位システムが採用される。ＲＴＫ－ＧＮＳＳ測位システムは、固定設置された固定局と、移動する移動局とのそれぞれで、ＧＰＳなどのＧＮＳＳ測位を行い、固定局から移動局に送信される補正信号でリアルタイムに測位データを補正することで、誤差数ｃｍの高精度な測位を実現するものである。また、移動局に所定の間隔をあけて２つのＧＮＳＳアンテナを設置すれば、移動局の絶対位置だけでなく、２つの測位結果に基づいて、移動局の進行方向（方位）も高精度に検出することが可能になる。 As shown in FIGS. 1 and 2, an RTK-GNSS receiver, which is a component of a positioning system 21 (position/orientation measuring means) for measuring the position and orientation of thetraveling body 1, is mounted on theroof portion 20 of thecontrol section 7. 22 is attached. As thepositioning system 21 of this embodiment, an RTK-GNSS positioning system capable of highly accurate positioning with an error of several cm is adopted. The RTK-GNSS positioning system performs GNSS positioning such as GPS with a fixed station and a moving mobile station, respectively, and corrects the positioning data in real time with a correction signal sent from the fixed station to the mobile station. By doing so, highly accurate positioning with an error of several centimeters is realized. In addition, if two GNSS antennas are installed on the mobile station with a predetermined interval, not only the absolute position of the mobile station but also the traveling direction (azimuth) of the mobile station can be detected with high accuracy based on the two positioning results. it becomes possible to

具体的に説明すると、測位システム２１は、図３に示すように、操舵ユニット１０とともに自動操舵システムＳを構成しており、本実施形態の測位システム２１には、ＲＴＫ－ＧＮＳＳ測位システムの移動局として走行機体１に設けられるＲＴＫ－ＧＮＳＳ受信装置２２と、ＲＴＫ－ＧＮＳＳ測位システムの固定局として適所に固定設置されるＲＴＫ固定局２３と、が含まれる。 Specifically, as shown in FIG. 3, thepositioning system 21 constitutes an automatic steering system S together with thesteering unit 10. Thepositioning system 21 of this embodiment includes a mobile station of the RTK-GNSS positioning system. and an RTK-GNSS receiver 22 provided on thetraveling body 1 as an RTK-GNSS positioning system.

ＲＴＫ－ＧＮＳＳ受信装置２２は、ＲＴＫ－ＧＮＳＳ測位を実行する制御ユニットであるＧＮＳＳコントローラ２４と、車幅方向に所定の間隔をあけて取付けられる位置計測用のＧＮＳＳアンテナ２５及び方位計測用のＧＮＳＳアンテナ２６と、ＧＮＳＳ衛星から位置計測用のＧＮＳＳアンテナ２５を介してＧＮＳＳ信号を受信する位置計測用のＧＮＳＳ受信機２７と、ＧＮＳＳ衛星から方位計測用のＧＮＳＳアンテナ２６を介してＧＮＳＳ信号を受信する方位計測用のＧＮＳＳ受信機２８と、ＲＴＫ固定局２３から無線アンテナ２９を介して補正信号を受信する特定小電力無線機３０と、加速度センサ及びジャイロセンサを用いて走行機体１の姿勢を検出する慣性計測装置３１と、ＲＴＫ－ＧＮＳＳ受信装置２２の各種状態を表示する７セグメントディスプレイ３２と、を備える。 The RTK-GNSS receiver 22 includes aGNSS controller 24, which is a control unit that performs RTK-GNSS positioning, aGNSS antenna 25 for position measurement and a GNSS antenna for azimuth measurement, which are mounted at predetermined intervals in the vehicle width direction. 26, aGNSS receiver 27 for position measurement that receives GNSS signals from GNSS satellites via aGNSS antenna 25 for position measurement, and a direction for receiving GNSS signals from GNSS satellites via aGNSS antenna 26 for direction measurement. AGNSS receiver 28 for measurement, a specified low-power radio 30 that receives a correction signal from the RTKfixed station 23 via aradio antenna 29, and an inertia that detects the attitude of thetraveling body 1 using an acceleration sensor and a gyro sensor. Ameasurement device 31 and a 7-segment display 32 for displaying various states of the RTK-GNSS receiver 22 are provided.

ＧＮＳＳコントローラ２４は、ＲＴＫ－ＧＮＳＳ測位により取得した位置データ及び方位データと、慣性計測装置３１が検出した機体姿勢データと、ＲＴＫ－ＧＮＳＳ測位のステータス情報を所定の出力先に出力する。第１の出力先は、前述した操舵ユニット１０であり、ＣＡＮなどの有線通信手段を介して送信される。また、第２の出力先は、ナビゲーション表示などを行うタブレット端末３２であり、近距離無線機３３を介して送信される。 TheGNSS controller 24 outputs the position data and orientation data obtained by RTK-GNSS positioning, the airframe attitude data detected by theinertial measurement device 31, and the RTK-GNSS positioning status information to a predetermined output destination. The first output destination is thesteering unit 10 described above, and is transmitted via wired communication means such as CAN. A second output destination is thetablet terminal 32 that performs navigation display, etc., and is transmitted via the short-rangewireless device 33 .

ＲＴＫ固定局２３は、ＧＮＳＳアンテナ３４と、ＧＮＳＳ衛星からＧＮＳＳアンテナ３４を介して補正用のＧＮＳＳ信号を受信するＧＮＳＳ受信機３５と、無線アンテナ３６を介してＲＴＫ－ＧＮＳＳ受信装置２２に補正用のＧＮＳＳ信号を送信する特定小電力無線機３７と、ＲＴＫ－ＧＮＳＳ受信装置２２及び特定小電力無線機３７に電力を供給するバッテリ３８と、を備える。 The RTKfixed station 23 includes aGNSS antenna 34, aGNSS receiver 35 that receives a GNSS signal for correction from a GNSS satellite via theGNSS antenna 34, and aradio antenna 36 to the RTK-GNSS receiver 22 for correction. A specific low-power radio 37 that transmits GNSS signals and abattery 38 that supplies power to the RTK-GNSS receiver 22 and the specific low-power radio 37 are provided.

操舵ユニット１０の操舵ユニットＥＣＵ１３は、自動操舵システムＳのメイン制御部として機能する。操舵ユニットＥＣＵ１３は、ハードウェアとソフトウェアとの協働により実現される機能的な構成として、自動操舵制御手段及び推定舵角算出手段を備える。 Thesteering unit ECU 13 of thesteering unit 10 functions as a main control section of the automatic steering system S. The steering unit ECU 13 includes automatic steering control means and estimated steering angle calculation means as functional components realized by cooperation of hardware and software.

自動操舵制御手段は、予め演算された所定方向の目標ラインに沿って走行するように走行機体１を自動的に操舵する自動制御機能である。目標ラインは、例えば、最初の作業行程である基準ラインの始点位置で第１の登録スイッチ（図示せず）を操作して始点の測位データを登録するとともに、基準ラインの終点位置で第２の登録スイッチ（図示せず）を操作して終点の測位データを登録すると自動的に演算される。具体的には、基準ラインと平行で、かつ等間隔（作業幅間隔）に並列する複数の目標ラインが演算される。 The automatic steering control means is an automatic control function that automatically steers thetraveling body 1 so as to travel along a target line in a predetermined direction that has been calculated in advance. For the target line, for example, a first registration switch (not shown) is operated at the starting point position of the reference line, which is the first work process, to register the positioning data of the starting point, and the second positioning data is registered at the end point position of the reference line. When the registration switch (not shown) is operated to register the positioning data of the end point, calculation is automatically performed. Specifically, a plurality of target lines parallel to the reference line and arranged at equal intervals (working width intervals) are calculated.

自動操舵制御手段は、演算した目標ラインのうち最も走行機体１に近い目標ラインの座標データと、測位システム２１による走行機体１の測位データに基づいて、目標ラインに対する走行機体１の誤差を演算するとともに、誤差に基づいて指示舵角を演算し、該指示舵角を目標としてステッピングモータ１２を制御することにより、走行機体１を目標ラインに沿って走行させる。また、指示舵角を目標としてステッピングモータ１２を制御する際には、現在の検出舵角（推定舵角）をフィードバックしつつ、現在の検出舵角と指示舵角との差分に応じてステッピングモータ１２の回転方向、回転量、回転速度などを制御するフィードバック制御が実行される。 The automatic steering control means calculates the error of the travelingbody 1 with respect to the target line based on the coordinate data of the target line closest to the travelingbody 1 among the calculated target lines and the positioning data of the travelingbody 1 by thepositioning system 21. At the same time, an instructed rudder angle is calculated based on the error, and thestepping motor 12 is controlled with the instructed rudder angle as a target, thereby causing thetraveling machine body 1 to travel along the target line. Further, when the steppingmotor 12 is controlled with the commanded steering angle as a target, the currently detected steering angle (estimated steering angle) is fed back, and the stepping motor is adjusted according to the difference between the currently detected steering angle and the commanded steering angle. Feedback control is executed to control the direction of rotation, the amount of rotation, the speed of rotation, etc. of 12.

推定舵角算出手段は、測位システム２１が計測した走行機体１の位置データ及び方位データの変化量に基づいて走行機体１の車速Ｖ及び角速度ωを算出し、且つ算出した車速Ｖ及び角速度ωに基づいて走行機体１の推定舵角φを算出する。推定舵角φの算出には、例えば、下記に示す計算式（図４の推定舵角計算式５２）を用いることができる。ただし、Ｌは走行機体１のホイールベースである。
φ＝ｔａｎ^－１（Ｌ・２πω／Ｖ）The estimated steering angle calculation means calculates the vehicle speed V and the angular velocity ω of the travelingbody 1 based on the amount of change in the position data and the azimuth data of thetraveling body 1 measured by thepositioning system 21, and calculates the calculated vehicle speed V and the angular velocity ω. Based on this, the estimated steering angle φ of thetraveling body 1 is calculated. For example, the following calculation formula (estimated steeringangle calculation formula 52 in FIG. 4) can be used to calculate the estimated steering angle φ. However, L is the wheelbase of thetraveling machine body 1 .
φ=tan⁻¹ (L・2πω/V)

このような推定舵角算出手段によれば、前輪５の舵角を機械的に検出する前輪舵角検出機構が不要になるため、部品点数や組付工数を削減して大幅なコストの削減を実現できるだけでなく、多様な機種に適応可能な汎用性の高い自動操舵システムＳを提供できる。 According to such an estimated steering angle calculation means, a front wheel steering angle detection mechanism for mechanically detecting the steering angle of thefront wheels 5 is not required. It is possible to provide an automatic steering system S that is not only realizable but also highly versatile and adaptable to a variety of models.

また、本実施形態の推定舵角算出手段は、慣性計測装置３１の検出値に基づいて推定舵角φを補正する。例えば、図４に示すように、方位データの変化量と、慣性計測装置３１の検出値（ジャイロセンサのヨー方向の角速度値）を相補フィルタ５１に入力し、その出力値を角速度ωとして推定舵角計算式５２に代入する。相補フィルタ５１は、例えば、ローパスフィルタを通した方位データの変化量と、ハイパスフィルタを通したジャイロセンサ値とを所定の割合で合成して角速度ωとする。 Further, the estimated steering angle calculation means of this embodiment corrects the estimated steering angle φ based on the detection value of theinertial measurement device 31 . For example, as shown in FIG. 4, the amount of change in azimuth data and the detection value of the inertial measurement device 31 (angular velocity value in the yaw direction of the gyro sensor) are input to thecomplementary filter 51, and the output value is used as the angular velocity ω to estimate the rudder. Substitute intoangle calculation formula 52. Thecomplementary filter 51 synthesizes, for example, the amount of change in the low-pass-filtered azimuth data and the high-pass-filtered gyro sensor value at a predetermined ratio to obtain the angular velocity ω.

このような推定舵角φの補正によれば、ローパスフィルタによって中周波～高周波のノイズ成分を除去した方位データの変化量と、ハイパスフィルタによって累積ドリフト成分を除去したジャイロセンサ値とに基づいて角速度ωを高精度に特定し、推定舵角φの算出精度が高められる。 According to such correction of the estimated steering angle φ, the angular velocity is calculated based on the amount of change in the azimuth data from which medium to high frequency noise components are removed by a low-pass filter and the gyro sensor value from which accumulated drift components are removed by a high-pass filter. ω can be specified with high accuracy, and the accuracy of calculating the estimated steering angle φ can be enhanced.

また、本実施形態の推定舵角算出手段は、車速Ｖが所定の速度閾値（例えば、０．１ｍ／ｓｅｃ）よりも速いとき、推定舵角計算式５２で算出した推定舵角φを出力し、車速Ｖが所定の速度閾値よりも遅いとき、操舵角センサ１４が検出したステアリングハンドル４の操作角度を推定舵角φとして出力する。例えば、図４に示すように、ＮＧ判定部５３において車速Ｖと速度閾値との比較を行い、その結果に応じて出力選択部５４が出力する推定舵角φを切り換える。 Further, the estimated steering angle calculation means of the present embodiment outputs the estimated steering angle φ calculated by the estimated steeringangle calculation formula 52 when the vehicle speed V is faster than a predetermined speed threshold value (for example, 0.1 m/sec). , when the vehicle speed V is lower than a predetermined speed threshold, the operation angle of thesteering wheel 4 detected by thesteering angle sensor 14 is output as an estimated steering angle φ. For example, as shown in FIG. 4, theNG determination unit 53 compares the vehicle speed V with the speed threshold, and switches the estimated steering angle φ output by theoutput selection unit 54 according to the result.

このような推定舵角算出手段によれば、測位システム２１による車速（ＧＰＳ速度）の検出精度や車速変動などに起因し、推定舵角φの算出精度が低下しやすい低速走行時であっても、操舵角センサ１４の検出値を推定舵角φとすることで、操舵ユニット１０を適切にフィードバック制御することができる。 According to such an estimated steering angle calculation means, even when the vehicle is traveling at a low speed where the accuracy of calculating the estimated steering angle φ tends to decrease due to the accuracy of detection of the vehicle speed (GPS speed) by thepositioning system 21 and fluctuations in the vehicle speed. By setting the detected value of thesteering angle sensor 14 as the estimated steering angle φ, thesteering unit 10 can be feedback-controlled appropriately.

なお、上記の例では、車速Ｖが所定の速度閾値（例えば、０．１ｍ／ｓｅｃ）よりも速いとき、推定舵角計算式５２で算出した推定舵角φを出力し、車速Ｖが所定の速度閾値よりも遅いとき、操舵角センサ１４が検出したステアリングハンドル４の操作角度を推定舵角φとして出力するが、車速Ｖが所定の第１速度閾値（例えば、０．４ｍ／ｓｅｃ）よりも速いとき、推定舵角計算式５２で算出した推定舵角φを出力し、車速Ｖが所定の第２速度閾値（例えば、０．１ｍ／ｓｅｃ）よりも遅いとき、操舵角センサ１４が検出したステアリングハンドル４の操作角度を推定舵角φとして出力し、車速Ｖが所定の第１速度閾値よりも遅く、かつ所定の第２速度閾値よりも速いとき、推定舵角計算式５２で算出した推定舵角φと操舵角センサ１４が検出したハンドル操作角度φ’との混成値αを推定舵角φとして出力する変形例としてもよい。この混成値αの算出には、例えば、下記の計算式を用いることができる。ただし、Ｋは、速度に応じて「０～１」の範囲で段階的又は無段階に変化する係数であり、例えば、車速Ｖが第２速度閾値のとき「０」、車速Ｖが第１速度閾値のとき「１」とする。
α＝Ｋφ＋（１－Ｋ）φ’In the above example, when the vehicle speed V is faster than a predetermined speed threshold (for example, 0.1 m/sec), the estimated steering angle φ calculated by the estimated steeringangle calculation formula 52 is output, and the vehicle speed V reaches the predetermined speed V. When the speed is slower than the speed threshold, the operation angle of thesteering wheel 4 detected by thesteering angle sensor 14 is output as the estimated steering angle φ. When the vehicle speed is fast, the estimated steering angle φ calculated by the estimated steeringangle calculation formula 52 is output. The operation angle of thesteering wheel 4 is output as an estimated steering angle φ, and when the vehicle speed V is slower than a predetermined first speed threshold and faster than a predetermined second speed threshold, the estimated steering angle calculated by the estimated steering angle calculation formula 52 A modified example in which a mixed value α of the steering angle φ and the steering wheel operation angle φ′ detected by thesteering angle sensor 14 is output as the estimated steering angle φ may be employed. For example, the following formula can be used to calculate the mixed value α. However, K is a coefficient that changes stepwise or steplessly in the range of "0 to 1" according to the speed. When the threshold is set to "1".
α=Kφ+(1-K)φ'

このような変形例によれば、操舵角センサ１４が検出したハンドル操作角度φ’のみでは、正確性が足りず、頻繁に構成が必要となり、かつ、推定舵角計算式５２で算出した推定舵角φのみでは、推定舵角φの算出精度が十分ではない車速域においても、操舵ユニット１０を適切にフィードバック制御することができる。また、係数Ｋは、車速Ｖの増加に応じて大きくなるので、推定舵角φの算出精度の上昇に応じて推定舵角φの混成割合を増加させることができる。 According to such a modified example, only the steering angle φ′ detected by thesteering angle sensor 14 is insufficient in accuracy, and frequent configuration is required. With only the angle φ, thesteering unit 10 can be appropriately feedback-controlled even in a vehicle speed range where the accuracy of calculating the estimated steering angle φ is not sufficient. Also, since the coefficient K increases as the vehicle speed V increases, it is possible to increase the mixing ratio of the estimated steering angle φ in accordance with the increase in the calculation accuracy of the estimated steering angle φ.

また、本実施形態の推定舵角算出手段は、ＲＴＫ－ＧＮＳＳ測位のステータス情報に基づいてＲＴＫ－ＧＮＳＳ測位の動作不良を判断し、動作不良であると判断したとき、操舵角センサ１４が検出したステアリングハンドル４の操作角度を推定舵角φとして出力する。例えば、図４に示すように、ＮＧ判定部５３においてＲＴＫ－ＧＮＳＳ測位の動作不良を判断し、その結果に応じて出力選択部５４が出力する推定舵角φを切り換える。 Further, the estimated steering angle calculation means of the present embodiment determines malfunction of the RTK-GNSS positioning based on the status information of the RTK-GNSS positioning, and when it is determined that the malfunction occurs, thesteering angle sensor 14 detects An operation angle of thesteering wheel 4 is output as an estimated steering angle φ. For example, as shown in FIG. 4, theNG determination unit 53 determines that the RTK-GNSS positioning is malfunctioning, and switches the estimated steering angle φ output by theoutput selection unit 54 according to the result.

このような推定舵角算出手段によれば、ＲＴＫ－ＧＮＳＳ測位の動作不良時であっても、操舵角センサ１４の検出値を推定舵角φとすることで、操舵ユニット１０を適切にフィードバック制御することができる。なお、図４において、５５は算出した推定舵角φの中周波～高周波のノイズ成分を除去するローパスフィルタである。 According to such an estimated steering angle calculation means, even when the RTK-GNSS positioning operation is malfunctioning, thesteering unit 10 is appropriately feedback-controlled by setting the detected value of thesteering angle sensor 14 to the estimated steering angle φ. can do. In FIG. 4,reference numeral 55 denotes a low-pass filter for removing noise components of intermediate to high frequencies of the calculated estimated steering angle φ.

つぎに、上記のような自動操舵制御手段及び推定舵角算出手段を含む操舵ユニットＥＣＵ１３の具体的な処理手順（自動操舵制御）について、図５を参照して説明する。 Next, a specific processing procedure (automatic steering control) of thesteering unit ECU 13 including the automatic steering control means and the estimated steering angle calculation means as described above will be described with reference to FIG.

図５に示すように、操舵ユニットＥＣＵ１３は、自動操舵制御において、まず、ＲＴＫ－ＧＮＳＳ受信装置２２から走行機体１の位置データ、方位データ、機体姿勢データ及び測位ステータ情報を入力した後（Ｓ１）、目標ラインに対する走行経路の誤差を算出するとともに（Ｓ２）、この誤差を解消可能な指示舵角を算出する（Ｓ３）。 As shown in FIG. 5, in the automatic steering control, thesteering unit ECU 13 first inputs the position data, direction data, body attitude data, and positioning status information of the travelingbody 1 from the RTK-GNSS receiver 22 (S1). , the error of the travel route with respect to the target line is calculated (S2), and the instructed steering angle capable of resolving this error is calculated (S3).

つぎに、操舵ユニットＥＣＵ１３は、測位ステータス情報に基づいてＲＴＫ－ＧＮＳＳ測位の動作状態が正常であるか否か判断するとともに（Ｓ４）、現在の車速Ｖが車速閾値よりも速いか否かを判断する（Ｓ５）。操舵ユニットＥＣＵ１３は、ステップＳ４、Ｓ５の判断結果がいずれもＹＥＳの場合は、前述した推定舵角計算式５２で推定舵角φを算出し（Ｓ６）、この推定舵角φを使用して操舵ユニット１０（ステッピングモータ１２）をフィードバック制御する（Ｓ７）。また、操舵ユニットＥＣＵ１３は、ステップＳ４、Ｓ５のうち、少なくともいずれかの判断結果がＮＯの場合は、操舵角センサ１４の検出値を取得し（Ｓ８）、この検出値を推定舵角φとして使用して操舵ユニット１０をフィードバック制御する（Ｓ９）。 Next, thesteering unit ECU 13 determines whether or not the operating state of RTK-GNSS positioning is normal based on the positioning status information (S4), and determines whether or not the current vehicle speed V is faster than the vehicle speed threshold. (S5). If the determination results of steps S4 and S5 are both YES, thesteering unit ECU 13 calculates the estimated steering angle φ using the estimated steering angle calculation formula 52 (S6), and uses the estimated steering angle φ to perform steering. The unit 10 (stepping motor 12) is feedback-controlled (S7). Further, if the determination result of at least one of steps S4 and S5 is NO, thesteering unit ECU 13 acquires the detected value of the steering angle sensor 14 (S8) and uses this detected value as the estimated steering angle φ. Then, thesteering unit 10 is feedback-controlled (S9).

叙述の如く構成された本実施形態によれば、モータ動力で走行機体１を操舵する操舵ユニット１０と、走行機体１の位置及び方位を計測する測位システム２１と、測位システム２１が計測した走行機体１の位置及び方位に基づいて操舵ユニット１０を制御する操舵ユニットＥＣＵ１３と、を備える自動操舵システムＳであって、操舵ユニットＥＣＵ１３は、測位システム２１が計測した走行機体１の位置及び方位の変化量に基づいて走行機体１の車速Ｖ及び角速度ωを算出し、且つ算出した車速Ｖ及び角速度ωに基づいて走行機体１の推定舵角φを算出する推定舵角算出手段を備え、推定舵角算出手段が算出した推定舵角φを使用して操舵ユニット１０をフィードバック制御するので、前輪５の舵角を機械的に検出する前輪舵角検出機構が不要になる。その結果、部品点数や組付工数を削減して大幅なコストの削減を実現できるだけでなく、多様な機種に適応可能な汎用性の高い自動操舵システムＳを提供することが可能になる。 According to this embodiment configured as described above, thesteering unit 10 that steers the travelingbody 1 by motor power, thepositioning system 21 that measures the position and orientation of the travelingbody 1, and the traveling body measured by thepositioning system 21 and asteering unit ECU 13 that controls thesteering unit 10 based on the position and orientation of 1. Thesteering unit ECU 13 measures the amount of change in the position and orientation of the travelingbody 1 measured by thepositioning system 21. and an estimated steering angle calculation means for calculating an estimated steering angle φ of the travelingbody 1 based on the calculated vehicle speed V and the angular velocity ω, the estimated steering angle calculation Since thesteering unit 10 is feedback-controlled using the calculated estimated steering angle φ, the front wheel steering angle detection mechanism for mechanically detecting the steering angle of thefront wheels 5 becomes unnecessary. As a result, it is possible not only to reduce the number of parts and the number of assembly man-hours and achieve a significant cost reduction, but also to provide a highly versatile automatic steering system S that can be adapted to various models.

また、自動操舵システムＳは、走行機体１の機体姿勢を検出する慣性計測装置３１を更に備え、推定舵角算出手段は、慣性計測装置３１の検出値に基づいて推定舵角φを補正するので、推定舵角φの算出精度を高め、操舵ユニット１０の制御精度を向上させることができる。 Further, the automatic steering system S further includes aninertial measurement device 31 for detecting the body attitude of the travelingbody 1, and the estimated steering angle calculation means corrects the estimated steering angle φ based on the detected value of theinertial measurement device 31. , the accuracy of calculating the estimated steering angle φ can be increased, and the control accuracy of thesteering unit 10 can be improved.

また、自動操舵システムＳは、走行機体１を操舵するステアリングハンドル４の操作角度を検出する操舵角センサ１４を更に備え、操舵ユニットＥＣＵ１３は、車速Ｖが所定速度よりも速いとき、推定舵角算出手段が算出した推定舵角φを使用して操舵ユニット１０をフィードバック制御し、車速Ｖが所定速度よりも遅いとき、操舵角センサ１４が検出したステアリングハンドル４の操作角度を推定舵角φとして操舵ユニット１０をフィードバック制御するので、推定舵角φの算出精度が低下しやすい低速走行時でも操舵ユニット１０を適切にフィードバック制御することができる。 The automatic steering system S further includes asteering angle sensor 14 that detects the operation angle of thesteering wheel 4 that steers the travelingbody 1. Thesteering unit ECU 13 calculates an estimated steering angle when the vehicle speed V is higher than a predetermined speed. Thesteering unit 10 is feedback-controlled using the estimated steering angle φ calculated by the means. Since theunit 10 is feedback-controlled, thesteering unit 10 can be appropriately feedback-controlled even during low-speed running, in which the accuracy of calculating the estimated steering angle φ tends to decrease.

以上、本発明の実施形態について説明したが、本発明は上記実施形態に限定されないことは勿論であり、変更及び追加が可能である。例えば、上記実施形態の推定舵角算出手段は、操舵ユニットを制御する操舵ユニットＥＣＵの機能構成としているが、操舵ユニットの外部に設けられる制御装置の機能構成としてもよい。この場合、制御装置が算出した推定舵角を操舵ユニットに入力する必要があるが、入力先の操舵ユニットが、前輪舵角センサ（例えば、ポテンショメータ）の接続コネクタを備える既存の操舵ユニットである場合は、算出した推定舵角を前輪舵角センサの検出値に対応するアナログ電圧値に変換する処理を行うことで、既存の操舵ユニットを使用することも可能になる。 Although the embodiments of the present invention have been described above, the present invention is of course not limited to the above embodiments, and modifications and additions are possible. For example, although the estimated steering angle calculation means in the above embodiment is a functional configuration of the steering unit ECU that controls the steering unit, it may be a functional configuration of a control device provided outside the steering unit. In this case, it is necessary to input the estimated steering angle calculated by the control device to the steering unit. If the input steering unit is an existing steering unit equipped with a connector for a front wheel steering angle sensor (for example, a potentiometer) can use an existing steering unit by converting the calculated estimated steering angle into an analog voltage value corresponding to the detected value of the front wheel steering angle sensor.

また、上記実施形態では、測位システムにより検出した位置データ、方位データ及び機体姿勢データのノイズを除去するために、ローパスフィルタやハイパスフィルタを使用しているが、突発的な変動が生じやすい車両（例えば、田植機）に適用する場合は、検出値の上限及び下限を設定し、突発的な検出値の揺れを排除するようにしてもよい。 In the above embodiment, a low-pass filter and a high-pass filter are used to remove noise from the position data, orientation data, and aircraft attitude data detected by the positioning system. For example, when applied to a rice transplanter, upper and lower limits of detection values may be set to eliminate sudden fluctuations in detection values.

また、測位システムを使用した自動操舵制御では、測位システムの動作状況に応じて直進性に乱れが生じる可能性があるが、このような直進性の乱れを抑制する手段を設けてもよい。例えば、通信状況に応じてＲＴＫ補正信号の有無が切り換わり、直進性が乱れる可能性がある場合は、ＲＴＫ補正信号の有無による誤差に応じて目標ラインや目標方位を変更することで、直進性の乱れを抑制することができる。 Further, in automatic steering control using a positioning system, there is a possibility that the straightness of the vehicle will be disturbed depending on the operating conditions of the positioning system. For example, if there is a possibility that the presence or absence of the RTK correction signal is switched according to the communication situation and the straightness is disturbed, by changing the target line or the target bearing according to the error due to the presence or absence of the RTK correction signal, the straightness can be improved. disturbance can be suppressed.

Ｓ自動操舵システム
１走行機体
４ステアリングハンドル
５前輪
１０操舵ユニット
１２ステッピングモータ
１３操舵ユニットＥＣＵ
１４操舵角センサ
２１測位システム
２２ＧＮＳＳ受信装置
２３ＲＴＫ固定局
３１慣性計測装置
５１相補フィルタ
５２推定舵角計算式
５３ＮＧ判定部
５４出力選択部SAutomatic steering system 1Running body 4 Steering handle 5Front wheel 10Steering unit 12Stepping motor 13 Steering unit ECU
14Steering angle sensor 21Positioning system 22GNSS receiver 23 RTK fixedstation 31Inertial measurement device 51Complementary filter 52 Estimated steeringangle calculation formula 53NG determination unit 54 Output selection unit

Claims

Translated fromJapanese

アクチュエータの動力で車両を操舵する操舵ユニットと、
前記車両の位置及び方位を計測する位置・方位計測手段と、
前記位置・方位計測手段が計測した前記車両の位置及び方位に基づいて前記操舵ユニットを制御する制御部と、を備える自動操舵システムであって、
前記制御部は、前記位置・方位計測手段が計測した前記車両の位置及び方位の変化量に基づいて前記車両の車速及び角速度を算出し、且つ算出した前記車速及び前記角速度に基づいて前記車両の推定舵角を算出する推定舵角算出手段を備え、前記推定舵角算出手段が算出した前記推定舵角を使用して前記操舵ユニットをフィードバック制御することを特徴とする自動操舵システム。a steering unit that steers the vehicle with the power of the actuator;
a position/orientation measuring means for measuring the position and orientation of the vehicle;
an automatic steering system comprising: a control unit that controls the steering unit based on the position and orientation of the vehicle measured by the position/orientation measuring means,
The control unit calculates a vehicle speed and an angular velocity of the vehicle based on the amount of change in the position and orientation of the vehicle measured by the position/orientation measuring means, and calculates the vehicle speed and the angular velocity based on the calculated vehicle speed and the angular velocity. An automatic steering system comprising estimated steering angle calculation means for calculating an estimated steering angle, wherein feedback control of the steering unit is performed using the estimated steering angle calculated by the estimated steering angle calculation means.

前記車両の機体姿勢を検出する機体姿勢検出手段を更に備え、
前記推定舵角算出手段は、前記機体姿勢検出手段の検出値に基づいて前記推定舵角を補正することを特徴とする請求項１に記載の自動操舵システム。Further comprising an aircraft attitude detection means for detecting an aircraft attitude of the vehicle,
2. The automatic steering system according to claim 1, wherein said estimated steering angle calculation means corrects said estimated steering angle based on a value detected by said aircraft posture detection means.

前記車両を操舵するステアリングハンドルの操作角度を検出するハンドル角度検出手段を更に備え、
前記制御部は、前記車速が所定速度よりも速いとき、前記推定舵角算出手段が算出した前記推定舵角を使用して前記操舵ユニットをフィードバック制御し、前記車速が所定速度よりも遅いとき、前記ハンドル角度検出手段が検出した前記ステアリングハンドルの前記操作角度を使用して前記操舵ユニットをフィードバック制御することを特徴とする請求項１又は２に記載の自動操舵システム。Further comprising steering wheel angle detection means for detecting an operation angle of a steering wheel for steering the vehicle,
The control unit feedback-controls the steering unit using the estimated steering angle calculated by the estimated steering angle calculation means when the vehicle speed is higher than a predetermined speed, and performs feedback control of the steering unit when the vehicle speed is lower than the predetermined speed. 3. The automatic steering system according to claim 1, wherein the steering unit is feedback-controlled using the operation angle of the steering wheel detected by the steering wheel angle detection means.