Phương pháp xác định góc định hướng của tên lửa chống tăng B72, sử dụng cảm biến vi cơ điện tử MEMS

vận tốc góc  . 
Tên lửa & Thiết bị bay 
N. Đ. Duy, , P. T. Sơn, “Phương pháp xác định góc định hướng ... vi cơ điện tử MEMS.” 166 
Vận tốc góc của tên lửa so với hệ tọa độ địa lý có thể biểu diễn dưới dạng: 
b b b
gb gr rb    (2) 
 Trong đó bgr , 
b
rb - lần lượt là vận tốc góc của hệ tọa độ Resal so với hệ tọa độ 
địa lý và vận tốc góc của hệ tọa độ liên kết so với hệ tọa độ Resal. Cả hai vận tốc 
góc trên được chiếu xuống các trục của hệ tọa độ liên kết (chỉ số b ở trên). 
sin
cos cos sin
cos sin cos
b
gr
 
     
    
 
 



, 0
0
b
rb

 

 (3) 
Trên cơ sở kết quả mô phỏng quỹ đạo bay của tên lửa chống tăng B72 [8], ta 
thấy ở chế độ bay bằng trong phần lớn thời gian bay của tên lửa, vận tốc góc 
8,5 2   rad/s, còn theo các góc hướng và góc chúc ngóc, tên lửa dao động với 
biên độ nhỏ, khoảng 1º và 2º, tần số khoảng 1 và 2 Hz vì thế giá trị sin  rất nhỏ 
so với  . Do đó ta có thể coi: 
, ,
b b
gb x rb x     (4) 
Hình 2. Kết quả mô phỏng góc chúc ngóc của tên lửa B72 trong quá trình bay. 
2.2. Thuật toán xác định góc quay của tên lửa B72 
Để xác định các tham số góc của vật thể bay: góc hướng, góc chúc ngóc, góc 
cren, trong lý thuyết dẫn đường quán tính người ta có thể sử dụng nhiều phươ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 167
pháp khác nhau như sử dụng các phương trình động học Ơ-le, phương trình 
Poison, hay sử dụng quatenion [3]. Để xác định các góc định hướng của tên lửa 
B72, ta sẽ sử dụng quaternion. 
Thông tin về hình chiếu của véc tơ vận tốc góc tuyệt đối của tên lửa (vận tốc 
góc của hệ tọa độ liên kết so với hệ tọa độ quán tính) được đo bởi khối IMU gắn 
trên tên lửa: 
, , ,
b b b b
ib ib x ib y ib z    (5) 
Vì tên lửa B72 có thời gian làm việc ngắn (khoảng 26s) nên ta có thể bỏ qua véc 
tơ vận tốc mang (bao gồm vận tốc quay của trái đất và chuyển động của tên lửa 
trên bề mặt trái đất) và xác định các góc định hướng so với hệ tọa độ quán tính, tức 
là : 
b b
ib gb  (6) 
Giả sử biQ là quaternion thể hiện sự quay từ hệ tọa độ quán tính sang hệ tọa độ 
liên kết, bgQ là quaternion thể hiện sự quay từ hệ tọa độ địa lý sang hệ tọa độ liên 
kết. 
Phương trình Poison đối với quaternion biQ có dạng: 
2 ( )b b bi ib iQ M Q
 (7) 
Trong đó: 
0 1 2 3
b b b b b
i i i i iQ Q Q Q Q (8) 
, , ,
, , ,
, , ,
, , ,
0
0
( )
0
0
b b b
ib x ib y ib z
b b b
ib x ib z ib yb
ib b b b
ib y ib z ib x
b b b
ib z ib y ib x
M
  
  

  
  
  



 
 (9) 
( )bibM  ma trận quaternion tạo bởi hình chiếu của véc tơ vận tốc góc 
b
ib lên 
các trục của hệ tọa độ liên kết. 
Giống như khi giải phương trình vi phân vô hướng, nghiệm của phương trình vi 
phân ma trận với chu kỳ lấy mẫu T0 có thể viết ở dạng: 
1
( )
2( 1) ( )
k
b b
i iQ k e Q k

  (10) 
trong đó : 
( 1) 0
0
0 ( ) ( ) ( )
( ) 0 ( ) ( )
( ) ( ( ))
( ) ( ) 0 ( )
( ) ( ) ( ) 0
x y z
k T
x z yb
ib
y z xkT
z y x
k k k
k k k
k M t dt
k k k
k k k
  
  
 
  
  

  

 


 , 
1
( )
20,1,...
k
k e

 (11) 
Hàm số mũ 
1
( )
2
k
e

 có thể được khai triển theo dạng chuỗi: 
1 2
( )
2
4 4
( ) 1 ( ) 1 ( )
2 2! 4 ! 2
n
k
x n
k k k
e E
n
   
       (12) 
Trong đó 4 4xE là ma trận đơn vị kích thước 4x4. 
Tên lửa & Thiết bị bay 
N. Đ. Duy, , P. T. Sơn, “Phương pháp xác định góc định hướng ... vi cơ điện tử MEMS.” 168 
Giả sử trong khoảng thời gian bằng chu kỳ lấy mẫu T0, vận tốc góc ( )
b
ib k thay 
đổi không đáng kể thì 0( ) ( ( ))
b
ibk T M k  . Khi đó chuỗi số có dạng: 
1 2 2
( )
0 02
4 4
( ( )) ( ( ))1
2 2! 4
b b
k
ib ib
x
T M k T M k
e E
  
     (13) 
Khi đó nghiệm của phương trình vi phân ma trận có dạng: 
0
0
( ( ))1
( 1) [ ] ( )
! 2
b
b n bib
i i
n
T M k
Q k Q k
n


  (14) 
Tiến hành khai triển biểu thức ( 1)biQ k  đến số hạng thứ n thì phần dư còn lại 
có dạng: 
0
11
( )
0 2
(0,5 ( ))
( 1)!
b
ib
b n
T M
ib
n
T M
R e
n
  


, với 0 1  . (15) 
Phần dư nR cũng là một ma trận kích thước 4x4. Giả sử chọn hệ số 0,5  , 
chu kỳ lấy mẫu 0 0,01T  (s) ta có thể tính giá trị phần dư đối với các vận tốc góc 
khác nhau trên cơ sở công thức vô hướng dành cho một phần tử của ma trận: 
,
1 1
, 2
(0,5 )
( 1)!
b
ib m
b n
ib m
nr e
n
 


 (16) 
rn ,
b
ib m , rad/s 
3,14 (1 Hz) 9,42 (3 Hz) 53,38 (8,5 Hz) 
r1 41, 408 10 31, 268 10 0,041 
r2 77,371 10 51,99 10 33,621 10 
r3 92,893 10 72,343 10 42, 416 10 
r4 129,084 10 92, 207 10 51, 29 10 
r5 142,377 10 111,733 10 55,737 10 
Từ các kết quả tính toán trên ta thấy rằng nếu tên lửa có vận tốc góc nhỏ thì 
trong thuật toán tính góc định hướng, ta chỉ cần tính đến 2 – 3 số hạng, còn nếu tên 
lửa có vận tốc góc lớn thì cần tính đến 4 – 5 số hạng. Đối với tên lửa B72, vận tốc 
góc cren lớn (8,5 Hz), còn dao động theo các góc hướng và góc chúc ngóc nhỏ (từ 
1 – 2 Hz). Vì thế khi tính các góc định hướng của tên lửa B72, ta nên sử dụng sơ 
đồ phân chia, tức là góc cren được tính theo thuật toán bậc cao, còn góc hướng và 
góc chúc ngóc được tính theo bậc thấp theo sơ đồ trên hình 3. 
Trên cơ sở quaternion brQ , ta tính được sin và cos của góc cren để sử dụng cho 
việc điều khiển tên lửa [5], [6]: 
0 1
2 2
0 1
sin ( 1) 2 ( 1) ( 1)
cos ( 1) [ ( 1)] [ ( 1)]
b b
r r
b b
r r
k Q k Q k
k Q k Q k


   
    
 (17) 
Các góc hướng và góc chúc ngóc được tính trên cơ sở quaternion riQ : 
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 169
1 3 0 2
2 2
1 0
2 ( 1) ( 1) 2 ( 1) ( 1)
( 1) [ ]
2[ ( 1)] 2[ ( 1)] 1
r r r r
i i i i
r r
i i
Q k Q k Q k Q k
k arctg
Q k Q k

    
 
   
 (18) 
1 2 0 3( 1) arcsin[2 ( 1) ( 1) 2 ( 1) ( 1)]
r r r r
i i i ik Q k Q k Q k Q k        (19) 
Hình 3. Sơ đồ phân chia xác định các góc định hướng của tên lửa B72. 
2.3. Kết quả mô phỏng trong môi trường Matlab-Simulink 
Trên cơ sở thuật toán đã trình bày ở trên, ta xây dựng chương trình mô phỏng 
thuật toán xác định góc định hướng của tên lửa B72 trong môi trường Matlab-
Simulink theo sơ đồ truyền thống và sơ đồ phân chia: 
Hình 4. Sơ đồ mô phỏng thuật toán phân chia xác định góc định hướng 
của tên lửa B72. 
Tên lửa & Thiết bị bay 
N. Đ. Duy, , P. T. Sơn, “Phương pháp xác định góc định hướng ... vi cơ điện tử MEMS.” 170 
Trên hình 5 và hình 6 trình bày kết quả mô phỏng góc quay của tên lửa B72 khi 
sử dụng thuật toán phân chia và thuật toán BINS truyền thống. 
Hình 5. Kết quả mô phỏng thuật toán tính góc định hướng 
tên lửa B72 trong thời gian ngắn. 
Hình 6. Kết quả mô phỏng thuật toán tính góc định hướng 
tên lửa B72 trong thời gian 26(s). 
3. KẾT LUẬN 
Kết quả nghiên cứu cho thấy thuật toán tính góc định hướng của tên lửa B72 
theo sơ đồ phân chia có độ chính xác cao hơn so với sơ đồ BINS truyền thống. Độ 
chính xác của thuật toán truyền thống tăng lên khi ta giảm chu kỳ lấy mẫu xuống 
10 lần, trong khi sơ đồ phân chia có sai số về biên độ và pha nhỏ ngay ở chu kỳ lấy 
mẫu 0,01s. Khi so sánh kết quả mô phỏng ta thấy thuật toán truyền thống chỉ cho 
độ chính xác chấp nhận được trong thời gian ngắn cỡ vài giây, còn khi thời gian 
mô phỏng tăng lên thì rõ ràng độ chính xác của sơ đồ phần chia lớn hơn hẳn so với 
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 171
sơ đồ truyền thống. Đặc biệt khi tăng tốc độ góc cren lên thì sai số xác định góc 
định hướng của sơ đồ truyền thống càng rõ rệt hơn so với sơ đồ phân chia. Điều 
này làm cho thuật toán phân chia có thể ứng dụng để tính góc quay cren không chỉ 
của tên lửa B72 và các loại tên lửa khác có tần số quay lớn. 
TÀI LIỆU THAM KHẢO 
[1]. Управляемый снаряд 9М14М, Техническое описание. Изд Министертсва 
Обороны СССР 1966. 
[2]. Tổ hợp tên lửa chống tăng 9K11. Giáo trình huấn luyện. Bộ tư lệnh pháo binh. 
Hà Nội, 2006 
[3]. В.В. Матвеев, В.Я.Распопов. Основы построения бесплатформенных 
инерциальных навигационных систем. учеб.пособие, СПб.: ГНЦ РФ 
ОАО "Концерн "ЦНИИ Электроприбир", 2009. - 280с. 
[4]. В.В.Матвеев, А.П.Шведов, С.И.Серегин, “Алгоритм ориентации для 
вращающегося по крену летательного аппарата”, Журнал 
Мехатроника, автоматизация, управление, №9, 2012. 
[5]. Бранец.В.Н, Шмылевский И.П. “Применение кватернионов в задачах 
ориентации твердого тела. М. Наука”, 973 – 320 с. 
[6]. Paul G. Savage, "Strapdown Inertial Navigation Integration Algorithm Design 
Part 1: Attitude Algorithms", Journal of Guidance, Control, and Dynamics, 
Vol. 21, No. 1 (1998), pp. 19-28 
[7]. Phạm Văn Phúc, Trần Đức Thuận, “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ế”, Tạp chí nghiên cứu KHCNQS, số 49, tháng 6/2017. 
[8]. Nguyễn Văn Chúc, Nguyễn Phú Thắng, Phạm Khắc Lâm, “Mô phỏng bán tự 
nhiên thời gian thực tên lửa điều khiển tầm gần kiểu B72”, Tạp chí nghiên cứu 
KHCNQS số đặc san, tháng 9/2016. 
ABSTRACT 
METHOD FOR DETERMINING ORIENTATION ANGLES OF ANTI TANK 
MISSILE B72, USING SENSORS MEMS 
The article presents a method for determining the orientation angles of a B72 
missile based on an INS algorithm using information from micro-electro-
mechanical sensors. The new algorithm makes it possible to determine the angles 
of the rocket's orientation with high accuracy and can be applied to the rockets 
rotating rapidly around the longitudinal axis (frequency up to 20 Hz) and then to 
replace the mechanical gyroscope on the B72 missile. 
Keywords: INS; MEMS; Gyroscope. 
Nhận bài ngày 30 tháng 01 năm 2019 
 Hoàn thiện ngày 08 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ên lửa – Viện KHCN quân sự. 
 *Email: ariolvietnam@gmail.com.