JP7386757B2 - 作業車両の自律走行システム - Google Patents

Description

本発明は、作業車両の自律走行システムに関し、特に、前輪が操舵輪である作業車両を自律走行させる作業車両の自律走行システムに関するものである。

例えば圃場や枕地で予め定められた走行経路に沿って前進直進と前進旋回とを繰り返す等といった具合に、予め定められ走行パターンにて作業車両を走行させる場合には、オペレータの負担を軽減するべく、作業車両を自律走行させることが従来から行われている。

例えば特許文献１には、圃場内に予め設定された走行経路上の前方目標点と作業車両とを結ぶ線分と、走行経路と、がなす前方目標角度を算出するとともに、作業車両に配置された角速度センサから取得したヨーレートを実現する演算タイヤ角度を算出し、前方目標角度と演算タイヤ角度との比較結果に基づいて舵角を制御する、前方注視方式を採用した自律操舵装置が開示されている。

特開２０１９－１６６９８１号公報

ところで、特許文献１のもののように、前方注視方式を採用した自律操舵装置では、作業車両に設けられたＧＮＳＳアンテナが走行経路上に位置するようにフィードバック補正を行うことが、具体的には、操舵制御後のＧＮＳＳアンテナの位置を検出し、検出結果に基づいて舵角の補正を行うのが一般的である。

しかしながら、前輪が操舵輪である作業車両の後進旋回時に、かかる前方注視方式を採用した場合には、以下のような問題を生ずるおそれがある。

すなわち、旋回の向きと前輪の操舵の向きとが一致する前進旋回とは異なり、後進旋回の場合には、旋回の向きとは反対の向きに前輪を操舵する必要がある。それ故、後進旋回を開始すると、前輪側が例えば円弧状の走行経路の外側に膨らむとともに、ＧＮＳＳアンテナ位置が走行経路の内側に入ることになるため、旋回の向きと同じ向きに前輪を操舵する補正が入ることになる。すると、今度は、ＧＮＳＳアンテナ位置が走行経路の外側に出ることになるため、旋回の向きとは反対の向きに前輪を操舵する補正が入ることになる。このような操舵制御とフィードバック補正とが繰り返されることで、操舵制御が発散し、作業車両が蛇行するという問題が生じることになる。

本発明はかかる点に鑑みてなされたものであり、その目的とするところは、前輪が操舵輪である作業車両を、自律走行で後進旋回させる場合に、作業車両が蛇行するのを抑える技術を提供することにある。

前記目的を達成するため、本発明に係る作業車両の自律走行システムでは、予測モデルを用いたモデル予測制御により、所定の制御周期毎に最適な制御量を算出するようにしている。

具体的には、本発明は、前輪が操舵輪である作業車両を自律走行させる作業車両の自律走行システムを対象としている。

そして、この作業車両の自律走行システムは、上記作業車両が走行する目標となる走行経路を生成する経路生成手段と、上記作業車両に設定された制御中心の位置を検出可能な位置検出手段と、上記前輪の操舵角の制御量を演算して出力する演算手段と、上記演算手段によって出力された制御量に基づいて前輪を制御する操舵手段と、を備え、上記演算手段は、後進旋回時には、上記走行経路からの上記制御中心の偏差と上記操舵角の制御量との相関を状態方程式で表した予測モデルに基づき、所定の制御周期毎に、所定の評価関数を用いて最適な制御量を所定のステップ先まで算出し、第１番目のステップの制御量のみを出力するとともに、当該制御量に対応する上記制御中心の予測位置と、上記位置検出手段によって検出された当該制御中心の実測位置とに基づいて、当該予測モデルを補正するように構成されていることを特徴とするものである。

なお、本発明において「自律走行」とは、少なくとも操舵を自律的に行って、オペレータの運転操作なしで作業車両を走行させることを意味する。

この構成によれば、予測モデルに基づき所定の制御周期毎に、最適な制御量を所定のステップ先まで算出することから、換言すると、数ステップ先を予測した上で最適な制御量を算出することから、制御中心の予測位置と実測位置とに基づいて予測モデルをフィードバック補正する際に、操舵制御が発散するのを抑えることができる。したがって、前輪が操舵輪である作業車両を自律走行で後進旋回させる場合にも、作業車両が蛇行するのを抑えることができる。

また、上記作業車両の自律走行システムでは、上記制御中心は、上記作業車両の後輪の車軸における車幅方向の中心位置に設定されており、上記位置検出手段は、上記作業車両に設けられたＧＮＳＳアンテナの現在位置に基づいて、上記制御中心の現在位置を検出するように構成されていることが好ましい。

作業車両を自律走行させる場合には、通常、ＧＮＳＳアンテナの位置で作業車両を制御するところ、この構成によれば、制御中心をＧＮＳＳアンテナの位置ではなく、作業車両の後輪の車軸における車幅方向の中心位置に設定することから、換言すると、仮想のＧＮＳＳアンテナの位置（制御中心）を、後進旋回時における車両中心に設定することから、蛇行の影響をより一層抑えることができる。

ところで、前進直進してきた作業車両を、後進旋回させて直角に方向転換させるような場合には、作業車両を前進直進からいきなり後進旋回させる訳ではなく、短い後進直進を経て後進旋回を開始するのが一般的である。このように後進直進から後進旋回へ切り替える場合には、曲率が０から１／曲率半径へ急激に変化するが、操舵手段にはダイナミクス（動的特性）があるため、不可避的に応答遅れが生じてしまい、旋回のタイミングが遅れることになる。かかる応答遅れを抑えるために、制御量を大きくすると、実際の操舵角が大きくオーバーシュートしてしまうおそれがある。なお、このような問題は、後進旋回から前進直進または後進直進へ切り替える場合にも生じ得る。

そこで、上記作業車両の自律走行システムでは、上記演算手段は、後進直進から後進旋回へ切り替える場合、または、後進旋回から直進へ切り替える場合には、上記走行経路における切替え予定位置に到達する前に、切替え後の制御量を出力するように構成されていることが好ましい。

この構成によれば、走行経路における切替え予定位置に到達する前に、先を見越して演算手段が早めに切替え後の制御量を出力することで、オーバーシュートの発生を抑えつつ、操舵手段のダイナミクスに起因する遅れを抑えることができる。

以上説明したように、本発明に係る作業車両の自律走行システムによれば、前輪が操舵輪である作業車両を、自律走行で後進旋回させる場合に、作業車両が蛇行するのを抑えることができる。

本発明の実施形態に係る自律走行システムの概要を模式的に示す図である。 自律走行システムを模式的に示すブロック図である。 直線状走行経路に沿ってロボットトラクタを自律走行させる手法を模式的に説明する図である。 ヨーレートから演算タイヤ角度を算出する手法を模式的に説明する図である。 前進旋回用走行経路に沿ってロボットトラクタを自律走行させる手法を模式的に説明する図である。 枕地において、ロボットトラクタを進行方向と直角に方向転換させる態様の一例を模式的に説明する図である。 時計回りの後進旋回時のプラントモデルを模式的に説明する図である。 反時計回りの後進旋回時のプラントモデルを模式的に説明する図である。 モデル予測制御の概念を模式的に説明する図である。 後進直進から後進旋回に切り替える場合、および、後進旋回から後進直進に切り替える場合における、曲率の変化を模式的に説明する図である。 後進直進から後進旋回への切替え予定位置までの距離と、後進旋回時の曲率との関係を示すグラフ図である。 後進旋回から後進直進への切替え予定位置までの距離と、後進旋回時の曲率との関係を示すグラフ図である。 後進旋回時のロボットトラクタの軌跡を模式的に示す図である。

以下、本発明を実施するための形態を図面に基づいて説明する。

－自律走行システム－
図１は、本実施形態に係る自律走行システム１の概要を模式的に示す図である。この自律走行システム１は、ＧＮＳＳ衛星６５を利用してロボットトラクタ１０の位置を把握しながら、無線通信端末５０に予め設定された走行経路に沿って、圃場内や枕地等においてロボットトラクタ１０を自律走行させるものである。なお、「自律走行」とは、少なくとも操舵を自律的に行って、オペレータ（図示せず）の運転操作なしでロボットトラクタ１０を走行させることを意味する。

ロボットトラクタ（作業車両）１０は、図１に示すように、車体部１１と、左右１対の前輪１３と、左右一対の後輪１５と、作業機１７と、を備えている。車体部１１の前部には、エンジン１９が配置されており、エンジン１９で発生した動力がＨＭＴ（Hydro Mechanical Transmission）２１（図２参照）を介して前輪１３および後輪１５に伝達されるようになっている。一方、車体部１１の後部には、オペレータが搭乗するキャビン２３が設けられている。キャビン２３の内部には、操舵輪である前輪１３をオペレータが操舵するための操舵ハンドル２５が配置されている。また、キャビン２３の上部には、ＧＮＳＳ衛星６５からの搬送波位相を検出することが可能なＧＮＳＳアンテナ２７や、無線通信端末５０と無線通信を行うための通信アンテナ２９や、後述する慣性計測装置４５等が設けられている。作業機１７は、車体部１１の後端部に設けられたリンク機構１１ａに装着されている。

このロボットトラクタ１０は、オペレータがキャビン２３内に搭乗して操舵ハンドル２５等を実際に操作することで走行する他、オペレータによって設定された無線通信端末５０からの指示等に基づいて自律的に走行することが可能に構成されている。なお、ロボットトラクタ１０は、無線通信端末５０を操作するオペレータの乗車の有無に関係なく、換言すると、オペレータがキャビン２３内に搭乗していてもしていなくても、自律走行を行うことが可能となっている。以下、このような自律走行を可能とする本実施形態の自律走行システム１について詳細に説明する。

図２は、自律走行システム１を模式的に示すブロック図である。本実施形態の自律走行システム１は、図２に示すように、ロボットトラクタ１０に搭載された走行制御装置３０と、オペレータによって操作される無線通信端末５０と、基準局６０と、を中心に構成されている。

〈走行制御装置〉
ロボットトラクタ１０の走行制御装置３０は、公知のコンピュータからなり、図示は省略するが、ＣＰＵ（Central Processing Unit）、ＲＯＭ（Read Only Memory）、ＲＡＭ（Random Access Memory）、入力インターフェース、出力インターフェース等を含んでいる。ＣＰＵは、ＲＯＭに記憶された各種制御プログラムやマップに基づいて演算処理を実行する。ＲＯＭには、各種制御プログラムや、それら各種制御プログラムを実行する際に参照されるマップ等が記憶されている。ＲＡＭは、ＣＰＵによる演算結果や各センサの検出結果等を一時的に記憶するメモリである。この走行制御装置３０には、図２に示すように、車速センサ４１と、舵角センサ４３と、慣性計測装置４５と、ＧＮＳＳアンテナ２７と、通信アンテナ２９と、エンジンＥＣＵ３１と、ＨＭＴＥＣＵ３３と、ステアリングＥＣＵ３５と、が接続されている。

車速センサ４１は、例えば前輪１３の車軸に配置されていて、前輪１３の車軸の回転に応じたパルス信号を発生するように構成されている。車速センサ４１による検出結果（パルス信号）は、走行制御装置３０に入力される。

舵角センサ４３は、ステアリングシャフト（図示せず）に配置されていて、ステアリング角度（ステアリングシャフトの回転角度）を検出するように構成されている。なお、舵角センサ４３は、タイヤ角度（ロボットトラクタ１０の前後方向軸に対する前輪１３の車輪の傾斜角）を検出するように構成されていてもよい。舵角センサ４３による検出結果は、走行制御装置３０に入力される。

慣性計測装置４５は、３つのジャイロセンサ（角速度センサ）と３つの加速度センサを備えている。これにより、慣性計測装置４５は、上下方向を回転軸としたロボットトラクタ１０の角速度であるヨーレートの他、ロールや、ピッチを検出することが可能となっている。慣性計測装置４５が検出するロボットトラクタ１０の角速度および加速度は、走行制御装置３０に入力される。

ＧＮＳＳアンテナ２７は、ＧＮＳＳ衛星６５からの搬送波位相を検出して、検出結果を走行制御装置３０へ出力する。ＧＮＳＳアンテナ２７は、ロボットトラクタ１０における車幅方向の中央部に設けられている。走行制御装置３０は、後述するように、基準局６０からの補正情報を受信した上で、ＲＴＫを利用して測位を行うことで、ロボットトラクタ１０の位置を例えば緯度および経度の情報として算出する。

通信アンテナ２９は、無線通信端末５０の通信アンテナ５３や、基準局６０の無線通信装置６３との間で各種の信号を送受信可能に構成されている。これにより、走行制御装置３０は、適宜の方式で変調処理または復調処理を行うことで、無線通信端末５０や基準局６０との間で情報の遣り取りを行うことが可能となっている。

エンジンＥＣＵ３１は走行制御装置３０の指令に基づいてエンジン１９の制御を行うとともに、エンジン１９の状態に関する情報を走行制御装置３０へ出力するように構成されている。また、ＨＭＴＥＣＵ３３は走行制御装置３０の指令に基づいてＨＭＴ２１の制御を行うとともに、ＨＭＴ２１の状態に関する情報を走行制御装置３０へ出力するように構成されている。走行制御装置３０は、車速センサ４１の検出結果により得られた、ロボットトラクタ１０の現在の車速Ｖが目標の車速に近づくように、エンジン１９の回転速度および／またはＨＭＴ２１の変速比を変更することで、ロボットトラクタ１０の車速Ｖを自律的に変更する車速制御を行うようになっている。

ステアリングＥＣＵ３５は、走行制御装置３０との間で指令や情報の遣り取りを行うことが可能になっており、走行制御装置３０の指令に基づいて、前輪１３の電動パワーステアリング３７の制御を行うように構成されている。走行制御装置３０は、舵角センサ４３の検出結果により得られた現在の舵角に基づいて、例えば電動パワーステアリング３７の操舵トルクを制御するアシストモータ（図示せず）を駆動させることで、ロボットトラクタ１０の舵角を自律的に変更する操舵制御を行うことが可能となっている。

〈無線通信端末〉
無線通信端末５０は、タブレット型のコンピュータであり、図２に示すように、制御部５１と、通信アンテナ５３と、ディスプレイ５５と、操作部５７と、を備えている。

制御部５１は、公知のコンピュータとして構成されており、図示は省略するが、ＣＰＵ、ＲＯＭ、ＲＡＭ、入力インターフェース、出力インターフェース等を含んでいる。そして、制御部５１は、走行経路生成部５１ａと、記憶部５１ｂと、を備えている。

走行経路生成部５１ａは、オペレータの操作に基づいて、ロボットトラクタ１０が走行する目標となる走行経路を生成する処理を行う。走行経路には、直線状走行経路７１（図３参照）、前進旋回用走行経路７３（図５参照）および後進旋回用走行経路７５（図６参照）、並びに、これらを組み合わせた経路等がある。記憶部５１ｂには、ロボットトラクタ１０の設定に関する情報、オペレータが作成した走行経路７１，７３，７５、および、走行経路７１，７３，７５を生成するための各種条件等が記憶されている。

なお、請求項との関係では、本実施形態の無線通信端末５０の走行経路生成部５１ａが、本発明でいうところの「作業車両が走行する目標となる走行経路を生成する経路生成手段」に相当する。

通信アンテナ５３は、ロボットトラクタ１０と無線通信を行うための近距離通信用のアンテナで構成されている。制御部５１は、適宜の方式で変調処理または復調処理を行うことで、ロボットトラクタ１０の走行制御装置３０や基準局６０との間で情報を遣り取りすることが可能となっている。

ディスプレイ５５は、液晶ディスプレイや有機ＥＬディスプレイ等であり、画像を表示可能に構成されている。ディスプレイ５５は、制御部５１の制御によって、例えば、自律走行に関する情報、ロボットトラクタ１０の設定に関する情報、各種センサの検出結果および警告情報等を表示することが可能となっている。操作部５７は、例えば、ディスプレイ５５に重ねて配置されたタッチパネルであり、ディスプレイ５５上におけるオペレータの指等による操作を検出するように構成されている。

〈基準局〉
本実施形態では、衛星測位システムを用いた測位方法としてＲＴＫ（Real Time Kinematic）を採用しており、基準点からの補正情報と、側位点の衛星測位情報と、を用いて、ロボットトラクタ１０の現在位置を求めるようにしている。

具体的には、ロボットトラクタ１０の走行の邪魔にならない位置に基準点となる基準局６０を設置している。なお、基準局６０を設置した箇所は、ロボットトラクタ１０の走行制御装置３０や無線通信端末５０によって記憶される。基準局６０は、ＧＮＳＳアンテナ６１と、ロボットトラクタ１０の通信アンテナ２９との間で各種の信号を送受信可能な無線通信装置６３と、を備えている。これにより、基準点でも、ＧＮＳＳ衛星６５からの搬送波位相を検出することができるとともに、基準局６０とロボットトラクタ１０との間で各種の情報が送受信可能になっている。

そうして、基準点に設置された基準局６０のＧＮＳＳアンテナ６１と、側位点となるロボットトラクタ１０のＧＮＳＳアンテナ２７との両方でＧＮＳＳ衛星６５からの搬送波位相を検出する。基準局６０では、ＧＮＳＳ衛星６５から搬送波位相を検出する毎に、検出した搬送波位相と基準点の位置情報等を含む補正情報を生成して、無線通信装置６３からロボットトラクタ１０の通信アンテナ２９に補正情報を送信する。これに対し、ロボットトラクタ１０の走行制御装置３０では、ＧＮＳＳアンテナ２７にて検出した搬送波位相と、基準局６０から送信された補正情報とを用いて、上述の如く、ロボットトラクタ１０の現在位置情報を緯度および経度の形式で算出する。

－自律操舵制御（前進時）－
〈直線状走行経路〉
次に、図３および図４を参照して、ロボットトラクタ１０を直線状走行経路７１に沿って自律走行させる場合の操舵制御について説明する。

図３は、直線状走行経路７１に沿ってロボットトラクタ１０を自律走行させる手法を模式的に説明する図であり、図４は、ヨーレートから演算タイヤ角度βを算出する手法を模式的に説明する図である。図３に示す直線状走行経路７１から離れた位置を走行しているロボットトラクタ１０を、当該直線状走行経路７１に沿って自律走行させる場合、走行制御装置３０は、以下に説明するように、前方注視方式にて目標ステアリング角速度を算出するように構成されている。この場合、操舵制御の入力は、前方目標角度φおよび演算タイヤ角度βである。

前方目標角度φは、ロボットトラクタ１０を直線状走行経路７１に沿って走行させる際に用いられる角度であり、前方目標点Ｐ１に基づいて、走行制御装置３０によって算出される。ロボットトラクタ１０には予め前方目標距離が設定されており、走行制御装置３０は、ロボットトラクタ１０のＧＮＳＳアンテナ２７から直線状走行経路７１までの距離が前方目標距離となる点（前方目標点Ｐ１）を特定する。また、走行制御装置３０は、ロボットトラクタ１０のＧＮＳＳアンテナ２７の位置と前方目標点Ｐ１とを接続する線分（線Ｌ１）を引き、線Ｌ１と直線状走行経路７１とがなす角を前方目標角度φとして算出する。

また、走行制御装置３０は、慣性計測装置４５が検出したヨーレートに基づいて、演算タイヤ角度βを算出する。演算タイヤ角度βは、検出したヨーレートでロボットトラクタ１０を旋回させるための理想的な（理論上の）タイヤ角度、換言すれば、検出されたヨーレートをタイヤ角度に変換した値である。演算タイヤ角度βは、４輪車であるロボットトラクタ１０を、図４に示すように、前輪１３および後輪１５が左右一対ではなく中央に１つずつあると仮定した２輪車モデルに変換して算出される。

図４において、ωはヨーレートであり、Ｖは車速であり、Ｌはホイールベースであり、Ｒは２輪車モデルにおける旋回半径である。なお、車速Ｖは車速センサ４１によって検出され、また、ホイールベースＬは予め走行制御装置３０に記憶されている。そうして、２輪車モデルにおける旋回半径Ｒは、下記の（式１）で表される。

また、ヨーレート（角速度）ωと車速Ｖと旋回半径Ｒの関係は、（式１）を用いて下記の（式２）で表すことができる。

さらに、演算タイヤ角度βの値は、（式２）を変形して、下記の（式３）表すことができる。

このような処理を行うことで、慣性計測装置４５が検出したヨーレートωから演算タイヤ角度βを算出することが可能となる。

そうして、走行制御装置３０は、前方目標角度φから演算タイヤ角度βを減算する。これにより、タイヤ角度と進行方向との差異が含まれないように、換言すると、横滑りの影響がなくなるように、前方目標角度φが補正されることになる。

走行制御装置３０は、前方目標角度φから演算タイヤ角度βを減算した処理結果に基づいて目標ステアリング角速度を算出し、ステアリングＥＣＵ３５に出力する。ステアリングＥＣＵ３５が、目標ステアリング角速度に基づいて、前輪１３の電動パワーステアリング３７の制御を行うことで、ロボットトラクタ１０の舵角が自律的に変更される。これにより、前方目標角度φおよび演算タイヤ角度βの値が変化するため、前方目標角度φから演算タイヤ角度βを減算した値も変化し、新たな目標ステアリング角速度が出力される。以上の処理を継続して行うことで、ロボットトラクタ１０を直線状走行経路７１に沿って走行させることができる。

〈前進旋回用走行経路〉
次に、図５を参照して、ロボットトラクタ１０を前進旋回用走行経路７３に沿って自律走行させる場合の操舵制御について説明する。

図５は、前進旋回用走行経路７３に沿ってロボットトラクタ１０を自律走行させる手法を模式的に説明する図である。前進旋回用走行経路７３に沿ってロボットトラクタ１０を自律走行させる場合には、走行制御装置３０は、直線状走行経路７１の場合と同様に、前方注視方式にて目標ステアリング角速度を算出するように構成されている。この場合、操舵制御の入力は、前方目標角度φ、演算タイヤ角度βおよび旋回演算タイヤ角度である。

前方目標角度φは、ロボットトラクタ１０を前進旋回用走行経路７３に沿って走行させる際に用いられる角度であり、前方目標点Ｐ１に基づいて、走行制御装置３０によって算出される。走行制御装置３０は、ロボットトラクタ１０から前進旋回用走行経路７３までの距離が前方目標距離となる点（前方目標点Ｐ１）を特定する。また、走行制御装置３０は、ロボットトラクタ１０のＧＮＳＳアンテナ２７の位置と前方目標点Ｐ１とを接続する線分（線Ｌ１）を引く。

次に、走行制御装置３０は、ロボットトラクタ１０のＧＮＳＳアンテナ２７の位置と、前進旋回用走行経路７３の旋回中心Ｐ２と、を結ぶ線分である線Ｌ２を引く。そうして、走行制御装置３０は、線Ｌ２に垂直であってＧＮＳＳアンテナ２７の位置を通る線Ｌ３を引き、線Ｌ１と線Ｌ３がなす角を前方目標角度φとして算出する。ここで、線Ｌ３は、線Ｌ２と前進旋回用走行経路７３との交点における前進旋回用走行経路７３の接線であるため、現在のロボットトラクタ１０位置における前進旋回用走行経路７３の方向を示している。

また、走行制御装置３０は、直線状走行経路７１の場合と同様に、慣性計測装置４５が検出したヨーレートωに基づいて、演算タイヤ角度βを算出する。そうして、走行制御装置３０は、前方目標角度φから演算タイヤ角度βを減算する。

さらに、走行制御装置３０は、前進旋回用走行経路７３に沿って旋回するためのヨーレートである旋回ヨーレートを、前進旋回用走行経路７３の曲率半径等に基づいて推測し、推測した旋回ヨーレートを図４に示す手法で旋回演算タイヤ角度に変換する。そうして、走行制御装置３０は、減算後の前方目標角度φに対して、旋回演算タイヤ角度を加算する。後の処理は、直線状走行経路７１の場合と同様なので、説明を省略する。

－自律操舵制御（後進旋回時）－
次に、ロボットトラクタ１０を後進旋回用走行経路７５に沿って自律走行させる場合の操舵制御について説明する。

図６は、枕地５において、ロボットトラクタ１０を進行方向と直角に方向転換させる態様の一例を模式的に説明する図である。本実施形態では、圃場３の外側に設定された枕地５において、ロボットトラクタ１０を進行方向と直角に方向転換させる態様として、図６に示すように、αターンを採用している。

ここで、「αターン」とは、図６の白抜き矢印で示すように、直線状走行経路７１ａに沿って前進直進してきたロボットトラクタ１０を、図６の黒塗り矢印で示すように、後進旋回用走行経路７５に沿って後進旋回させて直角に方向転換させた後、図６のハッチング矢印で示すように、方向転換後のロボットトラクタ１０を直線状走行経路７１ｂに沿って前進直進させる方向転換態様を指す。

なお、後進旋回用走行経路７５には、図６に示すように、円弧経路７５ｂの両側に短い直線経路７５ａおよび直線経路７５ｃが含まれている。それ故、より正確には、αターンとは、直線状走行経路７１ａに沿って前進直進してきたロボットトラクタ１０を、一旦直線経路７５ａに沿って短く後進させてから、円弧経路７５ｂに沿って後進旋回させて直角に方向転換させた後、直線経路７５ｃに沿って短く後進させてから、方向転換後のロボットトラクタ１０を直線状走行経路７１ｂに沿って前進直進させる方向転換態様を指す。

ここで、ロボットトラクタ１０を、後進旋回用走行経路７５に沿って後進旋回させる場合にも、前進の場合と同様に、前方注視方式を採用することが考えられる。しかしながら、前輪１３が操舵輪であるロボットトラクタ１０の後進旋回時に、前方注視方式を採用した場合には、以下のような問題を生ずるおそれがある。

すなわち、旋回の向きと前輪１３の操舵の向きとが一致する前進旋回とは異なり、後進旋回の場合には、旋回の向きとは反対の向きに前輪１３を操舵する必要がある。それ故、後進旋回を開始すると、前輪１３側が後進旋回用走行経路７５の外側に膨らむとともに、ＧＮＳＳアンテナ２７の位置が後進旋回用走行経路７５の内側に入ることになるため、旋回の向きと同じ向きに前輪１３を操舵する補正が入ることになる。すると、今度は、ＧＮＳＳアンテナ２７の位置が後進旋回用走行経路７５の外側に出ることになるため、旋回の向きとは反対の向きに前輪１３を操舵する補正が入ることになる。このような操舵制御とフィードバック補正とが繰り返されることで、操舵制御が発散し、ロボットトラクタ１０が蛇行するという問題が生じることになる。

そこで、本実施形態の自律走行システム１では、予測モデルを用いたモデル予測制御により、所定の制御周期毎に最適な制御量を算出するようにしている。

具体的には、後進旋回時には、後進旋回用走行経路７５からの制御中心ＣＣの偏差と操舵角の制御量との相関を状態方程式で表した予測モデルに基づき、所定の制御周期毎に、所定の評価関数Ｊを用いて最適な制御量を所定のステップ先まで算出し、第１番目のステップの制御量のみを出力するとともに、制御量に対応する制御中心ＣＣの予測位置と、ＧＮＳＳアンテナ２７等によって検出される制御中心ＣＣの実測位置とに基づいて、予測モデルを補正するように、走行制御装置３０を構成している。以下、ロボットトラクタ１０を後進旋回用走行経路７５に沿って自律走行させる場合のモデル予測制御について詳細に説明する。

〈状態方程式〉
本実施形態では、状態方程式（差分方程式）を下記の（式４）のように定義する。

ここで、ｋ（０以上の整数）は予測ステップ数であり、Ａは状態係数ベクトルであり、ｘ［ｋ］は第ｋステップにおけるモデルの状態ベクトルであり、ｂ_１は入力係数ベクトルであり、ｕ［ｋ］は第ｋステップにおける入力（制御量）であり、ｂ_２は外乱係数ベクトルであり、ｄ［ｋ］は第ｋステップにおける既知外乱である。

そうして、上記（式４）から、以下のような状態時系列を考える。

以上の状態系列をまとめて、行列で表現すると、下記の（式５）のように表すことができる。なお、（式５）におけるＥは単位行列である。

ここで、状態系列ベクトルＸ、入力系列ベクトルＵ、外乱系列ベクトルＤ、行列Ｈ_１、行列Ｈ_２および行列Ｈ_３を、下記の（式６）～（式１１）のように定義する。

すると、上記（式５）は、下記の（式１２）のように表すことができ、この（式１２）で表される状態方程式が、下記の評価関数Ｊによる評価対象となる。

〈評価関数〉
本実施形態では、評価関数Ｊを下記の（式１３）のように定義する。なお、Ｗ_１およびＷ_２は重み行列である。

上記（式１３）における１／２は、後の計算に出てくる２を打ち消すために与えられた単なる係数である。それ故、上記（式１３）の評価関数Ｊは、状態系列ベクトルＸの２乗と入力系列ベクトルＵの２乗との和を重み付けして評価しているといえる。

上記（式１３）に上記（式１２）の状態方程式を代入して整理すると、評価関数Ｊを下記の（式１４）のように表すことができる。なお、（式１２）を整理する際に出てくる、ＵＨ_２ ^ＴＷ_１Ｈ_１ｘ_０、Ｕ^ＴＨ_２ ^ＴＷ_１Ｈ_３ＤおよびＤ^ＴＨ_１ ^ＴＷ_１Ｈ_１ｘ_０は、最終的にはスカラーになることから、それぞれｘ_０ ^ＴＨ_１ ^ＴＷ_１Ｈ_２Ｕ、Ｄ^ＴＨ_３ ^ＴＷ_１Ｈ_２Ｕおよびｘ_０ ^ＴＨ_１ ^ＴＷ_１Ｈ_１Ｄに変換可能である。

上記（式１４）に示す評価関数Ｊを、入力系列ベクトルＵで微分すると、下記の（式１５）が得られる。

ここで、∂(Ｊ(Ｕ))/∂Ｕ＝０となる入力系列ベクトルＵで、評価関数Ｊが最小となることから、∂(Ｊ(Ｕ))/∂Ｕ＝０を解くことで、下記の（式１６）で示す、最適な入力系列ベクトルＵが得られる。

そうして、走行制御装置３０は、例えば、予測サンプリング時間ｄｔ＝０．１（秒）毎に、上記（式１６）に基づき最適な入力（制御量）を５ステップ先まで算出し、第１番目のステップの制御量のみを出力する。それ故、請求項との関係では、予測サンプリング時間ｄｔが、本発明でいうところの「所定の制御周期」に相当し、５ステップが、本発明でいうところの「所定のステップ」に相当する。

〈プラントモデル〉
図７は、時計回りの後進旋回時のプラントモデルを模式的に説明する図であり、図８は、反時計回りの後進旋回時のプラントモデルを模式的に説明する図である。本実施形態では、図７および図８に示すように、後進旋回時のプラントモデルとして、前進時の操舵制御と同様に、２輪車モデルを採用している。

もっとも、後進旋回時のプラントモデルでは、ＧＮＳＳアンテナ２７の位置を基準として制御を行う前進時の操舵制御と異なり、ロボットトラクタ１０の制御中心ＣＣを、後輪１５の車軸における車幅方向の中心位置に設定している。ＧＮＳＳアンテナ２７と後輪１５の車軸の中心位置との位置関係は予め走行制御装置３０に記憶されている。走行制御装置３０は、上述の如く、ＧＮＳＳアンテナ２７、基準局６０およびＧＮＳＳ衛星６５等を用いて、ＧＮＳＳアンテナ２７の現在位置を算出し、かかるＧＮＳＳアンテナ２７の現在位置に基づいて、制御中心ＣＣの現在位置を検出するように構成されている。

このように、制御中心ＣＣをロボットトラクタ１０の後輪１５の車軸における車幅方向の中心位置に設定することから、換言すると、ＧＮＳＳアンテナ２７の仮想位置（制御中心ＣＣ）を、後進旋回時の車両中心に設定することから、蛇行の影響をより一層抑えることができる。

なお、請求項との関係では、走行制御装置３０、ＧＮＳＳアンテナ２７、基準局６０およびＧＮＳＳ衛星６５等が、本発明でいうところの「作業車両に設定された制御中心を検出可能な位置検出手段」に相当する。なお、以下では、これらをまとめて「位置検出システム」とも称する。

そうして、本実施形態では、上記（式４）の状態方程式の状態ベクトルｘを、下記の（式１７）のように定義している。

なお、図７および図８に示すように、ｙは、後進旋回用走行経路７５からの制御中心ＣＣの横方向偏差であり、θは、後進旋回用走行経路７５の接線７５’に対する後輪１５のタイヤ角度偏差であり、δは、前輪１３のタイヤ角度である。

ここで、時計回りの後進旋回時における、第ｋステップでの横方向偏差ｙ［ｋ］と第ｋ＋１ステップでの横方向偏差ｙ［ｋ＋１］との関係は、図７を参照して、車速Ｖ、第ｋステップでのタイヤ角度偏差θ［ｋ］、および、予測サンプリング時間ｄｔを用いて、下記の（式１８）のように表すことができる。

また、時計回りの後進旋回時における、第ｋステップでのタイヤ角度偏差θ［ｋ］と第ｋ＋１ステップでのタイヤ角度偏差θ［ｋ＋１］との関係は、図７を参照して、車速Ｖ、ホイールベースＬ、第ｋステップでの前輪１３のタイヤ角度δ［ｋ］、第ｋステップでの曲率ｃ［ｋ］、および、予測サンプリング時間ｄｔを用いて、下記の（式１９）のように表すことができる。

さらに、ステアリングダイナミクスの時定数をτと規定し、第ｋステップでの前輪１３のタイヤ角度の制御量（ステアリング角度）をｕ［ｋ］とすると、第ｋステップでのタイヤ角度δ［ｋ］と第ｋ＋１ステップでのタイヤ角度δ［ｋ＋１］との関係は、図７を参照して、時定数τ、第ｋステップでの制御量ｕ［ｋ］、および、予測サンプリング時間ｄｔを用いて、下記の（式２０）のように表すことができる。

以上の（式１８）～（式２０）をまとめて、時計回りの後進旋回時におけるプラントモデルの運動方程式は、下記の（式２１）のように定義される。

一方、反時計回りの後進旋回時においても、第ｋステップでの横方向偏差ｙ［ｋ］と第ｋ＋１ステップでの横方向偏差ｙ［ｋ＋１］との関係、および、第ｋステップでのタイヤ角度δ［ｋ］と第ｋ＋１ステップでのタイヤ角度δ［ｋ＋１］との関係は、上記（式１８）および（式２０）と同じになる。

これに対し、反時計回りの後進旋回時における、第ｋステップでのタイヤ角度偏差θ［ｋ］と第ｋ＋１ステップでのタイヤ角度偏差θ［ｋ＋１］との関係は、図８を参照して、下記の（式２２）のように表すことができる。

よって、（式１８）、（式２２）および（式２０）をまとめて、反時計回りの後進旋回時におけるプラントモデルの運動方程式は、下記の（式２３）のように定義される。

これら（式２１）および（式２３）で表されるプラントモデルに上記（式４）の状態方程式を適用する場合には、上記（式４）の状態方程式と上記（式２１）のプラントモデルの運動方程式とを見比べれば分かるように、状態方程式における状態係数ベクトルＡ、入力係数ベクトルｂ_１および外乱係数ベクトルｂ_２は、それぞれ下記の（式２４）、（式２５）および（式２６）で規定される。

これら状態係数ベクトルＡ、入力係数ベクトルｂ_１および外乱係数ベクトルｂ_２を、上記（式９）～（式１１）に代入することで、これらを要素とする行列Ｈ_１、行列Ｈ_２および行列Ｈ_３を算出することができる。そうして、これら行列Ｈ_１、行列Ｈ_２および行列Ｈ_３を上記（式１２）に代入したものが、本実施形態の予測モデルとなる。

また、重み行列Ｗ_１および重み行列Ｗ_２は、適宜設定されることから、上記（式１６）に基づいて、最適な制御量を算出することが可能となる。

〈モデル予測制御〉
次に、時計回りの後進旋回を例として、上記予測モデルおよび評価関数Ｊを用いたモデル予測制御について説明する。なお、図９は、モデル予測制御の概念を模式的に説明する図である。また、各制御周期は、予測サンプリング時間ｄｔ＝０．１である。

≪第１制御周期≫
先ず、本実施形態のモデル予測制御は、図６に示すように、直線状走行経路７１ａに沿って前進直進してきたロボットトラクタ１０の制御中心ＣＣが、圃場３のコーナー近傍の枕地５に設定された制御開始位置ＳＰに到達したことが、位置検出システムによって検出されたときに開始される。これにより、ロボットトラクタ１０が直線経路７５ａに沿って後進し始める。

そうして、本実施形態のモデル予測制御では、図９の（Ａ）に示すように、位置検出システム等によって検出されたプラントモデルの毎ステップの観測値ｘ_０を、上記（式１２）の予測モデルに代入して、上記（式１６）から最適な入力系列を算出する。

具体的には、後進旋回開始時（ｋ＝０）における、制御中心ＣＣの横方向偏差ｙ_０は、位置検出システムによって検出されたロボットトラクタ１０の現在位置情報から求めることができる。また、後進旋回開始時（ｋ＝０）における、タイヤ角度偏差θ_０は、ロボットトラクタ１０の位置変化情報から求めることができる。なお、タイヤ角度偏差θ_０は、方位センサ（図示せず）で求めてもよい。さらに、後進旋回開始時（ｋ＝０）における、前輪１３のタイヤ角度δ_０は、舵角センサ４３の検出結果から求めることができる。よって、プラントモデルの毎ステップの観測値であるｘ_０＝［ｙ_０ θ_０ δ_０］^Ｔを算出することができる。

また、外乱系列ベクトルＤ＝［ｄ［１］ ｄ［１］ ｄ［２］…ｄ［Ｎ－１］］^Ｔは、後進旋回用走行経路７５が円弧であり、曲率半径（旋回半径Ｒ）が一定であれば、ｃ［ｋ］＝１/Ｒなので一定となる。

これらにより、上記（式１６）から、５ステップ分の（０．５秒先までの）入力系列ベクトルＵ＝［ｕ［１］ ｕ［２］ ｕ［３］ ｕ［４］ ｕ［５］］^Ｔを算出することができる。もっとも、走行制御装置３０は、算出した第１～第５ステップのうち第１番目のステップの制御量ｕ［１］（ステアリング角度（操舵角））のみをステアリングＥＣＵ３５へ出力し、ステアリングＥＣＵ３５は制御量ｕ［１］に基づいて前輪１３を制御する。それ故、請求項との関係では、走行制御装置３０が、本発明でいうところの「前輪の操舵角の制御量を演算して出力する演算手段」に相当し、ステアリングＥＣＵ３５が、本発明でいうところの「演算手段によって出力された制御量に基づいて前輪を制御する操舵手段」に相当する。

なお、位置検出システムによって検出された制御中心ＣＣの現在位置を、図９の（Ａ）に示すような毎ステップの観測値として用いるのは、第１制御周期だけである。

≪第２制御周期以降≫
第２制御周期では、走行制御装置３０は、制御量に対応する制御中心ＣＣの予測位置と、位置検出システムによって検出された制御中心ＣＣの実測位置とに基づいて、予測モデルを補正する。より詳しくは、走行制御装置３０は、制御量ｕ［１］に基づいて前輪１３が操舵された後に、位置検出システムによって検出された制御中心ＣＣの現在位置と、制御量ｕ［１］に対応する制御中心ＣＣの予測位置との誤差を、図９の（Ｂ）に示すように、外乱系列ベクトルＤ＝［ｄ［１］ ｄ［２］ ｄ［３］ ｄ［４］ ｄ［５］］^Ｔとして予測モデルに反映させて、５ステップ分の入力系列ベクトルＵ＝［ｕ［２］ ｕ［３］ ｕ［４］ ｕ［５］ ｕ［６］］^Ｔを算出する。そうして、走行制御装置３０は、算出した第２～第５ステップのうち第１番目のステップの制御量ｕ［２］のみをステアリングＥＣＵ３５へ出力し、ステアリングＥＣＵ３５は制御量ｕ［２］に基づいて前輪１３を制御する。

同様に、第３制御周期では、外乱系列ベクトルＤ＝［ｄ［２］ ｄ［３］ ｄ［４］ ｕ［５］ ｕ［６］］^Ｔとして予測モデルに反映させて、５ステップ分の入力系列ベクトルＵ＝［ｕ［３］ ｕ［４］ ｕ［５］ ｕ［６］ ｕ［７］］^Ｔを算出し、制御量ｕ［３］のみをステアリングＥＣＵ３５へ出力する。以後、このような制御を継続して行うことで、ロボットトラクタ１０を後進旋回用走行経路７５に沿って走行させる。

このように、本実施形態によれば、予測モデルに基づき０．１（秒）毎に、最適な制御量を５ステップ先まで算出することから、換言すると、５ステップ先を予測した上で最適な制御量を算出することから、制御中心ＣＣの予測位置と実測位置とに基づいて、予測モデルをフィードバック補正する際に、操舵制御が発散するのを抑えることができる。つまり、ロボットトラクタ１０の状態を０．５秒先まで予測した上で、最適な制御量を算出することから、制御中心ＣＣの実測位置とのずれが予測の範囲内であれば、蛇行を生じるような制御量が出力されるのを抑えることができる。したがって、前輪１３が操舵輪であるロボットトラクタ１０を自律走行で後進旋回させる場合にも、ロボットトラクタ１０が蛇行するのを抑えることができる。

〈制御タイミング〉
図１０は、後進直進から後進旋回に切り替える場合、および、後進旋回から後進直進に切り替える場合における、曲率ｃの変化を模式的に説明する図である。上述の如く、前進直進してきたロボットトラクタ１０をαターンさせる場合には、ロボットトラクタ１０を前進直進からいきなり後進旋回させる訳ではなく、直線経路７５ａに沿った後進直進を経てから、円弧経路７５ｂに沿った後進旋回を開始する。このように後進直進から後進旋回へ切り替える場合には、図１０に示すように、曲率ｃが０から１／Ｒへ急激に変化するが、電動パワーステアリング３７にはダイナミクス（動的特性）があるため、不可避的に応答遅れが生じてしまい、旋回のタイミングが遅れることになる。

かかる応答遅れを抑えるために、制御量ｕ［ｋ］を大きくすると、実際の操舵角が大きくオーバーシュートしてしまうおそれがある。なお、このような問題は、円弧経路７５ｂに沿った後進旋回から前進直進または直線経路７５ｃに沿った後進直進へ切り替える場合にも生じ得る。

そこで、本実施形態の自律走行システム１では、後進直進から後進旋回へ切り替える場合、または、後進旋回から直進へ切り替える場合には、後進旋回用走行経路７５における切替え予定位置に到達する前に、切替え後の制御量を出力するように走行制御装置３０を構成している。以下、このような制御について詳細に説明する。

≪後進直進から後進旋回へ切り替える場合≫
先ず、直線経路７５ａの終点Ｅ１（切替え予定位置）では、曲率ｃ＝１／Ｒとなることが好ましい。また、仮にステップ状に曲率ｃを増加させても電動パワーステアリング３７は追従することができないため、終点Ｅ１に到達するまでに曲率ｃを滑らかに増加させるのが好ましい。これらを考慮すると、下記の（式２７）で表される、図１１に示すようなグラフを描くことができる。

なお、（式２７）において、ＡＥＤは、直線経路７５ａ上での終点Ｅ１（＝０）までの距離であり、また、ＸＰ１は、直線経路７５ａ上において曲率ｃの増加を開始する位置であり、どこから曲率ｃを増加させるかに応じて適宜設定される値である。

ここで、速度の横方向成分は微小であると考えて、ｄ（ＡＥＤ）／ｄｔ＝車速Ｖとすると、（式２７）における傾きの時間変化は、下記の（式２８）で表される。

これを予測サンプリング時間ｄｔで離散化（オイラー近似）すると、第ｋ＋１ステップでの曲率ｃ［ｋ＋１］と、第ｋステップでの曲率ｃ［ｋ］との関係は、下記の（式２９）で表される。

そうして、初期値ｃ［０］を下記の（式３０）のように定め、上記（式２９）を用いて予測区間を求める。ただし、曲率ｃ［ｋ］は１／Ｒで飽和させる。

≪後進旋回から直進へ切り替える場合≫
円弧経路７５ｂに沿った後進旋回が終わるまで同じ曲率１／Ｒで旋回し続けると、円弧経路７５ｂの終点Ｅ２から惰性で進むときにロボットトラクタ１０が切れ込み過ぎてしまうおそれがある。

それ故、後進旋回から直進へ切り替える場合には、円弧経路７５ｂの終点Ｅ２（切替え予定位置）では、曲率ｃ＝０となることが好ましい。また、仮にステップ状に曲率ｃを減少させても電動パワーステアリング３７は追従することができないため、終点Ｅ２に到達するまでに曲率ｃを滑らかに減少させるのが好ましい。これらを考慮すると、下記の（式３１）で表される、図１２に示すようなグラフを描くことができる。

なお、（式３１）において、ＡＥＤは、円弧経路７５ｂ上での終点Ｅ２（＝０）までの距離であり、また、ＸＰ２は、円弧経路７５ｂ上において曲率ｃの減少を開始する位置であり、どこから曲率ｃを減少させるかに応じて適宜設定される値である。

ここで、ｄ（ＡＥＤ）／ｄｔ＝車速Ｖとすると、（式３１）における傾きの時間変化は、下記の（式３２）で表される。

これを予測サンプリング時間ｄｔで離散化すると、第ｋ＋１ステップでの曲率ｃ［ｋ＋１］と、第ｋステップでの曲率ｃ［ｋ］との関係は、下記の（式３３）で表される。

そうして、初期値ｃ［０］を下記の（式３４）のように定め、上記（式３３）を用いて予測区間を求める。ただし、曲率ｃ［ｋ］は０で飽和させる。

以上の操作を施すことで、図１２に示すように、後進直進から後進旋回へ切り替える場合には、終点Ｅ１よりも手前で、また、後進旋回から直進へ切り替える場合には、終点Ｅ２よりも手前で、走行制御装置３０が、先を見越して早めに切替え後の制御量を出力することで、オーバーシュートの発生を抑えつつ、操舵手段のダイナミクスに起因する遅れを抑えることができる。

（その他の実施形態）
本発明は、実施形態に限定されず、その精神又は主要な特徴から逸脱することなく他の色々な形で実施することができる。

上記実施形態では、トラクタに本発明を適用したが、これに限らず、例えば、田植機、コンバイン、土木・建築作業装置、除雪車等に本発明を適用してもよい。

また、上記実施形態では、衛星測位システムを用いた測位方法としてＲＴＫを採用したが、これに限らず、例えば、ＤＧＰＳ（Differential GPS）や、基準点を用いない単独測位を採用してもよい。

さらに、上記実施形態では、前進については前方注視方式を採用したが、これに限らず、前進についてもモデル予測制御を採用するようにしてもよい。

このように、上述の実施形態はあらゆる点で単なる例示に過ぎず、限定的に解釈してはならない。さらに、特許請求の範囲の均等範囲に属する変形や変更は、全て本発明の範囲内のものである。

本発明によると、前輪が操舵輪である作業車両を、自律走行で後進旋回させる場合に、作業車両が蛇行するのを抑えることができるので、作業車両の自律走行システムに適用して極めて有益である。

１ 自律走行システム
１０ ロボットトラクタ（作業車両）
１３ 前輪
１５ 後輪
２７ ＧＮＳＳアンテナ（位置検出手段）
３０ 走行制御装置（演算手段）（位置検出手段）
３５ ステアリングＥＣＵ（操舵手段）
５１ａ 走行経路生成部（経路生成手段）
６０ 基準局（位置検出手段）
６５ ＧＮＳＳ衛星（位置検出手段）
７５ 後進旋回用走行経路
ＣＣ 制御中心
Ｅ２ 終点（切替え予定位置）
Ｅ１ 終点（切替え予定位置）
ｕ［ｋ］ ステアリング角度（操舵角）

Claims (3)

1. 前輪が操舵輪である作業車両を自律走行させる作業車両の自律走行システムであって、
   上記作業車両が走行する目標となる走行経路を生成する経路生成手段と、
   上記作業車両に設定された制御中心の位置を検出可能な位置検出手段と、
   上記前輪の操舵角の制御量を演算して出力する演算手段と、
   上記演算手段によって出力された制御量に基づいて前輪を制御する操舵手段と、を備え、
   上記演算手段は、後進旋回時には、上記走行経路からの上記制御中心の偏差と上記操舵角の制御量との相関を状態方程式で表した予測モデルに基づき、所定の制御周期毎に、所定の評価関数を用いて最適な制御量を所定のステップ先まで算出し、第１番目のステップの制御量のみを出力するとともに、当該制御量に対応する上記制御中心の予測位置と、上記位置検出手段によって検出された当該制御中心の実測位置とに基づいて、当該予測モデルを補正するように構成されていることを特徴とする作業車両の自律走行システム。
2. 上記請求項１に記載の作業車両の自律走行システムにおいて、
   上記制御中心は、上記作業車両の後輪の車軸における車幅方向の中心位置に設定されており、
   上記位置検出手段は、上記作業車両に設けられたＧＮＳＳアンテナの現在位置に基づいて、上記制御中心の現在位置を検出するように構成されていることを特徴とする作業車両の自律走行システム。
3. 上記請求項１または２に記載の作業車両の自律走行システムにおいて、
   上記演算手段は、後進直進から後進旋回へ切り替える場合、または、後進旋回から直進へ切り替える場合には、上記走行経路における切替え予定位置に到達する前に、切替え後の制御量を出力するように構成されていることを特徴とする作業車両の自律走行システム。

Images