27 thang2 2365

Đo lường & Tin học

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 Khun, 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 tố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í ln ln đượ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 tố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 vng góc với nhau. Tâm 2 trục quay là O1  O2 .
Các hệ trục tọa độ được thiết lập như trên hình 1. Trong đó:
Ox 0 y 0 z 0 : Hệ tọa độ cố định gắn với bệ tĩnh; O 1 x 1 y 1 z 1 , O 2 x 2 y 2 z 2 : 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.
l 5 : Khoảng cách từ tâm khớp các đăng đến tâm tấm đế động.

234

L. T. Thắng, …, C. Đ. Chình, “Xây dựng bộ đo góc … đài quan sát quang điện tử.”


Nghiên cứu khoa học công nghệ

q 1 , q 2 : 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.
z0
x2

y2

q2

z2

O2
l5

z

x1

P

y1
q1

l4

O1

z1

C1

l4
C2
l3

y
h x

l3

l2
O

l1

B1

1

x0

B2

2

y0

A2

l1
l2
A1

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: OO 1O 2 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

q1

0

0

 / 2

2
2
l5
l5
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:

3

q2



cos θ  -sin θ .cos α  sin θ .sin α  cos θ .a 


sin θ  cos θ .cos α  -cos θ .sin α  sin θ .a 

H=
i  0
sin α 
cos α 
d 


0
0
1 

 0
Tạp chí Nghiên cứu KH&CN quân sự, Số Đặc san TĐH, 04 - 2019

 / 2

(1)

235


Đo lường & Tin học

Các ma trận chuyển tương ứng với bảng tham số D-H được tính như sau:
 cos  q1  t   0 -sin  q1  t  
0 0 1 0 

1 0 0 0 
 sin  q1  t   0 cos  q1  t  
;
H2 = 
H1 = 
0 1 0 h 
0
-1
0



0
0

0
1



0
0
0
1


 cos  q 2  t   0 -sin  q 2  t   2 cos  q 2  t   l5 2 


 sin  q  t   0 cos  q  t   1 sin  q  t   l 2 
2
2
2
5

H3 = 
2


1


0
-1
0

- l5 2
2




0
0
0
1



0
-1
0

 cos q t
 1    0 -sin  q1  t  
D 2 =H1.H 2 = 
 sin  q  t   0 cos  q  t  
1
1

0
0
0


0


0

0
1 

0
0 
h

1 

(2)

(3)

D3 = H1.H2.H3 
1


-sin q2  t  
0
-cos q2  t  
- sin q2  t   l5 2


2


1

1
cos q  t  cos q  t  sin q  t  -cos q  t  sin q  t 
1  2  1 
 1   2  2 cos q1  t cos q2  t l5 2+ 2 sin q1  t l5 2  (4)



sin q1  t   cos q2  t   -cos q1  t   -sin q1  t   sin q2  t   1 sin q1  t   cos q2  t   l5 2- 1 cos q1  t   l5 2+h


2
2


0
0
0
1



Ma trận cosin chỉ hướng


-sin  q 2  t  
0
-cos  q 2  t  


R=  cos  q1  t   cos  q 2  t   sin  q1  t   -cos  q1  t   sin  q 2  t   



 sin  q1  t   cos  q 2  t   -cos  q1  t   -sin  q1  t   sin  q 2  t   

(5)

Tọa độ tâm tấm đế động

1


- sin  q 2  t   l5 2


2


1
1

rP =
cos  q1  t   cos  q 2  t   l5 2+ sin  q1  t   l5 2 
 2

2
1

 sin  q1  t   cos  q 2  t   l5 2- 1 cos  q1  t   l5 2+h 
 2


2

236

(6)

L. T. Thắng, …, C. Đ. Chình, “Xây dựng bộ đo góc … đài quan sát quang điện tử.”


Nghiên cứu khoa học công nghệ

Tọa độ các điểm C1 , C2 trong hệ tọa độ Pxyz:
(7)

Tọa độ các điểm C1 , C2 trong hệ tọa độ Ox 0 y0z 0 :



-sin  q 2  t   l4


rC10 = rP + R.rC1 =  cos  q1  t   cos  q 2  t   l4 


h+sin  q1  t   cos  q 2  t   l4 



0



rC20 = rP + R.rC2 =  l 4sin  q1  t   


 h-l 4 cos  q1  t   

(9)

0
rA20 = l1 
 0 


l1 +l2 cos  φ1  t   
0




r
=
l
+l
cos
φ
t
rB10 = 
0
 2   
 ; B20  1 2



 l sin φ  t  
 l 2sin  φ 2  t   
 2  1  
l1 
rA10 =  0  ;
 0 

 l +l cos  φ  t   -sin  q  t   l 2
2
1
4
 1 2
2

+ l2sin  φ2  t   -h+cos  q1  t   l4
= l32


2
2
2
 l1 +l2cos  φ1  t   +sin  q2  t   l4 +cos  q1  t   cos  q2  t   l42

2

+ l2sin  φ1  t   -h-sin  q1  t   cos  q2  t   l4 = l32




(8)

(10)

(11)













(12)



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:

Tạp chí Nghiên cứu KH&CN quân sự, Số Đặc san TĐH, 04 - 2019

237


Đo lường & Tin học

- Trong bài toán động học thuận ta biết trước các góc 1 (t ), 2 (t ) . Từ hệ
phương trình trên ta xác định được các góc q1 (t ), q2 (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 q1 (t ), q2 (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 (t ), 2 (t )
Bài tốn động học thuận:
Ở đây ta biết trước các góc 1 (t ), 2 (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à q1 (t ), q2 (t ) .
Phương trình đầu trong hệ phương trình (12) sẽ có dạng

A sin  q1  t    B cos  q1  t    C , từ đó suy ra ta có [2]:

Tính được q1 (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 q2 (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 q1 (t ), q2 (t ) của khớp các đăng. Yêu cầu xác
định các góc 1 (t ), 2 (t ) . Tương tự như trên phương trình đầu của hệ (12) có dạng:

A 1 sin( 2 (t ))  B1 cos( 2 (t ))  C1

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 q1 (t ), q2 (t ) theo

1 (t ), 2 (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  q1 (t )

2  q2 (t )
238

L. T. Thắng, …, C. Đ. Chình, “Xây dựng bộ đo góc … đài quan sát quang điện tử.”


Nghiên cứu khoa học cơng nghệ

Đạ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  q1

 2   2  q2
Từ bài toán động học ngược, ta xác định được các góc quay 1 (t ), 2 (t ) theo

q1 (t ), q2 (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 (t )

 '
2  2 (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 (t )
 '
 2  2 (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 tố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ế.
Roll
Vận tốc góc
3 trục

Pitch

Bộ lọc Kalman

Roll*

Pitch*

Gia tốc góc

3 trục

Khối
hiệu chuẩn

Tính tốn
Pitch Roll

Yaw
Cảm biến từ trường
3 trục

Bù nghiêng

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.

Tạp chí Nghiên cứu KH&CN quân sự, Số Đặc san TĐH, 04 - 2019

239


Đo lường & Tin học

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 tố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 xun.
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:
Y (k  1)  Y (k )  Y (k )t  Yg (k )t  bY (k )t
(13)

bY (k  1)  bY (k )  Yb (k )
Trong đó :
Y (k  1) 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;
bY (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:
(14)
Y (k )
 X (k )  acr tan( acc ),
Z acc (k )
X (k )
Y (k )  acr tan( acc ),
Z acc (k )
X (k )
 Z (k )  acr tan( acc ).

Yacc (k )
Á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:

 xY (k  1)  AxY (k )  BuY (k )  Y (n)
 y (k )  Cx (k )  v (n)
 vY
Y
Y

 xX (k  1)  AxX (k )  Bu X (k )   X (n)
 yvX (k )  CxX (k )  vX (n)

(15)

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 tốn sẽ gặp hiện tượng trôi và vấn đề khởi tạo giá trị ban đầu. Ngồi ra từ

240

L. T. Thắng, …, C. Đ. Chình, “Xây dựng bộ đo góc … đài quan sát quang điện tử.”