(57)【要約】【課題】  ＤＧＰＳのような低コストセンサ類を使用して車両を自律走行させ、その走行誤差を小さくすることができる車両自律走行制御システムを提供する。【解決手段】  ＧＰＳ受信機201による測位信号の差分によって方位角を演算し、左右の駆動輪の回転数に基づいて速度と走行距離を演算し、これらと目標経路との比較によって方位角誤差、走行誤差を演算し、求めた方位角誤差と走行誤差に基づいて左右の駆動輪の操作量を演算して左右駆動輪の回転速度を増減制御する。【選択図】      図２
2005250696.tif 000002
【特許請求の範囲】
【請求項１】
ＧＰＳ受信機による受信信号によって絶対位置情報を取得するＧＰＳ測位手段と、
前記ＧＰＳ測位手段の取得した絶対位置情報の差分によって方位角を演算する方位角演算手段と、
左右の駆動輪の回転数に基づいて速度と走行距離を演算する速度・距離演算手段と、
目標経路の方位角と走行距離を設定する目標経路設定手段と、
前記目標経路の方位角と前記方位角算定手段の算出する方位角との誤差を演算する方位角誤差演算手段と、
前記方位角演算手段の算出する方位角と前記速度・距離演算手段の算出する走行距離とに基づいて現在位置を特定し、前記目標経路との距離偏差である走行誤差を演算する走行誤差演算手段と、
前記方位角誤差と走行誤差に対して前記目標経路に一致するのに必要な左右の駆動輪の操作量を演算して左右駆動輪の回転速度を増減制御する駆動制御手段とを備えた車両自律走行制御システム。
【請求項２】
地磁気を感知して進行方向の方位角を算定する地磁気方位角センサと、
前記方位角演算手段は、あらかじめ設定されている所定のタイミングに前記ＧＰＳ測位手段の検出信号に基づく方位角を前記地磁気方位角センサによる方位角で補正する方位角補正手段とを備えたことを特徴とする請求項１に記載の車両自律走行制御システム。
【請求項３】
車両のＵターン時に前記走行誤差をリセットする走行誤差リセット手段を備えたことを特徴とする請求項１又は２に記載の車両自律走行制御システム。
【請求項４】
前記ＧＰＳ受信機は、ＤＧＰＳ受信機であることを特徴とする請求項１〜３のいずれかに記載の車両自律走行制御システム。
【請求項５】
ＧＰＳ受信機による受信信号によって絶対位置情報を取得するＧＰＳ測位手段と、
前記絶対位置情報に基づいて車両の走行距離と方位角を演算する距離・方位角演算手段と、
車両の方位角を検出する方位角検出手段と、
車両の速度を検出する速度検出手段と、
車両の転回角速度を検出する角速度検出手段と、
前記速度検出手段の検出する速度から走行距離を演算する走行距離演算手段と、
目標経路の方位角と走行距離を設定する目標経路設定手段と、
前記距離・方位角演算手段の算出する走行距離と方位角、前記方位角検出手段の検出する方位角、前記速度検出手段の検出する速度、前記走行距離演算手段の算出する走行距離、前記角速度検出手段の検出する転回角速度を入力とし、前記目標経路設定手段の設定する目標経路上の該当位置と比較して走行誤差推定値と方位角誤差推定値を演算して出力するカルマンフィルタと、
前記走行誤差推定値と方位角誤差推定値に対して前記目標経路に一致するのに必要な左右の駆動輪の操作量を演算して左右駆動輪の回転速度を増減制御する駆動制御手段とを備えた車両自律走行制御システム。
【請求項６】
前記カルマンフィルタは、拡張カルマンフィルタであり、かつＪｏｓｅｐｈの安定化アルゴリズムを用いて推定値を得るものであることを特徴とする請求項５に記載の車両自律走行制御システム。
【請求項７】
前記ＧＰＳ測位手段は、ＤＧＰＳ受信機であることを特徴とする請求項５又は６に記載の車両自律走行制御システム。
【請求項８】
ＧＰＳ測位手段の取得する絶対位置情報の差分によって方位角を演算する方位角演算ステップと、
左右の駆動輪の回転数に基づいて速度と走行距離を演算する速度・距離演算ステップと、
目標経路の方位角と走行距離を設定する目標経路設定ステップと、
前記目標経路の方位角と前記方位角演算ステップで算出する方位角との誤差を演算する方位角誤差演算ステップと、
前記方位角演算ステップで算出する方位角と前記速度・距離演算ステップで算出する走行距離とに基づいて現在位置を特定し、前記目標経路との距離偏差である走行誤差を演算する走行誤差演算ステップと、
前記方位角誤差と走行誤差に対して前記目標経路に一致するのに必要な左右の駆動輪の操作量を演算して左右駆動輪の回転速度を増減制御する駆動制御ステップとを有する車両自律走行制御方法。
【請求項９】
ＧＰＳ受信機による受信信号によって絶対位置情報を取得するＧＰＳ測位ステップと、
前記絶対位置情報に基づいて車両の走行距離と方位角を演算する距離・方位角演算ステップと、
車両の方位角を検出する方位角検出ステップと、
車両の速度を検出する速度検出ステップと、
車両の転回角速度を検出する角速度検出ステップと、
前記速度検出ステップで検出する速度から走行距離を演算する走行距離演算ステップと、
目標経路の方位角と走行距離を設定する目標経路設定ステップと、
カルマンフィルタに前記距離・方位角演算ステップで算出する走行距離と方位角、前記方位角検出ステップで検出する方位角、前記速度検出ステップで検出する速度、前記走行距離演算ステップで算出する走行距離、前記角速度検出ステップで検出する転回角速度を入力し、前記目標経路設定ステップで設定した目標経路上の該当位置と比較して走行誤差推定値と方位角誤差推定値を演算して出力するフィルタリングステップと、
前記走行誤差推定値と方位角誤差推定値に対して前記目標経路に一致するのに必要な左右の駆動輪の操作量を演算して左右駆動輪の回転速度を増減制御する駆動制御ステップとを有する車両自律走行制御方法。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、車両自律走行制御システム及び方法に関する。
【背景技術】
【０００２】
農林水産省農林業センサスの調査によると、平成１２年度の道内農業事業体数は６９，８４１戸となっており，１０年前と比較して２７％減少している。また、後継者のいない農家の割合も７割に達し，早急なる農作業の自動化が求められている。
【０００３】
本発明者らは先に、ＲＴＫ−ＧＰＳ（ＲｅａｌＴｉｍｅ  Ｋｉｎｅｍａｔｉｃ  ＧＰＳ）及び光ファイバジャイロスコープ（Ｆｉｂｅｒ  Ｏｐｔｉｃａｌ  Ｇｙｒｏｓｃｏｐｅ。以下「ＦＯＧ」と表記する。）を用い、作業計画経路に沿って自律走行し、所定の農作業を行う作業支援型自律走行履帯トラクタを開発した（非特許文献１）。
【０００４】
同車両は、最大±５ｃｍの走行誤差（目標経路からのズレ量）で作業計画経路に追従走行可能、すなわち走行経路を畝列に設定しても、作物を踏破することなく自律走行可能である。しかし、走行制御用センサとして高価なＲＴＫ−ＧＰＳ及びＦＯＧを使用したため、農耕車両そのものの代価に対してセンサコストが大きな比率を占めることになり、実用化に当たりコスト面で課題があった。
【０００５】
一方、上記の作業支援型自律走行履帯トラクタに単純にＤＧＰＳ（Ｄｉｆｆｅｒｅｎｔｉａｌ  ＧＰＳ）のような低コストセンサを搭載させた場合、その測定誤差が大きいために許容範囲の走行誤差で作業計画経路に追従走行させ、自律走行させることが困難である問題点があった。
【０００６】
この問題点を解決するためには、ＤＧＰＳの検出信号に対して適切な信号処理を施した後、走行制御に使用する必要がある。特にＤＧＰＳは捕捉衛星数が変化した時に出力値が急激に変化するため、走行制御用位置センサとして使用する場合、他のセンサ情報を利用して出力値の不連続性を平滑化する必要があった（非特許文献２，３）。
【非特許文献１】中西洋介他９名著、「農作業車両自律走行作業支援システムの開発」、北海道立工業試験場報告Ｎｏ．３００、４３〜５１ページ、２００１年。
【非特許文献２】野口昇他４名著、「ＮＥＤＯ地域コンソーシアムで開発したロボットトラクタ（第２報）−動的経路生成法−」、第６０回農業機械学会年次大会講演要旨、２６５−２６６ページ、２００１年。
【非特許文献３】井上慶一他４名著、「自律走行のためのＧＰＳとジャイロのカルマンフィルタによるセンサフュージョン技術（第１報）」、農業機械学会誌、Ｖｏｌ．６１，Ｎｏ．４、１０３−１１３ページ、１９９９年。
【発明の開示】
【発明が解決しようとする課題】
【０００７】
本発明は上述した従来の技術的課題に鑑みてなされたもので、ＤＧＰＳのような低コストセンサ類を使用して車両を自律走行させ、その走行誤差を小さくすることができる車両自律走行制御技術を提供することを目的とする。
【０００８】
本発明はまた、例えば農耕車両に適用して自律走行による走行誤差を許容範囲まで小さくできる車両自律走行制御技術を提供することを目的とする。
【課題を解決するための手段】
【０００９】
請求項１の発明の車両自律走行制御システムは、ＧＰＳ受信機による受信信号によって絶対位置情報を取得するＧＰＳ測位手段と、前記ＧＰＳ測位手段の取得した絶対位置情報の差分によって方位角を演算する方位角演算手段と、左右の駆動輪の回転数に基づいて速度と走行距離を演算する速度・距離演算手段と、目標経路の方位角と走行距離を設定する目標経路設定手段と、前記目標経路の方位角と前記方位角算定手段の算出する方位角との誤差を演算する方位角誤差演算手段と、前記方位角演算手段の算出する方位角と前記速度・距離演算手段の算出する走行距離とに基づいて現在位置を特定し、前記目標経路との距離偏差である走行誤差を演算する走行誤差演算手段と、前記方位角誤差と走行誤差に対して前記目標経路に一致するのに必要な左右の駆動輪の操作量を演算して左右駆動輪の回転速度を増減制御する駆動制御手段とを備えたものである。
【００１０】
請求項２の発明は、請求項１の車両自律走行制御システムにおいて、地磁気を感知して進行方向の方位角を算定する地磁気方位角センサと、前記方位角演算手段は、あらかじめ設定されている所定のタイミングに前記ＧＰＳ測位手段の検出信号に基づく方位角を前記地磁気方位角センサによる方位角で補正する方位角補正手段とを備えたことを特徴とするものである。
【００１１】
請求項３の発明は、請求項１又は２の車両自律走行制御システムにおいて、車両のＵターン時に前記走行誤差をリセットする走行誤差リセット手段を備えたことを特徴とするものである。
【００１２】
請求項４の発明は、請求項１〜３の車両自律走行制御システムにおいて、前記ＧＰＳ受信機は、ＤＧＰＳ受信機であることを特徴とするものである。
【００１３】
請求項５の発明の車両自律走行制御システムは、ＧＰＳ受信機による受信信号によって絶対位置情報を取得するＧＰＳ測位手段と、前記絶対位置情報に基づいて車両の走行距離と方位角を演算する距離・方位角演算手段と、車両の方位角を検出する方位角検出手段と、車両の速度を検出する速度検出手段と、車両の転回角速度を検出する角速度検出手段と、前記速度検出手段の検出する速度から走行距離を演算する走行距離演算手段と、目標経路の方位角と走行距離を設定する目標経路設定手段と、前記距離・方位角演算手段の算出する走行距離と方位角、前記方位角検出手段の検出する方位角、前記速度検出手段の検出する速度、前記走行距離演算手段の算出する走行距離、前記角速度検出手段の検出する転回角速度を入力とし、前記目標経路設定手段の設定する目標経路上の該当位置と比較して走行誤差推定値と方位角誤差推定値を演算して出力するカルマンフィルタと、前記走行誤差推定値と方位角誤差推定値に対して前記目標経路に一致するのに必要な左右の駆動輪の操作量を演算して左右駆動輪の回転速度を増減制御する駆動制御手段とを備えたものである。
【００１４】
請求項６の発明は、請求項５の車両自律走行制御システムにおいて、前記カルマンフィルタは、拡張カルマンフィルタであり、かつＪｏｓｅｐｈの安定化アルゴリズムを用いて推定値を得ることを特徴とするものである。
【００１５】
請求項７の発明は、請求項５又は６の車両自律走行制御システムにおいて、前記ＧＰＳ測位手段は、ＤＧＰＳ受信機であることを特徴とするものである。
【００１６】
請求項８の発明の車両自律走行制御方法は、ＧＰＳ測位手段の取得する絶対位置情報の差分によって方位角を演算する方位角演算ステップと、左右の駆動輪の回転数に基づいて速度と走行距離を演算する速度・距離演算ステップと、目標経路の方位角と走行距離を設定する目標経路設定ステップと、前記目標経路の方位角と前記方位角演算ステップで算出する方位角との誤差を演算する方位角誤差演算ステップと、前記方位角演算ステップで算出する方位角と前記速度・距離演算ステップで算出する走行距離とに基づいて現在位置を特定し、前記目標経路との距離偏差である走行誤差を演算する走行誤差演算ステップと、前記方位角誤差と走行誤差に対して前記目標経路に一致するのに必要な左右の駆動輪の操作量を演算して左右駆動輪の回転速度を増減制御する駆動制御ステップとを有するものである。
【００１７】
請求項９の発明の車両自律走行制御方法は、ＧＰＳ受信機による受信信号によって絶対位置情報を取得するＧＰＳ測位ステップと、前記絶対位置情報に基づいて車両の走行距離と方位角を演算する距離・方位角演算ステップと、車両の方位角を検出する方位角検出ステップと、車両の速度を検出する速度検出ステップと、車両の転回角速度を検出する角速度検出ステップと、前記速度検出ステップで検出する速度から走行距離を演算する走行距離演算ステップと、目標経路の方位角と走行距離を設定する目標経路設定ステップと、カルマンフィルタに前記距離・方位角演算ステップで算出する走行距離と方位角、前記方位角検出ステップで検出する方位角、前記速度検出ステップで検出する速度、前記走行距離演算ステップで算出する走行距離、前記角速度検出ステップで検出する転回角速度を入力し、前記目標経路設定ステップで設定した目標経路上の該当位置と比較して走行誤差推定値と方位角誤差推定値を演算して出力するフィルタリングステップと、前記走行誤差推定値と方位角誤差推定値に対して前記目標経路に一致するのに必要な左右の駆動輪の操作量を演算して左右駆動輪の回転速度を増減制御する駆動制御ステップとを有するものである。
【発明の効果】
【００１８】
本発明の車両自律走行制御システムによれば、速度検出手段の検出する速度から算出する走行距離とＧＰＳ測位手段の絶対位置情報から算出する絶対方位角を用いて車両位置を近似計算することによって、実用上問題無い精度で自律走行制御することができる。
【００１９】
また本発明の車両自律走行制御システムによれば、カルマンフィルタを用いてＧＰＳ出力値を他のセンサ情報と複合することでＧＰＳの出力値の不連続性を平滑化し、カルマンフィルタを用いることで、たとえ低価格で精度の高くないＧＰＳ受信機でも走行制御用位置センサとして使用可能にし、長距離走行時においても走行誤差が漸増することなく自律走行制御することができる。
【００２０】
本発明の自律走行制御方法によれば、これをコンピュータにソフトウェアとして組み込み車両に搭載することにより、速度検出手段の検出する速度から算出する走行距離とＧＰＳ測位手段の絶対位置情報から算出する絶対方位角を用いて車両位置を近似計算し、実用上問題無い精度で車両を自律走行させることができる。
【００２１】
また本発明の自律走行制御方法によれば、これをコンピュータにソフトウェアとして組み込み車両に搭載することにより、カルマンフィルタを用いてＧＰＳ出力値を他のセンサ情報と複合することでＧＰＳの出力値の不連続性を平滑化し、カルマンフィルタを用いることで、たとえ低価格で精度の高くないＧＰＳ受信機を走行制御用位置センサとして使用可能にし、車両の長距離走行時においても走行誤差が漸増することなく自律走行させることができる。
【発明を実施するための最良の形態】
【００２２】
以下、本発明の実施の形態を図に基づいて詳説する。
【００２３】
一般に低コストセンサ類は出力値にオフセット成分あるいは不連続性を有する等測定誤差が大きい。したがって、制御用として使用する場合、冗長なセンサ群を使用して適宜補間し合う等のセンサフュージョン技術が必要である。
【００２４】
本発明の１つの実施の形態の車両自律走行制御システム及び制御方法について説明する。本実施の形態の車両自律走行制御システムは、相対距離センサであるロータリエンコーダとＤＧＰＳで車両位置を近似計算し、車両を自律走行制御する。この制御システムでは、ＤＧＰＳをその通常の用途である測位センサとしてではなく、オフセット誤差の小さい絶対方位角を算出するためのセンサとして利用し、車両の絶対方位角をこのＤＧＰＳの出力から算定することを特徴とする。
【００２５】
図１に実証走行試験に使用した車両１としての履帯トラクタを示す。この車両１には図２に示す機能構成の自律走行制御システム２を搭載している。この自律走行制御システム２は、外部センサ群として測位センサであるＤＧＰＳ受信機２０１、左右車輪の回転速度を検出するロータリエンコーダ（右輪用）２０２Ｒ、ロータリエンコーダ（左輪用）２０２Ｌ、車両１の向きを検出する地磁気方位センサ２０３、そして振動ジャイロ２０４を備えている。自律走行制御システム２はこれらのセンサ群からの信号を入力し、あらかじめ設定したプログラムにより演算処理を行い、左右輪の速度制御を行う演算処理装置２１と、この演算処理装置２１の出力である左右輪それぞれに対する速度指令に基づいてモータ駆動するアクチュエータとしてのモータドライバ２０７Ｒ，２０７Ｌ、そしてこれらのモータドライバ２０７Ｒ，２０７Ｌにより回転駆動する右輪モータ２０８Ｒ、左輪モータ２０８Ｌを備えている。
【００２６】
演算処理装置２１はマイクロコンピュータで成るが、その演算処理機能を示すと、ＤＧＰＳ受信機２０１からの地球座標系の測位信号を用いて車両の方位角を演算する方位角演算部２２と、地磁気方位角センサ２０３のヨー角のオフセット誤差を補正するためのオフセット誤差補正部２３と、方位角演算部２２の出力、オフセット誤差補正部２３にて補正された地磁気方位センサ２０３からの方位角信号、左右輪のロータリエンコーダ２０２Ｒ，２０２Ｌの速度信号、地磁気方位センサ２０３の方位角信号、そして振動ジャイロ２０４の信号を用いて車両位置、速度、走行誤差推定値、方位角誤差推定値を演算する位置・速度・誤差演算部２４と、この位置・速度・誤差演算部２４の出力により車両１の左右輪の駆動モータ２０８Ｒ，２０８ＬをＰＩＤ制御するための操作量ｕi，−ｕiを出力する自律走行制御補償器２１６を有している。
【００２７】
図３に示すように走行車両１を目標経路Ａに追従して走行させるためには、車両１の目標経路Ａからの位置及び方向のズレ量である走行誤差ｘi、方位角誤差θiが小さくなるように制御する必要がある。本システムでは、演算処理装置２１において両誤差に（１）式で示されるＰＩＤ演算を施すことにより、車両を走行制御する。
【数１】
2005250696.tif 000003
【００２８】
（１）式で算出された操作量ｕi，−ｕiを車両駆動用アクチュエータであるモータドライバ２０７Ｒ，２０７Ｌに与える。モータドライバ２０７Ｒ，２０７Ｌはこの操作量ｕi，−ｕiだけ速度指令ｖiを操作し、これに見合う電圧を左右輪のモータ２０８Ｒ，２０８Ｌそれぞれに印加して自律走行を支援する。
【００２９】
ＲＴＫ−ＧＰＳ（測定誤差±２ｃｍＭａｘ）で走行誤差を計算する場合、その出力値を走行座標系に変換するだけでよい。図３に示したように、走行座標系とは、目標経路方向をｙ軸正方向とする直交座標系である。また、ＦＯＧ（光ファイバジャイロ）を使用すれば、適切なドリフト補正を施すことにより極めて信頼性の高い方位角誤差を得ることができる。このように、測定精度が高く、かつ信頼性の高いセンサを使用すれば、位置及び角度センサを各々１個ずつ使用することで車両を走行目標経路に対し精度良く追従走行させることが可能である。しかしながら、本実施の形態の制御システム２はこのような高価なセンサ類を採用せず、測定精度が低く、価格の安いセンサ類の出力値を適宜組合わせて使用し、自律走行制御、すなわち（１）式に使用する走行誤差及び方位角誤差を精度良く計算し、車両の自律走行を支援する。図４の表１に、本実施の形態の車両自律走行制御システムで採用したセンサ類及びそれらの主な仕様を示す。
【００３０】
地磁気方位センサ２０３は車両１の後部に設置している。また左右輪のロータリエンコーダ２０２Ｒ，２０２Ｌそれぞれは、左右輪の駆動軸それぞれに設置し、車体移動量及び車速を求める際には位置・速度・誤差演算部２４においてそれらの平均値を取る。またこの左右輪のロータリエンコーダ２０２Ｒ，２０２Ｌの信号によって位置・速度・誤差演算部２４で左右輪の走行誤差を求める。
【００３１】
図５に示したのはＤＧＰＳ受信機２０１の特性であり、ＤＧＰＳ受信機２０１を９０分間衛星電波環境が良好な屋上に放置して測定した定点測位結果を示している。ＲＴＫ−ＧＰＳ受信機の精度が±２ｃｍであるのに対して、この定点測位結果からＤＧＰＳ受信機２０１の場合には、その出力値が±１ｍの範囲内でゆっくり推移していることが分かる。
【００３２】
また図６（Ａ），（Ｂ）は、秒速１ｍでの車両走行時のＲＴＫ−ＧＰＳ受信機の出力値と本実施の形態で使用するＤＧＰＳ受信機２０１の出力値を各々走行座標系に変換して計算した走行誤差とその時ＤＧＰＳが捕捉していた衛星数を示したものである。
【００３３】
車両１の走行開始より１４秒経過した時点で捕捉衛星数が６個から７個に変化し、それに伴いＤＧＰＳ座標変換値が２００ｍｍ程度急激に変化していることが分かる。本発明者らの行った走行試験では、最大２ｍの出力値の急変が観察された。長距離長時間走行する場合、捕捉衛星数が頻繁に変化すると、ＤＧＰＳ受信機２０１の出力値を単純に走行座標系に変換して得られた走行誤差を自律走行制御に使用することはできない。
【００３４】
一方、コイルで地磁気を検出して絶対方位角を計測する地磁気方位センサ２０３は以下の特性を持つ。
【００３５】
（ｉ）オフセット誤差成分がヨー方向で変化する。
【００３６】
（ii）傾斜角（ロール、ピッチ角）により出力値が変化する。
【００３７】
図７は地磁気方位センサ２０３のヨー方向特性を調べるため、傾斜角０度を保ちながら、真北から真南（０〜１８０度）に１８０度回転させた時の出力値の変化を示したものである。図７より、東を向いた時に最大１８度のヨー方向オフセット誤差が発生していることが分かる。そこで本実施の形態では、オフセット誤差補正部２３においてまず、図７の出力曲線を５次の近似曲線で近似し、同近似式でヨー方向オフセット誤差を補正するようにしている。
【００３８】
次に、本実施の形態の車両自律走行制御システム２による自律走行制御動作を説明する。ＤＧＰＳ受信機２０１は捕捉衛星数変化時に出力値が不連続となるため、出力値を走行座標系に変換して計算した走行誤差を、車両操舵用アクチュエータへの指令値ｕi，−ｕiを計算するための（１）式へ代入することはできない。そこで本実施の形態では、ロータリエンコーダ２０２Ｒ，２０２ＬとＤＧＰＳ受信機２０１との複数のセンサ情報を使用した方法で走行誤差を計算し、車両１を自律走行制御する。
【００３９】
そのためにまず方位角演算部２２において、ＤＧＰＳ受信機２０１の出力の微分値から絶対方位角を計算し、位置・速度・誤差演算部２４では、この絶対方位角と相対距離センサであるロータリエンコーダ２０２Ｒ，２０２Ｌの出力値とを組み合わせることで走行誤差計算を行う。この計算方法によれば簡便であるが、ＤＧＰＳ受信機２０１が本来検出する絶対位置情報が走行誤差に反映されることはない。その計算方法は次の通りである。
【００４０】
図８に示すように、走行誤差は一定時間毎の走行距離と方位角誤差で計算することができる。すなわち、ａiをΔｔ（＝ｔi−ｔi-1）秒間のエンコーダパルス数、θiを時刻ｔiにおける方位角誤差、Ｘiniを初期値と、走行誤差は（２）式で積算することができる。
【数２】
2005250696.tif 000004
【００４１】
この（２）式は積算方式であるため、θiにオフセット成分が含まれていると走行誤差が漸増する。したがって、極力オフセット誤差の小さい方位角誤差をθiとして代入する必要がある。
【００４２】
地磁気方位センサ２０３は大きなオフセット誤差を含むため、（２）式に使用することはできない。振動ジャイロ２０４から方位角を得ることも可能だが、角速度出力を積算しなければならないため時間経過とともにオフセット誤差が大きくなる。ＦＯＧも高価格なものほどオフセット誤差が小さいが、長時間使用時には（２）式で走行誤差を計算するに当たり無視することができないオフセット誤差が発生する。
【００４３】
ＤＧＰＳ受信機２０１の出力値は長期的には±１ｍの範囲内でドリフトするが、捕捉衛星数が変化しない限り短期的なばらつき量は小さいので、このＤＧＰＳ出力値でオフセット誤差が小さい絶対方位角が計算できる。そこで、本自律走行制御システム２は、図９に示すようにＤＧＰＳ出力値の更新時間Δｔ（１００ｍ秒）の間は車両が直進していると仮定してＤＧＰＳ出力値から絶対方位角を算出し、目標方位角との差を取ることでオフセット誤差の小さい方位角誤差を計算する。
【００４４】
すなわち、（αi，βi）を時刻ｔiにおけるＤＧＰＳ出力値、φを目標方位角とし、（３），（４）式によってＤＧＰＳ出力値から方位角φiと方位角誤差θiを計算する。
【数３】
2005250696.tif 000005
【００４５】
なお（４）式の計算結果は、車速が小さい走行開始時及びＤＧＰＳ出力値が不安定となる捕捉衛星数変化時のような値に信頼性がないときには、必要に応じて地磁気方位センサ２０３の出力値で補間する。
【実施例１】
【００４６】
本実施の形態の車両自律走行制御システムによる動作計算精度確認のために走行試験を実施した。走行制御に使用する走行誤差（制御量）ｘiは、（１）式で表されるＰＩＤ制御補償器２１６へ入力され、車両操舵用アクチュエータであるモータドライバ２０７Ｒ，２０７Ｌを駆動する。したがって、制御量として使用する場合、走行誤差は十分に滑らかである必要がある。（２）式に（４）式を代入して計算した走行誤差が滑らかであるか、また数値的に安定しているか確認するため、車両をＲＴＫ−ＧＰＳで計算した走行誤差で自律走行させ、その時のロータリエンコーダ及びＤＧＰＳ計測値から（２），（４）式を用いて走行誤差を計算した。図１０にその計算結果を示してある。
【００４７】
図１０（ａ）のグラフにおいて、曲線（Ａ）はＲＴＫ−ＧＰＳ出力値を走行座標系に変換して計算した走行誤差で、この値が真の走行誤差と考えられる。曲線（Ｂ）は（２），（４）式で計算した走行誤差であり、曲線（Ｃ）は（２）式にＦＯＧで計算した方位角誤差を代入して計算した走行誤差である。曲線（Ｃ）は車両１の走行開始から１２秒間で真値とみなせる曲線（Ａ）との差が１．２ｍに達し、その後ＦＯＧオフセット成分の影響で徐々に漸増しながら推移している。曲線（Ｃ）が０〜１２秒間で急増したのは、ＦＯＧリセット時の車両初期方位角φ０と目標方位角φが一致していないためである。一方、（２），（４）式で計算した本実施の形態のシステムに対応する曲線（Ｂ）は漸増することなく、ＲＴＫ−ＧＰＳで計算した曲線（Ａ）の走行誤差とほぼ一定間隔を保ちながら推移している。これは（４）式、すなわちＤＧＰＳで計算した方位角誤差にオフセット成分が含まれていないことを意味する。また曲線（Ｂ）に不連続点はなく、（１）式に代入し滑らかにアクチュエータを駆動することが可能であることが確認できた。
【００４８】
図１０（ｂ）のグラフにおいて、曲線（Ａ）はＦＯＧと目標方位角φとの差を取ることで計算した方位角誤差である（走行誤差の曲線（Ｃ）はこの値を（２）式に代入して計算している）。なお、曲線（Ａ）は、曲線（Ｃ）で示されるＦＯＧドリフト補正角で補正済みのものである。曲線（Ｂ）が（４）式で計算した方位角誤差θiである（図１０（ａ）における曲線（Ｂ）の走行誤差はこの値を（２）式に代入して計算した得たものである）。オフセット成分は極めて小さいが、ばらつきが大きいためフィルタリングした後、（２）式に代入して走行誤差を計算している。
【００４９】
図１０（ｃ）のグラフは、Δｔ（１００ｍ秒）毎の左右輪のロータリエンコーダ２０２Ｒ，２０２Ｌの出力値である。（２）式には、これらの平均値を代入した。
【００５０】
実証走行試験の結果は、次の通りである。図１１に（２），（４）式で計算した走行誤差、すなわちロータリエンコーダとＤＧＰＳで計算した走行誤差を、フィードバック制御量として（１）式に代入し、車両を約８０ｍ直進走行させた結果を示す。車速は毎秒１ｍである。
【００５１】
図１１（ａ）のグラフは走行誤差、同図（ｂ）はＤＧＰＳ受信機の出力値を用いて計算した方位角のグラフ、同図（ｃ）は方位角誤差のグラフを示す。まず図１１（ａ）において、曲線（Ａ）がＲＴＫ−ＧＰＳ座標変換値（真値）、曲線（Ｂ）が（２），（４）式で計算した走行誤差（制御量）、曲線（Ｃ）がＤＧＰＳ座標変換値である。曲線（Ｂ）の場合、高精度のＲＴＫ−ＧＰＳによる走行誤差を示す曲線（Ａ）と比較してみると、走行開始から４５秒、距離にして約４５ｍ走行した時点で最大３００ｍｍ（進行方向に対して左方）の走行誤差が発生していることが分かる。これは、４０秒付近でＤＧＰＳの捕捉衛星数が変化し、その結果ＤＧＰＳ出力値が急激に変化したことが原因である。本システムでは走行誤差計算にＤＧＰＳ受信機２０１による方位角を使用しているため、ＤＧＰＳ出力値の変化量が大きい場合に多少その影響を受けるが、ＤＧＰＳ座標変換値である曲線（Ｃ）の値を制御量とした場合には走行誤差が２ｍ以上になることを考えると、この３００ｍｍの走行誤差は小さいといえる。
【００５２】
また、図１２のグラフはＤＧＰＳ出力値が急激に変動した時の走行軌跡を拡大して表示したものである。ＲＴＫ−ＧＰＳで示される走行軌跡が目標経路から（進行方向に対して）左方に３００ｍｍ程度オフセットしているのに対し、ＤＧＰＳ計測値は右方に２ｍ以上オフセットしている。それでも本システムのように走行誤差計算にＤＧＰＳによる方位角を使用することで、上述したように走行誤差を小さく抑えることができる。
【００５３】
図１１（ｂ）のグラフは、ＤＧＰＳ出力値を用いて（３）式で計算した絶対方位角である。曲線（Ａ）に（３）式の計算結果を示すが、捕捉衛星数変化時にＤＧＰＳ出力値が不安定になるため（４０秒付近）、信頼性のない値となっている。そこでこのような場合には地磁気方位センサ２０３の信号で補正する。曲線（Ｂ）に補正後の（３）式の計算結果を示してある。走行誤差を計算する（２）式には、この曲線（Ｂ）の値を代入した。
【００５４】
図１１（ｃ）のグラフは、制御補償器２１６における（１）式の演算に代入する制御量としての方位角誤差である。（４）式で計算する方位角誤差は、Ｓ／Ｎ比が小さいため（（ｂ）図参照）、制御量として（１）式に代入することはできない。そのため、地磁気方位センサ２０３の出力値のオフセット量を（４）式で補正して、制御量としての方位角誤差を計算した。曲線（Ａ）が地磁気方位センサ出力値（図７でのヨー方向補正済）と目標方位角との差であり、この値を（４）式で補正し、オフセット量を除去したものが曲線（Ｂ）であり、これが制御量である。曲線（Ｂ）にオフセット量が含まれていないことは、図１１（ａ）に示した曲線（Ｂ）の走行誤差がオフセットしていないことから確認することができる。
【００５５】
図１３に本実施の形態の車両自律走行制御システムにより車両１を１００ｍの直進距離を３往復させ、合計６００ｍだけ自律走行させた結果を示している。図中の走行軌跡のプロットには車両１にレファレンスのためにＤＧＰＳ受信機と同時にＲＴＫ−ＧＰＳ受信機を搭載し、そのＲＴＫ−ＧＰＳ受信機の出力値を用いた。本実施の形態のシステムによる１２回の自律走行試験中、発生した走行誤差は最大１．１ｍ程度であった。この走行誤差以内であれば、耕耘作業、牧草地での施肥作業、融雪剤散布作業等で実用的に使用できることが確認できた。
【００５６】
次に、本発明の第２の実施の形態の車両自律走行制御システムについて説明する。第１の実施の形態のシステムでは、簡易な自律走行支援のために、方位角誤差に測位センサであるＤＧＰＳ受信機の出力値を利用し、相対距離センサであるロータリエンコーダで制御量としての走行誤差を計算し、車両を走行制御するものである。この第１の実施の形態のシステムでも比較的狭い耕地の自動作業には有効であるが、長距離走行時に走行誤差が大きくなる可能性が否定できない。
【００５７】
第２の実施の形態の車両自律走行制御システムは、低価格のセンサ類を用いながらさらに高精度の自律走行制御を可能にしたものである。
【００５８】
ＲＴＫ−ＧＰＳ受信機は精度が±２ｃｍと高精度であるが、それ故に高価格である。それに比べてＤＧＰＳ受信機は市販価格がその１／５程度であるが、その分、測位精度が±１ｍとＲＴＫ−ＧＰＳ受信機にくらべて１／５０程度劣る。このＤＧＰＳ受信機は測位精度が±１ｍであるため、これを測位センサとして使用する場合には図５に示したようなドリフトが避けられず、捕捉衛星数が変化したような場合に最大で２ｍの測位誤差が発生し、出力値に大きな不連続性を示すことがある。しかしながら、絶対位置情報が反映するＤＧＰＳ座標変換値を走行誤差計算に使用して車両を走行制御すれば、長距離走行時においても走行誤差が蓄積して漸増することはない。
【００５９】
本発明の第２の実施の形態の車両自律走行制御システムは、このような技術的課題を、カルマンフィルタを利用することでＤＧＰＳ受信機の出力値の不連続性を吸収し、低価格のＤＧＰＳ受信機を絶対位置座標測定用のセンサとして用いて車両の精度の高い自律走行を可能にしたものである。第２の実施の形態の車両自律走行制御システムは、カルマンフィルタにＤＧＰＳ座標変換値を入力し、同フィルタ出力として走行誤差を計算し、ＤＧＰＳ絶対位置情報を走行誤差計算に間接的に利用することを特徴とする。
【００６０】
図１４は第２の実施の形態の車両自律走行制御システムの機能構成を示している。図１に示した車両１と同様の車両に図１４に示す機能構成の自律走行制御システム２０を搭載して当該車両１を自律走行させる。この自律走行制御システム２０は、外部センサ群として測位センサであるＤＧＰＳ受信機２０１、左右車輪の回転速度を検出するロータリエンコーダ（右輪用）２０２Ｒ、ロータリエンコーダ（左輪用）２０２Ｌ、車両１の向きを検出する地磁気方位センサ２０３、そして振動ジャイロ２０４を備えている。自律走行制御システム２はこれらのセンサ群からの信号を入力し、あらかじめ設定したプログラムにより演算処理を行い、左右輪の速度制御を行う演算処理装置２０６と、この演算処理装置２０６の出力である左右輪それぞれに対する速度指令に基づいてモータを制御するモータドライバ２０７Ｒ，２０７Ｌ、そしてこれらのモータドライバ２０７Ｒ，２０７Ｌにより駆動制御される右輪モータ２０８Ｒ、左輪モータ２０８Ｌを備えている。
【００６１】
演算処理装置２０６はマイクロコンピュータで成るが、その演算処理機能を示すと、ＤＧＰＳ受信機２０１からの地球座標系を車両の走行座標系に変換する座標系変換演算部２０９、ＤＧＰＳ受信機２０１からの地球座標系の信号を利用して車両の正規の方位からの誤差を演算する第１の方位角演算部（１）２１０を有している。また演算処理装置２０６は、左右輪のロータリエンコーダ２０２Ｒ，２０２Ｌの速度信号を用いて走行誤差を演算する走行位置演算部２１１と車速を演算する車速演算部２１２を有している。演算処理装置２０６はまた、地磁気方位センサ２０３の方位角信号を用いて方位角誤差を演算する第２の方位角演算部（２）２１３、振動ジャイロ２０４の信号を用いて角速度を演算する角速度演算部２１４を有している。
【００６２】
さらに演算処理装置２０６は、これら各演算部からの内部入力（ｉ）〜（ｖｉｉ）とロータリエンコーダ２０２Ｒ，２０２Ｌからの外部入力（ｉ），（ｉｉ）としての左右輪の速度信号を用いて後述する演算処理を行い走行誤差推定値ｘi、方位角誤差推定値θiを求めて出力するカルマンフィルタ２１５、そしてこのカルマンフィルタ２１５の出力により車両１の左右輪の駆動モータ２０８Ｒ，２０８Ｌを制御する操作量ｕi，−ｕiを出力する自律走行制御補償器２１６を有している。
【００６３】
カルマンフィルタ２１５は、観測対象を表現するモデルを必要とする。フィルタ推定値の精度は、同モデルの良否に大きく左右される。本実施の形態では図１５に示すように、状態変数を位置（ｘｙ座標）、車速、方位角誤差、及び重心回り角速度からなる５次元ベクトルで表現し、走行中の車両１の左右輪速度（ｍ／ｓ）を外部入力とする離散化漸化式として表現し、走行誤差推定値ｘと方位角誤差θを算出して自律走行制御補償器２１６に出力する。
【００６４】
（５）式に車両のシステム方程式、（６）式に観測方程式を示す。
【数４】
2005250696.tif 000006
【数５】
2005250696.tif 000007
【００６５】
（５）式から分かるように、車両の走行モデルは非線形確率システムとして表現している。このため、本実施の形態ではカルマンフィルタ２１５には数６，７式に示す拡張カルマンフィルタを採用している。
【数６】
2005250696.tif 000008
【数７】
2005250696.tif 000009
【００６６】
しかしながら、拡張カルマンフィルタのアルゴリズムで計算した走行誤差をシミュレーションして確認したところ、数値的に安定性のない推定結果が得られた。これは、逆行列演算を行うにあたり、要素の有効桁数が少なくなり、カルマンゲインＫｋが正しく計算できなかったことによるものである。そこで、本実施の形態ではさらに、観測雑音の共分散行列が対角行列となることを利用し、逆行列演算を行わずに推定値計算を行うＪｏｓｅｐｈの安定化アルゴリズムを用いて推定値を計算した。数８〜１０式にＪｏｓｅｐｈの安定化アルゴリズムを示してある。
【数８】
2005250696.tif 000010
【数９】
2005250696.tif 000011
【数１０】
2005250696.tif 000012
【００６７】
図１６にはこのカルマンフィルタ２１５の詳しいブロック線図が示してある。また図１７の表２にカルマンフィルタ２１５への入力値を示してある。図１４の構成、またこの図１７の表２に示すように、走行誤差としてＤＧＰＳ座標変換値と（２）式のロータリエンコーダ積算値を、また方位角誤差としてオフセット補正した地磁気方位センサ２０３の出力値及び（４）式のＤＧＰＳ計算値をそれぞれ入力する。すなわち５個の状態変数のために、７個の冗長な観測情報を入力するようにしている。
【実施例２】
【００６８】
本実施の形態の車両自律走行制御システムによる走行精度確認のための走行試験を最初に行った。図１８にロータリエンコーダによる走行誤差（図中の曲線（Ｂ））を制御量として約８０ｍの直進距離を自律走行させた時に計算したフィルタ推定値（図中の曲線（Ｄ））を示す。なお、捕捉衛星数変化時における出力値の不連続性を平滑化することを目的として、フィルタ入力であるＤＧＰＳ座標変換値（図中の曲線（Ｃ））に対する観測誤差標準偏差を１ｍに設定した。
【００６９】
この試験より、推定値の曲線（Ｄ）は発散することなく、ＲＴＫ−ＧＰＳによる走行誤差の曲線（Ａ）と定性的にほぼ同傾向を示すことが確認できた。特に、捕捉衛星数（図中の曲線（Ｅ））が７個から６個となる３０秒付近で、ＤＧＰＳ座標変換値の曲線（Ｃ）が急峻に変化しているのに対し、推定値の曲線（Ｄ）は真値の曲線（Ａ）とほぼ同傾向で変化した。また、曲線（Ｃ）が７秒かけて約７０ｃｍ変化しているのに対し（真値の曲線（Ａ）との相対量）、カルマンフィルタ２１５を通した推定値の曲線（Ｄ）の変化量は２０ｃｍに抑えることができた。なお、フィルタ推定値に±５ｃｍ程度の周期的ノイズが含まれているが、これはフィルタ２１５への外部入力である左右輪ロータリエンコーダ２０２Ｒ，２０２Ｌの出力値が図１０（ｃ）に示したように周期的に激しく変動するためである。
【００７０】
以上より、フィルタ推定値を制御量として車両を自律走行させるにあたり、ＤＧＰＳ座標変換値に対する観測誤差標準偏差を１ｍに設定し、また推定値に含まれる周期的ノイズを平滑化して（１）式に入力するようにした。
【００７１】
次に、以上の計算精度確認走行試験の結果を踏まえ、実証走行試験を実施した。図１９にカルマンフィルタ２１５で推定した走行誤差を制御量として車両を自律走行させた時の走行軌跡を示す。秒速１ｍで直進距離約７０ｍを３往復させたものである。５回の走行試験中、発生した走行誤差は最大１．２ｍであった。図２０に走行開始から３回目の直進走行時におけるフィルタ入力値（表２参照）及び推定された走行誤差（図中の曲線（Ｄ））を示している。図２０より、推定値の曲線（Ｄ）は発散することなく安定した推定結果が得られることが確認できた。
【００７２】
なお、１８０度枕地旋回部は車両が急旋回するため、履帯の滑りの影響でロータリエンコーダで計算した走行誤差と車速が不安定となった。その結果、これらの２つ値を入力情報とするフィルタ推定値が信頼性のない値となる。したがって枕地旋回部はＤＧＰＳ座標変換による走行誤差を制御量として車両を自律走行させた。枕地旋回に要する時間は短時間であり、捕捉衛星数が変化する確率は小さい。長距離走行となる直進部のみフィルタ推定値を制御量として車両を自律走行させることにより、誤差の少ない自律走行が可能である。
【図面の簡単な説明】
【００７３】
【図１】本発明の第１の実施の形態の車両自律走行制御システムを搭載した車両の写真。
【図２】上記第１の実施の形態の車両自律走行制御システムの機能構成を示すブロック図。
【図３】自律走行の目標経路と走行座標系、車両の方位角誤差、走行誤差との関係を示す説明図。
【図４】上記第１の実施の形態で使用するセンサ類の表１。
【図５】上記第１の実施の形態で使用するＤＧＰＳ受信機のドリフト特性を示すグラフ。
【図６】上記第１の実施の形態で使用するＤＧＰＳ受信機とＲＴＫ−ＧＰＳ受信機の捕捉衛星数の変化による測位精度の変化を対比したグラフ。
【図７】上記第１の実施の形態で使用する地磁気方位センサのヨー方向特性を示すグラフ。
【図８】上記第１の実施の形態において、目標経路に対する走行誤差を算出する方法の説明図。
【図９】上記第１の実施の形態において、ＤＧＰＳ受信機の出力値を用いて目標経路に対する方位角誤差を算出する方法の説明図。
【図１０】上記第１の実施の形態において、車両をＲＴＫ−ＧＰＳ受信機の出力値で計算した走行誤差で自律走行させ、その時のロータリエンコーダ及びＤＧＰＳ受信機の計測値から（２），（４）式を用いて走行誤差を計算した結果を示すグラフ。
【図１１】上記第１の実施の形態において、（２），（４）式で計算した走行誤差、ＤＧＰＳ受信機の測定値で計算した方位角、その方位角を用いて求めた方位角誤差のグラフ。
【図１２】上記第１の実施の形態において、ＤＧＰＳ受信機の出力値が急激に変動した時の走行軌跡を示す図。
【図１３】上記第１の実施の形態の車両自律走行制御システムにより車両を１００ｍの直進距離を３往復させ、合計６００ｍだけ自律走行させた際の実走行経路と目標経路とを対比して示した図。
【図１４】本発明の第２の実施の形態の車両自律走行制御システムの機能構成を示すブロック図。
【図１５】上記実施の形態において、位置（ｘｙ座標）、車速、方位角誤差、及び重心回り角速度からなる状態変数の５次元ベクトルを示す説明図。
【図１６】上記第２の実施の形態におけるカルマンフィルタのブロック線図。
【図１７】上記第２の実施の形態において、カルマンフィルタの入力値と状態変数とを対比した表２。
【図１８】上記第２の実施の形態において、ロータリエンコーダによる走行誤差を制御量として約８０ｍの直進距離を自律走行させた時に計算したフィルタ推定値の推移を示すグラフ。
【図１９】上記第２の実施の形態によって車両を秒速１ｍで直進距離約７０ｍを３往復自律走行させた時の走行軌跡を示す図。
【図２０】上記第２の実施の形態によって車両を秒速１ｍで直進距離約７０ｍを３往復自律走行させた際の走行開始から３回目の直進走行時におけるフィルタ入力値及び推定された走行誤差の推移を示すグラフ。
【符号の説明】
【００７４】
１  車両
２  車両自律走行制御システム
２１  演算処理装置
２２  方位角演算部
２３  オフセット補正部
２４  位置・速度・誤差演算部
２０１  ＤＧＰＳ受信機
２０２Ｒ，２０２Ｌ  ロータリエンコーダ
２０３  地磁気方位センサ
２０４  振動ジャイロ
２０６  演算処理装置
２０７Ｒ，２０７Ｌ  モータドライバ
２０８Ｒ，２０８Ｌ  モータ
２０９  座標系変換演算部
２１０  方位角演算部（１）
２１１  走行位置演算部
２１２  車速演算部
２１３  方位角演算部（２）
２１４  角速度演算部
２１５  カルマンフィルタ
２１６  自律走行制御補償器
【図１】
2005250696.tif 000013
【図２】
2005250696.tif 000014
【図３】
2005250696.tif 000015
【図４】
2005250696.tif 000016
【図５】
2005250696.tif 000017
【図６】
2005250696.tif 000018
【図７】
2005250696.tif 000019
【図８】
2005250696.tif 000020
【図９】
2005250696.tif 000021
【図１０】
2005250696.tif 000022
【図１１】
2005250696.tif 000023
【図１２】
2005250696.tif 000024
【図１３】
2005250696.tif 000025
【図１４】
2005250696.tif 000026
【図１５】
2005250696.tif 000027
【図１６】
2005250696.tif 000028
【図１７】
2005250696.tif 000029
【図１８】
2005250696.tif 000030
【図１９】
2005250696.tif 000031
【図２０】
2005250696.tif 000032