JP2022151104A - 自動走行制御装置、自動走行制御方法および自動走行制御用プログラム - Google Patents

Abstract

【課題】適切なタイミングで目標線分を切り替えることで、切り替え後の目標線分に対する位置偏差および方位偏差を迅速に解消することができるようにする。【解決手段】３次元空間を２分する関数φt＝Φ（ｐ，θ）を用いて、次の目標線分に対する位置偏差ｐ２および方位偏差θ２と現在の曲率φからなる３次元座標が、所定値を用いて表される監視座標が存在する３次元空間上の境界に達するか当該境界を跨いで変位した状態といえるタイミングで目標線分の切り替えを行う目標線分切替部１５を備えることにより、制御対象を切り替えた際に、切り替え後の目標線分との関係で算出される３次元空間の座標が既に境界の上または近傍に存在するようにして、３次元空間上の座標を境界に近づけて一致させるようにするための無駄な制御を不要にする。【選択図】図１

Description

本発明は、自動走行制御装置、自動走行制御方法および自動走行制御用プログラムに関し、特に、あらかじめ設定した目標経路に沿って車両を自動的に走行させるようにした自動走行制御装置に用いて好適なものである。

従来、車両の現在位置および方位を検出する機能を作業車両に搭載し、逐次検出される現在位置および方位に基づいて、圃場内にあらかじめ設定した目標経路に沿って作業車両を自動的に走行させるようにした自動走行制御装置が知られている（例えば、特許文献１参照）。

特許文献１に記載の自動走行制御装置では、作業車両が旋回する際に走行の制御値として用いる曲率（作業車両の操舵角と一定の関係を持つパラメータ）として、目標経路に対する自車方位の現在の方位偏差および目標経路に対する自車位置の現在の位置偏差を解消するための目標曲率を算出し、当該算出された目標曲率に対して実際の曲率が一致するように作業車両の走行を制御するようにしている。

特許文献１に記載の自動走行制御装置では、目標経路を直線として取り扱っている。すなわち、作業車両の全体の目標経路は複数の線分を連結したものとなっており、自動走行制御装置は、それぞれの線分について、目標曲率に対して実際の曲率が一致するように作業車両の走行を制御する。以下では、個々の線分を目標線分といい、これらを連結した全体の経路を目標経路といって両者を区別することにする。

特開２０２０－８２７５２号公報

上記特許文献１では、１つの目標線分に対して目標曲率を設定する制御に関してのみ開示しており、複数の目標線分に対する制御をどのように切り替えるかについては言及していない。しかし、複数の目標線分に対する制御を適切に切り替えることができなければ、作業車両を全体として目標経路に沿ってスムーズに走行させることができない。

すなわち、現在の目標線分から次の目標線分に対する制御を適切なタイミングで切り替えないと、次の目標線分に対する制御の開始時点で位置偏差や方位偏差が大きくなってしまう。これは、作業車両が次の目標線分のかなり手前から次の目標線分に向かい始める動作、または、作業車両が次の目標線分を通り過ぎてから次の目標線分に向かい始める動作として現れるため、位置偏差および方位偏差を解消させるまでに要する時間が長くなるという問題が生じる。

本発明は、このような問題を解決するために成されたものであり、複数の目標線分を連結して成る目標経路に沿って車両を自動的に走行させるようにした自動走行制御装置において、現在の目標線分から次の目標線分に対する制御を適切なタイミングで切り替えることができるようにすることで、次の目標線分についても位置偏差および方位偏差を迅速に解消することができるようにすることを目的とする。

上記した課題を解決するために、本発明では、車両が旋回する際に用いる走行の制御値として、位置偏差および方位偏差を解消するための目標曲率を算出し、当該算出した目標曲率に対して実際の曲率が一致するように車両の走行を制御する自動走行制御装置において、次の目標線分に対する位置偏差および方位偏差と現在の曲率とからなる３次元空間における座標を監視し、当該座標が、位置偏差、方位偏差および目標曲率が採り得る値によって定まる曲面により２分される３次元空間における一方の領域と他方の領域との境界に達するか当該境界を跨いで変位した状態といえるタイミングで、次の目標線分を現在の目標線分とするように切り替えるようにしている。

上記のように構成した本発明のように、次の目標線分との関係で算出される位置偏差および方位偏差と現在の曲率とからなる３次元空間における座標が、当該位置偏差、方位偏差および目標曲率が採り得る値によって定まる曲面により２分される一方の領域と他方の領域との境界に達するか当該境界を跨いで変位した状態といえるタイミングで目標線分の切り替えを行うと、制御対象を現在の目標線分から次の目標線分に切り替えた際に、切り替え後の現在の目標線分との関係で特定される３次元空間の座標が既に境界の上または近傍に存在することになる。そのため、上記３次元空間における座標を境界に近づけて一致させるようにするための無駄な制御が不要となる。これにより、本発明によれば、現在の目標線分から次の目標線分に対する制御を適切なタイミングで切り替えることができ、次の目標線分についても位置偏差および方位偏差を迅速に解消することができるようになる。

本実施形態による自動走行制御装置の機能構成例を示す図である。 本実施形態において用いる目標経路の例を示す図である。 本実施形態の偏差検出部が検出する方位偏差および位置偏差を示す図である。 本実施形態の位置・方位制御部が作業車両の走行制御に用いる関数により表される３次元空間の一例および目標線分切替部の処理内容を説明するための模式図である。 本実施形態による位置・方位制御部の機能構成例を示すブロック図である。 本実施形態の仮位置偏差算出部が算出する仮位置偏差を説明するための図である。 本実施形態による仮位置偏差算出部および仮曲率勾配算出部の動作例を示すフローチャートである。 本実施形態による目標曲率設定部の動作例を示すフローチャートである。 本実施形態の目標曲率設定部が新たに曲率を設定する際の二分法を説明するための図である。 本実施形態による目標線分切替部の機能構成例を示すブロック図である。 次の目標線分に対する仮位置偏差を説明するための図である。 本実施形態の仮位置偏差算出部が算出する仮位置偏差の他の例を説明するための図である。 変形例に係る位置・方位制御部および目標線分切替部の機能構成例を示す図である。 変形例に係る目標線分切替部の機能構成例を示す図である。 変形例に係る目標線分切替部の機能構成例を示す図である。

以下、本発明の一実施形態を図面に基づいて説明する。図１は、本実施形態による自動走行制御装置１００の機能構成例を示す図である。図１に示すように、本実施形態の自動走行制御装置１００は、その機能構成として、目標線分取得部１１、偏差検出部１２、位置・方位制御部１３、走行制御部１４および目標線分切替部１５を備えている。また、本実施形態の自動走行制御装置１００には、経路データ記憶部２００、位置・方位センサ３００および操舵装置４００が接続されている。

上記各機能ブロック１１～１５は、ハードウェア、ＤＳＰ（Digital Signal Processor）、ソフトウェアの何れによっても構成することが可能である。例えばソフトウェアによって構成する場合、上記各機能ブロック１１～１５は、実際にはコンピュータのＣＰＵ、ＲＡＭ、ＲＯＭなどを備えて構成され、ＲＡＭやＲＯＭ、ハードディスクまたは半導体メモリ等の記録媒体に記憶されたプログラムが動作することによって実現される。

経路データ記憶部２００は、作業車両を自動走行させる際に設定される目標経路を示す経路データを記憶する。本実施形態では、図２に示すように、作業車両ＷＶの全体の目標経路は複数の目標線分ＴＰ１，ＴＰ２，ＴＰ３，・・・を連結したものとなっており、自動走行制御装置１００は、あらかじめ設定された複数の目標線分ＴＰ１，ＴＰ２，ＴＰ３，・・・から成る目標経路に沿って作業車両ＷＶを自動的に走行させるように制御する。図２に示す破線は、自動走行制御装置１００により制御されて作業車両ＷＶが走行する軌跡を示している。

目標経路の設定は、公知の技術を用いてあらかじめ行われ、その結果得られた経路データが経路データ記憶部２００に記憶される。本実施形態では、一例として、作業車両が圃場の一辺と平行に直進し、直進の終点である圃場端において１８０度旋回をした後、旋回前の走行経路と平行に直進するような往復走行を繰り返す走行経路が設定されているものとする。この場合、経路データは、圃場の一端付近から他端付近までの往路を表す目標線分（以下、往路目標線分という）と、圃場の他端付近から一端付近までの復路を表す目標線分（以下、復路目標線分という）と、往路目標線分の終端と復路目標線分の始端との間および復路目標線分の終端と往路目標線分の始端との間を結ぶ目標線分（以下、旋回目標線分という）との組み合わせにより構成される。

位置・方位センサ３００は、作業車両の現在位置および現在方位を検出するセンサである。位置・方位センサ３００は、例えば、ＧＮＳＳ（Global Navigation Satellite System：全球測位衛星システム）により構成される。一例として、位置・方位センサ３００は、ＧＰＳ（Global Positioning System）受信機であり、複数のＧＰＳ衛星から送られてくる電波をＧＰＳアンテナで受信して、３次元測位処理あるいは２次元測位処理を行って作業車両の絶対位置および絶対方位を計算する。

また、位置・方位センサ３００は、ジャイロセンサ、加速度センサや磁気方位センサを用いた自立航法センサであってもよい。自立航法センサは、所定走行距離毎に１個のパルスを出力して作業車両の移動距離を検出する距離センサと、作業車両の回転角度（移動方位）を検出する振動ジャイロ等の角速度センサ（相対方位センサ）とを含む。自立航法センサは、これらの距離センサおよび角速度センサによって車両の相対位置および相対方位を検出する。なお、距離センサに代えて加速度センサを用いてもよい。

なお、ここに示した位置・方位センサ３００は一例に過ぎず、これに限定されるものではない。作業車両の現在位置および現在方位を検出可能なセンサであれば、何れを用いてもよい。

操舵装置４００は、後述するようにして自動走行制御装置１００により算出された目標曲率に基づいて、作業車両の操舵を制御する。以下、自動走行制御装置１００の機能構成について詳細に説明する。

目標線分取得部１１は、経路データ記憶部２００に記憶されている経路データに基づいて、目標線分を取得する。上述のように、本実施形態では、作業車両が圃場内を往復走行する際の目標とすべき目標経路が設定されており、その目標経路を表す経路データが経路データ記憶部２００に記憶されている。目標線分取得部１１は、直進走行しているときの走行制御に必要な目標線分（往路目標線分、復路目標線分の何れか）や、旋回時の走行制御に必要な目標線分（旋回目標線分、往路目標線分、復路目標線分の何れか）を適宜取得する。

本実施形態では、目標線分取得部１１は、現在の制御対象としている目標線分ＴＰ１および次に制御対象とする目標線分ＴＰ２を経路データ記憶部２００から取得して偏差検出部１２に出力する。ここで、目標線分取得部１１は、目標線分切替部１５から供給される切替指示に応じて、経路データ記憶部２００から取得する目標線分ＴＰ１，ＴＰ２の切り替えを行う。すなわち、目標線分取得部１１は、目標線分切替部１５から切替指示を受けたときに、それまで次の目標線分ＴＰ２として取得していたものを現在の目標線分ＴＰ１とするように切り替えるとともに、新たに次の目標線分ＴＰ２を経路データ記憶部２００から取得し、これらの新たな組み合わせの目標線分ＴＰ１，ＴＰ２を偏差検出部１２に出力する。

偏差検出部１２は、目標線分取得部１１により取得された目標線分ＴＰ１，ＴＰ２を入力するとともに、位置・方位センサ３００により検出された作業車両の現在位置および現在方位を入力する。位置・方位センサ３００としてＧＰＳ受信機を用いている場合、ＧＰＳ受信機からは作業車両の現在の絶対位置および絶対方位を示す信号が出力される。偏差検出部１２は、このＧＰＳ受信機から出力される絶対位置および絶対方位をもとに、経路データ記憶部２００に経路データとして記憶されている目標経路の座標系上における作業車両の現在位置および現在方位を検出する。

また、位置・方位センサ３００として自立航法センサを用いている場合、自立航法センサからは作業車両の現在の相対位置および相対方位を示す信号が出力される。偏差検出部１２は、この自立航法センサから出力される相対位置および相対方位から絶対位置および絶対方位を計算し、当該計算した絶対位置および絶対方位をもとに、経路データ記憶部２００に経路データとして記憶されている目標経路の座標系上における作業車両の現在位置および現在方位を検出する。

偏差検出部１２は、作業車両に搭載された位置・方位センサ３００からの出力に基づいて上述のようにして検出した作業車両の現在位置および現在方位と、目標線分取得部１１により取得された目標線分ＴＰ１，ＴＰ２とに基づいて、目標線分ＴＰ１，ＴＰ２に対する自車方位の現在の方位偏差および目標線分ＴＰ１，ＴＰ２に対する自車位置の現在の位置偏差を検出する。ここで、偏差検出部１２は、現在の目標線分ＴＰ１に対する位置偏差および方位偏差を検出するとともに、次の目標線分ＴＰ２に対する位置偏差および方位偏差を検出する。

図３は、偏差検出部１２が検出する方位偏差および位置偏差を模式的に示す図である。図３は、作業車両ＷＶを目標線分ＴＰに沿って走行させるように制御する際の状況を示している。図３に示すように、作業車両ＷＶ上の所定の一点Ｏ（図３では一例として、車体の前後方向の中心線ＣＬと、左右の後輪間を結ぶ直線との交点としている）を基点とし、車体の前後方向の中心線ＣＬと、作業車両ＷＶ上の所定の一点Ｏを通り目標線分ＴＰに平行な線ＰＬとが成す角度が方位偏差θである。また、目標線分ＴＰに平行な線ＰＬと目標線分ＴＰとの距離が位置偏差ｐである。

以下の説明において、位置偏差ｐは、目標線分ＴＰの進行方向（図の矢印の向き）を基準として、作業車両ＷＶが目標線分ＴＰの右側にいるときに正の値、目標線分ＴＰの左側にいるときに負の値をとるものとする。方位偏差θは、目標線分ＴＰに対して走行車両ＷＶが右方向に向かって走行している場合に正の値、左方向に向かって走行している場合に負の値をとるものとする。さらに、後述する曲率φは作業車両ＷＶが右に旋回しているときの曲率が正の値をとり、曲率勾配λは右方向に操舵するときに正の値をとるものとする。曲率φは、作業車両ＷＶの操舵角と一定の関係を持つパラメータである。また、曲率勾配λは、作業車両ＷＶの操舵速度および走行速度と一定の関係を持つパラメータである。

また、以下の説明において、目標線分ＴＰに対する位置偏差をｐ、方位偏差をθ、曲率をφ、曲率勾配をλで表し、現在の目標線分ＴＰ１に対する値であることを明示する場合はｐ１やθ１、次の目標線分に対する値であることを明示する場合はｐ２やθ２と表記し、数字を省略した場合は、目標線分ＴＰ１、目標線分ＴＰ２の両方に対する説明であるものとする。なお、実際の曲率φと曲率勾配λについては目標線分によらずに一定の値となるため数字を付さないが、計算上の値の場合はどちらの目標線分に対する計算において使用しているかを明示するために数字を付して説明する。また必要に応じて計算上の値であることを明示するために、目標曲率φ_t、仮位置偏差ｐ’などと追加の記号を付す場合があるが、これらについても同様とする。

位置・方位制御部１３は、目標線分ＴＰに対する位置偏差ｐおよび方位偏差θと、車両の操舵角に関連する曲率φとを３軸とする３次元空間において、位置偏差ｐおよび方位偏差θを何れもゼロに収束させることが可能な目標曲率φ_tを算出する関数であって、位置偏差ｐ、方位偏差θおよび目標曲率φ_tの組み合わせとして採り得る値により定まる座標の集合からなる曲面によって３次元空間を２分する関数を用いて、作業車両ＷＶが旋回する際に用いる走行の制御値を算出する。制御値の一例として、本実施形態では、偏差検出部１２により検出された方位偏差θおよび位置偏差ｐを解消するための目標曲率φ_tを算出する。すなわち、位置・方位制御部１３は、目標線分ＴＰに対する位置偏差ｐおよび方位偏差θを何れもゼロに収束させることが可能な目標曲率φ_tを算出する関数として、位置偏差ｐおよび方位偏差θに基づいて目標曲率φ_tを算出する関数Φを用いて、φ_t＝Φ（ｐ，θ）と表記される。実際には曲率φや目標曲率φ_tの代わりに、舵角や目標舵角、クローラの左右の回転差やその目標値を利用してもよいが、いずれも曲率を変化させることが目的であり、以下ではこれらすべてを曲率φや目標曲率φ_tと表現する。

なお、後述する図５に示す目標曲率設定部１３Ａ、仮位置偏差算出部１３Ｂおよび仮曲率勾配算出部１３Ｃ、図１３に示す目標曲率設定部１３Ａ’、図１０に示す仮位置偏差算出部１５Ｂおよび仮曲率勾配算出部１５Ｃ、図１４に示す目標曲率設定部１５Ｅ、あるいは図１５に示す想定方位設定部１５Ｆと、関数Φと、制御面４０とは全て同じ機能を示すものである。すなわち、ブロック図で示された機能を数学的表現にしたものが関数Φであり、３次元曲面で空間に図示したものが制御面４０である。従って、「算出」とは、ブロック図に示される手順によるもの、数学的表現に従って演算されるもの、あるいは空間座標として記憶されているものの何れかから、所望の値を取り出すことをいう。

ここで、位置・方位制御部１３は、偏差検出部１２により検出された現在の目標線分ＴＰ１に対する位置偏差ｐ１および方位偏差θ１に基づいて、これらを解消するための目標曲率φ１_tを算出する。これは関数Φを用いて、関数φ１_t＝Φ（ｐ１，θ１）と表記され、関数Φには、上述の条件を満たす関数であって、位置偏差ｐ１および方位偏差θ１から目標曲率φ１_tを算出することが可能な関数であれば何れも用いることが可能である。なお、その具体例については後述する。

図４は、位置・方位制御部１３が作業車両ＷＶの走行制御に用いる関数により表される３次元空間の一例および後述する目標線分切替部１５の処理内容を説明するための模式図である。図４は、曲率φ、位置偏差ｐおよび方位偏差θの３つの変数をそれぞれ座標軸として有する直交系の３次元空間を示している。曲率φ、位置偏差ｐおよび方位偏差θは、何れも正の値と負の値とを有し、曲率φ、位置偏差ｐ、方位偏差θの値が何れもゼロの位置が３次元空間の原点Ｏである。

上述のように、位置・方位制御部１３は、目標線分ＴＰに対する位置偏差ｐおよび方位偏差θを何れもゼロに収束させることが可能な目標曲率φ_tを算出する関数Φを含む式φ_t＝Φ（ｐ，θ）を用いる。この場合、３次元空間は、この式φ_t＝Φ（ｐ，θ）を満たす値として取り得る３つの値（ｐ，θ，φ_t）の組み合わせから成る３次元座標によって特定される３次元空間上の座標の集合から成る曲面（以下、制御面という）４０を境界として、２つの領域４１，４２に２分される。

位置・方位制御部１３が式φ_t＝Φ（ｐ，θ）により算出する目標曲率φ_tを用いて走行制御部１４が行う作業車両ＷＶの走行制御は、３つの値（ｐ，θ，φ_t）の組み合わせによって特定される３次元空間上の座標を、制御面４０の上を徐々に移動させながら原点Ｏに近づけていく制御に相当する。本実施形態において用いる目標線分ＴＰは直線状であるため、位置・方位制御部１３は、位置偏差ｐおよび方位偏差θが何れもゼロに収束した状態で曲率φもゼロに収束するように、すなわち、３つの値（ｐ，θ，φ_t）の組み合わせによって特定される３次元空間上の座標が原点Ｏに徐々に近づくように、目標曲率φ_tを逐次算出する。

走行制御部１４は、位置・方位制御部１３により現在の目標線分ＴＰ１について算出された目標曲率φ１_tに対して実際の曲率φが一致するように作業車両ＷＶの走行を制御する。本実施形態では、走行制御部１４は、位置・方位制御部１３により算出された目標曲率φ１_tをもとに操舵角を決定し、当該決定した操舵角に基づいて作業車両ＷＶの走行を制御する。すなわち、走行制御部１４は、位置・方位制御部１３により算出された目標曲率φ１_tを所定の相関関数に基づいて操舵角に変換し、これによって得られた操舵角情報を操舵装置４００に出力する。操舵装置４００は、走行制御部１４から取得した操舵角情報に基づいてモータ電圧を決定し、作業車両ＷＶの操舵を制御する。

目標線分切替部１５は、位置・方位制御部１３に入力される位置偏差ｐおよび方位偏差θと、位置・方位制御部１３が出力する目標曲率φ_tとが採り得る値によって定まる曲面によって２分される３次元空間において、偏差検出部１２により検出された次の目標線分ＴＰ２に対する位置偏差ｐ２および方位偏差θ２と現在の曲率φとからなる３次元座標を監視し、当該３次元座標が曲面により２分される一方の領域と他方の領域との境界に達するか当該境界を跨いで変位した状態といえるタイミングで、次の目標線分ＴＰ２を現在の目標線分ＴＰ１とするように切り替える。

本実施形態において、目標線分切替部１５は、上述の関数を用いて算出される所定値（詳細は後述する）に基づいて、現在の目標線分ＴＰ１に対する走行制御から次の目標線分ＴＰ２に対する走行制御への切り替えを制御する。具体的には、目標線分切替部１５は、偏差検出部１２により検出された次の目標線分ＴＰ２に対する位置偏差ｐ２および方位偏差θ２と現在の曲率φとを用いて関数により所定値を算出し、位置偏差ｐ２、方位偏差θ２および現在の曲率φからなる３次元空間における座標が、当該所定値を用いて表される３次元空間上の座標に達するか当該座標を跨いで変位した状態といえるタイミングで、次の目標線分ＴＰ２を現在の目標線分ＴＰ１とするように切り替える。

ここで、位置偏差ｐ２、方位偏差θ２および現在の曲率φからなる３次元空間における座標が、所定値により表される３次元空間上の座標（以下、監視座標という）に達するか当該監視座標を跨いで変位した状態とは、逐次算出される位置偏差ｐ２、方位偏差θ２および現在の曲率φからなる座標（ｐ２，θ２，φ）が、所定値により表される３次元空間上の監視座標が存在する図４の制御面４０に一致するか、制御面４０を跨いで一方の領域４１から他方の領域４２へ、またはその逆の方向へと変位した状態をいう。

図４では、座標（ｐ２，θ２，φ）が、次の目標線分ＴＰ２との関係で算出される監視座標が存在する制御面４０に対して一方の領域４１にある状態を軌跡４３で表している。また、座標（ｐ２，θ２，φ）が制御面４０に一致した状態または制御面４０を跨いで変位した直後の状態を★印の点４４で表している。座標（ｐ２，θ２，φ）が点４４となったときが目標線分ＴＰ１，ＴＰ２の切り替えタイミングである。また、軌跡４５は、次の目標線分ＴＰ２を現在の目標線分ＴＰ１に切り替えた後に、当該現在の目標線分ＴＰ１との関係で位置・方位制御部１３により制御される動作の軌跡を示している。

座標（ｐ２，θ２，φ）が軌跡４３で表している位置にあるときは、当該座標（ｐ２，θ２，φ）が制御面４０から離れた状態にある。このように、座標（ｐ２，θ２，φ）が制御面４０から離れているときに目標線分ＴＰ１，ＴＰ２を切り替えると、切り替え後の現在の目標線分ＴＰ１との関係で算出される座標（ｐ１，θ１，φ）を制御面４０に近づけるための余計な制御が必要となり、作業車両ＷＶに無駄な動きが生じる。

これに対し、座標（ｐ２，θ２，φ）が制御面４０に一致するか制御面４０を跨いで変位したときに目標線分ＴＰ１，ＴＰ２を切り替えれば、座標（ｐ１，θ１，φ）を制御面４０に近づけるための余計な制御を行うことが不要であり、作業車両ＷＶに無駄な動きが生じるのを抑止することができる。座標（ｐ２，θ２，φ）が制御面４０に一致したタイミングで目標線分ＴＰ１，ＴＰ２を切り替えるのが理想的であるが、所定値を算出する時間間隔の間に、座標（ｐ２，θ２，φ）が制御面４０に一致するタイミングを過ぎてしまうこともある。この場合は、座標（ｐ２，θ２，φ）が制御面４０を跨いで変位したことを検知したタイミングで目標線分ＴＰ１，ＴＰ２を切り替える。

以下に、位置・方位制御部１３による具体的な処理例を説明する。図５は、位置・方位制御部１３の機能構成例を示すブロック図である。図５に示すように、位置・方位制御部１３は、具体的な機能構成として、目標曲率設定部１３Ａ、仮位置偏差算出部１３Ｂおよび仮曲率勾配算出部１３Ｃを備えている。

位置・方位制御部１３は、目標曲率設定部１３Ａ、仮位置偏差算出部１３Ｂおよび仮曲率勾配算出部１３Ｃの動作によって目標曲率φ１_tを算出する。すなわち、仮位置偏差算出部１３Ｂおよび仮曲率勾配算出部１３Ｃによって、現在の目標線分ＴＰ１に対する現在の方位偏差θ１を解消するために必要な仮曲率勾配λ１’を算出しつつ、当該仮曲率勾配λ１’に従って作業車両ＷＶが走行したと仮定した場合に現在の目標線分ＴＰ１との間に生じる仮位置偏差ｐ１’を算出する。そして、その仮位置偏差ｐ１’を解消するために必要な目標曲率φ１_tを目標曲率設定部１３Ａによって設定する。この動作を上述の関数Φを用いた式で表現すると、φ１_t＝Φ（ｐ１，θ１）と表される。

目標曲率設定部１３Ａは、偏差検出部１２により検出された現在の目標線分ＴＰ１に対する現在の方位偏差θ１および現在の位置偏差ｐ１を仮位置偏差算出部１３Ｂに供給するとともに、暫定目標曲率φ１_t’を設定して仮位置偏差算出部１３Ｂに供給し、仮位置偏差ｐ１’を算出するよう仮位置偏差算出部１３Ｂに指示をした後、仮位置偏差算出部１３Ｂが算出した仮位置偏差ｐ１’を受け取る。

目標曲率設定部１３Ａは、仮位置偏差算出部１３Ｂから受け取った仮位置偏差ｐ１’が所定の条件を満たすか否かを判定する。そして、所定の条件を満たす場合は、仮位置偏差算出部１３Ｂが仮位置偏差ｐ１’を算出する際に使用した暫定目標曲率φ１_t’を目標曲率φ１_tとして走行制御部１４に対して出力する。一方、所定の条件を満たしていない場合は、新たに暫定目標曲率φ１_t’を設定して仮位置偏差ｐ１’を再算出するよう仮位置偏差算出部１３Ｂに指示をする。

仮位置偏差算出部１３Ｂは、目標曲率設定部１３Ａから与えられた暫定目標曲率φ１_t’、現在の方位偏差θ１および現在の位置偏差ｐ１を受け取ると、仮曲率勾配算出部１３Ｃに仮曲率φ１’を与えるとともに、現在の方位偏差θ１を仮方位偏差θ１’として与え、仮曲率勾配λ１’の算出を指示する。仮位置偏差算出部１３Ｂは、目標曲率設定部１３Ａから与えられた暫定目標曲率φ１_t’、現在の方位偏差θ１、現在の位置偏差ｐ１を初期条件として、仮曲率勾配算出部１３Ｃにより算出された仮曲率勾配λ１’を用いて、仮方位偏差θ１’が解消する仮位置まで作業車両ＷＶを走行させたと仮定した場合に生じる、現在の目標線分ＴＰ１（特許請求の範囲の所定位置に相当）との位置偏差である仮位置偏差ｐ１’の算出を行う。

ここで、仮位置偏差算出部１３Ｂは、作業車両ＷＶの方位偏差θ１および位置偏差ｐ１を算出する三角関数からなる非線形要素が含まれる演算（後述する（式２）（式３）（式４）に対応する演算）処理と、当該処理により算出される仮曲率φ１’および仮方位偏差θ１’を仮曲率勾配算出部１３Ｃに供給して仮曲率勾配λ１’を算出する処理とを含むステップ処理を、仮方位偏差θ１’が解消されるまで順次繰り返す。そして、この繰り返しによって算出した仮位置偏差ｐ１’を目標曲率設定部１３Ａに出力する。

仮曲率勾配算出部１３Ｃは、仮位置偏差算出部１３Ｂから与えられた仮曲率φ１’および仮方位偏差θ１’をゼロに収束させるような仮曲率勾配λ１’を算出する関係式（後述する（式１））に基づいて、仮位置偏差算出部１３Ｂから指示された仮曲率φ１’および仮方位偏差θ１’を用いて仮曲率勾配λ１’を算出し、これを仮位置偏差算出部１３Ｂに出力する。

以下に、目標曲率設定部１３Ａ、仮位置偏差算出部１３Ｂおよび仮曲率勾配算出部１３Ｃの処理内容をより詳細に説明する。

まず、目標曲率設定部１３Ａは、偏差検出部１２により検出された現在の目標線分ＴＰ１に対する現在の方位偏差θ１および現在の位置偏差ｐ１を仮位置偏差算出部１３Ｂに供給するとともに、仮位置偏差算出部１３Ｂおよび仮曲率勾配算出部１３Ｃの演算処理に用いるための暫定目標曲率φ１_t’を設定して仮位置偏差算出部１３Ｂに供給し、仮位置偏差ｐ１’を算出するよう仮位置偏差算出部１３Ｂに指示をする。

仮位置偏差算出部１３Ｂは、目標曲率設定部１３Ａから供給された現在の方位偏差θ１、現在の位置偏差ｐ１および暫定目標曲率φ１_t’を、仮位置偏差ｐ１’を算出する際の値として用いる。また、仮位置偏差算出部１３Ｂは、目標曲率設定部１３Ａから供給された暫定目標曲率φ１_t’および現在の方位偏差θ１を、仮曲率勾配算出部１３Ｃが仮曲率勾配λ１’を算出する際に使用する仮曲率φ１’および仮方位偏差θ１’の値として仮曲率勾配算出部１３Ｃに供給する。

仮曲率勾配算出部１３Ｃは、作業車両ＷＶが旋回走行しているときの作業車両ＷＶの旋回する曲率φ１と方位偏差θ１から、最終的に方位偏差θ１を解消するための制御値として、仮曲率φ１’が単位走行距離（例えば１ｍ）当たりに変化する量である仮曲率勾配λ１’を算出する。

すなわち、仮曲率勾配算出部１３Ｃは、仮方位偏差θ１’および仮曲率φ１’を変数として与えた上で、仮方位偏差θ１’および仮曲率φ１’がゼロに収束するために必要となる仮曲率勾配λ１’を、例えば次の（式１）に示す所定の関数Λを用いた計算によって算出する。
λ１’＝Λ（θ１’，φ１’）・・・（式１）
この（式１）は、仮方位偏差θ１’および仮曲率φ１’がゼロに収束する機能を持った式であり、その一例として、λ１’＝－Ａθ１’－Ｂφ１’を用いることが可能である（Ａ，Ｂはシミュレーションまたは実機の動作から適宜設定されるパラメータである）。

具体的には、仮位置偏差算出部１３Ｂから曲率φの値および方位偏差θ１の値としての仮曲率φ１’および仮方位偏差θ１’が与えられる都度、（式１）に基づいて仮曲率勾配λ１’を算出し、仮位置偏差算出部１３Ｂに返す。なお、仮位置偏差算出部１３Ｂから仮曲率勾配算出部１３Ｃに与えられる仮曲率φ１’および仮方位偏差θ１’の初期値は、目標曲率設定部１３Ａから供給された暫定目標曲率φ１_t’および現在の方位偏差θ１の値である。

仮方位偏差θ１’および仮曲率φ１’がゼロに収束するとは、両方の値が完全にゼロに等しくなることを必須とするものではなく、それぞれが所定の閾値未満となることを含む。あるいは、仮方位偏差θ１’および仮曲率φ１’から所定の算出式によって算出される評価値が所定の閾値未満となることを含む。本明細書では、このように仮方位偏差θ１’および仮曲率φ１’が所定の収束条件（以下、方位偏差収束条件という）を満たした状態を、仮方位偏差θ１’が解消された状態と表現する。

なお、仮方位偏差θ１’および仮曲率φ１’の値が与えられれば、これらをゼロに収束するために必要となる仮曲率勾配λ１’の値は（式１）によって一義的に定まる。つまり、仮方位偏差θ１’および仮曲率φ１’の値が与えられれば、（式１）により一の仮曲率勾配λ１’を得ることができる。そこで、仮曲率勾配λ１’の値を仮方位偏差θ１’および仮曲率φ１’の値と紐づけした値の情報として記憶媒体に記憶しておき、仮曲率勾配算出部１３Ｃは、与えられた仮方位偏差θ１’および仮曲率φ１’の値をもとに仮曲率勾配λ１’の値を記憶媒体から読み出して利用するようにしてもよい。このように記憶媒体にデータをあらかじめ記憶しておいて、それを読み出して利用する構成とすることにより、処理時間を短縮することができる。なお、記憶媒体に記憶されるデータは離散値となるので、それを補間するための計算を行うようにしても良い。

この仮曲率勾配λ１’の算出の際に、仮曲率勾配算出部１３Ｃは、仮方位偏差θ１’を解消することのみを考慮し、位置偏差を解消することは考慮しない。そのため、仮に、仮曲率勾配算出部１３Ｃにより算出される仮曲率勾配λ１’に従って作業車両ＷＶを走行させた場合、図６に示すように、仮方位偏差θ１’が解消する時点（作業車両ＷＶの中心線ＣＬ１が目標線分ＴＰ１に平行になる時点）の仮位置で、作業車両ＷＶの位置と目標線分ＴＰ１との間に位置偏差（仮位置偏差ｐ１’）が生じ得る。なお、作業車両ＷＶが圃場を往復走行するときの実際の旋回は、図６に示す状況よりも大きな曲率φで急旋回するが、ここでは説明の便宜上、実際よりも緩やかな曲率φで旋回する様子を示している。以下に説明する他の図面も同様である。

このように、実際の現在位置から、仮曲率勾配算出部１３Ｃにより算出された仮曲率勾配λ１’に従って仮方位偏差θ１’が解消される仮位置まで作業車両ＷＶを走行させたと仮定した場合、仮位置偏差ｐ１’がゼロになるとは限らない。

そこで、仮位置偏差算出部１３Ｂは、仮曲率勾配算出部１３Ｃにより算出された仮曲率勾配λ１’に従って仮方位偏差θ１’が解消される仮位置まで作業車両ＷＶを走行させたと仮定した場合に生じる、現在の目標線分ＴＰ１との仮位置偏差ｐ１’を求める。この仮位置偏差ｐ１’を求める際に、仮位置偏差算出部１３Ｂは、仮方位偏差θ１’の三角関数値を用いた演算を行う。

すなわち、仮位置偏差算出部１３Ｂは、仮曲率勾配λ１’と仮曲率φ１’、仮曲率φ１’と仮方位偏差θ１’および仮方位偏差θ１’と仮位置偏差ｐ１’との関係を示した次の（式２）（式３）（式４）に従って、（式１）により算出された仮曲率勾配λ１’に従って仮方位偏差θ１’が解消されるまで作業車両ＷＶを走行させたと仮定した場合に生じる目標線分ＴＰ１との仮位置偏差ｐ１’を算出する。ここで、Ｌは走行距離を表す。

具体的には、仮位置偏差算出部１３Ｂは、目標曲率設定部１３Ａから供給された暫定目標曲率φ１_t’、現在の方位偏差θ１および現在の位置偏差ｐ１を初期値とし、方位偏差θ１が解消したと判断されるまで、都度、仮曲率勾配算出部１３Ｃから仮曲率勾配λ１’の値を取得しつつ（式２）（式３）（式４）の演算を繰り返す。そして方位偏差θ１が解消されたと判断されたときの位置偏差の値を仮位置偏差ｐ１’として目標曲率設定部１３Ａに供給する。

より具体的には、仮位置偏差算出部１３Ｂは、（式２）（式３）（式４）を微小区間ごとに繰り返し演算するものであり、目標曲率設定部１３Ａから供給された暫定目標曲率φ１_t’、現在の方位偏差θ１および現在の位置偏差ｐ１を初期値とし、演算により得られた曲率の値（仮曲率φ１’）、方位偏差の値（仮方位偏差θ１’）および仮方位偏差ｐ１’を再び（式２）（式３）（式４）に投入するとともに、仮曲率φ１’、仮方位偏差θ１’を仮曲率勾配算出部１３Ｃに与える。そして得られた仮曲率勾配λ１’、仮曲率φ１’および仮方位偏差θ１’をこれらの式に繰り返し投入する演算を、仮方位偏差θ１’が解消したと判断されるまで繰り返す。仮位置偏差算出部１３Ｂは、仮方位偏差θ１’が解消したと判断すると、その時の仮位置偏差ｐ１’を目標曲率設定部１３Ａに送る。

なお、仮曲率勾配算出部１３Ｃおよび仮位置偏差算出部１３Ｂは、次の（式５）に示すように、曲率φについて設定された下限値および上限値の範囲内で、仮方位偏差θ１’を解消するための仮曲率勾配λ１’を算出するとともに、仮位置偏差ｐ１’を求める。ここで、φ_minは曲率として採り得る値の最小値（左方向に最大限操舵した状態に相当する）、φ_maxは曲率として採り得る値の最大値（右方向に最大限操舵した状態に相当する）である。φ_minの絶対値とφ_maxの絶対値は同じであってもよいし、異なっていてもよい。
φ_min≦φ１’(Ｌ)≦φ_max ・・・（式５）

目標曲率設定部１３Ａは、仮位置偏差算出部１３Ｂにより算出された仮位置偏差ｐ１’を受けて、当該仮位置偏差ｐ１’の値に応じて暫定目標曲率φ１_t’を変更し、変更後の暫定目標曲率φ１_t’を仮位置偏差算出部１３Ｂに供給することにより、仮位置偏差ｐ１’を再算出するように指示する。目標曲率設定部１３Ａは、この処理を繰り返し行うことにより、都度算出される仮位置偏差ｐ１’に基づいて、現在の位置偏差ｐ１を解消させるための制御値として目標曲率φ１_tを設定する。本実施形態では、仮位置偏差算出部１３Ｂにより算出される仮位置偏差ｐ１’を微小化させるような目標曲率φ１_tを設定する。

すなわち、目標曲率設定部１３Ａは、まず、暫定目標曲率φ１_t’を設定して仮位置偏差算出部１３Ｂに供給し、その応答として、現在の目標線分ＴＰ１との仮位置偏差ｐ１’を仮位置偏差算出部１３Ｂから得る。ここで、この仮位置偏差ｐ１’の絶対値が大きくて所定の収束条件（以下、位置偏差収束条件という）を満たしておらず、仮位置偏差ｐ１’が解消されていないと判断される場合は、目標曲率設定部１３Ａは、暫定目標曲率φ１_t’の値を変えて仮位置偏差算出部１３Ｂに供給し、仮位置偏差ｐ１’の算出を再度行わせることにより、仮位置偏差算出部１３Ｂから仮位置偏差ｐ１’を再度取得する。

再度取得した仮位置偏差ｐ１’が位置偏差収束条件を満たしていない場合は、目標曲率設定部１３Ａは、暫定目標曲率φ１_t’の値を再度変えて仮位置偏差算出部１３Ｂに供給し、仮位置偏差ｐ１’の計算を再実行させることにより、仮位置偏差算出部１３Ｂから仮位置偏差ｐ１’を再度取得する。以降、再度取得した仮位置偏差ｐ１’が位置偏差収束条件を満たすまでこの動作を繰り返し、位置偏差収束条件を満たしたときに用いた暫定目標曲率φ１_t’の値を、最終的に求める目標曲率φ１_tとして走行制御部１４に出力する。

仮位置偏差ｐ１’が位置偏差収束条件を満たしていない場合に暫定目標曲率φ１_t’を再設定する方法としては、任意の方法を用いることが可能である。例えば、暫定目標曲率φ１_t’の値をランダムに変えるようにしてもよいし、公知の技術に従って処理のルールに基づき値を変えるようにしてもよい。本実施形態では、公知の二分法によって暫定目標曲率φ１_t’の値を再設定する。すなわち、目標曲率設定部１３Ａは、仮位置偏差算出部１３Ｂにより仮位置偏差ｐ１’が求められる毎に、仮位置偏差算出部１３Ｂに指示した暫定目標曲率φ１_t’の値を、現在の目標線分ＴＰ１に対する仮位置偏差ｐ１’の方向（仮位置偏差ｐ１’が正の値か負の値か）に応じて上端値または下端値の何れかに設定した上で、当該上端値または下端値を設定した範囲内で新たに暫定目標曲率φ１_t’を設定し、仮位置偏差ｐ１’を再算出するよう仮位置偏差算出部１３Ｂに指示する。そして、このような処理を繰り返し行うことにより、仮位置偏差ｐ１’を微小化させるような目標曲率φ１_tを設定する。

上述したように、目標曲率設定部１３Ａは、仮位置偏差ｐ１’を微小化させることを位置偏差収束条件として用いている。仮位置偏差ｐ１’が微小化するとは、例えば、仮位置偏差ｐ１’の絶対値が所定の閾値未満になることをいう。なお、二分法によって暫定目標曲率φ_t’の値を再設定する場合は、仮位置偏差ｐ１’の算出を繰り返すことによって、仮位置偏差ｐ１’が徐々に小さくなっていくことがある程度保証される。そのため、仮位置偏差ｐ１’が実際に所定の閾値未満となったか否かを判定することに代えて、目標曲率設定部１３Ａの動作の繰り返し回数が所定の閾値に達したことを検出することをもって、仮位置偏差ｐ１’を微小化させるための位置偏差収束条件を満たしたとみなすことが可能である。

図７および図８は、位置・方位制御部１３の動作例を示すフローチャートである。図８に示すフローチャートは、目標曲率設定部１３Ａの動作例を示すものであり、例えば、作業車両ＷＶの自動走行制御を開始した後、所定時間毎または所定走行距離毎に繰り返し実行される。また、図７に示すフローチャートは、仮位置偏差算出部１３Ｂおよび仮曲率勾配算出部１３Ｃの動作例を示すものであり、図８のステップＳ１３において呼び出されて実行されるものである。

図７において、仮位置偏差算出部１３Ｂは、まず、仮曲率勾配λ１’および仮位置偏差ｐ１’を算出する際の初期値を設定する（ステップＳ１）。すなわち、仮位置偏差算出部１３Ｂは、仮位置偏差ｐ１’の初期値を現在の位置偏差ｐ１に設定し、仮方位偏差θ１’の初期値を現在の方位偏差θ１の値に設定し、仮曲率φ１’の初期値を暫定目標曲率φ１_t’の値に設定する。これらの初期値は何れも、目標曲率設定部１３Ａから与えられる。仮位置偏差算出部１３Ｂは、設定した仮方位偏差θ１’および仮曲率φ１’を仮曲率勾配算出部１３Ｃに供給する。

次に、仮曲率勾配算出部１３Ｃは、仮位置偏差算出部１３Ｂから供給された仮方位偏差θ１’および仮曲率φ１’を用いて、上述した（式５）に示す制約条件のもと、（式１）に従って仮曲率勾配λ１’を算出する（ステップＳ２）。次いで、仮位置偏差算出部１３Ｂは、ステップＳ２で仮曲率勾配算出部１３Ｃが算出した仮曲率勾配λ１’に従って作業車両ＷＶが所定走行距離ΔＬだけ移動したと仮定した場合の仮位置偏差ｐ１’、仮方位偏差θ１’および仮曲率φ１’を、上述の（式２）～（式４）に従って算出する（ステップＳ３）。

例えば、仮位置偏差算出部１３Ｂは、公知のオイラー法に従って、仮位置偏差ｐ１’、仮方位偏差θ１’および仮曲率φ１’を算出する。すなわち、新たな仮位置偏差ｐ１’は、現在の仮位置偏差ｐ１’の値に（ｓｉｎθ１’）・ΔＬの値を加えた値として算出する。新たな仮方位偏差θ１’は、現在の仮方位偏差θ１’の値にφ１’・ΔＬの値を加えた値として算出する。新たな仮曲率φ１’は、現在の仮曲率φ１’の値にλ１’・ΔＬの値を加えた値として算出する。なお、ここに示したオイラー法は演算の一例であって、これに限定されるものではない。例えば、より精度の高いルンゲクッタ法など用いて算出することも可能である。

そして、仮位置偏差算出部１３Ｂは、ステップＳ３で算出した仮方位偏差θ１’および仮曲率φ１’が所定の方位偏差収束条件を満たしたか否かを判定する（ステップＳ４）。ここで、所定の方位偏差収束条件を満たしていないと判定された場合、処理はステップＳ２に戻る。一方、所定の方位偏差収束条件を満たすと判定された場合、仮位置偏差算出部１３Ｂは、そのときステップＳ３で算出された仮位置偏差ｐ１’を目標曲率設定部１３Ａに出力する（ステップＳ５）。これにより、図８のステップＳ１３における１回の処理が終了する。

図８において、目標曲率設定部１３Ａは、まず、二分法の初期値を設定する（ステップＳ１１）。すなわち、目標曲率設定部１３Ａは、曲率を二分する範囲を定める上端値φ_Hおよび下端値φ_Lの初期値として、上端値φ_Hの初期値をφ_max、下端値φ_Lの初期値をφ_minに設定するとともに、暫定目標曲率φ_t’の初期値をφ_maxとφ_minの平均値（左右の最大曲率が等しい通常の車両であればφ_min＝－φ_maxとなるため、その場合の初期値は０）に設定する。また、目標曲率設定部１３Ａは、仮位置偏差ｐ１’を微小化させるための位置偏差収束条件を判定するために用いる処理回数のカウント値ｉの初期値を０に設定する（ステップＳ１２）。

次に、目標曲率設定部１３Ａは、現在設定されている暫定目標曲率φ_t’の値を図７に示す計算の初期値として設定し、図７に示すフローチャートの処理を仮位置偏差算出部１３Ｂおよび仮曲率勾配算出部１３Ｃに実行させることにより、仮位置偏差算出部１３Ｂから仮位置偏差ｐ１’を得る（ステップＳ１３）。そして、これにより算出された仮位置偏差ｐ１’が正の値か否かを判定する（ステップＳ１４）。仮位置偏差ｐ１’が正の値である場合は、作業車両ＷＶが現在の目標線分ＴＰ１の右側で仮方位偏差θ１’が解消された場合に相当する。一方、仮位置偏差ｐ１’が負の値である場合は、作業車両ＷＶが現在の目標線分ＴＰ１の左側で仮方位偏差θ１’が解消された場合に相当する。

仮位置偏差ｐ１’が正の値であると判定された場合、目標曲率設定部１３Ａは、曲率を二分する範囲の上端値φ_Hに現在の暫定目標曲率φ１_t’を設定した上で（ステップＳ１５）、当該上端値φ_H（＝φ１_t’）と下端値φ_Lとの平均値を新たに暫定目標曲率φ１_t’として設定する（ステップＳ１７）。図９（ａ）は、この場合の設定内容を模式的に示す図である。なお、図９の模式図は、図８の処理を開始してから１回目（ｉ＝０）の処理におけるステップＳ１７の処理内容を示しており、「現φ１_t’」と書かれた現在の暫定目標曲率φ１_t’の値が、ステップＳ１１で初期値として設定されたφ_maxとφ_minの平均値の場合（図９の場合は“０”）を示している。

図９（ａ）に示すように、現在位置ｐ１が負の値（現在の目標線分ＴＰ１の左側）で仮位置偏差ｐ１’が正の値（現在の目標線分ＴＰ１の右側）である場合は、現在の暫定目標曲率φ１_t’の絶対値が小さ過ぎて、仮方位偏差θ１’が解消されるまでに作業車両ＷＶが現在の目標線分ＴＰ１を超えてしまう状態に相当する。よって、この場合に目標曲率設定部１３Ａは、現在の暫定目標曲率φ１_t’（これを上端値φ_Hに設定する）と、暫定目標曲率φ１_t’の値として設定可能な下端値φ_L（１回目の処理の場合は下限値φ_min）との平均値を新たに暫定目標曲率φ１_t’として設定することにより、作業車両ＷＶがより急峻なカーブを描いて旋回するように設定を変える。

一方、仮位置偏差ｐ１’が正の値ではない（負の値である）と判定された場合、目標曲率設定部１３Ａは、曲率を二分する範囲の下端値φ_Lに現在の暫定目標曲率φ１_t’を設定した上で（ステップＳ１６）、当該下端値φ_L（＝φ１_t’）と上端値φ_Hとの平均値を新たに暫定目標曲率φ１_t’として設定する（ステップＳ１７）。図９（ｂ）は、この場合の設定内容を模式的に示す図である。

図９（ｂ）に示すように、現在位置ｐ１が負の値（現在の目標線分ＴＰ１の左側）で仮位置偏差ｐ１’も負の値（現在の目標線分ＴＰ１の左側）である場合は、現在の暫定目標曲率φ１_t’の絶対値が大き過ぎて、仮方位偏差θ１’が解消される位置において作業車両ＷＶが目標線分ＴＰ１まで至らない状態に相当する。よって、この場合に目標曲率設定部１３Ａは、現在の暫定目標曲率φ１_t’（これを下端値φ_Lに設定する）と、暫定目標曲率φ１_t’の値として設定可能な上端値φ_H（１回目の処理の場合は上限値φ_max）との平均値を新たに暫定目標曲率φ１_t’として設定することにより、作業車両ＷＶがより緩やかなカーブを描いて旋回するように設定を変える。

その後、目標曲率設定部１３Ａは、処理回数のカウント値ｉをインクリメントし（ステップＳ１８）、カウント値ｉが所定の閾値Ｎに達したか否か、すなわち、位置偏差収束条件を満たしたか否かを判定する（ステップＳ１９）。ここで、処理回数ｉが所定の閾値Ｎにまだ達していない場合、処理はステップＳ１３に戻る。一方、処理回数ｉが所定の閾値Ｎに達した場合、目標曲率設定部１３Ａは、現在設定されている暫定目標曲率φ１_t’を、最終的に求める目標曲率φ１_tに代入する（ステップＳ２０）。目標曲率設定部１３Ａは目標曲率φ１_tを走行制御部１４に出力する（ステップＳ２１）。これにより、目標曲率設定部１３Ａによる１回の処理が終了する。

次に、目標線分切替部１５による具体的な処理例を説明する。図１０は、目標線分切替部１５の機能構成例を示すブロック図である。図１１は、次の目標線分に対する仮位置偏差を説明するための図である。図１０に示すように、目標線分切替部１５は、具体的な機能構成として、判定部１５Ａ、仮位置偏差算出部１５Ｂおよび仮曲率勾配算出部１５Ｃを備えている。

目標線分切替部１５は、仮位置偏差算出部１５Ｂおよび仮曲率勾配算出部１５Ｃの動作によって、図１１に示すように次の目標線分ＴＰ２との仮位置偏差ｐ２’を算出し、これを所定値として判定部１５Ａに出力する。そして、判定部１５Ａの動作によって、偏差検出部１２により検出された次の目標線分ＴＰ２に対する位置偏差ｐ２および方位偏差θ２と現在の曲率φとからなる３次元空間における座標（ｐ２，θ２，φ）が、所定値（仮位置偏差ｐ２’）を用いて表される３次元空間上の監視座標（図４の制御面４０上に存在する座標）に達するか監視座標を跨いで変位した状態といえるタイミングで、次の目標線分ＴＰ２を現在の目標線分ＴＰ１とするように切り替えるよう、目標線分取得部１１に対して目標線分ＴＰ１，ＴＰ２の切り替えを指示する。

ここで、判定部１５Ａは、仮位置偏差ｐ２’がゼロであるか正の値であるか負の値であるかを監視することにより、座標（ｐ２，θ２，φ）が、仮位置偏差ｐ２’を用いて表される３次元空間上の監視座標（ｐ２－ｐ２’，θ２，φ）に達するか監視座標（ｐ２－ｐ２’，θ２，φ）を跨いで変位した状態といえるか否かを判定する。すなわち、判定部１５Ａは、仮位置偏差ｐ２’がゼロとなったときに座標（ｐ２，θ２，φ）が監視座標（ｐ２－ｐ２’，θ２，φ）に一致した判定する。また、仮位置偏差ｐ２’が正の値から負の値に変化したときまたは負の値から正の値に変化したときに、座標（ｐ２，θ２，φ）が監視座標（ｐ２－ｐ２’，θ２，φ）を跨いで変位したと判定する。

仮位置偏差算出部１５Ｂおよび仮曲率勾配算出部１５Ｃの処理内容は、仮位置偏差算出部１３Ｂおよび仮曲率勾配算出部１３Ｃの処理内容と同様であり、処理に使用する値が異なるのみである。すなわち、位置・方位制御部１３の仮位置偏差算出部１３Ｂおよび仮曲率勾配算出部１３Ｃは、現在の目標線分ＴＰ１との関係で算出または設定される各値（位置偏差ｐ１、方位偏差θ１、仮方位偏差θ１’、仮曲率勾配λ１’、暫定目標曲率φ１_t’、仮曲率φ１’）を用いて仮位置偏差ｐ１’を算出するのに対し、目標線分切替部１５の仮位置偏差算出部１５Ｂおよび仮曲率勾配算出部１５Ｃは、次の目標線分ＴＰ２との関係で算出または設定される各値（位置偏差ｐ２、方位偏差θ２、仮方位偏差θ２’、仮曲率勾配λ２’、仮曲率φ２’）および現在の実際の曲率φを用いて、関数Φを用いた式φ＝Φ（ｐ２－ｐ２’，θ２）に基づき仮位置偏差ｐ２’を算出する。

すなわち、判定部１５Ａは、偏差検出部１２により検出された次の目標線分ＴＰ２に対する現在の方位偏差θ２および現在の位置偏差ｐ２を仮位置偏差算出部１５Ｂに供給するとともに、走行制御部１４から与えられる現在の曲率φを仮位置偏差算出部１５Ｂに供給し、仮位置偏差ｐ２’を算出するよう仮位置偏差算出部１５Ｂに指示をした後、仮位置偏差算出部１５Ｂが算出した仮位置偏差ｐ２’を受け取る。これを所定時間毎または所定走行距離毎に繰り返す。

判定部１５Ａは、仮位置偏差算出部１５Ｂから逐次受け取る仮位置偏差ｐ２’がゼロであるか正の値であるか負の値であるかを監視することにより、座標（ｐ２，θ２，φ）が、仮位置偏差ｐ２’を用いて表される３次元空間上の監視座標（ｐ２－ｐ２’，θ２，φ）に達するか監視座標（ｐ２－ｐ２’，θ２，φ）を跨いで変位した状態といえるか否かを判定する。そして、座標（ｐ２，θ２，φ）が監視座標（ｐ２－ｐ２’，θ２，φ）に達するか監視座標（ｐ２－ｐ２’，θ２，φ）を跨いで変位した状態といえると判定した場合、判定部１５Ａは、目標線分ＴＰ１，ＴＰ２の切替指示を目標線分取得部１１に出力する。目標線分取得部１１は、この切替指示を受けて、経路データ記憶部２００から取得する目標線分ＴＰ１，ＴＰ２を切り替える。

仮位置偏差算出部１５Ｂは、判定部１５Ａから与えられた現在の曲率φ、現在の方位偏差θ２および現在の位置偏差ｐ２を受け取ると、仮曲率勾配算出部１５Ｃに仮曲率φ２’を与えるとともに、現在の方位偏差θ２を仮方位偏差θ２’として与え、仮曲率勾配λ２’の算出を指示する。仮位置偏差算出部１５Ｂは、判定部１５Ａから与えられた現在の曲率φ、現在の方位偏差θ２、現在の位置偏差ｐ２を初期条件として、仮曲率勾配算出部１５Ｃにより算出された仮曲率勾配λ２’を用いて、仮方位偏差θ２’が解消する仮位置まで作業車両ＷＶを走行させたと仮定した場合に生じる、次の目標線分ＴＰ２（特許請求の範囲の所定位置に相当）との位置偏差である仮位置偏差ｐ２’の算出を行う。

ここで、仮位置偏差算出部１５Ｂは、作業車両ＷＶの方位偏差θ２および位置偏差ｐ２を算出する三角関数からなる非線形要素が含まれる演算（上述の（式２）（式３）（式４）に対応する演算）処理と、当該処理により算出される仮曲率φ２’および仮方位偏差θ２’を仮曲率勾配算出部１５Ｃに供給して仮曲率勾配λ２’を算出する処理とを含むステップ処理を、仮方位偏差θ２’が解消されるまで順次繰り返す。そして、この繰り返しによって算出した仮位置偏差ｐ２’を判定部１５Ａに出力する。

仮曲率勾配算出部１５Ｃは、仮位置偏差算出部１５Ｂから与えられた仮曲率φ２’および仮方位偏差θ２’をゼロに収束させるような仮曲率勾配λ２’を算出する関係式（上述の（式１）と同様の式）に基づいて、仮位置偏差算出部１５Ｂから指示された仮曲率φ２’および仮方位偏差θ２’を用いて仮曲率勾配λ２’を算出し、これを仮位置偏差算出部１５Ｂに出力する。

以上詳しく説明したように、本実施形態では、目標線分ＴＰに対する位置偏差ｐおよび方位偏差θと、車両の操舵角に関連する曲率φとを３軸とする３次元空間において、位置偏差ｐおよび方位偏差θを何れもゼロに収束させることが可能な目標曲率φ_tを算出する関数であって、位置偏差ｐ、方位偏差θおよび目標曲率φ_tの組み合わせとして採り得る値により定まる座標の集合によって３次元空間を２分する関数Φを用いて、車両が旋回する際に用いる走行の制御値として、位置偏差ｐおよび方位偏差θを解消するための目標曲率φ_tを算出し、当該算出した目標曲率φ_tに対して実際の曲率φが一致するように作業車両ＷＶの走行を制御する自動走行制御装置１００において、次の目標線分ＴＰ２に対する位置偏差ｐ２および方位偏差θ２と現在の曲率φとを用いて関数Φを用いた式φ＝Φ（ｐ２－ｐ２’，θ２）により算出される仮位置偏差ｐ２’がゼロであるか正の値であるか負の値であるかを監視することにより、座標（ｐ２，θ２，φ）が、仮位置偏差ｐ２’を用いて表される３次元空間上の監視座標（ｐ２－ｐ２’，θ２，φ）に達するか当該監視座標（ｐ２－ｐ２’，θ２，φ）を跨いで変位した状態といえるタイミングで、次の目標線分ＴＰ２を現在の目標線分ＴＰ１とするように目標線分ＴＰ１，ＴＰ２の切り替えを行うようにしている。

このようなタイミングで目標線分ＴＰ１，ＴＰ２の切り替えを行うと、制御対象を現在の目標線分ＴＰ１から次の目標線分ＴＰ２（目標線分ＴＰ１，ＴＰ２を切り替えた後は現在の目標線分ＴＰ１となる）に切り替えた際に、切り替え後の現在の目標線分ＴＰ１との関係で特定される座標（ｐ１，θ１，φ）が既に制御面４０の上または近傍に存在することになる。そのため、座標（ｐ１，θ１，φ）を制御面４０に近づけて一致させるようにするための無駄な制御が不要となる。これにより、本実施形態によれば、現在の目標線分ＴＰ１から次の目標線分ＴＰ２に対する制御を適切なタイミングで切り替えることができ、作業車両ＷＶに無駄な動作を生じさせることなく、次の目標線分ＴＰ２（切り替えた後の現在の目標線分ＴＰ１）についても、目標線分ＴＰ１に対する位置偏差ｐ１および方位偏差θ１を迅速に解消することができるようになる。

なお、上記実施形態では、仮方位偏差θ’が解消する時点で作業車両ＷＶの位置と目標線分ＴＰとの間に生じる位置偏差を仮位置偏差ｐ’とし、これが微小化する（ゼロに近づく）ように制御する例について説明したが、本発明はこれに限定されない。例えば、図１２に示すように、現在位置から目標線分ＴＰの方向に対する作業車両ＷＶの移動量（作業車両ＷＶの現在位置と、仮方位偏差θ’が解消される仮位置との位置偏差）を仮位置偏差ｐ”とし、この仮位置偏差ｐ”と現在の位置偏差ｐとの差が微小化する（ゼロに近づく）ように制御するようにしてもよい。このようにすれば、あらゆる条件の走行シミュレーションを事前に行って、その結果（仮位置偏差ｐ”）をテーブル情報として保持することも可能であるという点で好ましい。

図１３は、テーブル情報を用いた構成とする場合における位置・方位制御部１３および目標線分切替部１５の機能構成例を示すブロック図である。なお、この図１３において、図５および図１０に示した符号と同一の符号を付したものは同一の機能を有するものであるので、ここでは重複する説明を省略する。

テーブル情報を用いた構成とする場合、位置・方位制御部１３は、図１３（ａ）に示すように、仮位置偏差算出部１３Ｂおよび仮曲率勾配算出部１３Ｃに代えて、テーブル情報を記憶した仮位置偏差取得部１３Ｄを備えるようにする。また、目標曲率設定部１３Ａに代えて目標曲率設定部１３Ａ’を備えるようにする。この場合の仮位置偏差取得部１３Ｄは、現在の目標線分ＴＰ１との関係で偏差検出部１２により検出された現在の方位偏差θ１と、目標曲率設定部１３Ａ’が設定する暫定目標曲率φ１_t’とを入力として、テーブル情報を参照することによって仮位置偏差ｐ１”を取得するように構成する。

また、この構成の場合に目標曲率設定部１３Ａ’は、暫定目標曲率φ１_t’を設定して仮位置偏差ｐ１”を取得するよう仮位置偏差取得部１３Ｄに指示した後、仮位置偏差取得部１３Ｄが取得した仮位置偏差ｐ１”を受け取る。そして、受け取った仮位置偏差ｐ１”が所定の条件（位置偏差収束条件）を満たしていない場合は、新たに暫定目標曲率φ１_t’を設定して仮位置偏差ｐ１”を再取得するよう仮位置偏差取得部１３Ｄに指示する。一方、受け取った仮位置偏差ｐ１”が所定の条件を満たす場合は、仮位置偏差取得部１３Ｄが仮位置偏差ｐ１”を取得する際に使用した暫定目標曲率φ１_t’を目標曲率φ１_tとして走行制御部１４に対して出力する。

同様に、目標線分切替部１５は、図１３（ｂ）に示すように、仮位置偏差算出部１５Ｂおよび仮曲率勾配算出部１５Ｃに代えて、テーブル情報を記憶した仮位置偏差取得部１５Ｄを備えるようにする。また、判定部１５Ａに代えて判定部１５Ａ’を備えるようにする。この場合の仮位置偏差取得部１５Ｄは、次の目標線分ＴＰ２との関係で偏差検出部１２により検出された現在の方位偏差θ２と、走行制御部１４から判定部１５Ａ’を介して供給される現在の曲率φとを入力として、テーブル情報を参照することによって仮位置偏差ｐ２”を取得するように構成する。

判定部１５Ａ’は、次の目標線分ＴＰ２との関係で仮位置偏差取得部１５Ｄにより取得される仮位置偏差ｐ２”と、偏差検出部１２により検出された次の目標線分ＴＰ２に対する位置偏差ｐ２とを比較することにより、座標（ｐ２，θ２，φ）が、仮位置偏差ｐ２”を用いて表される３次元空間上の監視座標（ｐ２”，θ２，φ）（制御面４０上の座標）に達するか監視座標（ｐ２”，θ２，φ）を跨いで変位した状態といえるか否かを判定する。すなわち、判定部１５Ａ’は、仮位置偏差ｐ２”と位置偏差ｐ２との差がゼロとなったときに座標（ｐ２，θ２，φ）が監視座標（ｐ２”，θ２，φ）に一致した判定する。また、仮位置偏差 ｐ２”と位置偏差ｐ２との差が正の値から負の値に変化したときまたは負の値から正の値に変化したときに、座標（ｐ２，θ２，φ）が監視座標（ｐ２”，θ２，φ）を跨いで変位したと判定する。

また、上記実施形態では、目標線分切替部１５は、次の目標線分ＴＰ２との関係で関数Φを用いた式φ＝Φ（ｐ２－ｐ２’，θ２）または式φ＝Φ（ｐ２”，θ２）により算出される仮位置偏差ｐ２’またはｐ２”を所定値として用い、当該仮位置偏差ｐ２’を用いて表される３次元空間上の監視座標（ｐ２－ｐ２’，θ２，φ）または当該位置偏差ｐ２”を用いて表される３次元空間上の監視座標（ｐ２”，θ２，φ）をもとに目標線分ＴＰ１，ＴＰ２の切り替えを行う例について説明したが、本発明はこれに限定されない。例えば、次の目標線分ＴＰ２との関係で関数Φを用いた式φ２_t＝Φ（ｐ２，θ２）により算出される目標曲率φ２_tを所定値として用い、座標（ｐ２，θ２，φ）が、当該目標曲率φ２_tを用いて表される３次元空間上の監視座標（ｐ２，θ２，φ２_t）に達するか監視座標（ｐ２，θ２，φ２_t）を跨いで変位した状態といえるタイミングで目標線分ＴＰ１，ＴＰ２の切り替えを行うようにしてもよい。

図１４は、この場合における目標線分切替部１５の機能構成例を示すブロック図である。この図１４において、図１０に示した符号と同一の符号を付したものは同一の機能を有するものであるので、ここでは重複する説明を省略する。図１４に示すように、目標曲率φ２_tを用いて表される３次元空間上の監視座標（ｐ２，θ２，φ２_t）をもとに目標線分ＴＰ１，ＴＰ２の切り替えを行う構成とする場合、目標線分切替部１５は目標曲率設定部１５Ｅを更に備えている。また、判定部１５Ａに代えて判定部１５Ａ”を備えている。

目標曲率設定部１５Ｅの処理内容は目標曲率設定部１３Ａと同様である。すなわち、目標曲率設定部１５Ｅは、目標曲率設定部１３Ａが現在の目標線分ＴＰ１との関係で目標曲率φ１_tを算出する際に用いていた各値（位置偏差ｐ１、方位偏差θ１、仮方位偏差θ１’、仮曲率勾配λ１’、暫定目標曲率φ１_t’、仮曲率φ１’）に代えて、次の目標線分ＴＰ２との関係で算出または設定される各値（位置偏差ｐ２、方位偏差θ２、仮方位偏差θ２’、仮曲率勾配λ２’および仮曲率φ２’）を用いて、関数Φを用いた式φ２_t＝Φ（ｐ２，θ２）に基づき次の目標線分ＴＰ２に対する目標曲率φ２_tを算出する。そして、算出した目標曲率φ２_tを判定部１５Ａ”に供給する。

判定部１５Ａ”は、次の目標線分ＴＰ２との関係で関数Φを用いた式φ２_t＝Φ（ｐ２，θ２）により算出される目標曲率φ２_tと実際の曲率φとを比較することにより、座標（ｐ２，θ２，φ）が、目標曲率φ２_tを用いて表される３次元空間上の監視座標（ｐ２，θ２，φ２_t）（制御面４０上の座標）に達するか監視座標（ｐ２，θ２，φ２_t）を跨いで変位した状態といえるか否かを判定する。すなわち、判定部１５Ａ”は、目標曲率φ２_tと実際の曲率φとの差がゼロとなったときに座標（ｐ２，θ２，φ）が監視座標（ｐ２，θ２，φ２_t）に一致した判定する。また、目標曲率φ２_tと実際の曲率φとの差が正の値から負の値に変化したときまたは負の値から正の値に変化したときに、座標（ｐ２，θ２，φ）が監視座標（ｐ２，θ２，φ２_t）を跨いで変位したと判定する。

また、上記実施形態では、目標線分ＴＰに対する位置偏差ｐおよび方位偏差θを何れもゼロに収束させることが可能な目標曲率φ_tを算出する関数の一例として、仮位置偏差ｐ’を算出し、さらに、当該仮位置偏差ｐ’と現在の位置偏差ｐとを比較しながら、当該仮位置偏差ｐ’を解消するために必要な目標曲率φ_tを算出するための関数Φを用いた式φ_t＝Φ（ｐ，θ）を用いる例について説明したが、本発明はこれに限定されない。例えば、位置・方位制御部１３および目標線分切替部１５において、仮位置偏差ｐ’を算出し、さらに、当該仮位置偏差ｐ’と現在の位置偏差ｐとを比較しながら、当該仮位置偏差ｐ’を解消するために必要な想定方位θ_tを算出するため、関数をφ＝Φ（ｐ，θ_t）のように用いてもよい。

図１５は、この場合における目標線分切替部１５の機能構成例を示すブロック図である。この図１５において、図１４に示した符号と同一の符号を付したものは同一の機能を有するものであるので、ここでは重複する説明を省略する。図１５に示すように、関数Φを用いた式φ＝Φ（ｐ２，θ２_t）を用いて算出される想定方位θ２_tを用いて表される３次元空間上の監視座標（ｐ２，θ２_t，φ）をもとに目標線分ＴＰ１，ＴＰ２の切り替えを行う構成とする場合、判定部１５Ａ”に代えて判定部１５Ａ'''を備えるとともに、目標曲率設定部１５Ｅに代えて想定方位設定部１５Ｆを備えている。

想定方位設定部１５Ｆは、仮位置偏差算出部１５Ｂにより算出された仮位置偏差ｐ２’と現在の位置偏差ｐ２とを比較しながら、当該仮位置偏差ｐ２’がゼロとなる仮想の方位である想定方位θ２_tを算出する。例えば、図９にて説明した２分法と同様の方法により、目標曲率φ２_tに代えて想定方位θ２_tを算出する。そして、算出した想定方位θ２_tを判定部１５Ａ'''に出力する。

判定部１５Ａ'''は、次の目標線分ＴＰ２との関係で関数φ＝Φ（ｐ２，θ２_t）により算出される想定方位θ２_tを所定値として用い、座標（ｐ２，θ２，φ）が、想定方位θ２_tを用いて表される３次元空間上の監視座標（ｐ２，θ２_t，φ）（制御面４０上の座標）に達するか監視座標（ｐ２，θ２_t，φ）を跨いで変位した状態といえるタイミングで、次の目標線分ＴＰ２を現在の目標線分ＴＰ１とするように切り替える。

ここで、判定部１５Ａ'''は、次の目標線分ＴＰ２との関係で関数φ＝Φ（ｐ２，θ２_t）により算出される想定方位θ２_tと、目標線分ＴＰ２に対する方位偏差θ２とを比較することにより、座標（ｐ２，θ２，φ）が、想定方位θ２_tを用いて表される３次元空間上の監視座標（ｐ２，θ２_t，φ）（制御面４０上の座標）に達するか監視座標（ｐ２，θ２_t，φ）を跨いで変位した状態といえるか否かを判定する。すなわち、判定部１５Ａ'''は、想定方位θ２_tと実際の現在方位θとの差がゼロとなったときに座標（ｐ２，θ２，φ）が監視座標（ｐ２，θ２_t，φ）に一致したと判定する。また、想定方位θ２_tと実際の現在方位θとの差が正の値から負の値に変化したときまたは負の値から正の値に変化したときに、座標（ｐ２，θ２，φ）が監視座標（ｐ２，θ２_t，φ）を跨いで変位したと判定する。

上記図１４および図１５に示した変形例は、図１２に示す仮位置偏差ｐ”を適用して実施することも可能である。すなわち、図１４および図１５に示した仮位置偏差算出部１５Ｂおよび仮曲率勾配算出部１５Ｃに代えて、図１３に示した仮位置偏差取得部１５Ｄを用いるようにしてもよい。

また、上記実施形態では、目標線分ＴＰに対する位置偏差ｐおよび方位偏差θを何れもゼロに収束させることが可能な目標曲率φ_tを算出する関数の一例として、方位偏差θおよび位置偏差ｐを算出する三角関数からなる非線形要素が含まれる演算（上述した（式２）（式３）（式４）に対応する演算）によって仮位置偏差ｐ’を求める関数を用いて非線形制御を行う例について説明したが、本発明はこれに限定されない。

例えば、仮位置偏差ｐ’を求めることなく、φ_t＝－ａ・ｐ－ｂ・ｓｉｎθ（ａ，ｂは係数）のような数式ベースの関数を用いて非線形制御を行うようにしてもよい。また、φ_t＝－ａ・ｐ－ｂ・θ（ａ，ｂは係数）のような関数を用いて線形制御を行うようにしてもよい。これらの場合、目標線分切替部１５は、例えば、次の目標線分ＴＰ２との関係で上記の関数により算出される目標曲率φ２_tを所定値として用い、目標曲率φ２_tと実際の曲率φとを比較する（目標曲率φ２_tと実際の曲率φとの差がゼロであるか正の値であるか負の値であるかを監視する）ことにより、座標（ｐ２，θ２，φ２_t）が、目標曲率φ２_tを用いて表される３次元空間上の監視座標（ｐ２，θ２，φ２_t）に達するか監視座標（ｐ２，θ２_t，φ）を跨いで変位した状態といえるか否かを判定する。あるいはこれらの式をｐまたはθを求める式に変形した関数を用いるようにしても良い。

また、上記実施形態のように、２分法を用いて目標曲率φ_tを求める構成の場合、３次元空間を斜め方向から見たような関数を用いてもよい。例えば、以下のように曲率φの軸周りに（p－θ平面で）所定量αだけ回転させた新たな座標系を作り、２分法の演算と関数Φを含む式φ_t＝Φ（ｐ，θ）を用いて目標曲率φ_tを求めるようにすることも可能である。これは、φ軸に限らず任意の軸周りに回転させた場合も同様である。
ｐ＝ｘ・ｃｏｓα－ｙ・ｓｉｎα
θ＝ｘ・ｓｉｎα＋ｙ・ｃｏｓα

また、上記実施形態において、目標経路を構成する全ての目標線分について同じ制御を適用するようにしてもよいし、目標線分ごとに制御を変えるようにしてもよい。例えば、ある目標線分に対しては非線形制御を行い、ある目標線分に対しては線形制御を行うようにすることも可能である。すなわち、現在の目標線分ＴＰ１に対する位置偏差ｐ１、方位偏差θ１および目標曲率φ_tが採り得る値によって定まる曲面と、次の目標線分ＴＰ２に対する位置偏差ｐ２、方位偏差θ２および目標曲率φ_tが採り得る値によって定まる曲面とが異なってもよい。例えば、現在の目標線分ＴＰ１に対する次の目標線分ＴＰ２の角度が所定値より大きい場合に、次の目標線分ＴＰ２に対して非線形制御を行う一方、角度が所定値以下の場合に、次の目標線分ＴＰ２に対して線形制御を行うようにすることが可能である。目標線分切替部１１が次の目標線分ＴＰ２を現在の目標線分ＴＰ１とするように切り替えた後は、切り替え前に目標線分切替部１１が監視に使用していた曲面を、位置・方位制御部１３による制御に使用する。

なお、上記実施形態では、同様の処理を行う仮位置偏差算出部１３Ｂと仮位置偏差算出部１５Ｂとを位置・方位制御部１３および目標線分切替部１５のそれぞれに備え、同様の処理を行う仮曲率勾配算出部１３Ｃと仮曲率勾配算出部１５Ｃとを位置・方位制御部１３および目標線分切替部１５のそれぞれに備える構成について説明したが、この構成に限定されない。例えば、仮位置偏差算出部１５Ｂおよび仮曲率勾配算出部１５Ｃを省略し、仮位置偏差算出部１３Ｂおよび仮曲率勾配算出部１３Ｃにおいて、現在の目標線分ＴＰ１に対する処理と次の目標線分ＴＰ２に対する処理とを行うようにしてもよい。同様に、図１３の仮位置偏差取得部１５Ｄを省略し、仮位置偏差取得部１３Ｄにおいて、現在の目標線分ＴＰ１に対する処理と次の目標線分ＴＰ２に対する処理とを行うようにしてもよい。

また、上記実施形態では、（式２）～（式４）のように、作業車両ＷＶの走行距離Ｌを用いて仮曲率勾配λ’や仮位置偏差ｐ’を算出する例について説明したが、本発明はこれに限定されない。例えば、作業車両ＷＶの走行距離Ｌに代えて走行時間ｔを用いるようにしてもよい。

また、上記実施形態では、位置・方位制御部１３が仮曲率勾配λ’および仮位置偏差ｐ’を算出する際に、（式５）に示すような曲率の制約条件を用いる例について説明したが、これに加えて、以下に示す曲率勾配の制約条件を用いるようにしてもよい。
λ_min≦λ’(Ｌ)≦λ_max
このようにすれば、仮位置偏差ｐ’を算出する際に、仮方位偏差θ’の三角関数値、最大舵角に起因する曲率の飽和に加えて、車速および最大操舵速度（操舵モータの速度）に起因する曲率勾配の飽和という非線形要素も考慮した演算によって仮位置偏差ｐ’を求めることができる。

また、上記実施形態では、走行制御部１４が作業車両ＷＶの操舵角を制御する例について説明したが、本発明はこれに限定されない。例えば、車輪ではなくクローラを備えるような作業車両ＷＶの場合は、目標曲率φ_tに対して実際の曲率φが一致するように左右のクローラの回転量の差を制御するようにしてもよい。

その他、上記実施形態は、何れも本発明を実施するにあたっての具体化の一例を示したものに過ぎず、これによって本発明の技術的範囲が限定的に解釈されてはならないものである。すなわち、本発明はその要旨、またはその主要な特徴から逸脱することなく、様々な形で実施することができる。

１１ 目標線分取得部
１２ 偏差検出部
１３ 位置・方位制御部
１３Ａ，１３Ａ’ 目標曲率設定部
１３Ｂ 仮位置偏差算出部
１３Ｃ 仮曲率勾配算出部
１３Ｄ 仮位置偏差取得部
１４ 走行制御部
１５ 目標線分切替部
１５Ａ，１５Ａ’，１５Ａ”，１５Ａ''' 判定部
１５Ｂ 仮位置偏差算出部
１５Ｃ 仮曲率勾配算出部
１５Ｄ 仮位置偏差取得部
１５Ｅ 目標曲率設定部
１５Ｆ 想定方位設定部
１００ 自動走行制御装置

Claims (10)

1. あらかじめ設定した複数の目標線分を連結して成る目標経路に沿って車両を自動的に走行させるようにした自動走行制御装置であって、
   上記車両に搭載された位置・方位センサからの出力に基づいて検出される上記車両の現在位置および現在方位に基づいて、上記目標線分に対する自車位置の現在の位置偏差および上記目標線分に対する自車方位の現在の方位偏差を検出する偏差検出部と、
   上記車両が旋回する際に用いる走行の制御値として、上記偏差検出部により検出された上記位置偏差および上記方位偏差を解消するための目標曲率を算出する位置・方位制御部と、
   上記位置・方位制御部により算出された上記目標曲率に対して実際の曲率が一致するように上記車両の走行を制御する走行制御部と、
   現在の目標線分に対する走行制御から次の目標線分に対する走行制御への切り替えを制御する目標線分切替部とを備え、
   上記偏差検出部は、上記現在の目標線分に対する位置偏差および方位偏差を検出するとともに、上記次の目標線分に対する位置偏差および方位偏差を検出し、
   上記位置・方位制御部は、上記偏差検出部により検出された上記現在の目標線分に対する位置偏差および方位偏差を解消するための上記目標曲率を算出し、
   上記走行制御部は、上記位置・方位制御部により上記現在の目標線分について算出された上記目標曲率に対して上記実際の曲率が一致するように上記車両の走行を制御し、
   上記目標線分切替部は、上記位置・方位制御部に入力される上記位置偏差および上記方位偏差と、上記位置・方位制御部が出力する上記目標曲率とが採り得る値によって定まる曲面によって２分される３次元空間において、上記偏差検出部により検出された上記次の目標線分に対する位置偏差および方位偏差と現在の曲率とからなる上記３次元空間における座標を監視し、上記座標が上記曲面により２分される一方の領域と他方の領域との境界に達するか当該境界を跨いで変位した状態といえるタイミングで、上記次の目標線分を上記現在の目標線分とするように切り替える
   ことを特徴とする自動走行制御装置。
2. 上記位置・方位制御部は、上記偏差検出部により検出された上記方位偏差と上記現在の曲率とから取得される、曲率が所定走行距離当たりに変化する量である曲率勾配に従って、上記方位偏差が解消される仮位置まで上記車両を走行させたと仮定した場合に生じる、上記目標線分または上記車両の現在位置の何れかである所定位置と上記仮位置との位置偏差である仮位置偏差を算出し、さらに、上記仮位置偏差を解消するために必要な上記目標曲率を算出し、
   上記目標線分切替部は、上記次の目標線分との関係で算出される上記仮位置偏差または上記仮位置偏差から算出される値を所定値として、上記偏差検出部により検出された上記次の目標線分に対する位置偏差および方位偏差と現在の曲率とからなる上記座標が、上記所定値を用いて表される上記３次元空間上の監視座標に達するか上記監視座標を跨いで変位した状態といえるタイミングで、上記次の目標線分を上記現在の目標線分とするように切り替える
   ことを特徴とする請求項１に記載の自動走行制御装置。
3. 上記目標線分切替部は、上記次の目標線分との関係で算出される上記目標曲率を所定値として、上記所定値である上記目標曲率と上記実際の曲率とを比較することにより、上記偏差検出部により検出された上記次の目標線分に対する位置偏差および方位偏差と現在の曲率とからなる上記座標が、上記所定値を用いて表される上記３次元空間上の監視座標に達するか上記監視座標を跨いで変位した状態といえるタイミングで、上記次の目標線分を上記現在の目標線分とするように切り替える
   ことを特徴とする請求項１に記載の自動走行制御装置。
4. 上記仮位置偏差は、上記車両の現在位置と上記仮位置との位置偏差であり、
   上記目標線分切替部は、上記次の目標線分との関係で算出される上記仮位置偏差を上記所定値として、上記所定値である上記仮位置偏差と上記偏差検出部により検出された上記次の目標線分に対する位置偏差とを比較することにより、上記座標が上記監視座標に達するか上記監視座標を跨いで変位した状態といえるか否かを判定する
   ことを特徴とする請求項２に記載の自動走行制御装置。
5. 上記仮位置偏差は、上記目標線分と上記仮位置との位置偏差であり、
   上記目標線分切替部は、上記次の目標線分との関係で算出される上記仮位置偏差を上記所定値として、上記所定値である上記仮位置偏差がゼロであるか正の値であるか負の値であるかを監視することにより、上記座標が上記監視座標に達するか上記監視座標を跨いで変位した状態といえるか否かを判定する
   ことを特徴とする請求項２に記載の自動走行制御装置。
6. 上記位置・方位制御部は、上記仮位置偏差を算出し、さらに、上記仮位置偏差と上記偏差検出部により検出された上記現在の位置偏差とを比較しながら、上記仮位置偏差を解消するために必要な上記目標曲率を算出し、
   上記目標線分切替部は、上記次の目標線分との関係で算出される上記目標曲率を所定値として、上記所定値である上記目標曲率と実際の曲率とを比較することにより、上記座標が上記監視座標に達するか上記監視座標を跨いで変位した状態といえるか否かを判定する
   ことを特徴とする請求項２に記載の自動走行制御装置。
7. 上記位置・方位制御部は、上記仮位置偏差を算出し、さらに、上記仮位置偏差と上記偏差検出部により検出された上記現在の位置偏差とを比較しながら、上記仮位置偏差を解消するために必要な想定方位を算出し、
   上記目標線分切替部は、上記次の目標線分との関係で算出される上記想定方位を所定値として、上記所定値である上記想定方位と上記車両の実際の現在方位とを比較することにより、上記座標が上記監視座標に達するか上記監視座標を跨いで変位した状態といえるか否かを判定する
   ことを特徴とする請求項２に記載の自動走行制御装置。
8. 上記現在の目標線分に対する上記位置偏差、上記方位偏差および上記目標曲率とが採り得る値によって定まる曲面と、上記次の目標線分に対する上記位置偏差、上記方位偏差および上記目標曲率とが採り得る値によって定まる曲面とが異なることを許容し、
   上記目標線分切替部が上記次の目標線分を上記現在の目標線分とするように切り替えた後は、切り替え前に上記目標線分切替部が監視に使用していた曲面を、上記位置・方位制御部による制御に使用する
   ことを特徴とする請求項１～７の何れか１項に記載の自動走行制御装置。
9. あらかじめ設定した複数の目標線分を連結して成る目標経路に沿って車両を自動的に走行させるようにした自動走行制御方法であって、
   自動走行制御装置の偏差検出部が、上記車両に搭載された位置・方位センサからの出力に基づいて検出される上記車両の現在位置および現在方位に基づいて、上記目標線分に対する自車位置の現在の位置偏差および上記目標線分に対する自車方位の現在の方位偏差を検出する偏差検出ステップと、
   上記自動走行制御装置の位置・方位制御部が、上記車両が旋回する際に用いる走行の制御値として、上記偏差検出部により検出された上記位置偏差および上記方位偏差を解消するための目標曲率を算出する位置・方位制御ステップと、
   上記自動走行制御装置の走行制御部が、上記位置・方位制御部により算出された上記目標曲率に対して実際の曲率が一致するように上記車両の走行を制御する走行制御ステップと、
   上記自動走行制御装置の目標線分切替部が、現在の目標線分に対する走行制御から次の目標線分に対する走行制御への切り替えを制御する目標線分切替ステップとを有し、
   上記偏差検出ステップでは、上記偏差検出部が、上記現在の目標線分に対する位置偏差および方位偏差を検出するとともに、上記次の目標線分に対する位置偏差および方位偏差を検出し、
   上記位置・方位制御ステップでは、上記位置・方位制御部が、上記偏差検出部により検出された上記現在の目標線分に対する位置偏差および方位偏差を解消するための上記目標曲率を算出し、
   上記走行制御ステップでは、上記走行制御部が、上記位置・方位制御部により上記現在の目標線分について算出された上記目標曲率に対して上記実際の曲率が一致するように上記車両の走行を制御し、
   上記目標線分切替ステップでは、上記目標線分切替部が、上記位置・方位制御部に入力される上記位置偏差および上記方位偏差と、上記位置・方位制御部が出力する上記目標曲率とが採り得る値によって定まる曲面によって２分される３次元空間において、上記偏差検出部により検出された上記次の目標線分に対する位置偏差および方位偏差と現在の曲率とからなる上記３次元空間における座標を監視し、上記座標が上記曲面により２分される一方の領域と他方の領域との境界に達するか当該境界を跨いで変位した状態といえるタイミングで、上記次の目標線分を上記現在の目標線分とするように切り替える
   ことを特徴とする自動走行制御方法。
10. あらかじめ設定した複数の目標線分を連結して成る目標経路に沿って車両を自動的に走行させるようにした自動走行制御装置に適用される自動走行制御用プログラムであって、
    上記車両に搭載された位置・方位センサからの出力に基づいて検出される上記車両の現在位置および現在方位に基づいて、上記目標線分に対する自車位置の現在の位置偏差および上記目標線分に対する自車方位の現在の方位偏差を検出する偏差検出手段、
    上記車両が旋回する際に用いる走行の制御値として、上記偏差検出手段により検出された上記位置偏差および上記方位偏差を解消するための目標曲率を算出する位置・方位制御手段、
    上記位置・方位制御手段により算出された上記目標曲率に対して実際の曲率が一致するように上記車両の走行を制御する走行制御手段、および
    現在の目標線分に対する走行制御から次の目標線分に対する走行制御への切り替えを制御する目標線分切替手段
    としてコンピュータを機能させるためのプログラムであって、
    上記偏差検出手段は、上記現在の目標線分に対する位置偏差および方位偏差を検出するとともに、上記次の目標線分に対する位置偏差および方位偏差を検出し、
    上記位置・方位制御手段は、上記偏差検出手段により検出された上記現在の目標線分に対する位置偏差および方位偏差を解消するための上記目標曲率を算出し、
    上記走行制御手段は、上記位置・方位制御手段により上記現在の目標線分について算出された上記目標曲率に対して上記実際の曲率が一致するように上記車両の走行を制御し、
    上記目標線分切替手段は、上記位置・方位制御手段に入力される上記位置偏差および上記方位偏差と、上記位置・方位制御手段が出力する上記目標曲率とが採り得る値によって定まる曲面によって２分される３次元空間において、上記偏差検出手段により検出された上記次の目標線分に対する位置偏差および方位偏差と現在の曲率とからなる上記３次元空間における座標を監視し、上記座標が上記曲面により２分される一方の領域と他方の領域との境界に達するか当該境界を跨いで変位した状態といえるタイミングで、上記次の目標線分を上記現在の目標線分とするように切り替える
    ことを特徴とする自動走行制御用プログラム。

Images