Ứng dụng bộ lọc kalman trong hệ thống dẫn đường điện tử

Bộ giáo dục và đào tạo
Tr-ờng đại học bách khoa hà nội
---------------------------------------------

Trần Thọ Tuân

ứng dụng bộ lọc Kalman trong
hệ thống dẫn đ-ờng điện tử

Luận văn thạc sĩ : iện tử viễn thông

Hà nội - 2004


Bộ giáo dục và đào tạo
Tr-ờng đại học bách khoa hà nội
--------------------------------------------------------------------------------------------

Trần Thọ Tuân

ứng dụng bộ lọc Kalman trong
hệ thống dẫn đ-ờng điện tử

Chuyên ngành : Điện tử
Luận văn thạc sÜ : ĐiƯn tư viƠn th«ng
Ng-êi h-íng dÉn khoa häc:
TS. Ngun Qc Trung

Hµ néi - 2004

I

Các Ký hiệu
Ký hiệu

ý nghĩa

x

Vi phân theo thời gian của tham số vị trí x. Trong trờng hợp ở đây có đơn vị là [m/s]

x

Vi phân cấp hai theo thời gian của tham số vị trí x. Đơn vị là [m/s2]

F

Tuyến tính hoá phơng trình hệ thống cho bởi F

aN

Tham số gia tốc theo h-ớng Bắc trong hệ toạ độ dẫn đờng [m/s2]

aE

Tham số gia tốc theo hớng Đông trong hệ toạ độ dẫn đờng [m/s2]

aD

Tham số gia tốc theo hớng xuống dới trong hệ toạ độ dẫn đờng [m/s2]

xk

Phần tử thứ k của một chuỗi, x=[x1,x2,...xn]T

bi

Biểu thị độ sai lệch

c0

Vận tốc truyền ánh sáng 299792458[m/s]

cij

Biểu thị phần tử của ma trận C. i,j=1-3.

Cbn

Ma trận chuyển đổi từ hệ toạ ®é dÉn ®-êng sang hƯ to¹ ®é khung

Cnb

Ma trËn chun ®ỉi tõ hƯ to¹ ®é khung sang hƯ to¹ ®é dẫn đ-ờng

Di

Độ dịch tần Doppler ứng với vệ tinh thứ i

E=[e0,e1,e2,e3

Các tham số biểu thị theo Quaternion

]
f

Ma trận hệ thống trong miỊn thêi gian liªn tơc, x =fx

ge

Vector gia tèc trọng tr-ờng [m/s2]

Iixi

Ma trận đơn vị kích th-ớc ixi

K

Ma trận hệ số khuếch đại Kalman

L

Vector nhìn thẳng từ vị trí a đến vị trí b, l=(b-a)/|b-a|

L1,L2

Tần số làm việc của hƯ thèng GPS, L1=1575,42[Mhz], L2=1227,60[Mhz].

0ixj

Ma trËn 0 víi i hµng và j cột

p

Vận tốc quay đo đ-ợc xung quanh trục x của hệ toạ độ khung [rad/s]

q

Vận tốc quay đo đ-ợc xung quanh trục y của hệ toạ độ khung [rad/s]

r

Vận tốc quay đo đ-ợc xung quanh trục z của hệ toạ độ khung [rad/s]


II
P

Ma trận hiệp biến trạng thái

Q

Ma trận hiệp biến xử lý

R

Ma trận hiệp biến phép đo

Re

Bán kính xích đạo của trái đất, 6379137[m]

t

Biến biểu thị thời gian [s]

v

Biến biểu thị vận tốc [m/s]

vD

Tham số vận tốc theo ph-ơng thẳng đứng trong hệ toạ độ dẫn đ-ờng [m/s]

vE

Tham số biểu thị vận tốc theo h-ớng Đông trong hệ toạ độ dẫn đ-ờng [m/s]

vN

Tham số biểu thị vận tốc theo h-ớng Bắc trong hệ toạ độ dẫn đ-ờng [m/s]

z

Vector đo l-ờng



Gia tốc gãc,  =    [rad/s2]

(u-v)

Hµm xung Delta rêi rạc, (0)=1,(n)=0 n0.

ion

Thời gian trễ trong tầng điện ly [s]

trop

Thời gian trễ trong tầng đối l-u [s]

u

Sai số phân do sai lệch của đồng hồ thiết bị thu[s]

u

Độ dịch của đồng hồ thiết bị thu [s/s]

i

Sai số về khoảng cách giả đến vệ tinh thứ i [m]

i

Sai số về tốc độ biến đổi khoảng cách giả đến vệ tinh thứ i [m/s]

t

Thời gian giữa các mẫu [s]



Kinh độ [rad]



B-ớc sóng [m]



Vận tốc góc, ở đây

e

Vận tốc quay của trái đất, 7,2921151467E-5[rad/s]



Vĩ độ [rad]



Góc nghiêng cánh [rad]



Vận tốc thay đổi góc nghiêng cánh, theo cách biểu thị góc Euler [rad/s]

Ma trận chuyển đổi trạng thái rời rạc theo thời gian

c

Ma trận chuyển đổi trạng thái rời rạc theo thời gian của tham số trạng thái đồng hồ

[,, ] [rad/s]

GPS.


Góc h-ớng, theo ph-ơng chính Bắc thì bằng 0, [rad/s]


III


Vận tốc thay đổi góc h-ớng, [rad/s]



Khoảng cách giả [m]



Vận tốc thay đổi khoảng cách giả [m/s]

Độ lệch chuẩn



Góc ngẩng [rad]



VËn tèc thay ®ỉi gãc ngÈng [rad/s]


IV

Các từ viết tắt
Từ viết tắt

ý nghĩa

AAV

Ph-ơng tiện hàng không vận hành tự động (Autonomous Aerial Vehicle)

ADC

Bộ chuyển đổi t-ơng tù - sè (Analog to Digital Converter)

C/A

M· C/A (Course/Acquisition)

DAC

Bé chuyÓn ®ỉi sè - t-¬ng tù (Digital to Analog Converter)

DGPS

HƯ thèng ®Þnh vÞ vƯ tinh vi sai (Differential GPS)

DOP

Tham sè suy giảm độ chính xác (Dilution of Precision)

ECEF

Hệ toạ độ gắn với trái đất (Earth Centered Earth Fixed Frame)

ECI

Hệ toạ độ quán tính tâm trái đất (Earth Centered Inertial Frame)

EKF

Bộ lọc Kalman mở rộng (Extended Kalman Filter)

GDOP

Tham số suy giảm độ chính xác về khoảng cách hình học (Geometric DOP)

GPS

Hệ thống định vị toàn cầu (Global Positioning System)

IMU

Khối đo quán tính (Inertial Measuring Unit)

INS

Hệ thống dẫn đ-ờng quán tính (Inertial Navigation System)

KF

Bộ lọc Kalman (Kalman Filter)

PDOP

Tham số suy giảm độ chính xác về vị trí (Position DOP)

PPS

Dịch vụ dẫn đ-ờng chính xác (Precise Positioning Service)

PSD

Mật độ phổ công suất (Power Spectrum Density)

RMS

Giá trị trung bình bình ph-ơng (Root Mean Square)

SA

Chế độ làm việc có sai số chủ định của hệ thống GPS (Selective Availability)

SPS

Dịch vụ dẫn đ-ờng chuẩn (Standard Positioning Service)

TAS

Vận tốc gió (True Air Speed)

TDOP

Tham số suy giảm độ chính xác về thời gian (Time DOP)

UAV

Máy bay không ng-ời lái (Unmanned Aerial Vehicle)

VDOP

Tham số suy giảm độ chính xác theo ph-ơng đứng (Vertical DOP)

WGS-84

Hệ toạ độ trắc địa toàn cầu ra đời năm 1984 ( World Geodetic System 1984)

ZOH

Giữ mẫu kh«ng (Zero and Hold)


V

Danh mục các bảng

Bảng 4.1. Các tham số chuyển đổi

55

Bảng 4.2. Các tham số tính toán vị trí của thiết bị thu

58

Bảng 5.1. Các ph-ơng trình cập nhật thời gian của bộ lọc Kalman rời rạc
75
Bảng 5.2. Ph-ơng trình cập nhật kết quả đo của bộ lọc Kalman rời rạc

75

Bảng 5.3. Ph-ơng trình cập nhật thời gian của bộ lọc Kalman EKF
81
Bảng 5.4. Ph-ơng trình cập nhật phép đo của bé läc Kalman EKF

81


VI

Danh mục các hình

Hình 2.1. Hệ toạ độ khung B (Body Frame)

6

Hình 2.2. Hệ toạ độ vùng NED

6

Hình 2.3. Hệ toạ độ ECI và ECEF

7

Hình 2.4. Trục quay của ph-ơng tiện

9

Hình 2.5. Hình vẽ biểu thị a) góc nghiêng cánh, b) góc ngẩng và c) góc h-ớng

10

Hình 2.6. Góc nghiêng cánh

11

Hình 2.7. Góc ngẩng (Pitch)

11

Hình 2.8. Góc h-ớng mũi (Yaw)

12

Hình 2.9. H-ớng chuyển động thực của hệ thống

13

Hình 2.10. Góc lệch giữa h-ớng chuyển động và h-ớng mũi hệ thống()

14

Hình 2.11. Các vector hệ toạ độ tích chéo với [p,q,r]T

15

Hình 2.12. Giản đồ khối biểu diễn quá trình tính ma trận Cbn

16

Hình 2.13. Sơ đồ khối biểu thị quá trình biểu diễn góc Euler

20

Hình 2.14. Sơ đồ khối biểu thị quá trình biểu diễn Quaternion

23

Hình 2.15. Vector tạo ra khi quay vector quanh trục q

24

Hình 3.1. Mô hình làm việc của cảm biến gia tốc

31

Hình 3.2. Mô hình làm việc của cảm biến quay

32

Hình 3.3. Vị trí lắp đặt khối IMU trên hệ thống

35

Hình 3.4. Cấu trúc vị trí bộ cảm biến Tetrad

36

Hình 3.5. Cấu trúc phần cứng bộ cảm biến Tetrad

36

Hình 3.6. Cấu trúc a)Strapdown và b)Gimbal

38

Hình 3.7. Mô h×nh xư lý tÝn hiƯu trong hƯ thèng Gimbal

39

H×nh 3.8. Mô hình xử lý tín hiệu trong hệ thống Strapdown

40

Hình 3.9. Mô hình tính toán trong hệ thống INS

42

Hình 3.10. Vector gia tèc h-íng tÊm

43


VII
Hình 4.1. Quĩ đạo của các vệ tinh trong hệ thống GPS

46

Hình 4.2. Nguyên tắc xác định vị trí của thiết bị thu

47

Hình 4.3. Vị trí của thiết bị thu trong tr-ờng hợp không lý t-ởng

48

Hình 4.4. Mô hình truyền sóng của tín hiệu GPS

49

Hình 4.5. ảnh h-ởng của hiện t-ợng truyền nhiều đ-ờng lên hàm t-ơng quan mà C/A

51

Hình 4.6. Mô hình biểu thị sai số của đồng hồ thạch anh

54

Hình 4.5. Thiết bị thu GPS nhận tín hiệu từ bốn vệ tinh khác nhau

58

Hình 4.6. Sự ảnh h-ởng của hiệu ứng Doppler
62
Hình 4.7. Khoảng cách giả và vận tốc khoảng cách giả

62

Hình 4.8. Vị trí Anten trên máy bay

63

Hình 5.1. Chu trình làm việc của bộ lọc Kalman rời rạc

75

Hình 5.2. Biểu đồ giải thuật làm việc của bộ lọc Kalman

76

Hình 5.3. Sơ đồ mô tả quá trình làm việc của bộ lọc Kalman

76

Hình 5.4. Nguyên lý làm việc của bộ lọc Kalman mở rộng

82

Hình 5.5. L-u đồ thuật toán làm việc của bộ lọc Kalman mở rộng EKF

83

Hình 6.1. Mô hình cấu trúc có hồi tiếp

85

Hình 6.2. Mô hình cấu trúc không có hồi tiếp

86

Hình 6.3. Sơ đồ khối cấu trúc hệ thống GPS ghép trên ph-ơng tiện mặt đất

87

Hình 6.4. Cấu trúc ghép lỏng GPS/INS

89

Hình 6.5. Sơ đồ cấu trúc hệ thống GPS/INS ghép chặt

91

Hình 7.1. Mô hình mô phỏng mối quan hệ giữa góc Euler và Quaternion

93

Hình 7.2. Kết quả mô phỏng

94

Hình 7.3. Kết quả mô phỏng thuật toán lọc Kalman đối với hằng số, R=0.1

95

Hình 7.4. Kết quả mô phỏng thuật toán lọc Kalman đối với hằng số, R=5

96

Hình 7.5. Kết quả mô phỏng thuật toán lọc Kalman đối với hằng số, R=0.001

97

Hình 7.6. Kết quả mô phỏng thuật toán lọc Kalman đối với tham số biến đổi, R=0.1

98

Hình 7.7. Kết quả mô phỏng thuật toán lọc Kalman với tham số biến đổi, R=5

99

Hình 7.8. Kết quả mô phỏng thuật toán lọc Kalman với tham số biến đổi, R=0.001

99

Hình 7.9. Mô hình -ớc l-ợng khoảng cách sử dụng bộ lọc EKF

101

Hình 7.10. Quĩ đạo chuyển động thực tế và -ớc l-ợng của ph-ơng tiện

102

Hình 7.11. Sai số -ớc l-ợng khoảng cách

103


VIII
Hình 7.12. Vị trí thực tế, vị trí đo và vị trí -ớc l-ợng

104


1

Ch-ơng 1 - giới thiệu

Dẫn đ-ờng đà xuất hiện từ rất lâu d-ới các dạng hình thái khác nhau. Loài chim,
loài ong và hầu nh- tất cả mọi loài động vật khác trong tự nhiên đều có khả năng dẫn

đ-ờng bẩm sinh. Với con ng-ời, hình thái dẫn đ-ờng sơ khai nhất là dẫn đ-ờng thiên
văn, tức là dựa vào vị trí của mặt trời và các vì sao để xác định ra ph-ơng h-ớng. Trải
qua hàng triệu năm cùng víi sù ph¸t triĨn cđa khoa häc kü tht con ng-ời đà phát
minh ra các ph-ơng thức dẫn đ-ờng mới, với độ chính xác và tính sẵn sàng cao hơn
rất nhiều. Hiện nay tồn tại rất nhiều các ph-ơng thức dẫn đ-ờng khác nhau nh- định
vị bằng năng l-ợng nhiệt, quang, sóng siêu âm, sóng điện từ... Tuy nhiên, -u điểm
hơn hẳn vẫn là ph-ớng pháp dẫn đ-ờng điện tử. Ph-ơng pháp dẫn đ-ờng điện tử dựa
trên cơ sở của các hệ thống tính toán, điều khiển kết hợp với các thiết bị thu phát
sóng điện từ để thực hiện quá trình xác định vị trí, điều khiển và dẫn đ-ờng cho hệ
thống chuyển động đúng quĩ đạo.
Trên thực tế, bản thân hệ thống dẫn đ-ờng điện tử cũng có rất nhiều các hệ thống
khác nhau. Do đặc thù riêng của mỗi hệ thống nh- : kỹ thuật sử dụng trong hệ thống,
độ chính xác, tính sẵn sàng và khả năng áp dụng vào thực tế nên trong khuôn khổ của
luận văn tác giả chỉ đề cập đến hai hệ thống hiện nay đang đ-ợc các nhà khoa học
trên thế giới quan tâm và phát triển nhiều nhất.
Thứ nhất, đó là hệ thống dẫn đ-ờng quán tính INS (Inertial Navigation System).
Hệ thống dẫn đ-ờng quán tính làm việc dựa trên cơ sở của các định luật về chuyển
động của Newton. Bằng các bộ cảm biến gia tốc hệ thống INS có thể tự xác định
đ-ợc tham số gia tốc chuyển ®éng cđa hƯ thèng, tõ ®ã tÝnh to¸n ra c¸c tham số
chuyển động khác nh- vận tốc, vị trí, độ cao. -u điểm của hệ thống INS đó chính là
tính bí mật cao, có thể làm việc trong môi tr-ờng khắc nghiệt (không thể hoặc khó
thực hiện việc truyền sóng điện từ). Vì vậy hệ thống INS th-ờng đ-ợc sử dụng trong
các hệ thống dẫn đ-ờng của quân sự, hệ thèng dÉn ®-êng vị trơ hay hƯ thèng dÉn
®-êng cho tàu ngầm.


2
Thứ hai, đó là hệ thống dẫn đ-ờng vệ tinh GPS. Trên thế giới tồn tại rất nhiều hệ
thống dẫn ®-êng vƯ tinh kh¸c nhau nh-ng hƯ thèng GPS nỉi bật hơn cả bởi tính phổ
biến và độ chính xác của nó. Hệ thống GPS làm việc dựa trên cơ së thu nhËn tÝn hiƯu

ph¸t tõ c¸c vƯ tinh chun động trên quĩ đạo tham chiếu chuẩn. Trên cơ sở các tín
hiệu thu nhận đ-ợc này thiết bị thu GPS sẽ tính toán ra khoảng cách đến các vệ tinh
tham chiếu rồi từ đó xác định ra vị trí của mình. Với đặc tính chính xác và tính sẵn
sàng cao nên hệ thống GPS đ-ợc dùng trong rất nhiều các hệ thống dẫn đ-ờng khác
nhau nh- giao thông đ-ờng bộ, hàng hải và đặc biệt là trong lĩnh vực hàng không.
Hai hệ thống dẫn đ-ờng điện tử đề cập ở trên có rất nhiều các -u điểm nh-ng vẫn
tồn tại những nh-ợc điểm khác nhau. Ví dụ nh-, hệ thống GPS yêu cầu phải làm việc
trong điều kiện thiết bị thu GPS và các vệ tinh nằm trong tầm nhìn thẳng. Số l-ợng vệ
tinh quan sát của thiết bị thu tại mọi thời điểm tối thiểu là 4 vệ tinh. Trong khi đó hệ
thống INS làm việc dựa trên cơ sở thông tin đ-a đến từ các bộ cảm biến, do đó yêu
cầu về các bộ cảm biến phải có độ chính xác cao. Ngoài ra, sai số của hệ thèng INS
cã tÝnh chÊt tÝch luü theo thêi gian, v× thế giá thành của một hệ thống INS có chất
l-ợng vừa phải cũng là rất cao.
Để giảm những yêu cầu làm việc hết sức khắt khe đối với mỗi hệ thống dẫn đ-ờng,
giảm chi phí cho mỗi hệ thống dẫn đ-ờng nh-ng vẫn đảm bảo đ-ợc tính chính xác
của hệ thống ng-ời ta đà đ-a ra mô hình hệ thống ghép. Hệ thống ghép đ-ợc cấu tạo
từ hai hệ thống riêng lẻ GPS, INS trên cơ sở thuật toán của bộ lọc Kalman. Tuỳ thuộc
vào yêu cầu của từng hệ thống dẫn đ-ờng khác nhau mà ta sử dụng các cấu trúc ghép
khác nhau. Với sự kết hợp này, các nh-ợc điểm của mỗi hệ thống hầu nh- đà đ-ợc
khắc phục hoàn toàn, do đó tính năng của hệ thống dẫn đ-ờng đ-ợc cải thiện rõ rệt.
Nội dung của luận văn này đề cập đến những vấn đề xoay quanh hệ thống ghép
GPS và INS với sự trợ giúp của bộ lọc Kalman. Trong luận văn tác giả trình bày về
nguyên tắc làm việc của các hệ thống, nguyên tắc ghép của hệ thống. Phần cuối của
luận văn là ch-ơng trình mô phỏng nhằm đánh giá đặc tính của hệ thống dẫn đ-ờng
ghép.
Luận văn với tên gọi : ứng dụng bộ lọc Kalman trong hệ thống dẫn đ-ờng
điện tử được chia thành các phần như sau:


3

Ch-ơng 2. Ph-ơng pháp biểu diễn vị trí trong không gian ở các hệ thống dẫn
đ-ờng. Khái niệm về các hệ toạ độ sử dụng trong dẫn đ-ờng. Biểu diễn và tham số cơ
bản để biểu diễn vị trí của hệ thống trong không gian. L-ợc đồ tính toán chuyển đổi
giữa các hệ toạ độ tham chiếu khác nhau.

Ch-ơng 3. Hệ thống dẫn đ-ờng quán tính INS. Nguyên lý làm việc của hệ thống
dẫn đ-ờng quán tính INS. Cấu hệ thống dẫn đ-ờng INS. Ph-ơng trình tính toán và các
sai số trong hệ thống INS.
Ch-ơng 4. Hệ thống dẫn đ-ờng vệ tinh GPS. Cấu trúc của hệ thống GPS. Nguyên
tắc làm việc của hệ thống GPS. Ph-ơng trình tính toán vị trí, sai số và hạn chế sai số
trong hệ thống GPS.
Ch-ơng 5. Bộ lọc Kalman rời rạc. Ph-ơng trình trạng thái của hệ thống liên tục
theo thời gian, rời r¹c theo thêi gian. HƯ thèng tun tÝnh, phi tun. Thuật toán
Kalman và nguyên lý làm việc của bộ lọc Kalman. Bé läc Kalman më réng.

Ch-¬ng 6. øng dơng bé lọc Kalman trong các hệ thống dẫn đ-ờng. Cấu trúc và
nguyên lý làm việc của các hệ thống dẫn đ-ờng cã sư dơng bé läc Kalman. HƯ thèng
ghÐp GPS/INS.
Ch-¬ng 7. Mô phỏng. Mục đích và cấu trúc ch-ơng trình mô phỏng. Kết quả mô
phỏng.
Đây là một đề tài còn hết sức mới mẻ, hiện đang nhận đ-ợc sự quan tâm của rất
nhiều nhà khoa học trên thế giới và nó vẫn còn đang trong thời gian nghiên cứu và
phát triển. Việc kiểm nghiệm và triển khai thực tế để đánh giá hệ thống là hết sức khó
khăn. Kết quả đ-a ra ở đây là dựa trên cơ sở mô phỏng và kết quả của một số các bài
báo khoa học đà đ-ợc công bố.


4
Tác giả xin bày tỏ lòng biết ơn sâu sắc ®èi víi sù gióp ®ì cđa TS. Ngun Qc
Trung, c¸c thầy cô giáo khoa Điện tử Viễn thông tr-ờng Đại học Bách Khoa Hà Nội

cùng bạn bè đồng nghiệp đà giúp đỡ hoàn thành luận văn này.
Trong quá trình làm luận văn chắc chắn không tránh khỏi những sơ suất về nội
dung và hình thức, rất mong nhận đ-ợc sự góp ý của độc giả.


5

Ch-ơng 2 - Biểu diễn tọa độ vị trí

2.1. Hệ toạ độ tham chiếu

Một khung (frame) đ-ợc gọi là một hệ toạ độ tham chiếu. Các hệ toạ độ tham
chiếu đ-ợc dùng để biểu diễn và mô tả tính chất chuyển động của ph-ơng tiện có
gắn hệ thống INS. Khi thực hiện quá trình dẫn đ-ờng ta cần ít nhất là hai hệ toạ độ.
Một hệ toạ độ dùng để biểu diễn cấu trúc của ph-ơng tiện cần dẫn đ-ờng(vehicle),
và một hệ toạ độ dùng để biểu diễn quá trình dẫn đ-ờng(map). Thông th-ờng có
năm hệ toạ độ tham chiếu khác nhau để biểu diễn tính chất chuyển động của ph-ơng
tiện.

2.1.1. Hệ toạ độ khung (Body Frame, kí hiệu là B)

Đây là hệ toạ độ cơ sở dùng biểu diễn cho các cảm biến quán tính. Các trục của
hệ toạ độ này t-ơng tự nh- cấu trúc khung của ph-ơng tiện(vehicle). Hệ toạ độ này
đ-ợc coi nh- hệ toạ độ tham chiếu có các trục trực giao gắn với ph-ơng tiện cần dẫn
đ-ờng. Trục xB có h-ớng chỉ về phía tr-ớc, trục yB có h-ớng vuông góc về bên phải
so với trục xB, còn trục zB vuông góc với mặt phẳng tạo bởi hai trục xB, yB và có
h-ớng chỉ xuống phía d-ới. Thông th-ờng hệ toạ độ này có gốc trùng với tâm khối
của ph-ơng tiện hoặc đ-ợc xác định bởi giao của các trục của đồng hồ đo gia tèc
hay c¸c con quay håi chun. VËn tèc cđa ph-ơng tiện trong hệ toạ độ khung đ-ợc
biểu diễn bằng c¸c tham sè [u, v, ].

6

Hình 2.1. Hệ toạ độ khung B (Body Frame)

2.1.2. Hệ toạ độ vùng NED (Local Level Frame, kí hiệu là NED)
Đây là hệ toạ độ trực giao dùng cho quá trình dẫn đ-ờng. Gốc đ-ợc xác định
nh- hệ toạ độ khung, trïng víi t©m khèi cđa hƯ thèng. Trơc N chỉ theo h-ớng bắc,
trục E chỉ theo h-ớng đông và vu«ng gãc víi trơc N, trơc D h-íng xng d-íi theo
ph-ơng của gia tốc trọng tr-ờng tại vị trí hiện tại của ph-ơng tiện. Hai trục N và E
tạo thành mặt phẳng tiếp xúc với bề mặt trái đất tại vị trí xét.
Z
N
E

Đ-ờng kinh tuyến gốc

D
y

z

Y
x

X

Mặt phẳng xích đạo

Hình 2.2. Hệ toạ độ vùng NED


7

2.1.3. Hệ toạ độ trắc địa toàn cầu WGS-84 (kí hiệu là W)
Đây là một hệ toạ độ cầu, sử dụng hai góc và độ cao để biểu diễn vị trí của một
điểm. Thông th-ờng hệ toạ độ này đ-ợc sử dụng trong tr-ờng hợp dẫn đ-ờng ở
khoảng cách xa. Các vị trí ứng với mực n-ớc biển đ-ợc biểu thị với độ cao bằng
không.


Kinh độ : ứng với đ-ờng kinh tuyến gốc(đ-ờng kinh tuyến đi qua Luân Đôn,
Anh) góc kinh độ bằng không. Góc kinh độ có giá trị nằm trong khoảng từ 03600 và đ-ợc tính quay theo h-ớng đông. Giá trị của góc kinh độ biểu thị độ
lệch về h-ớng Đông hay h-ớng Tây so với đ-ờng kinh tuyến gốc.



Vĩ độ : Tại mặt phẳng xích đạo thì vĩ độ có giá trị bằng không. Góc vĩ độ
bằng 900 ứng với điểm cực bắc, và bằng -900 ứng với điểm cực nam. Giá trị
của góc vĩ độ biểu thị độ lệch so với mặt phẳng xích đạo.



Độ cao h: Biểu thị độ cao so với mực n-ớc biển.



Các tham số đ-ợc định nghĩa trong hệ toạ độ WGS-84:
➢ B¸n kÝnh trơc lín: a = 6378,137(km)

 = 7292115,1467E-10(rad/s)
➢ Vận tốc góc của trái đất:
e

Hằng số gia tèc träng tr-êng:  = 3986005E8 (m3/s2)
Tõ c¸c tham sè này ta có thể tính ra đ-ợc hệ số uốn cong của bề mặt trái đất và
bán kính trục nhỏ.
2.1.4. Hệ toạ độ quán tính ECI (Earth Center Inertial Frame, kí hiệu là I)
Hệ toạ độ này dùng để tham chiếu tham số quán tính. Đây là một hệ toạ ®é
kh«ng quay, kh«ng cã gia tèc øng víi kh«ng gian quán tính. Bằng cách bỏ qua sự
chuyển động của trái đất xung quanh mặt trời, gốc của hệ toạ độ I đ-ợc giả thiết đặt
tại tâm khối của trái đất. Hai trục xI và yI nằm trên mặt phẳng xích đạo của trái đất.
Trục xI chỉ theo h-ớng một ngôi sao đ-ợc gọi là điểm phân xuân (Vernal Equinox).
Trục yI tạo với hai trục xI và zI một hệ toạ ®é vu«ng gãc víi h-íng cđa trơc yI n»m


8
về phía bến phải của mặt phẳng xz. Trục zI trùng với trục quay của trái đất và có
h-ớng dọc theo cực Bắc.
ZECEF = ZECI



N
Đ-ờng kinh tuyến gốc

Mặt phẳng xích đạo

YECEF

YECI

XECI
Điểm phân xuân

(t-t0)

XECEF

Hình 2.3. Hệ toạ độ ECI và ECEF
2.1.5. Hệ toạ độ ECEF ( Earth Center Earth Frame, kí hiệu là E hay
ECEF):
Hệ toạ độ này đ-ợc gắn cố định với trái đất và chuyển động theo sự quay của trái
đất. Hệ toạ độ E th-ờng đ-ợc sử dụng trong hệ thống GPS và đ-ợc gọi là hệ toạ độ
WGS-84. Thông th-ờng các tham số đo của hệ thống GPS đ-ợc biểu diễn trong hệ
toạ độ này. Do tính chất của các trục xE, yE và zE trong hệ toạ độ E nên đôi khi nó
đ-ợc gọi là hệ toạ độ Decac trái đất.
Đây là một hệ toạ độ trực giao với gốc toạ độ đặt tại tâm khối của trái đất. Trục
xE chỉ theo h-ớng kinh tuyến gốc (có giá trị kinh độ bằng 0) và nằm trên mặt phẳng
xích đạo. Trục zE trùng với trục quay của trái đất và có h-ớng theo cực Bắc. Trục yE
hợp với hai trục xE và zE tạo thành một hệ toạ độ trực giao và có h-ớng về phía bên
phải của mặt phẳng tạo bởi hai trục xE và zE.


9

2.2. Tính quay của hệ thống
Trong phần này ta đề cập đến ba loại quay khác nhau của hệ thống. Đó là: quay
theo trục dọc ph-ơng tiện hay còn gọi là lắc ngang - thay đổi góc nghiêng cánh
(roll), quay theo trục ngang của ph-ơng tiện hay còn gọi là lắc dọc - thay đổi góc

ngẩng (pitch) và quay theo trục thẳng đứng hay còn gọi là xoay - thay đổi góc h-ớng
(yaw). Ngoài ra trong phần này cũng đề cập đến các ph-ơng pháp biểu diễn góc
khác nhau nh- lµ biĨu diƠn theo gãc Euler, ma trËn Cbn vµ biĨu diƠn theo gãc sè
phøc suy réng, gäi lµ gãc Quaternion.
Trục đứng (Yaw)

Trục ngang
h-ớng mũi (Roll)

Trục ngang
h-ớng cánh (Pitch)
Hình 2.4. Trục quay của ph-ơng tiện
Ta đứng trong hệ tạo độ dẫn đ-ờng (đứng trên mặt đất) và quan sát sự chuyển
động của ph-ơng tiện. Vấn đề chúng ta cần quan tâm ở đây chính là tham số gia tốc
và tốc độ chuyển động của ph-ơng tiện quan sát. Đây chính là các thông tin cần
thiết để thực hiện qúa trình dẫn đ-ờng.
Trên ph-ơng tiện chuyển động ta gắn khối IMU để đo các giá trị tham số gia tốc
[ax, ay, az] vµ tham sè vËn tèc quay [p, q, r]. Ta cần thực hiện việc chuyển đổi các
vector này sang hệ toạ độ dẫn đ-ờng. Ma trận chuyển đổi đ-ợc sử dụng ở đây chính
là ma trận Cbn. Kí hiệu bn ở đây biểu thị quá trình chuyển đổi các vector tõ hƯ to¹


10
độ khung(Body frame) sang hệ toạ độ dẫn đ-ờng(Navigation frame). Tính chất quay
của ph-ơng tiện đ-ợc biểu thị bởi ba góc, gọi là các góc Euler [, , ]. Trong kỹ
thuật hàng không, các góc này đ-ợc gọi là góc nghiêng cánh(roll), góc ngẩng(pitch)
và góc h-ớng(yaw). Tính chất quay đ-ợc biểu diễn trên cơ sở hệ toạ độ trục.


11

b)
b)

a)

c)
Hình 2.5. Hình vẽ biểu thị a) góc nghiêng cánh, b) gãc ngÈng vµ c) gãc h-íng

2.2.1. TÝnh chÊt quay của góc nghiêng cánh (Roll)
Tính chất quay này dùng để biểu thị sự nghiêng của cánh lên hoặc xuống. Góc
nghiêng này đ-ợc xác định so với đ-ờng trung tâm của khung ph-ơng tiện. Góc
nghiêng cánh có giá trị d-ơng khi h-ớng quay là cùng chiều kim đồng hồ. Khi góc
nghiêng cánh =0 thì điều này đồng nghĩa với trạng thái hai cánh cân bằng và song
song với mặt phẳng ngang.
Hình sau biểu thị trong không gian ba chiều một máy bay có góc nghiêng cánh
=300.


12
Đông(East)




H-ớng xuống d-ới(Down)

Hình 2.6. Góc nghiêng cánh
Ma trận biểu thị tính chất quay của góc nghiêng cánh xung quanh trục x có dạng
ph-ơng trình (2.1) sau:

0
0
1

R( ) 0 cos sin  
0  sin  cos 

(2.1)

2.2.2. Gãc ngẩng (Pitch)
Trong kỹ thuật hàng không thì góc ngẩng dùng để biểu thị h-ớng mũi của máy
bay là lên cao hay xuống thấp. Khi máy bay ở trạng thái cân bằng, nghĩa là song
song với mặt phẳng ngang, góc ngẩng có giá trị bằng 0. Góc ngẩng có giá trị d-ơng
khi mũi máy bay h-ớng lên trên.
Hình sau biểu diễn trong không gian ba chiều một máy bay có góc ngẩng bằng
d-ơng 300.




H-ớng xuống d-ới(Down)

Hình 2.7. Góc ngẩng (Pitch)

Bắc(North)


13

Biểu thức toán học d-ới dạng ma trận biểu thị tÝnh chÊt quay cđa gãc chói xung

quanh trơc y cã dạng ph-ơng trình (2.2):
cos
R( ) 0
sin

0  sin  
1
0 
0 cos 

(2.2)

2.2.3. Gãc h-íng cđa khung ph-¬ng tiƯn  (Yaw)
TÝnh chÊt quay cđa gãc  của ph-ơng tiện dùng để biểu thị sự lệch của mũi
ph-ơng tiện sang trái hay sang phải xung quanh trục theo h-ớng vector gia tốc trọng
tr-ờng. Cần phân biệt góc này với góc h-ớng (heading) biểu thị h-ớng chuyển động
của hệ thống trong hệ toạ độ dẫn đ-ờng. Góc h-ớng của khung ph-ơng tiện và góc
h-ớng chuyển động th-ờng là bằng nhau. Sự khác nhau chỉ xảy ra khi hệ thống thực
hiện chuyển động giạt (sideslip). Chuyển động sideslip xảy ra do sự tác động của
gió hay tính chất động häc cđa hƯ thèng. Gãc h-íng b»ng kh«ng øng víi tr-ờng hợp
hệ thống đang xét chuyển động theo h-ớng chính Bắc. Giá trị độ lớn của góc h-ớng
đ-ợc tính theo chiỊu kim ®ång hå xung quanh trơc cđa vector gia tốc trọng tr-ờng.
Hình (2.8) biểu thị trong không gian ba chiều một máy bay có góc h-ớng bằng
300.
Bắc(North)



Đông(East)

Hình 2.8. Góc h-íng mịi (Yaw)
Gãc h-íng cã thĨ biĨu thÞ b»ng sù quay cđa khung hƯ thèng xung quanh trơc z.


14
 cos
R( )   sin
 0

sin
cos
0

0
0
1

(2.3)

2.2.4. Chun ®éng thùc (true attack) và góc chuyển động (attack angle)
Nếu h-ớng chuyển ®éng cđa hƯ thèng kh«ng trïng víi h-íng gãc ngÈng, khi đó
sẽ có sự sai lệch nhỏ giữa h-ớng gió và góc ngẩng. Góc giạt (sideslip angle) đ-ợc kí
hiệu là , khi đó độ lớn của góc giạt đ-ợc tính theo c«ng thøc sau:
 =   arctan(


u2  v2

(2.4)

)

u

H-íng gió(TAS)

w

Hình 2.9. H-ớng chuyển động thực của hệ thống
2.2.5. Chuyển ®éng Sideslip
NÕu gãc h-íng chun ®éng cđa hƯ thèng (heading) khác với h-ớng mũi của
ph-ơng tiện(yaw) thì có nghĩa là hệ thống đang trong trạng thái chuyển động
sideslip. Góc chuyển động sideslip đ-ợc ký hiệu là , biểu thị sự chênh lệch giữa
h-ớng chuyển động và h-ớng mũi của hệ thèng.
v
u

    heading    ac tan 

(2.5)


15
u


H-ớng gió(TAS)

v

Hình 2.10. Góc lệch giữa h-ớng chuyển động và h-íng mịi hƯ thèng().

2.3. VËn tèc quay cđa khung
Tèc ®é quay của khung đ-ợc đo bởi khối IMU trong hệ toạ độ trục chính là
[p,q,r]T. Bằng cách thực hiện nhân chéo(crossproduct) với các vector riêng biểu diễn
trong hệ toạ độ khung ta sẽ thu đ-ợc tham số của ma trận biĨu diƠn vËn tèc quay.
NÕu ta cã mét vector [1, 1, 1]T thì kết quả nhân chéo với [p, q, r]T sẽ là [q-r,r-p,p-q]T.
Vector [1,1,1]T là một tổ hợp tuyến tính của các vector cơ sở [1,0,0]T, [0,1,0]T và
[0,0,1]T(những vector cơ sở này có thể đ-ợc sắp xếp thành ma trận I3x3). Tích chéo
của các vector này với vận tốc quay sẽ cho ta kết quả là một ma trận bất đối xứng
nh- trong ph-ơng trình (2.6). Trong hình (2.11) phÝa d-íi ta cã vector [1,1,1]T, c¸c
vËn tèc quay [p,q,r]T xung quanh các trục X, Y và Z và kết quả chính là vector biểu
thị tốc độ sự thay đổi.
Theo hình (2.11) ta thấy vector vận tốc hoàn toàn có thể đ-ợc tính toán và đ-ợc
biểu diễn bằng ma trận bÊt ®èi xøng sau:
PQR( p, q, r )  I

33

 p  0  r q 
  q    r
0  p
 r   q p
0 

(2.6)