JP3587623B2

JP3587623B2 - 位置推定装置

Info

Publication number: JP3587623B2
Application number: JP19036396A
Authority: JP
Inventors: 俊宏青野; 徳久三宅; 健二郎藤井; 有司松田; 慎太郎初本; 敬之神谷; 正美乙母; 憲二清野
Original assignee: Hitachi Ltd; Hitachi KE Systems Ltd
Current assignee: Hitachi Ltd; Hitachi KE Systems Ltd
Priority date: 1996-07-19
Filing date: 1996-07-19
Publication date: 2004-11-10
Anticipated expiration: 2016-07-19
Also published as: JPH1038585A

Description

【０００１】
【発明の属する技術分野】
本発明は、自己の位置を推定しながら自動で移動する自律走行車両に関する。
【０００２】
【従来の技術】
自律走行車両が決まった経路に沿って走行できるようにするものとしては、例えば特開平５ー１１９８３３号公報に記載のように、地中に埋設した誘導ケーブルから漏れ出す電磁波をたよりに誘導する方法、特開平４ー７７１６号公報に記載のように、地上に固定された反射板に光を投射し、その反射する方向から位置を推定する方法が知られている。◆
また、特開平５ー２８４８０６号公報に記載のように、方位センサーと距離センサーを用いて位置を推定するものもある。
【０００３】
【発明が解決しようとする課題】
上記従来の技術の項の最初に述べた、特開平５ー１１９８３３号公報に記載の例においては、地面を掘り起こしてケーブルを埋めるという余分な作業が必要になり、場合によってはケーブルの敷設が困難なところもあり、費用と時間との両面において不具合がある。また、特開平４ー７７１６号公報に記載の例では、反射板の設置、反射板の位置の測量、およびその結果の自律移動車への入力という多大な労力を要する。
【０００４】
これらの不具合を解消するために、特開平５ー２８４８０６号公報に記載の例では、ケーブルや反射板の設置や測量が不要な代わりに、方位センサーと距離センサーのデータを累積する位置推定法を用いているが、精度の面で要求される仕様を十分に満足しているとは言い難い。つまり、特開平５ー２８４８０６号公報に記載の位置推定法では車輪の滑りを考慮しておらず、カーブの回転半径などの条件によって車輪がすべることに起因する精度の低下を無視している。
【０００５】
本発明の目的は、上記従来技術の課題を解決し、カーブの回転半径により変化する車輪の滑りをも考慮して、速度および位置を推定すること、及びその方法を適用した位置推定装置を提供することにある。◆
本発明の他の目的は、簡単な方法で位置を推定できる信頼性の高い位置推定方法を提供することにある。
【０００６】
【課題を解決するための手段】
上記目的を達成するための本発明の第１の態様は、自律走行車両の位置を推定する位置推定装置において、前記車両の移動軌跡を記憶する移動軌跡記憶手段と、この車両に備えられた車輪の回転速度を検出する回転速度検出手段と、車両の回点半径と車輪に実効周長との関係を記憶する実効周長記憶手段とを設け、前記移動軌跡記憶手段に記憶された移動軌跡と、前記回転速度検出手段で検出された車輪の回転速度と、前記実効周長記憶手段に記憶された実効周長とに基づいて車両の位置を推定する推定手段を設けたものである。
【０００７】
そして、移動軌跡の所定位置を検出する位置検出手段を設ける、車両の回転角を検知する回転角検知手段を設け、この回転角検知手段が検知した回転角に基づいて車両の回転半径を推定する回転半径推定手段を設けたる、車両の移動距離を検出する移動距離検出手段と、前記車両の回転半径を検出する回転半径検出手段とを設け、前記推定手段はこの移動距離検出手段が検出した移動距離と、回転半径検出手段が検出した回転半径とに基づいて車両の位置を推定するようにしてもよい。また、移動距離検出手段はグローバルポジショニングシステム装置であってもよい。
【０００８】
上記目的を達成するための本発明の第２の態様は、自律走行する車両の位置を推定する位置推定方法において、車両の回転半径に基づいて車輪の実効周長を算出し、この実効周長に基づいて車両の進行速度を演算し、この演算した進行速度から車両の位置を推定するものである。
【０００９】
ところで、図１に模式的に示した３輪移動車において、前２輪で移動車の速度を推定する場合、あるいは図２に模式的に示した４輪移動車において、前２輪または後２輪で移動車の速度を推定する場合を考える。前２輪あるいは後２輪のように対になった２つの車輪の回転速度をｎ_１、ｎ_２とすると、滑りがなければ進行速度ｖは、２つの車輪の周の長さｄ_１、ｄ_２を用いて（式１）で表される。◆
ｖ＝（ｄ_１＊ｎ_１＋ｄ_２＊ｎ２_）／２ ……………（式１）
実際の移動車においては、車輪が滑らないという仮定が成り立つことは稀であり、速度の推定に高い精度を要求されるときは滑りも考慮して進行速度を推定する必要がある。滑りは、図３に示すように主として車体の回転半径ｒに依存し、大回りする場合は滑りが小さく、小回りする場合は滑りが大きい。そこで、滑りも考慮した１回転あたりの進行距離ｄ_１（ｒ）およびｄ_２（ｒ）を用いると、進行速度ｖは（式２）で表される。◆
ｖ＝｛ｄ_１（ｒ）＊ｎ_１＋ｄ_２（ｒ）＊ｎ_２｝／２ ………………（式２）
ｄ_１（ｒ）およびｄ_２（ｒ）を実効周長と呼ぶ。ｄ_１（ｒ）、ｄ_２（ｒ）の一例を図４に示す。この図４では、右向きの回転のときは回転半径を正で、左向きの回転のときは回転半径を負の値で表わしている。
【００１０】
一方、図３に示すように車体の進行角度をθで表すと、車体の位置（ｘ，ｙ）は（式３）および（式４）で表される。
【００１１】
ｘ＝∫ｖ・ｃｏｓθｄｔ ………………（式３）
ｙ＝∫ｖ・ｓｉｎθｄｔ ………………（式４）
（式３），（式４）を用いることにより、移動体の位置（ｘ，ｙ）を高精度に推定できる。
【００１２】
【発明の実施の形態】
以下に、本発明の一実施例を図面を用いて説明する。◆
図５に、本発明に係る速度推定装置の実施例の模式図を示す。自動移動車の対をなす前輪または後輪１、２には、エンコーダ３、４が各々設けられており、車輪の回転に応じてパルス信号を出力する。このパルス信号はカウンタ５、６に入力され、カウンタ５、６はパルス信号を回転速度に対応したディジタル信号、すなわち前述のｎ_１、ｎ_２に変換する。実効周長乗算器７、８は後述する方法により求められた回転半径を用いて実効周長メモリ１０、１１を参照して各々の車輪の実行周長を演算する。なお、実効周長メモリ１０、１１には予め図４に示したような回転半径と実効周長ｄ１（ｒ），ｄ２（ｒ）との関係が記憶されている。実効周長乗算器７、８においてはさらに、得られた実効周長にカウンタ５、６から送られた回転速度を乗算し、（式２）で示された車輪１、２の進行速度を求める。そして、平均器９において、各車輪の進行速度ｖの平均を求めることにより、移動車の速度を推定する。
【００１３】
つぎに、上記移動車の速度推定において、回転半径をどのように求めるかを以下に示す。図６に、回転半径を記憶手段に記憶された情報から求める場合の例を示す。◆
対をなす２つのエンコーダ３、４から出力されたパルス信号はカウンタ５、６に入力され、カウンタ５、６において回転速度に対応したディジタル信号に変換される。ところで、図示しない制御装置には、予め移動車の移動軌道が記憶されており、制御装置はこの軌道上を移動車が追従するよう制御指令を発生する。軌道の曲がり部に達するまでは、移動車は直進を続ける。このとき移動車は移動の原点（０，０）から点Ｐ（ｘ_０，ｙ_０）まで移動している。このときの移動距離λ_０はλ_０＝ＳＱＲＴ（ｘ_０ ^２＋ｙ_０ ^２）となる。
【００１４】
一方、車輪に設けられたエンコーダ３、４から出力されたパルスをカウンタ５、６が計測しており、このパルス数を累積して移動距離λに対応した値が求められる。ここで、移動距離λがλ_０に達したときに、移動車が向きを変えるように制御装置が指令を与える場合を考える。目標半径メモリ１２には、移動車の距離λがλ_０に達すると右に回転半径ｒ_０で回転する指令が記憶されているので、移動車に搭載した実効周長乗算器７、８は、移動距離λを用いて目標半径メモリ１３を参照し、回転半径を求める。以下の手順は図５に示した例と同様に実行される。速度の推定に用いる回転半径を得る他の例を、図７に示す。◆
上記実施例では、回転半径として目標軌道の半径を用いたが、実際の軌道の回転半径を実測して実効周長を求め、実効周長から速度を推定する。回転半径推定器１４は、移動車に設けられた角度センサ１３から出力された移動車の進行角度θと、左右の車輪の進行距離とから平均速度ｖを求める平均器９の出力を用いて、（式５）により移動体の回転半径ｒを演算する。
【００１５】
ｒ＝ｖ／Δθ …………（式５）
以下は、上記実施例と同様の方法で、速度の推定値を求める。
【００１６】
つぎに、このようにして得られた速度の推定値から、位置を推定する方法及び装置について図８を用いて説明する。角度センサと位置推定器が新たに移動車に付け加えられている。図５の実施例と同様に構成された速度推定装置１００から出力された速度の推定値ｖと、角度センサ１３から出力された移動車の角度θとから、位置推定器１４は（式３）、（式４）を用いて位置を推定する。
【００１７】
なお、上記位置推定においては、何らかの方法により得られた回転半径に基づいて実効周長を求めていたが、目標半径メモリに記憶されたデータに基づいて回転半径を求め、それにより実効周長を演算しても良い。この様子を図９に示す。速度推定装置１００から出力された速度ｖと移動車に設けられた角度センサ１３の出力である車体の角度θとを用いて、位置推定器１４が（式３）、（式４）を用いて位置を推定する。
【００１８】
なお、図７に示した実施例と同様の方法で回転半径を推定しても良い。この例を図１０に示す。速度推定装置１００から出力された速度ｖと角度センサ１３から出力された車体の角度θとを用いて、位置推定器１４は（式３）、（式４）を用いて位置を推定する。
【００１９】
次に上記種々の実施例において、実効周長メモリに記憶された車輪の回転数と移動車が実際に進行した距離の関係について説明する。図１１に、実際の進行距離を演算可能な実効周長設定装置一例を示す。移動車には上述した速度推定装置１００の他に、移動距離・回転半径計測器５１と回転数−距離比計算機５２、５３が設けられている。回転数−距離比計算機は、所定時間内に移動距離・回転半径計測器５１が計測した車体の移動距離とカウンタ５、６から出力された車輪の回転数の比を計算する。そして、この比を移動距離・回転半径計測器５１が計測した回転半径の関数として実効周長メモリ１０、１１に書き込む。これにより、速度推定および位置推定の際に用いられる実効周長が実効周長メモリに設定される。
【００２０】
なお、上記移動距離・回転半径計測器５１としては、グローバル・ポジショニング・システム（ＧＰＳ）５４を用いることができる。この例を図１２に示す。
【００２１】
ＧＰＳ５４は、移動車に設置されて移動車の位置を所定時間毎に計測する。刻々と検出される位置の時系列情報から移動距離と回転半径を計算する。この情報を用いて上述の実効周長を実効周長メモリに設定する。
【００２２】
つぎに、上記実施例に記載の位置推定装置を自走芝刈り機等の自律移動機に搭載した場合の自走運転について、図１３を用いて説明する。上述した位置推定装置に、目標経路を記憶する経路メモリ６１と、駆動部６３と、駆動部６３へ送る制御量を計算する制御量計算部６２とが付加されている。経路メモリ６１には、直線（ＬＩＮＥ）か曲線（ＡＲＣ）かの区別と、直線の場合はその始点（（ｘｏ，ｙｏ），（ｘ２，ｙ２））と、終点（（ｘ１，ｙ１），（ｘ４，ｙ４））の組が、曲線の場合は始点（（ｘ１，ｙ１），（ｘ４，ｙ４）），終点（（ｘ２，ｙ２），（ｘ５，ｙ５））及び回転中心（（ｘ３，ｙ３），（ｘ６，ｙ６））の組がデータとして記憶されている。制御量計算機６２はこのデータを１個づつ読み出し、位置推定器の出力と指定経路との偏差に基づいて制御量を計算し駆動部６３へ送信する。このとき、制御量計算機６２は経路データから目標経路の回転半径を計算し、目標メモリ１２に書き込む。このように構成することにより、自律運転が可能になる。そして、例えば芝刈り機に本発明を搭載すれば、予めプログラミングするか位置検出手段を芝刈り機に設けるだけで、自律運転により芝刈りが達成される。
【００２３】
【発明の効果】
本発明によれば、実効周長メモリに記憶された実効周長を車輪の周長の代わりに用いるので、滑りをも考慮した進行速度を推定することができる。これにより、位置の推定および速度の推定の少なくともいずれかの推定精度が向上する。
【図面の簡単な説明】
【図１】本発明に係る３輪自動車の模式図である。
【図２】本発明に係る４輪自動車の模式図である。
【図３】車体の進行角度と回転半径の関係を説明する図である。
【図４】本発明の一実施例に係る実効周長と回転半径との関係を示す図である。
【図５】本発明の一実施例にかかる位置推定装置のブロック図である。
【図６】図６に示した実施例の変形例のブロック図である。
【図７】本発明の一実施例に係る速度推定装置のブロック図である。
【図８】本発明に係る位置推定装置の他の実施例のブロック図である。
【図９】本発明に係る位置推定装置のさらに他の実施例のブロック図である。
【図１０】本発明に係る位置推定装置のさらに他の実施例のブロック図である。
【図１１】本発明に係る位置推定装置に備えられた実効周長設定装置の一例のブロック図である。
【図１２】本発明に係る位置推定装置に備えられた実効周長設定装置の他の例のブロック図である。
【図１３】本発明に係る位置推定装置の更に他の実施例のブロック図である。
【符号の説明】
１……車輪、２……車輪、３……エンコーダ、４……エンコーダ、
５……カウンタ、６……カウンタ、７……実効周長乗算機、
８……実効周長乗算機、９……平均器、１０……実効周長メモリ、
１１……実効周長メモリ、１２……目標半径メモリ、１３……角度センサ、
１４……回転半径推定器、１５……位置推定器、
５１……移動距離および回転半径計測器、５２……回転数および距離比計算器、
５３……回転数および距離比計算器、５４……ＧＰＳ、１００……速度推定器、
２０１……前輪、２０２……後輪。

Claims

自律走行車両の位置を推定する位置推定装置において、前記車両の移動軌道を記憶する移動軌道記憶手段と、この車両に備えられた車輪の回転速度を検出する回転速度検出手段（３、４）と、車両の回転半径と車輪の実効周長との関係を記憶する実効周長記憶手段（１１）とを設け、前記移動軌道記憶手段に記憶された移動軌道と、前記回転速度検出手段が検出した車輪の回転速度と、前記実効周長記憶手段に記憶された実効周長とに基づいて車両の速度を推定する速度推定装置（１００）を有することを特徴とする位置推定装置。
前記回転速度検出信号が検出した信号から車両の移動距離に変換する移動距離変換手段（５、６）を設けたことを特徴とする請求項１に記載の位置推定装置。
前記車両にこの車両の回転角を検知する角度センサ（１３）と、この角度センサが検知した回転角に基づいて車両の回転半径を推定する回転半径推定手段（１４）とを設け、前記速度推定手段は回転半径推定手段が推定した回転半径を用いて車両の実行周長を演算することを特徴とする請求項１に記載の位置推定装置。
前記車両にこの車両の回転角を検知する角度センサ（１３）と、この角度センサが検知した回転角に基づいて車両の回転半径を推定する回転半径推定手段（１４）とを設け、前記移動距離変換手段から得られた移動距離と、回転半径推定手段が推定した回転半径とに基づいて車両の位置を推定する位置推定手段を設けたことを特徴とする請求項２に記載の位置推定装置。
所定時間内の車両の移動距離を検出する移動距離・回転半径計測器（５１）を有し、この移動距離・回転半径計測器はグローバルポジショニングシステム装置であることを特徴とする請求項１に記載の位置推定装置。