JP7083445B2 - 自律走行システム - Google Patents

Description

本発明は、自律走行システムに関する。

従来から、ＧＮＳＳ衛星から受信した電波に基づいて作業車両の位置情報を取得し、予め設定された経路に沿って作業車両を自律的に走行させる自律走行システムが知られている。特許文献１は、この種の自律走行システム（走行制御システム）を開示する。この特許文献１に記載の自律走行システムは、予め設定された経路の開始位置に対して、自律走行させる作業車両を、その前進方向を所定の方向に向けた状態で配置することで、自律走行を開始可能としている。このような自律走行を実現するためには、作業車両の向きが、作業車両を自律走行させる自律走行制御装置によって把握されていることが必要であると考えられる。

作業車両の向きを取得する方法の１つとして、３つのジャイロセンサ（角速度センサ）と３つの加速度センサを備える公知の慣性計測装置を作業車両に設けることが考えられる。この慣性計測装置は、互いに直交する３軸のそれぞれを中心とする回転の角速度をジャイロセンサで検知するとともに、当該３軸に沿う向きの加速度を加速度センサで検知することができる。慣性計測装置は、上記の３軸が、車両のピッチ軸、ロール軸、ヨー軸とそれぞれ平行となるように取り付けられる。この構成で、ある時点から現在までに得られた角速度を積分することで、当該時点に対する現在の作業車両の向きの変化量を取得することができる。

自律走行制御装置が自律走行を行う場合、作業車両の位置は、基本的にはＧＮＳＳ測位により得られた位置情報により求められるが、ＧＮＳＳ電波の受信状況等によっては、測位を良好に行うことができない場合もある。しかしながら、上記の慣性計測装置が作業車両に備えられていれば、ＧＮＳＳ測位ができない場合でも、慣性計測装置の検出結果を用いた慣性航法によって作業車両の位置を得ることができる。

ところで、慣性計測装置のジャイロセンサは作業車両の向きの変化を検出することはできるが、向きそのものを検出することはできない。従って、ある時点での車両の姿勢、具体的にはピッチ角、ロール角、及びヨー角を、慣性計測装置が把握していることが必要である。なお、以下では、上記の３つの角度のうち少なくともヨー角を求める処理を、初期化処理と呼ぶことがある。

上述したとおり、慣性計測装置は加速度センサを備えている。そこで、重力加速度が常に地球の中心を向くことを利用し、例えば車両を静止させた状態として、加速度センサを用いて重力加速度の向きを求めることで、車両のピッチ角及びロール角を求めることができる。一方、車両のヨー角については、原則として、慣性計測装置が備えるセンサによって求めることができない。

なお、地球の自転をジャイロセンサで検出することによってヨー角を求める技術も実用化されているが（ジャイロコンパス）、この方式は高精度のジャイロセンサが必要になるため、慣性計測装置のコストが増大してしまう。また、特許文献１のように方位センサを作業車両に設けることも考えられるが、これもやはりコストが増大する原因となる。

そこで、ＧＮＳＳ衛星から電波を受信できる構成を作業車両が備えていることを利用し、作業車両を前進させるようにユーザに手動運転させ、このときの地球に対する車両の位置の変化をＧＮＳＳ衛星の電波に基づいて検出し、この位置が変化した向きを車両の向きとみなして車両のヨー角を求めることが考えられる。

国際公開第２０１５／１１９２６５号

しかし、上記の構成では、ユーザが前記初期化処理のために作業車両を実際に移動させるときに、例えば前方に障害物が存在する等の種々の事情により、前進でなく一時的に後進しなければならない場合も考えられる。この場合には、作業車両の向きを実際とは１８０°異なる向きで把握してしまう誤った初期化がされてしまうおそれがあり、意図しない制御が行われる原因となる点で改善の余地があった。

本発明は以上の事情に鑑みてされたものであり、その目的は、自律走行システムにおいて作業車両の位置を意図的に変化させながら慣性計測装置の所定処理を行う場合に、作業車両の向きの誤検出を防止することにある。

課題を解決するための手段及び効果

本発明の解決しようとする課題は以上の如くであり、次にこの課題を解決するための手段とその効果を説明する。

本発明の第１の観点によれば、以下の構成の自律走行システムが提供される。即ち、この自律走行システムは、位置情報取得部と、自律走行制御部と、前後進検出部と、慣性計測部と、を備える。前記位置情報取得部は、衛星から受信した電波に基づいて作業車両の位置情報を取得する。前記自律走行制御部は、前記位置情報に基づいて、予め設定された経路に沿って前記作業車両を自律走行させることが可能である。前記前後進検出部は、前記作業車両の前進又は後進を検出可能である。前記慣性計測部は、前記作業車両の角速度及び加速度を検出する。前記慣性計測部は、前記作業車両の起動毎に、前記前後進検出部により前記前進が検出された場合は、前記作業車両の向きを前記位置情報に基づいて、初期化処理を実行する。

これにより、慣性計測部による検出結果の信頼性を高めることができる。

本発明の第２の観点によれば、以下の構成の自律走行システムが提供される。即ち、この自律走行システムは、位置情報取得部と、自律走行制御部と、前後進検出部と、慣性計測部と、を備える。前記位置情報取得部は、衛星から受信した電波に基づいて作業車両の位置情報を取得する。前記自律走行制御部は、前記位置情報に基づいて、予め設定された経路に沿って前記作業車両を自律走行させることが可能である。前記前後進検出部は、前記作業車両の前進又は後進を検出可能である。前記慣性計測部は、前記作業車両の角速度及び加速度を検出する。前記慣性計測装置は、前記作業車両の起動毎に、前記前後進検出部により前記後進が検出された場合は、前記位置情報の向きを反転させることにより前記作業車両の前進状態とみなすことで初期化処理を実行することを可能とする。

これにより、作業車両を後進させた場合でも、初期化処理を行うことができる。従って、ユーザの利便性を高めることができる。

本発明の一実施形態に係る自律走行システムに備えられるロボットトラクタの全体的な構成を示す側面図。 ロボットトラクタの平面図。 ロボットトラクタの主要な電気的構成を示すブロック図。 第１実施形態において、慣性計測装置の初期化処理で行われる判定及び処理を説明するフローチャート。 第２実施形態において、慣性計測装置の初期化処理で行われる判定及び処理を説明するフローチャート。

次に、図面を参照して本発明の実施の形態を説明する。以下では、図面の各図において同一の部材には同一の符号を付し、重複する説明を省略することがある。また、同一の符号に対応する部材等の名称が、簡略的に言い換えられたり、上位概念又は下位概念の名称で言い換えられたりすることがある。

本発明は、例えば下記の実施形態に示すように、予め定められた圃場内で１台又は複数台の作業車両を走行させて、圃場内における農作業の全部又は一部を実行させるときに、作業車両を自律的に走行させる自律走行システムに適用される。本実施形態では、作業車両としてトラクタを例に説明するが、作業車両としては、トラクタの他、田植機、コンバイン、土木・建設作業装置、除雪車等、乗用型作業機に加え、歩行型作業機も含まれる。本明細書において自律走行とは、トラクタが備える制御部（ＥＣＵ）によりトラクタが備える走行に関する構成が制御されて予め定められる経路に沿ってトラクタが走行することを意味し、自律作業とは、トラクタが備える制御部によりトラクタが備える作業に関する構成が制御されて、予め定められた経路に沿ってトラクタが作業を行うことを意味する。これに対して、手動走行・手動作業とは、トラクタが備える各構成がユーザにより操作され、走行・作業が行われることを意味する。

以下の説明では、自律走行・自律作業されるトラクタを「無人（の）トラクタ」又は「ロボットトラクタ」と称することがあり、手動走行・手動作業されるトラクタを「有人（の）トラクタ」と称することがある。圃場内において農作業の一部が無人トラクタにより実行される場合、残りの農作業は有人トラクタにより実行される。単一の圃場における農作業を無人トラクタ及び有人トラクタで実行することを、農作業の協調作業、追従作業、随伴作業等と称することがある。本明細書において無人トラクタと有人トラクタの違いは、ユーザによる操作の有無であり、各構成は基本的に共通であるものとする。即ち、無人トラクタであってもユーザが搭乗（乗車）して操作することが可能であり（即ち、有人トラクタとして使用することができ）、あるいは有人トラクタであってもユーザが降車して自律走行・自律作業させることが可能である（即ち、無人トラクタとして使用することができる）。なお、農作業の協調作業としては、「単一の圃場における農作業を無人車両及び有人車両で実行すること」に加え、「隣接する圃場等の異なる圃場における農作業を同時期に無人車両及び有人車両が実行すること」が含まれていてもよい。

＜第１実施形態＞
次に、図面を参照して本発明の第１実施形態を説明する。図１は、本発明の一実施形態に係る自律走行システム１００に備えられるロボットトラクタ１の全体的な構成を示す側面図である。図２は、ロボットトラクタ１の平面図である。図３は、ロボットトラクタ１の主要な電気的構成を示すブロック図である。

本実施形態のロボットトラクタ１は、自律走行システム１００において生成した自律走行経路（経路）に沿って自律走行しながら自律作業を行うように構成されている。

ロボットトラクタ１は、無線通信端末４６との間で近距離無線網を介して無線通信を行うことにより操作される。ユーザが無線通信端末４６のタッチパネル３９を操作して、当該トラクタ１の制御部（自律走行制御部）４との間で信号のやり取りを適宜行うことにより、トラクタ１を自律走行・自律作業させることができる。

トラクタ１は、圃場内を自律走行することが可能な走行機体２を備える。走行機体２には、図１及び図２に示す作業機３が着脱可能に取り付けられている。この作業機３としては、例えば、耕耘機、プラウ、施肥機、草刈機、播種機等の種々の作業機があり、これらの中から必要に応じて所望の作業機３を選択して走行機体２に装着することができる。走行機体２は、装着された作業機３の高さ及び姿勢を変更可能に構成されている。

トラクタ１の構成について、図１及び図２を参照してより詳細に説明する。トラクタ１の走行機体２は、図１に示すように、その前部が左右１対の前輪７，７で支持され、その後部が左右１対の後輪８，８で支持されている。

走行機体２の前部にはボンネット９が配置されている。このボンネット９内には、トラクタ１の駆動源であるエンジン１０等が収容されている。このエンジン１０は、例えばディーゼルエンジンにより構成することができるが、これに限るものではなく、例えばガソリンエンジンにより構成してもよい。また、駆動源としては、エンジン１０に加えて、又はこれに代えて、電気モータを使用してもよい。

ボンネット９の後方には、ユーザが搭乗するためのキャビン１１が配置されている。このキャビン１１の内部には、ユーザが操向操作するためのステアリングハンドル１２と、ユーザが着座可能な座席１３と、各種の操作を行うための様々な操作装置と、が主として設けられている。ただし、作業車両は、キャビン１１付のものに限るものではなく、キャビン１１を備えないものであってもよい。

上記の操作装置としては、図２に示すモニタ装置１４、スロットルレバー１５、主変速レバー２７、複数の油圧操作レバー１６、ＰＴＯスイッチ１７、ＰＴＯ変速レバー１８、副変速レバー１９、及び作業機昇降スイッチ２８等を例として挙げることができる。これらの操作装置は、座席１３の機能、又はステアリングハンドル１２の近傍に配置されている。

モニタ装置１４は、トラクタ１の様々な情報を表示可能に構成されている。スロットルレバー１５は、エンジン１０の出力回転数を設定するための操作具である。主変速レバー２７は、トラクタ１の走行を無段階で変更するための操作具である。ＰＴＯスイッチ１７は、トランスミッション２２の後端から突出した図略のＰＴＯ軸（動力取出軸）への動力の伝達／遮断を切換操作するための操作具である。即ち、ＰＴＯスイッチ１７がＯＮ状態であるときＰＴＯ軸に動力が伝達されてＰＴＯ軸が回転し、作業機３が駆動される一方、ＰＴＯスイッチ１７がＯＦＦ状態であるときＰＴＯ軸への動力が遮断されてＰＴＯ軸が回転せず、作業機３が停止される。ＰＴＯ変速レバー１８は、作業機３に入力される動力の変更操作を行うものであり、具体的にはＰＴＯ軸の回転速度の変速操作を行うための操作具である。副変速レバー１９は、トランスミッション２２内の走行副変速ギア機構の変速比を切り換えるための操作具である。作業機昇降スイッチ２８は、走行機体２に装着された作業機３の高さを所定範囲内で昇降操作するための操作具である。

図１に示すように、走行機体２の下部には、トラクタ１のシャーシ２０が設けられている。当該シャーシ２０は、機体フレーム２１、トランスミッション２２、フロントアクスル２３、及びリアアクスル２４等から構成されている。

機体フレーム２１は、トラクタ１の前部における支持部材であって、直接、又は防振部材等を介してエンジン１０を支持している。トランスミッション２２は、エンジン１０からの動力を変化させてフロントアクスル２３及びリアアクスル２４に伝達する。フロントアクスル２３は、トランスミッション２２から入力された動力を前輪７に伝達するように構成されている。リアアクスル２４は、トランスミッション２２から入力された動力を後輪８に伝達するように構成されている。

図３に示すように、トラクタ１は、走行機体２の動作（前進、後進、停止及び旋回等）並びに作業機３の動作（昇降、駆動及び停止等）を制御するための制御部４を備える。制御部４は、図示しないＣＰＵ、ＲＯＭ、ＲＡＭ、Ｉ／Ｏ等を備えて構成されており、ＣＰＵは、各種プログラム等をＲＯＭから読み出して実行することができる。制御部４には、トラクタ１が備える各構成（例えば、エンジン１０等）を制御するためのコントローラ、及び他の無線通信機器と無線通信するための無線通信機等がそれぞれ、ＣＡＮ等の規格によって電気的に接続されている。

上記のコントローラとして、トラクタ１は少なくとも、エンジンコントローラ４１、車速コントローラ４２、操向コントローラ４３及び昇降コントローラ４４を備える。それぞれのコントローラは、制御部４からの電気信号に応じて、トラクタ１の各構成を制御することができる。

エンジンコントローラ４１は、エンジン１０の回転数等を制御するものである。エンジンコントローラ４１は、エンジン１０に設けられる燃料噴射装置としてのコモンレール装置（図略）と電気的に接続されている。コモンレール装置は、エンジン１０の各気筒に燃料を噴射するものである。この場合、エンジン１０の各気筒に対するインジェクタの燃料噴射バルブが開閉制御されることによって、燃料供給ポンプによって燃料タンクからコモンレール装置に圧送された高圧の燃料が各インジェクタからエンジン１０の各気筒に噴射され、各インジェクタから供給される燃料の噴射圧力、噴射時期、噴射期間（噴射量）が高精度にコントロールされる。エンジンコントローラ４１は、コモンレール装置を制御することで、例えばエンジン１０への燃料の供給を停止させ、エンジン１０の駆動を停止させることができる。

車速コントローラ４２は、トラクタ１の車速を制御するものである。具体的には、トランスミッション２２には、例えば可動斜板式の油圧式無段変速装置（図略）が設けられている。車速コントローラ４２は、油圧式無段変速装置の斜板の角度をアクチュエータによって変更することで、トランスミッション２２の変速比を変更し、所望の車速を実現することができる。

操向コントローラ（走行装置）４３は、ステアリングハンドル１２の回動角度を制御するものである。具体的には、ステアリングハンドル１２の回転角（ステアリングシャフト）の中途部には、操向アクチュエータが設けられている。この構成で、予め定められた経路をトラクタ１が（無人トラクタとして）走行する場合、制御部４は、当該経路に沿ってトラクタ１が走行するようにステアリングハンドル１２の適切な回動角度を計算し、得られた回動角度となるように操向コントローラ４３に制御信号を出力する。操向コントローラ４３は、制御部４から入力された制御信号に基づいて操向アクチュエータを駆動し、ステアリングハンドル１２の回動角度（操舵角）を制御する。なお、操向コントローラ４３はステアリングハンドル１２の回動角度を調整するものではなくトラクタ１の前輪７の舵角を直接的に調整するものであってもよく、その場合、旋回走行を行ったとしてもステアリングハンドル１２は回転しない。

昇降コントローラ４４は、作業機３の昇降を制御するものである。具体的には、トラクタ１は、作業機３を走行機体２に連結している３点リンク機構の近傍に、油圧シリンダ等からなる昇降アクチュエータ（図略）を備えている。この構成で、昇降コントローラ４４は、制御部４から入力された制御信号に基づいて昇降アクチュエータを駆動して作業機３を適宜に昇降動作させることにより、所望の高さで作業機３により農作業を行わせることができる。この制御により、作業機３を、退避高さ（農作業を行わない高さ）及び作業高さ（農作業を行う高さ）等の所望の高さで支持することができる。

なお、上述した複数のコントローラ４１，４２，４３，４４は、制御部４から入力される信号に基づいてエンジン１０等の各部を制御していることから、制御部４が実質的に各部を把握していると把握することができる。

上述のような制御部４を備えるトラクタ１は、ユーザがキャビン１１内に搭乗して各種操作をすることにより、当該制御部４によりトラクタ１の各部（走行機体２、作業機３等）を制御して、圃場内を走行しながら農作業を行うことができるように構成されている。加えて、本実施形態のトラクタ１は、ユーザがトラクタ１に搭乗しなくても、無線通信端末４６により出力される所定の制御信号に基づいて自律走行及び自律作業をさせることが可能となっている。

具体的には、図３等に示すように、トラクタ１は、自律走行・自律作業を可能とするための各種構成を備えている。例えば、トラクタ１は、衛星（測位衛星）１０５，１０５，・・・から受信した電波に基づいて自ら（走行機体２）の位置情報を時々刻々と取得するために必要な測位用アンテナ６等を備える。このような構成により、トラクタ１は、衛星１０５，１０５，・・・から受信した電波に基づいて自らの位置情報を取得して、圃場の予め設定された経路上を自律走行することが可能となっている。

次に、自律走行を可能とするためにトラクタ１が備える構成について、より詳細に説明する。具体的には、本実施形態のトラクタ１は、図３等に示すように、測位用アンテナ６、位置情報取得部５２、無線通信用アンテナ４８、近距離無線通信機５１、カメラ５６、慣性計測装置５４、及び記憶部５５等を備える。

測位用アンテナ６は、衛星測位システム（ＧＮＳＳ）を構成する衛星１０５，１０５，・・・からの電波を受信するものである。なお、本実施形態では、ＧＮＳＳとして全地球測位システム（ＧＰＳ）が用いられている。図１に示すように、測位用アンテナ６は、トラクタ１のキャビン１１のルーフ２６の上面に取り付けられている。

位置情報取得部５２は、入力された測位信号に基づいて、公知のＧＮＳＳ－ＲＴＫ法を利用することにより位置情報（緯度・経度情報）を取得する。ＧＮＳＳ－ＲＴＫ法は公知であるので詳細な説明は省略するが、位置情報取得部５２は、自らが測位することにより位置情報を取得するとともに、位置が既知の基準局で取得（計算）された測位補正情報を無線通信により時々刻々と受信し、この測位補正情報に基づいて前記位置情報を補正する。また、位置情報取得部５２及び基準局においては、ＧＮＳＳ電波で受信したデータ（航法メッセージ）に基づく計測だけでなく、波としてのＧＮＳＳ電波（搬送波）の位相の検出に基づく計測が行われる。これにより、走行機体２の位置情報を通常のＧＮＳＳ測位よりも相当に高い精度で、具体的には誤差数センチメートル程度の精度で取得することができる。位置情報取得部５２で取得された位置情報は、記憶部５５に記憶されるとともに、適時に記憶部５５から読み出されて制御部４に入力されて、自律走行に利用される。

無線通信用アンテナ４８は、ユーザが使用する無線通信端末４６との通信に用いられる近距離無線網の規格に対応したアンテナである。無線通信用アンテナ４８は、トラクタ１のキャビン１１が備えるルーフ２６の上面に配置されている。無線通信用アンテナ４８で受信した無線通信端末４６からの信号は、近距離無線通信機５１で信号処理された後、制御部４に入力される。また、制御部４等から無線通信端末４６に送信する信号は、近距離無線通信機５１で信号処理された後、無線通信用アンテナ４８から送信されて無線通信端末４６で受信される。

近距離無線通信機５１は、無線通信用アンテナ４８で受信された無線通信端末４６からの信号を復調処理して取り出し、制御部４に入力する。これにより、無線通信端末４６を用いてユーザが発した指示等が制御部４に入力されて、トラクタ１の各部を制御するのに用いられる。

カメラ５６は、トラクタ１の周辺の環境を撮影するものである。図１及び図２には示していないが、カメラ５６はトラクタ１のルーフ２６に取り付けられている。カメラ５６で撮影された映像データは、近距離無線通信機５１で変調処理された後、無線通信用アンテナ４８から送信され、無線通信端末４６で受信される。この受信された映像データに基づいて、無線通信端末４６の表示制御部３１により表示用データが生成され、当該表示用データに基づいて、カメラ５６で撮影された映像がディスプレイ３７に表示される。

記憶部５５は、トラクタ１を自律走行させるために予め定められた経路（走行経路）を記憶したり、トラクタ１（厳密には、測位用アンテナ６）の位置情報（位置情報、速度ベクトル情報等）を記憶したりするメモリである。

慣性計測装置５４は、トラクタ１の走行機体２の姿勢や加速度等を特定することが可能なセンサユニットである。具体的には、慣性計測装置５４は、互いに直交する第１軸、第２軸、及び第３軸のそれぞれに対して、角速度センサと加速度センサとを取り付けたセンサ群を備える。

詳述すると、慣性計測装置５４は、第１軸方向の加速度を検出する第１加速度センサと、第２軸方向の加速度を検出する第２加速度センサと、第３軸方向の加速度を検出する第３加速度センサと、前記第１軸回りの角速度を検出する第１角速度センサと、前記第２軸回りの角速度を検出する第２角速度センサと、第３軸回りの角速度を検出する第３角速度センサと、を備えるものである。

慣性計測装置５４は、第１角速度センサがトラクタ１のロール角速度を、第２角速度センサがトラクタ１のピッチ角速度を、第３角速度センサがトラクタ１のヨー角速度を検出できるように、トラクタ１の走行機体２に対して方向が定められて、走行機体２の重心位置に取り付けられている。言い換えれば、第１軸は、走行機体２の前後方向と一致するように、即ちロール回転軸となるように配置される。第２軸は、走行機体２の左右方向と一致するように、即ちピッチ回転軸となるように配置される。第３軸は、走行機体２の上下方向と一致するように、即ちヨー回転軸となるように配置される。

このような構成の慣性計測装置５４の検出結果により、トラクタ１の走行機体２の姿勢変化の角速度（ロール角速度、ピッチ角速度、及びヨー角速度）、並びに、前後方向、左右方向、及び上下方向の加速度を特定できるようになっている。そして、得られた角速度を積分した結果が、走行機体２の姿勢を取得するために用いられる。走行機体２の姿勢に関する情報は制御部４に入力されて、位置情報取得部５２が取得した位置情報を補正するのに用いたり、その他の制御に用いられたりする。

また、慣性計測装置５４で取得された走行機体２の姿勢変化及び加速度に関する情報を用いて公知の慣性航法演算を行うことにより、衛星１０５，１０５，・・・からの電波が一時的に途切れる等して位置情報を算出できなくなった場合に、その間のトラクタ１の位置を求めることができる。

このような構成のトラクタ１に適切に自律走行を行わせるためには、トラクタ１の位置情報が制御部４によって正確に把握されているだけでは足りず、トラクタ１（走行機体２）の向きが制御部４によって正確に把握されていることが必要である。この点、慣性計測装置５４の角速度センサは、トラクタ１の向きの変化を検出することはできるが、トラクタ１の向きそのものを検出することはできない。

ここで、前述したとおり、走行機体２のロール角とピッチ角に関しては、トラクタ１が停止中（静止中）に作用する重力加速度の方向を３つの加速度センサで検出することにより求めることができるが、ヨー角に関しては重力加速度を手掛かりにして求めることができない。なお、ヨー角を求めるために例えば電子コンパスや機械ジンバル式コンパス等の公知のコンパスをトラクタ１に備えることも考えられるが、この場合はコストが上昇する原因となる。

そこで、本実施形態では、ユーザの手動操作によりトラクタ１を実際に前進させるとともに、ＧＮＳＳ電波を用いてこのときのトラクタ１の位置変化を求め、当該位置変化の向きに基づいてヨー角を求めることとしている。なお、この所定処理（以下、初期化処理と呼ぶことがある。）の処理の詳細は後述する。

このようにトラクタ１の姿勢（ロール角、ピッチ角及びヨー角）の初期値が与えられれば、その後のトラクタ１の向きの変化を角速度センサで継続的に検出している限り、前記初期値に対して角速度センサの検出値の積分結果を加算することで、当該トラクタ１の現在の姿勢をリアルタイムで得ることができる。そして、ＧＮＳＳ測位ができない場合でも、トラクタ１の姿勢と、加速度センサの検出結果と、に基づいて、公知の慣性航法によりトラクタ１の位置を推定することができる。

以下では、慣性計測装置５４の初期化処理に伴う種々の判定を行うために、自律走行システム１００に備えられる構成について、図３を参照して詳細に説明する。具体的には、本実施形態の自律走行システム１００は、前後進検出部５３、操舵角検出部（旋回検出部）５７、振動検出部５８、及び車速検出部５９を備える。

前後進検出部５３は、前輪７の回転を検出することにより、トラクタ１が前向きに進んでいる場合、後ろ向きに進んでいる場合、及び中立状態の場合（前向きにも後ろ向きにも進んでいない場合）を検出できるものである。前後進検出部５３の検出結果は、制御部４に入力される。

操舵角検出部５７は、ステアリングハンドル１２の操舵角を検出するものである。操舵角検出部５７としては、公知の様々なセンサを採用し得るが、例えばロータリポテンショメータにより構成することができる。操舵角検出部５７の検出結果は、制御部４に入力される。

振動検出部５８は、トラクタ１の走行機体２の振動を検出するものである。具体的には、本実施形態の振動検出部５８は、慣性計測装置５４の３つの加速度センサの検出値を記憶部５５から読み出し、これを総合的に評価することにより走行機体２の振動を検出する。振動検出部５８の検出結果は、制御部４に入力される。

車速検出部（車速特定部）５９は、トラクタ１の車速（走行速度）を検出するものである。車速を検出する方法は様々であるが、例えば、トランスミッション２２の内部に配置される図示しないギアの外周に対向するように図略の回転センサを設け、この回転センサがギア歯を検出して出力するパルス信号をカウントすることが考えられる。車速検出部５９が取得した車速の情報は、制御部４に入力される。

以下では、本実施形態の自律走行システム１００で行われる慣性計測装置５４の初期化処理について、図４を参照して詳細に説明する。図４は、本実施形態の慣性計測装置５４の初期化処理で行われる判定及び処理を説明するフローチャートである。なお、図４に示す一連の判定及び処理は、トラクタ１が起動されたこと（具体的には、図示しないエンジンスイッチがＯＮされたこと）をきっかけにして行われる。言い換えれば、図４に示す一連の判定及び処理は、トラクタ１の起動毎に行われる。これにより、例えば慣性計測装置５４への電力供給が無い状態でトラクタ１の向きが運搬等によって変化した場合でも、変化後のトラクタ１の向きに対して慣性計測装置５４を適切に初期化することができる。

トラクタ１が起動されると、制御部４は、トラクタ１が備えるモニタ装置１４に、「初期化のため、トラクタをできるだけ真っ直ぐ前進させて下さい」というメッセージを表示させる。これを見たユーザは、スロットルレバー１５及び主変速レバー２７等を操作し、トラクタ１を前進させる。

初めに、制御部４は、前後進検出部５３の検出結果を取得して、走行機体２の前進が検出されているか否かを判定する（ステップＳ１０１）。

ステップＳ１０１の判定の結果、前後進検出部５３で走行機体２の前進が検出されていなかった場合（ステップＳ１０１、Ｎｏ）、制御部４は、走行機体２の前進が検出されるまで、当該判定を繰り返し行って待機する。

ステップＳ１０１の判定の結果、前後進検出部５３で走行機体２の前進が検出されていた場合（ステップＳ１０１、Ｙｅｓ）、続いて制御部４は、車速検出部５９の検出結果を取得して、走行機体２の車速が閾値を上回っているか否かを判定する（ステップＳ１０２）。

ステップＳ１０２の判定の結果、車速検出部５９で検出された車速が閾値以下である場合（ステップＳ１０２、Ｎｏ）、検出される車速の向きに対する後述の誤差の影響が相対的に大きくなるので、走行機体２の向きを正確に得ることができないと考えられる。この場合、制御部４はステップＳ１０１に戻って上述の判定を繰り返す。

ステップＳ１０２の判定の結果、振動検出部５８で検出された車速が閾値を上回る場合（ステップＳ１０２、Ｙｅｓ）、続いて制御部４は、振動検出部５８の検出結果を取得して、走行機体２の振動の大きさが閾値未満であるか否かを判定する（ステップＳ１０３）。

ステップＳ１０３の判定の結果、振動検出部５８で検出された振動の大きさが閾値以上である場合（ステップＳ１０３、Ｎｏ）、走行機体２の挙動が不安定であるために、当該走行機体２の向きを正確に得ることができない可能性がある。この場合、制御部４はステップＳ１０１に戻って上述の判定を繰り返す。

ステップＳ１０３の判定の結果、振動検出部５８で検出された振動の大きさが閾値未満である場合（ステップＳ１０３、Ｙｅｓ）、続いて制御部４は、操舵角検出部５７の検出結果を取得して、ステアリングハンドル１２の操舵角が閾値未満であるか否かを判定する（ステップＳ１０４）。

ステップＳ１０４の判定の結果、操舵角検出部５７で検出された操舵角が閾値以上であった場合（ステップＳ１０４、Ｎｏ）、走行機体２の向きが急激に変化しているために、当該走行機体２の向きを正確に得ることができない可能性がある。この場合、制御部４はステップＳ１０１に戻って上述の判定を繰り返す。

ステップＳ１０４の判定の結果、操舵角検出部５７で検出された操舵角の大きさが閾値未満であった場合（ステップＳ１０４、Ｙｅｓ）、走行機体２の向きを正確に検出できる条件が揃ったと言うことができる。そこで、制御部４は初期化処理を行う（ステップＳ１０５）。

ステップＳ１０５の処理を具体的に説明すると、制御部４は、衛星１０５，１０５，・・・から受信したＧＮＳＳ電波に乗せられた航法メッセージの衛星軌道情報から求めた衛星１０５，１０５，・・・の速度と、ＧＮＳＳ電波の受信時にドップラー効果によって生じる搬送波の周波数変化を計測することにより求めた衛星１０５，１０５，・・・に対する走行機体２の相対速度と、に基づいて、走行機体２の速度ベクトルを計算する。

ＧＮＳＳとしてＧＰＳを用いた場合、ＧＰＳの搬送波は複数種類あるが、例えばＬ１と呼ばれる搬送波（１５７５．４２ＭＨｚ）を計測する場合、その波長は約１９センチメートルである。従って、上記のように搬送波の波としての性質（ドップラー効果）を測定することで、衛星１０５，１０５，・・・と走行機体２との距離の変化を、誤差が数センチメートル毎秒というような高い精度で計測することができる。従って、ある程度以上の車速で走行機体２を走行させていれば（ステップＳ１０２）、その速度ベクトルを十分に高い精度で得ることができる。

ここで、上述したように、本来ユーザがモニタ装置１４の表示に従ってトラクタ１を前進させるべきところ、障害物の回避等の事情で後進させる場合も考えられる。しかし、本実施形態では、後進が検出された場合は初期化処理が行われないように制御部４が制御するので（ステップＳ１０１）、速度ベクトルの向きと走行機体２の向きが逆の状態で初期化処理がされることはない。従って、慣性計測装置５４の検出結果の信頼性を高めることができる。

得られた速度ベクトルは、北方向の速度成分と、東方向の速度成分と、下方向の速度成分と、により表すことができる。北方向の速度成分に対する東方向の速度成分の比について、逆正接を計算することにより、慣性計測装置５４を初期化するためのヨー角（初期ヨー角）を計算することができる。

このように、本実施形態では、慣性計測装置５４を初期化するためのヨー角を、ＧＰＳ電波のドップラー効果を利用して、高い精度で得ることができる。従って、これを基準として慣性計測装置５４が出力する走行機体２の姿勢も高精度となることが期待され、ＧＮＳＳ－ＲＴＫ測位ができなくなった場合に行われる慣性航法での位置特定を、ＧＮＳＳ－ＲＴＫ測位に代替可能な精度で行うことができる。

以上に説明したように、本実施形態の自律走行システム１００は、位置情報取得部５２と、制御部４と、前後進検出部５３と、慣性計測装置５４と、を備える。位置情報取得部５２は、衛星１０５，１０５，・・・から受信した電波に基づいてトラクタ１の位置情報を取得する。制御部４は、前記位置情報に基づいて、予め設定された経路に沿ってトラクタ１を自律走行させることが可能である。前後進検出部５３は、トラクタ１の前進又は後進を検出可能である。慣性計測装置５４は、トラクタ１の角速度及び加速度を検出する。慣性計測装置５４は、トラクタ１を移動させながら初期化処理を実行することにより、当該初期化処理の実行時点でのトラクタ１の向きを、衛星１０５，１０５，・・・から受信した前記電波に基づいて得られたトラクタ１の位置変化の向きに基づいて求めることが可能である。慣性計測装置５４は、前後進検出部５３により前進が検出された場合は初期化処理を実行する一方、前後進検出部５３により後進が検出された場合は初期化処理を実行しない。

これにより、トラクタ１の後進時は慣性計測装置５４が初期化処理を実行しないので、トラクタ１の向きを実際の向きと１８０°異なって把握してしまう誤った初期化処理を防止することができる。この結果、慣性計測装置５４による検出結果の信頼性を高めることができる。

また、本実施形態の自律走行システム１００は、トラクタ１の旋回の度合いを検出する操舵角検出部５７を備える。慣性計測装置５４は、操舵角検出部５７により検出された旋回の度合い（具体的には、操舵角）が閾値を超える場合に、初期化処理を実行しない。

これにより、例えば障害物を回避するために旋回が行われてトラクタ１の向きが急激に変化し、トラクタ１の向きを精度よく得ることが難しい場合に、慣性計測装置５４が初期化処理を行うことを防止することができる。従って、不正確な作業車両の向きに基づいた初期化処理を防止することができる。

また、本実施形態の自律走行システム１００は、トラクタ１の振動を検出する振動検出部５８を備える。慣性計測装置５４は、振動検出部５８により検出された振動が閾値を超える場合に、初期化処理を実行しない。

これにより、トラクタ１に発生する振動が大きく、トラクタ１の向きを精度よく得ることが難しい場合に、慣性計測装置５４が初期化処理を行うのを防止することができる。

また、本実施形態の自律走行システム１００は、トラクタ１の車速を特定する車速検出部（車速特定部）５９を備える。慣性計測装置５４は、車速検出部５９により特定された車速が閾値未満である場合に、初期化処理を実行しない。

これにより、車速が遅く、トラクタ１の位置変化の向きを精度よく得ることが難しい場合に、慣性計測装置５４が初期化処理を行うのを防止することができる。

＜第２実施形態＞
次に、図５を参照して、本発明の第２実施形態を説明する。図５は、第２実施形態において、慣性計測装置５４の初期化処理で行われる判定及び処理を説明するフローチャートである。なお、本実施形態の説明においては、前述の実施形態と同一又は類似の部材には図面に同一の符号を付し、説明を省略する場合がある。

第２実施形態に係る自律走行システム１００では、ユーザが走行機体２を前向きに移動させた場合だけでなく、後ろ向きに移動させた場合にも初期化処理を行えるようにしている点で、第１実施形態に係る自律走行システム１００とは異なっている。

図５に示す一連の判定及び処理は、前記第１実施形態と同様に、トラクタ１が起動する度に行われる。

初めに、制御部４は、初期化処理の開始条件を満たすか否かを判定するために、ステップＳ２０１からステップＳ２０３までの処理を行う。ステップＳ２０１は上記のステップＳ１０２と、ステップＳ２０２は上記のステップＳ１０３と、ステップＳ２０３は上記のステップＳ１０４と、それぞれ同様であるため、説明を省略する。

ステップＳ２０４において、制御部４は、第１実施形態（ステップＳ１０５）で説明したのと同様に、ＧＮＳＳ電波のドップラー効果による搬送波の周波数変化を利用して、走行機体２の速度ベクトルを計算する。

次に、制御部４は、前後進検出部５３の検出結果を取得して、走行機体２の前進が検出されているか否かを判定する（ステップＳ２０５）。

ステップＳ２０５の判定の結果、前後進検出部５３で走行機体２の後進が検出されていた場合（ステップＳ２０５、Ｎｏ）、ステップＳ２０４で生じていた走行機体２の位置の変化は後ろ向きだったことになる。従って、制御部４は、ステップＳ２０４で得られた速度ベクトルの向きを反転させる（ステップＳ２０６）。一方、前後進検出部５３で走行機体２の前進が検出されていた場合（ステップＳ２０５、Ｙｅｓ）、上記のステップＳ２０６の処理はスキップされる。その後、制御部４は、前記速度ベクトルに基づいてヨー角を求める（ステップＳ２０７）。

本実施形態では、ユーザが走行機体２を前向きに移動させた場合だけではなく、後ろ向きに移動させた場合にも、初期化処理を適切に行うことができる。よって、慣性計測装置５４の初期化処理を圃場の状態等に合わせて臨機応変に行うことが可能となり、ユーザの利便性を向上させることができる。

以上に説明したように、本実施形態の自律走行システム１００は、位置情報取得部５２と、制御部４と、前後進検出部５３と、慣性計測装置５４と、を備える。位置情報取得部５２は、衛星１０５，１０５，・・・から受信した電波に基づいてトラクタ１の位置情報を取得する。制御部４は、前記位置情報に基づいて、予め設定された経路に沿ってトラクタ１を自律走行させることが可能である。前後進検出部５３は、トラクタ１の前進又は後進を検出可能である。慣性計測装置５４は、トラクタ１の角速度及び加速度を検出する。慣性計測装置５４は、トラクタ１を移動させながら初期化処理を実行することにより、当該初期化処理の実行時点でのトラクタ１の向きを、衛星１０５，１０５，・・・から受信した前記電波に基づいて得られたトラクタ１の位置変化の向きに基づいて求めることが可能である。慣性計測装置５４は、初期化処理の実行中において前後進検出部５３により前進が検出された場合は、前記位置変化の向きをトラクタ１の向きとする一方、初期化処理の実行中において前後進検出部５３により後進が検出された場合は、前記位置変化と反対の向きをトラクタ１の向きとする。

これにより、トラクタ１を前進させた場合でも、後進させた場合でも、慣性計測装置５４がトラクタ１の向きを正しく把握して初期化処理を行うことができる。従って、ユーザの利便性を高めることができる。

以上に本発明の好適な実施の形態を説明したが、上記の構成は例えば以下のように変更することができる。

慣性計測装置５４の初期化処理は、上記のようにドップラー効果に基づいて速度ベクトルを求める方法に代えて、トラクタ１の移動前と移動後の位置情報をそれぞれＧＮＳＳ測位により求め、２つの位置情報の推移から位置変化の向きを求めてトラクタ１の向きを得る方法で行ってもよい。ＧＮＳＳ－ＲＴＫ法を用いることで移動前と移動後の位置は精度よく求められるので、上記の方法によっても、トラクタ１の正確な向きを求めて慣性計測装置５４の初期化処理を行うことができる。ただし、上記の実施形態のようにドップラー効果を用いる方法は、基準局を用いずに、かつ、ＧＮＳＳ－ＲＴＫ法でいわゆる整数バイアスを決定する処理を待たずに、トラクタ１の速度を高精度にかつ早期に得られる点で有利である。

前後進検出部５３は、前輪７の回転を検出する構成に代えて、前後進を指示する操作部材の操作位置を検出する構成に変更することができる。

上記の実施形態では、車速検出部（車速特定部）５９は、図略のギアの回転を検出することによりトラクタ１の車速を特定するものとしたが、必ずしもこれに限るものではない。例えばこれに代えて、車速特定部を、前輪７又は後輪８の回転数を検出することにより車速を検出する車速センサ等としてもよい。

図４のステップＳ１０５で説明したようなトラクタの速度ベクトルの算出処理を短い時間間隔で反復して行うように構成し、トラクタ１に振動が発生しているか否かを、速度ベクトルの変化が閾値以上であるか否かによって判定してもよい。また、旋回の度合いも、速度ベクトルの変化によって判定することができる。

トラクタ１の旋回の度合いは、ステアリングハンドル１２の操舵角を検出することに代えて、前輪７の舵角を切れ角センサにより検出することで判定してもよい。

上記の実施形態で示した慣性計測装置５４の初期化処理の開始条件は、一例に過ぎず、これらの開始条件のうちの一部が省略されてもよい。或いは、他の開始条件が更に加えられていてもよい。例えば、トラクタ１に、ユーザが慣性計測装置５４の初期化処理を開始したいときに操作する所定の操作具を備える構成とし、この操作具が操作されることを開始条件の１つに加えてもよい。

また、上記の実施形態で示した慣性計測装置５４の初期化処理の開始条件の判定の順序は、一例に過ぎず、順序が入れ替わってもよい。

慣性計測装置５４による慣性航法は、時間の経過により位置の検出誤差が累積していく性質を有する。これを考慮して、慣性計測装置５４の初期化処理を、トラクタ１の起動直後に限らず、起動後の適宜のタイミングで随時行うように構成してもよい。

上記の実施形態では、ＧＮＳＳ法で得られる位置情報に対して適宜補正を加えるいわゆるＧＮＳＳ－ＲＴＫ法を利用した高精度の衛星測位システムが用いられているが、これに限られるものではなく、高精度の位置座標が得られる限りにおいて他の測位システムを用いてもよい。例えば、相対測位方式（ＤＧＰＳ）、又は静止衛星型衛星航法補強システム（ＳＢＡＳ）を使用することが考えられる。

エンジン１０がＯＮにされてから一定の時間以上が経っても前記所定処理が行われない場合に、表示制御部３１によって無線通信端末４６のディスプレイ３７に警告画面を表示させること等により、ユーザに対して慣性計測装置５４の初期化処理を促すこととしてもよい。

上記の実施形態において図４及び図５の判定及び処理は、トラクタ１の起動をトリガとして行われることとしたが、トリガはこれに限られるものではない。例えば、第１実施形態では、トラクタ１の起動後、前後進検出部５３によりトラクタ１の前進が検出されることをトリガとし、第２実施形態では、トラクタ１の起動後、前後進検出部５３によりトラクタ１の前進又は後進が検出されることをトリガとしてもよい。この場合、図４のフローチャートでは、その後、ステップＳ１０２～Ｓ１０４の判定を行ったのち、これら初期化処理の開始条件が揃うことで初期化処理が実行され、図５のフローチャートでは、その後、ステップＳ２０１～Ｓ２０３の判定を行ったのち、これら初期化処理の開始条件が揃うことで前後進検出部５３の検出結果が前進であるか後進であるかに応じて適切に初期化処理が実行される。トラクタ１を起動した後、後進しなければならない場合や、移動せずに所定の作業を行う場合等にＩＭＵ初期化処理を開始しないことで、制御部４はＩＭＵ初期化処理以外の各種処理を優先して実行することが可能となる。

１ トラクタ（ロボットトラクタ、作業車両）
４ 制御部（自律走行制御部）
５２ 位置情報取得部
５３ 前後進検出部
５４ 慣性計測装置
１００ 自律走行システム
１０５ 衛星（測位衛星）

Claims (2)

1. 衛星から受信した電波に基づいて作業車両の位置情報を取得する位置情報取得部と、
   前記位置情報に基づいて、予め設定された経路に沿って前記作業車両を自律走行させることが可能な自律走行制御部と、
   前記作業車両の前進又は後進を検出可能な前後進検出部と、
   前記作業車両の角速度及び加速度を検出する慣性計測部と、
   を備え、
   前記慣性計測部は、前記作業車両の起動毎に、前記前後進検出部により前記前進が検出された場合は、前記作業車両の向きを前記位置情報に基づいて、初期化処理を実行することを特徴とする自律走行システム。
2. 衛星から受信した電波に基づいて作業車両の位置情報を取得する位置情報取得部と、
   前記位置情報に基づいて、予め設定された経路に沿って前記作業車両を自律走行させることが可能な自律走行制御部と、
   前記作業車両の前進又は後進を検出可能な前後進検出部と、
   前記作業車両の角速度及び加速度を検出する慣性計測部と、
   を備え、
   前記慣性計測部は、前記作業車両の起動毎に、前記前後進検出部により前記後進が検出された場合は、前記位置情報の向きを反転させることにより前記作業車両の前進状態とみなすことで初期化処理を実行することを可能とすることを特徴とする自律走行システム。

Images