Kết hợp cảm biến khoảng cách và cảm biến quán tính trong hệ thống đo từ xa

Chia sẻ: _ _ | Ngày: | Loại File: PDF | Số trang:5

Thêm vào BST

Báo xấu

10
lượt xem 2
download

Download Vui lòng tải xuống để xem tài liệu đầy đủ

Cảm biến quán tính (Inertial Measurement Unit – IMU) hiện đang được sử dụng rất rộng rãi trong nhiều lĩnh vực của kỹ thuật và đời sống. Bài viết Kết hợp cảm biến khoảng cách và cảm biến quán tính trong hệ thống đo từ xa đề xuất một hệ thống đo từ xa sử dụng một cảm biến khoảng cách dùng laser kết hợp với một cảm biến quán tính.

Chủ đề:

Bình luận(0) Đăng nhập để gửi bình luận!

Lưu

Nội dung Text: Kết hợp cảm biến khoảng cách và cảm biến quán tính trong hệ thống đo từ xa

ISSN 1859-1531 - TẠP CHÍ KHOA HỌC VÀ CÔNG NGHỆ ĐẠI HỌC ĐÀ NẴNG, SỐ 11(120).2017 - Quyển 2 17 KẾT HỢP CẢM BIẾN KHOẢNG CÁCH VÀ CẢM BIẾN QUÁN TÍNH TRONG HỆ THỐNG ĐO TỪ XA COMBINATION OF A DISTANCE SENSOR AND AN INERTIAL MEASUREMENT UNIT (IMU) IN A REMOTE MEASUREMENT SYSTEM Phạm Duy Dưởng1, Nguyễn Anh Duy2, Đoàn Quang Vinh3 1 NCS ngành Điều khiển và tự động hóa, khóa K35 tại Trường Đại học Bách khoa – ĐHĐN; duyduongd2@gmail.com 2 Trường Cao đẳng Công nghệ - Đại học Đà Nẵng; naduy@dct.udn.vn 3 Đại học Đà Nẵng; dqvinh@ac.udn.vn Tóm tắt - Cảm biến quán tính (Inertial Measurement Unit – IMU) hiện Abstract - The Inertial Measurement Unit (IMU) is now widely used đang được sử dụng rất rộng rãi trong nhiều lĩnh vực của kỹ thuật và in many areas of technology and life. This paper proposes a remote đời sống. Bài báo này đề xuất một hệ thống đo từ xa sử dụng một measurement system that uses a laser distance sensor in cảm biến khoảng cách dùng laser kết hợp với một cảm biến quán conjunction with an IMU. By combining the analysis and tính. Bằng cách kết hợp phân tích và tính toán quỹ đạo chuyển động computation of the system's trajectory (estimated from the IMU’s của hệ thống (được ước lượng từ dữ liệu của cảm biến quán tính) và data) and distances from the system to the points under test khoảng cách từ hệ thống đến các điểm cần đo (đo được từ cảm biến (measured from the distance sensor), we can calculate 3-D khoảng cách), ta có thể tính được tọa độ không gian của các điểm coordinates of the points to be measured. Based on those data it cần đo. Từ các dữ liệu đó có thể tính toán các thông số hình học khác is possible to calculate other geometric parameters such as như khoảng cách, góc, diện tích... Kết quả thực nghiệm cho thấy khả distance, angle, area... Experimental results show the actual năng ứng dụng thực tế của hệ thống được đề xuất. applicability of the proposed system. Từ khóa - đo xa; cảm biến quán tính; cảm biến khoảng cách; bộ Key words - remote measurement; inertial measurement unit; lọc Kalman; đo chiều dài từ xa. distance sensor; Kalman filter; remote length measurement. 1. Đặt vấn đề đơn giản gồm cảm biến khoảng cách một trục (1-D Lidar) Ngày nay, LIDAR (Light Detection and Ranging) – kỹ và cảm biến quán tính (cảm biến gia tốc và vận tốc góc thuật viễn thám dùng ánh sáng, được sử dụng rộng rãi trong quay theo 3 trục) để đo độ dài từ xa. Bằng cách kết hợp tạo bản đồ bằng cách gắn một thiết bị LIDAR lên máy bay, phân tích và tính toán quỹ đạo chuyển động của hệ thống rô bốt hoặc tàu xe. Ngoài ra, LIDAR cũng được sử dụng (được ước lượng từ dữ liệu của cảm biến quán tính) và rộng rãi trong các hệ thống đo độ dài, ví dụ như hệ thống khoảng cách từ hệ thống đến các điểm cần đo (đo được từ LM-100, 414D và S910. Những thiết bị này còn có thể cảm biến khoảng cách), ta có thể tính được tọa độ không được sử dụng kèm với các thiết bị phụ trợ khác để có thêm gian của các điểm cần đo. Từ các tọa độ không gian này, ta các chức năng đặc biệt. Mặc dù những hệ thống này có thể có thể tính được các thông tin quan trọng khác (ví dụ như cho độ chính xác cao, tuy nhiên, việc sử dụng chúng độ dài, độ cao giữa 2 điểm, góc tạo bởi 3 điểm, diện tích, thường không linh hoạt. Ví dụ, hệ thống S910 phải được khoảng cách…). Hiện nay, đa số các điện thoại thông minh sử dụng với hệ thống chân đế phức tạp (gồm Leica đều được tích hợp cảm biến quán tính nên hệ thống được FTA360-S và Leica TRI 70) để có được các chức năng đo đề xuất này có thể được xây dựng bằng việc gắn một cảm điểm-đến-điểm, đo góc, đo diện tích. biến khoảng cách với một điện thoại. Có một số hướng nghiên cứu liên quan đến việc đo từ 2. Tổng quan về hệ thống xa đã được đề xuất. Các tác giả trong bài báo [1] đã đề xuất Hệ thống đo độ dài từ xa được đề xuất (Hình 1) bao một hệ thống đo khoảng cách sử dụng cảm biến quán tính gồm một cảm biến khoảng cách bằng laser (Laser-lite và một mono camera. Trong nghiên cứu này, 2 hình ảnh sensor, Công ty PulsedLight Inc., Bend) [4] và một cảm của đối tượng được chụp tại điểm đầu và điểm cuối của một biến quán tính (Mti, Công ty Xsens). Cảm biến khoảng chuyển động. Chuyển động từ điểm đầu đến điểm cuối này cách này hoạt động theo nguyên tắc tính thời gian truyền được ước lượng bằng cảm biến quán tính và được sử dụng đến đối tượng và phản xạ lại của tia laser (time-of-flight) với vai trò là đường cơ sở (baseline) cho việc ước lượng với phạm vi đo đến 40 m và tần số lấy mẫu là 33,33 Hz. khoảng cách. Trong [2], các tác giả đã đề xuất một thuật Cảm biến quán tính bao gồm cảm biến gia tốc và cảm biến toán ước lượng độ sâu của các đối tượng trong ảnh bằng vận tốc góc theo 3 trục với tần số lấy mẫu là 100 Hz. Một cách sử dụng motion stereo camera. Trong [3], các tác giả bút laser được gắn vào hệ thống để hiển thị điểm mà hệ đã gắn một camera lên xe để phát hiện và đo khoảng cách thống đang chỉ vào. Do kích thước nhỏ gọn nên hệ thống đến 2 đường biên của lề đường. có thể được sử dụng như là một thiết bị cầm tay. Bằng cách Hệ thống đo bằng LIDAR có thể trở nên rất linh hoạt chỉ hệ thống vào một điểm và di chuyển để hệ thống chỉ bằng cách thay thế các phụ kiện phức tạp (ví dụ hệ thống vào điểm khác, chúng ta có thể ước lượng được mối quan chân đế) bằng một cảm biến quán tính. Hệ thống bao gồm hệ (độ dài, độ cao…) giữa 2 điểm đó cũng như có thể mở một cảm biến quán tính kết hợp với một thiết bị LIDAR rộng đối với những thông tin khác giữa nhiều điểm. này trở thành một hệ thống cầm tay rất thuận tiện. Bài báo này sử dụng một hệ trục tọa độ biến đổi và một Trong bài báo này, nhóm tác giả đề xuất một hệ thống hệ trục tọa độ cố định. Hệ trục tọa độ biến đổi hay còn gọi
18 Phạm Duy Dưởng, Nguyễn Anh Duy, Đoàn Quang Vinh là hệ tọa độ người dùng được sử dụng gắn liền với hệ thống xuống sàn nhà được đo bằng thước. (BCS - Body Coordinate System) và được chọn trùng với CẢM BIẾN hệ trục tọa độ của cảm biến quán tính. Hệ trục tọa độ cố QUÁN TÍNH  rl b định hay còn gọi là hệ trục tọa độ toàn cầu (WCS – World CẢM BIẾN Coordinate System). Sự chuyển động của hệ thống được đề KHOẢNG CÁCH (LIDAR) xuất sẽ được biểu diễn trong WCS. Trục 𝑧 của WCS hướng lên trên theo phương thẳng đứng (trùng với phương của gia nl b h tốc trọng trường). Gốc và phương 𝑥 của WCS không ảnh r hưởng đến quá trình đo các thông số (độ dài, độ cao, góc d và diện tích) nên được chọn tùy ý. Để phân biệt tọa độ đang xét trong hệ nào trong trường hợp dễ xảy ra hiểu nhầm, bài báo này sử dụng chỉ số dưới. Ví dụ: [𝑝]𝑏 hay ([𝑝]𝑤 ) được sử dụng để thể hiện vector 𝑝 ∈ 𝑅3 được biểu diễn trong hệ tọa độ BCS (hay trong hệ tọa độ WCS). zw yw  b w ĐIỂM ĐO TỪ XA CẢM BIẾN xw  rl b KHOẢNG nl b WCS CẢM BIẾN QUÁN CÁCH TÍNH Hình 2. Phương pháp xác định các thông số của cảm biến khoảng cách Do [𝑟𝑙 ] = 𝑘[𝑛𝑙 ]𝑏 nên công thức (1) trở thành: BÚT LASER [𝑏]𝑤 = 𝐶𝑏𝑤 (𝑘 + 𝑑)[𝑛𝑙 ]𝑏 + 𝑟 (2) Hình 1. Tổng quan hệ thống đề xuất Do mặt sàn là phẳng và nằm ngang nên độ cao của hệ Việc xác định mối quan hệ về vị trí và hướng của cảm thống được xác định bằng công thức (thành phần theo trục 𝑧): biến khoảng cách và cảm biến quán tính là rất cần thiết [0 0 1](𝑘 + 𝑑)𝐶𝑏𝑤 [𝑛𝑙 ]𝑏 = −ℎ (3) trong hệ thống đề xuất. Trong Hình 1, [𝑟𝑙 ]𝑏 ∈ 𝑅3 thể hiện vị trí của cảm biến khoảng cách đối với cảm biến quán tính Trong đó, ℎ là độ cao của hệ thống và được đo bằng thước. và [𝑛𝑙 ]𝑏 thể hiện hướng của cảm biến khoảng cách đối với Nếu chúng ta lặp lại việc đo này 𝑛 lần với các góc cảm biến quán tính. Những thông số này cần được ước nghiêng khác nhau, ta có: lượng thông qua thuật toán xác định thông số của cảm biến 𝑤 [0 0 1](𝑘 + 𝑑1 )𝐶1,𝑏 −ℎ1 khoảng cách ở Mục 3 tiếp theo. 𝑤 [0 0 1](𝑘 + 𝑑2 )𝐶2,𝑏 −ℎ2 [𝑛𝑙 ]𝑏 = [ ] (4) ⋮ ⋮ 3. Thuật toán xác định các thông số của cảm biến 𝑤 −ℎ𝑛 [[0 0 1](𝑘 + 𝑑𝑛 )𝐶𝑛,𝑏 ] khoảng cách 𝑤 Trong phần này, nhóm tác giả trình bày thuật toán nhằm 𝐶𝑖,𝑏 , 𝑖 = 1, 2, … , 𝑚 là ma trận quay từ BCS sang WCS xác định thông số vị trí [𝑟𝑙 ]𝑏 và hướng [𝑛𝑙 ]𝑏 của cảm biến tại thời điểm 𝑖 được ước lượng từ thuật toán định vị quán khoảng cách đối với cảm biến quán tính. Trong đó, hệ thống tính (xem Mục 4). được cầm trên tay trong khi cảm biến khoảng cách chỉ xuống Như vậy, [𝑛𝑙 ]𝑏 được ước lượng bằng việc tối thiểu sai số: sàn nhà. Tọa độ của điểm trên sàn nhà mà hệ thống đang chỉ ‖𝑆1 [𝑛𝑙 ]𝑏 − 𝑆2 ‖22 (5) vào trong hệ tọa độ WCS được tính theo công thức: 𝑤 [0 0 1](𝑘 + 𝑑1 )𝐶1,𝑏 [𝑏]𝑤 = 𝐶𝑏𝑤 [𝑏]𝑏 + 𝑟 = 𝐶𝑏𝑤 ([𝑟𝑙 ] + 𝑑[𝑛𝑙 ]𝑏 ) + 𝑟 (1) −ℎ1 𝑤 [0 0 1](𝑘 + 𝑑2 )𝐶2,𝑏 −ℎ Trong đó: 𝑟 là vị trí của cảm biến quán tính trong hệ tọa Với 𝑆1 = 𝑣à 𝑆2 = [ 2 ] ⋮ ⋮ độ WCS, 𝐶𝑏𝑤 là ma trận quay từ BCS sang WCS, 𝑤 [[0 0 1](𝑘 + 𝑑𝑛 )𝐶𝑛,𝑏 ] −ℎ𝑛 [𝑏]𝑏 = [𝑟𝑙 ]𝑏 + 𝑑[𝑛𝑙 ]𝑏 là vị trí trong BCS của điểm trên sàn Như vậy, tham số [𝑛𝑙 ]𝑏 được tối ưu bởi công thức: nhà mà hệ thống đang chỉ vào và 𝑑 là khoảng cách từ hệ thống đến điểm đó và được xác định bằng cảm biến khoảng cách. 𝑟 [𝑛𝑙 ]𝑏 = (𝑆1𝑇 𝑆1 )−1 𝑆1𝑇 𝑆2 (6) và 𝐶𝑏𝑤 được ước lượng bằng thuật toán định vị quán tính (INA – Inertial Navigation Algorithm) được trình bày ở Mục 4. 4. Thuật toán định vị quán tính (INA) sử dụng bộ lọc Kalman Nhóm tác giả cố gắng bố trí cảm biến khoảng cách và cảm biến quán tính sao cho [𝑟𝑙 ]𝑏 = 𝑘[𝑛𝑙 ]𝑏 (Hình 1). Trong Trong phần này, nhóm tác giả trình bày về INA và việc đó, [𝑛𝑙 ]𝑏 là véc-tơ đơn vị, 𝑘 là khoảng cách giữa cảm biến ứng dụng bộ lọc Kalman vào bài toán định vị quán tính để khoảng cách và cảm biến quán tính. Do 𝑘 thường rất nhỏ xác định các tham số 𝐶𝑏𝑤 và 𝑟 trong công thức (1). (khoảng 3 cm) so với 𝑑 (từ 3 ~ 40 m), nên 𝑘 không ảnh Đặt 𝑣 ∈ 𝑅3 và 𝑟 ∈ 𝑅3 là vận tốc và vị trí của cảm biến hưởng lớn đến kết quả phép đo và được đo bằng thước. quán tính trong WCS. Đặt 𝐶𝑤𝑏 (𝑞) ∈ 𝑅3×3 là ma trận quay Phương pháp ước lượng [𝑛𝑙 ]𝑏 được mô tả trong Hình từ WCS sang BCS tương ứng với quaternion [5] 𝑞 ∈ 𝑅4 . 2. Trong đó, cảm biến khoảng cách luôn chỉ xuống sàn nhà Quarernion 𝑞, vận tốc 𝑣 và vị trí 𝑟 của thiết bị liên quan ở các góc nghiêng khác nhau và khoảng cách từ hệ thống với nhau qua công thức [6]:
ISSN 1859-1531 - TẠP CHÍ KHOA HỌC VÀ CÔNG NGHỆ ĐẠI HỌC ĐÀ NẴNG, SỐ 11(120).2017 - Quyển 2 19 0 −𝜔𝑥 −𝜔𝑦 −𝜔𝑧 Trong quá trình sử dụng hệ thống, có những thời điểm 1 𝜔𝑥 0 𝜔𝑧 −𝜔𝑦 vận tốc của hệ thống bằng 0 (ZVI – Zero Velocity Interval). 𝑞̇ = [𝜔 ]𝑞 2 𝑦 −𝜔𝑧 0 𝜔𝑥 Chúng ta có thể sử dụng các ZVI này để cập nhật lại sai số 𝜔𝑧 𝜔𝑦 −𝜔𝑥 0 (7) cho giá trị vận tốc trong INA. Trong [10], các ZVI này có 𝑣̇ = 𝐶𝑤𝑏 (𝑞)𝑇 [𝑎] 𝑏 thể được phát hiện trực tiếp bởi cảm biến vận tốc Doppler. 𝑟̇ = 𝑣 Tuy nhiên, chúng ta có thể phát hiện các ZVI gián tiếp bằng Trong đó, 𝜔 là vận tốc góc của BCS trong WCS và cách sử dụng thuật toán phát hiện vận tốc bằng 0 [11, 12]. [𝑎]𝑏 ∈ 𝑅3 là gia tốc tịnh tiến trong BCS. Trong bài báo này, nhóm tác giả sử dụng một thuật toán Giá trị đầu ra của cảm biến vận tốc góc (𝑦𝑔 ∈ 𝑅3 ) và phát hiện ZVI đơn giản. Nếu những điều kiện dưới đây được thỏa mãn thì thời điểm gián đoạn 𝑚 phải thuộc ZVI: cảm biến gia tốc (𝑦𝑎 ∈ 𝑅3 ) được cho bởi công thức: 𝑁𝑔 𝑁𝑔 𝑦𝑔 = 𝜔 + 𝑣𝑔 + 𝑏𝑔 ‖𝑦𝑔,𝑖 ‖ ≤ 𝐵𝑔 , 𝑚− ≤𝑖≤𝑚+ (8) 2 2 (12) 𝑦𝑎 = [𝑎]𝑏 + 𝐶𝑤𝑏 (𝑞)[𝑔̃]𝑤 + 𝑣𝑎 + 𝑏𝑎 𝑁𝑎 𝑁𝑎 ‖𝑦𝑎,𝑖 − 𝑦𝑎,𝑖−1 ‖ ≤ 𝐵𝑎 , 𝑚 − ≤𝑖≤𝑚+ Trong đó, [𝑔̃]𝑤 ∈ 𝑅3 là véc-tơ gia tốc trọng trường trong 2 2 WCS. 𝑏𝑔 ∈ 𝑅3 và 𝑏𝑎 ∈ 𝑅3 là thành phần nhiễu thay đổi chậm Trong đó, 𝑁𝑔 và 𝑁𝑎 là các số nguyên. 𝐵𝑔 , 𝐵𝑎 là các giá của cảm biến vận tốc góc và cảm biến gia tốc. 𝑣𝑔 và 𝑣𝑎 là thành trị đặt ngưỡng. phần nhiễu trắng của cảm biến vận tốc góc và cảm biến gia tốc. Trong khoảng ZVI này, ta có phương trình cập nhật vận Thuật toán tích phân để lấy tích phân công thức (7) tốc bằng 0: (thay [𝑎]𝑏 bằng 𝑦𝑎 − 𝐶𝑤𝑏 (𝑞)𝑔̃ và thay 𝜔 bằng 𝑦𝑔 ) được thể 𝑧𝑣 = 𝐻𝑣 𝑥 + 𝑣𝑧𝑒𝑟𝑜 (13) hiện trong [7]. Đặt 𝑞̂, 𝑟̂ và 𝑣̂ lần lượt là giá trị ước lượng Với: của quaternion, vị trí và vận tốc của cảm biến quán tính. Do các cảm biến luôn có thành phần nhiễu nên các giá 𝑧𝑣 = 03×1 − 𝑣̂ ∈ 𝑅3×1 trị tích phân đó chưa phải là giá trị đúng. Gọi 𝑞̅ ∈ 𝑅3 , 𝐻𝑣 = [03×9 𝐼3 03×3 ] 𝑟̅ ∈ 𝑅3 và 𝑣̅ ∈ 𝑅3 lần lượt là sai số của quaternion, vị trí và vận tốc của cảm biến quán tính: 5. Thuật toán ước lượng điểm từ xa 𝑞̅ = [03×1 𝐼3 ] (𝑞̂∗ ⊗ 𝑞) Trong phần này, một thuật toán để ước lượng vị trí không 𝑟̅ = 𝑟 − 𝑟̂ (9) gian của một điểm ở xa được đề xuất. Thuật toán được xây 𝑣̅ = 𝑣 − 𝑣̂ dựng nhằm xác định mối tương quan giữa 2 điểm ở xa 𝐴𝑝 và Trong đó, ⊗ là phép nhân quaternion và 𝑞 ∗ là 𝐵𝑝 . Việc ước lượng mối quan hệ của nhiều điểm chỉ đơn giản quaternion liên hợp của 𝑞. Phương trình (9) biểu diễn 3 là sự lặp lại nhiều lần của việc xác định mối quan hệ giữa 2 thành phần của sai số quaternion 𝑞̅ ∈ 𝑅3 (thay vì dùng 4 điểm. Quá trình ước lượng vị trí bắt đầu bằng việc chỉ hệ thống thành phần) theo [8]. vào điểm 𝐴𝑝 với giả sử rằng hệ thống sẽ đứng im trong khoảng Các biến trạng thái được sử dụng trong bộ lọc Kalman: thời gian ngắn. Sau đó, hệ thống được di chuyển để chỉ vào 𝑞̅ điểm 𝐵𝑝 và giữ im trong thời gian ngắn. Như vậy, việc ước 𝑏𝑔 lượng vị trí giữa 2 điểm gồm 3 bước được miêu tả ở Bảng 1. 𝑥 = 𝑟̅ ∈ 𝑅15 (10) Trong đó 𝑀𝑖 (1 ≤ 𝑖 ≤ 3) là thời điểm cuối của bước thứ 𝑖. 𝑣̅ Ba bước này có thể được phân ra một cách tự động bằng [𝑏𝑎 ] cách sử dụng thuật toán phát hiện ZVI (Bước 1 và Bước 3), Phương trình trạng thái cho bộ lọc Kalman được dẫn như đã trình bày ở Mục 4. trong [9]: Bảng 1. 3 bước cho quá trình ước lượng tương quan vị trí 𝑥̇ (𝑡) = 𝐴(𝑡)𝑥(𝑡) + 𝜔(𝑡) (11) giữa 2 điểm Trong đó: Bước Thời điểm Miêu tả hoạt động 1 Giữ im hệ thống khi đang chỉ vào [−𝑦𝑔 ×] − 𝐼 0 0 0 Bước 1 1 ≤ 𝑚 ≤ 𝑀1 2 điểm 𝐴𝑝 0 0 0 0 0 Bước 2 𝑀1 + 1 ≤ 𝑚 ≤ 𝑀2 Chuyển thiết bị chỉ vào điểm 𝐵𝑝 𝐴(𝑡) = 0 0 0 𝐼 0 Bước 3 𝑀2 + 1 ≤ 𝑚 ≤ 𝑀3 Giữ im hệ thống khi đang chỉ vào 𝐵𝑝 −2𝐶𝑤𝑏 (𝑞̂)𝑇 [𝑦𝑎 ×] 0 0 0 0 [ 0 0 0 0 0] Đặt [𝑟𝑚 ]𝑤 là vị trí của hệ thống trong WCS và 𝑤 1 𝐶𝑚,𝑏 ∈ 𝑅3×3 là ma trận quay từ BCS sang WCS tại thời − 𝑣𝑔 2 điểm 𝑚. Đặt [𝑏𝑚 ]𝑤 ∈ 𝑅3 là vị trí của điểm mà hệ thống 𝑤𝑏𝑔 𝑤(𝑡) = đang chỉ đến. Khoảng cách điểm này đến cảm biến khoảng 0 cách được kí hiệu là 𝑑𝑚 . Từ công thức (2) ta có: −𝐶𝑤𝑏 (𝑞̂)𝑇 𝑣𝑎 𝑤 (𝑘 [𝑏𝑚 ]𝑤 = 𝐶𝑚,𝑏 [ 𝑤𝑏𝑎 ] + 𝑑𝑚 )[𝑛𝑙 ]𝑏 + [𝑟𝑚 ]𝑤 (14) [𝑎 ×] ∈ 𝑅3×3 là ma trận đối xứng lệch tương ứng với 𝑤 Trong đó, 𝐶𝑚,𝑏 và [𝑟𝑚 ]𝑤 được ước lượng từ thuật toán véc-tơ 𝑎 ∈ 𝑅3×1 . Nhiễu 𝑤𝑏𝑔 và 𝑤𝑏𝑎 đại diện cho sự thay định vị quán tính (xem Mục 4), [𝑛𝑙 ]𝑏 và 𝑘 được xác định đổi nhỏ của thành phần nhiễu chậm tương ứng. từ thuật toán được trình bày ở Mục 3, 𝑑𝑚 chính là giá trị
20 Phạm Duy Dưởng, Nguyễn Anh Duy, Đoàn Quang Vinh của cảm biến khoảng cách. Như vậy, ta hoàn toàn tính được Bước 2 𝑀1 + 1 ≤ 𝑚 ≤ 𝑀2 Di chuyển hệ thống trong khi vẫn tọa độ không gian của mọi điểm ở xa theo công thức (14). chỉ vào mặt phẳng 𝐸 Dừng việc di chuyển hệ thống và Gọi [𝑏𝐴𝑝 ] và [𝑏𝐵𝑝 ] là vị trí của 2 điểm 𝐴𝑝 và 𝐵𝑝 cần Bước 3 𝑀2 + 1 ≤ 𝑚 ≤ 𝑀3 vẫn chỉ vào mặt phẳng 𝐸 𝑤 𝑤 đo. Khoảng cách và độ cao giữa 2 điểm được xác định theo Di chuyển hệ thống để chỉ vào mặt Bước 4 𝑀3 + 1 ≤ 𝑚 ≤ 𝑀4 công thức: phẳng 𝐹 Giữ im hệ thống khi đang chỉ vào Bước 5 𝑀4 + 1 ≤ 𝑚 ≤ 𝑀5 𝐿𝐴𝑝 𝐵𝑝 = ‖𝑏𝐴𝑝 − 𝑏𝐵 𝑝 ‖ mặt phẳng 𝐹 (15) Di chuyển thiết bị trong khi vẫn 𝐻𝐴𝑝 𝐵𝑝 = [0 0 1](𝑏𝐴𝑝 − 𝑏𝐵 𝑝 ) Bước 6 𝑀5 + 1 ≤ 𝑚 ≤ 𝑀6 chỉ vào mặt phẳng 𝐹 Giữ im thiết bị khi đang chỉ vào 6. Thuật toán ước lượng khoảng cách giữa hai mặt phẳng Bước 7 𝑀6 + 1 ≤ 𝑚 ≤ 𝑀7 mặt phẳng 𝐹 Trong phần này, nhóm tác giả trình bày thuật toán ước lượng khoảng cách giữa hai mặt phẳng song song (ví dụ 7. Thí nghiệm và kết quả như sàn nhà và trần nhà). Hệ thống thực nghiệm để đánh giá độ chính xác của Quá trình đo khoảng cách giữa hai mặt phẳng song song phương pháp đo độ dài từ xa được thể hiện trong Hình 3. được thực hiện thông qua 7 bước như trong Bảng 2. Phương trình mặt phẳng 𝐸 được tính từ các điểm 𝑏𝑚 (𝑀1 + 1 ≤ 𝑚 ≤ 𝑀2 ). Tương tự, phương trình mặt phẳng 𝐹 được tính từ các điểm 𝑏𝑚 (𝑀5 + 1 ≤ 𝑚 ≤ 𝑀6 ). Do toàn bộ dữ liệu có được trong quá trình di chuyển (Bước 2 và Bước 6) được dùng để ước lượng các mặt phẳng nên thuật toán làm trơn [13] được sử dụng trong mục đích này. Thuật toán này cũng đóng vai trò là một INA. Đặt 𝑏𝑚 ∈ 𝑅3 là các điểm trong mặt phẳng 𝐸 hoặc 𝐹. Nếu 2 mặt phẳng là song song thì chúng có cùng véc-tơ pháp tuyến. Gọi 𝑛𝑏 ∈ 𝑅3 là véc-tơ đơn vị pháp tuyến của hai mặt Hình 3. Hệ thống đo xa dùng trong thí nghiệm phẳng. Khi đó, phương trình mặt phẳng được xác định bởi: Để chứng minh độ chính xác của hệ thống đề xuất, nhóm 𝑛𝑏𝑇 𝑏𝑚 = 𝛿𝐸 , 𝑀1 + 1 ≤ 𝑚 ≤ 𝑀2 tác giả tiến hành thí nghiệm trong 2 trường hợp: đo mối quan (16) 𝑛𝑏𝑇 𝑏𝑚 = 𝛿𝐹 , 𝑀5 + 1 ≤ 𝑚 ≤ 𝑀6 hệ điểm-đến-điểm (theo thuật toán ở Mục 5) và khoảng cách giữa 2 mặt phẳng (theo thuật toán ở Mục 6) với kết quả của việc Trong đó, 𝛿𝐸 ∈ 𝑅, 𝛿𝐹 ∈ 𝑅 là khoảng cách từ gốc của ước lượng các thông số của cảm biến khoảng cách (theo Mục WCS đến các mặt phẳng 𝐸, 𝐹 tương ứng. 3) là 𝑘 = 3,5 𝑐𝑚 và 𝑛𝑙 = [0,9993 0,0307 0,0198]. Viết lại phương trình (16) ta có: 7.1. Thí nghiệm xác định mối quan hệ điểm-đến-điểm 𝑛𝑏 C 382 cm B 𝑆3 [𝛿𝐸 ] = 0𝑀𝑝𝑙𝑎𝑛𝑒 ×1 (17) 17.20 𝛿𝐹 0 87.7 117 cm Trong đó: 𝑀𝑝𝑙𝑎𝑛𝑒 = (𝑀2 − 𝑀1 ) + (𝑀6 − 𝑀5 ) 75.10 395 cm 𝑇 𝑏𝑀 1 +1 −1 0 A 190 cm 𝑇 𝑏𝑀 1 +2 −1 0 80 cm Mặt phẳng tường ⋮ ⋮ ⋮ 𝑇 𝑏𝑀 2 −1 0 𝑆3 = 𝑇 zw 𝑏𝑀 5 +1 0 −1 yw xw Mặt phẳng sàn 𝑇 𝑏𝑀 5 +2 0 −1 ⋮ ⋮ ⋮ Hình 4. Thí nghiệm đo các tham số của tam giác 𝑇 [ 𝑏𝑀6 0 −1] Để kiểm chứng thuật toán xác định tọa độ không gian của Các tham số của phương trình mặt phẳng (𝑛𝑏 , 𝛿𝐸 và điểm từ xa (Mục 5), nhóm tác giả áp dụng thuật toán này để 𝛿𝐹 ) có thể được tính bằng cách giải bài toán tối ưu: đo các thông số của một tam giác từ xa với các tham số như trên Hình 4. Tam giác đó được xác định bởi 3 điểm trên bức min‖𝑆3 𝑥‖22 (18) tường và người dùng đứng cách bức tường khoảng 5,5 m. Kết 𝑥 quả của thí nghiệm được thể hiện trong Bảng 3. Để đánh giá sao cho ‖[𝐼3 03×2 ]𝑥‖2 = 1. Lời giải cho bài toán tối ưu độ chính xác, nhóm tác giả sử dụng căn bậc 2 của trung bình này được thể hiện trong A5.4.2 trong [14]. bình phương sai số (𝐸𝑅𝑀𝑆 – Root Mean Square of Error). Bảng 2. 7 bước cho quá trình ước lượng khoảng cách giữa 2 mặt phẳng Có thể thấy, giá trị lớn nhất của 𝐸𝑅𝑀𝑆 theo chiều dài, chiều cao và góc tương ứng là 1,36 cm; 2,22 cm và 1,250 . Do giới Bước Thời điểm Miêu tả hoạt động hạn đo của cảm biến khoảng cách lên đến 40 m và độ phân Bước 1 1 ≤ 𝑚 ≤ 𝑀1 Giữ im hệ thống chỉ vào mặt phẳng 𝐸 giải là 1 cm nên giá trị sai lệnh nêu trên là chấp nhận được.
ISSN 1859-1531 - TẠP CHÍ KHOA HỌC VÀ CÔNG NGHỆ ĐẠI HỌC ĐÀ NẴNG, SỐ 11(120).2017 - Quyển 2 21 Bảng 3. Kết quả thí nghiệm đo tham số của tam giác khoảng cách từ hệ thống đến vật. Bằng sự kết hợp phân tích Tham Chiều dài (cm) Chiều cao (cm) Góc (độ) và tính toán quỹ đạo chuyển động và khoảng cách đến vật, số 𝑳𝑨𝑩 𝑳𝑩𝑪 𝑳𝑨𝑪 𝑯𝑨𝑩 𝑯𝑩𝑪 𝑯𝑨𝑪 𝑨𝑪𝑩 ̂ 𝑩𝑨𝑪 ̂ 𝑨𝑩𝑪 ̂ tọa độ không gian của các điểm đo được xác định. Từ đó ta Giá trị 395,0 382,0 117,0 110,0 7,0 117,0 87,7 75,1 17,2 có thể tính các thông số khác như độ dài, độ cao, góc thật nghiêng, diện tích… Lần 1 396,6 380,5 118,7 112,6 6,2 118,8 89,0 73,6 17,4 Để làm được điều này, bài báo đã đề xuất một phương Lần 2 397,0 379,7 116,9 113,0 3,7 116,8 89,8 73,0 17,1 Lần 3 394,1 382,9 116,0 110,6 5,2 115,9 86,9 76,0 17,1 pháp ước lượng các thông số của cảm biến khoảng cách so Lần 4 395,9 382,5 116,9 112,3 4,7 116,9 87,9 74,9 17,2 với cảm biến quán tính, một thuật toán xác định mối quan Lần 5 395,3 381,2 117,4 111,8 5,6 117,4 88,2 74,5 17,3 hệ điểm-đến-điểm từ xa và thuật toán xác định khoảng cách 𝑬𝑹𝑴𝑺 1,29 1,36 0,9 2,22 2,1 0,97 1,19 1,25 0,12 giữa 2 mặt phẳng song song. Các kết quả thí nghiệm chứng minh các thuật toán đã đạt được độ chính xác cho phép 7.2. Thí nghiệm xác định khoảng cách giữa 2 mặt phẳng trong các ứng dụng không cần độ chính xác cao. Để đánh giá độ chính xác của thuật toán xác định Hệ thống đặc biệt hữu ích để đo thông số của các đối khoảng cách giữa 2 mặt phẳng (Mục 6), nhóm tác giả tiến tượng ở những nơi không với tới được. Hệ thống đề xuất hành thí nghiệm đo kích thước của một căn phòng có kích còn có thể được thiết lập bằng cách gắn một cảm biến thước các chiều sâu, rộng và cao tương ứng là khoảng cách vào một chiếc điện thoại thông minh có sẵn 7,03 × 7,9 × 2,86 𝑚. Kết quả của thí nghiệm được thể cảm biến quán tính. hiện trong Bảng 4. Trong đó, thuật toán xác định khoảng cách giữa 2 mặt phẳng được tiến hành lặp lại 3 lần để đo TÀI LIỆU THAM KHẢO các chiều sâu, rộng và cao. Hình 5 thể hiện các điểm trên các mặt phẳng mà hệ thống phát hiện được dùng để ước [1] Y.S. Suh, N.H.Q. Phuong, H.J. Kang, “Distance estimation using inertial lượng các mặt phẳng. Trong đó, các điểm được kí hiệu bởi sensor and vision”, Int. J. Control Autom. Syst., 11 (1), 2013, pp. 211-215. [2] R. Nevatia, “Depth measurement by motion stereo”, Comput. dấu ‘’ được dùng để đo chiều sâu, các điểm được kí hiệu Graph. Image Process, 5 (2), 1976, pp. 203-214. bởi dấu ‘•’ dùng để đo chiều rộng và các điểm được kí hiệu [3] T. Sugimoto, A. Tohshima, “Estimation method for the bởi dấu ‘○’ dùng để đo chiều cao của căn phòng. consciousness level while driving vehicles”, Comput. Stand. Interfaces, 33 (2), 2011, pp. 136-141. [4] LIDAR (Light Detection and Ranging Module) Datasheet, https://cdn.sparkfun.com/datasheets/Sensors/Proximity/lidarlite2D S.pdf. [5] Kuipers, J.B., Quaternions and Rotation Sequences: A Primer with Applications to Orbits, Aerospace, and VirtualReality, Princeton University Press: Princeton, NJ, USA, 1999. [6] Nam, C. N. K., Kang, H. J., Suh, Y. S., “Golf Swing Motion Tracking Using Inertial Sensors and a Stereo Camera”, IEEE Trans. Instrum. Meas, 63, 2014, pp. 943-952. [7] Savage, P. G., “Strapdown Inertial Navigation Integration Algorithm Design Part 1: Attitude Algorithms”, J. Guid. Control Dyn., 21, 1998, pp. 19-28. [8] Markley, F.L., Multiplicative vs. Additive Filtering for Spacecraft Attitude Determination, Proceedings of the 6th Cranfield Conference on Dynamics and Control of Systems and Structures in Space, Hình 5. Các điểm dùng trong ước lượng các mặt phẳng Riomaggiore, Italy, 18–22 July 2004, pp. 467-474. Bảng 4. Kết quả thí nghiệm đo tham số một căn phòng [9] Suh, Y. S., Park, S., Pedestrian Inertial Navigation with Gait Phase Detection Assisted Zero Velocity Updating, Proceedings of the 4th Chiều (m) Sâu Rộng Cao International Conference on Autonomous Robots and Agents, Giá trị thật 7,03 7,90 2,86 Wellington, New Zealand, 10–12 February 2009, pp. 336-341. Lần đo 1 7,08 7,86 2,94 [10] Hawkinson, W., Samanant, P., McCroskey, R., Ingvalson, R., Lần đo 2 6,98 7,87 2,83 Kulkarni, A., Haas, L., English, B. Glanser, Geospatial Location, Lần đo 3 6,98 7,91 2,84 Accountability, and Navigation System for Emergency Responders, Lần đo 4 7,06 7,93 2,82 Proceedings of the Position Location and Navigation Symposium, Myrtle Beach, SC, 24–26 April 2012, pp. 98-105. Lần đo 5 7,02 7,95 2,88 [11] Park, S. K., Suh, Y.S., “A Zero Velocity Detection Algorithm Using 𝐄𝐑𝐌𝐒 0,041 0,035 0,044 Inertial Sensors for Pedestrian Navigation Systems”, Sensors, 2010, Có thể thấy trong Bảng 4, giá trị 𝐸𝑅𝑀𝑆 lớn nhất là 10, pp. 9163-9178. 4,4 cm, trong khi độ dài các chiều là từ 2,8 ~ 8 m. Như vậy, [12] Skog, I., Nilsson, J. O., Handel, P., Evaluation of zero velocity detectors for foot-mounted inertial navigationsystems, Proceedings độ chính xác của thuật toán là có thể chấp nhận được. of the International Conference on Indoor Positioning and Indoor Navigation, Zurich, Switzerland, 15–17 September 2010, pp. 1-6. 8. Kết luận [13] R.G. Brown, P.Y. Hwang, Introduction to Random Signals and Bài báo đã đề xuất hệ thống đo các thông số từ xa sử Applied Kalman Filtering, John Wiley and Sons, New York, NY, USA, 1997. dụng một cảm biến quán tính để ước lượng chuyển động [14] R. Hartley, A. Zisserman, Multiple View Geometry in Computer của hệ thống và một cảm biến khoảng cách (LIDAR) để đo Vision, 2nd ed., Cambridge University Press, Cambridge, UK, 2003. (BBT nhận bài: 02/06/2017, hoàn tất thủ tục phản biện: 15/06/2017)