Đầu ra Mô tả Khoảng giá trị Đơn vị
p Tốc độ góc theo trục Xb -249÷249 Deg/s q Tốc độ góc theo trục Yb -249÷249 Deg/s r Tốc độ góc theo trục Zb -249÷249 Deg/s ax Gia tốc dài theo trục Xb -19.62÷19.62 g ay Gia tốc dài theo trục Yb -19.62÷19.62 g az Gia tốc dài theo trục Zb -19.62÷19.62 g
Press Áp suất khí quyển N/m^2
temp Nhiệt độ môi trường Độ C P Tốc độ góc gắn thân
Một yếu tố quan trọng khi sử dụng bộ điều khiển này là định vị chính xác vị trí của thiết bọ bay so với điểm mốc ban đầu. Vì thế bộ lọc Kalman được đề xuất sử dụng để kết hợp GPS và INS (Hình 3.17)
58
Hình 3. 10: Sơ đồ mơ phỏng sử dụng thuật tốn kết hợp INS/GPS
Tín hiệu GPS: Trong thời gian ngắn sẽ cho kết quả khơng chính xác, sai số
có thể trên 10m. Nhưng trong thời gian dài có thể cho kết quả chính xác và ổn định. Khơng có sai số tích lũy theo thời gian.
Tín hiệu INS: Sử dụng cảm biến gia tốc (Accelrometer) và cảm biến tốc độ
góc (Gyrometter) để định vị vị trí tương đối của thiết bị so với điểm mốc ban đầu. Trong thời gian ngắn kết quả đo không bị sai số quá lớn như GPS. Nhược điểm của INS là có sai số tích lũy theo thời gian.
Vì vậy, việc sử dụng Kalman sẽ hạn chế nhược điểm của 2 phương pháp định vị và cho kết quả tốt hơn.
Có 4 khối chính để thực hiện thuật tốn điều khiển tự động: 1: Khối chuyển đổi tọa độ trắc địa (GPS) sang tọa độ NED
2: Khối INS – thực hiện ước tính tọa độ của thiết bị với gốc ban đầu, tọa độ này cũng được đồng bộ về hướng với tọa độ NED bên trên.
3: Bộ lọc Kalman: Sử dụng dữ liệu GPS (đã qua qui đổi hệ tọa độ) và INS để đưa ra vị trí tối ưu.
1
2
3
59
4: Khối dẫn đường – Thực chất mới sử dụng để ổn định tọa độ khi thiết bị bay cất cánh lên thẳng. Vì thế tọa độ điều khiển mong muốn là các hằng số tọa độ gốc (x:0, y: 0, z: 300). Độ cao điều khiển là 300 m.
Bên cạnh đó, dữ liệu GPS và dữ liệu cảm biến IMU thu được thông qua khối mô phỏng động học. Khối này nhận các tín hiệu điều khiển tạo ra đáp ứng các động cơ từ đó mơ phỏng q trình vật lý về động lực học cho thiết bị bay đông thời mô phỏng các khối đo lường IMU và khối GPS.
Bộ lọc Kalman trong kiến trúc mô phỏng được thực hiện lặp vịng qua 2 khâu: - Khâu ước tính dự đốn.
- Khâu cập nhật
Khâu ước tính dự đốn:
Thực hiện dự đốn ví trị sơ bộ (tiên ước), dựa vào INS và ma trận hiệp phương sai lỗi ở thời điểm k. Vì dựa vào ước tính hệ thống INS nên khâu này cần sử dụng kết quả ước tính của bộ lọc Kalman vào thời điểm trước:
Bước 1: Dự đốn vị trí sơ bộ:
𝑥𝑘|𝑘−1 = 𝐹 × 𝑥𝑘−1|𝑘−1+ 𝐵 × 𝑣𝑘
Trong đó: F là ma trận chuyển của mơ hình được định nghĩa: 𝐹 = [1 −∆𝑡
0 1 ]
B là ma trận điều khiển: 𝐵 = [∆𝑡
0] 𝑥𝑘|𝑘−1 là trạng thái ước tính hệ thống
𝑥𝑘−1|𝑘−1 là trạng thái ước tính của bộ lọc Kalman ở thời điểm trước 𝑣𝑘 là tốc độ thơng qua tính tốn hệ thống (INS)
Bước 2: Tiên ước ma trận hiệp phương sai lỗi dựa trên ma trận hiệp phương
sai lỗi trước:
𝑃𝑘|𝑘−1 = 𝐹 × 𝑃𝑘−1|𝑘−1× 𝐹𝑇 + 𝑄𝑘
Trong đó P là ma trận 2x2 có dạng:
𝑃 = [𝑃00 𝑃01 𝑃10 𝑃11]
60
𝑄 = [𝑄0𝑥 𝑄0
𝑣𝑒] × ∆𝑡
Khâu cập nhật
Bước 1: Tính sai khác giữa giá trị đo 𝑧𝑘 và giá trị tiên ước 𝑥𝑘|𝑘−1:
𝑦𝑘 = 𝑧𝑘 − 𝐻 × 𝑥𝑘|𝑘−1
Trong đó H được sử dụng để đưa trạng thái ước tính hệ thống sang khơng gian giá trị đo. Nên thực chất 𝑦𝑘 không phải là ma trận. H gọi là ma trận mơ hình
quan sát:
𝐻 = [1 0]
Bước 2: Dự đoán ma trận hiệp phương cải thiện:
𝑆𝑘 = 𝐻 × 𝑃𝑘|𝑘−1× 𝐻𝑇 + 𝑅
Trong đó R là phương sai đo đạc (do ta chuyển đổi tọa độc trắc địa sang NED nên giá trị của phương sai được tính theo m đói với 3 trục X, Y và Z. Giá trị này thu được trên đánh giá thực nghiệm với từng thiết bị khác nhau. Trong q trình mơ phỏng, giá trị này được thiết lập tùy ý cho phù hợp với yêu cầu về độ chính xác thiết bị đo) :
𝑅 = 𝑣𝑎𝑟(𝑒)
Bước 3: Tính hệ số Kalman (độ lợi Kalman):
𝐾𝑘 = 𝑃𝑘|𝑘−1 × 𝐻𝑇× 𝑆𝑘−1
Có thể hiểu đơn giản ở đây kết hợp 2 trọng số là P và S. P đại diện cho phương pháp tính tốn hệ thống để ước tính kết quả đo. S đại diện cho phép đo trực tiếp. K là giá trị tổng hợp cho 2 trọng số này. Cịn H mang tính chất tốn học đưa 2 trọng số về cùng không gian.
Bước 4: Cập nhật lại giá trị ước tính cuối cùng:
𝑥𝑘|𝑘 = 𝑥𝑘|𝑘−1+ 𝐾𝑘 × 𝑦𝑘
Bước 5: Cập nhật lại ma trận hiệp phương sai số P:
61
Trong đó I được gọi là ma trận đặc trưng, các tham số khác được tính bên trên
𝐼 = [0 1 1 0]
Bước 3 sẽ cho ta kết quả cần tính. Các bước khác để liên tục cập nhật các tham số tính tốn cho bộ lọc Kalman. Nhằm đơn giản các bước mô phỏng, các chiều x,y,z được coi là độc lập và tính bằng các 3 bộ lọc kalman riêng.
3.3.1.1. Khối LLAtoNED
Hình 3. 11: Khối LLA to NED
Chức năng: Chuyển đổi từ tọa độ trắc địa (từ GPS) sang tọa độ NED với gốc
0,0,0 của tọa độ NED được gắn với tọa độ trắc địa có tọa độ LLA_OFFSET như trên hình.
Dữ liệu vào: tọa độ trắc địa Lat, Lon, Alt (Kinh độ, vĩ độ và độ cao). Tọa độ
này thu được từ khối mô phỏng GPS như đã trình bày trong phần trên.
Dữ liệu ra: tọa độ X, Y, Z theo hệ tọa độ NED với gốc ở điểm có tọa độ GPS
62
3.3.1.2. Khối INS
Hình 3. 12: Khối INS
Chức năng: thực hiện thuật tốn INS ước tính vị trí và vận tốc của thiết bị bay
trong quá trình hoạt động. Dựa vào dữ liệu thô thu được từ IMU.
Dữ liệu vào: 𝑝, 𝑞, 𝑟, 𝑎𝑥, 𝑎𝑦, 𝑎𝑧 là giá trị thơ của tốc độ góc theo 3 trục và gia tốc
tịnh tiến theo 3 trục IMU. IMU được mô phỏng trong khối động học.
Dữ liệu ra: 𝑋𝑒, 𝑌𝑒, 𝑍𝑒, 𝑉𝑥𝑒, 𝑉𝑦𝑒, 𝑉𝑧𝑒 là giá trị tọa độ và tốc độ của thiết bị theo
3 trục trong hệ tọa độ NED, dữ liệu này sau đó sẽ được sử dụng trong bộ lọc kalman để ước tính tọa độ kết hợp GPS.
3.3.1.3. Khối Kalman
Hình 3. 13: Khối Kalman
Chức năng: Dựa trên dữ liệu GPS và INS đưa ra kết quả ước tính vị trí tối ưu
phục vụ cho thuật tốn điều khiển vị trí.
Dữ liệu vào:
Tọa độ đã chuyển đổi sang hệ NED của GPS – hay còn gọi là giá trị đo:
𝑋, 𝑌, 𝑍 [𝑚]
Tốc độ và tọa độ ước tính của INS - hay cịn gọi là giá trị ước tính hệ thống:
63
Dữ liệu ra:
Tọa độ đã được tối ưu theo hệ quy chiếu NED: 𝑋_𝑘, 𝑌_𝑘, 𝑍_𝑘 [𝑚]
Hoạt động:
Thực hiện bộ lọc Kalman với từng cặp dữ liệu về tọa độ theo từng phương của trục tọa độ NED. Tức ta sẽ sử dụng riêng bộ lọc cho tọa độ theo phương X, phương Y và phương Z.
Bộ lọc Kalman kết hợp 2 giá trị đo trực tiếp (𝑋, 𝑌, 𝑍 [𝑚]) và giá trị ước tính hệ thống theo thời gian (𝑋𝑒, 𝑌𝑒, 𝑍𝑒[𝑚]; 𝑉𝑥𝑒, 𝑉𝑦𝑒, 𝑉𝑧𝑒 [𝑚/𝑠]), vì vậy cần thiết phải xác định các hiệp phương sai số của GPS và IMU, từ đó tính ra hiệp phương sai số của kết quả sau chuyển đổi LLAtoNED và hiệp phương sai số ước tính vận tốc và tọa độ của INS.
Dưới đây là hình 3.14, 3.15, 3.16 thể hiện giao diện điều khiển của chương trình mơ phỏng Octocopter sau khi khởi chạy:
Trước khi thực hiện mô phỏng cần thiết lập giá trị điều khiển ban đầu. Thiết lập giá trị các kênh điều khiển tại điểm giữa (giá trị tương ứng là 1500)
Set giá trị điều khiển các
kênh là 1500 (vị trí cân bằng) Bắt đầu chương trình
Hình 3.14 - Giải thích các thơng số trên giao diện mơ phỏng
64 8 1 4 3 9 7 6 2 5
Hình 3.15 - Phân tích các thành phần trên giao diện mô phỏng
(1) Khởi chạy hoặc dừng chương trình mơ phỏng
(2) Chọn các thơng số sẽ được hiển thị. Tắt bớt các thông số sẽ giúp việc hiển thị được hiệu quả hơn
(3) Các kênh điều khiển Octocopter:
Ch1: điều khiển nghiêng góc Roll. Giá trị điều khiển 1000÷2000, tương ứng góc nghiêng từ −35𝑜 đến +35𝑜
Ch2: điều khiển nghiêng góc Pitch. Giá trị điều khiển 1000÷2000, tương ứng góc nghiêng từ −35𝑜 đến +35𝑜
Ch3: điều khiển mức ga chung của các động cơ, quyết định đến việc cất cánh hay hạ cánh của Octocopter.
Ch4: điều khiển tốc độ xoay của góc Yaw. Giá trị điều khiển 1000÷2000 (4) Các giá trị xung PWM điều khiển động cơ do thuật tốn điều khiển tính tốn (5) Các thông số trạng thái của Octocopter trong q trình mơ phỏng, đây là các
thơng số mơ phỏng trạng thái không thông qua khối mô phỏng cảm biến. (6) Đồ thị giá trị gia tốc kế theo thời gian (trên khối mô phỏng IMU) đo được.
Trong đó:
65
Đường màu xanh lá là gia tốc gắn thân theo trục Yb Đường màu xanh lam là gia tốc gắn thân theo trục Xb
(7) Đồ thị tốc độ góc do Gyroscope (trên khối mơ phỏng) đo được. Trong đó: Đường màu đỏ là tốc độ góc với trục Zb
Đường màu xanh lá là tốc độ góc theo trục Yb Đường màu xanh lam là tốc độ góc theo trục Xb
(8) Mơ hình đồ họa OctoRotor và tốc độ cập nhật hình ảnh trạng thái mơ phỏng. (9) Tọa độ của Octocopter trong q trình mơ phỏng trên mặt phẳng ngang. Việc
hiển thị này giúp người mơ phỏng có cái nhìn trực quan về quan hệ giữa việc nghiêng góc và thay đổi vị trí của Octocopter.
- Dấu cộng màu xanh-đỏ đánh dấu điểm xuất phát trong q trình mơ phỏng. Đường màu đỏ song song với trục Ye, dường màu xanh song song với Ye.đồ thị mơ phỏng có sự thể hiện không như thông lệ do hướng đầu của thiết bị bay được quy ước là trục Xb trùng với Xe ở thời điểm 0, khi thiết bị bay di chuyển hướng lên so với màn hình có nghĩa đang di chuyển theo trục Xe.
- Dấu trịn màu xanh đánh dấu vị trí di chuyển của Octocopter.
66
3.3.2. Thực hiện kịch bản mô phỏng
Thiết bị bay mô phỏng được thiết lập các thông số giống với các thông số của thiết bị bay thực tế:
Khối lượng :8 kg; kích thước: 1.5 m bề rộng Đường kính cánh quạt: 0.381 m
Lực nâng tối đa của một động cơ: 2.5kg Hệ số động cơ: 400 vòng/phút
Điện áp: 25.3Volt; hiệu suất cực đại động cơ: 82%
Thiết bị bay sẽ thực hiện việc cất cánh thẳng đứng tự động bằng việc để một giá trị mức ga cố định. Từ đó sẽ thơng qua phần mềm mơ phỏng sẽ đánh giá và tính tốn được khoảng sai số điều khiển khi sử dụng 3 phương pháp định vị bằng INS, bằng GPS, và tích hợp INS/GPS sử dụng bộ lọc Kalman (Hình 3.17).
Hình 3.17 – Giao diện thực hiện việc đánh giá sai số khi thực hiện dẫn đường bằng 3 phương pháp khác nhau
67
Kịch bản 1:
Quỹ đạo cất cánh thẳng đứng khi chỉ có tín hiệu GPS được chiếu lên các hệ trục toạ độ tương ứng là XY, XZ, YZ, XYZ.
Hình 3.18 - Đánh giá sai số theo hệ trục toạ độ XY trục toạ độ XY
Hình 3.19 – Đánh giá sai số theo hệ trục toạ độ YZ trục toạ độ YZ
Hình 3.20 - Đánh giá sai số theo hệ trục toạ độ XZ trục toạ độ XZ
Hình 3.21 - Đánh giá sai số theo hệ trục toạ độ XYZ trục toạ độ XYZ
68
Kịch bản 2:
Quỹ đạo cất cánh thẳng đứng khi chỉ có tín hiệu INS được chiếu lên các hệ trục toạ độ tương ứng là XY, XZ, YZ, XYZ
Hình 3.22 - Đánh giá sai số theo hệ trục toạ độ XY trục toạ độ XY
Hình 3.23 - Đánh giá sai số theo hệ trục toạ độ YZ trục toạ độ YZ
Hình 3.24 - Đánh giá sai số theo hệ trục toạ độ XZ trục toạ độ XZ
Hình 3.25 - Đánh giá sai số theo hệ trục toạ độ XYZ trục toạ độ XYZ
69
Kịch bản 3:
Quỹ đạo cất cánh thẳng đứng khi kết hợp INS và GPS sử dụng bộ lọc Kalman được chiếu lên các hệ trục toạ độ tương ứng là XY, XZ, YZ, XYZ
Hình 3.26 - Đánh giá sai số theo hệ trục toạ độ XZ trục toạ độ XZ
Hình 3.27 - Đánh giá sai số theo hệ trục toạ độ XZ trục toạ độ XZ
Hình 3.28 - Đánh giá sai số theo hệ trục toạ độ XZ trục toạ độ XZ
Hình 3.29 - Đánh giá sai số theo hệ trục toạ độ XYZ trục toạ độ XYZ
70