đề tài tìm hiểu về gps-ins

Một số phương trình vị trí và tọa độ trong n – frame.. Khái quát và đối tượng nghiên cứu Sự tích hợp của hệ thống định vị quán tính INS với hệ thống định vị toàn cầu GPS đã được làm cho

ĐẠI HỌC QUỐC GIA THÀNH PHỐ HỒ CHÍ MINH

ĐẠI HỌC BÁCH KHOA KHOA ĐIỆN –ĐIỆN TỬ

BỘ MÔN ĐIỀU KHIỂN TỰ ĐỘNG

BÁO CÁO ĐỒ ÁN MÔN HỌC 1

ĐỀ TÀI: TÌM HIỂU VỀ GPS/INS

GVHD: TS Nguyễn Vĩnh Hảo SVTH: Nguyễn Thanh Bình 41100306

MỤC LỤC

I Khái quát và đối tượng nghiên cứu 4

II Các hệ tọa độ 5

1 Hệ tọa độ quán tính (The Inertial Frame) 5

2 Hệ tọa độ trái đất (The Earth Frame) 5

3 Hệ tọa độ định vị (The Navigation Frame) 6

4 Hệ tọa độ vật thể (Body Frame) 7

III Các công thức chuyển đổi hệ trục tọa độ 7

1 Chuyển đổi tọa độ từ e – frame sang n – frame: 7

2 Chuyển đổi tọa độ từ n – frame sang e – frame : 7

3 Chuyển đổi tọa độ từ n – frame sang b – frame 8

4 Chuyển đổi tọa độ từ b – frame sang n – frame 8

5 Một số công thức chuyển trục vận tốc 8

IV Phương trình định vị quán tính 10

1 Một số phương trình vị trí và tọa độ trong n – frame 10

2 Xây dựng phương trình cập nhật vận tốc 10

3 Phương trình cập nhật độ cao: 12

V Cơ khí hóa INS 15

1 Phương trình cập nhật Quaternion 16

2 Phương trình cập nhật vận tốc 16

VI TÌM HIỂU CHƯƠNG TRÌNH MẪU 19

VII MỘT SỐ CHƯƠNG TRÌNH XỬ LÝ DỮ LIỆU 20

VIII CHƯƠNG TRÌNH KML GENERATOR 23

IX KHẢO SÁT SỰ ẢNH HƯỞNG CỦA MỘT SỐ YẾU TỐ LÊN VIỆC XÁC ĐỊNH ĐỘ CAO CỦA IMU 25

1 Ảnh hưởng của áp suất 25

2 Ảnh hưởng của độ ẩm 26

X KẾT QUẢ ĐẠT ĐƯỢC VÀ HƯỚNG PHÁT TRIỂN 27 PHỤ LỤC 28

I Khái quát và đối tượng nghiên cứu

Sự tích hợp của hệ thống định vị quán tính (INS) với hệ thống định vị toàn cầu (GPS)

đã được làm cho những lĩnh vực ứng dụng mà thông tin về trạng thái (attitude) là một phần quan trọng không thể thiếu và sự thu thập nhanh các thông tin địa lí được yêu cầu

Vì những hệ thống định vị quán tinh chất lượng cao chỉ phù hợp cho quân sự và máy bay thương mại, không phù hợp cho những ứng dụng thông thường Vì vậy những nghiên cứu gần đây nỗ lực tập trung vào sử dụng những thiết bị đo lường quán tính (IMU) giá thành thấp

Những thiết bị đo lường quán tính giá thành thấp đặc trưng bởi độ chính xác kém khi hoạt động độc lập và độ ổn định thấp, có thể gây ra lỗi lớn trong những khoảng thời gian ngắn Vì vậy, sự ước lượng phân cực cho con lắc hồi chuyển đóng vai trò quan trọng cho thiết bị đo lường quán tính (IMU) và sự gia tăng cảm biến cần thiết cho việc khởi tạo trạng thái của những IMU lớp tự động và lớp tiêu dùng

Đối tượng của đề tài là cải thiện độ chính xác của hệ thống định vị quán tính (INS) giá thành thấp để chúng có thể được sử dụng trong những hệ thống điều hướng độc lập trong khi hệ thống định vị toàn cầu (GPS) ngừng hoạt động Những kĩ thuật sau đây được phát triển và kiểm chứng:

 Phương pháp hiệu chỉnh trường: ước lượng phân cực và nhân tố đo lường của thiết bị đo lường quán tính (IMU)

 Điểu chỉnh phù hợp vận tốc: khởi tạo trạng thái của thiết bị đo lường quán tính IMU sử dụng thông tin vận tốc của hệ thống định vị toàn cầu GPS

 Sử dụng những liên kết nonholonom: giảm độ chính xác vị trí độc lập

II Các hệ tọa độ

1 Hệ tọa độ quán tính (The Inertial Frame)

Là hệ tọa độ có gốc tại tâm trái đất, không xoay theo trái đất được xác định dựa trên các ngôi sao cố định Trục z song song với trục quay trái đất, trục x chỉ về điểm Xuân Phân (Vernal equinox, là điểm giao giữa đường xích đạo

và đường hoàng đạo), trục y được xác định theo quy tắc tam diện thuận

2 Hệ tọa độ trái đất (The Earth Frame)

Là hệ tọa độ có gốc tại trọng tâm trái đất, xoay theo trái đất được xác định là: trục z song song với trục quay Trái Đất, trục x chỉ về Greenwich (nước Anh), trục y được xác định theo quy tắc tam diện thuận

3 Hệ tọa độ định vị (The Navigation Frame)

Là hệ tọa độ có gốc tại 1 điểm trên trái đất gồm 2 loại là NED và ENU Loại NED thì trục x chỉ về hướng Bắc, trục z trực giao với mặt elip trái đất, hướng xuống trục y được xác định theo quy tắc tam diện thuận

Loại ENU thì trục x chỉ về hướng Tây, trục z trực giao với mặt elip trái đất, hướng lên trục y được xác định theo quy tắc tam diện thuận

Ưu điểm của ENU là cao độ tăng theo chiều lên Còn ưu điểm của NED là 3 trục tọa độ trùng với 3 trục tọa độ góc Euler của phương tiện roll – pitch – heading Vậy nên hệ tọa độ NED được sử dụng phổ biến hơn

Hệ tọa độ trái đất và hệ tọa độ định vị loại NED

4 Hệ tọa độ vật thể (Body Frame)

Là hệ tọa độ có gốc tại vật thể 3 trục trùng với 3 trục roll - pitch – heading

III Các công thức chuyển đổi hệ trục tọa độ

1 Chuyển đổi tọa độ từ e – frame sang n – frame:

Ta sử dụng ma trận cosin định hướng (DCM) : Cen

=(

)

Trong đó : : vĩ độ : kinh độ 2 Chuyển đổi tọa độ từ n – frame sang e – frame : Ta sử dụng ma trận cosin định hướng , có được bằng cách chuyển vị ma trận ở phần 1: = (

)

3 Chuyển đổi tọa độ từ n – frame sang b – frame

Ma trận cosin định hướng = Rx()Ry( )Rz()

(

)

Trong đó: , ,  3 thành phần của góc Euler: roll, pitch, heading 4 Chuyển đổi tọa độ từ b – frame sang n – frame Ma trận cosin = ( )T = Rx( )Ry( )Rz( ) = (

) (

) (

)

= (

)

5 Một số công thức chuyển trục vận tốc

Vận tốc quay của e – frame so với i – frame:

- Trong hệ quy chiếu e – frame bằng vận tốc quay của trái đất trên trục z

= ( 0 0 ωe)T Trong đó : ωe là vận tốc tự quay của trái đất quanh trục = 7.2921158 rad/s

- Trong hệ quy chiếu n – frame : ta có được bằng cách nhân với ma trận cosin :

= = (ωe 0 e )T

Vận tốc dịch chuyển của n – frame so với e – frame trong n – frame bằng đạo hàm các kinh độ, vĩ độ theo công thức:

= ( ̇ ̇ ̇ Trong đó:

)

với vN, vE: là tốc độ theo hướng bắc và hướng đông, h là chiều cao elip & M, N là bán kính cong của kinh tuyến gốc và đường dọc chính được tính theo công thức

Với a và e là trực nửa lớn và độ lệch tâm tuyến tính của elip tham khảo

)

IV Phương trình định vị quán tính

1 Một số phương trình vị trí và tọa độ trong n – frame

Vị trí trong n – frame được xác định bởi hệ tọa độ cầu :

= ( (*) Vận tốc trong n – frame được tính bằng:

= ( ) = (

) (

̇ ̇ ̇

Từ (*)(**) ta có công thức đạo hàm vị trí trong n – frame:

̇ = (

̇ ̇ ̇) (

) ( )

2 Xây dựng phương trình cập nhật vận tốc

Ta có:

̇ Với ̇ ̇ )

)

Do đó, ta viêt lại :

̇ Đạo hàm phương trình trên ta có:

Thay ̇ ̇ ; I = vào phương trình trên, ta được:

Ta có :

(***)

Thay vào phương trình (***), ta được:

Với ̈ Trong đó :

là vector lực biểu diễn gia tốc của vật thể

là gia tốc trọng trường

Ta viết lại phương trình trên:

Ta được phương trình cập nhật vận tốc trong n – frame:

nhau ( hoặc song song nhau) dẫn đến hệ thống chỉ quay trong không gian 2 chiều

Ví dụ giả sử nếu góc Pitch = 90 độ khi đó theo thứ tự ta có trục X sẽ trùng với trục Z sau khi thực hiện xong góc quay quanh trục Y , khi đó khi quay quanh trục X thì sẽ giống như quay quanh trục Z Vật chỉ quay được quanh trục Y và Z mà thôi

Như vậy các góc Picth và Roll sẽ phải bị giới hạn trong tầm từ ( -π/2 , π/2)

Để khắc phục nhược điểm này của phương pháp góc Euler, ta sử dụng hệ tọa độ không gian 4 chiều Quaternion

Đây là hệ thống số được phát triển mở rộng từ hệ thống số phức Nó được định nghĩa lần đầu tiên bởi nhà toán học người Ai-len Sir illiam Rowan Hamilton vào năm 1 , và được ứng ụng để phân tích không gian chiều Một quaternion đơn vị có thể được định nghĩa như sau:

q = [q1 q2 q3 q4]

Ch ng ta có thể kết hợp quaternion với một chuyển động quay quanh một trục như sau:

Trong đó:

là góc quay (giá trị tính bằng ra ian của góc quay)

cos(x), cos(y), cos(z) là các cosine định hướng của trục quay Khi đó ma trân cosin trực tiếp được xác định bằng :

Và q tính từ ma trận bằng :

Khi đó sẽ được cập nhật thông qua việc cập nhật q :

Trong đó : ω x ; ω y ; ω z là 3 thành phần của vector vận tốc góc

Cuối cùng ta được hệ phương trình định vị ins:

Trong đó:

V Cơ khí hóa INS

; là tín hiệu đã qua hiệu chỉnh bias và scale factor

Vector độ thay đổi góc của b – frame so với n – frame được cho bởi:

Trong đó:

BIAS MODE ; SCALE FACTOR MODE

: chu kì lấy mẫu

Độ thay đổi vận tốc góc của b – frame so với n –frame do các ngoại lực gây

ra được cho bởi :

Thuật toán định vị INS được thể hiện bằng sơ đồ sau:

VI TÌM HIỂU CHƯƠNG TRÌNH MẪU

Ở phần này, em xin được trình bày những tìm hiểu của em về các chương trình mẫu, đồng thời so sánh với các công thức trong luận văn để hiểu được mục đích của các chương trình con Các chương trình con được thể hiện ở phần phụ lục

1 Chương trình ‘Geteuler.m’ được ùng để tính toán các thành phần của góc Euler (roll, pitch, yaw) từ các phân tử của ma trận cosine định hướng

Chương trình nhận tham số đầu vào là ma trận R có kích thước x đại diện cho

ma trận cosin định hướng Từ đó tính toán theo công thức để tính được các giá trị roll, pitch và yaw và trả về qua các thông số x, y, z

2 Chương trình ‘init_Rnb.m’ tính toán ma trận cosine định hướng từ các thông số roll, pitch và yaw được truyền vào chương trình qua các tham số x, y, z Chương trình trả về giá trị của ma trận cosine định hướng

3 Chương trình ‘Rmcal.m’ và ‘Rncal.m’ nhận đầu vào là giá trị vĩ độ và trả về bán kính cong của kinh tuyến gốc và đường dọc chính

4 Chương trình ‘Wcal.m’ được ùng để tính các giá trị tốc độ góc , và trong hệ tọa độ quán tính

5 Chương trình ‘Fcal.m’ tính toán các giá trị Frr, Frv, Fvr, Fvv, Fer, Fev trong ma trận động F của bộ lọc Kalman

6 Chương trình ‘initial_quaternion.m’ để khởi tạo giá trị cho quaternion

7 Chương trình ‘llattoecef.m’ chuyển đổi giá trị kinh độ, vĩ độ và trạng thái sang

hệ tọa độ Trái Đất (ECEF)

8 Chương trình ‘Quadcal.m’

Chương trình được ùng để cập nhật giá trị của ma trận cosine định hướng Chương trình nhận hai tham số đầu vào là a và qk, trong đó a là một vector 3x1 thể hiện góc gia tăng của vật thể trong hệ tọa độ quán tính, qk là thông số quaternion trước khi cập nhật ma trận cosine định hướng

Đầu ra của chương trình là các tham số R và qk, trong đó qk là các tham số quaternion sau khi cập nhật Các thông số đó được ùng để cập nhật ma trận cosine định hướng

VII MỘT SỐ CHƯƠNG TRÌNH XỬ LÝ DỮ LIỆU

1 Cấu trúc của file thu thập số liệu từ IMU bao gồm các dữ liệu về INS, GPS, RTK

a Định dạng của một dòng dữ liệu INS bao gồm:

- Các góc quay theo ba trục x, y, z: Phi, Theta, Psi (đơn vị 0.1o)

- Giá trị vận tốc góc ba trục x, y, z: ωx, ωy, ωz (mra /s)

- Giá trị gia tốc theo ba trục x, y, z: ax, ay, az (mm/s2)

- Giá trị từ trường theo ba trục x, y, z: mx, my, mz (mgauss)

- Giá trị áp suất từ barometer (hPa)

b Định dạng của dữ liệu GPS bao gồm:

6 = GPS quality indicator (0=invalid; 1=GPS fix; 2=Diff GPS fix)

7 = Number of satellites in use [not those in view]

8 = Horizontal dilution of position

9 = Antenna altitude above/below mean sea level (geoid)

10 = Meters (Antenna height unit)

11 = Geoidal separation (Diff between WGS-84 earth ellipsoid and

mean sea level -=geoid is below WGS-84 ellipsoid)

12 = Meters (Units of geoidal separation)

13 = Age in seconds since last update from diff reference station

14 = Diff reference station ID#

15 = Checksum

$GPVTG,t,T,,,s.ss,N,s.ss,K*hh

1 = Track made good

2 = Fixed text 'T' indicates that track made good is relative to true north

3 = not used

4 = not used

5 = Speed over ground in knots

6 = Fixed text 'N' indicates that speed over ground in in knots

7 = Speed over ground in kilometers/hour

8 = Fixed text 'K' indicates that speed over ground is in kilometers/hour

9 = Checksum

2 Chương trình ‘DataGet.m’ để lấy ữ liệu thô GPS, RTK, INS từ file thu thập Trong đó:

- Input: ile thu thập số liệu *.txt

- Output: Kết quả lưu vào file GPSINS raw ata.mat gồm GPS raw ata, RTK_rawdata, INS_rawdata

Từ file dữ liệu ban đầu, chương trình xử lí để giữ lại các giá trị cần thiết INS_rawdata bao gồm các giá trị: Phi, Theta, Psi, ωx, ωy, ωz, ax, ay, az, mx, my, mz, barometer GPS_rawdata và RTK_rawdata bao gồm các giá trị về thời gian, vĩ độ, kinh

độ, độ cao, tốc độ theo hướng Đông, tốc độ theo hướng Bắc và tốc độ theo chiều cao

3 Chương trình DataPlot.m để vẽ đồ thị của qu đạo của GPS và RTK

4 Chương trình DataProcess.m đê lấy tập ữ liệu GPS và INS sau khi xử l

- Input: ile GPSINS raw ata.mat ở mục 2

- Output: Kết quả lưu vào file GPSINS ata.mat gồm GPS ata, INS ata GPS ata là kết quả của GPS raw ata hoặc RTK_rawdata Cấu trúc của GPS_data bao gồm: thời gian (sec), vĩ độ ( eg), kinh độ (deg), 2 giá trị tọa độ UTM được chuyển đổi từ vĩ độ và kinh độ (m), độ cao (m), tốc độ theo ba trục East-North-Down (m/s) và giá trị chỉ hướng của vật thể (deg)

5 Chạy file DataPickup.m để lựa chọn vùng ữ liệu cần thiết cho ước lượng

- Input: ile GPSINS ata.mat ở bước , (N1, N2) để chọn vùng ữ liệu cần lấy

- Output: Kết quả lưu vào file GPSINS pic ata.mat gồm GPS ata, INS ata

6 Chạy file DataDelete.m để lựa chọn vùng ữ liệu cần tạo mất tín hiệu GPS

- Input: GPS ata, INS ata, (N1, N2) để chọn vùng ữ liệu cần làm mất

- Output: Kết quả lưu vào file GPSINS el ata.mat gồm GPS ata, INS ata

VIII CHƯƠNG TRÌNH KML GENERATOR

Chương trình KML Generator được thiết kế để chuyển đổi tập tin dữ liệu GPS chứa các chuỗi định dạng NMEA sang phần mềm Google EarthTM tương thích với tập tin KML

Trước hết ta chọn một file dữ liệu đầu vào Sau đó nhấn Generate KML ile để tạo

ra file *.kml tương thích với phần mềm Google Earth

Mở file vừa tạo bằng phần mềm Google Earth ta được như hình sau

IX KHẢO SÁT SỰ ẢNH HƯỞNG CỦA MỘT SỐ YẾU TỐ

LÊN VIỆC XÁC ĐỊNH ĐỘ CAO CỦA IMU

1 Ảnh hưởng của áp suất

Độ cao được tính từ áp suất theo công thức

trong đó:

Pb: Áp suất tĩnh (pascals)

Tb: Nhiệt độ tiêu chuẩn (K)

Lb: Gradient của nhiệt độ (K/m)

h: Độ cao trên mực nước biển (meters)

hb: Độ cao tại đáy của lớp b (meters; ví dụ: h1 = 11,000 meters)

R*: Hằng số khí = 8.31432 N·m /(mol·K)

g0: Gia tốc trọng trường (9.80665 m/s2)

M: Khối lượng mol của không khí (0.0289644 kg/mol)