JP4532334B2 - Relative position and orientation estimation device for optical space communication system - Google Patents
Relative position and orientation estimation device for optical space communication system Download PDFInfo
- Publication number
- JP4532334B2 JP4532334B2 JP2005139816A JP2005139816A JP4532334B2 JP 4532334 B2 JP4532334 B2 JP 4532334B2 JP 2005139816 A JP2005139816 A JP 2005139816A JP 2005139816 A JP2005139816 A JP 2005139816A JP 4532334 B2 JP4532334 B2 JP 4532334B2
- Authority
- JP
- Japan
- Prior art keywords
- laser beam
- communication system
- relative position
- space communication
- optical space
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Expired - Fee Related
Links
Images
Landscapes
- Mechanical Light Control Or Optical Switches (AREA)
- Optical Communication System (AREA)
Description
本発明は、空間中にレーザー光を伝播させて通信を行う光空間通信システムにおいて、送信装置と受信装置間の相対位置・姿勢を送信装置及び受信装置内部の情報のみを基にして推定することの可能な相対位置姿勢推定装置に関するものである。 In an optical space communication system that performs communication by propagating a laser beam in space, the present invention estimates a relative position / attitude between a transmission device and a reception device based only on information in the transmission device and the reception device. The present invention relates to an apparatus for estimating a relative position and orientation.
例えばビル−ビル間光空間通信のような通常の光空間通信では2つの送受信機を固定的に対向配置し風や振動などによる微小な軸ズレの影響を補正するメカニズムが採用されている。一方、端末の移動を伴う光空間通信では広範囲な相対位置変化に伴う軸ズレに対応する光軸調整機構が不可欠となる(例えば、非特許文献1参照。)。この場合送受信機間の距離や向きの変化を光軸調整サーボシステムに正しく反映させることが軸合わせの精度を保つ上で重要となる。このことは仮に送受信機の180度の相対的な向きの変化が無視されれば光軸調整のフィードバックが逆方向に作用しサーボ機構の用を成さないことを考えれば明らかである。このような相対位置・姿勢情報を例えば1つの移動局に対して複数の固定局を用意して三角測量の原理で提供することが考えられるが、そのような状態の定常的な保証可能性や設備コストの増大といった問題が生じてくる。 For example, in a normal optical space communication such as a building-to-building optical space communication, a mechanism is adopted in which two transmitters / receivers are fixedly arranged opposite to each other to correct the influence of a small axis shift caused by wind or vibration. On the other hand, in optical space communication involving movement of a terminal, an optical axis adjustment mechanism that corresponds to an axial shift accompanying a wide range of relative position changes is indispensable (see, for example, Non-Patent Document 1). In this case, it is important to accurately reflect the change in the distance and direction between the transceivers in the optical axis adjustment servo system in order to maintain the alignment accuracy. This is apparent from the fact that if the change in the relative direction of 180 degrees of the transmitter / receiver is ignored, the feedback of the optical axis adjustment acts in the reverse direction and does not use the servo mechanism. It is conceivable that such relative position / posture information is provided on the principle of triangulation by preparing a plurality of fixed stations for one mobile station, for example. Problems such as increased equipment costs arise.
本発明は上記の事情に鑑みてなされたもので、新たな外部装置などを用いることなく送信装置及び受信装置内部の情報のみに基いて送信装置と受信装置間の相対位置と姿勢を検出する光空間通信システムの相対位置姿勢推定装置を提供することを目的とする。 The present invention has been made in view of the above circumstances, and is a light that detects the relative position and orientation between a transmission device and a reception device based on only information inside the transmission device and the reception device without using a new external device or the like. An object of the present invention is to provide a relative position and orientation estimation apparatus for a spatial communication system.
上記目的を達成するために本発明の光空間通信システムの相対位置姿勢推定装置は、空間中にレーザー光を伝播させて通信を行う光空間通信システムであって、送信装置側に送信用レーザー光源によるレーザー光の発射方向(パン・チルト角)が調整可能なように2自由度の可動式反射鏡を備え、受信装置側に送信側から到達したレーザー光の向きを調整し直列に配置された2つのビームスプリッターへ導く2自由度の可動式反射鏡と最初のビームスプリッターにより2分した一方の分岐光を次のビームスプリッターによりさらに2分したレーザー光それぞれの照射位置を検知する2つのPSD(Position Sensing Device)を備え、最初のビームスプリッターによる他方の分岐光を受信用PD(Photo Detector)で受光することが可能な光空間通信システムにおいて、各可動式反射鏡の動きデータとPSD上のレーザービームスポットの軌跡データから送信装置における射出ビームの発射位置及び発射方向ベクトルとそれらの微分ベクトル、さらに受信装置における入射ビームの到達位置及び入射方向ベクトルとそれらの微分ベクトルを求めて、これより送信装置と受信装置の相対位置と姿勢を推定することを特徴とするものである。 In order to achieve the above object, a relative position and orientation estimation apparatus for an optical space communication system according to the present invention is an optical space communication system that performs communication by propagating a laser beam in space, and includes a laser beam source for transmission on the transmitter side. Equipped with a two-degree-of-freedom movable reflector so that the laser beam emission direction (pan / tilt angle) can be adjusted by adjusting the direction of the laser beam arriving from the transmission side on the receiver side. Two PSDs that detect the irradiation position of each of the laser beams obtained by dividing one split light divided into two by the second beam splitter into a two-degree-of-freedom movable reflector guided to the two beam splitters and the first beam splitter. Position Sensing Device), the other split light from the first beam splitter is received by a PD (Photo Dete tor) in a space optical communication system capable of receiving light, and from the movement data of each movable reflector and the locus data of the laser beam spot on the PSD, the emission position and emission direction vectors of the emitted beam in the transmitter and their derivatives The arrival position and incident direction vectors of the incident beam in the receiving apparatus and the differential vectors thereof are obtained, and the relative position and orientation of the transmitting apparatus and the receiving apparatus are estimated from the vectors.
また本発明は、前記光空間通信システムの相対位置姿勢推定装置において、送信装置からみた受信装置の位置ベクトルに相対姿勢をノルム1の四元数で表したものを合わせた7次元の状態ベクトルを定義し、可動式反射鏡の角度とPSDにより捉えられたレーザービームスポット位置の時系列データにおいて時間的に隣り合うデータをある時刻において同時に観測されたものとみなして導出される観測方程式を基に拡張カルマンフィルタを構成して送信装置と受信装置の相対位置と姿勢を推定することを特徴とするものである。
According to the present invention, in the relative position and orientation estimation apparatus of the optical space communication system, a 7-dimensional state vector obtained by combining the position vector of the receiving apparatus viewed from the transmitting apparatus with the relative attitude expressed by a quaternion of
本発明によれば、移動を伴う光空間通信システムの送信装置と受信装置間の相対位置・姿勢を新たな外部装置等の付加なしに送信装置及び受信装置の内部情報のみによって検出可能という効果がある。 According to the present invention, it is possible to detect the relative position / attitude between a transmission device and a reception device of an optical space communication system with movement only by internal information of the transmission device and the reception device without adding a new external device or the like. is there.
また本発明は、位置ベクトルに姿勢を表す四元数を組み合わせて状態ベクトルを定義し可動式反射鏡の動きとPSDによるレーザー光のビームスポット軌跡の時系列データに拡張カルマンフィルタを適用することによって耐ノイズ性に優れた相対位置・姿勢の滑らかな推定が可能となる。これにより光軸調整サーボシステムを適切に作動させることができるとともに移動局の現在位置情報に基いてユーザーにとって有益な情報を提供したり最も適切な固定局への切替を促したりするといった付加価値の創出が期待できるという効果がある。 In addition, the present invention defines a state vector by combining a position vector with a quaternion representing an attitude, and applies an extended Kalman filter to the time-series data of the movement of the movable reflector and the beam spot trajectory of the laser beam by PSD. Smooth estimation of relative position / posture with excellent noise characteristics is possible. As a result, the optical axis adjustment servo system can be properly operated, and the value added such as providing useful information for the user based on the current position information of the mobile station and prompting the switch to the most appropriate fixed station. There is an effect that the creation can be expected.
以下図面を参照して本発明の実施の形態例を詳細に説明する。
図1は本発明が対象とする光空間通信システムを示す構成説明図である。図1において、送信装置11にはレーザー光Lを発射する送信用LD(Laser Diode)が設けられ、前記送信用LDからのレーザー光照射位置にはモーターD1の回転軸に取り付けられたミラー(反射鏡)M1が設けられる。前記モーターD1の回転軸に直交するように回転軸が配置されたモーターD2の回転軸にはミラーM2が設けられる。ミラーM2はミラーM1からの反射光が照射される位置に設けられる。この場合、送信用LDからのレーザー光Lの発射方向がモーターD2の回転軸方向になるように配置する。送信用LDからミラーM1上のモーターD1の回転軸に対応する位置にレーザー光Lを発射して、2自由度の可動式反射鏡であるミラーM1及びミラーM2での反射を経て受信側へ送出する。送信用LDによるレーザー光の発射方向(パン・チルト角)はモーターD1,D2を回転してミラーM1,M2の角度を変化させることにより調整できる。
Embodiments of the present invention will be described below in detail with reference to the drawings.
FIG. 1 is an explanatory diagram showing a configuration of an optical space communication system targeted by the present invention. In FIG. 1, a
受信装置12に送信側から到達したレーザー光LはモーターD3の回転軸に取り付けられたミラーM3に入射される。前記モーターD3の回転軸に直交するように回転軸が配置されたモーターD4の回転軸にはミラーM4が設けられる。ミラーM4はミラーM3からの反射光が照射される位置に設けられる。ミラーM4からの反射光が照射される位置には直列に配置された2つのビームスプリッターS1,S2が設けられる。2自由度の可動式反射鏡であるミラーM3及びミラーM4は送信側から到達したレーザー光Lの向きを調整し直列に配置された2つのビームスプリッターS1,S2へ導く。最初のビームスプリッターS1により2分した一方の分岐光を次のビームスプリッターS2によりさらに2分したレーザー光Lそれぞれの照射位置には2つのPSD(Position Sensing Device)1,PSD2が設けられ、PSD1,PSD2はそれぞれレーザー光Lを検知する。最初のビームスプリッターS1による他方の分岐光を受光する位置には受信用PD(Photo Detector)が設けられる。
The laser light L reaching the
送信装置11及び受信装置12にはそれぞれ制御部C1,C2が設けられ、電波または赤外線などによる通常の無線LANにより互いの情報を交換できるようなインターフェイスを持っている。
The
いま、受信装置12への到達光がミラーM3で反射されさらにミラーM4で反射されたレーザー光が直列に配置された2つのビームスプリッターS1,S2を貫通するようにモーターD3,D4の回転角が調整されているものとする。このときビームスプリッターS1のプリズム斜面P1に対する受信用PDの鏡像をPD′、ビームスプリッターS2のプリズム斜面P2に対するPSD2の鏡像をPSD2′とするとき、PSD1とPSD2′が重ならないように配置されていればミラーM4からの反射光とPD′の光軸との関係はミラーM4からの反射光とPSD1及びPSD2′受光面との交点の座標によりパラメトライズされることがわかる。従って、ミラーM4からの反射光とPDの光軸が一致する場合の2つのPSD1及びPDS2上のレーザースポット座標を基準点とすれば、送信装置11及び受信装置12の相対位置が変化して光軸が外れたときもPSD1及びPDS2上のレーザースポットが基準点に戻るようにモーターM3,M4の回転角を調整することにより常に光軸を一致させることが可能となる。このようなモーター回転角の存在する条件とその場合の光軸を一致させるアルゴリズムが非特許文献1に記載されている。以降、送信装置11及び受信装置12に固定された座標系をそれぞれΣT,ΣR,ΣTからみたΣRの位置ベクトルと回転(姿勢)行列をそれぞれr,Rとする。
Now, the rotation angle of the motors D3 and D4 is such that the light reaching the
本発明の第1の実施形態例は以下の通りである。
図1に示すような光空間通信システムにおいて、各可動式反射鏡であるミラーM1〜M4の動きデータとPSD1及びPSD2上のレーザービームスポットの軌跡データから送信装置11における射出レーザービームの発射位置及び発射方向ベクトルとそれらの微分ベクトル、さらに受信装置12における入射ビームの到達位置及び入射方向ベクトルとそれらの微分ベクトルを求めて、これより送信装置11と受信装置12の相対位置と姿勢が推定可能な相対位置姿勢推定装置を提供することができる。
The first embodiment of the present invention is as follows.
In the optical space communication system as shown in FIG. 1, the emission position of the emitted laser beam in the
すなわち、PSD1とPSD2で捕らえられたレーザー光のビームスポットが送信装置11のミラーM1,M2の微小な動きに対して変化する様子が記録されたときに2つのPSD1及びPSD2から逆に光線追跡を行った場合に得られるΣRからみたミラーM3上のレーザースポット位置とレーザー光の入射ベクトル(単位ベクトル)をそれぞれr3,p3、また、その微分ベクトルを
p2=Rp3 (1)
That is, when the state in which the beam spot of the laser light captured by PSD1 and PSD2 changes with the minute movement of the mirrors M1 and M2 of the
また、ミラーの動きに対してr,Rの動きが無視できるとき
一方、kを適当なスカラーとすればr2とr3の間に次式が成立する。
Rr3+r=kp2+r2 (4)
On the other hand, if k is an appropriate scalar, the following equation is established between r 2 and r 3 .
Rr 3 + r = kp 2 + r 2 (4)
同様にミラーM1〜M4の動きに対して送信装置11と受信装置12間の相対位置変化が無視できるときは
これよりkが
次に本発明の第2の実施形態例について説明する。
第1の実施形態例に係る光空間通信システムの相対位置姿勢推定装置において、送信装置11からみた受信装置12の位置ベクトルに相対姿勢をノルム1の四元数で表したものを合わせた7次元の状態ベクトルを定義し、可動式反射鏡であるミラーM1〜M4の角度とPSD1及びPSD2により捉えられたビームスポット位置の時系列データにおいて時間的に隣り合うデータをある時刻において同時に観測されたものとみなして導出される観測方程式を基に拡張カルマンフィルタを構成して送信装置11と受信装置12の相対位置と姿勢が推定可能な相対位置姿勢推定装置を提供することができる。
Next, a second embodiment of the present invention will be described.
In the relative position and orientation estimation apparatus of the optical space communication system according to the first embodiment, the 7-dimensional combination of the position vector of the receiving
すなわち、ΣTからみたΣRの位置と姿勢を受信装置12上のPSD1及びPSD2で捉えられたビームスポット軌跡の時系列測定値から拡張カルマンフィルタを用いて推定する。ただし、回転(姿勢)行列はノルム1の四元数q=[q1 q2 q3 q4]Tにより表現する。ノルム1の複素数が2次元の回転を表すようにノルム1の四元数は3次元の回転を表すことができ、回転行列との間に次のような関係式が成立する。
ここで、r=[x y z]Tを含め推定すべき状態ベクトルXを次のように定義する。
さらに、時系列の測定データから状態推定を行うため次のような状態変化モデルを導入する。
Xk+1=Xk+wk (8)
yk=h(Xk,θk)+vk (9)
Furthermore, the following state change model is introduced in order to estimate the state from time series measurement data.
X k + 1 = X k + w k (8)
y k = h (X k , θ k ) + v k (9)
ただし、yは2つのPSD1及びPSD2上のビームスポット位置に関する観測値ベクトル、h(Xk,θk)は送信装置11及び受信装置12の相対位置・姿勢Xと各モーター回転角θ(=[θ1 θ2 θ3 θ4]T)からビームスポット位置への写像を表す関数である。また、w、vはそれぞれシステムノイズと観測ノイズベクトルを表す。各変数の添え字kは時系列上のサンプリング数を示す。
However, y is an observation value vector regarding the beam spot positions on the two
明らかに上記の状態変化モデルにおいてPSD1及びPSD2′上のビームスポットの測定値を単独に観測値ベクトルとして状態推定を行うことは不可能である。そこで1サンプリング時間でのXkの変化は微小であると仮定して、連続する2サンプルの観測データを観測値ベクトルへ割り当てることを考え、新たにPSD1及びPSD2におけるビームスポットへの写像に関する次のような関数
対応する観測値ベクトルは次のように拡張しておく。
ただし、それぞれ3番目の要素は姿勢を表す四元数のノルムが1であることに対応している。式(9)に代わって観測方程式を新たに次のように定義する。
ここで、wk,vkは平均値ベクトル0で互いに独立としその共分散行列がそれぞれΣw,Σvで与えられるものとすれば次のような拡張カルマンフィルタを適用することにより状態推定が可能となる。
なお、本発明は、上記実施形態例そのままに限定されるものではなく、実施段階ではその要旨を逸脱しない範囲で構成要素を変形して具体化できる。また、上記実施形態例に開示されている複数の構成要素の適宜な組み合せにより種々の発明を形成できる。例えば、実施形態例に示される全構成要素から幾つかの構成要素を削除してもよい。更に、異なる実施形態例に亘る構成要素を適宜組み合せてもよい。 Note that the present invention is not limited to the above-described embodiment as it is, and can be embodied by modifying the constituent elements without departing from the scope of the invention in the implementation stage. In addition, various inventions can be formed by appropriately combining a plurality of constituent elements disclosed in the above embodiment. For example, some components may be deleted from all the components shown in the embodiment. Furthermore, constituent elements over different embodiment examples may be appropriately combined.
11…送信装置、12…受信装置、LD…レーザーダイオード(レーザー光源)、D1〜D4…モーター、M1〜M4…ミラー、PD…ホトディテクタ、PSD1,PSD2…ポジションセンスデバイス、S1,S2…ビームスプリッター、C1,C2…制御部。
DESCRIPTION OF
Claims (2)
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
JP2005139816A JP4532334B2 (en) | 2005-05-12 | 2005-05-12 | Relative position and orientation estimation device for optical space communication system |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
JP2005139816A JP4532334B2 (en) | 2005-05-12 | 2005-05-12 | Relative position and orientation estimation device for optical space communication system |
Publications (2)
Publication Number | Publication Date |
---|---|
JP2006319625A JP2006319625A (en) | 2006-11-24 |
JP4532334B2 true JP4532334B2 (en) | 2010-08-25 |
Family
ID=37539892
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
JP2005139816A Expired - Fee Related JP4532334B2 (en) | 2005-05-12 | 2005-05-12 | Relative position and orientation estimation device for optical space communication system |
Country Status (1)
Country | Link |
---|---|
JP (1) | JP4532334B2 (en) |
Families Citing this family (1)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
JP4380691B2 (en) | 2006-11-28 | 2009-12-09 | 日産自動車株式会社 | Sub-chamber internal combustion engine |
Citations (5)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
JPH01292919A (en) * | 1988-05-19 | 1989-11-27 | Toshiba Corp | Optical slip ring device |
JPH0464082A (en) * | 1990-07-02 | 1992-02-28 | Sony Corp | Reflector for automatic tracking device |
JPH04165597A (en) * | 1990-10-30 | 1992-06-11 | Sony Corp | Optical space transmitter |
JPH04116740U (en) * | 1991-04-02 | 1992-10-20 | 三菱電機株式会社 | Spatial optical transmission device |
JPH05346467A (en) * | 1992-06-15 | 1993-12-27 | Hitachi Zosen Corp | Moving body tracking type communication equipment |
-
2005
- 2005-05-12 JP JP2005139816A patent/JP4532334B2/en not_active Expired - Fee Related
Patent Citations (5)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
JPH01292919A (en) * | 1988-05-19 | 1989-11-27 | Toshiba Corp | Optical slip ring device |
JPH0464082A (en) * | 1990-07-02 | 1992-02-28 | Sony Corp | Reflector for automatic tracking device |
JPH04165597A (en) * | 1990-10-30 | 1992-06-11 | Sony Corp | Optical space transmitter |
JPH04116740U (en) * | 1991-04-02 | 1992-10-20 | 三菱電機株式会社 | Spatial optical transmission device |
JPH05346467A (en) * | 1992-06-15 | 1993-12-27 | Hitachi Zosen Corp | Moving body tracking type communication equipment |
Also Published As
Publication number | Publication date |
---|---|
JP2006319625A (en) | 2006-11-24 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN106646355B (en) | Signal sending device and three-dimensional space positioning system | |
Maheepala et al. | Light-based indoor positioning systems: A review | |
CN110573833B (en) | Imaging device and monitoring device | |
US10126415B2 (en) | Probe that cooperates with a laser tracker to measure six degrees of freedom | |
Famili et al. | Pilot: High-precision indoor localization for autonomous drones | |
KR101365291B1 (en) | Method and apparatus for estimating location in the object | |
JP2011232335A (en) | System and method for estimating position and direction | |
CN112098942B (en) | Positioning method of intelligent equipment and intelligent equipment | |
JP2006220465A (en) | Position specifying system | |
Vakulya et al. | Fast adaptive acoustic localization for sensor networks | |
Li et al. | Indoor localization based on radio and sensor measurements | |
KR101618795B1 (en) | Device for detecting three-dimensional pose and position of moving object | |
Aparicio-Esteve et al. | Estimation of the Polar Angle in a 3D Infrared Indoor Positioning System based on a QADA receiver | |
Wang et al. | Indoor visible light localization algorithm with multi-directional PD array | |
US20130162971A1 (en) | Optical system | |
Sun et al. | Aim: Acoustic inertial measurement for indoor drone localization and tracking | |
Aparicio-Esteve et al. | Experimental evaluation of a machine learning-based RSS localization method using Gaussian processes and a quadrant photodiode | |
CN113014658B (en) | Device control, device, electronic device, and storage medium | |
JP4532334B2 (en) | Relative position and orientation estimation device for optical space communication system | |
JP4713263B2 (en) | Mobile tracking optical space communication system | |
Dai et al. | Deep odometry systems on edge with EKF-lora backend for real-time indoor positioning | |
Aalimahmoodi et al. | An image sensor based indoor VLP system | |
JP4585378B2 (en) | Hybrid optical axis correction device for optical space communication system | |
Aparicio-Esteve et al. | Centralized optical 3D positioning system for emitting tags | |
US10803848B1 (en) | Audio instrument with three-dimensional localization |
Legal Events
Date | Code | Title | Description |
---|---|---|---|
A621 | Written request for application examination |
Free format text: JAPANESE INTERMEDIATE CODE: A621 Effective date: 20070807 |
|
TRDD | Decision of grant or rejection written | ||
A01 | Written decision to grant a patent or to grant a registration (utility model) |
Free format text: JAPANESE INTERMEDIATE CODE: A01 Effective date: 20100608 |
|
A01 | Written decision to grant a patent or to grant a registration (utility model) |
Free format text: JAPANESE INTERMEDIATE CODE: A01 |
|
A61 | First payment of annual fees (during grant procedure) |
Free format text: JAPANESE INTERMEDIATE CODE: A61 Effective date: 20100610 |
|
FPAY | Renewal fee payment (event date is renewal date of database) |
Free format text: PAYMENT UNTIL: 20130618 Year of fee payment: 3 |
|
FPAY | Renewal fee payment (event date is renewal date of database) |
Free format text: PAYMENT UNTIL: 20140618 Year of fee payment: 4 |
|
S531 | Written request for registration of change of domicile |
Free format text: JAPANESE INTERMEDIATE CODE: R313531 |
|
R350 | Written notification of registration of transfer |
Free format text: JAPANESE INTERMEDIATE CODE: R350 |
|
LAPS | Cancellation because of no payment of annual fees |