Nghiên cứu phát triển thuật toán tối ưu hóa tính toán quỹ đạo chuyển động cho robot tự hành trong nhà có kể đến tương tác với con người

BỘ GIÁO DỤC VÀ ĐÀO TẠO BỘ QUỐC PHÒNG HỌC VIỆN KỸ THUẬT QUÂN SỰ HOÀNG VĂN BẢY NGHIÊN CỨU PHÁT TRIỂN THUẬT TỐN TỐI ƯU HĨA TÍNH TỐN QUỸ ĐẠO CHUYỂN ĐỘNG CHO ROBOT TỰ HÀNH TRONG NHÀ CÓ KỂ ĐẾN TƯƠNG TÁC VỚI CON NGƯỜI LUẬN ÁN TIẾN SĨ KỸ THUẬT HÀ NỘI - 2023 BỘ GIÁO DỤC VÀ ĐÀO TẠO BỘ QUỐC PHÒNG HỌC VIỆN KỸ THUẬT QUÂN SỰ HOÀNG VĂN BẢY NGHIÊN CỨU PHÁT TRIỂN THUẬT TỐN TỐI ƯU HĨA TÍNH TỐN QUỸ ĐẠO CHUYỂN ĐỘNG CHO ROBOT TỰ HÀNH TRONG NHÀ CÓ KỂ ĐẾN TƯƠNG TÁC VỚI CON NGƯỜI LUẬN ÁN TIẾN SĨ KỸ THUẬT Chuyên ngành: TỰ ĐỘNG HÓA Mã số: 52 02 16 NGƯỜI HƯỚNG DẪN KHOA HỌC: TS ĐỖ ĐÌNH NGHĨA PGS TS TRƯƠNG XUÂN TÙNG HÀ NỘI - 2023 LỜI CAM ĐOAN Tôi xin cam đoan luận án cơng trình nghiên cứu tơi Các số liệu, kết trình bày luận án hồn tồn trung thực chưa công bố cơng trình trước Các kết sử dụng tham khảo trích dẫn đầy đủ theo quy định Hà Nội, ngày tháng 12 năm 2023 Tác giả LỜI CẢM ƠN Trong trình nghiên cứu hoàn thành luận án, nghiên cứu sinh nhận giúp đỡ nhiều người Đầu tiên xin gửi lời cảm ơn sâu sắc đến Thầy giáo hướng dẫn PGS TS Trương Xuân Tùng TS Đỗ Đình Nghĩa Thầy khơng người hướng dẫn, giúp đỡ nghiên cứu sinh hoàn thành luận án mà người định hướng, truyền thụ động lực ý chí tâm q trình nghiên cứu đầy gian khổ Tôi chân thành cảm ơn Thầy giáo, Cô giáo Bộ môn Tự động kỹ thuật tính, Khoa Kỹ thuật điều khiển, Học viện Kỹ thuật quân sự, nơi nghiên cứu sinh làm việc, tận tình hướng dẫn bảo thời gian nghiên cứu sinh nghiên cứu Tiếp theo, xin chân thành cảm ơn đồng nghiệp nghiên cứu sinh, nhóm nghiên cứu thầy Trương Xuân Tùng, Phòng Sau đại học - Học viện Kỹ thuật quân sự, Trường Học viện Phịng khơng khơng qn giúp đỡ nghiên cứu sinh hoàn thành luận án Cuối cùng, xin gửi lời cảm ơn sâu sắc tới gia đình, bạn bè đồng nghiệp ln động viên, chia sẻ khó khăn sống, gia đình xã hội, giúp nghiên cứu sinh đạt kết hôm Mục lục MỤC LỤC i DANH MỤC CÁC TỪ VIẾT TẮT iv DANH MỤC HÌNH VẼ vi DANH MỤC BẢNG x DANH MỤC KÝ HIỆU TOÁN HỌC xi MỞ ĐẦU Chương TỔNG QUAN VỀ TÍNH TỐN QUỸ ĐẠO CHO ROBOT TỰ HÀNH 1.1 Giới thiệu chung robot tự hành 1.2 Hệ thống điều khiển robot tự hành 1.2.1 Thu thập xử lý thông tin 10 1.2.2 Định vị xây dựng đồ 11 1.2.3 Lập kế hoạch đường tránh vật cản 12 1.2.4 Điều khiển chuyển động 15 1.3 Cơ sở lý thuyết thiết yếu phục vụ tính tốn quỹ đạo cho robot tự hành tương tác với người 16 1.3.1 Hệ quy chiếu cho robot 16 1.3.2 Thu thập nhận biết thông tin hành vi người xã hội i 18 1.3.3 Vùng tương tác có tính xã hội động 20 1.3.4 Xác định tư robot tiếp cận người 23 1.3.5 Tính tốn quỹ đạo chuyển động cho robot tự hành sử dụng thuật toán TEB 23 1.3.6 Các số đánh giá mức độ an toàn thân thiện robot tự hành gần người 34 1.4 Tình hình nghiên cứu ngồi nước 36 1.4.1 Tình hình nghiên cứu ngồi nước 36 1.4.2 Tình hình nghiên cứu nước 40 1.5 Kết luận chương 42 Chương THIẾT KẾ QUỸ ĐẠO CHUYỂN ĐỘNG CHO ROBOT TỰ HÀNH TRÁNH NGƯỜI VÀ TIẾP CẬN NGƯỜI TRONG MÔI TRƯỜNG ĐỘNG 44 2.1 Đặt vấn đề 44 2.2 Mơ tả tốn 44 2.3 Đề xuất thuật toán STEB 47 2.3.1 Xây dựng thuật toán STEB 47 2.3.2 Tích hợp thuật tốn STEB vào hệ thống điều khiển robot 54 2.4 Đề xuất thuật toán TDTEB 56 2.4.1 Xây dựng thuật toán TDTEB 56 2.4.2 Tích hợp thuật tốn TDTEB vào hệ thống điều khiển robot 59 2.5 Đề xuất thuật toán GTEB 60 2.5.1 Xây dựng thuật toán GTEB 60 ii 2.5.2 Tích hợp thuật tốn GTEB vào hệ thống điều khiển robot 63 2.6 Kết luận chương 64 Chương MÔ PHỎNG VÀ THỰC NGHIỆM KIỂM CHỨNG CÁC THUẬT TOÁN 65 3.1 Chuẩn bị mô thực nghiệm 65 3.1.1 Công cụ phần mềm 65 3.1.2 Xây dựng robot hai bánh vi sai 66 3.2 Kịch mô thực nghiệm 71 3.3 Các kết mô 74 3.3.1 Mô robot tránh người sử dụng thuật toán STEB 74 3.3.2 Mô số tình đặc biệt robot tránh người sử dụng thuật toán TDTEB 78 3.3.3 Mô robot tiếp cận người sử dụng thuật toán GTEB 80 3.4 Các kết thực nghiệm 85 3.4.1 Thực nghiệm robot tránh người sử dụng thuật toán STEB 85 3.4.2 Thực nghiệm robot tiếp cận người sử dụng thuật toán GTEB 88 3.5 Kết luận chương 91 KẾT LUẬN VÀ HƯỚNG NGHIÊN CỨU TƯƠNG LAI 93 DANH MỤC CÁC CƠNG TRÌNH ĐÃ CÔNG BỐ 95 PHỤ LỤC iii 108 DANH MỤC CÁC TỪ VIẾT TẮT Từ viết tắt Nghĩa Tiếng Anh Nghĩa Tiếng Việt AUV Autonomous Underwater Phương tiện tự lái Vehicle nước DFS Depth First Search Tìm kiếm theo độ sâu DSZ Dynamic Social Zone Vùng tương tác có tính xã hội động DWA Dynamic Window Ap- Tiếp cận cửa sổ động proach EB Elastic Band Thuật toán tối ưu quỹ đạo theo bóng đàn hồi EKF External Kalman Filter Bộ lọc Kalman mở rộng GPS Global Position System Hệ thống định vị toàn cầu GTEB Goal Timed Elastic Band Thuật tốn tối ưu quỹ đạo theo thời gian có chọn điểm tiếp cận HCSI Human Comfortable Safety Indices Các số đánh giá mức độ an toàn thân thiện robot gần người KF Kalman Filter Bộ lọc Kalman iv LMR Legged Mobile Robot Robot di động có chân PCL Point Cloud Library Thư viện đám mây điểm ROS Robot operating system Hệ điều hành robot SGI Social group index Chỉ số đánh giá mức độ an toàn thân thiện robot gần nhóm người SII Social individual index Chỉ số đánh giá mức độ an toàn thân thiện robot gần người STEB Social Timed Elastic Band Thuật tốn tối ưu quỹ đạo theo thời gian có đưa vào ràng buộc mang tính xã hội TDTEB Time-Dependent Timed Elastic Band Thuật toán tối ưu quỹ đạo theo thời gian có dự đốn va chạm với người TEB Timed Elastic Band Thuật toán tối ưu quỹ đạo theo thời gian UAV Unmanned Aerial Vehicle Phương tiện bay không người lái VO Velocity Obstacle Vận tốc vật cản WMR Wheeled Mobile Robot Robot di động có bánh xe SCM Social Cost Maps Bản đồ chi phí xã hội v DANH MỤC HÌNH VẼ 1.1 Sơ đồ khối hệ thống điều khiển robot tự hành 10 1.2 Minh họa thu thập thông tin cảm biến Kinect laser Rplidar 11 1.3 Mơ tả thuật tốn A* 14 1.4 Hệ quy chiếu toàn cục Xg Og Yg hệ quy chiếu cục robot Xr Or Yr 17 1.5 Sơ đồ trình lấy hành vi xã hội người 19 1.6 Không gian cá nhân người [30] 21 1.7 Mối quan hệ trạng thái người tư robot [28] 22 1.8 Minh họa vùng DSZ theo phân bố Gaussian 2D: (a) Ảnh chụp ba người đứng, (b) Biểu đồ hình chiếu đứng phân bố Gaussian 2D với người p1 nhóm hai người p2 , p3 , (c) Biểu đồ hình chiếu ngang phân bố Gaussian 2D với người p1 nhóm hai người p2 , p3 22 1.9 Minh họa vùng tiếp cận tư tiếp cận người: (a) Vùng tiếp cận tư tiếp cận người, (b) Vùng tiếp cận tư tiếp cận nhóm ba người 24 1.10 Mô tả ba tư liên tiếp robot đồ tồn cục 25 1.11 Giải thích mặt hình học ràng buộc động học non-holonomic 26 1.12 Đồ thị đường xem xét cặp điểm hai bên vật cản vi 29 maxSGI.data = global_SGI;Public_SII.publish(maxSII); Public_SGI.publish(maxSGI); Public_pose_obstacle.publish(poseArray); tf::Transform g_currentPose; g_currentPose.getOrigin().setX(g_r_x_); g_currentPose.setRotation( tf::createQuaternionFromRPY(0, 0, theta)); g_transformBroadcaster.sendTransform( tf:: ros::spinOnce();loop_rate.sleep(); ros::spin();return 0; double angleBetween2Lines(Eigen::Vector2d pStart, Eigen:: Vector2d pGoal, Eigen::Vector2d pObsFirst, Eigen::Vector2d pObsSecond) double getAngle = 0.0; if (pObsSecond.y() == pObsFirst.y() and pObsSecond.x() == pObsFirst.x()) return getAngle;//return previous angle double atan2_o = atan2(pObsSecond.y() - pObsFirst.y(), pObsSecond.x() - pObsFirst.x()); getAngle = atan2_r - atan2_o; return getAngle; double determine_left_right_on(Eigen::Vector2d P, Eigen:: Vector2d A, Eigen::Vector2d B) double cross_product; double dis = std::sqrt(std::pow(B.x() - P.x(), 2) + std::pow(B.y() - P.y(), 2)); cross_product = A.x() * B.y() - A.y() * B.x(); if (cross_product > 0)cross_product = -90; else if (cross_product < 0) cross_product = 90; 111 else if (dis == dis1 + dis2)cross_product = 0; else cross_product = 180; return cross_product; double disPointToLine(Eigen::Vector2d P, Eigen::Vector2d I, Eigen::Vector2d J) double A = P.x() - I.x();double B = P.y() - I.y(); double dot = A * C + B * D;double len_sq = C * C + D * D; double param = -1;if (len_sq != 0) //in case of length line param = dot / len_sq;double xx, yy; else if (param > 1) xx = J.x();yy = J.y(); else xx = I.x() + param * C;yy = I.y() + param * D; double dx = P.x() - xx;double dy = P.y() - yy; return sqrt(dx * dx + dy * dy); double dis2Points(Eigen::Vector2d P1, Eigen::Vector2d P2) double collisionIndex(double x, double y, double x0, double y0, double A, double sigmax, double sigmay, double skew) double dx = x - x0, dy = y - y0;double h = sqrt(dx * dx + dy * dy); double angle = atan2(dy, dx);double mx = cos(angle - skew) * h; double my = sin(angle - skew) * h; f2 = pow(my, 2.0) / (2.0 * sigmay * sigmay); return A * exp(-(f1 + f2)); double interactionIndex(double x, double y, double x0, double sigmax, double sigmay, double skew) double dx = x - x0, dy = y - y0;double h = sqrt(dx * dx + dy * dy); double angle = atan2(dy, dx); double mx = cos(angle - skew) * h; double my = sin(angle - skew) * h; f2 = pow(my, 2.0) / (2.0 * sigmay * sigmay); return A * exp(-(f1 + f2)); 112 void kinematicModel(double vellinear, double velangular, double dt) double x = r_pose_.position.x;double y = r_pose_.position.y; double theta = r_pose_.orientation.z; double get_r_instantaneous_ = vellinear * cos(theta);if (get_r_instantaneous_ > 0.1) get_r_instantaneous_ = 0.1; r_pose_.position.x = x + get_r_instantaneous_ * dt; r_pose_.position.y = y + vellinear * sin(theta) * dt; r_pose_.orientation.z = theta + velangular * dt * 0.5; //get translational and angular velocity g_r_instantaneous_x_ = get_r_instantaneous_; g_r_instantaneous_angularx_ = velangular * 0.5; void HomotopyClassPlanner::getKinematicModel (TebOptimalPlannerPtr best_teb_arg) g_set_origin_ = 0; double dt = 0; if (cfg_-> SocialCoefficients.reset_origin) dt = 0.1;else dt = cfg_->SocialCoefficients.robot_dt; double vy_; double theta; int n = best_teb_arg->teb().sizePoses(); if (!cfg_->SocialCoefficients.robot_pause and !g_r_pause_) best_teb_arg->getVelocityCommand(vx_, vy_, theta, 0); kinematicModel(vx_, theta, dt); //pass extern variable g_r_x_ = r_pose_.position.x;g_r_y_ = r_pose_.position.y; if (cfg_->SocialCoefficients.reset_origin) r_pose_.position.x = -5; r_pose_.position.y = 0; 113 r_pose_.orientation.z = 0; if (r_pose_.position.x >= 5) r_pose_.position.x = -5; g_set_origin_ = 1; //calculator SII double SII = std::numeric_limits::min(); double sigmaxySII = 0.9 / 2.2;int count_o = 0; for (ObstContainer::const_iterator it_obst = obstacles()->begin(); it_obst != obstacles()->end(); ++it_obst) PointCenter.x() = (*it_obst)->getCentroid().x(); PointCenter.y() = (*it_obst)->getCentroid().y(); double getCollisionIndex = collisionIndex(r_pose_.position.x, r_pose_.position.y, PointCenter.x(), sigmaxySII, sigmaxySII, 0); if (SII getCurrentCost() * cfg_->hcp.selection_cost_hysteresis; // small hysteresis last_best_teb_ = best_teb_; 114 else last_best_teb_.reset(); if (initial_plan_teb) min_cost_initial_plan_teb = initial_plan_teb->getCurrentCost() * cfg_->hcp.selection_prefer_initial_plan; // small hysteresis best_teb_.reset(); // reset pointer //initial origin best_teb_.reset(); // reset pointer new_best_teb_.reset(); //set Teta zero Teta = 0.0;sum_xCentro = 0.0, sum_yCentro = 0.0; //get centroId obstacles Start_.x() = g_r_x_; Start_.y() = g_r_y_; Eigen::Vector2d diff(Goal_.x() - Start_.x(), Goal_.y() - Start_.y()); Eigen::Vector2d normal(-diff.y(), diff.x()); normal.normalize(); normal = normal * cfg_->obstacles.min_obstacle_dist; for (ObstContainer::const_iterator it_obst = obstacles()-> begin(); it_obst != obstacles()->end(); ++it_obst) PointCenter.x() = (*it_obst)->getCentroid().x(); PointCenter.y() = (*it_obst)->getCentroid().y(); PointR.x() = PointCenter.x() - normal.x(); PointL.x() = PointCenter.x() + normal.x(); PointL.y() = PointCenter.y() + normal.y(); Teta = angleBetween2Lines(Start_, Goal_, PointCenterPrev, PointCenter); if (cos(Teta) = sqrt(3) / 2) //check point left right vs Robot-Goal if (determine_left_right_on(PointR, Start_, Goal_) == -90) PointSel = PointL; else if (determine_left_right_on(PointR, Start_, Goal_) == 90) PointSel = PointR; double k_minDis = cfg_->SocialCoefficients.k_minDis; double k_centerDis = cfg_->SocialCoefficients.k_centerDis; for (TebOptPlannerContainer::iterator it_teb = tebs_.begin(); it_teb != tebs_.end(); ++it_teb) TebOptimalPlannerPtr getTeb; getTeb = *it_teb;double local_dis = std::numeric_limits::max(); double get_minDis = 0;double* get_dis = get_minDis; const Eigen::Ref ref_pointObs(PointR); getTeb->teb().findClosestTrajectoryPose(ref_pointObs, get_dis, 0); double teb_cost; if (*it_teb == last_best_teb_) teb_cost = min_cost_last_best; // skip already known teb_cost = min_cost_initial_plan_teb; else teb_cost = it_teb->get()->getCurrentCost(); double new_teb_cost_;if (get_minDis < min_)min_ = get_minDis; double CenterDis = disPointToLine(PointCenter, Start_, Goal_); double minDisCoefficient = k_minDis * tanh(get_minDis); double limitedCoefficient = (1 - tanh(k_centerDis * CenterDis)); double deltaCost = minDisCoefficient * limitedCoefficient; 116 new_teb_cost_ = teb_cost + deltaCost; if (teb_cost < min_cost) best_teb_ = *it_teb; min_cost = teb_cost; if (new_teb_cost_ < new_min_cost) new_best_teb_ = *it_teb; new_min_cost = new_teb_cost_; if (last_best_teb_ best_teb_ != last_best_teb_) ros::Time now = ros::Time::now(); if ((now - last_eq_class_switching_time_).toSec() > cfg_ ->hcp.switching_blocking_period) best_teb_ = last_best_teb_; return best_teb_; Mã nguồn thực thi giải thuật TDTEB int directionOfPoint(PoseSE2 A, PoseSE2 B, PoseSE2 P) B.x() -= A.x(); B.y() -= A.y(); P.x() -= A.x(); P.y() -= A.y(); // Determining cross Product int cross_product = B.x() * P.y() - B.y() * P.x(); // return RIGHT if cross product is positive if (cross_product > 0) return RIGHT; // return LEFT if cross product is negative if (cross_product < 0) return LEFT; // return ZERO if cross product is zero return ZERO; void lineLineIntersection(PoseSE2 A, PoseSE2 B, PoseSE2 C, PoseSE2 D) flag_human = false; 117 // Line AB represented as a1x + b1y = c1 double a1 = B.y() - A.y(); double c1 = a1*(A.x()) + b1*(A.y()); // Line CD represented as a2x + b2y = c2 double b2 = C.x() - D.x(); double c2 = a2*(C.x())+ b2*(C.y()); double determinant = a1*b2 - a2*b1; if (determinant == 0) intersection3.x() = 0; intersection3.y() = 0; // return intersection3; else intersection3.x() = (b2*c1 - b1*c1)/determinant; intersection3.y() = (a1*c2 + a2*c1 - a2*c4)/determinant; // return intersection3; if (directionOfPoint(A,B,C) == set_case == 3)//left flag_human = true; // return intersection3; double dis2Points(PoseSE2 P1,PoseSE2 P2) return std::sqrt( std::pow(P2.x()-P1.x(),5) + std::pow(P2.y()-P1.y(),1) ); //set variables human asross TebOptimalPlannerPtr HomotopyClassPlanner::acrossSelectBestTeb() double min_cost = std::numeric_limits::max(); double new_min_cost = std::numeric_limits::max(); double min_cost_last_best = std::numeric_limits::max(); 118 TebOptimalPlannerPtr initial_plan_teb = getInitialPlanTEB(); if (best_teb_ std::find(tebs_.begin(), tebs_.end(), best_teb_) != tebs_.end()) min_cost_last_best = best_teb_->getCurrentCost() * cfg_->hcp selection_cost_hysteresis; // small hysteresis last_best_teb_ = best_teb_; else last_best_teb_.reset(); if (initial_plan_teb) // the validity was already checked in getInitialPlanTEB() min_cost_initial_plan_teb = initial_plan_teb->getCurrentCost() * cfg_->hcp.selection_prefer_initial_plan; // small hysteresis //initial origin best_teb_.reset(); // reset pointernew_best_teb_across.reset(); double dt = cfg_->SocialCoefficients.robot_dt; double freq_ = cfg_->SocialCoefficients.subcribe_frequency; double vr = vx_ * dt * freq_; double th = global_sh / global_vh; double sr_compare = vr * th; double getS = 0; if (global_xr < 0) getS = abs(global_xr) - sr_compare; else getS = 0; Teta = 0.0; sum_xCentro = 0.0, sum_yCentro = 0.0; //get centroId obstacles Start_.x() = global_xr; Start_.y() = global_yr; Eigen::Vector2d diff(Goal_.x() - Start_.x(), Goal_.y() - Start_.y()); Eigen::Vector2d normal(-diff.y(),diff.x()); normal.normalize(); if (obstacles()->size() 119 begin(); it_obst != obstacles()->end(); ++it_obst) PointCenter.x() = (*it_obst)->getCentroid().x(); PointCenter.y() = (*it_obst)->getCentroid().y(); Point1.x() = PointCenter.x() - normal.x(); Point2.x() = PointCenter.x() + normal.x(); Point2.y() = PointCenter.y() + normal.y(); global_two_obstacles = 0.0; //get teb from set ’if’ if (getS < -1.0) //Overtaking (select left) if (determine_left_right_on(Point1,PointCenter,Goal_) == -90)//left PointSel = Point1; else if (determine_left_right_on(Point1,PointCenter,Goal_) == 90) PointSel = Point2; else//avoiding (select right) //check point left right vs Robot-Goal if (determine_left_right_on(Point1,PointCenter,Goal_) == -90) PointSel = Point2; else if (determine_left_right_on(Point1,PointCenter,Goal_) == 90) PointSel = Point1; return best_teb_; TebOptimalPlannerPtr HomotopyClassPlanner::AvoidTheDoorselectBestTeb() double min_cost = std::numeric_limits::max(); double new_min_cost = std::numeric_limits::max(); double min_cost_last_best = std::numeric_limits::max(); double min_cost_initial_plan_teb = std::numeric_limits::max(); TebOptimalPlannerPtr initial_plan_teb = getInitialPlanTEB(); // check if last best_teb is still a valid candidate 120 if (best_teb_ std::find(tebs_.begin(), tebs_.end(), best_teb_) != tebs_.end() // get cost of this candidate min_cost_last_best = best_teb_->getCurrentCost() * cfg_->hcp.selection_cost_hysteresis; // small hysteresis last_best_teb_ = best_teb_; else last_best_teb_.reset(); if (initial_plan_teb) // the validity was already checked in getInitialPlanTEB() // get cost of this candidate min_cost_initial_plan_teb = initial_plan_teb->getCurrentCost() * cfg_->hcp.selection_prefer_initial_plan; // small hysteresis //initial origin best_teb_.reset(); // reset pointer new_best_teb_avoidTheDoor.reset(); Start_.x() = global_xr; Start_.y() = global_yr; Eigen::Vector2d diff(Goal_.x() - Start_.x(), Goal_.y() - Start_.y()); Point1.x() = global_sh - normal.x(); Point1.y() = - normal.y(); for (TebOptPlannerContainer::iterator it_teb = tebs_.begin(); it_teb != tebs_.end(); ++it_teb) double teb_cost;double get_minDis =0; double *get_dis = get_minDis; const Eigen::Ref ref_pointObs(PointSel); getTeb->teb().findClosestTrajectoryPose(ref_pointObs, get_dis,0); if (getTeb == last_best_teb_) teb_cost = min_cost_last_best; else if (getTeb == initial_plan_teb) 121 teb_cost = min_cost_initial_plan_teb; teb_cost = it_teb->get()->getCurrentCost(); //old TEB if (teb_cost < min_cost) best_teb_ = getTeb; min_cost = teb_cost; if (global_sh > 0) if (get_minDis < new_min_cost) new_best_teb_avoidTheDoor = getTeb; else new_best_teb_avoidTheDoor = best_teb_; return best_teb_; Mã nguồn thực thi giải thuật GTEB Eigen::Quaternionf rpy2Quaternion(double roll, double pitch, double yaw) Eigen::Quaternionf r_m = Eigen::AngleAxisf(roll, Eigen::Vector3f::UnitX()) * Eigen::AngleAxisf(pitch, Eigen::Vector3f::UnitY()) * Eigen::AngleAxisf(yaw, Eigen::Vector3f::UnitZ()); return r_m.normalized() animated_marker_msgs::AnimatedMarker humanVirtualUsingVel (int id, Vector2 pos, Vector2 vel, std::string name) animated_marker_msgs::AnimatedMarker marker; marker.mesh_use_embedded_materials = true; marker.header.frame_id = "odom"; marker.header.stamp = ros::Time(); marker.lifetime = ros::Duration(1); std::stringstream ss; ss « name « id; marker.ns = ss.str(); marker.id = id; 122 marker.type = animated_marker_msgs::AnimatedMarker::MESH_RESOURCE; marker.action = 0; // add or modify marker.pose.position.x = pos.getX(); marker.pose.position.y = pos.getY(); marker.animation_speed = sqrt(vel.getX()* vel.getX()+vel.getY()*vel.getY()); double theta = atan2 ( vel.getY(), vel.getX() ); Eigen::Quaternionf q = rpy2Quaternion (M_PI / 2.0, theta + (M_PI / 2.0), 0.0); marker.pose.orientation.z = q.z(); marker.pose.orientation.w = q.w(); marker.mesh_resource = "package: const double person_scale6 = 1.8 / 8.5 * 1.8; marker.scale.x = person_scale6; marker.scale.y = person_scale6; marker.scale.z = person_scale6; if ( id marker.color.r=0.15; marker.color.g=0.9; marker.color.b=0.9;marker.color.a=1; elseif(id marker.color.r=0.9; marker.color.g=0.9; marker.color.b=0.15;marker.color.a=1; else marker.color.r=0.9; marker.color.g=0.15; marker.color.b=0.9;marker.color.a=1; return marker; void setHuman(Vector2 h_pos, int id, double k) Vector2 vel,pos;vel.setX(foot_speed*cos(k)); 123 vel.setY(foot_speed*sin(k));//M_PI pos = h_pos; human_marker_array.markers.push_back (humanVirtualUsingVel(id, pos, vel,"human_obst_")); costmap_converter::ObstacleMsg obst_; geometry_msgs::Point32 new_pt; obst_.header.frame_id = "odom"; obst_.header.stamp = ros::Time(); obst_.id = 0; double theta = atan2 ( vel.getY(), vel.getX() ); Eigen::Quaternionf q = rpy2Quaternion(M_PI / 2.0, theta + (M_PI / 2.0), 0.0); obst_.orientation.x = q.x(); obst_.orientation.y = q.y(); obst_.orientation.z = k;//q.z(); obst_.orientation.w = q.w(); new_pt.x = h_pos.getX(); new_pt.y = h_pos.getY();new_pt.z = 0; obst_.polygon.points.push_back(new_pt); human_arr.obstacles.push_back(obst_); // =============== Main function ================= int main( int argc, char** argv ) ros::init(argc, argv, "human1");ros::NodeHandle n(" "); cfg_.loadRosParamFromNodeHandle(n); detected_human_vis_pub = n.advertise("/human1/human1_vis_pub",1); detected_human_pub = n.advertise("/test_optim_node/obstacles", 1); Vector2 h1,h2;double xh1 = -0.5, xh2 = -0.5; bool revse_ = true;ros::Rate loop_rate(20); while (ros::ok())human_arr.obstacles.clear(); human_marker_array.markers.clear();human_pos.clear(); human_agle_factor.clear(); foot_speed = 0.7; h1.setX(xh1);h1.setY(0.5);h2.setX(xh2);h2.setY(-0.5); human_pos.push_back(h1);human_pos.push_back(h2); if (revse_)human_agle_factor.push_back(0); human_agle_factor.push_back(0); xh1 = xh1 + 0.02;xh2 = xh2 + 0.02;foot_speed = 0.7; if (xh1 > 2.0)revse_ = false;else human_agle_factor.push_back(M_PI); human_agle_factor.push_back(M_PI); xh1 = xh1 - 0.02;xh2 = xh2 - 0.02; foot_speed = 0.7;if (xh1 < -1.5)revse_ = true; for (int i=0;i < human_pos.size();++i) setHuman(human_pos[i], i,human_agle_factor[i]); detected_human_vis_pub.publish(human_marker_array); detected_human_pub.publish(human_arr); ros::spinOnce();loop_rate.sleep(); ros::spin();return 0; 125