利用人工势场法的最短路径寻找

利用人工势场法的最短路径寻找人工势场法也可以用作机器人避障。我目前思考的是使其作为全局规划器,规划全局路径,也可以做局部规划直接下达至速度计算,目前暂时先看看全局路径计算。它将整个地图环境抽象为势场,机器人同时受到目标点的引力与障碍物的斥力,向合力的方向移动,当机器人逐步接近障碍物,受到的斥力越来越大以致偏离障碍物,达到避障的效果。如果做一个简化,每次计算便向合力方向延伸一个步长,便可逐渐到达终点。在栅格地图中,障碍物很…

大家好,又见面了,我是你们的朋友全栈君。

人工势场法也可以用作机器人避障。我目前思考的是使其作为全局规划器,规划全局路径,也可以做局部规划直接下达至速度计算,目前暂时先看看全局路径计算。它将整个地图环境抽象为势场,机器人同时受到目标点的引力与障碍物的斥力,向合力的方向移动,当机器人逐步接近障碍物,受到的斥力越来越大以致偏离障碍物,达到避障的效果。如果做一个简化,每次计算便向合力方向延伸一个步长,便可逐渐到达终点。

在栅格地图中,障碍物很多,看看栅格地图的说明就知道:

Raw Message Definition

# This represents a 2-D grid map, in which each cell represents the probability of
# occupancy.
Header header
#MetaData for the map
MapMetaData info
# The map data, in row-major order, starting with (0,0).  Occupancy
# probabilities are in the range [0,100].  Unknown is -1.
int8[] data

在一幅2000×2000,分辨率为0.02的地图中,动辄有一万以上的障碍物栅格,若是遍历障碍物来寻找当前栅格附近的障碍物的话消耗太大,在此用KD树保存障碍栅格并用K近邻算法找到最近的K个栅格,在此K要足够大以包括进附近的障碍。可以用PCL库的KD树来实现障碍栅格的储存和查找。

当然,障碍物需要设置一个“影响范围”,我们所处的位置如果在某障碍物范围外,即便是K近邻也不该受到其影响。障碍物的斥力可以由万有引力公式求出(与距离平方成反比),而目标点的引力可以与距离成正比也可以是类似重力的恒定值。调节两个受力参数是决定路径效果的一个重要因素。

但是用栅格来代表障碍物,计算量还是太大了,如果对栅格进行预处理,将一平方米的栅格,计算出密度与质心,这样可以将2000×2000的地图转化为40×40的障碍物集合,减少迭代时间。

克服一下懒劲,继续更新。。。

我们将地图处理为点云并将其每一平方米进行降采样(降采样后的点基本上就是质心),再对其进行K近邻查找。代码如下:

#include <ros/ros.h>
#include <nav_msgs/Path.h>
#include <geometry_msgs/PoseStamped.h>
#include <geometry_msgs/PoseWithCovarianceStamped.h>
#include <nav_msgs/OccupancyGrid.h>
#include <sensor_msgs/PointCloud2.h>
#include <pcl/kdtree/kdtree_flann.h>
#include <pcl_conversions/pcl_conversions.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/point_types.h>
#include <pcl/point_cloud.h>
#include "tf/tf.h"
#include <tf/transform_broadcaster.h>

using namespace std;

bool updateMap = false;
pcl::PointCloud<pcl::PointXYZ>::Ptr mapCloud(new pcl::PointCloud<pcl::PointXYZ>);

ros::Publisher cloudPub, pathPub;
double searchStep = 0.5;
/* 引力常数与斥力常数 */
double attraction = 10;
double repulsion = 15;
/* 认定到达终点的阈值 */
double goalTolerance = 0.5;


geometry_msgs::PoseStamped tempPose, goalPose;
nav_msgs::Path path;

pcl::KdTreeFLANN<pcl::PointXYZ> kdtree;

void mapCB(const nav_msgs::OccupancyGrid::ConstPtr &map)
{
  if(!updateMap)
  {
    mapCloud->clear();
    /* map data */
    double origin_x, origin_y, resolution;
    int width, height;
    origin_x = map->info.origin.position.x;
    origin_y = map->info.origin.position.y;
    //cout<<origin_x<<origin_y<<endl;
    width = map->info.width;
    height = map->info.height;
    resolution = map->info.resolution;
    //cout<<width<<" "<<height<<" "<<resolution<<endl;

    pcl::PointXYZ tempPoint;

    for(int i = 0; i < width; i++)
    {
      for(int j = 0; j < height; j++)
      {
        if(map->data[j * width + i] >= 90)
        {
          tempPoint.x = i * resolution + origin_x;
          tempPoint.y = j * resolution + origin_y;
          //cout<<tempPoint.x<<" "<<tempPoint.y<<endl;
          mapCloud->push_back(tempPoint);
        }
      }
    }

    pcl::PointCloud<pcl::PointXYZ>::Ptr temp_ptr(new pcl::PointCloud<pcl::PointXYZ>);
    temp_ptr = mapCloud;
    /* down size */
    pcl::VoxelGrid<pcl::PointXYZ> sor;
    sor.setInputCloud(temp_ptr);
    sor.setLeafSize(1, 1, 1);
    sor.filter(*mapCloud);

    //DEBUG
    sensor_msgs::PointCloud2 cloud;
    //cout<<"road_cloud_->size() = "<<road_cloud_->size()<<endl;
    pcl::toROSMsg(*mapCloud, cloud);
    cloud.header.frame_id = "map";
    cloudPub.publish(cloud);

    updateMap = true;
  }
}

void initialCB(const geometry_msgs::PoseWithCovarianceStamped::ConstPtr& pose)
{
  ROS_INFO_STREAM("initial");
  tempPose.pose = pose->pose.pose;
  path.header.frame_id = "map";
  path.poses.push_back(tempPose);
}

double distanceToGoal(const geometry_msgs::Pose& currentPose)
{
  double distance;
  distance = sqrt(pow(currentPose.position.x - goalPose.pose.position.x, 2) + pow(currentPose.position.y - goalPose.pose.position.y, 2));
  return distance;
}

double distance(pcl::PointXYZ x1, pcl::PointXYZ x2)
{
  double distance;
  distance = sqrt(pow(x1.x - x2.x, 2) + pow(x1.y - x2.y, 2));
  return distance;
}

void findOrientation(tf::Vector3& cross)
{
  pcl::PointXYZ searchPoint;
  searchPoint.x = tempPose.pose.position.x;
  searchPoint.y = tempPose.pose.position.y;
  int K = 20;
  std::vector<int> pointIdxNKNSearch(K);
  std::vector<float> pointNKNSquaredDistance(K);

  double crossX, crossY;

  if(kdtree.nearestKSearch(searchPoint, K, pointIdxNKNSearch, pointNKNSquaredDistance) > 0)
  {
    double currentForceX, currentForceY;
    double force;
    double forceX, forceY;
    double repulsionX = 0, repulsionY = 0;
    double attractForce, attractForceX, attractForceY;
    for(int i = 0; i < K; i++)
    {
      /* 计算受力的合力,对于斥力来说模型是类似于电磁力的斥力,与距离平方成反比 */
      force = double(repulsion / pow(pointNKNSquaredDistance[i], 2));
      forceX = force * (searchPoint.x - mapCloud->points[pointIdxNKNSearch[i]].x) / distance(searchPoint, mapCloud->points[pointIdxNKNSearch[i]]);
      forceY = force * (searchPoint.y - mapCloud->points[pointIdxNKNSearch[i]].y) / distance(searchPoint, mapCloud->points[pointIdxNKNSearch[i]]);
      repulsionX += forceX;
      repulsionY += forceY;
    }
    /* 对于引力,是与距离成正比的,也就是说可以把环境理解为抛物面模型 */
    attractForce = attraction * distanceToGoal(tempPose.pose);
    attractForceX = attractForce * (goalPose.pose.position.x - searchPoint.x)/distanceToGoal(tempPose.pose);
    attractForceY = attractForce * (goalPose.pose.position.y - searchPoint.y)/distanceToGoal(tempPose.pose);

    currentForceX = attractForceX + repulsionX;
    currentForceY = attractForceY + repulsionY;

    crossX = currentForceX / sqrt(pow(currentForceX, 2) + pow(currentForceY, 2));
    crossY = currentForceY / sqrt(pow(currentForceX, 2) + pow(currentForceY, 2));
    tf::Vector3 temp(crossX, crossY, 0);
    cross = temp;
  }
  else
  {
    ROS_ERROR("No neighbor?");
  }
}

void goalCB(const geometry_msgs::PoseStamped::ConstPtr& pose)
{
  ROS_INFO_STREAM("goal");
  goalPose = *pose;
  kdtree.setInputCloud(mapCloud);

  double distance = distanceToGoal(tempPose.pose);
  while(distance > goalTolerance)
  {
    tf::Vector3 cross;
    findOrientation(cross);
    tempPose.pose.position.x = tempPose.pose.position.x + searchStep * cross.getX();
    tempPose.pose.position.y = tempPose.pose.position.y + searchStep * cross.getY();
    path.poses.push_back(tempPose);
    distance = distanceToGoal(tempPose.pose);
  }
  tempPose.pose = pose->pose;
  path.poses.push_back(tempPose);
  pathPub.publish(path);
}

int main(int argc, char** argv)
{
  ros::init(argc, argv, "force_path");
  ros::NodeHandle n;
  pathPub = n.advertise<nav_msgs::Path>("force_path", 1);
  cloudPub = n.advertise<sensor_msgs::PointCloud2>("map_cloud", 1);
  ros::Subscriber initSub = n.subscribe<geometry_msgs::PoseWithCovarianceStamped>("initialpose", 1, &initialCB);
  ros::Subscriber goalSub = n.subscribe<geometry_msgs::PoseStamped>("move_base_simple/goal", 1, &goalCB);
  ros::Subscriber mapSub = n.subscribe<nav_msgs::OccupancyGrid>("map", 1, &mapCB);
  ros::spin();
  return 0;
}

 效果图如下,仅限参考。。。好像引力和斥力常数需要再调整一下,防止震荡。。。各位看官有空还是看一下论文吧。最初提出的论文应该是Oussama Khatib,Real-Time obstacle Avoidance for Manipulators and Mobile Robots. Proc of The 1994 IEEE

以及这一篇http://kovan.ceng.metu.edu.tr/~kadir/academia/courses/grad/cs548/hmws/hw2/report/apf.pdf

利用人工势场法的最短路径寻找

版权声明:本文内容由互联网用户自发贡献,该文观点仅代表作者本人。本站仅提供信息存储空间服务,不拥有所有权,不承担相关法律责任。如发现本站有涉嫌侵权/违法违规的内容, 请发送邮件至 举报,一经查实,本站将立刻删除。

发布者:全栈程序员-用户IM,转载请注明出处:https://javaforall.cn/150624.html原文链接:https://javaforall.cn

【正版授权,激活自己账号】: Jetbrains全家桶Ide使用,1年售后保障,每天仅需1毛

【官方授权 正版激活】: 官方授权 正版激活 支持Jetbrains家族下所有IDE 使用个人JB账号...

(0)
blank

相关推荐

  • 大屏数据可视化案例「建议收藏」

    大屏数据可视化案例「建议收藏」数据可视化:把相对复杂的、抽象的数据通过可视的、交互的方式进行展示,从而形象直观地表达数据蕴含的信息和规律。数据可视化是数据空间到图形空间的映射,是抽象数据的具象表达。数据可视化交互的基本原则:总览为先,缩放过滤按需查看细节。大屏数据可视化是当前可视化领域的一项热门应用,通常可以分为信息展示类、数据分析类及监控预警类。大屏数据可视化应用的难点并不在于图表类型的多样化,而在于如何能在…

  • URAL 1146

    URAL 1146题目大意:给出一个N*N的矩阵,每个元素的范围为[-127,+127]。求其所有子矩阵中各元素之和的最大值。TimeLimit:1000MS     MemoryLimit:65536KB     64bitIOFormat:%I64d&%I64u数据规模:N理论基础:无。题目分析:求和问题,用前缀和(每一行)+枚举(对列的起始与终点和行的起始与终点枚举)即

  • 开发发版流程_文件签发流程

    开发发版流程_文件签发流程迭代流程开发人员:周一到周五产品设计:周一到周五测试人员:周六收集需求:周一周二周三周四需求梳理周五用户意见周六第二次需求梳理需求阶段第一次需求梳理会议开发人员和测试人员通过此会议了解下一次迭代

  • Conda Install Package Error[通俗易懂]

    Conda Install Package Error[通俗易懂]问题:conda无法安装更新,报错内容如下:参考链接:condahttperrorhttpnonenoneforurlnoneAnaconda更新失败$condacreate-ntensorflowpython=3.5Fetchingpackagemetadata…….CondaHTTPError:HTTP000CONNECTIONFAILED

  • iOS插件列表

    iOS插件列表

  • matlab 画折线图

    matlab 画折线图代码:效果图:x=1:1:5就是x轴上的数据,从1开始到5结束(即应该有五个数据),每个数据的间隔是1.把开始的1改成2,结束的5改成6,整个折线图就会向右平移一个单位。plot(x,a,’-*b’,x,b,’-or’)是设置折线图中相应点和线的特征的,函数说明如下:对于‘’内的线条形状,总结了如下图:线型:线条宽度:指定线条的宽度,取值为整数(…

发表回复

您的电子邮箱地址不会被公开。

关注全栈程序员社区公众号