ĐẠI HỌC QUỐC GIA TP.

HCM

TRƯỜNG ĐẠI HỌC BÁCH KHOA

HỌ VÀ TÊN

HUỲNH THANH TUẤN

TÊN ĐỀ TÀI LUẬN VĂN THẠC SĨ

NGHIÊN CỨU TỰ HÀNH ROBOT DỰA TRÊN PHƯƠNG

PHÁP KẾT HỢP NHIỀU CẢM BIẾN

(Research on autonomous robots based on the multiple sensor fusion methods)

Chuyên ngành: CƠ ĐIỆN TỬ

Mã số: 8520114

Số điện thoại: 0936641141

Email: 1870245@hcmut.edu.vn

LUẬN VĂN THẠC SĨ

TP. HỒ CHÍ MINH, tháng 12 năm 2021

I
 ĐẠI HỌC QUỐC GIA TP.HCM CỘNG HOÀ XÃ HỘI CHỦ NGHĨA VIỆT NAM

TRƯỜNG ĐẠI HỌC BÁCH KHOA Độc lập - Tự do - Hạnh phúc

NHIỆM VỤ LUẬN VĂN THẠC SĨ

Họ và tên học viên: HUỲNH THANH TUẤN MSHV: 1870245

Ngày tháng năm sinh: 11/05/1992 Nơi sinh: Tỉnh Bình Định

Chuyên ngành: Kỹ thuật Cơ điện tử Mã số: 8520114

I. TÊN ĐỀ TÀI:
Nghiên cứu tự hành robot dựa trên phương pháp kết hợp nhiều cảm biến.
(Research on autonomous robots based on the multiple sensor fusion methods)
II. NHIỆM VỤ VÀ NỘI DUNG:
- Thiết kế giải thuật kết hợp nhiều cảm biến để định vị robot tự hành.
- Mô phỏng giải thuật kết hợp nhiều cảm biến để định vị robot tự hành.
- Thiết kế bộ điều khiển robot bám quỹ đạo.
- Mô phỏng điều khiển robot bám quỹ đạo dựa trên sự kết hợp nhiều cảm biến.
III. NGÀY GIAO NHIỆM VỤ: 06/09/2021
V. NGÀY HOÀN THÀNH NHIỆM VỤ: 28/12/2021
IV. CÁN BỘ HƯỚNG DẪN: PGS.TS. NGUYỄN TẤN TIẾN

Tp. HCM, ngày … tháng … năm 20…

CÁN BỘ HƯỚNG DẪN CHỦ NHIỆM BỘ MÔN ĐÀO TẠO

(Họ tên và chữ ký) (Họ tên và chữ ký)

TRƯỞNG KHOA
(Họ tên và chữ ký)

II
 LỜI CẢM ƠN

Trước tiên tôi xin phép được gửi lời cảm ơn chân thành đến PGS. TS. NGUYỄN
TẤN TIẾN là giảng viên hướng dẫn trực tiếp của tôi. Cảm ơn thầy đã luôn hỗ trợ mỗi
khi tôi gặp vấn đề hoặc có câu hỏi về vấn đề nghiên cứu của mình. Thầy vẫn luôn cho
phép tôi tự do bày tỏ quan điểm đồng thời đưa ra những nhận xét, góp ý, dẫn dắt tôi
đi đúng hướng trong suốt thời gian nghiên cứu, thực hiện đề tài luận văn.

Tôi cũng xin cảm ơn các thầy cô trong khoa Cơ Khí - Trường Đại học Bách Khoa
- Đại học quốc gia tp. Hồ Chí Minh đã truyền đạt cho tôi những kiến thức về chuyên
ngành trong suốt thời gian học tập để tôi có được nền tảng kiến thức hỗ trợ rất lớn
cho tôi trong quá trình làm luận văn.

Tôi cũng muốn bày tỏ sự biết ơn đến TS. DƯƠNG VĂN TÚ, anh đã có nhiều nhận
xét và góp ý rất có giá trị giúp tôi hoàn thiện luận văn này.

Cuối cùng, tôi xin gửi lời cảm ơn đến gia đình và bạn bè vì đã luôn hỗ trợ tôi và
khuyến khích liên tục trong suốt những năm học tập và qua quá trình nghiên cứu và
viết luận văn này. Kết quả này sẽ không thể có được nếu không có họ.

Xin chân thành cảm ơn!

III
 TÓM TẮT LUẬN VĂN

Nhiệm vụ của luận văn tập trung vào việc nghiên cứu phương pháp kết hợp dữ liệu
của nhiều loại cảm biến để xác định vị trí và góc của robot tự hành trong quá trình di
chuyển. Phương pháp kết hợp dữ liệu cảm biến được áp dụng trong luận văn là sử
dụng bộ lọc Kalman mở rộng để kết hợp hai loại cảm biến có ưu điểm bổ sung cho
nhau trong bài toán định vị robot là cảm biến định vị cục bộ và định vị toàn cục:
Odometry và GPS, UWB.

Luận văn còn trình bày thiết kế bộ điều khiển robot bám theo quỹ đạo dựa trên sự
kết hợp nhiều cảm biến.

Luận văn đã hoàn thành yêu cầu thiết kế giải thuật kết hợp dữ liệu cảm biến và
thiết kế bộ điều khiển để điều khiển robot bám quỹ đạo. Trên cơ sở mô hình đã chế
tạo tiến hành thực nghiệm khả năng bám quỹ đạo và kết quả thực nghiệm đáp ứng
được các yêu cầu của đề bài.

IV
 ABSTRACT

The task of the thesis focuses on studying the method of combining data of many
types of sensors to determine the position and pose of the self-propelled robot during
the movement. The method of combining sensor data applied in the thesis is to use
an extended Kalman filter to combine two types of sensors with complementary
advantages in the robot positioning problem, local localization sensors and global
localization sensors. Sensor used in the thesis Odometry and GPS, UWB.

The thesis also presents the design of the controller to control the robot to follow the
trajectory based on the combination of many sensors.

The thesis has completed the requirements of designing an algorithm that

combines sensor data and designing a controller to control the orbiting robot. On the
basis of the manufactured model, conduct orbital tracking and experimental results
meet the requirements of the topic.

V
 LỜI CAM ĐOAN

Tôi xin cam đoan tất cả nội dung trong luận văn này không sao chép các công trình
nghiên cứu của các cá nhân hay tổ chức khác.

Tôi thực hiện nghiêm túc việc trích dẫn các công trình bài báo, sách, tham luận và
các nguồn khác có được sử dụng trong luận văn.

Tác giả luận văn

Huỳnh Thanh Tuấn

VI
 MỤC LỤC:
NHIỆM VỤ LUẬN VĂN THẠC SĨ ................................................................................................ II
LỜI CẢM ƠN .................................................................................................................................. III
TÓM TẮT LUẬN VĂN .................................................................................................................. IV
ABSTRACT ...................................................................................................................................... V
LỜI CAM ĐOAN ............................................................................................................................ VI
MỤC LỤC:...................................................................................................................................... VII
MỤC LỤC HÌNH ẢNH: ................................................................................................................. IX
Chương 1. MỞ ĐẦU: .................................................................................................................. 1
1.1. Nhu cầu thực tiễn của đề tài: .............................................................................................. 1
1.1.1. Đặt vấn đề: ................................................................................................................. 1
1.1.2. Những ưu điểm và hạn chế của việc kết hợp cảm biến: ............................................. 1
1.2. Mục tiêu của đề tài: ............................................................................................................ 4
Chương 2. TỔNG QUAN: .......................................................................................................... 5
2.1. Tổng quan về tổng hợp dữ liệu: ......................................................................................... 5
2.1.1. Phân loại các phương pháp kết hợp dữ liệu: .............................................................. 7
2.1.2. Các kỹ thuật kết hợp dữ liệu: ................................................................................... 15
2.1.3. Các phương pháp ước tính trạng thái: ...................................................................... 20
2.2. Phương án thực hiện trong luận văn: ............................................................................... 24
Chương 3. CƠ SỞ LÝ THUYẾT: ............................................................................................ 26
3.1. Bộ lọc Kalman: ................................................................................................................ 26
3.1.1. Tổng quan về tính toán:............................................................................................ 26
3.1.2. Mô hình bộ lọc Kalman tiêu chuẩn: ......................................................................... 27
Chương 4. THIẾT KẾ GIẢI THUẬT KẾT HỢP CẢM BIẾN VÀ BỘ ĐIỀU KHIỂN:..... 31
4.1. Thiết kế giải thuật kết hợp cảm biến bằng bộ lọc Kalman mở rộng (EKF): .................... 31
4.1.1. Định vị robot bằng EKF: .......................................................................................... 31
4.1.2. Kết hợp cảm biến trong định vị robot bằng EKF: .................................................... 36
4.1.3. Mô phỏng kết hợp cảm biến trong định vị robot bằng EKF với tín hiệu giả lập: .... 41
4.1.4. Kiểm chứng giải thuật kết hợp cảm biến với dữ liệu cảm biến thực:....................... 44
4.2. Thiết kế bộ điều khiển:..................................................................................................... 52
4.2.1. Mô hình động học của robot: ................................................................................... 52
4.2.2. Thiết kế bộ điều khiển: ............................................................................................. 53
4.3. Kết quả mô phỏng điều khiển robot bám quỹ đạo khi kết hợp cảm biến bằng EKF: ...... 54
Chương 5. KẾT LUẬN VÀ HƯỚNG PHÁT TRIỂN: ........................................................... 61

VII
 5.1. Kết luận: ........................................................................................................................... 61
5.2. Hướng phát triển: ............................................................................................................. 61
Tài liệu tham khảo: ........................................................................................................................ 62
Phụ lục chương trình Matlab: ...................................................................................................... 67
1. Mô phỏng lọc bằng EKF cảm biến GPS giả lập: ................................................................. 67
2. Mô phỏng fusion bằng EKF với odo và GPS giả lập: .......................................................... 69
3. Mô phỏng lọc bằng EKF cảm biến UWB thực tế: ............................................................... 71
4. Mô phỏng fusion với tín hiệu cảm biến odo và UWB thực tế: ............................................ 73
5. Mô phỏng điều khiển robot bám quỹ đạo khi fusion với tín hiệu cảm biến odo và GPS giả
lập tần số 100ms:.......................................................................................................................... 75
6. Mô phỏng điều khiển robot bám quỹ đạo khi fusion với tín hiệu cảm biến odo và GPS giả
lập tần số 1ms:.............................................................................................................................. 79

VIII
 MỤC LỤC HÌNH ẢNH:
Hình 1 Sơ đồ khối mô tả sensor fusion và Multisensor intergration ................................................. 6
Hình 2 Kiểu phân loại của Durant-Whyte dựa trên mối quan hệ của nguồn vào.............................. 8
Hình 3 Sơ đồ kết hợp dữ liệu kiểu JDL........................................................................................... 10
Hình 4 Mô hình bên trong bộ lọc Kalman ...................................................................................... 28
Hình 5 Độ dịch chuyển của robot dạng 2 bánh ............................................................................... 31
Hình 6 Mô phỏng tín hiệu giả lập cảm biến GPS và Odometry...................................................... 42
Hình 7: Mô phỏng kết hợp cảm biến GPS và Odometry bằng EKF và chỉ lọc tín hiệu GPS bằng
EKF .................................................................................................................................................. 43
Hình 8 Mô phỏng kết hợp cảm biến GPS và Odometry bằng EKF và chỉ lọc tín hiệu GPS bằng
EKF - ở đoạn thẳng .......................................................................................................................... 43
Hình 9 Mô phỏng kết hợp cảm biến GPS và Odometry bằng EKF và chỉ lọc tín hiệu GPS bằng
EKF - ở góc rẽ. ................................................................................................................................. 44
Hình 10 Modul DWM1000 ............................................................................................................. 45
Hình 11 Thời gian truyền (TOF) trong phương pháp two‐way ranging. ........................................ 46
Hình 12 Thuật toán định vị Trilateral centroid ............................................................................... 47
Hình 13 Robot Pioneer DX3 ........................................................................................................... 48
Hình 14 Quỹ đạo di chuyển của robot. ........................................................................................... 49
Hình 15: Tín hiệu thực tế của cảm biến UWB và Odometry .......................................................... 50
Hình 16 Kết quả lọc nhiễu tín hiệu UWB bằng EKF ...................................................................... 50
Hình 17 Kết quả fusing sensor đối với UWB và Odometry ........................................................... 51
Hình 18 So sánh kết quả fusing sensor giữa UWB với Odometry bằng EKF và kết quả chỉ lọc
UWB bằng EKF ............................................................................................................................... 51
Hình 19 Mô hình động học của robot ............................................................................................. 52
Hình 20: Control block diagram ..................................................................................................... 54
Hình 21 Tín hiệu cảm biến mô phỏng trong quá trình bám quỹ đạo của robot với thời gian lấy mẫu
100ms ............................................................................................................................................... 55
Hình 22: Kết quả mô phỏng điều khiển robot bám quỹ đạo khi kết hợp cảm biến bằng EKF với
thời gian lấy mẫu 100ms .................................................................................................................. 56
Hình 23: Tracking error với thời gian lấy mẫu 100ms .................................................................... 57
Hình 24 Tín hiệu cảm biến mô phỏng trong quá trình bám quỹ đạo của robot với thời gian lấy mẫu
100ms ............................................................................................................................................... 58
Hình 25 Kết quả mô phỏng điều khiển robot bám quỹ đạo khi kết hợp cảm biến bằng EKF với thời
gian lấy mẫu 1ms. ............................................................................................................................ 59

IX
Hình 26 Kết quả mô phỏng điều khiển robot bám quỹ đạo khi kết hợp cảm biến bằng EKF với thời
gian lấy mẫu 1ms – phóng to. .......................................................................................................... 59
Hình 27 Tracking error với thời gian lấy mẫu 1ms ......................................................................... 60
Hình 28 Tracking error với thời gian lấy mẫu 1ms – phóng to. ...................................................... 60

X
Chương 1. MỞ ĐẦU:
1.1. Nhu cầu thực tiễn của đề tài:
1.1.1. Đặt vấn đề:
Robot tự hành có nhiều đóng góp trong sản xuất công nghiệp, khoa học y tế, khoa
học xã hội, nông nghiệp, giáo dục, trò chơi, nghiên cứu vũ trụ, v.v. Điều hướng là
một bài toán khó đối với robot di động tự động. Để thực hiện điều hướng, robot sẽ
trải qua các giai đoạn khác nhau như: nhận thức, định vị, nhận thức và điều khiển
chuyển động. Trong giai đoạn nhận thức, robot trích xuất dữ liệu có ý nghĩa bằng
cách phân tích các cảm biến của nó. Trong bước định vị, robot ước tính vị trí hiện tại
của nó trong môi trường làm việc bằng cách sử dụng thông tin từ các cảm biến bên
ngoài.
Định vị là một vấn đề không đơn giản đối với robot tự hành đặc biệt là khi robot
phải hoạt động trong các môi trường bất ổn như môi trường ngoài trời. Khi hoạt động
ở các môi trường như thế, các phương pháp định vị cục bộ khó có thể đảm bảo tính
chính xác và phải cần đến các phương pháp định vị toàn cục như GPS, … Tuy nhiên
bản thân các cảm biến toàn cục cũng có những nhược điểm riêng. Do đó phát sinh
nhu cầu kết hợp các loại cảm biến có ưu nhược điểm bổ khuyết cho nhau để tăng
cường khả năng định vị của robot trong môi trường bất ổn.
1.1.2. Những ưu điểm và hạn chế của việc kết hợp cảm biến:
1.1.2.1. Ưu điểm:

Các hệ thống sử dụng phương pháp kết hợp cảm biến sẽ có một số ưu điểm hơn so
với các hệ thống cảm biến đơn lẻ. Một phép đo cảm biến vật lý thường gặp phải các
vấn đề sau:

• Cảm biến mất tín hiệu: Sự cố của một phần tử cảm biến gây ra mất nhận thức
về đối tượng mong muốn.
• Phạm vi không gian hạn chế: Thông thường một cảm biến riêng lẻ chỉ bao
phủ một khu vực hạn chế.
1
 • Phạm vi thời gian hạn chế: Một số cảm biến cần một thời gian thiết lập cụ
thể để thực hiện và truyền một phép đo, do đó giới hạn tần số đo tối đa.
• Tính không chính xác: Các phép đo từ các cảm biến riêng lẻ được giới hạn ở
độ chính xác của phần tử cảm biến được sử dụng.
• Sự không chắc chắn: Sự không chắc chắn, khác với sự không chính xác, phụ
thuộc vào đối tượng được quan sát hơn là thiết bị quan sát.

Một giải pháp cho các vấn đề này là kết hợp cảm biến. Trong hệ thống kết hợp
cảm biến có thể chống lại việc mất cảm biến bằng cách sử dụng các cảm biến có cùng
phạm vi cảm nhận của đối tượng mong muốn. Điều này có thể áp dụng với các cảm
biến cùng loại cũng như với một bộ cảm biến không đồng nhất.

Những lợi thế sau có thể có được từ sự kết hợp dữ liệu cảm biến từ một tập hợp
các cảm biến không đồng nhất hoặc đồng nhất [1]:

• Ổn định và đáng tin cậy hơn: Nhiều bộ cảm biến có khả năng dự phòng cố
hữu cho phép hệ thống cung cấp thông tin ngay cả trong trường hợp hỏng hóc
một phần.
• Mở rộng phạm vi không gian và thời gian: Thêm cảm biến thì có thể bố trí
cảm biến có thể quan sát vào nơi mà những cảm biến khác không thể tương
ứng có thể thực hiện phép đo trong khi những cảm biến khác không thể.
• Tăng độ tin cậy: Phép đo của một cảm biến được xác nhận bằng các phép đo
của các cảm biến khác trên cùng một miền.
• Giảm sự mơ hồ và không chắc chắn: Thông tin chung làm giảm tập hợp các
cách diễn giải không rõ ràng về giá trị đo lường.
• Chống nhiễu mạnh mẽ hơn: Bằng cách tăng kích thước của không gian đo
(ví dụ: đo đại lượng mong muốn bằng cảm biến quang học và cảm biến siêu
âm), hệ thống trở nên ít bị ảnh hưởng bởi nhiễu hơn.

2
 • Cải thiện độ phân giải: Khi nhiều phép đo độc lập của cùng một thuộc tính
được hợp nhất, độ phân giải của giá trị kết quả sẽ tốt hơn so với phép đo của
một cảm biến đơn lẻ.

Một ưu điểm nữa của sự kết hợp cảm biến là khả năng giảm độ phức tạp của hệ
thống. Trong một hệ thống được thiết kế truyền thống, các phép đo cảm biến được
đưa vào ứng dụng, hệ thống này phải đối phó với một số lượng lớn các luồng dữ liệu
không chính xác, không rõ ràng và không đầy đủ. Trong một hệ thống mà dữ liệu
cảm biến được xử lý trước bằng phương pháp hợp nhất, đầu vào cho ứng dụng điều
khiển có thể được tiêu chuẩn hóa độc lập với các loại cảm biến được sử dụng, do đó
tạo điều kiện thuận lợi cho việc triển khai ứng dụng và cung cấp khả năng sửa đổi
trong hệ thống cảm biến về số lượng và loại cảm biến được sử dụng mà không sửa
đổi của phần mềm ứng dụng.

1.1.2.2. Hạn chế:

Sensor fusion đã dần phát triển khả năng kết hợp dữ liệu thành một kiểu nhận dạng
đáng tin cậy và giàu tính năng. Tuy nhiên, fusing sensors không phải là một phương
pháp toàn năng. Fowler đã phát biểu một lời chỉ trích gay gắt vào năm 1979 để nhấn
mạnh vào việc thay đổi dữ liệu ban đầu làm mất đi tính tin cậy và tăng khối lượng
tính toán và gây ra sự chậm trễ thời gian.

Kể từ khi điều này được công bố, nhiều người đã cố gắng chứng minh điều ngược
lại. Nahin và Pokoski [29] đã trình bày một bằng chứng lý thuyết rằng việc bổ sung
các cảm biến sẽ cải thiện hiệu suất trong các trường hợp cụ thể đối với đa số ý kiến
tán thành và lý thuyết khả năng xảy ra tối đa trong kết hợp thông tin để ra quyết định.
Hiệu suất được định nghĩa là xác suất đưa ra quyết định đúng đắn mà không liên quan
đến nỗ lực về khả năng xử lý và giao tiếp.

Có thể kết luận từ kiến thức hiện có về hiệu suất fusing sensors rằng mặc dù tiềm
năng to lớn của fusing sensors nhưng vẫn có chút hoài nghi về phương pháp kết hợp
“hoàn hảo” hoặc “tối ưu”.

3
1.2. Mục tiêu của đề tài:
Mục tiêu chính của đề tài là:
• Nghiên cứu, tìm hiểu các phương pháp kết hợp dữ liệu nhằm thiết kế giải thuật
kết hợp nhiều cảm biến để định vị robot tự hành.
• Thiết kế giải thuật kết hợp nhiều cảm biến để định vị robot tự hành.
• Mô phỏng giải thuật kết hợp nhiều cảm biến để định vị robot tự hành.
• Thiết kế và mô phỏng điều khiển robot bám quỹ đạo dựa trên sự kết hợp nhiều
cảm biến.

4
Chương 2. TỔNG QUAN:
2.1. Tổng quan về tổng hợp dữ liệu:

Kết hợp cảm biến là sự kết hợp của dữ liệu cảm quan hoặc dữ liệu thu được từ dữ
liệu cảm biến sao cho thông tin kết quả theo một nghĩa nào đó tốt hơn khi các nguồn
này được sử dụng riêng lẻ.

Có một thuật ngữ tương tự với ‘Fusion sensor’ là ‘Multisensor integration’. Sự

khác biệt giữa ‘Fusion sensor’ và ‘Multisensor integration’ được mô tả trong hình
2.1. Các hình tròn S1, S2 và S3 mô tả các cảm biến vật lý nhận biết các điều kiện của
môi trường. Sơ đồ khối 2.1 (a) cho thấy rằng dữ liệu cảm biến được chuyển đổi bởi
khối hợp nhất cảm biến thành một biểu diễn tương ứng của các biến của môi trường
quá trình. Những dữ liệu này sau đó được sử dụng bởi một ứng dụng điều khiển.
Ngược lại, hình 2.1 (b) cho thấy ý nghĩa của ‘Multisensor integration’, trong đó dữ
liệu cảm biến khác nhau được ứng dụng điều khiển xử lý trực tiếp.

5
 Hình 1 Sơ đồ khối mô tả sensor fusion và Multisensor intergration

Các nguồn dữ liệu cho quá trình fusion không nhất thiết là bắt nguồn từ các cảm
biến giống hệt nhau. McKee phân biệt kết hợp trực tiếp, kết hợp gián tiếp và phản
ứng tổng hợp đầu ra của hai phương pháp này [14].

Kết hợp trực tiếp có nghĩa là hợp nhất dữ liệu cảm biến từ một tập hợp các cảm
biến không đồng nhất hoặc đồng nhất, cảm biến mềm và các giá trị lịch sử của dữ
liệu cảm biến.

Kết hợp gián tiếp sử dụng các nguồn thông tin như kiến thức tiên nghiệm về môi
trường và đầu vào của con người. Do đó, kết hợp cảm biến mô tả các hệ thống kết
hợp trực tiếp, trong khi phản ứng tổng hợp thông tin cũng bao gồm các quá trình kết
hợp gián tiếp.

6
2.1.1. Phân loại các phương pháp kết hợp dữ liệu:

Tổng hợp dữ liệu là một lĩnh vực đa ngành liên quan đến một số lĩnh vực khác và
rất khó để thiết lập một phân loại rõ ràng và chặt chẽ. Các phương pháp và kỹ thuật
được sử dụng có thể được phân chia theo các tiêu chí sau:

• Xem xét các mối quan hệ giữa các nguồn dữ liệu đầu vào, như đề xuất của
Durrant-Whyte [16]. Các quan hệ này có thể được định nghĩa là (a) bổ sung,
(b) dư thừa, hoặc (3) hợp tác dữ liệu.
• Theo các kiểu dữ liệu đầu vào / đầu ra và bản chất của chúng, như đề xuất của
Dasarathy [5].
• Theo mức độ trừu tượng của dữ liệu được sử dụng: (a) phép đo thô, (b) tín
hiệu, và (c) đặc điểm hoặc quyết định.
• Dựa trên các mức tổng hợp dữ liệu khác nhau được xác định bởi JDL.
• Tùy thuộc vào kiểu kiến trúc: (a) tập trung, (b) phi tập trung, hoặc (c) phân
tán.
2.1.1.1. Phân loại dựa trên mối quan hệ giữa nguồn dữ liệu:
Dựa trên mối quan hệ của các nguồn (xem Hình 1), Durant-Whyte [16] đề xuất các
tiêu chí phân loại sau:
• Bổ sung cho nhau (complementary): khi thông tin được cung cấp bởi các
nguồn đầu vào đại diện cho các phần khác nhau của hiện trường và do đó có
thể được sử dụng để thu được thông tin toàn cục đầy đủ hơn. Ví dụ, trong
trường hợp của mạng cảm biến thị giác, thông tin về cùng một mục tiêu được
cung cấp bởi hai máy ảnh có trường nhìn khác nhau được coi là bổ sung.
• Dư thừa (redundant): khi hai hoặc nhiều nguồn đầu vào cung cấp thông tin về
cùng một mục tiêu và do đó có thể được hợp nhất để tăng thêm ý nghĩa. Ví
dụ, dữ liệu đến từ các khu vực chồng chéo trong mạng cảm biến trực quan
được coi là dư thừa.

7
 • Hợp tác (cooperative): khi thông tin được cung cấp được kết hợp thành thông
tin mới thường phức tạp hơn thông tin ban đầu. Ví dụ: kết hợp dữ liệu đa
phương thức (âm thanh và video) được coi là hợp tác.

Hình 2 Kiểu phân loại của Durant-Whyte dựa trên mối quan hệ của nguồn vào

2.1.1.2. Phân loại theo kiểu của Dasarathy.

Một trong những hệ thống phân loại tổng hợp dữ liệu nổi tiếng nhất được cung cấp
bởi Dasarathy [5] và bao gồm năm loại sau (xem Hình 2):
• Data in-data out (DAI-DAO): loại này là phương pháp tổng hợp dữ liệu cơ
bản nhất được xem xét trong phân loại. Loại quy trình kết hợp dữ liệu này đầu
vào và đầu ra đều là dữ liệu thô, kết quả thường đáng tin cậy hoặc chính xác
hơn. Quá trình tổng hợp dữ liệu ở cấp độ này được tiến hành ngay sau khi dữ
liệu được thu thập từ các cảm biến. Các thuật toán được sử dụng ở cấp độ này
dựa trên các thuật toán xử lý tín hiệu và hình ảnh.

8
 • Data in-feature out (DAI-FEO): ở cấp độ này, quá trình tổng hợp dữ liệu sử
dụng dữ liệu thô từ các nguồn để trích xuất các tính năng hoặc đặc điểm mô
tả một thực thể trong môi trường.
• Feature in-feature out (FEI-FEO): ở cấp độ này, cả đầu vào và đầu ra của quá
trình tổng hợp dữ liệu đều là các tính năng. Do đó, quá trình tổng hợp dữ liệu
giải quyết một tập hợp các tính năng để cải thiện, tinh chỉnh hoặc có được các
tính năng mới. Quá trình này còn được gọi là hợp nhất tính năng, hợp nhất ký
hiệu, hợp nhất thông tin hoặc hợp nhất cấp trung gian.
• Feature in-decision out (FEI-DEO): cấp độ này nhận một tập hợp các tính
năng làm đầu vào và cung cấp một tập hợp các quyết định dưới dạng đầu ra.
Hầu hết các hệ thống phân loại thực hiện quyết định dựa trên đầu vào của cảm
biến đều thuộc loại phân loại này.
• Decision In-Decision Out (DEI-DEO): Loại phân loại này còn được gọi là
hợp nhất quyết định. Nó kết hợp các quyết định đầu vào để có được các quyết
định tốt hơn hoặc mới hơn.
Đóng góp chính của phân loại của Dasarathy là đặc điểm kỹ thuật của mức trừu
tượng dưới dạng đầu vào hoặc đầu ra, cung cấp một khuôn khổ để phân loại các
phương pháp hoặc kỹ thuật khác nhau.
2.1.1.3. Phân loại theo cấp độ trừu tượng:
Luo và cộng sự. [31] cung cấp bốn mức trừu tượng sau:
• Tín hiệu: giải quyết trực tiếp các tín hiệu thu được từ các cảm biến.
• Pixel: hoạt động ở mức hình ảnh và có thể được sử dụng để cải thiện các tác
vụ xử lý hình ảnh.
• Đặc tính: sử dụng các tính năng được trích xuất từ hình ảnh hoặc tín hiệu (tức
là hình dạng hoặc vận tốc).
• Ký hiệu: ở cấp độ này, thông tin được biểu diễn dưới dạng ký hiệu, mức này
còn được gọi là mức quyết định.

9
 Sự tổng hợp thông tin thường đề cập đến ba cấp độ trừu tượng: (1) phép đo, (2)
đặc điểm và (3) quyết định. Các cách phân loại khác có thể có của tổng hợp dữ liệu
dựa trên các mức trừu tượng như sau:
• Tổng hợp mức thấp: dữ liệu thô được cung cấp trực tiếp như một đầu vào cho
quá trình tổng hợp dữ liệu, cung cấp dữ liệu chính xác hơn (tỷ lệ tín hiệu trên
nhiễu thấp hơn) so với các nguồn riêng lẻ.
• Tổng hợp mức độ trung bình: các đặc điểm hoặc tính năng (hình dạng, kết cấu
và vị trí) được hợp nhất để có được các đặc điểm có thể được sử dụng cho các
nhiệm vụ khác. Mức này còn được gọi là mức đặc trưng hoặc mức đặc trưng.
• Tổng hợp ở cấp cao: cấp độ này, còn được gọi là hợp nhất quyết định, lấy các
biểu diễn tượng trưng làm nguồn và kết hợp chúng để thu được quyết định xác
hơn. Các phương pháp của Bayesian thường được sử dụng ở cấp độ này.
• Kết hợp nhiều mức: mức này giải quyết dữ liệu được cung cấp từ các mức trừu
tượng khác nhau (tức là khi một phép đo được kết hợp với một tính năng để
có được quyết định).
2.1.1.4. Kiểu kết hợp dữ liệu JDL:

Hình 3 Sơ đồ kết hợp dữ liệu kiểu JDL

Phân loại này là mô hình khái niệm phổ biến nhất trong cộng đồng tổng hợp dữ
liệu. Ban đầu nó được đề xuất bởi JDL và Bộ Quốc phòng Hoa Kỳ (DoD) [23]. Các

10
tổ chức này đã phân loại quá trình tổng hợp dữ liệu thành các mức xử lý đặc biệt, một
cơ sở dữ liệu liên quan và một luồng thông tin kết nối năm thành phần (xem Hình 3).
Năm cấp độ có thể được nhóm thành hai nhóm, fusion cấp thấp và fusion cấp cao,
bao gồm các thành phần sau:
• Nguồn: các nguồn chịu trách nhiệm cung cấp dữ liệu đầu vào. Các loại nguồn
khác nhau có thể được sử dụng, chẳng hạn như cảm biến, thông tin tiên
nghiệm, cơ sở dữ liệu và đầu vào của con người.
• Tương tác giữa người và máy tính (HCI): HCI là một giao diện cho phép đầu
vào hệ thống từ người vận hành và tạo ra kết quả đầu ra cho người vận hành.
HCI bao gồm các truy vấn, lệnh và thông tin về các kết quả thu được và các
cảnh báo.
• Hệ thống quản lý cơ sở dữ liệu: hệ thống quản lý cơ sở dữ liệu lưu trữ thông
tin được cung cấp và các kết quả hợp nhất. Hệ thống này là một thành phần
quan trọng vì lượng lớn thông tin đa dạng được lưu trữ.
Năm cấp độ xử lý dữ liệu được xác định như sau:
• Mức 0 - tiền xử lý nguồn: tiền xử lý nguồn là mức thấp nhất của quá trình tổng
hợp dữ liệu và nó bao gồm sự hợp nhất ở mức tín hiệu và pixel. Trong trường
hợp nguồn văn bản, cấp độ này cũng bao gồm quá trình trích xuất thông tin.
Mức này làm giảm lượng dữ liệu và duy trì thông tin hữu ích cho các quá trình
ở mức cao.
• Cấp 1 - tái cấu trúc đối tượng: tái cấu trúc đối tượng sử dụng dữ liệu đã xử lý
từ cấp trước. Các thủ tục phổ biến của cấp độ này bao gồm liên kết không gian-
thời gian, liên kết, tương quan, kỹ thuật phân cụm hoặc nhóm, ước lượng trạng
thái, loại bỏ kích hoạt giả, hợp nhất nhận dạng và kết hợp các đặc điểm được
trích xuất từ hình ảnh. Kết quả của giai đoạn này là phân biệt đối tượng (phân
loại và nhận dạng) và theo dõi đối tượng (trạng thái của đối tượng và hướng).
Giai đoạn này chuyển đổi thông tin đầu vào thành các cấu trúc dữ liệu nhất
quán.

11
 • Cấp độ 2 - đánh giá tình huống: cấp độ này tập trung vào cấp độ suy luận cao
hơn cấp độ 1. Đánh giá tình huống nhằm xác định các tình huống có thể xảy
ra với các sự kiện quan sát và dữ liệu thu được. Nó thiết lập các mối quan hệ
giữa các đối tượng. Các mối quan hệ (nghĩa là gần gũi, giao tiếp) được đánh
giá cao để xác định mức độ quan trọng của các thực thể hoặc đối tượng trong
một môi trường cụ thể. Mục tiêu của cấp độ này bao gồm thực hiện các suy
luận cấp cao và xác định các hoạt động và sự kiện quan trọng (các mẫu nói
chung). Đầu ra là một tập hợp các suy luận cấp cao.
• Cấp độ 3 - đánh giá tác động: cấp độ này đánh giá tác động của các hoạt động
được phát hiện ở cấp độ 2 để có được quan điểm phù hợp. Tình hình hiện tại
được đánh giá, và dự báo trong tương lai được thực hiện để xác định các rủi
ro có thể xảy ra, các điểm yếu và các cơ hội hoạt động. Mức độ bao gồm (1)
đánh giá rủi ro hoặc mối đe dọa và (2) dự đoán kết quả hợp lý.
• Cấp độ 4 - cải tiến quy trình: cấp độ này cải thiện quy trình từ cấp độ 0 lên cấp
độ 3 và cung cấp khả năng quản lý tài nguyên và cảm biến. Mục đích là để đạt
được quản lý tài nguyên hiệu quả trong khi tính toán các ưu tiên nhiệm vụ, lập
kế hoạch và kiểm soát các nguồn lực sẵn có.
Hợp nhất cấp cao thường bắt đầu ở cấp 2 vì loại, localization, chuyển động và số
lượng của các đối tượng đã được biết ở cấp đó. Một trong những hạn chế của JDL
phương pháp là cách sử dụng độ không đảm bảo về các kết quả trước đó hoặc sau đó
để tăng cường quá trình hợp nhất (vòng phản hồi). Llinas và cộng sự. [20] đề xuất
một số cải tiến và mở rộng cho mô hình JDL. Blasch và Plano [13] đề xuất thêm một
cấp độ mới để hỗ trợ người dùng là con người trong vòng lặp kết hợp dữ liệu. Mô
hình JDL đại diện hiệu quả để cung cấp một mô hình chi tiết và một thuật ngữ chung
cho miền tổng hợp dữ liệu. Tuy nhiên, vì nguồn gốc của chúng bắt nguồn từ lĩnh vực
quân sự, các điều khoản được sử dụng hướng đến những rủi ro thường xảy ra trong
các tình huống này. Mô hình Dasarathy khác với mô hình JDL về thuật ngữ được
chấp nhận và cách tiếp cận được sử dụng. Trước đây là định hướng về sự khác biệt
giữa các kết quả đầu vào và đầu ra, độc lập với phương pháp tổng hợp được sử dụng.
12
Tóm lại, mô hình Dasarathy cung cấp một phương pháp để hiểu mối quan hệ giữa các
nhiệm vụ tổng hợp và dữ liệu được sử dụng, trong khi mô hình JDL trình bày một
quan điểm tổng hợp thích hợp để thiết kế hệ thống tổng hợp dữ liệu.
2.1.1.5. Phân loại dựa trên loại cấu trúc:
Một trong những câu hỏi chính nảy sinh khi thiết kế hệ thống hợp nhất dữ liệu là
quá trình tổng hợp dữ liệu sẽ được thực hiện ở đâu. Dựa trên tiêu chí này, có thể xác
định các loại kiến trúc sau:
• Cấu trúc tập trung - Centralized architecture: trong cấu trúc tập trung, nút
hợp nhất nằm trong bộ xử lý trung tâm nhận thông tin từ tất cả các nguồn đầu
vào. Do đó, tất cả các quá trình tổng hợp được thực hiện trong một bộ xử lý
trung tâm sử dụng các phép đo thô được cung cấp từ các nguồn. Trong giản
đồ này, các nguồn chỉ thu được quan sát dưới dạng các phép đo và truyền
chúng đến bộ xử lý trung tâm, nơi quá trình tổng hợp dữ liệu được thực hiện.
Nếu chúng ta giả định rằng việc liên kết dữ liệu và liên kết dữ liệu được thực
hiện chính xác và thời gian cần thiết để truyền dữ liệu là không đáng kể, thì sơ
đồ tập trung về mặt lý thuyết là tối ưu. Tuy nhiên, các giả định trước đây
thường không phù hợp với các hệ thống thực. Hơn nữa, số lượng lớn băng
thông được yêu cầu để gửi dữ liệu thô qua mạng là một bất lợi khác đối với
phương pháp tiếp cận tập trung. Vấn đề này trở thành một nút thắt cổ chai khi
loại kiến trúc này được sử dụng để kết hợp dữ liệu trong mạng cảm biến trực
quan. Cuối cùng, độ trễ về thời gian khi chuyển thông tin giữa các nguồn khác
nhau có thể thay đổi và ảnh hưởng đến kết quả trong sơ đồ tập trung ở mức độ
lớn hơn so với các chương trình khác.
• Cấu trúc phi tập trung: một kiến trúc phi tập trung bao gồm một mạng lưới
các nút trong đó mỗi nút có khả năng xử lý riêng và không có điểm tổng hợp
dữ liệu duy nhất. Do đó, mỗi nút kết hợp thông tin cục bộ của nó với thông tin
nhận được từ các nút tương ứng. Quá trình tổng hợp dữ liệu được thực hiện
một cách độc lập, với mỗi nút tính theo thông tin cục bộ của nó và thông tin
nhận được từ các nút tương đương. Các thuật toán tổng hợp dữ liệu phi tập
13
 trung thường truyền đạt thông tin bằng cách sử dụng các phép đo Fisher và
Shannon thay vì trạng thái của đối tượng. Nhược điểm chính của kiến trúc này
là chi phí truyền thông, ở mỗi bước truyền thông, trong đó số lượng nút là bao
nhiêu. Kiểu kiến trúc này có thể gặp vấn đề về khả năng mở rộng khi số lượng
nút tăng lên.
• Cấu trúc phân tán: trong cấu trúc phân tán, các phép đo từ mỗi nút nguồn
được xử lý độc lập trước khi thông tin được gửi đến nút hợp nhất; nút hợp nhất
giải thích thông tin nhận được từ các nút khác. Nói cách khác, liên kết dữ liệu
và ước lượng trạng thái được thực hiện trong nút nguồn trước khi thông tin
được truyền tới nút hợp nhất. Do đó, mỗi nút cung cấp một ước lượng về trạng
thái đối tượng chỉ dựa trên các khung nhìn cục bộ của chúng và thông tin này
là đầu vào cho quá trình hợp nhất, cung cấp một chế độ xem toàn cục được
hợp nhất. Loại kiến trúc này cung cấp các tùy chọn và biến thể khác nhau từ
chỉ một nút hợp nhất đến một số nút hợp nhất trung gian.
• Cấu trúc phân cấp: các kiến trúc khác bao gồm sự kết hợp của các nút phân
cấp và phân tán, tạo ra các lược đồ phân cấp trong đó quá trình tổng hợp dữ
liệu được thực hiện ở các cấp độ khác nhau trong hệ thống phân cấp.
Về nguyên tắc, một hệ thống tổng hợp dữ liệu phi tập trung khó triển khai hơn vì
các yêu cầu về tính toán và giao tiếp. Tuy nhiên, trong thực tế, không có kiến trúc tốt
nhất duy nhất và việc lựa chọn kiến trúc thích hợp nhất cần được thực hiện tùy thuộc
vào yêu cầu, nhu cầu, mạng hiện có, tính khả dụng của dữ liệu, khả năng xử lý nút và
tổ chức của hệ thống tổng hợp dữ liệu.
Giữa cấu trúc phi tập trung và phân tán có những khác biệt rõ ràng như:
Đầu tiên, trong một kiến trúc phân tán, một quá trình xử lý trước các phép đo thu
được được thực hiện, kết quả là cung cấp một vectơ của các đặc trưng (các đặc trưng
được hợp nhất sau đó). Ngược lại, trong kiến trúc phi tập trung, quá trình tổng hợp
dữ liệu hoàn chỉnh được tiến hành trong mỗi nút và mỗi nút cung cấp kết quả hợp
nhất toàn cục.

14
 Thứ hai, các thuật toán tổng hợp phi tập trung thường truyền đạt thông tin, sử dụng
các phép đo Fisher và Shannon. Ngược lại, các thuật toán phân tán thường chia sẻ
một khái niệm chung về trạng thái (vị trí, vận tốc và nhận dạng) với các xác suất liên
quan của chúng, được sử dụng để thực hiện quá trình hợp nhất [21].
Thứ ba, bởi vì các thuật toán tổng hợp dữ liệu phi tập trung trao đổi thông tin thay
vì trạng thái và xác suất, chúng có lợi thế là dễ dàng tách kiến thức cũ khỏi kiến thức
mới hiểu biết. Tuy nhiên, trong các thuật toán tổng hợp dữ liệu phân tán (tức là được
phân phối bởi Bộ lọc Kalman), trạng thái sắp được hợp nhất không phải là liên kết và
khi nào và cách thức tính toán các ước tính được hợp nhất là có liên quan. Tuy nhiên,
trái ngược với các kiến trúc tập trung, các thuật toán phân tán giảm chi phí giao tiếp
và tính toán cần thiết bởi vì một số tác vụ được tính toán trong các nút phân tán trước
khi quá trình tổng hợp dữ liệu được thực hiện trong nút hợp nhất.
2.1.2. Các kỹ thuật kết hợp dữ liệu:
Mục tiêu của liên kết dữ liệu là thiết lập tập hợp các quan sát hoặc phép đo được
tạo ra bởi cùng một mục tiêu theo thời gian. Hall và Llinas [12] đã đưa ra định nghĩa
sau về liên kết dữ liệu: “Quá trình gán và tính toán trọng số liên quan đến các quan
sát hoặc đường đi (Đường có thể được định nghĩa là một tập hợp các điểm có thứ tự
theo một đường dẫn và được tạo bởi cùng một mục tiêu.) từ bộ dữ liệu này đến việc
quan sát các dấu vết của bộ dữ liệu khác.
Liên kết dữ liệu thường được thực hiện trước ước tính trạng thái của các mục tiêu
được phát hiện. Hơn nữa, nó là một bước quan trọng vì ước tính hoặc phân loại sẽ
hoạt động không chính xác nếu giai đoạn kết hợp dữ liệu không hoạt động một cách
mạch lạc. Quá trình liên kết dữ liệu cũng có thể xuất hiện ở tất cả các cấp độ dung
hợp, nhưng mức độ chi tiết khác nhau tùy thuộc vào mục tiêu của từng cấp độ.
2.1.2.1. Nearest Neighbors and K-Means:

Nearest neighbor (NN) là kỹ thuật liên kết dữ liệu đơn giản nhất. NN là một thuật
toán phân cụm nổi tiếng để chọn hoặc nhóm các giá trị giống nhau nhất. Mức độ gần
của phép đo này với phép đo khác phụ thuộc vào số liệu khoảng cách được sử dụng

15
và thường phụ thuộc vào ngưỡng được thiết lập bởi nhà thiết kế. Nói chung, các tiêu
chí được tuyển dụng có thể dựa trên:

• Khoảng cách tuyệt đối.

• Khoảng cách Euclide.
• Một hàm thống kê của khoảng cách.

NN là một thuật toán đơn giản có thể tìm ra một giải pháp khả thi (gần đúng) trong
một khoảng thời gian nhỏ. Tuy nhiên, trong một môi trường lộn xộn, nó có thể cung
cấp nhiều cặp có cùng xác suất và do đó có thể tạo ra sự lan truyền lỗi không mong
muốn [15]. Hơn nữa, thuật toán này có hiệu suất kém trong các môi trường mà các
phép đo sai thường xuyên xảy ra, trong các môi trường có độ ồn cao.

Phương pháp 𝐾-Means [34] là một cải tiến của thuật toán NN. 𝐾-Means chia các
giá trị tập dữ liệu thành 𝐾 các cụm khác nhau. Thuật toán 𝐾-Means tìm ra kết quả
định vị tốt nhất của các trung tâm cụm có nghĩa là tâm nằm ở trung tâm của cụm dữ
liệu.

𝐾-Means là một thuật toán phổ biến đã được sử dụng rộng rãi; tuy nhiên, nó có
những nhược điểm sau:

• Thuật toán không phải lúc nào cũng tìm ra giải pháp tối ưu cho các trung tâm
cụm.
• Số lượng cụm phải được biết trước và người ta phải giả định rằng số này là tối
ưu.
• Thuật toán giả định rằng hiệp phương sai của tập dữ liệu là không liên quan
hoặc nó đã được chuẩn hóa.

Có một số cách để khắc phục những hạn chế này. Cách đầu tiên, có thể thực hiện
thuật toán nhiều lần và thu được giải pháp có ít phương sai hơn. Cách thứ hai, có thể
bắt đầu với giá trị thấp của 𝐾 và tăng dần các giá trị của 𝐾 cho đến khi thu được kết

16
quả thích hợp. Cách thứ ba là nhân dữ liệu với nghịch đảo của ma trận hiệp phương
sai.

2.1.2.2. Kết hợp dữ liệu bằng xác suất (Probabilistic Data Association):

Thuật toán liên kết dữ liệu xác suất (PDA) được đề xuất bởi Bar-Shalom và Tse
[45]. Thuật toán này chỉ định một xác suất liên kết cho mỗi giả thuyết từ một phép đo
hợp lệ của một mục tiêu.

Trong thuật toán PDA, ước tính trạng thái của mục tiêu được tính dưới dạng tổng
trọng số của trạng thái ước tính theo tất cả các giả thuyết. Thuật toán có thể liên kết
các phép đo khác nhau với một mục tiêu cụ thể. Do đó, sự liên kết của các phép đo
khác nhau với một mục tiêu cụ thể giúp PDA ước tính trạng thái mục tiêu và xác suất
liên kết được sử dụng làm trọng số. Những nhược điểm chính của thuật toán PDA là:

• Mất dấu vết: vì PDA bỏ qua sự can thiệp của các mục tiêu khác, nên đôi khi
nó có thể phân loại sai các thông tin gần nhất. Do đó, nó có hiệu suất kém khi
các mục tiêu gần nhau hoặc giao nhau.
• Kết quả là xấp xỉ: khi nguồn thông tin không chắc chắn, PDA là xấp xỉ Bayes
tối ưu cho bài toán liên kết.
• Chỉ có một mục tiêu: PDA ban đầu được thiết kế để kết hợp một mục tiêu
trong một môi trường ít lộn xộn. Số lượng cảnh báo sai thường được mô hình
hóa với phân phối Poisson và chúng được giả định là phân bố đồng nhất trong
không gian. PDA hoạt động không chính xác khi có nhiều mục tiêu do mô
hình báo động giả không hoạt động tốt.
PDA thường tốt khi theo dõi các mục tiêu không tạo ra các thay đổi đột ngột trong
kiểu chuyển động của chúng. PDA rất có thể sẽ mất mục tiêu nếu nó thực hiện các
thay đổi đột ngột trong các kiểu chuyển động của nó.
2.1.2.3. Joint Probabilistic Data Association:

Joint Probabilistic Data Association (JPDA) là một phương pháp tiếp cận tối ưu để
theo dõi nhiều mục tiêu trong môi trường lộn xộn. JPDA tương tự như PDA, với sự

17
khác biệt là xác suất liên kết được tính bằng cách sử dụng tất cả các quan sát và tất cả
các mục tiêu. Do đó, trái ngược với PDA, JPDA xem xét các giả thuyết khác nhau
với nhau và kết hợp chúng.

Phương pháp này kết hợp tất cả các quan sát (bên trong vùng lân cận của vị trí dự
đoán của mục tiêu) để cập nhật vị trí ước tính bằng cách sử dụng xác suất sau là tổng
trọng số của các phần dư.

Các hạn chế của JPDA là:

• Một phép đo không thể đến từ nhiều hơn một mục tiêu.
• Hai phép đo không thể được bắt nguồn bởi cùng một mục tiêu (tại một thời
điểm tức thì).
• Tổng tất cả các xác suất của phép đo được gán cho một mục tiêu phải là 1: ∑𝑚
(𝑘) 𝑖 = 0 𝛽𝑖𝑡 (𝑘) = 1.

Những nhược điểm chính của JPDA là:

• Nó yêu cầu một cơ chế rõ ràng để khởi tạo theo dõi. Tương tự như PDA, JPDA
không thể khởi tạo các kết quả mới hoặc loại bỏ các kết quả nằm ngoài vùng
quan sát.
• JPDA là một thuật toán có khối lượng tính toán lớn khi nó được áp dụng theo
môi trường làm việc theo nhiều mục tiêu vì số lượng giả thuyết được tăng lên
theo cấp số nhân với số lượng mục tiêu.

Nói chung, JPDA thích hợp hơn MHT trong các tình huống mà mật độ các phép
đo sai cao (tức là các ứng dụng sonar).

2.1.2.4. Multiple Hypothesis Test:

Ý tưởng cơ bản của Multiple Hypothesis Test (MHT) dựa trên việc sử dụng hơn
hai lần quan sát liên tiếp để tạo ra mối liên hệ với kết quả tốt hơn. Các thuật toán khác
chỉ sử dụng hai lần quan sát liên tiếp có xác suất tạo ra lỗi cao hơn.

18
 Ngược lại với PDA và JPDA, MHT ước tính tất cả các giả thuyết có thể có và duy
trì các giả thuyết mới trong mỗi lần lặp.

MHT được phát triển để theo dõi nhiều mục tiêu trong môi trường lộn xộn, kết quả
là nó kết hợp vấn đề liên kết dữ liệu và theo dõi thành một khuôn khổ thống nhất, trở
thành một kỹ thuật ước tính. Quy tắc Bayes hoặc mạng Bayes thường được sử dụng
để tính toán giả thuyết MHT. Nhìn chung, các nhà nghiên cứu đã tuyên bố rằng MHT
hoạt động tốt hơn JPDA. Tuy nhiên, nhược điểm chính của MHT là chi phí tính toán
khi số lượng thông tin nhận được hoặc các giả định tăng lên.

Nhược điểm chính của thuật toán này là chi phí tính toán, tăng theo cấp số nhân
cùng với số lượng đường đi và phép đo. Do đó, việc triển khai thực tế của thuật toán
này bị hạn chế vì nó là cấp số nhân cả về thời gian và bộ nhớ.

2.1.2.5. Distributed Joint Probabilistic Data Association:

Distributed Joint Probabilistic Data Association (JPDA-D) được trình bày bởi
Chang và các cộng sự. [24]. Trong kỹ thuật này, trạng thái ước tính của mục tiêu (sử
dụng hai cảm biến).

Phương pháp này thu được giả định rằng giao tiếp tồn tại sau mỗi lần quan sát và
chỉ có giá trị gần đúng trong trường hợp giao tiếp là rời rạc và khi một lượng lớn
nhiễu xuất hiện.

Do đó, thuật toán này là một mô hình lý thuyết có một số hạn chế trong ứng dụng
thực tế.

2.1.2.6. Distributed Multiple Hypothesis Test:

Distributed Multiple Hypothesis Test MHT (MHT-D) [43, 44] tuân theo cấu trúc
tương tự như thuật toán JPDA-D.

Thuật toán MHT-D bao gồm các bước sau:

19
 • Hình thành giả thuyết: đối với mỗi cặp giả thuyết, có thể được hợp nhất, trong
đó mỗi cặp đến từ một nút và có thể xuất phát từ cùng một mục tiêu. Kết quả
cuối cùng của giai đoạn này là một tập hợp các giả thuyết và các bản nhạc hợp
nhất.
• Đánh giá giả thuyết: trong giai đoạn này, xác suất kết hợp của mỗi giả thuyết
và trạng thái ước tính của mỗi đường dẫn hợp nhất được thu được. Thuật toán
ước lượng phân tán được sử dụng để tính toán khả năng của các liên kết có thể
có và các ước tính thu được tại mỗi liên kết cụ thể.

Nhược điểm chính của MHT-D là khối lượng tính toán rất cao.

2.1.3. Các phương pháp ước tính trạng thái:

Các kỹ thuật ước lượng trạng thái nhằm xác định trạng thái của mục tiêu đang
chuyển động (điển hình là vị trí) khi quan sát hoặc đo lường. Kỹ thuật ước lượng
trạng thái còn được gọi là kỹ thuật theo dõi. Ở dạng chung của chúng, không đảm bảo
rằng các quan sát mục tiêu là phù hợp, có nghĩa là một số quan sát thực sự có thể đến
từ mục tiêu và những quan sát khác chỉ có thể là nhiễu.

Giai đoạn ước tính trạng thái là giai đoạn phổ biến trong các thuật toán tổng hợp
dữ liệu vì quan sát của mục tiêu có thể đến từ các cảm biến hoặc nguồn khác nhau và
mục tiêu cuối cùng là thu được trạng thái mục tiêu chung từ các quan sát.

Bài toán ước lượng liên quan đến việc tìm các giá trị của trạng thái vectơ (ví dụ:
vị trí, vận tốc và kích thước) phù hợp nhất có thể với dữ liệu quan sát được. Chúng ta
có một tập hợp nhiều các quan sát và mục tiêu là tìm tập hợp các tham số cung cấp
sự phù hợp nhất với dữ liệu quan sát. Nói chung, những quan sát này bị lỗi do sai số
và sự lan truyền của nhiễu trong quá trình đo. Các phương pháp ước lượng trạng thái
thuộc cấp độ 1 của phân loại JDL và có thể được chia thành hai nhóm rộng hơn:

• Mô hình động lực học và các phép đo đều là tuyến tính: Khi đó bài toán ước
lượng có một lời giải tiêu chuẩn. Cụ thể, khi các phương trình của trạng thái

20
 đối tượng và các phép đo là tuyến tính, nhiễu tuân theo phân bố Gauss, và
chúng ta không coi nó là một môi trường lộn xộn, trong trường hợp này, giải
pháp lý thuyết tối ưu dựa trên bộ lọc Kalman.
• Mô hình động lực học và các phép đo đều là phi tuyến: vấn đề ước lượng trạng
thái trở nên khó khăn, và không có một giải pháp phân tích để giải quyết vấn
đề một cách tổng quát. Về nguyên tắc, không có thuật toán thực tế nào có sẵn
để giải quyết vấn đề này một cách thỏa đáng.
Hầu hết các phương pháp ước lượng trạng thái đều dựa trên lý thuyết điều khiển
và sử dụng luật xác suất để tính trạng thái vectơ từ phép đo vectơ hoặc dòng các phép
đo vectơ.
2.1.3.1. Maximum Likelihood and Maximum Posterior:
Kỹ thuật khả năng xảy ra tối đa - The maximum likelihood (ML) là một phương
pháp ước lượng dựa trên lý thuyết xác suất. Phương pháp ước lượng xác suất thích
hợp khi biến trạng thái tuân theo phân phối xác suất chưa biết [10]. Trong bối cảnh
tổng hợp dữ liệu, 𝑥 là trạng thái đang được ước lượng, và 𝑧 = (𝑧 (1), ..., 𝑧 (𝑘)) là một
chuỗi của 𝑘 quan sát trước đó của 𝑥.

Nhược điểm chính của phương pháp này trong thực tế là nó yêu cầu phải biết mô
hình phân tích hoặc thực nghiệm của cảm biến để cung cấp phân phối trước và tính
toán hàm khả năng xảy ra. Phương pháp này cũng có thể đánh giá thấp một cách có
hệ thống phương sai của phân phối, điều này dẫn đến vấn đề sai lệch. Tuy nhiên, độ
chệch của giải pháp ML trở nên ít có ý nghĩa hơn khi số 𝑁 điểm dữ liệu tăng lên và
bằng với phương sai thực của phân phối đã tạo ra dữ liệu ở giới hạn 𝑁 → ∞.

Phương pháp Maximum Posterior (MAP) dựa trên lý thuyết Bayes. Nó được sử
dụng khi tham số 𝑥 được ước lượng là đầu ra của một biến ngẫu nhiên có hàm mật
độ xác suất đã biết 𝑝 (𝑥). Trong bối cảnh tổng hợp dữ liệu, 𝑥 là trạng thái đang được
ước lượng và 𝑧 = (𝑧 (1), ..., 𝑧 (𝑘)) là một chuỗi của 𝑘 quan sát trước đó của 𝑥.

21
 Cả hai phương pháp (ML và MAP) đều nhằm mục đích tìm giá trị khả dĩ nhất cho
trạng thái 𝑥. Tuy nhiên, ML giả định rằng 𝑥 là một điểm cố định nhưng chưa biết từ
không gian tham số, trong khi MAP coi 𝑥 là đầu ra của một biến ngẫu nhiên có hàm
mật độ xác suất tiên nghiệm đã biết. Cả hai phương pháp này đều tương đương khi
không có thông tin tiên nghiệm về 𝑥, nghĩa là khi chỉ có các quan sát.

2.1.3.2. Bộ lọc Kalman:

Bộ lọc Kalman là kỹ thuật ước tính phổ biến nhất. Ban đầu nó được đề xuất bởi
Kalman [8] và đã được nghiên cứu và ứng dụng rộng rãi kể từ đó.

Bộ lọc Kalman chủ yếu được sử dụng để kết hợp dữ liệu mức thấp. Nếu hệ thống
có thể được mô tả như một mô hình tuyến tính và sai số có thể được mô hình hóa
dưới dạng nhiễu Gaussian, thì bộ lọc Kalman đệ quy sẽ thu được các ước lượng thống
kê tối ưu [30]. Tuy nhiên đối với các mô hình phi tuyến và các phép đo phi tuyến cần
các phương pháp khác để giải quyết như Bộ lọc Kalman mở rộng (EKF) và bộ lọc
unscented Kalman filter (UKF). Bộ lọc Kalman mở rộng (EKF) là một cách tiếp cận
tối ưu để triển khai bộ lọc đệ quy phi tuyến [40]. EKF là một trong những phương
pháp thường được sử dụng nhất để tổng hợp dữ liệu trong các ứng dụng robot.

2.1.3.3. Bộ lọc Particle:

Bộ lọc hạt là triển khai đệ quy của các phương pháp Monte Carlo [11]. Phương
pháp này xây dựng hàm mật độ ở kỳ cập nhật bằng cách sử dụng một số mẫu ngẫu
nhiên được gọi là hạt. Các hạt được nhân giống theo thời gian với sự kết hợp của các
bước lấy mẫu và lấy mẫu lại. Ở mỗi lần lặp lại, bước lấy mẫu được sử dụng để loại
bỏ một số hạt, làm tăng mức độ liên quan của các vùng có xác suất sau cao hơn. Trong
quá trình lọc, một số hạt có cùng biến trạng thái được sử dụng và mỗi hạt có một
trọng lượng liên quan cho biết chất lượng của hạt. Do đó, ước tính là kết quả của tổng
trọng số của tất cả các hạt. Thuật toán lọc hạt tiêu chuẩn có hai giai đoạn: (1) giai
đoạn dự đoán và (2) giai đoạn cập nhật. Trong giai đoạn dự đoán, mỗi hạt được sửa
đổi theo mô hình hiện có và tính tổng nhiễu ngẫu nhiên để mô phỏng hiệu ứng nhiễu.

22
Sau đó, trong giai đoạn cập nhật, trọng lượng của mỗi hạt được đánh giá lại bằng cách
sử dụng quan sát cảm biến cuối cùng có sẵn và các hạt có trọng lượng thấp hơn sẽ bị
loại bỏ.

Bộ lọc hạt linh hoạt hơn bộ lọc Kalman và có thể đối phó với sự phụ thuộc phi
tuyến và mật độ phi Gaussian trong mô hình động và lỗi nhiễu.

Tuy nhiên, chúng có một số nhược điểm. Một số lượng lớn các hạt được yêu cầu
để có được một phương sai nhỏ trong bộ ước lượng. Cũng khó thiết lập trước số lượng
hạt tối ưu và số lượng hạt ảnh hưởng đáng kể đến chi phí tính toán.

2.1.3.4. Bộ lọc Kalman phân tán:

Bộ lọc Kalman phân tán yêu cầu đồng bộ thời gian chính xác giữa mỗi nguồn. Nói
cách khác, để sử dụng chính xác bộ lọc Kalman phân phối, thời gian từ tất cả các
nguồn phải được đồng bộ hóa. Sự đồng bộ hóa này thường đạt được thông qua việc
sử dụng các giao thức sử dụng thời gian đồng bộ toàn cầu, chẳng hạn như giao thức
thời gian mạng (NTP).

Các vấn đề đồng bộ hóa thời gian đã được chứng minh là có ảnh hưởng đến độ
chính xác của bộ lọc Kalman, tạo ra các ước tính không chính xác [26].

Nếu các ước lượng nhất quán và hiệp phương sai chéo đã biết (hoặc các ước lượng
không tương quan) thì có thể sử dụng các bộ lọc Kalman phân tán [19]. Tuy nhiên,
hiệp phương sai chéo phải được xác định chính xác hoặc các quan sát phải nhất quán.

2.1.3.5. Bộ lọc Particle phân tán:

Coates [25] đã sử dụng một bộ lọc hạt phân tán để theo dõi một môi trường có thể
được thu nhận bởi mô hình không gian-trạng thái Markovian, liên quan đến các quan
sát và động lực học phi tuyến.

Ngược lại, những nỗ lực trước đó để giải quyết các phép đo ngoài trình tự bằng
cách sử dụng bộ lọc hạt dựa trên việc tái tạo hàm mật độ xác suất đến thời điểm tức
thời của phép đo ngoài trình tự [42]. Trong bộ lọc hạt, bước này đòi hỏi chi phí tính
23
 toán lớn, ngoài không gian cần thiết để chứa các hạt trước đó. Để tránh vấn đề này,
Orton và Marrs [27] đã đề xuất lưu trữ thông tin về các hạt tại mỗi thời điểm tức thời,
tiết kiệm chi phí tính toán lại thông tin này. Kỹ thuật này gần đạt mức tối ưu, và khi
độ trễ tăng lên, kết quả chỉ bị ảnh hưởng một chút. Tuy nhiên, nó đòi hỏi một lượng
không gian rất lớn để lưu trữ trạng thái của các hạt tại mỗi thời điểm tức thời.

2.2. Phương án thực hiện trong luận văn:

Các loại cảm biến toàn cục sử dụng trong luận văn là các loại cảm biến không dây,
mà Gaussian Processes đã được đề xuất và sử dụng thành công để định vị trên tín
hiệu không dây trong các tài liệu [3], [7], [6]. Vậy nên trong luận văn cũng sẽ áp dụng
Gaussian Processes trong quá trình định vị robot.
Nếu hệ thống có thể được mô tả như một mô hình tuyến tính và sai số có thể được
mô hình hóa dưới dạng nhiễu Gaussian, thì bộ lọc Kalman sẽ cho ra được các ước
lượng tối ưu [4]. Tuy mô hình động học của robot không phải là mô hình tuyến tính
nhưng có thể tuyến tính hoá nó và áp dụng Kalman mở rộng. Vấn đề này đã được áp
dụng trong tài liệu [2].
Dựa vào ưu nhược điểm của các phương pháp ước tính trạng thái đã được đề cập
ở phần 2.1 và việc sử dụng Gaussian Processes thì phương pháp kết hợp dữ liệu cảm
biến sử dụng bộ lọc Kalman mở rộng là phù hợp.
Hướng áp dụng này cũng có nhiều bài báo trong và ngoài nước đã triển khai nhưng
chưa trình bày thực sự rõ ràng cách xác định ma trận mô hình quan sát để kết hợp
nhiều loại cảm biến.
Luận văn sử dụng bộ lọc Kalman mở rộng để kết hợp 2 loại cảm biến và trong đó
tập trung trình bày cách xác định ma trận mô hình quan sát để kết hợp nhiều loại cảm
biến.
Mục tiêu của luận văn là muốn áp dụng giải thuật kết hợp cảm biến này để định vị
robot khi di chuyển trong điều kiện ngoài trời nên loại cảm biến toàn cục mong muốn
được sử dụng là RTK – GPS, tuy nhiên vì tình hình dịch bệnh Covid nên không có
điều kiện để thực nghiệm ngoài trời loại cảm biến này. Do đó trong luận văn sử dụng

24
RTK – GPS trong quá trình mô phỏng và sử dụng cảm biến UWB để lấy kết quả thực
nghiệm. Cảm biến định vị cục bộ sử dụng trong luận văn là Odometry.
Điều khiển robot bám theo quỹ đạo theo luật ổn định Lyapunov và dựa trên sự kết
hợp nhiều cảm biến.

25
Chương 3. CƠ SỞ LÝ THUYẾT:
3.1. Bộ lọc Kalman:
3.1.1. Tổng quan về tính toán:

Bộ lọc Kalman sử dụng mô hình động lực học của hệ thống (ví dụ: các quy luật
vật lý của chuyển động), các đầu vào điều khiển đã biết cho hệ thống đó và nhiều
phép đo tuần tự (chẳng hạn như từ cảm biến) để tạo ra ước tính về các đại lượng thay
đổi của hệ thống (trạng thái của nó) tốt hơn so với ước tính thu được khi chỉ sử dụng
một phép đo bất kỳ. Như vậy, nó là một thuật toán tổng hợp cảm biến và kết hợp dữ
liệu phổ biến.

Dữ liệu cảm biến nhiễu, giá trị gần đúng trong các phương trình mô tả sự phát triển
của hệ thống và các yếu tố bên ngoài không được tính đến tất cả các giới hạn về khả
năng xác định trạng thái của hệ thống. Bộ lọc Kalman xử lý hiệu quả độ không đảm
bảo do dữ liệu cảm biến nhiễu và ở một mức độ nào đó cũng với các yếu tố ngẫu
nhiên bên ngoài.

Bộ lọc Kalman đưa ra ước tính về trạng thái của hệ thống dưới dạng giá trị trung
bình của trạng thái được dự đoán của hệ thống và của phép đo mới bằng cách sử dụng
giá trị trung bình có trọng số. Mục đích của trọng số là các giá trị có độ không đảm
bảo đo ước tính tốt hơn (tức là nhỏ hơn) được “tin cậy” hơn. Trọng số được tính toán
từ hiệp phương sai, một phép đo độ không đảm bảo đo ước tính của dự đoán về trạng
thái của hệ thống.

Kết quả của giá trị trung bình có trọng số là một ước lượng trạng thái mới nằm
giữa trạng thái được dự đoán và đo lường, và có độ không đảm bảo đo được ước tính
tốt hơn một trong hai. Quá trình này được lặp lại ở mọi bước thời gian, với ước tính
mới và hiệp phương sai của nó thông báo cho dự đoán được sử dụng trong lần lặp
sau. Điều này có nghĩa là bộ lọc Kalman hoạt động đệ quy và chỉ yêu cầu "phỏng
đoán tốt nhất" cuối cùng, thay vì toàn bộ lịch sử, về trạng thái của hệ thống để tính
trạng thái mới.

26
 Tính chắc chắn tương đối của các phép đo và ước tính trạng thái hiện tại là một
yếu tố quan trọng cần xem xét và thông thường người ta sẽ thảo luận về phản ứng của
bộ lọc về độ lợi của bộ lọc Kalman. Hệ số Kalman là trọng số tương đối được cung
cấp cho các phép đo và ước tính trạng thái hiện tại, và có thể được “điều chỉnh” để
đạt được hiệu suất cụ thể. Với mức hệ số Kalman cao, bộ lọc đặt nhiều trọng số hơn
vào các phép đo gần đây nhất và do đó theo dõi chúng một cách phản hồi hơn. Với
hệ số Kalman thấp, bộ lọc theo sát các dự đoán của mô hình hơn. Ở các cực trị, độ lợi
cao gần bằng một sẽ dẫn đến quỹ đạo ước tính nhảy vọt hơn, trong khi hệ số Kalman
gần bằng 0 sẽ làm dịu nhiễu nhưng làm giảm khả năng phản hồi.

Khi thực hiện các tính toán thực tế cho bộ lọc, ước tính trạng thái và hiệp phương
sai được thể hiện thành ma trận để xử lý nhiều thứ nguyên trong khối lượng trong
một tập hợp tính toán. Điều này cho phép biểu diễn các mối quan hệ tuyến tính giữa
các biến trạng thái khác nhau (chẳng hạn như vị trí, vận tốc và gia tốc) trong bất kỳ
mô hình chuyển đổi hoặc hiệp phương sai nào.
3.1.2. Mô hình bộ lọc Kalman tiêu chuẩn:

Các bộ lọc Kalman dựa trên hệ thống động lực học tuyến tính trong miền thời gian.
Chúng được mô hình hóa trên một chuỗi Markov được xây dựng trên các toán tử
tuyến tính bị xáo trộn bởi các error có thể bao gồm nhiễu Gaussian. Trạng thái của hệ
thống được biểu diễn dưới dạng vector các số thực. Tại mỗi khoảng tăng thời gian rời
rạc, một toán tử tuyến tính được áp dụng cho trạng thái để tạo ra trạng thái mới, với
một số nhiễu được trộn vào và tùy chọn một số thông tin từ các điều khiển trên hệ
thống nếu chúng được biết đến. Sau đó, một toán tử tuyến tính khác được trộn với
nhiều nhiễu hơn tạo ra các kết quả đầu ra quan sát được từ trạng thái thực (“ẩn”). Bộ
lọc Kalman có thể được coi là tương tự với mô hình Markov ẩn, với điểm khác biệt
chính là các biến trạng thái ẩn nhận giá trị trong một không gian liên tục (trái ngược
với một không gian trạng thái rời rạc như trong mô hình Markov ẩn). Có một sự đối
ngẫu mạnh mẽ giữa các phương trình của Bộ lọc Kalman và các phương trình của mô

27
hình Markov ẩn. Đánh giá về mô hình này và các mô hình khác được đưa ra trong
Roweis và Ghahramani (1999), [32] và Hamilton (1994), Chương 13. [27]

Để sử dụng bộ lọc Kalman để ước tính trạng thái bên trong của một quá trình chỉ
đưa ra một chuỗi các quan sát nhiễu, người ta phải lập mô hình quá trình phù hợp với
khuôn khổ của bộ lọc Kalman. Điều này có nghĩa là xác định các ma trận sau: Fk là
mô hình chuyển đổi trạng thái, Hk là mô hình quan sát, Qk là hiệp phương sai của
nhiễu quá trình, Rk là hiệp phương sai của tiếng nhiễu quan sát; và đôi khi là Bk, mô
hình đầu vào điều khiển, cho mỗi bước thời gian, k, như được mô tả bên dưới.

Hình 4 Mô hình bên trong bộ lọc Kalman

Mô hình bộ lọc Kalman giả định trạng thái đúng tại thời điểm k được phát triển từ
trạng thái ở (k-1) theo:

Xk = Fk xk-1 + Bk uk + Wk (1)
Với:

• Fk là mô hình chuyển trạng thái được áp dụng cho trạng thái trước đó Xk₋1

• Bk là mô hình đầu vào điều khiển được áp dụng cho vectơ điều khiển uk

• Wk là nhiễu quá trình được giả định rút ra từ phân phối chuẩn đa biến trung bình
bằng 0 với hiệp phương sai Qk.

28
 Wk ∼ N (0; Qk)

Tại thời điểm k, một quan sát (hoặc phép đo) zk của trạng thái thực Xk được thực hiện
theo:

zk = Ck Xk + vk (2)
trong đó Ck là mô hình quan sát ánh xạ không gian trạng thái thực vào không gian
quan sát và vk là nhiễu quan sát được giả định là nhiễu trắng Gaussian trung bình bằng
0 với hiệp phương sai Rk.

vk ∼ N (0; Rk)

Trạng thái ban đầu và các vectơ nhiễu ở mỗi bước {X0, w1, …, wk, v1, …, vk} đều
được giả định là độc lập lẫn nhau.

Bộ lọc Kalman được áp dụng bằng cách thực hiện các bước sau:

Ở bước ước lượng tiên nghiệm trạng thái của hệ thống ở thời điểm k+1 được xác định
như sau:

𝐗 𝑘+1|𝑘 = 𝐹𝑘 𝐗 𝑘|𝑘 + 𝐁𝑘 𝐮𝑘 (3)

Sau đó, tính toán ma trận hiệp phương sai sai số dự đoán P cho thời điểm k + 1:

𝐏𝑘+1|𝑘 = 𝐅𝑘 𝐏𝑘|𝑘 𝐅𝑘 𝑇 + Q (4)

Ma trận hệ số Kalman:

𝐏𝑘+1|𝑘 𝑪𝑇
𝐊 𝑘+1 = (5)
𝑪𝐏𝑘+1|𝑘 𝑪𝑇 + 𝑹
Cập nhật lại trạng thái của hệ thống dựa vào kết quả đo lường:

𝐗 𝑘+1|𝑘+1 = 𝐗 𝑘+1|𝑘 + 𝐊 𝑘+1 (𝒛𝑘+1 − 𝑪𝐗 𝑘+1|𝑘 ) (6)

Ma trận hiệp phương sai mới được cập nhật lại như sau:

𝐏𝑘+1|𝑘+1 = (𝑰 − 𝐊 𝑘+1 𝑪)𝐏𝑘+1|𝑘 (𝑰 − 𝐊 𝑘+1 𝑪)𝑇 + 𝐊 𝑘+1 𝑹𝐊 𝑘+1 𝑻 (7)

Với 𝑰 là ma trận đơn vị.

29
Hoạt động của giải thuật có thể được trình bày trong Pseudocode sau:

Algorithm 1: Bộ lọc Kalman

Input: 𝐮k , 𝒛𝑘+1 , 𝑿𝑘 , 𝐏k

Output: 𝐗 k+1 , 𝑷𝑘+1

Data: 𝑭𝑘 , 𝑩𝑘 , C, R, Q

Initialization: X0, P0

for current time k do

Bước ước tính KF:

𝐗 𝑘+1|𝑘 = 𝑭𝑘 𝐗 𝑘|𝑘 + 𝐁𝑘 𝐮𝑘

𝐏𝑘+1|𝑘 = 𝐅𝑘 𝐏𝑘|𝑘 𝐅𝑘 𝑇 + Q

Bước hiệu chỉnh KF:

𝐏𝑘+1|𝑘 𝑪𝑇
𝐊 𝑘+1 =
𝑪𝐏𝑘+1|𝑘 𝑪𝑇 + 𝑹

𝐗 𝑘+1|𝑘+1 = 𝐗 𝑘+1|𝑘 + 𝐊 𝑘+1 (𝐳𝑘+1 − 𝑪𝐗 𝑘+1|𝑘 )

𝐏𝑘+1|𝑘+1 = (𝑰 − 𝐊 𝑘+1 𝑪)𝐏𝑘+1|𝑘 (𝑰 − 𝐊 𝑘+1 𝑪)𝑇 + 𝐊 𝑘+1 𝑹𝐊 𝑘+1 𝑻

end

30
Chương 4. THIẾT KẾ GIẢI THUẬT KẾT HỢP CẢM BIẾN VÀ BỘ ĐIỀU
KHIỂN:
4.1. Thiết kế giải thuật kết hợp cảm biến bằng bộ lọc Kalman mở rộng (EKF):
4.1.1. Định vị robot bằng EKF:
Vì quá trình di chuyển của robot quỹ đạo thường không phải là dạng tuyến tính
nên không thể áp dụng trực tiếp bộ lọc Kalman vào việc định vị robot. Thay vào đó
sử dụng bộ lọc Kalman mở rộng sẽ giải quyết được vấn đề này.
Với mô hình của xe hai bánh được trình bày trong Hình 1. Quãng đường đi được
của mỗi bánh xe trái và phải được xác định như sau:

𝜋𝐷𝑛
∆𝑆𝐿,𝑅 = 𝑁 (8)
𝑛𝐶𝑒 𝐿,𝑅

Với:

𝐷𝑛 là đường kính bánh xe

𝐶𝑒 là độ phân giải của encoder

n là tỷ số truyền giữa động cơ và bánh xe

N là số xung tăng được ghi nhận bởi encoder

Hình 5 Độ dịch chuyển của robot dạng 2 bánh

31
Với khoảng thời gian lấy mẫu của bộ mã hóa là T, vận tốc tuyến tính và vận tốc góc
của robot có thể được tính như sau:

∆𝑆𝑅 +∆𝑆𝐿
ʋ= (9)
2𝑇
∆𝑆𝑅 −∆𝑆𝐿
ω= (10)
𝑏𝑇

Với b là khoảng cách của hai odometry encoder.

Nếu chu kỳ lấy mẫu rất nhỏ, và đặt trạng thái của robot là
X(t)=[x(t) y(t) θ(t)]𝑇 thì mô hình động học của robot có thể được mô tả bằng
các phương trình sau:

𝑥̇ (𝑡 ) 𝑣(𝑡)𝑐𝑜𝑠θ(𝑡)
𝐗̇(t) = [𝑦̇ (𝑡 )] = [ 𝑣(𝑡)𝑠𝑖𝑛𝜃(𝑡)] (11)
𝜃̇(𝑡 ) 𝜔(𝑡)

Đến đây phương trình trạng thái của robot:

𝑥(𝑡−1) 𝑣(𝑡)𝑐𝑜𝑠θ(𝑡)
̇ 𝑦
X(t) = X(t-1) + 𝐗 (𝑡) = [ (𝑡−1) ] + [𝑣(𝑡)𝑠𝑖𝑛𝜃(𝑡)] (12)
𝜃(𝑡−1) 𝜔(𝑡)

Với control input u(t)=[v(t) ω(t)]𝑇 , có thể viết lại mô hình động học với dạng
phương trình vi phân như sau:

𝑋 (𝑡 ) = 𝑓(𝑿(𝑡 ), 𝒖(𝑡 )) (13)

Giả định rằng trong thời gian lấy mẫu này, vận tốc tuyến tính và vận tốc góc
không đổi và xe đang đi theo đường vòng cung, khi đó:

Nếu xét bước bắt đầu là k giai đoạn cập nhật:

32
 𝐗−
k+1 = 𝑓(𝑿(𝑘 ), 𝒖(𝑘 )) = 𝐗 k + 𝐁k 𝐮k (14)
−
𝐏k+1 = 𝑨k 𝐏k 𝑨Tk + 𝐐k (15)
Với:

𝑿𝑘 = [𝑥𝑘 𝑦𝑘 𝜃𝑘 ]𝑇

𝑩𝑘 được xác định như sau:

𝜕𝑔𝑥 𝜕𝑔𝑥 𝜕𝑔𝑥

𝜕𝑥 𝜕𝑦 𝜕𝜃 Δ𝜃𝑘
𝑇𝑐𝑜𝑠 (𝜃𝑘 + ) 0
𝜕𝑔𝑦 𝜕𝑔𝑦 𝜕𝑔𝑦 2
𝑩𝑘 = = Δ𝜃𝑘 (16)
𝜕𝑥 𝜕𝑦 𝜕𝜃 𝑇𝑠𝑖𝑛 (𝜃𝑘 + ) 0
𝜕𝑔𝜃 𝜕𝑔𝜃 𝜕𝑔𝜃 2
[ 0 1]
[ 𝜕𝑥 𝜕𝑦 𝜕𝜃 ]

𝑨𝑘 được xác định như sau:

1 0 −𝑣𝑘 𝑇 sin 𝜃𝑘
𝑨𝑘 = [0 1 𝑣𝑘 𝑇 cos 𝜃𝑘 ] (17)
0 0 1

Theo như Wang, C. M. [10] thì ma trận nhiễu quá trình khi tuyến tính hóa được xác
định như sau:

𝑸𝑘 = [𝑸𝟏 𝑸𝟐 𝑸𝟑 ] (18)

𝑄11 𝑇 + 𝑄33 (𝑇 3 /3)𝑣𝑘2 𝑠𝑖𝑛2 𝜃𝑘

𝑸𝟏 = [−𝑄33 (𝑇 3 /3)𝑣𝑘2 sin 𝜃𝑘 cos 𝜃𝑘 ] (19)
−𝑄33 (𝑇 2 /2)𝑣𝑘 sin 𝜃𝑘

−𝑄33 (𝑇 3 /3)𝑣𝑘2 sin 𝜃𝑘 cos 𝜃𝑘

𝑸𝟐 = [𝑄22 𝑇 + 𝑄33 (𝑇 3 /3)𝑣𝑘2 𝑐𝑜𝑠 2 𝜃𝑘 ] (20)
𝑄33 (𝑇 2 /2)𝑣𝑘 cos 𝜃𝑘

33
 −𝑄33 (𝑇 2 /2)𝑣𝑘 sin 𝜃𝑘
𝑸𝟑 = [ 𝑄33 (𝑇 2 /2)𝑣𝑘 cos 𝜃𝑘 ] (21)
𝑄33 𝑇
Với Q11=𝜎𝑥2 , Q22=𝜎𝑦2 , Q33=𝜎𝜃2 là đường chéo các phần tử của ma trận hiệp
phương sai Q (t) của w(t) trong phương trình (1).

Ở giai đoạn hiệu chỉnh:

− T − T −1
𝐊 k+1 = 𝐏k+1 𝐂k+1 [𝐂k+1 𝐏k+1 𝐂k+1 + 𝐑 k+1 ] (22)

𝐗 k+1 = 𝐗 − −
k+1 + 𝐊 k+1 [𝒛k+1 − 𝐂k+1 𝐗 k+1 ] (23)

𝑷𝑘+1 = [𝐈 − 𝑲𝑘+1 𝑪𝑘+1 ]𝑷−

𝑘+1 (24)
Trong đó:

Ma trận nhiễu đo lường:

𝜎𝑥2 0 0
𝑹=[0 𝜎𝑦2 0] (25)
0 0 𝜎𝜃2

Vector giá trị đo lường:

𝑥𝑚𝑒𝑎
𝑧𝑘+1 =[𝑦𝑚𝑒𝑎 ] (26)
𝜃𝑚𝑒𝑎
𝑥𝑚𝑒𝑎 , 𝑦𝑚𝑒𝑎 , 𝜃𝑚𝑒𝑎 lần lượt là giá trị đo lường của x, y, 𝜃 thông qua các cảm biến.

Ma trận quan sát:

34
 1 0 0
𝑪𝑘+1 =[0 1 0] (27)
0 0 1
Ma trận I là ma trận đơn vị:

1 0 0
I =[0 1 0] (28)
0 0 1
Hoạt động của giải thuật có thể được trình bày trong Pseudocode sau:

Algorithm 2: Định vị robot bằng EKF

Input: 𝐮k , 𝒛𝑘+1 , 𝑿𝑘 , 𝐏k

Output: 𝐗 k+1 , 𝑷𝑘+1

Data: 𝑨𝑘 , 𝑩𝑘 , C phương trình (26), R phương trình (25), 𝑸𝑘

Initialization: X0, P0

for current time k do

Bước ước tính EKF:

𝐗−
k+1 = 𝐗 k + 𝐁k 𝐮k

−
𝐏k+1 = 𝑨k 𝐏k 𝑨Tk + 𝐐k

Bước hiệu chỉnh EKF:

− T − T −1
𝐊 k+1 = 𝐏k+1 𝐂k+1 [𝐂k+1 𝐏k+1 𝐂k+1 + 𝐑 k+1 ]

𝐗 k+1 = 𝐗 − −
k+1 + 𝐊 k+1 [𝐳k+1 − 𝐂k+1 𝐗 k+1 ]

𝑷𝑘+1 = [𝐈 − 𝑲𝑘+1 𝑪𝑘+1 ]𝑷−

𝑘+1

Ước lượng vị trí dùng phương trình (14)

Ước tính ma trận hiệp phương sai dùng phương trình (15)

end

35
4.1.2. Kết hợp cảm biến trong định vị robot bằng EKF:
Kết hợp nhiều cảm biến trong định vị robot bằng EKF khác biệt với định vị robot
bằng EKF dùng một cảm biến ở chỗ lựa chọn ma trận quan sát C sao cho phù hợp với
vector đo lường z.
4.1.2.1. Xác định mô hình quan sát khi kết hợp cảm biến – ma trận C:
Đối với mô hình quan sát không có kết hợp dữ liệu thì ma trận C sẽ là ma trận
vuông nxn với n là kích thước theo hàng của vector đo lường z.
Khi kết hợp cảm biến, ở bước hiệu chỉnh trạng thái của robot, kết quả quan sát:

𝐳k+1 − 𝐂k+1 𝐗 −
k+1 (29)
Trong đó:

𝑥𝑠1
…
𝑥𝑠𝑖
𝑦𝑠1
𝐳k+1 = … (30)
𝑦𝑠𝑗
𝜃𝑠1
…
[𝜃𝑠𝑛 ]
Với:
𝑥𝑠1 , …, 𝑥𝑠𝑖 là tọa độ theo phương x được ghi nhận theo các cảm biến số 1 đến thứ i
𝑦𝑠1 , …, 𝑦𝑠𝑗 là tọa độ theo phương y được ghi nhận theo các cảm biến số 1 đến thứ j
𝜃𝑠1 , …, 𝜃𝑠𝑛 là hướng của robot được ghi nhận theo các cảm biến số 1 đến thứ n

−
xk+1
− −
𝐗 k+1 = [yk+1 ]
− (31)
𝜃k+1

Để thực hiện được phương trình (29) thì kích thước của ma trận C phải là (mx3) với
m= i+j+n.

36
Mặt khác các phần tử bên trong ma trận C:

1 0 0
… 0 0
1 0 0
0 1 0
C= 0 … 0 (32)
0 1 0
0 0 1
0 0 …
[0 0 1 ]mx3
Khi đó phương trình (29) sẽ có kết quả:

𝑥𝑠1 1 0 0
… … 0 0
𝑥𝑠𝑖 1 0 0
𝑦𝑠1 −
0 1 0 xk+1
−
𝐳k+1 − 𝐂k+1 𝐗 −
k+1 = … − 0 … 0 . [yk+1 ]
𝑦𝑠𝑗 0 1 0
−
𝜃k+1
𝜃𝑠1 0 0 1
… 0 0 …
[𝜃𝑠𝑛 ] [0 0 1 ]mx3
− (33)
𝑥𝑠1 − xk+1
…
−
𝑥𝑠𝑖 − xk+1
−
𝑦𝑠1 − yk+1
= …
−
𝑦𝑠𝑗 − yk+1
−
𝜃𝑠1 − 𝜃k+1
…
−
[𝜃𝑠𝑛 − 𝜃k+1 ]𝑚𝑥1

Với ma trận C vừa được xác định thì ma trận hệ số Kalman được xác định ở phương
trình (22) trở thành:

37
 𝑘1
𝑘
K = [ 2] (34)
…
𝑘𝑚 𝑚𝑥1

Trong luận văn, kết hợp hai cảm biến là encoder và một cảm biến định vị toàn cục:
GPS trong mô phỏng và UWB trong kiểm tra thực nghiệm.
Khi đó ma trận z là:

𝑥_𝐺𝑃𝑆
𝑥_𝑜𝑑𝑜
𝒛(𝑡) = 𝑦_𝐺𝑃𝑆 (35)
𝑦_𝑜𝑑𝑜
[ 𝜃 ]

Do đó ma trận quan sát C là:

1 0 0
1 0 0
𝑪= 0 1 0 (36)
0 1 0
[0 0 1]

4.1.2.2. Xác định ma trận R:

Ma trận sai số đo lường R được xác định như sau:

𝜎𝑥_𝐺𝑃𝑆 2 0 0 0 0
2
0 𝜎𝑥_𝑜𝑑𝑜 0 0 0
𝑹= 0 0 𝜎𝑦_𝐺𝑃𝑆 2 0 0 (37)
2
0 0 0 𝜎𝑦_𝑜𝑑𝑜 0
[ 0 0 0 0 𝜎𝜃 2 ]
Tuy nhiên đối với encoder thì sai số bị cộng dồn theo từng chu kỳ nên hệ số sai số
trong ma trận R của encoder cũng sẽ được cộng dồn.
4.1.2.3. Phương đầy đủ của giải thuật kết hợp cảm biến bằng EKF:

38
 Bước cập nhật:

𝐗−
k+1 = 𝐗 k + 𝐁k 𝐮k (38)
−
𝐏k+1 = 𝑨k 𝐏k 𝑨Tk + 𝐐k (39)

Bước hiệu chỉnh:

− T − T −1
𝐊 k+1 = 𝐏k+1 𝐂k+1 [𝐂k+1 𝐏k+1 𝐂k+1 + 𝐑 k+1 ] (40)
𝐗 k+1 = 𝐗 − −
k+1 + 𝐊 k+1 [𝒛k+1 − 𝐂k+1 𝐗 k+1 ] (41)
𝑷𝑘+1 = [𝐈 − 𝑲𝑘+1 𝑪𝑘+1 ]𝑷−
𝑘+1 (42)

Với:

u(k)= [𝑣 (𝑘 ) 𝜔(𝑘)]𝑇

𝑿𝑘 = [𝑥𝑘 𝑦𝑘 𝜃𝑘 ]𝑇
Δ𝜃𝑘
𝑇𝑐𝑜𝑠 (𝜃𝑘 + ) 0
2
𝑩𝑘 = Δ𝜃𝑘 (43)
𝑇𝑠𝑖𝑛 (𝜃𝑘 + ) 0
2
[ 0 1]
1 0 −𝑣𝑘 𝑇 sin 𝜃𝑘
𝑨𝑘 = [0 1 𝑣𝑘 𝑇 cos 𝜃𝑘 ] (44)
0 0 1
𝑸𝑘 = [𝑸𝟏 𝑸𝟐 𝑸𝟑 ] (45)
𝑄11 𝑇 + 𝑄33 (𝑇 3 /3)𝑣𝑘2 𝑠𝑖𝑛2 𝜃𝑘 (46)
𝑸𝟏 = [−𝑄33 (𝑇 3 /3)𝑣𝑘2 sin 𝜃𝑘 cos 𝜃𝑘 ]
−𝑄33 (𝑇 2 /2)𝑣𝑘 sin 𝜃𝑘
−𝑄33 (𝑇 3 /3)𝑣𝑘2 sin 𝜃𝑘 cos 𝜃𝑘 (47)
𝑸𝟐 = [𝑄22 𝑇 + 𝑄33 (𝑇 3 /3)𝑣𝑘2 𝑐𝑜𝑠 2 𝜃𝑘 ]
𝑄33 (𝑇 2 /2)𝑣𝑘 cos 𝜃𝑘

39
 −𝑄33 (𝑇 2 /2)𝑣𝑘 sin 𝜃𝑘 (48)
𝑸𝟑 = [ 𝑄33 (𝑇 2 /2)𝑣𝑘 cos 𝜃𝑘 ]
𝑄33 𝑇
𝑥_𝐺𝑃𝑆
𝑥_𝑜𝑑𝑜
𝒛(𝑡) = 𝑦_𝐺𝑃𝑆 (49)
𝑦_𝑜𝑑𝑜
[ 𝜃 ]
1 0 0 (50)
1 0 0
𝑪= 0 1 0
0 1 0
[0 0 1]
𝜎𝑥_𝐺𝑃𝑆 2 0 0 0 0
2
0 𝜎𝑥_𝑜𝑑𝑜 0 0 0
𝑹= 0 0 𝜎𝑦_𝐺𝑃𝑆 2 0 0 (51)
0 0 0 𝜎𝑦_𝑜𝑑𝑜 2 0
[ 0 0 0 0 𝜎𝜃 2 ]

Q11=𝜎𝑥2 , Q22=𝜎𝑦2 , Q33=𝜎𝜃2

Hoạt động của giải thuật có thể được trình bày trong Pseudocode sau:

Algorithm 2: Kết hợp cảm biến khi định vị robot bằng EKF

Input: 𝐮k , 𝒛𝑘+1 , 𝑿𝑘 , 𝐏k

Output: 𝐗 k+1 , 𝑷𝑘+1

Data: 𝑨𝑘 , 𝑩𝑘 , C phương trình (36), R phương trình (37), 𝑸𝑘

Initialization: X0, P0

for current time k do

Bước ước tính EKF:

𝐗−
k+1 = 𝐗 k + 𝐁k 𝐮k

40
 −
𝐏k+1 = 𝑨k 𝐏k 𝑨Tk + 𝐐k

Bước hiệu chỉnh EKF:

− T − T −1
𝐊 k+1 = 𝐏k+1 𝐂k+1 [𝐂k+1 𝐏k+1 𝐂k+1 + 𝐑 k+1 ]

𝐗 k+1 = 𝐗 − −
k+1 + 𝐊 k+1 [𝐳k+1 − 𝐂k+1 𝐗 k+1 ]

𝑷𝑘+1 = [𝐈 − 𝑲𝑘+1 𝑪𝑘+1 ]𝑷−

𝑘+1

Ước lượng vị trí dùng phương trình (14)

Ước tính ma trận hiệp phương sai dùng phương trình (15)

end

4.1.3. Mô phỏng kết hợp cảm biến trong định vị robot bằng EKF với tín hiệu giả lập:
4.1.3.1. Thông số tín hiệu cảm biến giả lập:
Các loại cảm biến được dùng trong quá trình mô phỏng là cảm biến RTK-GPS và
Encoder.
Cảm biến GPS được giả sử là cảm biến RTK-GPS với sai số theo phương x và
phương y là 150 mm, Odometry được giả sử sai số 1mm theo mỗi phương x, y.

xGPS(i) = x(i) + rand*exGPS;

yGPS(i) = y(i) + rand*eyGPS;

Với:
xGPS, yGPS là tọa độ giả lập của cảm biến GPS.
exGPS, eyGPS lần lượt là sai số theo phương x và y của cảm biến GPS.

exGPS = eyGPS = 0.15

Cảm biến Odometry:

xodo(i)=xodo(i-1) + 𝑥̇ (i)+rand*exodo;
yodo(i)=yodo(i-1) + 𝑦̇ (i)+rand*eyodo;

41
Với:
xodo, yodo là tọa độ giả lập của Odometry.
exodo, eyodo lần lượt là sai số theo phương x và y của Odometry.
exodo = eyodo = 0.001
Kết quả mô phỏng tín hiệu cảm biến GPS và Odometry khi robot di chuyển theo biên
dạng zigzag:

Hình 6 Mô phỏng tín hiệu giả lập cảm biến GPS và Odometry

4.1.3.2. Kết quả mô phỏng:

So sánh kết quả áp dụng kết hợp cảm biến GPS và Odometry bằng EKF và chỉ lọc tín
hiệu GPS bằng EKF:

Theo như kết quả mô phỏng ở Hình 7, Hình 8, Hình 9 thì việc kết hợp cảm biến
GPS và Odometry cho ra kết quả ước lượng vị trí của robot phẳng và mịn hơn so với
việc chỉ dùng EKF lọc tín hiệu GPS. Tuy nhiên đối với các góc rẽ thì chỉ dùng EKF
lọc tín hiệu GPS lại bám sát đường true hơn.

42
 Hình 7: Mô phỏng kết hợp cảm biến GPS và Odometry bằng EKF và chỉ lọc tín hiệu GPS bằng
EKF

Hình 8 Mô phỏng kết hợp cảm biến GPS và Odometry bằng EKF và chỉ lọc tín hiệu GPS bằng EKF - ở
đoạn thẳng

43
Hình 9 Mô phỏng kết hợp cảm biến GPS và Odometry bằng EKF và chỉ lọc tín hiệu GPS bằng EKF - ở
góc rẽ.

4.1.4. Kiểm chứng giải thuật kết hợp cảm biến với dữ liệu cảm biến thực:
Để kiểm chứng khả năng xác định vị trí và hướng của robot trong quá trình di
chuyển thực tế thì trong luận văn có thực hiện một thực nghiệm như sau:
Robot được bố trí hai loại cảm biến là Encoder và UWB sau đó cho di chuyển theo
một quỹ đạo cho trước với tốc độ nhất định, đọc dữ liệu cảm biến trong quá trình di
chuyển và áp dụng giải thuật kết hợp cảm biến để quan sát kết quả đạt được.
So sánh kết quả áp dụng giải thuật kết hợp cảm biến bằng EKF với tín hiệu của
Odometry, UWB cũng như so sánh tính hiệu quả của nó đối với kết quả chỉ dùng bộ
lọc EKF để lọc tín hiệu UWB.
4.1.4.1. Thông số cảm biến:
a) Cảm biến UWB (DWM1000): là modul thu phát không dây theo tiêu chuẩn
IEEE802.15.4-2011, UWB dựa trên IC DW1000 của Decawave. Modul này cho
phép xác định vị trí của các đối tượng trong hệ thống định vị thời gian thực (RTLS)
44
 với độ chính xác 10 cm trong nhà, truyền thông tốc độ dữ liệu cao lên đến 6,8
Mb/s và phạm vi hoạt động 300m.

Hình 10 Modul DWM1000

Thông số kỹ thuật:
• Sử dụng chip DWM1000 của hãng DecaWave Giao thức: IR-UWB
• Hỗ trợ: 110 Kbps, 850 Kbps & amp; 6.8 Mbps data rate
• Có 6 dãy tần số: từ 3.4 Ghz đến 6.5 Ghz
• Ăng ten được tích hợp trên module
• Năng lượng truyền tải có thể thiết lập từ -35 dBm/MHz đến -62 dBm/MHz
• Hỗ trợ dung lượng gói tin lên tới 1023 byte
• Nguồn hoạt động: 2.8V - 3.6V.
b) Nguyên lý hoạt động của cảm biến UWB - Định vị bằng phương pháp Ultra-
wideband:
Hệ thống định vị UWB bao gồm 1 thẻ tag di động được cố định trên thân AGV
và 4 điểm neo anchor được đặt dưới sàn với vị trí cố định.
- Thuật toán xác định thời gian truyền Time of Flight (TOF): Theo phương
pháp two‐way ranging [39], đo thời gian truyền (TOF) của các thẻ tag đến
anchor, sau đó nhân TOF với tốc độ ánh sáng (xem tốc độ truyền sóng bằng

45
 tốc độ ánh sáng) thì ta có thể thu được khoảng cách giữa các thẻ tag và thẻ
anchor. Nguyên tắc của thuật toán được thể hiện trong Hình 11.

Hình 11 Thời gian truyền (TOF) trong phương pháp two‐way ranging.
Thẻ tag gởi tín hiệu yêu cầu đến điểm neo anchor và ghi lại thời gian hiện tại
Tt1. Điểm neo ngay lập tức trả lại tín hiệu phản hồi sau khi nhận được tín hiệu,
sau đó ghi lại thời điểm Ta1 khi tín hiệu được nhận và thời điểm Ta2 khi tín
hiệu phản hồi được gởi đi. Thẻ tag cũng ghi lại thời gian nhận được tín hiệu
phản hồi Tt2. Sau đó, nó tính toán chênh lệch thời gian giữa Tt1 - Tt2 giữa lúc
gởi tính hiệu và nhận tín hiệu của thẻ tag, cũng như thời gian phản hồi Ta1 - Ta2
của điểm neo. Tính Time of Flight (TOF) theo công thức:
(𝑇𝑡1 − 𝑇𝑡2 ) − (𝑇𝑎1 − 𝑇𝑎2 )
𝑇𝑡𝑜𝑓 =
2
Bởi vì UWB là sóng điện từ nên có thể xem tốc độ truyền bằng với tốc độ ánh
sáng. Từ đó ta tính được khoảng cách từ thẻ tag đến các điểm neo:
S=𝑐. 𝑇𝑡𝑜𝑓
Với c là tốc độ ánh sáng 3.108 m/s.
- Thuật toán định vị trilateral centroid: Sau khi đo được khoảng cách giữa
các giữa thẻ tag và các điểm neo anchor, áp dụng thuật toán định vị trilateral
centroid để xác định vị trí của thẻ tag. Trong trường hợp lý tưởng, kết quả đo
khoảng cách là chính xác, ứng với 3 vòng tròn, trong đó tâm là mỗi điểm neo
và bán kính là khoảng cách từ điểm neo đến thẻ tag, phải giao nhau tại vị trí
46
của thẻ. Tuy nhiên, sẽ luôn có sai số trong thực tế, dẫn đến ba vòng trong
không giao nhau tại cùng một điểm như Hình 12.

Hình 12 Thuật toán định vị Trilateral centroid

Ta có các phương trình hình tròn:
(𝑥 − 𝑥1 )2 + (𝑦 − 𝑦1 )2 = 𝑆12
Giao giữa hình tròn tâm A và hình tròn tâm B: {
(𝑥 − 𝑥2 )2 + (𝑦 − 𝑦2 )2 = 𝑆22
(𝑥 − 𝑥1 )2 + (𝑦 − 𝑦1 )2 = 𝑆12
Giao giữa hình tròn tâm A và hình tròn tâm C: {
(𝑥 − 𝑥3 )2 + (𝑦 − 𝑦3 )2 = 𝑆32
(𝑥 − 𝑥2 )2 + (𝑦 − 𝑦2 )2 = 𝑆22
Giao giữa hình tròn tâm B và hình tròn tâm C: {
(𝑥 − 𝑥3 )2 + (𝑦 − 𝑦3 )2 = 𝑆32
Trong đó ta có tọa độ của 3 tâm đường tròn tương ứng là vị trí của các điểm
neo A(𝑥1 , 𝑦1 ), B(𝑥2 , 𝑦2 ), C(𝑥3 , 𝑦3 ) với bán kính các đường tròn S1, S2, S3, được
tính toán ở thuật toán phía trên.
Từ đó ta giải được các phương trình và tìm được 3 điểm giao tương ứng của
từng cặp đường tròn với nhau: (xab, yab), (xac, yac), (xbc, ybc). Ta tính toán trọng
tâm của tam giác tạo từ 3 điểm này sẽ tìm được tọa độ của thẻ tag:
𝑥𝑎𝑏+ 𝑥𝑏𝑐+ 𝑥𝑎𝑐 𝑦𝑎𝑏+ 𝑦𝑏𝑐+ 𝑦𝑎𝑐
(x, y) =( , )
3 3

47
4.1.4.2. Thông số robot:

Bảng 1: Thông số robot

Kích thước bao L455 × W381 × H237 mm

Khối lượng xe 9 kg

Khả năng tải 17 kg

Tốc độ cao nhất 1,2 m/s

Gia tốc lớn nhất 2 m/s2

Động cơ PITTMAN GM9236

Hình 13 Robot Pioneer DX3

4.1.4.3. Thông số quỹ đạo di chuyển:

Robot di chuyển theo quỹ đạo hình zigzag, các khoảng di chuyển theo phương x
là 0.6m, các khoảng di chuyển theo phương y là 1.8 m. Vận tốc khi di chuyển là 0.02
m/s. Tốc độ góc là 0.3142 rad/s khi robot rẽ tại các góc.

48
 Hình 14 Quỹ đạo di chuyển của robot.

4.1.4.4. Kết quả áp dụng giải thuật kết hợp cảm biến:

Hình 13 cho thấy rõ đặc trưng của tín hiệu cảm biến UWB và Encoder. Hai loại
cảm biến này thể hiện đầy đủ tính chất của hai phương pháp định vị cục bộ Odometry
và định vị toàn cục. Tín hiện encoder trơn mịn, ít dao động hơn nhiều so với tín hiệu
cảm biến UWB tuy nhiên bị sai số cộng dồn và càng ngày càng tăng. Tín hiệu UWB
thì ngược lại không bị ảnh hưởng nhiều bởi sai số cộng dồn nhưng lại dao động lớn.

Rõ ràng hai phương án định vị này mang những ưu nhược điểm có thể bù trừ cho
nhau.

Theo như kết quả được thể hiện trong Hình 17 việc áp dụng giải thuật kết hợp cảm
biến bằng EKF đã cho ra quỹ đạo mang được hai ưu điểm của odometry và UWB là
mịn hơn, ít biến động hơn so với UWB cũng như ít bị tác động của sai số cộng dộn
của Odometry.

Để kiểm tra tính hiệu quả của giải thuật kết hợp cảm biến bằng EKF, ta chỉ áp dụng
bộ lọc EKF đối với tín hiệu UWB xem thử kết quả hiệu quả ra sao. Kết quả quá trình
này thể hiện trong Hình 16.

49
Hình 15: Tín hiệu thực tế của cảm biến UWB và Odometry

Hình 16 Kết quả lọc nhiễu tín hiệu UWB bằng EKF

50
 Hình 17 Kết quả fusing sensor đối với UWB và Odometry

Hình 18 So sánh kết quả fusing sensor giữa UWB với Odometry bằng EKF và kết quả chỉ lọc UWB
bằng EKF

Xét tín hiệu thực tế của cảm biến UWB được thể hiện trong Hình 15 có thể tín hiệu
này rất xấu so với mô phỏng. Lý do gây ra vấn đề này là thời gian lấy mẫu của cảm
biến UWB hiện tại là 100 ms lớn hơn rất nhiều so với cảm biến mô phỏng là 1ms.

51
 Nếu tập trung vào xét tính hiệu quả của giải thuật kết hợp cảm biến thì có thể thấy
trong Hình 17 kết quả fusing sensor giữa UWB với Odometry bằng EKF có khả năng
định vị robot tốt hơn việc chỉ sử dụng UWB hay Odometry. Tín hiệu kết hợp mượt
hơn tín hiệu UWB, ít sai lệch hơn so với Odometry.

Khi so sánh kết quả fusing sensor giữa UWB với Odometry bằng EKF và kết quả
chỉ lọc UWB bằng EKF trong Hình 18 ta có thể nhận thấy kết quả fusion tốt hơn,
mượt hơn.

4.2. Thiết kế bộ điều khiển:

4.2.1. Mô hình động học của robot:

Hình 19 Mô hình động học của robot

Xét điểm C trùng với tâm của robot khi đó phương trình động của điểm 𝐶:

𝑥̇ 𝐶 = 𝑥̇
{ 𝑦̇ 𝐶 = 𝑦̇ (52)
𝜃̇𝐶 = 𝜃̇
Phương trình động lực của điểm 𝑅 (Reference) với 𝑣𝑅 là vận tốc mong muốn của
robot.

52
 𝑥̇ 𝑟 = 𝑣𝑟 cos 𝜃𝑟
{ 𝑦̇𝑟 = 𝑣𝑟 sin 𝜃𝑟 (53)
𝜃̇𝑟 = 𝜔𝑟
Phương trình quan hệ giữa 𝑣, 𝜔 và tốc độ góc của 2 bánh robot:
𝜔𝑟𝑤 1/𝑟 𝑏/𝑟 𝑣
[ 𝜔 ]=[ ][ ]
𝑙𝑤 1/𝑟 −𝑏/𝑟 𝜔 (54)

Bộ điều khiển sẽ được thiết kế để điểm C của robot bám theo điểm R trên quỹ đạo
cho trước với tốc độ 𝑣𝑅 . Sai số error được xác định như sau:

𝑒1 cos 𝜑 sin 𝜑 0 𝑥𝑟 − 𝑥𝐶
[𝑒2 ] = [− sin 𝜑 cos 𝜑 0] [ 𝑦𝑟 − 𝑦𝐶 ] (55)
𝑒3 0 0 1 𝜑𝑟 − 𝜑𝐶
Đạo hàm cả hai vế phương trình trên ta được:

𝑒1̇ 𝑣𝑟 cos 𝑒3 −1 𝑒2
𝑣
[𝑒2̇ ] = [ 𝑣𝑟 sin 𝑒3 ] + [ 0 − 𝑑 −𝑒1 ] [ ] (56)
𝜔𝑟 𝜔
𝑒3̇ 0 −1
Nếu robot đi theo hướng ngược lại thì thay d bằng -d.

4.2.2. Thiết kế bộ điều khiển:

Vấn đề theo dõi quỹ đạo mong muốn có thể hiểu là:
Với đường reference 𝒒𝒓 (t) = [ 𝑥𝑟 (𝑡 ) 𝑦𝑟 (𝑡) 𝜃𝑟 (𝑡)]T và vận tốc reference 𝒛𝒓 =
[𝒗𝒓 𝝎𝒓 ]T. Vấn đề bám quỹ đạo là tìm luật điều khiển vận tốc: 𝒛𝒅 = 𝑓𝑑 (𝒆, 𝒗𝒓 , 𝑪) sao
cho lim 𝒒(𝒕) = 𝒒𝒓 (𝒕). Với e, zr và C lần lượt là sai số vị trí, vận tốc reference và độ
𝑡→∞

lợi điều khiển.

Bộ điều khiển được thiết kế để đạt được e → 0 khi t → ∞; nói cách khác, điểm P bám
theo điểm tham chiếu R với vận tốc mong muốn.
Chọn hàm Lyapunov:

1 2 1
𝑉1 = (𝑒1 + 𝑒22 ) + (1 − cos 𝑒3 ) ≥ 0 (57)
2 𝐶2
Đạo hàm V1 ta được:

53
 1
𝑉1̇ = 𝑒1 𝑒̇1 + 𝑒2 𝑒̇2 + 𝑒̇ sin 𝑒3
𝐶2 3
1 (58)
= 𝑒1 (−𝜈 + 𝜈𝑟 cos 𝑒3 ) + sin 𝑒3 (𝜔𝑟 − 𝜔 + 𝐶2 𝑒2 𝜈𝑟 )
𝐶2

Chọn luật điều khiển:

𝜈 𝜈𝑟 cos 𝑒3 + 𝐶1 𝑒1
𝒖𝒅 = [ ] = [ ] (59)
𝜔 𝜔𝑟 + 𝐶2 𝜈𝑟 𝑒2 + 𝐶3 sin 𝑒3
Với 𝐶1 , 𝐶2 , 𝐶3 là các hằng số dương

Thay 𝒖𝒅 vào 𝑉1̇ ta được:

1 𝐶3
𝑉1̇ = 𝑒1 (−𝐶1 𝑒1 ) + sin 𝑒3 (−𝐶3 sin 𝑒3 ) = −𝐶1 𝑒1 2 − sin 𝑒3 2 (60)
𝐶2 𝐶2

Vì với 𝐶1 , 𝐶2 , 𝐶3 là các hằng số dương, và các 𝑒1 , 𝑒2 , 𝑒3 bất kỳ 𝑉1̇ luôn nhỏ hơn hoặc
bằng 0 do đó 𝑒1 , 𝑒2 , 𝑒3 sẽ tiến về 0 khi 𝑡 → ∞ theo tiêu chuẩn ổn định Lyapunov.

4.3. Kết quả mô phỏng điều khiển robot bám quỹ đạo khi kết hợp cảm biến bằng EKF:

Hình 20: Control block diagram

Mô phỏng điều khiển robot bám quỹ đạo khi kết hợp cảm biến bằng EKF với các
thông số như sau:

54
 Chiều dài robot: hagv =0.230 m
Chiều rộng robot: wagv=0.350 m
Khoảng cách 2 bánh: b=0.3328 m
Bán kính bánh xe: r=0.0975 m
Khoảng cách cảm biến và trục bánh xe: d=0.0 m
Hệ số điều khiển: 𝐶1 =15; 𝐶2 =120; 𝐶3 =20;
Chu kỳ: 100 ms
Sai số theo phương x của GPS: e_xGPS=150/1000;
Sai số theo phương x của Encoder: e_xodo=3/1000;
Sai số theo phương y của GPS: e_yGPS=150/1000;
Sai số theo phương y của Encoder: e_yodo=3/1000;
Vận tốc dài ref của robot là: v_ref = 1m/s;

Hình 21 Tín hiệu cảm biến mô phỏng trong quá trình bám quỹ đạo của robot với thời gian lấy mẫu
100ms

55
Tín hiệu cảm biến giả lập:

xGPS(i) = x(i) + rand. exGPS

(61)
yGPS(i) = y(i) + rand. eyGPS
xodo(i)=xodo(i-1) + 𝑥̇ (i)+rand. exodo (62)
yodo(i)=yodo(i-1) + 𝑦̇ (i)+rand. eyodo

Với kết quả mô phỏng kết hợp cảm biến với tín hiệu cảm biến UWB và Odometry
thực tế ở phần 4.1.4 trong chương 4, khi thời gian lấy mẫu thấp ảnh hưởng nhiều
đến chất lượng của giải thuật kết hợp. Do đó trong phần mô phỏng điều khiển bám
quỹ đạo này tiến hành mô phỏng cảm biến GPS và Odometry với thời gian lấy mẫu
là 100ms, kết quả mô phỏng tín hiệu cảm biến thể hiện ở Hình 24.

Hình 22: Kết quả mô phỏng điều khiển robot bám quỹ đạo khi kết hợp cảm biến bằng EKF với
thời gian lấy mẫu 100ms

56
 Hình 23: Tracking error với thời gian lấy mẫu 100ms

Tín hiệu mô phỏng cảm biến GPS biến động lớn nhưng dao động quanh quỹ đạo,
còn Odometry thì mượt hơn nhưng do ảnh hưởng của việc sai số cộng dồn nên sai
lệch tăng dần.
Sau khi áp dụng luật điều khiển động học robot ở phương trình (59) và định vị
bằng giải thuật kết hợp cảm biến EKF thì nhận được kết quả bám quỹ đạo như Hình
22. Tuy vẫn còn sai lệch nhiều do ảnh hưởng của việc thời gian lấy mẫu của cảm biến
lớn (100ms) nhưng robot vẫn bám được quỹ đạo với sai số tracking thể hiện ở Hình
23. Biên độ dao động của sai số về vị trí là xấp xỉ 0.1 m.
Để đánh giá sự ảnh hưởng của chất lượng cảm biến cũng như thời gian lấy mẫu
đến kết quả điều khiển bám quỹ đạo thì ta tiến hành mô phỏng điều khiển bám quỹ
đạo với cảm biến GPS và Encoder với thời gian lấy mẫu 1 ms. Ta nhận được kết quả
mô phỏng tín hiệu cảm biến giả lập ở Hình 24. Với thời gian lấy mẫu tăng lên thì tín

57
hiệu GPS dao động dày hơn còn đối với Odometry thì càng sai lệch lớn hơn do cộng
dồn nhiều lần hơn.

Hình 24 Tín hiệu cảm biến mô phỏng trong quá trình bám quỹ đạo của robot với thời gian lấy mẫu
100ms

Với thời gian lấy mẫu như thế này giúp cho quá trình ước lượng của giải thuật kết
hợp cảm biến trở lên tốt hơn và kéo theo đó kết quả điều khiển bám quỹ đạo cũng
được cải thiện đáng kể. Kết quả này được thể hiện ở Hình 25. Robot bám quỹ đạo
khá tốt mặc dù vẫn còn dao động ở mức nhỏ, khi phóng to lên ta nhận thấy rõ hơn
ở Hình 26. Vì kết quả điều khiển bám quỹ đạo tốt hơn nên dẫn đến sai số tracking
cũng tốt hơn nhiều, sai số tối đa thấp hơn ngưỡng 0.01 m, giảm 10 lần so với trường
hợp thời gian lấy mẫu cảm biến là 100ms thể hiện ở Hình 27 và Hình 28.

58
Hình 25 Kết quả mô phỏng điều khiển robot bám quỹ đạo khi kết hợp cảm biến bằng EKF với thời
gian lấy mẫu 1ms.

Hình 26 Kết quả mô phỏng điều khiển robot bám quỹ đạo khi kết hợp cảm biến bằng EKF với thời
gian lấy mẫu 1ms – phóng to.

59
 Hình 27 Tracking error với thời gian lấy mẫu 1ms

Hình 28 Tracking error với thời gian lấy mẫu 1ms – phóng to.

60
Chương 5. KẾT LUẬN VÀ HƯỚNG PHÁT TRIỂN:
5.1. Kết luận:
Với bài toán đặt ra là thiết kế giải thuật kết hợp dữ liệu nhiều cảm biến trong vấn
đề xác định vị trí và hướng của robot tự hành, Luận văn đã nghiên cứu, tìm hiểu các
phương pháp khả thi liên quan nhằm ứng dụng giải quyết bài toán. Từ đó lựa chọn
phương án kết hợp dữ liệu cảm biến bằng bộ lọc Kalman mở rộng để áp dụng cho
việc tự hành robot. Các đóng góp của luận văn gồm:
• Thứ nhất, tổng hợp, phân loại và đánh giá ưu nhược điểm của các phương pháp
kết hợp dữ liệu.
• Thứ hai, xây dựng mô hình tổng quát cho việc kết hợp dữ liệu của nhiều loại
cảm biến khi xác định vị trí và hướng của robot tự hành.
• Thứ ba, thiết kế giải thuật điều khiển robot tự hành dựa vào kết hợp dự liệu
của nhiều loại cảm biến.
• Thứ tư, tiến hành lập trình giải thuật và mô phỏng giải thuật đã đề xuất trên
nền tảng Matlab.
5.2. Hướng phát triển:

Nhìn chung luận văn chỉ mới dừng lại ở mức áp dụng các phương pháp kết hợp dữ
liệu đã được nghiên cứu thành công nên có những điểm cần tiếp tục phát triển thêm
trong tương lai:

Nghiên cứu thêm các giải pháp để cải thiện các thuật toán kết hợp dữ liệu nhằm
tăng khả năng hội tụ của các dự đoán cũng như giảm khối lượng tính toán của giải
thuật.

61
Tài liệu tham khảo:

[1]. A. Bosse, J. Roy, and D. Grenier. Data Fusion Concepts Applied to a Suite of
Dissimilar Sensors. Canadian Conference on Electrical and Computer Engineering,
1996, 2:692–695, May 1996.

[2]. Andre M. Santana, "Localization of a Mobile Robot based in Odometry and

Natural Landmarks using Extended Kalman Filter", ICINCO 2008, Proceedings of
the Fifth International Conference on Informatics in Control, Automation and
Robotics, Robotics and Automation 2, Funchal, Madeira, Portugal, May 11-15, 2008

[3]. Anton Schwaighofer, Marian Grigoras, Volker Tresp, and Clemens Hoffmann.
GPPS: A Gaussian Process Positioning System for Cellular Networks. In NIPS, 2003.

[4]. B. C. Luo and M. G. Kay, “Data fusion and sensor integration: state-of-the-art
1990s,” in Data Fusion in Robotics and Machine Intelligence, pp. 7–135, 1992.

[5]. B. V. Dasarathy, “Sensor fusion potential exploitation-innovative architectures

and illustrative applications,” Proceedings of the IEEE, vol. 85, no. 1, pp. 24–38,
1997.

[6]. Brian Ferris, Dieter Fox, and Neil D Lawrence. WiFi-SLAM Using Gaussian
Process Latent Variable Models. In IJCAI, volume 7, pages 2480–2485, 2007.

[7]. Brian Ferris, Dirk Haehnel, and Dieter Fox. Gaussian processes for signal
strength-based location estimation. In In proc. of robotics science and systems, 2006.

[8]. C. Brown, H. Durrant-Whyte, J. Leonard, B. Rao, and B. Steer, “Distributed data

fusion using Kalman filtering: a robotics application,” in Data, Fusion in Robotics
and Machine Intelligence, M. A. Abidi and R. C. Gonzalez, Eds., pp. 267–309, 1992.

[9]. C. Edwards, and S. Spurgeon, Sliding Mode Control: Theory and Applications.
London: Taylor and Franci, 1998.

62
[10]. C. M. Wang. Location estimation and uncertainty analysis for mobile robots.
Proceeding of the IEEE International Conference on Robotics and Automation (pp.
1230-1235),1988.

[11]. D. Crisan and A. Doucet, “A survey of convergence results on particle filtering

methods for practitioners,” IEEE Transactions on Signal Processing, vol. 50, no. 3,
pp. 736–746, 2002.

[12]. D. L. Hall and J. Llinas, “An introduction to multisensor data fusion,”

Proceedings of the IEEE, vol. 85, no. 1, pp. 6–23, 1997

[13]. E. P. Blasch and S. Plano, “JDL level 5 fusion model “user refinement” issues
and applications in group tracking,” in Proceedings of the Signal Processing, Sensor
Fusion, and Target Recognition XI, pp. 270–279, April 2002.

[14]. G. T. McKee. What can be fused? Multisensor Fusion for Computer Vision,
Nato Advanced Studies Institute Series F, 99:71–84, 1993.

[15]. H. F. Durrant-Whyte and M. Stevens, “Data fusion in decentralized sensing

networks,” in Proceedings of the 4th International Conference on Information Fusion,
pp. 302–307, Montreal, Canada, 2001.

[16]. H. F. Durrant-Whyte, “Sensor models and multisensor integration,”

International Journal of Robotics Research, vol. 7, no. 6, pp. 97–113, 1988.

[17]. H. K. Khalil, Nonlinear Systems, 3rd ed. Prentice Hall, 1996

[18]. Hamilton, J. (1994), Time Series Analysis, Princeton University Press. Chapter
13, 'The Kalman Filter'

[19]. J. K. Uhlmann, “Covariance consistency methods for faulttolerant distributed

data fusion,” Information Fusion, vol. 4, no. 3, pp. 201–215, 2003.

[20]. J. Llinas, C. Bowman, G. Rogova, A. Steinberg, E. Waltz, and F. White,

“Revisiting the JDL data fusion model II,” Technical Report, DTIC Document, 2004.

63
[21]. J. Manyika and H. Durrant-Whyte, Data Fusion and Sensor Management: A
Decentralized Information-Theoretic Approach, Prentice Hall, Upper Saddle River,
NJ, USA, 1995.

[22]. J.J.E. Slotine, Aplied Nonlinear Control. Prentine Hall, 1996

[23]. JDL, Data Fusion Lexicon. Technical Panel For C3, F.E. White, San Diego,
Calif, USA, Code 420, 1991.

[24]. K. C. Chang, C. Y. Chong, and Y. Bar-Shalom, “Joint probabilistic data

association in distributed sensor networks,” IEEE Transactions on Automatic
Control, vol. 31, no. 10, pp. 889–897,1986

[25]. M. Coates, “Distributed particle filters for sensor networks,” in Proceedings of

the 3rd International symposium on Information Processing in Sensor Networks
(ACM ’04), pp. 99–107, New York, NY, USA, 2004.

[26]. M. Manzo, T. Roosta, and S. Sastry, “Time synchronization in networks,” in

Proceedings of the 3rd ACM Workshop on Security of Ad Hoc and Sensor Networks
(SASN ’05), pp. 107–116, November 2005.

[27]. M. Orton and A. Marrs, “A Bayesian approach to multi-target tracking and data
fusion with Out-of-Sequence Measurements,” IEE Colloquium, no. 174, pp. 15/1–
15/5, 2001.

[28]. Mohinder S. Grewal and Angus P. Andrews

[29]. P. J. Nahin and J. L. Pokoski. NCTR Plus Sensor Fusion Equals IFFN or Can
Two Plus Two Equal Five? IEEE Transactions on Aerospace and Electronic Systems,
16(3):320–337, May 1980.

[30]. R. C. Luo and M. G. Kay, “Data fusion and sensor integration: state-of-the-art
1990s,” in Data Fusion in Robotics and Machine Intelligence, pp. 7–135, 1992.

64
[31]. R. C. Luo, C.-C. Yih, and K. L. Su, “Multisensor fusion and integration:
approaches, applications, and future research directions,” IEEE Sensors Journal, vol.
2, no. 2, pp. 107–119, 2002.

[32]. Roweis, S; Ghahramani, Z (1999). “A unifying review of linear gaussian

models”. Neural computation. 11 (2): 305–45. doi:10.1162/089976699300016674.
PMID 9950734

[33]. S. Lloyd, “Least squares quantization in pcm”, IEEE Transactions on

Information Theory, vol. 28, no. 2, pp. 129–137, 1982.

[34]. S. S. Blackman, “Association and fusion of multiple sensor data,” in

Multitarget-Multisensor: Tracking Advanced Applications, pp. 187–217, Artech
House, 1990.

[35]. Steffen L. Lauritzen. “Time series analysis in 1880. A discussion of

contributions made by T.N. Thiele”. International Statistical Review 49, 1981, 319–
333. JSTOR 1402616

[36]. Stratonovich, R. L. (1959). Optimum nonlinear systems which bring about a

separation of a signal with constant parameters from noise. Radiofizika, 2:6, pp. 892–
901.

[37]. T. E. Fortmann, Y. Bar-Shalom, and M. Scheffe, “Multi-target tracking using

joint probabilistic data association,” in Proceedings of the 19th IEEE Conference on
Decision and Control including the Symposium on Adaptive Processes, vol. 19, pp.
807– 812, December 1980.

[38]. V.I Utkin et al, Sliding Mode Control in Electromechanical Systems. Taylor &
Francis, 1999.

[39]. Wei, Z.; Lang, Y.; Yang, F.; Zhao, S. A TOF Localization Algorithm Based on
Improved Double‐sided Two Way Ranging. DEStech Trans. Comput. Sci. Eng. 2018,
25, 307‐315.

65
[40]. Welch and G. Bishop, An Introduction to the Kalman Filter, ACM SIC-CRAPH,
2001 Course Notes, 2001.

[41]. Y. Bar-Shalom and E. Tse, “Tracking in a cluttered environment with

probabilistic data association,” Automatica, vol. 11, no. 5, pp. 451–460, 1975.

[42]. Y. Bar-Shalom, “Update with out-of-sequence measurements in tracking: exact

solution,” IEEE Transactions on Aerospace and Electronic Systems, vol. 38, no. 3,
pp. 769–778, 2002.

[43]. Y. Chong, S. Mori, and K. C. Chang, “Distributed multitarget multisensor

tracking,” in Multitarget-Multisensor Tracking: Advanced Applications, vol. 1, pp.
247–295, 1990.

[44]. Y. Chong, S. Mori, and K. C. Chang, “Information lusion in distributed sensor

networks,” in Proceedings of the 4th American Control Conference, Boston, Mass,
USA, June 1985.

[45]. Zinober, Variable Structure and Lyapunov Control. London: Springer - Verlag,
1994.

66
Phụ lục chương trình Matlab:
1. Mô phỏng lọc bằng EKF cảm biến GPS giả lập:

clear all;
close all;
clc;

load('Quy_dao.mat');

dt= 10/1000; % 10 ms

vx_r(1)=0.1;
vy_r(1)=0;
w_r(1)=0;
v_r(1)=0.1;

Q11= 150/1000; % sai so x

Q22= 150/1000;% sai so y
Q33= pi/(360*2);% sai so goc

ex_z1=1/1000;
ey_z1=1/1000;
et_z1=pi/(360*2);

y=[x_GPS;y_GPS;the_z]; % gia tri do cam bien GPS

P=[ex_z1 0 0;
0 ey_z1 0;
0 0 et_z1];

R=[Q11 0 0;
0 Q22 0;
0 0 Q33];

C=[1 0 0;
0 1 0;
0 0 1];

I= [1 0 0;
0 1 0;
0 0 1];

for i=2:size(xr,2)

vx_r(i)=xr(i)-xr(i-1);
vy_r(i)=yr(i)-yr(i-1);
v_r(i)= sqrt(vx_r(i)+vy_r(i));
w_r(i)=ther(i)-ther(i-1);

67
end

X(:,1)=[xr(1); yr(1); ther(1)];

u(:,1)=[v_r(1);w_r(1) ];

u=[v_r;w_r];

d_the(1)=0;

N= size(xr,2);

for i=1:N-1

B=[dt*cos(X(3,i)+d_the(i)/2) 0;dt*sin(X(3,i)+d_the(i)/2) 0; 0 1];

A=[1 0 -v_r(i)*dt*sin(X(3,i)); 0 1 v_r(i)*dt*cos(X(3,i)); 0 0 1];

Q1=[Q11*dt+Q33*(dt^3/3)*(v_r(i)^2)*sin(X(3,i))^2;
-Q33*(dt^3/3)*(v_r(i)^2)*sin(X(3,i))*cos(X(3,i));
-Q33*(dt^2/2)*v_r(i)*sin(X(3,i))];

Q2=[-Q33*(dt^3/3)*(v_r(i)^2)*sin(X(3,i))*cos(X(3,i));
Q22*dt+Q33*(dt^3/3)*(v_r(i)^2)*cos(X(3,i))^2;
Q33*(dt^2/2)*v_r(i)*cos(X(3,i))];

Q3=[-Q33*(dt^2/2)*v_r(i)*sin(X(3,i));
Q33*(dt^2/2)*v_r(i)*cos(X(3,i));
Q33*dt];

Q=[Q1 Q2 Q3];

X_p(:,i+1)=X(:,i)+B*u(:,i);

P_p=A*P*A'+Q;

K= P_p*C/(C*P_p*C'+R);

X(:,i+1)= X_p(:,i+1)+K*(y(:,i+1)- C*X_p(:,i+1));

P=(I-K*C)*P_p;

d_the(i+1)=X(3,i+1)-X(3,i);
end

x_GPS_EKF = X(1,:);
y_GPS_EKF = X(2,:);

plot(xr,yr, x_GPS,y_GPS,x_odo,y_odo);
hold on

68
 plot (x_GPS_EKF,y_GPS_EKF,'LineWidth',1.5);

legend('True','GPS','odo','GPS(EKF)')

2. Mô phỏng fusion bằng EKF với odo và GPS giả lập:

clear all;
close all;
clc;

load('Quy_dao.mat');

dt= 10/1000; % 10 ms

vx_r(1)=0.1;
vy_r(1)=0;
w_r(1)=0;
v_r(1)=0.1;

Q11= 15/1000; % sai so x

Q22= 15/1000;% sai so y
Q33= pi/(360*2);% sai so goc

ex_z1=10/1000;
ey_z1=10/1000;
et_z1=pi/(360*2);

e_xGPS=100/1000;
e_xodo=20/1000;
e_yGPS=100/1000;
e_yodo=20/1000;
e_the=pi/(360*2);

% tin hieu cam bien:

y=[x_GPS;x_odo;y_GPS;y_odo;the_z];

P=[ex_z1 0 0;
0 ey_z1 0;
0 0 et_z1]; % ok

R=[e_xGPS 0 0 0 0;
0 e_xodo 0 0 0;
0 0 e_yGPS 0 0;
0 0 0 e_yodo 0;
0 0 0 0 e_the];

C=[1 1 0 0 0; 0 0 1 1 0; 0 0 0 0 1]';

I= [1 0 0;
0 1 0;
0 0 1];

69
for i=2:size(xr,2)

vx_r(i)=xr(i)-xr(i-1);
vy_r(i)=yr(i)-yr(i-1);
v_r(i)= sqrt(vx_r(i)^2+vy_r(i)^2);
w_r(i)=ther(i)-ther(i-1);

end

X(:,1)=[xr(1); yr(1); ther(1)];

u(:,1)=[v_r(1);w_r(1) ];

u=[v_r;w_r];

d_the(1)=0;

N= size(xr,2);

e_xodo_m(1) = e_xodo;
e_yodo_m(1) = e_yodo;

for i=1:N-1

if i>1
e_xodo_m(1,i) = e_xodo_m(i-1)+ e_xodo;
e_yodo_m(1,i) = e_yodo_m(i-1)+ e_yodo;
end

R=[e_xGPS 0 0 0 0;
0 e_xodo_m(1,i) 0 0 0;
0 0 e_yGPS 0 0;
0 0 0 e_yodo_m(1,i) 0;
0 0 0 0 e_the];

B=[dt*cos(X(3,i)+d_the(i)/2) 0;dt*sin(X(3,i)+d_the(i)/2) 0; 0 1];

A=[1 0 -v_r(i)*dt*sin(X(3,i)); 0 1 v_r(i)*dt*cos(X(3,i)); 0 0 1];

Q1=[Q11*dt+Q33*(dt^3/3)*(v_r(i)^2)*sin(X(3,i))^2;
-Q33*(dt^3/3)*(v_r(i)^2)*sin(X(3,i))*cos(X(3,i));
-Q33*(dt^2/2)*v_r(i)*sin(X(3,i))];

Q2=[-Q33*(dt^3/3)*(v_r(i)^2)*sin(X(3,i))*cos(X(3,i));
Q22*dt+Q33*(dt^3/3)*(v_r(i)^2)*cos(X(3,i))^2;
Q33*(dt^2/2)*v_r(i)*cos(X(3,i))];

Q3=[-Q33*(dt^2/2)*v_r(i)*sin(X(3,i));
Q33*(dt^2/2)*v_r(i)*cos(X(3,i));
Q33*dt];

70
 Q=[Q1 Q2 Q3];

X_p(:,i+1)=X(:,i)+B*u(:,i);

P_p=A*P*A'+Q;

K= P_p*C'/(C*P_p*C'+R);

X(:,i+1)= X_p(:,i+1)+K*(y(:,i+1)- C*X_p(:,i+1));

P=(I-K*C)*P_p;

d_the(i+1)=X(3,i+1)-X(3,i);
end

x_fusion_EKF = X(1,:);
y_fusion_EKF = X(2,:);

figure (1);

plot(xr,yr,x_GPS,y_GPS,x_odo,y_odo );
legend('True','GPS','Odo')

figure (2);
plot(xr,yr,x_fusion_EKF,y_fusion_EKF,x_odo,y_odo,x_GPS_EKF,y_GPS_EKF );
legend('True','(GPS+Odo)EKF','Odo','(GPS)EKF' )

3. Mô phỏng lọc bằng EKF cảm biến UWB thực tế:

clear all;
close all;
clc;

% Loc UWB bang EKF

load('du_lieu_UWB_Odo.mat')
xr=x_ref;
yr=y_ref;
ther=the_ref;

dt= 100/1000; % 100 ms

vx_r(1)=0.1;
vy_r(1)=0;
w_r(1)=0;
v_r(1)=0.1;

Q11= 150/1000; % sai so x

Q22= 150/1000;% sai so y
Q33= pi/(360*2);% sai so goc

ex_z1=1/1000;
ey_z1=1/1000;

71
et_z1=pi/(360*2);

y=[x_uwb_d1;y_uwb_d1;the_ref]; % ok

P=[ex_z1 0 0;
0 ey_z1 0;
0 0 et_z1];

R=[Q11 0 0;
0 Q22 0;
0 0 Q33];

C=[1 0 0;
0 1 0;
0 0 1];

I= [1 0 0;
0 1 0;
0 0 1];

% tinh v_r va w_r

for i=2:size(xr,2)
vx_r(i)=xr(i)-xr(i-1);
vy_r(i)=yr(i)-yr(i-1);
v_r(i)= sqrt(vx_r(i)+vy_r(i));
w_r(i)=ther(i)-ther(i-1);
end

X(:,1)=[xr(1); yr(1); ther(1)];

u(:,1)=[v_r(1);w_r(1) ];

u=[v_r;w_r];

d_the(1)=0;

N= size(xr,2);

for i=1:N-1

B=[dt*cos(X(3,i)+d_the(i)/2) 0;dt*sin(X(3,i)+d_the(i)/2) 0; 0 1];

A=[1 0 -v_r(i)*dt*sin(X(3,i)); 0 1 v_r(i)*dt*cos(X(3,i)); 0 0 1];

Q1=[Q11*dt+Q33*(dt^3/3)*(v_r(i)^2)*sin(X(3,i))^2;
-Q33*(dt^3/3)*(v_r(i)^2)*sin(X(3,i))*cos(X(3,i));
-Q33*(dt^2/2)*v_r(i)*sin(X(3,i))];

Q2=[-Q33*(dt^3/3)*(v_r(i)^2)*sin(X(3,i))*cos(X(3,i));
Q22*dt+Q33*(dt^3/3)*(v_r(i)^2)*cos(X(3,i))^2;
Q33*(dt^2/2)*v_r(i)*cos(X(3,i))];

Q3=[-Q33*(dt^2/2)*v_r(i)*sin(X(3,i));
Q33*(dt^2/2)*v_r(i)*cos(X(3,i));
Q33*dt];

72
 Q=[Q1 Q2 Q3];

X_p(:,i+1)=X(:,i)+B*u(:,i);

P_p=A*P*A'+Q;

K= P_p*C/(C*P_p*C'+R);

X(:,i+1)= X_p(:,i+1)+K*(y(:,i+1)- C*X_p(:,i+1));

P=(I-K*C)*P_p;

d_the(i+1)=X(3,i+1)-X(3,i);
end

x_UWB_EKF = X(1,:);
y_UWB_EKF = X(2,:);

plot(xr,yr, x_uwb_d1,y_uwb_d1);
hold on
plot (x_UWB_EKF,y_UWB_EKF,'LineWidth',1.2);
legend('True','UWB','(UWB)-EKF')

4. Mô phỏng fusion với tín hiệu cảm biến odo và UWB thực tế:

clear all;
close all;
clc;

% Fusion UWB va odo

load('du_lieu_UWB_Odo.mat')
xr=x_ref;
yr=y_ref;
ther=the_ref;

dt= 100/1000; % 100 ms

vx_r(1)=0.1;
vy_r(1)=0;
w_r(1)=0;
v_r(1)=0.1;

Q11= 15/1000;
Q22= 15/1000;
Q33= pi/(360*2);

ex_z1=10/1000;
ey_z1=10/1000;
et_z1=pi/(360*2);

e_xUWB=150/1000;
e_xodo=5/1000;

73
e_yUWB=150/1000;
e_yodo=5/1000;
e_the=pi/(360*2);

y=[x_uwb_d1;x_odo_d1;y_uwb_d1;y_odo_d1;the_ref];

P=[ex_z1 0 0;
0 ey_z1 0;
0 0 et_z1];

R=[e_xUWB 0 0 0 0;
0 e_xodo 0 0 0;
0 0 e_yUWB 0 0;
0 0 0 e_yodo 0;
0 0 0 0 e_the];

C=[1 1 0 0 0; 0 0 1 1 0; 0 0 0 0 1]';

I= [1 0 0;
0 1 0;
0 0 1];

% tinh v_r va w_r

for i=2:size(xr,2)

vx_r(i)=xr(i)-xr(i-1);
vy_r(i)=yr(i)-yr(i-1);
v_r(i)= sqrt(vx_r(i)^2+vy_r(i)^2);
w_r(i)=ther(i)-ther(i-1);

end

X(:,1)=[xr(1); yr(1); ther(1)];

u(:,1)=[v_r(1);w_r(1) ];

X_r=[xr;yr;ther];

u=[v_r;w_r];

d_the(1)=0;

N= size(xr,2);

e_xodo_m(1) = e_xodo;
e_yodo_m(1) = e_yodo;

for i=1:N-1

if i>1

74
 e_xodo_m(1,i) = e_xodo_m(i-1)+ e_xodo;
e_yodo_m(1,i) = e_yodo_m(i-1)+ e_yodo;
end

R=[e_xUWB 0 0 0 0;
0 e_xodo_m(1,i) 0 0 0;
0 0 e_yUWB 0 0;
0 0 0 e_yodo_m(1,i) 0;
0 0 0 0 e_the];

B=[dt*cos(X(3,i)+d_the(i)/2) 0;dt*sin(X(3,i)+d_the(i)/2) 0; 0 1];

A=[1 0 -v_r(i)*dt*sin(X(3,i)); 0 1 v_r(i)*dt*cos(X(3,i)); 0 0 1];

Q1=[Q11*dt+Q33*(dt^3/3)*(v_r(i)^2)*sin(X(3,i))^2;
-Q33*(dt^3/3)*(v_r(i)^2)*sin(X(3,i))*cos(X(3,i));
-Q33*(dt^2/2)*v_r(i)*sin(X(3,i))];

Q2=[-Q33*(dt^3/3)*(v_r(i)^2)*sin(X(3,i))*cos(X(3,i));
Q22*dt+Q33*(dt^3/3)*(v_r(i)^2)*cos(X(3,i))^2;
Q33*(dt^2/2)*v_r(i)*cos(X(3,i))];

Q3=[-Q33*(dt^2/2)*v_r(i)*sin(X(3,i));
Q33*(dt^2/2)*v_r(i)*cos(X(3,i));
Q33*dt];

Q=[Q1 Q2 Q3];

X_p(:,i+1)=X(:,i)+B*u(:,i);

P_p=A*P*A'+Q;

K= P_p*C'/(C*P_p*C'+R);

X(:,i+1)= X_p(:,i+1)+K*(y(:,i+1)- C*X_p(:,i+1));

P=(I-K*C)*P_p;

d_the(i+1)=X(3,i+1)-X(3,i);
end

x_EKF_UWB_Odo = X(1,:);
y_EKF_UWB_Odo = X(2,:);

plot(xr,yr,x_odo_d1,y_odo_d1,x_uwb_d1,y_uwb_d1,x_EKF_UWB_Odo,y_EKF_UWB_
Odo);
legend('True','Odo','UWB','(Odo+UWB)-EKF')

5. Mô phỏng điều khiển robot bám quỹ đạo khi fusion với tín hiệu cảm biến odo và
GPS giả lập tần số 100ms:
75
close all
clear all

load('Quy_dao_100ms.mat');

%system dynamics
hagv=0.230; %width of agv, m
wagv=0.350; %length of agv, m
b=0.3328; %wheel distance, m
r=0.0975; %wheel radius, m
d=0.0; %agv center to agv's axe, m
bl=0.000; %line width

%controler parameters
k1=15; k2=120; k3=20;

%sampling time and data time

dt=0.1;
t(1)=0;
n=length(xr);

%for initial values

% tracking point
xc(1)=xr(1)-0.010;
yc(1)=yr(1)-0.010;
phc(1)=phr(1)+10*pi/180;

% AGV center
ph(1)=phc(1);
w(1)=0;
v(1)=0;
x(1)=xc(1)-d*cos(ph(1));
y(1)=yc(1)-d*sin(ph(1));

%=== for error of tracking ===

E0=[cos(ph(1)) sin(ph(1)) 0; -sin(ph(1)) cos(ph(1)) 0; 0 0 1]*[xr(1)-
xc(1); yr(1)-yc(1); phr(1)-phc(1)];
e1(1)=E0(1);
e2(1)=E0(2);
e3(1)=E0(3);

%=== for wheel dynamics ===

vwl(1)=0;
vwr(1)=0;

e_xGPS=150/1000;
e_xodo=3/1000;
e_yGPS=150/1000;
e_yodo=3/1000;
e_the=pi/(360*2);

76
x_GPS(1)=x(1)+rand*e_xGPS;
y_GPS(1)=y(1)+rand*e_yGPS;
x_odo(1)=x(1);
y_odo(1)=x(1);

Q11= 150/1000;
Q22= 150/1000;
Q33= pi/(360*2);

e_xodo_m(1) = e_xodo;
e_yodo_m(1) = e_yodo;

ex_z1=150/1000;
ey_z1=150/1000;
et_z1=pi/(360*2);

P_k=[ex_z1 0 0;
0 ey_z1 0;
0 0 et_z1];

C_k=[1 1 0 0 0; 0 0 1 1 0; 0 0 0 0 1]';

for i=2:n

t(i)=(i-1)*dt;

%error dynamics
E=([-1;0;0]*v(i-1) + [e2(i-1);-d-e1(i-1);-1]*w(i-1) + [vr*cos(e3(i-
1)); vr*sin(e3(i-1)); wr(i-1)])*dt+E0;
e1(i)=E(1);
e2(i)=E(2);
e2(i)=e2(i);
e3(i)=E(3);
E0=E;

%controller dynamics
w(i)=wr(i)+k2*vr*e2(i)+k3*sin(e3(i));
v(i)=vr*cos(e3(i))+k1*e1(i);

%tracking point dynamics

ph(i)=phr(i)-e3(i);
Ae=[cos(ph(i)) sin(ph(i)) 0; -sin(ph(i)) cos(ph(i)) 0; 0 0 1];
er=inv(Ae)*E;
xc(i)=xr(i)-er(1);
yc(i)=yr(i)-er(2);
phc(i)=phr(i)-er(3);

phc_dot(i)=phc(i)-phc(i-1);

%wheel dynamics [rpm]

vwl(i)=(1/r)*(v(i)+b*w(i))*(60/(2*pi));
vwr(i)=(1/r)*(v(i)-b*w(i))*(60/(2*pi));

77
%robot center
x(i)=xc(i)-d*cos(phc(i));
y(i)=yc(i)-d*sin(phc(i));

%%%%%%%%%%% gia lap tin hieu cam bien %%%%%%%%%%%%%%

q_t(:,i)=[ x(i);y(i);phc(i)];

% phuong trinh 61
x_GPS(i)=x(i)+rand*e_xGPS;
y_GPS(i)=y(i)+rand*e_yGPS;

q_GPS(:,i)=[x_GPS(i);y_GPS(i);phc(i)];

% phuong trinh 62
x_odo(i)=x_odo(i-1)+x(i)-x(i-1)+rand*e_xodo;
y_odo(i)=y_odo(i-1)+y(i)-y(i-1)+rand*e_yodo;

q_odo(:,i)=[x_odo(i);y_odo(i);phc(i)];

d_the(i)=phc_dot(i);

%%%%%%%%%%% uoc tinh trang thai %%%%%%%%%%%%%%

z(:,i)=[v(i);w(i)];

u_k=z;
X=q_t;

[A_k, B_k, Q_k] = ham_tinh_he_so(dt,d_the,u_k, X, Q11,Q22,Q33,i);

mea(:,i)=[x_GPS(i);x_odo(i);y_GPS(i);y_odo(i);phc(i)];

if i>1
e_xodo_m(1,i) = e_xodo_m(i-1)+ e_xodo;
e_yodo_m(1,i) = e_yodo_m(i-1)+ e_yodo;
end

R=[e_xGPS 0 0 0 0;
0 e_xodo_m(1,i) 0 0 0;
0 0 e_yGPS 0 0;
0 0 0 e_yodo_m(1,i) 0;
0 0 0 0 e_the];

[q_kalman, P_k]=kalman_day_du(mea, X,u_k,P_k,A_k,B_k,C_k,Q_k,R,i);

x(i)=q_kalman(1,i);
y(i)=q_kalman(2,i);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

78
end

Ref=[xr;yr;phr];

Robot=[x;y;phc];

error=Robot-Ref;

plot(t,error(1,:),t,error(2,:),'-.',t,error(3,:),'--');
xlabel('Time (s)');
ylabel('Tracking Error (m,rad)');
legend('e1','e2','e3');

figure;
plot(x,y,'lineWidth',1.2);
xlabel('X Coordinate (m)');
ylabel('Y Coordinate (m)');
axis equal; hold on;
plot(xr,yr);
legend('Robot','Ref');

figure
plot(xr,yr,x_odo, y_odo,x_GPS, y_GPS );
xlabel('X Coordinate (m)');
ylabel('Y Coordinate (m)');
legend('Ref', 'odo','GPS');

6. Mô phỏng điều khiển robot bám quỹ đạo khi fusion với tín hiệu cảm biến odo và
GPS giả lập tần số 1ms:

close all
clear all

load('Quy_dao_1ms.mat');

%system dynamics
hagv=0.230; %width of agv, m
wagv=0.350; %length of agv, m
b=0.3328; %wheel distance, m
r=0.0975; %wheel radius, m
d=0.0; %agv center to agv's axe, m
bl=0.000; %line width

%controler parameters
k1=15; k2=12; k3=200;

%sampling time and data time

dt=0.001;
t(1)=0;
n=length(xr);

79
%for initial values
% tracking point
xc(1)=xr(1)-0.010;
yc(1)=yr(1)-0.010;
phc(1)=phr(1)+10*pi/180;

% AGV center
ph(1)=phc(1);
w(1)=0;
v(1)=0;
x(1)=xc(1)-d*cos(ph(1));
y(1)=yc(1)-d*sin(ph(1));

%=== for error of tracking ===

E0=[cos(ph(1)) sin(ph(1)) 0; -sin(ph(1)) cos(ph(1)) 0; 0 0 1]*[xr(1)-
xc(1); yr(1)-yc(1); phr(1)-phc(1)];
e1(1)=E0(1);
e2(1)=E0(2);
e3(1)=E0(3);

%=== for wheel dynamics ===

vwl(1)=0;
vwr(1)=0;

e_xGPS=150/1000;
e_xodo=0.1/1000;
e_yGPS=150/1000;
e_yodo=0.1/1000;
e_the=pi/(360*2);

x_GPS(1)=x(1)+rand*e_xGPS;
y_GPS(1)=y(1)+rand*e_yGPS;
x_odo(1)=x(1);
y_odo(1)=x(1);

Q11= 15/1000;
Q22= 15/1000;
Q33= pi/(360*2);

e_xodo_m(1) = e_xodo;
e_yodo_m(1) = e_yodo;

ex_z1=15/1000;
ey_z1=15/1000;
et_z1=pi/(360*2);

P_k=[ex_z1 0 0;
0 ey_z1 0;
0 0 et_z1];

C_k=[1 1 0 0 0; 0 0 1 1 0; 0 0 0 0 1]';

80
for i=2:n

t(i)=(i-1)*dt;

%error dynamics
E=([-1;0;0]*v(i-1) + [e2(i-1);-d-e1(i-1);-1]*w(i-1) + [vr*cos(e3(i-
1)); vr*sin(e3(i-1)); wr(i-1)])*dt+E0;
e1(i)=E(1);
e2(i)=E(2);
e2(i)=e2(i);
e3(i)=E(3);
E0=E;

%controller dynamics
w(i)=wr(i)+k2*vr*e2(i)+k3*sin(e3(i));
v(i)=vr*cos(e3(i))+k1*e1(i);

%tracking point dynamics

ph(i)=phr(i)-e3(i);
Ae=[cos(ph(i)) sin(ph(i)) 0; -sin(ph(i)) cos(ph(i)) 0; 0 0
1];
er=inv(Ae)*E;
xc(i)=xr(i)-er(1);
yc(i)=yr(i)-er(2);
phc(i)=phr(i)-er(3);

phc_dot(i)=phc(i)-phc(i-1);

%wheel dynamics [rpm]

vwl(i)=(1/r)*(v(i)+b*w(i))*(60/(2*pi));
vwr(i)=(1/r)*(v(i)-b*w(i))*(60/(2*pi));

%robot center
x(i)=xc(i)-d*cos(phc(i));
y(i)=yc(i)-d*sin(phc(i));

%%%%%%%%%%% gia lap tin hieu cam bien %%%%%%%%%%%%%%

q_t(:,i)=[ x(i);y(i);phc(i)];

x_GPS(i)=x(i)+rand*e_xGPS;
y_GPS(i)=y(i)+rand*e_yGPS;

q_GPS(:,i)=[x_GPS(i);y_GPS(i);phc(i)];

x_odo(i)=x_odo(i-1)+x(i)-x(i-1)+rand*e_xodo;
y_odo(i)=y_odo(i-1)+y(i)-y(i-1)+rand*e_yodo;

q_odo(:,i)=[x_odo(i);y_odo(i);phc(i)];

d_the(i)=phc_dot(i);

%%%%%%%%%%% uoc tinh trang thai %%%%%%%%%%%%%%

81
 z(:,i)=[v(i);w(i)];

u_k=z;
X=q_t;

[A_k, B_k, Q_k] = ham_tinh_he_so(dt,d_the,u_k, X, Q11,Q22,Q33,i);

mea(:,i)=[x_GPS(i);x_odo(i);y_GPS(i);y_odo(i);phc(i)];

if i>1
e_xodo_m(1,i) = e_xodo_m(i-1)+ e_xodo;
e_yodo_m(1,i) = e_yodo_m(i-1)+ e_yodo;
end

R=[e_xGPS 0 0 0 0;
0 e_xodo_m(1,i) 0 0 0;
0 0 e_yGPS 0 0;
0 0 0 e_yodo_m(1,i) 0;
0 0 0 0 e_the];

[q_kalman, P_k]=kalman_day_du(mea, X,u_k,P_k,A_k,B_k,C_k,Q_k,R,i);

x(i)=q_kalman(1,i);
y(i)=q_kalman(2,i);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

end

Ref=[xr;yr;phr];

Robot=[x;y;phc];

error=Robot-Ref;

plot(t,error(1,:),t,error(2,:),'-.',t,error(3,:),'--');
xlabel('Time (s)');
ylabel('Tracking Error (m,rad)');
legend('e1','e2','e3');

figure;
plot(x,y,'lineWidth',1.2);
xlabel('X Coordinate (m)');
ylabel('Y Coordinate (m)')
axis equal; hold on;
plot(xr,yr);
legend('Robot','Ref');

figure

82
plot(xr,yr,x_odo, y_odo,x_GPS, y_GPS );
xlabel('X Coordinate (m)');
ylabel('Y Coordinate (m)')
legend('Ref', 'odo','GPS');

83