Luận văn thạc sĩ Kỹ thuật cơ điện tử: Nâng cao độ chính xác cảm biến MARG sử dụng trong điều khiển Quadrotor

Nó được sử dụng để ước lượng góc quay của hệ trục đặt trên cảm biến so với hệ trục tọa độ trái đất, hỗ trợ cho việc giữ thăng bằng của các hệ thống tự động như robot vượt địa hình, máy b

TỔNG QUAN

Ý nghĩa, tầm quan trọng của việc nghiên cứu luận văn

Autopilot control* - GPS hold*- Yaw/rudder- Throttle- Pitch/Elevator- Roll/ Aileron-

In puts Se rv o O utp ut s

G P S I2 C Pi lo t Zi gb ee Pi lo t

Power from ESC Power to RC

Hình 1.1 Cấu trúc phần cứng một hệ thống điều khiển Quadrotor tiêu biểu

Hình 1.2 Cấu trúc phần mềm một hệ thống điều khiển Quadrotor tiêu biểu

Theo sơ đồ trên hình 1.1, một mô hình bay Quadrotor hoạt động được nhờ có bộ điều khiển bay (Control board) và cơ cấu chấp hành (motor, esc…) Trong đó bộ điều khiển bay đóng vai trò chính trong việc điều khiển mô hình Bộ điều khiển có nhiệm vụ nhận lệnh từ bộ điều khiển (RC) và nhận dữ liệu từ các cảm biến (IMU shield, GPS, ) chuyển đổi, lọc nhiễu và ước lượng trạng thái mô hình,từ đó đưa ra các chỉ thị thích hợp cho các bộ điều khiển tốc độ điện tử (ESC) Trong các sản phẩm Quadrotor thương mại, các control board và IMU shield (các khung nét đứt màu đỏ trên Hình 1.2) thường được đóng gói chung và không cho phép sự can thiệp từ bên ngoài vào cấu trúc và mã bên trong Do đó, đề tài luận văn này tác giả tập trung phát triển thuật toán cho thiết bị đo góc phục vụ điều khiển Quadrotor (khung nét đứt màu xanh trên Hình 1.2) để làm tiền đề cho việc phát triển các bộ điều khiển bay và cải thiện chất lượng đo về sau, để có thể chủ động trong các nghiên cứu sâu hơn về điều khiển Quadrotor

Hình 1.3 Cấu trúc một thiết bị đo kỹ thuật số theo Analog Devices

Dữ liệu từ cảm biến (bao gồm nhiễu và các loại sai số) được xử lý qua thuật toán ước lượng nhằm kết hợp, chuyển đổi và lọc nhiễu các tín hiệu đo sao cho trạng thái ước lượng có sai số nhỏ nhất Ở đây quá trình đo đạc và ước lượng gồm 2 phần chính: phần cảm biến gồm nhiều loại cảm biến khác nhau được kết hợp với nhau (gia tốc kế, con quay hồi chuyển, cảm biến từ trường hay còn gọi là cảm biến MARG) Và phần thuật toán để lọc nhiễu và ước lượng tức là khối (Signal Conditioning and Conversion) trên

Hình 1.3 Tất cả cấu thành nên bộ ước lương góc quay trong không gian Để có thể điều khiển mô hình ổn định thì việc ước lượng, lọc nhiễu từ dữ liệu đo được từ cảm biến là rất quan trọng Vì vậy mục tiêu đề tài muốn tập trung vào thuật toán ước lượng để làm tăng độ chính xác của kết quả đo góc

Khối đo quán tính (IMU) là thiết bị điện tử đo và truyền dữ liệu vận tốc góc, góc quay và lực trọng trường của đối tượng mà IMU được thiết lập để đo, là tập hợp của nhiều loại cảm biến như cảm biến đo vận tốc góc (gyrometers), cảm biến gia tốc (accelerometers) và cảm biến la bàn (magnetometers) còn được gọi là MARG (magnetic, angular rate and gravity) hay cảm biến 9 bậc tự do (DOF) Nó được sử dụng để ước lượng góc quay của hệ trục đặt trên cảm biến so với hệ trục tọa độ trái đất, hỗ trợ cho việc giữ thăng bằng của các hệ thống tự động như robot vượt địa hình, máy bay,… Mỗi cảm biến có một chức năng riêng, có ưu điểm và nhược điểm riêng Việc kết hợp nhiều loại cảm biến lại cho phép robot, mô hình bay có được nhiều nguồn dữ liệu tin cậy từ đó đưa ra những quyết định chính xác hơn IMU được sử dụng trong rất nhiều lĩnh vực và trong nhiều ứng dụng cuộc sống ví dụ như trong lĩnh lực hàng không: mô hình bay không người lái [1][2], trong lĩnh vực dân dụng: hệ thống hỗ trợ đỗ xe tự động [3], trong y học: hệ thống vi giải phẫu [4], hệ thống giám sát chuyển động của cơ thể người [5][6] và trong các hoạt động định vị và định hướng cho robot tự hành [7][3]

Mặc dù IMU đã được ứng dụng rộng rãi trên thế giới nhưng hướng nghiên cứu và chế tạo IMU ở trong nước còn khá mới mẻ Lý do để chế tạo được một IMU tốt cần phải hội đủ hai yếu tố: cảm biến tốt và thuật toán ước lượng trạng thái tối ưu trong không gian Cảm biến tốt chúng ta có thể mua được, nhưng để ước lượng trạng thái tối ưu cần phải có kiến thức toán học về vật chuyển động trong không gian và đặc biệt phải có kinh nghiệm trong việc thiết kế các bộ ước lượng số phi tuyến tối ưu Trong 5 năm trở lại đây, ở Việt Nam đã có một vài nhóm nghiên cứu trong các trường đại học tiến hành nghiên cứu và chế tạo thử nghiệm IMU dùng bộ lọc tuyến tính Kalman mở rộng Tuy nhiên, hạn chế của các IMU này là cho ra kết quả ước lượng không chính xác vì thuật toán ước lượng góc quay được xây dựng theo bộ lọc tuyến tính, trong khi bản chất của chuyển động quay IMU là phi tuyến, khối lượng tính toán của thuật toán quá lớn dẫn đến độ trễ trong quá trình ước lượng lớn làm giảm khả năng tác động của đối tượng trước sự thay đổi về trạng thái thực tế Hơn nữa, khi xây dựng bộ ước lượng, các nhóm nghiên cứu chưa đánh giá đầy đủ ảnh hưởng của gia tốc ngoài và từ trường ngoài tác động tới thiết bị nên kết quả ước lượng góc ngõ ra không chính xác

Ngày nay, việc nghiên cứu chế tạo mô hình bay ngày càng phát triển và được ứng dụng trong nhiều lĩnh vực của cuộc sống hơn và rất nhiều học sinh, sinh viên từ các trường và nhiều tổ chức đã bắt tay nghiên cứu để chế tạo ra các mô hình bay Tuy nhiên khi tiếp xúc với các nhóm nghiên cứu mô hình máy bay không người lái, ta thấy họ gặp rất nhiều khó khăn trong việc triển khai vì không nắm được công nghệ IMU Trong khi đó, các kết quả nghiên cứu IMU trong nước chưa thể áp dụng vào công nghiệp hoặc máy bay mô hình vì đặc tính của động cơ máy bay phát sinh nhiễu rất lớn làm cho kết quả ước lượng không chính xác

Vì vậy đề tài muốn nghiên cứu thuật toán có thể ước lượng các góc trong không gian với sai số thấp, khả năng lọc nhiễu tốt và có khả năng ứng dụng trong thực tế cao Đề tài tập sử dụng mô hình bay Quadrotor để kiểm nghiệm khả năng ứng dụng và sai số của thuật toán trong môi trường bay thực tế Đó sẽ là nền tảng để nâng cao được các tính năng bay tự động, cũng như phát triển các chức năng bay mới cho mô hình bay trong tương lai.

Tổng quan về tình hình nghiên cứu trong và ngoài nước

Để xác định các góc trong không gian ta cần các cảm biến đo góc Hiện nay trên thị trường có rất nhiều loại cảm biến đo góc:

Hình 1.4 Một số cảm biến đo góc

Cảm biến đo góc cho độ chính xác rất cao Ngõ ra là analog tiện lợi cho việc sử dụng Chống chọi với điều kiện môi trường tốt Ít ảnh hưởng bởi nhiễu Nhưng nếu ứng dụng trên mô hình bay quadrotor sẽ có một số bất lợi

Khối lượng cảm biến lớn sẽ làm tăng khối lượng mô hình bay Kích thước lớn ảnh hưởng tới không gian bố trí thiết bị điện trên mô hình Nếu muốn thay đổi cảm biến sẽ ảnh hưởng tới trọng tâm của mô hình cũng như khối lượng của mô hình dẫn đến thay đổi về giải thuật điều khiển

Ngoài ra trên mô hình bay cần tích hợp thêm vi xử lý và nhiều cảm biến để tăng các chức năng bay khác Ví dụ các chức năng bay tự động như loiter hay RTL cần có cảm biến áp suất và cảm biến GPS để xác định độ cao và tọa độ mô hình Khi đó sẽ ảnh hưởng đến khả năng tích hợp cảm biến trên cùng một board, tăng diện tích và khối lượng thiết bị điện Điện áp sửa dụng của một số cảm biến góc là 24VDC nên cần phải có thêm nguồn cho cảm biến làm tăng số lượng thiết bị điện trên mô hình

Ngày nay, cùng với sự phát triển của công nghệ MEMS và các dòng vi điều khiển hỗ trợ tính toán số thực, các cảm biến có sai số càng nhỏ và dễ dàng tích hợp với vi điều khiển tạo nên IMU nhỏ gọn Một khối vi cơ IMU (Inertial Measurement Unit) bao gồm cảm biến vận tốc góc và một cảm biến gia tốc để đo sự định hướng, gia tốc dựa trên hệ thống cảm biến bên trong mà không phụ thuộc vào các yếu tố bên ngoài hay môi trường

Một hệ thống cảm biến MARG (Magnetic, Angular Rate, và Gravity) chính là sự kết hợp giữa IMU và một cảm biến từ trường Khi kết hợp các cảm biến vi cơ thành một cấu trúc tổng thể thì thường tạo ra sai số Sai số mắc phải trong việc sử dụng các cảm biến vi cơ này có ở 2 cấp độ, cấp độ cảm biến và cấp độ nhóm cảm biến Ở cấp độ cảm biến là sai số của từng cảm biến cấu tạo thành khối IMU, còn ở cấp độ nhóm cảm biến là sai số tổ hợp của nhóm cảm biến với nhau Các cảm biến vi cơ kết cấu hỗ trợ với nhau theo cấu trúc gắn liền (hình 1.5a) hoặc theo cấu trúc nổi (hình 1.5b)

Hình 1.5 Các cấu trúc của khối IMU vi cơ [19]

Trên thực tế khối IMU có cấu trúc kiểu Strapdown được sử dụng rộng rãi hơn bởi cấu trúc này đơn giản và có giá thành chế tạo thấp với độ chính xác có thể chấp nhận được

Trên thế giới hiện nay có rất nhiều dòng sản phầm IMU Sau đây là ví dụ về một số sản phẩm thương mại đạt độ chính xác cao:

Hình 1.6 Một số IMU thương mại [21]

Các sản phẩm IMU ước lượng ba góc quay được sử dụng rộng rãi trong các lĩnh vực như: robot tự cân bằng, điều khiển máy bay, điều khiển Quadrotor, hệ thống dẫn đường INS kết hợp IMU và GPS,… Hiện nay các bộ điều khiển bay đã được phát triển rất nhiều Bằng việc sử dụng các cảm biến nhỏ gọn tích hợp với vi xử lý đã giúp cho việc phát triển các ứng dụng cũng như chức năng bay của mô hình ngày càng đa dạng hơn

Hình 1.7 Bộ điều khiển bay Quadcopter [20]

Trên đây là một số bộ điều khiển bay đang được sử dụng phổ biến Hình 1.7 (a) là bộ điều khiển “Naza-M V2” được sử dụng trên các mô hình bay thương mại với độ ổn định cao Thông số thiết lập đơn giản và hỗ trỡ một số tính năng bay tự động Cho phép mô hình bay giữ cố định vị trí trên không nhờ GPS và bảm biến IMU để giữ thăng bằng

Ngoài ra mô hình cũng hỗ trợ chức năng tự động quay lại điểm xuất phát mà ko cần điều khiển, đề phòng trong trường hợp mất tín hiệu từ tay điểu khiển hay hết pin Bộ điều khiển được sử dụng trên mô hình “DJI Phantom 2 VISION PLUS” ( hình 1.8(a)) chế tạo và cân chỉnh bởi cùng nhà sản xuất Hình 1.7(b) là bộ điều khiển APM 2.6 sử dụng nhiều trên các mô hình tự chế tạo bởi người chơi Với số lượng thông số thiết lập nhiều và phức tạp nên cần nhiều thời gian để cân chỉnh cũng như thử nghiệm các chế độ bay khác nhau Bởi vì mỗi mô hình tự chế đều có các thông số về động cơ, cánh quạt, khung, pin… khác nhau Vì vậy mà bộ điều khiển này ứng dụng được trên nhiều mô hình bay khác nhau

Hình 1.8 Mô hình bay Quadrotor được sử dụng phổ biến.[20]

(a) Mô hình Quadcopter DJI Phantom 2 VISION PLUS (b)Mô hình Quadcopter tự làm

Các bộ điều khiển bay trên đều sử dụng cùng cấu hình phần cứng là cảm biến sáu bậc tự do MPU6050, cảm biến la bàn HCM 5883L và các cảm biến khác như cảm biến áp suất, GPS

Các cảm biến MPU6050 và cảm biến HCM 5883L được sử dụng trong tính toán ước lượng góc nghiêng và góc quay của mô hình bay trong không gian Với lợi thế là cảm biến dạng IC, nên có khối lượng nhỏ, dễ dàng tích hợp với vi xử lý và các cảm biến khác trên cùng một board điều khiển, làm giảm kích thước và khối lượng board điểu khiển và số lượng thiết bị điện trên mô hình

Hình 1.9 Một số board cảm biến MARG (9DOF)

Tuy nhiên các cảm biến này có nhược điểm là dễ ảnh hưởng bởi nhiễu Đặc biệt là nhiễu rung động do động cơ, sự không đối xứng của cánh quạt… gây ra Những nhiễu này tác động lên cảm biến là cho giá trị góc đo được bị sai lệch nhiều dẫn đến sai số lớn trong việc xác định góc nghiêng của mô hình Vì vậy khi sử dụng những cảm biến này cần phải tiến hành lọc nhiễu bằng giải thuật toán để có được góc ít nhiễu nhất và sai số với góc thực tế là nhỏ nhất

Các thuật toán nghiên cứu về lọc nhiễu trên IMU hiện nay tập trung phát triển và hoàn thiện hai loại thuật toán cho các bộ IMU là bộ lọc dựa trên phương pháp Quaternion ở [5], [8] và lọc dựa trên phương pháp Euler ở [3], [9], [10], [11] Trong đó bộ lọc dựa trên phương pháp Quaternion tuy có ưu thế hơn về độ chính xác so với bộ lọc dựa trên phương pháp Euler nhưng phức tạp hơn và khối lượng tính toán nhiều hơn Ngoài ra còn có bộ lọc dựa trên phương pháp DCM cho kết quả tốt hơn hai bộ lọc còn lại và đang được nghiên cứu phát triển rộng rãi Với sự phát triển của các dòng vi điều khiển hỗ trợ tính toán số thực thì khối lượng tính toán không phải là vấn đề lớn nhưng để đáp ứng được với một số ứng dụng đòi hỏi khả năng đáp ứng nhanh thì lại là một vấn đề cần nghiên cứu Sai số của các góc tính toán từ IMU phần lớn được quyết định ở chất lượng của thuật toán đã được phát triển, do đó mục tiêu chính của luận văn này là xây dựng và đánh giá các thuật toán hiện nay để chọn ra thuật toán lọc nhiễu tốt nhất, sai sai số góc tính toán nhỏ nhất, giảm khối lượng tính toán từ đó đưa ra các cải tiến để hoàn thiện bộ lọc

Các bài báo sau đây được tóm tắt lại một số nghiên cứu về các thuật toán ước lượng góc nghiêng đã được nghiên cứu.

Một số bài báo liên quan

3.1 Thuật toán dựa trên phương pháp Euler Orientation tracking by Inertial Sensing-A Review - Anamika Gahlot, Deepika

Bài báo là một bài tổng hợp về những nghiên cứu trong việc sử dụng cảm biến quán tính để đo đạc tính toán góc nghiêng Bài báo đem lại một cái nhìn tổng quát về cảm biến (cảm biến vận tốc, cảm biến gia tốc, cảm biến từ trường) và những giới hạn của những loại cảm biến đó Phương pháp ước lượng góc nghiêng bằng bộ lọc Kalman và bộ lọc bổ phụ được so sánh và tổng hợp lại Một vài thuật toán ước lượng góc nghiêng khác như phương pháp Euler và phương pháp Quaternion cũng được thảo luận trong bài báo Bài báo là tiền đề để thực hiện đề tài Dựa vào những mặt giới hạn của cảm biến cũng như của những phương pháp đã được thực hiện trước đó giúp tác giả xác định được hướng đi của đề tài, những mục tiêu cần đạt được để khắc phục những vấn đề còn tồn tại trong việc ước lượng góc nghiêng khi sử dụng cảm biến MARG

An Introduction to the Kalman Filter - Greg Welch and Gary Bishop

Bài báo cung cấp cái nhìn thực tế về bộ lọc Kalman rời rạc Bài báo bao gồm các vấn đề cơ bản và một số thảo luận xung quanh bộ lọc Kalman như nguồn gốc

(derivation), mô tả (description) và một số thảo luận về bộ lọc Kalman mở rộng (Exteded Kalman Filter) đồng thời các ví dụ đơn giản với số liệu và kết quả thực cũng được đưa ra Bài báo là cơ sở lý thuyết để tính toán bộ lọc Kalman khi áp dụng các lý thuyết khác vào để nâng cao độ chính xác cũng như cải tiến các tính năng của bộ lọc

3D Orientation Tracking Based on Unscented Kalman Filtering of Accelerometer and Magnetometer Data - Benoıt Huyghe, Jan Doutreloigne

Bài báo trình bày về thuật toán ước lượng góc quay dựa trên tín hiệu ngõ ra của cảm biến gia tốc và cảm biến từ trường Quá trình ước lượng sử dụng Unscented Kalman Filter và kết hợp với một bộ lọc bổ phụ và tinh chỉnh các thông số để giảm ảnh hưởng của nhiễu chuyển động trên cảm biến Kết quả mô phỏng và thực nghiệm dựa trên chuyển động đơn và chuyển động lắc cho thấy bộ lọc đã giảm nhiễu tối đa từ 45.2° còn 6.4° với thời gian delay là 100ms Bài báo cho thấy việc sử dụng cảm biến từ trường và cảm biến gia tốc trong ước lượng góc quay cho kết quả khá tốt với sai số khi có nhiễu thấp Hiện tại kết quả bài báo sai số vẫn còn tương đối lớn nhưng cho tác giả hướng phát triển để đạt kết quả tốt hơn và kinh nghiệm trong việc kiểm nghiệm lại kết quả từ mô phỏng

Limitations of Attitude Estimation Algorithms for Inertial/Magnetic Sensor Modules - Eric R Bachmann, Xiaoping Yun and Anne Brumfield

Bài báo trình bày vai trò của cảm biến từ trường trong việc ước lượng góc quay khi kết hợp với các cảm biến quán tính khác Cảm biến từ trường giúp tăng độ chính xác và giảm nhiễu của các cảm biến khác Tuy nhiên bản thân cảm biến từ trường cũng rất dễ nhiễu trong môi trường có sự hiện diện của các thiết bị sử dụng điện hoặc từ các đối tượng được làm bằng kim loại màu Các thuật toán ước lượng giả định hướng từ trường địa phương là không đổi dẫn đến sai số trong việc ước lượng góc quay Bài báo phân tích những lý do dẫn đến sai số trong cảm biến từ và trong các thuật toán ước lượng

Dựa vào bài báo tác giả đã hiểu rõ những nguyên nhân dẫn đến sai số trong việc sử dụng cảm biến từ trường từ đó tìm phương pháp làm giảm sai số và kiểm nghiệm lại bằng thực nghiệm sẽ cho độ chính xác cao hơn

Hình 1.10 Modul cảm biến MARG III tích hợp cảm biến từ trường sử dụng trong bài báo [13]

3.2 Thuật toán dựa trên phương pháp Quaternion Implementation and Experimental Results of a Quaternion-Based Kalman Filter for Human Body Motion Tracking - Xiaoping Yun, Conrado Aparicio, Eric R

Bài báo trình bày hệ thống giám sát chuyển động con người sử dụng cảm biến MARG bằng phương pháp Quaternion dựa trên bộ lọc Kalman Kết quả thực nghiệm chỉ ra rằng bộ lọc Kalman được thiết kế mới đáp ứng tốt trong việc theo dõi chuyển động của cánh tay người trong thời gian thực trong mọi điều kiện Bài báo minh chứng cho việc sử dụng cảm biến MARG nâng cao được chất lượng trong việc ước lượng góc quay và với bộ lọc Kalman được nâng cao cho kết quả tốt hơn

Kalman Filtering for Real-Time Orientation Tracking of Handheld Microsurgical Instrument – Wei Tech Ang, Pradeep K.Khosia and Cameron

N.Reiviere Bài báo trình bày lý thuyết và mô hình hóa bộ lọc Kalman dựa trên phương pháp Quaternion sử dụng để ước lượng góc quay ứng dụng trong thiết bị vi giải phẫu Bộ lọc Kalman được đề xuất để ước lượng góc quay không trôi và tăng cường độ chính xác trong các chuyển động của thiết bị Thiết bị vi giải phẫu sử dụng hai nguồn thông tin cảm biến từ cảm biến từ trường và cảm biến gia tốc để gia tăng độ chính xác Bài báo minh chứng cho việc sử dụng cảm biến MARG trong việc ước lượng góc quay với bộ lọc Kalman được biến đổi cho độ chính xác cao hơn Qua bài báo tác giả thấy được những vấn đề khi kết hợp bộ lọc Kalman với những phương pháp khác cũng như những khó khăn trong việc giải quyết những vấn đề về nhiễu do cảm biến gây ra, cân bằng giữa độ chính xác mô hình hóa và hiệu quả tính toán, từ đó lựa chọn phương pháp để đạt được kết quả tốt nhất

An efficient orientation filter for inertial and inertial/magnetic sensor arrays -

Bài báo trình bày thuật toán ước lượng góc quay sử dụng cảm biến quán tính 6DOF và cảm biến từ trường MARG sử dụng phương pháp Quaternion Dựa vào việc sử dụng dữ liệu từ cảm biến gia tốc và cảm biến từ trường để phân tích tính toán và tối ưu đường dốc thuật toán từ đó tính toán hướng sai số của cảm biến vận tốc Kết quả sau khi mô phỏng và thực nghiệm bộ lọc đạt độ chính xác cao, nhỏ hơn 0.6° sai số RMS tĩnh, nhỏ hơn 0.8° sai số RMS động Cải thiện được hiệu suất tính toán khi sử dụng phương pháp Kalman thông thường và hiệu quả với tần số lấy mẫu thấp 10Hz Bài báo cho thấy hiệu quả việc ước lượng góc quay khi sử dụng cảm biến 9DOF cho độ chính xác cao hơn và hiệu suất tính toán tốt hơn so với bộ lọc Kalman thông thường Nhưng vẫn tồn tại hạn chế là chỉ hiệu quả với thời gian lấy mẫu thấp

An attitude error correction method based on MARG sensor array - Mingjian

Li, Chen Chen, Ning Han and Fei Yan Bài báo trình bày về phương pháp làm giảm sai số trong việc ước lượng góc quay khi sử dụng cảm biến MARG Phương pháp sử dụng ma trận Jacobian để thay thế cho đại lượng sử dụng trong công thức Extended Kalman Filter để làm giảm tối đa khối lượng tính toán Thuật toán yêu cầu tinh chỉnh hệ số độ lợi của bộ lọc để sai số đo từ cảm biến vận tốc là nhỏ nhất Từ kết quả mô phỏng, sau đó là so sánh với bộ lọc Extended Kalman Filter truyền thống cho thấy cả hai thuật toán đạt được độ chính xác gần như nhau nhưng thuật toán MARG có thể giảm thời gian hội tụ Khối lượng tính toán của bộ lọc Extended Kalman Filter nhiều hơn 7 lần so với thuật toán MARG nên thời gian tính toán của bộ lọc Extended Kalman Filter sẽ lớn hơn dẫn tới độ trễ cao và làm đáp ứng của hệ thống sẽ chậm hơn Với thời gian lấy mẫu thấp (10 Hz), sai số tính toán của thuật toán MARG cho độ chính xác cao với sai số nhỏ hơn 0.5° Bài báo cho tác giả thấy được điểm mạnh cũng như những điểm yếu của bộ lọc Extended Kalman Filter trong việc ước lượng góc nghiêng khi sử dụng cảm biến MARG và minh chứng rõ khi biến đổi các thông số của bộ lọc EKF sẽ cho kết quả tốt hơn Đây cũng là hướng đi của đề tài trong việc tăng độ chính xác của thuật toán ước lượng góc nghiêng và giảm những điểm bất lợi trong thuật toán trước đó

Hình 1.11 Lưu đồ giải thuật của thuật toán ước lượng góc nghiêng sử dụng cảm biến

A Smoother for Attitude Estimation Using Inertial and Magnetic Sensors -

Young Soo Suh, Tri Nhut Do, Young Sik Ro, and Hee Jun Kang Bài báo trình bày về thuật toán ước lượng góc nghiêng sử dụng cảm biến quán tính (cảm biến vận tốc quay ba trục, cảm biến gia tốc ba trục và cảm biến từ trường ba trục) thông qua bộ lọc Indirect Kalman được sử dụng như bộ lọc forward và backward Sau đó các thông số Quaternion về chuyển động quay được thuật toán trung bình Quaternion tính toán để đạt được độ chính xác cao nhất và sai số là nhỏ nhất Bài toán minh chứng cho việc sử dụng cảm biến MARG kết hợp với Kalman đã được biến đổi theo phương pháp khác cho kết quả tốt hơn là chỉ sử dụng cảm biến đo vận tốc quay và Kalman thông thường Với bộ lọc Kalman dựa trên phương pháp Quatenion cho độ chính xác cao nhưng khối lượng tính toán lớn dẫn đến thời gian trễ lớn Đây là điểm cần tham khảo để giúp cho đề tài tốt hơn và tránh được những hạn chế mà các phương pháp khác gặp phải

3.3 Thuật toán dựa trên phương pháp DCM An Improvement Strategy on Direction Cosine Matrix based Attitude Estimation for Multi-Rotor Autopilot - Nazia Ahsan Dilshad & Mohammed Kawser

Jahan Bài báo trình bày phương pháp giúp cải thiện việc ước lượng góc quay dựa trên phương pháp Direction Cosine Matrix (DCM) sử dụng cho mô hình bay Multicopters với dữ liệu được lấy từ khối cảm biến quán tính MEMS, cảm biến từ trường và cảm biến GPS thương mại, bằng cách sử dụng lý thuyết ước lượng góc nghiêng DCM, một lý thuyết mới được thiết kế cho phép đạt được độ chính xác cao hơn và giảm được thời gian trễ trong môi trường thời gian thực Từ kết quả thực nghiệm cho thấy thiết kế mới đáp ứng rất tốt với nhiễu hay với việc mất tín hiệu GPS Tuy nhiên độ chính xác đạt được chưa có do thời gian cập nhật dữ liệu của GPS còn thấp 5Hz Bài báo minh chứng rõ ràng cho việc ứng dụng DCM vào việc ước lượng góc nghiêng khi sử dụng cảm biến

MARG cho kết quả tốt với khối lượng tính toán thấp Đây cũng là lý thuyết tiền đề cho đề tài khi sử dụng DCM và với những thiếu sót trong thuật toán giúp tác giả mở ra hướng tìm hiểu mới cho đề tài

Hình 1.12 Thuật toán ước lượng góc nghiêng sử dụng cho mô hình bay Multicopters

Implementation of Direction Cosine Matrix on a PSoC-5 Microcontroller for Robot Localization on Inclined Terrains - Garth Herman, Aleksander Milshteyn, Airs

Lin, Manuel Garcia, Charles Liu, Khosrow Rad, Darrel Guillaume, Helen Boussalis Bài báo chứng minh một thuật toán sử dụng thuật toán Direction Cosine Matrix (DCM) trên hệ thống Cypress Programmable System sử dụng nền tảng là chip-5 (PSoC- 5) để có được thông tin về góc nghiêng, dữ liệu vị trí chính xác nhất cho robot bán tự hành Trên PsoC-5 tích hợp sẵn nhiều loại cảm biến như cảm biến vận tốc góc, gia tốc góc, cảm biến từ trường Từ kết quả thực nghiệm đã chứng mình thuật toán DCM cung cấp những giá trị đo chính xác và tăng độ tin cậy cho cảm biến từ trường ở nhiều góc nghiêng khác nhau Bài báo cũng chỉ ra hướng phát triển tiếp theo cho thuật toán chính là kết hợp với bộ lọc Kalman, kết hợp với cảm biến GPS để cải tiến thuật toán ước lượng góc nghiêng hay độ chính xác cho vị trí của robot Dựa vào bài báo, tác giả đã hiểu hơn về thuật toán DCM cũng như từ đó có được hướng đi đúng cho đề tài

A GPS/INS Integration System for Land Vehicle Application - Nguyen Ho

Quoc Phuong, Hee-Jun Kang, Young-Soo Suh, Young-Shick Ro,Kyu-Chan Lee Bài báo trình bày về việc phát triện và thực hiện hệ thống tích hợp GPS/INS sử dụng cho ứng dụng đỗ xe Bài báo trình bày thuật toán mới tính toán góc quay 3D cho xe Ước lượng góc Roll và Pitch sử dụng cảm biến vận tốc (gyrometers) và cảm biến gia tốc (accelerometers) và góc Yaw được tính từ dữ liệu GPS Những dữ liệu từ các cảm biến được xử lý thông qua bộ lọc Kalman tuyến tính dựa trên phương pháp sử dụng DCM (Direction Cosine Method) Kết quả thực tế cho thấy hệ thống hoạt động ổn định, giữ được độ chính xác mà không cần khởi động lại hệ thống Tuy nhiên sai số vẫn còn lớn trong việc ước lượng góc quay và vị trí nhưng bài báo cũng chỉ ra lý do dẫn đến những sai số đó và hướng phát triển tiếp theo để nâng cao độ chính xác cho hệ thống

Hướng nghiên cứu chính của luận văn

Trong luận văn này tác giả tập trung giải quyết mục tiêu: nghiên cứu thuật toán ước lượng góc nghiêng từ dữ liệu đo của khối cảm biến quán tính MARG ứng dụng trong điều khiển Quadrotor

1 Nghiên cứu xây dựng thuật toán ước lượng góc nghiêng 2 Đánh giá sai số thuật toán ước lượng trong môi trường mô phỏng 3 Đánh giá chất lượng thuật toán trong thực tế

4.2 Hướng giải quyết Để giải quyết các mục tiêu, tác giả tiến hành tìm hiểu những nghiên cứu về cách thức hoạt động của cảm biến MARG và phương thức kết hợp các cảm biến lại, lọc nhiễu để có được chất lượng tốt hơn, cũng như những nghiên cứu liên quan trong việc ước lượng góc quay khi sử dụng cảm biến MARG Từ đó phân tích, so sánh và phát triển thuật toán ước lượng góc cho ít nhiễu hơn và giảm sai số, so sánh kết quả đạt được với kết quả tham khảo để đánh giá chính xác hơn

Hướng giải quyết mục tiêu 1: Sau khi tham khảo nhiều nghiên cứu trước đó về ước lượng góc quay, tác giả dự định xây dựng thuật toán ước lượng góc quay sử dụng bộ lọc

Kalman dựa trên phương pháp Direction Cosine Method (DCM) Vì đây là phương pháp đang được nghiên cứu nhiều gần đây và có nhiều ưu điểm hơn những phương pháp khác, như khả năng lọc nhiễu tốt, sai số nhỏ và khả năng ứng dụng vào thực tế cho việc điều khiển Quadrotor cao

Hướng giải quyết mục tiêu 2: Tác giả dự định sử dụng công cụ Simulink/Matlab để mô phỏng bộ cảm biến MARG, với việc mô phỏng được cảm biến MARG tác giả có thể giả lập các trường hợp hoạt động của cảm biến cũng như giả lập nhiễu để có được một cách tổng quát nhất để đánh giá thuật toán Tác giả sẽ mô phỏng thuật toán ước lượng góc quay đã xây dựng trên Matlab và sử dụng dữ liệu từ bộ cảm biến MARG được mô phỏng để ước lượng góc quay, sau đó sẽ so sánh với góc quay thực tế từ cảm biến MARG mô phỏng để tính toán sai số của thuật toán

Hướng giải quyết mục tiêu 3: Tác giả dự định sử dụng bộ cảm biến MARG thực tế là sự kết hợp của các loại cảm biến và chip vi xử lý Tiến hành đọc được dữ liệu từ cảm biến và đưa về Matlab đề xử lý Với thuật toán được xây dựng và mô phỏng trong Matlab, tác giả sẽ ước lượng góc quay từ dữ liệu đưa về để có được góc quay ước lượng Để đánh giá được sai số của thuật toán, tác giả sử dụng một cảm biến đo góc cho độ chính xác cao với ngõ ra là analog để có được góc quay thực tế làm chuẩn Từ đó tác giả sẽ tính toán được sai số của thuật toán trong điều kiện thực tế và đánh giá được sai số của thuật toán Ngoài ra tác giả còn dự định xây dựng mô hình bay Quadrotor để có được cái nhìn tổng quát hơn trong việc sử dụng bộ cảm biến MARG trong điều khiển Quadrotor bởi các chức năng bay tự động của Quadrotor sử dụng rất nhiều dữ liệu từ cảm biến MARG Tác giả sẽ đặt board cảm biến MARG và cảm biến góc lên trên mô hình Quadrotor để đo đạc và tính toán được sai số trong khi bay với nhiễu thực tế từ mô hình bay và môi trường xung quanh

Mục tiêu mong muốn của luận văn: Thiết kế thành công thuật toán ước lượng góc quay từ dữ liệu đo của khối cảm biến quán tính MARG sử dụng bộ lọc Kalman dựa trên phương pháp DCM, sai số tối đa không vượt quá 3° và sai số trung bình không vượt quá 2°.

CƠ SỞ LÝ THUYẾT

Bộ đo quán tính IMU

Thuật ngữ Gyroscope tức con quay hồi chuyển xuất hiện từ giữa thế kỉ XIX, và trong những thập niên gần đây nó được sử dụng rộng rãi và được thay thế trên toàn cầu với từ Gyro Những lý thuyết ban đầu về con quay hồi chuyển được áp dụng để giải thích về chuyển động của một vật thể quay như Trái đất và dần dần Gyro đã được phát triển và ứng dụng rộng rãi trong nhiều lĩnh vực, và đặc biệt là trong hệ thống định vị quán tính INS Có thể chia Gyro thành 3 loại với nguyên lý hoạt động, cấu tạo và khả năng ứng dụng khác nhau như sau: Gyro cơ, Gyro quang, và Gyro điện Khởi đầu trong sự phát triển là Gyro cơ, hoạt động dựa trên nguyên lý con quay hồi chuyển truyền thống Gyro cơ được sử dụng nhiều trong hệ thống Gimbaled INS Sau đó là sự ra đời của Gyro quang học, và gần đây nhất là sự phát triển của các loại Gyro điện ứng dụng bởi công nghệ vi cơ điện tử MEMS (micro-electromechanical systems), Gyro quang và Gyro điện ứng dụng nhiều trong Strapdown INS

Một gia tốc kế sử dụng quán tính của một vật để đo sự thay đổi giữa gia tốc động học trong không gian quán tính so với gia tốc trọng trường

Gia tốc kế dạng con lắc có cấu tạo khá giống như Gyro rung, gồm 1 vật nặng (proof mass) được treo bởi 1 lò xo Vật nặng có thể chuyển động dọc theo lò xo Con lắc được đặt vào môi trường giảm chấn để hạn chế ảnh hưởng của rung động Hình sau minh họa cho cấu tạo 1 gia tốc kế con lắc

Hình 2.1 Cấu tạo gia tốc kế con lắc [23]

Nếu như có 1 gia tốc thì lò xo sẽ biến dạng Dựa trên độ biến dạng của lò xo mà ta có thể tính được gia tốc của hệ thống Đó là nguyên lý hoạt động của gia tốc kế con lắc

Hiện nay, người ta thường dùng gia tốc kế vòng kín, là một loại gia tốc kế có khả năng làm việc tốt hơn hẳn gia tốc kế dạng con lắc và hầu như không có thành phần nào di chuyển, nhờ gắn thêm một cuộn dây bên ngoài proof mass Nguyên lý hoạt động của nó là khi có dịch chuyển nhỏ của proof mass thì sẽ sinh ra 1 dòng điện trong cuộn dây, tạo một lực điện từ theo chiều ngược lại để khử đi chuyển động Do đó, có thể suy ra được gia tốc chuyển động của hệ thống bằng cách đo dòng điện chạy trong cuộn dây

Với sự phát triển của công nghệ MEMS (Microelectromechanical systems) thì các loại gia tốc kế được sản xuất hàng loạt với giá thành thấp và chất lượng khá tốt

Cảm biến từ trường là một thiết bị dùng để đo cường độ và hướng của từ trường (được tạo ra trong phòng thí nghiệm hoặc từ trường Trái đất) Đơn vị trong hệ thống đo lường quốc tế là Tesla Nhưng đây là đơn vị khá lớn nên người ta dùng đơn vị nanotesla (nT) Trong thực tế thì ta thường đo từ trường với đơn vị là Gauss

1 Gauss = 100000 (nT ) Từ trường trái đất là một trường thế Nó thay đổi theo rất nhiều nguyên nhân khác nhau Từ trường trái đất có cường độ khoảng 20000 nT ở xích đạo và khoảng 80000nT ở gần các cực

Cảm biến từ trường được chia ra làm 2 loại chính là:

Scalar magnetometer: Đo tổng cường độ từ trường

Vector magnetometer: Đo từng thành phần của từ trường theo các trục, liên quan đến khả năng định hướng của thiết bị

1.2 Các tính toán xử lý dữ liệu sơ bộ từ các cảm biến

Trong các tính toán dưới đây ta điều giả sử rằng vật gắn thiết bị có tọa độ tuyệt đối là Xb’Yb’Zb’, trong khi tọa độ của cảm biến (tọa độ tương đối) là XbYbZb:

Mối quan hệ giữa hệ trục tọa độ tuyệt đối và hệ trục tọa độ tương đối được xác định như sau với ma trận xoay R xyz :

' cos cos cos sin sin sin cos sin sin cos sin cos ' cos sin cos cos sin sin sin sin cos cos sin sin sin sin cos cos cos

1.2.1 Xử lý dữ liệu tử cảm biến vận tốc góc (Gyro)

Việc tính toán số liệu từ Gyro là khá phức tạp vì nó cho ra dữ liệu là vận tốc góc nên để xác định được góc quay thì chúng ta phải tích phân theo thời gian

Các phương trình dùng để tính vận tốc thay đổi (vi phân) góc Euler từ Gyro như sau:

1 0 cos cos sin cos os cos sin sin cos cos x y z t t c t

Với Ψ, θ, Φ lần lượt là góc Euler tại thời điểm lấy mẫu trước đó (t-1) Góc Euler tại thời điểm hiện tại được xác định như sau:

1.2.2 Xử lý dữ liệu từ cảm biến trọng trường (Accellerometer)

Hình 2.2 Tính toán góc góc nghiêng (tilt) từ Accelerometer [23]

Từ accelerometer ta có thể xác định được góc nghiêng của hệ thống bằng cách xác định góc Roll và Pitch

Trong hệ tọa độ tương đối X b , Y , b Z b ta có các giá trị trong trường hợp cảm biến đặt vuông góc với phương của trọng lực như sau:

Nhưng trong hệ trục tọa độ tuyệt đối ta sẽ đo được các giá trị là A A A x , y , z , các giá trị này đã được chuẩn hóa (normalize)

Thay vào phương trình 2.1 ta được: cos cos cos cos sin 0 cos sin sin cos sin cos cos sin sin sin cos sin 0 cos sin cos sin sin sin cos sin cos sin cos cos 1 x y z

Từ đây ta có thể suy ra:

Chú ý rằng A A A x , y , z đã được chuẩn hóa, nghĩa là

Nếu biểu thức này không thỏa thì cảm biến là không tuyến tính hoặc có sai số trong việc calib

1.3 Các nguyên nhân có thể dẫn đến sai số hệ thống

Có rất nhiều nguyên nhân dẫn đến sai số hệ thống như:

- Các giá trị offset của cảm biến được cài đặt không đúng với giá trị thực tế

- Việc hàn IC lên PCB không đảm bảo các trục tọa độ vuông góc nhau Mỗi cảm biến sẽ có một hệ trục tọa độ riêng, dẫn đến không đồng nhất cùng một hệ trục tọa độ

- Việc lắp đặt thiết bị (IMU hay MARG) lên hệ thống cần đo cũng có thể dẫn đến sai lệch do hệ trục tọa độ của IMU hay MARG không trùng với hệ trục tọa độ của hệ thống cần đo

- Một nguyên nhân nữa là vấn đề nhiễu Nếu chúng ta sử dụng cảm biến mà đầu ra là tương tự thì vấn đề nhiễu là hoàn toàn có thể xảy ra và khắc phục cũng rất khó khăn

Nếu sử dụng các cảm biến có ngõ ra là tín hiệu số thì có thể tránh được vấn đề nhiễu trên dây dẫn tín hiệu Nhưng vẫn còn một loại nhiễu khác là do môi trường Bởi vì hệ thống chúng ta sử dụng từ trường và trọng trường như là hệ trục tham chiếu nên nếu có những yếu tố làm thay đổi hai thành phần này Khi đó hệ thống sẽ không còn chính xác nữa

Vấn đề sai số của từng loại cảm biến sẽ được đề cập như sau:

1.3.1 Sai số của cảm biến vận tốc góc

Sai số chủ yếu của cảm biến vận tốc góc là giá trị phân cực hay là giá trị offset hay là điều kiện đầu

Từ công thức (2.3) viết lại:

Các phương pháp mô tả phương hướng và thuật toán ước lượng góc quay trong không gian

Các góc Euler là 3 góc được định nghĩa bởi Leonhard Euler để xác định hướng của một đối tượng Để xác định hướng trong không gian Euclide 3 chiều, 3 tham số được đòi hỏi Chúng có thể được chọn theo nhiều cách khác nhau, và các góc Euler là một trong số đó

Các góc Euler thay thế cho ba chuyển động quay kết hợp, di chuyển hệ trục tham chiếu đến một hệ trục ta đang xét Hay nói một cách khác, bất kì một hướng nào trong không gian Euclide 3 chiều cũng có thể được xác định bằng sự kết hợp của 3 chuyển động xoay thành phần (chuyển động xoay quanh một trục cơ bản), và tương tự như thế, ma trận xoay từ hệ trục cố định tham chiếu đến hệ trục ta đang xét cũng có thể được phân tích thành 3 ma trận xoay thành phần

Không tính đến việc xét dấu của chuyển động quay cũng như việc di chuyển các hệ trục tham chiếu, có tất cả 12 quy ước khác nhau trong việc kết hợp chuyển động quay, từ đó là các quy ước về góc khác nhau Một trong số chúng được gọi là góc Euler chính xác Số còn lại được gọi là góc Tait-Bryan Đôi lúc chúng đều được gọi chung là góc Euler

2.1.1 Góc Tait–Bryan (Góc Euler) Đường cơ sở là giao giữa hai mặt phẳng XY và YZ;

Trường hợp này góc Euler tuân theo quy luật zyx, nghĩa là khi quay hệ trục tọa độ tuyệt đối theo trục z ta được góc Ψ, tiếp tục quay theo trục y ta được góc φ và tiếp tục quay theo trục x ta được góc θ

Hình 2.3 Góc Tait-Bryan (đương cơ sở y’ được kí hiệu màu vàng) [23]

Có thể xác định góc Tait-Bryan dựa vào hình 2.4 như sau:

- Góc φ là góc giữa đường cơ sở và trục X

- Góc θ là góc giữa đường cơ sở và trục Y

- Góc Ψ là góc giữa trục y và đường cơ sở

Trong thực tế người ta thường ứng dụng Góc Tait- Bryan xác định thuộc tính và định hướng của hệ thống INS, cụ thể là xác định vị trí và phương hướng của máy bay, đôi khi được gọi là hệ trục tọa độ hàng không (Aircraft convention)

Hình 2.4 Ứng dụng Góc Tait - Bryan trong hàng không [24]

Roll là góc quay quanh trục x (trục dọc thân máy bay) Pitch là góc quay quanh trục y (trục dọc cánh máy bay) Heading (Yaw) là góc quay quanh trục z (trục song song với trọng lực)

Hình 2.5 Hiện tượng Gimbal khi Pitch bằng 90 độ [23]

Gimbal Lock là hiện tượng mất một bậc tự do trong không gian 3 chiều khi 2 trong 3 trục trùng nhau (hoặc song song nhau) dẫn đến hệ thống chỉ quay trong không gian 2 chiều

Giả sử trong trường hợp góc Tait-Bryan: nếu góc Pitch bằng 90 độ khi đó theo thứ tự ta có trục X sẽ trùng với trục Z sau khi thực hiện xong góc quay quanh trục Y, khi đó khi quay quanh trục X thì sẽ giống như quay quanh trục Z Vật chỉ quay được quanh trục Y và Z mà thôi

Như vậy các góc Picth và Roll sẽ phải bị giới hạn trong tầm từ (-π/2 , π/2)

2.1.3 Thuật toán ước lượng góc dựa trên phương pháp Euler

Ta có phương trình động học của Quadrotor:

Từ phương trình trên ta thiết lập mô hình quá trình cho bộ lọc Kalman như sau:

Phương trình trạng thái của hệ thống:

Trong đó biến trạng thái ( ) b x b y b z x t

  là giá trị vận tốc góc lấy từ cảm biến gyrometer

Ma trận chuyển đổi trạng thái:

Mô hình đo lường sử dụng dữ liệu từ cảm biến gia tốc sin sin cos x y a g a g

 là thành phần gia tốc lấy từ cảm biến accelerormeter Áp dụng theo thuật toán Extended Kalman Filter ta sẽ có thuật toán ước lượng góc nghiêng dựa trên phương pháp Euler

Trong thuật toán góc nghiêng được ước lượng trực tiếp theo công thức , hai dòng trên cùng của ma trận chuyển đổi được cập nhật chỉ với hai biến , Với cách thức tính toán trên thuật toán sẽ rất dễ áp dụng với công thức đơn giản Khối lượng tính toán sẽ rất nhỏ nên khả nắng đáp ứng rất nhanh

Tuy nhiên, từ mối quan hệ của các biến và thành phần ma trận ta thấy khi góc Pitch tiến đến giá trị / 2, sẽ xuất hiện điểm kì dị (tan , sec     ) hay còn gọi là hiện tượng Gimbal Đây chính là nhược điểm của phương pháp Euler

Ngoài ra theo kết quả thực nghiệm của các bài báo thuật toán lọc nhiễu chưa tốt dẫn đến sai số lớn Vì vậy cần tìm thuật toán với khả năng lọc nhiểu tốt hơn và hạn chế điểm kì dị

Như đã đề cập ở trên, phương pháp biểu diễn theo góc Euler sẽ xảy ra hiện tượng Gimbal Lock nên người ta đã nghĩ đến giải pháp không gian 4 chiều Chính là hệ tọa độ Quaternion Đây là hệ thống số được phát triển mở rộng từ hệ thống số phức Nó được định nghĩa lần đầu tiên bởi nhà toán học người Ai-len Sir William Rowan Hamilton vào năm 1843 và được ứng dụng để phân tích không gian 3 chiều

Một Quaternion đơn vị có thể được định nghĩa như sau:

Chúng ta có thể kết hợp Quaternion với một chuyển động quay quanh một trục như sau:

0 1 2 3 cos( / 2) sin( / 2) cos( ) sin( / 2) cos( ) sin( / 2) cos( ) x y z q q q q

Trong các công thức trên, là góc quay (giá trị tính bằng radian của góc quay) và cos( x ), cos( y ), cos( z ) là các “cosine định hướng” của trục quay Khi đó ma trận quay của chuyển động quay này trong không gian 3 chiều Euclide được xác định:

2.2.2 Chuyển đổi từ ma trận xoay chuyển sang góc Euler

Giả sử ta có một ma trận xoay biết trước là:

Từ phương trình (2.1) và phương trình (2.12) đồng nhất 2 hệ số của 2 ma trận ta được: a 13 = -sinθ; tanΦ = a 32 / a 33; tanΨ = a 21 / a 11 :

Hình 2.6 Bảng tính toán giá trị góc Euler từ ma trận xoay 2.2.3 Thuật toán ước lượng góc dựa trên phương pháp Quaternion

Thuật toán ước lượng dựa trên phương pháp Quaternion sử dụng dữ liệu từ cảm biến vận tốc góc trong mô hình quá trình Dữ liệu từ cảm biến gia tốc đo véc tơ trọng trường và cảm biến la bàn đo véc tơ từ trường trái đất được sử dụng như mô hình đo lường

Véc tơ Quaternion q  [ e T , q 4 ] T , với e  [ q q q 1 2 3 ] T Ta có phương trình vi phân của Quaternion sử dụng giá trị vận tốc góc: q   q (2.20)

Khai triển phương trình trên và chuyển đổi mối quan hệ giữa các thành phần quaternion và vận tốc góc ta có:

  là giá trị vận tốc góc lấy từ cảm biến gyrometer

Từ phương trình vi phân trên, ta thiết lập mô hình quá trình cho bộ lọc Extended Kalman Filter như sau:

     (2.22) Ở đây, mô hình quá trình có 7 thành phần, 4 thành phần quaternion và 3 thành phần vận tốc góc

THUẬT TOÁN ƯỚC LƯỢNG GÓC NGHIÊNG

Hệ thống tham chiếu góc nghiêng theo mô hình DCM (Direct Cosine Matrix)

Trong phần này tác giả sẽ trình bày hệ thống tham chiếu góc nghiêng AHRS (Attitude and Heading Reference Systems) và thuật toán DCM để ước lượng góc nghiêng Mô hình AHRS được xây dựng trên cơ sở cảm biến MARG (cảm biến vận tốc góc, gia tốc và từ trường)

Gọi 𝐵 𝐸 𝑅 là ma trận DCM từ hệ tọa độ {B} (hệ quy chiếu trực giao riêng của cảm biến) đến hệ tọa độ {E} (hệ quy chiếu quán tính của trái đất) Ma trận chuyển đổi từ hệ tọa độ {B} đến hệ tọa độ {E} là:

Trong đó R R R Z  Y   X lần lượt là các ma trận góc xoay quanh trục z, y, x theo các góc tương ứng Yaw(𝜓), Pitch(𝜃), Roll(𝜑) theo phương pháp góc Tait-Bryan (Euler) cos sin 0 sin cos 0

Ma trận được viết lại như sau: cos sin 0 cos 0 sin 1 0 0 sin cos 0 0 1 0 0 cos sin

      cos cos cos sin sin sin cos sin sin cos sin cos cos sin cos cos sin sin sin sin cos cos sin sin sin sin cos cos cos

Từ phương trình 3.5, góc Euler có thể được tính toán:

Vì vậy chúng ta ước lượng các thành phần của ma trận DCM thay vì ước lượng trực tiếp góc Euler Nhưng hiện tại nếu ước lượng cả 9 thành phần của ma trận DCM sẽ làm khối lượng tính toán rất lớn, ảnh hưởng đến tốc độ xử lý của thuật toán Trên thực tế, chúng ta chỉ cần ước lượng hai dòng của ma trận DCM Dòng còn lại của ma trận DCM được tính toán từ hai dòng ước lượng trước đó dựa vào tính chất trực giao của ma trận DCM.

Xác định góc định hướng Yaw

Từ trường của Trái đất đặc trưng bởi véc tơ từ trường gồm hướng và cường độ của nó tại một vùng nào đó trên Trái đất Cường độ từ trong mặt phẳng nằm ngang, là tổng của 2 thành phần véc tơ X và Y Độ lệch của hướng bắc cực từ so với hướng bắc địa lý được gọi là độ lệch từ hay góc lệch từ (Declination), ký hiệu là D (xem Hình 3.1 và Hình 3.2) Góc giữa cường độ từ trong mặt phẳng ngang H với tổng cường độ từ F, gọi là góc nghiêng hay góc từ khuynh (Inclination), ký hiệu là I Giá trị của góc lệch từ D và góc nghiêng từ khuynh I tại một vùng nào đó trên Trái đất có thể xác định bởi mô hình WMM 2010 (World Magnetic Model 2010) khi ta biết vĩ độ (Latitude) và kinh độ (Longtitude) của nó Khi tham khảo mô hình WMM, ta thấy từ trường không chỉ thay đổi theo vị trí mà còn thay đổi theo thời gian qua từng năm

Chúng ta sẽ xác định hướng lệch trong mặt phẳng ngang bằng dữ liệu từ cảm biến từ trường và các góc nghiêng Roll và Pitch

Hình 3.1 Hướng lệch của từ trường trái đất

Gọi hệ tọa độ từ trường là {M} Hệ tọa độ {M} xác định bằng cách xoay hệ tọa độ {E} quanh trục Z một góc là D Góc lệch giữa B X h và hướng Magnetic North là  , góc lệch giữa B X h và hướng Bắc (North) là , trong đó B X h là hình chiếu vuông góc của hướng B X lên mặt phẳng nằm ngang Lúc này, góc Yaw sẽ bằng:

Hướng Bắc Hướng Bắc từ trường

Ma trận DCM chuyển từ hệ tọa độ {B} sang hệ tọa độ {M} được xác định,

Véc tơ từ trường trong hệ tọa độ {M} là M m có dạng:

Hình 3.2 Biểu diễn góc lệch của từ trường so với hướng Bắc của trái đất

Véc tơ từ trường đo được từ cảm biến la bàn trong hệ tọa độ {B} là:

Với B m x , B m y , B m z là các thành phần từ tính trong hệ trục tọa độ {B} được đo từ cảm biến la bàn

Mối quan hệ giữa vector B m và M m xác định bởi ma trận DCM M B R :

B Z Y X Z Y X m  R m  R   R R   m  R    m  R R   m (3.12) Khai triển ma trận trên ta được: cos sin 0 cos 0 sin 1 0 0 sin cos 0 0 0 1 0 0 cos sin

          cos cos sin sin sin cos sin cos sin sin sin cos cos cos

Từ phương trình trên ta có thể suy ra: cos sin cos sin sin sin cos

Từ công thức (3.9) ở trên kết hợp với công thức (3.14) ta sẽ có được trạng thái của góc định hướng Yaw hoàn toàn

Hướng Bắc Hướng Bắc từ trường

Thiết kế bộ lọc Kalman dựa trên phương pháp DCM

Như đã xác định ở trên, mục đích cần thực hiện là tính toán các thành phần của hai dòng dưới ma trận DCM Từ dữ liệu đo được của cảm biến MARG và sử dụng bộ lọc Kalman, ta có thể kết hợp các dữ liệu cảm biến lại để tính toán các thành phần đó Để giảm khối lượng tính toán và tăng khả năng ứng dụng của thuật toán, ta sẽ sử dụng hai bộ lọc Kalman (gọi là Kalman_DCM 1 và Kalman_DCM 2) để lần lượt tính toán cho từng dòng ma trận DCM Dựa vào mối quan hệ giữa các thành phần DCM, thành phần cảm biến MARG và thành phần góc ước lượng ngõ ra để thiết lập thuật toán Ước lượng trạng thái góc nghiêng liên quan đến việc xác định góc Roll và góc Pitch bằng cách sử dụng cảm biến vận tốc góc và gia tốc Ước lượng góc Yaw sử dụng dữ liệu từ cảm biến la bàn Vì vậy dữ liệu từ cảm biến vận tốc góc xem như mô hình đối tượng, dữ liệu từ cảm biến gia tốc và từ trường là mô hình đo lường Phương trình vi phân ma trận DCM:

Với B     B  x B  y B  z   T là giá trị vận tốc góc được đo từ cảm biến Gyro trong hệ tọa độ {B} và

Phương trình (3.15) được viết lại :

Như đã giải thích ở trên, ta chọn hai dòng dưới của ma trận DCM để tiến hành ước lượng:

Dòng đầu tiên lựa chọn là dòng thứ ba là mô hình đối tượng cho bộ lọc Kalman thứ nhất vì mối quan hệ của các thành phần DCM dòng thứ ba và các góc Roll, Pitch theo công thức (3.6), (3.7)

Dòng thứ hai được sử dụng làm mô hình đối tượng cho hai bộ lọc Kalman thứ hai

Tiếp theo ta sẽ tìm phương trình đo lường cho hệ thống Ở đây các giá trị từ cảm biến gia tốc trọng trường và cảm biến từ trường được sử dụng làm mô hình đo lường cho hệ thống Đối với mô hình đo lường thứ nhất, ta sử dụng véc tơ gia tốc thẳng được xây dựng như sau:

Trong phương trình này, B a x , B a y , B a z là ba thành phần của véc tơ gia tốc được lấy từ cảm biến gia tốc trọng trường trong hệ trục tọa độ {B}, g là gia tốc trọng trường,

21 22 23 sin cos sin sin sin cos cos cos sin sin sin cos

(3.21) Được sử dụng là ma trận đo lường thứ hai, sử dụng véc tơ từ trường đo từ cảm biến la bàn và góc 𝜙, 𝜃 là góc Roll và Pitch được ước lượng từ bước Kalman thứ nhất, ta sẽ tính được góc Yaw theo công thức (3.14), lúc này ta có đủ các thành phần cho ma trận

Sử dụng Kalman dựa trên phương pháp DCM cho phép kết hợp các giá trị cảm biến với nhau để ước lượng các góc nghiêng từ đó sẽ hạn chế được nhiễu trên từng cảm biến

Bộ lọc Kalman tăng tính đáp ứng trước sự thay đổi vận tốc và gia tốc của các góc

Dựa vào các phương trình hệ thống và lưu đồ thuật toán Kalman mở rộng ta thiết kế thuật toán ước lượng theo sơ đồ và các hệ số được tính toán như sau:

Hình 3.3 Sơ đồ mô tả thuật toán ước lượng

3.2 Tính toán thành phần cho bộ lọc Kalman mở rộng Áp dụng các mối quan hệ ở trên vào thiết kế bộ lọc Kalman mở rộng, bộ ước lượng hướng và góc nghiêng dựa trên phương pháp DCM được chia thành 2 bước kế tiếp nhau, bước đầu tiên ước lượng trạng thái góc nghiêng (Attitude) và sau đó là ước lượng hướng (Heading) Từng bước tính toán và thiết lập thuật toán sẽ được lần lượt trình bày sau đây

3.2.1 Rời rạc và tuyến tính phương trình động học Đầu tiên ta cần tuyến tính phương trình động học và chuyển sang miền rời rạc để đưa vào bộ lọc Kalman Bởi vì phương trình động học hiện tại là phi tuyến và liên tục trong miền thời gian

Từ phương trình vi phân ma trận DCM chuyển sang miền rời rạc và sử dụng kí hiệu Φ 𝑘 có dạng:

 là ma trận chuyển đổi từ tk tới tk+1 [25]

Gọi Δ𝑇 là khoảng thời gian lấy mẫu Giả sử với Δ𝑇 đủ nhỏ để [𝜔] là hằng số trong khoảng thời gian từ tk tới tk+1 Khi đó Φ k e    Δ T

Tiến hành xấp xỉ theo hàm bậc một ta có ma trận Φ 𝑘 :

DCM, Ψ Đối với phương trình đo lường thứ nhất (3.20) và phương trình đo lường thứ hai (3.21) đã là tuyến tính nên ta không cần qua quá trình tuyến tính

3.2.2 Bước tính toán trong thuật toán Kalman_DCM Áp dụng các mô hình đối tượng và mô hình đo lường cho bộ lọc Kalman mở rộng theo công thức sau:

Hình 3.4 Lưu đồ thuật toán Kalman mở rộng

 Bước 0: Khởi tạo mô hình hệ thống rời rạc hoá theo thời gian DTS

Từ mô hình hệ thống rời rạc hoá theo thời gian, ta tính toán các tham số

 Bước 1: Tính toán dự báo trạng thái, xác định x 0 và P 0

Mô hình DTS(Discrete-Time System Model)

Tính toán dự báo trạng thái (State Predictition) và

Tính hệ số khuếch đại lọc (Kalman Gain)

Cập nhật trạng thái ước đoán (State

Cập nhật ma trận hiệp biến (Error

5 x 0 là số ước lượng của trạng thái ban đầu Đây là giá trị xác định phù hợp nhất của hệ thống khi bắt đầu tính toán lọc Kalman ( t k  0)

P 0 là ma trận hiệp phương sai tại thời điểm ban đầu, chỉ ra sự tin cậy trong đánh giá ban đầu Nếu chắc chắn rằng ta ước lượng trạng thái đầu chính xác, P 0 có thể có giá trị rất nhỏ Thông thường để đơn giản trong tính toán ta lấy trạng thái đầu là mô hình đứng yên, tức là trạng thái đầu x 0 tại ( t k  0) có P 0  0

 Bước 2: Dự báo trạng thái tiếp theo (tk) từ trạng thái trước (tk-1) Tại trạng thái đầu t0 = 0 giả sử mô hình đang đứng yên, ta có x  x 0 , P  P 0 Khi tính toán tại một bước bất kỳ, ta sử dụng trạng thái đánh giá x k  1 và ma trận hiệp biến

P k  kết hợp với trạng thái  k 1 và Q k  1 , ta tính toán được giá trị tại bước tiếp theo x k  và

- Giá trị x k  này sẽ được thay vào công thức ở khung 4 để tính giá trị x k

- Giá trị P k  này sẽ được thay vào công thức ở khung 3 để tính giá trị K k

 Bước 3: Tính hệ số khuếch đại lọc K k

Hệ số khuyếch đại lọc K k , còn gọi là độ lợi cần tìm của mạch lọc Kalman trong mỗi ước đoán được tính toán từ các giá trị P k  , H k và R k theo công thức ở khung 0 và khung

 Bước 4: Đánh giá trạng thái

Tại thời điểm k ta nhận được giá trị đo z k từ sensor Kết hợp với trạng thái dự báo

 x k và H k ta tính toán được trạng thái đánh giá x k

 Bước 5: Tính toán ma trận hiệp phương sai của bước đánh giá P k theo công thức ở Khung 5 Lặp lại từ bước 2

Từ các phương trình mô tả hệ thống và lưu đồ thuật toán Kalman, bước tiếp theo ta cần tính các hệ số cho 2 bộ lọc theo lưu đồ thuật toán hình 3.3

3.2.3 Hệ số cho bộ lọc Kalman_DCM1

Mô hình đối tượng và mô hình đo lường của bộ lọc Kalman thứ 1:

  là véc tơ biến trạng thái 3x1 (3.26)

  là véc tơ đo lường 3x1 (3.27)

Ma trận chuyển trạng thái và ma trận độ nhạy đo lường được cho bởi:

H k  gI (3.29) Δ𝑇 : là thời gian lấy mẫu Nhiễu hiệp phương sai của mô hình đối tượng và mô hình đo lường 1 w k , 1 v k được tính từ phương trình sau:

R E v v r I (3.31) Ở đây, giá trị 𝛼 1 được chọn để duy trì độ phẳng mượt của đáp ứng ngõ ra; 𝑟 𝑎 sẽ được chọn tùy theo mức độ ảnh hưởng nhiễu của môi trường (thường theo kinh nghiệm)

KẾT QUẢ MÔ PHỎNG

Mô phỏng khối cảm biến IMU (cảm biến MARG)

Để mô phỏng cho thuật toán, ta cần mô phỏng khối IMU để có được dữ liệu lấy ra gần giống với khối IMU thực tế nhất Dữ liệu lấy ra gồm có tín hiệu gia tốc từ cảm biến gia tốc, tín hiệu vận tốc từ cảm biến vận tốc, tín hiệu từ trường lấy từ cảm biến la bàn

Tất cả giá trị đo mô phỏng đều được cộng thêm nhiễu như là: nhiễu cảm biến, sai lệch trạng thái Đầu tiên sử dụng khối quán tính 6DOF trong Matlab/Simulink Khối quán tính 6DOF nhận biết sự thay đổi về góc của hệ trục tọa độ trực giao cảm biến {B} so với hệ trục tọa độ mặt đất {E} Bằng việc đưa vào các thành phần lực và momen là tín hiệu ngõ vào block, ta sẽ có được thông số ngõ ra là các góc Roll, Pitch, Yaw và các thông số để đưa vào block IMU (vận tốc góc trong hệ trục tọa độ {B}, gia tốc góc trong hệ trục tọa độ {B}, vận tốc dài và gia tốc trong hệ trục tọa độ {B}, ma trận DCM trong hệ tọa độ {E}

Tiếp theo, mô phỏng khối cảm biến MARG bao gồm cảm biến vận tốc, cảm biến gia tốc và cảm biến la bàn Khối được mô phỏng từ khối cảm biến Three-Axis Inertial Measurement Unit trong Matlab Aerospace Blockset

Giá trị đo khi chưa cộng nhiễu của khối IMU là: ea [ ( ) ]

Với giá trị gia tốc tuyệt đối bao gồm gia tốc trong hệ trục máy bay tại trọng tâm, các ảnh hưởng lên gia tốc khi trọng tâm không trùng với tâm quay và véc tơ gia tốc trọng trường Tất cả được chuyển lên hệ trục tọa độ {B} bằng cách nhân với ma trận xoay DCM Để mô phỏng cảm biến từ trường, giả sửa ta có véc tơ từ trường khởi tạo ban đầu trùng với hệ trục tọa độ trái đất (NED) Khi đó véc tơ từ trường được tính toán theo công thức: a

Ta thiết lập các giá trị nhiễu cho các cảm biến để mô phỏng giá trị cảm biến thu được

Với dữ liệu lấy từ khối IMU, ta có giá trị đo mô phỏng giá trị lấy từ các cảm biến, góc Roll, Pitch, Yaw chuẩn Áp đụng thuật toán đã được trình bày ở trên, chúng ta sẽ đánh giá được hiệu quả của thuật toán

Hình 4.3 Mô phỏng bộ cảm biến IMU Để đánh giá được chính xác thuật toán, ta sử dụng sai số trung bình 𝑒 𝑅𝑀𝑆 và sai số lớn nhất 𝑒 𝑀𝑎𝑥 để quan sát được giá trị sai số một cách rõ ràng nhất giúp đánh giá một cách chính xác hơn:

Kết quả mô phỏng

Trong trường hợp này, cảm biến MARG được đặt trong mặt phẳng cố định và dữ liệu từ các cảm biến được cộng thêm nhiễu Gauss để đánh giá được độ chính xác bộ lọc trong điều kiện tĩnh

Hình 4.4 Dữ liệu lấy từ cảm biến cảm biến MARG mô phỏng (a) dữ liệu vận tốc (gyroscope); (b) dữ liệu cảm biến gia tốc (Accelerometer);(c) dữ liệu cảm biến la bàn

Hình 4.5 Góc Roll sau khi qua ước lượng và được so sánh với góc Roll thực tế

Hình 4.6 Góc Pitch sau khi qua ước lượng và được so sánh với góc Pitch thực tế

8 Roll() (DCM-KALMAN) time(sec) deg

Raw Roll Estimated Roll Reference Roll

Pitch()(DCM-KALMAN) time(sec) deg

Raw PitchEstimated PitchReference Pitch

Hình 4.7 Góc Yaw sau khi qua ước lượng và được so sánh với góc Yaw thực tế

Hình 4 8 Sai số của ba góc sau khi ước lượng so với góc thực tế

Dựa vào hình 4.5, hình 4.6 ta thấy giá trị ước lượng hai góc Roll và Pitch đã bám theo góc Roll và Pitch thực tế rất tốt Với giá trị góc có nhiễu tối đa xấp xỉ 6°, sau khi qua bộ ước lượng góc có giá trị xấp xỉ tối đa 1 độ Dựa vào hình 4.8 ta thấy giá trị sai số trung bình (𝑒 𝑅𝑀𝑆 ) của hai góc lần lượt là 0.24231 và 0.23893 và giá trị sai số lớn nhất (𝑒 𝑀𝑎𝑥 ) là 0.58895 và 0.63306 Với góc Yaw hình 4.7, ta thấy giá trị ước lượng bám rất tốt tuy nhiên có sự vọt lố và cần một khoảng thời gian nhỏ để xác lập do giá trị góc Yaw có ảnh hưởng từ hai góc Roll và Pitch Trong thực tế thì khi cấp nguồn cho cảm biến thì thời gian xác lập rất ngắn và không có hiện tượng như mô phỏng Với sai số trung bình

𝑒 𝑅𝑀𝑆 =0.056735 và giá trị sai số lớn nhất 𝑒 𝑀𝑎𝑥 =0.63306 là giá trị sai số lớn nhất do vọt lố lúc đầu nhưng giá trị sau khi xác lập đều nhỏ hơn 0.2° Ta có thể kết luận thuật toán

-0.8 -0.6 -0.4 -0.2 0 0.2 0.4 0.6 0.8 yaw()(DCM-KALMAN) time(sec) deg

Raw Yaw Estimated Yaw Reference Yaw

Error Roll()(DCM-KALMAN) e-RMS  = 0.24231, e-Max  = 0.70635 deg

Error Pitch()(DCM-KALMAN) e-RMS  = 0.23893, e-Max  = 0.58895 deg

Error Yaw()(DCM-KALMAN) e-RMS  = 0.056735, e-Max  = 0.63306 time(sec) deg

Error Yaw ước lượng cho trường hợp cảm biến MARG được giữ cố định trong mặt phẳng cho đáp ứng tốt

2.2 Trường hợp 2: cảm biến MARG chuyển động chậm theo từng trục quay

Trong trường hợp này, cảm biến MARG sẽ được mô phỏng chuyển động theo từng trục để đánh giá được độ chính xác cho từng góc một cách rõ ràng hơn Dữ liệu cảm biến trong trường hợp này cũng được cộng thêm nhiễu Có thể thấy nhiễu được thêm vào qua hình 4.9 dữ liệu của 3 cảm biến lấy từ khối MARG mô phỏng

Hình 4.9 Dữ liệu lấy từ cảm biến cảm biến MARG mô phỏng (a) Dữ liệu vận tốc (gyroscope); (b) Dữ liệu cảm biến gia tốc (Accelerometer); (c)Dữ liệu cảm biến la bàn

Raw Pitch Estimated Pitch Reference Pitch

Hình 4.13 Sai số của ba góc sau khi ước lượng so với góc thực tế

Dựa vào Hình 4.10, Hình 4.11, Hình 4.12 ta thấy ba góc Euler (Roll, Pitch, Yaw) sau khi khi qua bộ ước lượng cho kết quả khá tốt Lượng nhiễu được triệt tiêu khá nhiều và bám rất tốt đối với góc thực tế Từ hình 4.13 với sai số trung bình (𝑒 𝑅𝑀𝑆 ) lần lượt là 0.31941 với góc Roll, 0.27546 với góc Pitch, 0.14745 với góc Yaw cho thấy thuật toán cho sai số rất nhỏ so với góc thực tế Ngay cả với sai số lớn nhất (𝑒 𝑀𝑎𝑥 ) cho từng góc 0.98929 với góc Roll, 0.76794 với góc Pitch và 0.73014 với góc Yaw cũng nhỏ hơn rất nhiều so với yêu cầu của đề tài là sai số tối đa nhỏ hơn 3° và sai số trung bình nhỏ hơn 2° Thuật toán đáp ứng rất tốt trong trường hợp từng góc chuyển động với vận tốc chậm

-5 0 5 10 15 20 25 yaw()(DCM-KALMAN) time(sec) deg

2.3 Trường hợp 3: cảm biến MARG chuyển động cùng lúc ba trục quay

Trong thực tế xảy ra rất nhiều trường hợp chuyển động, chuyển động theo từng trục đơn chỉ là một trường hợp, phần lớn trong các trường hợp là chuyển động kết hợp giữa các trục Vì vậy để xét được đáp ứng của bộ ước lượng một cách tổng quát hơn, ta xét tiếp trường hợp cảm biến MARG với chuyển động cùng lúc cả ba trục

Hình 4.14 Dữ liệu lấy từ cảm biến cảm biến MARG mô phỏng (a) Dữ liệu vận tốc (gyroscope); (b) Dữ liệu cảm biến gia tốc (Accelerometer);(c) Dữ liệu cảm biến la bàn

25 Pitch()(DCM-KALMAN) time(sec) deg

Với trường hợp 3, cả ba góc được mô phỏng chuyển động cùng lúc với vận tốc nhỏ và độ lớn góc chuyển động là gần như nhau Quan sát hình 4.15, hình 4.16, hình 4.17 ta thấy cả ba góc đều có nhiễu trong chuyển động của từng góc và với hình 4.14 ta sẽ quan sát rõ hơn nhiễu từ dữ liệu đo được của cảm biến MARG mô phỏng Với lượng nhiễu khá lớn nhưng sau khi qua bộ ước lượng góc ước lượng cho độ chính xác tốt hơn và gần giống với góc thực tế Với sai số trung bình (e RMS ) và sai số lớn nhất (𝑒 𝑀𝑎𝑥 ) lần lượt cho từng góc thông qua hình: 0.3263 và 0.98608 với góc Roll, 0.28952 và 0.83 với góc Pitch, 0.23118 và 0.76211 với góc Yaw Ta thấy ngay cả khi ba góc cùng chuyển động sai số vẫn rất nhỏ và với giá trị sai số lớn nhất vẫn nhỏ hơn 2°, sai số trung bình vẫn nhỏ hơn 0.4° trong trường hợp này Nếu nhận xét kĩ hơn, ta thấy góc Yaw trước khi ước lượng

-5 0 5 10 15 20 25 30 35 yaw()(DCM-KALMAN) time(sec) deg

Error Yaw đã được kết hợp với cảm biến từ trường đã cho kết quả là nhiễu ít hơn hai góc Roll và Pitch Sau khi qua ước lượng, kết quả còn tốt hơn với độ chính xác cao hơn

2.4 Trường hợp 4: cảm biến MARG chuyển động với vận tốc lớn (mô phỏng chuyển động lắc trong thực tế )

Qua những trường hợp trên ta đã từng bước đánh giá thuật toán đối với cảm biến MARG khi cố định trong mặt phẳng, chuyển động trong từng trục cũng như chuyển động trong nhiều trục cùng lúc Tuy nhiên trong các trường hợp đều xảy ra với chuyển động chậm và góc thay đổi chưa lớn Vậy nếu cảm biến chuyển động trong trường hợp giả thiết ở đây là chuyển động lắc hay là chuyển động với vận tốc lớn và góc thay đổi lớn Ta sẽ có một cái nhìn tổng quát hơn đối với thuật toán trong trường hợp mà nhiều thuật toán ước lượng khác cho sai số rất lớn

Hình 4.19 Dữ liệu lấy từ cảm biến cảm biến MARG mô phỏng (a) Dữ liệu vận tốc (gyroscope); (b) Dữ liệu cảm biến gia tốc (Accelerometer);(c) Dữ liệu cảm biến la bàn

Roll() (DCM-KALMAN) time(sec) deg

Trường hợp này góc Roll được mô phỏng với chuyển động như là chuyển động lắc, với góc thay đổi lớn từ khoảng gần 180 đến -180 trong khoảng thời gian 2 giây (dựa vào hình 4.20), hai góc Pitch và Yaw được giữ cố định Tần số trích mẫu sử dụng để lấy dữ liệu trong mô phỏng là 100 Hz (tương đương thời gian lấy mẫu là 0.01 giây) Để có thể ước lượng chính xác với chuyển động nhanh thì thời gian lấy mẫu nhanh cho dữ liệu đáp ứng tốt hơn với sự thay đổi đột ngột của chuyển động Dựa vào Hình 4.23 ta thấy sai số trung bình (𝑒 𝑅𝑀𝑆 ) và sai số lớn nhất (𝑒 𝑀𝑎𝑥 ) của góc Roll lần lượt là 1.5941 và 2.675 vẫn rất nhỏ Đối với sai số lớn nhất đạt được vẫn nhỏ hơn sai số đặt ra của đề tài là 3° và sai số trung bình nhỏ hơn 2° Với khả năng ước lượng góc cho độ chính xác cao ngay cả khi với chuyển động ở tốc độ cao và độ biến thiên vị trí lớn thuật toán cho thấy lợi thế hơn những thuật toán ước lượng khác về độ chính xác trong nhiều trường hợp và đã đáp ứng tốt mục tiêu mà đề tài đặt ra

2.5 yaw()(DCM-KALMAN) time(sec) deg

Error Pitch()(DCM-KALMAN) e-RMS  = 0.32209, e-Max  = 0.93357 deg Error Pitch

THỰC NGHIỆM

Phần cứng

Sử dụng board điều khiển “Multiwii and Megapirate AIO Flight Controller w/FTDI (ATmega 2560) V2.0” (flight control) sử dụng cho multicopter Board được thiết kế với chíp xử lý là ATMega 2560 Microcontroller và gồm các cảm biến: biến gia tốc gốc và vận tốc góc MPU 6050, cảm biến từ trường HMC 5883L và cảm biến áp suất độ chính xác cao MS5611-01BA01

Board được thiết kế theo board Adruno 2560 nên cho phép lập trình và giao tiếp dễ dàng với máy tính và thiết bị ngoại vi Với IMU được tích hợp theo chuẩn I2C giúp dễ dàng giao tiếp với các cảm biến và nhận dữ liệu

Hỗ trợ chuẩn giao tiếp Micro USB và sử dụng chip FT232RQ USB-UART giúp nạp code trực tiếp cho vi xử lý không qua board nạp Dữ liệu được truyền nhận trực tiếp giữa board và máy tính

Hình 5.1 Board “Multiwii and Megapirate AIO Flight Controller w/FTDI (ATmega 2560) V2.0” tích hợp cảm biến MPU6050 , cảm biến la bàn HCM5883L và cảm biến áp suất MS5611-01BA01 1.1.1 Cảm biến MPU6050

MPU-6050 là cảm biến của hãng InvenSense MPU-6050 là một trong những giải pháp cảm biến chuyển động đầu tiên trên thế giới có tới 6 (mở rộng tới 9) trục cảm biến tích hợp trong một chip duy nhất

Cảm biến MPU6050 có các đặc điểm kỹ thuật sử dụng như sau:

- Nguồn cung cấp cho cảm biến từ 3-5V do trên board “Multiwii and Megapirate AIO Flight Controller w/FTDI (ATmega 2560) V2.0” đã có sẵn LDO chuyển nguồn 5V - 3.3V

- Hỗ trợ giao tiếp với chuẩn truyền I2C có thể lên đến 400kHz

- Thiết kế MPU6050 có sử dụng bộ chuyển đổi tương tự - số (Anolog to Digital Converter - ADC) 16-bit để cho ra kết quả chi tiết về vận tóc góc, gia tốc, nhiệt độ Có thể chuyển định dạng ngõ ra thành chuỗi thập phân để phù hợp với định dạng dữ liệu khi đưa vào Matlab

- Thông số con quay hồi chuyển ba trục (3-axis MEMS Gyroscope)

 Con quay hồi chuyển: ±250 ±500 ±1000 ±2000 (degree per second) Trong đề tài thiết lập ở ±250

 Tần số ngõ ra: 100 Hz

- Thông số cảm biến gia tốc 3 chiều (3-axis MEMS accelerometer)

 Cảm biến gia tốc: ±2 ±4 ±8 ±16 (g), trong đề tài thiết lập ở ± 2(°/s)

 Tần số ngõ ra: 100Hz

Từ các giá trị mà cảm biến MPU6050 trả về Ta sẽ dùng các công thức sẽ đề cập bên dưới để tính toán:

- Đối với dữ liệu lấy về từ cảm biến vận tốc Gyro:

GyroRate: Vận tốc góc đơn vị là (degree/second) GyroRead: Dữ liệu (16bit) đọc về từ Gyro của MPU6050 [LSB Sensitivity]: Hệ số suy ra vận tốc góc, nó phụ thuộc vào việc thiết đặt chế độ hoạt động của Gyro trong MPU6050 ở đây ta thiết lập ở ±250 (°/s)

Tương tự với dữ liệu lấy về từ cảm biến gia tốc Accelerometer nhưng với [LSB Sensitivity] là ± 2g

1.1.2 Cảm biến la bàn HCM 5883L

Cảm biến la bàn HMC5883L do hãng Honeywell sản xuất, cảm biến la bàn số 3 trục này được sử dụng trong môi trường có từ tính thấp

- Nguồn cung cấp cho cảm biến từ 2.16 - 3.6V

- Cảm biến la bàn HMC5883L giao tiếp lấy dữ liệu qua chuẩn I2C

- Độ phân giải: 5 milli-gauss

- Tần số ngõ ra: 100Hz

- Hỗ trợ chuẩn giao tiếpI2C

1.2 Cảm biến góc có độ chính xác cao

Cảm biến đo góc “Inclination sensor INY360D-F99-2I2E2-V17” của hãng Pepperl- Fuchs cho ra tín hiệu góc trực tiếp với tín hiệu ngõ ra cả analog và digital Với độ phân giải nhỏ và cho phép đo được góc nghiêng theo hai trục Roll và Pitch cùng lúc với độ chính xác cao

Thông số cảm biến góc “Inclination sensor INY360D-F99-2I2E2-V17”:

- Loại: cảm biến góc 2 trục

- Độ chính xác tuyệt đối: ≤ ± 0.5 °

- Độ chính xác lặp lại : ≤ ± 0.1 °

- Ảnh hưởng của nhiệt độ: ≤ 0.027 °/K

- Điện áp hoạt động: 10 30 VDC

- Tiêu chuẩn bảo vệ: IP68 / IP69K

- Thiết lập từ nhà máy: Ngõ ra analog (X) -45 ° 45 °; Ngõ ra analog (Y) -45 ° 45 °

Hình 5.2 Cảm biến góc với độ chính xác cao (INY360D-F99-2I2E2-V17)

1.3 Mô hình máy bay Quadrotor

Hình 5.3 mô hình quadrotor sử dụng khung Tarot FY650 IRON MAN 650

- Động cơ sử dụng: 4 động cơ SunnySky X2212-13 980KV

- Cánh quạt: bốn cánh quạt Fun 10x4.7 (2 cánh thuận, 2 cánh nghịch)

- Bộ điều khiển bay: APM 2.6

- Pin : 2 viên pin Li-Po Wild Scorpion 2200mAh - 35C

- GPS: Ubox super precision LEA-6h GPS and Compass

- Bộ thu phát sóng radio: Telemetry radios (433MHz)

- Modun đo nguồn: 10s Power sensor and BEC Module ( XT-60 connectors)

- Khối lượng toàn bộ mô hình: 1,6 kg

- Tải trọng mang theo tối đa của mô hình: 2 kg

- Kích thước: 460 x 460 x 260( đã bao gồm chiều cao động cơ)

- Không gian làm việc: 714 x 714 x 260( với cánh 10x 4.7 )

Kết quả thực nghiệm trong điều kiện bình thường

Để đánh giá được độ chính xác của thuật toán, ta sử dụng một cảm biến đo góc

“Inclination sensor INY360D-F99-2I2E2-V17” có độ chính xác cao để tiến hành đánh giá Với cảm biến góc 2 trục ta sẽ có được góc Roll và góc Pitch đúng nhất để làm chuẩn

Cảm biến góc và cảm biến MARG được gắn trên cùng một mặt phẳng và đặt sát nhau Khi đó sai số về vị trí lắp đặt là nhỏ nhất Ở đây để cho việc thực nghiệm chính xác và hiệu quả nhất, board điều khiển (flight control) bao gồm IMU và cảm biến góc nghiêng được lắp trên mô hình bay quadcopter để tiến hành ước lượng và đo đạc các có Euler (Roll, Pitch, Yaw)

Hình 5.4 Cảm biến MARG và cảm biến góc nghiêng được lắp trên mô hình

Quadrotor để tiến hành làm thực nghiệm

Ta tiến hành quay toàn bộ mô hình theo hai trục Roll, Pitch và Yaw Ta sẽ thu được dữ liệu từ các cảm biến trên IMU và dữ liệu về góc chuẩn từ cảm biến góc Tất cả các dữ liệu được đưa vào Matlab để tiến hành mô phỏng đánh giá thuật toán Với dữ liệu đầu vào là dữ liệu thu thập được từ khối IMU là thông số thực tế đã bao gồm nhiễu, ta tiến hành ước lượng các góc Euler dựa vào thuật toán đã mô tả phía trên Sau khi đã ước lượng, sử dụng dữ liệu thu được từ cảm biến góc làm chuẩn (Reference) ta sẽ đánh giá được độ chính xác của góc ước lượng so với góc chuẩn Từ đó đánh giá được tính hiệu quả và chính xác của thuật toán trong điều kiện thực tế

Nhưng vì điều kiện thực hiện đề tài không cho phép, nên hiện tại vẫn chưa có cảm biến nào đủ độ chính xác cao để đo được dữ liệu góc Yaw Vì vậy đề tài sử dụng dữ liệu từ cảm biến la bàn trong khối IMU kết hợp với dữ liệu hai góc Roll và Pitch từ cảm biến góc để có được góc Yaw theo như công thức 3.14 ở trên Như vậy ta đã đủ 3 góc Roll, Pitch, Yaw làm chuẩn Cảm biến la bàn ở đây được đặt trong tình trạng lý tưởng nhất có thể như tránh xa các nguồn điện gây nhiễu, đặt ở không gian mở

- Điều kiện tĩnh: board MARG giữ cố định với nhiễu là nhiễu từ cả 3 cảm biến

- Điều kiện động: Di chuyển board ( theo từng trục, kết hợp các trục và di chuyển theo dạng lắc ) Nhiễu gồm có nhiễu của 3 cảm biến, nhiễu gia tốc ngoài và nhiễu chuyển động

Giống như trên mô phỏng, ta sử dụng sai số trung bình 𝑒 𝑅𝑀𝑆 và sai số lớn nhất 𝑒 𝑀𝑎𝑥 để quan sát được giá trị sai số một cách rõ ràng nhất, giúp đánh giá độ chính xác một cách chính xác hơn

2.1 Trường hợp 1: Cảm biến MARG được giữ cố định trong mặt phẳng

Trong trường hợp này, ta xét cảm biến MARG được giữ cố định trên mặt phẳng cùng với cảm biến góc nhằm xem xét độ chính xác của thuật toán trong điều kiện tĩnh

Cảm biến góc với độ phân giải nhỏ nhất là 0.1°

Hình 5.5 Dữ liệu lấy từ cảm biến cảm biến MARG thực tế (a) Dữ liệu vận tốc (Gyroscope); (b) Dữ liệu cảm biến gia tốc (Accelerometer); (c) Dữ liệu cảm biến la bàn

Hình 5.6 Góc Roll sau khi qua thuật toán ước lượng được so sánh với góc Roll chưa qua xử lý và góc Roll làm chuẩn lấy từ cảm biến góc

Raw Roll Reference Roll Estimated Roll

Hình 5.7 Góc Pitch sau khi qua thuật toán ước lượng được so sánh với góc Pitch chưa qua xử lý và góc Pitch làm chuẩn lấy từ cảm biến góc

Hình 5.8 Góc Yaw sau khi qua thuật toán ước lượng được so sánh với góc Yaw chưa qua xử lý và góc Yaw làm chuẩn lấy từ cảm biến góc

Hình 5.9 Sai số của ba góc sau khi qua thuật toán ước lượng so với góc làm chuẩn lấy từ cảm biến góc

Raw Pitch Reference Pitch Estimated Pitch

-0.8 -0.6 -0.4 -0.2 0 0.2 0.4 0.6 0.8 yaw()(DCM-KALMAN) time(sec) deg

Raw Yaw Reference Yaw Estimated Yaw

Dựa vào đồ thị Hình 5.6, Hình 5.7 ta thấy giá trị hai góc Roll và Pitch sau khi qua thuật toán ước lượng đã giảm được nhiễu khá tốt Giá trị ban đầu của hai góc dao động trong khoảng 0.2°, sau khi qua thuật toán giá trị chỉ giao động trong khoảng 0.05° bám rất tốt so với đường chuẩn là đường 0° được xét trong trường hợp này Giá trị của cảm biến góc với độ phân giải nhỏ nhất 0.1° cho thấy độ chính xác cao sẽ là cơ sở để đánh giá độ chính xác thực tế của thuật toán trong điều kiện thực Với góc Yaw hình 5.8, sau khi kết hợp với dữ liệu cảm biến la bàn (cảm biến từ trường) cho giá trị góc ban đầu dao động với giá trị tối đa trong khoảng 0.8°, sau khi qua thuật toán giá trị dao động tối đa trong khoảng 0.35° Giá trị sai số tối đa (e Max ) trong trường hợp này cho ba góc lần lượt là 0.34937; 0.23904; 0.72978 đều nhỏ hơn yêu cầu đề tài Cả ba góc đều cho giá trị tốt hơn cả giá trị làm chuẩn vì giá trị chuẩn bị giới hạn bởi độ phân giải nhỏ nhất là 0.1°

Từ đây có thể kết luận thuật toán ước lượng trong trường hợp tĩnh cho đáp ứng tốt trong điều kiện tĩnh

2.2 Trường hợp 2: Cảm biến MARG chuyển động trên từng trục đơn

Trong trường hợp này cảm biến MARG được cho chuyển động theo từng trục riêng rẽ để đánh giá độ chính xác trên từng trục rõ ràng hơn Chuyển động được bắt đầu từ góc Roll sau đó là góc Pitch, góc Yaw được chuyển động sau cùng Tuy nhiên góc Yaw vẫn chuyển động do trong quá trình làm thực nghiệm khi quay hai góc kia làm góc Yaw quay theo Từ trường hợp này trở xuống ta sẽ xem xét thuật toán ở trạng thái động

Hình 5.10 Dữ liệu lấy từ cảm biến cảm biến MARG thực tế (a) Dữ liệu vận tốc (Gyroscope); (b) Dữ liệu cảm biến gia tốc (Accelerometer);(c) Dữ liệu cảm biến la bàn

Kết quả thực nghiệm khi ứng dụng trên mô hình Quadrotor

Tiến hành thực nghiệm trên mô hình Quadrotor được bay ở chế độ giữ thăng bằng (Hover) Board cảm biến MARG được nâng lên cao để tránh nhiễu từ trường

Hình 5.25 Tổng quan mô hình Quadrotor và các cảm biến sử dụng trong thực nghiệm

Hình 5.26 Thực nghiệm trường hợp mô hình Quadrotor bay ở chế độ Hover

Hình 5.27 Dữ liệu lấy từ cảm biến cảm biến MARG thực tế; (a) Dữ liệu vận tốc (Gyroscope); (b) Dữ liệu cảm biến gia tốc (Accelerometer);(c) Dữ liệu cảm biến la bàn

Hình 5.29 Góc Pitch sau khi qua thuật toán ước lượng được so sánh với góc Pitch chưa qua xử lý và góc Pitch làm chuẩn lấy từ cảm biến góc

Hình 5.30 Góc Yaw sau khi qua thuật toán ước lượng được so sánh với góc Yaw chưa qua xử lý và góc Yaw làm chuẩn lấy từ cảm biến góc

Hình 5.31 Sai số của ba góc sau khi qua thuật toán ước lượng so với góc làm chuẩn lấy từ cảm biến góc

Quan sát hình 5.28,hình 5.29 ta thấy sự dao động rất lớn trong dữ liệu góc thu được Ở góc Roll sự dao động trung bình khoảng 5°, với góc Pitch sự dao động với biên độ lớn hơn khoảng 7° - 10° Dao động là do hoạt động của bốn động cơ không đồng bộ về tốc độ cũng như là rung động do cánh quạt, do giải thuật điều khiển bay khiến mô hình rung lắc làm các góc rung lắc mạnh và khác biệt ở từng góc Sau khi qua thuật toán ước lượng, đường ước lượng góc đã tốt hơn rất nhiều, mịn hơn và ít dao động hơn với sự sai lệch trung bình (𝐞 𝐑𝐌𝐒 ) và sai số tối đa (𝐞 𝐌𝐚𝐱 ) (hình 5.31) của từng góc lần lượt là Roll:

0.72327; 2.1798; Pitch: 0.87449; 2.5873 Sai số lớn hơn so với các trường hợp xét ở trên Tuy nhiên vẫn trong giới hạn mà đề tài đặt ra Còn với góc Yaw hình 5.30, góc sau khi ước lượng bám rất tốt với góc chuẩn và sai số rất nhỏ Vậy trường hợp kiểm nghiệm trên mô hình bay cho kết quả tốt

-5 0 5 10 15 yaw()(DCM-KALMAN) time(sec) deg

Error Roll()(DCM-KALMAN) e-RMS  = 0.72327, e-Max  = 2.1798 deg Error Roll

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

Luận văn đã thực hiện được các mục tiêu và nhiệm vụ đề ra là:

- Thiết kế bộ ước lượng góc nghiêng cho cảm biến MARG sử dụng bộ lọc Kalman dựa trên phương pháp DCM

- Đánh giá độ chính xác của thuật toán khi ứng dụng trên cảm biến MARG qua việc mô phỏng thuật toán trên Matlab qua nhiều trường hợp với các chuyển động khác nhau Tất cả các trường hợp đều cho độ chính xác cao với sai số lớn nhất nhỏ hơn 3° và sai số trung bình nhỏ hơn 2°

- Tiến hành ứng dụng thuật toán trong việc ước lượng góc nghiêng từ dữ liệu của board cảm biến MARG thực thế và đánh giá độ chính xác, sai số trong nhiều trường hợp chuyển động khác nhau và kiểm nghiệm trong điều kiện bay của mô hình Quadrotor ở chế độ giữ cân bằng (Hover) Sai số góc ước lượng với giá trị góc chuẩn lấy từ cảm biến góc có độ chính xác cao đều cho kết quả tốt Sai số lớn nhất nhỏ hơn 3° và sai số trung bình nhỏ hơn 2°

2 Hướng phát triển đề tài

Thuật toán vẫn chưa được xem xét trong trường hợp có nhiễu từ trường mạnh

Hướng phát triển của đề tài là xem xét và đánh giá thuật toán trong môi trường có nhiều nhiễu hơn, đặc biệt là trong môi trường điều khiển Quadrotor với độ nhiễu từ môi trường và nhiễu từ trường cao

Hệ số nhiễu quá trình và nhiễu đo lường vẫn được giữ cố định và lựa chọn theo kinh nghiệm nên khả năng đáp ứng vẫn còn hạn chế Tìm hiểu phương pháp để hệ số tự đáp ứng trong các trường hợp khác nhau để tăng tính đáp ứng và độ chính xác của thuật toán Đề tài có thể được ứng dụng vào các bài toán điều khiển cân bằng như: robot một bánh, xe tự hành Có thể kết hợp nhiều IMU thành hệ thống mô phỏng cử động người dùng trong y tế, điện ảnh,…Ngoài ra có thể tăng cường các cảm biến như cảm biến siêu âm, cảm biến khí áp để đo độ cao, kết hợp IMU và GPS thành hệ thống INS dùng cho định vị và dẫn đường,… đây chắc chắn là các yếu tố tối cần thiết để có thể thiết lập chương trình bay tự động cho Quadrotor

Ngoài ra tác giả còn được đăng bài báo trên hội nghị quốc tế “SEATUC 2015”.

Tiêu đề	Nâng cao độ chính xác cảm biến MARG sử dụng trong điều khiển Quadrotor
Tác giả	Trần Minh Phụng
Người hướng dẫn	TS. Trần Việt Hồng
Trường học	Trường Đại học Bách Khoa, Đại học Quốc gia Tp. HCM
Chuyên ngành	Kỹ thuật Cơ điện tử
Thể loại	Luận văn thạc sĩ
Năm xuất bản	2015
Thành phố	Tp.HCM

Định dạng
Số trang	92
Dung lượng	3,28 MB