Xây dựng bộ đo góc có độ chính xác cao ứng dụng trong ổn định bệ đài quan sát quang điện tử

Đo lường & Tin học 
L. T. Thắng, , C. Đ. Chình, “Xây dựng bộ đo góc  đài quan sát quang điện tử.” 234 
XÂY DỰNG BỘ ĐO GÓC CÓ ĐỘ CHÍNH XÁC CAO ỨNG DỤNG 
TRONG ỔN ĐỊNH BỆ ĐÀI QUAN SÁT QUANG ĐIỆN TỬ 
Lê Trần Thắng, Lê Văn Phúc*, Nguyễn Trọng Khuyên, Chu Đức Chình 
Tóm tắt: Việc ổn định bệ đài quan sát phải đồng thời xử lý bài toán động học bệ 
và bài toán đo lường phức tạp. Bài báo đã nghiên cứu, phân tích giải các bài toán 
động học bệ đồng thời xử lý tín hiệu đo từ con quay sợi quang kết hợp với cảm biến 
đo gia tốc góc, cảm biến từ trường nhằm xây dựng nên bộ đo góc theo 3 trục roll, 
pitch, yaw có độ chính xác cao, khắc phục nhược điểm của phương pháp đo, đưa ra 
kết quả đo với chất lượng cao làm đầu vào phục vụ cho việc ổn định bệ vũ khí trên 
phương tiện cơ động. 
Từ khóa: IMU quang; Đài quan sát; Ổn định; Góc Ơle. 
1. MỞ ĐẦU 
 Trong chiến tranh công nghệ cao, các loại vũ khí thông minh được áp dụng 
nhằm phát hiện tiêu diệt các hệ thống hỏa lực của đối phương trong đó có các tổ 
hợp hỏa lực phòng không. Điều này đỏi hỏi hệ thống vũ khí của chúng ta phải cơ 
động, tránh bị tiêu diệt, mặt khác còn phải triển khai tấn công nhanh, sẵn sàng tác 
chiến mọi lúc mọi nơi theo yêu cầu chiến thuật. Các hệ thống vũ khí, hệ thống 
trinh sát, bám bắt mục tiêu có điều khiển đặt trên các bệ di động phải đáp ứng được 
tính năng chiến đấu cơ động. Để làm được điều này các hệ thống khí tài trên cần 
phải ổn định bệ khi cơ động. Ổn định bệ cho hệ thống vũ khí tức là việc đảm bảo 
cho hệ thống vũ khí luôn luôn được ổn định trên một măt phẳng công tác. 
Bài báo đã phân tích giải các bài toán động học bệ ổn định, đồng thời xử lý tín 
hiệu đo từ con quay sợi quang kết hợp với cảm biến đo gia tốc góc, cảm biến từ 
trường nhằm xây dựng nên bộ đo góc theo 3 trục roll, pitch, yaw có độ chính xác 
cao, khắc phục nhược điểm của phương pháp đo, đưa ra kết quả đo với chất lượng 
cao làm đầu vào phục vụ cho việc ổn định bệ vũ khí trên phương tiện cơ động. 
2. MÔ HÌNH HÓA BỆ ĐÀI QUAN SÁT 
Đài quan sát quang điện tử của các khí tài cơ động được đặt trên bệ ổn định trục 
đứng. Nhiệm vụ của bệ ổn định trục đứng là giữ cho mặt phẳng đế của bệ đài quan 
sát luôn nằm trên một mặt phẳng song song với mặt phẳng trái đất, loại trừ đi mọi 
loại rung sóc ảnh hưởng đến góc tầm của đài quan sát quang điện tử. 
Cấu trúc bệ ổn định trục đứng được sử dụng để ổn định mặt phẳng đế cho bệ đài 
quan sát quang điện tử như hình 1 dưới đây. 
 Mô hình hóa bệ 
 Trong sơ đồ mô hình hóa bệ, tách khớp các đăng dạng chữ thập thành 2 khớp 
quay có trục quay vuông góc với nhau. Tâm 2 trục quay là 
1 2
O O . 
Các hệ trục tọa độ được thiết lập như trên hình 1. Trong đó: 
0 0 0Ox y z : Hệ tọa độ cố định gắn với bệ tĩnh; 1 1 1 1 2 2 2 2O x y z , O x y z : Hệ tọa độ 
gắn với các trục của khớp các đăng. Khớp các đăng sử dụng là khớp các đăng dạng 
chữ thập nên hai gốc tọa độ O1, O2 trùng nhau. 
 5l : Khoảng cách từ tâm khớp các đăng đến tâm tấm đế động. 
Nghiên cứu khoa học công nghệ 
Tạp chí Nghiên cứu KH&CN quân sự, Số Đặc san TĐH, 04 - 2019 235
 1 2q , q : Là 2 góc quay tương ứng với 2 trục của khớp các đăng. 
 h : Khoảng cách từ tâm O đến tâm khớp các đăng. 
Pxyz : Hệ tọa độ gắn với bệ động. 
O
1A
2A
0x
0y
0z
1B
1C
2B
2C
1O
2O
P
y
x
z
1z
2x
2y
2z
1x
1y
2l
3l
4l
h
5l
2 
1 
1q
2q
1l
1l
2l
3l
4l
Hình 1. Sơ đồ mô hình hóa bệ. 
Bài toán động học 
Bài toán động học giải quyết các vấn đề về động học, các tham số của bài toán 
là dữ liệu đầu vào cho bài toán điều khiển. 
Xét chuỗi động học: 
1 2
OO O P . Áp dụng phương pháp Denavit Hartenberg ta 
xây dựng bảng tham số D-H [2] [4]. 
Bảng 1. Bảng các tham số D-H. 
  d a 
1 / 2 H 0 / 2 
2 1q 0 0 / 2 
3 2q 5
2
l
2
 5
2
l
2
 / 2 
Các ma trận chuyển xác định theo phương pháp D-H. 
Ma trận D-H tổng quát: 
cos -sin .cos α sin .sin α cos .a
sin cos .cos α -cos .sin α sin .a
H = (1)
i 0 sin α cos α d
0 0 0 1
θ θ θ θ
θ θ θ θ
Đo lường & Tin học 
L. T. Thắng, , C. Đ. Chình, “Xây dựng bộ đo góc  đài quan sát quang điện tử.” 236 
Các ma trận chuyển tương ứng với bảng tham số D-H được tính như sau: 
1
0 0 1 0
1 0 0 0
H =
0 1 0
0 0 0 1
h
; 
1 1
1 1
2
cos q t 0 -sin q t 0
sin q t 0 cos q t 0
H =
0 -1 0 0
0 0 0 1
2 2 2 5
2 2 2 5
3
5
1
cos q t 0 -sin q t cos q t l 2
2
1
sin q t 0 cos q t sin q t l 2
H = (2)2
1
0 -1 0 - l 2
2
0 0 0 1
1 1
2 1 2
1 1
0 -1 0 0
cos q t 0 -sin q t 0
D =H .H = (3)
sin q t 0 cos q t h
0 0 0 1
3 1 2 3
2 2 2 5
1 2 1 1 2 1 2 5 1 5
1 2 1 1 2 1 2 5 1 5
D = H .H .H
1
-sin q t 0 -cos q t - sin q t l 2
2
1 1
cos q t cos q t sin q t -cos q t sin q t cos q t cos q t l 2+ sin q t l 2
2 2
1 1
sin q t cos q t -cos q t -sin q t sin q t sin q t cos q t l 2- cos q t l 2+h
2 2
0 0 0 1
(4) 
Ma trận cosin chỉ hướng 
2 2
1 2 1 1 2
1 2 1 1 2
-sin q t 0 -cos q t
R= cos q t cos q t sin q t -cos q t sin q t (5)
sin q t cos q t -cos q t -sin q t sin q t
Tọa độ tâm tấm đế động 
2 5
P 1 2 5 1 5
1 2 5 1 5
1
- sin q t l 2
2
1 1
r = cos q t cos q t l 2+ sin q t l 2 (6)
2 2
1 1
sin q t cos q t l 2- cos q t l 2+h
2 2
Nghiên cứu khoa học công nghệ 
Tạp chí Nghiên cứu KH&CN quân sự, Số Đặc san TĐH, 04 - 2019 237
Tọa độ các điểm 1 2C , C trong hệ tọa độ Pxyz: 
(7) 
Tọa độ các điểm 1 2C , C trong hệ tọa độ 0 0 0Ox y z : 
10 1
2 4
C P C 1 2 4
1 2 4
-sin q t l
r = r + R.r = cos q t cos q t l (8)
h+sin q t cos q t l
20C P C2 4 1
4 1
0
r = r + R.r = l sin q t (9)
h-l cos q t
10
1
A
l
r = 0
0
 ; 20A 1
0
r = l (10) 
0
10
1 2 1
B
2 1
l +l cos φ t
r = 0
l sin φ t
; 
20B 1 2 2
2 2
0
r = l +l cos φ t (11)
l sin φ t
2
1 2 2 1 4
2
2
2 2 1 4 3
2 2 2 2
1 2 1 2 4 1 2 4
2
2
2 1 1 2 4 3
l +l cos φ t -sin q t l
 + l sin φ t -h+cos q t l = l 
 (12)
l +l cos φ t +sin q t l +cos q t cos q t l
 + l sin φ t -h-sin q t cos q t l = l 
Hệ phương trình (12) có các tham biến là các góc quay của đế bệ đài quan sát, 
việc giải hệ này sẽ xác định được các góc quay tương ứng của bệ, giúp điều khiển 
bệ ổn định. 
3. GIẢI CÁC BÀI TOÁN ĐỘNG HỌC VÀ XỬ LÝ TÍN HIỆU ĐO 
Từ hệ (12) nhận thấy đây là hệ 2 phương trình 2 ẩn, việc giải các bài toán động 
học dựa vào hệ phương trình này được tiến hành như sau: 
Đo lường & Tin học 
L. T. Thắng, , C. Đ. Chình, “Xây dựng bộ đo góc  đài quan sát quang điện tử.” 238 
- Trong bài toán động học thuận ta biết trước các góc 1 2( ), ( )t t . Từ hệ 
phương trình trên ta xác định được các góc 1 2( ), ( )q t q t . Từ đó xác định được vị trí 
và hướng của đế động. 
- Trong bài toán động học ngược, ta biết trước vị trí và hướng của đế động, 
đồng nghĩa với biết trước các góc 1 2( ), ( )q t q t của khớp các đăng. Giải hệ phương 
trình trên ta xác định được các góc 1 2( ), ( )t t 
Bài toán động học thuận: 
Ở đây ta biết trước các góc 1 2( ), ( )t t và cần xác định hướng và tọa độ của tấm 
đế động. Giải hệ phương trình (12) với ẩn là 1 2( ), ( )q t q t . 
 Phương trình đầu trong hệ phương trình (12) sẽ có dạng 
 1 1sin cosA q t B q t C , từ đó suy ra ta có [2]: 
Tính được 1( )q t biết 1( )t thay vào phương trình thứ hai của hệ (12) và giải 
phương trình ta được 2( )q t . Dựa vào phương trình (5), (6) ta xác định được tọa 
độ trọng tâm và hướng của tấm đế di động. 
Bài toán động học ngược: 
Trong bài toán động học ngược, ta biết được vị trí và hướng của tấm đế di động, 
đồng nghĩa với việc biết các góc quay 1 2( ), ( )q t q t của khớp các đăng. Yêu cầu xác 
định các góc 1 2( ), ( )t t . Tương tự như trên phương trình đầu của hệ (12) có dạng: 
1 2 1 2 1
A sin( ( )) cos( ( ))t B t C 
Giải phương trình ta xác định được 2 (t) . 
Tương tự, từ phương trình thứ hai của hệ (12) ta xác định được 1 (t) . 
Bài toán vận tốc, gia tốc: 
Từ bài toán động học thuận, ta xác định được các góc quay 1 2( ), ( )q t q t theo 
1 2( ), ( )t t . Đạo hàm 2 phương trình của 2 góc quay ta xác định phương trình thể 
hiện mối liên hệ giữa vận tốc góc theo 2 trục của bệ động: 
1 1
2 2
( )
( )
q t
q t




Nghiên cứu khoa học công nghệ 
Tạp chí Nghiên cứu KH&CN quân sự, Số Đặc san TĐH, 04 - 2019 239
Đạo hàm hai phương trình trên theo thời gian t ta được phương trình gia tốc của 
2 góc nghiêng của bệ động: 
1 1 1
2 2 2
q
q
 
 
 
 
Từ bài toán động học ngược, ta xác định được các góc quay 1 2( ), ( )t t theo 
1 2( ), ( )q t q t . Đạo hàm 2 phương trình của 2 góc quay ta xác định phương trình thể 
hiện mối liên hệ giữa vận tốc góc của 2 khâu chủ động và vận tốc góc theo 2 trục 
của khớp các đăng: 
'
1 1
'
2 2
( )
( )
t
t
 
 
Đạo hàm 2 phương trình trên ta xác định được gia tốc của 2 khâu chủ động là: 
'
1 1
'
2 2
( )
( )
t
t
 
 


 Như vậy chúng ta đã xác định được các tọa độ tâm đế động, các góc quay cũng 
như vận tốc và gia tốc biến đổi các khớp. Giả sử thiết bị đo lường được gắn tại tâm 
đài quan sát đặt trên đế động, sau đây chúng ta tín hành xử lý tín hiệu đo khi đế 
dao động, cùng với các dữ liệu vừa nhận được làm đầu vào cho điều khiển ổn định. 
Muốn ổn định mặt phẳng bệ của hệ thống vũ khí thường sử dụng các bộ đo góc 
ba trục Roll, Pitch, Yaw. Khối đo lường có nhiệm vụ đo ba góc này cần được tích 
hợp từ bộ các cảm biến sau: Cảm biến vận tốc góc, gia tốc góc, cảm biến từ 
trường. Bên cạnh đó để tính toán xử lý dữ liệu đo thường dùng thuật toán lọc 
Kalman. Sơ đồ cấu trúc khối đo lường sử dụng Gyro quang và cảm biến gia tốc kế. 
Khối
hiệu chuẩn
Vận tốc góc
3 trục
Gia tốc góc 
3 trục
Cảm biến từ trường
3 trục
Bộ lọc Kalman
Tính toán 
Pitch Roll
Bù nghiêng
Roll
Pitch
Roll*
Pitch*
Yaw
Hình 2. Sơ đồ khối đo lường sử dụng Gyro quang, 
gia tốc kế và cảm biến từ trường. 
Đo lường & Tin học 
L. T. Thắng, , C. Đ. Chình, “Xây dựng bộ đo góc  đài quan sát quang điện tử.” 240 
Các phương pháp đo góc nghiêng đều có những loại sai số khác nhau và bộ lọc 
Kalman dùng cho các ứng dụng đo góc nghiêng có sử dụng cảm biến quán tính. Bộ 
lọc Kalman cho phép kết hợp cả hai phương pháp tính góc nghiêng trong thuật 
toán lọc để khắc phục nhược điểm của từng phương pháp riêng biệt. Phương pháp 
tích phân vận tốc góc theo thời gian được sử dụng như mô hình toán học của đối 
tượng, còn phương pháp tính góc nghiêng theo gia tốc được sử dụng như giá trị đo 
trong mô hình bộ lọc. Sai lệch tĩnh của vận tốc góc cũng có những thay đổi nhỏ 
theo thời gian và khó hiệu chỉnh về bằng không, nên trong mô hình của đối tượng 
của bộ lọc thành phần này được đưa vào như một biến trạng thái của phương trình 
ước lượng và nó sẽ được hiệu chỉnh thường xuyên. 
 Công thức tính góc nghiêng trong mô hình rời rạc bằng phương pháp tích phân 
vận tốc góc theo thời gian như sau: 
( 1) ( ) ( ) ( ) ( )Y Y Y Yg Yk k k t k t b k t   (13) 
( 1) ( ) ( )Y Y Ybb k b k k 
Trong đó : 
( 1)Y k là góc nghiêng ở trạng thái k+1; 
( )Y k là góc nghiêng ở trạng thái k; 
( )Y k vận tốc góc ở trạng thái k; 
t thời gian lấy mẫu; 
( )Yb k độ sai lệch; 
( ),Yg k ( )Yb k là nhiễu của vận tốc góc tương ứng lần lượt với trục Pitch, 
trục Roll và độ sai lệch vận tốc góc, chúng ta xem các nhiễu này là nhiễu trắng. 
 Theo phương pháp hình học, góc nghiêng của cảm biến theo các phương 
X(roll), Y(pitch) có thể tính từ các gia tốc thành phần theo các công thức sau: 
( )
( ) tan( ),
( )
acc
X
acc
Y k
k acr
Z k
( )
( ) tan( ),
( )
acc
Y
acc
X k
k acr
Z k
( )
( ) tan( ).
( )
acc
Z
acc
X k
k acr
Y k
(14) 
 Áp dụng bộ lọc Kalman cho khối đo nghiêng ta có phương trình trạng thái cho 
các góc roll và pitch như sau: 
( 1) ( ) ( ) ( )
( ) ( ) ( )
(15)
( 1) ( ) ( ) ( )
( ) ( ) ( )
Y Y Y Y
vY Y Y
X X X X
vX X X
x k Ax k Bu k n
y k Cx k v n
x k Ax k Bu k n
y k Cx k v n


 Góc xoay Yaw là góc phương vị nếu chỉ sử dụng cảm biến vận tốc của gyro để 
tính toán sẽ gặp hiện tượng trôi và vấn đề khởi tạo giá trị ban đầu. Ngoài ra từ 
Nghiên cứu khoa học công nghệ 
Tạp chí Nghiên cứu KH&CN quân sự, Số Đặc san TĐH, 04 - 2019 241
trường trái đất song song với mặt đất, do vậy sử dụng cảm biến từ trường có thể 
xác định được chính xác góc phương vị. Tuy nhiên trong ứng dụng của chúng ta, 
cảm biến từ trường được gắn lên phương tiện cơ động do vậy đối tượng di chuyển, 
nghiêng sẽ làm cảm biến từ trường cũng di chuyển ra khỏi mặt phẳng nằm ngang 
do vậy dẫn đến sai số trong phép đo phương vị. 
 Để khắc phục điều này chúng tôi kết hợp giữa cảm biến từ trường với góc 
nghiêng theo trục Roll, Pitch tính toán ra góc phương vị Yaw. 
Tín hiệu ra của cảm biến từ trường mx, my, mz được tính toán và chuẩn hóa. Gọi các 
góc α – góc Roll, β – góc Pitch là góc nghiêng được tính toán theo công thức (15). 
Góc phương vị theo trục Yaw sẽ được tính theo công thức sau: 
cos( ) sin( )sin( ) sin( ) cos( )
cos( ) sin( ) (16)
ar 2( )
x y z
y z
XH m m m
YH m m
YH
c
XH
   

4. THỬ NGHIỆM TRÊN HỆ THỐNG 
 Các kết quả nghiên cứu tính toán đã được đưa vào áp dụng thử nghiệm trên mô 
hình thực tại phòng thí nghiệm tại đơn vị. 
 Tín hiệu đo ổn định và dao động trong khoảng 0,05 độ (trong ngưỡng sai số cho 
phép) được thể hiện trên hình 3. 
Hình 3. Kết quả thử nghiệm bộ đo góc nghiêng. 
 Từ kết quả trên hình 3 chúng ta thấy rằng việc sử dụng thuật toán lọc Kalman 
kết hợp giữa cảm biến quang học đo vận tốc góc kết hợp với cảm biến gia tốc góc 
cho ra kết quả đo góc chính xác hơn so với việc chỉ sử dụng cảm biến gia tốc để 
Đo lường & Tin học 
L. T. Thắng, , C. Đ. Chình, “Xây dựng bộ đo góc  đài quan sát quang điện tử.” 242 
tính toán ra góc nghiêng. Trên hình 4 ta thấy khi góc đặt đầu vào thay đổi từ -20 độ 
tang dần đến +20 độ và dao động theo dạng hàm sin góc bệ vẫn bám sát với sai số 
trong ngưỡng cho phpes, như vậy bệ đã được ổn định. 
 Đồ thị ổn định bệ khi góc đặt đầu vào thay đổi trong khoảng từ - 20 độ đến + 
20 độ thể hiện trên hình 4. 
Hình 4. Đồ thị ổn định bệ. 
5. KẾT LUẬN 
Qua kết quả tính toán và thử nghiệm nhận thấy hệ thống ổn định bệ sử dụng 
thiết bị IMU quang và đo nghiêng kết hợp đã đáp ứng được yêu cầu đề ra cho ổn 
định bệ, tiến tới ổn định đường ngắm cho các hệ thống tự động điều khiển hỏa lực. 
TÀI LIỆU THAM KHẢO 
[1]. Thuyết minh kỹ thuật đề tài “Nghiên cứu thiết kế, chế tạo hệ thống trinh sát, 
phát hiện, bám mục tiêu, điều khiển hỏa lực trong tổ hợp phóng tên lửa phòng 
không tầm thấp A72 tác chiến ngày và đêm”, Viện TĐH KTQS 2015. 
[2]. Nguyễn Hữu Thắng. Báo cáo tổng hợp đề tài “Nghiên cứu thiết kế chế tạo bệ 
ổn định trục đứng ứng dụng trên phương tiện cơ động”, Viện KH&CN Quân 
sự 2016 
[3]. Vũ Huy Mừng, Lê Bá Yến, Phạm Việt Dũng, “Thiết kế bộ lọc Kalman trên vi 
điều khiển DSPic30F ứng dụng trong đo lường quán tính”, Tạp chí Nghiên 
cứu khoa học và công nghệ quân sự, 2010. 
[4]. Denavit, J. and Hartenberg, R.S. “A Kinematic Notation for Lower-Pair 
Mechanisms Based on Matrices”, ASME Journal of Applied Mechanics, 77, 
215-221, 1955. 
[5]. Matthew Leccadito (2013), “A Kalman Filter Based Attitude Heading 
ReferenceSystem Using a Low Cost Inertial Measurement Unit”, Virginia 
Commonwealth University 8/2013. 
Nghiên cứu khoa học công nghệ 
Tạp chí Nghiên cứu KH&CN quân sự, Số Đặc san TĐH, 04 - 2019 243
[6]. Fatemeh Abyarjoo, Armando Barreto, Jonathan Cofino, Francisco R. Ortega. 
“Implementing a Sensor Fusion Algorithm for 3D Orientation Detection with 
Inertial/Magnetic Sensors” Florida International University 11/2014. 
[7]. Gina C. Angelosanto, “Kalman Filtering of IMU Sensor for Robot Balance 
Control”, Massachusetts Institute of Technology, 2008. 
ABSTRACT 
BUILD IN HIGH-PRECISION ANGLE MEASURING KIT, APPLICATION 
IN STABILZING ELECTRONIC OPTICAL OBSERVATORY PEDESTALS 
The stability of the observatory pedestal must simultaneously handle 
pedestal kinetic problems and complex measurement problems. The paper has 
studied and analyzed solutions of pedestal dynamics and processed signals from 
optical fiber gyroscopes combined with angular acceleration sensors, magnetic 
field sensors to build a 3-angle angle measuring set. Roll, pitch, yaw axes have 
high accuracy, overcome the disadvantages of measuring methods, give high 
quality measurement results as input for stabilizing weapons base on mobile 
vehicles. 
Keywords: IMU Optical; Observatory; Stable pedestal. 
Nhận bài ngày 21 tháng 01 năm 2019 
Hoàn thiện ngày 06 tháng 3 năm 2019 
Chấp nhận đăng ngày 15 tháng 3 năm 2019 
Địa chỉ: Viện Tự động hóa KTQS / Viện KH-CNQS. 
 * Email: levanphuccapiti@gmail.com.