Thí nghiệm tiến hành bằng cách gá bo mạch có gắn các cảm biến lên giá quay ba chiều, thông tin về từ trường ba trục và VTG cần đo vì thế sẽ thay đổi. Chúng sẽ được biến đổi thành tín hiệu số và được dùng để cập nhật phép đo cho quá trình ước lượng.
Thông tin tiên nghiệm của các trạng thái có thể xác định nhờ việc thiết lập vị trí ban đầu của TBB. Đối với bài thực nghiệm thông tin tiên nghiệm được xác định bởi vị trí ban đầu của bo mạch đặt trên giá quay tự động.
Như ta đã biết, các thành phần VTG được coi là quá trình ngẫu nhiên Mackov [35], chúng được đặc trưng bởi hằng số thời gian và mật độ phổ của tạp hình thành. Dựa vào đặc trưng dải tần số của các thành phần vận tốc góc ta có thể xác định các hằng số thời gian của các quá trình Mackov đó, nó cũng chính là hằng số thời gian của bộ lọc tạo hình (bộ lọc thông thấp). Còn độ lớn của mật độ phổ tạp hình thành được xác định dựa vào độ tản mát của chính bản thân thành phần vận tốc góc. Như vậy ta có cơ sở để xác định được hằng số thời gian và mật độ phổ tạp hình thành, chúng là thông tin tiên nghiệm cần đưa vào thuật toán lọc.
Thông tin về sai số các phép đo có thể xác định nhờ việc xem xét bản chất và nguồn sai số đo của từng cảm biến. Thông qua việc tìm hiểu sai số đo ta có thể biết được ma trận phương sai của nhiễu đo, và đặt chúng làm thông tin tiên nghiệm để tổng hợp bộ lọc. Riêng với cảm biến đo tốc độ góc ta cấn phải quan tâm và xử lí bias của phép đo, giải pháp nâng cao độ chính xác lọc khi tính đến nguồn sai số này cũng đã được đề cập.
Các thông tin tiên nghiệm khác được tính toán trực tiếp từ các công thức đã dẫn. Tất cả các thông tin tiên nghiệm sẽ được khởi tạo thông qua bước khởi tạo của phần mềm.
4.2.1 Lựa chọn các loại cảm biến
1. Lựa chọn cảm biến đo từ trường ba trục [26]
Theo giới thiệu tại điểm 3 mục 2.2.2 (một số loại cảm biến từ trường công nghệ mới). Lựa chọn cảm biến từ trường Trái đất vào mục đích thử nghiệm định hướng vào loại HMC2003. Ngoài những thông tin cơ bản đã nêu, các đặc tính kỹ thuật chi tiết của loại cảm biến này có thể tham khảo ở [17, 21].
2. Lựa chọn cảm biến vận tốc góc bổ sung [27]
Để đo được các thành phần VTG của TBB theo VTT, ở chương 2 đã chỉ ra rằng phải sử dụng thêm ít nhất một cảm biến VTG độc lập. Cảm biến VTG một trục dưới dạng con quay vi cơ (MEMS) đáp ứng hoàn toàn những yêu cầu về kích thước, trọng lượng và giá thành, bảo đảm độ chính xác mức độ cho phép. Con quay vi cơ là sự kết hợp của một thiết bị chuyển động theo gia tốc được tích hợp trong một cấu trúc cơ khí siêu nhỏ. Nó bao gồm một phần tử cảm biến chuyển động được đặt trong rãnh trượt, có khả năng dịch chuyển tỉ lệ với góc quay dựa theo nguyên tắc Coriolis. Ở đây ta chọn con quay vi cơ LY510ALH của Hãng STMicroelectronic. USA (Hình 4.1).
a) Tính năng và đặc tính kỹ thuật của LY510ALH: + Làm việc bằng nguồn đơn trong dải: (2.7 3.6) V; + Dải nhiệt độ làm việc cho phép: (-40 85)°C; + Độ ổn định nhiệt độ cao;
+ Tín hiệu đầu ra dạng analog tuyệt đối; + Tích hợp bộ lọc thông thấp;
+ Điện năng tiêu thụ thấp; + Chịu được rung xóc cao; + Thân thiện với môi trường
b) Phạm vi ứng dụng: Hệ thống dẫn đường GPS; Phát hiện chuyển động; Hình 4.1 Cảm biến LY510ALH
động với giao diện người dùng; Công nghệ robot.
c) Mô tả. Con quay vi cơ một trục LY510ALH do hãng ST Microelectronic sản xuất. Tiêu thụ điện năng cực thấp, dùng để đo VTG quay dọc theo trục, độ ổn định làm việc theo nhiệt độ cao trong dải rộng. Dải đo của LY510ALH là ±100 °/s và có khả năng phân biệt trong dải ở mức -3dB tần số làm việc tới 140 Hz.
Sơ đồ chức năng của LY510ALH được trình bày trên hình 4.2.
Sơ đồ nguyên lý sử dụng LY510ALH và giá trị của các linh kiện mở rộng được thể hiện trên hình 4.3. Qua đó ta thấy LY510ALH cho phép có nhiều lựa chọn kết nối: có khuếch đại; không khuếch đại; hoặc sử dụng thêm bộ lọc đầu ra, các giá trị linh kiện của bộ lọc mở rộng được chỉ rõ.
Hình 4.9. Sơ đồ chân của LY510ALH
d) Các đặc tính kỹ thuật chi tiết của loại cảm biến này tham khảo ở [27]. 4.2.2 Thiết kế Board Sensores
1. Xây dựng sơ đồ chức năng, hình 4.4
Board Sensoes (BS) xây dựng trên cơ sở lai ghép hai mini-board là board HMC2003 và board LY510ALH có những thành phần chức năng sau:
a) Miniboard HMC2003, gồm có: + Mạch tạo nguồn nuôi ổn áp;
+ Mạch tạo dao động 500Hz; khuếch đại cặp pha dao động 500Hz; + Bộ đệm đầu vào cảm biến;
+ Cảm biến HMC2003;
+ Mạch lọc và khuếch đại đầu ra ba kênh X, Y và Z. b) Miniboard LY510ALH, gồm có:
+ Mạch tạo nguồn nuôi ổn áp và lọc nguồn; + Bộ lọc thông thấp đầu vào;
+ Bộ lọc thông cao đầu ra không khuếch đại. 2. Thiết kế sơ đồ nguyên lý, hình 4.5.
a) Miniboard HMC2003
b) Miniboard LY510ALH Hình 4.5 Sơ đồ nguyên lý Board Sensores phần thực nghiệm
Nguồn ổn áp 15VDC Mạch tạo F F=500Hz KĐ cặp pha F0 và F180 Bộ đệm đầu vào 3 kênh Sensor HMC2003 KĐ ra 3 kênh H1X,H1Y,H1Z Jack I/O (tới ADC) Miniboard HMC2003 Nguồn ổn áp 3.6VDC Bộ lọc LF đầu vào Sensor LY510ALH Bộ lọc HPF Đầu ra 1Y Jack I/O (tới ADC) Miniboard LY510ALH Hình 4.4. Sơ đồ chức năng Board Sensores (adsbygoogle = window.adsbygoogle || []).push({});
Căn cứ kích thước linh kiện, sơ đồ và kích thước chân các sensores như đã nêu trong mục 4.2.1, căn cứ mạch điện sơ đồ nguyên lý của Miniboard HMC-2003 và LY510ALH (hình 4.5), sử dụng phần mềm thiết kế mạch in “Orcad” ta có sơ đồ mạch in, phục vụ cho lắp ráp hai miniboard dưới đây, hình 4.6a,b.
a) Mạch in LY510ALH b) Mạch in HMC2003 Hình 4.6 Sơ đồ mạch in các miniboard sensores
4.2.3 Lựa chọn ADC trên cơ sở Platform ElVIS-II của NI
Platform ElVIS-II của NI (National Instruments) là một thiết bị trợ giúp thiết kế, thử nghiệm các mạch điện tử số và tương tự dạng Platform khá mạnh. Phần cứng của thiết bị cho phép triển khai thiết kế các loại mạch điện tử từ linh kiện rời (điện trở, tụ điện, cuộn cảm, bán dẫn, IC số,…). Platform ELVIS-II (xem hình 4.7) được tổ chức dạng ma trận chân cắm linh kiện với những quy định riêng chân nguồn và đất. Có hệ thống nguồn chuẩn thay đổi được trong dải rộng phục vụ thí nghiệm. Có hệ thống phát tín hiệu chuẩn nhờ phần chương trình kết nối với máy tính. Có các đầu vào/ra (I/O) có thể nối với các máy phát tín hiệu, máy đo, phân tích tín hiệu từ bên ngoài. Phần mềm của thiết bị này có cả những chương trình phân tích, biến đổi, hiển thị tín hiệu, trong đó có biến đổi ADC và DAC.
Hình 4.7 Giới thiệu tính năng của Platform ELVIS-II
Tận dụng khả năng hỗ trợ thiết kế, xử lý kết quả nghiên cứu (xem hình 4.8) của Platform ELVIS có trong trang bị Phòng thí nghiệm, ta sử dụng chức năng biến đổi AD và hiển thị các kết quả khảo sát thông qua kết nối platform ELVIS-II với máy tính.
Hình 4.8 Kết nối Platform ELVIS-II với máy tính và giao diện thực nghiệm Những thông số cơ bản của bộ ADC trong Platform ELVIS-II.
+ Số lượng kênh analog đầu vào: 8
+ Độ phân giải: 16bit
+ Tốc độ lấy mẫu cực đại: 1.25MS/s đối với một kênh
1.00MS/s đa kênh
+ Dải tín hiệu đầu vào, V: 10; 5; 2; 1; 0,5; 0,2; 0,1; + Trở kháng đầu vào, Omh: 820
4.2.4 Mô tả về giá thử con quay ba chiều
Giá quay ba chiều là thành phần không thể thiếu trong thử nghiệm đo VTG theo VTT. Có thể hình dung giá quay ba chiều là công cụ mô phỏng các chuyển động quay của hệ TĐLK của TBB mà trên đó ta đặt Board Sensores. Trong lựa chọn của tác giả, để tạo các chuyển động quay cho Board Sensores, ta sử dụng giá quay 3 trục thử các loại con quay điện – cơ trong PTN của Bộ môn tên lửa (xem hình 4.9).
Giá quay có cấu tạo của một bộ 3 khung Cardan quay độc lập theo 3 bậc tự do trong 3 mặt phẳng. Chuyển động quay trong hai mặt phẳng (gật và liệng) được thực hiện bằng tay quay (3, 5) và đo bằng kim trên thang khắc độ (4, 6). Độ chính xác trên thang chia độ nhỏ hơn hoặc bằng một vạch chia là 0,1 độ. Chuyển động quay trong mặt phẳng hướng được thực hiện tự động nhờ bộ truyền động điện của giá quay (1). Góc quay hướng được kiểm tra bằng thang chia độ (2). Để gá Board Sensores lên giá quay được chính xác (đồng trục với giá quay) ta sử dụng tấm gá chuyên dụng có hình dạng, kích thước như mô tả ở hình 4.10.
Hình 4.10 Kích thước tấm gá chuyên dụng trên giá quay
1 2 3 5 4 Hình 4.9 Giá quay ba trục
Sơ đồ chức năng kết nối phần cứng được thể hiện trên hình 4.11(a,b)
Quá trình thử nghiệm được tiến hành như sau: board cảm biến từ trường trái đất và cảm biến vận tốc góc được gắn trên giá quay 3 trục tự do. Board cảm biến và giá quay được cấp bằng hai nguồn độc lập. giá quay có thể quay xung quanh các trục của mình bằng tay quay hoặc quay bằng điện nhờ môtơ điện gắn trong giá quay. Các thành phần VTT theo các trục Hx, Hy, Hz và thành phầnVTG theo trục Y đo được từ các cảm biến trên tồn tại dưới dạng điện áp tương tự được biến đổi ADC và ghép nối vào máy tính cá nhân thông qua Board ELVIS II. Các chương trình phần mềm, theo những thuật toán lọc phi tuyến tối ưu đã giới thiệu ở mục 3.3 của chương 3 tiến hành ước lượng trạng thái và đưa ra các kết quả về các thành phần H1x, H1y, H1z, và các VTG 1x,, 1y, , 1z. trên cơ sở của các tham số này mà ta có thể tính ra được các góc gật, góc
Giá quay 3 bậc tự do có gắn board cảm biến từ trường trái đất + cảm biến vận tốc góc Board ELVIS II biến đổi AD và ghép nối với máy
tính Máy tính cá nhân sử lý và thực hiện các thuật toán lọc Bộ nguồn chất lượng cao Kết quả sử lý (H1x. H1y, H1z) ω1x. ω 1y ,ω 1z
a) Sơ đồ chức năng kết nối phần cứng
b) Hình ảnh kết nối phần cứng Hình 4.11 Sơ đồ kết nối phần cứng 1 2 3 4 1. Giá quay 3 bậc tự do 2. Board cảm biến
3. Board ELVIS II để biến đổi AD 4. Máy tính cá nhân
cho TBB.
4.3 Thiết kế và xây dựng phần mềm thực nghiệm
Với mục đích tổng hợp bộ lọc số, việc đầu tiên cần phải thực hiện đó là rời rạc hóa mô hình trạng thái (3.9) ở chương 3. Rời rạc hóa hệ phương trình đó có thể sử dụng các phương pháp xấp xỉ khác nhau, ở đây ta sẽ sử dụng xấp xỉ Euler. Còn việc xấp xỉ hệ phương trình (3.10) có thể thực hiện bằng giải tích. Với việc sử dụng các phương pháp đã nêu ta thu được mô hình rời rạc của bộ lọc như sau
* 1x 1x 1y 1z 1z 1y * 1y 1y 1x 1z 1z 1x * 1z 1z 1z 1y 1y 1x 1x 1x x 1x 1y 1y y 1y 1z H (k 1) H (k) T (k)H (k) (k)H (k) H (k 1) H (k) T (k)H (k) (k)H (k) H (k 1) H (k) T (k)H (k) (k)H (k) T (k 1) (1 ) (k) w (k) T T (k 1) (1 ) (k) w (k) T 1z z 1z T (k 1) (1 ) (k) w (k) T (4.1)
Để đơn giản cho sử dụng ta sẽ viết hệ phương trình (4.1) dưới dạng ma trận – vector như sau:
) k ( w ) t ), k ( x ( f ) 1 k ( x k k (4.2) Ở đây: T z 1 y 1 x 1 x 1 z 1 y 1 x 1 (k) H (k) H (k) H (k) (k) (k) (k) H ) k ( x (4.3) là vector trạng thái. (adsbygoogle = window.adsbygoogle || []).push({});
Để tổng hợp được thuật toán lọc ta cần phải lựa chọn các phép đo sao cho hệ thống là có thể quan sát được. Như đã đề cập ở các chương 2 và 3 thì việc chỉ sử dụng riêng các cảm biến từ trường theo ba trục không thể quan sát
nhất một cảm biến VTG bổ sung. Phương án tốt nhất đã đề cập, ta sẽ sử dụng một cảm biến VTG có trục nhạy trùng với trục OY1 của hệ TĐLK.
Như vậy vector quan sát đối với bộ lọc cần tổng hợp gồm có các phép đo từ trường ba trục theo hệ TĐLK và một phép đo VTG thẳng đứng, vector đo được biểu diễn như sau
T
1x 1y 1z 1y
z(k)H (k) H (k) H (k) (k) v(k)
(4.4)
Ở đây v(k) là vector nhiễu đo, được giả thiết là tạp trắng Gauss có mật độ phổ đã biết.
Như vậy nhiệm vụ của bài toán lọc là ở chỗ, dựa vào các quan sát (phép đo) ở (4.4) bằng cách tốt nhất ta sẽ ước lượng vector trạng thái x(k) ở (4.3) mà động học của các trạng thái này được mô tả bởi hệ phương trình (4.1).
Điểm mới mà ta có được khi sử dụng mô hình toán học đã nêu đó là việc ta sẽ ước lượng chính các thành phần VTT theo ba trục. Hơn nữa, lựa chọn mô hình này sẽ làm đơn giản đi rất nhiều việc thực hiện thuật toán lọc, vì tính các phép đo là hàm tuyến tính của các trạng thái cần ước lượng. Sau đây ta sẽ lần lượt xem xét phương pháp xây dựng các bộ lọc phi tuyến tối ưu, dưới dạng chương trình phần mềm, theo những thuật toán đã giới thiệu ở mục 3.2 của chương 3.
4.3.1 Phần mềm thuật toán Extended Kalman Filter (EKF) 1. Algorit chương trình 1. Algorit chương trình
Lọc Kalman mở rộng EKF dùng để ước lượng các trạng thái/tham số của hệ thống phi tuyến ngẫu nhiên. Tính phi tuyến thể hiện có thể hoặc ở mô hình động học trạng thái, hoặc ở mô hình đo, hoặc có thể ở cả hai mô hình. Tính ngẫu nhiên thể hiện ở sự bất định về mô hình động học và sai số của phép đo.
trình trạng thái, mô hình quan sát là tuyến tính. Hàm vector mô tả động học trạng thái f được xác định từ (4.2), ma trận độ nhạy phép đo H được xác định từ phương trình đo (4.4), các ma trận phương sai nhiễu quá trình Qk và ma trận phương sai nhiễu đo Rk là đã biết. Do đó có thể thực hiện thuật toán lọc Kalman mở rộng theo lưu đồ thuật toán ở hình 4.12
Vector ước lượng tối ưu ˆxk
Khởi tạo bộ lọc 0 0 0 T x 0 0 0 0 ˆx E x ˆ ˆ P E (x x )(x x ) Tính hệ số khuếch đại lọc T T 1 k k k k K P ( )H [HP ( )H R ] Cập nhật phép đo k k k k k ˆ ˆ ˆ x ( ) x ( ) K (z Hx ( )) Tính ma trận phương sai hậu nghiệm
k k k
P ( ) [I K H]P ( )
Thuật toán dự báo một bước
k 1 k ˆ ˆ x ( ) f (x ( )) k ˆ k 1 x x ( ) f A | x T k 1 k 1 k k 1 k P ( ) A P ( )A Q Tăng bước vòng lặp k k 1
Vector đo theo thời gian z1, z2,…
Hình 4.12 Lưu đồ thuật toán lọc EKF
Chương trình thực nghiệm sử dụng kết hợp hai ngôn ngữ lập trình Labview và Matlab để đảm bảo sự thuận tiện theo giao diện chương trình, cũng như khả năng thực hiện các phép toán phức tạp. Để thực hiện thuật toán lọc EKF phần mềm sẽ được tổ chức như sau:
Thứ nhất, phần mềm đảm bảo lấy mẫu các dữ liệu phép đo. Phần này được thực hiện nhờ các subVIs có trong thư viện của Labview. Các phép đo sẽ được lấy mẫu đồng thời, được đưa vào bộ đệm, sau đó đọc từ bộ đệm và đưa vào VI chính để cập nhật phép đo.
Thứ hai, phần mềm đảm bảo thực thi thuật toán. Phần này được thực hiện nhờ VI chính. Được viết trên ngôn ngữ Matlab. Phần này gồm có: