14- Tổng quát về bản thuyết minh:
3.3. Mobile base và Điều khiển
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 chƣơng trình sau hƣớng dẫn từng bƣớc các cách để lập trình giao tiếp trên Linux theo phƣơng pháp non-canonical.
Đầu tiên ta khai báo các thƣ viện .h của để giao tiếp cổng nối tiếp #include <sys/types.h> #include <sys/stat.h> #include <sys/time.h> #include <fcntl.h> #include <termios.h>
Kế tiếp trong chƣơng trình chính ta khai báo các biến file descriptor của cổng nối tiếp.
int fd = -1;
struct termios newtio;
FILE *fpSerial = NULL;
Kiểm tra việc kết nối với cổng nối tiếp và thoát nếu không tìm thấy thiết bị
fd = open(SERIALPORT, O_RDWR | O_NOCTTY | O_NDELAY );
if ( fd < 0 ) {
ROS_ERROR("serialInit: Could not open serial device %s",SERIALPORT);
return 0; }
Với SERIALPORT trong dòng trên là một macro thay cho tên của cổng nối tiếp trên máy tính đƣợc định nghĩa đầu chƣơng trình, ví dụ: #define SERIALPORT “/dev/ttyUSB0” hay /dev/ttyS1.
O_RDWR, O_NOCTTY, O_NDELAY là các macro trong file fcntl.h chọn chế độ mở modem nối tiếp để đọc ghi dữ liệu trực tiếp (O_RDWR), không có byte điều khiển(O_NOCTTY).
Đoạn chƣơng trình sau thiết lập các chế độ hoạt động 8 bit, không parity, 1 stop bit, không dùng timer xét time out giữa các byte, số byte một lần đọc là 20.
memset(&newtio, 0,sizeof(newtio));
newtio.c_cflag = CS8 | CLOCAL | CREAD; //8 bit, không parity, 1 stop bit
newtio.c_iflag |= IGNBRK; newtio.c_oflag = 0;
newtio.c_cc[VTIME] = 0; newtio.c_cc[VMIN] = 20;
tcflush(fd, TCIFLUSH);
Tiếp theo ta khởi tạo baudrate cho cổng nối tiếp. Ta phải dùng các macro baudrate chuẩn đƣợc mã hóa trong file termios.h mới hợp lệ với hàm cfsetispeed(), giá trị số khi nhập trực tiếp sẽ
không cho đúng giá trị baudrate mong muốn. Cuối cùng ta gán đối tƣợng file descriptor cho biến
fpSerial.
if (cfsetispeed(&newtio, BAUDMACRO) < 0 || cfsetospeed(&newtio, BAUDMACRO) < 0) {
ROS_ERROR("serialInit: Failed to set serial baud rate: %d", 460800);
close(fd);
return 0; }
ROS_INFO("Connection established with %s at %d.", SERIALPORT, BAUDMACRO);
tcsetattr(fd, TCSANOW, &newtio);
tcflush(fd, TCIOFLUSH);
fpSerial = fdopen(fd, "r+");
if (!fpSerial) {
ROS_ERROR("serialInit: Failed to open serial stream %s", SERIALPORT); fpSerial = NULL;
}
Sau khi thiết đặt xong cho đối tƣợng serial port, ta có thể đọc/ghi dữ liệu từ serial port. V í dụ sau đây gửi một frame 8 byte qua serial port với byte đầu là 'f', byte thứ 2 là 'T', 2 byte kế là chứa ID của frame, 2 byte kế tiếp là vận tốc đặt trên bánh trái và 2 byte cuối cùng là vận tốc đặt trên bánh phải. Nếu giá trị trả về res từ hàm write, là số byte thực sự gửi đƣợc bé hơn 8, ta xuất thông báo lỗi ra cửa sổ giám sát.
uint8_t cmd_frame[8];
cmd_frame[0] = 'f'; cmd_frame[1] = 'T';
*(int16_t *)(cmd_frame + 2) = frame_id++; *(int16_t *)(cmd_frame + 4) = velocity_left; *(int16_t *)(cmd_frame + 6) = velocity_right;
int res = write(fd, cmd_frame, 8);
if(res < 8)
Tƣơng tự đoạn chƣơng trình sau đọc dữ liệu nhận đƣợc từ serial port, hàm read sẽ trở về main khi ta đọc đủ số byte bằng RCVTHRESHOLD hoặc nhỏ hơn nếu không còn dữ liệu trong bộ đệm. Nếu giá trị đọc đƣợc bé hơn RCVTHRESHOLD, ta xuất thông báo mất đồng bộ khi truyền nhận frame.
uint8_t rspd_frame[RCVTHRESHOLD];
res = read(fd,rspd_frame, RCVTHRESHOLD);
if(res < RCVTHRESHOLD)
ROS_INFO("frame %d dropped!", pkg_id);
4.2. Transform Broadcaster.
Nhƣ đã trình bày trong phần 3.1.2 về khái niệm hệ tọa độ và chuyển đổi hệ tọa độ trong ROS. Việc chuyển đổi hệ tọa độ trong ROS đƣợc thực hiện dựa trên khái niệm sơ đồ phép chuyển đổi hệ tọa độ. Sau đây ta sẽ trình bày rõ hơn về khái niệm phép chuyển đổi hệ tọa độ và các thao tác cơ bản để lập trình một node cung cấp các phép chuyển đổi hệ tọa độ cho Navigation Stack và các chƣơng trình khác trên ROS.
Giả sử ta có một robot với cảm biến có hệ tọa độ “base_sensor” và mô hình di động “base_link” tại tâm của mỗi phần nhƣ hình thứ nhất. Khoảng cách giữa tâm cảm biến và tâm mobile base đƣợc mô tả trong hình thứ hai. Nếu ta có dữ liệu về một điểm trong không gian có tọa độ (0.3, 0.0, 0.0) trong hệ tọa độ base_sensor nhƣ hình thứ ba, ta cần chuyển đổi dữ liệu từ hệ base_sensor về hệ base_link để robot có thể tránh đƣợc các vật cản trong môi trƣờng. Tóm lại, ta cần phải định nghĩa quan hệ giữa base_link và base_laser qua tf.
Hình 4.1: Ví dụ về hai hệ tọa độ trên robot
Để định nghĩa và lƣu trữ quan hệ giữa base_link và base_sensor bằng tf, ta cần bổ sung chúng vào sơ đồ cây phép chuyển đổi. Quy luật chung cho sơ đồ cây phép chuyển đổi là mỗi node tƣơng ứng
base_sensor
con. Sơ đồ cây đảm bảo chỉ có một mạch liên kết giữa hai tọa độ bất kì và tất cả các nhánh đều hƣớng từ hệ tọa độ mẹ sang con.
Hình 4.2: Giá trị của điểm trong hệ tọa độ cảm biến, cây tọa độ đơn giản và giá trị chuyển đổi trong hệ tọa độ của mobile base.
Hình 4.3: Sơ đồ cây các hệ tọa độ chính của robot tự hành
Đoạn chƣơng trình sau minh họa cho một node làm nhiệm vụ publish một phần của sơ đồ cây phép chuyển đổi hệ tọa độ trên ROS.
#include <ros/ros.h>
#include <tf/transform_broadcaster.h>
int main(int argc, char** argv) {
ros::init(argc, argv, "robot_tf_publisher"); ros::NodeHandle n;
double tilt_angle;
double camera_height;
n.param("tilt_angle", tilt_angle, 0.2705); n.param("camera_height", camera_height, 0.4);
ros::Rate r(50); tf::TransformBroadcaster broadcaster; while(n.ok()) { broadcaster.sendTransform ( tf::StampedTransform ( tf::Transform ( tf::Quaternion(tilt_angle, 0, 0), tf::Vector3(0.0, 0.0, camera_height)
), //A spacial transform
ros::Time::now(), //parent frame
"ngatalie_base", //child frame
"camera_link" ) ); r.sleep(); } }
Trong chƣơng trình trên ta tạo hai thông số cho node robot_tf_publisher là tilt_angle
vàcamera_height. Hai thông số này đƣợc đăng ký với node master của ROS, và có giá trị mặc định là 0.2705 radiance và 0.4 mét. broadcaster là một đối tƣợng có nhiệm vụ publish phép biến đổi kiểu StampedTransform.
Argument đầu tiên của StampedTransform là phép biến đổi kết hợp giữa phép xoay