Proceedings VCM 2012 66 ứng dụng bộ lọc kalman phi tuyến mở rộng xây dựng

Ứng dụng bộ lọc Kalman phi tuyến mở rộng xây dựng thuật toán xác định tham số định hướng trên cơ sở kết hợp con quay tốc độ góc với từ kế và gia tốc kế Applying none-linear extended Kal

Ứng dụng bộ lọc Kalman phi tuyến mở rộng xây dựng

thuật toán xác định tham số định hướng trên cơ sở kết hợp

con quay tốc độ góc với từ kế và gia tốc kế Applying none-linear extended Kalman filter to construct

a algorithm for calculate directional parameters based on MEMs

Trần Đức Thuận, Bùi Hồng Huế, Trương Duy Trung, Trần Xuân Kiên, Nguyễn Sỹ Long

Viện Khoa học - công nghệ Quân sự e-Mail: txkien2003@gmail.com; tdtrung_tdh05@yahoo.com

Tóm tắt

Bài báo trình bày phương pháp ứng dụng bộ lọc Kalman phi tuyến mở rộng để xác định các tham số định

hướng cho các vật thể dịch chuyển trên cơ sở kết hợp con quay tốc độ góc với từ kế và với gia tốc kế theo

phương pháp ghép chặt

Abtract:

This paper presents a methodology utilizing nonlinear extended Kalman filter to determine the orientation

parameters for moving objects based on MEMs according to rigidly mounted method

1 Phần mở đầu

Trong các bài toán dẫn đường hoặc điều khiển các

vật thể chuyển động (bao gồm chuyển động dịch

chuyển của tâm khối và chuyển động quay) luôn

có hai vấn đề, đó là vấn đề định hướng và vấn đề

định vị Vấn đề định hướng là xác định tương quan

về góc giữa vật thể và hệ tọa độ chuẩn (hệ tọa độ

được lấy làm gốc để đánh giá sự chuyển động), tức

là tương quan giữa hệ tọa độ liên kết và hệ tọa độ

chuẩn Vấn đề định vị là việc xác định vị trí tâm

khối và vận tốc của nó so với hệ tọa độ chuẩn

Trong bài báo này chỉ xem xét vấn đề định hướng

Việc sử dụng thông tin về tốc độ góc của các con

quay vi cơ cung cấp để xác định các tham số định

hướng đã được nhiều sách giáo khoa của nước

ngoài trình bày [1], đã có một số bài báo ở Việt

Nam đề cập Tuy nhiên, thông tin về tốc độ góc do

con quay vi cơ đưa ra luôn có các thành phần sau:

tốc độ góc quay thực tế, nhiễu đo và độ trôi (thành

phần biến đổi chậm), vì vậy việc xác định các

tham số định hướng theo phương pháp trực tiếp

luôn có sai số và sai só tăng theo thời gian Đã có

một số công trình ở nước ngoài sử dụng thông tin

của từ kế và gia tốc kế để khắc phục vấn đề sai số

vừa nêu, song chủ yếu dùng bộ lọc Kalman tuyến

tính để ước lượng các sai số Có thể có những

công trình quân sự đã sử dụng phương án ứng

dụng bộ lọc Kalman phi tuyến mở rộng để kết hợp

con quay vi cơ với từ kế và gia tốc kế, song chúng

không được công bố Vì vậy việc xây dựng thuật

toán này là cần thiết trong giai đoạn hiện nay đang

có nhiều ứng dụng các phần tử vi cơ cho UAV, tay

máy, cơ cấu dịch chuyển cơ khí, phương tiệm ngầm, tầu thủy v.v

2 Ma trận cosin chỉ phương và các góc định hướng

Giả sử có vật thể chuyển động B với hệ tọa độ liên kết OXYZ (hệ tọa độ gắn liền với vật thể) được xét trong hệ tọa độ chuẩn A là O0X0Y0Z0 Dịch chuyển tịnh tiến gốc hệ tọa độ chuẩn đến tâm khối vật thể (các trục O0X0, O0Y0, O0Z0 luôn tự song song với với chính nó trong quá trình dịch chuyển-hình 1) Các góc mô tả quan hệ giữa hai hệ tọa độ này được gọi là góc định hướng, gồm góc hướng

 , góc chúc ngóc  và góc Cren  (định nghĩa các góc này thể hiện ở công trình [2 ])

H 1 Hệ toạ độ liên kết và hệ toạ độ chuẩn

1 - Mặt phẳng nằm ngang ;2 - Mặt phẳng đối

xứng TBB; 3 - Mặt phẳng đứng chứa trục dọc 4

- Mặt phẳng đứng chứa trục OZ

Từ các góc định hướng    , , [2] người ta xác

định ma trận cosin chỉ phương giữa hai hệ tọa độ

như sau:

,

T

(1)

(2)

Ma trận cosin chỉ phương cho phép chuyển đổi tọa

độ hoặc véctơ từ hệ tọa độ A sang B và ngược lại,

tức là nếu biết véctơ X A (x A y z A A)Ttrong hệ

tọa độ A thì véctơ đó trong hệ tọa độ B sẽ có giá

1

X  A X CX

Nếu bằng cách nào đó nhận được giá trị của các

phần tử aij của ma trận A từ các biểu thức trong ma

trận A (2) dễ dàng nhận thấy các góc  ,  ,  :

13 11

arctg a a



Việc xác định các góc  ,  ,  hoặc các phần tử aij

hoặc cij chính là lời giải của bài toán định hướng

Toán học đã chứng minh nếu bằng cách nào đó đo được tốc độ quay của hệ tọa độ B so với hệ tọa độ

A (hình 2) thì có thể xác định được các tham số định hướng nếu trên Thực tế ngày nay kỹ thuật đã chế tạo được các con quay vi cơ có kích thước nhỏ cho phép đo các vận tốc góc x,y,z của véctơ quay của hệ tọa độ B so với hệ tọa độ A nếu hệ tọa

độ A là hệ tọa độ quán tính

H 2 Sự quay của hệ tọa độ B so với hệ tọa độ A

Hiện nay có hai phương pháp cơ bản xác định ma

trận A trên cơ sở thông tin về tốc độ góc

z

y

x  

 , , Phương pháp dùng 4 số

Rodring-Haminton và phương pháp giải trực tiếp 9 phương

trình vi phân mô tả quan hệ các phần tử c ij của ma

trận C với các tốc độ góc x,y,z Quan hệ

giữa ma trận C và các tốc độ quay x,y,z

được [2] thể hiện như sau:

C C   (4)

trong đó [ ] là ma trận phản đối xứng:

 

0 0 0

z y

y x

 

 



(5)

Từ (4) và (5) có 9 phương trình vi phân sau:

X0

Z0

X'0

Z'0

X Z

y



x

 z



(6)

Từ hệ phương trình (6) cho thấy nếu có các điều

kiện ban đầu c o ij( )c ij0 và có thông tin

z

y

x  

 , , dễ dàng xác định các hàm c ij Song

trong thực tế các tốc độ góc x,y,zdo con

quay (phần tử đo) cung cấp thường có dạng sau:

0 1 0 2 0 3

b w

b w

b w

 

 

 

(7)

Trong đó o x,y o,z olà tốc độ quay thực của hệ

tọa độ B, b1, b2, b3 là các tham số biến đổi chậm

thể hiện độ trôi của con quay Còn w w w x, y, z là

các nhiễu đo thường có dạng ồn trắng Như vậy

nếu dùng thông tin của các con quay đưa vào

phương trình (6) để giải chúng sẽ nhận được các

hàm số c t ij( ) (i = 1, 2, 3, j= 1, 2, 3) không đúng

với thực tế Để khắc phục hiện tượng này bài báo

đề xuất một phương pháp dùng bộ lọc Kalman trực

tiếp với quan sát là các giá trị do từ kế, gia tốc kế

và các con quay vi cơ đo tốc độ góc cung cấp để

xác định các hàm c t ij( ) của ma trận C

3 Bộ lọc Kalman phi tuyến mở rộng và

thuật toán xác định ma trận cosin chỉ

phương

Bộ lọc Kalman là một công cụ toán học giúp

cho việc đánh giá vec tơ trạng thái hệ động học

trên cơ sở thông tin quan sát véc tơ tín hiệu đầu ra

Giả sử quá trình chuyển động của vật thể hoặc quá

trình công nghệ được mô tả bởi hệ phương trình

động học dưới dạng rời rạc như sau:

1( 1)

Z h X v



 

  (8)

1

, k

k X

X là trạng thái của véctơ trạng thái X

(véctơ n chiếu) ở bước thứ k và bước thứ (k-1),

k

Z là giá trị của véctơ đầu ra (véctơ m chiếu,

thường m  n) Véctơ đầu ra này có thể đo được

bằng các phương tiện đo Fk1 là véctơ hàm số F

ở bước thứ k-1:

F (f1, f2, f n)T (9)

1, 2, n

f f f - các hàm phi tuyến với biến số là véctơ X hk là véctơ hàm số h ở bước thứ k:

h(h1,h2, ,h m)T (10)

k

k 

 , - là véctơ nhiễu động lực và nhiễu đo có dạng ồn trắng với kỳ vọng toán học bằng 0:

~ (0, )

~ (0, )



Ma trận hiệp phương sai của véctơ sai số đánh giá trạng thái véctơ X như sau:

Pk ([ Xk Xk][ Xk Xk]T









 

Ở đây là ký hiệu kỳ vọng toán học

Thủ tục của thuật toán đánh giá trạng thái X trên

cơ sở véctơ quan sát (đo được) Z theo Kalman [3] như sau:

ˆ ( ) 1( ˆ 1( ))

X  F X  

1

ˆ

k

k F k X X X



   

ˆ ( ˆ ( ))

k k k

k

k k X X

 

   (11)

( ) ( ) T

P    P    Q

1

( ) T( ( ) T )

k k k k k k k

K P  H H P  H R 

ˆ ( ) ˆ ( ) ( ˆ )

X   X  K Z Z

P   I  K H P 

Ở đây I là véctơ đơn vị Đối với thiết bị dẫn đường quán tính thường gặp, việc xác định các phần tử cij của ma trận C được thực hiện trên cơ sở giải hệ phương trình vi phân (6) trên cơ sở thông tin đo được từ các con quay về tốc độ góc x,y,z Tuy nhiên, kết quả có sai

số lớn khi có yếu tố trôi b b b1, 2, 3 Trong bài báo này nhóm tác giả đề xuất phương pháp ước lượng các phần tử cij trên cơ sở dùng bộ lọc Kalman với thông tin đầu ra từ các từ kế và gia tốc kế gắn trên vật thể B Giả sử trên vật thể B gắn 3 từ kế và 3 gia tốc kế theo các trục OX, OY, OZ của hệ tọa độ liên kết (hình 3)

\

H 3 Sơ đồ bố trí con quay vi cơ đo tốc độ góc,

gia tốc kế và từ kế

Tx, Ty, Tz - các từ kế; Gx, Gy, Gz - các gia tốc kế;

, ,

x y z

   - các con quay vi cơ

Giả thiết trong vùng hoạt động của vật thể véctơ cường độ từ trường trái đất và véctơ gia tốc trọng trường có phương và giá trị không đổi Khi

hệ tọa độ liên kết trùng với hệ tọa độ chuẩn đo được các giá trị từ trường B B x0 y0B z0và gia tốc trọng trường g x0g y0 g z0 Khi vật thể B chuyển động thì hệ tọa độ B không trùng với hệ tọa độ chuẩn A và khi đó các từ kế và các gia tốc kế hiển nhiên sẽ cho các chỉ số như sau:

x y z x y z

B C B B B G C g g g (12) Đặt

1 11 2 12 3 13 4 21 5 22 6 23 7 31 8 32 9 33 10

11 12 13 1 14 2 15 3 1 2 15

x T



Từ hệ phương trình (6) có các phương trình động lực đối với 9 trạng thái của hệ 15 trạng thái như sau:

1 11 2 12 3 13 4 21 5 22 6 23 7 31 8 32 9 33 10

11 12 13 1 14 2 15 3 1 2 15

x T



Từ hệ (13) có dạng rời rạc đối với 9 biến số:

k k k k

x k f X x k T x k x









k k k k

x k f X









x k T x k x k x k x k

x k f X  x k T x k x k x k x k

(14)

ở đây T- là bước tính khi rời rạc hóa, trong

trường hợp tốc độ góc x,y,z biến đối chậm

có coi:

11 11 1 11

k k k







(15)

Độ trôi b1, b2, b3 cũng có thể viết dưới dạng:

13 13 1 13

15 15 1 15

k k k







(16)

Từ biểu thức (12) có thể nhận được hệ 6 phương trình quan sát của véc tơ quan sát

1 2 9

( )T

Z  z z z như sau:

1 1 0 11 0 21 0 31 0 1 0 4 0 7

2 2 0 12 0 22 0 32 0 2 0 5 0 8

3 3 0 13 0 23 0 33 0 3 0 6 0 9

4 4 0 11 0 21 0 31 0 1 0 4 0 7

5 5

(

k

z h X B c B c B c B x B x B x

z h X B c B c B c B x B x B x

z h X B c B c B c B x B x B x

z h X g c g c g c g x g x g x

z h X

6 6 0 13 0 23 0 33 0 3 0 6 0 9

)

g c g c g c g x g x g x

z h X g c g c g c g x g x g x

(17)

Từ 3 tín hiệu của 3 con quay cho 3 phương trình quan sát còn lại:

X

Z

Y

x



y



z



Gy

Gx

Gz

Ty Tx

Tz

7 7 0 1 10 13

( )

( )

( )

k x

k y

k z







(18)

Thực hiện các phép lấy đạo hàm riêng các hàm

1

i k

f X  i  theo các biến số xj nhận được

ma trận  như sau:

1

1 0 0 0 0 0 0 0 0 0 0

0 0 0 0 0 0

Tx



0 0 0 0 0 0 0 0 0 1 0 0 0 0 0

0 0 0 0 0 0 0 0 0 0 1 0 0 0 0

0 0 0 0 0 0 0 0 0 0 0 1 0 0 0

0 0 0 0 0 0 0 0 0 0 0 0 1 0 0

0 0 0 0 0 0 0 0 0 0 0 0 0 1 0

0 0 0 0 0 0 0 0 0 0 0 0 0 0 1

Thực hiện các phép lấy đạo hàm riêng các hàm

1

i k

h X  i  theo các biến số xj nhận được

ma trận H như sau:

0 0 0 0 0 0 0 0 0 0 0 0

0 0 0 0 0 0 0 0 0 0 0 0

0 0 0 0 0 0 0 0 0 0 0 0

0 0 0 0 0 0 0 0 0 0 0 0

0 0 0 0 0 0 0 0 0 0 0 0

0 0 0 0 0 0 0 0 0 0 0 0

0 0 0 0 0 0 0 0 0 1 0 0 1 0 0

0 0 0 0 0 0 0 0 0 0 1 0 0 1 0

0 0 0 0 0 0 0 0 0 0 0 1 0 0 1













(20)

Khi các nhiễu đo của các từ kế và các gia tốc kế

không tương quan lẫn nhau thì ma trận R sẽ là ma

trận đường chéo (các phần tử ngoài đường chéo

đều bằng 0) Các phần tử của đường chéo chính là

phương sai của nhiễu đo từng từ kế và từng gia tốc

kế và nhiễu đo của các con quay vi cơ đo vận tốc

góc Ma trận Q cùng là ma trận đường chéo với

các phần tử ở đường chéo được chọn hợp lý tùy thuộc vào phương tiện tính toán (sai số tính toán) Như vậy từ thông tin của 3 từ kế, 3 gia tốc kế và

3 con quay vi cơ đo tốc độ góc cho ta véc tơ hàm quan sát Z (hệ phương trình 17, 18) và từ hệ động học (14, 15, 16) cho phép sử thuật toán lọc Kalman (11) để nhận được 9 phần tử của ma trận cosin chỉ phương C và từ đó xác định 3 góc định hướng   , ,

4 Tiến hành mô phỏng kiểm nghiệm thuật toán

Để tiến hành mô phỏng kiểm nghiệm tính đúng đắn của thuật toán có thể dùng các công cụ mô phỏng sau Matlab hoặc Simulink Các bước mô phỏng được thực hiện gồm hai giai đoạn:

A.Giai đoạn xác định tham số định hướng thực

Trong giai đoạn này giải trực tiếp hệ phương trình (6) với hàm tốc độ góc giả định không có nhiễu và trôi để tạo véc tơ quan sát (mô phỏng từ kế và gia tốc kế) Giai đoạn này gồm các bước sau:

- Đặt các điều kiện ban đầu cho các phần tử cij

của hệ phương trình (6)

- Xây dựng chương trình thay đổi các tốc độ quay   x0, 0y, z0 trong hệ phương trình (7)

- Tiến hành giải hệ phương trình vi phân (6) với điều kiện ban đầu đã chọn và chương trình

0 0 0

, ,

x y z

   để xác định ma trận C

- Đặt các điều kiện ban đầu về từ trường và gia tốc trọng trường cho hệ tọa độ chuẩn, tức là đặt các giá trị B B x0 y0B z0 và g x0g y0g z0và xác định véctơ B và véctơ G

B Giai đoạn thực hiện thuật toán lọc Kalman

Việc thực hiện thuật toán locjc Kalman theo thứ tự

ở các biểu thức (11) để xác định các phần tử của

ma trận cosin chỉ phương C cần hai chú ý sau: + Trên cơ sở matrận cosin chỉ phương nhận được ở giai đoạn A xây dựng véc tơ quan sát Zk như sau:

1 2 9

( , , , )T

Z  z z z , trong đó:

( ,z z z, )T C B T wt, ( ,z z z, )T C G T wg

và

0

0

0







ở đây b1, b2, b3 được đặt tùy ý xong phải thỏa mãn điều kiện:

b1max(x),b2 max(y),b3 max(z)

wt và wg là véc tơ nhiễu ngẫu nhiên

và w w w x, y, zlà hàm ngẫu nhiên được lấy từ

chương trình tạo hàm ngẫu nhiên chuẩn của hệ

ngôn ngữ mô phỏng với kỳ vọng toán học bằng 0

+Véc tơ hàm quan sát đánh giá

ˆ ( ˆ ( ))

k k k

Z h X  được xác định theo biểu thức:

1 2 3

( ,z z z, )T C B T , ( ,z z z4 5, 6)T C G T với điều

kiện ma trận C được hình thành của kết quả ước

lượng Kalman ở bước trước Còn z z z7, 8, 9 được

xác định như sau:

z7  x10 x13, z8  x11 x14, z7  x12 x15

C.Tiến hành so sánh các hàm nhận được của

véctơ đánh giá X với các phần tử của ma trận C

nhận được ở giai đoan A như sau:

+x t1( ) so với c t11( ); x t2( ) so với c12( ) t ;

3( )

x t so với c13( ) t ; x t4( ) so với c21( ) t ; +x t5( ) so với c22( ) t ; x t6( ) so với c23( ) t ;

7( )

x t so với c31( ) t ; x t8( ) so với c32( ) t ; +x t9( ) so với c33( ) t ;

Nếu các hàm này trùng hoặc gần nhau có thể đánh gia thuật toán được lập có tính đúng đắn Trên các hình 4 là đồ thị quá trình mô phỏng kiểm nghiệm bằng ngôn ngữ mô phỏng Matlab với bước tính T=0.005 giây Qua mô phỏng cho kết quả khẳng định tính đúng đắn của thuật toán

11

c , c21- hệ số lý tưởng X1, X4-hệ số đánh giá

của bộ lọc Kalman

-0.5 0 0.5 1 1.5

Buoc tinh

-0.5 0 0.5 1 1.5

Buoc tinh

a Đồ thị hệ số C11 và X1

-0.5 0 0.5 1 1.5

Buoc tinh

-0.5 0 0.5 1 1.5

Buoc tinh

b Đồ thị hệ số C21 và X4

H 4 Đồ thị các hệ số của ma trận cosin chỉ phương

5 Kết luận

Việc ứng dụng bộ lọc Kalman phi tuyến mở

rộng cho phép thiết lập thuật toán xác định các

tham số định hướng trong điều kiện con quay vi cơ

đo tốc độ góc có độ trôi và nhiễu trên cơ sở thông

tin quan sát được của từ các từ kế, gia tốc kế và

các con quay vi cơ đặt trên thiết bị di động Tuy

nhiên, thuật toán này chỉ có giá trị với giả thiết

tổng véctơ các chỉ số của 3 gia tốc kế phải có giá

trị gần bằng gia tốc trọng trường nơi vật thể đang

có mặt và véctơ từ trường trái đất nơi vật thể đang

hoạt động không thay đổi Những giả thiết này

đúng và gần đúng cho các vật thể như tay máy,

người máy, máy bay không người lái, phương tiện

ngầm hoạt động cự ly ngắn Việc vật thể chuyển

động với vận tốc gần đều hoặc đứng yên thì lực

ngoài cân bằng với lực trọng trường, khi đó chỉ số

các gia tốc kế sẻ tuân thủ công thức thứ 2 của biểu

thức 12, còn khi vật thể chuyển động có gia tốc

công thức này không còn giá trị Việc vật thể hoạt

động trong cự ly ngắn thì các giá trị B x0,B y0,B z0

trong công thức thứ nhất của biểu thức 12 sẽ là

hằng số Đối với UAV (máy bay không người lái)

hoạt động cự ly xa cần phải có bản đồ từ trường và bản đồ gia tốc trọng trường đi kèm trong bộ nhớ của máy tính trên UAV Đối với tên lửa chuyển động có gia tốc, thuật toán sẽ khác và sẽ được trình bày ở công trình khác Điểm mới của bài báo

là dùng quan sát chỉ số từ kế, gia tốc kế và con quay hai bậc tự do với công cụ bộ lọc Kalman phi tuyến mở rộng để xác định các tham số định hướng với độ chính xác cao

Tài liệu tham khảo

1 Salychev O.S (1998) “Inertial Systems in Navigation and Geophysics”, Bauman MSTU Press,

Moscow

2 Распопов В Я Микросистемная авионика, учебное пособие

Тула "Гриф и к" 2010

3 Фильтрация и стохастическое управление в динамических системах Под редакциёй К Т Леондеса, перевод с английского Издательство

"Мир" Москва 1980