JPH05274039A - 移動体の姿勢及び位置測定装置 - Google Patents

Abstract

(57)【要約】 【目的】 例えば、不整地上を走行する車両や波動海面
上を航行する船舶等の移動体の各時点の３次元姿勢／位
置情報を比較的簡単に一基準点より遠隔計測し得る手段
を提供する。 【構成】 このため、基準点に一定の角速度で旋回する
レーザ光１９を発信するためのレーザ発信装置１１と、
このレーザ光１９が特定方向を指向したとき無指向無線
信号を発信する発信機１３とを設けると共に、移動体１
２上には、この無線信号受信機１５と、前記レーザ光１
９の各受光時刻信号３０，３１，３２と各受光位置信号
とを処理するための各信号処理回路２６，２７，２８及
び各タイマ３３，３４，３５等をそれぞれ備えた少くと
も３個のライン形光センサ１６，１７，１８を配設し、
これら各センサの出力信号により、前記移動体１２の各
時点における３次元姿勢／位置情報を演算手段４２によ
り算出するよう構成した。

Description

【発明の詳細な説明】

【０００１】

【産業上の利用分野】この発明は、移動体の姿勢及び３
次元位置を遠隔測定するための装置の提供に関するもの
である。

【０００２】

【従来の技術】従来、例えば不整地路面上を走行する車
両や波動海面上を航行する船舶、もしくは特殊な飛翔体
等、３次元経路に沿って移動する移動体の各時点におけ
る姿勢及び位置を遠隔的に認識するための方法／装置が
求められており、その一例として「レーザー灯台に用い
た移動体の位置方法測定法」（日本ロボット学会誌、第
２巻第６号、頁５３、１９８４年）等の論文が発表され
ている。

【０００３】この方法では、ある基準点より発信する旋
回レーザ光を３個の光センサで受光し、その３つの受光
時刻情報のみからレーザ旋回平面上における位置と進行
方向とを計測するよう構成している。

【０００４】

【発明が解決しようとする課題】しかしながら、応用の
目的により、これら移動体の各時点における姿勢／位置
制御等を必要とする場合など、以上のような従来方法例
にあっては、移動体の垂直方向位置（高さ）やピッチン
グ角あるいはローリング角等を計測することが不可能で
あるのみならず、移動体が有限のピッチング角やローリ
ング角等を有する場合には、前記レーザ旋回面上での現
在位置や進行方向（ヘディング角）等も正しく計測する
ことができなかった。

【０００５】この発明は、以上のような局面にかんがみ
てなされたもので、比較的簡単な構成で、この種の移動
体の姿勢／位置に関するすべての３次元情報、例えばヘ
ディング角，ピッチング角，ローリング角や３次元高さ
位置等を測定し得る手段の提供を目的としている。

【０００６】

【課題を解決するための手段】このため、この発明にお
いては、この種の移動体の姿勢及び位置測定装置を、一
基準点に設置され、一定角速度でレーザ光を発信するた
めのレーザ光発信手段と、そのレーザ光が特定方向に発
信されたとき無指向性信号を出力するための信号発信手
段と、測定すべき移動物体上に配設され、前記レーザ光
照射位置と照射時刻との情報を含む信号を出力する少な
くとも３個のライン形光センサと、前記無指向性信号を
受信するための受信手段と、前記各光センサからの出力
より、その光センサ上の受光位置を求めるための信号処
理手段と、これら各光センサのそれぞれのレーザ光受光
時刻と前記無指向性信号との受信時刻の差を計測するた
めの各計時手段と、その計時情報とレーザ受光位置情報
とより前記移動体の３次元位置及び姿勢データを算出す
るための演算手段とを備えるよう構成することにより、
前記目的を達成しようとするものである。

【０００７】

【作用】以上のようなこの発明の装置構成により、対象
とする移動体の各時点の姿勢／位置、例えばヘディング
角，ピッチング角，ローリング角等の６自由度の計測が
可能となり、移動体の完全な姿勢／位置制御情報を得る
ことができる。

【０００８】

【実施例】以下に、この発明を実施例に基づいて説明す
る。図１にこの発明に係るこの種の計測システムの一実
施例の概要説明図を示す。この図は、後述レーザ光旋回
面に平行な方向から視た図である。 （構成）１２は、不整地面Ｇ上を移動する不整地走行車
両等の移動体、１１は地上Ｇの測定基準点に配設された
一定の各速度で一平面上を旋回するレーザ光１９を発信
するためのレーザ発信装置、１４はある基準方向を指向
して配設された光センサ、１３はレーザ光１９がある方
向を指向してこれを横切ったとき、光センサ１４により
無指向性の無線信号を発生するための信号発信装置であ
る。

【０００９】一方、移動体１２上には、前記無指向性無
線信号を受信するための受信機１５（１５ａはそのアン
テナ）ならびに、レーザ光受信時刻と受光位置情報とを
出力し得る少なくとも３個のライン形光センサ１６，１
７，１８が配設されている。これらはそれぞれ光位置検
出素子（ＰＳＤ）や光センサアレイ等より成り、旋回レ
ーザ光１９を順次に受光すると、それぞれ受光時刻情報
とラインセンサ上での受光位置情報とを含む信号を出力
し得るよう構成されている。

【００１０】図２に、移動体１２上に配設された信号処
理手段のブロック図を、また図３にこの信号処理手段の
各信号波形タイミングチャートの一例を示す。図２にお
いて、２５は受信機１５のアンテナ１５ａから受信した
信号の処理回路、２９はその出力信号である。２６，２
７，２８はそれぞれ各光センサ１６，１７，１８の出力
信号処理回路で、３０，３１，３２はそれぞれのレーザ
光受光時刻を表わす信号、また３６，３７，３８はそれ
ぞれのレーザ光受光位置を表わす信号、３９，４０，４
１はこれらのアナログ信号をそれぞれディジタル信号に
変換するための各Ａ／Ｄ変換器である。４２はこれらの
各信号を演算処理するためのＣＰＵを示す。

【００１１】（動作）移動体１２上の各ライン形光セン
サ１６，１７，１８が旋回レーザ光１９を順次に受光す
ると、それぞれの信号処理回路２６，２７，２８を通っ
て各受光時刻信号３０，３１，３２と、そのラインセン
サ上での各受光位置信号３６，３７，３８に変換され
る。また受信機１５は、アンテナ１５ｂで送信機１３か
らの無指向性無線信号を受信すると、増幅回路２５に入
力されて、直ちにパルス信号２９を出力する。

【００１２】受光時刻信号３０，３１，３２の各時間間
隔ｔ₀ ，ｔ₁ ，ｔ₂ は、それぞれの計時手段としてのタ
イマ３３，３４，３５により計測されてディジタル信号
に変換される。また、レーザ受光位置情報の各信号処理
回路２６，２７，２８で受光位置信号３６，３７，３８
（ζ₀ ，ζ₁ ，ζ₂ ）に比例した電圧に変換され、各Ａ
／Ｄ変換器３９，４０，４１によりディジタル信号に変
換されて演算手段としてのＣＰＵ４２に入力される。こ
れら６つのディジタル信号は、ＣＰＵ４２で演算処理さ
れて、移動体１２の３つの位置座標情報（γ，θ，ｚ）
と３つの姿勢情報、すなわちヘディング角φ、ピッチン
グ角σ，ローリング角ρに変換される。

【００１３】図４に、この計測システム（図１）の真上
から視た位置関係説明図を示し、Ｏをレーザ発信機１３
位置、移動体１２の高さ位置を光センサ１６の付根部分
Ｓ₀と定義する。旋回レーザ発信機１１からの旋回レー
ザ光１９の旋回角度をωとし、移動体１２上の各光セン
サ１６，１７，１８のレーザ受光位置をそれぞれＲ₀，
Ｒ₁ ，Ｒ₂ 、各光センサ１６，１７，１８の付根部分位
置をそれぞれＳ₀ ，Ｓ ₁ ，Ｓ₂ とし、基準方向から各レ
ーザ受光位置Ｒ₀ ，Ｒ₁ ，Ｒ₂ を見込んだ角度をそれぞ
れθ₀ ，θ₁ ，θ₂ とすると、それらは θ₀ ＝ωｔ₀ （１） θ₁ ＝ωｔ₁ （２） θ₂ ＝ωｔ₂ （３） と表わされる。また、各光センサ１６，１７，１８間の
それぞれの間隔を図４に示すようにａ，ｂ，ｃとし、レ
ーザ受光位置間隔をＡ，Ｂ，Ｃとすると、

【００１４】

【数１】

【００１５】と表わされ、直ちに受光位置計測値ζ₀ ，
ζ₁ ，ζ₂ より算出し得る。

【００１６】つぎに、ここで求めた各間隔Ａ，Ｂ，Ｃを
用いてレーザ受光位置Ｒ₀ ，Ｒ₁ ，Ｒ₂ の座標を求め
る。

【００１７】正弦定理により、 ＯＲ₀ ／sin γ＝Ｂ／sin （θ₂ −θ₀ ） （７） ＯＲ₀ ／sin β＝Ａ／sin （θ₁ −θ₀ ） （８） であるから、

【００１８】

【数２】

【００１９】である。これをｋと置く。

【００２０】三角形Ｒ₀ ，Ｒ₁ ，Ｒ₂ について余弦定理
より、角αは

【００２１】

【数３】

【００２２】として求めることができる。

【００２３】さらに、 α＝θ₂ −θ₀ ＋γ＋θ₀ −θ₁ −β＝θ₂ −θ₁ ＋γ−β （１１） であるから、この式と（９）式とより ｋsin γ＝−sin （α−θ₂ ＋θ₁ −γ） ＝−sin （α−θ₂ ＋θ₁)cos γ＋cos(α−θ₂ ＋θ₁)sin γ (12) となる。この（１２）式よりγは、

【００２４】

【数４】

【００２５】となる。また（１１）式より、 β＝−（α−θ₂ ＋θ₁ −γ） （１４） （７）式より、

【００２６】

【数５】

【００２７】さらに余弦定理により、

【００２８】

【数６】

【００２９】となり、Ｒ₀ ，Ｒ₁ ，Ｒ₂ の座標は

【００３０】

【数７】

【００３１】と求めることができる。

【００３２】次に移動体１２上の３つの座標Ｓ₀ ，Ｓ
₁ ，Ｓ₂ の計算を行う。各ライン形センサ１６，１７，
１８のライン方向の大きさ１の方向ベクトルを

【００３３】

【数８】

【００３４】とすると、

【００３５】

【数９】

【００３６】となる。両辺の内積をとると、

【００３７】

【数１０】

【００３８】となる。ただし、

【００３９】

【数１１】

【００４０】とし、（１８），（１９），（２０）の各
式から求める。

【００４１】（２３），（２４）の両式より、

【００４２】

【数１２】

【００４３】ただし、 δ＝ｘ₁ ｙ₂ −ｘ₂ ｙ₁ （２９） とする。ここで求めたｎ_x ，ｎ_y を用いて、ｎ_z は

【００４４】

【数１３】

【００４５】となり、ベクトルｎが決定される。

【００４６】このｎを用いて、Ｓ₀ ，Ｓ₁ ，Ｓ₂ の各位
置ベクトルは、それぞれ

【００４７】

【数１４】

【００４８】と求められる。

【００４９】移動体の姿勢は、（３１），（３２），
（３３）の各式で与えられる点Ｓ₀ ，Ｓ₁ ，Ｓ₂ で一義
的に決定される。つぎにこれをヘディング角φ、ピッチ
ング角σ、ローリング角ρに変換する。移動体１２上
で、

【００５０】

【数１５】

【００５１】というベクトルが、移動体１２の機首の３
次元的な方向であるとすると、ヘディング角φは、

【００５２】

【数１６】

【００５３】で与えられ、ピッチング角σは、

【００５４】

【数１７】

【００５５】で与えられる。またローリング角ρは

【００５６】

【数１８】

【００５７】で与えられる。（３１），（３５），（３
６），（３７）の各式によって移動体１２の空間上の位
置と姿勢とを完全に決定することができる。

【００５８】この実施例は、以上のような移動体１２の
完全な３次元姿勢／位置情報を得ることができるため、
例えば土木用の測量ロボットや船舶等の制御に応用する
ことができる。

【００５９】

【発明の効果】以上説明したように、この発明の計測シ
ステムによれば、対象とする移動体の現在の３次元姿勢
／位置、例えばヘディング角，ピッチング角，ローリン
グ角等の６自由度の計測が可能となり、移動体の完全な
姿勢／位置制御情報を得ることができる。

【図面の簡単な説明】

【図１】 一実施例の概要説明図

【図２】 移動体上の信号処理手段ブロック図

【図３】 各信号波形タイミングチャート

【図４】 図１の位置関係説明上面図

【符号の説明】

１１ レーザ発信装置 １２ 移動体 １３ 信号発信装置 １４ 光センサ １５ 受信機 １６，１７，１８ 各ライン形光センサ １９ 旋回レーザ光 ２６，２７，２８ 各信号処理回路 ３０，３１，３２ 各受光時刻信号 ３３，３４，３５ 各タイマ ３６，３７，３８ 各受光位置信号 ４２ ＣＰＵ

Claims (1)

【特許請求の範囲】
1. 【請求項１】 一基準点に設置され、一定角速度でレー
   ザ光を発信するためのレーザ光発信手段と、そのレーザ
   光が特定方向に発信されたとき無指向性信号を出力する
   ための信号発信手段と、測定すべき移動物体上に配設さ
   れ、前記レーザ光照射位置と照射時刻との情報を含む信
   号を出力する少なくとも３個のライン形光センサと、前
   記無指向性信号を受信するための受信手段と、前記各光
   センサからの出力より、その光センサ上の受光位置を求
   めるための信号処理手段と、これら各光センサのそれぞ
   れのレーザ光受光時刻と前記無指向性信号との受信時刻
   の差を計測するための各計時手段と、その計時情報とレ
   ーザ受光位置情報とより前記移動体の３次元位置及び姿
   勢データを算出するための演算手段とを備えたことを特
   徴とする移動体の姿勢及び位置測定装置。