(19)【発行国】日本国特許庁(JP)
(12)【公報種別】公開特許公報(A)
(11)【公開番号】P2023177820
(43)【公開日】2023-12-14
(54)【発明の名称】軌道取得装置及び装着支援装置
(51)【国際特許分類】
A01B 59/043 20060101AFI20231207BHJP
【ＦＩ】
A01B59/043 Z
【審査請求】未請求
【請求項の数】4
【出願形態】ＯＬ
(21)【出願番号】P 2022090707
(22)【出願日】2022-06-03
(71)【出願人】
【識別番号】501203344
【氏名又は名称】国立研究開発法人農業・食品産業技術総合研究機構
(74)【代理人】
【識別番号】100077665
【弁理士】
【氏名又は名称】千葉 剛宏
(74)【代理人】
【識別番号】100116676
【弁理士】
【氏名又は名称】宮寺 利幸
(74)【代理人】
【識別番号】100191134
【弁理士】
【氏名又は名称】千馬 隆之
(74)【代理人】
【識別番号】100136548
【弁理士】
【氏名又は名称】仲宗根 康晴
(74)【代理人】
【識別番号】100136641
【弁理士】
【氏名又は名称】坂井 志郎
(74)【代理人】
【識別番号】100180448
【弁理士】
【氏名又は名称】関口 亨祐
(72)【発明者】
【氏名】趙 元在
(72)【発明者】
【氏名】元林 浩太
(72)【発明者】
【氏名】ヌウェン ヴァン ナン
【テーマコード（参考）】
2B041
【Ｆターム（参考）】
2B041AA09
2B041AA15
2B041AB05
2B041AC03
2B041CA04
2B041CA16
2B041CC02
2B041CC12
2B041CC22
2B041CG01
2B041CG10
(57)【要約】
【解決手段】軌道取得装置（５０）は、カメラ（５２）と、上部フック（３８）の位置と所定部位（５８）の位置とを対応付けた対応関係を予め記憶する記憶部（６２）と、リフトアーム（２８）が上下方向に動作している間に、リフトアームの複数の高さ位置の各々に対応する角度情報を取得すると共に、リフトアームが各々の高さに位置するときにカメラによって撮影された所定部位を含む画像を取得し、高さ位置毎に角度情報と画像とを対応付ける情報取得部（６４）と、情報取得部によって対応付けられた複数の組の角度情報及び画像と対応関係とを用いてリフトアームの回転角度に対応した上部フックの３次元の軌道を算出する軌道演算部（６６）とを備える。
【選択図】図１
【選択図】図１
【特許請求の範囲】
【請求項1】
作業機をトラクタに装着するために前記トラクタのリフトアームを動作させることによって前記トラクタに取り付けられたヒッチフレームの上部フックで前記作業機の上部ピンを掬い上げるときの前記上部フックの軌道を取得する軌道取得装置であって、
前記トラクタに取り付けられており、前記リフトアームの動作によって移動する所定部位を撮影するカメラと、
前記上部フックの位置と前記所定部位の位置とを対応付けた対応関係を予め記憶する記憶部と、
前記リフトアームが上下方向に動作している間に、前記リフトアームの複数の高さ位置の各々に対応する角度情報を取得すると共に、前記リフトアームが各々の高さに位置するときに前記カメラによって撮影された前記所定部位を含む画像を取得し、高さ位置毎に前記角度情報と前記画像とを対応付ける情報取得部と、
前記情報取得部によって対応付けられた複数の組の前記角度情報及び前記画像と前記対応関係とを用いて前記リフトアームの回転角度に対応した前記上部フックの３次元の前記軌道を算出する軌道演算部と
を備える、軌道取得装置。
前記トラクタに取り付けられており、前記リフトアームの動作によって移動する所定部位を撮影するカメラと、
前記上部フックの位置と前記所定部位の位置とを対応付けた対応関係を予め記憶する記憶部と、
前記リフトアームが上下方向に動作している間に、前記リフトアームの複数の高さ位置の各々に対応する角度情報を取得すると共に、前記リフトアームが各々の高さに位置するときに前記カメラによって撮影された前記所定部位を含む画像を取得し、高さ位置毎に前記角度情報と前記画像とを対応付ける情報取得部と、
前記情報取得部によって対応付けられた複数の組の前記角度情報及び前記画像と前記対応関係とを用いて前記リフトアームの回転角度に対応した前記上部フックの３次元の前記軌道を算出する軌道演算部と
を備える、軌道取得装置。
【請求項2】
請求項１に記載の軌道取得装置であって、
前記軌道が妥当であるか否かを判定する妥当性判定部を更に備え、
前記軌道演算部は、前記情報取得部によって対応付けられた複数の組のうち、一部の複数の組の前記角度情報及び前記画像と前記対応関係とを用いて前記上部フックの３次元の前記軌道を算出し、
前記妥当性判定部は、前記軌道演算部によって用いられていない１組以上の前記角度情報及び前記画像に基づいて定められる前記上部フックの３次元の位置が、前記軌道から所定距離離れた範囲内にあるか否かを判定し、前記上部フックの３次元の位置が前記範囲内にある場合に、前記軌道が妥当であると判定する、軌道取得装置。
前記軌道が妥当であるか否かを判定する妥当性判定部を更に備え、
前記軌道演算部は、前記情報取得部によって対応付けられた複数の組のうち、一部の複数の組の前記角度情報及び前記画像と前記対応関係とを用いて前記上部フックの３次元の前記軌道を算出し、
前記妥当性判定部は、前記軌道演算部によって用いられていない１組以上の前記角度情報及び前記画像に基づいて定められる前記上部フックの３次元の位置が、前記軌道から所定距離離れた範囲内にあるか否かを判定し、前記上部フックの３次元の位置が前記範囲内にある場合に、前記軌道が妥当であると判定する、軌道取得装置。
【請求項3】
請求項１又は２に記載の軌道取得装置によって取得された前記軌道を用いて前記作業機を前記トラクタに装着する作業を支援する装着支援装置であって、
前記トラクタの移動方向及び移動量を算出する動作演算部と、
前記動作演算部によって算出された前記移動方向及び前記移動量を報知装置に報知させる報知制御部と
を備え、
前記カメラは、前記作業機が有する所定の作業機側部位を撮影し、
前記記憶部は、前記作業機側部位の位置と前記作業機の各部位の位置とを対応付けた作業機側対応関係を予め記憶し、
前記動作演算部は、前記カメラによって撮影される前記作業機側部位を含む画像と前記作業機側対応関係とを用いて、前記上部ピンを含む前記作業機の各部位の３次元位置を算出し、３次元の前記軌道の一部と前記上部ピンの３次元位置とを一致させるために必要な前記移動方向及び前記移動量を算出する、装着支援装置。
前記トラクタの移動方向及び移動量を算出する動作演算部と、
前記動作演算部によって算出された前記移動方向及び前記移動量を報知装置に報知させる報知制御部と
を備え、
前記カメラは、前記作業機が有する所定の作業機側部位を撮影し、
前記記憶部は、前記作業機側部位の位置と前記作業機の各部位の位置とを対応付けた作業機側対応関係を予め記憶し、
前記動作演算部は、前記カメラによって撮影される前記作業機側部位を含む画像と前記作業機側対応関係とを用いて、前記上部ピンを含む前記作業機の各部位の３次元位置を算出し、３次元の前記軌道の一部と前記上部ピンの３次元位置とを一致させるために必要な前記移動方向及び前記移動量を算出する、装着支援装置。
【請求項4】
請求項１又は２に記載の軌道取得装置によって取得された前記軌道を用いて前記作業機を前記トラクタに装着する作業を支援する装着支援装置であって、
前記トラクタの移動方向及び移動量を算出する動作演算部と、
前記動作演算部によって算出された前記移動方向及び前記移動量に基づいて前記トラクタを自律走行させる走行制御部と
を備え、
前記カメラは、前記作業機が有する所定の作業機側部位を撮影し、
前記記憶部は、前記作業機側部位の位置と前記作業機の各部位の位置とを対応付けた作業機側対応関係を予め記憶し、
前記動作演算部は、前記カメラによって撮影される前記作業機側部位を含む画像と前記作業機側対応関係とを用いて、前記上部ピンを含む前記作業機の各部位の３次元位置を算出し、３次元の前記軌道の一部と前記上部ピンの３次元位置とを一致させるために必要な前記移動方向及び前記移動量を算出する、装着支援装置。
前記トラクタの移動方向及び移動量を算出する動作演算部と、
前記動作演算部によって算出された前記移動方向及び前記移動量に基づいて前記トラクタを自律走行させる走行制御部と
を備え、
前記カメラは、前記作業機が有する所定の作業機側部位を撮影し、
前記記憶部は、前記作業機側部位の位置と前記作業機の各部位の位置とを対応付けた作業機側対応関係を予め記憶し、
前記動作演算部は、前記カメラによって撮影される前記作業機側部位を含む画像と前記作業機側対応関係とを用いて、前記上部ピンを含む前記作業機の各部位の３次元位置を算出し、３次元の前記軌道の一部と前記上部ピンの３次元位置とを一致させるために必要な前記移動方向及び前記移動量を算出する、装着支援装置。
【発明の詳細な説明】
【技術分野】
【0001】
本発明は、ヒッチフレームの上部フックの軌道を取得する軌道取得装置、及び、トラクタに作業機を装着する作業を支援する装着支援装置に関する。
【背景技術】
【0002】
トラクタには作業機が装着される。一般にユーザは、作業機の装着作業を手動で行う。例えば、ユーザは、トラクタを運転してトラクタを作業機に近づける。ユーザは、作業機昇降装置を操作してリンク機構（３点リンク）に取り付けられたヒッチフレームを上方に移動させる。上部フックが移動する軌道に作業機の上部ピンが位置する場合、上部フックは上部ピンを掬い上げる。すると、作業機は上部ピンを中心にしてトラクタ側に回転移動し、作業機の下部ピンがヒッチフレームの下部支持部に嵌る。このようにして、作業機は、トラクタのヒッチフレームに装着される。
【0003】
特許文献１には、トラクタに作業機を装着する際に、トラクタを作業機に向けて自律走行させる技術が開示される。この技術は、トラクタ側の第１のカメラでトラクタの一部を撮影し、トラクタ側の第２のカメラで作業機側のターゲットを撮影し、各カメラの画像を用いてトラクタの走行制御を行う。
【先行技術文献】
【特許文献】
【0004】
【特許文献1】米国特許出願公開第２０１５／００７７５５７号明細書
【発明の概要】
【発明が解決しようとする課題】
【0005】
特許文献１の技術によれば、ユーザは、トラクタを運転する必要がない。その一方で、ユーザは、自ら作業機昇降装置を操作して、リンク機構に取り付けられたヒッチフレームに作業機を装着する必要がある。
【0006】
リンク機構は、複数の部材で構成される。トラクタへの作業機の装着作業前に、リンク機構の一部の部材の長さ、部材同士の接続位置等は適宜調整される。リンク機構が調整されると、ヒッチフレームの上部フックが移動する軌道は変わる。つまり、上部フックの軌道は一定でない。このため、ユーザは、作業機の装着作業時に、上部フックの位置をこまめに確認する必要がある。こうした理由から、作業機の装着作業はユーザにとって容易な作業でない。また、作業機の装着作業には時間を要する。
【0007】
本発明は上述した課題を解決することを目的とする。
【課題を解決するための手段】
【0008】
本発明の第１の態様は、作業機をトラクタに装着するために前記トラクタのリフトアームを動作させることによって前記トラクタに取り付けられたヒッチフレームの上部フックで前記作業機の上部ピンを掬い上げるときの前記上部フックの軌道を取得する軌道取得装置であって、前記トラクタに取り付けられており、前記リフトアームの動作によって移動する所定部位を撮影するカメラと、前記上部フックの位置と前記所定部位の位置とを対応付けた対応関係を予め記憶する記憶部と、前記リフトアームが上下方向に動作している間に、前記リフトアームの複数の高さ位置の各々に対応する角度情報を取得すると共に、前記リフトアームが各々の高さに位置するときに前記カメラによって撮影された前記所定部位を含む画像を取得し、高さ位置毎に前記角度情報と前記画像とを対応付ける情報取得部と、前記情報取得部によって対応付けられた複数の組の前記角度情報及び前記画像と前記対応関係とを用いて前記リフトアームの回転角度に対応した前記上部フックの３次元の前記軌道を算出する軌道演算部とを備える。
【0009】
本発明の第２の態様は、第１の態様の軌道取得装置によって取得された前記軌道を用いて前記作業機を前記トラクタに装着する作業を支援する装着支援装置であって、前記トラクタの移動方向及び移動量を算出する動作演算部と、前記動作演算部によって算出された前記移動方向及び前記移動量を報知装置に報知させる報知制御部とを備え、前記カメラは、前記作業機が有する所定の作業機側部位を撮影し、前記記憶部は、前記作業機側部位の位置と前記作業機の各部位の位置とを対応付けた作業機側対応関係を予め記憶し、前記動作演算部は、前記カメラによって撮影される前記作業機側部位を含む画像と前記作業機側対応関係とを用いて、前記上部ピンを含む前記作業機の各部位の３次元位置を算出し、３次元の前記軌道の一部と前記上部ピンの３次元位置とを一致させるために必要な前記移動方向及び前記移動量を算出する。
【0010】
本発明の第３の態様は、第１の態様の軌道取得装置によって取得された前記軌道を用いて前記作業機を前記トラクタに装着する作業を支援する装着支援装置であって、前記トラクタの移動方向及び移動量を算出する動作演算部と、前記動作演算部によって算出された前記移動方向及び前記移動量に基づいて前記トラクタを自律走行させる走行制御部とを備え、前記カメラは、前記作業機が有する所定の作業機側部位を撮影し、前記記憶部は、前記作業機側部位の位置と前記作業機の各部位の位置とを対応付けた作業機側対応関係を予め記憶し、前記動作演算部は、前記カメラによって撮影される前記作業機側部位を含む画像と前記作業機側対応関係とを用いて、前記上部ピンを含む前記作業機の各部位の３次元位置を算出し、３次元の前記軌道の一部と前記上部ピンの３次元位置とを一致させるために必要な前記移動方向及び前記移動量を算出する。
【発明の効果】
【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】
軌道演算部６６は、各々の画像から、第１座標系Ｃ１における第２座標系Ｃ２の原点位置（トラクタマーカ５８の所定部分の位置）の座標と、第１座標系Ｃ１における第２座標系Ｃ２の３軸の向きとを算出する。軌道演算部６６は、座標と向きを算出するために、例えば、既存の３次元ポーズ推定手法［ＰｎＰ（Ｐｅｒｓｐｅｃｔｉｖｅ－ｎ－Ｐｏｉｎｔ）等］を利用する。軌道演算部６６は、算出された座標及び３軸の向きを使用して第１処理を行い、第１座標系Ｃ１における上部フック３８の先端位置の座標を算出する。次に、軌道演算部６６は、第２処理を行い、第１座標系Ｃ１における上部フック３８の先端位置の座標を、第３座標系Ｃ３における上部フック３８の先端位置の座標に変換する。軌道演算部６６は、複数の画像の各々から、第３座標系Ｃ３における上部フック３８の座標を算出する。更に、軌道演算部６６は、上部フック３８の座標の各々に対して、リフトアーム２８の回転角度を対応付ける。
【0044】
次いで、軌道演算部６６は、算出された複数の座標を用いて、上部フック３８の先端の軌道を算出する。例えば、軌道演算部６６は、カーブフィッティング手法を利用することによって、３次元の軌道を算出することができる。軌道演算部６６は、以上の処理により、上部フック３８の先端の３次元の軌道を取得する。
【0045】
なお、軌道演算部６６は別の手法にて上部フック３８の軌道を算出してもよい。例えば、軌道演算部６６は、トラクタマーカ５８の所定部分の軌道を算出し、取得した軌道を上部フック３８の軌道に変換してもよい。
【0046】
ステップＳ８において、妥当性判定部６８は、軌道演算部６６によって算出された上部フック３８の軌道が適切であるかを確認する。すなわち、妥当性判定部６８は、上部フック３８の軌道の妥当性を判定する。妥当性判定部６８は、記憶部６２のテーブルから複数組の角度情報と画像ＩＤとを読み出す。ここで、妥当性判定部６８は、軌道演算部６６によって用いられていない角度情報と画像ＩＤとを読み出す。なお、妥当性判定部６８は、複数組でなく１組の角度情報と画像ＩＤとを読み出してもよい。加えて、軌道演算部６６は、読み出した各々の画像ＩＤに対応する画像を読み出す。妥当性判定部６８は、ステップＳ７において軌道演算部６６が行った手法（３次元ポーズ推定手法等）によって、トラクタマーカ５８の所定部位の座標を算出する。この座標を判定用座標と称する。妥当性判定部６８は、各々の判定用座標とステップＳ７において取得された上部フック３８の軌道とのユークリッド距離を算出する。更に、妥当性判定部６８は、各々のユークリッド距離の平均値を算出する。妥当性判定部６８は、平均値と妥当性判定用の閾値とを比較する。閾値は、例えば上部フック３８の左右方向の幅程度である。閾値は、記憶部６２に予め記憶されている。なお、妥当性判定部６８は、各々のユークリッド距離に関して、平均値の代わりに他の統計量、例えば分散、共分散等を算出してもよい。
【0047】
平均値が閾値未満である場合（ステップＳ９：ＹＥＳ）、妥当性判定部６８は、上部フック３８の軌道が妥当であると判定する。この場合、処理はステップＳ１０に移行する。一方、平均値が閾値以上である場合（ステップＳ９：ＮＯ）、妥当性判定部６８は、上部フック３８の軌道が妥当でないと判定する。この場合、処理はステップＳ６に戻る。なお、２回目以降のステップＳ６において、軌道演算部６６は、ステップＳ９で平均値が閾値以上であると判定された上部フック３８の軌道が再現されることを防止するために、前回以前のステップＳ６で読み出した組の角度情報と画像ＩＤとは別組の角度情報と画像ＩＤとを読み出す。
【0048】
ステップＳ１０において、軌道演算部６６は、ステップＳ８及びステップＳ９において妥当と判定された上部フック３８の軌道を、記憶部６２に記憶させる。以上で軌道取得処理は終了する。
【0049】
以上のようにして、軌道取得装置５０は、トラクタ１０への作業機１２の装着作業前に、リフトアーム２８の回転角度に対応した上部フック３８の３次元の軌道を取得する。トラクタ１０のキャビンには、バックモニタが設けられる。軌道取得装置５０は、カメラ５２又は他のカメラで撮影された映像をバックモニタに表示させる。軌道取得装置５０は、バックモニタに表示される映像に上部フック３８の軌道を重畳してもよい。ユーザは、作業機１２の上部ピン４６がこの軌道上に位置するようにトラクタ１０を移動させて、作業機昇降装置１６を操作すれば、ヒッチフレーム２０の上部フック３８は、作業機１２の上部ピン４６を掬い上げることができる。このように、ユーザがヒッチフレーム２０の上部フック３８の軌道を予め把握できると、作業機１２の装着作業の負担が低減する。つまり、軌道取得装置５０によれば、ユーザはトラクタ１０への作業機１２の装着作業を容易に行うことができる。また、軌道取得装置５０によれば、作業機１２の装着作業の時間を短縮することができる。
【0050】
［３ 装着支援装置７０］
図５～図８を用いて、トラクタ１０に設けられる装着支援装置７０について説明する。装着支援装置７０は、上部フック３８が上部ピン４６を掬い上げることができる位置までトラクタ１０を誘導する。
図５～図８を用いて、トラクタ１０に設けられる装着支援装置７０について説明する。装着支援装置７０は、上部フック３８が上部ピン４６を掬い上げることができる位置までトラクタ１０を誘導する。
【0051】
［３－１ 第１実施形態の装着支援装置７０］
［３－１－１ 装着支援装置７０の構成］
図５は、第１実施形態に係る装着支援装置７０の構成図である。第１実施形態に係る装着支援装置７０は、トラクタ１０の移動方向と移動量をユーザに報知することによって、トラクタ１０を作業機１２の正面位置まで誘導する。装着支援装置７０は、軌道取得装置５０を含む。このため、装着支援装置７０の各構成のうち、軌道取得装置５０と同じ構成には軌道取得装置５０と同一の符号を付し、その詳細な説明を省略する。
［３－１－１ 装着支援装置７０の構成］
図５は、第１実施形態に係る装着支援装置７０の構成図である。第１実施形態に係る装着支援装置７０は、トラクタ１０の移動方向と移動量をユーザに報知することによって、トラクタ１０を作業機１２の正面位置まで誘導する。装着支援装置７０は、軌道取得装置５０を含む。このため、装着支援装置７０の各構成のうち、軌道取得装置５０と同じ構成には軌道取得装置５０と同一の符号を付し、その詳細な説明を省略する。
【0052】
装着支援装置７０は、カメラ５２と、角度センサ５４と、慣性計測装置７２と、演算装置５６と、報知装置７４とを有する。また、装着支援装置７０は、トラクタマーカ５８と作業機マーカ７６とを有する。トラクタマーカ５８と同様に、作業機マーカ７６は、画像認識処理によってカメラ５２との離間距離と姿勢とを認識可能な特定のマークを有する。作業機マーカ７６は、作業機１２のうち、上部ピン４６との相対位置が変わらない箇所に取り付けられる。例えば、作業機マーカ７６はマスト４４に取り付けられる。なお、作業機マーカ７６は、作業機１２の種類毎に定められる。
【0053】
慣性計測装置７２は、３軸のジャイロと３方向の加速度センサとを有する。慣性計測装置７２は、トラクタ１０の加速度と旋回角速度とを計測する。慣性計測装置７２は、計測データを演算装置５６に送信する。
【0054】
演算部６０は、情報取得部６４と、軌道演算部６６と、妥当性判定部６８と、動作演算部７８と、報知制御部８０と、位置判定部８２として機能する。動作演算部７８は、軌道演算部６６によって算出された軌道の一部と作業機１２の上部ピン４６の位置とを一致させるために各種の算出を行う。報知制御部８０は、報知装置７４に報知指示を出力する。位置判定部８２は、トラクタ１０が目標位置に到達したか否かを判定する。
【0055】
記憶部６２は、座標変換に関する情報を記憶する。本実施形態では図１で示される５つの３次元座標系が使用される。第１座標系Ｃ１～第３座標系Ｃ３は、軌道取得装置５０が使用する３つの座標系と同じである。第４座標系Ｃ４は、作業機マーカ７６中の所定部分の位置（例えばマーカ中心位置）を原点とする。第５座標系Ｃ５は、上部ピン４６の中心部分（作業機１２の左右方向における中心部分）の真下の地面の位置、言い換えると上部ピン４６の中心部分を地面に投影した位置を原点とする。
【0056】
作業機１２が直立した状態において、第４座標系Ｃ４の３軸のうち、第１軸（Ｘ軸）は作業機マーカ７６のマーカ表示面に直交し、第２軸（Ｙ軸）は作業機マーカ７６の横方向と平行し、第３軸（Ｚ軸）は作業機マーカ７６の縦方向と平行する。第５座標系Ｃ５の３軸のうち、第１軸（Ｘ軸）は作業機１２の前後方向と平行し、第２軸（Ｙ軸）は作業機１２の左右方向と平行し、第３軸（Ｚ軸）は上下方向と平行する。
【0057】
第１実施形態では、カメラ５２で撮影された画像を用いて、最終的には第５座標系Ｃ５におけるトラクタ１０の目標位置と、トラクタ１０の向きとを算出する。記憶部６２は、第１座標系Ｃ１における第４座標系Ｃ４の原点位置と、第１座標系Ｃ１における第４座標系Ｃ４の３軸の向きとに基づいて、第１座標系Ｃ１における上部ピン４６の中心位置の座標と、上部ピン４６の姿勢とを算出するための処理方法（第３処理という）を記憶する。また、記憶部６２は、第４座標系Ｃ４の原点位置から第５座標系Ｃ５の原点位置を算出するための情報として、直立した作業機１２における上部ピン４６の高さを記憶する。
【0058】
報知装置７４は、例えば、ディスプレイ等を含む表示装置と、スピーカ等を含む音響装置のいずれか一方又は両方を有する。報知装置７４は、報知制御部８０が出力する報知指示に応じて報知する。
【0059】
［３－１－２ 誘導処理］
図６は、第１実施形態の誘導処理のフローチャートである。ユーザは、キャビン内の操作装置を操作して、装着支援装置７０に誘導処理を実行させる。
図６は、第１実施形態の誘導処理のフローチャートである。ユーザは、キャビン内の操作装置を操作して、装着支援装置７０に誘導処理を実行させる。
【0060】
ステップＳ１１において、装着支援装置７０は、図４で示される一連の軌道取得処理を実行する。これによって、軌道演算部６６は、第３座標系Ｃ３におけるヒッチフレーム２０の上部フック３８の先端の３次元の軌道を取得する。
【0061】
ステップＳ１２において、カメラ５２は、マスト４４に取り付けられた作業機マーカ７６を撮影する。情報取得部６４は、カメラ５２によって撮影された画像を取得する。なお、ユーザは、ステップＳ１２の前に、作業機マーカ７６がカメラ５２の画角内に入るように、トラクタ１０を作業機１２に近づけておくとよい。
【0062】
ステップＳ１３において、動作演算部７８は、ステップＳ１２で取得された画像を用いて、第４座標系Ｃ４の原点位置、すなわち作業機マーカ７６の所定部分の座標を算出する。例えば、動作演算部７８は、図４のステップＳ７において軌道演算部６６が行った手法（３次元ポーズ推定手法等）によって、第１座標系Ｃ１における第４座標系Ｃ４の原点位置（作業機マーカ７６の所定部分の座標）の座標と、第１座標系Ｃ１における第４座標系Ｃ４の３軸の向きとを算出する。
【0063】
ステップＳ１４において、動作演算部７８は、第５座標系Ｃ５におけるトラクタ１０の現在位置と、トラクタ１０の向きとを算出する。動作演算部７８は、次の処理を行う。
【0064】
動作演算部７８は、ステップＳ１３で算出された座標及び３軸の向きを使用して第３処理を行い、第１座標系Ｃ１における上部ピン４６の中心位置の座標と、上部ピン４６の姿勢とを算出する。次に、動作演算部７８は、第４座標系Ｃ４の原点の座標と、上部ピン４６の高さとから、第１座標系Ｃ１における第５座標系Ｃ５の原点の座標と、第５座標系Ｃ５の３軸の向きとを算出する。次に、動作演算部７８は、第２処理を行い、第１座標系Ｃ１における第５座標系Ｃ５の原点の座標を、第３座標系Ｃ３の座標に変換する。これにより、第５座標系Ｃ５の原点が、第３座標系Ｃ３の座標として定められる。次に、動作演算部７８は、第３座標系Ｃ３の原点の座標と第５座標系Ｃ５の原点の座標とに基づいて、第３座標系Ｃ３の原点を、第５座標系Ｃ５の座標として定める。また、動作演算部７８は、第５座標系Ｃ５における第３座標系Ｃ３の３軸の向きを算出する。以上の処理により、作業機１２を基準とするトラクタ１０の現在位置と、トラクタ１０の向きとが特定される。
【0065】
ステップＳ１５において、動作演算部７８は、トラクタ１０が到達すべき目標位置を設定する。本実施形態において、動作演算部７８は、トラクタ１０のヒッチフレーム２０の上部フック３８が作業機１２の上部ピン４６を掬い上げることができる位置にトラクタ１０を移動させるために、作業機１２の第５座標系Ｃ５におけるトラクタ１０の目標位置を設定する。目標位置において、トラクタ１０の前後方向と、作業機１２の前後方向は平行する。動作演算部７８は、目標位置を設定するために、次の処理を行う。
【0066】
第３座標系Ｃ３の原点と第５座標系Ｃ５の原点は共に地面に位置する。つまり、第３座標系Ｃ３における高さ方向の座標と第５座標系Ｃ５における高さ方向の座標は、互いに同じである。動作演算部７８は、第３座標系Ｃ３における上部フック３８の先端の軌道から、上部ピン４６の高さと一致する座標を取得する。これは、上部フック３８が上部ピン４６を掬い上げる位置の座標であり、第３座標系Ｃ３の座標である。この位置をフック位置と称する。フック位置の真下に第５座標系Ｃ５の原点が位置する。つまり、フック位置の前後方向の座標と左右方向の座標とは、第５座標系Ｃ５の原点と同じである。動作演算部７８は、第３座標系Ｃ３におけるフック位置の前後方向の座標と左右方向の座標とから、第５座標系Ｃ５における第３座標系Ｃ３の原点の前後方向の座標と左右方向の座標とを算出する。動作演算部７８は、この座標を、第５座標系Ｃ５における目標位置とする。
【0067】
ステップＳ１６において、動作演算部７８は、トラクタ１０の移動方向及び移動量を算出する。ここで、動作演算部７８は、トラクタ１０を現在位置から目標位置まで誘導するための移動方向及び移動量を算出する。動作演算部７８は、ステップＳ１４で算出したトラクタ１０の現在位置を、トラクタ１０の出発位置とする。更に、動作演算部７８は、慣性計測装置７２から取得した計測データを用いてトラクタ１０の実移動量と実移動方向を算出し、トラクタ１０の現在位置及びトラクタ１０の現在の向きとを更新し続ける。
【0068】
ステップＳ１７において、報知制御部８０は、ステップＳ１６で算出したトラクタ１０の移動方向及び移動量を、ステアリングの操作方向及び操作量に変換する。報知制御部８０は、ステアリングの操作方向及び操作量を、ユーザに報知する報知指示として報知装置７４に出力する。報知装置７４は、報知指示に従いステアリングの操作方向及び操作量をユーザに報知する。ユーザは、報知装置７４の報知内容に従いトラクタ１０を運転することによって、トラクタ１０を目標位置に向けて走行させることができる。
【0069】
ユーザは、トラクタ１０を目標位置に向かって走行させる場合に、ヒッチフレーム２０の高さ位置を調整する。具体的には、ユーザは、ヒッチフレーム２０の上部フック３８が作業機１２の上部ピン４６よりも下方に位置するように、リフトアーム２８の回転角度を調整する。これにより、トラクタ１０が作業機１２に接近しても、上部フック３８は上部ピン４６に当たらない。演算装置５６がリフトアーム２８の回転角度の調整を行ってもよい。リフトアーム２８の回転角度は、記憶部６２に記憶されてもよい。また、リフトアーム２８の回転角度は、作業機１２の種類に応じて定められてもよい。
【0070】
ステップＳ１８において、位置判定部８２は、トラクタ１０の現在位置と目標位置とを比較する。また、位置判定部８２は、トラクタ１０の現在の向きと目標位置におけるトラクタ１０の目標向きとを比較する。トラクタ１０の現在位置と目標位置とが一致し、且つ、トラクタ１０の現在の向きとトラクタ１０の目標向きとが一致する場合（ステップＳ１８：ＹＥＳ）、位置判定部８２は、トラクタ１０が目標位置に到達したと判定する。この場合、処理はステップＳ１９に移行する。一方、トラクタ１０の現在位置と目標位置とが一致しない場合、又は、トラクタ１０の現在の向きとトラクタ１０の目標向きとが一致しない場合（ステップＳ１８：ＮＯ）、位置判定部８２は、トラクタ１０が目標位置に到達していないと判定する。この場合、処理はステップＳ１６に戻る。
【0071】
ステップＳ１９において、報知制御部８０は、目標位置に到達したことをユーザに報知する報知指示を報知装置７４に出力する。報知装置７４は、報知指示に従いトラクタ１０が目標位置に到達したことをユーザに報知する。この段階で、ユーザはトラクタ１０を停止させる。以上で誘導処理は終了する。
【0072】
誘導処理が終了すると、ユーザは、作業機昇降装置１６を操作して、トラクタ１０への作業機１２の装着作業を行う。演算装置５６がトラクタ１０への作業機１２の装着作業を行ってもよい。装着作業におけるリフトアーム２８の回転角度は、記憶部６２に記憶されてもよい。また、装着作業におけるリフトアーム２８の回転角度は、作業機１２の種類に応じて定められてもよい。
【0073】
以上のようにして、装着支援装置７０は、トラクタ１０を作業機１２の正面に誘導する。装着支援装置７０によれば、ユーザはトラクタ１０への作業機１２の装着作業を容易に行うことができる。また、軌道取得装置５０によれば、作業機１２の装着作業の時間を短縮することができる。
【0074】
［３－２ 第２実施形態の装着支援装置７０］
［３－２－１ 装着支援装置７０の構成］
図７は、第２実施形態に係る装着支援装置７０の構成図である。第２実施形態に係る装着支援装置７０は、トラクタ１０を自律走行させることによって、トラクタ１０を作業機１２の正面位置まで誘導する。第１実施形態と同様に、装着支援装置７０は、軌道取得装置５０を含む。このため、装着支援装置７０の各構成のうち、軌道取得装置５０と同じ構成には軌道取得装置５０と同一の符号を付し、その詳細な説明を省略する。また、第１実施形態と同じ構成には第１実施形態と同一の符号を付し、その詳細な説明を省略する。
［３－２－１ 装着支援装置７０の構成］
図７は、第２実施形態に係る装着支援装置７０の構成図である。第２実施形態に係る装着支援装置７０は、トラクタ１０を自律走行させることによって、トラクタ１０を作業機１２の正面位置まで誘導する。第１実施形態と同様に、装着支援装置７０は、軌道取得装置５０を含む。このため、装着支援装置７０の各構成のうち、軌道取得装置５０と同じ構成には軌道取得装置５０と同一の符号を付し、その詳細な説明を省略する。また、第１実施形態と同じ構成には第１実施形態と同一の符号を付し、その詳細な説明を省略する。
【0075】
装着支援装置７０は、カメラ５２と、角度センサ５４と、慣性計測装置７２と、演算装置５６と、操舵装置８４と、駆動装置８６と、制動装置８８とを有する。
【0076】
演算部６０は、情報取得部６４と、軌道演算部６６と、妥当性判定部６８と、動作演算部７８と、走行制御部９０と、位置判定部８２として機能する。走行制御部９０は、操舵装置８４にトラクタ１０の操舵指示を出力する。また、走行制御部９０は、駆動装置８６にトラクタ１０の加速指示を出力する。また、走行制御部９０は、制動装置８８にトラクタ１０の減速停止指示を出力する。
【0077】
操舵装置８４は、走行制御部９０が出力する操舵指示に応じてトラクタ１０を操舵する。駆動装置８６は、走行制御部９０が出力する加速指示に応じてトラクタ１０を加速させる。制動装置８８は、走行制御部９０が出力する減速停止指示に応じてトラクタ１０を減速又は停止させる。
【0078】
［３－２－２ 誘導処理］
図８は、第２実施形態の誘導処理のフローチャートである。ユーザは、キャビン内の操作装置を操作して、装着支援装置７０に誘導処理を実行させる。図８で示されるステップＳ２１～ステップＳ２６の処理は、図６で示されるステップＳ１１～ステップＳ１６の処理と同じである。以下では、ステップＳ２７以降の処理を説明する。
図８は、第２実施形態の誘導処理のフローチャートである。ユーザは、キャビン内の操作装置を操作して、装着支援装置７０に誘導処理を実行させる。図８で示されるステップＳ２１～ステップＳ２６の処理は、図６で示されるステップＳ１１～ステップＳ１６の処理と同じである。以下では、ステップＳ２７以降の処理を説明する。
【0079】
ステップＳ２７において、走行制御部９０は、ステップＳ２６で算出したトラクタ１０の移動方向及び移動量に基づいて、トラクタ１０の操舵量と加速度と減速度とを演算する。走行制御部９０は、操舵装置８４に操舵量に応じた操舵指示を出力する。また、走行制御部９０は、駆動装置８６に加速度に応じた加速指示を出力する。また、走行制御部９０は、制動装置８８に減速度に応じた減速指示又は停止指示を出力する。これにより、トラクタ１０は、目標位置に向けて自律走行することができる。
【0080】
ステップＳ２８の処理は、ステップＳ１８の処理と同じである。トラクタ１０の現在位置と目標位置とが一致し、且つ、トラクタ１０の現在の向きとトラクタ１０の目標向きとが一致する場合（ステップＳ２８：ＹＥＳ）、処理はステップＳ２９に移行する。一方、トラクタ１０の現在位置と目標位置とが一致しない場合、又は、トラクタ１０の現在の向きとトラクタ１０の目標向きとが一致しない場合（ステップＳ２８：ＮＯ）、処理はステップＳ２６に戻る。
【0081】
ステップＳ２９において、走行制御部９０は、制動装置８８に停止指示を出力する。制動装置８８は、停止指示に応じてトラクタ１０を目標位置で停止させる。以上で誘導処理は終了する。
【0082】
誘導処理が終了すると、ユーザは、作業機昇降装置１６を操作して、トラクタ１０への作業機１２の装着作業を行う。
【0083】
以上のようにして、装着支援装置７０は、トラクタ１０を作業機１２の正面に誘導する。装着支援装置７０によれば、ユーザはトラクタ１０への作業機１２の装着作業を容易に行うことができる。また、軌道取得装置５０によれば、作業機１２の装着作業の時間を短縮することができる。
【0084】
［３－３ 他の実施形態の装着支援装置７０］
第１、第２実施形態では、トラクタ１０に特定の作業機１２が装着される。これに代わり、トラクタ１０に不特定の作業機１２が装着されてもよい。例えば、記憶部６２は、作業機マーカ７６の特徴量と、作業機１２の特有の情報（上部ピン４６の高さ、幅）とを関連付けるテーブルを記憶してもよい。この場合、動作演算部７８は、画像認識によって作業機マーカ７６の特徴量を算出し、その特徴量に対応する作業機１２の特有の情報を特定してもよい。
第１、第２実施形態では、トラクタ１０に特定の作業機１２が装着される。これに代わり、トラクタ１０に不特定の作業機１２が装着されてもよい。例えば、記憶部６２は、作業機マーカ７６の特徴量と、作業機１２の特有の情報（上部ピン４６の高さ、幅）とを関連付けるテーブルを記憶してもよい。この場合、動作演算部７８は、画像認識によって作業機マーカ７６の特徴量を算出し、その特徴量に対応する作業機１２の特有の情報を特定してもよい。
【0085】
トラクタ１０の外部にあるサーバが、作業機マーカ７６の特徴量と、作業機１２の特有の情報とを関連付けるテーブルを記憶してもよい。この場合、トラクタ１０とサーバとが相互に無線通信可能であるとよい。
【0086】
［４ 実施形態から得られる発明］
上記実施形態から把握しうる発明について、以下に記載する。
上記実施形態から把握しうる発明について、以下に記載する。
【0087】
本発明の第１の態様は、作業機（１２）をトラクタ（１０）に装着するために前記トラクタのリフトアーム（２８）を動作させることによって前記トラクタに取り付けられたヒッチフレーム（２０）の上部フック（３８）で前記作業機の上部ピン（４６）を掬い上げるときの前記上部フックの軌道を取得する軌道取得装置（５０）であって、前記トラクタに取り付けられており、前記リフトアームの動作によって移動する所定部位（５８）を撮影するカメラ（５２）と、前記上部フックの位置と前記所定部位の位置とを対応付けた対応関係を予め記憶する記憶部（６２）と、前記リフトアームが上下方向に動作している間に、前記リフトアームの複数の高さ位置の各々に対応する角度情報を取得すると共に、前記リフトアームが各々の高さに位置するときに前記カメラによって撮影された前記所定部位を含む画像を取得し、高さ位置毎に前記角度情報と前記画像とを対応付ける情報取得部（６４）と、前記情報取得部によって対応付けられた複数の組の前記角度情報及び前記画像と前記対応関係とを用いて前記リフトアームの回転角度に対応した前記上部フックの３次元の前記軌道を算出する軌道演算部（６６）とを備える。
【0088】
第１の態様において、軌道取得装置は、前記軌道が妥当であるか否かを判定する妥当性判定部（６８）を更に備え、前記軌道演算部は、前記情報取得部によって対応付けられた複数の組のうち、一部の複数の組の前記角度情報及び前記画像と前記対応関係とを用いて前記上部フックの３次元の前記軌道を算出し、前記妥当性判定部は、前記軌道演算部によって用いられていない１組以上の前記角度情報及び前記画像に基づいて定められる前記上部フックの３次元の位置が、前記軌道から所定距離離れた範囲内にあるか否かを判定し、前記上部フックの３次元の位置が前記範囲内にある場合に、前記軌道が妥当であると判定してもよい。
【0089】
本発明の第２の態様は、軌道取得装置によって取得された前記軌道を用いて前記作業機を前記トラクタに装着する作業を支援する装着支援装置（７０）であって、前記トラクタの移動方向及び移動量を算出する動作演算部（７８）と、前記動作演算部によって算出された前記移動方向及び前記移動量を報知装置（７４）に報知させる報知制御部（８０）とを備え、前記カメラは、前記作業機が有する所定の作業機側部位（７６）を撮影し、前記記憶部は、前記作業機側部位の位置と前記作業機の各部位の位置とを対応付けた作業機側対応関係を予め記憶し、前記動作演算部は、前記カメラによって撮影される前記作業機側部位を含む画像と前記作業機側対応関係とを用いて、前記上部ピンを含む前記作業機の各部位の３次元位置を算出し、３次元の前記軌道の一部と前記上部ピンの３次元位置とを一致させるために必要な前記移動方向及び前記移動量を算出する。
【0090】
本発明の第３の態様は、軌道取得装置によって取得された前記軌道を用いて前記作業機を前記トラクタに装着する作業を支援する装着支援装置であって、前記トラクタの移動方向及び移動量を算出する動作演算部と、前記動作演算部によって算出された前記移動方向及び前記移動量に基づいて前記トラクタを自律走行させる走行制御部（９０）とを備え、前記カメラは、前記作業機が有する所定の作業機側部位を撮影し、前記記憶部は、前記作業機側部位の位置と前記作業機の各部位の位置とを対応付けた作業機側対応関係を予め記憶し、前記動作演算部は、前記カメラによって撮影される前記作業機側部位を含む画像と前記作業機側対応関係とを用いて、前記上部ピンを含む前記作業機の各部位の３次元位置を算出し、３次元の前記軌道の一部と前記上部ピンの３次元位置とを一致させるために必要な前記移動方向及び前記移動量を算出する。
【符号の説明】
【0091】
１０…トラクタ １２…作業機
２８…リフトアーム ３８…上部フック
４６…上部ピン ５０…軌道取得装置
５２…カメラ ５８…トラクタマーカ（所定部位）
６２…記憶部 ６４…情報取得部
６６…軌道演算部 ６８…妥当性判定部
７０…装着支援装置 ７４…報知装置
７６…作業機マーカ（作業機側部位） ７８…動作演算部
８０…報知制御部 ９０…走行制御部
２８…リフトアーム ３８…上部フック
４６…上部ピン ５０…軌道取得装置
５２…カメラ ５８…トラクタマーカ（所定部位）
６２…記憶部 ６４…情報取得部
６６…軌道演算部 ６８…妥当性判定部
７０…装着支援装置 ７４…報知装置
７６…作業機マーカ（作業機側部位） ７８…動作演算部
８０…報知制御部 ９０…走行制御部
【図1】
【図2】
【図3】
【図4】
【図5】
【図6】
【図7】
【図8】