(LUẬN án TIẾN sĩ) 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 an an tien si TIEU LUAN MOI download : skknchat123@gmail.com mo 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 an an tien si TIEU LUAN MOI download : skknchat123@gmail.com mo 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 an an tien si TIEU LUAN MOI download : skknchat123@gmail.com mo 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, cô 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 an an tien si TIEU LUAN MOI download : skknchat123@gmail.com mo iii MỤC LỤC Trang DANH MỤC CÁC KÝ HIỆU, CÁC CHỮ VIẾT TẮT vi DANH MỤC CÁC BẢNG ix DANH MỤC CÁC HÌNH VẼ x MỞ ĐẦU .1 Chương TỔNG QUAN VỀ ROBOT TỰ HÀNH BỐN BÁNH ĐA HƯỚNG .6 1.1 Tổng quan mơ hình hóa FWOMR 1.1.1 Tổng quan FWOMR .6 1.1.2 Mơ hình hóa FWOMR 1.1.3 Cấu trúc hệ thống điều khiển robot tự hành 17 1.2 Một số thuật tốn điều khiển bám quỹ đạo cho FWOMR thơng dụng 19 1.2.1 Bộ điều khiển PID cho FWOMR 20 1.2.2 Bộ điều khiển trượt cho FWOMR .21 1.2.3 Thuật toán điều khiển mặt trượt động cho FWOMR 24 1.2.4 Thuật tốn điều khiển dự báo tựa mơ hình (MPC) 26 1.3 Hệ điều hành ROS 28 1.3.1 Giới thiệu ROS 28 1.3.2 Mục tiêu ROS 28 1.3.3 Các hoạt động ROS 29 1.3.4 Sự tương tác nút ROS .30 1.4 Tổng quan tình hình nghiên cứu ngồi nước .31 1.4.1 Tình hình nghiên cứu ngồi nước 31 1.4.2 Tình hình nghiên cứu nước 35 1.5 Kết luận chương 38 Chương THUẬT TỐN BẢN ĐỒ HĨA MƠI TRƯỜNG VÀ ĐỊNH VỊ ĐỒNG THỜI CHO FWOMR 40 2.1 Hệ thống nhận thức FWOMR .40 2.1.1 Phương pháp suy luận xác suất để nâng cao độ tin cậy cho toán định vị robot 40 an an tien si TIEU LUAN MOI download : skknchat123@gmail.com mo iv 2.1.2 Phương pháp sử dụng lọc Kalman cho toán định vị robot 42 2.2 Thuật toán SLAM 47 2.3 Thuật toán EKF-SLAM 50 2.3.1 Bước dự đoán (Prediction) 53 2.3.2 Bước chỉnh sửa (Correction) 55 2.4 Kết mô 60 2.4.1 Mơ thuật tốn EKF-SLAM Matlab .60 2.4.2 Kết mô trên phần mềm Gazebo 63 2.4.3 Kết thực nghiệm thuật tốn EKF-SLAM mơi trường thực tế 67 2.5 Kết luận chương 68 Chương THIẾT KẾ HỆ THỐNG ĐIỀU HƯỚNG, ĐIỀU KHIỂN BÁM QUỸ ĐẠO CHO FWOMR .70 3.1 Hệ thống điều hướng cho FWOMR 70 3.1.1 Lập kế hoạch di chuyển toàn cục 76 3.1.2 Lập kế hoạch di chuyển cục .78 3.2 Xây dựng điều khiển bám quỹ đạo cho FWOMR 83 3.2.1 Xây dựng hàm mục tiêu cho FWOMR 84 3.2.2 Tính tốn SQP 86 3.3 Chương trình mơ MPC cho FWOMR .90 3.4 Mô đánh giá quỹ đạo chuyển động FWOMR 91 3.4.1 Mô quỹ đạo chuyển động FWOMR sử dụng MPC .92 3.4.2 So sánh thuật toán MPC thuật toán SMC, DSC 96 3.4.3 Thử nghiệm quỹ đạo chuyển động FWOMR sử dụng MPC sở phần mềm ROS 99 3.5 Kết luận chương 102 Chương THỰC NGHIỆM THUẬT TOÁN ĐIỀU KHIỂN 104 4.1 Kiến trúc cài đặt thơng số mơ hình robot FWOMR 104 4.2 Thiết lập mơ hình cho robot .106 4.2.1 Thiết lập khung điều hướng 106 4.2.2 Thiết lập quỹ đạo 107 an an tien si TIEU LUAN MOI download : skknchat123@gmail.com mo v 4.3 Kết thực nghiệm 110 4.3.1 Thực nghiệm robot lấy thông tin đồ .111 4.3.2 Thực nghiệm robot tránh vật cản tĩnh 112 4.3.3 Thực nghiệm robot tránh vật cản động 117 4.4 Kết luận chương 120 KẾT LUẬN .121 DANH MỤC CÁC CƠNG TRÌNH KHOA HỌC ĐÃ CÔNG BỐ 123 TÀI LIỆU THAM KHẢO .124 PHỤ LỤC PL-1 an an tien si TIEU LUAN MOI download : skknchat123@gmail.com mo vi DANH MỤC CÁC KÝ HIỆU, CÁC CHỮ VIẾT TẮT ( x, y ,  ) q = x v  vx Hệ tọa độ toàn cục y θ T vy ω  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 an an tien si TIEU LUAN MOI download : skknchat123@gmail.com mo vii x1d Quỹ đạo đặt 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 Chuẩn ma trận bậc không gian F AGV Xe vận tải tự động (Autonomous Guided Vehicles) AI Trí tuệ nhân tạo (Artificial Intelligence) AMCL Phương pháp đồ hóa cục Monte Carlo (Adaptive Monte Carlo Localization) API Giao diện lập trình ứng dụng (Application Programming Interface) DSC Mặt trượt động (Dynamic Sliding Surface) DWA Thuật toán cửa sổ động (Dynamic Window Approach) EB Dải đàn hồi (Elastic Band) EKF Bộ lọc Kalman mở rộng (Extended Kalman Filter) FLS 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) an an tien si TIEU LUAN MOI download : skknchat123@gmail.com mo (LUAN.an.TIEN.si).nghien.cuu.he.thong.xay.dung.ban.do lap.quy.dao.va.dieu.khien.bam.quy.dao.cho.robot.tu.hanh.bon.banh.da.huong(LUAN.an.TIEN.si).nghien.cuu.he.thong.xay.dung.ban.do lap.quy.dao.va.dieu.khien.bam.quy.dao.cho.robot.tu.hanh.bon.banh.da.huong(LUAN.an.TIEN.si).nghien.cuu.he.thong.xay.dung.ban.do lap.quy.dao.va.dieu.khien.bam.quy.dao.cho.robot.tu.hanh.bon.banh.da.huong(LUAN.an.TIEN.si).nghien.cuu.he.thong.xay.dung.ban.do lap.quy.dao.va.dieu.khien.bam.quy.dao.cho.robot.tu.hanh.bon.banh.da.huong viii 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) (LUAN.an.TIEN.si).nghien.cuu.he.thong.xay.dung.ban.do lap.quy.dao.va.dieu.khien.bam.quy.dao.cho.robot.tu.hanh.bon.banh.da.huong(LUAN.an.TIEN.si).nghien.cuu.he.thong.xay.dung.ban.do lap.quy.dao.va.dieu.khien.bam.quy.dao.cho.robot.tu.hanh.bon.banh.da.huong(LUAN.an.TIEN.si).nghien.cuu.he.thong.xay.dung.ban.do lap.quy.dao.va.dieu.khien.bam.quy.dao.cho.robot.tu.hanh.bon.banh.da.huong(LUAN.an.TIEN.si).nghien.cuu.he.thong.xay.dung.ban.do lap.quy.dao.va.dieu.khien.bam.quy.dao.cho.robot.tu.hanh.bon.banh.da.huong an an tien si TIEU LUAN MOI download : skknchat123@gmail.com mo (LUAN.an.TIEN.si).nghien.cuu.he.thong.xay.dung.ban.do lap.quy.dao.va.dieu.khien.bam.quy.dao.cho.robot.tu.hanh.bon.banh.da.huong(LUAN.an.TIEN.si).nghien.cuu.he.thong.xay.dung.ban.do lap.quy.dao.va.dieu.khien.bam.quy.dao.cho.robot.tu.hanh.bon.banh.da.huong(LUAN.an.TIEN.si).nghien.cuu.he.thong.xay.dung.ban.do lap.quy.dao.va.dieu.khien.bam.quy.dao.cho.robot.tu.hanh.bon.banh.da.huong(LUAN.an.TIEN.si).nghien.cuu.he.thong.xay.dung.ban.do lap.quy.dao.va.dieu.khien.bam.quy.dao.cho.robot.tu.hanh.bon.banh.da.huong PL-8 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 % 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 end % 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 (LUAN.an.TIEN.si).nghien.cuu.he.thong.xay.dung.ban.do lap.quy.dao.va.dieu.khien.bam.quy.dao.cho.robot.tu.hanh.bon.banh.da.huong(LUAN.an.TIEN.si).nghien.cuu.he.thong.xay.dung.ban.do lap.quy.dao.va.dieu.khien.bam.quy.dao.cho.robot.tu.hanh.bon.banh.da.huong(LUAN.an.TIEN.si).nghien.cuu.he.thong.xay.dung.ban.do lap.quy.dao.va.dieu.khien.bam.quy.dao.cho.robot.tu.hanh.bon.banh.da.huong(LUAN.an.TIEN.si).nghien.cuu.he.thong.xay.dung.ban.do lap.quy.dao.va.dieu.khien.bam.quy.dao.cho.robot.tu.hanh.bon.banh.da.huong an an tien si TIEU LUAN MOI download : skknchat123@gmail.com mo (LUAN.an.TIEN.si).nghien.cuu.he.thong.xay.dung.ban.do lap.quy.dao.va.dieu.khien.bam.quy.dao.cho.robot.tu.hanh.bon.banh.da.huong(LUAN.an.TIEN.si).nghien.cuu.he.thong.xay.dung.ban.do lap.quy.dao.va.dieu.khien.bam.quy.dao.cho.robot.tu.hanh.bon.banh.da.huong(LUAN.an.TIEN.si).nghien.cuu.he.thong.xay.dung.ban.do lap.quy.dao.va.dieu.khien.bam.quy.dao.cho.robot.tu.hanh.bon.banh.da.huong(LUAN.an.TIEN.si).nghien.cuu.he.thong.xay.dung.ban.do lap.quy.dao.va.dieu.khien.bam.quy.dao.cho.robot.tu.hanh.bon.banh.da.huong PL-9 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); 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]; end % 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); (LUAN.an.TIEN.si).nghien.cuu.he.thong.xay.dung.ban.do lap.quy.dao.va.dieu.khien.bam.quy.dao.cho.robot.tu.hanh.bon.banh.da.huong(LUAN.an.TIEN.si).nghien.cuu.he.thong.xay.dung.ban.do lap.quy.dao.va.dieu.khien.bam.quy.dao.cho.robot.tu.hanh.bon.banh.da.huong(LUAN.an.TIEN.si).nghien.cuu.he.thong.xay.dung.ban.do lap.quy.dao.va.dieu.khien.bam.quy.dao.cho.robot.tu.hanh.bon.banh.da.huong(LUAN.an.TIEN.si).nghien.cuu.he.thong.xay.dung.ban.do lap.quy.dao.va.dieu.khien.bam.quy.dao.cho.robot.tu.hanh.bon.banh.da.huong an an tien si TIEU LUAN MOI download : skknchat123@gmail.com mo (LUAN.an.TIEN.si).nghien.cuu.he.thong.xay.dung.ban.do lap.quy.dao.va.dieu.khien.bam.quy.dao.cho.robot.tu.hanh.bon.banh.da.huong(LUAN.an.TIEN.si).nghien.cuu.he.thong.xay.dung.ban.do lap.quy.dao.va.dieu.khien.bam.quy.dao.cho.robot.tu.hanh.bon.banh.da.huong(LUAN.an.TIEN.si).nghien.cuu.he.thong.xay.dung.ban.do lap.quy.dao.va.dieu.khien.bam.quy.dao.cho.robot.tu.hanh.bon.banh.da.huong(LUAN.an.TIEN.si).nghien.cuu.he.thong.xay.dung.ban.do lap.quy.dao.va.dieu.khien.bam.quy.dao.cho.robot.tu.hanh.bon.banh.da.huong PL-10 u = reshape(full(sol.x(3*(N+1)+1:end))',3,N)'; y = u(1,:); end (LUAN.an.TIEN.si).nghien.cuu.he.thong.xay.dung.ban.do lap.quy.dao.va.dieu.khien.bam.quy.dao.cho.robot.tu.hanh.bon.banh.da.huong(LUAN.an.TIEN.si).nghien.cuu.he.thong.xay.dung.ban.do lap.quy.dao.va.dieu.khien.bam.quy.dao.cho.robot.tu.hanh.bon.banh.da.huong(LUAN.an.TIEN.si).nghien.cuu.he.thong.xay.dung.ban.do lap.quy.dao.va.dieu.khien.bam.quy.dao.cho.robot.tu.hanh.bon.banh.da.huong(LUAN.an.TIEN.si).nghien.cuu.he.thong.xay.dung.ban.do lap.quy.dao.va.dieu.khien.bam.quy.dao.cho.robot.tu.hanh.bon.banh.da.huong an an tien si TIEU LUAN MOI download : skknchat123@gmail.com mo (LUAN.an.TIEN.si).nghien.cuu.he.thong.xay.dung.ban.do lap.quy.dao.va.dieu.khien.bam.quy.dao.cho.robot.tu.hanh.bon.banh.da.huong(LUAN.an.TIEN.si).nghien.cuu.he.thong.xay.dung.ban.do lap.quy.dao.va.dieu.khien.bam.quy.dao.cho.robot.tu.hanh.bon.banh.da.huong(LUAN.an.TIEN.si).nghien.cuu.he.thong.xay.dung.ban.do lap.quy.dao.va.dieu.khien.bam.quy.dao.cho.robot.tu.hanh.bon.banh.da.huong(LUAN.an.TIEN.si).nghien.cuu.he.thong.xay.dung.ban.do lap.quy.dao.va.dieu.khien.bam.quy.dao.cho.robot.tu.hanh.bon.banh.da.huong PL-11 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: (LUAN.an.TIEN.si).nghien.cuu.he.thong.xay.dung.ban.do lap.quy.dao.va.dieu.khien.bam.quy.dao.cho.robot.tu.hanh.bon.banh.da.huong(LUAN.an.TIEN.si).nghien.cuu.he.thong.xay.dung.ban.do lap.quy.dao.va.dieu.khien.bam.quy.dao.cho.robot.tu.hanh.bon.banh.da.huong(LUAN.an.TIEN.si).nghien.cuu.he.thong.xay.dung.ban.do lap.quy.dao.va.dieu.khien.bam.quy.dao.cho.robot.tu.hanh.bon.banh.da.huong(LUAN.an.TIEN.si).nghien.cuu.he.thong.xay.dung.ban.do lap.quy.dao.va.dieu.khien.bam.quy.dao.cho.robot.tu.hanh.bon.banh.da.huong an an tien si TIEU LUAN MOI download : skknchat123@gmail.com mo (LUAN.an.TIEN.si).nghien.cuu.he.thong.xay.dung.ban.do lap.quy.dao.va.dieu.khien.bam.quy.dao.cho.robot.tu.hanh.bon.banh.da.huong(LUAN.an.TIEN.si).nghien.cuu.he.thong.xay.dung.ban.do lap.quy.dao.va.dieu.khien.bam.quy.dao.cho.robot.tu.hanh.bon.banh.da.huong(LUAN.an.TIEN.si).nghien.cuu.he.thong.xay.dung.ban.do lap.quy.dao.va.dieu.khien.bam.quy.dao.cho.robot.tu.hanh.bon.banh.da.huong(LUAN.an.TIEN.si).nghien.cuu.he.thong.xay.dung.ban.do lap.quy.dao.va.dieu.khien.bam.quy.dao.cho.robot.tu.hanh.bon.banh.da.huong PL-12 #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) { (LUAN.an.TIEN.si).nghien.cuu.he.thong.xay.dung.ban.do lap.quy.dao.va.dieu.khien.bam.quy.dao.cho.robot.tu.hanh.bon.banh.da.huong(LUAN.an.TIEN.si).nghien.cuu.he.thong.xay.dung.ban.do lap.quy.dao.va.dieu.khien.bam.quy.dao.cho.robot.tu.hanh.bon.banh.da.huong(LUAN.an.TIEN.si).nghien.cuu.he.thong.xay.dung.ban.do lap.quy.dao.va.dieu.khien.bam.quy.dao.cho.robot.tu.hanh.bon.banh.da.huong(LUAN.an.TIEN.si).nghien.cuu.he.thong.xay.dung.ban.do lap.quy.dao.va.dieu.khien.bam.quy.dao.cho.robot.tu.hanh.bon.banh.da.huong an an tien si TIEU LUAN MOI download : skknchat123@gmail.com mo (LUAN.an.TIEN.si).nghien.cuu.he.thong.xay.dung.ban.do lap.quy.dao.va.dieu.khien.bam.quy.dao.cho.robot.tu.hanh.bon.banh.da.huong(LUAN.an.TIEN.si).nghien.cuu.he.thong.xay.dung.ban.do lap.quy.dao.va.dieu.khien.bam.quy.dao.cho.robot.tu.hanh.bon.banh.da.huong(LUAN.an.TIEN.si).nghien.cuu.he.thong.xay.dung.ban.do lap.quy.dao.va.dieu.khien.bam.quy.dao.cho.robot.tu.hanh.bon.banh.da.huong(LUAN.an.TIEN.si).nghien.cuu.he.thong.xay.dung.ban.do lap.quy.dao.va.dieu.khien.bam.quy.dao.cho.robot.tu.hanh.bon.banh.da.huong PL-13 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