JPH10253372A - 移動体の位置認識装置 - Google Patents

Abstract

(57)【要約】 【課題】 操舵角を用いずに移動体の現在位置を認識す
る。 【構成】 移動体の各車輪にロータリーエンコーダを設
けて回転数を検出し、移動体の旋回角をジャイロセンサ
で検出する。回転数と旋回角から各車輪から旋回中心ま
での距離を求め、各車輪を中心として旋回中心を通る複
数の円の交点から旋回中心を求める。移動体が旋回中心
を中心として旋回角だけ旋回したものとして位置の変化
を求め、移動体の現在位置を認識する。

Description

【発明の詳細な説明】

【０００１】

【発明の利用分野】この発明は、無人走行車等の移動体
の位置認識装置に関する。

【０００２】

【従来技術】無人走行車等の移動体の走行には、現在位
置の認識が必要である。移動体の走行経路は短い時間の
範囲では直線あるいは円と近似でき、直線運動をしてい
る移動体の現在位置を求めるのは簡単である。問題は円
運動時の位置認識で、従来法では、移動体の各車輪の操
舵角から旋回中心への方向を求め、例えば４つの車輪か
ら旋回中心へ引いた線の交点に旋回中心が有るものとす
る。旋回中心が分かれば、移動体から旋回中心までの距
離（旋回半径）も判明し、現在位置は旋回中心から既知
半径の円周に沿って、車輪の回転数等から求めた走行距
離分進んだ点で定まることになる。

【０００３】しかしながらこの手法では、操舵角を正確
に計測できることが位置認識の条件となる。操舵角の計
測には、各車輪に対して精密な調整を行い、車輪の向き
の変化を確実に測定できるようにする必要がある。この
調整は時間と高い技術とを要し、実際に精密な調整を施
すことは困難である。

【０００４】

【発明の課題】この発明の課題は、操舵角の調整無し
で、移動体の位置を認識できるようにすることにある
（請求項１〜３）。

【０００５】

【発明の構成】この発明は、移動体の位置の変化分を積
算して、移動体の現在位置を認識する装置において、前
記移動体は複数の車輪を備え、少なくともその２つの車
輪に回転数の検出手段を設けると共に、前記移動体の旋
回角を求めるための手段と、各車輪の回転数と前記の旋
回角を用いて各車輪から移動体の旋回中心までの距離を
求めるための手段と、各車輪を中心として旋回中心を通
る複数の円の交点から旋回中心の位置を求めるための手
段と、求めた旋回中心を中心に前記の旋回角だけ前記移
動体が旋回した際の位置の変化分と旋回前の座標とか
ら、移動体の新たな位置を求めるための手段とを設けた
ことを特徴とする。

【０００６】好ましくは、移動体は所定の走行経路を無
軌道で走行する無人走行車で、走行経路の所定位置にマ
ークを設けると共に、前記移動体にマークに対する移動
体の位置を求めて、現在位置を校正するための手段を設
ける。

【０００７】また好ましくは、旋回角を求めるための手
段を、光ジャイロセンサで構成する。

【０００８】

【発明の作用と効果】この発明では、移動体の少なくと
も２つの車輪にロータリーエンコーダ等の回転数の検出
手段を設けて、各車輪の走行距離を求める。またジャイ
ロセンサ等の旋回角を求める手段を設けて、車輪の回転
数と旋回角から、各車輪から移動体の旋回中心までの距
離を求める。例えば旋回中心から各車輪までの距離をｒ
iとし、各車輪の走行距離をＬi、旋回角を△θとする
と、車輪は半径ｒiの円周上で角度△θの弧を走行した
ものと考えられるので、 ｒi＝Ｌi／△θ の関係があ
る。そこで各車輪から旋回中心までの距離が定まる。

【０００９】移動体の少なくとも２つの車輪に回転数の
検出手段を設けたので、車輪を中心として旋回中心を通
る円が２つ以上求まる。そこでこれらの円の交点から旋
回中心の位置が定まる。旋回中心の位置が定まると、移
動体はこの点を中心として旋回角△θだけ旋回したもの
と考えられる。これが移動体の座標の変化分である。そ
こで前回の移動体の座標に、座標の変化分を加えること
によって移動体の現在位置を認識することができる。

【００１０】この発明では、移動体の現在位置の認識
に、車輪の操舵角を必要としない。このためこの発明で
は、簡単に移動体の現在位置を認識することができる。

【００１１】移動体の種類は任意であるが、好ましくは
所定の限られた走行経路を無軌道で走行する無人走行車
とし、走行経路の所定位置にマークを設ける。そして移
動体にはマークに対する移動体の位置を求めるための手
段を設けて、位置認識の結果を校正するようにする。旋
回角を求めるための手段としてはジンバル型のジャイロ
センサ等でも良いが、好ましくは光ジャイロセンサ、特
に好ましくは光ファイバジャイロセンサを用いる。光ジ
ャイロセンサはジンバル型のメカニカルなジャイロセン
サに比べて安価であり、かつ長寿命である。またメカニ
カルなジャイロセンサでは、ジンバルの回転が安定する
まで長時間ウォームアップする必要があるが、光ジャイ
ロセンサでは数分程度のウォームアップで計測を開始で
きる。

【００１２】

【実施例】図１〜図７に実施例を示す。図１に位置認識
装置の構成を示すと、１は工場や倉庫等で無人で物品を
搬送するための無人走行車で、２Ｒは右側の前輪、２Ｌ
は左側の前輪で、同様に３Ｒは右側の後輪で、３Ｌは左
側の後輪である。各車輪にはロータリーエンコーダ４を
取り付けて車輪の回転数を測定する。ロータリーエンコ
ーダ４は少なくとも２つの車輪に取り付ければよく、ま
た車輪の回転数を検出し得るものであれば任意のセンサ
を用いることができる。６は光ファイバジャイロセンサ
で、８は位置認識部、１０はＣＣＤカメラで他のカメラ
でも良い。１２はキャリブレーション部で、ＣＣＤカメ
ラ１０で撮影したマークの画像により現在位置を校正す
るためのものである。また１４はアンテナで、ホストコ
ンピュータ等から走行指令を受信する。

【００１３】図２に各センサからの信号の処理を示す
と、１６は除算部で、ロータリーエンコーダ４で求めた
走行距離Ｌi（iは１〜４で、車輪番号を示す）をジャイ
ロセンサ６で求めた旋回角△θで除算し、各車輪から旋
回中心までの距離ｒi（iは１〜４）を求める。１８は交
点算出部で、各車輪から半径ｒiの円を引いた際の交点
を求め、この点を無人走行車１の旋回中心とする。旋回
中心の座標Ｒは位置算出部２０に送られ、旋回中心Ｒを
中心として、旋回角△θだけ無人走行車１が旋回運動し
たものとし、この間の無人走行車１の座標の変化分△Ｐ
を求める。そして△Ｐを以前の座標Ｐに加えると新たな
座標が求まる。

【００１４】以上の処理は無人走行車１が旋回している
際の処理であり、ジャイロセンサ６は位置算出部２０に
θの変化を報告しており、θが一定で無人走行車１が直
線運動している場合、以前と同じ方向に各車輪の走行距
離Ｌiだけ無人走行車１が直線移動したものとして新た
な位置Ｐを求める。旋回運動と直線運動とで処理を切り
替えるのに用いるのがスイッチＳ１，Ｓ２で、θが一定
の直線運動時にはスイッチＳ１を閉じて、除算部１６や
交点算出部１８は用いない。また旋回運動時にはスイッ
チＳ２を閉じて、上記のようにして新たな位置Ｐを求め
る。

【００１５】キャリブレーション部１２は変位算出部２
２とマークの位置を記憶したマップ２３とからなり、位
置算出部２０から入力された無人走行車１の座標Ｐとか
ら、どのマークを観察しているかを決定して、そのマー
ク座標Ｍｐを出力する。一方変位算出部２２は、例えば
マークの２点をＣＣＤカメラ１０で撮影し、三角測量の
原理に従ってマークに対するＣＣＤカメラ１０の変位を
算出する。この変位を２次元座標でＱとすると、 Ｍｐ
＋Ｑ で無人走行車１の座標が定まり、これを位置算出
部２０にフィードバックして現在位置を校正する。

【００１６】図３に光ファイバジャイロセンサの構成を
示すと、２４はレーザダイオード等の光源で、２５はフ
ォトダイオード等の検出器である。２６はカプラーで、
光源２４からの光を分岐させて両側から光ファイバ２７
に送り、また光ファイバ２７から戻った光を検出器２５
へ送る。無人走行車１が旋回している場合、リング状の
光ファイバ２７を時計回りに回る光と、反時計回りに回
る光との間には位相差が生じ、この位相差を検出器２５
で検出する。そしてこの位相差は無人走行車１の角速度
（θの変化率）に比例し、これから旋回角△θを求め
る。光ファイバジャイロセンサ６はジンバル等のメカニ
カルな可動部が無いので長寿命である。またジンバルの
ように回転が安定するのを待つ必要が無いので、ウォー
ムアップ時間が数分程度と極めて短く、精密な調整が不
要なので安価である。

【００１７】図４に、車輪から旋回中心までの距離ｒi
の算出を示す。短時間での無人走行車１の運動は旋回を
伴う場合、旋回中心を中心とした弧状の運動と近似でき
る。そして弧の長さＬはロータリーエンコーダ４により
車輪の回転数から求めることができる。またこの弧の弧
Ｌiの角△θはジャイロセンサ６で求めることができ
る。旋回中心から車輪までの距離ｒiと弧の長さＬiとの
間には、 Ｌi＝ｒi／△θ の関係がある。このように
してロータリーエンコーダ４で求めた走行距離Ｌiとジ
ャイロセンサで求めた旋回角△θとから、距離ｒiを容
易に求めることができる。

【００１８】なおここで車輪自体の向きが精密に分かっ
ていれば、図４から直ちに旋回中心の座標も判明する。
しかしながらこのためには各車輪の車軸の向きを精密に
調整し、かつ車軸の向きの変化を操舵角として精密に測
定する必要がある。そしてこの測定は極めて困難であ
る。そこでこの発明では、複数の車輪に対して旋回中心
までの距離ｒiを求めて、これらから旋回中心の座標Ｒ
を求める。

【００１９】この機構を図５に示すと、例えば右前輪２
Ｒと左後輪３Ｌとについて、旋回中心までの距離を各々
求めたものとする。すると実際の旋回中心は右前輪２Ｒ
から所定の半径の円と左後輪３Ｌから所定の半径の円と
の交点に存在する。実施例では４つの車輪に各々ロータ
リーエンコーダ４を設けるので、演算で求まる交点の数
はさらに増加し、これらを平均化してより精密に旋回中
心の座標Ｒを求めることができる。

【００２０】旋回中心の座標Ｒが求まると、この間の無
人走行車１の位置の変化△Ｐは、旋回中心Ｒを中心とし
て旋回角△θだけ移動した点で定まる。無人走行車１の
元の座標に基づいて各車輪の元の座標が定まり、それに
基づいて旋回中心Ｒの座標が求まる。次に定まった旋回
中心の座標Ｒと旋回角△θとにより無人走行車１の位置
の変化分△Ｐが定まる。そして元の座標Ｐに△Ｐを加算
すると、無人走行車１の新たな座標が求まる。このステ
ップを繰り返すと、旋回運動が続く間の無人走行車１の
座標をほぼ連続的に求めることができる。また直線運動
の場合、それ以前の旋回角△θの積算値から、直線運動
時の無人走行車１の進行方向が判明する。ここで各車輪
の走行距離は直線運動では一定で、無人走行車１の運動
方向の向きに各車輪の走行距離だけ前進したものとして
新たな座標が定まる。

【００２１】図６に位置認識のキャリブレーションを示
す。無人走行車１は所定の走行経路を走行するが、この
走行経路には分岐やループ等があり、また許された走行
経路の幅の中で実際にどの位置を走行するかは様々であ
る。そして走行経路に沿って複数の位置にマーク３０が
設置してある。このマーク３０は図６のように線分とこ
れらの交点や端点からなるもので、幾何学的にはグラフ
として処理し得るものである。さてＣＣＤカメラ１０で
マーク３０を撮影すると、マーク上の例えば２点を用い
て三角測量の原理により、マーク３０に対するカメラ１
０の位置を求めることができる。例えば図６の場合、マ
ーク３０の２点３１，３２を用いてカメラ１０の位置を
求める。

【００２２】図２に示したように、キャリブレーション
部１２にはマップ２３があり、走行経路上の各マークの
座標が記録してある。そして無人走行車１のキャリブレ
ーション前の座標は位置算出部２０で求まり、これから
どのマークを見ているのかが分かる。なおマークを取り
違えることを防ぐためには、マーク毎にパターンを変
え、マップ２３に各マーク毎のパターンを記録しておけ
ば良い。さてＣＣＤカメラ１０で撮影した画像により、
マーク３０を基準とする無人走行車１の座標Ｑが定ま
り、マーク３０の座標はマップ２３でＭｐとして既知で
ある。そこでＭｐ＋Ｑ が無人走行車１の実際の座標と
なり、これを用いて位置算出部２０をキャリブレーショ
ンする。

【００２３】このようにすれば走行経路に凹凸等があ
り、これらによって旋回中心Ｒの座標の検出精度が低下
することを補うことができる。即ち図４，図５では無人
走行車１は平でスリップの無い平面を走行するものと仮
定してあるが、走行経路に凹凸があれば、ロータリーエ
ンコーダ４で求めた各車輪の走行距離と、実際の走行距
離とが異なり、その分の誤差が生じることになる。そし
てこのような誤差は積算されると徐々に増加する。そこ
で走行経路にマーク３０を設けることにより、誤差の累
積を防止できる。

【００２４】図７に、実施例での位置認識のアルゴリズ
ムを示す。無人走行車１は例えば１日に１回ジャイロセ
ンサ６をウォームアップし、かつマークが存在する位置
の付近でＣＣＤカメラ１０でそのマークを撮影し、初期
位置をキャリブレーションするものとする。このように
して位置の初期値Ｐを求める。また旋回角△θの初期値
は０で、最初の運動方向が角度θの０方向の向きとな
る。無人走行車１が走行を開始すると、角θが変化する
か否かから直線運動か旋回運動かを判別し、直線運動で
あれば各車輪の走行距離Ｌiをそのまま無人走行車１の
走行距離とし、角度θを用いて走行方向を決定して新た
な座標Ｐを得る。

【００２５】旋回運動を伴う場合、図４のようにして各
車輪に対して旋回中心までの距離ｒiを求める。次に図
５のようにして各車輪から半径ｒiの円の交点を求め、
この交点を旋回中心とし座標をＲとする。旋回中心から
無人走行車１の重心までの距離をρとすると、ρは無人
走行車１の元の重心座標と旋回中心Ｒの座標の差で定ま
り、旋回に伴う位置の変化△Ｐはρ△θで定まる。そし
て無人走行車１の座標を△Ｐだけ修正することにより新
たな座標が定まる。

【００２６】無人走行車がマーク３０の付近を通過する
と、ＣＣＤカメラ１０を用いてマーク上の２点を観察
し、これから三角測量の原理でマークからの変位Ｑを求
める。そしてマップ２３からマークの座標Ｍｐを求め、
Ｐ＝Ｍｐ＋Ｑ として座標位置を校正する。

【００２７】これらの結果、操舵角の測定無しで無人走
行車１の現在位置を認識でき、マーク３０とＣＣＤカメ
ラ１０で現在位置を校正でき、かつジャイロセンサには
安価で長寿命かつウォーミングアップが容易な光ファイ
バジャイロセンサ６を用いることができる。

【図面の簡単な説明】

【図１】 実施例の移動体の位置認識装置の配置を示す
図

【図２】 実施例の移動体の位置認識装置のブロック図

【図３】 実施例で用いた光ファイバジャイロセンサの
構成を示す図

【図４】 実施例での車輪から旋回中心までの距離の算
出を示すを特性図

【図５】 実施例での旋回中心の決定と車両座標の変化
分の算出を示す特性図

【図６】 実施例でのカメラとマークによる座標キャリ
ブレーションを示す特性図

【図７】 実施例での移動体の位置認識アルゴリズムを
示すフローチャート

【符号の説明】

１ 無人走行車 ２Ｒ，２Ｌ 前輪 ３Ｒ，３Ｌ 後輪 ４ ロータリーエンコーダ ６ 光ファイバジャイロセンサ ８ 位置認識部 １０ ＣＣＤカメラ １２ キャリブレーション部 １４ アンテナ １６ 除算部 １８ 交点算出部 ２０ 位置算出部 ２２ 変位算出部 ２３ マップ ２４ 光源 ２５ 検出器 ２６ カプラー ２７ 光ファイバ ３０ マーク ３１，３２ マーク上の点 Ｓ１，Ｓ２ スイッチ

Claims (3)

【特許請求の範囲】
1. 【請求項１】 移動体の位置の変化分を積算して、移動
   体の現在位置を認識する装置において、 前記移動体は複数の車輪を備え、少なくともその２つの
   車輪に回転数の検出手段を設けると共に、前記移動体の
   旋回角を求めるための手段と、各車輪の回転数と前記の
   旋回角を用いて各車輪から移動体の旋回中心までの距離
   を求めるための手段と、各車輪を中心として旋回中心を
   通る複数の円の交点から旋回中心の位置を求めるための
   手段と、求めた旋回中心を中心に前記の旋回角だけ前記
   移動体が旋回した際の位置の変化分と旋回前の座標とか
   ら、移動体の新たな位置を求めるための手段とを設けた
   ことを特徴とする、移動体の位置認識装置。
2. 【請求項２】 前記移動体は所定の走行経路を無軌道で
   走行する無人走行車で、前記走行経路の所定位置にマー
   クを設けると共に、前記移動体にマークに対する移動体
   の位置を求めて、現在位置を校正するための手段を設け
   たことを特徴とする、請求項１の移動体の位置認識装
   置。
3. 【請求項３】 前記旋回角を求めるための手段を、光ジ
   ャイロセンサで構成したことを特徴とする、請求項１ま
   たは２の移動体の位置認識装置。