(19)【発行国】日本国特許庁(JP)
(12)【公報種別】特許公報(B2)
(11)【特許番号】
(24)【登録日】2023-04-25
(45)【発行日】2023-05-08
(54)【発明の名称】誤差補正装置
(51)【国際特許分類】
G05D 1/02 20200101AFI20230426BHJP
A01B 69/00 20060101ALI20230426BHJP
B62D 6/00 20060101ALI20230426BHJP
【ＦＩ】
G05D1/02 N
A01B69/00 303M
A01B69/00 303G
B62D6/00
【請求項の数】
5
(21)【出願番号】P 2020553966
(86)(22)【出願日】2019-10-30
(86)【国際出願番号】 JP2019042504
(87)【国際公開番号】W WO2020090863
(87)【国際公開日】2020-05-07
【審査請求日】2022-08-05
(31)【優先権主張番号】P 2018203759
(32)【優先日】2018-10-30
(33)【優先権主張国・地域又は機関】JP
(73)【特許権者】
【識別番号】000003388
【氏名又は名称】東京計器株式会社
(74)【代理人】
【識別番号】100101856
【弁理士】
【氏名又は名称】赤澤 日出夫
(72)【発明者】
【氏名】吉田 豊織
(72)【発明者】
【氏名】荒金 宏臣
(72)【発明者】
【氏名】船山 正行
(72)【発明者】
【氏名】岡村 信行
(72)【発明者】
【氏名】内田 朗忍
(72)【発明者】
【氏名】嘉屋 和樹
【審査官】田中 友章
(56)【参考文献】
【文献】特開２００３－３０３０２１（ＪＰ，Ａ）
【文献】特開２０１６－１４６０６１（ＪＰ，Ａ）
【文献】米国特許出願公開第２００８／０１９５２６８（ＵＳ，Ａ１）
【文献】米国特許出願公開第２０１７／０３５４０７９（ＵＳ，Ａ１）
(58)【調査した分野】(Int.Cl.，ＤＢ名)
Ｇ０５Ｄ １／０２
Ａ０１Ｂ ６９／００
Ｂ６２Ｄ ６／００
(57)【特許請求の範囲】
【請求項1】
作業車両に設けられてＧＮＳＳセンサとジャイロセンサとを備えたセンサ装置による測定値の誤差を補正する誤差補正装置であって、前記ＧＮＳＳセンサにより測定されたローカル座標系における前記作業車両の車両位置と、前記ジャイロセンサにより測定された前記センサ装置を基準とした３軸を有するセンサ座標系における角速度とを取得する第１取得部と、
前記センサ座標系において、前記センサ装置と異なる位置に設定された設定位置を始点とした前記センサ装置が設けられた測定位置の相対位置ベクトルを、前記取得された角速度に基づいて前記ローカル座標系における相対位置ベクトルに座標変換する座標変換部と、
前記ローカル座標系の原点を始点とした前記設定位置の位置ベクトルをＰａ、前記測定位置の位置ベクトルをＰｓ、前記座標変換された相対位置ベクトルをＬとして、Ｐａ＝Ｐｓ－Ｌの式によって前記設定位置の位置ベクトルＰａを算出して前記ＧＮＳＳセンサにより測定された車両位置の誤差を補正する車両位置補正部と
を備える誤差補正装置。
【請求項2】
前記作業車両は圃場に接地するように昇降制御される作業機を備え、前記センサ座標系における１軸は前記作業車両の上下方向であり、
前記設定位置は前記測定位置を前記圃場の表面に投影する位置に予め設定され、
前記作業機の昇降制御に係る昇降量を取得する第２取得部と、
前記取得された昇降量に基づいて前記設定位置を補正する設定位置補正部とを更に備え、
前記座標変換部は、補正された設定位置を始点とする測定位置の相対位置ベクトルを座標変換することを特徴とする請求項１に記載の誤差補正装置。
【請求項3】
前記第１取得部は、前記ＧＮＳＳセンサにより測定された前記ローカル座標系における前記作業車両の速度ベクトルを更に取得し、前記座標変換部は、更に、前記取得された角速度に基づく前記センサ座標系における前記作業車両の回転角速度ベクトルを、前記取得された角速度に基づいて前記ローカル座標系における回転角速度ベクトルに座標変換し、
前記測定位置における前記作業車両の速度ベクトルをＶｓ、前記設定位置における前記作業車両の速度ベクトルをＶａ、前記座標変換された回転角速度ベクトルをωとして、Ｖａ＝Ｖｓ－ω×Ｌの式によって前記設定位置における前記作業車両の速度ベクトルを算出し、前記ＧＮＳＳセンサにより測定された前記作業車両の速度ベクトルを補正する速度ベクトル補正部を更に備えることを特徴とする請求項１または請求項２に記載の誤差補正装置。
【請求項4】
前記ジャイロセンサにより測定された角速度の時系列信号に対して、予め設定された第１遮断周波数より高い高周波信号を逓減させる第１ローパスフィルタを更に備え、前記座標変換部は、前記高周波信号が逓減された角速度に基づいて前記相対位置ベクトルを座標変換することを特徴とする請求項１～請求項３のいずれか一項に記載の誤差補正装置。
【請求項5】
前記センサ装置は更に加速度センサを備え、前記加速度センサにより測定された加速度の時系列信号に対して、予め設定された第２遮断周波数より高い高周波信号を逓減させる第２ローパスフィルタを更に備えることを特徴とする請求項１～請求項４のいずれか一項に記載の誤差補正装置。
【発明の詳細な説明】
【技術分野】
【0001】
本発明の実施形態は、作業車両に設けられたセンサ装置による測定値の補正に関する。
【背景技術】
【0002】
従来、農作業の効率化を目的として、トラクタや田植機などの作業車両を指定した走行経路に追従させる自動操舵制御が行われている。この自動操舵制御によれば、目標とする走行経路上の位置と作業車両の位置との離間距離に基づいて、作業車両の位置を走行経路上に位置付けるように作業車両の操舵が制御される。
【0003】
また、作業車両の自動操舵に関する技術として、衛星からの衛星情報に基づいて位置情報を出力する衛星測位モジュールと、前進走行における作業車の操舵制御の基準位置となる前進基準位置を位置情報に基づいて算出する前進基準位置算出部と、後進走行における作業車の操舵制御の基準位置となる後進基準位置を位置情報に基づいて算出する後進基準位置算出部と、前進走行時に走行経路と前進基準位置との偏差に基づいて算出された前進用操舵制御信号を出力するとともに、後進走行時に走行経路と後進基準位置との偏差に基づいて算出された後進用操舵制御信号を出力する操舵制御部と、前進用操舵制御信号及び後進用操舵制御信号に基づいて前記作業車の操舵を行う操舵機構とを備えた自動操舵システム、が知られている（例えば、特許文献１参照）。
【先行技術文献】
【特許文献】
【0004】
【文献】特開２０１８－１２０３６４号公報
【発明の概要】
【発明が解決しようとする課題】
【0005】
このような自動操舵制御において、作業車両の車両位置を測定するＧＮＳＳ（Ｇｌｏｂａｌ Ｎａｖｉｇａｔｉｏｎ Ｓａｔｅｌｌｉｔｅ Ｓｙｓｔｅｍ）センサは、衛星からＧＮＳＳ信号を受信するのに適切な位置に設けられる。作業車両におけるＧＮＳＳ信号の受信に適切な位置は、本体から上方に延びるように設けられたアームの上端部分、またはこのアームによりその上端に接続されて支持される屋根部である。
【0006】
また、作業車両が起伏の激しい圃場を走行する場合にはその姿勢が傾斜することとなる。この際、ＧＮＳＳセンサが上方に取り付けられているために、圃場上の作業車両の位置に対して、ＧＮＳＳセンサにより測定される位置に誤差が生じ、延いては、この誤差によって自動操舵制御の精度が低下する、という問題がある。
【0007】
本発明は、上述した問題点を解決するためになされたものであり、ＧＮＳＳセンサにより測定された作業車両の車両位置の誤差を低減することができる誤差補正装置を提供することを目的とする。
【課題を解決するための手段】
【0008】
上述した課題を解決するため、本実施形態に係る誤差補正装置は、作業車両に設けられてＧＮＳＳセンサとジャイロセンサとを備えたセンサ装置による測定値の誤差を補正する誤差補正装置であって、前記ＧＮＳＳセンサにより測定されたローカル座標系における前記作業車両の車両位置と、前記ジャイロセンサにより測定された前記センサ装置を基準とした３軸を有するセンサ座標系における角速度とを取得する第１取得部と、前記センサ座標系において、前記センサ装置と異なる位置に設定された設定位置を始点とした前記センサ装置が設けられた測定位置の相対位置ベクトルを、前記取得された角速度に基づいて前記ローカル座標系における相対位置ベクトルに座標変換する座標変換部と、前記ローカル座標系の原点を始点とした前記設定位置の位置ベクトルをＰａ、前記測定位置の位置ベクトルをＰｓ、前記座標変換された相対位置ベクトルをＬとして、Ｐａ＝Ｐｓ－Ｌの式によって前記設定位置の位置ベクトルＰａを算出して前記ＧＮＳＳセンサにより測定された車両位置の誤差を補正する車両位置補正部とを備える。
【発明の効果】
【0009】
本発明によれば、ＧＮＳＳセンサにより測定された作業車両の車両位置の誤差を低減することができる。
【図面の簡単な説明】
【0010】
【図1】実施形態に係る農業用トラクタの構成を示す概略側面図である。
【図2】実施形態に係る制御システムの全体構成を示すブロック図である。
【図3】誤差補正装置のハードウェア構成を示すブロック図である。
【図4】誤差補正装置の機能構成を示すブロック図である。
【図5】誤差補正装置の動作を示すフローチャートである。
【図6】相対位置ベクトルを示す概略正面図である。
【図7】相対位置ベクトルを示す概略平面図である。
【図8】速度ベクトルを示す概略正面図である。
【発明を実施するための形態】
【0011】
以下、図面を参照しながら、本発明の実施形態について説明する。
【0012】
（作業車両の構成）
まず、本実施形態に係る誤差補正装置を備える作業車両について説明する。図１は、実施形態に係る農業用トラクタの構成を示す概略側面図である。
まず、本実施形態に係る誤差補正装置を備える作業車両について説明する。図１は、実施形態に係る農業用トラクタの構成を示す概略側面図である。
【0013】
本実施形態に係る自動操舵システムの操舵対象としての車両は、作業車両であり、具体的には、図１に示すような農業用のトラクタ１とする。このトラクタ１は、車体１０と、２つの前輪１１と２つの後輪１２とを備える四輪車両とするが、操舵可能な車両であれば、作業車両に限らずどのような車両であっても良い。また、トラクタ１は、運転者が座るための座席１３、ステアリングコラム１５、ステアリングハンドル１６、操舵駆動装置１７、アクセル、ブレーキ等を含むペダル類１８、４つの支持フレーム１９ａ～１９ｄ、屋根部１９ｒを備える。
【0014】
ステアリングコラム１５内には、前輪１１に偏角を与えてトラクタ１を操舵する操舵系が備えられるとともに、ステアリングハンドル１６または操舵駆動装置１７によるハンドル角を操舵系に入力するための入力軸１５１が内蔵され、この入力軸１５１の回転に基づく操舵角が前輪１１に与えられる。操舵駆動装置１７は、自動操舵制御をするための構成が備えられていない、手動操舵を前提としたトラクタ１を自動操舵するために後付けされる装置であり、上方においては、ステアリングハンドル１６が取り付けられるとともに、下方において、操舵系の入力軸１５１の上端部が嵌合される。車体１０上には、それぞれが長尺に形成された４つの支持フレーム１９ａ～１９ｄが起立した状態で前後左右の異なる位置に設けられ、これら４つの支持フレーム１９ａ～１９ｄ上には、全体として略平板上に形成された屋根部１９ｒが載置される。
【0015】
屋根部１９ｒの上面には、この屋根部１９ｒを架台としてセンサ装置３０が設けられる。このセンサ装置３０は、後述するようにＧＮＳＳセンサを含むことから、人工衛星から送信されるＧＮＳＳ信号を受信するために、屋根部１９ｒのような、トラクタ１における最上部に取り付けることが望ましく、トラクタ１が屋根部１９ｒを備えない場合は、屋根部１９ｒを支持する４つの支持フレーム１９ａ～１９ｄのいずれかを架台としてセンサ装置３０が設けられるものとする。また、前方に位置する支持フレーム１９ａ，１９ｂのいずれかには、自動操舵制御装置２１が設けられるものとする。この自動操舵制御装置２１は、トラクタ１の自動操舵を制御するものである。
【0016】
また、トラクタ１は、作業車両に設けられる作業機としてのロータリ耕耘機４１と、このロータリ耕耘機４１を昇降可能にトラクタ１と連結するリンク機構４２と、ロータリ耕耘機４１と圃場との距離を耕深位置として検出する接地センサ４３とを備える。接地センサ４３は、下端部が圃場に設置するように、上端部を支点として上下に揺動可能に設けられた接地部４３１と、この接地部４３１の揺動変位を検出する検知部４３２とを備える。
【0017】
（制御システムの構成）
図２は、実施形態に係る制御システムの全体構成を示すブロック図である。
図２は、実施形態に係る制御システムの全体構成を示すブロック図である。
【0018】
トラクタ１に備えられた制御システムは、自動操舵制御システムと耕深制御システムとを含み、図２に示すように、自動操舵制御システムは、操舵系における入力軸１５１を駆動する操舵駆動装置１７と、センサ装置３０と、自動操舵制御装置２１と、図１に図示されないハンドル角制御装置２２とにより構成され、耕深制御システムは、リンク機構４２を介してロータリ耕耘機４１を上下方向に移動する昇降駆動装置４５と、接地センサ４３により検出されたロータリ耕耘機４１の耕深位置に基づいて昇降駆動装置４５を制御する昇降制御装置４６とにより構成される。
【0019】
操舵駆動装置１７は、入力軸１５１に駆動力を伝達する伝達軸１７１、伝達軸１７１を駆動するステッピングモータであるモータ１７２、伝達軸１７１の回転量及び回転位置を検知するロータリエンコーダであるエンコーダ１７３を備える。なお、モータ１７２は、入力軸１５１を回転させるのに十分なトルクを出力可能なモータであれば、どのような種類のモータであっても良い。
【0020】
自動操舵制御装置２１は、センサ装置３０による測定値に基づいて操舵角を出力し、ハンドル角制御装置２２は、自動操舵制御装置２１により指示された操舵角に基づいて操舵駆動装置１７をフィードバック制御する。ここで、操舵駆動装置１７は、エンコーダ１７３により検知された回転位置が所望の回転位置となるようにモータ１７２を制御して伝達軸１７１を駆動させる。
【0021】
このように、トラクタ１に対して、操舵駆動装置１７、センサ装置３０、自動操舵制御装置２１及びハンドル角制御装置２２を後付けで設置することによって、手動操舵されるように構成されたトラクタ１において自動操舵を実現することが可能となる。
【0022】
センサ装置３０は、ジャイロセンサ３１、加速度センサ３２、ＧＮＳＳセンサ３３、誤差補正装置３４を備え、これらは同一の筐体内に収容される。このことは、ジャイロセンサ３１、加速度センサ３２、ＧＮＳＳセンサ３３が、トラクタ１において、略同一箇所に設けられることを意味する。ジャイロセンサ３１は、３軸周りの角速度を検出してトラクタ１の方位角ξ、ピッチ角θ、ロール角φを測定する。加速度センサ３２は、トラクタ１について３軸の加速度を測定する。ＧＮＳＳセンサ３３は、トラクタ１の位置である車両位置と、トラクタ１の速度ベクトルを測定する。誤差補正装置３４は、ジャイロセンサ３１、加速度センサ３２、ＧＮＳＳセンサ３３による測定値の誤差を補正する。
【0023】
自動操舵制御装置２１は、ＧＮＳＳセンサ３３により検出されたトラクタ１の位置である車両位置と、設定された目標走行経路とに基づいて、車両位置を目標走行経路に一致させるようなトラクタ１の進行方位である目標進行方位を演算し、この目標進行方位と、ＧＮＳＳセンサ３３による速度ベクトルに基づくトラクタ１の進行方位との偏差である進行方位偏差に基づいて、操舵角を演算してハンドル角制御装置２２へ出力する。
【0024】
昇降制御装置４６は、接地センサ４３による耕深位置に基づいて、ロータリ耕耘機４１が一定の耕深位置を維持するための昇降量を演算して昇降駆動装置４５へ出力する。昇降制御装置４６によれば、トラクタ１の自重により前輪１１、後輪１２が圃場に沈下した場合にはロータリ耕耘機４１を上昇させる昇降量が出力され、前輪１１、後輪１２が沈下から脱した場合にはロータリ耕耘機４１を下降させる昇降量が出力される。また、昇降制御装置４６は、昇降量を誤差補正装置３４へ出力する。
【0025】
【0026】
誤差補正装置３４は、ハードウェアとして、図３に示すように、ＣＰＵ（Ｃｅｎｔｒａｌ Ｐｒｏｃｅｓｓｉｎｇ Ｕｎｉｔ）９１、ＲＡＭ（Ｒａｎｄｏｍ Ａｃｃｅｓｓ Ｍｅｍｏｒｙ）９２、記憶装置９３、外部Ｉ／Ｆ（Ｉｎｔｅｒｆａｃｅ）９４を備える。
【0027】
ＣＰＵ９１及びＲＡＭ９２は協働して後述する各種機能を実行し、記憶装置９３は各種機能により実行される処理に用いられる各種データを記憶する。外部Ｉ／Ｆ９４は、ジャイロセンサ３１、加速度センサ３２、ＧＮＳＳセンサ３３、自動操舵制御装置２１とのデータの入出力を行う。なお、自動操舵制御装置２１もＣＰＵ、ＲＡＭ、記憶装置、外部Ｉ／Ｆを備えるものとし、誤差補正装置に代わって後述する各種機能を実行するようにしても良い。
【0028】
また、誤差補正装置３４は、機能として、図４に示すように、第１取得部３４１、第１ローパスフィルタ３４２ａ及び第２ローパスフィルタ３４２ｂを有するローパスフィルタ３４２、第２取得部３４３、設定位置補正部３４４、座標変換部３４５、車両位置補正部３４６、速度ベクトル補正部３４７を備える。
【0029】
第１取得部３４１は、ジャイロセンサ３１、加速度センサ３２、ＧＮＳＳセンサ３３のそれぞれにより測定された測定値を取得する。第２取得部３４３は、昇降制御装置４６により出力される昇降量を取得する。
【0030】
第１ローパスフィルタ３４２ａは、第１取得部３４１により取得されたジャイロセンサ３１による測定値の時系列信号に対して、予め設定された第１遮断周波数より高い高周波信号を逓減させる。第２ローパスフィルタ３４２ｂは、第１取得部３４１により取得された加速度センサ３２による測定値の時系列信号に対して、予め設定された第２遮断周波数より高い高周波信号を逓減させる。
【0031】
上述したように、センサ装置３０にＧＮＳＳセンサ３３が含まれていることから、ＧＮＳＳ信号を受信するには、４つの支持フレーム１９ａ～１９ｄのいずれか、またはこれらに支持される屋根部１９ｒにセンサ装置３０を取り付けることが望ましい。トラクタ１などの作業車両においては、本実施形態における４つの支持フレーム１９ａ～１９ｄに相当するような屋根を支持する部材は剛性が十分でないため、特に作業車両が凹凸の激しい路面を走行した際などにセンサ装置３０に高周波の振幅が加わることになる。このような高周波の振幅は、特にジャイロセンサ３１、加速度センサ３２による測定値に誤差を生じさせることとなるが、ローパスフィルタ３４２によれば、このような誤差を低減させることができる。
【0032】
座標変換部３４５は、センサ座標系における相対位置ベクトル及び角速度ベクトルのそれぞれをローカル座標系におけるベクトルに座標変換する。ここで、センサ座標系は、センサ装置３０が取り付けられた位置であり、センサ装置３０による測定がなされる位置である測定位置を原点として互いに直交するｘ，ｙ，ｚの３軸を有する座標系であり、ｘ軸はトラクタ１の直進方向前側に向き、ｙ軸はトラクタ１の幅方向右側に向き、ｚ軸はトラクタ１の上下方向下側に向くものとする。ローカル座標系は、北、東、下を向く３軸を有する座標系である。なお、相対位置ベクトル、角速度ベクトルについては後に詳述する。
【0033】
車両位置補正部３４６は、トラクタ１における上方、本実施形態においては屋根部１９ｒに取り付けられたセンサ装置３０の測定位置においてＧＮＳＳセンサ３３により測定されたローカル座標系におけるトラクタ１の車両位置を、センサ座標系において予め設定された設定位置におけるトラクタ１の車両位置に補正する。
【0034】
設定位置は、センサ装置３０の測定位置をトラクタ１の走行面、即ち圃場表面に投影した位置に設定される。本実施形態において、設定位置は、ｙ方向においてトラクタ１の幅方向中心に位置し、ｚ軸方向において前輪１１、後輪１２の下端またはこの下端より上方の下端近傍部に位置するように設定される。
【0035】
速度ベクトル補正部３４７は、センサ装置３０の測定位置においてＧＮＳＳセンサ３３によるローカル座標系におけるトラクタ１の速度ベクトルを、設定位置における速度ベクトルに補正する。
【0036】
（誤差補正装置の動作）
誤差補正装置の動作について説明する。図５は、誤差補正装置の動作を示すフローチャートである。図６、図７は、それぞれ、相対位置ベクトルを示す概略正面図、概略平面図である。図８は、速度ベクトルを示す概略正面図である。なお、図５に示す動作は、所定の周期毎に実行されるものとする。
誤差補正装置の動作について説明する。図５は、誤差補正装置の動作を示すフローチャートである。図６、図７は、それぞれ、相対位置ベクトルを示す概略正面図、概略平面図である。図８は、速度ベクトルを示す概略正面図である。なお、図５に示す動作は、所定の周期毎に実行されるものとする。
【0037】
図５に示すように、まず、第１取得部３４１は、ＧＮＳＳセンサ３３により測定されたローカル座標系における車両位置と速度ベクトルとを取得し（Ｓ１０１）、ジャイロセンサ３１により測定されたトラクタ１の角速度と、加速度センサ３２により測定されたトラクタ１の加速度とを取得する（Ｓ１０２）
【0038】
次に、ローパスフィルタ３４２は、第１取得部３４１により取得された角速度及び加速度のそれぞれについて、予め設定された第１遮断周波数、第２遮断周波数より高い高周波信号を逓減させる（Ｓ１０３）。
【0039】
次に、第２取得部３４３が、昇降制御装置４６により出力される昇降量を取得し（Ｓ１０４）、設定位置補正部３４４が、第２取得部３４３により取得された昇降量に基づいて設定位置におけるｚ軸上の位置を補正する（Ｓ１０５）。
【0040】
ここで、設定位置補正部３４４による設定位置の補正について説明する。設定位置は、上述したように、センサ装置３０の測定位置を圃場表面に投影した位置として、センサ座標系のｚ軸上において、トラクタ１の前輪１１、後輪１２の下端近傍に予め設定される位置であるが、前輪１１、後輪１２の少なくともいずれかが圃場に大きく沈下する場合、測定位置とトラクタ１の走行面としての圃場の表面とのｚ軸上の相対距離は、測定位置と設定位置とのｚ軸上の相対距離より小さいものとなる。また、同一の圃場であっても、その状態は必ずしも全体で均一となっておらず、位置によって状態が異なるため、トラクタ１の走行中に測定位置と圃場表面とのｚ軸上の相対距離が変動することとなる。
【0041】
設定位置補正部３４４は、昇降量に基づいて、圃場表面と一定の距離を保つように昇降制御されるロータリ耕耘機４１の上下方向位置を算出し、この上下方向位置が高くなるほど、測定位置と設定位置との相対距離が小さくなるように設定位置のｚ軸上の位置を補正する。これによって、前輪１１、後輪１２が圃場に沈下するような場合であっても、設定位置を圃場表面に投影した位置とすることができる。
【0042】
座標変換部３４５は、図５及び図６に示すような、センサ座標系において設定位置ａを始点とする測定位置ｓの相対位置ベクトルＬｓを、ジャイロセンサ３１により測定されるトラクタ１の方位角、ピッチ角、ロール角に基づいて、Ｌ＝ＣＬｓの式によりローカル座標系における相対位置ベクトルＬに座標変換する（Ｓ１０６）。ここで、Ｃは、センサ座標系からローカル座標系への座標変換を行うための座標変換行列を示し、
【0043】
【数1】
【0044】
【数2】
【0045】
また、座標変換に先立って、一般的にアライメントと呼ばれる初期化処理がなされる。この初期化処理によれば、トラクタ１の初期方位（方位角）と初期姿勢（ピッチ角度及びロール角）が決定され、座標変換部３４５による座標変換において、この初期方位及び初期姿勢がジャイロセンサ３１により計測される角速度により更新された方位角ξ、ピッチ角θ、ロール角φが座標変換行列Ｃに与えられる。
【0046】
初期姿勢におけるピッチ角度θとロール角φは、トラクタ１が静止した状態において加速度センサ３２により測定される重力加速度をｇとし、３軸の加速度のそれぞれをａｘ，ａｙ，ａｚとして、以下の式から求められる。
【0047】
【数3】
【0048】
また、初期方位としての方位角ξは、トラクタ１の走行中においてＧＮＳＳセンサ３３により測定される速度ベクトルに基づいて設定される。なお、初期姿勢及び初期方位は、予め設定された値を用いても良い。
【0049】
座標変換後、車両位置補正部３４６は、このローカル座標系の原点Ｏを始点とした設定位置ａの位置ベクトルをＰａ、測定位置ｓの位置ベクトルをＰｓとして、Ｐａ＝Ｐｓ－Ｌの式により、位置ベクトルＰａを算出することにより、測定位置ｓにおいて測定された車両位置を、設定位置ａにおける車両位置に補正する（Ｓ１０７）。
【0050】
次に、座標変換部３４５は、ジャイロセンサ３１により測定されるセンサ座標系におけるトラクタ１の回転角速度ベクトルωｓを、座標変換行列Ｃを用いて、ω＝Ｃωｓの式によりローカル座標系における回転角速度ベクトルωに座標変換する（Ｓ１０８）。
【0051】
座標変換後、速度ベクトル補正部３４７は、ＧＮＳＳセンサ３３により測定された測定位置ｓにおけるトラクタ１の速度ベクトルをＶｓとし、設定位置ａにおける速度ベクトルをＶａとして、Ｖａ＝Ｖｓ－ω×Ｌの式により、速度ベクトルＶａを算出することにより、測定位置ｓにおいて測定された速度ベクトルＶｓを、設定位置ａにおける速度ベクトルＶａに補正する（Ｓ１０９）。
【0052】
このように、誤差補正装置３４は、測定位置ｓにおいて測定された車両位置、速度ベクトルのそれぞれを、設定位置ａにおける車両位置、速度ベクトルに補正する。補正された車両位置及び速度ベクトルによれば、特に凹凸がある圃場をトラクタ１が走行した場合に生じるセンサ装置３０の振れに起因する測定値の誤差を低減した、より正確な車両位置、進行方位及び進行速度を自動操舵制御装置２１が得ることができる。これによって、自動操舵制御装置２１による正確なトラクタ１の自動操舵が達成される。
【0053】
なお、誤差補正装置３４により測定値の誤差が補正されるセンサ装置３０が設けられる作業車両としては、トラクタ１以外に、田植機、コンバイン、野菜移植機、野菜収穫機、芝刈り機など、圃場に接地されるように昇降制御される作業機が設けられる全ての作業車両が挙げられる。
【0054】
本発明の実施形態は、例として提示したものであり、発明の範囲を限定することは意図していない。この新規な実施形態は、その他の様々な形態で実施されることが可能であり、発明の要旨を逸脱しない範囲で、種々の省略、置き換え、変更を行うことができる。これら実施形態やその変形は、発明の範囲や要旨に含まれるとともに、特許請求の範囲に記載された発明とその均等の範囲に含まれる。
【符号の説明】
【0055】
３４ 誤差補正装置
３４１ 第１取得部
３４５ 座標変換部
３４６ 車両位置補正部
３４１ 第１取得部
３４５ 座標変換部
３４６ 車両位置補正部
【図1】
【図2】
【図3】
【図4】
【図5】
【図6】
【図7】
【図8】