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

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

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

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