4. Đề xuất cỏc giải phỏp mới, nội dung và phƣơng phỏp nghiờn cứu
4.4. Ứng dụng phƣơng phỏp hiệu chỉnh cỏc phần tử ma trận quay trong phỏt triển
triển firmware cho hệ thống nhỳng trờn xe bus
Giỏm sỏt một đối tƣợng chuyển động là tớnh toỏn cỏc thụng số chuyển động của đối tƣợng ở hệ tọa độ vật thể P (b-frame) so với một hệ quy chiếu G (Global), thƣờng là hệ tọa độ trỏi đất G (Ground hay e-frame). Giải phỏp trong luận ỏn quy ƣớc hệ tọa độ vật thể là Oxyz, cú cỏc vector đơn vị là (i, j, k) và hệ quy chiếu (NWU) là OXYZ, cú cỏc vector đơn vị là (I, J, K). Trong hệ tọa độ NWU này, I là vector chỉ hƣớng bắc,
K là vector chỉ thiờn (Zenith vector) và J là vector chỉ hƣớng tõy. Hệ tọa độ vật thể P
quy ƣớc theo cảm biến IMU, vỡ vậy trong thiết kế cần bố trớ chiều của IMU một cỏch phự hợp. Luận ỏn lựa chọn hệ tọa độ hiển thị hƣớng cỏc trục X, Y, Z tƣơng ứng so với hỡnh ảnh của chip vật lý hoặc thiết bị, cỏc trục X, Y, Z nhƣ trong tài liệu kỹ thuật của hóng GitHub. Tuy nhiờn điều này tựy thuộc vào từng cảm biến cụ thể và cần đƣợc điều chỉnh trong quỏ trỡnh Calib.
Hỡnh 4. 16: Hệ tọa độ vật thể P và hệ tọa độ quy chiếu G (NWU)
Hệ tọa độ quy chiếu G (NWU) cú vector đơn vị (I, J, K) và hệ tọa độ vật thể P
(b-frame) cú vector đơn vị (i, j, k) cú thể đƣợc biểu diễn nhƣ sau:
IG = {1,0,0} T, JG={0,1,0} T , KG = {0,0,1} T (4.7)
iB = {1,0,0} T, jB={0,1,0} T , kB = {0,0,1} T (4.8)
Đối với hệ tọa độ NWU, vector gia tốc trọng trƣờng hƣớng về tõm trỏi đất sẽ cú chiều ngƣợc với vector Zenith (KB). Ba trục đầu ra gia tốc là A = {Ax , Ay , Az},và giả sử rằng khụng cú gia tốc ngoài tỏc động hoặc đó đƣợc hiệu chỉnh thỡ ta cú thể ƣớc tớnh
KB = - A. Tƣơng tự với từ trƣờng, vớiM = {Mx , My , Mz }, IB chỉ cực bắc, ta cú IB = M,JB = KB IB. Nhƣ vậy từ cỏc vector gia tốc, gúc quay và vector từ trƣờng, ta cú thể tớnh đƣợc ma trận quay R, biểu diễn qua cỏc gúc Euler gọi là DCM, thể hiện thụng qua
DCMB và DCMG, với: DCMG = DCMBT = [IB, JB, KB]T , - [ ] [ ( ) ( ) ( ) ( ) ( ) ( ) ( ) ( ) ( ) ] (4.9) , - [ ] [ ( ) ( ) ( ) ( ) ( ) ( ) ( ) ( ) ( ) ] (4.10)
Nhƣ vậy, sự chuyển động của hệ tọa độ vật thể so với hệ quy chiếu thể hiện ở khoảng cỏc dịch chuyển r với vector đơn vị:
iG = {ixG , iyG , izG} T = { I.i, J.i, K.i}T jG= {jxG , jyG , jzG} T = { I.j, J.j, K.j}T , kG= {kxG , kyG , kzG} T = { I.k, J.k, K.k}T
Khoảng di chuyển của đối tƣợng đƣợc tớnh toỏn nhƣ sau:
rB= {rxB, ryB, rzB} T ; rG = { rxG , ryG , rzG } T ; [ ] [ ] [ ] (4.11)
Trong đú: DCM là ma trận cosine chỉ phƣơng (Direction Cosine Matrix), thể hiện sự quay giữa hai hệ tọa độ.
Tại thời điểm t0 ta biết vector chỉ thiờn của hệ tọa độ vật thể KB0, và từ giỏ trị Gyro đƣa ra ta sẽ tớnh toỏn đƣợc vận tốc gúc quay ɷ = {ɷx , ɷy , ɷz }. Với ɷg là vận tốc gúc tớnh toỏn đƣợc từ Gyro, dθg là gúc dịch chuyển, ta cú: dθg = dt ɷg. Nhƣ vậy trong khoảng thời gian dt hệ tọa độ vật thể quay một gúc nhỏ KB1Gso với phƣơng chỉ thiờn (Zenith), và vị trớ tiếp theo của vector chỉ thiờn sau khoảng thời gian dt là:
KB1G ≈ KB0 + v.dt = KB0 + dt (ɷg KB0) = KB0 + ( dθg KB0) (4.12)
Tƣơng tự với Gyro, ta tớnh toỏn với vector gia tốc KB1A từ cỏc giỏ trị đọc ra của cảm biến gia tốc của IMU. Từ giỏ trị này ta tớnh đƣợc giỏ trị của vận tốc gúc ɷa và khoảng thay đổidθa = dt ɷa
ɷa = KB0 va / | KB0|2 (4.13)
Với: va = (KB1A - KB0) / dt, trong đú KB0 là vector đơn vị cú | KB0|2 = 1, ta cú:
IMU đƣợc sử dụng trong thiết kế này là loại MEMS 9-DOF Razor cú ba trục từ trƣờng. Từ cỏc thụng số của ba trục này, gúc dịch chuyển của vector từ trƣờng đƣợc tớnh toỏn theo cụng thức: dθm = dt ɷm = IB0 (IB1M - IB0) (4.15) Đỏnh giỏ theo hàm trọng lƣợng: dθ = (sa dθa + sg dθg + sm dθm) / (sa + sg +sm), ta cập nhật ma trận DCM1 theo cụng thức: IB1 ≈ IB0 + ( dθ IB0) , KB1 ≈ KB0 + ( dθ KB0) (4.16) JB1 ≈ JB0 + ( dθ JB0)
Phỏt triển trờn hệ thống nhỳng Arduino, luận ỏn sử dụng hàm ghỡm constrain
để xử lý hàm trọng lƣợng, kết hợp với cỏc hệ số Kp, Ki đó đƣợc khảo sỏt phự hợp (chƣơng 3), từ đú tớnh ra giỏ trị điều chỉnh cho cỏc gúc quay. Nhƣ vậy tại thời điểm t0
cú ma trậnDCM0 , tại thời điểm t1 ta tớnh đƣợc ma trận DCM1 với sự cập nhật của cỏc vectorIB1, JB1, KB1. Áp dụng tƣơng tự cho cỏc bƣớc tiếp theo, với ký hiệu TCT là quỏ trỡnh tỏi cấu trỳc Normalizeed[a] , biểu diễn vector đơn vị đồng hƣớng với vector a, giải thuật của luận ỏn thực hiện chƣơng trỡnh hiệu chỉnh trờn cơ sở nguyờn lý lọc bổ sung RTL.
Ký hiệu Err đặc trƣng cho sai lệch (Error) trong tớnh toỏn hiệu chỉnh trục, ta tớnh toỏn sai số theo cụng thức:
Err = ( IB1 JB1 ) / 2 (4.17)
Tớnh toỏn trực giao:
ITGB1’ = IB1 – Err JB1
JTGB1’ = JB1 – Err IB1 (4.18)
KTGB1’ = ITGB1’ JTGB1’
ITCTB1’’ = TCT[ITGB1’] = = 0.5 (3 - ITGB1’ ITGB1’) ITGB1’ JTCTB1’’ = TCT[JTGB1’] = = 0.5 (3 - JTGB1’ JTGB1’) JTGB1’ (4.19) KTCTB1’’ = TCT[KTGB1’] = = 0.5 (3 - KTGB1’ KTGB1’) KTGB1’ Trong đú:
TCT: là quỏ trỡnh tỏi cấu trỳc (renormalization)
TG: là quỏ trỡnh tớnh toỏn trực giao (orthogonal)
Tớnh toỏn cập nhật:
Tớnh toỏn giỏ trị cập nhật cỏc phần tử ma trận quay R theo mỗi bƣớc tớnh của Omega_Vector, trong đú là phần tử tƣơng ứng Corrected Gyro_Vector data:
(4.20)
Từ giỏ trị cập nhật cỏc phần tử ma trận quay trờn, ta cập nhật lại ma trận quay rồi tớnh toỏn cỏc gúc Euler:
( )
( ) (4.21)
Lặp lại quỏ trỡnh tớnh toỏn cỏc phần tử của ma trận quay R tại cỏc thời điểm sau mỗi bƣớc xử lý tớn hiệu của INS, ta cú ma trận cosine chỉ phƣơng DCM đƣợc tổ chức lại, cập nhật thay đổi phự hợp với cỏc phần tử đó đƣợc hiệu chỉnh theo từng bƣớc tớnh từ cỏc vector tỏi cấu trỳc ITCT (IreormalizedBi’’), JTCT (JreormalizedBi’’), KTCT (KreormalizedBi’’) theo cụng thức (4.19) tƣơng ứng.
Cỏc phần tử trong ma trận quay R sẽ khụng bị trụi gõy sai số tớch lũy bởi vỡ cỏc vector này đƣợc điều chỉnh vào vị trớ trực giao tuyệt đối (orthogonal), sau đú đƣợc thụng qua quỏ trỡnh tỏi chuẩn húa (renormalized) quyết định bởi số liệu nhận từ gia tốc Acc và vector từ trƣờng để hiệu chỉnh lại. Chỳng cũng khụng chịu tỏc động tớch lũy của nhiễu từ bờn ngoài do giải phỏp sử dụng dữ liệu con quay hồi chuyển Gyro để cập nhật lại. Số liệu thu đƣợc từ cảm biến từ trƣờng cũng đƣợc dựng để hiệu chỉnh trƣợt cho gúc yaw, cũn gọi là quỏ trỡnh hiệu chỉnh trƣợt gúc hƣớng đối tƣợng (drifting heading). Từ cỏc dữ liệu đó đƣợc hiệu chỉnh tỏi chuẩn húa này, ta tớnh toỏn cỏc cỏc giỏ trị cập nhật cỏc phần tử ma trận quay (4.20), từ đú tớnh ra cỏc gúc Euler: roll, pitch, yaw (4.21) và tớnh toỏn lại vận tốc và khoảng di chuyển của đối tƣợng.
Phần này triển khai cụ thể cỏc vấn đề lý thuyết trong chƣơng 3 trờn một MEMS INS 9-DOF cụ thể trờn một hệ thống nhỳng cụ thể. Giải thuật tớnh toỏn và cỏc cụng thức cú thể ỏp dụng cho cỏc INS 9-DOF loại khỏc hay với cỏc hệ vi xử lý khỏc. Cỏc dữ liệu của MEMS INS 9-DOF luụn đƣợc cập nhật trong mỗi bƣớc tớnh, kết hợp với dữ liệu nhận đƣợc từ CAN bus (FMS J1939) và GPS theo thuật toỏn lọc UKF để hiệu chỉnh chớnh xỏc quỹ đạo và vận tốc của đối tƣợng đƣa vào hiển thị kết quả cuối cựng trong hệ thống giỏm sỏt tại trạm. Dữ liệu đƣợc ghi trong cỏc file trong suốt quỏ trỡnh hoạt động, dễ dàng truy cập để xem xột tớnh chất chống trụi dữ liệu của phƣơng phỏp này. Cấu trỳc dữ liệu cụ thể đƣợc thể hiện ở phụ lục 3. Bộ lọc UKF sẽ đƣợc đề cập chi tiết trong phần tiếp theo.
4.5. Thiết kế bộ lọc UKF và kết quả thực tế trờn hệ thống giỏm sỏt tại trạm ứng dụng cho xe bus