xây dựng bộ điều khiển dự báo bám quỹ đạo cho xe tự hành

Các bộ điều khiển như dự báo mô hình Model Predictive Control, Stanley có vai trò điều hướng cho xe bám vào quỹ đạo bằng việc thay đổi góc bẻ lái steering, vì thế bộ điều khiển vòng kín

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

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

-

ĐINH QUANG VŨ

XÂY DỰNG BỘ ĐIỀU KHIỂN DỰ BÁO BÁM QUỸ ĐẠO

CHO XE TỰ HÀNH TRAJECTORY TRACKING FOR AUTONOMOUS VEHICLE

BASED ON MODEL PREDICTIVE CONTROL

CÔNG TRÌNH ĐƯỢC HOÀN THÀNH TẠI TRƯỜNG ĐẠI HỌC BÁCH KHOA –ĐHQG -HCM Cán bộ hướng dẫn khoa học : TS Nguyễn Vĩnh Hảo

Cán bộ chấm nhận xét 1 : TS Nguyễn Hoàng Giáp

Cán bộ chấm nhận xét 2 : PGS TS Nguyễn Ngọc Sơn

Luận văn thạc sĩ được bảo vệ tại Trường Đại học Bách Khoa, ĐHQG Tp HCM ngày 20 tháng 01 năm 2024

Thành phần Hội đồng đánh giá luận văn thạc sĩ gồm:

(Ghi rõ họ, tên, học hàm, học vị của Hội đồng chấm bảo vệ luận văn thạc sĩ)

1 Chủ tịch : PGS.TS Nguyễn Tấn Lũy

2 Phản biện 1: TS Nguyễn Hoàng Giáp

3 Phản biện 2: PGS TS Nguyễn Ngọc Sơn

4 Thư kí : TS Trần Ngọc Huy

5 Ủy viên : TS Trần Đức Thiện

Xác nhận của Chủ tịch Hội đồng đánh giá LV và Trưởng Khoa quản lý chuyên ngành sau khi luận văn đã được sửa chữa (nếu có)

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

TRƯỜNG ĐẠI HỌC BÁCH KHOA CỘNG HÒA XÃ HỘI CHỦ NGHĨA VIỆT NAM Độc lập - Tự do - Hạnh phúc

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

Họ tên học viên: Đinh Quang Vũ MSHV: 2070630 Ngày, tháng, năm sinh: 13/05/1998 Nơi sinh: Kiên Giang Chuyên ngành: Kỹ Thuật Điều Khiển và Tự Động Hóa Mã số : 8520216

I TÊN ĐỀ TÀI: Xây dựng bộ điều khiển dự báo bám quỹ đạo cho xe tự hành

(Trajectory tracking for autonomous vehicle based on model predictive control)

II NHIỆM VỤ VÀ NỘI DUNG: Xây dựng mô hình đối tượng và áp dụng giải thuật

điều khiển bám quỹ đạo cho xe tự hành Nội dung của luận văn xoay quanh các vấn đề:

• Nghiên cứu và hệ thống lại mô hình chuẩn cho xe tự hành bằng Matlab Simulink

• Nghiên cứu, xây dựng mô hình và bộ điều khiển dự báo bám quỹ đạo cho xe tự hành

• Xây dựng hệ thống phần mềm, phần cứng hoàn chỉnh để thực nghiệm giải thuật

• Nghiên cứu, xây dựng bộ điều khiển truyền thống như: Stanley để so sánh giải thuật

• Xây dựng các giải thuật điều khiển vòng hở, điều khiển vòng kín bám theo giá trị đặt tùy vào bài toán điều khiển bao gồm PID

• Khảo sát thực nghiệm và đánh giá

III NGÀY GIAO NHIỆM VỤ : 06/02/2023

IV NGÀY HOÀN THÀNH NHIỆM VỤ: 28/12/2023

V CÁN BỘ HƯỚNG DẪN: Tiến sĩ – Nguyễn Vĩnh Hảo

Lời cảm ơn

Lời đầu tiên tôi xin cảm ơn tất cả các quý thầy cô đã dạy và hướng dẫn tôi trong suốt quá trình theo học Thạc sĩ Quá trình học tập tại trường Đại học Bách Khoa TP Hồ Chí Minh đã cung cấp cho tôi rất nhiều kiến thức bổ ích quý giá để tạo thành nền tảng vững chắc cho tôi trong sự nghiệp sau này

Luận văn này là cơ hội cho tôi tổng hợp lại tất cả những kiến thức của mình đã học được và khả năng nghiên cứu giải quyết vấn đề tôi đã đạt được để giải quyết một vấn đề lớn Tôi xin chân thành cảm ơn thầy Nguyễn Vĩnh Hảo đã tận tình hướng dẫn Thầy đã rất nhiệt tình giúp đỡ tôi trong việc định hướng và giải quyết các vấn đề khó khăn gặp phải, bên đó cũng xin cảm ơn sự giúp đỡ của những người bạn đã cùng nhau sát cánh và trưởng thành hơn trong môi trường học tập và nghiên cứu tại Trường Đại học Bách Khoa Tôi cũng xin dành lời cảm ơn này cho ba mẹ, các anh chị và những người thân luôn ở bên, hỗ trợ tôi trong cuộc sống và quá trình học tập và nghiên cứu tại trường

Tp Hồ Chí Minh, tháng 01 năm 2024

Đinh Quang Vũ

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

Luận văn tập trung vào việc tìm hiểu và nghiên cứu bộ điều khiển bám quỹ đạo cho xe

tự hành bằng phương pháp điều khiển dự báo mô hình (Model Predictive Control) Bước đầu thực hiện mô phỏng trên Matlab để kiểm tra tính khả thi, chất lượng bộ điều khiển Từ

cơ sở mô phỏng, xây dựng lên mô hình đối tượng và bộ điều khiển dự báo mô hình (Model Predictive Control) để điều khiển xe tự hành bám quỹ đạo cho trước Bên cạnh đó, bộ điều khiển truyền thống như Stanley cũng được thực nghiệm để đánh giá tính vượt trội của bộ điều khiển dự báo mô hình (Model Predictive Control) Tiếp theo, một hệ thống phần mềm hoàn chỉnh (GUI điều khiển, App điều khiển, cũng như hệ thống điều khiển) được xây dựng giúp cho việc dễ dàng thực các chức năng và ứng dụng vào thực tế sau này Các bộ điều khiển như dự báo mô hình (Model Predictive Control), Stanley có vai trò điều hướng cho

xe bám vào quỹ đạo bằng việc thay đổi góc bẻ lái (steering), vì thế bộ điều khiển vòng kín PID đóng vai trò điều khiển hệ thống bẻ lái của xe bám theo giá trị đặt

Mục tiêu của đề tài là ứng dụng vào các xe, máy kéo trong nông nghiệp hiện đại để thay thế vai trò của con người Do kinh phí của đề tài bị giới hạn, vì thế việc kiểm chứng giải thuật sẽ không thể thực hiện trên mô hình xe, máy kéo thực tế như đề cập ở trên Thay vào đó, 1 mô hình xe 4 bánh vừa và nhỏ sẽ được xây dựng và chạy thực nghiệm trong khuôn viên trường kết hợp với các cảm biến IMU, GPS có độ chính xác cao

ABSTRACT

The thesis focuses on understanding and researching trajectory tracking controllers for autonomous vehicles using the Model Predictive Control method Initially perform simulation on Matlab to check the feasibility and quality of the controller Building upon the simulation foundation, an Autonomous Vehicle Model and Model Predictive Controller are developed to control the autonomous vehicle along a predetermined trajectory Additionally, traditional controllers such as Stanley are tested to evaluate the superiority of the Model Predictive Control Next, a complete software system (GUI, Appplication, Source code) is built to make it easy to implement algorithm and evaluation in pratical environments Controllers such as Model Predictive Control and Stanley have only the role

of path tracking for Autonomous Vehicle by proposing the steering angle, so the PID loop controller plays the role of controlling steering's motor according to the set value The goal of the thesis is to apply vehicles and tractors in modern agriculture to replace the role of humans Due to limited funding for the thesis, algorithm validation will not be possible on actual vehicle and tractor models as mentioned above Instead, a small and medium-sized 4-wheeled vehicle model will be built and experimentally run on campus using with high-precision IMU and GPS sensors

closed-LỜI CAM ĐOAN

Tôi xin cam đoan luận văn này do tôi thực hiện, hoàn toàn không sao chép số liệu hay

kết quả của người khác Ngoài ra, trong bài báo cáo có sử dụng một số nguồn tài liệu tham

khảo đã được trích dẫn nguồn và chú thích rõ ràng Tôi xin hoàn toàn chịu trách nhiệm trước

bộ môn, khoa và nhà trường về sự cam đoan này

Tp Hồ Chí Minh, tháng 01 năm 2024

Đinh Quang Vũ

MỤC LỤC

MỞ ĐẦU 1

1.1 Mục tiêu nghiên cứu 1

1.2 Tổng quan đề tài 3

MÔ HÌNH VÀ HỆ THỐNG ĐIỀU KHIỂN 4

2.1 Xây dựng mô hình xe tự hành 4

2.1.1 Hệ thống phần cứng 5

2.1.2 Các thiết bị sử dụng trong mô hình 6

2.1.2.1 STM32F4 6

2.1.2.2 GPS 6

2.1.2.3 IMU 9

2.2 Hệ thống điều khiển 10

2.2.1 Hệ điều hành cho robot 10

2.2.1.1 Tổng quan về hệ điều hành cho robot: 10

2.2.1.2 Cấu trúc ROS: 10

2.2.2 Hệ thống phần mềm 12

2.2.2.1 Phần mềm nhúng điều khiển trên bo mạch 12

2.2.2.2 Phần mềm trạm giám sát mặt đất 14

XÂY DỰNG BỘ ĐIỀU KHIỂN DỰ BÁO MPC 16

3.1 Thiết kế bộ điều khiển MPC 16

3.1.1 Xây dựng phương trình động học 16

3.1.2 Xây dựng bộ điều khiển 17

3.1.3 Bộ điều khiển dự báo 17

3.1.4 Bộ điều khiển dự báo mô hình bám quỹ đạo với ràng buộc 19

KẾT QUẢ MÔ PHỎNG 24

4.1 Thiết lập chương trình mô phỏng thuật toán trên MATLAB 24

4.2 Khảo sát tham số của bộ điều khiển MPC 24

4.2.1 Khảo sát 1: quỹ đạo chuyển làn xe với Np 24

4.2.2 Khảo sát 2: quỹ đạo chuyển làn xe với Q3 28

4.2.3 Khảo sát 3: quỹ đạo chuyển làn xe với Q1, Q2 31

4.3 Mô phỏng bộ điều khiển MPC với bộ thông số tốt nhất 34

4.3.1 Trường hợp 1: quỹ đạo đường thẳng và đường tròn 34

4.3.2 Trường hợp 2: quỹ đạo số 8 36

4.4 Nhận xét và đánh giá 38

KẾT QUẢ THỰC NGHIỆM 39

5.1 Thực nghiệm bộ điều khiển PID hệ thống lái 40

5.1.1 Trường hợp 1: giá trị đặt hình sóng vuông 40

5.1.2 Trường hợp 2: giá trị đặt thay đổi theo thời gian 42

5.2 Thực nghiệm bộ điều khiển PID vận tốc 44

5.3 Khảo sát quỹ đạo chuyển làn xe 46

5.3.1 Khảo sát quỹ đạo chuyển làn xe tại 1m/s→2.2m/s 47

5.3.2 Nhận xét và đánh giá 49

5.4 Khảo sát quỹ đạo đường thẳng và tròn 50

5.4.1 Khảo sát quỹ đạo đường thẳng và tròn tại 1m/s→2.2m/s 51

5.4.2 Nhận xét và đánh giá 53

5.5 Khảo sát quỹ đạo thẳng kết hợp cut-off line 54

5.5.1 Khảo sát quỹ đạo đường thẳng và tròn tại 1m/s→2.2m/s 55

5.5.2 Nhận xét và đánh giá 57

5.6 Khảo sát quỹ đạo số 8 58

5.6.1 Khảo sát quỹ đạo số 8 tại 1m/s→2.2m/s 59

5.6.2 Nhận xét và đánh giá 61

KẾT LUẬN VÀ HƯỚNG PHÁT TRIỂN 62

6.1 Kết luận 62 6.2 Hướng phát triển 62 TÀI LIỆU THAM KHẢO 63

DANH MỤC HÌNH VẼ

Hình 1-1 Máy kéo Case IH Magnum và New Holland T8 NHDrive 2

Hình 1-2 Chiếc máy cày tự hành do công ty John Deere sản xuất [18] 2

Hình 2-1 Mô hình xe 4 bánh được sử dụng trong luận văn 4

Hình 2-2 Sơ đồ thiết kế phần cứng 5

Hình 2-3 Hệ thống phần cứng điều khiển 5

Hình 2-4 Chip STM32F407 6

Hình 2-5 Bộ cảm biến GPS-RTK 6

Hình 2-6 Sơ đồ mô hình hoạt động của GPS-RTK 7

Hình 2-7 Sơ đồ kết nối của GPS-RTK NEO-M8P 8

Hình 2-8 Bộ cảm biến IMU ADIS16480 9

Hình 2-9 Hệ điều hành ROS (Robot Operating System) 10

Hình 2-10 Cấu trúc ROS (Robot Operating System) 11

Hình 2-11 Sơ đồ giải thuật điều khiển xe tự hành 13

Hình 2-12 Nền tảng Qt 14

Hình 2-13 Giao diện điều khiển và hiển thị 15

Hình 3-1 Mô hình xe tự hành [7] 16

Hình 3-2 Sơ đồ hệ thống điều khiển dự báo mô hình 22

Hình 3-3 Miền dự báo tín hiệu ngõ ra 𝑦𝑘 và tín hiệu điều khiển 𝑢𝑘 [14] 23

Hình 3-4 Quỹ đạo dự báo mô hình và quỹ đạo tham chiếu [15] 23

Hình 4-1 Quỹ đạo chuyển làn của đối tượng và quỹ đạo tham chiếu của MPC với Np=5→20 25

Hình 4-2 Sai số bám quỹ đạo chuyển làn của MPC với Np=5→20 26

Hình 4-3 Góc bẻ lái (𝜙) quỹ đạo chuyển làn của MPC với Np=5→20 26

Hình 4-4 Quỹ đạo chuyển làn của đối tượng và quỹ đạo tham chiếu của MPC với 𝑄3=0→50 28

Hình 4-5 Sai số bám quỹ đạo chuyển làn của MPC với 𝑄3=0→50 29

Hình 4-6 Góc bẻ lái (𝜙) quỹ đạo chuyển làn của MPC với 𝑄3=0→50 29

Hình 4-7 Quỹ đạo chuyển làn của đối tượng và quỹ đạo tham chiếu của MPC với 𝑄1 = 𝑄2 = 0→50 31

Hình 4-8 Sai số bám quỹ đạo chuyển làn của MPC với 𝑄1 = 𝑄2 = 0→50 32

Hình 4-9 Góc bẻ lái (𝜙) quỹ đạo chuyển làn của MPC với 𝑄1 = 𝑄2 = 0→50 32 Hình 4-10 Quỹ đạo đường thẳng+vòng tròn của đối tượng và quỹ đạo tham chiếu 34

Hình 4-11 Sai số bám quỹ đạo đường thẳng+đường tròn 35

Hình 4-12 Góc xoay của mô hình (𝜃) và góc bẻ lái (𝜙) quỹ đạo đường thẳng+đường tròn 35

Hình 4-13 Quỹ đạo số 8 của đối tượng và quỹ đạo tham chiếu 36

Hình 4-14 Sai số bám quỹ đạo số 8 37

Hình 4-15 Góc xoay của mô hình (𝜃) và góc bẻ lái (𝜙) quỹ đạo số 8 37

Hình 5-1 Môi trường thực nghiệm 39

Hình 5-2 Mô hình xe tự hành trong môi trường thực nghiệm 40

Hình 5-3 Đáp ứng góc lái với giá trị đặt hình sóng vuông 40

Hình 5-4 Sai số giá trị đặt hình sóng vuông 41

Hình 5-5 Tín hiệu điều khiển với giá trị đặt hình sóng vuông 41

Hình 5-6 Đáp ứng góc lái với giá trị đặt thay đổi theo thời gian 42

Hình 5-7 Sai số giá trị đặt hình sóng vuông 42

Hình 5-8 Tín hiệu điều khiển với giá trị đặt hình sóng vuông 43

Hình 5-9 Đáp ứng vận tốc với các giá trị đặt vận tốc 44

Hình 5-10 Sai số vận tốc với các giá trị đặt vận tốc 45

Hình 5-11 Tín hiệu điều khiển vận tốc với các giá trị đặt vận tốc 45

Hình 5-12 Kết quả tạo quỹ đạo chuyển làn xe 46

Hình 5-13 Quỹ đạo chuyển làn xe của MPC và Stanley tại 1m/s→2.2m/s 47

Hình 5-14 Sai số bám quỹ đạo chuyển làn của MPC và Stanley tại 1m/s→2.2m/s 48

Hình 5-15 Góc bẻ lái (𝜙) quỹ đạo chuyển làn của MPC & Stanley tại 1.8m/s 48

Hình 5-16 Đồ thị sai số bám quỹ đạo chuyển làn của Stanley & MPC 49

Hình 5-17 Kết quả tạo quỹ đạo đường thẳng và tròn 50

Hình 5-18 Quỹ đạo đường thẳng & tròn của MPC và Stanley tại 1m/s→2.2m/s 51

Hình 5-19 Sai số bám quỹ đạo đường thẳng & tròn của MPC và Stanley tại

1m/s→2.2m/s 52

Hình 5-20 Góc bẻ lái (𝜙) quỹ đạo đường thẳng & tròn của MPC & Stanley tại 1.8m/s 52

Hình 5-21 Đồ thị sai số bám quỹ đạo đường thẳng & tròn của Stanley & MPC 53 Hình 5-22 Quỹ đạo trước và sau khi cut-off 54

Hình 5-23 Quỹ đạo cut-off line của MPC và Stanley tại 1m/s→2.2m/s 55

Hình 5-24 Sai số bám quỹ đạo cut-off line của MPC và Stanley tại 1m/s→2.2m/s 56

Hình 5-25 Góc bẻ lái (𝜙) quỹ đạo cut-off line của Stanley tại 1m/s→2.2m/s 56

Hình 5-26 Đồ thị sai số bám quỹ đạo cut-off line của Stanley & MPC 57

Hình 5-27 Kết quả tạo quỹ đạo số 8 58

Hình 5-28 Quỹ đạo số 8 của MPC tại 1m/s→2.2m/s 59

Hình 5-29 Sai số bám quỹ đạo số 8 của MPC tại 1m/s→2.2m/s 60

Hình 5-30 Góc bẻ lái (𝜙) quỹ đạo số 8 của MPC tại 1m/s→2.2m/s 60

Hình 5-31 Đồ thị sai số bám quỹ đạo số 8 của MPC 61

DANH MỤC BẢNG

Bảng 2-1 Thông số mô hình xe tự hành 4

Bảng 3-1 Tham số của mô hình 16

Bảng 5-1 Sai số hiệu dụng [RMS] đạo chuyển làn của Stanley & MPC 49

Bảng 5-2 Sai số hiệu dụng bám quỹ đạo thẳng & tròn của Stanley & MPC 53

Bảng 5-3 Sai số hiệu dụng bám quỹ đạo cut-off của Stanley & MPC 57

Bảng 5-4 Sai số hiệu dụng bám quỹ số 8 của MPC 61

DANH MỤC TỪ VIẾT TẮT

8 UART Universal asynchronous receiver-transmitter

MỞ ĐẦU

1.1 Mục tiêu nghiên cứu

Với sự phát triển nhanh chóng của nền kinh tế xã hội và ngành công nghiệp ô tô, mức độ phổ biến của ô tô ngày càng cao, kéo theo đó là vấn đề tai nạn giao thông thường xuyên Theo số liệu của tổ chức y tế thế giới, mỗi năm có khoảng 1,24 triệu người chết

vì tai nạn giao thông đường bộ [1] Trong dữ liệu liên quan do Tổ chức Y tế Thế giới công bố, tai nạn giao thông đường bộ nằm trong số 10 nguyên nhân gây tử vong hàng đầu trên toàn thế giới [2] Với tư cách là người điều khiển phương tiện, con người là yếu

tố dễ gây ra hầu hết các vụ tai nạn giao thông Công nghệ an toàn chủ động của phương tiện hiện có dựa trên quyết định của người lái để hỗ trợ hoạt động điều khiển ổn định và không thể loại bỏ cơ bản khả năng xảy ra tai nạn giao thông

Bên cạnh đó, bối cảnh đất nước ta có truyến thống nông nghiệp lâu năm, chiếm tỉ trọng cao trong nền kinh tế Chủ trương của nhà nước là phát triển nông nghiệp kết hợp với khoa học công nghệ giúp cải thiện năng suất và thu nhập cho người nông dân Các nước phát triển trên thế giới có số lượng dân cư là nông dân rất thấp, nhưng sản lượng lại rất lớn Cụ thể nền nông nghiệp Mỹ, lao động làm nông nghiệp, lâm nghiệp, thủy sản chỉ chiếm có 0,7% trong tổng số 153,9 triệu lao động trên toàn nước Mỹ, vậy mà nông nghiệp Mỹ lại là một mô hình công nghiệp hóa, hiện đại hóa rất điển hình [9] Để đạt được như vậy thì họ đã phải áp dụng phương tiện và kỹ thuật hiện đại trong sản xuất nông nghiệp Các công ty lớn trên thế giới cũng đã phát triển nhiều loại máy kéo đa năng, kết hợp với chế độ tự động giúp nông dân không phải tốn sức ra ngoài đồng và có thể điều khiển từ nhà bằng việc thiết lập lộ trình sẵn có cho máy kéo

Hình 1-1 Máy kéo Case IH Magnum và New Holland T8 NHDrive

của hãng CNH Industrial [17]

Hình 1-2 Chiếc máy cày tự hành do công ty John Deere sản xuất [18]

Có thể thấy, các mô hình xe, máy kéo tự hành đóng vai trò hết sức quan trọng trong đời sống sản xuất của con người Kiểm soát theo dõi đường dẫn là công nghệ chính của

xe tự hành, vì vậy đề tài sẽ tập trung vào bộ điều khiển bám quỹ đạo mong muốn cho trước của xe tự hành

Do kinh phí của đề tài bị giới hạn, vì thế việc kiểm chứng giải thuật sẽ không thể thực hiện trên mô hình xe, máy kéo thực tế như đề cập ở trên Thay vào đó, 1 mô hình

xe 4 bánh vừa và nhỏ sẽ được xây dựng và chạy thực nghiệm trong khuôn viên trường kết hợp với các cảm biến IMU, GPS có độ chính xác cao

1.2 Tổng quan đề tài

Điều khiển bám quỹ đạo là một phần quan trọng của xe tự hành, việc kiểm soát theo dõi đường đi của xe tự hành dựa trên sự kiểm soát của hệ thống lái của phương tiện [5] Điều khiển xe tự hành bao gồm việc điều khiển chiều ngang và chiều dọc, đây là hai yếu

tố cốt lõi trong việc nghiên cứu xe tự hành Điều khiển chiều ngang hướng tới điều khiển đối tượng bám vào quỹ đạo và hướng mong muốn, trong khi điều khiển chiều dọc đảm bảo đạt được tốc độ di chuyển long muốn của phương tiện [4] Để giải quyết vấn đề này, hàng loạt các bộ điều khiển đã được phát triển dựa vào lý thuyết điều khiển cổ điển, hiện đại, nâng cao như PID, điều khiển tối ưu, điều khiển cuốn chiếu, điều khiển trượt,… [4] Điều khiển dự báo mô hình là việc kết hợp dự báo mô hình, tối ưu hóa, hiệu chỉnh phản hồi đã giải quyết được những giới hạn của một số bộ điều khiển nhờ vào việc xem xét ràng buộc đầu vào và trạng thái cho phép của mô hình [4] Nghiên cứu gần đây cho thấy giải thuật MPC rất hữu ích để điều khiển động lực học của nhiều phương tiện khi xem xét các ràng buộc về an toàn và tính ổn định của các thuật toán này cũng đã được nghiên cứu kỹ lưỡng [4] Sơ đồ điều khiển bám quỹ đạo xem xét độ lệch và ổn định ngang để tránh chướng ngại vật đã được đề xuất trong [4] bằng cách sử dụng hệ thống lái và phanh kết hợp, trong đó mô hình xe mười bậc và mô hình xe đạp đơn giản được

sử dụng cho điều khiển dự báo mô hình phi tuyến tính (NMPC)

Trong đề tài này, bộ điều khiển dự báo (MPC) sẽ được sử dụng để điều khiển bám quỹ đạo cho mô hình xe tự hành bằng việc tuyến tính hóa mô hình tại điểm tham chiếu

ở mỗi chu kì điều khiển Kiểm chứng giải thuật sẽ được thực hiện trên mô hình và thử nghiệm trên một số quỹ đạo khác nhau cho trước

MÔ HÌNH VÀ HỆ THỐNG ĐIỀU KHIỂN

2.1 Xây dựng mô hình xe tự hành

Mô hình xe tự hành sẽ được xây dựng dựa trên ý tưởng mô hình xe 4 bánh Phía trước là hệ thống bẻ lái phía được điều khiển bởi 1 động cơ gắn encoder, hệ thống lái có vai trò điều hướng xe trong quá trình di chuyển Phía sau bao gồm 2 bánh xe có vai trò điều tốc, mỗi bánh sau được trang bị 1 động cơ gắn encoder để dễ dàng điều khiển độc lập để giải quyết vấn đề sai lệch, không đồng bộ giữa 2 động cơ dễ dàng hơn

Wifi GPS

v left

L

v right

Steering angle

2.1.1 Hệ thống phần cứng

Hình 2-2 Sơ đồ thiết kế phần cứng

Hệ thống điều khiển bao gồm một bảng điều khiển chính sử dụng máy tính nhúng và

ba board phụ (PWM, cảm biến và Định vị) sử dụng ARM MCU Máy tính nhúng được kết nối với các cảm biến bao gồm đơn vị đo quán tính (IMU) và hệ thống định vị toàn cầu (GPS), sau đó sẽ thu thập dữ liệu cảm biến và truyền lên trạm giám sát mặt đất Board driver nhận lệnh từ MainBoard để điều khiển các motor Hệ thống USV có thể truy cập không dây: Wifi/RF để truyền/nhận lệnh giữa trạm mặt đất và máy tính nhúng

để đưa ra lệnh điều khiển

DC Power

IMU PC

STM-Mainboard

Hình 2-3 Hệ thống phần cứng điều khiển

2.1.2 Các thiết bị sử dụng trong mô hình

2.1.2.1 STM32F4

Hình 2-4 Chip STM32F407

Dòng ARM Cortex STM32F4 là một bộ xử lí thế hệ mới với một kiến trúc chuẩn cho nhu cầu đa dạng về công nghệ Dòng Cortex là một lõi xử lí hoàn thiện, dựa vào một chuẩn CPU và kiến trúc hệ thống chung

2.1.2.2 GPS

GPS u-blox NEO M8P RTK là một mô-đun GPS RTK nhỏ, nhẹ và tiết kiệm năng lượng, dựa trên M8P u-blox Sử dụng RTK, PX4 có thể nhận được vị trí của nó với độ chính xác tới mức cm, chính xác hơn nhiều so với GPS thông thường

Hình 2-5 Bộ cảm biến GPS-RTK

Thông số cho cảm biến GPS:

• Giao tiếp bằng UART, tốc độ baud rate 115200

• Thời gian cập nhật: 100ms (10 hz)

• Bản tin từ cảm biến: $GPRMC

Dữ liệu từ bản tin $GPRMC

$GPRMC,083559.00,A,4717.11437,N,00833.91522,E,0.004,77.52,091202,,,A,V*57

Trong đó:

• $GPRMC: mã bản tin

• 083559.00: thời gan UTC

• A: trạng thái nhận A: dữ liệu hợp lê, V: dữ liệu không hợp lệ

• 4717.11437: tọa độ vĩ độ (Lattitude), 2 số đầu dành cho độ, phần còn lại là phút

Trạm GPS (base station) có tọa độ cố định và được xác định bằng các phương pháp

đo đạc chính xác cao, thông thường được gọi là survey-in Base sẽ dựa vào các thuật toán để ước lượng vị trí chính xác của nó và mất một khoảng thời gian để hoàn thành giai đoạn này Trong quá trình hoạt động, hệ thống định vị toàn cầu GNSS (Global Navigation Satellite System) được dùng để tính toán sai số vị trí tại vị trí của Base bằng cách so sánh với vị trí chính xác của Base(đã được ước lượng bằng survey-in trước đó) Sau khi sai số vị trí được xác định thì sẽ được gửi và cập nhật liên tục đến Rover bằng giao tiếp không dây Rover sử dụng những hiệu chỉnh này để cải thiện vị trí được tính toán của nó từ GNSS và đạt được độ chính xác đến từng centimet Trong một hệ thống nhiều Rover có thể sử dụng chung 1 Base

Bộ GPS truyền thống thông thường có sai số lên tới 2-4 m Nhưng việc kết hợp phương pháp RTK lại có thể đạt độ chính xác đến từng cm Điều này rất có lợi trong các ứng dụng cần độ chính xác cao như xe tự hành, drone,…

Hình 2-7 Sơ đồ kết nối của GPS-RTK NEO-M8P

2.1.2.3 IMU

Cảm biến vận tốc góc và gia tốc: từ hãng Analog Devices, mã hiệu ADIS16480 Cảm

biến này có một số tính năng nổi trội so với các hãng khác như: tầm đo vận tốc góc lớn

(4500/s), tầm đo gia tốc 8g, tần số cập nhật lớn (2460Hz), độ phân giải cao (32bit), độ

trôi theo nhiệt độ thấp, độ trực giao của 3 trục lớn Các thông số chính của ADIS16480:

• Tầm đo vận tốc góc thiết lập: +/- 450°/sec

• Tầm đo gia tốc: -18g đến +18g

• Thời gian khởi động: 120ms

• Giao tiếp: SPI

• Ngõ ra chuyển vị góc tương đối

• Tích hợp cảm biến nhiệt độ

• Lập trình thay đổi cấu hình làm việc: hiệu chỉnh bias auto/manual, băng

• thông 50/330 Hz, tốc độ lấy mẫu 256/2048Hz, lọc FIR, digital I/O, alarm,

• ngõ ra DAC

• Nguồn điện: +4,75V đến +5,25V

• Gia tốc làm việc cho phép: <2000g

• Nhiệt độ làm việc: -40°C đến +105°

Vi xử lý trung tâm: từ hãng STMicrochip, mã hiệu STM32F405 STM32F405 hoạt

động ở xung nhịp 168MHz, có khả năng xử lý phép tính dấu chấm động chỉ trong 1 chu

kỳ máy, và khả năng giao tiếp ngoại vi mạnh Do đó, chip STM32F405 được sử dụng

để tận dụng sức mạnh tính toán trong việc xây dựng các bộ ước lượng góc quay phi

tuyến phức tạp

Hình 2-8 Bộ cảm biến IMU ADIS16480

2.2 Hệ thống điều khiển

2.2.1 Hệ điều hành cho robot

2.2.1.1 Tổng quan về hệ điều hành cho robot:

Robot Operating System (ROS) [10] là một phần mềm trung gian (middleware) được dùng rất rộng rãi trong lĩnh vực Robotics nhờ nhiều đặc điểm nổi bật Có rất nhiều ứng dụng được xây dựng từ framework này

Hình 2-9 Hệ điều hành ROS (Robot Operating System)

Mặc dù ROS không phải là một hệ điều hành thực thụ nhưng nó cung cấp các dịch

vụ quan trọng của một hệ điều hành như: trừu tượng hóa phần cứng, chuyển thông tin giữa các tiến trình, quản lý dữ liệu theo các gói Bản thân ROS cũng không phải là một

hệ điều hành thời gian thực nhưng ta vẫn có thể tích hợp nó với các chương trình thời gian thực khác hoặc kết nối với vi điều khiển một cách dễ dàng qua giao thức Rosserial ROS cung cấp khả năng hoạt động tốt giữa nhiều nền tảng khác nhau: tin nhắn (message) trong ROS giúp người dùng dữ liệu giữa các nền tảng phần cứng khác nhau với các ngôn ngữ khác nhau Từ những máy tính nhúng chạy ngôn ngữ C để tối ưu hiệu năng đến những máy chủ cấp cao với python, java ROS có tính module mạnh, mọi thứ được kết nối với nhau bởi một hệ thống tin nhắn phân tán, nếu một thành phần bất kì gặp sự cố thì toàn hệ thống của người dùng vẫn sẽ không bị ảnh hưởng mà gián đoạn, sụp đổ Với những hệ thống đa luồng lớn, việc đọc ghi thông tin hoặc sử dụng tài nguyên sẽ rất dễ bị rối, điều mà gần như khó tránh khỏi với bất kì ứng dụng Robot nào Có nhiều cách để giải quyết vấn đề này Tuy nhiên ROS giúp người dùng dễ dàng hơn với cơ chế quảng bá (publish) và đăng ký (subscribe) thông qua một chủ đề (topic)

2.2.1.2 Cấu trúc ROS:

Đúng như cái tên của mình, ROS mô phỏng theo một hệ điều hành với triết lý chia một chương trình thành nhiều thư mục, mỗi thư mục có một chức năng riêng

ROS tạo ra một mạng, nơi tất cả những tiến trình được kết nối với nhau Mỗi một node được chia đều có thể dễ dàng truy cập, giao tiếp với các node khác, nhìn thấy thông tin mà các node khác gửi trong mạng

Những khái niệm cơ bản nhất của ROS là node, Master, Parameter Server, messages, services, topics và bags Mỗi loại cung cấp dữ liệu cho mô hình theo một cách khác nhau:

Hình 2-10 Cấu trúc ROS (Robot Operating System)

• Node: là một tiến trình nơi những tác vụ tính toán, điều khiển được thực thi Một node khi được khởi tạo sẽ đăng ký một tên độc nhất trong hệ thống qua master Node được viết với các thư viện ROS như roscpp (cho ngôn ngữ C++), rospy (cho ngôn ngữ python) Sử dụng Application Programming Interface (API) của các bộ thư viện này, người dùng có thể dễ dàng giao tiếp giữa các node bằng nhiều cách khác nhau, phổ biến nhất là gửi messages qua các topics Điều này khiến cho hệ thống dễ dàng mở rộng, sửa lỗi và bảo trì dễ hơn

• Master: Master cho phép các node đăng ký tên và tra cứu tên của các node khác Nếu không có Master trong hệ thống, ta không thể thực hiện được các tác vụ của ROS như giao tiếp với các node, gửi nhận service, …

• Parameter server: cho phép dữ liệu được lưu trữ bởi các từ khóa trong một vị trí trung tâm Tất cả node đều có thể truy cập và chỉnh sửa parameter Thông qua những parameter này, người dùng có thể cấu hình, sửa đổi một node ngay cả khi nó đang thực thi mà không cần phải dừng nó lại Parameter là một phần của Master

• Messages: các node giao tiếp với nhau thông qua messages Một message là một cấu trúc dữ liệu bao gồm các trường được định nghĩa như integer, floating point, … Người dùng cũng có thể tự tạo riêng một message của riêng mình với cấu trúc chuẩn này

• Topics: khi một node gửi hoặc nhận message thông qua một topic và master có nhiệm vụ phân phát các message đã được gửi đó Khi một node gửi message qua một topic, ta gọi node đó đang publish một topic, tương tự, khi một node nhận một message thông qua topic, ta nói node đang subscribe topic đó

• Service: mô hình publish/subscribe thì rất linh hoạt trong việc giao tiếp nhưng đặc điểm là chỉ truyền được đa đối tượng và một chiều Nhưng đôi khi lại không thích hợp cho việc truyền theo dạng request/reply, như yêu cầu robot làm một việc nhất định

và đợi kết quả Do đó, việc truyền nhận theo dạng request/reply được dùng thông qua services Service được định nghĩa một cặp cấu trúc dữ liệu: một cho request và một cho reply Một node cung cấp một service thông qua một thuộc tính name, và một client sử dụng service bằng việc gửi một request message và đợi phản hồi

2.2.2 Hệ thống phần mềm

Cấu trúc phần mềm điều khiển của hệ thống xe tự hành bao gồm hai phần: phần mềm nhúng điều khiển được sử dụng trên hệ thống xe và phần mềm giám sát được lập trình cho trạm trung tâm mặt đất

2.2.2.1 Phần mềm nhúng điều khiển trên bo mạch

Mỗi khối được thiết kế cho các thiết bị và nhiệm vụ cụ thể Nhiệm vụ của xe tự hành được điều khiển từ trạm giám sát mặt đất, sau đó sẽ lựa chọn và thực hiện hoạt động của

nó theo các nhiệm vụ tách biệt như các chế độ điều khiển, thuật toán điều khiển, vv…

Hệ thống điều khiển được sử dụng để khởi tạo các thông số điều khiển ban đầu cho mỗi

bộ điều khiển khác nhau Các tham số này sẽ được gửi đến thuật toán điều khiển chính

để xử lý các nhiệm vụ điều khiển như điều khiển từ xa, điều khiển góc bẻ lái, Ngoài

ra, các trạng thái của xe, thiết bị và thông tin cảm biến sẽ được ghi lại và đồng thời gửi đến trạm giám sát mặt đất

Hình 2-11 Sơ đồ giải thuật điều khiển xe tự hành

Bộ xử lí của hệ thống bao gồm 2 phần chính: máy tính nhúng và vi điều khiển Máy tính nhúng được cài đặt ROS (Robot Operating System) [10] trên hệ điều hành Linux (Ubuntu) [11] Với nền tảng ROS, máy tính nhúng có nhiệm vụ đọc dữ liệu cảm biến: IMU, GPS thông qua truyền thông nối tiếp, đồng thời nhận dữ liệu từ trạm điều khiển như: waypoints, thông số bộ điều khiển cung cấp cho giải thuật điều khiển Giải thuật điều khiển MPC, Stanley cũng được tính toán tại đây và kết quả tính toán sẽ được gửi đến vi điều khiển bằng giao thức UART (Universal asynchronous receiver-transmitter)

Vi điều khiển sau khi nhận thông tin từ máy tính nhúng (set-steering, set-speed) sẽ thực thi bộ điều khiển PID cho steering và tốc độ của xe bằng việc nhận giá trị phản hồi

từ encoder tại góc bẻ lái và 2 bánh sau Bên cạnh đó, vi điều khiển cũng tiến hành đo giá trị điện áp, dòng điện kết hợp với thông tin encoder gửi lên máy tính nhúng và kết hợp thêm các thông tin cảm biến khác để gửi về trạm giám sát giúp cho việc quan sát liên tục quá trình vận hành của xe

2.2.2.2 Phần mềm trạm giám sát mặt đất

Vai trò của trạm mặt đất là thiết bị đầu cuối cho người dùng giám sát và điều khiển

xe thông qua các kênh truyền thông không dây WIFI hoặc RF Cung cấp một giao diện thuận tiện để theo dõi quá trình hoạt động của xe Giao diện điều khiển GUI (Graphical User Interface) được xây dựng trên nền tảng Qt [12] là một nền tảng phát triển đa chức năng cho các ứng dụng desktop, nhúng và thiết bị di động Hỗ trợ chạy trên các hệ điều hành Linux, OS X, Windows, VxWorks, QNX, Android, iOS, BlackBerry, Sailfish OS,… Đặc biệt, Qt hỗ trợ các gói thư viện dễ dàng tích hợp ROS, thuận tiện cho việc giao tiếp, trao đổi bằng message trong nền tảng ROS Qt sử dụng ngôn ngữ lập trình C++, vì thế tốc độ xử lí sẽ nhanh hơn với các gói dữ liệu nhiều và phức tạp so với nền tảng cùng chức năng khác như C#

Hình 2-12 Nền tảng Qt

Giao diện được xây dựng đầy đủ chức năng, cho phép chọn chế độ điều khiển như: Joystick, Auto-Steering, MPC, Stanley,… và các ô nhập thông số tương ứng cho bộ điều khiển Phần hiển thị bao gồm bản đồ, đồ thị steering, đồ thị vận tốc được cập nhật liên tục, do đó việc khảo sát, hiệu chỉnh trở nên dễ dàng và tiết kiệm thời gian hơn Ngoài ra, phần bản đồ cho phép người dùng trực tiếp chọn vị trí waypoints, chọn loại quỹ đạo tùy

ý theo nhu cầu thực tế

Hình 2-13 Giao diện điều khiển và hiển thị

XÂY DỰNG BỘ ĐIỀU KHIỂN DỰ BÁO MPC

3.1 Thiết kế bộ điều khiển MPC

3.1.1 Xây dựng phương trình động học

Thuật toán điều khiển được đề xuất để bám quỹ đạo đi dựa vào MPC, do đó, các mô hình tuyến tính xe đạp 2 bậc tự do được dùng để tạo ra 4 tham số bao gồm vị trí (x, y) của trung điểm trục sau được xác định bằng GPS, góc heading được xác định bằng cảm biến IMU, góc bẻ lái  xác định bằng encoder Khoảng cách từ trục trước đến trục sau là 𝑙 và bán kính bánh xe là r như Hình 3-1.

hoàn toàn được điều khiển bởi 2 tín hiệu 𝑣 𝑣à 𝜙̇ (tương ứng vận tốc tịnh tiến của xe và vận tốc góc bẻ lái)

Phương trình vi phân của hệ được thể hiện như sau:[4]

[

𝑥̇

𝑦̇

𝜃𝜙̇

]

=[

𝑐𝑜𝑠𝜃𝑠𝑖𝑛𝜃

𝑡𝑎𝑛𝜙 𝑙

0 ]

𝑢1+ [

0001

3.1.2 Xây dựng bộ điều khiển

Điểm khác biệt dễ nhận thấy nhất của MPC so với các phương pháp điều khiển tối

ưu khác (ví dụ như LQR) là luật điều khiển của MPC được tính online dựa vào trạng thái hiện tại của hệ thống và là một chuỗi hữu hạn, trong khi những phương pháp khác lại tính offline theo open-loop Mặc dù MPC có thể được sử dụng ở chế độ offline bằng cách dùng Dynamic Programming (DP) – là công cụ có thể tính ra luật điều khiển tối ưu một cách tổng quát theo bất kỳ trạng thái đầu, nhưng việc áp dụng DP đôi lúc lại bất khả thi trừ khi hệ thống tuyến tính, hàm chi phí toàn phương, không ràng buộc Điểm mạnh lớn nhất của MPC khi thực hiện online là khả năng tính toán nhanh thậm chí đối với hệ thống phi tuyến, hard constraints

Tất cả công thức và lập luận trong chương này dựa trên giả sử rằng: không có sai số

mô hình, nhiễu ngoại tác động và trạng thái được xác định chính xác

3.1.3 Bộ điều khiển dự báo

u k

( ) ( )

,0

MPC sẽ lấy phần tử đầu tiên trong chuỗi 0( )

u x để làm luật điều khiển close-loop

3.1.4 Bộ điều khiển dự báo mô hình bám quỹ đạo với ràng buộc

Mô hình xe tại phương trình (1) có dạng phi tuyến và có thể tuyến tính hóa xấp xỉ và rời rạc hóa Phương trình (1) có thể xem như phương trình vi phân liên tục bậc nhất

𝑋̇ = 𝑓(𝑋, 𝑢) (4) Với x là vector trạng thái 𝑥 = [𝑥, 𝑦, 𝜃, 𝜙]′, u là tín hiệu điều khiển 𝑢 = [𝑢1, 𝑢2]′ Phi tuyến bậc nhất của (4) có thể xấp xỉ bằng chuỗi taylor tại điểm tham chiếu (𝑥𝑟, 𝑢𝑟):

𝑋̇ = 𝑓(𝑥𝑟, 𝑢𝑟) + 𝑓𝑥,𝑟(𝑋 − 𝑥𝑟) + 𝑓𝑢,𝑟(𝑢 − 𝑢𝑟) (5) Trong đó, 𝑓𝑥,𝑟 𝑣à 𝑓𝑢,𝑟 là hàm Jacobi của x và u tại điểm tham chiếu (𝑥𝑟, 𝑢𝑟)

Thay (5) vào (4) ta được phương trình tuyến tính xấp xỉ như sau:

𝑋 ̃̇ = 𝐴(𝑡)𝑋 ̃ (𝑡) + 𝐵(𝑡)𝑢̃(𝑡) (6)

Trong đó, 𝑋 ̃ (𝑡) = 𝑋(𝑡) − 𝑋𝑟(𝑡) =

[

𝑥(𝑡) − 𝑥𝑟(𝑡)𝑦(𝑡) − 𝑦𝑟(𝑡)𝜃(𝑡) − 𝜃𝑟(𝑡)𝜙(𝑡) − 𝜙𝑟(𝑡)]

(7)

𝑣à 𝑢̃(𝑡) = 𝑢(𝑡) − 𝑢𝑟(𝑡) = [𝑢1(𝑡) − 𝑢𝑟1(𝑡)

𝑢2(𝑡) − 𝑢𝑟2(𝑡)]

Phương trình tuyến tính xấp xỉ hệ liên tục (6) có thể được rời rạc hóa tại thời điểm

𝑘 𝑣à 𝑘 + 1 = 𝑘 + Δ𝑡, với Δ𝑡 là khoảng thời gian chu kì lấy mẫu Tín hiệu điều khiển rời rạc 𝑢(𝑘) sẽ giữ nguyên trong khoảng thời gian từ 𝑘 đế𝑛 𝑘 + 1 Dạng rời rạc cho bộ điều khiển dự báo sẽ như sau:

𝑢̃(𝑘) = 𝑢(𝑘) − 𝑢𝑟(𝑘) = [𝑢1(𝑘) − 𝑢𝑟1(𝑘)

𝑢2(𝑘) − 𝑢𝑟2(𝑘)]

Trên đây là mô hình rời rạc xấp xỉ, bao gồm 2 tín hiệu điều khiển: tốc độ của xe

𝑢1(𝑘) − 𝑢𝑟1(𝑘), tốc độ góc bẻ lái 𝑢2(𝑘) − 𝑢𝑟2(𝑘) Có 4 ngõ ra được đo lường 𝑦(𝑘) =

𝑌 ̃ (𝑘) = 𝐶(𝑘)𝑋 ̃ (𝑘) Các ngõ ra này sẽ được cập nhật tại mỗi chu kì điều khiển Mô hình rời rạc (8) là một mô hình thay đổi theo thời gian, vì nó sẽ được cập nhật tại mỗi chu kì điều khiển

Giải thuật điều khiển dự báo mô hình được diễn giải trong chuỗi hữu hạn của giá trị ngõ ra và ngõ vào điều khiển Để cho đơn giản, ta kí hiệu hai giá trị này tương ứng là

𝑁𝑦 𝑣à 𝑁𝑢 và tạm thời cho 2 giá trị này bằng nhau 𝑁𝑦 = 𝑁𝑢 Đối tượng của bộ điều khiển

dự báo mô hình cho bám quỹ đạo của mô hình xe tự hành với ràng buộc

𝑉(𝑥(𝑘)) =1

2𝑥′(𝑘)𝑌𝑥(𝑘) + min {1

2𝑈′𝐻𝑈 + 𝑥′(𝑘)𝐹𝑈} (10) Bất đẳng thức ma trận tuyến tính (linear matrices inequality - LMI), 𝐺𝑈 ≤ 𝑊 + 𝐸𝑥(𝑡) với 𝑈 ≜ {Δ𝑢𝑘, … , Δ𝑢𝑘+𝑁𝑢−1} ∈ ℝ𝑠, 𝑠 ≜ 𝑚𝑁𝑢 là vector tối ưu, 𝐻 = 𝐻′ > 0 và

𝐻, 𝐹, 𝑌, 𝐺, 𝑊, 𝐸 được tính ra từ 𝑄, 𝑅 và phương trình (9) Chỉ vector tối ưu hóa U là cần thiết trong mỗi vòng lặp điều khiển Việc giải bài toán tối ưu cho (10) được thực hiện

nhờ vào quadratic programming (QP) Bộ điều khiển dự báo mô hình sẽ tính toán đầu

vào điều khiển tối ưu hóa 𝑈 ≜ {Δ𝑢𝑘, … , Δ𝑢𝑘+𝑁𝑢−1} trong điều kiện ràng buộc tín hiệu

điều khiển đầu vào 𝑢𝑘 ∈ 𝑈, 𝑣à 𝑢𝑘+𝑖 ∈ [𝑢𝑚𝑎𝑥𝑚𝑖𝑛] 𝑦𝑘 ∈ 𝑌, 𝑣à 𝑦𝑘+𝑖 ∈ [𝑦𝑚𝑎𝑥𝑚𝑖𝑛],

∆𝑢𝑘+𝑖 ∈ [∆𝑢𝑚𝑎𝑥𝑚𝑖𝑛]

Sơ đồ điều khiển sau đây thể hiện hệ thống bám quỹ đạo sử dụng bộ điều khiển dự báo

mô hình:

Hình 3-2 Sơ đồ hệ thống điều khiển dự báo mô hình

Tại mỗi chu kì điều khiển, điểm tham chiếu sẽ được xác định bằng việc xác định hình chiếu vuông góc từ điểm tọa độ mô hình tới quỹ đạo tham chiếu, từ đó dữ liệu tham chiếu được xác định x k r( ),y k r( ),r( ),k r( ),k u k r( ) Trạng thái của mô hình cũng được cập nhật ( ), ( ), ( ), ( )x k y k  k  k Hai bộ dữ liệu trên được sử dụng bởi bộ điều khiển dự báo (MPC) để điều khiển bám vào quỹ đạo Đầu ra của MPC là u k( + =1) u k( )−u k( − , từ 1)

đó ta xác định được ( )u k là tín hiệu điều khiển cho chu kì điều khiển tiếp theo Vòng điều khiển lặp lại cho đến khi vị trí của mô hình cách điểm cuối quỹ đạo nhỏ hơn một giá trị nhất định

Hình 3-3 Miền dự báo tín hiệu ngõ ra 𝑦𝑘 và tín hiệu điều khiển 𝑢𝑘 [14]

Hình 3-4 Quỹ đạo dự báo mô hình và quỹ đạo tham chiếu [15]

KẾT QUẢ MÔ PHỎNG

4.1 Thiết lập chương trình mô phỏng thuật toán trên MATLAB

Đối tượng mô hình xe đạp được sử dụng để mô phỏng giải thuật bộ điều khiển dự báo mô hình bám theo quỹ đạo mong muốn Hệ thống được mô phỏng bằng phần mềm MATLAB Một số quỹ đạo cho xe như đường thẳng, đường cong, kết hợp đường thẳng

và đường cong,… sẽ được áp dụng vào mô phỏng để kiểm chứng tính khả thi cũng như chất lượng của giải thuật

Để đánh giá chất lượng kết quả giải thuật, ta sử dụng 2 chỉ tiêu chất lượng: sai số bám và sai số vận tốc tương ứng với khoảng các từ mô hình đến vị trí gần nhất của quỹ đạo và sai lệch giữa vận tốc mô hình với vận tốc tham chiếu Tuy nhiên, trong trường hợp này ta chỉ chú trọng yếu tố đảm bảo đối tượng bám được quỹ đạo nên chỉ tập trung vào sai số bám

Các thông số của bộ điều khiển và mô hình được thể hiện ở bảng sau:

Khoảng cách trục trước-sau – 𝑙(𝑚) 0.65

4.2 Khảo sát tham số của bộ điều khiển MPC

4.2.1 Khảo sát 1: quỹ đạo chuyển làn xe với N p

Quỹ đạo chuyển làn xe [13] là một đoạn đường thẳng của đường hai làn trong đó phương tiện được yêu cầu thực hiện thao tác chuyển một làn Thao tác chuyển làn đường

là một bài kiểm tra phổ biến để xử lý phương tiện vì nó thể hiện thao tác tránh va chạm cần thiết

Các thông số cho bộ điều khiển như sau:

𝑅 = [0.01 0

0 0.01]

5 ≤ 𝑁𝑝 = 𝑁𝑢 ≤ 20 Với 𝑁𝑝 = 𝑁𝑢, ta lần lượt mô phỏng với các giá trị 5, 10, 15, 20

Kết quả:

Hình 4-1 Quỹ đạo chuyển làn của đối tượng và quỹ đạo tham chiếu của MPC

với Np=5→20