JP2941103B2 - 移動体の位置検知装置 - Google Patents

Description

【発明の詳細な説明】

【０００１】

【産業上の利用分野】本発明は、移動体を移動させなが
ら位置が既知な３つの光反射手段に対する移動体との角
度と、この間の移動量をそれぞれ検出して移動体の位置
を検知する装置に関するものである。

【０００２】

【従来の技術および発明の背景】位置が既知の３つの光
反射手段の方位を３つの位置で検出し、その間の移動体
の移動量と移動方向とを検出することにより位置を正確
に検出する方法が同一出願人により提案された（特願昭
６３−３１５１７２）。

【０００３】この既提案のものでは車体に設けた１つの
レーザ投受光器を３６０°回転させることにより、種々
の方向に位置する光反射手段との間で投受光を行う。し
かしこのように１つの投受光器を３６０°回転させる場
合には、この投受光器の全周囲に障害物がないことが必
要である。すなわちこの車両搭載物により死角ができな
いようにする必要がある。このため投受光器は車体の最
も高い位置に配置する必要が生じ、設置自由度が減少す
るという問題が生じる。

【０００４】また複数の投受光器を設けて死角がなくな
るようにすることが考えられる。しかしこの場合には特
に近距離にある光反射手段による反射を用いる時に演算
誤差が大きくなるという問題が生じる。

【０００５】

【発明の目的】本発明はこのような事情に鑑みなされた
ものであり、位置が既知の３つの光反射手段からの反射
光を用いて移動体の位置を検知する場合に、車体に設け
る投受光器の設置自由度を増大し、また位置検出精度も
高くすることが可能な移動体の位置検知装置を提供する
ことを目的とする。

【０００６】

【発明の構成】本発明によればこの目的は、位置が既知
の多数の光反射手段のうち３個により、移動体からの射
出光を移動体方向へ反射して移動体の位置を検出する装
置において、移動体に設けた複数の投受光器と、各投受
光器の追尾方向管理手段と、各光反射手段からの受光方
向を検出する角度検出手段と、移動体の移動量を求める
移動量検出手段と、移動体の移動方向を求める移動方向
検出手段と、演算手段とを備え、前記演算手段は移動体
上の各投受光器の相対座標を絶対座標に座標変換する一
方、移動体の３位置でのいずれかの投受光器から見た３
つの光反射手段の方向と、その３位置間の移動量および
移動方向とにより移動体の移動方向ベクトルと受光ベク
トルとを求め、これらの内積による３つの連立方程式を
解くことにより移動体の位置を演算することを特徴とす
る移動体の位置検知装置により達成される。

【０００７】

【原理】この構成により前記目的が達成できることを第
５図に基づき説明する。移動体上の一点（例えば重心）
を原点とするの座標系Ｘ−Ｙ（相対座標系という）を設
け、その原点Ｏを車両の認識位置Ｐとする。そしてこの
座標系Ｘ−Ｙにおける各投受光器Ｌ₁ 〜Ｌ₄ の座標を、 Ｌ₁ （Ｘ_L1 ，Ｙ_L1 ） Ｌ₂ （Ｘ_L2 ，Ｙ_L2 ） Ｌ₃ （Ｘ_L3 ，Ｙ_L3 ） Ｌ₄ （Ｘ_L4 ，Ｙ_L4 ） とする。

【０００８】この相対座標系Ｘ−Ｙを絶対座標系ｘ−ｙ
に変換すれば、これらの座標は、 Ｌ₁ （ｘ_L1 ，ｙ_L1 ） Ｌ₂ （ｘ_L2 ，ｙ_L2 ） Ｌ₃ （ｘ_L3 ，ｙ_L3 ） Ｌ₄ （ｘ_L4 ，ｙ_L4 ） となる。この座標変換は座標の平行移動と座標回転とを
含むものであるが公知の変換公式を用いることにより可
能である。

【０００９】一方この図においてＰ₀ は移動体の認識位
置Ｐの現在位置、Ｐ₁ 、Ｐ₂ はそれぞれある時間前にお
ける移動体の位置とする。絶対座標系ｘ−ｙを図のよう
にとり、各位置の座標を次のように決める。 Ｐ₀ （ｘ，ｙ） Ｐ₁ （ｘ−Δｘ₁ ，ｙ−Δｙ₁ ） Ｐ₂ （ｘ−Δｘ₂ ，ｙ−Δｙ₂ ） ここにｘ、ｙが求めようとする座標であり、Δｘ₁ ，Δ
ｘ₂ ，Δｙ₁ ，Δｙ₂は移動体の移動量と移動方向とを
監視することにより求めることができる。

【００１０】また移動体の位置Ｐ₀ におけるｘ軸方向に
対する移動方向をθと仮定すれば、位置Ｐ₁ 、Ｐ₂ にお
ける移動方向はθ−Δθ₁ 、θ−Δθ₂ として求められ
る。ここにΔθ₁ 、Δθ₂ は、移動中の移動方向の変化
量であるから移動中監視し続けることにより求められ
る。

【００１１】Ｒ₁ 、Ｒ₂ 、Ｒ₃ はコーナーキューブなど
で作られた光反射手段としてのリフレクタであり、これ
らの座標は図に示すように既知である。位置Ｐ₀ 、Ｐ
₁ 、Ｐ₂ においては、リフレクタＲ₁ 、Ｒ₂ 、Ｒ₃ との
間で投受光器Ｌ₂ 、Ｌ₄ 、Ｌ₁が投受光するものとす
る。またこれらの位置Ｐ₀ 、Ｐ₁ 、Ｐ₂ における移動方
向とリフレクタＲ₁ 、Ｒ₂ 、Ｒ₃ による受光方向とのな
す角（受光角）をα、β、γとすれば、この受光角も検
出可能である。

【００１２】位置Ｐ₀ 、Ｐ₁ 、Ｐ₂ における移動方向を
示す方位ベクトルυ（υ₁ ，υ₂ ，υ₃ ）を次のように
定義する。 ベクトルυ₀ ＝（cos θ，sin θ） ベクトルυ₁ ＝（cos （θ−Δθ₁ ），sin （θ−Δθ₁ ）） ベクトルυ₂ ＝（cos （θ−Δθ₂ ），sin （θ−Δθ₂ ）） またリフレクタＲ₁ 、Ｒ₂ 、Ｒ₃ との間で投受光する投
受光器をＬ₂ 、Ｌ₄ 、Ｌ₁ との間での投受光に対する方
位と距離を示す受光ベクトルａ、ｂ、ｃを次式で定義す
る。

【００１３】 ベクトルａ＝｛ｘ_r1−（ｘ＋ｘ_L2），ｙ_r1−（ｙ＋ｙ_L2）｝ ベクトルｂ＝｛ｘ_r2−（ｘ−Δｘ₁ ＋ｘ_L4），ｙ_r2−（ｙ−Δｙ₁ ＋ｙ_L4）｝ ベクトルｃ＝｛ｘ_r3−（ｘ−Δｘ₂ ＋ｘ_L1），ｙ_r3−（ｙ−Δｙ₂ ＋ｙ_L1）｝ 従って内積（スカラー積）の定義から、 ｜ベクトルａ｜・｜ベクトルυ₀ ｜ ＝｛ｘ_r1−（ｘ＋ｘ_L2）｝cos θ＋｛ｙ_r1−（ｙ＋ｙ_L2）｝sin θ ＝｜ベクトルａ｜・｜ベクトルυ₀ ｜cos α…(1) ｜ベクトルｂ｜・｜ベクトルυ₁ ｜ ＝｜ベクトルｂ｜・｜ベクトルυ₁ ｜cos β…(2) ｜ベクトルｃ｜・｜ベクトルυ₂ ｜ ＝｜ベクトルｃ｜・｜ベクトルυ₂ ｜cos γ…(3)

【００１４】(1) 、(2) 、(3) の連立方程式の解として
ｘ、ｙ、θを求めることができる。すなわち本発明は、
各投受光器Ｌの車両上における相対座標系（Ｘ−Ｙ）を
絶対座標系（ｘ−ｙ）に座標変換する一方、各投受光器
Ｌに対するリフレクタＲの方向をそれぞれ求める。そし
て過去の位置Ｐ₁ 、Ｐ₂ における方位ベクトルυと、受
光ベクトルａ、ｂ、ｃと、Ｐ₂ 、Ｐ₁ 、Ｐ₀ 間の移動量
および移動方向とを検出し、現在位置Ｐ₀ の座標（ｘ、
ｙ）と移動方向θとを連立方程式の解として求めるもの
である。この演算は例えば数値計算の手法によりマイク
ロコンピュータを用いることにより容易に行うことがで
きる。

【００１５】

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

【００１６】第３図において符号１０は車体フレーム、
１２（１２ａ、１２ｂ）は左右一対の後輪、１４は１個
の操向前輪である。車体フレーム１０は後輪１２間から
上方へ起立し上端が前方へ水平にのびる上部フレーム１
０ａを有する。後輪１２には電動走行モータ１６の回転
がチェーン１８、２０、差動装置２２を介して伝えられ
る。左右の後輪１２ａ、１２ｂの回転量は左右一対のエ
ンコーダ２４（２４ａ、２４ｂ）により別々に検出され
る。

【００１７】前輪１４は操向軸筒２６に保持された操向
軸２８の下端に取付けられている。この操向軸２８の上
端にはクラッチ３０を介して操舵用モータ３２が接続さ
れる一方、リンク３４によってハンドル軸３６の回転が
伝えられるようになっている。すなわちクラッチ３０の
断続により、モータ３２かハンドル３８かのいずれかに
よって前輪１４の操舵が行われるようになっている。

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

【００１９】５２（５２ａ〜ｄ）はレーザー投受光器で
あり、図５のＬ₁〜Ｌ₄ に対応する。これらは上フレー
ム１０ａの前端に取付けられている。これらの投受光器
５２は、水平な軸回りに高速回転するポリゴナルミラー
によってレーザーをほぼ垂直方向に主走査しつつ、全体
を垂直な軸回りに回転させて周囲方向にそれぞれ独立に
副走査される。

【００２０】モータ５４（５４ａ〜ｄ）はこの副走査方
向へ投受光器５２を回転させる一方、この回転角度はエ
ンコーダ５６（５６ａ〜ｄ）により検出される。またこ
れらの投受光器５２には射出したレーザーのリフレクタ
による反射光を検出する受光素子が取付けられ、レーザ
ー光が前記第５図で説明したリフレクタＲで反射されて
戻る光を検出する。

【００２１】次にＣＰＵ装置６０を説明する。この装置
６０はデジタルコンピュータからなるＣＰＵ６２、半導
体メモリ（記憶装置）６４、パルスカウンタ６６（６６
ａ、６６ｂ）、６８（６８ａ〜ｄ）等を有する。両パル
スカウンタ６６は左右後輪１２の回転に伴ってエンコー
ダ２４が出力するパルスを別々にカウントする。４つの
パルスカウンタ６８はそれぞれの投受光器５２の回転に
伴ってエンコーダ５６が出力するパルスを別々にカウン
トする。

【００２２】これらカウンタ６６、６８のカウント値は
コントローラ４０に設けたインターフェース（図示せ
ず）を介してＣＰＵ６２に読込まれる。メモリ６４には
ＣＰＵ６２の動作プログラムの他に、光反射手段として
のリフレクタＲ（第５図参照）の絶対座標、各投受光器
５２（Ｌ）の相対座標、走行予定ルート等のデータが記
憶されている。

【００２３】ＣＰＵ６２は第１図に示す機能の演算を、
メモリ６４に記憶したプログラムに従って順次繰り返し
行う。ＣＰＵ６２は、まずメモリ６４に記憶した走行予
定ルートに従って操舵用モータ３２と走行用モータ１６
とを駆動する信号をコントローラ４０に送る。この結果
前輪１４が操舵され後輪１２が駆動されて、走行予定ル
ートにほぼ沿って自走する。ＣＰＵ６２の追尾方向管理
手段６２Ａは走行中に投受光器５２を作動させ、レーザ
ーを上下方向に主走査させつつ垂直軸回りに副走査させ
る。

【００２４】この時方向管理手段６２Ａは、メモリ６４
から検出したリフレクタの位置が第４図に示すように最
も近い３つのｒ₁ 、ｒ₂ 、ｒ₃ を選び、それぞれに別々
の投受光器５２を向ける。例えば第４図でＰ₁ の位置で
は投受光器５２ｂ（Ｌ₂ ）をｒ₃ に、５２ｃ（Ｌ₃ ）を
ｒ₂ に、５２ｄ（Ｌ₄ ）をｒ₁ に向けて適宜の角度範囲
内で副走査させて追尾する。この場合副走査の角度範囲
は、移動体と光反射手段ｒとの距離が遠いほど小さく近
いほど大きくする。

【００２５】また投受光器５２ａ（Ｌ₁ ）は補助的に使
われ、受光失敗回数が所定時間内に一定回数以上になっ
たリフレクタ例えばｒ₃ に向けられる。またさらに大き
い一定回数以上になった場合には、そのリフレクタは機
能していないものと判断し、授受光器５２ａ（Ｌ₁ ）は
別のリフレクタを探すことに用いられる。

【００２６】レーザーがリフレクタｒ₁ で反射して投受
光器５２ｄ（Ｌ₄ ）に戻るとこの反射光が受光素子で検
出され、この時の受光信号に基づいてパルスカウンタ６
８ｄのカウント値ｎがＣＰＵ６２に読み込まれる。ＣＰ
Ｕ６２の角度検出手段６２Ｂは、このカウント値ｎに基
づき投受光器５２ｄ（Ｌ₄ ）から見た車両走行方向に対
するリフレクタｒ₁ の角度γを求める。

【００２７】ＣＰＵ６２はこの時の車両の位置を Ｐ₁₂｛ｘ−（Δｘ₂ ＋ｘ_L4），ｙ−（Δｙ₂ ＋ｙ_L4）｝ としてメモリ６４に記憶する一方、この位置Ｐ₁₂におけ
る走行方向を示す方位ベクトルυ₂ と、リフレクタｒ₁
による受光方向ベクトルｃとを次式のように決める。 ベクトルυ₂ ＝｛cos （θ−Δθ₂ ），sin （θ−Δθ₂ ）｝ ベクトルｃ＝｛ｘ_r3−（ｘ−Δｘ₂ ＋ｘ_L4），ｙ_r3−（ｙ−Δｙ₂ ＋ｙ_L4）｝

【００２８】ＣＰＵ６２はその後走行予定コースに従っ
て車両を走行させ、投受光器５２ｃ（Ｌ₃ ）がリフレク
タｒ₂ からの反射光を受光すると、その時の投受光器５
２ｃ（Ｌ₃ ）から見たリフレクタｒ₂ の角度βを求める
と共に、その位置をＰ₁₁として Ｐ₁₁｛ｘ−（Δｘ₁ ＋ｘ_L3），ｙ−（Δｙ₁ ＋ｙ_L3）｝ としてメモリ６４に記憶する。

【００２９】ＣＰＵ６２はまた方位ベクトルυ₁ と、受
光方向ベクトルｂとを次のように決める。 ベクトルυ₁ ＝｛cos(θ−Δθ₁)，sin(θ−Δθ₁)｝ ベクトルｂ＝｛ｘ_r2−（ｘ−Δｘ₁ ＋ｘ_L3），ｙ_r2−（ｙ−Δｙ₁ ＋ｙ_L3）｝

【００３０】さらにＣＰＵ６２は走行して投受光器５２
ｂ（Ｌ₂ ）がリフレクタｒ₃ からの反射光を受光する
と、その時の投受光器５２ｂ（Ｌ₂ ）からみたリフレク
タｒ₃の角度αを求めると共に、その位置をＰ₁₀として Ｐ₁₀ (ｘ，ｙ） をメモリし、方位ベクトルυ₀ と受光方向ベクトルａと
を次式により決める。

【００３１】 ベクトルυ₀ ＝（cos θ，sin θ） ベクトルａ＝｛ｘ_r1−（ｘ＋ｘ_L2），ｙ_r1−（ｙ＋ｙ_L2）｝ ここに移動量Δｘ₁ 、Δｘ₂ 、Δｙ₁ 、Δｙ₂ や移動方
向Δθ₁ 、Δθ₂ は第１図における移動量検出手段６２
Ｃ、移動方向検出手段６２Ｄにおいて、左右の各後輪１
２の回転量の平均値と、回転量の差とを用いてそれぞれ
算出するものである。またｘ_L1、ｘ_L2、ｘ_L3、ｙ_L1、ｙ
_L2、ｙ_L3は座標変換により求められるものである。

【００３２】ＣＰＵ６２は次にこれらの方位ベクトルυ
₀ 、υ₁ 、υ₂ と、受光方位ベクトルａ、ｂ、ｃと、角
度α、β、γとを用いて、前記（原理）で説明した(1)
、(2) 、(3) の連立方程式を解く（演算手段６２
Ｅ）。このように異なる位置Ｐ₁₂、Ｐ₁₁、Ｐ₁₀における
異なる投受光器５２に対するリフレクタｒ₁ 、ｒ₂ 、ｒ
₃ の反射方向と、その間の車両の移動量、移動方向を自
ら検出することにより、現在位置Ｐ₁₀の座標（ｘ、ｙ）
と走行方向θとを高精度に求めることができる。

【００３３】移動方向Δθは、この実施例では左右の後
輪の回転量の差から検出しているが、ジャイロスコープ
などから検出してもよいのは勿論である。また投受光器
５２は２個でもよいが、３以上に増やして１つの投受光
器が回転する角度を小さくするのが望ましい。

【００３４】図６は他の移動体である４輪のゴルフカー
トの平面図、図７はその側面図である。この実施例で
は、投受光器５２ａ〜ｄが車体の４隅付近の比較的低い
位置に配設されている。またモータ１６の回転は差動装
置７０を介して左右の後輪１２に伝えられる一方、モー
タ１４の回転軸７２にはディスクブレーキ７４が取り付
けられている。図７で７６はアンテナであり、遠隔操縦
用の操作盤からの電波を受信する。なお図６、７におい
ては、前記図１〜４で説明した実施例と同一部分に同一
符号を付したので、その説明は繰り返さない。

【００３５】

【発明の効果】本発明は以上のように、車体上に設けた
複数の投受光器から、座標が既知の複数のリフレクタの
うちの３個の方向を３つの位置で求め、その間の移動量
と移動方向とを監視すると共に、各投受光器の車体上の
座標を絶対座標系に座標変換することにより、移動体の
移動方位ベクトルと受光方位ベクトルとを求める。そし
て、これらの内積による３つの連立方程式から移動体の
現在位置とその移動方向とを求めるものである。従って
各投受光器は３６０°回転する必要がなくなり、これら
の設置自由度が増大する。

【００３６】また各投受光器が車体上の異なる位置に設
置されていても、これらの座標が座標変換により演算に
反映されるから、近距離の光反射手段による反射を用い
た場合にも高精度な位置検知が可能になる。

【図面の簡単な説明】

【図１】本発明の構成を示す機能ブロック図

【図２】移動体の制御系を示すブロック図

【図３】移動体としてのゴルフカートの側面図

【図４】動作説明図

【図５】原理説明図

【図６】他のゴルフスカートの平面図

【図７】同じく側面図

【符号の説明】 ５２ａ〜ｄ（Ｌ₁ 〜Ｌ₄ ） 投受光器 ６２ ＣＰＵ ６２Ａ 追尾方向管理手段 ６２Ｂ 角度検出手段 ６２Ｃ 移動量検出手段 ６２Ｄ 移動方向検出手段 ６２Ｅ 演算手段 Ｒ₁ Ｒ₂ Ｒ₃ 、ｒ₁ 〜ｒ₅ 光反射手段

Claims (1)

(57)【特許請求の範囲】
1. 【請求項１】 位置が既知の多数の光反射手段のうち３
   個により、移動体からの射出光を移動体方向へ反射して
   移動体の位置を検出する装置において、移動体に設けた
   複数の投受光器と、各投受光器の追尾方向管理手段と、
   各光反射手段からの受光方向を検出する角度検出手段
   と、移動体の移動量を求める移動量検出手段と、移動体
   の移動方向を求める移動方向検出手段と、演算手段とを
   備え、前記演算手段は移動体上の各投受光器の相対座標
   を絶対座標に座標変換する一方、移動体の３位置でのい
   ずれかの投受光器から見た３つの光反射手段の方向と、
   その３位置間の移動量および移動方向とにより移動体の
   移動方向ベクトルと受光ベクトルとを求め、これらの内
   積による３つの連立方程式を解くことにより移動体の位
   置を演算することを特徴とする移動体の位置検知装置。