Về một phương pháp xác định tư thế ban đầu cho hệ dẫn đường quán tính không đế ở chế độ la bàn trên đối tượng xe cơ giới

Kỹ thuật điều khiển

VỀ MỘT PHƯƠNG PHÁP XÁC ĐỊNH TƯ THẾ BAN ĐẦU
CHO HỆ DẪN ĐƯỜNG QN TÍNH KHƠNG ĐẾ Ở CHẾ ĐỘ LA BÀN
TRÊN ĐỐI TƯỢNG XE CƠ GIỚI
Vũ Hải Hà*, Hồ Sỹ Dương, Cao Đức Sáng
Tóm tắt: Đối với hệ thống dẫn đường quán tính, việc xác định chính xác góc tư thế ban
đầu có vai trị vơ cùng quan trong, ảnh hưởng trực tiếp đến quá trình điều hướng sau này
của hệ thống. Bài báo đề cập tới thuật toán xác định tư thế ban đầu bao gồm hướng bắc
thực cho hệ thống dẫn đường quán tính không đế SINS trên vật thể không chuyển động ở
chế độ la bàn con quay gyrocompass. Thuật toán được thực hiện theo hai giai đoạn là xác
định tư thế ban đầu sơ bộ và xác định tư thế ban đầu chính xác dựa trên các vectơ trạng
thái của khối đo lường quán tính IMU. Kết quả nhận được đã chứng minh tính chính xác
của thuật tốn.
Từ khóa: Alignment; SINS; IMU; Góc định hướng; La bàn.

1. ĐẶT VẤN ĐỀ
Theo nghĩa rộng, có thể hiểu việc xác định tư thế của hệ thống dẫn đường qn tính
có thể chia thành xác định tư thế ban đầu và xác định tư thế khi vận động. Xác định, hiệu
chỉnh góc tư thế ban đầu (initial alignment) của hệ thống dẫn đường qn tính khơng đế
(SINS) là một quá trình quan trọng được thực hiện trước khi đến bước điều hướng thông
thường [1], như ta đã biết rằng kết quả xác định, hiệu chỉnh góc ban đầu của hệ thống có
tầm quan trọng cơ bản ảnh hưởng trực tiếp đến độ chính xác của quá trình điều hướng
cho hệ thống dẫn đường qn tính.
Để bắt đầu các thuật toán trong hệ thống dẫn đường quán tính (SINS), trước tiên cần
nhập các điều kiện ban đầu: tọa độ, tốc độ và các tham số hướng đối tượng. Bản chất của
việc đưa ra các tham số góc ban đầu là xác định vị trí góc của vật thể so với hệ tọa độ địa
lý. Có hai loại phương pháp đưa ra góc ban đầu đó là: xác định góc ban đầu ở chế độ tự
trị của SINS và xác định góc ban đầu được lấy từ các nguồn thơng tin bên ngồi (ví dụ
như: la bàn, máy đo từ trường, hệ thống định vị vệ tinh).
Trong chế độ tự trị việc xác định góc ban đầu lấy thông tin đo đầu vào là gia tốc trọng

trường của Trái đất, tốc độ góc của Trái đất và tọa độ vật thể từ đó đưa ra giá trị các góc
so với hệ tọa độ địa lý OXgYgZg. Vị trí này được mơ tả bởi các góc heading ψ0, pitch θ0,
roll ϕ0, là các góc cần tìm của bài tốn xác định tư thế ban đầu [2]. Về mặt lý thuyết, các
phương pháp phân tích, xác định tư thế ban đầu của hệ thống dẫn đường qn tính khơng
đế ở chế độ tự trị có chức năng tương đương với các la bàn con quay vật lý trên các hệ
gắn chặt.
2. XÁC ĐỊNH GÓC BAN ĐẦU VỚI SINS GẮN TRÊN VẬT THỂ ĐỨNG YÊN
Mục tiêu xác định tư thế ban đầu của SINS trên cơ sở cố định là xác định hướng ban
đầu của hệ tọa độ liên kết so với hệ tọa độ dẫn đường bằng cách đọc các cảm biến quán
tính (gia tốc kế accelerometer, cảm biến vận tốc góc gyroscopes).
Hệ tọa độ dẫn đường (n-frame) OXnYnZn là hệ tọa độ đo đạc cục bộ với tâm của nó
trùng khớp với hệ toạ độ của cảm biến quán tính, trục X chỉ về hướng Bắc, trục Y chỉ về
hướng Đơng, trục Z vng góc trực giao với mặt phẳng chiếu được hướng xuống dưới
(NED) hoặc lên trên (NEU), và trục z xác định theo quy tắc bàn tay phải. Hệ tọa độ dẫn

72

V. H. Hà, H. S. Dương, C. Đ. Sáng, “Về một phương pháp xác định … đối tượng xe cơ giới.”


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

đường quay so với hệ tọa độ trái đất tốc độ góc ωe gây ra do chuyển động của phương
tiện so với trái đất (hình 1).
Hệ tọa độ liên kết là hệ tọa độ gắn liền lấy tâm của vật thể làm tâm gốc hệ tọa độ, từ
đó mơ tả các chuyển động theo các hướng trước sau, phải trái, lên xuống và các góc quay
euler. Là hệ trục toạ độ trực giao tương ứng với các góc euler: góc nghiêng ngang (góc
roll  ), góc chúc ngóc (góc pitch  ) và góc hướng (góc heading ) của phương tiện
(hình 2).

Hình 1. Hệ tọa độ dẫn đường hay hệ tọa độ địa lý NED n-frame.

Hình 2. Hệ tọa độ liên kết b-frame.
Thuật ngữ cơ sở cố định có nghĩa là vật thể có gắn SINS đứng yên so với Trái đất.
Tọa độ địa lý của vật thể (trùng với điểm O là trọng tâm của vật thể) φ, λ, h, được cho
trước với độ chính xác vừa đủ, vận tốc tuyến tính của điểm O so với Trái đất bằng không.
Ước lượng sai số vận tốc
Ước lượng sai số
góc

IIR

2state
KF

Navigation
Algorithm

IMU
IIR

2state
KF

Sai số vận tốc

The closedloop
Kalman
filter

Hình 3. Sơ đồ hiệu chỉnh góc tư thế ban đầu.
Việc xác định góc tư thế ban đầu được thực hiện theo 2 giai đoạn:
- Giai đoạn 1: Xác định thô (5-30 giây);
Mục đích: Đánh giá sơ bộ các góc định hướng, góc tư thế bằng cách lấy trung bình
các giá trị của cảm biến quán tính IMU.

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

73


Kỹ thuật điều khiển

- Giai đoạn 2: Xác định tinh (5-15 phút);
Mục đích: Sàng lọc các góc định hướng bằng thuật toán ước lượng động [3].
2.1. Giai đoạn xác định thô
Như ta đã biết ma trận cosin chỉ hướng Cnb là ma trận chuyển hệ tọa độ dẫn đường
OXnYnZn sang hệ tọa độ liên kết OXbYbZb:
[XbYbZb]= Cnb [XnYnZn]
cos  cos


C = sin  sin  cos − cos  sin
cos  sin  cos + sin  sin

cos  sin
sin  sin  sin + cos  cos
cos  sin  sin − sin  cos

b

n

(1)
− sin  
sin  cos  
cos  cos  

(2)

Theo đó, có thể xác định vecto trọng lực trên hệ tọa độ gắn liền như sau:
GXb 
GXn 
 0   − g sin  0 
 b


b  n
b  
 GY  = Cn  GY  = Cn  0  =  g sin 0 cos  0 
b
n
 GZ 
 GZ 
 g   g cos 0 cos  0 
 
 

(3)

Trong đó, G Xb GYbGZb là các giá đo được bằng gia tốc kế từ đó có thể xác định được các

góc pitch θ0, roll ϕ0 ban đầu từ phương trình (6):



;
 (G b ) 2 + (G b ) 2 
)
Z
 ( Y
 Gb 
0 = arctan  Yb 
 GZ 

 0 = arctan 

−GXb

(4)

Để xác định sơ bộ góc hướng ban đầu phải sử dụng dữ liệu của con quay gyroscope
dựa theo biểu thức sau:
 Xb 
 Xn 
 b
b  n
 X  = Cn  Y 
 Xb 
 Zn 
 
 

(5)

 Xb YbZb là các giá trị vận tốc góc đo được từ gyroscope. Trong trường hợp xác định
tư thế ban đầu khi vật thể đứng yên hình chiếu vecto vận tốc góc quay của trái đất trên hệ
tọa độ dẫn đường là các giá trị  Xn YnZn được tính theo cơng thức:
 Xn = e cos 
Yn = 0

(6)

 = −e sin 
n
Z

Trong đó: φ là vĩ độ tại điểm đặt của vật thể;
ωe = 7.292*10-5 rad/s là vận tốc góc quay của trái đất;
g là gia tốc trọng trường tại điểm O.
Từ phương trình (5) và (6) kết hợp ma trận cosin định hướng có thể xác định sơ bộ
góc hướng (heading) ψ0 theo cơng thức sau:

74

V. H. Hà, H. S. Dương, C. Đ. Sáng, “Về một phương pháp xác định … đối tượng xe cơ giới.”


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

 Xb =  Xn cos 0cos 0 + Zn (− sin  0 );
cos 0 =

 Xb + Yn sin  0
;
 Xn cos0

(7)

 b +  n sin  0
 b − e sin  sin  0
 0 = arccos X n Z
= arccos X
.
 X cos 0
e cos cos 0

Để xử lý tín hiệu đầu ra từ IMU nhóm nghiên cứu sử dụng bộ lọc trước. Nó bao gồm
bộ lọc thơng thấp Infinite Impulse Response (IIR) với chức năng giảm nhiễu tần số cao
sau đó đầu ra của IIR được mơ hình hóa và bộ lọc Kalman 2 trạng thái được sử dụng để
giảm nhiễu hơn nữa. Đầu ra cảm biến của con quay sợi quang FOG sẽ bị ảnh hưởng bởi
nhiễu rất lớn, ví dụ như khi hệ thống được đặt trên một chiếc xe có động cơ đang bật,
phương sai nhiễu ồn sẽ tăng lên rất nhiều. Khi đó sử dụng bộ lọc IIR có thể hạn chế
nhiễu tần số cao. Tuy nhiên, trong việc căn chỉnh tần số cắt của IIR không thể đặt quá
thấp để tránh việc thơng tin hữu ích sẽ mất và kết quả được lọc có thể bị sai lệch [5].
Trong hệ thống của chúng tôi, bộ lọc IIR 4 bậc với tần số cắt 10 Hz được thiết kế [6] do
các phương tiện mặt đất chuyển động với tần số thấp (từ 0-5Hz). Hệ số bộ lọc IIR được
chọn như sau: b0=0.0048423; b1=0.0192974; b2=0.0289461; b3=0.0192984;
b4=0.0048243; a1=-2.369513; a2= 2.3139884; a3=-1.0546654; a4=0.1873795.
Hình ảnh tín hiệu gyroscope trước và sau bộ lọc thể hiện như hình 4.

Hình 4. Đầu ra của gyroscope trước và sau bộ lọc.

2.2. Giai đoạn xác định tinh tư thế của SINS gắn trên đối tượng đứng yên
Căn chỉnh thô dựa trên sự lý tưởng hóa trong đó khơng có lỗi gia tốc kế và con quay
hồi chuyển. Nhưng thực tế dữ liệu gia tốc và dữ liệu đầu ra của con quay đều có lỗi, đặc
biệt là con quay hồi chuyền có thể có trơi lớn. Vì vậy, ma trận trạng thái hay các góc
Euler được đưa ra bởi căn chỉnh thơ có thể có một số lỗi. Với căn chỉnh thơ có thể đo các
góc roll và pitch với sai số 0,010 hoặc tốt hơn, tuy nhiên, với góc hướng ( - heading)
sai số đưa ra có thể là rất lớn.
Căn chỉnh chính xác được thực hiện trên cơ sở căn chỉnh thơ và góc lệch giữa hệ tọa
độ điều hướng và hệ tọa độ điều hướng thực được điều chỉnh để làm cho nó càng gần 0
càng tốt. Để đạt được ước lượng trạng thái ban đầu chính xác hơn, các phép đo liên tiếp
được sử dụng để thực hiện tự liên kết trong một khoảng thời gian. Trong điều kiện chuẩn
tinh, các phép đo vận tốc bằng không được sử dụng trong bộ lọc Kalman trong giai đoạn
căn chỉnh ban đầu để hiệu chỉnh vận tốc và trạng thái góc tư thế. Ở đây, mơ hình sai số
góc các góc tư thế ψN,E,D được xét đến với giả định bias của cảm biến được mơ hình hóa
trong khung điều hướng là khơng thay đổi. Mơ hình trạng thái của hệ thống [6] có thể

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

75


Kỹ thuật điều khiển

được viết như sau:
 N   0
  
 E   −2eD
 N   0

 

 E   0
   0
 D =
 N   0

 
 E   0
   0
 N  
 E   0
   0
 D 

2eD
0
0
0
0
0
0
0
0
0

0
g
0
−g
0
0

0
eD 0
−eD
0 eN
0
eN 0
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
1
0
0
0
0
0
0
0
0

0
0
1
0
0
0
0
0
0
0

0
0
0
1
0

0
0
0
0
0

0   N 


0   E 
0   N 


0   E 
1   D 
 + wk

0   N 
0   E 


0   N 


0   E 

0   D 

(8)

Trong đó,  là bias của gia tốc kế, ε độ trơi của gyroscope. Phương trình 8 có thể viết
lại dưới dạng như sau:
xk = Fk −1 xk −1 + wk −1
(9)
Xk là trạng thái tại thời điểm k, Fk-1 biểu thị ma trận chuyển trạng thái, wk-1 biểu thị
nhiễu quá trình (thường là nhiễu trắng Gaussian) với mật độ phổ công suất Q.
q ~ N ( 0, Q ) ,

Q = diag  qa2 , qa2 , qg2 , qg2 , qg2 , 0, 0, 0, 0, 0 

(10)

Trong đó, qa đại diện cho nhiễu ngẫu nhiên của gia tốc kế và qg là nhiễu ngẫu nhiên
của con quay hồi chuyển.
Khi xe ở trạng thái đứng yên, thông tin vận tốc bằng 0 trong khung điều hướng được
sử dụng làm tham chiếu bên ngồi và vector đo có thể được xây dựng như:
vN ( INS ) − vN ( REAL )  vN − 0  vN 
Z =

=

vE ( INS ) − vE ( REAL )  vE − 0   vE 

(11)

Trong đó, vN và vE là vận tốc phương ngang của xe được tính bởi SINS. Như vậy, vận
tốc đầu ra của SINS chính là sai số vận tốc và nó có thể được coi là vectơ đo hay vector
quan sát. Khi đó, phương trình đo lường của hệ thống [7, 8] được biểu diễn như phương
trình sau:
 N  1 0 0 0 0 0 0 0 0 0 

  = 0 1 0 0 0 0 0 0 0 0  x + v ( t )
(12)

 E 
Z k = H k xk + vk

Trong đó: Zk là vector đo hay vector quan sát; Hk là ma trận đo hay ma trạn quan sát,
vk biểu thị nhiễu đo với mật độ phổ công suất R.
r ~ N ( 0, R ) ,

R = diag  R , R 

(13)

Bộ lọc Kalman vòng kín (hình 5) 10 trạng thái được triển khai để thực hiện ước lượng
và hiệu chỉnh các tham số điều hướng, khoảng thời gian tính tốn của bộ lọc Kalman là
0.1s. Hệ thống dẫn đường quán tính sử dụng khối đo lường qn tính IMU500 của hãng
Optolink-Nga. Độ trơi của con quay hồi chuyển sợi quang (Fiber-Optic Gyroscope –
FOG) là 0,10/h, turn-on drift với phương sai Allan là 0.0050/h, nhiễu ngẫu nhiên của

76

V. H. Hà, H. S. Dương, C. Đ. Sáng, “Về một phương pháp xác định … đối tượng xe cơ giới.”


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

FOG là 20/h. Độ trôi ngẫu nhiên của gia tốc kế là 20μg và sai số ngẫu nhiên là 0.5mg.
Các ma trận hiệp phương sai được thiết lập như sau:
P = diag (0.3m / s)2 , (0.3m / s) 2 , (1) 2 , (1) 2 , ( PD ) 2 , (20  g ) 2 , (20  g ) 2 , (0.10 / h)2 , (0.10 / h)2 , (0.10 / h)2 

Q = diag (0.5mg )2 , (0.5mg ) 2 , (0.0050 / h) 2 , (0.0050 / h) 2 , (0.0050 / h)2 , 0, 0, 0, 0, 0
R = diag (0.0025m / s) 2 , (0.0025m / s)2 
Tư thế đầu ra sau hiệu chỉnh

SINS
Ước lượng lỗi quán tính
vINS

Zero-velocity

v=0

Kalman Filter

Hình 5. Bộ lọc Kalman vịng kín cho xác định tư thế ban đầu của SINS.
3. THỬ NGHIỆM VÀ KẾT QUẢ
Trên cơ sở hệ dẫn đường quán tính ta xây dựng cấu hình phần cứng bộ thử nghiệm
gồm các thành phần sau: khối đo lường quán tính IMU500, module nguồn cách ly, bo
mạch xử lý DSP và module giao tiếp (hình 6).

Hình 6. Phần cứng bộ thiết bị thử nghiệm.
Kết quả thử nghiệm xác định góc tư thế ban đầu ở hai vị trí 89B Lý Nam Đế và 147
Phú Viên được thể hiện như trên hình 7, 8 và trên bảng 1.

Hình 7. Kết quả thử nghiệm xác định góc phương vị tại vị trí 1.

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

77

Kỹ thuật điều khiển

Hình 8. Kết quả thử nghiệm xác định góc phương vị tại vị trí 2.
Kết quả góc phương vị ban đầu của khối dẫn đường quán tính sử dụng con quay hồi
chuyển sợi quang IMU500.
Bảng 1. Kết quả căn chỉnh góc phương vị ban đầu tại các vị trí thử nghiệm.
STT
Vị trí 1 (89B Lý Nam Đế)
Ví trí 2 (147 Phú Viên)
Ψ0 (Độ)
Ψ0 (Độ)
1
10.01
-28.82
2
10.03
-28.83
3
10.04
-28.86
4
10.05
-28.82
5
10.01
-28.84
6
10.04
-28.85

7
10.03
-28.86
8
10.04
-28.83
9
10.05
-28.87
10
10.02
-28.85
Qua số liệu từ bảng kết quả căn chỉnh tại các vị trí khác nhau và đồ thị thu được từ
giao diện phần mềm thu thập và hiển thị số liệu có thể thấy thơng qua bước căn chỉnh thơ
có thể tính tốn sơ bộ các góc tư thế, từ đó làm giảm phương sai ước tính ban đầu và
tăng tốc độ hội tụ khi xác định góc hướng. Cũng qua thí nghiệm khi vật thể đứng yên,
việc ước lượng được độ trôi bias drift góc hướng đã đạt 0.1độ/h tương đương với chỉ tiêu
của nhà sản xuất.
4. KẾT LUẬN
Như vậy, trong thuật toán căn chỉnh góc tư thế ban đầu, việc xác định sơ bộ các góc
giúp tăng tốc độ hội tụ của bộ lọc Kalman trong gian đoạn căn chỉnh tinh, các góc roll ϕ0
và pitch θ0 ở thời điểm đối tượng đứng yên được xác định trực tiếp thông qua các giá trị
đo của gia tốc kế và sai số cũng phụ thuộc vào sai số của gia tốc kế nên có độ chính xác
khá cao, giá trị ổn định khơng gặp hiện tượng trôi như con quay hồi chuyển. Việc triệt
tiêu nhiễu của các cảm biến trong môi trường thực được xử lý thông qua bộ lọc trước gồm
2 thành phần bộ lọc thông thấp IIR và bộ lọc Kalman 2 trạng thái. Ngồi ra, có thể thấy
rằng phương sai ước tính ban đầu của sai số góc phương vị là yếu tố chính ảnh hưởng đến
tốc độ hội tụ của sai số góc phương vị. Bằng cách điều chỉnh nó, lỗi góc phương vị sẽ
sớm hội tụ và vấn đề lỗi góc phương vị lớn có thể được giải quyết. Tính đúng đắn và khả
thi của thuật tốn được xác minh bằng mô phỏng và qua thử nghiệm thực tế.

Bài báo này đã được báo cáo tại Hội thảo Quốc gia: Ứng dụng Công nghệ cao vào thực tiễn – 60
năm phát triển Viện KH-CN quân sự.

78

V. H. Hà, H. S. Dương, C. Đ. Sáng, “Về một phương pháp xác định … đối tượng xe cơ giới.”


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

TÀI LIỆU THAM KHẢO
[1]. H. W. Park, J. G. Lee, and C. G. Park, “Covariance analysis of strapdown INS considering
gyrocompass characteristics,” IEEE Transactions on Aerospace and Electronic Systems,
vol. 31, no. 1, pp. 320–328, 1995.
[2]. Мелешко, В.В. “Бесплатформенные инерциальные навигационные системы: учебное
пособие”/ В.В. Мелешко, О.И. Нестеренко. – Кировоград: Полимед-Сервис, 2011. – 171с.
[3]. Голован А. А., Парусников Н. А. “Математические основы навигационных систем”.
М.: Моск. гос. университет, 2010. Т. 1–3.
[4]. Бромберг П. В. “Теория инерциальных систем навигации”/ П. В. Бромберг – М.:
Наука, 1980. – 296 с.
[5]. J.G. Proakis, D.G. Manolakis, “Digital Signal Processing: Principles, Algorithms, and
Applications”, third ed., Prentice-Hall, Englewood Cliffs, NJ, 1996.
[6]. Shaolin Lu, Ling Xie, Jiabin Chen. “New techniques for initial alignment of strapdown
inertial navigation system”. Journal of the Franklin Institute 346 (2009) 1021–1037.
[7]. Liu M, Gao Y, Li G, Guang X, Li S. “An Improved Alignment Method for the Strapdown
Inertial Navigation System (SINS)”. Sensors (Basel). 2016;16(5):621. Published 2016 Apr
29. doi:10.3390/s16050621
[8]. Feng Sun, Tong Cao, Bo Xu, Yueyang Ben and Yudan Wang. “Initial Alignment for Strapdown
Inertial Navigation System Based on Inertial Frame”. Proceedings of the 2009 IEEE.
International Conference on Mechatronics and Automation. August 9-12, Changchun, China.

ABSTRACT
A METHOD OF DETERMINING THE INITIAL ATTITUDE ANGLES
FOR THE STRAPDOWN INERTIAL NAVIGATION SYSTEM
IN GYROCOMPASS MODE ON THE MOTOR VEHICLES
For the strapdown inertial navigation system, determining the exact angle of the initial
attitudes plays an extremely important role, directly affecting the later navigation process
of the system. The paper deals with the initial alignment determination algorithm
including true NORTH for the SINS on non-moving objects in gyrocompass mode. The
algorithm is carried out in two stages: determining the preliminary initial posture and
determining the correct initial posture based on the state vectors of the IMU inertial
measurement unit. The results obtained proved the accuracy of the algorithm.
Keywords: Alignment; SINS; IMU; Euler angles; Gyrocompass.

Nhận bài ngày 28 tháng 7 năm 2020
Hoàn thiện ngày 10 tháng 12 năm 2020
Chấp nhận đăng ngày 15 tháng 12 năm 2020
Địa chỉ: Viện Tự động hóa KTQS/ Viện Khoa học và Cơng nghệ qn sự.
*Email:

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

79