Kalma Filter 로 IMU 와 결합 지구자전효과와중력모델을고려하지않은 INS 설계 KIECH 양광웅작성 관성항법시스템 (INS, Iertial Naiatio Sstem) 은 ehicle의초기위치로부터자이로와가속도센서의각속도와가속도를적분하여현재 ehicle의자세와속도, 위치를계산한다. 는 INS 시스템을단순화하여지구자전효과와중력모델을고려하지않는다. 그리고자세를계산하기위하여 quaterio을사용한다. INS( 관성항법시스템 ) 의알고리즘구성도 Gro(IMU) Accel(IMU) ω f HD HS ω. Attitude Kalma Filter Λ, f 2. Velocit, Positio Kalma Filter or, l r r r Λ 항법좌표계에서오일러각으로차량의방위표시 오일러각에대응하는회전행렬 항법좌표계에서차량의속도 (X-북쪽, Y-동쪽, Z-지구중심방향속도 ) r 항법좌표계에서차량의위치 항법좌표계에서단위중력가속도 f f f f 가속도센서로측정한가속도 ( 동체기준,, 축방향가속도 ) ω 자이로센서로측정한각속도 ( 동체기준,, 축각속도 ) r h 에서측정한차량의위도, 경도, 고도, 에서측정한차량의속도와진행방향
에서측정한차량의위도, 경도, 고도와항법좌표계에서차량의위치는다음관계가있다. r Dr D N h ( E h)cos IMU 센서의보정 IMU 센서의출력이각속도와가속도 ω, f 일때 HD(Heuristic Drift eductio) 알고리즘과 HS(Heuristic Scale eulatio) 알고리즘을사용하여드리프트와스캐일이보정된 계산한다. ω 와 f 를 ω ω 바이어스값 를계산하기위해 HD 알고리즘을사용한다. (HD 문서참조 ) f s a f 스케일펙터 sa 를계산하기위해 HS 알고리즘을사용한다. (HS 문서참조 ). Attitude Kalma Filter Iitialie Kalma Filter 에사용할상태변수를회전행렬과오일러각으로표시하고, 공분산행렬은오일러각의 공분산으로표시한다. ( 회전행렬에대한공분산이아님에주의 ) 상태변수 : ( 회전행렬에대한상태변수 ) Λ ( 오일러각에대한상태변수 ) 오일러각에대한공분산행렬 :
P 상태변수와공분산행렬은다음과같이초기화한다. ˆ I 3 3 ˆ P 8 I 33 Attitude Predict 시스템모델 센서에서측정되는각속도는회전행렬의미분과다음과같은관계를가진다. Ω Ω 는 ω 의 sew smmetric 행렬로다음과같이정의된다. Ω ω 그리고오일러각의미분은각속도와다음과같은관계가있다. Λ C ω C 은자이로센서에서측정한각속도를오일러각의변화율로변환하기위한행렬이다. C si ta cos ta cos si si / cos cos / cos 상기행렬에서 /2 인경우 cos 이므로행렬원소의분모가 에가까워지는경우가 발생한다. 하지만이러한경우에는차량이수직으로서는경우를의미하기때문에자동차와같은경우는발생하지않을것이다.
Predict ˆ ˆ ( I Ω t) ˆ ( t) ( t) ( t) ˆ ˆ C ω t 오일러각에대한공분산은다음과같이업데이트한다. P P C QC ( t) 2 Q,, ) 는다음과같이계산한다. 2 2 2 dia( ω Attitude Update Grait 중력에의한차량의자세측정모델 중력에의한차량의각도측정모델 을참고하면, 를구할수있다. 이각들로부터차량의자 세를나타내는행렬은다음과같이보정된다. ( ) ( ) 그리고오일러각은다음과같이보정된다. Λ Λ C C 은다음과같다. C cos / cos si / cos si cos cos ta si ta 차량의자세측정모델함수 h() 는다음과같이오일러각을변환없이그대로사용한다. h( Λ ) Λ
자세측정값 는, 를사용하여다음과같이계산된다. Λ C Attitude update ˆ ( K ) ( K ) ˆ ˆ ˆ K ( h( ˆ )) K 는 K 의 행 열원소이고, K 는 K 의 2행 2열원소이다. P P K P K P ( P C C ) 2 2 2 dia(,, ) 이득이커야하고 에서멀어질수록이득이적어야한다. 을다음과같이계산한다. 왜냐하면, 가속도의크기가 근처일때. f ω, 8 Attitude Update Headi ad Velocit 의위치변위에의한각도측정모델 차량의형태가자동차와같은경우는운동방향이 X 축으로고정되어있다. 그래서 에서측정 한속도와방위데이터로부터차량의자세를업데이트할수있다. 먼저, 의속도 다음과같이계산할수있다. 와방위 를읽어온다. 그리고회전행렬 로부터차량의 aw 각을 a 2 ta a aij 는회전행렬 의 i번째행과 j번째열의원소다.
이제 에서측정한각 다음과같이보정된다. 와차량의진행방향 로부터차량의자세를나타내는행렬은 ( ), 그리고오일러각은다음과같이보정된다. Λ Λ C 차량의자세측정모델함수 h() 는다음과같이오일러각을그대로사용한다. h( Λ ) Λ 자세측정값 는 를사용하여다음과같이계산된다. Λ C Attitude update ˆ ( K ) ˆ ˆ ˆ K ( h( ˆ )) K 는 K 의 3행 3열원소이다. P P K P K P ( P C C ) 을다음과같이계산한다. ( 2 2 2 dia(,, ), ) 6 2 8,
2. Velocit, Positio Kalma Filter Positio, Velocit Predict 시스템모델 차량좌표계에서측정된가속도센서값을항법좌표계로변환후중력을제거한다. f 차량의속도에대한식은다음과같다. r 상기수식들을다음과같이상태방정식형태로만든다. F Gu I f r I r 상태변수 : ˆ r Predict ˆ Aˆ Gu t P AP A GQG t 2 A I F t Q 2 2 2 2 2 2 dia ( f,,,,, ) f f
Velocit Update 차량의속도측정모델 차량의좌우바퀴에장착된엔코더를읽어와차량의전진속도를계산한다. 차량은전방 ( 차량좌 표계의 X 축방향 ) 으로만운동이가능하므로, 차량좌표계에서차량의속도는다음과같은식이된 다. r 2 l r, l 은각각오른쪽바퀴의속도와왼쪽바퀴의속도다. 좌우바퀴의공분산 V 는차량의속도에대한공분산 로다음과같이전파된다. ZVZ 2 2 Z ( r, l) V 2 2 dia(, ) r l 항법좌표계에서의속도 는차량좌표계의속도로다음과같다. h( ) h H Update
ˆ K ( H ˆ ) ˆ P I K H ) P ( K P H ( HP H ) Positio Update 위치측정모델 "M 좌표와 좌표 (WGS84) 간의관계 " 를이용하여 로측정한위도 ( ), 경도 ( ), 고도 ( h ) 를항법좌표계의 XYZ 좌표로변환한다. Dr 항법좌표계의 XYZ 좌표로변환된데이터를장시간수집한후, 이들데이터들로부터 산을계산한다. 의공분 Var( ) 항법좌표계에서차량의위치 r 는변환없이 M 좌표로표시된다. h( r ) h H I Update ˆ K ( H ˆ ) ˆ P I K H ) P ( K P H ( HP H )
Normaliatio of otatio Matri 회전행렬이계속해서업데이트된다면수치계산의미소한오류가누적되어행렬의직교성이만족 되지않는다 ( ). I 먼저, 가장간단한방법은회전행렬 을특이값분해 (siular alue decompositio) 하여특이값을 로바꾼후재조립하면된다. 또다른방법으로다음과같은수식을사용할수도있다. 2 ( ) 항법좌표와 좌표 (WGS84) 간의관계 먼저, 로부터수신한위도 ( ), 경도 ( ), 고도 ( h ) 를 M 좌표계로변환한다. M 좌표계는지표면에서남북방향이 - 축이고, 동서방향이 - 축이다. - 축은북쪽이 + 방향이고, - 축은동쪽이 + 방향이다. 우리나라 M(raserse Mercator) 좌표계원점의경위도값 : - 중부원점 : N38, E27 M N h ( h)cos M E M h h 는고도를나타내며 N 과 미한다그리고 와 는 M좌표계원점의위도와경도상수다. E 는각각북쪽자오선방향과동쪽방향곡률반경을의 N 2 ( e ), 2 2 3/2 ( e si ) E ( e si ) 2 2 /2 637837. 와 e.8899 는각각지구타원의장축과이심률이다. 그리고, M 좌표를항법좌표로변환한다.
M M M 중력에의한차량의자세측정 중력가속도는항상지구중심으로향하기때문에가속도센서에다른힘이작용하지않을경우 (,, 9.8) 가측정된다. 가속도센서에서측정한가속도와중력가속도를비교함으로센서의 자세를보정할수있다. 하지만이러한조건은가속도센서에작용하는힘이오직중력만있을 때가능하다. 중력가속도와이러한힘을분리하여측정할수없기때문에, 중력가속도외다른힘 이작용하고있는조건은 a 인지확인해보는것이제일간단한방법이다. 가속도센서에서측정한가속도 a a a a 한종류의가속도가포함되어있다. 이를식으로나타내면다음과같다. 에는중력가속도와센서의가속에의한다양 a ω ( ) si cos si cos cos 값 (,, 9.8) 는선가속도이며 을가진다. 는관성좌표계에서중력가속도 위식에서선가속도 가 이고각속도 ω 가 일때는다음과같이간단히쓸수있다. a si a cossi a coscos 오일러각으로정리하면다음과같다. a a ta, si. a a ata 2 a, a, asi.
Calculatio of Euler Ale Velocit usi Aular Velocit 오일러각의회전속도 (Euler Ale Velocit ) 는각속도 (Aular Velocit ) 로부터계산가능하다. ZYX(oll-Pitch-Yaw) Ales: 오일러각이 Z축Y축X축변환순서를따르는경우, 기준좌표계에대한각속도 (,, ) 와오일러각 (,, ) 의회전속도사이에는다음과같은관계가성립한다. ( ) ( ) ( ) 상기식을정리하면아래와같다. cos cos si, si cos cos C C si cos / cos si / cos, si cos C C cos ta si ta 물체좌표계에대한각속도와오일러각의회전속도사이에는다음과같은관계가성립한다. ( 주의 : 기준좌표계에대한물체좌표계가 ω 로회전하고있기때문에좌표계를동일하게만들기 위해서 ω 를기준좌표계로변환하여야한다 : ( ) ( ) ( ) ω ) ( ) ( ) ( ) ( ) ( ) ( ) ( ) ( ) ( )
상기식을정리하면아래와같다. si, cos si cos C C si cos cos si ta cos ta, cos si C C si / cos cos / cos Euler Ales 오일러각 (Euler Ales) 는 3차원공간에서물체의방위를표시하기위한 3개각도의조합이다. 오일러각으로물체를회전할때는회전순서에주의하여야한다. 회전순서 (X축Y축Z축 or Z축 Y축X축등각축별회전순서의조합 ) 에따라물체의회전된최종방위가달라지기때문이다. 일반적으로 ZYX (oll-pitch-yaw) 회전과 XYZ 회전이주로사용된다. ZYX(oll-Pitch-Yaw) Ales: 기준좌표계에대한물체좌표계의회전결과는다음과같이계산된다 :. 기준좌표계를 X-축 (Yaw) 을중심으로 만큼회전한다 : ( ) 2. 기준좌표계를 Y-축 (Pitch) 을중심으로 만큼회전한다 : ( ) 3. 기준좌표계를 Z-축 (oll) 을중심으로 만큼회전한다 : ( ) ( ) ( ) ( ) cos cos si si cos cos si cos si cos si si = cos si si si si cos cos cos si si si cos si si cos cos cos ( ) cos si si cos, cos si ( ) si cos, cos si ( ) si cos