JPH1185274A - 自律走行車の走行軌跡検出装置 - Google Patents

Abstract

(57)【要約】 【課題】 自車両の前レーンの走行軌跡をマーカによっ
て正確に検出し、平行度を要求される往復直線走行に対
して平行度を確保し、また、作業跡を検出することが困
難な場合の倣い走行に対しても正確な追従走行を可能と
する。 【解決手段】 ステレオカメラで撮像したマーカ列の画
像を処理して距離画像を生成し、この距離画像から２次
元多数決処理によって不良距離データを排除し、２次元
移動平均化処理によって距離画像内のある一定の広さの
矩形エリアで平均値を取って距離データを平滑化する
と、ラプラス変換を行って距離段差部分の情報を得る。
そして、このラプラス変換後の段差情報に基づいて、
＋，−のピーク値と、その間隔とから段差のある場所を
エッジとして検出し、ハフ変換によって段差部分の検出
点からマーカ列に相関のある直線式を求める。さらに、
このハフ変換で求めた直線近傍の検出点を収集し、その
点の集合を用いて最小二乗法によって最終的な直線式を
再計算し、マーカ列の近似直線式を決定する。

Description

【発明の詳細な説明】

【０００１】

【発明の属する技術分野】本発明は、マーカを散布して
自車両の走行軌跡を表す自律走行車の走行軌跡検出装置
に関する。

【０００２】

【従来の技術】近年、ゴルフ場、河川敷堤防、公園等の
各種フィールドで行う草刈、芝刈り作業等を無人で行な
うことのできる自律走行車が開発されている。このよう
な自律走行車においては、作業の関係上、所定の領域で
車体の旋回を伴った往復走行を行う場合があり、このよ
うな往復走行を制御するシステムには、往路と復路の方
位を地磁気センサやジャイロ等の方位センサによって検
知し、平行に走行するよう制御するものや、光センサ等
の無接触センサあるいは機械的な接触式センサ等を用い
てフィールド上の既処理部と未処理部との作業境界を検
出し、検出した作業境界に沿った倣い走行を行なうもの
がある。

【０００３】

【発明が解決しようとする課題】しかしながら、平行度
を要求される往復直線走行においては、図２１（ａ）に
示すように往路と復路の進行方向は１８０度異なること
が要求され、方位検出センサによって走行方位を検出し
て往路及び復路の方向転換を行うと、方位検出センサの
誤差によって往路と復路とが必ずしも完全に平行にはな
らない。このため、長距離の往復走行の間には、往路・
復路で平行度の誤差が大きくなり、図２１（ｂ）に示す
ように、進路が開いて刈り残しが生じたり、図２１
（ｃ）に示すように、進路が狭くなってオーバーラップ
量が増加する等の問題がある。

【０００４】また、光センサ等の無接触センサあるいは
機械的な接触式センサ等を用いて倣い走行を行うもので
は、例えば、草・芝刈り作業を行うためのカッタを備え
たリールを車体前方に備える芝刈り機等の自律走行作業
車の場合、リールの更に前方に刈り跡を検出するための
センサを設置しなければならず、実現性に乏しいばかり
でなく、センサとリール間の距離が長く取れないため、
刈り跡を検出してから倣い走行を実施するまでの余裕が
なく、制御性が著しく悪化する。さらには、草・芝刈り
作業における刈り込み量が微小な場合やクロスカットの
ように二重に刈り込みを行う場合には、刈り跡の段差そ
のものが殆ど無いため、刈り跡を検出することが困難で
ある。

【０００５】本発明は上記事情に鑑みてなされたもの
で、自車両の前レーンの走行軌跡をマーカによって正確
に検出し、平行度を要求される往復直線走行に対して平
行度を確保することができ、また、作業跡を検出するこ
とが困難な場合の倣い走行に対しても正確な追従走行を
可能とする自律走行車の走行軌跡検出装置を提供するこ
とを目的としている。

【０００６】

【課題を解決するための手段】請求項１記載の発明は、
車両の進行に伴って複数のマーカを散布し、車体周辺の
地表面にマーカ列を形成する手段と、車両搭載のステレ
オカメラで撮像した車体周辺の画像を処理して得られる
距離画像に対し、距離データを微分した微分値の変化に
基づいて上記マーカによる距離段差を検出する手段と、
上記距離段差の検出点に基づいて上記マーカ列を近似す
るラインを自車両の走行軌跡として算出する手段とを備
えたことを特徴とする。

【０００７】請求項２記載の発明は、請求項１記載の発
明において、上記距離データの微分値に対し、閾値より
上の極大値と閾値より下の極小値とが互いに連続して存
在し、各極値間の距離が設定範囲内の位置を検出し、上
記マーカによる距離段差とすることを特徴とする。

【０００８】請求項３記載の発明は、請求項１又は請求
項２記載の発明において、上記距離段差の検出点からハ
フ変換によって上記マーカ列を近似する直線を求めた
後、さらに、この直線近傍の上記距離段差の検出点の集
合から最小二乗法によって最終的な直線を求めることを
特徴とする。

【０００９】請求項４記載の発明は、請求項１，２，３
のいずれか一に記載の発明において、上記距離画像に対
して２次元多数決処理を行って不良距離データを排除し
た後、上記マーカによる距離段差を検出することを特徴
とする。

【００１０】請求項５記載の発明は、請求項１，２，３
のいずれか一に記載の発明において、上記距離画像に対
して２次元移動平均化処理を行って距離データを平滑化
した後、上記マーカによる距離段差を検出することを特
徴とする。

【００１１】請求項６記載の発明は、請求項４記載の発
明において、上記不良距離データの排除後、さらに、２
次元移動平均化処理を行って距離データを平滑化するこ
とを特徴とする。

【００１２】すなわち、本発明の自律走行作業車の走行
軌跡検出装置では、走行中に複数のマーカを散布して車
体周辺の地表面にマーカ列を形成し、車両に搭載したス
テレオカメラで車体周辺を撮像してマーカ列による走行
軌跡を認識する。この走行軌跡の認識は、ステレオ撮像
画像を処理して得られる距離画像の距離データを微分し
た微分値の変化に基づいてマーカによる距離段差を検出
し、この距離段差の検出点からマーカ列を近似するライ
ンを算出することで行う。

【００１３】この場合、距離データの微分値の中で閾値
より上の極大値と閾値より下の極小値とが互いに連続し
て存在し、各極値間の距離が設定範囲内の位置を、マー
カによる距離段差として検出することが望ましく、この
距離段差の検出点からハフ変換によって作業境界を近似
する直線を求めた後、さらに、この直線近傍の距離段差
の検出点の集合から最小二乗法によって最終的な直線を
求めることが望ましい。また、距離画像に対しては、予
め、２次元多数決処理を行って不良距離データを排除し
ておくことが望ましく、さらに、２次元移動平均化処理
による距離データの平滑化を併用することが望ましい。

【００１４】

【発明の実施の形態】以下、図面を参照して本発明の実
施の形態を説明する。図１〜図２０は本発明の実施の一
形態に係わり、図１は走行軌跡検出ルーチンのフローチ
ャート、図２は２次元多数決処理ルーチンのフローチャ
ート、図３は２次元平均化処理ルーチンのフローチャー
ト、図４はラプラス変換処理ルーチンのフローチャー
ト、図５はエッジ検出処理ルーチンのフローチャート、
図６はハフ変換処理ルーチンのフローチャート、図７は
最小二乗近似処理ルーチンのフローチャート、図８は走
行制御ルーチンのフローチャート、図９は制御系のブロ
ック図、図１０はフォームマーカ検出部のブロック図、
図１１は自動芝刈機の外観図、図１２はフォーム発生装
置の外観図、図１３はフォーム発生装置の断面図、図１
４はフォームマーカの撮像画像を模式的に示した説明
図、図１５はカメラからフォームマーカまでの距離を示
す説明図、図１６は小領域での距離値ヒストグラム、図
１７は距離画像に対する横方向ラプラス変換の説明図、
図１８は距離段差が存在したときのラプラス変換後のデ
ータを示す説明図、図１９は段差検出点を示す説明図、
図２０は走行軌跡の近似直線を示す説明図である。

【００１５】図１１（ａ）において、符号１は無人で自
律走行が可能な自律走行作業車を示し、本実施の形態に
おいては、ゴルフ場等の草・芝刈り作業を自動的に行う
自動芝刈機である。この自動芝刈機１は、車体２の前部
に複数の芝刈リール３を連設したリール式の芝刈機であ
り、車体２の前方を撮像する１組のＣＣＤカメラ４ａ，
４ｂを備えるとともに、推測航法によって自己位置を測
位するための地磁気センサや車輪エンコーダ等の図示し
ないセンサ類を備えている。

【００１６】また、上記自動芝刈機１には、図１１
（ｂ）に示すように、地表面に複数の泡（フォーム）の
塊を散布して走行軌跡をマーキングするフォーム発生装
置５が車体両脇に備えられ、それぞれが車体２側部に延
出したアーム６の先端に取り付けられている。このフォ
ーム発生装置５は、図１２に示すように、本体部７の下
部に散布用ノズル１４を装着して構成され、本体部７に
上記アーム６を介して圧縮空気及び界面活性剤溶液が供
給される。

【００１７】詳細には、図１３に示すように、上記本体
部７にはベンチュリ部８とフロート室９とが備えられ、
上記散布用ノズル１４には、スポンジ１５と、このスポ
ンジ１５の下部に配設される網１６とが内蔵されてい
る。上記本体部７にはニップル１０が立設されており、
このニップル１０からフロートピン１１を介して界面活
性剤溶液が上記フロート室９内に供給され、上記フロー
トピン１１に連設されるフロート１２によって定液面に
保たれる。

【００１８】そして、上記本体部７上部の空気口７ａに
圧縮空気を供給することで、上記フロート室９内の界面
活性剤溶液がベンチュリ部８の吐出ノズル１３からベン
チュリ効果によって吸い出され、上記散布用ノズル１４
に内蔵されるスポンジ１５に浸透する。上記スポンジ１
５に浸透した界面活性剤は空気圧によって押し出されて
泡となって上記網１６を通過し、上記散布用ノズル１４
から出てゆく。この際、界面活性剤の泡は塊となって自
重により落下するが、圧縮空気の供給を一定にすること
で、泡の落下間隔を略一定にすることができる。

【００１９】以上の自動芝刈機１は、複数のマイクロコ
ンピュータからなる分散処理系によって制御される。こ
の分散処理系は、図９に示すような機能構成となってお
り、上記ＣＣＤカメラ４ａ，４ｂからの画像信号を処理
して地表面に散布された複数の泡の塊によるマーカ列を
検出するフォームマーカ検出部２５、図示しない地磁気
センサや車輪エンコーダ等からの信号を処理する推測航
法位置検出部３５、図示しない無接触型センサや接触型
センサからの信号を処理する障害物検出部４０、各検出
部２５，３５，４０からの信号に基づいて自動芝刈機１
の自律走行を制御する走行制御部５０、この走行制御部
５０によって参照される作業データや地形データマップ
等が格納されている作業データ蓄積部６０、上記走行制
御部５０からの指示によって車両制御を行なう車両制御
部７０等が備えられ、さらに、この車両制御部７０から
の出力に基づいて自動芝刈機１の各機構部を駆動するた
め、フォームマーカ制御部７５、駆動制御部８０、操舵
制御部９０、刈刃制御部１００等が備えられている。

【００２０】上記フォームマーカ検出部２５では、上記
ＣＣＤカメラ４ａ，４ｂで撮像した車両周辺の画像を処
理し、フォームマーカによるマーカ列を検出する。上記
ＣＣＤカメラ４ａ，４ｂは、車外の対象物を異なる位置
から撮像するためのステレオカメラであり、後述するよ
うに、ＣＣＤカメラ４ａ，４ｂで撮像した１対の画像に
おける同一物体に対する視差から三角測量の原理により
画像全体に渡る３次元の距離分布を算出し、この距離分
布からマーカ列の軌跡を検出する。このフォームマーカ
検出部２５についての詳細は後述する。

【００２１】また、上記推測航法位置検出部３５では、
車輪エンコーダからの出力パルスを積算する等して走行
距離を求め、この走行距離を地磁気センサで検出した走
行方向の変化に対応させて累積することにより、基準地
点からの走行履歴を算出して自車両の現在位置を測定す
る。さらに、上記障害物検出部４０では、無接触型セン
サや接触型センサ等によって予測できない障害物を検出
し、その検出信号を上記走行制御部５０に出力する。

【００２２】上記走行制御部５０では、上記フォームマ
ーカ検出部２５、上記推測航法位置検出部３５からの情
報に基づいて、作業データ蓄積部６０のマップ（作業内
容及び草・芝刈り作業を行う作業領域の地形データ等の
走行用地図）を参照して自己の現在位置と目標位置との
誤差量を算出し、この誤差量分を補正すべく走行経路指
示や車両制御指示を決定し、上記車両制御部７０に出力
する。また、上記障害物検出部４０からの信号により、
障害物が検出されたときには、障害物回避あるいは車両
停止を指示する。

【００２３】上記車両制御部７０では、上記走行制御部
５０からの指示を具体的な制御指示量に変換し、この制
御指示量を上記フォームマーカ制御部７５、駆動制御部
８０、操舵制御部９０、刈刃制御部１００に出力する。
これにより、上記フォームマーカ制御部７５では、上記
フォーム発生装置５へ供給する圧縮空気を発生する空圧
ポンプ１７を駆動制御し、上記駆動制御部８０では、油
圧モータ１８を介して駆動機構部１９を駆動する。ま
た、上記操舵制御部９０では、前輪操舵用油圧制御弁２
０を介して前輪操舵機構部２１をサーボ制御し、上記刈
刃制御部１００では、刈刃制御用油圧制御弁２２を介し
て刈刃機構部２３をサーボ制御する。

【００２４】以上の自動芝刈機１の作業領域における草
・芝刈り作業に伴う往復自律走行においては、基本的に
は地磁気センサ等の方位センサで検出した走行方位によ
って往路及び復路の方向転換を行うようにしているが、
例えば、直線の往復走行で、ある走行方位に対して１８
０度転回した走行方位を地磁気センサによって検出し、
自動芝刈機１を移動させても、地磁気センサのキャリブ
レーションのずれや誤差により必ずしも完全に平行には
ならず、長距離の往復走行の間には、往路と復路の平行
度に誤差が大きくなり、刈り残しやオーバーラップ量が
増加する虞がある。また、刈り跡による倣い走行を行う
場合においても、刈り込み量が微小な場合やクロスカッ
トのように二重に刈り込みを行う場合等には、刈り跡の
段差そのものが殆ど無いため、刈り跡を検出することが
困難である。

【００２５】従って、往復走行の最初の１レーンを自動
的に草・芝刈り作業を行いながら走行するとき、次に転
回する側のフォーム発生装置５から白色の泡を一定間隔
で落としてゆき、その後、車両転回して復路の草・芝刈
り作業を始める際、上記フォームマーカ検出部２５でマ
ーカ列を検出して直前のレーンの走行軌跡として走行制
御部５０に送り、この直前のレーンの走行軌跡に基づい
て進行方位を補正するようにしている。

【００２６】これにより、平行度を要求される往復直線
走行に対しても平行度を確保して蛇行発生を防止し、ま
た、曲率の大きな任意走行軌跡に対しても正確な追従走
行を実現することができる。さらに、刈り跡の段差その
ものが殆ど無い場合であっても、正確な往復走行を行う
ことができる。

【００２７】ここで、上記フォームマーカ検出部２５に
よるマーカ列検出について説明する。図１０に示すよう
に、上記フォームマーカ検出部２５には、ステレオ画像
処理回路２６、距離画像メモリ２７、ＣＰＵ２８、ワー
クメモリ２９、通信回路３０が備えられており、上記ス
テレオ画像処理回路２６では、２台のＣＣＤカメラ４
ａ，４ｂからの撮像画像を処理して画像のような形態を
した距離分布データ（距離画像）を生成し、距離画像メ
モリ２７に記憶する。上記ＣＰＵ２８では、上記距離画
像メモリ２７からデータを読み込み、上記ワークメモリ
２４を介してワークデータの記憶、データ参照を行いな
がらマーカ列検出処理を行い、その処理結果を上記通信
回路３０を介して上記走行制御部５０へ送信する。

【００２８】尚、上記距離画像の生成については、本出
願人によって先に提出された特開平５−１１４０９９号
に詳述されている。

【００２９】すなわち、自動芝刈機１の車体前方に設置
したステレオカメラである２台のＣＣＤカメラ４ａ，４
ｂで撮像した画像では、図１４に模式的に示すように、
地表面（芝地平面）に散布された白色のフォームによっ
て既刈り部と未刈り部との作業境界が形成されており、
左右の撮像画像を処理して距離画像に変換する。

【００３０】図１５に示すように、ＣＣＤカメラ４ａ，
４ｂからフォームマーカを形成する泡の塊までの距離
は、地表面（芝地平面）の泡の無い部分までの距離より
も短くなっており、上記ＣＰＵ２８では、図１に示すフ
ォームマーカ軌跡検出処理のプログラムを実行して上記
ステレオ画像処理回路２６から出力される距離画像から
泡の有無による距離の違いを検出して距離段差部分の直
線式を求め、この直線式のデータを直前のレーンの走行
軌跡データとして上記走行制御部５０へ送る。

【００３１】以下、上記ＣＰＵ２８で実行される走行軌
跡検出ルーチンについて、図１のフローチャートに従っ
て説明する。

【００３２】このルーチンでは、まず、ステップS200
で、図２の２次元多数決処理ルーチンを実行し、距離画
像内のある一定の広さの矩形エリア（小領域）内での多
数決処理によって不良距離データを排除する。

【００３３】すなわち、図２のステップS201で、指定さ
れた小領域で距離分布のヒストグラムを作成すると、ス
テップS202で、指定された小領域からデータを１つ取り
出し、図１６に示すように、その距離値を中心として指
定された幅のエリアにある分布数を求める。

【００３４】次いで、ステップS203へ進み、上記ステッ
プS202で求めた分布数が指定されたスレッシュ値を越え
ているか否かを調べ、分布数がスレッシュ値を越えてい
るとき、ステップS204へ進んで、取り出した距離値は有
効と判定してステップS206へ進み、分布数がスレッシュ
値以下のとき、ステップS205へ進んで、取り出した距離
値は無効と判定してステップS206へ進む。

【００３５】ステップS206では、小領域の全ての距離値
のチェックが終了したか否かを調べ、チェックが残って
いるときには、上記ステップS202へ戻ってチェックを続
け、全ての距離値のチェックが終了したとき、ステップ
S207へ進んで全画面に対して処理が終了したか否かを調
べる。

【００３６】そして、全画面の処理が終了していないと
きには、上記ステップS201へ戻って小領域の位置をずら
しながら以上の処理を繰り返し、全画面に対する処理が
終了したとき、ルーチンを抜けて走行軌跡検出ルーチン
へ戻る。

【００３７】以上の２次元多数決処理によって不良距離
データを排除すると、走行軌跡検出ルーチンのステップ
S300へ戻り、図３の２次元移動平均化処理ルーチンを実
行する。この２次元移動平均化処理ルーチンでは、距離
画像内のある一定の広さの矩形エリアで平均値を取り、
距離データの平滑化を行う。

【００３８】すなわち、図３のステップS301で指定され
た小領域内の距離値を平均し、ステップS302で、この平
均値を小領域内の代表とする。そして、ステップS303で
全画面に対する処理が終了したか否かを調べ、全画面に
対する処理が終了していないときには、上記ステップS3
01へ戻って小領域の位置をずらしながら平均化処理を続
け、全画面に対する処理が終了したとき、ルーチンを抜
けて走行軌跡検出ルーチンへ戻る。

【００３９】次に、走行軌跡検出ルーチンでは、ステッ
プS400,S450で、横方向のラプラス変換処理、縦方向の
ラプラス変換処理を実行し、距離画像の横方向ラプラス
変換、距離画像の縦方向ラプラス変換により、横方向及
び縦方向の距離段差部分の情報を得る。

【００４０】この場合、横方向ラプラス変換と縦方向ラ
プラス変換とは、処理方向が異なるのみで、実質的な内
容は同じであり、ここでは、ステップS400における図４
の横方向ラプラス変換処理ルーチンについて説明する。
この横方向ラプラス変換処理では、まず、ステップS401
で、図１７に示すように、画像横方向の隣接した距離値
Ｄ0，Ｄ1，Ｄ2を取得し、ステップS402で横方向２次微
分値Ｒを計算し、その場所のラプラス変換値とする。こ
の横方向２次微分値は、隣接データ間の差分の差分とし
て、Ｒ＝（Ｄ2−Ｄ1）−（Ｄ1−Ｄ0）＝Ｄ2−２×Ｄ1＋
Ｄ0によって計算することができ、距離変化が存在する
とき、ラプラス変換によって図１８に示すような段差情
報が得られる。

【００４１】次いで、ステップS403へ進み、１ラインの
処理が終了したか否かを調べ、１ラインの処理が終了し
ていないときには、上記ステップS401へ戻って処理対象
を横に１カラムずらして横方向のラプラス変換値を求め
る処理を繰り返し、１ラインの処理が終了すると、上記
ステップS403からステップS404へ進んで全画面の処理が
終了したか否かを調べる。そして、全画面の処理が終了
していない段階では、上記ステップS404から最初のステ
ップS401へ戻って処理対象を縦に１ラインずらして上述
の処理を繰り返し、全画面の処理を行う。

【００４２】同様にして、ステップS450の縦方向のラプ
ラス変換処理を全画面について行った後、走行軌跡検出
ルーチンでは、ステップS500へ進んでエッジ検出処理を
実行し、ラプラス変換後の段差情報に基づいて、＋，−
のピーク値と、その間隔とから段差のある場所をエッジ
として検出する。このエッジ検出処理を縦方向及び横方
向について行い、図１９に示すような段差検出点を得
る。

【００４３】この縦方向及び横方向のエッジ検出処理
は、互いに処理方向が異なるのみであるため、横方向エ
ッジ検出処理で代表して説明すると、図５のステップS5
01で、ラプラス変換後のデータを横方向にチェックし、
その値が局所的な最小値あるいは最大値となる極値か否
かを調べる。その結果、上記ステップS501で極値でない
ときには、ステップS506へ進んで１ラインの処理終了を
調べ、１ラインの処理が終了していないとき、上記ステ
ップS50１へ戻って横方向にラプラス変換値を再検索す
る。

【００４４】そして、ラプラス変換後のデータが極値で
あるとき、上記ステップS501からステップS502へ進ん
で、その極値の値と指定されたスレッシュ値とを比較
し、極値が極大値のときには、スレッシュ値を越えてい
る条件、極値が極小値のときには、スレッシュ値を下回
っている条件を満足するか否かを調べる。

【００４５】上記ステップS502において、極値が極大値
でスレッシュ値以下のとき、あるいは、極値が極小値で
スレッシュ値を下回っていないときには、エッジでない
と判断して上記ステップS502から１ラインの処理終了を
調べる前述のステップS506へ分岐し、極値が極大値でス
レッシュ値を越えているとき、あるいは、極値が極小値
でスレッシュ値を下回っているときには、エッジの可能
性があるため、上記ステップS502からステップS503へ進
む。

【００４６】ステップS503では、上記ステップS501,S50
2の条件を満足する極値の位置を調べ、極大値の次に極
小値が来ている条件、あるいは、極小値の次に極大値が
来ている条件を満足するか否かを調べ、条件を満足しな
いとき、同様に、前述のステップS506へ分岐し、条件を
満足するとき、ステップS504へ進んで極値間の距離が指
定された範囲内に収まっているか否かを調べる。

【００４７】そして、上記ステップS504で極値間の距離
が指定された範囲内に収まっているときには、ステップ
S505でエッジがあると判断し、次のエッジを検出すべく
ステップS506へ進む。すなわち、ラプラス変換後のデー
タ列の中で、指定スレッシュ値より上の＋のピーク値
と、指定スレッシュ値より下の−のピーク値とが連続し
て存在し、且つ、＋，−のピーク値の間が指定間隔内で
あるときには、そこがフォームマーカの有る場所と無い
場所との段差によるエッジであると判断するのである。

【００４８】以上の処理を繰り返し、１ラインの処理が
終了すると、ステップS506からステップS507へ進んで全
画面の処理が終了したか否かを調べ、全画面の処理が終
了していないとき、ステップS507から最初のステップS5
01へ戻って処理対象を縦に１ラインずらして上述の処理
を繰り返し、全画面の処理を行う。

【００４９】以上のエッジ検出処理により、段差検出点
を取得すると、走行軌跡検出ルーチンのステップS600へ
進み、図６のハフ変換処理ルーチンを実行して段差部分
の検出点からフォームマーカによる軌跡に相関のある直
線式を求める。

【００５０】このハフ変換処理ルーチンでは、ステップ
S601で、２つのエッジ検出点を選び、両点を結ぶ直線方
程式を求めてワークメモリ２４に記憶すると、ステップ
S602へ進んで全てのエッジ検出点の組み合わせが終了し
たか否かを調べ、ＮＯのとき、ステップS601へ戻って処
理を継続する。

【００５１】やがて、全てのエッジ検出点の組み合わせ
について直線方程式を求めると、上記ステップS602から
ステップS603へ進み、得られた全ての直線式のパラメー
タ（傾きａ’、切片ｂ’）についての分布ヒストグラム
を作成する。そして、ステップS604で、分布数が最大の
パラメータを検索し、このパラメータの直線をハフ変換
直線として走行軌跡検出ルーチンへ戻る。

【００５２】次に、走行軌跡検出ルーチンでは、ステッ
プS700で、図７の最小二乗近似処理ルーチンを実行し、
ハフ変換で求めた直線近傍の検出点を収集して、その点
の集合を用いて最小二乗法で直線式を再計算する。これ
により、白色のフォームマーカ以外の検出点を排除し、
画像分解能以上の精度で最終的な直線式を求めることが
できる。

【００５３】この最小二乗近似処理ルーチンでは、ステ
ップS701で、図２０に示すように、上記ハフ変換によっ
て得られた直線の近傍の全てのエッジ検出点を検索し、
ステップS702で、全てのエッジ検出点（ｎ個）につい
て、ｘ座標値の総和Σｘ、ｙ座標値の総和Σｙ、ｘ座標
値の二乗の総和Σ(ｘ・ｘ)、ｘ座標値とｙ座標値の積の
総和Σ(ｘ・ｙ)を計算する。

【００５４】次いで、ステップS703へ進み、上記ステッ
プS702で計算した各総和を用い、ａ＝（ｎ・Σ（ｘ・ｙ)
−Σｘ・Σｙ）／（ｎ・Σ(ｘ・ｘ)−Σｘ・Σｘ）によって
計算した値ａを近似直線の傾きとし、ステップS704で、
ｂ＝（Σｙ−ａ・Σｘ）／ｎによって計算した値ｂを近
似直線の切片として走行軌跡検出ルーチンに戻る。

【００５５】以上により、走行軌跡の近似直線式を決定
すると、走行軌跡検出ルーチンのステップS800では、こ
の近似直線式の傾きａ，切片ｂを軌跡データとして通信
回路３０を介して走行制御部５０へ送信し、ルーチンを
抜ける。

【００５６】走行制御部５０では、フォームマーカによ
る前レーンの走行軌跡を検出したことを確認して図８の
走行制御ルーチンを実行し、フォームマーカによる軌跡
に沿った自動芝刈機１の走行を制御して倣い走行を開始
する。また、この倣い走行中には、次レーン側のフォー
ム発生装置５から泡の塊を散布して次レーンのためのフ
ォームマーキングを行う。尚、前レーンの走行軌跡を示
すフォームマーカは、芝刈リール３の刈刃によって飛ば
され、刈り後の痕跡を残さない。

【００５７】この走行制御ルーチンは、本形態では、１
行程毎に往復しながら芝刈り作業を行なう形式で、地磁
気センサ等の方位検出センサによる車両進行方向をフォ
ームマーカによる軌跡データによって補正しながら走行
を行なう場合に、１レーン毎に実行されるルーチンであ
る。すなわち、まず、ステップS101で、推測航法位置検
出部３５で測位した現在の車両位置データとフォームマ
ーカ検出部２５で検出した軌跡直線データとから、現在
の車両横位置Ｇａとフォームマーカの軌跡位置との誤差
Ｅｐを求める。

【００５８】次いで、ステップS102へ進み、現在の車両
進行方位Ｇｐと軌跡方位との誤差Ｅａを求めると、ステ
ップS103で、現在の車両進行方位Ｇｐを軌跡方位との誤
差Ｅｐで補正するととともに、現在の車両横位置Ｇａを
軌跡位置との誤差Ｅａで補正し、車両進行方位修正角Ｏ
ａを求め（Ｏａ＝Ｇｐ・Ｅｐ＋Ｇａ・Ｅａ）、ステップ
S104ヘ進む。

【００５９】ステップS104では、現在方位角Ａに上記車
両進行方位修正角Ｏａを加算して車両目標方位角Ｒａと
し（Ｒａ＝Ａ＋Ｏａ）、ステップS105で、車両方位角が
車両目標方位角Ｒａとなるよう操舵制御部９０へ操舵指
令を出力し、前輪操舵用油圧制御弁２０を介して前輪操
舵機構部２１をサーボ制御する。

【００６０】その後、ステップS106へ進んで１レーンの
芝刈り作業が終了したか否かを調べ、１レーン未終了の
とき、上記ステップS101へ戻って以上の処理を繰り返
し、１レーンの芝刈り作業が終了すると、ルーチンを終
了する。そして、車両転回を行った後、再度、この走行
制御ルーチンが実行される。

【００６１】

【発明の効果】以上説明したように本発明によれば、走
行中に複数のマーカを散布して車体周辺の地表面にマー
カ列を形成し、車両に搭載したステレオカメラで車体周
辺を撮像して距離画像の距離データを微分した微分値の
変化に基づいてマーカによる距離段差を検出する。そし
て、この距離段差の検出点からマーカ列を近似するライ
ンを算出するため、平行度を要求される往復直線走行に
対しても平行度を確保して蛇行発生を防止することがで
き、また、作業跡を検出することが困難な場合の倣い走
行に対しても正確な追従走行を可能とすることができ
る。さらには、曲率の大きな任意刈り跡に対しても正確
な追従走行を実現することができる等優れた効果が得ら
れる。

【図面の簡単な説明】

【図１】本発明の一実施の形態に係わり、走行軌跡検出
ルーチンのフローチャート

【図２】同上、２次元多数決処理ルーチンのフローチャ
ート

【図３】同上、２次元平均化処理ルーチンのフローチャ
ート

【図４】同上、ラプラス変換処理ルーチンのフローチャ
ート

【図５】同上、エッジ検出処理ルーチンのフローチャー
ト

【図６】同上、ハフ変換処理ルーチンのフローチャート

【図７】同上、最小二乗近似処理ルーチンのフローチャ
ート

【図８】同上、走行制御ルーチンのフローチャート

【図９】同上、制御系のブロック図

【図１０】同上、フォームマーカ検出部のブロック図

【図１１】同上、自動芝刈機の外観図

【図１２】同上、フォーム発生装置の外観図

【図１３】同上、フォーム発生装置の断面図

【図１４】同上、フォームマーカの撮像画像を模式的に
示した説明図

【図１５】同上、カメラからフォームマーカまでの距離
を示す説明図

【図１６】同上、小領域での距離値ヒストグラム

【図１７】同上、距離画像に対する横方向ラプラス変換
の説明図

【図１８】同上、距離段差が存在したときのラプラス変
換後のデータを示す説明図

【図１９】同上、段差検出点を示す説明図

【図２０】同上、走行軌跡の近似直線を示す説明図

【図２１】従来の往復作業走行における走行軌跡を示す
説明図

【符号の説明】

１ …自動芝刈機 ４ａ，４ｂ…ＣＣＤカメラ ５ …フォーム発生装置 ２５ …フォームマーカ検出部 ２６ …ステレオ画像処理回路 ２８ …ＣＰＵ

Claims (6)

【特許請求の範囲】
1. 【請求項１】 車両の進行に伴って複数のマーカを散布
   し、車体周辺の地表面にマーカ列を形成する手段と、 車両搭載のステレオカメラで撮像した車体周辺の画像を
   処理して得られる距離画像に対し、距離データを微分し
   た微分値の変化に基づいて上記マーカによる距離段差を
   検出する手段と、 上記距離段差の検出点に基づいて上記マーカ列を近似す
   るラインを自車両の走行軌跡として算出する手段とを備
   えたことを特徴とする自律走行車の走行軌跡検出装置。
2. 【請求項２】 上記距離データの微分値に対し、閾値よ
   り上の極大値と閾値より下の極小値とが互いに連続して
   存在し、各極値間の距離が設定範囲内の位置を検出し、
   上記マーカによる距離段差とすることを特徴とする請求
   項１記載の自律走行車の走行軌跡検出装置。
3. 【請求項３】 上記距離段差の検出点からハフ変換によ
   って上記マーカ列を近似する直線を求めた後、さらに、
   この直線近傍の上記距離段差の検出点の集合から最小二
   乗法によって最終的な直線を求めることを特徴とする請
   求項１又は請求項２記載の自律走行車の走行軌跡検出装
   置。
4. 【請求項４】 上記距離画像に対して２次元多数決処理
   を行って不良距離データを排除した後、上記マーカによ
   る距離段差を検出することを特徴とする請求項１，２，
   ３のいずれか一に記載の自律走行車の走行軌跡検出装
   置。
5. 【請求項５】 上記距離画像に対して２次元移動平均化
   処理を行って距離データを平滑化した後、上記マーカに
   よる距離段差を検出することを特徴とする請求項１，
   ２，３のいずれか一に記載の自律走行車の走行軌跡検出
   装置。
6. 【請求項６】 上記不良距離データの排除後、さらに、
   ２次元移動平均化処理を行って距離データを平滑化する
   ことを特徴とする請求項４記載の自律走行車の走行軌跡
   検出装置。