JPH02161373A - 移動体の位置検知方法および装置 - Google Patents

Abstract

(57)【要約】本公報は電子出願前の出願データであるた
め要約のデータは記録されません。

Description

【発明の詳細な説明】 （産業上の利用分野） 本発明は、位置が既知な３つの光反射手段と移動体との
角度を検出して移動体の位置を検知する装置に関するも
のである。

Ｃ発明の背景） 移動体の位置を検知する手段として、車輪の回転から移
動量と移動方向とを計測し加算してゆくことが考えられ
るが、この方法では誤差が累積して不正確になる問題が
ある。

そこで相対位置が既知な３つのりフレフタを用い、移動
体から２つのりフレフタの挟角を検知して移動体の位置
を求める方法が提案された（特開昭５９−６７４７６号
参照）。すなわちこれは２つのりフレフタをつなぐ距離
が既知の一辺を弦とし、この弦を挟む前記挟角の軌跡に
よって決まる２つの円を求め、２つの円の交点として移
動体の位置を決めるものである。しかしこの方法は３つ
のりフレフタの反射光を同時に検知して２つの挟角を同
時に知ることを前提とするものであり、２つの挟角の同
時検出ができない場合には不可能または不正確、になる
という不都合があった。

（発明の目的） 本発明はこのような事情に鑑みなされたものであり、固
定位置が既知の３つの光反射手段に対する移動体の方位
検知を時間差をもって行っても移動体の位置を正確に検
知することが可能な移動体の位置検知装置を提供するこ
とを目的とする。

（発明の構成） 本発明によればこの目的は、固定位置が既知の３個の光
反射手段により、移動体からの射出光を移動体方向へ反
射して移動体の位置を検出する装置において、移動体に
より各光反射手段からの受光方向を検出する角度検出手
段と、移動体の移動量を求める移動量検出手段と、移動
体の移動方向を求め移動方向検出手段と、演算手段とを
備え、前記演算手段は移動体の３つの位置での各光反射
手段の方向と、その位置間の移動量および移動方向とを
求め、これら３つの位置での移動体の移動方位ベクトル
と受光方位ベクトルとの内積な用いて移動体の位置を演
算することを特徴とする移動体の位置検知装置により達
成される。

（原理） この構成により前記目的が達成できることを第４図に基
づき説明する。

この図において、Ｐｏは移動体の現在位置、ＰＩ、Ｐｉ
はそれぞれ成る時間前における移動体の位置である。座
標系χ−ｙを図のようにとり、各位置の座標を次のよう
に決める。

Ｐ、（χ、ｙ） ｐ、（χ−Δχ＋＋　ｙ−Δｙ＋） ｐ、（χ−Δχ２１３’−Δｙｚ） ここにχ、ｙが求めようとする座標であり、Δχ１．Δ
χ２．Δｙｌｌ　Δｙ２は移動体の移動量と移動方向と
を監視することにより求めることができる。

また移動体の位置Ｐ０におけるχ軸方向に対する移動方
向を、θと仮定すれば、位置Ｐ、、Ｐ。

における移動方向はθ−Δθ３、θ−Δθ２として求め
られる。ここにΔθ１、Δθ２は、移動中の移動方向の
変化量であるから移動中監視し続けることにより求めら
れる。

Ｒ＋　、Ｒｚ　、Ｒｓはコーナーキューブなどで作られ
た光反射手段としてのりフレツクであり、これらの座標
は図に示すように既知である０位置ＰＧ、ＰＩ、Ｐ２に
おける移動方向とりフレツクＲ＋　、Ｒａ　、Ｒ３によ
る受光方向とのなす角（受光角）をα、β、γとすれば
、この受光角も検出可能である。

位置Ｐ０．Ｐ＋　、Ｐ２における移動方向を示す方位ベ
クトルυ（υ３．υ２．υ３）を次のように定義する。

υ、　＝　（ｃｏｓθ、　ｓｉｎθ） υ、　＝　（ｃｏｓ　　（θ−Δθｌ）。

ｓｉｎ　　（θ−Δθ＋）） υ、　＝　（ｃｏｓ　　（θ−Δθ２）。

ｓｉｎ　　（θ−Δθ２）） またリフレクタＲに対する方位と距離を示す受光ベクト
ルａ、ｂ、ｃを次式で定義する。

ａ＝（χ１１−χ、ｙ□−ｙ） ｂ＝（χｒ２−χ＋ΔＸ＋、ｙｒｚ３’＋Δｙ＋）ｃ＝
（χｒ３−χ＋Δχｍ＋３’ｒｓ−ｙ＋Δｙｇ）従って
内積（スカラー積）の定義から、ａ・　υ。＝　（χ１
．−χ）ｃｏｓ゛θ＋（ｙｒ＋−ｙ）　　ｓｉｎ　　θ ＝ｌａｌｌ　υ、１ｃｏｓａ　　　　＝１１）（１）　
　　（２）、（３）の連立方程式の解としてχ、ｙ、θ
を求めることができる。すなわち本発明は、過去の位置
Ｐ１．Ｐ２における方位ベクトルυと、受光ベクトルａ
、ｂ、ｃと、Ｐｇ、Ｐ＋、２０間の移動量および移動方
向とを検出し、現在位置Ｐ。の座標（χ、ｙ）と移動方
向θとを連立方程式の解として求めるものである。

（実施例） 第１図は本発明の構成を示す機能ブロック図、第２図は
移動体の制御系を示すブロック図、第３図は移動体とし
てのゴルフカートの側面図である。

第３図において符号１０は車体フレーム、１２（１２ａ
、１２ｂ）は左右一対の後輪、１４は１個の操向前輪で
ある。車体フレーム１０は後輪１２間から上方へ起立し
上端が前方へ水平にのびる上部フレーム１０ａを有する
。後輪１２には電動走行モータ１６の回転がチェーン１
８．２０、差動装置２２を介して伝えられる。左右の後
輪１２ａ、１２ｂの回転量は左右一対のエンコーダ２４
　（２４ａ、２４ｂ）により別々に検出される。前輪１
４は操向軸筒２６に保持された操向軸２８の下端に取付
けられている。この操向軸２８の上端にはクラッチ３０
を介して操舵用モータ３２が接続される一方、リンク３
４によってハンドル軸３６の回転が伝えられるようにな
っている。すなわちクラッチ３０の断続により、モータ
３２かハンドル３８かのいずれかによって前輪１４の操
舵が行われるようになっている。

４０はコントローラであり走行用と操舵用の各モータ１
６．３２の電力制御を行う回路や、インターフェース等
を有するものである。４２は車体中央付近の下部に搭載
された鉛蓄電池である。車体フレーム１０の後部には運
転者が立てるようにステップ４６が突設されている。運
転者は必要に応じてこのステップ４６に立ち、入力装置
４８、メインスイッチ５０、ハンドル３８等を操作でき
るようになっている。

５２はレーザー投受光器であり、上フレーム１０ａの前
端に取付けられている。この投受光器５２は、水平な軸
回りに高速回転するポリゴナルミラーによってレーザー
をほぼ垂直方向に主走査しつつ、全体を垂直な軸回りに
回転させて周囲方向に副走査される。モータ５４はこの
副走査方向へ投受光器５２を回転させる一方、この回転
角度はエンコーダ５６により検出される。またこの投受
光器５２には射出したレーザーのりフレツクによる反射
光を検出する受光素子が取付けられ、レーザー光が前記
第４図で説明したりフレフタＲで反射されて戻る光を検
出する。

次にＣＰＵ装置６０を説明する。この装置６０はデジタ
ルコンピュータからなるＣＰＵ６２、半導体メモリ（記
憶装置）６４、パルスカウンタ６６　（６６ａ、６６ｂ
）　、６８等を有する。両パルスカウンタ６６は左右後
輪１２の回転に伴ってエンコーダ２４が出力するパルス
を別々にカウントする。パルスカウンタ６８は投受光器
５２の回転に伴ってエンコーダ５６が出力するパルスを
カウントする。これらカウンタ６６．６８のカウント値
は、コントローラ４０に設けたインターフェース（図示
せず）を介してＣＰＵ６２に読込まれる。メモリ６４に
はＣＰＵ６２の動作プログラムの他に、光反射手段とし
てのりフレフタＲ〔第４図参照）の座標、走行予定ルー
ト等のデータが記憶されている。

ＣＰＵ８２は第１図に示す機能の演算を、メモリ６４に
記憶したプログラムに従って順次繰り返し行う。ＣＰＵ
６２は、まずメモリ６４に記憶した走行予定ルートに従
って操舵用モータ３２と走行用モータ１６とを駆動する
信号をコントローラ４０に送る。この結果前輪１４が操
舵され後輪１２が駆動されて、走行予定ルートにほぼ沿
って自走する。ＣＰＵ６２は走行中に投受光器５２を作
動させ、レーザーを上下方向に主走査させつつ垂直軸回
りに副走査させる。レーザーがリフレクタＲ３で反射し
て投受光器５２に戻るとこの反射光が受光素子で検出さ
れ、この時の受光信号に基づいてパルスカウンタ６８の
カウント値ｎがＣＰＵ６２に読み込まれる。ＣＰＵ６２
の角度検出手段６２Ａはこのカウント値ｎに基づき車輌
走行方向から見たりフレフタＲ，の角度γを求める。Ｃ
ＰＵ６２はこの時の車輌の位置を Ｐ２　（χ−Δχ２１　ｙ−Δｙｚ） としてメモリ６４に記憶する一方、この位置Ｐ２におけ
る走行方向を示す方位ベクトルυ２と、リフレクタＲ３
による受光方向ベクトルＣとを次式のように決める。

υ、　　＝　　（ｃｏｓ　　（θ−Δθ２）。

ｓｉｎ　　（θ−Δθ２）） ｃ＝（χｒ３−χ＋Δχｉ＋　　ｙｒａ　　Ｙ＋Δｙｚ
）ＣＰＵ６２はその後走行予定コースに従って車輌を走
行させ、投受光器５２がリフレクタＲ２からの反射光を
受光すると、その時のりフレツクＲ２の角度βを求める
と共に、その位置をＰ、として Ｐ、（χ−Δχ＋＋　　ｙ−Δｙ＋） としてメモリ６４に記憶する。ＣＰＵ６２はまた方位ベ
クトルυ、と、受光方向ベクトルｂとを次のように決め
る。

ｔｌ＋　＝　（ｃｏｓ（θ−Δθ＋）、　５ｉｎ（θ−
Δθ１））ｂ＝（χ、２−χ＋Δχ＋１３’ｒ□−ｙ＋
Δｙ＋）さらにＣＰＵ６２は走行して投受光器５２がリ
フレクタＲ１からの反射光を受光すると、その時のりフ
レフタＲ３の角度αを求めると共に、その位置をＰｏと
して Ｐ、（χ、ｙ） をメモリし、方位ベクトルυ。と、受光方向ベクトルａ
とを次式により決める。

υ、　＝　（ｃｏｓθ、　ｓｉｎθ） ａ＝（χ１．−χ＋ｙｒ＋ｙ） ここに移動量Δχ１、Δχ２、Δ９□、Δｙ２や移動方
向Δθ１、Δθ２は第１図における移動量検出手段６２
Ｂ、移動方向検出手段６２Ｃにおいて、左右の各後輪１
２の回転量の平均値と、回転量の差とを用いてそれぞれ
算出するものである。

ＣＰＵ６２は次にこれらの方位ベクトルυ。、υ１、υ
２と、受光方位ベクトルａ、ｂ、ｃと、角度α、β、γ
とを用いて、前記（原理）で説明した（１）　　　（２
）、（３）の連立方程式を解く　（演算手段６２Ｄ）。

このように３つの位置Ｐ２．Ｐ１．ＰＧにおける異なる
リフレクタＲｓ　、Ｒｚ　、Ｒ＋の反射方向と、その間
の車輌の移動量、移動方向を自ら検出することにより、
現在位置Ｐ０の座標（χ、ｙ）と走行方向θとを高精度
に求めることができる。

また位置Ｐ、、Ｐ、、Ｐ、が同一位置の場合、位置Ｐ、
、Ｐ、が同一の場合、位置ｐ、、ｐ、が同一の場合も同
様である。

この実施例では走行予定コースをメモリ６４に予め記憶
しておき、このコースに乗って走行しつつ正確な位置を
求め走行方向を修正してゆくものであるが、本発明はこ
れに限られるものではない。例えば任意の方向へ移動中
であっても連続する３つの位置で異なるリフレクタの反
射光を受光すれば、その位置を正確に決定できる。

また移動方向Δθは、この実施例では左右の後輪の回転
量の差から検出しているが、ジャイロスコープなどから
検出してもよいのは勿論である。

さらに本実施例では１つの投受光器５２で３つの光反射
手段の方向を検出しているので、部品点数が減少する効
果も得られるが、本発明は複数の投受光器を用いてもよ
い。

（発明の効果） 本発明は以上のように、３つの座標が既知のりフレフタ
の方向を３つの位置で求め、その間の移動量と移動方向
とを監視することにより、移動体の移動方位ベクトルと
受光方位ベクトルとを求め、これらの内積による３つの
連立方程式から移動体の現在位置とその移動方向とを求
めるものである。このため３つのりフレフタからの反射
光を同時に受光しなくても正確な位置検知ができ、移動
体の高精度な位置決定が可能となる。

【図面の簡単な説明】

第１図は本発明の構成を示す機能ブロック図、第２図は
移動体の制御系を示すブロック図、第３図は移動体とし
てのゴルフカートの側面図である。また第４図は本発明
の原理説明図である。 ６２・・・ＣＰＵ、６２Ａ・・・角度検出手段、６２Ｂ
・・・移動量検出手段、 ６２Ｃ・・・移動方向検出手段、 ６２Ｄ・・・演算手段、 Ｒ＋　Ｒｚ　Ｒ−・・・光反射手段。 特許出願人　ヤマハ発動機株式会社

Claims (1)

1. 【特許請求の範囲】 固定位置が既知の３個の光反射手段により、移動体から
   の射出光を移動体方向へ反射して移動体の位置を検出す
   る装置において、 移動体により各光反射手段からの受光方向を検出する角
   度検出手段と、移動体の移動量を求める移動量検出手段
   と、移動体の移動方向を求め移動方向検出手段と、演算
   手段とを備え、前記演算手段は移動体の３つの位置での
   各光反射手段の方向と、その３つの位置間の移動量およ
   び移動方向とを求め、これら３つの位置での移動体の移
   動方位ベクトルと受光方位ベクトルとの内積を用いて移
   動体の位置を演算することを特徴とする移動体の位置検
   知装置。