ĐẠI HỌC ĐÀ NẴNG
TRƯỜNG ĐẠI HỌC BÁCH KHOA
_________________

QUẢNG ĐINH CHÁNH

NGHIÊN CỨU GIẢI PHÁP ĐỊNH VỊ CHÍNH XÁC CHO
THIẾT BỊ BAY TỰ ĐỘNG KHÔNG NGƯỜI LÁI DỰA TRÊN
PHƯƠNG PHÁP SENSOR FUSION

Chuyên ngành : KỸ THUẬT CƠ ĐIỆN TỬ
Mã số
: 85.20.11.4

TÓM TẮT LUẬN VĂN THẠC SĨ KỸ THUẬT CƠ ĐIỆN TỬ

Đà Nẵng - Năm 2022


Cơng trình được hồn thành tại
TRƯỜNG ĐẠI HỌC BÁCH KHOA

Người hướng dẫn khoa học: TS. Nguyễn Đình Sơn

Phản biện 1:

TS. Đỗ Thế Cần

Phản biện 2:

TS. Nguyễn Quận

Luận văn sẽ được bảo vệ trước Hội đồng chấm Luận văn tốt nghiệp
thạc sĩ Kỹ thuật cơ điện tử họp tại Trường Đại học Bách khoa vào
ngày 26 tháng 6 năm 2022

Có thể tìm hiểu luận văn tại:
 Trung tâm Học liệu, Đại học Đà Nẵng tại Trường Đại học Bách
khoa
 Thư viện Khoa Cơ khí, Trường Đại học Bách khoa - ĐHĐN

THƯ VIỆN TRƯỜNG ĐẠI HỌC BÁCH KHOA – ĐẠI HỌC ĐÀ NẴNG.

Lưu hành nội bộ


1
CHƯƠNG 1: GIỚI THIỆU
1.1. Lý do chọn đề tài
Một vấn đề phổ biến với các hệ thống theo dõi UAV trước đây
là sự phụ thuộc vào dữ liệu đo chỉ từ một cảm biến duy nhất. Tính hiệu
quả và ổn định của hệ thống UAV có thể bị sụp đổ trong trường hợp
xảy ra sự cố hoặc hư hỏng từ một cảm biến. Hơn nữa, hầu hết tất cả
các cảm biến theo dõi gắn trên UAV đều có lỗi và nhiễu do giới hạn
phần cứng của chúng. Hơn nữa, chúng cũng có nhược điểm đáng kể
là chỉ hoạt động trong những môi trường và điều kiện cụ thể. Đặc biệt,
việc ước lượng tư thế có độ chính xác cao là một thách thức lớn khi
UAV bay trong môi trường lộn xộn có nhiễu động, ví dụ như gió giật.
Một phương tiện bay không người lái UAV (Unmaned Aerial
Vehicle) đang ngày trở nên phổ biến với những ưu thế mà UAV mang
lại như dễ dàng điều khiển, tốc độ và khả năng vận hành cao, giá thành

hợp lí và giúp người vận hành nó tránh được nguy hiểm, UAV đã
nhanh chóng mở rộng sang các ứng dụng khác như nông nghiệp, địa
trắc, cứu hộ hoặc giúp các lực lượng chức năng truy bắt tội phạm.
Sự phát triển của khoa học công nghệ chính là tiền đề cho sự
phát triển của các hệ thống UAV. Một vấn đề thiết yếu trong hệ thống
UAV là việc truyền nhận dữ liệu giữa các bộ phận trong UAV như
cảm biến, bộ điều khiển, thiết bị lưu trữ. Sự phát triển của mạng cảm
biến (sensor network) truyền thông với tốc độ cao và khoảng cách xa
đã dẫn đến sự phát triển của việc kết hợp cảm biến trên UAV sau này.
Hơn thế nữa, các thuật toán cũng được nghiên cứu ra đời để cải thiện
độ chính xác và ổn định cho hệ thống UAV, điều mà trước đây còn rất
hạn chế.

THƯ VIỆN TRƯỜNG ĐẠI HỌC BÁCH KHOA – ĐẠI HỌC ĐÀ NẴNG.

Lưu hành nội bộ


2
Dựa trên những lập luận trên, nghiên cứu này sẽ xem xét giải
quyết vấn đề định vị chính xác cho UAV bằng phương pháp Sensor
Fusion vì nó có thể tăng chất lượng dữ liệu, độ tin cậy của hệ thống.
Đóng góp trong chủ đề này sẽ làm cho hệ thống định vị UAV trở nên
thiết thực và hiệu quả trong thực tế.
1.2 Đối tượng nghiên cứu chính – thuật tốn Sensor Fusion
Gần đây, Sensor Fusion đã thu hút được sự chú ý của giới
nghiên cứu, với các ứng dụng rộng rãi trong khoa học và đời thực, đặc
biệt là trong điều hướng thiết bị di động như robot hoặc drone. Nó
mang lại nhiều lợi ích cho hệ thống tự động: tăng độ chính xác, độ tin
cậy và khả năng chịu lỗi của các đầu vào cảm biến; mở rộng phạm vi

bao phủ không gian và thời gian từ các hệ thống cảm biến; cải thiện
độ phân giải và nhận biết tốt hơn về môi trường xung quanh, đặc biệt
là trong mơi trường động; Sensor Fusion có thể giảm chi phí và độ
phức tạp của hệ thống bằng cách sử dụng các thuật toán xử lý trước
dữ liệu và cho phép sử dụng nhiều loại cảm biến khác nhau mà không
làm thay đổi phần mềm ứng dụng rô bốt cơ bản và vượt qua được
những nhược điểm từ phần cứng. Chính vì vậy, hiện nay trong các hệ
thống địi hỏi độ chính xác cao hoặc để giải quyết hạn chế của cảm
biến, Sensor Fusion là giải pháp bướt ngoặt để kết hợp dữ liệu từ nhiều
nguồn với nhau thông qua thuật tốn để cung cấp kết quả chính xác và
đáng tin cậy nhất.
Hiện nay đất nước Việt Nam đang đẩy mạnh phát triển khoa học
công nghệ vào phục vụ đời sống người dân, qua đó việc áp dụng
Sensor Fusion có thể giúp cho nhiều lĩnh vực như nông nghiệp, quân
sự hoặc địa trắc. Sensor Fusion là cơng nghệ mang tính bước ngoặt để

THƯ VIỆN TRƯỜNG ĐẠI HỌC BÁCH KHOA – ĐẠI HỌC ĐÀ NẴNG.

Lưu hành nội bộ


3
có thể hiện đại hóa nhiều lĩnh vực trong đời sống của người dân. Đây
chính là kỷ nguyên mà Sensor Fusion có đủ điều kiện để có thể phát
triển mạnh mẽ khi thể hiện được nhiều tính ưu việt so với những
phương pháp truyền thống trước đây.
1.3 Phạm vi nghiên cứu – hệ thống định vị UAV
Hiện nay việc nghiên cứu triển khai nhiệm vụ cho các UAV
dưới mọi điều kiện mơi trường và địa hình đang được đẩy mạnh. Sự
phát triển tối ưu của các UAV đã giúp cho nó nhỏ, nhẹ, có khả năng

vận hành cao và mang vác được vật nặng đã kéo theo các ứng dụng
trong các hịa nhà, hầm nơi tín hiệu GPS bị hạn chế bởi các cấu trúc
lớn. Điều này kéo theo yêu cầu phát triển hệ thống với độ chính xác
cao và ít phụ thuộc vào GPS. Hệ thống UAV cần phải được hoạt động
ổn định trong thời gian thực, tốc độ phản ứng nhanh và tiết kiệm năng
lượng. Với việc áp dụng Sensor Fusion sẽ giúp hệ thống kết hợp nhiều
cảm biến có kích thước nhỏ, nhẹ, chi phí thấp và tiết kiệm năng lượng
với nhau, hơn nữa giúp tăng sự ổn định khi có lỗi hoặc hỏng hóc xảy
ra ở cảm biến, tránh hiện tượng UAV mất ổn định và gây nguy hiểm
cho người xung quanh.
Uav là một đối tượng cần độ chính xác và ổn định cao vì nó
bay với tốc độ nhanh và dễ bị các tác nhân xung quanh tác động như
các luồng gió mạnh, tải nặng cũng có thể gây chịng chành cho UAV.
Các hệ thống UAV cũ thường có sai số lớn, khó có thể điều hướng
chính xác dưới tác động ngoại lực do chỉ sử dụng một cảm biến đơn
lẻ để định vị. Điều này cho thấy việc áp dụng Sensor Fusion lên hệ
thống UAV. Nếu ta có thể kết hợp một cảm biến có thể bù trừ được
những nhược điểm từ GPS, hệ thống sẽ trở nên ổn định hơn rất nhiều.

THƯ VIỆN TRƯỜNG ĐẠI HỌC BÁCH KHOA – ĐẠI HỌC ĐÀ NẴNG.

Lưu hành nội bộ


4
Điều này dẫn đến một giải pháp cho vấn đề này, đó là sự kết hợp của
dữ liệu từ nhiều cảm biến để xác định mức chênh lệch của từng cảm
biến và thu được kết quả chính xác hơn.
Sensor Fusion tới nay đã và đang được áp dụng rộng rãi trên các
phương tiện, tác nhân di động nhưng những tài liệu và mơ hình áp

dụng trên UAV cịn rất ít. Việc áp dụng Sensor Fusion trong hệ thống
UAV sẽ giúp đẩy nhanh công nghệ và phát triển những ứng dụng to
lớn mà nó có thể mang lại cho nhiều lĩnh vực và cuộc sống hiện nay.
1.4 Mục tiêu nghiên cứu – cảm biến và thuật toán sử dụng
1.4.1 Mạng các cảm biến sử dụng trong hệ thống định vị UAV
Các phương pháp định vị dựa trên cảm biến đã được phát
triển mạnh hiện nay, có thể kể đến một số phương pháp như:
Inertial navigation systems (INS), global positioning systems
(GPS), motion capture, computer vision, Light Detection and
Ranging(Lidar) và Radio Detection And Ranging(Radar). Mỗi hệ
thống có những ưu nhược điểm riêng và cần lựa chọn phương pháp
phù hợp để áp dụng vào Sensor Fusion trong hệ thống UAV.
Trong đề tài này, các cảm biến GPS, IMU và Radar được
kết hợp với nhau nhằm đưa ra kết quả định vị chính xác nhất.
Thuật tốn Sensor Fusion sẽ được mơ phỏng qua phần mềm
Matlab nơi mà người dùng có thể tùy chỉnh động lực học, môi
trường, cảm biến và bộ lọc của UAV. Kết quả chỉ ra rằng độ chính
xác vị trí của UAV phụ thuộc nhiều nhất vào tính khả dụng của
các phép đo vị trí tốc độ cập nhật cao cùng với tính khả dụng của
phép đo GPS.

THƯ VIỆN TRƯỜNG ĐẠI HỌC BÁCH KHOA – ĐẠI HỌC ĐÀ NẴNG.

Lưu hành nội bộ


5
1.4.2 Thuật toán kết hợp cảm biến trong hệ thống định vị UAV
Thuật toán phổ biến nhất cho phương pháp Sensor Fusion
hiện nay chính là bộ lọc Kalman (KF). Bộ lọc Kalman thực hiện

phương pháp truy hồi đối với chuỗi các giá trị đầu vào bị nhiễu,
nhằm tối ưu hóa giá trị ước đốn trạng thái của hệ thống. Nó đặc
biệt hữu ích khi phân tích dữ liệu nhiễu; chẳng hạn, các cảm biến
trên xe tự hành đôi khi ghi lại dữ liệu khơng đầy đủ hoặc bị nhiễu,
sau đó có thể được sửa chữa bằng thuật tốn Kalman. Bộ lọc
Kalman là một dạng của bộ lọc Bayes. Trong lọc Bayes, thuật tốn
xen kẽ giữa dự đốn (ước tính trạng thái hiện tại) và cập nhật (quan
sát của các cảm biến). Về cơ bản, thuật toán sẽ thực hiện dự đốn
và sửa nó theo giá trị cập nhật ước tính, thực hiện theo hai bước
này cho đến khi đạt được độ chính xác mong muốn. Bộ lọc
Kalman đưa ra dự đốn trong thời gian thực bằng cách sử dụng
mơ hình tốn học dựa trên trạng thái (bao gồm vị trí và tốc độ) và
độ khơng chắc chắn. Nó hoạt động với các chức năng tuyến tính,
nhưng một số đầu ra cảm biến, như dữ liệu radar, khơng tuyến
tính. Bộ lọc Kalman khơng địi hỏi phải biết phương trình động
lực học hệ thống một cách chính xác (system dynamics) đã được
so sánh với các giải pháp định vị dùng bộ quan sát nhiễu
(disturbance observer).
Trong trường hợp các mơ hình là phi tuyến, Bộ lọc Extened
Kalman filter (EKF) được sử dụng, trong đó bằng cách mở rộng
chuỗi Taylor, hệ thống phi tuyến được tuyến tính hóa và xấp xỉ
xung quanh mỗi ước tính trạng thái hiện tại. Bộ lọc Kalman tuyến
tính sau đó được áp dụng để tạo ra ước tính trạng thái tiếp theo.
Nhờ đó ta có thể sử dụng thuật toán EKF để áp dụng cho các dữ

THƯ VIỆN TRƯỜNG ĐẠI HỌC BÁCH KHOA – ĐẠI HỌC ĐÀ NẴNG.

Lưu hành nội bộ



6
liệu cảm biến phi tuyến. Một đặc điểm nổi bật của bộ lọc KF và
EKF đó chính là chúng khơng u cầu khối lượng tính tốn lớn và
dễ dàng áp dụng trong thời gian thực. Điều này làm cho chúng trở
nên phù hợp với hệ thống UAV tong đề tài này.
1.5 Kết quả đóng góp của nghiên cứu
Luận văn đưa ra những kết quả đóng góp chính như sau:


Thiết kế cơng cụ mơ phỏng Matlab cung cấp một mơi trường
có thể tùy chỉnh các thông số của cảm biến, các nhiễu và sai
số từ mơi tường qua đó thể hiện việc kết hợp các cảm biến qua
thuật toán Sensor Fusion.



Cung cấp thuật toán Extended kalman filter thể hiện sự ưu việt
khi kết hợp dữ liệu từ nhiều loại cảm biến khác nhau so với
khi chỉ sử dụng GPS, từ đó đưa ra cho người dùng nhiều sự
lựa chọn khi sử dụng kết hợp cảm biến.

CHƯƠNG 2: TỔNG QUAN
2.1. Mơ hình phương tiện bay không người lái (UAV)
2.1.1. Cấu trúc của một UAV
Bảng 2.1 So sánh giữa các mơ hình UAV
Ưu điểm
Multi-

-Thiết kế nhỏ


Rotor

gọn

Nhược điểm
-Thời gian
bay khơng cao

Ứng dụng

Giá

điển hình

thành

-Giám sát
trên không

THƯ VIỆN TRƯỜNG ĐẠI HỌC BÁCH KHOA – ĐẠI HỌC ĐÀ NẴNG.

Thấp

Lưu hành nội bộ


7
-Khả năng di

-Cứu hộ ở


chuyển linh

không gian

hoạt hơn

hẹp

-Dễ điều khiển

-Giao hàng

-Dễ gắn các

chính xác

thiết bị ngoại vi
và tải trọng
-Tốc độ rất cao

FixedWing

-Tầm bay lớn

-Cần bệ phóng -Truy bắt tội

-Thời gian bay

và hạ cánh


phạm

lâu

-Nguy hiểm

-Triển khai

-Khơng thể lơ

và khó điều

vũ khí qn

lửng và chuyển

khiển hơn

sự

Cao

hướng đột ngột
-Khơng hồn
-Có thể cất
cánh và hạ
VTOL

cánh thẳng

đứng
-Hoạt động bền
bỉ

hảo trong việc
lơ lửng và bay
về phía trước
-Khó điều
khiển và vẫn

-Vẽ bản đồ
trên khơng

Cao

đang trong
q trình phát
triển

Qua bảng so sánh trên, có thể thấy multirotor là tối ưu cho các
ứng dụng cần định vị với độ chính xác cao. Trong cấu hình của
multirotor, lực nâng được tạo từ các cánh quạt .Các cánh quạt này kiểm

THƯ VIỆN TRƯỜNG ĐẠI HỌC BÁCH KHOA – ĐẠI HỌC ĐÀ NẴNG.

Lưu hành nội bộ


8
soát hướng và độ cao của máy bay bằng cách thay đổi tốc độ của mỗi

cánh quạt để thay đổi lượng lực đẩy và mô-men xoắn được tạo ra. Điều
này mang lại cho máy bay một bộ đặc tính xử lý độc đáo, cho phép nó
bay với độ chính xác cực cao trong cả khơng gian kín và khơng gian
mở. Động cơ đa năng bốn cánh quạt (quadcopter) là thiết kế phổ biến
nhất vì nó cung cấp sự cân bằng tốt nhất giữa lực nâng, khả năng kiểm
soát, khả năng cơ động và chi phí.
2.1.2 Những yếu tố gây nhiễu trên UAV
2.2 Các loại cảm biến
2.2.1 Cảm biến IMU
2.2.2 Cảm biến GPS
2.2.3 Cảm biến hình ảnh
2.2.4 Cảm biến Lidar
2.2.5 Cảm biến Radar
2.2.6 Kết luận
Việc lựa chọn cảm biến cho phương pháp Sensor Fusion sử
dụng trên hệ thống UAV phù hợp với những yêu cầu như độ chính xác
cao, giá thành và chi phí vận hành thấp, có thể phù hợp với nhiều loại
mơi trường khác nhau và khối lượng tính toán đáp ứng trong thời gian
thực là rất quan trong. Trong thực tế các cảm biến sẽ có nhiễu và tùy
mức độ nhiễu khâu xử lý trước khi áp dụng vào thuật toán là cần thiết.
Để đáp ứng yêu cầu đề tài, những cảm biến sẽ được lựa chọn là GPS,
IMU và RADAR. Bên dưới là bảng phân tích so sánh ưu và nhược
điểm của các loại cảm biến.

THƯ VIỆN TRƯỜNG ĐẠI HỌC BÁCH KHOA – ĐẠI HỌC ĐÀ NẴNG.

Lưu hành nội bộ


9

Bảng 2.2 So sánh ưu và nhược điểm các loại cảm biến
(Chú thích: ●○○ khơng tốt, ●●○: tốt, ●●● rất tốt)
GPS

IMU

RADAR

LIDAR

CAMERA

●○○

●●●

●○○

●●○

●●●

●○○

●●○

●●●

●●○


●●●

●○○

●●●

●●●

●●○

●●○

●○○

●●●

●●●

●●●

●●●

●●○

●●○

●●●

●○○


●○○

Kích thước

●●●

●●●

●●●

●○○

●●●

Giá thành

●●●

●●●

●●○

●○○

●●●

Độ phân
giải
Độ chính
xác

Ảnh hưởng
bởi mơi
trường
Phụ thuộc
vào tín hiệu
vơ tuyến
Ảnh hưởng
bởi tốc độ

2.3 Các loại bộ lọc
Sensor Fusion là sự kết hợp của dữ liệu cảm quan hoặc dữ liệu
thu được từ các nguồn khác nhau sao cho thông tin kết quả có độ khơng
chắc chắn thấp hơn so với những gì có thể có nếu các nguồn này được
sử dụng riêng lẻ. Trong trường hợp cảm biến sai lệch nhiều so với thực
tế do ảnh hưởng bởi thời tiết, tầm nhìn, tốc độ, vị trí thì việc thu thập
dữ liệu từ nhiều cảm biến sẽ giúp cải thiện độ chính xác của kết quả

THƯ VIỆN TRƯỜNG ĐẠI HỌC BÁCH KHOA – ĐẠI HỌC ĐÀ NẴNG.

Lưu hành nội bộ


10
thu thập được. Phương pháp Sensor Fusion mang tới những lợi ích ưu
việt như tăng chất lượng dữ liệu mà hệ thống nhận được, làm tăng độ
tin cậy của hệ thống, nó có thể giúp ước tính các trạng thái khơng đo
được và tăng vùng phủ sóng.

Nguồn
dữ liệu

Cảm biến 1
Cảm biến 2

Trạng thái hệ thống

Vị trí
SENSOR FUSION

Tốc độ

Mơ hình
tốn học

Hình 2.1 Nguyên lý hoạt động của phương pháp Sensor Fusion
Trong hệ thống UAV, Sensor Fusion làm tăng số lượng cảm
biến thu thập, đa dạng hóa các loại cảm biến và sử dụng các mơ hình
tốn học để tổng hợp và tinh chỉnh thơng tin do cảm biến thu thập. Vì
hợp nhất cảm biến cho phép xác định vị trí, định vị, phát hiện và theo
dõi chính xác hơn, nên nó cải thiện nhận thức tình huống của hệ thống
tự hành và làm cho các hệ thống nhất quán, chính xác và đáng tin cậy
hơn. Nó cũng có thể cho phép chúng tơi kết hợp các cảm biến có tốc
độ lấy mẫu khác nhau: thơng thường, cảm biến chính xác với tốc độ
lấy mẫu thấp và cảm biến kém chính xác hơn với tốc độ lấy mẫu cao.
Dưới đây là một số thuật toán phổ biến đã và đang được phát triển sử
dụng cho phương pháp Sensor fusion.

THƯ VIỆN TRƯỜNG ĐẠI HỌC BÁCH KHOA – ĐẠI HỌC ĐÀ NẴNG.

Lưu hành nội bộ



11
2.3.1 Bộ lọc complementary
2.3.2 Bộ lọc Kalman
Để hiểu việc áp dụng Kalman Filter trong phương pháp Sensor
Fusion, ta lấy ví dụ cho việc theo dõi một đối tượng như robot hoặc
UAV.

Mơ hình đối tượng đang được xem xét

Nhận biết ban đầu của hệ thống tại thời điểm t=0. Phân bố Gaussian
màu đỏ đại diện cho niềm tin(sự chắc chắn) ban đầu về vị trí của
đối tượng. Mũi tên chỉ sang phải biểu thị vận tốc ban đầu đã biết
của đối tượng.

Bước dự đốn, dự đốn của vị trí đối tượng tại t=1 với mức độ
không chắc chắn được thể hiện.

THƯ VIỆN TRƯỜNG ĐẠI HỌC BÁCH KHOA – ĐẠI HỌC ĐÀ NẴNG.

Lưu hành nội bộ


12

Bước đo đạc, phân bố Gaussian màu xanh thể hiện dữ liệu đo vị trí
tại thời điểm t=1 với mức độ khơng chắc chắn trong phép đo nhiễu
đó

Bước cập nhật, phân bố Gaussian màu xanh là sự kết hợp của kết

quả đo và dự đốn vị trí tại thời điểm t=1 cho thấy sự ước lượng tốt
nhất về vị trí của đối tượng
Hình 2.2 Nguyên lý bộ lọc Kalman filter
Kalman Filtering sử dụng các phép đo khơng hồn hảo được
quan sát theo thời gian và tạo ra các ước tính của các biến chưa biết.
Bộ lọc Kalman khơng cần tồn bộ lịch sử của các phép đo và trạng
thái trong q khứ để thực hiện cơng việc của nó, nó chỉ sử dụng phép
đo đầu vào hiện tại và trạng thái cũng như độ khơng đảm bảo được
tính tốn trước đó. Bộ lọc này khơng cần mơ hình tốn học của đối
tượng để ước lượng kết quả. Đây là tính ưu việt của bộ lọc Kalman
Filter khiến nó được áp dụng rộng rãi hiện nay.
2.3.3 Bộ lọc Kalman mở rộng (Extended kalman filter)
Trong phần trước, bộ lọc Kalman chỉ áp dụng được khi cả mơ
hình q trình và mơ hình đo lường đều là các q trình Gaussian tuyến
tính. Nhiễu của các phép đo và của quá trình chuyển đổi trạng thái phải

THƯ VIỆN TRƯỜNG ĐẠI HỌC BÁCH KHOA – ĐẠI HỌC ĐÀ NẴNG.

Lưu hành nội bộ


13
là phân bố Gaussian. Hàm chuyển đổi trạng thái và phép đo sang hàm
trạng thái phải tuyến tính.
Nếu cung cấp cho phân bố Gaussian một hàm tuyến tính thì kết
quả chúng ta thu được cũng sẽ là hàm tuyến tính. Ngược lại, đối với
trường hợp những hàm khơng tuyến tính thì kết quả nhận được sẽ
khơng phải là phân bố Gaussian. Trong trường hợp này sẽ không thể
áp dụng bộ lọc Kalman Filter, tính phi tuyến tính phá hủy Gaussian và
khơng có ý nghĩa khi tính giá trị trung bình và phương sai.

Với trường hợp dữ liệu cảm biến không trả về hàm tuyến tính.
Ví dụ đối với cảm biến Radar trả về dữ liệu là góc đơn vị rad nên hàm
trả về của chúng sẽ là hàm lượng giác (khơng tuyến tính). Ý tưởng để
giải quyết bài tốn này là ta sẽ tuyến tính hóa các hàm phi tuyến, khi
đó có thể áp dụng các kết quả đã có trong bộ lọc Kalman Filter. Đây
chính là chìa khóa cho thuật toán Extended Kalman Filter. Bộ lọc này
sẽ sử dụng một phương pháp gọi là First Order Taylor Expansion
(chuỗi Taylor) để tuyến tính hóa các hàm phí tuyến.
2.3.4 Bộ lọc hạt (Particle Filter)
2.3.5 Mạng nơ-ron (Neural network)
2.3.6 Kết luận
Bảng 2.3 Tóm tắt ưu nhược điểm của các bộ lọc thuật toán trong
Sensor Fusion
Thuật

Kalman

Extended

Particle

Neural

toán

Filter

Kalman

Filter


Network

Filter

THƯ VIỆN TRƯỜNG ĐẠI HỌC BÁCH KHOA – ĐẠI HỌC ĐÀ NẴNG.

Lưu hành nội bộ


14
Mơ hình

Tuyến tính

Phi tuyến

Phi tuyến

Phi tuyến

Gaussian

Gaussian

Khơng

Khơng

phải


phải

Gaussian

Gaussian

Cao

Cao

Khơng cần

Cần

tốn học
Phân bố

Chi phí và

Thấp

khối

Thấp và
vừa

lượng tính
tốn
Cần phải


Khơng cần

Khơng cần

dạy mơ
hình thực
tế
Với u cầu cho hệ thống UAV đặt ra phải hoạt động ổn định
trong nhiều điều kiện khác nhau, lựa chọn chính xác thuật toán cho đề
tài là rất quan trọng. Việc áp dụng nhiều cảm biến với dữ liệu phi tuyến
là bất khả thi với thuật toán Kalman Filter. Đối với thuật tốn Particle
rất tốn tài ngun tính tốn và độ phức tạp cao nên sẽ khó khăn cho
việc áp dụng cho hệ thống UAV trong thời gian thực. Tương tự đối
với Neural network, khó có thể thích nghi với hệ thống động học động
trong UAV và kiểm sốt q trình xử lý dữ liệu. Điều đó đưa đến kết
luận sử dụng thuật tốn phù hợp với u cầu đề tài đó là Extended
Kalman Filter.

THƯ VIỆN TRƯỜNG ĐẠI HỌC BÁCH KHOA – ĐẠI HỌC ĐÀ NẴNG.

Lưu hành nội bộ


15
2.4. Các hệ thống định vị đang được sử dụng trong hệ thống UAV
2.4.1 Hệ thống định vị chỉ sử dụng GPS
2.4.2 Hệ thống định vị GPS kết hợp với Lidar
2.4.3 Hệ thống định vị Radar
2.4.4 Hệ thống định vị bằng Camera

2.5 Kết luận
Chương này đã so sánh và đưa ra kết luận:


Về mơ hình động học: Sử dụng một quadrotor



Về cảm biến: sử dụng RTK, IMU, Radar



Về bộ lọc thuật toán: Sử dụng Extended Kalman Filter

CHƯƠNG 3: THIẾT KẾ CHƯƠNG TRÌNH SỬ DỤNG
PHƯƠNG PHÁP SENSOR FUSION
3.1 Bộ lọc Extended Kalman
3.1.1 Cấu trúc chung của bộ lọc Extended Kalman

Hình 3.1 Sơ đồ nguyên lý bộ lọc Kalman mở rộng.

THƯ VIỆN TRƯỜNG ĐẠI HỌC BÁCH KHOA – ĐẠI HỌC ĐÀ NẴNG.

Lưu hành nội bộ


16

Hình 3.2 Sơ đồ thuật tốn sử dụng bộ lọc Extended Kalman.


THƯ VIỆN TRƯỜNG ĐẠI HỌC BÁCH KHOA – ĐẠI HỌC ĐÀ NẴNG.

Lưu hành nội bộ


17
Khi bắt đầu thuật toán, các giá trị trạng thái ban đầu x0 , ma trận
hiệp phương sai P0 , nhiễu quá trình Q 0 và nhiễu phép đo R 0 cần phải
được thiết lập. Sau đó, vịng lặp chính của thuật tốn sẽ gồm hai q
trình dự đốn và cập nhật trạng thái. Khi có giá trị đo được từ một
trong các cảm biến từ hệ thống, ma trận trạng thái 𝑥̂𝑘|𝑘−1 và ma trận
hiệp phương sai 𝑃𝑘|𝑘−1 sẽ được dự đốn và biểu diễn bởi cơng thức:
𝑥̂𝑘|𝑘−1 = 𝑓(𝑥̂𝑘−1|𝑘−1 ,𝑢𝑘 )
𝑃𝑘|𝑘−1 = 𝐹𝑘 𝑃𝑘−1|𝑘−1 𝐹𝑘𝑇 +𝑄𝑘

(3.1)

Ở bước cập nhật tiếp theo, kết quả ước tính trạng thái 𝑥𝑘|𝑘 với
ma trận hiệp phương sai 𝑃𝑘|𝑘 sẽ là sự kết hợp giữa hai phân bố
Gaussian (phép nhân giữa hai phân phối) của trạng thái dự đoán và giá
trị đo được dựa trên các công thức sau:
𝑦𝑘 = 𝑧𝑘 − ℎ(𝑥̂𝑘|𝑘−1 )
𝑆𝑘 = 𝐻𝑘 𝑃𝑘|𝑘−1 𝐻𝑘𝑇 + 𝑅𝑘
𝐾𝑘 = 𝑃𝑘|𝑘−1 𝐻𝑘𝑇 𝑆𝑘−1

(3.2)

𝑥𝑘|𝑘 = 𝑥̂𝑘|𝑘−1 + 𝐾𝑘 𝑦𝑘
𝑃𝑘|𝑘 = (I − 𝐾𝑘 𝐻𝑘 ) 𝑃𝑘|𝑘−1
Q trình dự đốn và cập nhật sẽ được lặp lại liên tục theo thời

gian thực để đạt được kết quả ước tính tối ưu nhất.
3.1.2 Bộ lọc extended kalman filter cho trường hợp sử dụng cảm
biến GPS và RADAR
Thuật toán sẽ kết hợp giá trị đo từ cảm biến GPS và RADAR
để có ước tính về vị trí (𝑃𝑥 , 𝑃𝑦 ) và vận tốc (𝑣𝑥 , 𝑣𝑦 ) của UAV. Cảm biến
GPS sẽ cung cấp vị trí UAV ở tọa độ đề các, trong khi đó cảm biến

THƯ VIỆN TRƯỜNG ĐẠI HỌC BÁCH KHOA – ĐẠI HỌC ĐÀ NẴNG.

Lưu hành nội bộ


18
RADAR. Cảm biến radar có thể đo tốc độ trong đường ngắm của nó
(𝜌̇ ) bằng cách sử dụng một thứ gọi là hiệu ứng doppler. Nó cũng có
thể đo khoảng cách của các đối tượng gần đó có thể dễ dàng chuyển
đổi sang tọa độ cực (𝜌, 𝜃) với 𝜌 hay r chính là khoảng cách từ điểm
gốc đến vị trí cần đo.
Ma trận x giữ ước tính về trạng thái của đối tượng UAV đang
được theo dõi, tổng cộng là bốn giá trị: vị trí và vận tốc trong tọa độ
đề các 𝑥
Ma trận hiệp phương sai P thể hiện độ không chắc chắc của các
trạng thái hệ thống. Trong đó mỗi phần tử của ma trận là hiệp phương
sai của hai biến. Ta có ma trận hiệp phương sai P =
2
0
0
0
𝜎𝑝𝑥
2

0
𝜎𝑝𝑦 0
( 0
)
2
0 𝜎𝑣𝑥 0
0
2
0 𝜎𝑣𝑦
0
0
Bảng 3.1 Ma trận giá trị đo và hiệp phương sai của cảm biến GPS và
RADAR
GPS

RADAR

𝑝𝑥
z = (𝑝 )
𝑦

√𝑝𝑥2 + 𝑝𝑦2

𝜌
𝑝𝑥
𝜑
z = ( ) = arctan(𝑝𝑦 )
𝜌̇
𝑝𝑥 𝑣𝑥 +𝑝𝑦 𝑣𝑦
(


𝜎𝑥2
R=(
0

0
)
𝜎𝑦2

𝜎𝜌2
R=(0
0

2
√𝑝𝑥2 +𝑝𝑦

0
𝜎𝜑2

0
0)

0

𝜎𝜌̇2

)

Thật không may, giá trị cảm biến RADAR ở tọa độ cực nên sẽ
khơng thể trích xuất trực tiếp. Điều này làm cho việc trích xuất khơng


THƯ VIỆN TRƯỜNG ĐẠI HỌC BÁCH KHOA – ĐẠI HỌC ĐÀ NẴNG.

Lưu hành nội bộ


19
tuyến tính và khơng thể áp dụng trong cơng thức của bộ lọc Kalman.
Ta cần sử dụng ma trận Jacobian để tuyến tính hóa sự khơng tuyến
tính này dựa trên phương pháp chuỗi Taylor:
𝜕h(𝜇)

h(x) ≈ h(𝜇) + ⏟
(x- 𝜇)
𝜕𝑥

(3.3)

𝐻𝑗

Như vậy đạo hàm của h (x) đối với x được gọi là Jacobians và
nó sẽ là một ma trận chứa tất cả các đạo hàm riêng. Cụ thể hơn đối với
cảm biến RADAR, hàm h mô tả ba thành phần: 𝜌, 𝜃, 𝜌̇ , và trạng thái
là một vectơ có bốn thành phần (𝑝𝑥 , 𝑝𝑦 , 𝑣𝑥 , 𝑣𝑦 ). Từ đó ta có ma trận
Jacobian như sau:
3.1.3 Bộ lọc extended kalman filter cho trường hợp sử dụng cảm
biến GPS và IMU

Hình 3.3 Mơ hình hệ thống UAV.
Để xác định vị trí của rơ bốt trên mặt phẳng, một mối quan hệ

được thiết lập giữa hệ quy chiếu toàn cục của mặt phẳng và hệ quy
chiếu cục bộ của rô bốt, như trong Hình. Các trục xác định hệ quy
chiếu tồn cục và hệ quy chiếu rơ bốt tương ứng . Vị trí của rơ bốt
trong mặt phẳng hồn tồn được xác định bởi 3 biến x, y và θ. Có thể

THƯ VIỆN TRƯỜNG ĐẠI HỌC BÁCH KHOA – ĐẠI HỌC ĐÀ NẴNG.

Lưu hành nội bộ


20
thấy có hai đầu vào v và 𝜔 cho hệ thống. Trong đó vận tốc tuyến tính
v của UAV ln hướng theo hướng của hệ quy chiếu của rô bốt và vận
tốc góc 𝜔 có thể được coi là tốc độ quay của hệ quy chiếu cục bộ đối
với hệ quy chiếu toàn cục. Ta xét véc tơ trạng thái của hệ thống gồm
6 giá trị 𝑥𝑘 = (𝑝𝑥𝑘 , 𝑝𝑦𝑘 , 𝜃𝑘 , 𝜔𝑘 , 𝑣𝑘 , 𝑎𝑘 ). Với 𝑝𝑥𝑘 , 𝑝𝑦𝑘 là giá trị vị trí
theo phương x,y và 𝑣𝑘 , 𝑎𝑘 là vận tốc, gia tốc của đối tượng.
Giả sử hệ thống có gia tốc và gia tốc góc khơng đổi, ta có mơ
hình động học của hệ thống:
𝑝𝑥𝑘
𝑝𝑦𝑘
𝜃𝑘
𝑥𝑘 =
= 𝑓(𝑥𝑘−1 , 𝑢𝑘 )
𝜔𝑘
𝑣𝑘
( 𝑎𝑘 )
𝑥𝑘−1 + 𝑣𝑘−1 ∆𝑡𝑐𝑜𝑠(𝜃𝑘−1 )
𝑦𝑘−1 + 𝑣𝑘−1 ∆𝑡𝑠𝑖𝑛(𝜃𝑘−1 )
𝜃𝑘−1 + 𝜔𝑘−1 ∆𝑡

=
𝜔𝑘−1
𝑣𝑘−1 + 𝑎𝑘−1 ∆𝑡
𝑎𝑘−1
(
)

(3.4)

𝐹𝑗 là ma trận Jacobian để chuyển đổi trạng thái và 𝑄𝑘 là nhiễu
quá trình. Áp dụng cơng thức tuyến tính hóa ta có:
𝜕𝑓(𝑥)

1
0
= 0
0
0
(0

0
1
0
0
0
0

𝐹𝑗 = 𝜕𝑥 |x=𝑥̂𝑘−1|𝑘−1
−𝑠𝑖𝑛(𝜃𝑘−1 )𝑣𝑘−1 ∆𝑡 0 𝑐𝑜𝑠(𝜃𝑘−1 )∆𝑡
𝑐𝑜𝑠(𝜃𝑘−1 )𝑣𝑘−1 ∆𝑡

0 𝑠𝑖𝑛(𝜃𝑘−1 )∆𝑡
1
∆𝑡
0
0
1
0
0
0
1
0
0
0

THƯ VIỆN TRƯỜNG ĐẠI HỌC BÁCH KHOA – ĐẠI HỌC ĐÀ NẴNG.

0
0
0
0
∆𝑡
1)

(3.5)

Lưu hành nội bộ


21
3.2 Đánh giá và kết luận

CHƯƠNG 4: MÔ PHỎNG VÀ KẾT QUẢ
4.1 Mơ phỏng thuật tốn EKF bằng phần mềm Matlab
4.1.1 Mô phỏng quỹ đạo bay và dữ liệu cảm biến của UAV
4.1.2 Thiết lập thông số đầu vào bộ lọc EKF cho các trường hợp kết
hợp cảm biến
4.2 Kết quả mơ phỏng

Hình 4.1 Kết quả sử dụng bộ lọc

Hình 4.2 Kết quả sử dụng bộ

EKF đối với chỉ cảm biến GPS

lọc EKF đối với chỉ cảm biến
RADAR

THƯ VIỆN TRƯỜNG ĐẠI HỌC BÁCH KHOA – ĐẠI HỌC ĐÀ NẴNG.

Lưu hành nội bộ


22

Hình 4.3 Kết quả sử dụng bộ

Hình 4.4 Kết quả sử dụng bộ lọc

lọc EKF khi kết hợp cảm

EKF khi kết hợp cảm biến GPS và


biến GPS và RADAR

IMU

Hình 4.5 Kết quả sử dụng bộ

Hình 4.6 So sánh kết quả giữa các

lọc EKF kết hợp 3 cảm biến

trường hợp kết hợp cảm biến

GPS, RADAR và IMU

THƯ VIỆN TRƯỜNG ĐẠI HỌC BÁCH KHOA – ĐẠI HỌC ĐÀ NẴNG.

Lưu hành nội bộ


23

Hình 4.7 Thử nghiệm khi cảm biến RADAR bị lỗi một khoảng
thời gian
4.3 Nhận xét và kết luận
Từ kết quả đồ thị mơ phỏng, có thể thấy độ chính xác của vị trí
hệ thống càng được cải thiện khi kết hợp nhiều dữ liệu đo từ nhiều
cảm biến hơn. Việc sử dụng cảm biến GPS riêng lẻ có RMSE là 0.55
sẽ đem lại kết quả thấp hơn rất nhiều so với việc kết hợp ba loại cảm
biến GPS, RADAR và IMU có RMSE là 0.11. Qua đó cho thấy sự tối

ưu của việc áp dụng phương pháp Sensor Fusion kết hợp nhiều loại
cảm biến cho hệ thống định vị UAV. Việc so sánh giữa nhiều trường
hợp khác nhau giúp người dùng cân nhắc sử dụng kết hợp cảm biến
phù hợp để đáp ứng yêu cầu thực tế.
Hơn thế nữa, ở trường hợp cảm biến RADAR bị hỏng trong một
khoảng thời gian ngắn, kết quả ước tính sẽ bị lệch xa hơn so với dữ
liệu thực tế nhưng sau đó đã thu hẹp khoảng cách và bắt lại đường đi
thực. Nó cho thấy khả năng chống nhiễu tốt và trường hợp cảm biến

THƯ VIỆN TRƯỜNG ĐẠI HỌC BÁCH KHOA – ĐẠI HỌC ĐÀ NẴNG.

Lưu hành nội bộ