Xây dựng thuật toán xác định góc lắc cho phương tiện chuyển động trên cơ sở kết hợp con quay tốc độ góc với gia tốc kế

Nghiên cứu khoa học công nghệ 
Tạp chí Nghiên cứu KH&CN quân sự, Số 49, 06 - 2017 3
XÂY DỰNG THUẬT TOÁN XÁC ĐỊNH GÓC LẮC CHO 
PHƯƠNG TIỆN CHUYỂN ĐỘNG TRÊN CƠ SỞ KẾT HỢP 
CON QUAY TỐC ĐỘ GÓC VỚI GIA TỐC KẾ 
Phạm Văn Phúc1, Trần Đức Thuận2*, Nguyễn Sỹ Long2, 
Nguyễn Trọng Yến2, Đặng Văn Thành2 
Tóm tắt: Bài báo trình bày một phương pháp sử dụng thông tin của các gia tốc 
kế, con quay vi cơ để xác định góc quay xung quanh trục của các phương tiện 
chuyển động trên cơ sở ứng dụng bộ lọc Kalman để kết hợp thông tin của hai phần 
tử đo quán tính nhằm khắc phục được yếu tố trôi của con quay vi cơ và nhiễu đo 
của các gia tốc kế. 
Từ khóa: Con quay vi cơ, Góc lắc, Bộ lọc Kalman. 
1. ĐẶT VẤN ĐỀ 
Việc xác định góc nghiêng của các vật thể hoạt động nói chung đã được nhiều tác giả 
nghiên cứu. Hiện nay, nhiều tác giả [1,3,4,5] đã nghiên cứu ứng dụng các cơ cấu đo vi cơ 
quán tính kết hợp với việc đo véc tơ từ trường trái đất hoặc các thiết bị đo vận tốc (như 
thiết bị GPS) để xác định góc nghiêng vật thể. Việc phải kết hợp các thiết bị đo là để khắc 
phục yếu tố độ trôi tham số đo của phần tử vi cơ quán tính đo vận tốc góc. Tuy nhiên, đối 
với vật thể hoạt động trong môi trường nước có một số đặc điểm sau: Không có thông tin 
của GPS và chuyển động chậm nên khó có khả năng dùng thông tin về vận tốc để hiệu 
chỉnh; Không đo được từ trường trái đất vật thể chuyển động vì có quá nhiều vật thể nhiễm 
từ và sinh từ. Trong bài báo này, nhóm tác giả đề xuất một giải pháp sử dụng bộ lọc 
Kalman [2] kết hợp thông tin của phần tử vi cơ quán tính đo vận tốc góc với gia tốc kế đo 
véc tơ gia tốc trọng trường trái đất để xác định góc nghiêng của vật thể chuyển động. 
2. BỘ LỌC KALMAN VÀ THUẬT TOÁN XÁC ĐỊNH GÓC NGHIÊNG VẬT THỂ 
2.1. Bộ lọc Kalman 
Bộ lọc Kalman là một công cụ toán học giúp cho việc đánh giá véc 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 [2]: 
 1 1 1 1 2( ) ( ) ( ), ( , ,..., )
T
k k k k nX F X G X k F f f f (1) 
 1( ) ( ); ( ,..., )
T
k k mZ h X k h h h (2) 
Trong đó: kX , 1kX là trạng thái của véc tơ trạng thái X (véc tơ n chiều) ở bước thứ k 
và bước thứ (k-1); G là ma trận nhiễu tích thước nxl ; ( )k là véc tơ nhiễu động học l 
chiều có dạng tạp trắng với kỳ vọng toán học bằng 0; kZ là giá trị của véc tơ đầu ra (véc 
tơ m chiều, thường thì m n ). Véc tơ đầu ra này có thể đo được bằng các phương tiện 
đo; 1kF là véc tơ hàm số F ở bước thứ (k-1); 1, 2 ,... nf f f là các hàm phi tuyến với biến 
số là véc tơ X ; kZ là véc tơ hàm số h ở bước thứ k, 1 2( , ,..., )
T
mh h h h , v k là các véc 
tơ nhiễu đo m chiều, có dạng tạp trắng với kỳ vọng toán học bằng 0. Ma trận hiệp phương 
sai của véc tơ sai số đánh giá trạng thái véc tơ X như sau: 
Tên lửa & Thiết bị bay 
P. V. Phúc, T. Đ. Thuận, , “Xây dựng thuật toán xác định góc lắc với gia tốc kế.” 4 
k
T
k k k kP X X X X 
, trong đó:  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ơ véc tơ quan sát (đo được) Z theo 
Kalman [2] như sau: 
 ( ) ( )1 1( )k k kX F X
 (3) 
1
1
1 ;
k
k
k X X
F
X

 

 (4) 
k
k
k X X
h
H
X


 ; (5) 
1 1 1 1 1 1 ;
T T
k k k k k k kP P G Q G
   (6) 
 ;k k k kP I K H P
 (7) 
 ;k k k k kX X K Z Z 
 (8) 
 ;k k kZ h X 
 (9) 
1
T T
k k k k k k kK P H H P H R
 (10) 
Trong đó, I là ma trận đơn vị. Bản chất của bộ lọc Kalman là xác định đánh giá trạng 
thái X
 sao cho hiệp phương sai đạt giá trị nhỏ nhất có thể. Điều kiện đánh giá được khi 
và chỉ khi hạng của ma trận 
1
...
nT T T TH H H
  bằng bậc của hệ động học, tức là: 
1
...
nT T T Trank H H H n
  (11) 
2.2. Thuật toán kết hợp phương tiện đo xác định góc nghiêng vật thể hoạt động ở môi 
trường nước 
Giả sử có vật thể hoạt động trong môi trường nước được thể hiện trên hình 1. Cần xác 
định góc nghiêng của sàn vật thể so với phương nằm ngang. Trên vật thể có gắn thiết bị 
đo vi cơ quán tính TB. Thiết bị TB (hình 2) bao gồm một con quay vi cơ C đo vận tốc góc 
lắc vật thể  , hai gia tốc kế đo các thành phần gia tốc theo các phương ox và oy. Gia tốc 
kế A1 đo gia tốc theo phương ox, còn gia tốc kế A2 đo gia tốc theo phương oy. Dễ dàng 
nhận thấy góc nghiêng có liên hệ với vận tốc quay  theo biểu thức sau: 
   (12) 
Nhìn vào biểu thức (12) ta thấy chỉ cần tích phân tín hiệu từ con quay C là xác định 
được góc nghiêng . Tuy nhiên con quay vi cơ C cho ta tín hiệu có dạng sau: 
 b    (13) 
Trong đó:  - Chỉ số do con quay đưa ra;  - Tốc độ góc quay thực của vật thể xung 
quanh trục oz (vuông góc với mặt phẳng oxy); b - Tham số độ trôi của con quay (biến đổi 
chậm, có thể coi là hằng số chưa rõ ở từng giai đoạn cụ thể);  - Nhiễu của con quay vi cơ 
có dạng ồn trắng. 
Từ biểu thức (13) cho thấy không thể dùng trực tiếp tín hiệu từ con quay vi cơ để xác 
định góc nghiêng . Do phương đo của gia tốc kế A1, A2 vuông góc với bán kính từ tâm 
quay nên gia tốc pháp tuyến không ảnh hưởng đến chỉ số đo của nó; Gia tốc tiếp tuyến 
và gia tốc dài của chuyển động tâm khối cũng rất ít ảnh hưởng đến các chỉ số của gia tốc 
kế A1, A2 [2]. Vì vậy, có thể thấy các chỉ số z1,z2 của gia tốc kế A1, A2 sẽ là: 
Nghiên cứu khoa học công nghệ 
Tạp chí Nghiên cứu KH&CN quân sự, Số 49, 06 - 2017 5
 1 1sinz g v (14) 
 2 2cosz g v (15) 
Trong đó: g – Giá trị gia tốc trọng trường nơi vật thể đang hoạt động, 1 2,v v - Nhiễu đo 
của các gia tốc kế có dạng ồn trắng. 
Hình 1. Vật thể trong môi trường nước và việc xác định góc nghiêng. 
Hình 2. Bố trí các phần tử vi cơ trong thiết bị TB. 
Từ (14) và (15) ta có thể xác định trực tiếp góc nghiêng . Tuy nhiên, để làm được 
điều này cần phải biết chính xác giá trị gia tốc trọng trường nơi vật thể đang hoạt động. 
Điều này không phải lúc nào cũng thực hiện được. Từ (14) và (15) cho ta biểu thức gần 
đúng sau: 
 11 1
2 2
sin sin
cos cos cos
g vz v
z tg v
z g v g
 (16) 
Sự gần đúng của biểu thức (16) thỏa mãn khi 2 cos ,v g tức là góc không gần 
góc 090 . Từ (12) và (13) ta có: 
 b     (17) 
Giải phương trình vi phương (17) theo phương pháp Ơle với bước tính t nhận được: 
 1 / ,k k t  suy ra: 
A1 
A2 
C 
g 
x y 
TB 
Phương nằm ngang 
Tên lửa & Thiết bị bay 
P. V. Phúc, T. Đ. Thuận, , “Xây dựng thuật toán xác định góc lắc với gia tốc kế.” 6 
 1k k k b t    (18) 
Tham số độ trôi của con quay b biến đổi chậm nên có thể chấp nhận đẳng thức sau: 
1k kb b (19) 
 Đặt 1 2, .x x b Từ (18) và (19) ta có: 
 1 1 1 2 1 1 2, 1 1x k f x x g x k x k t t  (20) 
 2 2 1 2 2( ) , 1x k f x x x k (21) 
Ở đây, 1 2,x k x k là các giá trị của 1 2,x x ở bước tính thứ .k Hai phương trình 
(20), (21) tạo nên hệ thống phương trình (1) của thuật toán lọc Kalman. Dễ dàng nhận thấy 
véc tơ hàm 1 1k kF X trong công thức (3) có dạng: 
1 1 2 1 2
1 1
2 1 2 2
, 1 1
, 1k k
f x x x k x k t
F X
f x x x k 
 (22) 
Từ (16) ta có: 1 2 1,z h x x v tg x v (23) 
Trong đó: 1 2 1,h x x tg x (24) 
Phương trình (23) tạo nên hệ thống phương trình (2) của thuật toán lọc Kalman. Tiếp 
theo cần xác định các ma trận ,k kH ở các công thức (4) và (5): 
1 1 1 2
2 1 2 2
/ / 1
/ / 0 1
k
f x f x t
f x f x
    
     
 (25) 
   21 2 1/ / 1/ cos 0kH h x h x x     (26) 
 Như vậy, hàm quan sát ở bài toán lọc Kalman nay là phi tuyến (biểu thức (24)). Từ 
các phương trình (1) và (20), (21) dễ dàng nhận thấy ma trận G trong phương trình động 
học (1) có dạng: 
0
t
G
 (27) 
Theo [2] các ma trận cường độ nhiễu ,Q R là phương sai của nhiễu , ,v tức là: 
 2 ,Q  
2
vR  (28) 
Như vậy, đã đủ yếu tố để thực hiện thuật toán lọc Kalman. Trên hình 3 là sơ đồ thuật 
toán lọc Kalman để xác định góc nghiêng (giá trị 1( )x k ). 
Kết quả mô phỏng Matlab-simulink và thực nghiệm cho thấy: thuật toán cho kết quả 
đạt độ chính xác trong các điều kiện sau: 
- Thiết bị hoạt động trong giới hạn 0 060 60 . Điều này đảm bảo cho biểu thức 
(16) được thỏa mãn. 
- Việc đặt hai gia tốc kế A1, A2 trong thiết bị TB đảm bảo tính vuông góc với các trục 
oy và ox. Nếu điều này không đạt được thì trong chỉ số của các gia tốc kế ngoài thành 
phần như các biểu thức (14), (15) còn có thành phần phản ánh gia tốc pháp tuyến của 
chuyển động quay, tỉ lệ với bình phương tốc độ quay 2 . 
Nghiên cứu khoa học công nghệ 
Tạp chí Nghiên cứu KH&CN quân sự, Số 49, 06 - 2017 7
Hình 3. Sơ đồ thuật toán lọc Kalman. 
Kết thúc 
Bắt đầu 
0, ,Q R X 
 00
1
X X
k
 1G X k 
 Xˆ k (3) 
 1k (4) 
 Zˆ k (9) 
 H k (5) 
 P k (6) 
 K k (10) 
 Z k 
 P k (7) 
1k k 
KTPK 
 Xˆ k (8) 
Từ thiết bị đo 
Có Không 
Tên lửa & Thiết bị bay 
P. V. Phúc, T. Đ. Thuận, , “Xây dựng thuật toán xác định góc lắc với gia tốc kế.” 8 
3. KẾT LUẬN 
Bằng giải pháp ứng dụng bộ lọc Kalman phi tuyến đã kết hợp các thông tin của con 
quay vi cơ đo tốc độ góc có độ trôi với các gia tốc kế đo tương quan của hai thành phần 
véc tơ gia tốc trọng trường (mà không cần biết chính xác giá trị gia tốc trọng trường) cho 
phép xác định góc nghiêng của vật thể chuyển động. Thuật toán này có giá trị cho việc xác 
định góc cren của các chủng loại tên lửa quay quanh trục dọc hoặc các vật thể hoạt động 
trong môi trường nước, mà ở đó không có thông tin của các thiết bị ngoài khác (như định 
vị vệ tinh GPS). 
TÀI LIỆU THAM KHẢO 
[1]. Распопов В.Я., “Микросистемная авионика”, учебное пособие. Тула " Грифик", 
2010. 
[2]. “Фильтрация истохастическое управление в динамических системах”. Под 
редакциёй К. Т. Леондеса. Издательство "Мир", Москва, 1980. 
[3]. Trần Đức Thuận, Bùi Ngọc Mỹ. “Thiết bị dẫn đường quán tính và đo cao trong hệ 
thống điều khiển thiết bị bay”. NXB Khoa học và Kỹ thuật, Hà Nội, 2015. 
[4]. Dương Bạch Phi, Ngô Khánh Hiếu, Nguyễn Vĩnh Hào. “Phát triển hệ thống dẫn 
đường tích hợp GPS/INS cho máy bay mô hình”. Tr.162-167, Kỷ yếu Hội nghị toàn 
quốc lần thứ 2 về điều khiển và tự động hóa VCCA-2013. Đà Nẵng, 2013. 
[5]. Songlai Han, Jinling Wang. “Integrated GPS/INS navigation system with daul-rate 
Kalman Filter”. Springer. Oct.2011. 
ABSTRACT 
AN ALGORITHM FOR DETERMINATION THE ANGLE FOR DEVICES 
OPERATING IN WATER BASED ON THE COMBINATION OF ANGLE SPEED 
GYROS AND ACCELEROMETERS 
In this paper, a method of using the information of the accelerometer, micro 
mechanical gyros to determine the angle of rotation around the axis of the means of 
operation in water environment on the basis of extended Kalman filter application 
for nonlinear systems in order to overcome the drift of micro mechanical gyros and 
noise measurement of accelerometers is described. 
Keywords: Micromechanical gyros, Angle, Kalman. 
Nhận bài ngày 09 tháng 9 năm 2016 
Hoàn thiện ngày 04 tháng 11 năm 2016 
Chấp nhận đăng ngày 20 tháng 6 năm 2017 
Địa chỉ: 1Trường Trung cấp Kỹ thuật Hải quân. 
 2 Viện KH-CNQS; 
*Email: thuandauto@yahoo.com