(57)【特許請求の範囲】
【請求項１】
目標走行経路に沿って作業路面を自動走行するように作業用車両を操舵する作業用車両の自動操舵システムであって、  車両の位置、速度、方位及び姿勢を含む計測値を計測する計測部と、  処理部と、を備え、  前記処理部は、車両特徴量を演算する車両特徴量演算部と、路面特徴量を演算する路面特徴量演算部と、車両特徴量と路面特徴量とを用いて操舵量を演算する操舵量演算部と、を備え、  自動操舵システムは、作業路面を走行するときの作業モードと、作業モードの事前に行う調整モードと、を備え、  調整モードにおいて、前記車両特徴量演算部は、前記計測部によって得られた計測値を用いて車両特徴量を求め、  作業モードにおいて、前記車両特徴量演算部は前記車両特徴量の更新を行い、前記路面特徴量演算部は、前記計測部によって得られた計測値を用いて路面特徴量を求める、  ことを特徴とする作業用車両の自動操舵システム。
【請求項２】
前記車両特徴量は、操舵量−旋回半径関係、車体傾き及び車体自身の車体振動の少なくとも１つを含むことを特徴とする請求項１記載の作業用車両の自動操舵システム。
【請求項３】
前記路面特徴量は、路面傾き及び路面の滑りやすさの少なくとも１つを含むことを特徴とする請求項１または２に記載の作業用車両の自動操舵システム。
【請求項４】
前記処理部は、さらに、路面特徴量演算部で求めた路面特徴量を閾値と比較して、閾値を超えた場合に警告信号を出力する危険警告手段を備えることを特徴とする請求項１ないし３のいずれか１項に記載の作業用車両の自動操舵システム。
【発明の詳細な説明】
【技術分野】
【０００１】  本発明は、作業路面を走行する作業用車両の位置、速度、姿勢等を検出して、自動操舵を行う作業用車両の自動操舵システムに関する。
【背景技術】
【０００２】  従来、この種の作業用車両の自動操舵としては、例えば、特許文献１に記載された農用自律走行車両の操舵方法が知られている。この操舵方法では、地磁気方位センサやジャイロなどの車両走行方位センサにより走行方位を検出し、目標方位との偏差値を確認して車両が常に目標方位に向って走行するように自立走行車両の操舵機構を制御している。【０００３】  また、特許文献２では、自動田植機に磁気センサまたはジャイロ等の方位センサを備えており、教習行程で目標方位を習得すると共に、自律走行行程では、教習行程で習得した目標方位に自動田植機が進行方位をとるように自動操舵装置で操縦するようになっており、さらに、現在取得した方位データの平均処理をした平均方位を目標方位として更新していくことで、往復直進走行の平行性を向上できるようにしている。【０００４】  また、特許文献３では、トラクタの無人運転において、光ファイバジャイロ及び地磁気方位センサによって時々刻々得られる位置情報と方位情報に基づいて、予め設定した作業経路上の目標位置及び目標方位に対する偏差を算出し、この偏差をなくすように、トラクタを操向する操舵角を求めて時々刻々の舵角制御を行っている。
【先行技術文献】
【特許文献】
【０００５】
【特許文献１】特開平７−１８４４１１号公報
【特許文献２】特開２００３−４４１３６号公報
【特許文献３】特開平１０−６６４０５号公報
【発明の概要】
【発明が解決しようとする課題】
【０００６】  しかしながら、以上のような作業用車両が走行する路面は、平坦な舗装道路とは限らず、凹凸、傾斜、または水、草、木、その他の異物の存在といった様々な路面環境にあることが多く、そのような路面環境によって発生する車両の姿勢の変化、振動が自動操舵に大きな影響を及ぼすという問題がある。【０００７】  また、様々な路面環境における長期に渡る運用が想定される作業用車両は、経年変化や車両個体差が大きく、これらが自動操舵に大きな影響を及ぼすという問題がある。【０００８】  本発明はかかる課題に鑑みなされたもので、様々な路面環境や、経年変化、車両個体差といった要因の影響を低減して、自動操舵を安定的に行うことができる自動操舵システムを提供することをその目的とする。
【課題を解決するための手段】
【０００９】  上記目的を達成するために、本発明は、目標走行経路に沿って作業路面を自動走行するように作業用車両を操舵する作業用車両の自動操舵システムであって、  車両の位置、速度、方位及び姿勢を含む計測値を計測する計測部と、  処理部と、を備え、  前記処理部は、車両特徴量を演算する車両特徴量演算部と、路面特徴量を演算する路面特徴量演算部と、車両特徴量と路面特徴量とを用いて操舵量を演算する操舵量演算部と、を備え、  自動操舵システムは、作業路面を走行するときの作業モードと、作業モードの事前に行う調整モードと、を備え、  調整モードにおいて、前記車両特徴量演算部は、前記計測部によって得られた計測値を用いて車両特徴量を求め、  作業モードにおいて、前記車両特徴量演算部は前記車両特徴量の更新を行い、前記路面特徴量演算部は、前記計測部によって得られた計測値を用いて路面特徴量を求める、  ことを特徴とする。【００１０】  前記車両特徴量は、操舵量−旋回半径関係、車体傾き及び車体自身の車体振動の少なくとも１つを含むことができる。【００１１】  前記路面特徴量は、路面傾き及び路面の滑りやすさの少なくとも１つを含むことができる。【００１２】  前記処理部は、さらに、路面特徴量演算部で求めた路面特徴量を閾値と比較して、閾値を超えた場合に警告信号を出力する危険警告手段を備えることができる。
【発明の効果】
【００１３】  本発明によれば、作業路面を走行するときの作業モードと、作業モードの事前に行う調整モードと、を備え、調整モードにおいて車両特徴量を求め、作業モードにおいて車両特徴量の更新を行うと共に、路面特徴量を求めることにしたので、車両特徴量と路面特徴量とを分離してそれぞれを正確に求めることができるようになる。これによって、車両の経年変化や個体差を車両特徴量で把握し、路面の影響を路面特徴量で把握して、これらの影響を加味した上で操舵量を求めることにより、様々な路面環境や、経年変化、車両個体差といった要因の影響を低減して、自動操舵を安定的に行うことができる。
【図面の簡単な説明】
【００１４】
【図１】本発明の実施形態による自動操舵システムの全体図である。
【図２】図１の自動操舵システムの部分ブロック図である。
【図３】調整モード時を表すブロック図である。
【図４】作業モード時を表すブロック図である。
【図５】目標走行経路の一例である。
【図６】車体または路面の傾きと、ＧＰＳアンテナの取付位置と車両の位置との関係を表す図である。
【発明を実施するための形態】
【００１５】  以下、図面を参照しながら、本発明の実施形態について説明する。
【００１６】  図１に示すように、本実施形態による自動操舵システム１０は、作業用車両１００に搭載される。作業用車両１００としては、農耕、土木、伐採、芝刈り、その他の作業用トラクタ、建設機器等とすることができ、作業用車両１００は、舗装路面に限らず、作業を実行するために作業場である圃場、森林、未舗装道路の、凹凸、傾斜、または水、草、木その他の異物が存在するような様々な路面（作業路面と称する）を走行する。自動操舵システム１０は、かかる作業用車両の運転の負担を軽減するために作業用車両１００に搭載され、作業用車両１００の自動操舵を行う。
【００１７】  自動操舵システム１０は、大まかに、車両特徴量及び路面特徴量を更新し、操舵量を演算する主処理部１２と、車両の位置、速度、姿勢等の運動情報等を検出する計測部１４と、車両が走行するべき目標走行経路を入力または演算により決定し、それを出力する目標走行経路決定部１６と、ユーザからの入力及びユーザへの出力を行うユーザインターフェース１８と、主処理部１２から得られた操舵量に基づき車両のハンドル２００を制御するステアリング制御機構２０と、を備える。
【００１８】  図２に示したように、主処理部１２は、車両特徴量を演算する車両特徴量演算部１２２と、路面特徴量を演算する路面特徴量演算部１２４と、操舵量を演算する操舵量演算部１２６と、加速度振動演算部１２８と、角速度振動演算部１３０と、危険警告部１３６と、自動操舵停止手段１３８と、を備える。これらの詳細については後述する。
【００１９】  計測部１４は、複数の計測部を有しており、衛星航法システムを構成するＧＰＳアンテナ及びＧＰＳ受信機を備え、車両の位置、速度、進行方向を計測する位置・速度計測部１４１と、３軸回りの角速度をそれぞれ検出するジャイロ、３軸方向の加速度をそれぞれ検出する加速度計及び磁気方位角を検出する磁気方位センサを備え、車両の加速度、角速度、姿勢、方位を計測する姿勢・方位計測部１４２と、車両のハンドル２００の回転角を計測するロータリエンコーダからなるハンドル回転角センサ１４４と、を備え、主処理部１２へと送られて格納・使用される。これら位置・速度計測部１４１と姿勢・方位計測部１４２とは、必要に応じて互いの計測値のやりとりをして補正・補間を行い計測値の精度を高めることができる。
【００２０】  目標走行経路決定部１６は、作業用車両が作業場を走行するべき目標走行経路を決定する。目標走行経路は、直線または曲線のいずれともすることができ、図５に示すように、１つ以上の直線または曲線からなる１つ以上の区間から構成することができる。複数の区間は、連続してもまたは非連続であってもよい。直線区間の場合には、少なくとも始点と終点を含む複数のウエイポイントＷＰの位置で特定することができる。また、曲線区間の場合には、少なくとも始点と終点を含む複数のウエイポイントＷＰの位置、曲線の種類（円弧、クロソイド等）、曲率半径等で特定することができる。
【００２１】  ユーザインターフェース１８は、ユーザからの既知の車両情報等の入力を受け付けると共に、ユーザに対して危険発生時の警報等を視覚的または音響的に出力することができる。入力された車両情報等は、主処理部１２へと送られて格納・使用され、車両特徴量演算部１２２において車両特徴量の初期値または初期値の導出に使用される。
【００２２】  ステアリング制御機構２０は、前記操舵量演算部１２６で演算された操舵量に基づきモータドライバを介して駆動される電動モータ等を備え、伝達機構を介して電動モータからの回転をハンドル２００またはステアリングロッドに伝達し、演算された操舵量だけ回転させるようになっている。
【００２３】  自動操舵システム１０は、「調整モード」と「作業モード」といったモードを備えている。調整モードは、作業モードの事前に行われ、図３に示すブロック図に示すように、車両を動作させ、作業場またはそれ以外の場所の可能な限り平坦な路面において、マニュアル走行または自動操舵による走行を行い、計測部１４からの計測値を車両モデルに適用し、車両モデルを更新して、車両特徴量ccを求める。この調整モードの際に、少なくとも右回り及び左回りのステアリングをそれぞれ行い、車両を右旋回及び左旋回させるようにする。この時、左右回りにそれぞれ限界までステアリングを行うことで特徴量の推定精度を向上させることができる。
【００２４】  また、作業モードは、図４に示すブロック図に示すように、実際の作業路面の自動操舵制御による走行である。作業モードでは、車両を動作させ、作業場で自動操舵を行い、計測部１４からの計測値を車両モデルに適用し、車両モデルを更新して、車両特徴量ccを求めると共に、路面モデルに適用し、路面モデルを更新して、路面特徴量crを求める。
【００２５】  以下、具体的に説明する。主処理部１２では、例えば以下の変数を取り扱う。
【００２６】
【表１】
0005953426.tif 000002
【００２７】(1)  車両特徴量  車両特徴量演算部１２２は、作業場での作業中に大きく変化しない車両の状態等を表す特徴量である車両特徴量を求める。よって、車両特徴量は、経年変化または車両個体差を特徴づけるものとなる。
【００２８】  具体的には、作業前の調整モードにおいて計測部１４によって計測された計測値に基づき車両特徴量の初期値を車両モデルに適用し、車両モデルを更新する。また、作業モードにおいてその車両特徴量の更新を適宜行う。作業前の調整モードにおいて車両特徴値を決定することで、走行中の更新量を小さくして安定した特性を得る。
【００２９】  車両特徴量として具体的には、ハンドル回転角（操舵量）−旋回半径関係、車両傾き、車両自身の運動による車体振動等を例示することができる。
【００３０】(1-1)  ハンドル回転角（操舵量）−旋回半径関係  ハンドル回転角と旋回半径の関係、すなわち、車両のハンドルを回転させた場合に標準状態でどの程度旋回するかを表す特徴量であり、以下の４つの特徴量を挙げることができる。
【００３１】(1-1-1)  ハンドル回転角−タイヤ回転角比：g=(glgr)  （次元：なし）  ハンドル回転角とタイヤ回転角との比であり、右回りと左回りに対してそれぞれ存在する。
【００３２】(1-1-2)  ホイールベース：l  （次元：m）  前輪軸と後輪軸の距離である。初期値はユーザ等によってユーザインターフェース１８に入力された入力値，あるいは標準値とする。
【００３３】  上記２つの特徴量は以下の手順により同時に求められる。
【００３４】  調整モードにおいて入力されたハンドル回転角と旋回半径から次式にて導出する。旋回半径は、位置・速度計測部１４１で計測される車両の位置の軌跡から求める。左右について、同様に求めることができる。      g=α/(sin-1(l/R))
【００３５】  作業モードにおいては、入力した複数のハンドル回転角とそれにより計測された旋回半径による多項式を基に値を更新する。第一、第二のハンドル回転角と旋回半径をそれぞれα1,α2,R1,R2とした時、推定ハンドル−タイヤ回転角比gx、推定ホイールベースlxについて以下の２式が立つ。      R1=lx/(sin(α1/gx))      R2=lx/(sin(α2/gx))
【００３６】  上記、２式より、推定ハンドル−タイヤ回転角比gx、推定ホイールベースlxが求められる。これらの値と現在値の誤差に応じて、それぞれの値を更新する。ここで、εg、εlは更新量である。      g[n]=g[n-1]+εg(gx-g[n-1])      l[n]=l[n-1]+εl(lx-l[n-1])
【００３７】(1-1-3)  ハンドル回転限界（操舵量限界）：αRr, αRl  （次元：rad）  ハンドルの右回転限界と左回転限界の角度を表す特徴量である。調整モードにおいてハンドル左右回転限界までハンドル２００を操作し、その際のハンドル回転角センサ１４４のハンドル回転角の計測値から得る。
【００３８】  作業モード中に、計測したハンドル回転角がハンドル回転限界を超えた場合には値を更新する。
【００３９】  この右回転限界と左回転限界から、その中点となるハンドルの原点の基準値を決定することができる。
【００４０】(1-1-4)  ハンドル非対称性（操舵非対称性）：β  （次元：rad）  ハンドルの原点が、左右ハンドル回転限界の中点からずれている量を表す特徴量である。初期値は０とする。
【００４１】  本来、ハンドルは対称であるので、平坦な一様路面で車両が直進をしている場合には、ハンドルは左右の回転限界の中点であるはずであるが、経年変化または個体誤差によって、ハンドルの原点がずれている場合がある。
【００４２】  作業モード中に、位置・速度計測部１４１より求められる車両位置から一定時間直進が認められるにも関わらず、ハンドル回転角センサ１４４で計測されるハンドル回転角が原点からずれている場合にその誤差に応じて、原点を更新する。ただし、傾斜による横滑りに対向するハンドル回転角は誤差から除外する。
【００４３】        β=( Σiα[i])/I-αC  ここで、iは直進が認められた範囲のサンプルを表し，その総数をIとする。α[i]はi時点のハンドル回転角の計測値、αCは後述する、(3-4)斜面横滑り量C分の位置ずれを補正するために必要なハンドル回転角である。
【００４４】(1-2)  車体傾き：ba=( bφ bθ)  （次元：(rad rad)）  タイヤの圧力や取付位置の歪み等により常に車体にかかる傾きを表す特徴量であり、ロール角とピッチ角がある。この特徴量の導出は、調整モードにおいてその計測区間内に姿勢・方位計測部１４２で計測されるロール角、ピッチ角の平均を求めることで行う。
【００４５】      ba=(Σia[i])/I  ここで、iは調整モード中の全サンプルを表し、その総数をIとする。a[i]はi時点の姿勢計測値である。この特徴量は、危険警告部１３６において、閾値と比較することができ、閾値を超えた場合、危険警告部１３６が警告信号を出力し、ユーザインターフェース１８で車両・タイヤ取付に問題がある旨の警告を視覚的又は音響的に出力して、ユーザに知らしめる。
【００４６】  作業モード中には車体傾きと現在の傾きの誤差に応じて更新する。      ba[n]=ba[n-1]+εa(a-ba[n-1])  ここで、nは更新時を示し、εaは更新量である。
【００４７】  この特徴量は、計測部１４にフィードバックされて、センサの取付位置の補正に使用されることができる。即ち、図６（ａ）に示すように、位置・速度計測部１４１で位置を求めるＧＰＳアンテナの取付位置を地面に投影した位置Ｐ１と、車両の位置Ｐ２（通常は車両の幅方向中心位置、例えば、左右前輪中心または左右後輪中心）との間の偏差は、車体傾きの影響を受けるために、得られた車体傾きによって位置・速度計測部１４１から出力される位置を補正することができる。
【００４８】(1-3)  車両自身の運動による車体振動：bv[k]=(bAx[k] bAy[k] bAz[k] bΩx[k] bΩy[k] bΩz[k])  （次元：(m/s2 m/s2 m/s2 rad/s rad/s rad/s)  エンジンやタイヤの凹凸などにより常に車体にかかる振動を表す特徴量である。この特徴量の導出は、調整モードにおいてその計測区間内の全振動の平均を求めることで行う。
【００４９】      bv[k]= (ΣiV[i][k])/I  ここで、iは調整モード中の全サンプルを表し、その総数をIとする。V[i][k]はi時点の車体振動である。この特徴量は、危険警告部１３６において、閾値と比較することができ、閾値を超えた場合、危険警告部１３６が警告信号を出力し、ユーザインターフェース１８で車両・タイヤ取付に問題がある旨の警告を視覚的又は音響的に出力して、ユーザに知らしめる。
【００５０】  また、作業モード中は、車両自身の運動による車体振動と現在の車体振動の誤差に応じて車体振動を更新する。      bv[n][k]=bv[n][k]+εbv(V[k]-bv[n][k])  ここで、nは更新時を示し、εbvは更新量である。
【００５１】(2)  路面特徴量  路面特徴量演算部１２４は、作業場での作業中に常に変化する路面の状態を表す特徴量である路面特徴量を求める。具体的には、作業モードにおいて、計測部１４によって計測された計測値に基づき路面特徴量を路面モデルに従って演算することで、走行中の状態に応じて即座に適応して速応性を高める。
【００５２】  路面特徴量として具体的には、路面傾き、路面傾きの変化量、計測滑りやすさ、路面状態による車体による車体振動、推定滑りやすさ、推定される不安定性、斜面横滑り量等を例示することができる。
【００５３】(2-1)  路面傾き：ha =( hφhθ )  （次元：(rad rad)）  土地の起伏による車体の傾斜を表す特徴量であり、ロール角とピッチ角がある。この特徴量の導出は、姿勢・方位計測部１４２で計測されるロール角、ピッチ角から(1-2)で求めた車体傾きbaを減ずる。      ha=a-ba
【００５４】  この特徴量は、車体傾きの場合と同様に、計測部１４にフィードバックされて、センサの取付け位置の補正に使用されることができる。即ち、図６（ｂ）に示すように、位置・速度計測部１４１で位置を求めるＧＰＳアンテナの取付位置を地面に投影した位置Ｐ１と、車両の位置Ｐ２（通常は車両の幅方向中心位置、例えば、左右前輪中心または左右後輪中心）との間の偏差は、路面傾きの影響を受けるために、得られた路面傾きによって位置・速度計測部１４１から出力される位置を補正することができる。
【００５５】(2-2)  路面傾きの変化量：Δha =( Δhφ Δhθ )  （次元：(rad rad)）  土地の起伏による車体の傾斜の変化量を表す特徴量であり、ロール角とピッチ角のそれぞれの変化量がある。この特徴量の導出は、路面傾きの前時点サンプルと現時点サンプルの差分から得る。      Δha[n]= ha[n]- ha[n-1]  ここで、nは現時点を表す。
【００５６】  この特徴量は、危険警告部１３６において、閾値と比較することができ、閾値を超えた場合、危険警告部１３６が警告信号をユーザインターフェース１８に出力し、ユーザインターフェース１８で危険がある旨の警告を視覚的又は音響的に出力してユーザに知らしめる。また、自動操舵停止手段１３８において自動操舵の停止を行うことができる。
【００５７】(2-3)  計測滑りやすさ：SR  （次元：なし）  計測される車両の旋回半径と、所望の旋回半径の比率を表す特徴量である。期待される旋回半径に対して実際の旋回半径との差異を表す。この特徴量の導出は、位置・速度計測部１４１で求められる車両の位置の軌跡から求めた旋回半径の計測値を、所望の旋回半径で除することで得る。      SR=Rm/R  ここで、Rmは旋回半径の計測値であり、Rは制御しているハンドル回転角によって得られるべき所望の旋回半径である。よって、所望の旋回半径を計測滑りやすさで補正した後、その補正旋回半径に対応するハンドル操作量を求めることで、所望の旋回半径を得ることができる。
【００５８】(2-4)  路面状態による車体振動：gv[k]=(gAx[k] gAy[k] gAz[k] gΩx[k] gΩy[k] gΩz[k]) （次元：(m/s2 m/s2m/s2rad/s rad/s rad/s)）  路面の凹凸などにより一時的に車体にかかる振動を表す特徴量である。
【００５９】  この特徴量は、姿勢・方位計測部１４２から出力される３軸加速度及び３軸角速度の一定時間前までのパワースペクトルを加速度振動演算部１２８及び角速度振動演算部１３０において求め、計測値から(1-3)で求めた車両自身の運動による車体振動を減ずる。      gv[k]=V[k]- bv[k].  以下の(2-5)の推定滑りやすさの導出、(2-6)の推定不安定性の導出に使用する。
【００６０】(2-5) 推定滑りやすさ ：Sv  （次元：なし）  路面状態による車体振動から推定される路面の滑りやすさを表す特徴量である。砂利地等で高くなる。
【００６１】  この特徴量は、路面状態による車体振動の中から高周波成分を抽出することにより求める。      Sv=ζΣkWS[k] gv[k]  ここで、WS[k]は高周波成分を抽出するための窓関数であり、Σkは全帯域の値を総和することを表す。また、ζは高周波成分量から滑りやすさに変換するパラメータである。以下の(2-7)の斜面横滑り量の導出に使用する。
【００６２】(2-6) 推定される不安定性：Dv  次元：なし  路面状態による車体振動から推定される路面の不安定性を表す特徴量である。凸凹した荒地などほど高くなる。
【００６３】  この特徴量は、路面状態による車体振動の中から低周波成分を抽出することにより求める。      Dv=ΣkWD[k] gv[k]  ここで、WD[k]は低周波成分を抽出するための窓関数であり、Σkは全帯域の値を総和することを表す。この特徴量は、危険警告部１３６において、閾値と比較することができ、閾値を超えた場合、危険警告部１３６が警告信号をユーザインターフェース１８に出力し、ユーザインターフェース１８で危険がある旨の警告を視覚的又は音響的に出力してユーザに知らしめる。また、自動操舵停止手段１３８において自動操舵の停止を行うことができる。
【００６４】(2-7) 斜面横滑り量：C  （次元：m）  傾斜と滑りやすさから推定される横滑り量を表す特徴量である。この特徴量は、路面傾きと滑りやすさを乗ずることにより求める。      C= Sv hφ  この斜面横滑り量Ｃは、以下のハンドル回転角の算出に使用する。
【００６５】(3)  ハンドル回転角  最終的な車両の自動操舵の目標は、    ・車両を目標走行経路上に走行させる。    ・車両を目標方向に向けさせる。ことである。
【００６６】  これらを満たすため、操舵量演算部１２６は、以上の特徴量から操舵量であるハンドル回転角を、以下の手順で求める。
【００６７】(3-1)  目標走行経路の決定  目標走行経路決定部１６で決定された目標走行経路が操舵量演算部１２６へと入力され、計測部１４で求めた現在車両位置から目標走行経路のうちの現在の区間を特定する。
【００６８】(3-2)  目標進行方向の決定  操舵量演算部１２６は、特定された目標走行経路の現在の区間と、計測部１４で求めた現在車両位置との位置偏差wを求める。
【００６９】  この位置偏差wは、例えば、直線区間であれば、現在車両位置から目標走行経路に対する垂線に沿って計った現在車両位置と目標走行経路との距離とすることができる。または、直線区間の終点を通り区間に直交する直線と、現在車両位置との距離を求め、その距離に対応する直線区間の終点手前の位置と、現在車両位置との距離とすることができる。
【００７０】  曲線区間であれば、区間始点に対応する地点を通過した時刻から現時刻までの時間で車両の速度を積分することにより距離を求め、その距離に相当する曲線区間の始点から地点（この地点を「目標位置」とする）と、現在車両位置との距離とすることができる。
【００７１】  目標進行方向θwは目標走行経路の目標方位θtに偏差をw[m]の比例定数aw倍した値を加えて求める。すなわち、      θw=θt+awwである。ここで、比例定数awは目標走行経路への収束速度を決める定数であり、安全性等を考慮して決定する値である。また、awwはその値の上限を９０[deg]とする。
【００７２】  目標方位は、目標走行経路が直線区間であれば直線の方位であり、曲線区間であれば、上記目標位置における接線の方位とすることができる。
【００７３】(3-3)  旋回半径の決定  次に、操舵量演算部１２６は、目標進行方向θwと、位置・速度計測部１４１によって計測された車両進行方向θwの偏差θdから車両の所望の旋回半径を決定する。
【００７４】  この時、所望の旋回半径Rはθdに反比例する。すなわち、      R=ad/θdである。ここで、比例定数adは目標進行方向への収束速度を決める定数であり、安全性等を考慮して決定する値である。また、Rは車両の特性による制約を受ける（一定以上小さなRは取れない）。
【００７５】(3-4)  ハンドル回転角αの決定  所望の旋回半径に対するハンドル回転角は次式(右旋回の場合)で求めることができる。      α=gRSin-1(SR/R)-β+αC  ただし、αがαRrを超える場合にはαをαRrにする。
【００７６】  ここで、αCは、斜面横滑りC分の位置ずれを補正するために必要なハンドル回転角である。斜面横滑り量Ｃがある場合、この斜面横滑り量Ｃによる位置ずれを補正するために必要なハンドル回転角を求めて差し引く。必要なハンドル回転角は、上記(3-2)、(3-3)に準じて行うことができ、      αC=gRSin-1(SR  awC/ ad)から求めることができる。
【００７７】  左旋回の場合も同様に求めることができる。
【００７８】  以上によって求められたハンドル回転角αをステアリング制御機構２０に出力して自動操舵を行うことができる。また、危険警告部１３６によって危険が検出されたときには、自動操舵停止手段１３８が自動操舵を自動解除することで、危険を回避することができる。自動解除によって、例えばステアリング制御機構２０の電動モータのトルクを０にすることができる。または、自動操舵停止手段１３８は、車両を停止させる信号を出力してもよい。危険警告部１３６が危険がどうかを検出するために使用する各閾値は、固定値としてもよいし、または可変値としてもよい。
【００７９】  こうして、路面や車両の状態が変化する場合でも、目標走行経路に車両を誘導することができる。
【符号の説明】
【００８０】１０  自動操舵システム１２  主処理部１４  計測部１００  作業用車両１２２  車両特徴量演算部１２４  路面特徴量演算部１２６  操舵量演算部１３６  危険警告部
【図１】
0005953426.tif 000003
【図２】
0005953426.tif 000004
【図３】
0005953426.tif 000005
【図４】
0005953426.tif 000006
【図５】
0005953426.tif 000007
【図６】
0005953426.tif 000008