ros機器人路徑遠離障礙物的方法

A星或dijkstra規(guī)劃的路徑會貼著障礙物，如果膨脹半徑設置過小機器人在跟蹤路徑運動時會碰到障礙物，特別是在轉(zhuǎn)彎的時候。
這里提供一種路徑優(yōu)化的方法讓路徑與膨脹層保持一定距離。
步驟：
1、遍歷所有的路徑點，記錄下路徑點周圍一定范圍length(可設置)的障礙物。
2、若所有的障礙物都在路徑的同一測則找到距離該路徑點A最近的障礙物點B，設最近的距離為d。
3、將路徑點A平移，平移的方向為A指向B的方向，平移的距離為length - d，即讓A與B的保持距離為length。
效果圖如下：

紅色的路徑為未優(yōu)化的全局路徑，綠色的為優(yōu)化后的局部路徑

代碼如下：

void filtePath(std::vector<geometry_msgs::PoseStamped> &plan,double safe_distance){if(plan.empty()){ROS_INFO("PurePlannerROS::filtePath: plan is empty.");return;}int safe_cell = (int)( safe_distance / this->costmap_->getResolution() );if(safe_cell < 1){ROS_INFO("The safety distance is too small.");return;}size_t point_size = plan.size();geometry_msgs::PoseStamped tem_point;geometry_msgs::PoseStamped before_point;geometry_msgs::PoseStamped next_point;geometry_msgs::PoseStamped nearest_obstacle;unsigned int mx_min,mx_max,my_min,my_max,mx,my;for(size_t i=0;i<point_size;i++){tem_point = plan[i];before_point = i>0?plan[i-1]:plan[i];next_point = i<point_size-1?plan[i+1]:plan[i];this->costmap_->worldToMap(tem_point.pose.position.x,tem_point.pose.position.y,mx,my);mx_min = mx>safe_cell?mx-safe_cell:mx;mx_max = mx+safe_cell<this->costmap_->getSizeInCellsX()?mx+safe_cell:mx;my_min = my>safe_cell?my-safe_cell:my;my_max = my+safe_cell<this->costmap_->getSizeInCellsY()?my+safe_cell:my;std::vector<geometry_msgs::Point> obstacle_vec;geometry_msgs::Point obstacle;obstacle_vec.clear();for(unsigned int j=mx_min;j<mx_max;j++) //Find all obstacles within a safe distance.{for(unsigned int k=my_min;k<my_max;k++){if(this->costmap_->getCost(j,k) != costmap_2d::FREE_SPACE){this->costmap_->mapToWorld(j,k,obstacle.x,obstacle.y);obstacle_vec.push_back(obstacle);}}}if(obstacle_vec.empty() != true){//Check if the points are on the same side.bool same_side_flag = false;if(next_point.pose.position.x != before_point.pose.position.x){double lk = 0,lb = 0,ly = 0,num = 0;lk = (next_point.pose.position.y-before_point.pose.position.y) / (next_point.pose.position.x-before_point.pose.position.x);lb = next_point.pose.position.y - lk * next_point.pose.position.x;for(size_t m=0;m<obstacle_vec.size();m++){ly = lk * obstacle_vec[m].x + lb;if(ly != 0)break;}for(size_t m=0;m<obstacle_vec.size();m++){num = ly*(lk * obstacle_vec[m].x + lb);if(num < 0){same_side_flag = true;break;}}}else{double const_x = next_point.pose.position.x;double err = 0,num = 0;for(size_t m=0;m<obstacle_vec.size();m++){err = const_x - obstacle_vec[m].x;if(err != 0)break;}for(size_t m=0;m<obstacle_vec.size();m++){num = err*(const_x - obstacle_vec[m].x);if(num < 0){same_side_flag = true;break;}}}if(same_side_flag == true){ROS_INFO("These points are not on the same side.");continue;}double distance=0,min_distance_obst = 1000.0;size_t min_obst_index = 0;double diff_x,diff_y;for(size_t l=0;l<obstacle_vec.size();l++) //find nearest obstacle{diff_x = obstacle_vec[l].x - tem_point.pose.position.x;diff_y = obstacle_vec[l].y - tem_point.pose.position.y;distance = sqrt(diff_x*diff_x+diff_y*diff_y);if(min_distance_obst > distance){min_distance_obst = distance;min_obst_index = l;}}if(safe_distance - min_distance_obst < 0.0){continue;}nearest_obstacle.pose.position.x = obstacle_vec[min_obst_index].x;nearest_obstacle.pose.position.y = obstacle_vec[min_obst_index].y;distance = safe_distance - min_distance_obst;double err_x,err_y,theta,finally_x,finally_y;theta = atan2(tem_point.pose.position.y-nearest_obstacle.pose.position.y,tem_point.pose.position.x-nearest_obstacle.pose.position.x);err_x = distance*cos(theta);err_y = distance*sin(theta);finally_x = tem_point.pose.position.x + err_x;finally_y = tem_point.pose.position.y + err_y;this->costmap_->worldToMap(finally_x,finally_y,mx,my);if(this->costmap_->getCost(mx,my) == costmap_2d::FREE_SPACE){plan[i].pose.position.x = finally_x;plan[i].pose.position.y = finally_y;}}}}

總結

以上是生活随笔為你收集整理的ROS：：ros机器人路径远离障碍物的方法的全部內(nèi)容，希望文章能夠幫你解決所遇到的問題。

如果覺得生活随笔網(wǎng)站內(nèi)容還不錯，歡迎將生活随笔推薦給好友。