tf MessageFilter|ROSROS Kinetic知识

tf::MessageFilter可以订阅任何的ROS消息，然后将其缓存，直到这些消息可以转换到目标坐标系，然后进行相应的处理（进入回调函数）

tf::MessageFilter的初始化需要message_filters::Subscriber，tf转换，目标坐标系，等待时间。当message_filters::Subscriber的消息能够由tf转换到目标坐标系时，调用回调函数，回调函数由tf::MessageFilter::registerCallback()进行注册。

步骤:

  1. 定义数据：TransformListener、message_filters::Subscriber、tf::MessageFilter
  2. 用话题的名称来初始化message_filters::Subscriber
  3. 用tf message_filters::Subscriber 目标坐标系来初始化tf::MessageFilter
  4. 给tf::MessageFilter注册callback
    ，编写callback，并在回调中完成坐标转换，至此完成消息订阅+坐标转换
(三) 源码分析1 总体和laserCallback 第1部分|激光SLAMgmapping

到gmapping包里，查看CMakeLists可知，gmapping所用的可执行文件主要是slam_gmapping，对应slam_gmapping.cpp。

用到了头文件gridslamprocessor.h，其位于/opt/ros/kinetic/include/gmapping/gridfastslam/

gmapping用到的很重要的动态库有/opt/ros/kinetic/lib/libgridfastslam.so，它的来源是：

1
2
3
4
5
6
7
8
add_library(gridfastslam
    gfsreader.cpp
    gridslamprocessor.cpp
    gridslamprocessor_tree.cpp
    motionmodel.cpp
)
# 又链接库 scanmatcher  sensor_range
target_link_libraries(gridfastslam scanmatcher sensor_range)

scanmatcher库的来源：
add_library(scanmatcher eig3.cpp scanmatcher.cpp scanmatcherprocessor.cpp smmap.cpp)


main函数如下，显然就看SlamGMapping了：

1
2
3
4
ros::init(argc, argv, "slam_gmapping");
SlamGMapping gn;
gn.startLiveSlam();
ros::spin();

gmapping流程图.png

构造函数

构造函数只有两行，定义了随机数种子，供之后的高斯采样使用。然后是init函数，它也很简单，先是下面两行，看来主要是类GridSlamProcessor

1
2
gsp_ = new GMapping::GridSlamProcessor();
tfB_ = new tf::TransformBroadcaster();

GridSlamProcessor构造函数其实有用的就一句period_ = 5.0;，其他是参数赋值，但在initMapper里又会重新赋值，其实是多余的。 然后声明两个指针和布尔量，剩下的代码全是读取和配置参数了

startLiveSlam

1
2
3
4
5
6
7
8
9
10
11
12
//注册entryopy  map  map_metadata话题　　　dynamic_map服务
entropy_publisher_ = private_nh_.advertise<std_msgs::Float64>("entropy", 1, true);
sst_ = node_.advertise<nav_msgs::OccupancyGrid>("map", 1, true);
sstm_ = node_.advertise<nav_msgs::MapMetaData>("map_metadata", 1, true);
ss_ = node_.advertiseService("dynamic_map", &SlamGMapping::mapCallback, this);

scan_filter_sub_ = new message_filters::Subscriber<sensor_msgs::LaserScan>(node_, "scan", 5);
scan_filter_ = new tf::MessageFilter<sensor_msgs::LaserScan>(*scan_filter_sub_, tf_, odom_frame_, 5);
scan_filter_->registerCallback(boost::bind(&SlamGMapping::laserCallback, this, _1));

// 以成员函数publishLoop作为线程启动，参数为　transform_publish_period,默认　0.05
transform_thread_ = new boost::thread(boost::bind(&SlamGMapping::publishLoop, this, transform_publish_period_));

mapCallback是service的回调函数，留给使用者定义客户端，这样一来重点是laserCallback，稍后再分析。


线程transform_thread_也很简单：

1
2
3
4
5
6
7
8
9
if(transform_publish_period == 0)
  return;

ros::Rate r(1.0 / transform_publish_period);
while(ros::ok())
{
  publishTransform();
  r.sleep();
}


publishTransform如下：

1
2
3
4
5
map_to_odom_mutex_.lock();
ros::Time tf_expiration = ros::Time::now() + ros::Duration(tf_delay_);
// 构造函数里定义的广播，　发布map和odom之间的变换
tfB_->sendTransform( tf::StampedTransform (map_to_odom_, tf_expiration, map_frame_, odom_frame_));
map_to_odom_mutex_.unlock();

laserCallback 第1部分

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
laser_count_++;
/*每隔throttle_scans_ （默认值 1）帧数据计算一次，限流作用*/
if ((laser_count_ % throttle_scans_) != 0)
  return;

static ros::Time last_map_update(0,0);

if(!got_first_scan_)
{
  /*重要参数的初始化，将slam里的参数传递到 openslam 里 ，设定坐标系，坐标原点，以及
  采样函数随机种子的初始化，还调用了GridSlamProcessor::init,初始化了粒子数，子地图大小*/
  if(!initMapper(*scan))
      return;
  got_first_scan_ = true;
}

下面分析函数 bool SlamGMapping::initMapper(const sensor_msgs::LaserScan& scan)

  • 这个函数在收到的第一帧激光雷达数据的时候会被调用一次，之后就再也不会被调用了。
  • 这个函数的功能主要就是对gmapping算法中需要的一些参数进行赋值，即对gmapping算法进行初始化
  1. 判断激光雷达是否是水平放置的，如果不是 则报错。
  2. 假设激光雷达数据的角度是对称的 & 递增的 为每个激光束分配角度。
  3. 为gmapping算法设置各种需要的参数。

initMapper 第1部分

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
laser_frame_ = scan.header.frame_id;
// 雷达坐标系原点在base_link中的位置
tf::Stamped<tf::Pose> ident;
tf::Stamped<tf::Transform> laser_pose;
ident.setIdentity();
ident.frame_id_ = laser_frame_;   // "laser"
ident.stamp_ = scan.header.stamp;
ROS_INFO("ident : %f %f %f",ident.getOrigin().x(), ident.getOrigin().y(), ident.getOrigin().z());
try
{
  // 输出laser_pose，雷达坐标系原点在base_link中的位置
  tf_.transformPose(base_frame_, ident, laser_pose);
}
catch(tf::TransformException e)
{
  ROS_WARN("Failed to compute laser pose, aborting initialization (%s)",
           e.what());
  return false;
}
ROS_INFO("laser_pose: %f %f %f",laser_pose.getOrigin().x(), laser_pose.getOrigin().y(), laser_pose.getOrigin().z());
// base_link坐标系中，创建laser_pose１米之上的点，再把它转到laser坐标系
tf::Vector3 v;
v.setValue(0, 0, 1 + laser_pose.getOrigin().z());
tf::Stamped<tf::Vector3> up(v, scan.header.stamp, base_frame_);
try
{
  tf_.transformPoint(laser_frame_, up, up);
  ROS_DEBUG("Z-Axis in sensor frame: %.3f", up.z());
}
catch(tf::TransformException& e)
{
  ROS_WARN("Unable to determine orientation of laser: %s",
           e.what());
  return false;
}

// gmapping doesn't take roll or pitch into account.
// 这里是判断雷达是否倾斜
if (fabs(fabs(up.z()) - 1) > 0.001)
{
  ROS_WARN("Laser has to be mounted planar! Z-coordinate has to be 1 or -1, but gave: %.5f",
               up.z());
  return false;
}
gsp_laser_beam_count_ = scan.ranges.size();
// 扇形扫描面的中心
double angle_center = (scan.angle_min + scan.angle_max)/2;
// 雷达是否正装
if (up.z() > 0)
{
  do_reverse_range_ = scan.angle_min > scan.angle_max;
  // laser坐标系中的点(0,0,0), 方向(0,0,angle_center)
  centered_laser_pose_ = tf::Stamped<tf::Pose>(
    tf::Transform(tf::createQuaternionFromRPY(0,0,angle_center), 
      tf::Vector3(0,0,0)),  ros::Time::now(),  laser_frame_);
  ROS_INFO("Laser is mounted upwards.");
}
else
{
  do_reverse_range_ = scan.angle_min < scan.angle_max;
  centered_laser_pose_ = tf::Stamped<tf::Pose>(tf::Transform(tf::createQuaternionFromRPY(M_PI,0,-angle_center),
                                                             tf::Vector3(0,0,0)), ros::Time::now(), laser_frame_);
  ROS_INFO("Laser is mounted upside down.");
}
// Compute the angles of the laser from -x to x, basically symmetric and in increasing order
laser_angles_.resize(scan.ranges.size());
// 把角度调成对称，也就是扇形要关于ｚ轴对称，扫描角度从-3.124～3.142变为-3.133～3.133
double theta = - std::fabs(scan.angle_min - scan.angle_max)/2;
for(unsigned int i=0; i<scan.ranges.size(); ++i)
{
  laser_angles_[i]=theta;
  theta += std::fabs(scan.angle_increment);
}

ROS_INFO("Laser angles in laser-frame: min: %.3f max: %.3f inc: %.3f", scan.angle_min, scan.angle_max,
          scan.angle_increment);
ROS_INFO("Laser angles in top-down centered laser-frame: min: %.3f max: %.3f inc: %.3f", laser_angles_.front(),  laser_angles_.back(), std::fabs(scan.angle_increment));

// 这个是Gmapping算法接受的点类型
GMapping::OrientedPoint gmap_pose(0, 0, 0);
// 设置激光雷达的最大观测距离和最大使用距离
ros::NodeHandle private_nh_("~");
if(!private_nh_.getParam("maxRange", maxRange_))
    maxRange_ = scan.range_max-0.01;
if(!private_nh_.getParam("maxUrange", maxUrange_))
    maxUrange_=maxRange_;

GMapping::OrientedPoint

这个是Gmapping算法接受的点类型

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
template <class T, class A>
struct orientedpoint: public point<T>{
  inline orientedpoint() : point<T>(0,0), theta(0) {};
    inline orientedpoint(const point<T>& p);
    inline orientedpoint(T x, T y, A _theta): point<T>(x,y), theta(_theta){}
        inline void normalize();
    inline orientedpoint<T,A> rotate(A alpha)
    {
        T s=sin(alpha), c=cos(alpha);
        A a=alpha+theta;
        a=atan2(sin(a),cos(a));
        return orientedpoint(
            c*this->x-s*this->y,
            s*this->x+c*this->y, 
            a);
    }
    A theta;
};

template <class T, class A>
void orientedpoint<T,A>::normalize()
{
  if (theta >= -M_PI && theta < M_PI)
    return;
  
  int multiplier = (int)(theta / (2*M_PI));
  theta = theta - multiplier*2*M_PI;
  if (theta >= M_PI)
    theta -= 2*M_PI;
  if (theta < -M_PI)
    theta += 2*M_PI;
}

常用数据集|激光SLAM其他

我看到很多篇论文使用了log格式的数据集进行建图算法的仿真，比如这里的，发现这输入carmen框架，已经比较古老

  1. 进入informatik.uni-freiburg选择download log file下载数据集
  2. 看到一个文本网页,另存下载或者新建一個文件如data.clf保存.注意是.clf格式
  3. 这个clf文件显然不是我们想要的,可以转换为bag文件. 使用clf_to_bag.py


MIT data center的数据集页面
数据集2011-04-11-07-34-27.bag所用odom坐标系是odom_combined


俄勒冈大学的数据集

数据集hallway_slow_2011-03-04-21-41-33.bag所用的雷达话题是base_laser1_scan

参考：各类数据集

Matlab加载栅格地图|Matlab

实现二进制栅格地图

The object keeps track of three reference frames: world, local, and, grid. 世界坐标系的原点由GridLocationInWorld决定, which defines the bottom-left corner of the map relative to the world frame. LocalOriginInWorld property specifies the location of the origin of the local frame relative to the world frame. The first grid location with index (1,1) begins in the top-left corner of the grid.

map = binaryOccupancyMap creates a 2-D binary occupancy grid with a width and height of 10m. The default grid resolution is one cell per meter.

1
2
3
4
5
6
image = imread('F:\map.pgm');
%imshow(image)

imageBW = image < 100;
map = binaryOccupancyMap(imageBW);
show(map)

读取建好的pgm地图,如果直接imshow，结果就是pgm文件的本来样子

加下面几句后，结果是这样的
处理后的栅格地图.png

实现栅格地图

occupancyMap对象support local coordinates, world coordinates, and grid indices. The first grid location with index (1,1) begins in the top-left corner of the grid.

Use the occupancyMap class to create 2-D maps of an environment with probability values representing different obstacles in your world. You can specify exact probability values of cells or include observations from sensors such as laser scanners.

Probability values are stored using a binary Bayes filter to estimate the occupancy of each grid cell. A log-odds representation is used, with values stored as int16 to reduce the map storage size and allow for real-time applications.

1
2
3
4
5
image = imread('F:\map.pgm');
imageNorm = double(image)/255;
imageOccupancy = 1 - imageNorm;
map = occupancyMap(imageOccupancy,20);
show(map)

pgm文件中的值是0~255的uint8类型，将其归一化:先转为double类型,再除以255. 图片中的障碍物对应值为0,应该用1减去它,这样1就代表障碍物了. 否则图片显示出来是一团黑.

使用occupancyMap函数创建栅格地图,分辨率为1米20个cell,所支持的分辨率极限是±0.001

读取雷达扫描结果

1
2
3
4
5
6
7
8
9
10
11
12
13
14
# 建一个空地图,宽和高依次为10m,分辨率20,也就是1米有20个cell
map = occupancyMap(10,10,20);
# 机器人位姿[x,y,theta]
pose = [5,5,pi/2];
# ones函数是生成100行1列的矩阵, 元素全是1, 又都乘以3
# ones(n)函数是生成nXn的矩阵
ranges = 3*ones(100,1);
angles = linspace(-pi/2,pi/2,100);
maxrange = 20;
scan = lidarScan(ranges,angles);
# 把laser scan的数据插入栅格地图
insertRay(map,pose,scan,maxrange);

show(map)

linspace(x,y,n)是Matlab中的一个指令，用于产生x,y之间n点行矢量。其中x是起始值、y是中止值，n表示元素个数，如果缺省，则默认n为100。

linspace(1,10,2)为1，10. linspace(1,10,4)为1，4，7，10

laserScan对象

使用laserScan对象作为一些机器人算法的输入,例如matchScans, controllerVFH, monteCarloLocalization.

1
2
3
4
// ranges and angles inputs are vectors of the same length
scan = lidarScan(ranges,angles)
scan = lidarScan(cart)
scan = lidarScan(scanMsg) # 从ROS message中创建

plot(scan)可以显示雷达扫描的曲线

1
2
3
4
5
minRange = 0.1;
maxRange = 7;
scan2 = removeInvalidData(scan,'RangeLimits',[minRange maxRange]);
hold on
plot(scan2)

根据指定的范围,移除invalid数据

参考:Matlab occupancy map

全局路径规划(一) global_planner|路径规划move_base分析

概述

ROS 的navigation官方功能包提供了三种全局路径规划器：carrot_planner、global_planner、navfn，默认使用的是navfn。
继承关系 1.png

  • carrot_planner检查需要到达的目标是不是一个障碍物，如果是一个障碍物，它就将目标点替换成一个附近可接近的点。因此，这个模块其实并没有做任何全局规划的工作。在复杂的室内环境中，这个模块并不实用。

  • navfn使用Dijkstra算法找到最短路径。

  • global planner是navfn的升级版。它支持A*算法； 可以切换二次近似； 切换网格路径；

目前常用的是global_planner，需要先设定move_base的参数： base_global_planner: "global_planner/GlobalPlanner"。global_planner根据给定的目标位置进行总体路径的规划，只处理全局代价地图中的数据。提供快速的、内插值的全局规划，目前已经取代navfn。遵循navcore::navcore 包中指定的BaseGlobalPlanner接口。它接受costmap生成的全局代价地图规划出从起始点到目标点的路径，为local_planner规划路径作出参考。

global_planner没有提供类似D*这样的动态方法，而是用了定时规划路径，ROS是启动了一个线程，在移动过程中对路径不断的重新规划。这个feature是可以去掉的，特别是当你的运算负载很高，处理器又有限的情况下。还有重新规划(当找不到路径，也就是走着走着新扫描到未知区域的障碍或者动态增加的障碍)两种办法。加上了定时规划和重新规划之后的A*和D*几乎是一模一样的。

配置

move_base是通过plugin调用全局规划器的，文件bgp_plugin.xml

1
2
3
4
5
6
7
<library path="lib/libglobal_planner">
  <class name="global_planner/GlobalPlanner" type="global_planner::GlobalPlanner" base_class_type="nav_core::BaseGlobalPlanner">
    <description>
      A implementation of a grid based planner using Dijkstras or A*
    </description>
  </class>
</library>

在package.xml的配置中，加入如下行：
1
2
3
<export>
    <nav_core plugin="${prefix}/bgp_plugin.xml" />
</export>

参数

  • allow_unknown: true    是否允许规划器规划在未知区域创建规划，只设置该参数为true还不行,还要在costmap_commons_params.yaml中设置 track_unknown_space 参数也为true才行，

  • default_tolerance: 0.0    当设置的目的地被障碍物占据时，需要以该参数为半径寻找到最近的点作为新目的地点.

  • visualize_potential: false    是否显示从PointCloud2计算得到的势区域. 这个参数可以让你看见potential array的图像，看计算出的cost是怎么样子(颜色深浅代表距离起始点的远近)

  • use_dijkstra: true    设置为true,将使用dijkstra算法, 否则使用A*算法

  • use_quadratic: true    设置为true,将使用二次函数近似函数计算potential，否则使用更加简单的计算方式，这样节省硬件资源

  • use_grid_path: false    默认使用梯度下降法，路径更为光滑，从周围八个栅格中找到下降梯度最大的点。 如果为true，使用栅格路径，从终点开始找上下或左右4个中最小的栅格直到起点，会规划一条沿着网格边界的路径,偏向于直线穿越网格

  • old_navfn_behavior: false    navfn是非常旧的ROS系统中使用的，现在已经都用global_planner代替navfn了，所以不建议设置为true.

  • lethal_cost: 253    致命代价值,默认是设置为253,可以动态来配置该参数.

  • neutral_cost: 50    中等代价值,默认设置是50,可以动态配置该参数.

  • cost_factor: 3.0    代价地图与每个代价值相乘的因子.

  • publish_potential: true    是否发布costmap的势函数.

  • orientation_mode: 0    如何设置每个点的方向（None = 0,Forward = 1,Interpolate = 2,ForwardThenInterpolate = 3,Backward = 4,Leftward = 5,Rightward = 6）（可动态重新配置）

  • orientation_window_size: 1    根据orientation_mode指定的位置积分来得到使用窗口的方向.默认值1,可以动态重新配置.

       A*和Dijkstra两种算法

两种算法的效果对比
A.png
Dijkstra.png

A*比Dijkstra少计算很多，但可能不会产生相同路径。另外，在global_planner的A*里，the potentials are computed using 4-connected grid squares, while the path found by tracing the potential gradient from the goal back to the start uses the same grid in an 8-connected fashion. Thus, the actual path found may not be fully optimal in an 8-connected sense. (Also, no visited-state set is tracked while computing potentials, as in a more typical A* implementation, because such is unnecessary for 4-connected grids).

话题

发布的话题是~<name>/plan(nav_msgs/Path)，即最新规划出的路径，每次规划出新路径就要发布一次，主要用于观测。

GlobalPlanner::initialize

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
if (!initialized_)
{
    ros::NodeHandle private_nh("~/" + name);
    costmap_ = costmap;
    frame_id_ = frame_id;   //costmap_ros->getGlobalFrameID()

    unsigned int cx = costmap->getSizeInCellsX(), cy = costmap->getSizeInCellsY();
    // 参数赋值, 默认false
    private_nh.param("old_navfn_behavior", old_navfn_behavior_, false);
    if(!old_navfn_behavior_)
        convert_offset_ = 0.5;
    else
        convert_offset_ = 0.0;

    bool use_quadratic;   // 二次方的
    private_nh.param("use_quadratic", use_quadratic, true);

    // p_calc_声明为PotentialCalculator指针
    // QuadraticCalculator是它的派生类, 唯一区别是覆盖了calculatePotential函数
    if (use_quadratic)    // 默认使用二次
        p_calc_ = new QuadraticCalculator(cx, cy);
    else
        p_calc_ = new PotentialCalculator(cx, cy);

    bool use_dijkstra;
    private_nh.param("use_dijkstra", use_dijkstra, true);
    //DijkstraExpansion 和 AStarExpansion 都继承 Expander类
    if (use_dijkstra)
    {
        DijkstraExpansion* de = new DijkstraExpansion(p_calc_, cx, cy);
        if(!old_navfn_behavior_)
            de->setPreciseStart(true);
        planner_ = de;      // Expander* planner_;
    }
    else   // 如果不用dijkstra,就用A星算法.
        planner_ = new AStarExpansion(p_calc_, cx, cy);

    bool use_grid_path;
    private_nh.param("use_grid_path", use_grid_path, false);
    // GradientPath 和 GridPath都继承了Traceback
    if (use_grid_path)
        path_maker_ = new GridPath(p_calc_);
    else
        path_maker_ = new GradientPath(p_calc_);
        
    orientation_filter_ = new OrientationFilter();
    // NavfnROS::initialize中也注册此话题
    plan_pub_ = private_nh.advertise<nav_msgs::Path>("plan", 1);
    potential_pub_ = private_nh.advertise<nav_msgs::OccupancyGrid>("potential", 1);
    // 一系列参数赋值
    private_nh.param("allow_unknown", allow_unknown_, true);
    planner_->setHasUnknown(allow_unknown_);
    private_nh.param("planner_window_x", planner_window_x_, 0.0);
    private_nh.param("planner_window_y", planner_window_y_, 0.0);
    private_nh.param("default_tolerance", default_tolerance_, 0.0);
    private_nh.param("publish_scale", publish_scale_, 100);

    double costmap_pub_freq;
    private_nh.param("planner_costmap_publish_frequency", costmap_pub_freq, 0.0);

    //get the tf prefix
    ros::NodeHandle prefix_nh;
    tf_prefix_ = tf::getPrefixParam(prefix_nh);

    make_plan_srv_ = private_nh.advertiseService("make_plan", &GlobalPlanner::makePlanService, this);

    dsrv_ = new dynamic_reconfigure::Server<global_planner::GlobalPlannerConfig>(ros::NodeHandle("~/" + name));
    dynamic_reconfigure::Server<global_planner::GlobalPlannerConfig>::CallbackType cb = boost::bind(
            &GlobalPlanner::reconfigureCB, this, _1, _2);
    dsrv_->setCallback(cb);

    initialized_ = true;
} else
    ROS_WARN("This planner has already been initialized, you can't call it twice, doing nothing");

参考：
ROS- global_panner
global_planner历史背景和概述

Dijkstra算法|路径规划全局路径算法

以下面的加权图举例，说明算法的过程
加权图
加权图中不可有负权边
伪代码.png
或者这样理解：

  1. 第一个核心步骤：找到当前未处理过的顶点中 最小的点 V，（由于起点到起点的消耗为0，所以算法开始时 V 必定代表起点）；
  2. 第二个核心步骤：若V有邻居，则计算经过 V 的情况下起点到达各邻居的消耗 ，并选择是否更新 V 邻居的值。若没有邻居则对该点的处理结束
  3. 重复以上两个核心步骤，直到满足算法终止的条件：有向图中所有的点都被处理过。

算法每处理完一个顶点后，该顶点对应的 值就是起点到该点的最短路径长度，且在这之后不会被更改。这就是最短路径的原因
过程 1
过程 2

Dijkstra的特点是从起点开始，由近及远，层层扩展。越靠前处理的点离起点越近，最后一个处理的点离起点最远。
即使我只想找两个点之间的最短路径，Dijkstra还是需要遍历所有节点，因此时间复杂度为，所以不适合复杂路径等大规模场景。

A星算法|路径规划全局路径算法

地图和起点，终点

算法的伪代码.png

1.jpg

2.jpg

3.jpg

公式表示为：f(n)=g(n)+h(n)，其中f(n)是经过节点n从初始点到目标点的代价函数，g(n)表示从初始节点到节点n的代价，h(n)表示从节点n到目标点的启发式代价

  • Dijkstra是无目的的扩展，A星是启发式，选择离目标点最近的方向扩展，但不一定是最短路径。
  • Dijkstra算法的实质是广度优先搜索，是一种发散式的搜索，所以空间复杂度和时间复杂度都比较高。
  • 对路径上的当前点，A*算法不但记录其到源点的代价，还计算当前点到目标点的期望代价，是一种启发式算法，也可以认为是一种深度优先的算法。
Matlab中使用ROS|Matlab

把Matlab编写的路径规划算法用于ROS中的自主导航，matlab也有ros的接口，可以通过话题直接把算法结果发给ROS环境，控制机器人移动

实现Matlab和ROS的通信

master可以在Matlab上，但这种需求很少，一般在Ubuntu上，下面演示从Matlab连接ROS的过程

执行setenv('ROS_MASTER_URI','http://192.168.1.7:11311')，IP为master

然后执行rosinit，正常的话会出现下面提示：

1
2
The value of the ROS_MASTER_URI environment variable, http://192.168.1.7:11311, will be used to connect to the ROS master.
Initializing global node /matlab_global_node_37037 with NodeURI http://192.168.1.8:20811/

现在二者通信成功了，可以使用rosnodel list等命令了。但是与标准的ROS命令有所不同，以下列出常用的几个：
常用命令.png

rosshutdown用于退出ROS网络，在此之后Matlab和ROS不再通信。
rosshutdown.png

参考：Matlab中的ROS

Matlab常用操作|Matlab

matlab执行dos命令

dos函数： dos('ping 192.168.0.109')

for循环

1
2
3
4
5
6
7
% ii  ---循环变量，也就是循环次数
clc;clear;

for ii = 1:10 
  fprintf('value of a: %d\n', ii);
end
  fprintf('跳出循环后，value of a: %d\n', ii);

控制表达式产生了一个1ⅹ10数组，所以语句1到n将会被重复执行10次。注意在循环体在最后一次执行后，循环系数将会一直为10。

randn 和 rand 函数

randn：产生正态分布的随机数或矩阵的函数

randn：产生均值为0，方差σ^2 = 1，标准差σ = 1的正态分布的随机数或矩阵的函数。

  • Y = randn(n)：返回一个n*n的随机项的矩阵。如果n不是个数量，将返回错误信息。 n可以是0，此时为空矩阵

  • Y = randn(m,n) 或 Y = randn([m n])： 返回一个m*n的随机项矩阵。

产生一个随机分布的指定均值和方差的矩阵：将randn产生的结果乘以标准差，然后加上期望均值即可。例如：产生均值为0.6，方差为0.1的一个5*5的随机数方式如下：

1
x = .6 + sqrt(0.1) * randn(5)


rand函数产生由在(0, 1)之间均匀分布的随机数组成的数组

  • Y = rand(n): 返回一个n*n的随机矩阵如果n不是数量，则返回错误信息

  • Y = rand(m,n) 或 Y = rand([m n]): 返回一个m x n的随机矩阵

laser_filters|激光SLAM雷达

scan_tools提供了一系列用于激光SLAM的工具,在github的分支只到indigo,所以无法从ros直接安装,但可以编译源码安装. 其中重要的有:

  • laser_scan_matcher: an incremental laser scan matcher, using Andrea Censi’s Canonical Scan Matcher implementation. It downloads and installs Andrea Censi’s Canonical Scan Matcher [1] locally.

  • scan_to_cloud_converter: converts LaserScan to PointCloud messages.

laser_filters 过滤雷达数据

过滤器的机制和代价地图是类似的，节点scan_to_scan_filter_chain相当于代价地图，每一个filter相当于地图的每一层，通过加载yaml而加载filter。

以range过滤器为例，修改range_filter.yaml如下：

1
2
3
4
5
6
7
8
9
scan_filter_chain:
- name: box_filter
  type: laser_filters/LaserScanRangeFilter
  params:
    # use_message_range_limits: false   # if not specified defaults to false
    lower_threshold: 0.18  # 默认0
    upper_threshold: 0.22  # 默认100000.0
    lower_replacement_value: 0    # 默认 NaN
    upper_replacement_value: 999  # 默认 NaN

运行launch: roslaunch laser_filters range_filter_example.launch，里面就是节点scan_to_scan_filter_chain和yaml文件

结果查看scan_filtered话题，只显示出0.18~0.22距离的数据，太小的显示为0，太大的显示为999.

又比如使用LaserScanAngularBoundsFilter，只取-45°~45°内的scan，结果如下，要把laser坐标系的x轴放到水平向右的方向观察 ：

-45°至45°.png

参考：
scan_filtered的使用
laser_filters