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

admin 管理员组

文章数量: 1086019

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

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

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

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);//DEBUGsensor_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

以及这一篇.pdf

本文标签： 利用人工势场法的最短路径寻找