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

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’、現在の方位偏差θおよび現在の位置偏差ｐを初期値とし、それにより得られた曲率φの値（以下、仮曲率φ’と呼ぶ）、方位偏差の値（以下、仮方位偏差θ’と呼ぶ）及び仮方位偏差ｐ’を再び（式２）（式３）（式４）に投入するとともに、仮曲率φ’、仮方位偏差θ’を仮曲率勾配算出部１３Ｃに与える。そして得られた仮曲率勾配λ’、仮曲率φ’および仮方位偏差θ’をこれらの式に繰り返し投入する演算を、仮方位偏差θ’が解消したと判断されるまで繰り返す。仮位置偏差算出部１３Ｂは、仮方位偏差θ’が解消したと判断すると、その時の仮位置偏差ｐ’を目標曲率設定部１３Ａに送る。

なお、仮曲率勾配算出部１３Ｃおよび仮位置偏差算出部１３Ｂは、次の（式５）に示すように、曲率φについて設定された下限値および上限値の範囲内で、仮方位偏差θ’を解消するための仮曲率勾配λ’を算出するとともに、仮位置偏差ｐ’を求める。ここで、φ_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として走行制御部１４に対して出力する。

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

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

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

１１ 目標経路取得部
１２ 偏差検出部
１３ 位置・方位制御部
１３Ａ 目標曲率設定部
１３Ｂ 仮位置偏差算出部（仮位置偏差取得部）
１３Ｃ 仮曲率勾配算出部（仮位置偏差取得部）
１３Ｄ 仮位置偏差取得部
１４ 走行制御部
１００ 自動走行制御装置

Claims (10)

1. あらかじめ設定した目標経路に沿って車両を自動的に走行させるようにした自動走行制御装置であって、
   上記車両に搭載された位置・方位センサからの出力に基づいて検出される上記車両の現在位置および現在方位に基づいて、上記目標経路に対する自車方位の現在の方位偏差および上記目標経路に対する自車位置の現在の位置偏差を検出する偏差検出部と、
   上記車両が旋回する際に走行の制御値として用いる経路の曲率として、上記偏差検出部により検出された上記方位偏差および上記位置偏差を解消するための目標曲率を算出する位置・方位制御部と、
   上記位置・方位制御部により算出された上記目標曲率に対して実際の曲率が一致するように上記車両の走行を制御する走行制御部とを備え、
   上記位置・方位制御部は、
   所定の関係式によって方位偏差と曲率とから取得される、曲率が所定走行距離当たりに変化する量である曲率勾配に従って、三角関数からなる非線形要素が含まれる関係式に従って上記方位偏差が解消されるまで上記車両を走行させたと仮定した場合に生じる、上記目標経路または上記車両の現在位置の何れかである所定位置からの位置偏差である上記仮位置偏差を取得する仮位置偏差取得部と、
   上記仮位置偏差取得部により取得された上記仮位置偏差と上記偏差検出部により検出された上記現在の位置偏差とを比較しながら、上記仮位置偏差を解消するために必要な上記目標曲率を設定する目標曲率設定部とを備えたことを特徴とする自動走行制御装置。
2. 上記仮位置偏差取得部は、仮位置偏差算出部と仮曲率勾配算出部とを備え、
   上記目標曲率設定部は、暫定目標曲率を設定して上記仮位置偏差を算出するよう上記仮位置偏差算出部に指示した後、上記仮位置偏差算出部が算出した上記仮位置偏差を受け取り、当該受け取った仮位置偏差が所定の条件を満たす場合は、上記仮位置偏差算出部が上記仮位置偏差を算出する際に使用した上記暫定目標曲率を目標曲率として上記走行制御部に対して出力し、上記受け取った仮位置偏差が上記所定の条件を満たしていない場合は、新たに暫定目標曲率を設定して上記仮位置偏差を再算出するよう上記仮位置偏差算出部に指示し、
   上記仮位置偏差算出部は、上記目標曲率設定部から与えられた上記暫定目標曲率と、上記偏差検出部により検出された上記現在の方位偏差と、上記仮曲率勾配算出部により算出された仮曲率勾配とに基づいて、上記仮曲率勾配に従って上記車両を走行させたと仮定した場合に生じる上記所定位置との位置偏差である上記仮位置偏差を、上記三角関数からなる非線形要素が含まれる演算によって算出する処理と、上記仮曲率勾配算出部に指示する仮曲率および仮方位偏差を算出する処理とを含むステップ処理を、上記仮方位偏差が解消されるまで順次繰り返し、算出した上記仮位置偏差を上記目標曲率設定部に出力し、
   上記仮曲率勾配算出部は、与えられた仮曲率および仮方位偏差をゼロに収束させるような仮曲率勾配を算出する関係式に基づいて、上記仮位置偏差算出部から指示された仮曲率および仮方位偏差を用いて上記仮曲率勾配を算出し、これを上記仮位置偏差算出部に出力する
   ことを特徴とする請求項１に記載の自動走行制御装置。
3. 上記位置・方位制御部は、上記曲率について設定された下限値および上限値の範囲内で、上記仮方位偏差を解消するための上記仮曲率勾配を算出するとともに、上記仮位置偏差を求めることを特徴とする請求項２に記載の自動走行制御装置。
4. 上記位置・方位制御部は、上記曲率勾配について設定された下限値および上限値の範囲内で、上記仮方位偏差を解消するための上記仮曲率勾配を算出するとともに、上記仮位置偏差を求めることを特徴とする請求項３に記載の自動走行制御装置。
5. 上記目標曲率設定部は、上記仮位置偏差算出部から上記仮位置偏差を受け取る毎に、上記仮位置偏差算出部に指示した上記暫定目標曲率の値を、上記目標経路に対する上記仮位置偏差の方向に応じて上端値または下端値の何れかに設定し、当該上端値または下端値を設定した範囲内で新たに上記暫定目標曲率を設定して上記仮位置偏差を再算出するよう上記仮位置偏差算出部に指示することを特徴とする請求項２～４の何れか１項に記載の自動走行制御装置。
6. 上記仮位置偏差算出部は、上記仮曲率勾配算出部により算出された仮曲率勾配に従って上記仮方位偏差が解消されるまで上記車両を走行させたと仮定した場合に生じる上記現在位置との位置偏差を上記仮位置偏差として算出し、
   上記目標曲率設定部は、上記仮位置偏差算出部により算出された上記仮位置偏差と上記偏差検出部により検出された上記現在の位置偏差との差を微小化させるような上記目標曲率を設定することを特徴とする請求項２～５の何れか１項に記載の自動走行制御装置。
7. 上記仮位置偏差算出部は、上記仮曲率勾配算出部により算出された仮曲率勾配に従って上記仮方位偏差が解消されるまで上記車両を走行させたと仮定した場合に生じる上記目標経路との位置偏差を上記仮位置偏差として算出し、
   上記目標曲率設定部は、上記仮位置偏差算出部により算出された上記仮位置偏差を微小化させるような上記目標曲率を設定することを特徴とする請求項２～５の何れか１項に記載の自動走行制御装置。
8. 上記仮位置偏差取得部は、上記偏差検出部により検出された上記現在の方位偏差および上記現在の位置偏差と、上記目標曲率設定部が設定する暫定目標曲率とを入力として、テーブル情報を参照することによって上記仮位置偏差を取得するように構成され、
   上記目標曲率設定部は、上記暫定目標曲率を設定して上記仮位置偏差を取得するよう上記仮位置偏差取得部に指示した後、上記仮位置偏差取得部が取得した上記仮位置偏差を受け取り、当該受け取った仮位置偏差が所定の条件を満たす場合は、上記仮位置偏差取得部が上記仮位置偏差を取得する際に使用した上記暫定目標曲率を目標曲率として上記走行制御部に対して出力し、上記受け取った仮位置偏差が上記所定の条件を満たしていない場合は、新たに暫定目標曲率を設定して上記仮位置偏差を再取得するよう上記仮位置偏差取得部に指示する
   ことを特徴とする請求項１に記載の自動走行制御装置。
9. あらかじめ設定した目標経路に沿って車両を自動的に走行させるようにした自動走行制御方法であって、
   自動走行制御装置の偏差検出部が、上記車両に搭載された位置・方位センサからの出力に基づいて検出される上記車両の現在位置および現在方位に基づいて、上記目標経路に対する自車方位の現在の方位偏差および上記目標経路に対する自車位置の現在の位置偏差を検出する偏差検出ステップと、
   上記自動走行制御装置の位置・方位制御部が、上記車両が旋回する際に走行の制御値として用いる経路の曲率として、上記偏差検出部により検出された上記方位偏差および上記位置偏差を解消するための目標曲率を算出する位置・方位制御ステップと、
   上記自動走行制御装置の走行制御部が、上記位置・方位制御部により算出された上記目標曲率に対して実際の曲率が一致するように上記車両の走行を制御する走行制御ステップとを有し、
   上記位置・方位制御ステップは、仮位置偏差取得ステップと目標曲率設定ステップとを含み、
   上記仮位置偏差取得ステップでは、上記位置・方位制御部が、所定の関係式によって方位偏差と曲率とから取得される、曲率が所定走行距離当たりに変化する量である曲率勾配に従って、三角関数からなる非線形要素が含まれる関係式に従って上記方位偏差が解消されるまで上記車両を走行させたと仮定した場合に生じる、上記目標経路または上記車両の現在位置の何れかである所定位置からの位置偏差である上記仮位置偏差を取得し、
   上記目標曲率設定ステップでは、上記位置・方位制御部が、上記仮位置偏差取得ステップで取得された上記仮位置偏差と上記偏差検出部により検出された上記現在の位置偏差とを比較しながら、上記仮位置偏差を解消するために必要な上記目標曲率を設定する
   ことを特徴とする自動走行制御方法。
10. あらかじめ設定した目標経路に沿って車両を自動的に走行させるようにした自動走行制御装置に適用される自動走行制御用プログラムであって、
    上記車両に搭載された位置・方位センサからの出力に基づいて検出される上記車両の現在位置および現在方位に基づいて、上記目標経路に対する自車方位の現在の方位偏差および上記目標経路に対する自車位置の現在の位置偏差を検出する偏差検出手段、
    上記車両が旋回する際に走行の制御値として用いる経路の曲率として、上記偏差検出手段により検出された上記方位偏差および上記位置偏差を解消するための目標曲率を算出する位置・方位制御手段、および
    上記位置・方位制御手段により算出された上記目標曲率に対して実際の曲率が一致するように上記車両の走行を制御する走行制御手段、
    としてコンピュータを機能させるためのプログラムであって、
    上記位置・方位制御手段は、
    所定の関係式によって方位偏差と曲率とから取得される、曲率が所定走行距離当たりに変化する量である曲率勾配に従って、三角関数からなる非線形要素が含まれる関係式に従って上記方位偏差が解消されるまで上記車両を走行させたと仮定した場合に生じる、上記目標経路または上記車両の現在位置の何れかである所定位置からの位置偏差である上記仮位置偏差を取得する仮位置偏差取得手段と、
    上記仮位置偏差取得手段により取得された上記仮位置偏差と上記偏差検出手段により検出された上記現在の位置偏差とを比較しながら、上記仮位置偏差を解消するために必要な上記目標曲率を設定する目標曲率設定手段とを備える
    ことを特徴とする自動走行制御用プログラム。

Images