Nghiên cứu hệ thống xây dựng bản đồ, lập quỹ đạo và điều khiển bám quỹ đạo cho robot tự hành bốn bánh đa hướng

Nghiên cứu hệ thống xây dựng bản đồ, lập quỹ đạo và điều khiển bám quỹ đạo cho robot tự hành bốn bánh đa hướng.Nghiên cứu hệ thống xây dựng bản đồ, lập quỹ đạo và điều khiển bám quỹ đạo cho robot tự hành bốn bánh đa hướng.Nghiên cứu hệ thống xây dựng bản đồ, lập quỹ đạo và điều khiển bám quỹ đạo cho robot tự hành bốn bánh đa hướng.Nghiên cứu hệ thống xây dựng bản đồ, lập quỹ đạo và điều khiển bám quỹ đạo cho robot tự hành bốn bánh đa hướng.Nghiên cứu hệ thống xây dựng bản đồ, lập quỹ đạo và điều khiển bám quỹ đạo cho robot tự hành bốn bánh đa hướng.Nghiên cứu hệ thống xây dựng bản đồ, lập quỹ đạo và điều khiển bám quỹ đạo cho robot tự hành bốn bánh đa hướng.Nghiên cứu hệ thống xây dựng bản đồ, lập quỹ đạo và điều khiển bám quỹ đạo cho robot tự hành bốn bánh đa hướng.Nghiên cứu hệ thống xây dựng bản đồ, lập quỹ đạo và điều khiển bám quỹ đạo cho robot tự hành bốn bánh đa hướng.Nghiên cứu hệ thống xây dựng bản đồ, lập quỹ đạo và điều khiển bám quỹ đạo cho robot tự hành bốn bánh đa hướng.Nghiên cứu hệ thống xây dựng bản đồ, lập quỹ đạo và điều khiển bám quỹ đạo cho robot tự hành bốn bánh đa hướng.

BỘ GIÁO DỤC VÀ ĐÀO TẠO i BỘ QUỐC PHÒNG VIỆN KHOA HỌC VÀ CÔNG NGHỆ QUÂN SỰ ĐỖ QUANG HIỆP NGHIÊN CỨU HỆ THỐNG XÂY DỰNG BẢN ĐỒ, LẬP QUỸ ĐẠO VÀ ĐIỀU KHIỂN BÁM QUỸ ĐẠO CHO ROBOT TỰ HÀNH BỐN BÁNH ĐA HƯỚNG Ngành: Kỹ thuật điều khiển tự động hóa Mã số: 52 02 16 LUẬN ÁN TIẾN SĨ KỸ THUẬT Hà Nội – Năm 2022 BỘ GIÁO DỤC VÀ ĐÀO TẠO ii BỘ QUỐC PHÒNG VIỆN KHOA HỌC VÀ CÔNG NGHỆ QUÂN SỰ ĐỖ QUANG HIỆP NGHIÊN CỨU HỆ THỐNG XÂY DỰNG BẢN ĐỒ, LẬP QUỸ ĐẠO VÀ ĐIỀU KHIỂN BÁM QUỸ ĐẠO CHO ROBOT TỰ HÀNH BỐN BÁNH ĐA HƯỚNG Ngành: Kỹ thuật điều khiển tự động hóa Mã số: 52 02 16 LUẬN ÁN TIẾN SĨ KỸ THUẬT NGƯỜI HƯỚNG DẪN KHOA HỌC: TS Lê Trần Thắng GS.TS Phan Xuân Minh Hà Nội – Năm 2022 i LỜI CAM ĐOAN Tơi xin cam đoan cơng trình nghiên cứu riêng tơi Các kết trình bày luận án trung thực chưa công bố cơng trình khác, liệu tham khảo trích dẫn đầy đủ Hà nội, ngày … tháng … năm 2022 Tác giả luận án Đỗ Quang Hiệp ii LỜI CẢM ƠN Lời đầu tiên, tác giả xin gửi lời cảm ơn sâu sắc tới TS Lê Trần Thắng GS.TS Phan Xuân Minh tận tình hướng dẫn tác giả suốt trình thực luận án Tác giả xin gửi lời trân trọng cảm ơn tới Ban Giám đốc Viện Khoa học Công nghệ Quân sự, Thủ trưởng cán bộ, nhân viên Phòng Đào tạo, Thủ trưởng Viện Tự động hóa KTQS thầy, giáo truyền đạt kiến thức, động viên, giúp đỡ tạo môi trường nghiên cứu thuận lợi để tác giả hồn thành cơng trình Tác giả xin cảm ơn tới Ban Giám hiệu trường Đại học Kinh tế- Kỹ thuật Công nghiệp, Ban Chủ nhiệm khoa Điện đồng nghiệp nơi công tác tạo điều kiện tốt để luận án hoàn thành Cuối cùng, tác giả xin gửi tới gia đình, bạn bè, người thân lời cảm ơn sâu sắc động viên, giúp đỡ đồng hành tác giả suốt thời gian qua Hà Nội, ngày tháng năm 2022 Tác giả luận án iii DANH MỤC CÁC KÝ HIỆU, CÁC CHỮ VIẾT TẮT (x, y,θ ) q = [x Hệ tọa độ toàn cục y θ] v = vx v y T Véc tơ toạ độ robot hệ tọa độ toàn cục ω T  Véc tơ vận tốc hệ tọa độ liên kết θ Góc lệch robot so hệ tọa độ gốc (rad) vi Vận tốc dài bánh xe (m/s) ωi Vận tốc góc bánh xe (rad/s) fi Lực kéo bánh (N) M Khối lượng robot (kg) d Khoảng cách tâm robot tới bánh (m) H Ma trận chuyển hệ trục tọa độ r Bán kính bánh xe (m) M (q) Ma trận khối lượng momen quán tính V Ma trận lực hướng tâm Corilis, V (q, q) = C Ma trận hệ số lực ma sát nhớt G Ma trận hệ số lực ma sát coulomb G(q) Véc tơ trọng lực, G(q) = Sig(v) Véc tơ hàm dấu phụ thuộc vận tốc τd Véc tơ thành phần nhiễu bất định τ Véc tơ tín hiệu đưa từ điều khiển e1 Véc tơ sai lệch bám quỹ đạo robot e2 Véc tơ sai lệch bám vận tốc robot J Mơ men qn tính xe (N.m) q Biến khớp F Lực ma sát (N) n Số bánh xe robot ℜ Ma trận trọng số mạng Nơ-ron iv x1d T Quỹ đạo đặt Thời gian trích mẫu (sec) Hệ số mặt trượt λ τeq Tín hiệu điều khiển giữ trạng thái hệ thống mặt trượt τsw Tín hiệu điều khiển, lái trạng thái hệ thống mặt trượt C1,C2 , C3 Ma trận hệ số điều khiển DSC Θ Véc tơ chứa thành phần bất định mơ hình robot S Véc tơ mặt trượt h(S) Véc tơ hàm dấu Chuẩn ma trận bậc F AGV AI AMCL API DSC DWA EB EKF FLS Chuẩn ma trận bậc không gian F Xe vận tải tự động (Autonomous Guided Vehicles) Trí tuệ nhân tạo (Artificial Intelligence) Phương pháp đồ hóa cục Monte Carlo (Adaptive Monte Carlo Localization) Giao diện lập trình ứng dụng (Application Programming Interface) Mặt trượt động (Dynamic Sliding Surface) Thuật toán cửa sổ động (Dynamic Window Approach) Dải đàn hồi (Elastic Band) Bộ lọc Kalman mở rộng (Extended Kalman Filter) Hệ logic mờ (Fuzzy Logic System) FWO Bốn bánh xe đa hướng (Four Wheels Omni) FWOMR Robot tự hành bốn bánh xe đa hướng (Four Wheeled Omnidirectional Mobile Robot) IMU Khối đo lường quán tính (Inertial Measurement Unit) KF Bộ lọc Kalman (Kalman Filter) MAE Sai số tuyệt đối trung bình (Mean Absolute Error) MAP Ước tính tối đa hậu nghiệm (Maximum A Posteriori) MMSE Ước lượng sai số bình phương trung bình tối thiểu (Minimum Mean Square Error) MPC Bộ điều khiển dự báo (Model Preditive Control) MSSC Bộ điều khiển đa mặt trượt (Multi Surface Sliding Control) NMPC Bộ điều khiển dự báo phi tuyến (Nonlinear Model Preditive Control) ODE Phương trình vi phân thơng thường (Ordinary Differential Equation) OMR Robot tự hành đa hướng (Omnidirectional Mobile Robot) PFM Phương pháp trường (Potential Field Methods) PID Bộ điều khiển tỉ lệ vi tích phân (Proportional Integral Derivative) RBFNN Mạng Nơ-ron bán kính xuyên tâm (Radial Basic Function Neural Network) ROS Hệ điều hành robot (Robot Operating System) SLAM Thuật tốn định vị đồ hóa đồng thời (Simultaneous Localization And Mapping) SMC Điều khiển trượt (Sliding Mode Control) SQP Phương pháp lập trình tồn phương liên tiếp (Sequential Quadratic Programming) QP Phương pháp lập trình tồn phương (Quadratic Programming) TEB Dải đàn hồi thời gian (Timed Elastic Band) URDF Gói định dạng mơ tả robot hợp (Unified Robot Description Format) VSS Hệ thống có cấu trúc thay đổi (Variable Structure System) DANH MỤC CÁC BẢNG Trang Bảng 1.1 Bảng thông số FWOMR 11 Bảng 2.1 Bảng đánh giá độ lệch chuẩn đồ 63 Bảng 2.2 Bảng đánh giá độ lệch chuẩn vị trí robot: 63 Bảng 3.1 Bảng tham số vận tốc theo x 93 Bảng 3.2 Bảng tham số vận tốc theo trục y 94 Bảng 3.3 Bảng tham số vận tốc theo trục ω 94 Bảng 3.4 Sai lệch theo phương x 95 Bảng 3.5 Sai lệch theo phương y 95 Bảng 3.6 Chi phí tính tốn trung bình miền dự báo 96 Bảng 3.7 Bảng so sánh phương pháp 98 Bảng 3.8 Sai lệch bám quỹ đạo 102 Bảng 4.1 Thời gian tiến hành thực nghiệm 117 Bảng 4.2 Thời gian tiến hành thực nghiệm tránh vật cản động 120 DANH MỤC CÁC HÌNH VẼ Trang Hình 1.1 Robot tự hành sử dụng bánh đa hướng Hình 1.2 Một số ứng dụng robot tự hành dạng holonom Hình 1.3 Cấu trúc bánh xe Omni Hình 1.4 Bánh xe Omni Hình 1.5 Các hướng di chuyển robot .10 Hình 1.6 Biểu diễn O ' x ' y ' hệ trục tọa độ Oxy 11 Hình 1.7 Sơ đồ tổng quan hệ thống robot tự hành 18 Hình 1.8 Sơ đồ mơ tả miền dự báo tín hiệu 26 Hình 1.9 Sơ đồ khối điều khiển MPC 27 Hình 1.11 Cấu trúc hệ điều hành ROS 28 Hình 1.12 Hệ thống files ROS 29 Hình 1.13 Sơ đồ giao tiếp nút 30 Hình 2.1 Phương pháp phối hợp đa cảm biến sử dụng kỹ thuật Bayesian 41 Hình 2.2 Sơ đồ thuật tốn lọc Kalman chuẩn rời rạc 43 Hình 2.3 Sơ đồ thuật tốn lọc Kalman mở rộng 45 Hình 2.4 Kỹ thuật SLAM 48 Hình 2.5 Quy trình thuật toán EKF-SLAM 51 Hình 2.6 Giao diện mơ thuật tốn EKF-SLAM 61 Hình 2.7 Tiến hành chạy thuật toán 61 Hình 2.8 Kết đồ thu 62 Hình 2.9 Độ lệch chuẩn đồ 62 Hình 2.10 Độ lệch chuẩn vị trí 63 Hình 2.11 Mơ hình mô cho FWOMR 64 Hình 2.12 Mơ hình 3D cho FWOMR mơi trường mơ 64 Hình 2.13 Q trình SLAM robot 65 Hình 2.14 Bản đồ ảo xây dựng Gazebo 65 Hình 2.15 Bản đồ thu Rviz 66 Hình 2.16 Bản đồ hố mơi trường 66 Hình 2.17 Mơi trường hoạt động FWOMR Rviz 67 Hình 2.18 Hình ảnh mơi trường thực tế 68 Hình 3.1 Cấu trúc gói điều hướng 71 Hình 3.2 Sơ đồ lập kế hoạch di chuyển 72 Hình 3.3 Biểu đồ khoảng cách an tồn cục (Inflation costmap) .75 Hình 3.4 Minh hoạ thuật toán A* 77 Hình 3.5 Sự thay đổi vị trí robot q trình di chuyển 78 Hình 3.6 Quỹ đạo tính tốn TEB 80 Hình 3.7 Khoảng cách ngắn TEB với vật cản (hoặc điểm đặt) .81 Hình 3.8 Cấu trúc thuật toán lập kế hoạch di chuyển cục TEB 82 Hình 3.9 Sơ đồ cấu trúc điều khiển MPC 83 Hình 3.10 Quỹ đạo di chuyển robot với miền dự báo 92 Hình 3.11 Các tham số điều khiển 93 Hình 3.12 Sai lệch bám robot miền dự báo 94 Hình 3.13 Quỹ đạo bám robot 96 Hình 3.14 So sánh quỹ đạo bám điều khiển .97 Hình 3.15 So sánh sai lệch bám điều khiển .98 Hình 3.16 Sơ đồ khối điều khiển MPC ROS 99 Hình 3.17 Quỹ đạo bám theo phương xy (Rviz) 100 Hình 3.18 Đầu tín hiệu điều khiển robot 101 Hình 3.19 Sai lệch bám quỹ đạo robot 101 Hình 4.1 Kết cấu FWOMR 104 Hình 4.2 Cấu trúc phần cứng điều khiển FWOMR 105 Hình 4.3 Nút lập đồ vị trí sử dụng ROS 106 Hình 4.4 Cấu trúc thiết lập gói Gmapping 107 Hình 4.5 Quỹ đạo toàn cục 108 Hình 4.6 Tham số inflation_radius=1,5 109 Hình 4.7 Thơng số inflation_radius=0,55 110 Hình 4.8 Mơi trường thực nghiệm cho robot 110 Hình 4.9 Bản đồ ảo xây dựng Gazebo 111 f = Function('f',{states,controls},{rhs}); % Nonlinear mapping function f(x,u) U = SX.sym('U',n_controls,N); % Control variables P = SX.sym('P',n_states + N*(n_states + n_controls)); X = SX.sym('X',n_states,(N+1)); obj = 0; g = []; % Objective function % constraints vector st = X(:,1); g = [g;st-P(1:3)]; % Initial state % Initial condition constraints % Euler discretization for k = 1:N st = X(:,k); = U(:,k); obj = obj+(st-P(6*k-2:6*k+0))'*Q*(st-P(6*k-2:6*k+0)) + (con-P(6*k+1:6*k+3))'*R*(con-P(6*k+1:6*k+3)) ; % calculate obj end % The number is (n_states+n_controls) st_next = X(:,k+1); f_value = f(st,con); st_next_euler = st+ (T*f_value); g = [g;st_next-st_next_euler]; % compute constraints % Make the decision variable one column vector OPT_variables = [reshape(X,3*(N+1),1);reshape(U,3*N,1)]; nlp_prob = struct('f', obj, 'x', OPT_variables, 'g', g, 'p', P); opts = struct; opts.ipopt.max_iter = 2000; opts.ipopt.print_level =0;%0,3 opts.print_time = 0; opts.ipopt.acceptable_tol =1e-8; opts.ipopt.acceptable_obj_change_tol = 1e-6; solver = nlpsol('solver', 'ipopt', nlp_prob, opts); args = struct; args.lbg(1:3*(N+1)) = 0; args.ubg(1:3*(N+1)) = 0; % Equality constraints % Equality constraints args.lbx(1:3:3*(N+1),1) = x_min; % State x lower bound args.ubx(1:3:3*(N+1),1) args.lbx(2:3:3*(N+1),1) args.ubx(2:3:3*(N+1),1) args.lbx(3:3:3*(N+1),1) bound args.ubx(3:3:3*(N+1),1) bound = = = = x_max; y_min; y_max; theta_min; = theta_max; args.lbx(3*(N+1)+1:3:3*(N+1)+3*N,1) lower bound args.ubx(3*(N+1)+1:3:3*(N+1)+3*N,1) upper bound args.lbx(3*(N+1)+2:3:3*(N+1)+3*N,1) lower bound args.ubx(3*(N+1)+2:3:3*(N+1)+3*N,1) upper bound args.lbx(3*(N+1)+3:3:3*(N+1)+3*N,1) lower bound args.ubx(3*(N+1)+3:3:3*(N+1)+3*N,1) upper bound % % % % State State State State x upper bound y lower bound y upper bound theta lower % State theta upper = vx_min; % vx = vx_max; % vx = vy_min; % vy = vy_max; % vy = omega_min; % omega = omega_max; % omega function y = nmpc( u ) %NMPC cur_time = u(1); state = u(2:4); ref = u(5:10); args.p(1:3) = state; for k = 1:N %new - set the reference to track t_pred = cur_time + (k-1)*T; pred_ref = reference(t_pred); x_ref = pred_ref(1); y_ref = pred_ref(2); theta_ref = pred_ref(3); vx_ref = pred_ref(4); vy_ref = pred_ref(5); omega_ref = pred_ref(6); end args.p(6*k-2:6*k+0) = [x_ref, y_ref, theta_ref]; args.p(6*k+1:6*k+3) = [vx_ref, vy_ref, omega_ref]; % initial value of the optimization variables args.x0 = [reshape(X0',3*(N+1),1);reshape(u0',3*N,1)]; sol = solver('x0', args.x0, 'lbx', args.lbx, 'ubx', args.ubx, 'lbg', args.lbg, 'ubg', args.ubg,'p',args.p); u = reshape(full(sol.x(3*(N+1)+1:end))',3,N)'; y = u(1,:); end Code ROS (thử nghiệm chương thực nghiệm chương 4): #include " /acado_mpc_export/acado_common.h" #include " /acado_mpc_export/acado_auxiliary_functions.h" #include "Eigen-3.3/Eigen/Core" #include "Eigen-3.3/Eigen/QR" #include #include #include // #include "acado.h" // ROS #include #include #include #include #include #include "geometry_msgs/Vector3.h" #include "geometry_msgs/Quaternion.h" #include "tf/transform_datatypes.h" using namespace std; /* Some convenient definitions */ #define NX ACADO_NX /* Number of differential state variables */ #define NXA ACADO_NXA /* Number of algebraic variables */ #define NU ACADO_NU /* Number of control inputs */ #define NOD ACADO_NOD /* Number of online data values */ #define NY ACADO_NY /* Number of measurements/references on nodes N - */ #define NYN ACADO_NYN /* Number of measurements/references on node N */ #define N horizon */ ACADO_N /* Number of intervals in the #define NUM_STEPS iterations */ #define VERBOSE */ 10 /* Number of real-time /* Show iterations: 1, silent: #define Ts 0.1 // sampling time #define Lf 1.0 /* Global variables used by the solver */ ACADOvariables acadoVariables; ACADOworkspace acadoWorkspace; // MPC init functions vector init_acado(); void init_weight(); vector run_mpc_acado(vector states, vector ref_states, vector previous_u); vector calculate_ref_states(const vector &ref_x, const vector &ref_y, const vector &ref_q, const double &reference_vx, const double &reference_vy, const double &reference_w); vector motion_prediction(const vector &cur_states, const vector &prev_u); vector update_states(vector state, double vx_cmd, double vy_cmd, double w_cmd); /* ROS PARAMS*/ double weight_x, weight_y, weight_q, weight_vx, weight_vy, weight_w; nav_msgs::Odometry odom; void stateCallback(const nav_msgs::Odometry& msg) { odom = msg; } nav_msgs::Path path; void pathCallback(const nav_msgs::Path& msg) { path = msg; } bool is_target(nav_msgs::Odometry cur, double goal_x, double goal_y) { if(abs(cur.pose.pose.position.x - goal_x) < 0.05 && abs(cur.pose.pose.position.y - goal_y) < 0.05) { return true; } else return false; } float quaternion2Yaw(geometry_msgs::Quaternion orientation) { double q0 = orientation.x; double q1 = orientation.y; double q2 = orientation.z; double q3 = orientation.w; float yaw = atan2(2.0*(q2*q3 + q0*q1), 1.0 - 2.0*(q1*q1 + q2*q2)); return yaw; } int main(int argc, char **argv) { ros::init(argc, argv, "mpc_node"); ros::NodeHandle nh("~"); ros::Subscriber state_sub = nh.subscribe("/odom", 10, stateCallback); ros::Subscriber path_sub = nh.subscribe("/path", 1, pathCallback); ros::Publisher vel_pub = nh.advertise("/cmd_vel", 10); ros::Publisher predict_pub = nh.advertise("/predict_path", 10); ros::Publisher odom_path_pub = nh.advertise("/odom_path_pub", 10); /* ROS PARAM */ ros::param::get("~weight_x", weight_x); ros::param::get("~weight_y", weight_y); ros::param::get("~weight_q", weight_q); ros::param::get("~weight_vx", weight_vx); ros::param::get("~weight_vy", weight_vy); ros::param::get("~weight_w", weight_w); cout