JP2014002638A - 自律走行装置 - Google Patents

Abstract

【課題】距離センサにより計測される環境地図と予め記憶された環境地図に基づき自己位置状態を推定し、この推定された自己位置状態に基づき自律走行する自律走行装置において、好適な方向により双方の環境地図を比較解析し、相互のずれ量を好適に把握する。
【解決手段】自律走行装置１は、駆動輪１２，１３と、モータ６，７と、装置１周辺の環境地図を計測する距離センサ２０と、距離センサ２０からの計測信号に基づき、モータ制御信号を出力する制御器３０と、を備える。そして、制御器３０には所定の走行経路に対応した装置１周辺の環境地図が予め記録されており、制御器３０は、距離センサ２０からの距離信号に基づき装置１周辺の環境地図を形成し、形成される環境地図と予め記憶された環境地図を貪欲法により比較解析し、この比較解析により自己位置状態を推定し、自己位置状態に基づき所定の走行経路上を走行すべくモータ制御信号を出力する。
【選択図】図１

Description

この発明は、自律走行装置に関し、特に、レーザレンジファインダなどの距離センサにより走行時の環境地図を計測し、計測される環境地図と予め記憶された環境地図に基づき自己位置状態を推定し、この推定された自己位置状態に基づき自律走行する自律走行装置に関する。

上記のように、距離センサにより装置本体から周辺環境における各計測点までの距離を計測し、計測された周辺環境の距離情報（環境地図）に基づき自律走行する自律走行装置を示すものとして、例えば、特開２０１１−０４３４０５号公報（特許文献１）および特開２００８−２５０９０５号公報（特許文献２）がある。

これらに示される自律走行装置では、予め記憶された環境地図と、走行時に距離センサにより計測される環境地図とを比較し、比較された結果に基づいて自己位置状態（走行時における走行装置の位置および走行方向）が推定される。そして、この推定された自己位置状態に基づいて走行方向が制御される。

特開２０１１−０４３４０５号公報 特開２００８−２５０９０５号公報

上記のような自律走行装置においては、予め記憶された環境地図と、走行時に計測される環境地図とを比較することにより、自己位置状態（位置および走行方向）が推定され、この推定された自己位置状態に基づき走行制御が行われる。

ここで、自己位置状態の推定は、両方の環境地図の比較解析し、相互のずれ量（「位置ずれ」と「走行方向ずれ」）を把握することによりなされる。そして、双方の環境地図の比較解析による相互のずれ量の把握を好適な方法により行う手段が求められる。

そこで、この発明は、上述した事情に鑑みてなされたものであって、距離センサにより計測される環境地図と予め記憶された環境地図に基づき自己位置状態を推定し、この推定された自己位置状態に基づき自律走行する自律走行装置において、双方の環境地図を比較解析し、自己位置状態を好適かつ正確に把握することができる自律走行装置を提供することである。

上記の課題を解決するために、本発明の自律走行装置は、装置本体と、少なくとも一つは駆動輪である複数の車輪と、駆動輪を駆動するモータと、装置本体の周辺に存する物体までの装置本体からの距離を検出する距離センサと、少なくとも距離センサからの距離信号に基づき、モータ制御信号を演算する制御器と、バッテリに接続され制御器からのモータ制御信号に基づきモータに制御電力を供給するモータドライバと、を備える。

そして、制御器には、所定の走行経路に対応した装置周辺の環境地図が予め記憶されており、制御器は、距離センサからの距離信号に基づき装置周辺の環境地図を形成し、この形
成される環境地図と上記の予め記憶された環境地図を貪欲法により比較解析し、この比較解析により自己位置状態を推定するとともに、推定された自己位置状態に基づき前述の所定の走行経路上を走行すべくモータ制御信号を演算し出力することを特徴とする。

ここで、本発明における貪欲法による双方の環境地図の比較解析手法は、ＩＣＰアルゴリズムなどの他の比較解析手法に比して、簡易な比較解析手法であり、貪欲法によって双方の環境地図から正確に自己位置状態を推定でき、正確に自己位置状態を推定する手法として、貪欲法は好適な手法である。

本発明の自律走行装置は、距離センサにより計測される環境地図と予め記憶された環境地図に基づき自己位置状態を推定し、この推定された自己位置状態に基づき自律走行する自律走行装置であって、双方の環境地図を比較解析し、自己位置状態を好適にかつ正確に把握することができる。

本発明の実施形態における自律走行装置の上面図である。 本発明の実施形態における自律走行装置のシステムブロック図である。 本発明の実施形態の制御器で実行される解析を説明する模式図である。 本発明の実施形態の制御器で実行される環境地図の解析手法を説明するフローチャートである。 図４に示すフローチャートにおいて、サブシステム（修正処理）を説明するフローチャートである。

次に、この発明の実施形態の自律走行装置を図１に基づいて説明する。ここで、図１は、本発明の実施形態の自律走行装置１の上面図である。

図１に示すように、自律走行装置１は、主に、装置本体２と、装置本体２の前方に配置されたモータ（６，７）と、それぞれのモータ（６，７）に取り付けられた駆動輪（１２，１３）と、装置本体２の後方に配置された従動輪（１８，１９）と、制御装置１００と、バッテリＢＴと、回転センサ（８，９）と、距離センサ２０と、舵角センサ２１と、を備える。上記のように装置１は、２つの駆動輪（１２，１３）および２つの従動輪（１８，１９）からなる４つの車輪（１２，１３，１８，１９）を備える。また、制御装置１００は、制御器としてのマイクロコンピュータ（以下「マイコン」という）３０と、モータドライバ４０と、を備える。

装置本体２の前方における中央部には、軸部材３が固定配置されており、軸部材３には回転部材（ベアリング）４が回転自在に取り付けられている。そして、ベアリング４には操舵プレート５が取り付けられており、操舵プレート５は、ベアリング４を介して装置本体２に対し回転自在に取り付けられている。ここで、操舵プレート５には、舵角センサとしてのエンコーダ２１が設けられており、エンコーダ２１により、操舵プレート５の舵角φが計測される。

また、操舵プレート５の両端部には、回転軸（１０，１１）をそれぞれ有するモータ（６，７）が取り付けられている。そして、それぞれのモータ（６，７）には、回転軸（１０，１１）の回転を検出する回転センサとしてのエンコーダ（８，９）が取り付けられている。また、駆動輪（１２，１３）はモータ（６，７）の回転軸（１０，１１）に一体的に取り付けられている。そして、エンコーダ（８，９）により、回転軸（１０，１１）及び駆動輪（１２，１３）の回転位置が検出される。

上記のように駆動輪（１２，１３）が、モータ（８，９）を介してエンコーダ２１が設けられた操舵プレート５に取り付けられる。そのため、駆動輪（１２，１３）は、走行方向（回転駆動方向）を装置本体２に対して自在に変更することができる状態にて、操舵プレート５を介して装置本体２に設けられておる。そして、エンコーダ２１により操舵プレート５を介して駆動輪（１２，１３）の舵角が検出されることになる。

また、装置本体２の後方の左右にはそれぞれ固定部（１４、１５）が設けされており、それぞれの固定部（１４，１５）には車軸（１６，１７）が一体的に取り付けられている。そして、それぞれの車軸（１６，１７）に従動輪（１８，１９）が回転自在に取り付けられている。

装置本体２の先方の中央部には、距離センサとしてのレーザレンジファインダ（以下「ＬＲＦ」という。）２０が取り付けられている。ＬＲＦ２０は、装置本体２から装置本体２の周辺に存する物体までの距離（装置本体⇔物体）を検出する。ＬＲＦ２０は、略水平面内において装置本体２の前方を所定の角度範囲内において、放射方向にて所定の角度おきにスキャニングしながら、置本体２の周辺に存する物体までの距離を検出する。このように、本実施形態におけるＬＲＦ２０は、走行路面に対し平行な平面内において、装置本体２の周辺に存する物体までの距離を検出する。

そのため、ＬＲＦ２０により、装置１の前方および左右（一部後方も含む）に存する物体（周辺に存する物体）までの距離を所定の角度範囲内において網羅的に計測することができる。そして、このＬＲＦ２０からの距離信号ｄにより、装置本体２周辺の環境地図を形成することができる。なお、本実施形態におけるＬＲＦ２０は、所定の角度範囲として２７０°に設定されているとともに、０．２５°の角度おきにスキャニングすることができる。また、このスキャニングにより距離信号ｄから環境地図信号ｍを形成することができる。ここで、装置１にて形成される環境地図信号ｍは、上述のように走行路面に対し平行な平面内において検出される物体までの距離信号ｄより、２次元空間内の２値画像信号（「０」または「１」）として形成される。

制御装置１００におけるモータドライバ４０は、給電装置４１および電流センサ（４２，４３）を備える。そして、給電装置４１は、バッテリＢＴ、モータ（６，７）およびマイコン３０に接続されており、マイコン３０からのモータ制御信号ｃに対応し、バッテリＢＴから供給される電力を制御し、制御された制御電力をモータ（６，７）に供給する。ここで、電流センサ（４２，４３）は給電装置４１からモータ（６，７）に供給される制御電力の電流値Ｉを検出する。

また、制御装置１００におけるマイコン３０には、エンコーダ（８，９）からの回転位置信号ｒ、ＬＲＦ２０からの距離信号ｄ、エンコーダ２１からの舵角信号φ、および電流センサ（４２，４３）からの電流信号Ｉが供給され、これらの信号に基づきモータ制御信号ｃを演算し、出力する。

ここで、本発明の実施形態の自律走行装置１は、自律して、すなわち操作者又は乗客により走行方向を操作することなく、自動で予め定められた所定の走行経路上を略トレースして自動で走行することができる機能を有する。

そして、この機能は、以下のような手順により実現される。

ａ）前もって所定の走行経路上を操作者の操作におり装置１を走らせる（以下「教示走行」という）。この教示走行において、デッドレコニングにより、走行時の走行長さＬ、位
置（ｘ，ｙ）、走行方向θなどの自己位置状態を記録する。また、この自己位置状態の記録とともに、ＬＲＦ２０からの距離信号により形成される環境地図も記録する。すなわち、各自己位置状態に対応した環境地図が予め記録される。

ｂ）そして、自律走行時においては、ＬＲＦ２０からの距離信号により形成される環境地図と、予め記録された環境地図とを比較し、駆動輪（１２，１３）の舵角φの目標値φｒｅｆが演算され、この目標値φｒｅｆに基づき駆動輪（１２，１３）を駆動する各モータ（６，７）に供給される制御電流が制御される。この制御においては、自律走行時における装置１の自己位置状態もデッドレコニングにより計測され、計測された自己位置状態に関する情報、および教示走行における自己位置状態に関する情報がそれぞれ利用される。

上記の制御電流を決定する処理を行っているのは、マイコン３０である。以下、図２および図３に基づきマイコン３０内でなされる演算処理を、装置１の駆動状態と対比しながら説明する。

図２に示すように、マイコン３０には、外部から切替信号ｋ、エンコーダ２１からの舵角信号φ、ＬＲＦ２０からの距離信号ｄ、エンコーダ（８，９）からの回転位置信号ｒ、および電流センサ（４２，４３）からの電流値信号I、が供給され、これらの信号に基づきモータ制御信号ｃを演算し、モータドライバ４０の給電装置４１に供給する。

マイコン３０は、位置状態演算手段３１と、切替手段３２と、第１記録手段３３と、補正演算手段３４と、第２記録手段３５と、舵角指令演算手段３６と、制御信号演算手段３７と、を有する。なお、技術内容を明確に説明するため、マイコン３０内を各手段（３１〜３８）に分けて記載し、通常マイコン３０内の単一の処理装置でなされている処理を各手段（３１〜３８）で分担して処理されるよう記載した。また、本実施の形態は、マイコン３０は、上記のように単一の処理装置で構成されてよく、複数の処理装置で構成されてよい。

位置状態演算手段３１は、いわゆるデッドレコニングを行う処理部である。位置状態演算手段３１には、所定のサンプリング周期において、装置１の走行時における舵角信号φおよび回転位置信号ｒとが取り込まれる。そして、これらの信号（φ，ｒ）に基づき、所定の起点（スタート地点）から走行している装置１の自己位置状態が計測される。

位置状態演算手段３１によるデッドレコニングでは、自己位置状態として、イ）スタート地点を原点（０，０）とする装置１のｘｙ座標位置（ｘ，ｙ）、ロ）装置１の走行方向θ、および、ハ）スタート地点からの走行距離Ｌが、演算される。

切替手段３２は、教示走行若しくは自律走行に対応する外部からの切替信号ｋに基づき、マイコン３０内での信号の供給先を切り替える。すなわち、切替信号ｋに基づき、ＬＲＦ２０からの距離信号ｄ、位置状態演算手段３１からのイ）位置信号（ｘ，ｙ）、ロ）走行方向信号θ、およびハ）走行距離信号Ｌの供給先を切り替える。

（教示走行）
次に、教示走行時、すなわち、教示走行に対応する切替信号ｋが外部から切替手段３２に供給された場合におけるマイコン３０での処理について説明する。ここで、教示走行時には、位置状態演算手段３１からは、位置信号（ｘｍ，ｙｍ）、走行方向信号θｍおよび走行距離信号Ｌｍが供給される。また、距離信号ｄに基づき環境地図信号ｍｍがマイコン３０内で形成される。

第１記録手段３３には、位置信号（ｘｍ，ｙｍ）、走行方向信号θｍおよび環境地図信号
ｍｍが供給され、位置状態信号（ｘｍ，ｙｍ，θｍ）と、その位置状態における環境地図信号ｍｍとが、互いに関連づけられて記憶される。すなわち、各位置状態における環境地図が、第１記録手段３３に記録される。

第２記録手段３５には、教示走行時の走行距離信号Ｌｍ、走行方向信号θｍおよび指令速度信号ｖｒｅｆが記録される。ここで、走行方向信号θｍおよび指令速度信号ｖｒｅｆは、走行距離信号Ｌｍに関連づけられて記録されている。すなわち、所定のサンプリングで記録される各走行距離信号Ｌｍに対して、各走行方向信号θｍおよび各指令速度信号ｖｒｅｆは一義的に定まる。

（自律走行）
次に、自律走行時、すなわち、自律走行に対応する切替信号ｋが外部から切替手段３２に供給された場合におけるマイコン３０での処理について説明する。ここで、自律走行時には、位置状態演算手段３１からは、位置信号（ｘａ，ｙａ）、走行方向信号θａが供給される。また、距離信号ｄに基づき環境地図信号ｍａがマイコン３０内で形成される。

上記のように自律走行時においても、教示走行時と同様に位置状態演算手段３１によりデッドレコニングが行われ、エンコーダ（８，９）からの回転位置信号ｒおよびエンコーダ２１からの舵角信号φに基づき、自己位置状態が位置信号（ｘａ，ｙａ）・走行方向信号θａとして大凡推定される。しかし、駆動輪（１２，１３）の路面との滑りなどにより、位置信号（ｘａ，ｙａ）・走行方向信号θａからでは、正確に自己位置状態が推定できない場合がある。

そこで、補正演算手段３４により、教示走行時の環境地図信号ｍｍと、自律走行時の環境地図信号ｍａとが、比較解析され、この比較解析により自律走行時の自己位置状態が補正され正確に推定される。そして、補正演算手段３４により正確に推定された自己位置状態は、位置信号（ｘａ’，ｙａ’）・走行方向信号θａ’として、出力される。なお、補正演算手段３４による自己位置状態の補正方法の詳細については後述する。

補正演算手段３４により補正・推定された自己位置状態（ｘａ’，ｙａ’，θa’）は、舵角指令演算手段３６に供給される。図２および図３に示すように、舵角指令演算手段３６では、自己位置状態（ｘａ’，ｙａ’，θa’）と、第２記憶手段３５に記憶されている教示走行時における走行距離信号Ｌｍと走行方向信号θｍとに基づき、参照走行距離Ｌａが算出される。ここで、参照走行距離Ｌａは、図３に示すように、自律走行時の装置１の位置（ｘａ’，ｙａ’）と所定の走行経路とを、最短の距離にて結んだ線（垂線）の交点を求め、起点からこの交点までの教示走行時における走行距離である。

そして、舵角指令演算手段３６は、参照走行距離Ｌａと第２記憶手段３５とを照会し、参照走行距離Ｌａにおける走行方向信号θｍを求め、この走行方向信号θｍを目標とする方向を参照走行信号θｒｅｆとする。そして、さらに、この参照走行方向信号θｒｅｆから駆動輪（１２，１３）が旋回すべき目標舵角信号φｒｅｆを算出する。

そして、制御信号演算手段３７では、エンコーダ２１からの舵角信号φ、第２記憶手段３５からの目標速度信号ｖｒｅｆ、舵角指令演算手段３６からの目標舵角信号φｒｅｆ、回転位置信号ｒを速度信号ｖに変換する速度変換手段３８からの速度信号ｖ、電流センサ（４２，４３）からの電流値信号Ｉに基づき、モータ制御信号ｃを演算し、モータドライバ４０に供給する。

そして、モータドライバ４０は、モータ制御信号ｃに基づき、バッテリＢＴから供給される電力を制御し、制御された制御電力をモータ（６，７）に供給する。

（貪欲法）
次に、補正演算手段３４による装置１の自己位置状態の推定について、図３、並びに、図４および図５に基づき説明する。なお、図４および図５は、補正演算手段３４における情報の処理の内容を示すフローチャートである。

補正演算手段３４では、教示走行時に予め記録された環境地図信号ｍｍと、それに対応する自律走行時における環境地図信号ｍａとが、比較解析される。装置１の教示走行・自律走行における自己位置状態は、これまでに説明したように、ともにデッドレコニングによりにより把握される。そして、補正演算手段３４により、比較解析される環境地図信号（ｍｍ，ｍａ）は、それぞれデッドレコニングにより推定される位置状態（（ｘｍ，ｙｍ，θａｍ）、（ｘａ，ｙａ，θａ））における環境地図信号である。

ここで、上記の自律走行時の自己位置状態は、デッドレコニングのよりある程度の確からしさをもって、教示走行時の自己位置状態と略同程度の自己位置状態であると認識されるものである。本実施形態の補正演算手段３４は、このある程度の確からしいと判断される自律走行時の自己位置状態（ｘａ，ｙａ，θａ）を、相互の環境地図信号（ｍｍ，ｍａ）をそれぞれ比較解析にすることにより、デッドレコニングにより把握される自己位置状態（ｘａ，ｙａ，θａ）を補正し、すなわち、自律走行時の自己位置状態をより正確に把握し、把握されたより正確な自己位置状態（ｘａ’，ｙａ’，θａ’）を求めるものである。

上記の環境地図信号（ｍｍ，ｍａ）の比較解析による自律走行時の自己位置状態の貪欲法による推定は、概ね下記の考え方に基づき行われる。

例えば、図３に示されるように装置１の右舷前方の物体Ｏを環境地図信号（ｍｍ，ｍａ）として計測する場合について説明する。デッドレコニングにより把握される自律走行時の自己位置状態（ｘａ，ｙａ，θａ）が、正確に自己位置状態を推定するものである場合、自律走行時に計測される物体Ｏに関する環境地図信号ｍａは、教示走行時に計測される物体Ｏに関する環境地図図信号ｍｍを、双方の自己位置状態の差分を加算すれば、加算された環境地図図信号ｍｍと略一致したものとなる。

しかしながら、駆動輪（１２，１３）の滑り等により、デッドレコニングにより把握される自律走行時の自己位置状態（ｘａ，ｙａ，θａ）が真の自己位置状態からずれている場合、上記のように環境地図図信号ｍｍに双方の自己位置状態の差分を加算したとしても、双方の環境地図信号（ｍｍとｍａ）は、一致点は少ないものとなってしまう。

そこで、補正演算手段３４では、とりあえず、デッドレコニングにより把握される相互（教示走行および自律走行時）の自己位置状態の差分を用いて、この環境地図信号ｍａを２次元剛体変換により変換する。そして、双方の環境地図信号（ｍｍ，（変換後の）ｍａ）の何れか一方を固定し、他の一方を微小の「長さ」および「角度」にて少しずつ遷移させる。

ここで、前記の微小の「長さ」および「角度」による遷移は、双方の環境地図信号（ｍｍ，ｍａ）の一致点が、より大きくなるように検査しながら行われる。そして、この一致点が最大となる「長さ（座標）」および「角度」を補正位置状態量（ｄｘ，ｄｙ，ｄθ）として把握し、この補正位置状態をデッドレコニングにより把握される自律走行時の自己位置状態（ｘａ，ｙａ，θａ）に加算（若しくは減算）することにより、補正された自律走行時の自己位置状態（ｘａ’，ｙａ’，θａ’）を求める。

次に、図４および図５に示されるフローチャートに基づき、補正演算手段３４にける情報の処理の内容を具体的に説明する。

補正演算手段３４での処理が開始されると（Ｓ１）、デッドレコニングにより計測される教示および自律走行時の自己位置状態の差分により、補正位置状態量（ｄｘｃｕｒ，ｄｙｃｕｒ，ｄθｃｕｒ）を算出する（ｄｘｃｕｒ＝ｘａ−ｘｍ，ｄｙｃｕｒ＝ｙａ−ｙｍ，ｄθｃｕｒ＝θａ−θｍ）（Ｓ２）。そして、算出された補正位置状態量（ｄｘｃｕｒ，ｄｙｃｕｒ，ｄθｃｕｒ）を計算位置状態量（ｄｘｔｒｙ，ｄｙｔｒｙ，ｄθｔｒｙ）に設定する（Ｓ３）。そして、この計算位置状態量により環境地図信号ｍａの位置角度を２次元剛体変換により変換させ、環境地図信号ｍｍと変換後の環境地図信号ｍａの比較のより初期スコアＳを求め、求められた初期スコアＳを記憶する（Ｓ４）。

ここで、スコアＳは、教示走行時の環境地図信号ｍｍと変換後の環境地図信号ｍａの一致率を示すものであり、以下の式にて求められる。
Ｓ＝｜（環境地図信号ｍｍ）∩（遷移後の環境地図信号ｍａ）｜／｜環境地図信号ｍｍ｜ここで、Ｓの分母は、環境地図信号ｍｍにおいて、物体の存在を示す画素（＝１）の画素数を示す。Ｓの分子は、双方の環境地図信号（ｍｍ，ｍａ）における物体の存在を示す画素（＝１）において、ともに重なる画素の画素数を示す。

次に、環境地図信号ｍｍを微小の「長さ」および「角度」において遷移するための「遷移量」が設定される（Ｓ５）。なお、この「遷移量」によって環境地図信号ｍｍを遷移させることによって、双方の環境地図信号（ｍｍ，（変換後の）ｍａ）の一致状況から適正な補正位置状態量を把握することができる。ここで、微小の「長さ」として、ΔｘおよびΔｙはともに２００ｍｍに、微小の「角度」としてΔθは１°に初期設定される。また、フラグは、スコアＳが上昇したことを示す値として初期的に「１」が設定される（Ｓ５）。

ここで、本実施の形態における貪欲法では、「遷移量」を三段階に精密化する手法が用いられる（Ｓ６Ａ〜Ｓ６Ｂ）。すなわち、初期的には「遷移量」として、Δｘ・Δｙを２００ｍｍに、Δθを１°に設定して（Ｓ５）、１回目の遷移処理（Ｓ１００）を行う。そして、１回目の遷移処理（Ｓ１００）の後に、「遷移量」を半分（Δθ＝Δθ／２，Δｘ＝Δｘ／２，Δｙ＝Δｙ／２）にして（Ｓ８）、２回目の遷移処理を行う（Ｓ１００）。そして、２回目回の遷移処理（Ｓ１００）の後に、「遷移量」をさらに半分（Δθ＝Δθ／２，Δｘ＝Δｘ／２，Δｙ＝Δｙ／２）にして（Ｓ８）、３回目の遷移処理を行う（Ｓ１００）。

次に、本実施の形態における貪欲法における遷移処理（Ｓ１００）について説明する。なお、上記に示す１回目から３回目までの遷移処理（Ｓ１００）は、スコアＳが上昇しなくなるまで実効される（ｗｈｉｌｅ（ｆｌｇ＝＝１））（Ｓ７Ａ〜Ｓ７Ｂ）。すなわち、遷移処理（Ｓ１００）において、教示走行時の環境地図信号ｍｍと変換後の環境地図信号ｍａの一致率が上昇しなくなるまで環境地図信号ｍｍを遷移させる処理がなされる。すなわち、これにより適正な補正位置状態量を把握することができる。

図５に示すように、本実施の形態における貪欲法における遷移処理（Ｓ１００）は、フラグを「０」に設定する処理から始まる（Ｓ１０１）。そして、θ、ｘ、ｙの順に計算位置状態量（ｄθｔｒｙ→ｄｘｔｒｙ→ｄｙｔｒｙ）を遷移させ、スコアＳの上昇の有無が判断される（Ｓ１０２〜Ｓ１０８，Ｓ１０９〜Ｓ１１５，Ｓ１１６〜Ｓ１２２）。

以下、図５に基づき、計算位置状態量について、ｄθｃｕｒ→ｄｘｃｕｒ→ｄｙｃｕｒの順に説明する。

（ｄθｃｕｒについて）
まず、θの計算位置状態量ｄθｔｒｙが、補正位置状態量ｄθｃｕｒに遷移量Δθを加算した値として設定される。ここで、ｘ、ｙの計算位置状態量（ｄｘｔｒｙ，ｄｙｔｒｙ）は、補正位置状態量（ｄｘｃｕｒ，ｄｙｃｕｒ）がそのまま設定される（Ｓ１０２）。そして、スコアＳが計算され（Ｓ１０３）、スコアＳが上昇したか否か判断される（Ｓ１０４）。

ここで、スコアＳが上昇したと判断された場合には、θの補正位置状態量ｄθｃｕｒとして、上記の計算位置状態量ｄθｔｒｙが設定され、フラグが「１」に設定される。一方、スコアＳが上昇したと判断されない場合には、処理Ｓ１０６が行われる。

処理Ｓ１０６では、θの計算位置状態量ｄθｔｒｙが、補正位置状態量ｄθｃｕｒに遷移量Δθを減算した値として設定される。ここで、ｘ、ｙの計算位置状態量（ｄｘｔｒｙ，ｄｙｔｒｙ）は、補正位置状態量（ｄｘｃｕｒ，ｄｙｃｕｒ）がそのまま設定される（Ｓ１０６）。そして、スコアＳが計算され（Ｓ１０７）、スコアＳが上昇したか否か判断される（Ｓ１０８）。

ここで、スコアＳが上昇したと判断された場合には、θの補正位置状態量ｄθｃｕｒとして、上記の計算位置状態量ｄθｔｒｙが設定され、フラグは「１」に設定される。一方、スコアＳが上昇したと判断されない場合には、次に処理Ｓ１０９が行われる。

（ｄｘｃｕｒについて）
同様に、まず、ｘの計算位置状態量ｄｘｔｒｙが、補正位置状態量ｄｘｃｕｒに遷移量Δｘを加算した値として設定される。ここで、ｙ、θの計算位置状態量（ｄｙｔｒｙ，ｄθｔｒｙ）は、補正位置状態量（ｄｙｃｕｒ，ｄθｃｕｒ）がそのまま設定される（Ｓ１０９）。そして、スコアＳが計算され（Ｓ１１０）、スコアＳが上昇したか否か判断される（Ｓ１１１）。

ここで、スコアＳが上昇したと判断された場合には、ｘの補正位置状態量ｄｘｃｕｒとして、上記の計算位置状態量ｄｘｔｒｙが設定され、フラグは「１」に設定される。一方、スコアＳが上昇したと判断されない場合には、処理Ｓ１１３が行われる。

処理Ｓ１１３では、ｘの計算位置状態量ｄｘｔｒｙが、補正位置状態量ｄｘｃｕｒに遷移量Δｘを減算した値として設定される。ここで、ｙ、θの計算位置状態量（ｄｙｔｒｙ，ｄθｔｒｙ）は、補正位置状態量（ｄｙｃｕｒ，ｄθｃｕｒ）がそのまま設定される（Ｓ１１３）。そして、スコアＳが計算され（Ｓ１１４）、スコアＳが上昇したか否か判断される（Ｓ１１５）。

ここで、スコアＳが上昇したと判断された場合には、ｘの補正位置状態量ｄｘｃｕｒとして、上記の計算位置状態量ｄｘｔｒｙが設定され、フラグが「１」に設定される。一方、スコアＳが上昇したと判断されない場合には、次に処理Ｓ１１６が行われる。

（ｄｙｃｕｒについて）
同様に、まず、ｙの計算位置状態量ｄｙｔｒｙが、補正位置状態量ｄｙｃｕｒに遷移量Δｙを加算した値として設定される。ここで、ｘ、θの計算位置状態量（ｄｘｔｒｙ，ｄθｔｒｙ）は、補正位置状態量（ｄｘｃｕｒ，ｄθｃｕｒ）がそのまま設定される（Ｓ１１６）。そして、スコアＳが計算され（Ｓ１１７）、スコアＳが上昇したか否か判断される（Ｓ１１８）。

ここで、スコアＳが上昇したと判断された場合には、ｙの補正位置状態量ｄｙｃｕｒと
して、上記の計算位置状態量ｄｙｔｒｙが設定され、フラグは「１」に設定される。一方、スコアＳが上昇したと判断されない場合には、処理Ｓ１２０が行われる。

処理Ｓ１２０では、ｙの計算位置状態量ｄｙｔｒｙが、補正位置状態量ｄｙｃｕｒに遷移量Δｙを減算した値として設定される。ここで、ｘ、θの計算位置状態量（ｄｘｔｒｙ，ｄθｔｒｙ）は、補正位置状態量（ｄｘｃｕｒ，ｄθｃｕｒ）がそのまま設定される（Ｓ１２０）。そして、スコアＳが計算され（Ｓ１２１）、スコアＳが上昇したか否か判断される（Ｓ１２２）。

ここで、スコアＳが上昇したと判断された場合には、ｙの補正位置状態量ｄｙｃｕｒとして、上記の計算位置状態量ｄｙｔｒｙが設定され、フラグが「１」に設定される。

上記の遷移処理（Ｓ１００）はスコアＳが「０」になるまで繰り返され（Ｓ７Ａ）、前述のように「遷移量」を三段階に精密化して行われる（Ｓ６Ａ〜Ｓ６Ｂ）。

そして、上記の一連の処理（Ｓ２〜Ｓ６Ｂ）がなされた後に、スコアＳが予め定められた設定スコアｔｓを超えるか否か判断する（Ｓ９）。ここで、スコアＳが設定スコアｔｓを超えると判断された場合には、上記の一連の処理（Ｓ２〜Ｓ６Ｂ）により得られた補正位置状態量（ｄｘｃｕｒ，ｄｙｃｕｒ，ｄθｃｕｒ）を用い、この補正位置状態量のより自律走行時の自己位置状態（ｘａ，ｙａ，θａ）が適正に推定され（Ｓ１０）、処理を終了する（Ｓ１１）。なお、本実施の形態では、設定スコアｔｓは「０．３」に設定されている。

ここで、スコアＳが設定スコアｔｓを超えないと判断された場合には、上記の一連の処理（Ｓ２〜Ｓ６Ｂ）のより得られた補正位置状態量（ｄｘｃｕｒ，ｄｙｃｕｒ，ｄθｃｕｒ）を用いることなく、デッドレコニングにより求められた値により自己位置状態（ｘａ，ｙａ，θａ）を推定する。

本実施形態のマイコン３０内で処理される環境地図信号（ｍｍ，ｍａ）は、上述のように２次元空間内の２値画像信号である。そのため、かかる信号（ｍｍ，ｍａ）にて行われる貪欲法を用いた本実施形態における論理積演算は、３次元空間内の環境信号に基づく従来の論理積演算に比して、処理すべき情報量が少なく、通常の処理装置においても、非常に高速に演算されることが可能である。

なお、本発明は上述の実施形態に限られるものではなく、本発明の趣旨を逸脱しない範囲において、上述の実施形態に種々の変更を加えたものを含む。

１…自律走行装置、２…装置本体、３…軸部材、４…回転部材、５…操舵プレート、６…モータ、７…モータ、８…回転センサ（モータエンコーダ）、９…回転センサ（モータエンコーダ）、１０…回転軸、１１…回転軸、１２…車輪（駆動輪）、１３…車輪（駆動輪）、１４…固定部、１５…固定部、１６…車軸、１７…車軸、１８…車輪（従動輪）、１９…車輪（従動輪）、２０…距離センサ（レーザレンジファインダ）、２１…舵角センサ（エンコーダ）、３０…制御器（マイクロコンピュータ）、３１…位置状態演算手段、３２…切替手段、３３…第１記録手段、３４…補正演算手段、３５…第２記憶手段
３６…舵角指令演算手段、３７…制御信号演算手段、３８…速度変換手段、４０…モータドライバ、４１…給電装置、４２…電流センサ、４３…電流センサ、１００…制御装置、ＢＴ…バッテリ

Claims (1)

1. 装置本体と、少なくとも一つは駆動輪である複数の車輪と、前記駆動輪を駆動するモータと、前記装置本体から前記装置本体の周辺に存する物体までの距離を検出する距離センサと、少なくとも前記距離センサからの距離信号に基づき、モータ制御信号を演算する制御器と、バッテリに接続され前記制御器からのモータ制御信号に基づき前記モータに制御電力を供給するモータドライバと、を備える自律走行装置において、
   前記駆動輪は走行方向を自在に変更できる状態にて前記装置本体に設けられており、
   前記制御器には、所定の走行経路に対応した当該装置周辺の環境地図が予め記憶されており、
   前記制御器は、前記距離センサからの距離信号に基づき当該装置周辺の環境地図を形成し、前記形成される環境地図と前記予め記録された環境地図を貪欲法により比較解析し、前記比較解析により自己位置状態を推定し、前記推定された自己位置状態に基づき略前記所定の走行経路上を走行できるよう前記駆動輪の走行方向を制御すべく前記モータ制御信号を演算し、演算された前記モータ制御信号を出力することを特徴とする自律走行装置。

Images