36
Nhƣ trình bày ở trên ta đã xác định đƣợc góc nghiêng nhờ đến cảm biến accellerometer. Bây giờ ta sẽ xác định góc xoay theo phƣơng ngang (Yaw) theo cảm biến từ trƣờng. Cách xác định đƣợc trình bày nhƣ sau:
Gọi Mx, My, Mz là hình chiều của vector cƣờng độ từ trƣơng B lên các trục tọa độ tƣơng đối và M’x, M’y, M’z là hình là các giá trị đo đƣợc trong hệ tọa độ tuyệt đối.
Trong hệ tọa độ tuyệt đối thì rõ ràng Yaw sẽ đƣợc tính nhƣ sau: Yaw = Ψ = arctan( M’y /M’x)
Nếu thiết bị nghiêng theo các góc Roll và Pitch thì ta quy về hệ tọa độ tuyệt đối nhƣ sau:
M’x = Mxcosθ + Mysinθ
M’y = Mx sinΦsinθ + MycosΦ - MzsinΦcosθ
M’z = Mx cosΦsinθ + MysinΦ - MzcosΦcosθ
Ta có các trƣờng hợp nhƣ sau:
Yaw = Ψ = arctan( M’y /M’x) ( nếu Mx >0 và My>0 Yaw =Ψ = 180 + arctan (M’y/M’x) ( nếu M’x <0)
Yaw =Ψ =360 + arctan ( M’y/M’x) ( nếu M’x > 0 và M’y<=0) Yaw =Ψ = 90o ( nếu M’x = 0 và M’y <0)
Yaw =Ψ = 270 ( nếu M’x = 0 và M’y >0)
Chú ý rằng tƣơng tự nhƣ trƣờng hợp cảm biến gia tốc ta cũng có M’x, M’y, M’z cũng đƣợc chuẩn hóa, nghĩa là
2 2 2
' x ' y ' z 1
37
3.4 Bộ lọc sai số sử dụng cho IMU
Bộ lọc tác giả triển khai trên IMU là bộ lọc bù. Bộ lọc bù dựa trên cơ sở sự thay đổi nhanh của cảm biến vận tốc góc thì đáng tin cậy hơn giá sự thay đổi từ cảm biến từ trƣờng và trọng trƣờng. Đây là bộ lọc dễ triển khai và tốn ít tài nguyên hệ thống nhất.
Để tránh sự phân kì bởi phân cực Gyro, thì sự hội tụ ổn định của hệ thống đạt đƣợc bằng cách sử dụng cảm biến gia tốc và từ trƣờng chứ khơng phải là từ cảm biến vận tốc góc
Phƣơng trình để thỏa mãn hai yêu cầu này nhƣ sau:
⃗⃗⃗ ̂ ( ) ⃗⃗⃗ ̂ ( ) ⃗⃗⃗ ̂( )
Hệ số cao nhất 0.98 đƣợc chọn cho phép hệ thống đáp ứng nhanh với sự thay đổi của phép đo.
Trong trƣờng hợp feedback đƣợc đƣợc đƣa lại hệ thống và phƣơng trình đƣợc viết lại nhƣ sau:
⃗⃗⃗ ̂( ) ⃗⃗⃗ ̂ ( ) ⃗⃗⃗ ̂( )
Feedback cho phép lấy đƣợc đáp ứng của cả cảm biến gia tốc và cảm biến từ trƣờng để tăng độ hội tụ cho cảm biến vận tốc góc. Hằng số của hệ thống đƣợc xác định theo công thức sau:
38
3.5 Dead Reckoning
Dead reckoning là một thuật ngữ dùng trong ngành hàng hải truyền thống, dùng để chỉ phƣơng pháp xác định vị trí thuyền trên biển của các hoa tiêu. Những ngƣời hoa tiêu có thể tiên đốn đƣợc vị trí của thuyền trong một khoảng thời gian nhất định nhờ vào độ bẻ lái và tốc độ của thuyền, do đó có thể ƣớc lƣợng đƣợc những chƣớng ngại vật sắp tới trên đƣờng đi, hay vị trí của đất liền dựa vào kinh nghiệm về địa lý. Tới ngày nay, cùng với sự phát triển của khoa học kỹ thuật, dead- reckoning trở thành một phƣơng pháp định vị cho các hệ thống dò đƣờng. Phƣơng pháp này dựa trên trạng thái hiện tại của hệ thống nhƣ vận tốc, gia tốc, vị trí, hƣớng để có thể ƣớc lƣợng đƣợc trạng thái kế tiếp của hệ thống.
Mô hình phƣơng tiện bay vào hệ trục tọa độ XYZ tƣơng ứng hệ North-East- Up nhƣ hình vẽ.