JP7269255B2 - 誤差補正装置 - Google Patents
誤差補正装置 Download PDFInfo
- Publication number
- JP7269255B2 JP7269255B2 JP2020553966A JP2020553966A JP7269255B2 JP 7269255 B2 JP7269255 B2 JP 7269255B2 JP 2020553966 A JP2020553966 A JP 2020553966A JP 2020553966 A JP2020553966 A JP 2020553966A JP 7269255 B2 JP7269255 B2 JP 7269255B2
- Authority
- JP
- Japan
- Prior art keywords
- sensor
- vector
- coordinate system
- work vehicle
- 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.)
- Active
Links
- 238000012937 correction Methods 0.000 title claims description 41
- 239000013598 vector Substances 0.000 claims description 57
- 238000005259 measurement Methods 0.000 claims description 23
- 230000001133 acceleration Effects 0.000 claims description 15
- 230000009466 transformation Effects 0.000 claims description 9
- 230000001131 transforming effect Effects 0.000 claims description 3
- 238000006243 chemical reaction Methods 0.000 description 5
- 238000010586 diagram Methods 0.000 description 5
- 230000005540 biological transmission Effects 0.000 description 4
- 239000011159 matrix material Substances 0.000 description 3
- 241000209094 Oryza Species 0.000 description 2
- 235000007164 Oryza sativa Nutrition 0.000 description 2
- 230000007246 mechanism Effects 0.000 description 2
- 238000000034 method Methods 0.000 description 2
- 238000012986 modification Methods 0.000 description 2
- 230000004048 modification Effects 0.000 description 2
- 230000008569 process Effects 0.000 description 2
- 238000012545 processing Methods 0.000 description 2
- 235000009566 rice Nutrition 0.000 description 2
- 235000013311 vegetables Nutrition 0.000 description 2
- 230000008859 change Effects 0.000 description 1
- 238000006073 displacement reaction Methods 0.000 description 1
- 238000005516 engineering process Methods 0.000 description 1
- 238000009420 retrofitting Methods 0.000 description 1
- 238000000926 separation method Methods 0.000 description 1
Images
Classifications
-
- A—HUMAN NECESSITIES
- A01—AGRICULTURE; FORESTRY; ANIMAL HUSBANDRY; HUNTING; TRAPPING; FISHING
- A01B—SOIL WORKING IN AGRICULTURE OR FORESTRY; PARTS, DETAILS, OR ACCESSORIES OF AGRICULTURAL MACHINES OR IMPLEMENTS, IN GENERAL
- A01B69/00—Steering of agricultural machines or implements; Guiding agricultural machines or implements on a desired track
- A01B69/007—Steering or guiding of agricultural vehicles, e.g. steering of the tractor to keep the plough in the furrow
- A01B69/008—Steering or guiding of agricultural vehicles, e.g. steering of the tractor to keep the plough in the furrow automatic
-
- B—PERFORMING OPERATIONS; TRANSPORTING
- B62—LAND VEHICLES FOR TRAVELLING OTHERWISE THAN ON RAILS
- B62D—MOTOR VEHICLES; TRAILERS
- B62D6/00—Arrangements for automatically controlling steering depending on driving conditions sensed and responded to, e.g. control circuits
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
- G01S19/00—Satellite radio beacon positioning systems; Determining position, velocity or attitude using signals transmitted by such systems
- G01S19/38—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system
- G01S19/39—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system the satellite radio beacon positioning system transmitting time-stamped messages, e.g. GPS [Global Positioning System], GLONASS [Global Orbiting Navigation Satellite System] or GALILEO
- G01S19/40—Correcting position, velocity or attitude
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
- G01S19/00—Satellite radio beacon positioning systems; Determining position, velocity or attitude using signals transmitted by such systems
- G01S19/38—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system
- G01S19/39—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system the satellite radio beacon positioning system transmitting time-stamped messages, e.g. GPS [Global Positioning System], GLONASS [Global Orbiting Navigation Satellite System] or GALILEO
- G01S19/42—Determining position
- G01S19/48—Determining position by combining or switching between position solutions derived from the satellite radio beacon positioning system and position solutions derived from a further system
- G01S19/49—Determining position by combining or switching between position solutions derived from the satellite radio beacon positioning system and position solutions derived from a further system whereby the further system is an inertial position system, e.g. loosely-coupled
-
- B—PERFORMING OPERATIONS; TRANSPORTING
- B60—VEHICLES IN GENERAL
- B60Y—INDEXING SCHEME RELATING TO ASPECTS CROSS-CUTTING VEHICLE TECHNOLOGY
- B60Y2200/00—Type of vehicle
- B60Y2200/20—Off-Road Vehicles
- B60Y2200/22—Agricultural vehicles
-
- B—PERFORMING OPERATIONS; TRANSPORTING
- B62—LAND VEHICLES FOR TRAVELLING OTHERWISE THAN ON RAILS
- B62D—MOTOR VEHICLES; TRAILERS
- B62D15/00—Steering not otherwise provided for
- B62D15/02—Steering position indicators ; Steering position determination; Steering aids
- B62D15/025—Active steering aids, e.g. helping the driver by actively influencing the steering system after environment evaluation
-
- B—PERFORMING OPERATIONS; TRANSPORTING
- B62—LAND VEHICLES FOR TRAVELLING OTHERWISE THAN ON RAILS
- B62D—MOTOR VEHICLES; TRAILERS
- B62D6/00—Arrangements for automatically controlling steering depending on driving conditions sensed and responded to, e.g. control circuits
- B62D6/002—Arrangements for automatically controlling steering depending on driving conditions sensed and responded to, e.g. control circuits computing target steering angles for front or rear wheels
Landscapes
- Engineering & Computer Science (AREA)
- Remote Sensing (AREA)
- Radar, Positioning & Navigation (AREA)
- Life Sciences & Earth Sciences (AREA)
- Mechanical Engineering (AREA)
- General Physics & Mathematics (AREA)
- Physics & Mathematics (AREA)
- Computer Networks & Wireless Communication (AREA)
- Soil Sciences (AREA)
- Environmental Sciences (AREA)
- Transportation (AREA)
- Combustion & Propulsion (AREA)
- Chemical & Material Sciences (AREA)
- Guiding Agricultural Machines (AREA)
- Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
- Aviation & Aerospace Engineering (AREA)
- Automation & Control Theory (AREA)
- Steering Control In Accordance With Driving Conditions (AREA)
Description
本発明の実施形態は、作業車両に設けられたセンサ装置による測定値の補正に関する。
従来、農作業の効率化を目的として、トラクタや田植機などの作業車両を指定した走行経路に追従させる自動操舵制御が行われている。この自動操舵制御によれば、目標とする走行経路上の位置と作業車両の位置との離間距離に基づいて、作業車両の位置を走行経路上に位置付けるように作業車両の操舵が制御される。
また、作業車両の自動操舵に関する技術として、衛星からの衛星情報に基づいて位置情報を出力する衛星測位モジュールと、前進走行における作業車の操舵制御の基準位置となる前進基準位置を位置情報に基づいて算出する前進基準位置算出部と、後進走行における作業車の操舵制御の基準位置となる後進基準位置を位置情報に基づいて算出する後進基準位置算出部と、前進走行時に走行経路と前進基準位置との偏差に基づいて算出された前進用操舵制御信号を出力するとともに、後進走行時に走行経路と後進基準位置との偏差に基づいて算出された後進用操舵制御信号を出力する操舵制御部と、前進用操舵制御信号及び後進用操舵制御信号に基づいて前記作業車の操舵を行う操舵機構とを備えた自動操舵システム、が知られている(例えば、特許文献1参照)。
このような自動操舵制御において、作業車両の車両位置を測定するGNSS(Global Navigation Satellite System)センサは、衛星からGNSS信号を受信するのに適切な位置に設けられる。作業車両におけるGNSS信号の受信に適切な位置は、本体から上方に延びるように設けられたアームの上端部分、またはこのアームによりその上端に接続されて支持される屋根部である。
また、作業車両が起伏の激しい圃場を走行する場合にはその姿勢が傾斜することとなる。この際、GNSSセンサが上方に取り付けられているために、圃場上の作業車両の位置に対して、GNSSセンサにより測定される位置に誤差が生じ、延いては、この誤差によって自動操舵制御の精度が低下する、という問題がある。
本発明は、上述した問題点を解決するためになされたものであり、GNSSセンサにより測定された作業車両の車両位置の誤差を低減することができる誤差補正装置を提供することを目的とする。
上述した課題を解決するため、本実施形態に係る誤差補正装置は、作業車両に設けられてGNSSセンサとジャイロセンサとを備えたセンサ装置による測定値の誤差を補正する誤差補正装置であって、前記GNSSセンサにより測定されたローカル座標系における前記作業車両の車両位置と、前記ジャイロセンサにより測定された前記センサ装置を基準とした3軸を有するセンサ座標系における角速度とを取得する第1取得部と、前記センサ座標系において、前記センサ装置と異なる位置に設定された設定位置を始点とした前記センサ装置が設けられた測定位置の相対位置ベクトルを、前記取得された角速度に基づいて前記ローカル座標系における相対位置ベクトルに座標変換する座標変換部と、前記ローカル座標系の原点を始点とした前記設定位置の位置ベクトルをPa、前記測定位置の位置ベクトルをPs、前記座標変換された相対位置ベクトルをLとして、Pa=Ps-Lの式によって前記設定位置の位置ベクトルPaを算出して前記GNSSセンサにより測定された車両位置の誤差を補正する車両位置補正部とを備える。
本発明によれば、GNSSセンサにより測定された作業車両の車両位置の誤差を低減することができる。
以下、図面を参照しながら、本発明の実施形態について説明する。
(作業車両の構成)
まず、本実施形態に係る誤差補正装置を備える作業車両について説明する。図1は、実施形態に係る農業用トラクタの構成を示す概略側面図である。
まず、本実施形態に係る誤差補正装置を備える作業車両について説明する。図1は、実施形態に係る農業用トラクタの構成を示す概略側面図である。
本実施形態に係る自動操舵システムの操舵対象としての車両は、作業車両であり、具体的には、図1に示すような農業用のトラクタ1とする。このトラクタ1は、車体10と、2つの前輪11と2つの後輪12とを備える四輪車両とするが、操舵可能な車両であれば、作業車両に限らずどのような車両であっても良い。また、トラクタ1は、運転者が座るための座席13、ステアリングコラム15、ステアリングハンドル16、操舵駆動装置17、アクセル、ブレーキ等を含むペダル類18、4つの支持フレーム19a~19d、屋根部19rを備える。
ステアリングコラム15内には、前輪11に偏角を与えてトラクタ1を操舵する操舵系が備えられるとともに、ステアリングハンドル16または操舵駆動装置17によるハンドル角を操舵系に入力するための入力軸151が内蔵され、この入力軸151の回転に基づく操舵角が前輪11に与えられる。操舵駆動装置17は、自動操舵制御をするための構成が備えられていない、手動操舵を前提としたトラクタ1を自動操舵するために後付けされる装置であり、上方においては、ステアリングハンドル16が取り付けられるとともに、下方において、操舵系の入力軸151の上端部が嵌合される。車体10上には、それぞれが長尺に形成された4つの支持フレーム19a~19dが起立した状態で前後左右の異なる位置に設けられ、これら4つの支持フレーム19a~19d上には、全体として略平板上に形成された屋根部19rが載置される。
屋根部19rの上面には、この屋根部19rを架台としてセンサ装置30が設けられる。このセンサ装置30は、後述するようにGNSSセンサを含むことから、人工衛星から送信されるGNSS信号を受信するために、屋根部19rのような、トラクタ1における最上部に取り付けることが望ましく、トラクタ1が屋根部19rを備えない場合は、屋根部19rを支持する4つの支持フレーム19a~19dのいずれかを架台としてセンサ装置30が設けられるものとする。また、前方に位置する支持フレーム19a,19bのいずれかには、自動操舵制御装置21が設けられるものとする。この自動操舵制御装置21は、トラクタ1の自動操舵を制御するものである。
また、トラクタ1は、作業車両に設けられる作業機としてのロータリ耕耘機41と、このロータリ耕耘機41を昇降可能にトラクタ1と連結するリンク機構42と、ロータリ耕耘機41と圃場との距離を耕深位置として検出する接地センサ43とを備える。接地センサ43は、下端部が圃場に設置するように、上端部を支点として上下に揺動可能に設けられた接地部431と、この接地部431の揺動変位を検出する検知部432とを備える。
(制御システムの構成)
図2は、実施形態に係る制御システムの全体構成を示すブロック図である。
図2は、実施形態に係る制御システムの全体構成を示すブロック図である。
トラクタ1に備えられた制御システムは、自動操舵制御システムと耕深制御システムとを含み、図2に示すように、自動操舵制御システムは、操舵系における入力軸151を駆動する操舵駆動装置17と、センサ装置30と、自動操舵制御装置21と、図1に図示されないハンドル角制御装置22とにより構成され、耕深制御システムは、リンク機構42を介してロータリ耕耘機41を上下方向に移動する昇降駆動装置45と、接地センサ43により検出されたロータリ耕耘機41の耕深位置に基づいて昇降駆動装置45を制御する昇降制御装置46とにより構成される。
操舵駆動装置17は、入力軸151に駆動力を伝達する伝達軸171、伝達軸171を駆動するステッピングモータであるモータ172、伝達軸171の回転量及び回転位置を検知するロータリエンコーダであるエンコーダ173を備える。なお、モータ172は、入力軸151を回転させるのに十分なトルクを出力可能なモータであれば、どのような種類のモータであっても良い。
自動操舵制御装置21は、センサ装置30による測定値に基づいて操舵角を出力し、ハンドル角制御装置22は、自動操舵制御装置21により指示された操舵角に基づいて操舵駆動装置17をフィードバック制御する。ここで、操舵駆動装置17は、エンコーダ173により検知された回転位置が所望の回転位置となるようにモータ172を制御して伝達軸171を駆動させる。
このように、トラクタ1に対して、操舵駆動装置17、センサ装置30、自動操舵制御装置21及びハンドル角制御装置22を後付けで設置することによって、手動操舵されるように構成されたトラクタ1において自動操舵を実現することが可能となる。
センサ装置30は、ジャイロセンサ31、加速度センサ32、GNSSセンサ33、誤差補正装置34を備え、これらは同一の筐体内に収容される。このことは、ジャイロセンサ31、加速度センサ32、GNSSセンサ33が、トラクタ1において、略同一箇所に設けられることを意味する。ジャイロセンサ31は、3軸周りの角速度を検出してトラクタ1の方位角ξ、ピッチ角θ、ロール角φを測定する。加速度センサ32は、トラクタ1について3軸の加速度を測定する。GNSSセンサ33は、トラクタ1の位置である車両位置と、トラクタ1の速度ベクトルを測定する。誤差補正装置34は、ジャイロセンサ31、加速度センサ32、GNSSセンサ33による測定値の誤差を補正する。
自動操舵制御装置21は、GNSSセンサ33により検出されたトラクタ1の位置である車両位置と、設定された目標走行経路とに基づいて、車両位置を目標走行経路に一致させるようなトラクタ1の進行方位である目標進行方位を演算し、この目標進行方位と、GNSSセンサ33による速度ベクトルに基づくトラクタ1の進行方位との偏差である進行方位偏差に基づいて、操舵角を演算してハンドル角制御装置22へ出力する。
昇降制御装置46は、接地センサ43による耕深位置に基づいて、ロータリ耕耘機41が一定の耕深位置を維持するための昇降量を演算して昇降駆動装置45へ出力する。昇降制御装置46によれば、トラクタ1の自重により前輪11、後輪12が圃場に沈下した場合にはロータリ耕耘機41を上昇させる昇降量が出力され、前輪11、後輪12が沈下から脱した場合にはロータリ耕耘機41を下降させる昇降量が出力される。また、昇降制御装置46は、昇降量を誤差補正装置34へ出力する。
(誤差補正装置の構成)
誤差補正装置のハードウェア構成及び機能構成について説明する。図3、図4は、それぞれ、誤差補正装置のハードウェア構成、機能構成を示すブロック図である。
誤差補正装置のハードウェア構成及び機能構成について説明する。図3、図4は、それぞれ、誤差補正装置のハードウェア構成、機能構成を示すブロック図である。
誤差補正装置34は、ハードウェアとして、図3に示すように、CPU(Central Processing Unit)91、RAM(Random Access Memory)92、記憶装置93、外部I/F(Interface)94を備える。
CPU91及びRAM92は協働して後述する各種機能を実行し、記憶装置93は各種機能により実行される処理に用いられる各種データを記憶する。外部I/F94は、ジャイロセンサ31、加速度センサ32、GNSSセンサ33、自動操舵制御装置21とのデータの入出力を行う。なお、自動操舵制御装置21もCPU、RAM、記憶装置、外部I/Fを備えるものとし、誤差補正装置に代わって後述する各種機能を実行するようにしても良い。
また、誤差補正装置34は、機能として、図4に示すように、第1取得部341、第1ローパスフィルタ342a及び第2ローパスフィルタ342bを有するローパスフィルタ342、第2取得部343、設定位置補正部344、座標変換部345、車両位置補正部346、速度ベクトル補正部347を備える。
第1取得部341は、ジャイロセンサ31、加速度センサ32、GNSSセンサ33のそれぞれにより測定された測定値を取得する。第2取得部343は、昇降制御装置46により出力される昇降量を取得する。
第1ローパスフィルタ342aは、第1取得部341により取得されたジャイロセンサ31による測定値の時系列信号に対して、予め設定された第1遮断周波数より高い高周波信号を逓減させる。第2ローパスフィルタ342bは、第1取得部341により取得された加速度センサ32による測定値の時系列信号に対して、予め設定された第2遮断周波数より高い高周波信号を逓減させる。
上述したように、センサ装置30にGNSSセンサ33が含まれていることから、GNSS信号を受信するには、4つの支持フレーム19a~19dのいずれか、またはこれらに支持される屋根部19rにセンサ装置30を取り付けることが望ましい。トラクタ1などの作業車両においては、本実施形態における4つの支持フレーム19a~19dに相当するような屋根を支持する部材は剛性が十分でないため、特に作業車両が凹凸の激しい路面を走行した際などにセンサ装置30に高周波の振幅が加わることになる。このような高周波の振幅は、特にジャイロセンサ31、加速度センサ32による測定値に誤差を生じさせることとなるが、ローパスフィルタ342によれば、このような誤差を低減させることができる。
座標変換部345は、センサ座標系における相対位置ベクトル及び角速度ベクトルのそれぞれをローカル座標系におけるベクトルに座標変換する。ここで、センサ座標系は、センサ装置30が取り付けられた位置であり、センサ装置30による測定がなされる位置である測定位置を原点として互いに直交するx,y,zの3軸を有する座標系であり、x軸はトラクタ1の直進方向前側に向き、y軸はトラクタ1の幅方向右側に向き、z軸はトラクタ1の上下方向下側に向くものとする。ローカル座標系は、北、東、下を向く3軸を有する座標系である。なお、相対位置ベクトル、角速度ベクトルについては後に詳述する。
車両位置補正部346は、トラクタ1における上方、本実施形態においては屋根部19rに取り付けられたセンサ装置30の測定位置においてGNSSセンサ33により測定されたローカル座標系におけるトラクタ1の車両位置を、センサ座標系において予め設定された設定位置におけるトラクタ1の車両位置に補正する。
設定位置は、センサ装置30の測定位置をトラクタ1の走行面、即ち圃場表面に投影した位置に設定される。本実施形態において、設定位置は、y方向においてトラクタ1の幅方向中心に位置し、z軸方向において前輪11、後輪12の下端またはこの下端より上方の下端近傍部に位置するように設定される。
速度ベクトル補正部347は、センサ装置30の測定位置においてGNSSセンサ33によるローカル座標系におけるトラクタ1の速度ベクトルを、設定位置における速度ベクトルに補正する。
(誤差補正装置の動作)
誤差補正装置の動作について説明する。図5は、誤差補正装置の動作を示すフローチャートである。図6、図7は、それぞれ、相対位置ベクトルを示す概略正面図、概略平面図である。図8は、速度ベクトルを示す概略正面図である。なお、図5に示す動作は、所定の周期毎に実行されるものとする。
誤差補正装置の動作について説明する。図5は、誤差補正装置の動作を示すフローチャートである。図6、図7は、それぞれ、相対位置ベクトルを示す概略正面図、概略平面図である。図8は、速度ベクトルを示す概略正面図である。なお、図5に示す動作は、所定の周期毎に実行されるものとする。
図5に示すように、まず、第1取得部341は、GNSSセンサ33により測定されたローカル座標系における車両位置と速度ベクトルとを取得し(S101)、ジャイロセンサ31により測定されたトラクタ1の角速度と、加速度センサ32により測定されたトラクタ1の加速度とを取得する(S102)
次に、ローパスフィルタ342は、第1取得部341により取得された角速度及び加速度のそれぞれについて、予め設定された第1遮断周波数、第2遮断周波数より高い高周波信号を逓減させる(S103)。
次に、第2取得部343が、昇降制御装置46により出力される昇降量を取得し(S104)、設定位置補正部344が、第2取得部343により取得された昇降量に基づいて設定位置におけるz軸上の位置を補正する(S105)。
ここで、設定位置補正部344による設定位置の補正について説明する。設定位置は、上述したように、センサ装置30の測定位置を圃場表面に投影した位置として、センサ座標系のz軸上において、トラクタ1の前輪11、後輪12の下端近傍に予め設定される位置であるが、前輪11、後輪12の少なくともいずれかが圃場に大きく沈下する場合、測定位置とトラクタ1の走行面としての圃場の表面とのz軸上の相対距離は、測定位置と設定位置とのz軸上の相対距離より小さいものとなる。また、同一の圃場であっても、その状態は必ずしも全体で均一となっておらず、位置によって状態が異なるため、トラクタ1の走行中に測定位置と圃場表面とのz軸上の相対距離が変動することとなる。
設定位置補正部344は、昇降量に基づいて、圃場表面と一定の距離を保つように昇降制御されるロータリ耕耘機41の上下方向位置を算出し、この上下方向位置が高くなるほど、測定位置と設定位置との相対距離が小さくなるように設定位置のz軸上の位置を補正する。これによって、前輪11、後輪12が圃場に沈下するような場合であっても、設定位置を圃場表面に投影した位置とすることができる。
座標変換部345は、図5及び図6に示すような、センサ座標系において設定位置aを始点とする測定位置sの相対位置ベクトルLsを、ジャイロセンサ31により測定されるトラクタ1の方位角、ピッチ角、ロール角に基づいて、L=CLsの式によりローカル座標系における相対位置ベクトルLに座標変換する(S106)。ここで、Cは、センサ座標系からローカル座標系への座標変換を行うための座標変換行列を示し、
また、座標変換に先立って、一般的にアライメントと呼ばれる初期化処理がなされる。この初期化処理によれば、トラクタ1の初期方位(方位角)と初期姿勢(ピッチ角度及びロール角)が決定され、座標変換部345による座標変換において、この初期方位及び初期姿勢がジャイロセンサ31により計測される角速度により更新された方位角ξ、ピッチ角θ、ロール角φが座標変換行列Cに与えられる。
初期姿勢におけるピッチ角度θとロール角φは、トラクタ1が静止した状態において加速度センサ32により測定される重力加速度をgとし、3軸の加速度のそれぞれをax,ay,azとして、以下の式から求められる。
また、初期方位としての方位角ξは、トラクタ1の走行中においてGNSSセンサ33により測定される速度ベクトルに基づいて設定される。なお、初期姿勢及び初期方位は、予め設定された値を用いても良い。
座標変換後、車両位置補正部346は、このローカル座標系の原点Oを始点とした設定位置aの位置ベクトルをPa、測定位置sの位置ベクトルをPsとして、Pa=Ps-Lの式により、位置ベクトルPaを算出することにより、測定位置sにおいて測定された車両位置を、設定位置aにおける車両位置に補正する(S107)。
次に、座標変換部345は、ジャイロセンサ31により測定されるセンサ座標系におけるトラクタ1の回転角速度ベクトルωsを、座標変換行列Cを用いて、ω=Cωsの式によりローカル座標系における回転角速度ベクトルωに座標変換する(S108)。
座標変換後、速度ベクトル補正部347は、GNSSセンサ33により測定された測定位置sにおけるトラクタ1の速度ベクトルをVsとし、設定位置aにおける速度ベクトルをVaとして、Va=Vs-ω×Lの式により、速度ベクトルVaを算出することにより、測定位置sにおいて測定された速度ベクトルVsを、設定位置aにおける速度ベクトルVaに補正する(S109)。
このように、誤差補正装置34は、測定位置sにおいて測定された車両位置、速度ベクトルのそれぞれを、設定位置aにおける車両位置、速度ベクトルに補正する。補正された車両位置及び速度ベクトルによれば、特に凹凸がある圃場をトラクタ1が走行した場合に生じるセンサ装置30の振れに起因する測定値の誤差を低減した、より正確な車両位置、進行方位及び進行速度を自動操舵制御装置21が得ることができる。これによって、自動操舵制御装置21による正確なトラクタ1の自動操舵が達成される。
なお、誤差補正装置34により測定値の誤差が補正されるセンサ装置30が設けられる作業車両としては、トラクタ1以外に、田植機、コンバイン、野菜移植機、野菜収穫機、芝刈り機など、圃場に接地されるように昇降制御される作業機が設けられる全ての作業車両が挙げられる。
本発明の実施形態は、例として提示したものであり、発明の範囲を限定することは意図していない。この新規な実施形態は、その他の様々な形態で実施されることが可能であり、発明の要旨を逸脱しない範囲で、種々の省略、置き換え、変更を行うことができる。これら実施形態やその変形は、発明の範囲や要旨に含まれるとともに、特許請求の範囲に記載された発明とその均等の範囲に含まれる。
34 誤差補正装置
341 第1取得部
345 座標変換部
346 車両位置補正部
341 第1取得部
345 座標変換部
346 車両位置補正部
Claims (5)
- 作業車両に設けられてGNSSセンサとジャイロセンサとを備えたセンサ装置による測定値の誤差を補正する誤差補正装置であって、
前記GNSSセンサにより測定されたローカル座標系における前記作業車両の車両位置と、前記ジャイロセンサにより測定された前記センサ装置を基準とした3軸を有するセンサ座標系における角速度とを取得する第1取得部と、
前記センサ座標系において、前記センサ装置と異なる位置に設定された設定位置を始点とした前記センサ装置が設けられた測定位置の相対位置ベクトルを、前記取得された角速度に基づいて前記ローカル座標系における相対位置ベクトルに座標変換する座標変換部と、
前記ローカル座標系の原点を始点とした前記設定位置の位置ベクトルをPa、前記測定位置の位置ベクトルをPs、前記座標変換された相対位置ベクトルをLとして、Pa=Ps-Lの式によって前記設定位置の位置ベクトルPaを算出して前記GNSSセンサにより測定された車両位置の誤差を補正する車両位置補正部と
を備える誤差補正装置。 - 前記作業車両は圃場に接地するように昇降制御される作業機を備え、
前記センサ座標系における1軸は前記作業車両の上下方向であり、
前記設定位置は前記測定位置を前記圃場の表面に投影する位置に予め設定され、
前記作業機の昇降制御に係る昇降量を取得する第2取得部と、
前記取得された昇降量に基づいて前記設定位置を補正する設定位置補正部とを更に備え、
前記座標変換部は、補正された設定位置を始点とする測定位置の相対位置ベクトルを座標変換することを特徴とする請求項1に記載の誤差補正装置。 - 前記第1取得部は、前記GNSSセンサにより測定された前記ローカル座標系における前記作業車両の速度ベクトルを更に取得し、
前記座標変換部は、更に、前記取得された角速度に基づく前記センサ座標系における前記作業車両の回転角速度ベクトルを、前記取得された角速度に基づいて前記ローカル座標系における回転角速度ベクトルに座標変換し、
前記測定位置における前記作業車両の速度ベクトルをVs、前記設定位置における前記作業車両の速度ベクトルをVa、前記座標変換された回転角速度ベクトルをωとして、Va=Vs-ω×Lの式によって前記設定位置における前記作業車両の速度ベクトルを算出し、前記GNSSセンサにより測定された前記作業車両の速度ベクトルを補正する速度ベクトル補正部を更に備えることを特徴とする請求項1または請求項2に記載の誤差補正装置。 - 前記ジャイロセンサにより測定された角速度の時系列信号に対して、予め設定された第1遮断周波数より高い高周波信号を逓減させる第1ローパスフィルタを更に備え、
前記座標変換部は、前記高周波信号が逓減された角速度に基づいて前記相対位置ベクトルを座標変換することを特徴とする請求項1~請求項3のいずれか一項に記載の誤差補正装置。 - 前記センサ装置は更に加速度センサを備え、
前記加速度センサにより測定された加速度の時系列信号に対して、予め設定された第2遮断周波数より高い高周波信号を逓減させる第2ローパスフィルタを更に備えることを特徴とする請求項1~請求項4のいずれか一項に記載の誤差補正装置。
Applications Claiming Priority (3)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
JP2018203759 | 2018-10-30 | ||
JP2018203759 | 2018-10-30 | ||
PCT/JP2019/042504 WO2020090863A1 (ja) | 2018-10-30 | 2019-10-30 | 誤差補正装置 |
Publications (2)
Publication Number | Publication Date |
---|---|
JPWO2020090863A1 JPWO2020090863A1 (ja) | 2021-09-30 |
JP7269255B2 true JP7269255B2 (ja) | 2023-05-08 |
Family
ID=70462085
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
JP2020553966A Active JP7269255B2 (ja) | 2018-10-30 | 2019-10-30 | 誤差補正装置 |
Country Status (5)
Country | Link |
---|---|
EP (1) | EP3874926B1 (ja) |
JP (1) | JP7269255B2 (ja) |
KR (1) | KR20210083247A (ja) |
CN (1) | CN113038821B (ja) |
WO (1) | WO2020090863A1 (ja) |
Families Citing this family (2)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN114771656A (zh) * | 2022-05-11 | 2022-07-22 | 山东理工大学 | 一种拖拉机前轮转向角测量方法及系统 |
CN116972854B (zh) * | 2023-09-20 | 2023-12-26 | 无锡新松机器人自动化有限公司 | 基于gps定位的农机导航路径规划方法及系统 |
Citations (4)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
JP2003303021A (ja) | 2002-04-10 | 2003-10-24 | Mitsubishi Agricult Mach Co Ltd | 作業用走行車 |
US20080195268A1 (en) | 2007-02-09 | 2008-08-14 | Novariant, Inc. | Implement control system and method of using same |
JP2016146061A (ja) | 2015-02-06 | 2016-08-12 | 国立研究開発法人農業・食品産業技術総合研究機構 | 走行制御装置 |
US20170354079A1 (en) | 2016-06-10 | 2017-12-14 | Cnh Industrial America Llc | Planning and control of autonomous agricultural operations |
Family Cites Families (17)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
US8140223B2 (en) * | 2003-03-20 | 2012-03-20 | Hemisphere Gps Llc | Multiple-antenna GNSS control system and method |
US7477973B2 (en) * | 2005-10-15 | 2009-01-13 | Trimble Navigation Ltd | Vehicle gyro based steering assembly angle and angular rate sensor |
JP4876204B2 (ja) * | 2006-01-05 | 2012-02-15 | ヒロボー株式会社 | 小型姿勢センサ |
US7418328B2 (en) * | 2006-05-08 | 2008-08-26 | Deere & Company | Steering logic for self-propelled mower |
US7844378B2 (en) * | 2006-10-05 | 2010-11-30 | Trimble Navigation Limited | Farm apparatus having implement sidehill drift compensation |
JP2012154769A (ja) * | 2011-01-26 | 2012-08-16 | Seiko Epson Corp | 加速度検出方法、位置算出方法及び加速度検出装置 |
JP2013145168A (ja) * | 2012-01-13 | 2013-07-25 | Denso Corp | 車載用ジャイロの角速度誤差補正装置 |
CN102790068B (zh) * | 2012-07-26 | 2014-10-22 | 北京京东方光电科技有限公司 | 一种传感器的制造方法 |
US9708001B2 (en) * | 2013-03-29 | 2017-07-18 | Tokyo Keiki Inc. | Automatic steering system for working vehicle |
US9880560B2 (en) * | 2013-09-16 | 2018-01-30 | Deere & Company | Vehicle auto-motion control system |
BR102015013228B1 (pt) * | 2014-06-13 | 2020-11-24 | Cnh Industrial America Llc | SISTEMA E METODO DE CONTROLE PARA UM VEfCULO AGRiCOLA |
JP6055528B1 (ja) * | 2015-09-25 | 2016-12-27 | 富士重工業株式会社 | 車両の操舵制御装置 |
CN105203098B (zh) * | 2015-10-13 | 2018-10-02 | 上海华测导航技术股份有限公司 | 基于九轴mems传感器的农业机械全姿态角更新方法 |
JP6473684B2 (ja) * | 2015-11-11 | 2019-02-20 | 日立建機株式会社 | 車輪の滑り角推定装置及びその方法 |
CN109154817B (zh) * | 2016-05-30 | 2021-09-24 | 株式会社久保田 | 自动行驶作业车辆 |
JP2018025924A (ja) * | 2016-08-09 | 2018-02-15 | ヤンマー株式会社 | 自律走行システム |
JP6994831B2 (ja) | 2017-01-24 | 2022-01-14 | 株式会社クボタ | 自動操舵システム |
-
2019
- 2019-10-30 KR KR1020217006482A patent/KR20210083247A/ko unknown
- 2019-10-30 EP EP19878452.2A patent/EP3874926B1/en active Active
- 2019-10-30 CN CN201980071147.0A patent/CN113038821B/zh active Active
- 2019-10-30 WO PCT/JP2019/042504 patent/WO2020090863A1/ja unknown
- 2019-10-30 JP JP2020553966A patent/JP7269255B2/ja active Active
Patent Citations (4)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
JP2003303021A (ja) | 2002-04-10 | 2003-10-24 | Mitsubishi Agricult Mach Co Ltd | 作業用走行車 |
US20080195268A1 (en) | 2007-02-09 | 2008-08-14 | Novariant, Inc. | Implement control system and method of using same |
JP2016146061A (ja) | 2015-02-06 | 2016-08-12 | 国立研究開発法人農業・食品産業技術総合研究機構 | 走行制御装置 |
US20170354079A1 (en) | 2016-06-10 | 2017-12-14 | Cnh Industrial America Llc | Planning and control of autonomous agricultural operations |
Also Published As
Publication number | Publication date |
---|---|
EP3874926A1 (en) | 2021-09-08 |
EP3874926A4 (en) | 2021-12-29 |
JPWO2020090863A1 (ja) | 2021-09-30 |
EP3874926B1 (en) | 2022-12-21 |
CN113038821A (zh) | 2021-06-25 |
KR20210083247A (ko) | 2021-07-06 |
WO2020090863A1 (ja) | 2020-05-07 |
CN113038821B (zh) | 2022-12-06 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
JP6525902B2 (ja) | 圃場作業車両 | |
JP6552420B2 (ja) | 作業車 | |
US11760293B2 (en) | Tractor and working vehicle | |
EP3467606B1 (en) | Autonomous work vehicle | |
US11780499B2 (en) | Working vehicle and tractor | |
JP6860387B2 (ja) | 作業車 | |
KR20170024028A (ko) | 병주 작업 시스템 | |
JP7269255B2 (ja) | 誤差補正装置 | |
JP6811655B2 (ja) | 作業車 | |
JP2017112962A (ja) | 作業車 | |
JP7072496B2 (ja) | 自動走行作業車のための制御装置 | |
JP6850759B2 (ja) | 自律操舵装置 | |
JP6887370B2 (ja) | トラクタ | |
JP2019168939A (ja) | 自律走行システム | |
JP6815980B2 (ja) | 作業車 | |
WO2021187594A1 (ja) | 誤差補正装置 | |
JP6820828B2 (ja) | トラクタ | |
JP3949078B2 (ja) | 農作業機のローリング制御装置 | |
JP3669947B2 (ja) | 作業機のローリング制御装置 | |
JP3838959B2 (ja) | 農作業機のローリング制御装置 | |
WO2022024528A1 (ja) | 作業機械 | |
JP6546362B2 (ja) | 作業車 | |
JP7134217B2 (ja) | 作業車 | |
JP2004113028A (ja) | 農作業機のローリング制御装置 | |
JP2019162149A (ja) | 作業車 |
Legal Events
Date | Code | Title | Description |
---|---|---|---|
A621 | Written request for application examination |
Free format text: JAPANESE INTERMEDIATE CODE: A621 Effective date: 20220805 |
|
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: 20230418 |
|
A61 | First payment of annual fees (during grant procedure) |
Free format text: JAPANESE INTERMEDIATE CODE: A61 Effective date: 20230421 |
|
R150 | Certificate of patent or registration of utility model |
Ref document number: 7269255 Country of ref document: JP Free format text: JAPANESE INTERMEDIATE CODE: R150 |