Bài viết đề 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.
Trang 1VỀ 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
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 toá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 quán 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 quán 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 quán 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 ngoà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 toá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 quán 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) OX n Y n Z n 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 vuông 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
Trang 2đườ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
IIR
IIR
IMU Navigation Algorithm
The closed-loop Kalman filter
Sai số vận tốc
Ước lượng sai số vận tốc Ước lượng sai số
góc
2state KF
2state KF
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
Trang 3- 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 b
n
C là ma trận chuyển hệ tọa độ dẫn đường
OXnYnZn sang hệ tọa độ liên kết OXbYbZb:
[XbYbZb]= b
n
sin sin cos cos sin sin sin sin cos cos sin cos cos sin cos sin sin cos sin sin sin cos cos cos
b
n
C
−
(2)
Theo đó, có thể xác định vecto trọng lực trên hệ tọa độ gắn liền như sau:
0
cos cos
(3)
Trong đó, b b b
X Y Z
G G G 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):
0
0
( ) ( ) arctan
b X
b Y b Z
G
G G
(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:
C
(5)
b b b
X Y Z
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ị n n n
X Y Z
được tính theo công thức:
cos 0 sin
n
X e n Y n
=
=
= −
(6)
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:
Trang 40 0 0
0 0
0
0
cos cos ( sin );
sin
cos
b n
X Y n X
n
+
=
(7)
Để 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ể
Trang 5được viết như sau:
N
E
N
E
D
g g
N
E
N
E
D k N
E
N
E
D
w
(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:
1 1 1
X k là trạng thái tại thời điểm k, F k-1 biểu thị ma trận chuyển trạng thái, w k-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 q q q q q a2 , a2 , g2 , g2 , 2g, 0, 0, 0, 0, 0 (10)
Trong đó, q a đại diện cho nhiễu ngẫu nhiên của gia tốc kế và q g 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 ngoài và vector đo có thể được xây dựng như:
( ) ( ) ( ) ( )
0 0
Z
−
= − − =
Trong đó, v N và v E 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:
( )
1 0 0 0 0 0 0 0 0 0
0 1 0 0 0 0 0 0 0 0
N
E
v
v
=
+
Trong đó: Z k là vector đo hay vector quan sát; H k là ma trận đo hay ma trạn quan sát,
v k 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 toá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 quán 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
Trang 6FOG 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:
(0.3 / ) , (0.3 / ) , (1) , (1) , ( ) , (20 ) , (20 ) , (0.1 / ) , (0.1 / ) , (0.1 / )
(0.5 ) , (0.5 ) , (0.005 / ) , (0.005 / ) , (0.005 / , 0, 0, 0, 0,
2
)
(0.00 5 / ) , (0.0025 /
0
)
D
Q
m
diag
SINS
Zero-velocity
Kalman Filter
v=0
v INS
Ước lượng lỗi quán tính
Tư thế đầu ra sau hiệu chỉnh
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
Trang 7Hì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
Ψ 0 (Độ) Ψ 0 (Độ)
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 toá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 Ngoà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 toá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ự
Trang 8TÀ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ệ quân sự
*Email: vuhatdh@gmail.com