Luận án 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

Nội dung text: Luận án 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

1. 123 DANH MỤC CÁC CÔNG TRÌNH KHOA HỌC ĐÃ CÔNG BỐ CT1. Đỗ Quang Hiệp, Ngô Mạnh Tiến, Nguyễn Mạnh Cường, Lê Trần Thắng, Phan Xuân Minh (2020), Xây dựng hệ thống nhận thức môi trường cho robot tự hành Omni 4 bánh dựa trên thuật toán EKF-SLAM và hệ điều hành ROS, “Tạp chí nghiên cứu KH&CN Quân sự”, Số đặc san Hội thảo Quốc gia FEE, pp. 30-37. CT2. Hiep Do Quang, Tien Ngo Manh, Cuong Nguyen Manh, Dung Pham Tien, Manh Tran Van, Duyen Ha Thi Kim, Van Nguyen Thi Thanh, Duan Dam Hong (2019), Mapping and Navigation with Four-wheeled Omnidirectional Mobile Robot based on Robot Operating System, “International Conference on MechatrOnics, Robotics & Systerm Engineering”, Morse, indexed in IEEE Explore, ISBN: 978-1- 7281-3984-5 (Electronic). CT3. Hiep Do Quang, Tien Ngo Manh, Cuong Nguyen Manh, Dung Pham Tien, Manh Tran Van, Kiem Nguyen Tien, Duy Nguyen Duc (2020), An Approach to Design Navigation System for Omnidirectional Mobile Robot Based on ROS, “International Journal of Mechanical Engineering and Robotics Research (IJMERR)”, ISSN 2278-0149, Vol. 9, No. 11, Scopus Q3, pp. 1502-1508. CT4. Ha Thi Kim Duyen, Ngo Manh Tien, Pham Ngoc Minh, Thai Quang Vinh, Phan Xuan Minh, Phạm Tien Dung, Nguyen Duc Dinh, Do Quang Hiep (2020), Fuzzy Adaptive Dynamic Surface Control for Omnidirectional Robot, “The International Conference on Artificial Intelligence and Computational Intelligence, AICI 2020”, The Springer-Verlag book series “Computational Intelligence” indexed in Scopus and Compendex (Ei), ISSN: 1860-9503 (electronic), ISBN: 978-3-030- 49536-7 (eBook). CT5. Hiep Do Quang, Tien Ngo Manh, Cuong Nguyen Manh, Thang Le Tran, Toan Nguyen Nhu, Nam Bui Duy (2022), Design a nonliear MPC for autonomous mobile robot navigation system based on ROS, “International Journal of Mechanical Engineering and Robotics Research” (IJMERR), ISSN 2278-0149, Vol. 11, No. 6, Scopus Q3, pp.379-388.
2. 125 11. Anh, N.T.L. (2021), Developing effcient localization and motion planning systems for a wheelled mobile robot in a dynamic environment, Doctoral Dissertation, Military Technical Academy. 12. Araújo, A., et al. (2013), "Integrating Arduino-based educational mobile robots in ROS", 2013 13th International Conference on Autonomous Robot Systems. 13. Bayes, T.T., and Richard P. (1963), "An Essay Towards Solving a Problem in Doctrine of Chances", Philosophical Transactions, 53(1) pp. 370 - 418. 14. Bhattacharya, P., and Gavrilova M.L. (2007), "Voronoi diagram in optimal path planning", Proceedings of the 4th International Symposium on Voronoi Diagrams in Science and Engineering (ISVD ’07). 15. Bonalli, R., et al. (2019), "Gusto: Guaranteed sequential trajectory optimization via sequential convex programming", IEEE International Conference on Robotics and Automation, pp. 6741–6747. 16. Buyval, A., Afanasyev I.M., and Magid E. (2016), "Comparative analysis of ROS-based monocular SLAM methods for indoor navigation", Ninth International Conference on Machine Vision. 17. Canny, J. (1987), "A new algebraic method for robot motion planning and real geometry", in 28th Annual Symposium on Foundations of Computer Science. 18. Courtney, J.D., and Jain A.K. (1994), "Mobile robot localization via classification of multisensor maps", Proceedings of the 1994 IEEE International Conference on Robotics and Automation. 19. Cousins, and Steve (2011), Exponential growth of ros [ros topics] IEEE Robotics & Automation Magazine 1.18, 19-20. 20. Chen, J., et al. (2014 ), "Adaptive Sliding Mode Control Based on a Filter for Four-Wheel Omni-Directional Mobile Robots", CYBERNETICS AND INFORMATION TECHNOLOGIES 14(2 ). 21. Chikurtev, D. (2016), "Indoor navigation for service mobile robots using Robot Operating System ROS)". 22. Dechter, R., and Pearl J. (1985), "Generalized best-first search strategies and the optimality of A∗", Journal of the ACM (JACM), 32 no. 3, pp. 505–536.
3. 127 Proceedings 2001 ICRA. IEEE International Conference on Robotics and Automation (Cat. No.01CH37164). 35. Hans, P.M. (1988), Sensor Fusion in Certainty Grids for Mobile Robots, in AI Magazine, pp. 61-74. 36. Hoa, D.K., et al. (2018), "An Improved Navigation Method for Robot in Indoor Dynamic Environment Based on Ground Extraction", Journal of Science & Technology of Technical Universities, 131, pp. 062-068. 37. Hoa, D.K., et al. (2018), "An efective depth map navigation for mobile robot in ondoor enviroments", JP Journal of Heat and Mass Transfer, Special Volume, Issue II, Advances in Mechanical System and ICT-convergence, , pp. 221 – 237 38. Hoang, T.T., et al. (2012), "A Path Following Algorithm for Wheeled Mobile Robot Using Extended Kalman Filter", IEICE Proc. of the 3th Int. Conf. on Integrated. 39. Hoang, T.T., et al. (2012), "Multi-sensor perceptual system for mobile robot and sensor fusion-based localization", 2012 International Conference on Control, Automation and Information Sciences (ICCAIS). 40. Hoang, T.T., et al. (2012), "Stabilization Control of the Differential Mobile Robot Using Lyapunov function And Extended Kalaman Filter", Vietnam Journal of Sciences and Technology 50(4), pp. 441-452. 41. Huang, H. (2011), "Intelligent motion controller design for four-wheeled omnidirectional mobile robots using hybrid GA-PSO algorithm", 2011 IEEE International Conference on Systems, Man, and Cybernetics. 42. J.Julier, S., Uhlmann J.K., and Durrant-Whyte H.F. (1995), "A new approach for filtering nonlinear systems", Proceedings of the American Control Conference. 43. Jabin, S. (2010), Robot Learning. Sciyo 44. Jensfelt, P. (2001), Approaches to Mobile Robot Localization in Indoor Environments, Doctoral Thesis, Department of Signals, Sensors and Systems, Royal Institute of Technology Stockholm.
4. 129 57. Martinez, A., and Fernández E. (2013), Learning ROS for Robotics Programming. Birmingham - Mumbai,. 58. Megalingam, R.K.e.a. (2018), "ROS based autonomous indoor navigation simulation using SLAM algorithm", Int. J. Pure Appl. Math, pp. 199-205. 59. Moreno-Caireta, I., Celaya E., and Ros L. (2021), "Model Predictive Control for a Mecanum-wheeled Robot Navigating among Obstacles", IFAC- PapersOnLine, 54(6), pp. 119-125. 60. Morgan, Q., and Brian G. (2009), "ROS: an open-source Robot Operating System", ICRA workshop on open source software. 61. Morgan, Q., Gerkey B., and Smart W.D. (2015), Programming Robots with ROS: a practical introduction to the Robot Operating System. O'Reilly Media, Inc. 62. Nguyen, T.T.V., et al. (2012), "Mobile robot localization using fuzzy neural network based extended Kalman filter", 2012 IEEE International Conference on Control System, Computing and Engineering. 63. Ocando, M.G., et al. (2017), "Autonomous 2D SLAM and 3D mapping of an environment using a single 2D LIDAR and ROS", 2017 Latin American Robotics Symposium (LARS) and 2017 Brazilian Symposium on Robotics (SBR). 64. Ogren, P., and Leonard N.E. (2005), "A convergent dynamic window approach to obstacle avoidance", IEEE Transactions on Robotics and Automation, 21( 2), pp. 188–195. 65. Oliveira, H.P., et al. (2008), "Precise Modeling of a Four Wheeled Omni- directional Robot", Proc. 8th Conf. Auton. Robot Syst. Compet. 66. Oliveira, H.P., et al. (2009), "Modeling and Assessing of Omni-directional Robots with Three and Four Wheels", INESC-Porto Portugal. 67. Olsen, M.M., and Petersen H.G. (2001), "A new method for estimating parameters of a dynamic robot model", IEEE Transactions on Robotics and Automation, 17(1), pp. 95-100. 68. Pajaziti, and Arbnor (2014), "SLAM – Map Building and Navigation via ROS", International Journal of Intelligent Systems and Applications in Engineering, 2(4), pp. 71-75.
5. 131 80. Rösmann, C.F.H.a.T.B. (2017), Online trajectory planning in ROS under kinodynamic constraints with timed-elastic-bands. Robot Operating System (ROS). Springer, Cham. 81. Santos, J.M., Portugal D., and Rocha R.P. (2013), "An evaluation of 2D SLAM techniques available in Robot Operating System", 2013 IEEE International Symposium on Safety, Security, and Rescue Robotics (SSRR). 82. Sciavicco, L., and Siciliano B. (1996), Modeling and control of Robot Manipulators. International Editions. 83. Schoels, T., et al. (2019), "An nmpc approach using convex inner approximations for online motion planning with guaranteed collision avoidance", IEEE International Conference on Robotics and Automation (ICRA). 84. Sicilian, B., et al. (2009), "Robotics Modelling, Planning and Control", Advanced Textbooks in Control and Signal Processing series 85. Skiena, S. (1990), Dijkstra’s algorithm. Addison-Wesley, Boston, Mass, USA, : Implementing Discrete Mathematics: Combinatorics and Graph Theory with Mathematica. 86. Smith, Self, and Cheesman: (1922), "Estimating uncertain spatial relationships in robotics". 87. Spong, M.W., Hutchinson S., and Vidyasagar M. (2004), Robot Dynamics and Control. Addison – Wesley Publishing Company. 88. Swaroop, D., et al. (2000), "Dynamic surface control for a class of nonlinear systems", IEEE Transactions on Automatic Control, 45(10), pp. 1893-1899. 89. Takaya, K., et al. (2016), "Simulation environment for mobile robots testing using ROS and Gazebo", 2016 20th International Conference on System Theory, Control and Computing (ICSTCC). 90. Talluri, R., and Aggarwal J. (1993), "Position Estimation Techniques for an Autonomous Mobile Robot- a Review", In Handbook of Pattern Recognition and Computer Vision, pp. 769-801.
6. 133 103. Yuan-Pao, H., et al. (2009), "Hybrid navigation of a four-wheeled tour- guide robot", 2009 ICCAS-SICE. 104. Zhang, X., Liniger A., and Borrelli F. (2020), "Optimization-Based Collision Avoidance", IEEE Transactions on Control Systems Technology, 29(3), pp. 972-983.
7. PL-2 [World.y, lmks_visible] = sen_rf.constrainMeasurement(World.y); enough_correlations = 0; last_scan_good = this_scan_good; pose_last_scan = pose_this_scan; this_scan_good = 0; % Initialise this_scan_good for this iteration scan_ref = World.scan_data; World.scan_data = -ones(3, numSegments * (round(sen.fov/sen.angular_res) + 1)); scan_ndx = 1; for i = 1:length(Obstacles) scan_data = Obstacles(i).getMeasured(sen, rob); if ~isempty(scan_data) World.scan_data(:, scan_ndx:scan_ndx+size(scan_data, 2)-1) = scan_data; scan_ndx = scan_ndx + size(scan_data, 2); end end World.scan_data = World.scan_data(:, World.scan_data(2, :) ~= -1); World.scan_data = removeDuplicateLasers(World.scan_data); World.scan_data = World.scan_data(2:3, :); % Remove index row v = repmat(sen.noise, 1, size(World.scan_data, 2)) .* randn(2,size(World.scan_data, 2)); World.scan_data = World.scan_data + v; if ~isempty(World.scan_data) obstaclesDetected = 1; World.scan_data = getInvMeasurement(World.scan_data); if size(World.scan_data, 2) > World.scan_corr_tolerance this_scan_good = 1; end else obstaclesDetected = 0; World.scan_global = []; end if last_scan_good && this_scan_good n = rob.q .* randn(2,1); % Noise in odometry measurement [R, T, correl, icp_var] = doICP(scan_ref, World.scan_data, 1, rob.u + n); if length(correl) > World.scan_corr_tolerance enough_correlations = 1;
8. PL-4 weight_scan = 1; weight_odo = 0; end rob.r = weight_odo * dr_odo + weight_scan * dr_scan + rob.r; World.x(r) = rob.r; P_rr = World.P(r,r); World.P(r,:) = R_r * World.P(r,:); World.P(:,r) = World.P(r,:)'; World.P(r,r) = R_r * P_rr * R_r' + weight_scan * cov_scan + weight_odo * cov_odo; r_old = rob.r; lmks_visible_known = find(World.l(1,lmks_visible)); lids = lmks_visible(lmks_visible_known); for lid = lids [e, E_r, E_l] = scanPoint(rob.r, World.x(World.l(:,lid))); E_rl = [E_r E_l]; rl = [r World.l(:,lid)']; E = E_rl * World.P(rl,rl) * E_rl'; yi = World.y(:,lid); z = yi - e; z(2) = getPiToPi(z(2)); Z = World.M + E; K = World.P(:, rl) * E_rl' * Z^-1; World.x = World.x + K * z; World.P = World.P - K * Z * K'; rob.r = World.x(r); end lmks_visible_unknown = find(World.l(1,lmks_visible)==0); lids = lmks_visible(lmks_visible_unknown); if ~isempty(lids) for lid = lids s = find(World.mapspace, 2); if ~isempty(s) World.mapspace(s) = 0; World.l(:,lid) = s'; % Measurement yi = World.y(:,lid); [World.x(World.l(:,lid)), L_r, L_y] = invScanPoint(rob.r, yi);
9. PL-6 odo_error = abs(dr_odo-dr_true); odo_error(3) = abs(getPiToPi(odo_error(3))); odo_error(1) = pdist([0 0; odo_error(1) odo_error(2)]); odo_error(2) = []; World.scan_error_hist(:, t) = scan_error; World.odo_error_hist(:, t) = odo_error; World.turning_hist(t) = rob.u(2); World.weight_scan_hist(t) = weight_scan; World.weight_odo_hist(t) = weight_odo; Graphics.lmks_all = lmks_all; Graphics.lmks_visible = lmks_visible; doGraphics(rob, World, Graphics, Obstacles(movingObstacles), AxesDir); end set(Graphics.R_hist, 'xdata', World.R_hist(1,:), 'ydata', World.R_hist(2, :)) set(Graphics.r_hist, 'xdata', World.r_hist(1,:), 'ydata', World.r_hist(2, :)) figure('color', 'white') subplot(2, 1, 1) plot(1:World.tend, World.error_hist) title('EKF SLAM: position error over time') xlabel('Time (s)'), ylabel('Position Error (m)') subplot(2, 1, 2) plot(1:World.tend, sqrt(World.Pr_hist(1, :)), 'r', 1:World.tend, sqrt(World.Pr_hist(2, :)), 'b') title('EKF SLAM: position uncertainty over time') xlabel('Time (s)'), ylabel('Standard Deviation (m)') legend('St. Dev. in x', 'St. Dev. in y') figure('color', 'white') subplot(4, 1, 1) AX = plotyy(1:World.tend, World.scan_error_hist(1,:), 1:World.tend, World.scan_error_hist(2,:), 'plot'); set(get(AX(1),'Ylabel'),'String',['Position' char(10) 'Error (m)']) set(get(AX(2),'Ylabel'),'String',['Angular' char(10) 'Error (rad)']) title('Scan errors over time') xlabel('Time (s)') subplot(4, 1, 2) AX2 = plotyy(1:World.tend, World.odo_error_hist(1,:), 1:World.tend, World.odo_error_hist(2,:), 'plot'); set(get(AX2(1),'Ylabel'),'String',['Position' char(10) 'Error (m)'])
10. 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; % Objective function g = []; % constraints vector st = X(:,1); % Initial state g = [g;st-P(1:3)]; % Initial condition constraints % Euler discretization for k = 1:N st = X(:,k); con = 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 6 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; % Equality constraints args.ubg(1:3*(N+1)) = 0; % Equality constraints args.lbx(1:3:3*(N+1),1) = x_min; % State x lower bound
11. PL-10 u = reshape(full(sol.x(3*(N+1)+1:end))',3,N)'; y = u(1,:); end
12. 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) {
13. PL-14 nav_msgs::Path odom_path; odom_path.header.frame_id = "/odom"; odom_path.header.stamp = ros::Time::now(); vector > control_output; control_output = init_acado(); // cout cur_state = {px, py, pq}; // Update odom path geometry_msgs::PoseStamped cur_pose; cur_pose.header = odom_path.header; cur_pose.pose.position.x = px; cur_pose.pose.position.y = py; cur_pose.pose.orientation.w = 1.0;
14. PL-16 nav_msgs::Path predict_path; predict_path.header.frame_id = "/odom"; predict_path.header.stamp = ros::Time::now(); for (int i = 0; i = path.poses.size(); if(goal) { vel.linear.x = 0; vel.linear.y = 0; vel.angular.z = 0; // cout << "Done!" << endl; } else { vel.linear.x = control_output[0][0]; vel.linear.y = control_output[1][0]; vel.angular.z = control_output[2][0]; } vel_pub.publish(vel); ros::spinOnce(); r.sleep(); count++; } return 0; } void init_weight() { for (int i = 0; i < N; i++) { // Setup diagonal entries
15. PL-18 else if(j == 1 ) { control_output_vy.push_back(acadoVariables.u[i * ACADO_NU + j]); } else { control_output_w.push_back(acadoVariables.u[i * ACADO_NU + j]); } } } init_weight(); return {control_output_vx, control_output_vy, control_output_w}; } vector > run_mpc_acado(vector states, vector ref_states, vector > previous_u) { /* Some temporary variables. */ int i, iter; acado_timer t; /* Initialize the states and controls. */ for (i = 0; i < NX * (N + 1); ++i) { acadoVariables.x[ i ] = (real_t) states[i]; } for (i = 0; i < NX; ++i) { acadoVariables.x0[i] = (real_t)states[i]; } /* Initialize the measurements/reference. */ for (i = 0; i < NY * N; ++i) { acadoVariables.y[i] = (real_t)ref_states[i]; } for (i = 0; i < NYN; ++i) { acadoVariables.yN[i] = (real_t)ref_states[NY * (N - 1) + i]; }
16. PL-20 /* Read the elapsed time. */ real_t te = acado_toc(&t); vector control_output_vx; vector control_output_vy; vector control_output_w; real_t *u = acado_getVariablesU(); for (int i = 0; i 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 result;
17. PL-22 if (cur_state[3] next_state = update_states(cur_state, old_vx_cmd[i], old_vy_cmd[i], old_w_cmd[i]); predicted_states.push_back(next_state); } vector result; for (int i = 0; i < (ACADO_N + 1); ++i) { for (int j = 0; j < NX; ++j) { result.push_back(predicted_states[i][j]); } } return result; }