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 PDF

Info

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
Application number
JP2005139816A
Other languages
Japanese (ja)
Other versions
JP2006319625A (en
Inventor
耕一 吉田
健 辻村
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Nippon Telegraph and Telephone Corp
Original Assignee
Nippon Telegraph and Telephone Corp
Priority date (The priority date 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 date listed.)
Filing date
Publication date
Application filed by Nippon Telegraph and Telephone Corp filed Critical Nippon Telegraph and Telephone Corp
Priority to JP2005139816A priority Critical patent/JP4532334B2/en
Publication of JP2006319625A publication Critical patent/JP2006319625A/en
Application granted granted Critical
Publication of JP4532334B2 publication Critical patent/JP4532334B2/en
Expired - Fee Related legal-status Critical Current
Anticipated expiration legal-status Critical

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.

Koichi Yoshida,Tatsuro Yano,and Takeshi Tsujimura:“Automatic Optical Axis Alignment for Active Free Space Optics”,SICE Annual Conference in Sapporo,August4-6,2004,Hokkaido Institute of Tecnology,Japan,p.2035-2040Koichi Yoshida, Tatsuro Yano, and Takeshi Tsujimura: “Automatic Optical Axis Alignment for Active Free Space Optics”, SICE Annual Conference in Sapporo, August 4-6, 2004, Hokkaido Institute of Tecnology, Japan, p.2035-2040

本発明は上記の事情に鑑みてなされたもので、新たな外部装置などを用いることなく送信装置及び受信装置内部の情報のみに基いて送信装置と受信装置間の相対位置と姿勢を検出する光空間通信システムの相対位置姿勢推定装置を提供することを目的とする。   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 norm 1 is used. Defined based on the observation equation derived by assuming that the time-series data of the laser beam spot position captured by the angle of the movable reflector and the PSD of the movable reflector were simultaneously observed at a certain time The extended Kalman filter is configured to estimate the relative position and orientation of the transmission device and the reception device.

本発明によれば、移動を伴う光空間通信システムの送信装置と受信装置間の相対位置・姿勢を新たな外部装置等の付加なしに送信装置及び受信装置の内部情報のみによって検出可能という効果がある。   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 transmission device 11 is provided with a transmission LD (Laser Diode) that emits a laser beam L, and a mirror (reflection) attached to a rotation shaft of a motor D1 at a laser beam irradiation position from the transmission LD. Mirror) M1 is provided. A mirror M2 is provided on the rotation axis of the motor D2 in which the rotation axis is arranged so as to be orthogonal to the rotation axis of the motor D1. The mirror M2 is provided at a position where the reflected light from the mirror M1 is irradiated. In this case, it arrange | positions so that the emission direction of the laser beam L from LD for transmission may turn into the rotating shaft direction of the motor D2. The laser beam L is emitted from the transmitting LD to a position corresponding to the rotation axis of the motor D1 on the mirror M1, and is transmitted to the receiving side after being reflected by the mirror M1 and the mirror M2, which are two-degree-of-freedom movable reflectors. To do. The laser beam emission direction (pan / tilt angle) by the transmission LD can be adjusted by rotating the motors D1 and D2 to change the angles of the mirrors M1 and M2.

受信装置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 receiving device 12 from the transmission side is incident on a mirror M3 attached to the rotation shaft of the motor D3. A mirror M4 is provided on the rotation axis of the motor D4, the rotation axis of which is arranged so as to be orthogonal to the rotation axis of the motor D3. The mirror M4 is provided at a position where the reflected light from the mirror M3 is irradiated. Two beam splitters S1 and S2 arranged in series are provided at a position where the reflected light from the mirror M4 is irradiated. The mirrors M3 and M4, which are two-degree-of-freedom movable reflectors, adjust the direction of the laser light L that has arrived from the transmission side and guide it to two beam splitters S1 and S2 arranged in series. Two PSDs (Position Sensing Devices) 1 and PSD2 are provided at the irradiation positions of the laser light L, which is obtained by dividing one branched light divided by the first beam splitter S1 and further divided into two by the next beam splitter S2. Each PSD 2 detects the laser beam L. A receiving PD (Photo Detector) is provided at a position for receiving the other branched light from the first beam splitter S1.

送信装置11及び受信装置12にはそれぞれ制御部C1,C2が設けられ、電波または赤外線などによる通常の無線LANにより互いの情報を交換できるようなインターフェイスを持っている。   The transmission device 11 and the reception device 12 are provided with control units C1 and C2, respectively, and have interfaces that can exchange information with each other by a normal wireless LAN using radio waves or infrared rays.

いま、受信装置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に固定された座標系をそれぞれΣ,Σ,ΣからみたΣの位置ベクトルと回転(姿勢)行列をそれぞれr,Rとする。 Now, the rotation angle of the motors D3 and D4 is such that the light reaching the receiving device 12 is reflected by the mirror M3 and the laser light reflected by the mirror M4 passes through the two beam splitters S1 and S2 arranged in series. It shall be adjusted. At this time, when the mirror image of the receiving PD with respect to the prism slope P1 of the beam splitter S1 is PD 'and the mirror image of PSD2 with respect to the prism slope P2 of the beam splitter S2 is PSD2', the PSD1 and PSD2 'are arranged so as not to overlap. For example, the relationship between the reflected light from the mirror M4 and the optical axis of PD ′ is parametrized by the coordinates of the intersection of the reflected light from the mirror M4 and the PSD1 and PSD2 ′ light receiving surfaces. Therefore, if the laser spot coordinates on the two PSDs 1 and PDS2 when the reflected light from the mirror M4 coincides with the optical axis of the PD are used as reference points, the relative positions of the transmission device 11 and the reception device 12 change and the light Even when the axis is off, the optical axes can always be matched by adjusting the rotation angles of the motors M3 and M4 so that the laser spots on the PSD1 and PDS2 return to the reference point. Non-Patent Document 1 describes an algorithm for matching the condition where such a motor rotation angle exists with the optical axis in that case. Thereafter, the transmitting apparatus 11 and secured to the receiver 12 coordinate system, respectively Σ T, Σ R, Σ T viewed from sigma rotation position vector R (attitude) matrix, respectively r, and R.

本発明の第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 transmitter 11 and the movement data of the mirrors M1 to M4 that are movable reflectors and the locus data of the laser beam spots on PSD1 and PSD2 The firing direction vectors and their differential vectors, as well as the arrival position and incident direction vector of the incident beam in the receiving device 12 and their differential vectors are obtained, and the relative positions and orientations of the transmitting device 11 and the receiving device 12 can be estimated from this. A relative position / orientation estimation apparatus can be provided.

すなわち、PSD1とPSD2で捕らえられたレーザー光のビームスポットが送信装置11のミラーM1,M2の微小な動きに対して変化する様子が記録されたときに2つのPSD1及びPSD2から逆に光線追跡を行った場合に得られるΣからみたミラーM3上のレーザースポット位置とレーザー光の入射ベクトル(単位ベクトル)をそれぞれr,p、また、その微分ベクトルを

Figure 0004532334
とする。一方、これに対応する送信装置11のミラーM2上でのビームスポットのΣからみた位置ベクトルとレーザー光の射出ベクトル(単位ベクトル)をr,p、微分ベクトルを
Figure 0004532334
で表す。明らかに
=Rp (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 transmission device 11 is recorded, the ray tracing from the two PSD1 and PSD2 is reversed. laser spot position and the laser beam incidence vector on the mirror M3 as viewed from the obtained sigma R when performing (unit vector) respectively r 3, p 3, also the differential vector
Figure 0004532334
And On the other hand, the position vector seen from Σ T of the beam spot on the mirror M2 of the transmitter 11 corresponding to this and the emission vector (unit vector) of the laser beam are r 2 and p 2 , and the differential vector is
Figure 0004532334
Represented by Apparently p 2 = Rp 3 (1)

また、ミラーの動きに対してr,Rの動きが無視できるとき

Figure 0004532334
であることから、Rは次のように求められる。
Figure 0004532334
Also, when the movement of r and R can be ignored with respect to the movement of the mirror
Figure 0004532334
Therefore, R is obtained as follows.
Figure 0004532334

一方、kを適当なスカラーとすればrとrの間に次式が成立する。
Rr+r=kp+r (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間の相対位置変化が無視できるときは

Figure 0004532334
が成り立つが、ここで両辺のpと垂直な平面への射影を考える。pはΣではpで置き換えられるため新たに
Figure 0004532334
を定義し、さらに
Figure 0004532334
とおく。このとき
Figure 0004532334
が直交することを考慮すれば上式より次式が成立する。
Figure 0004532334
Similarly, when the relative position change between the transmitter 11 and the receiver 12 can be ignored with respect to the movement of the mirrors M1 to M4
Figure 0004532334
Here, let us consider the projection onto a plane perpendicular to p 2 on both sides. p 2 is newly because it is replaced with p 3 in sigma R
Figure 0004532334
Define
Figure 0004532334
far. At this time
Figure 0004532334
Considering that the two are orthogonal, the following equation is established from the above equation.
Figure 0004532334

これよりkが

Figure 0004532334
より求まるので、これを式(4)へ代入してrを得る。 K is more than this
Figure 0004532334
Therefore, r is obtained by substituting this into equation (4).

次に本発明の第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 apparatus 12 viewed from the transmitting apparatus 11 and the relative attitude represented by a quaternion of norm 1 The state vector is defined and the time-series data of the beam spot positions captured by PSD1 and PSD2 are observed at the same time at the same time in the time series data of the angles of mirrors M1 to M4 which are movable reflectors It is possible to provide a relative position / orientation estimation apparatus capable of estimating the relative positions and attitudes of the transmission apparatus 11 and the reception apparatus 12 by configuring an extended Kalman filter based on the observation equation derived as follows.

すなわち、ΣからみたΣの位置と姿勢を受信装置12上のPSD1及びPSD2で捉えられたビームスポット軌跡の時系列測定値から拡張カルマンフィルタを用いて推定する。ただし、回転(姿勢)行列はノルム1の四元数q=[qにより表現する。ノルム1の複素数が2次元の回転を表すようにノルム1の四元数は3次元の回転を表すことができ、回転行列との間に次のような関係式が成立する。

Figure 0004532334
In other words, it estimated using an extended Kalman filter from the time series measurements of beam trajectories captured by PSD1 and PSD2 on receiver 12 the position and orientation of the sigma T viewed from sigma R. However, the rotation (posture) matrix is expressed by a quaternion q = [q 1 q 2 q 3 q 4 ] T of norm 1. The quaternion of norm 1 can represent a three-dimensional rotation so that the complex number of norm 1 represents a two-dimensional rotation, and the following relational expression holds between the rotation matrix and the quaternion.
Figure 0004532334

ここで、r=[x y z]を含め推定すべき状態ベクトルXを次のように定義する。

Figure 0004532334
Here, a state vector X to be estimated including r = [x y z] T is defined as follows.
Figure 0004532334

さらに、時系列の測定データから状態推定を行うため次のような状態変化モデルを導入する。
k+1=X+w (8)
=h(X,θ)+v (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(X,θ)は送信装置11及び受信装置12の相対位置・姿勢Xと各モーター回転角θ(=[θ θ θ θ)からビームスポット位置への写像を表す関数である。また、w、vはそれぞれシステムノイズと観測ノイズベクトルを表す。各変数の添え字kは時系列上のサンプリング数を示す。 However, y is an observation value vector regarding the beam spot positions on the two PSDs 1 and 2, h (X k , θ k ) is a relative position / posture X of each of the transmission device 11 and the reception device 12 and each motor rotation angle θ (= [ θ 1 θ 2 θ 3 θ 4 ] T ) to a beam spot position. W and v represent system noise and observation noise vector, respectively. The subscript k of each variable indicates the number of samplings in time series.

明らかに上記の状態変化モデルにおいてPSD1及びPSD2′上のビームスポットの測定値を単独に観測値ベクトルとして状態推定を行うことは不可能である。そこで1サンプリング時間でのXの変化は微小であると仮定して、連続する2サンプルの観測データを観測値ベクトルへ割り当てることを考え、新たにPSD1及びPSD2におけるビームスポットへの写像に関する次のような関数

Figure 0004532334
を構成する。
Figure 0004532334
Obviously, in the state change model described above, it is impossible to perform state estimation using the measured values of the beam spots on PSD1 and PSD2 'alone as an observation value vector. Therefore 1 change in X k at the sampling time is assumed to be very small, consider assigning observations consecutive two samples to the observed value vector, the following about the newly mapped to the beam spot in PSD1 and PSD2 Functions like
Figure 0004532334
Configure.
Figure 0004532334

対応する観測値ベクトルは次のように拡張しておく。

Figure 0004532334
The corresponding observation vector is expanded as follows.
Figure 0004532334

ただし、それぞれ3番目の要素は姿勢を表す四元数のノルムが1であることに対応している。式(9)に代わって観測方程式を新たに次のように定義する。

Figure 0004532334
However, each of the third elements corresponds to a quaternion norm representing a posture being 1. Instead of equation (9), the observation equation is newly defined as follows.
Figure 0004532334

ここで、w,vは平均値ベクトル0で互いに独立としその共分散行列がそれぞれΣ,Σで与えられるものとすれば次のような拡張カルマンフィルタを適用することにより状態推定が可能となる。

Figure 0004532334
Here, if w k and v k are independent from each other by the mean value vector 0 and their covariance matrices are given by Σ w and Σ v , respectively, state estimation can be performed by applying the following extended Kalman filter. It becomes.
Figure 0004532334

なお、本発明は、上記実施形態例そのままに限定されるものではなく、実施段階ではその要旨を逸脱しない範囲で構成要素を変形して具体化できる。また、上記実施形態例に開示されている複数の構成要素の適宜な組み合せにより種々の発明を形成できる。例えば、実施形態例に示される全構成要素から幾つかの構成要素を削除してもよい。更に、異なる実施形態例に亘る構成要素を適宜組み合せてもよい。   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.

本発明が対象とする光空間通信システムを示す構成説明図である。1 is a configuration explanatory diagram showing an optical space communication system targeted by the present invention. FIG.

符号の説明Explanation of symbols

11…送信装置、12…受信装置、LD…レーザーダイオード(レーザー光源)、D1〜D4…モーター、M1〜M4…ミラー、PD…ホトディテクタ、PSD1,PSD2…ポジションセンスデバイス、S1,S2…ビームスプリッター、C1,C2…制御部。   DESCRIPTION OF SYMBOLS 11 ... Transmitter, 12 ... Receiver, LD ... Laser diode (laser light source), D1-D4 ... Motor, M1-M4 ... Mirror, PD ... Photo detector, PSD1, PSD2 ... Position sense device, S1, S2 ... Beam splitter , C1, C2... Control unit.

Claims (2)

空間中にレーザー光を伝播させて通信を行う光空間通信システムであって、送信装置側に送信用レーザー光源によるレーザー光の発射方向(パン・チルト角)が調整可能なように2自由度の可動式反射鏡を備え、受信装置側に送信側から到達したレーザー光の向きを調整し直列に配置された2つのビームスプリッターへ導く2自由度の可動式反射鏡と最初のビームスプリッターにより2分した一方の分岐光を次のビームスプリッターによりさらに2分したレーザー光それぞれの照射位置を検知する2つのPSD(Position Sensing Device)を備え、最初のビームスプリッターによる他方の分岐光を受信用PD(Photo Detector)で受光することが可能な光空間通信システムにおいて、各可動式反射鏡の動きデータとPSD上のレーザービームスポットの軌跡データから送信装置における射出ビームの発射位置及び発射方向ベクトルとそれらの微分ベクトル、さらに受信装置における入射ビームの到達位置及び入射方向ベクトルとそれらの微分ベクトルを求めて、これより送信装置と受信装置の相対位置と姿勢を推定することを特徴とする光空間通信システムの相対位置姿勢推定装置。   An optical space communication system that communicates by propagating a laser beam in space, and has two degrees of freedom so that the laser beam emission direction (pan / tilt angle) by the laser beam source for transmission can be adjusted on the transmitter side. It is equipped with a movable reflector and adjusts the direction of the laser beam that arrives from the transmitter side to the receiver side, and guides it to two beam splitters arranged in series. Two PSDs (Position Sensing Devices) for detecting the irradiation position of each laser beam obtained by further dividing the one split beam by the next beam splitter, and receiving the other split beam from the first beam splitter (Photo) In an optical space communication system capable of receiving light by a detector, each movable reflector From the laser beam spot trajectory data on the PSD and the emission position and direction vector and their differential vectors of the outgoing beam in the transmitter, and the arrival position and direction vector and their differential vectors of the incident beam in the receiver A relative position and orientation estimation device for an optical space communication system, characterized in that the relative position and orientation of a transmission device and a reception device are estimated from this. 請求項1に記載の光空間通信システムの相対位置姿勢推定装置において、送信装置からみた受信装置の位置ベクトルに相対姿勢をノルム1の四元数で表したものを合わせた7次元の状態ベクトルを定義し、可動式反射鏡の角度とPSDにより捉えられたレーザービームスポット位置の時系列データにおいて時間的に隣り合うデータをある時刻において同時に観測されたものとみなして導出される観測方程式を基に拡張カルマンフィルタを構成して送信装置と受信装置の相対位置と姿勢を推定することを特徴とする光空間通信システムの相対位置姿勢推定装置。   The relative position / orientation estimation apparatus for an optical space communication system according to claim 1, wherein a 7-dimensional state vector obtained by combining a position vector of a receiving apparatus viewed from a transmitting apparatus with a relative attitude represented by a quaternion of norm 1 is combined. Defined based on the observation equation derived by assuming that the time-series data of the laser beam spot position captured by the angle of the movable reflector and the PSD of the movable reflector were simultaneously observed at a certain time A relative position and orientation estimation device for an optical space communication system, wherein an extended Kalman filter is configured to estimate a relative position and orientation of a transmission device and a reception device.
JP2005139816A 2005-05-12 2005-05-12 Relative position and orientation estimation device for optical space communication system Expired - Fee Related JP4532334B2 (en)

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)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JP4380691B2 (en) 2006-11-28 2009-12-09 日産自動車株式会社 Sub-chamber internal combustion engine

Citations (5)

* Cited by examiner, † Cited by third party
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

Patent Citations (5)

* Cited by examiner, † Cited by third party
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