Hiệu chỉnh vị trí

Các phương trình của bước hiệu chỉnh vị trí tại thời điểm k+1:

1 1 1 T 1( 1 1 T 1 T 1) k k k k k k k             K P H H P H R (3.24) 1 ( 1 1) 1 k k k k        P I K H P (3.25) 1 1 1 1 1 ˆk  ˆk  k ( k h(ˆk)) x x K z x (3.26)

55

Trong đó, zk1 là véc-tơ chứa giá trị đo đạc vị trí của robot được thu thập từ các cảm

biến (cảm biến VLC và Gyro) tại thời điểm k+1. Phép đo zk1 đã được mô hình hóa trong công thức (3.19) ở tiểu mục 3.2.1. Ma trận Hk+1 là ma trận Jacobin chứa các đạo

hàm riêng của hàm biểu diễn phép đo phi tuyến h(.) theo véc tơ vị trí ước lượng tiên

nghiệm ˆk 1  x : ˆ 1 1 ( ) k k h x        x x H x (3.27.1)                   1 ˆk x x x x y y y y x y x y h h h h h h h h h                                              x x x x x x x x x x x 1 ˆ 1 0 0 0 1 0 0 0 1 k              x x (3.27.2)

Có thể dễ dàng thấy trong công thức (3.27.2), ma trận Jacobin Hk+1 chỉ chứa các hằng số, do đó chúng ta không cần phải tính toán lại sau mỗi thời điểm. Ma trận Rk+1 là ma trận hiệp phương sai của véc-tơ sai số Gauss vk+1 trong công thức (3.18) ở tiểu mục 3.2.2. Trong đó, phương sai của sai số trong phép đo AOA-RSS là 0.0036. Độ chính xác của cảm biến Gyro là 30 tương ứng với phương sai của sai số này là 0.0028 trong hệ đơn vị rad.

Ngoài ra, Kk+1 là trọng số của bộ lọc Kalman; ma trận Pk+1 là ma trận hiệp

phương sai của sai số trong bước hiệu chỉnh vị trí. Cuối cùng, xˆk1 là vị trí ước lượng hậu nghiệm của robot được ước tính từ giá trị tiên nghiệm ˆk 1



x trong bước ước đoán vị trí bằng việc cộng thêm một lượng điều chỉnh bằng tích của độ sai lệch giữa hai phép

đo thu được từ encoder và các cảm biến và trọng số Kk+1.

3.4. Điều khiển robot bám quỹ đạo di chuyển

Như chúng ta đã thảo luận trước đó, các robot di động trong trong quá trình di chuyển sẽ sinh ra các sai số hệ thống do các nguyên nhân như độ trượt mặt sàn cao, mặt sàn không bằng phẳng, độ phân giải của các encoder thấp, v.v. Các sai số hệ thống này có đặc tính tích lũy, do đó robot sẽ bị lệch khỏi quỹ đạo của mình sau một khoảng thời gian di chuyển nhất định nào đó nếu không có một cơ chế điều khiển thích hợp (xem hình 3.5).

56

Hình 3.5. Robot bị lệch khỏi quỹ đạo của nó do các sai số hệ thống.

Cho đến nay, có rất nhiều các thuật toán điều khiển bám quỹ đạo cho robot di động đã được đề suất và nghiên cứu như thuật toán cân bằng Lyapunov. Trong các mục trước, bộ lọc Kalman mở rộng được nghiên cứu để nâng cao độ chính xác cho phương pháp định vị dựa trên công nghệ VLC. Trong mục này, chúng ta sẽ xem xét vai trò của bộ lọc Kalman mở rộng như là một “cơ cấu bù” để chống lại các sai số hệ thống đó.

Giả sử rằng robot di động di chuyển trên một quỹ đạo thẳng với vận tốc không đổi. Khi đó, chúng ta có thể lấy mẫu theo khoảng cách ssample đủ nhỏ. Bằng việc lấy mẫu theo khoảng cách ta thu được một tập hợp XPOST { } (xi i=1,2...N); trong đó, xi

là các vị trí trên quỹ đạo tại các điểm lấy mẫu, N là số mẫu thu được trên quỹ đạo của

robot. Như đã biết, sau mỗi một chu kỳ lấy mẫu, bộ lọc Kalman được tính toán lại để ước lượng vị trí hiện tại của robot. Khi đó, robot sẽ dựa trên thông tin từ vị trí ước lượng hiện tại và vị trí trên quỹ đạo trong chu kỳ lấy mẫu tiếp theo để tính toán độ dịch chuyển khoảng cách snextvà góc next mà robot phải thực hiện (xem hình 3.7). Sau đó, từ phương trình (3.1), robot tính toán véc-tơ điều khiển u ( sˆL,sˆR) để điều khiển cơ cấu chấp hành của robot trong bước di chuyển tiếp theo:

ˆ ˆ 2 ˆ 2 R s b s      ; ˆ 2 ˆ ˆ 2 L s b s      (3.29)

57

Hình 3.6. Robot và quỹ đạo di chuyển của nó.

Hình 3.7. “Cơ chế bù” và các thông số.

Hình 3.8 là sơ đồ khối mô tả hoạt động của “cơ chế bù” - bộ lọc Kalman mở rộng để chống lại các sai số hệ thống.

58

Hình 3.8. Sơ đồ khối điều khiển của “cơ chế bù”.

3.5. Tóm tắt chương 3

Trong chương này, chúng ta đã thảo luận về các mô hình hệ thống và mô hình phép đo sử dụng phương pháp định vị dựa trên công nghệ VLC và cảm biến Gyro. Đồng thời, chúng ta cũng đã mô hình hóa các sai số hệ thống và sai số của phép đo. Các sai số này có phân bố Gauss, độc lập và có giá trị trung bình bằng 0, được sinh ra do các yếu tố từ hệ thống và môi trường.

Để nâng cao độ chính xác của thuật toán định vị kết hợp AOA-RSS, chúng ta sử dụng bộ lọc Kalman mở rộng để kết hợp nó với thông tin về vị trí từ hệ thống nhằm đạt được một vị trí ước lượng tốt nhất. Trong phần nội dung của chương này, tôi đã

trình bày hai bước trong thuật toán bộ lọc Kalman mở rộng: ước định vị trí và hiệu chỉnh vị trí để tìm vị trí ước lượng tối ưu của robot di động. Đây là nội dung chính của

chương ba cũng như của toàn bộ luận văn này. Ngoài ra, bộ lọc Kalman mở rộng còn đóng vai trò như một “cơ cấu bù” để chống lại các sai số gây ra bởi hệ thống và môi trường. Các sai số này có đặc tính tích lũy theo thời gian do đó nó làm cho robot lệch khỏi quỹ đạo của nó. Khi sử dụng bộ lọc Kalman mở rộng, robot luôn tính toán lại véc-tơ điều khiển u ( sˆL,sˆR)sau mỗi chu kỳ lấy mẫu từ vị trí ước lượng tối ưu của robot để điều khiển cơ cấu chấp hành của robot thực hiện việc bám theo quỹ đạo định trước.

59

Chương 4

KẾT QUẢ MÔ PHỎNG VÀ ĐÁNH GIÁ

4.1. Mô phỏng phương pháp định vị kết hợp AOA–RSS

4.1.1. Kịch bản mô phỏng

Trước tiên, chúng ta sẽ xem xét kịch bản mô phỏng cho phương pháp định vị kết hợp AOA–RSS đã được đề cập trong mục 2.2. Mô hình được thực hiện trong mô phỏng là các hành lang dài trong các thư viện, bệnh viện, viện bảo tàng, nhà máy, v.v. (xem hình 4.1). Mô hình hành lang dài là mô hình phổ biến nhất cho sự hoạt động của các robot di động trong nhà như: các robot đóng vai trò như các hướng dẫn viên trong các viện bảo tàng hay phòng trưng bày các tác phẩm nghệ thuật; các robot vệ sinh trong bệnh viện hay các tòa nhà văn phòng; các robot vận chuyển hàng hoặc vật liệu trong các dây truyền sản xuất của nhà máy. Như nội dung đã thảo luận trong mục 2.2, do phương pháp định vị kết hợp AOA–RSS sử dụng công suất quang nhận được tại các PD để xác định góc hướng của nó. Do đó, độ chính xác của phương pháp định vị này phụ thuộc vào vị trí tương đối của robot di động so với đèn LED phát mà nó đang kết nối. Hay nói cách khác là khi robot di dộng ở càng xa đèn LED phát thì sai số càng lớn và ngược lại. Để kiểm chứng độ chính xác của phương pháp định vị kết hợp AOA–RSS thì chúng ta sẽ cho robot di động di chuyển theo một đường cong dạng hình sin có biên độ bằng với độ rộng thông thường của các hành lang là 2 (m) (xem hình 4.2). Với kịch bản mô phỏng này chúng ta có thể đánh giá được hiệu suất của phương pháp định vị này từ trường hợp tốt nhất (khi robot ở vị trí ngay dưới đèn LED) cho đến trường hợp tồi nhất (khi robot di chuyển đến sát tường của hành lang, tức là xa đèn LED nhất). Ngoài ra, phương pháp định vị đề suất cũng hoàn toàn đúng với các kịch bản khác cho sự hoạt động của robot di động trong nhà.

4.1.2. Chương trình mô phỏng

Chương trình mô phỏng phương pháp định vị kết hợp AOA–RSS được thực hiện trên phần mềm Matlab. Chương trình mô phỏng này được xây dựng để đánh giá độ chính xác của phương pháp định vị này trong một kịch bản đặc biệt – các hành lang dài trong các tòa nhà nơi các hoạt động của các robot diễn ra một cách phổ biến nhất.

Code của chương trình mô phỏng phương pháp định vị kết hợp AOA–RSS được trình bày chi tiết trong phần phụ lục A. Chương trình này gồm các tập tin chính sau (xem hình 4.3):

60

Hình 4.1. Mô hình mô phỏng.

Hình 4.2. Kịch bản mô phỏng phương pháp định vị kết hợp AOA–RSS.

AOA_RSS_main.m: là tập tin chính của chương trình mô phỏng. Tập tin này thực hiện một số nhiệm vụ chính như sau:

61

 Thực hiện phương pháp định vị AOA–RSS.  Tính toán sai số.

InputParameters.m: là tập tin chứa các thông số để cấu hình cho hệ thống. Các thông số này được phân loại như sau:

 Các thông số cấu hình chung như: số lượng PD, khoảng cách lấy mẫu.  Các thông số cấu hình cho mô hình hệ thống VLC như: chiều cao của

đèn LED, góc nửa công suất, góc FOV của PD, v.v.

 Các thông số để tính toán nhiễu Gauss tại đầu ra của bộ nhận quang PD: độ nhạy thu của PD, tốc độ truyền, v.v.

AOA_RSS_Estimation.m: là tập tin thực hiện hai bước của phương pháp định vị kết hợp AOA–RSS:

 Tính toán góc ước lượng αi-max (hàm ComputeEstAng).

 Tính toán vị trí ước lượng của robot từ giá trị αi-max (hàm

ComputeEstPosition).

ComputeEstAng.m: là tập tin thực hiện việc tính toán góc ước lượng αi-max

trong bước một của phương pháp định vị đề suất. Các nhiệm vụ của nó bao gồm:  Tính toán công suất nhận được tại các PD (hàm ComputePower) trong

trường hợp không có nhiễu.

 Tính toán công suất nhận được tại các PD trong trường hợp có nhiễu.  Xác định PD có công suất nhận được lớn nhất.

 Tính góc của PD nhận được công suất lớn nhất đó.

ComputeEstPosition.m: là tập tin thực hiện việc tính toán vị trí ước lượng của robot trong bước hai của thuật toán định vị đề xuất.

ComputePower.m: là tập tin thực hiện việc tính toán công suất nhận được tại các PD trong trường hợp không có nhiễu.

ComputeNoise.m: là tập tin thực hiện việc tính toán nhiễu tác động lên tín hiệu đầu ra của các PD bao gồm nhiễu nhiệt và tạp âm kim.

CreatePDArray.m: là tập tin thực hiện việc tính toán vị trí của các PD theo vị trí của robot.

DrawCircle: là tập tin thực hiện việc vẽ các vùng phủ tưởng tượng của các đèn LED.

62

Hình 4.3. Sơ đồ khối của chương trình mô phỏng phương pháp định vị kết hợp AOA– RSS.

4.1.3. Kết quả mô phỏng

Ngoài sự phụ thuộc vào khoảng cách tương đối giữa vị trí hiện tại của robot di động và đèn LED thì độ chính xác của phương pháp định vị kết hợp AOA–RSS còn phụ thuộc vào số lượng đèn LED được gắn trên robot. Số lượng đèn LED càng nhiều thì độ chính xác của phương pháp định vị này càng cao. Thật vậy, điều này được chứng minh qua việc mô phỏng ba kịch bản của phương pháp định vị đề xuất tương

ứng với số lượng PD được gắn trên robot luần lượt là K1 = 8 PD, K2 = 12 PD, K3 = 16 PD. Các hình 4.4, 4.5, 4.6 lần lượt biểu diễn sai số của phương pháp định vị kết hợp AOA–RSS trong các trường hợp ứng với số lượng PD được gắn trên robot khác nhau. Từ các hình vẽ trên chúng ta có thể dễ dàng nhận thấy sai số của phương pháp định vị này giảm khi chúng ta tăng số lượng PD gắn trên robot lên. Cụ thể, sai số của phương pháp định vị này lên tới 40 (cm) khi số lượng PD được gắn trên robot chỉ có 8 PD trong hình 4.4. Sai số này giảm xuống 25 (cm) khi số lượng PD tăng lên 12 PD trong hình 4.5. Sai số này tiếp tục giảm xuống 20 (cm) khi số lượng PD gắn trên robot là 16 PD trong hình 4.6. Tuy nhiên, chúng ta không thể tăng mãi đại lượng này do vấn đề chế tạo mô hình trong thực tế. Việc sử dụng quá nhiều PD dẫn đến công suất nhận được tại các PD xấp xỉ bằng nhau, dẫn đến việc không thể xác định được góc hướng của robot. Do đó, mô hình của robot trong phạm vi luận văn này sử dụng 16 PD để thực hiện việc định vị với sai số đạt được là 20 (cm). Để tiếp tục giảm sai số này, chúng ta cần phải sử dụng các phương pháp ước lượng như bộ lọc Kalman mở rộng như đã thảo luận trong các phần trước. Trong phần tiếp theo, chúng ta sẽ thực hiện mô phỏng bộ lọc Kalman mở rộng để đánh giá việc cải thiện độ chính xác của phương pháp định vị đề suất của nó.

63

Hình 4.4. Sai số của phương pháp định vị kết hợp AOA–RSS trong trường hợp số PD là K1 = 8.

Hình 4.5. Sai số của phương pháp định vị kết hợp AOA–RSS trong trường hợp số PD là K2 = 12.

Hình 4.6. Sai số của phương pháp định vị kết hợp AOA–RSS trong trường hợp số PD là K3 = 16.

64

4.2. Mô phỏng hoạt động của bộ lọc Kalman mở rộng

4.2.1. Kịch bản mô phỏng

Trong phần này, chúng ta sẽ đánh giá hiệu quả của việc sử dụng bộ lọc Kalman mở rộng để cải thiện độ chính xác của phương pháp định vị kết hợp AOA–RSS khi mà việc tăng số lượng PD gắn trên robot không còn khả thi nữa. Một kịch bản mô phỏng đơn giản nhất đó là điều khiển robot di chuyển trên một đường thẳng từ vị trí có tọa độ (1, 1) đến vị trí có tọa độ (8, 1) trong hệ tọa độ toàn cầu (đơn vị tính là mét). Với kịch bản mô phỏng này, chúng ta có thể kiểm chứng hiệu quả của việc sử dụng bộ lọc Kalman mở rộng trong vai trò làm giảm sai số của phương pháp định vị kết hợp AOA– RSS. Đồng thời, chúng ta cũng có thể kiểm chứng khả năng triệt tiêu các sai số tích lũy của hệ thống trong vai trò một cơ cấu bù của hệ thống. Mô hình robot được giả sử chạy với một vận tốc đều 0,1(m/s). Do đó, thời gian lấy mẫu trong mô phỏng được giả sử là 1(s). Nghĩa là cứ sau khoảng 1(s) robot sẽ thực hiện việc cập nhật lại vị trí của mình.

4.2.2. Chương trình mô phỏng

Chương trình mô phỏng hoạt động của bộ lọc Kalman mở rộng được thực hiện trên phần mềm Matlab để thực hiện đánh giá hiệu quả của bộ lọc Kalman mở rộng trong việc cải thiện độ chính xác của phương pháp định vị kết hợp AOA–RSS.

Cấu trúc của chương trình mô phỏng này được minh họa trong hình 4.7. Toàn bộ code của chương trình mô phỏng này được trình bày chi tiết trong phụ lục B. Nó bao gồm một số tập tin chính sau:

EKF_AOA_RSS_main.m: là tập tin chính của chương trình mô phỏng bộ lọc Kalman mở rộng. Các nhiệm vụ chính của nó là:

 Cung cấp phép đo vị trí robot (phương pháp định vị AOA–RSS).  Thực hiện phép ước lượng sử dụng bộ lọc Kalman mở rộng.  Tính toán véc-tơ control u để triệt tiêu sai số hệ thống.

CreateLEDsMatrix.m: là tập tin cấu hình cho hệ thống đèn LED. Nó trả ra một mảng lưu vị trí của các đèn LED.

Compute_EKF.m: là tập tin tính toán các ma trận bộ lọc Kalman mở rộng để tính toán vị trí của robot.

AOA_RSS_measurement.m: là tập tin thực hiện phép đo định vị kết hợp AOA–RSS.

ComputeControlU.m: là tập tin thực hiện tính toán véc-tơ điều khiển u để triệt tiêu các sai số hệ thống sau mỗi chu kỳ lấy mẫu.

65

Các tập tin: ComputeEstAng.m, ComputeEstPosition.m, ComputePower.m, ComputeNoise.m và CreatePDArray.m nằm trong tập tin EstimateAOA_RSS.m tương tự như trong chương trình mô phỏng phương pháp định vị kết hợp AOA–RSS trong mục 4.1.

Hình 4.7. Sơ đồ khối của chương trình mô phỏng bộ lọc Kalman mở rộng.