使用載波相位方法之室內物體位置姿態判定統

(1)

An Indoors Location and Attitude Determination System Using

(2)

An Indoors Location and Attitude Determination System Using

Carrier Phase Techniques

Student

Yen-Ting Chen

Advisor

Tsung-Lin Chen

A Thesis

Submitted to Department of Mechanical Engineering National Chiao Tung University

in partial Fulfillment of the Requirements for the Degree of

Master In

Mechanical Engineering June 2011

(3)

(4)

An Indoors Location and Attitude Determination System Using

Carrier Phase Techniques

Student:Yen-Ting Chen Advisor:Dr. Tsung-Lin Chen

Department of Mechanical Engineering

National Chiao Tung University

Abstract

This paper proposed an indoors location and attitude determination system using a commercial production system (Cricket). The Cricket system was originally designed for the 3D positioning for indoors applications. This research intends to enhance its functionality for the 3-axis attitude determination by using the carrier phase techniques. A similar approach can be found in the multi-antenna GPS system. In the GPS system, the signals from one satellite to different antennas are assumed to be parallel. This assumption is acceptable because the distance between antenna and satellite is quite long. However, this assumption cannot be applied to the indoor positioning system since the distance between transmitter and receivers is relatively short. Therefore, we derived new equations for the indoors positioning system so that it can do the attitude determination without the parallel-signal assumption.

In this approach, we used the “baseline vectors,” “line of sight vectors,” and phase differences between received signal of different receivers to determine the attitude of the object. The baseline vector is the relative position vector between different receiver, which is obtained by design. The line-of-sight vector is the relative position vector between transmitter and receiver, This can be obtained by the original Cricket function. And, the phase difference between received signals can be

obtained by the “correlation analysis” method. With these three information, the

attitude determination was formulated into an optimization problem and solved by the “Wahba method.”

The proposed method is verified using Matlab simulations and some preliminary experimental results. According to the simulations, the system needs, at least, three transmitters and four receivers to achieve 3D positioning and 3D attitude

(5)

(6)

... I ABSTRACT ... II ... III ... IV ... VII ... VIII ... 1 ... 9

(7)

V

... 35

(8)

... 54

(9)

VII ... 40 ... 45 ... 46 ... 48 ... 49 ... 50 ... 50

(10)

... 2 ... 3 ... 4 ... 5 ... 6 ... 7 ... 10 ... 10 ... 11 ... 12 ... 14 ... 14 ... 15 ... 17 ... 18 ... 22 ... 25 ... 36 ... 36 ... 38 ... 38 ... 41 ... 43 ... 43 ... 47 ... 49 ... 53 ... 55 ... 56

(11)

(12)

s b   θ   GPS θ

b

s

(13)

3

(14)

d1

d3

(15)

5 2 2 2 2 1 1 1 1 2 2 2 2 2 2 2 2 2 2 2 2 3 3 3 3 ( ) ( ) ( ) ( ) ( ) ( ) ( ) ( ) ( ) m m m m m m m m m d x x y y z z d x x y y z z d x x y y z z                   2 2 2 ( ) ( _i) ( _i) ( _i) _i , 1, 2, 3 e i  xx  yy  z z d i ( ) 2 1 ( ) ( ) n k ss i f x E e i   



( )k x ( 1) ( ) ( ) 1 ( ) ( ) ( ) k k k k x  x F x  f x ( )k

x

(k 1) x  f f x1 x2

(16)

us rf d d T v v    344 /m s 3 10 / 8 m s us d T v

(17)

(18)

(19)

(20)

Y

y

(21)

11 1 0 0 ( , ) , ( , )= 0 cos( ) sin( ) 0 sin( ) cos( ) x X y R x Y R x z Z              _                        cos( ) 0 sin( ) ( , ) , ( , )= 0 1 0 sin( ) 0 cos( ) x X y R x Y R x z Z               _                       Z X z θ θ x

(22)

cos( ) sin( ) 0 ( , ) , ( , )= sin( ) cos( ) 0 0 0 1 x X y R x Y R x z Z              _   _                    Y y Ψ Ψ x X

(23)

13

( , ) ( , ) ( , )

1 0 0 cos( ) 0 sin ( ) cos( ) sin ( ) 0

0 cos( ) sin ( ) 0 1 0 sin ( ) cos( ) 0

0 sin ( ) cos( ) sin ( ) 0 cos( ) 0 0 1

cos( )cos( ) cos( ) sin ( ) sin ( )

sin ( ) sin ( )cos( ) co b ECI R R x  R y R z                                   _ _{ } _{ }  _              

  s( ) sin ( ) sin ( ) sin ( ) sin ( )+cos( )cos( ) sin ( )cos( )

cos( ) sin ( )cos( )+ sin ( ) sin ( ) cos( ) sin ( ) sin ( ) sin ( )cos( ) cos( )cos( )

                                b ECI x X y R Y z Z      _               b E C I R

(24)

t



t



Listener Amplifier Zero crossing

detector

Timer Rectifier

(25)

(26)

 



 



i i i i i A A A A A i i i i A A A A t t t N S N S                            

 

i A t  i A  i A N i A S  i i i i AB AB AB AB AB SD    N   S 



AB



(27)

17 i AB SD 1 sin( ) sin i AB i AB SD L SD L         _ _   i AB

SD

L B A θ

(28)



j i



t 360 T        = t 360 T     

(29)

19

 

   

1 2 0 1 2 0 1 0 1 1 1 N x n N y n N xy n R x n N R y n N R x n y n N         



(30)

1 cos xy x y R R R  _  Wahba’s problem Wahba

 

2 1 2 n i i i i i i _{i F} J R w b Ru w b Ru     



i b u_i w_i

 

  

T T



2

 

T J R tr B B tr U U  tr RA

(31)

21









1 1 2 2 1 1 2 2 , ,..., , ,..., T n n n n A BU B w b w b w b U w u w u w u   

 

ˆ ₂ T _{max .} J R  tr RA  ( , ) ( , ) ( , )

1 0 0 cos( ) 0 sin ( ) cos( ) sin ( ) 0

0 cos( ) sin ( ) 0 1 0 sin ( ) cos( ) 0

0 sin ( ) cos( ) sin ( ) 0 cos( ) 0 0 1

cos( )cos( ) cos( ) sin ( ) sin ( )

sin ( ) sin ( )cos( ) cos opt A R x R y R z                                   _ _{ } _{ }  _              

  ( ) sin ( ) sin ( ) sin ( ) sin ( )+cos( )cos( ) sin ( )cos( )

cos( ) sin ( )cos( )+ sin ( ) sin ( ) cos( ) sin ( ) sin ( ) sin ( )cos( ) cos( )cos( )

                               

 



 



 

1 2,3 1 1 1, 2

tan , sin 1,3 , tan

3,3 1,1 opt opt opt opt opt A A A A A             _ _    _ _     opt A Wahba’s problem

(32)

Wahba’s problem 3.2.3 Wahba’s problem [3] r s sb bb   θ b r s Rs   GPS θ r y r z b z b y θ b

s

b

s

b

(33)

23

 





2 1 1 p q mn mn m n m n J R w  b Rs   



 

 





2 1 2 1 2 11 1 1 1 2 ₃ 1 2 3 ˆ ˆ ˆ ˆ ˆ ˆ T B S F q p pq _{p q} q _q p p J R W B RS W S s s s B b b b                      _ _      _  _   _  _

 



 







1 2 1 2 1 2 1 2 1 2 1 2 2 T T T T S B S S B S T T S B S J R tr W W W tr W S R BW B RSW tr W W B RSW          Wahba’s problem

 





 



  

1 2 1 2 ˆ _{max .} ˆ T T S B S T T T S B T B S J R tr W W B RSW J R tr RSW W B tr RA A BW W S             

(34)

 

   

1 0 0 0 1 0 0 0 det det opt A U V d d U V             Wahba’s problem Wahba’s problem s b

(35)

25





2 2 2 2 mn n m n m v  s  b  s b





2 2 2 ˆ ˆ ₂ ˆ mn n m n m v  s  Rb  s Rb nm 2 2 2 nm nm ˆ ˆ ˆ ˆ ˆ ˆ ˆ ₂ ˆ mn n mn n n v s d v s s d d       r

z

21 v 31 v 1 b 11 v 3 b 1 s r

x

r

y

2 b

(36)









2 2 2 2 2 2 ˆ ˆ ˆ ₂ ˆ ˆ ₂ ˆ 1 _ˆ _ˆ ˆ ₂ ˆ 2 n n mn mn n m n m n m m n nm nm s s d d s Rb s Rb s Rb Rb s d d           



2 ₂



1 _ˆ _ˆ _ˆ 2 2 Rbm  s dn nmdnm  nm

 





2 1 1 ˆ p q nm n m m n J R  s Rb   



   11 1 1 1 2 ₃ 1 2 3 ˆ ˆ ˆ ˆ ˆ ˆ p q qp _{q p} q _q p p S s s s B b b b                  _ _      _  _   _  _

 







 







2 T F T T T T T T J R S RB J R tr tr S RB S RB tr S RB              

(37)

27

 





 



  

ˆ _{max .} ˆ T T T T T T J R tr S RB J R tr RB S tr RA A S B               

   

1 0 0 0 1 0 0 0 det det opt A U V d d U V             1 2 3 10 0 0 0 , 10 , 0 0 0 10 b b b             _{ } _{ } _{ }             1 2 0 10 0 , 0 30 30 s s         _{ } _{ }        

(38)

nm   m1, 2,3 n1, 2













2 2 1 1 1 1 11 11 2 2 1 2 2 1 12 12 2 2 1 3 3 1 13 13 1 _ˆ _ˆ ˆ ₂ ˆ _222.2529 2 1 _ˆ _ˆ ˆ ₂ ˆ _31.5121 2 1 _ˆ _ˆ ˆ ₂ ˆ _199.0242 2 s Rb Rb s d d s Rb Rb s d d s Rb Rb s d d                













2 2 2 1 1 2 21 21 2 2 2 2 2 2 22 22 2 2 2 3 3 2 23 23 1 _ˆ _ˆ ˆ ₂ ˆ _173.0125 2 1 _ˆ _ˆ ˆ ₂ ˆ _27.1703 2 1 _ˆ _ˆ ˆ ₂ ˆ _263.3029 2 s Rb Rb s d d s Rb Rb s d d s Rb Rb s d d                  222.2529 31.5121 199.0242 173.0125 27.1703 263.3029      _ _   

(39)

29 5 0.1730 0.0272 0.2633 1 10 0 0 0 1.1858 0.0130 1.3870 T A S B         _ _    

   

5 0.1688 0.9856 0 0 0 1.0000 0.9856 0.1688 0 1.8514 0 0 1 10 0 0.0486 0 0 0 0 0.6471 0.6104 0.4568 0.0045 0.5961 0.8029 0.7624 0.5216 0.3830 1 0 0 0 1 0 , det det 0 0 T opt opt U S V A U U d U V d A      _  _            _ _           _   _           _ _      0.4924 0.5868 0.6428 0.4568 0.8029 0.3830 0.7408 0.1050 0.6634             opt A = 30 , = 40 , = 50    _   _ 

(40)

1 2 3 10 0 0 0 , 10 , 0 0 0 10 b b b             _{ } _{ } _{ }             1 30 30 30 s            nm   m1, 2,3 n1













2 2 1 1 1 1 11 11 2 2 1 2 2 1 12 12 2 2 1 3 3 1 13 13 1 _ˆ _ˆ ˆ ₂ ˆ _300.0000 2 1 _ˆ _ˆ ˆ ₂ ˆ _109.8076 2 1 _ˆ _ˆ ˆ ₂ ˆ _409.8076 2 s Rb Rb s d d s Rb Rb s d d s Rb Rb s d d               



300.0000 109.8076 409.8076



   T A S B

(41)

31 5 0.5774 0.5774 0.5774 0.5774 0.7887 0.2113 0.5774 0.2113 0.7887 2.7000 0 0 1 10 0 0 0 0 0 0 0.577 U S V         _  _            _ _       T 4 0.8165 0 0.2113 0.1494 0.9659 0.7887 0.5577 0.2588 1 0 0 0 1 0 , det ( ) det ( ) 0 0 0.1381 0.3494 0.9267 0.9773 0.2000 0.0702 0.1 opt opt A U V d U V d A   _ _ _              _ _         608 0.9154 0.3691           opt A = 10.7710 , = 67.9332 , = 68.4375    _    1 2 10 0 0 , 10 0 0 b b         _{ } _{ }        

(42)

1 2 0 10 0 , 0 30 30 s s         _{ } _{ }         nm   m1, 2 n1, 2









2 ₂ 1 1 1 1 11 11 2 ₂ 1 2 2 1 12 12 1 _ˆ _ˆ ˆ ₂ ˆ _7.5604 2 1 _ˆ _ˆ ˆ ₂ ˆ _224.3484 2 s Rb Rb s d d s Rb Rb s d d          









2 ₂ 2 1 1 2 21 21 2 ₂ 2 2 2 2 22 22 1 _ˆ _ˆ ˆ ₂ ˆ _56.8008 2 1 _ˆ _ˆ ˆ ₂ ˆ _165.6660 2 s Rb Rb s d d s Rb Rb s d d           7.5604 224.3484 56.8008 165.6660         

(43)

33 5 0.1445 0.9895 0 0 0 1.0000 0.9895 0.1442 0 1.1984 0 0 1 10 0 0.0288 0 0 0 0 0.1663 0.9861 U S V      _  _            _ _       T 0 0.9861 0.1663 0 0 0 1.0000 1 0 0 0 1 0 , det ( ) det ( ) 0 0 0.9998 0.0223 0 0 0 1.0000 0.0223 0.9998 0 opt opt A U V d U V d A   _ _             _ _           _  _     opt A =  90.000 , = 0 ,     = 1.280  Wahba’s problem   nm   Wahba’s problem S

(44)

(45)

(46)

L d1 d 2 h



d 3 d4 1mm 5mm 5mm 2 ….... di 1( )

(47)

37 i

d

2 2 2 1 1 1 1 2 2 2 2 2 2 1 1 1 1 1 2 2 2 2 1 1 2 2 2 2 1 1 2 2 2 1

( cos ) 2 ( cos )cos , 2, 3,4,5 , 5 * ( 2)

cos 2 cos 2 cos 2 cos

cos cos sin , 3.4.5..., 5 * ( 2) i i d d d j d d j i j mm i d d jd j d jd d d j d d d j d j i j mm i                                    1 , 2,3, 4, 22 i d d i

(48)

(49)

(50)

(51)

(52)

(53)

43 Beacon3 1 0 2 3 1 1 s 11 v v21 31 v 1 b 2 b 3 b 2 3

(54)

0 10 0 10

_ 0 0 , _1 0 , _ 2 10 , _ 3 10

0 0 0 5

Listener Listener Listener Listener

       

       

_{ } _{ } _{ } _{ }

       

(55)

45

7.081e 15 10.000 7.954e 15 10.000

_ 0 6.970e 15 , _1 7.240e 15 , _ 2 10.000 , _ 3 10.000

1.673e-15 4.741e 18 3.231 16 5.000

                     _  _  _  _ _ _ _ _                    1 2 3

10.000 8.728e 16

10.000 2.691e 16 ,

10.000 ,

10.000 1.677e 15

1.350e 15

5.000 b

b



























 

_



_



_

_



_

_





























(56)

0.000 3.535 6.123 6.123

_ 0 0.000 , _1 5.732 , _ 2 7.391 , _ 3 3.427

0.000 7.391 2.803 13.257

                _ _  _ _ _ _ _ _                 7.081e-15 3.535 6.123 6.123 _ 0 6.970e-15 , _1 5.732 , _ 2 7.391 , _ 3 3.427 1.673e-15 7.391 2.803 13.257

                  _ _  _ _ _ _ _ _                 1 2 10.000 7.081e-15 6.971e-15 , 10.000 30.000 30.000 s s         _ _ _ _        

(57)

47 





























































0 1 1 1 11 2 1 12 3 1 13 00 1 11 1 21 22 1 22 33 1 sin 2 sin 2 sin 2 sin 2 sin 2 sin 2 sin 2 sin 2 sin 2 sin 2 sin 2 sin 2 sin 2 sin 2 sin 2 sin 2 US US US US US US US US US f t f t US f t f t US f t f t US f t f t US f t f t US f t f t US f t f t US f t f t                                          



23



0~3 US US_00~33 f₁ US f

(58)

(59)

49 mV n n 1 2 n4 n1 n2 n3 N1 N2

(60)

nm





11   ₂₁ 12   ₂₂ 13   ₂₃

(61)

51 257.124 145.347 458.947 164.430 158.027 431.997          257.124 145.347 112.951 164.430 158.027 219.078 1264.664 910.125 996.091 T A S B            0.162 0.707 0.688 0.162 0.707 0.688 0.973 5.306e-06 0.229 1899.972 0 0 0 100.039 0 0 0 1.403e-13 0.683 0.655 0.320 0.492 U S V        _   _                    

   

0.089 0.865 0.538 0.750 0.383 1 0 0 0 1 0 , det det 0 0 0.353 0.612 0.707 0.573 0.739 0.353 0.739 0.280 0.612 T opt opt A U U d U V d A                  _ _            _ _     opt A

= 30.0103 (Roll) , = 45.0010 (Pitch) , = 60.0128 (Yaw)

(62)

(63)

(64)

(65)

55 DSP X Y Z RS232

(66)

(67)

57

1. Nissanka Bodhi Priyantha, “The Cricket Indoor Location System,”

Massachusetts Institute of Technology, Ph.D Thesis, June 2005.

2. Gang Lu, “Development of a GPS Multi-Antenna System for Attitude

Determination, ” University of Calgary, Department of Geomatics Engineering,

Ph.D Thesis , Jan 1995.

3. Shane-Woei Lee, “On-The-Fly Carrier Phase Ambiguity Resolution Without Using Pseudorange Measurement For Satellite-Based Differential Positioning,” Ohio University, Department of Electrical and Computer Engineering, M.S. thesis, Nov 1994.

4. GPS 2010 9

5. -- 2008

4

6. JiunHan Keong, “GPS/GLONASS Attitude Determination with a Common Clock

using a Single Difference Approach, ” University of Calgary, Department of

Geomatics Engineering, 1999.

7. Gang Lu, M.E.Cannon, G.Lachapelle, “Attitude Determination in a Survey

Launch Using Multi-Antenna GPS Technologies,” University of Calgary, Department of Geomatics Engineering, 1993.

8. WAHBA,G., “A Least Squares Estimate of Spacecraft Attitude, ” SIAM Review,

Vol.7, No.3, pp. 409, July 1965.

9. Cohen,C.E., H.S.Cobb, B.W.Parkinson, “Two Studies of High Performance

Attitude Determination Using GPS: Generalizing Wahba's Problem for High Output Rates and Evaluation of Static Accuracy Using A Theodolite,” Proceedings of the Fifth International Technical Meeting of the Satellite Division

(68)

of the ION, GPS-92, Albuquerque, N.M.16-18, Sep 1992.

10. F.Landis Markley, “Attitude Determination using Vector Observations and the

Singular Value Decomposition,” the Journal of the Astronautical Sciences, Vol.38, No.3, pp. 245-258, July-September 1988.

11. F. L. Markley, “Attitude Determination using Two Vector Measurements,”

Proceedings, FlightMechanics Symposium, NASA Goddard Space Flight Center, Greenbelt, MD, NASA Conference Publication NASA/CP-19989-209235, pp. 39-52, May 1999. 12. “ ” 2003 13. 2005 6 14. “ ” , 2008 15. http://www.ils.org.tw