14- Tổng quát về bản thuyết minh:
3.2. Camera khoảng cách Kinect
3.2.1. Công nghệ và đặc tính
Hình 3.18: Kinect
Kinect là sản phẩm của Microsoft dựa trên công nghệ camera xử lý chùm sáng có cấu trúc (structured-light imaging) đƣợc phát triển bởi PrimeSense, đƣợc tung ra lần đầu tiên vào tháng 11
ngƣời thông qua cử chỉ. Khả năng giao tiếp giữa Kinect với con ngƣời dựa trên hai đặc tính: thông tin về độ sâu (depth map), khả năng phát hiện và bám theo đặc tính cơ thể ngƣời (body skeleton tracking).
Hình 3.19: Những thành phần chính của Kinect
Những thành phần chính của Kinect theo hình 3.19.
(1) Cảm biến độ sâu: độ sâu đƣợc thu về nhờ sự kết hợp của hai cảm biến: đèn chiếu hồng ngoại (IR Projector) và camera hồng ngoại (IR Camera).
(2) RGB camera: là một camera có độ phân giải VGA (640x480), tốc độ tối đa 30 fps.
(3) Động cơ điều khiển góc ngẩng: là loại động cơ DC nhỏ, cho phép điều chỉnh thay đổi góc nhìn của camera, từ đó có thề tập trung vùng nhìn thấy của Kinect vào khu vực quan tâm.
(4) Dãy đa microphone: gồm bốn microphones đƣợc bố trí dọc Kinect, dành cho các ứng dụng điều khiển bằng giọng nói.
Luồng video RGB mặc định của Kinect sử dụng độ phân giải VGA (640x480 pixel), tuy nhiên phần cứng thiết bị có thể cung cấp video có độ phân giải lên đến 1280x1024 ở tốc độ thấp hơn. Giá trị độ sâu của mỗi điểm ảnh có độ phân giải 11 bit. Kinect có vùng nhìn thấy thực tế trong khoảng 0.8 – 4m trong chế độ mặc định và 0.4 – 3m trong chế độ gần. Cảm biến có góc nhìn trong khoảng 57° theo chiều ngang và 43.5° theo chiều dọc cùng với khả năng của motor trên đế có thể ngẩng hoặc cúi camera trong khoảng 27°. Lấy khoảng cách 0.8 từ camera, ta có frame ảnh có kích thƣớc thực tế là 0.87 0.63, với độ phân giải VGA ta có kích thƣớc mỗi pixel
biểu diễn một diện tích chỉ 1.8 mm2, cho thấy độ chính xác hơn của Kinect. Trong ứng dụng robot tự hành trong nhà, robot có thể phát hiện đƣợc các vật thể cao hơn 30cm trong khoảng cách 0.6m.
Hình 3.20: Vùng nhìn thấy của camera khoảng cách Kinect.
0.638m
Hình 3.21: Hình ảnh thực tế từ Kinect trên mobile base
Hình 3.22: Khoảng cách từ tâm mobile base tới vật cản (mỗi ô vuông có kích thƣớc 0.1x0.1 m2
)
Kinect có một hạn chế ở phần nguồn nuôi do không thể lấy trực tiếp nguồn từ cổng USB máy tính mà phải thông qua một adaptor AC sang DC. Để hoạt động trên robot từ hành thiết bị, ta phải thiết kết một nguồn ổn áp DC 12V 1A. Đầu kết nối của Kinect có thiết kế duy nhất nên ta phải cắt lấy đầu này trên adaptor và nối với nguồn DC của robot.
Hình 3.23: Kinect adaptor, connector và supply cable 3.2.2. Package driver và điều khiển cho Kinect
Từ mục đích ban đầu là nhận diện cơ thể ngƣời và xử lý cử chỉ, chức năng đo khoảng cách của kinect đã đƣợc khai thác cho các ứng dụng khác nhƣ quét ảnh 3D, mapping và xác định địa hình di chuyển cho robot. Từ khi xuất hiện, cộng đồng ROS đã nhanh chóng phát triển các package driver cho Kinect do những ƣu thế của Kinect so với các cảm biến khoảng cách đang có nhƣ stereo camera, cảm biến laser scan và cảm biến siêu âm. Trong số các driver, phổ biến nhất là các package dựa trên nền tảng của OpenNI từ PrimeSense là công ty phát triển kỹ thuật đo khoảng cách ứng dụng cho Kinect. Thƣ viện OpenNI đƣợc xem là thƣ viện mạnh nhất hỗ trợ
có thể viết các ứng dụng trên Kinect một cách dễ dàng. Mục đích chính của OpenNI là xây dựng các hàm API chuẩn, cho phép thƣ viện có khả năng kết hợp với các middleware nhằm làm tăng sức mạnh cho Kinect. Một số package ROS sử dụng OpenNI nhƣ openni_camera hỗ trợ truy cập data từ các camera và openni_launch xử lý dữ liệu từ camera cho ra định dạng PointCloud trƣớc khi ứng dụng cho các giải thuật trong ROS. Ngoài ra ta còn có package độc lập kinect_aux có ứng dụng điều khiển góc ngẩng của Kinect.
Hình 3.24: Ứng dụng của openni_launch trong ccny_rgbd tìm visual_odometry và dựng bản đồ 3D với keyframe_mapper
Các driver của Kinect đều hƣớng đến việc xử lý dữ liệu từ camera để cho ra đƣợc định dạng cơ bản là point cloud. Với định dạng này ta có thể ứng dụng các hỗ trợ từ Point Cloud Library
(PCL). PCL là thƣ viện hộ trợ cho việc xử lý ảnh trong không gian 3D. Thƣ viện này đƣợc xây dựng với nhiều giải thuật nhƣ lọc (filtering), khôi phục bề mặt (surface reconstruction), phân vùng (segmentation), ƣớc lƣợng đặc tính vật (feature estimation),…PCL có thể dùng trên nhiều platform nhƣ Linus, MacOS, Windows and Android. Để đơn giản trong việc phát triển, PCL phân thành các thƣ viện nhỏ hơn và có thể biên dịch một cách riêng lẻ.
Hình 3.25: Logo của point cloud
Có thể nói PCL là sự kết hợp của nhiều module nhỏ. Những module này thực chất cũng là các thƣ viện thực hiện chức nẳng riêng lẻ trƣớc khi đƣợc PCL đóng gói. Các thƣ viện cơ bản là:
Eigen: một thƣ viện mở hỗ trợ cho các phép tính toán tuyến tính.
FLANN: (Fast Library for Approximate Nearest Neighbors) hộ trợ trong việc tìm kiếm nhanh các điểm lân cận trong không gian 3D.
Boost: giúp cho việc chia sẻ con trỏ trên tất cả các module và thuật toán trong PCL để tránh việc sao chép trùng lặp dữ liệu đã đƣợc lấy về trong hệ thống.
VTK: (Visualization Toolkit) hỗ trợ cho nhiều platform cho việc thu về dữ liệu 3D, hỗ trợ việc hiển thị và ƣớc lƣợng thể tích vật.
CMinPack: một thƣ viện mở giúp cho việc giải quyết các phép toán tuyến tính và phi tuyến.
3.3. Mobile base và Điều khiển 3.3.1. Thiết kế mô hình di động 3.3.1. Thiết kế mô hình di động
Mô hình di động trong luận văn là một robot lái bằng hiệu tốc độ có kích thƣớc 40x40x35 cm. Bánh xe gắn thẳng trục động cơ, nhằm loại bỏ hiện tƣợng trƣợt khi dùng dây truyền động. Máy tính điều khiển đƣợc đặc ở phần kệ cách sàn 25 cm, và cảm biến Kinect ở phần kệ cao cách mặt đất 35 cm. Phía dƣới là vị trí để mạch điều khiển điều khiển động cơ và các bộ regulator giúp
cấp nguồn 12V cho Kinect, 5V cho kit điều khiển STM32F4 Discovery và 3.3V cho encoder. Các đƣờng màu xanh là các đƣờng tín hiệu, các đƣờng màu cam/vàng là đƣờng power supply.
MOBILE BASE CONTROL
MOBILE BASE CONTROL
STM32F4 DISCOVERY KIT PWM PWM Counter Counter Capture Capture l_left v_left l_right v_right u_left u_right DMA USART TX v_put_left v_put_right USART RX DMA sync/ID v_angular s_right s_left sync/ID v_linear Full Bridge Driver Full Bridge Driver LEFT ENCODER RIGHT ENCODER SUPPLY (LIPO BATTERY) REGULATORS `
Hình 3.26: Sơ đồ chi tiết mạch điều khiển mobile base Cơ khí
Khung robot đƣợc làm bằng nhôm và nhựa để hạn chế khối lƣợng robot. Giá nhôm ở giữa là nơi đặt máy tính. Đế nhựa dƣới cùng đặt mạch điều khiển và pin. Phía đầu là nơi đặt Kinect. Do trọng lƣợng lớn gồm khung robot, máy tính, Kinect, mạch điều khiển và bản thân động cơ, đồng thời do yêu cầu robot di chuyển trong một môi trƣờng bằng phẳng và không cần tốc độ cao để có thể xử lý tránh vật cản di động, ta giới hạn vận tốc là tối đa 20cm/s, cho nên ta cần momen lớn và ổn định ở vận tốc thấp. Robot sử dụng động cơ servo 24V 60W, encoder 13 xung, chiều dài 45mm, khối lƣợng 0.85kg mỗi động cơ. Động cơ có bộ giảm tốc planet hệ số 19.2.
Hình 3.27: Thiết khung robot
Hình 3.29: Hình ảnh thực tế của robot Điều khiển
Giải thuật điều khiển của mô hình robot differential drive đƣợc triển khai trên vi điều khiển nhƣ sau: PI(z) LEFT MOTOR u_left v_linear PI(z) RIGHT MOTOR v_left v_right I(z) + - + + + -+ u_right - - v_angular
Đại lƣợng cần điều khiển là vận tốc của mỗi động cơ. Mỗi động cơ có một bộ điều khiển PI riêng. Giá trị đặt của bộ điều khiển là động cơ v_linear và v_angular đƣợc gởi từ máy tính. Bộ điều khiển PI sẽ tính toán giá trị điều khiển u_left, u_right, giá trị điều khiển thực tế trong mô hình phải đƣợc đƣợc xén bớt để robot không bị mất kiểm soát.
3.3.2. Thiết kế chƣơng trình nhúng
Chƣơng trình nhúng trên MCU sử dụng tối đa chức năng của các ngoại vi hardware trên STM32F4 cho việc truyền dữ liệu, đọc cảm biến, timing để hạn chế ngắt dành tối đa hiệu suất của bộ xử lý cho giải thuật điều khiển.
ID V_PUT_LEFT DMA INPUT PROCESSING CONTROL & PUBLISHING CMD ID L_LEFT L_RIGHT USART RX CMD V_PUT_RIGHT RX Not Empty ENCODER COUNT SPEED scheduler V_LEFT V_RIGHT TIME STAMP
DMA USART RX TX Not Empty CPU WORKLOAD
Các tính năng của STM32F4 đƣợc khai thác gồm có:
- Vi điều khiển ARM Cortex M4 với tốc độ 168MHz, bộ xử lý số thực FPU và timer hệ thống System Tick đảm bảo tính real time cho hệ thống khi tần số lấy mẫu của bộ điều khiển vòng kín là 200Hz ( ).
- USART với DMA: Giao tiếp giữa node serial server và MCU thông qua các frame dữ liệu tối giản. Frame truyền từ máy tính có nội dung là CMD – mode điều khiển, ID – số nhận diện đƣợc lặp lại trong frame phản hồi từ MCU để đồng bộ và V_PUT_LEFT, V_PUT_RIGHT là các số nguyên có dấu mang vận tốc đặt trên mỗi bánh, đơn vị là số đếm encoder mỗi giây. Frame phản hồi từ MCU lặp lại giá trị CMD và ID và các giá trị trạng thái của từng bánh nhƣ quãng đƣờng đi đƣợc, vận tốc trên từng bánh và time stamp do kernel của hệ điều hành thời gian thực freeRTOS quản lý. Ngoại vi USART của STM32F4 cho phép truyền dữ liệu theo chuẩn RS232 với tốc độ baud lên đến hàng Mbps và đƣợc kết nối với bộ DMA. Cơ chế này tự động lấy dữ liệu thu đƣợc về vùng nhớ RAM của các biến, hay từ vùng nhớ chứa các biến về ngoại vi. CPU chỉ cần truy cập các biến và xử lý trong giải thuật, hoàn toàn không dùng chƣơng trình ngắt để lấy dữ liệu nhận đƣợc từ thanh ghi USART RX hay gửi dữ liệu tới USART TX.
- Timer với chức năng Encoder Interface – Input Capture – PWM Generator: STM32F4 có 14 bộ timer, chƣa kể các bộ timer WatchDog và SysTick với cấu trúc phức tạp có tốc độ cao. Hầu hết timer đều đa chức năng, có thể đƣợc thiết đặt thành một trong ba ngoại vi điều khiển động cơ. Với một MCU STM32F4, mỗi động cơ có một timer giải mã encoder, một timer capture tốc độ và một timer để phát xung PWM. Cho phép ta thiết đặt đầy đủ các chức năng điều khiển động cơ để thu thập thông tin về đƣờng đi, tốc độ và điều khiển tự động và độc lập. Việc giải mã encoder hoàn toàn tự động và chƣơng trình chỉ cần truy cập tới các thanh ghi hoạt động của Timer để lấy thông tin về trạng thái của các động cơ, hoàn toàn không dùng tới ngắt.
Hình 3.32: Sơ đồ khối của Timer điều khiển nâng cao (Advanced-control Timer) của STM32F4
Encoder Interface: Cho phép ta thiết lập bộ giải mã xung encoder 2 kênh. Từ đó có tể biết đƣợc chuyển động tuyệt đối cùng chiều quay của động cơ. Với phƣơng pháp giải mã tổng quát nhƣ hình dƣới, bộ giải mã encoder có thể phân biệt đƣợc cạnh lên và xuống trên mỗi kênh A,B. Từ đó tăng số lƣợng xung đếm đƣợc lên đến 4 lần. Với động cơ planet 13 xung, hệ số bánh răng là 19.2 và bộ giải mã có thể nhân bốn, có số xung mỗi vòng ở trục ra động cơ là 1000 xung, đảm bảo điều khiển chính xác động cơ.
Hình 3.33: Giản đồ xung giải mã quadrature encoder của Encoder Interface trên STM32F4
Input Capture: Bộ Timer /Counter trên STM32F4 có thể đƣợc thiết đặt với chức năng đo thời gian giữa các xung encoder, cho phép ta đo trực tiếp tốc độ động cơ. Đầu vào của bộ Timer/Counter cho phép ta XOR các tín hiệu từ kênh A và kênh B của encoder, làm tăng gấp đôi độ phân giải của tốc độ. Lợi ích thực tế của việc này là, khi robot có tốc độ chậm, sự kiện capture có thể không xảy ra trong chu kỳ lấy mẫu, làm cho giải thuật điều khiển hiểu nhầm thành tốc độ bằng không. Việc tăng độ phân giải làm tăng số lần capture trên mỗi đơn vị thời gian, giúp cho việc điều khiển ổn định hơn.
PWM Generator: Bộ Timer /Counter có thể đƣợc thiết đặt làm bộ phát xung PWM có thể đảo chiều động cơ bằng cách điều khiển mỗi nửa cầu riêng biệt bằng hai trong bốn ngõ ra của bộ PWM. Khi giá trị đặt đảo dấu, ngõ ra hiện thời bị bất hoạt và ngõ ra còn lại đƣợc kích hoạt.
Hình 3.34: Khối XOR ngõ vào giúp tăng đôi tần số encoder và các ngõ ra PWM giúp điều khiển đảo chiều động cơ
CHƢƠNG IV – LẬP TRÌNH TÍCH HỢP NAVIGATION STACK TRÊN MÔ HÌNH ROBOT
Để Navigation Stack hoạt động, ta cần phải thiết kế các node đặc thù hỗ trợ cho Navigation Stack cho mô hình robot trong đề tài. Chƣơng sau đây sẽ đề cập các thao tác lập trình thực tế để ứng dụng Navigation Stack.
Xem thêm phụ lục 1 về cách tạo một package mới trong ROS và sử dụng Môi trƣờng Phát triển Tích hợp (Integrated Development Environment – IDE) để lập trình cho ROS.
4.1. Serial Server cho Mobile Base
Nhƣ đã giới thiệu ở mục 3.1.3 để điều khiển mobile base, ta cần node serial_server thực hiện thao tác subscribe tới topic “cmd_vel” đƣợc publish từ move_base và chuyển đổi các giá trị vận tốc dài và vận tốc xoay trong topic này sang giá trị vận tốc trên mỗi bánh xe. “cmd_vel” là một message kiểu geometry_msgs/Twist đƣợc định nghĩa trong thƣ viện std_msgs.h của ROS. Khai báo của
geometry_msgs/Twist trong std_msgs.h nhƣ sau:
geometry_msgs/Vector3 linear geometry_msgs/Vector3 angular
Với geometry_msgs/Vector3 là một bộ ba số thực 64 bit float64 ứng với ba trục x, y, z. Nếu ta thiết đặt thông số holonomic_robot với giá trị falsecho base_local_planner, move_base sẽ xuất “cmd_vel” tƣơng thích với mô hình differential drive, với thành phần y và z của linear
bằng 0 và thành phần x, y của angular cũng bằng 0.
ROS hỗ trợ ngôn ngữ lập trình C++ và Python, đoạn chƣơng trình sau nên đƣợc đặt trong hàm main() của package serial_server, ta khai báo một ros::Subscriber để đăng ký nhận tin từ topic “cmd_vel”, kích thƣớc queue để lƣu message này là 1.
ros::NodeHandle rosNode; ros::Subscriber ucCommandMsg;
ucCommandMsg = rosNode.subscribe<geometry_msgs::Twist> (
"cmd_vel", 1,
ucCommandCallback là hàm callback để xử lý giá trị trong message “cmd_vel” đƣợc gửi tới. Trong hàm main() ta gọi hàm ros::spinOnce() để kiểm tra queue có đang chứa message, nếu queue đang có dữ liệu, hàm callback sẽ đƣợc gọi. Sử dụng phƣơng trình 2.11. Đoạn chƣơng trình sau minh họa hàm ucCommandCallback:
#define L_div_2 0.2 //Distance between 2 wheels
#define M_TO_ENC 10000/3.141592654 //Conversion from m/s to encoder_counts/s
double velocity_left, velocity_right;
void ucCommandCallback(const geometry_msgs::Twist::ConstPtr& msg)
{
//Do some conversion to send speed values to MCU
velocity_left = (int16_t)((msg->linear.x - msg->angular.z*L_div_2)*M_TO_ENC); velocity_right = (int16_t)((msg->linear.x + msg->angular.z*L_div_2)*M_TO_ENC); ROS_INFO("ROS Command:\n\
left_speed: %d\n\ right_speed: %d", velocity_left, velocity_right); }
serial_server là một node trung gian có nhiệm vụ trao đổi dữ liệu hai chiều giữa mobile base và Navigation Stack. Để thực hiện chức năng này ta phải lập trình giao tiếp cổng nối tiếp trên máy tính. ROS có hỗ trợ giao tiếp qua cổng nối tiếp với package rosserial, tuy nhiên rosserial có một protocol riêng và chỉ mới đƣợc ứng dụng với dòng MCU Arduino. Việc lập trình lại phƣơng pháp cổng nối tiếp không quá phức tạp, giúp tối ƣu hóa việc truyền nhận cho các nền tảng robot mà ta thiết kế, sau đây là một số thao tác lập trình cần thiết để tạo serial_server.
Cổng nối tiếp trên nền tảng Linux có hai chế độ xử lý dữ liệu nhận đƣợc là canonical và non- canonical và việc giao tiếp đƣợc thực hiện qua khái niệm đối tƣợng file mô tả - file descriptor. Đối với kiểu giao tiếp canonical, một lệnh đọc sẽ chỉ trả về một dòng (line) đầy đủ. Một dòng mặc định đƣợc kết thúc bằng một ký tự NL (ASCII LF), là ký tự kết thúc file, hoặc kết thúc dòng văn bản, ngoài ra các giá trị ASCII không in đƣợc sẽ đƣợc dùng để điều khiển chế độ hoạt động của cổng. Chế độ non- canonical là chế độ gửi dữ liệu dạng nhị phân, số byte mỗi lần đọc là không đổi và biết trƣớc. Đoạn