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

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..

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

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

 Vi ph©n cÊp hai theo thêi gian cña tham sè vÞ trÝ x §¬n vÞ lµ [m/s 2 ]

x k PhÇn tö thø k cña mét chuçi, x=[x 1 ,x 2 , x n ] T

E=[e 0 ,e 1 ,e 2 ,e 3

]

C¸c tham sè biÓu thÞ theo Quaternion

II

 Gia tốc góc,  =  [rad/s 2 ]

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

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

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

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ồ

GPS

IV

Các từ viết tắt

DGPS Hệ thống định vị vệ tinh vi sai (Differential GPS)

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

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

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

V

Danh môc c¸c b¶ng

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.10 Góc lệch giữa h-ớng chuyển động và h-ớng mũi hệ thống() 14

VII

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.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.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 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.3 Sơ đồ khối cấu trúc hệ thống GPS ghép trên ph-ơng tiện mặt đất 87

VIII

H×nh 7.12 VÞ trÝ thùc tÕ, vÞ trÝ ®o vµ vÞ trÝ -íc l-îng 104

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 chuyển độ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 tuyến tính, phi tuyến 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 Nguyễn Quốc 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 chuyển 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

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 xuống 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

x y

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ừ 0-

3600 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)

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

➢ 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

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

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

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

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ạ

Trục đứng (Yaw)

Trục ngang h-ớng cánh (Pitch)

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

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

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

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

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

b)

sincos

0

00

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

sin0

cos)

(

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

0sincos

2.2.4 Chuyển độ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( )

2 2

15

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:

0)

,,

p q

p r

q r r

q

p I

r q p

p z

y x

00

00

000

p t

z

t y

t x

00

000exp)

(

)(

)(

(2.8)

(Trong đó C là một hằng số bất kỳ) Nếu ta chỉ xét trong một khoảng thời gian nhỏ t, khi đó góc nghiêng cánh

0

sincos

0

00

1

00

00

000

!

1)

00

00

000exp(

1

3 3

17

2.4 Ma trận Cbn, biểu diễn h-ớng theo cosin

Trong cách biểu diễn này thì các góc đ-ợc biểu diễn ở dạng các phần tử bên trong ma trận Cbn Tốc độ quay là tham biến thông tin đầu vào của ma trận Ta có thể tính các góc từ các phần tử của ma trận Cbn, từ đó có thể xác định vị trí của ph-ơng tiện

Tính chất quay của các góc nghiêng cánh, góc ngẩng và góc h-ớng có thể đ-ợc viết thành chuỗi các sự kiện liên tiếp Kết quả là cho ta là ma trận quay thực hiện việc chuyển đổi một vector trong hệ toạ độ dẫn đ-ờng sang hệ toạ độ khung với các góc [,,] Ma trận quay này đ-ợc gọi là ma trận Cbn và đ-ợc biểu diễn nh- trong ph-ơng trình (2.10)

23 22 21

13 12 11

c c c

c c c

c c c

Ma trận Cbn là một ma trận trực giao có kích th-ớc 3x3 với tổng số phần tử là 9

Ma trận này có tính chất là detCbn = 1 Điều này có nghĩa là giá trị tổng đ-ợc bảo

p q

18 toàn và không có sự thay đổi ở các tham số tỷ lệ Các hàng và các cột của ma trận

Cbn là trực giao với nhau

sinsin

sinsincoscos

sinsin

sincoscos

cossin

cos

cossincossin

sincos

sinsinsin

coscos

cos)

1)

,,

2.4.2 Chuyển đổi sang góc Euler từ các tham số C bn

Nếu ta đã biết ma trận Cbn thì ta hoàn toàn có thể tính toán đ-ợc các góc Euler từ các ph-ơng trình (2.15), (2.16) và (2.17) Các ph-ơng trình này đ-ợc xây dựng từ ph-ơng trình (2.13) Các tham số cij đ-ợc xác định trong ma trận Cbn

coscos

cossinarctan(

)arctan(

coscos

sincosarctan(

)arctan(

2.4.3 Tính toán góc Euler trong điều kiện góc ngẩng lớn

Nếu ta đã có ma trận Cbn với tr-ờng hợp góc ngẩng thoả mãn điều kiện cos0 thì ta không thể sử dụng các ph-ơng trình (2.15), (2.16) và (2.17) đ-ợc vì các

19 ph-ơng trình này vô định Khi đó ta phải sử dụng các ph-ơng trình khác cho quá trình tính toán[3]

c23 - c12 = (sin+1)sin(-) (2.18)

c13 + c22 = (sin+1)cos(-) (2.19)

c23 + c12 = (sin -1)sin(+) (2.20)

c13 - c22 = (sin-1)cos(+) (2.21) Các ph-ơng trình (2.18), (2.19), (2.20) và (2.21) có ba biến ch-a biết, vì vậy hệ ph-ơng trình này ta hoàn toàn có thể giải đ-ợc Nếu ta chia ph-ơng trình (2.18) cho ph-ơng trình (2.19) ta có đ-ợc ph-ơng trình (2.23), ph-ơng trình (2.20) và (2.21) ta thu đ-ợc ph-ơng trình (2.22)

22 13

12 23

c c

c c

12 23

c c

c c

12 23

c c

c c

12 23

c c

c c

12 23

c c

c c

12 23

c c

c c





(2.24.2) Góc  đ-ợc tính theo ph-ơng trình (2.16) ở phần tr-ớc

 = arcsin(-c31) = arcsin(sin) (2.25)

2.4.4 Tốc độ thay đổi của ma trận C bn

Ta đã biết cách tính toán ma trận vận tốc quay, bây giờ ta sẽ tính toán vận tốc thay đổi của ma trận Cbn Ma trận Cbn coi hệ toạ độ khung nh- là hệ toạ độ tham chiếu bởi sự quay luôn đ-ợc thực hiện ở hệ toạ độ khung Vector [p,q,r] biểu thị tốc

độ quay quanh mỗi trục Nh- đã chỉ ra trong phần (2.3), tích chéo có thể đ-ợc viết nh- là một ma trận bất đối xứng Ma trận Cbn đ-ợc nhân với (2.6) và kết quả cho ta một cách để tính tốc độ thay đổi của ma trận Cbn

p q

p r

q r C

Để tính đ-ợc ma trận Cbn lấy tích phân ta có:

p q

p r

q r C

0

(2.27)

2.4.5 Tính tích phân ma trận C bn trong miền thời gian rời rạc

Ph-ơng trình (2.27) là tích phân đ-ợc tính trong miền thời gian liên tục Vì vậy ta cần thực hiện rời rạc hoá để có thể thực hiện đ-ợc bằng phần mềm trên máy tính Sự thay đổi của ma trận Cbn trong khoảng thời gian nhỏ kí hiệu là t = tk+1 - tk đ-ợc tính bằng Cbn.T Sự thay đổi này cần phải đ-ợc công thêm vào giá trị ma trận Cbn(k) thực tế Một tích phân ZOH đơn giản có thể đ-ợc viết nh- sau:

p q

p r

q r k

C T k

1)()

r q

3 sin 1 cos

A A

q

p r

q r

(2.30)

21

2 2 2

2 2

2 2 2

)(

)(

)(

T r q qr

pr

qr r

p pq

pr pq

r q

Nếu ma trận Cbn liên tục đ-ợc cập nhật (quay liên tục) thì sẽ dẫn đến sự thay đổi

hệ số tỷ lệ của các vector hàng Vector hàng lúc này sẽ không thoả mãn điều kiện trực giao với nhau Giả sử ta có ma trận C~bnvới ~ ~T 3x3

bn

bn C I

C  , khi đó ta có thể thực hiện việc chuẩn hoá ma trận nh- sau[3]:

bn bn bn

C  ~ 0,5(~ ~ )~ (2.33)

2.5 Biểu diễn góc Euler

Theo cách biểu diễn bằng góc Euler, tham số vị trí đ-ợc biểu thị trong vector [,,] Các tham số tốc độ quay [p,q,r] cung cấp các thông tin cập nhật cho cho vector [,,] Việc này đ-ợc thực hiện thông qua quá trình lấy tích phân

[e 0 ,e 1 ,e 2 ,e 3 ]

C bn

,,

p q

r

C bn Tính ma trận C bn

Tính các góc dạng phức

[p,q,r]

Vận tốc góc Euler

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

vector

*

Vector sau khi quay

22

2.5.1 Tính các tham số [,,] dựa trên các tham số [p,q,r]

Các phần tử trong ma trận Cbn ta hoàn toàn có thể đ-ợc xác định nếu nh- ta biết

đ-ợc các góc Euler Khi đã có giá trị của các tham số này thì ta có thể tính đ-ợc tốc

độ thay đổi của các góc Euler với các tham số vận tốc quay đóng vai trò của Argument Thực hiện việc chuyển đổi tốc độ quay trong hệ toạ độ khung [p,q,r] sang hệ toạ độ dẫn đ-ờng, kết quả ta sẽ thu đ-ợc vận tốc góc của các góc Euler

0)(0

r q

sin

sincoscos

0

0sin

1

r q

sin0

sincos

0

tancostan

sin1

| pqr

Khi đó các góc Euler có thể đ-ợc tính theo ph-ơng trình (2.38)

sincos

0

tancostan

sin1

Ph-ơng trình (2.39) sẽ không xác định khi sec rơi vào tr-ờng hợp đặc biệt Tuy nhiên khi hệ thống rơi vào tr-ờng hợp này thì các nghiệm xấp xỉ sẽ đ-ợc thay thế, mặc dù ph-ơng trình các nghiệm này không có tính khả thi thực tế[23]

• (•p)sin (qsin rcos )cos (2.40.3)

2.5.2 Tính tích phân góc Euler trong các hệ thống rời rạc theo thời gian

Trong hệ thống dẫn đ-ờng ta cần phải xác định đ-ợc các góc Euler Các con quay trong IMU thực hiện việc đo các vận tốc quay của khung, rồi thực hiện việc chuyển đổi sang các tốc độ góc Euler Để cập nhật dữ liệu cho các góc Euler ta đ-a tham số thay đổi vận tốc góc Euler qua bộ tích phân ZOH (Zero and Hold)

24

T k

2.6 Biểu diễn theo thành phần Quaternion

ý t-ởng của mô hình này là sử dụng bốn biến khác nhau để biểu thị tính chất quay của hệ thống trong không gian ba chiều R3x3 thay vì ba tham số nh- tr-ớc Với cách này ta có thể biểu thị mọi vị trí của hệ thống khá dễ dàng Các vận tốc quay [p,q,r] đ-ợc sử dụng để cập nhật thông số cho các tham số quaternion

Từ mô hình này ta thấy tham số quaternion đ-ợc biểu thị d-ới dạng:

3 2 1

Vận tốc góc Euler

3 2 1

Tốc độ thay đổi C bn [e 0 ,e 1 ,e 2 ,e 3 ]

Tính C bn

bn C

25

E = [e0, e1, e2, e3] (2.42) Gọi u là vector tr-ớc khi quay R3x3 xung quanh trục q và v là vecotr sau khi quay

Kí hiệu  là góc hợp bởi hai vector u và v, khi đó góc  đ-ợc tính theo công thức:

arccos | || |

v u

v u

)(

2)

(2

)(

2)(

)(

2

)(

2)

(2)(

2 3 2 2 2 1 2 0 1

0 3 2 2

0 3 1

1 0 3 2 2

3 2 2 2 1 2 0 3

0 2 1

2 0 3 1 3

0 2 1 2

3 2 2 2 1 2 0

e e e e e

e e e e

e e e

e e e e e

e e e e

e e e

e e e e e

e e e e

e e e

u

q 2

/

 2 /



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

26

2.6.1 Tính các góc Euler theo tham số Quaternion

Các góc Euler có thể đ-ợc tính theo các tham số của góc Quaternion trong ph-ơng trình (2.44) theo mối quan hệ qua ma trận Cbn ở ph-ơng trình (2.13)

))(

2arctan(

)

3 2 2 2 1 2 0

3 2 1 0 33

32

e e e e

e e e e c

2arcsin(

)arctan(c31  e0e2e1e3



))(

2arctan(

)

3 2 2 2 1 2 0

2 1 3 0 11

21

e e e e

e e e e c

2.6.2 Tính tham số quaternion theo các góc Euler

Nếu ta không có các tham số của ma trận Cbn thì ta có thể sử dụng các góc Euler [p,q,r] để tính các phần tử vector quaternion theo các ph-ơng trình (2.46), (2.47), (2.48) và (2.49) sau:

2

sin2

sin2

sin2

cos2

cos2

sin2

cos2

cos2

cos2

cos2

sin2

cos2

sin2

sin2

sin2

sin2

cos2

2.6.3 Tính tham số quaternion theo các tham số của ma trận C bn

Từ ph-ơng trình (2.44) ta có thể tính đ-ợc các thành phần của vector quaternion theo tham số của ma trận Cbn

2 0 33

22 11

2

11

2

1

e c

c c

))(

2)(

2(4

1)(

4

1

1 0 3 2 1

0 3 2 0 23

32 0

1 e e e e e e e e

e c

c e

27

))(

2)(

2(4

1)(

4

1

2 0 3 1 2

0 3 1 0 31

13 0

2 e e e e e e e e

e c

c e

))(

2)(

2(4

1)(

4

1

3 0 2 1 3

0 2 1 0 12

21 0

3 e e e e e e e e

e c

c e

2.6.4 Chuẩn hoá vector Quaternion (Quaternion Norm)

Hàm giá trị thực (norm) L2 của vector quaternion luôn bằng 1, và giá trị này đ-ợc dùng để hiệu chỉnh các sai số đ-ợc tích luỹ trong quá trình tính

3 2 2 2 1 2 0

|

|

1 E  e e e e (2.51) Nếu một vector quaternion E~ không có hàm giá trị bằng 1 thì ta cần thực hiện việc chuẩn hoá theo ph-ơng trình (2.52) Từ ph-ơng trình (2.45) ta thấy các góc Euler đ-ợc tính dựa trên các phần tử của vector quaternion Điều này dẫn đến một số

ảnh h-ởng ngoài mong muốn Nếu một trong các thành phần của quaternion có sai

số nó sẽ ảnh h-ởng đến tất cả các thành phần còn lại khi ta thực hiện việc chuẩn hoá Ng-ợc lại, nếu ta dùng các góc Euler, giả sử có sai số ở góc ngẩng (pitch) thì sai số này sẽ tác động đến tất cả các thành phần quaternion Khi vector đ-ợc chuẩn hoá, sai số sẽ ảnh h-ởng đến cả các góc nghiêng cánh (roll) và góc h-ớng (yaw) Ph-ơng trình chuẩn hoá vector quaternion

2 3 2 2 2 1 2

E E

2.6.5 Tốc độ thay đổi tham số Quaternion

Các thành phần đ-ợc ký hiệu [p,q,r] biểu thị vận tốc góc trong hệ toạ độ khung Mối quan hệ giữa tốc độ thay đổi tham số Quaternion và [p,q,r] có thể đ-ợc biểu diễn theo ph-ơng trình sau[3]:

e e e

e e

e

e e e

e e e

e e e e

0 1 2

1 0

3

2 3 0

3 2 1

3 2 1 0

21

(2.53)

28 Ph-ơng trình này là liên tục theo thời gian và là một hệ của các ph-ơng trình vi phân Quá trình tính toán sẽ đ-ợc thực hiện trên máy tính vì vậy tham số quaternion

sẽ đ-ợc giữ nguyên ở dạng vector Viết lại ph-ơng trình (2.53) trên ta có :

E r q p A E e e e e p q r

p q r

r q

r p

q p

e e e

e

),,(

0000

21

3 2 1 0

3 2 1

E( ) ( ( ), ( ), ( )) (2.55)

2.6.6 Cập nhật tham số Quaternion trong miền thời gian rời rạc

Theo (2.55) ta có ph-ơng trình tính góc liên tục trong miền thời gian, tuy nhiên

để thực hiện quá trình tính toán trên máy tính thì ta cần phải thực hiện rời rạc hoá

),,(

1

n

n n

n

t

t

n n

t

t n n

n

E 1   •  (  ) (2.57) Ph-ơng trình (2.57) t-ơng đ-ơng với ph-ơng trình sau:

n n

e e e e t p

t q

t r

t p t

q t r

t r t q

t r t

p

t q t p

e e e e

1 3 2 1 0

111

28

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 là dựa trên cơ sở của các

định luật Newton về chuyển động Hệ thống sử dụng các cảm biến (sensors) để xác

định lực tác động và vận tốc chuyển động của ph-ơng tiện Đây là một hệ thống dẫn

đ-ờng hoàn toàn độc lập, tự bản thân hệ thống có thể xác định đ-ợc các thông tin về góc và tham số chuyển động nh-: vị trí, vận tốc, độ cao, vận tốc góc và gia tốc Tuy nhiên ph-ơng pháp dẫn đ-ờng này bị giới hạn về độ chính xác do những hạn chế về chất l-ợng của các phép đo từ các bộ cảm biến và tính chất sai số tích luỹ theo thời gian

3.1 Cảm biến quán tính

Các cảm biến quán tính chủ yếu là dùng để đo tham số gia tốc và tốc độ quay Để thực hiện việc đo gia tốc đ-ợc chính xác ta cần phải có các cảm biến với độ nhạy cao Sai số về vị trí sẽ tăng theo hàm mũ bậc hai theo thời gian đối với sai số gia tốc Công thức biểu thị sự phụ thuộc này đ-ợc cho trong công thức (3.1)

3.1.1 Bù nhiệt cho cảm biến

Một cảm biến, đặc biệt là cảm biến con quay, bị ảnh h-ởng rất lớn bởi nhiệt độ bên ngoài Nếu một khối IMU không có chế độ ổn định hay bù nhiệt thì sẽ gây ra sai

số không thể chấp nhận đ-ợc Thậm chí ngay cả khi nhiệt độ đ-ợc kiểm soát và duy trì không đổi thì giá trị của nó vẫn bị dao động Để có đ-ợc một hệ thống IMU/INS chất l-ợng cao ta cần phải có mô hình toán học để biểu thị sự ảnh h-ởng của nhiệt độ

đến các tham số đo đ-ợc từ các cảm biến quán tính Ph-ơng trình (3.2) biểu thị sự phụ thuộc này với độ chính xác khá cao

29 S(t,T) = B0 exp( t) + B1(T - Tref) + B2(T - Tref) (3.2)

đạt đ-ợc đến nhiệt độ làm việc Khoảng thời gian để các cảm biến đạt đ-ợc tới nhiệt

độ này sẽ phụ thuộc vào mỗi loại cảm biến và môi tr-ờng làm việc của cảm biến Các tham số , Tref, B0, B1 và B2 đ-ợc tính toán trong quá trình thực nghiệm

3.1.2 Tính phi tuyến của cảm biến quán tính

Mỗi cảm biến là duy nhất Vì vậy mỗi cảm biến sẽ có một giá trị đầu ra khác nhau ngay cả đối với các cảm biến cùng loại và cùng điều kiện làm việc Thông th-ờng các cảm biến đ-ợc cung cấp cùng với số liệu hiệu chỉnh t-ơng ứng Điều này là hoàn toàn bình th-ờng đối với các cảm biến có độ chính xác cao Nói cách khác, nếu các cảm biến đ-ợc cung cấp mà không kèm theo tài liệu hiệu chỉnh thì độ chính xác của cảm biến sẽ rất Khi đó các cảm biến đ-ợc điều chỉnh theo các tham số chuẩn Tín hiệu

đầu ra từ bộ cảm biến quán tính th-ờng là tín hiệu t-ơng tự Tín hiệu này bao gồm cả

độ sai lệch b (bias) mà ta cần phải biết hay tính toán Mỗi cảm biến sẽ làm việc trong một dải nhất định, trong tr-ờng hợp lý t-ởng thì cảm biến là tuyến tính trong phạm vi này Một vài cảm biến thì gần nh- là tuyến tính và không cần phải thực hiện việc tính toán các thành phần phi tuyến Một mô hình đơn giản và hết sức thông dụng để cải thiện chất l-ợng của một cảm biến đ-ợc cho trong ph-ơng trình (3.3)

Viết lại ta có: