源码分析(一) 总体逻辑|激光SLAMamcl和粒子滤波

文件amcl_node.cpp最主要的几个函数是构造函数，handleMapMessage, laserReceived。尤其是最后一个

杂项

sigintHandler回调函数在关闭程序之前只有一句amcl_node_ptr->savePoseToServer();，也就是关闭前把最近一次的机器人位姿存入参数服务器，这就可以理解为什么关掉程序后，用rosparam get获得的initial_pose_x等位姿仍然是以前的。

amcl根据输入参数来决定运行方式。从代码中可以看到amcl支持数据回放的，运行时只需要通过参数--run-from-bag指定bag文件即可

从main函数可以看出，amcl的所有业务逻辑都是由AmclNode类的构造函数完成

AmclNode构造函数

通过参数服务器，根据launch文件里的大量参数，对各类变量赋值

成员函数updatePoseFromServer，该函数就是用来从参数服务器上获取机器人的初始位姿和位姿误差的协方差矩阵的。

发布amcl_pose和particlecloud两个主题，分别用于输出机器人位姿估计和粒子集合。

接着注册三个服务：global_localization用于获取机器人的全局定位，request_nomotion_update则用于手动的触发粒子更新并发布新的位姿估计，set_map用于设定机器人位姿和地图信息。 这几个平时都用不到



接下来构建激光传感器的消息过滤器对象和tf2的过滤器，并注册回调函数laserReceived。

1
2
3
4
5
6
7
8
laser_scan_sub_ = new message_filters::Subscriber<sensor_msgs::LaserScan>(nh_, scan_topic_, 100);
laser_scan_filter_ = new tf::MessageFilter<sensor_msgs::LaserScan>(
                                  *laser_scan_sub_, 
                                  *tf_, 
                                  odom_frame_id_, 
                                  100 );
laser_scan_filter_->registerCallback(boost::bind(&AmclNode::laserReceived,
                                                 this, _1));

这里的message_filter为ROS系统提供了一些通用的消息过滤方法， 它对接收到的消息进行缓存，只有当满足过滤条件后才输出，在需要消息同步的时候应用比较多。这里主要是同时监听激光扫描消息和里程计坐标变换，同步两者的输出。 这一套路在GMapping中也有用到。

订阅用于初始化机器人位姿估计的主题initialpose

根据运行参数use_map_topic_获取地图，或者订阅地图主题，或者通过requestMap请求static_map服务。因为一般取false，所以后面我们直接看requestMap函数，也就是如下逻辑：

定义m_force_update为false，后面会用到

最后定义一个计时器用于每隔15s检查一次激光雷达的接收数据，如果期间没有收到新的数据给出告警

requestMap 和 handleMapMessage

requestMap函数请求static_map服务成功后，调用函数handleMapMessage处理接收到的地图数据，进一步完成AmclNode的初始化工作，内容庞大的其实是handleMapMessage

handleMapMessage的流程图在这里

释放了与地图相关的内存和对象之后，通过函数convertMap将地图消息转换成amcl中的地图数据结构。

接着构建粒子滤波器对象，并完成初始化：

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
// 构建粒子滤波器对象
pf_ = pf_alloc(min_particles_,
               max_particles_,
               alpha_slow_,
               alpha_fast_,
               (pf_init_model_fn_t)AmclNode::uniformPoseGenerator,
               (void *)map_ );
pf_->pop_err = pf_err_;
pf_->pop_z = pf_z_;

// 从参数服务器获取初始位姿及方差放到pf中
updatePoseFromServer();
pf_vector_t pf_init_pose_mean = pf_vector_zero();
pf_init_pose_mean.v[0] = init_pose_[0];   // initial_pose_x
pf_init_pose_mean.v[1] = init_pose_[1];   // initial_pose_y
pf_init_pose_mean.v[2] = init_pose_[2];   // initial_pose_a

pf_matrix_t pf_init_pose_cov = pf_matrix_zero();
pf_init_pose_cov.m[0][0] = init_cov_[0];
pf_init_pose_cov.m[1][1] = init_cov_[1];
pf_init_pose_cov.m[2][2] = init_cov_[2];

// 初始化高斯粒子滤波器
pf_init(pf_, pf_init_pose_mean, pf_init_pose_cov);
pf_init_ = false;

这里面有两个重要函数pf_alloc和pf_init

构建里程计对象，根据launch文件里的参数配置里程计模型

构建雷达传感器对象，根据运行参数laser_model_type_构建不同模型的雷达

最后考虑到，在接收到地图数据之前，有可能已经接收到了机器人的初始位姿，这里调用函数applyInitialPose处理

pf_alloc

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
// Create a new filter
pf_t *pf_alloc(int min_samples, int max_samples,
               double alpha_slow, double alpha_fast,
               pf_init_model_fn_t random_pose_fn, void *random_pose_data)
{
  int i, j;
  pf_t *pf;
  pf_sample_set_t *set;
  pf_sample_t *sample;
  
  srand48(time(NULL));

  pf = calloc(1, sizeof(pf_t));

  pf->random_pose_fn = random_pose_fn;
  pf->random_pose_data = random_pose_data;

  pf->min_samples = min_samples;
  pf->max_samples = max_samples;

  // Control parameters for the population size calculation.  [err] is
  // the max error between the true distribution and the estimated
  // distribution.  [z] is the upper standard normal quantile for (1 -
  // p), where p is the probability that the error on the estimated
  // distrubition will be less than [err].
  pf->pop_err = 0.01;
  pf->pop_z = 3;
  pf->dist_threshold = 0.5; 
  pf->current_set = 0;
  for (j = 0; j < 2; j++)
  {
    set = pf->sets + j;

    set->sample_count = max_samples;
    set->samples = calloc(max_samples, sizeof(pf_sample_t));

    for (i = 0; i < set->sample_count; i++)
    {
      sample = set->samples + i;
      sample->pose.v[0] = 0.0;
      sample->pose.v[1] = 0.0;
      sample->pose.v[2] = 0.0;
      sample->weight = 1.0 / max_samples;
    }

    // HACK: is 3 times max_samples enough?
    set->kdtree = pf_kdtree_alloc(3 * max_samples);

    set->cluster_count = 0;
    set->cluster_max_count = max_samples;
    set->clusters = calloc(set->cluster_max_count, sizeof(pf_cluster_t));

    set->mean = pf_vector_zero();
    set->cov = pf_matrix_zero();
  }

  pf->w_slow = 0.0;
  pf->w_fast = 0.0;
  pf->alpha_slow = alpha_slow;
  pf->alpha_fast = alpha_fast;

  //set converged to 0
  pf_init_converged(pf);
  return pf;
}

pf_init

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
// Initialize the filter using a guassian
void pf_init(pf_t *pf, pf_vector_t mean, pf_matrix_t cov)
{
  int i;
  pf_sample_set_t *set;
  pf_sample_t *sample;
  pf_pdf_gaussian_t *pdf;
  
  set = pf->sets + pf->current_set;
  
  // Create the kd tree for adaptive sampling
  pf_kdtree_clear(set->kdtree);

  set->sample_count = pf->max_samples;

  pdf = pf_pdf_gaussian_alloc(mean, cov);
    
  // Compute the new sample poses
  for (i = 0; i < set->sample_count; i++)
  {
    sample = set->samples + i;
    sample->weight = 1.0 / pf->max_samples;
    sample->pose = pf_pdf_gaussian_sample(pdf);

    // Add sample to histogram
    pf_kdtree_insert(set->kdtree, sample->pose, sample->weight);
  }

  pf->w_slow = pf->w_fast = 0.0;

  pf_pdf_gaussian_free(pdf);
    
  // Re-compute cluster statistics
  pf_cluster_stats(pf, set); 

  //set converged to 0
  pf_init_converged(pf);

  return;
}
在命名空间中用非成员函数取代成员函数|C++C++ 面向对象

这个就是Effective C++条款23，假如类有多个成员函数，现在需要在一个函数里调用这几个函数，那么最好不要把它定义为成员函数，而是类外的普通函数。

最常见的做法是定义一个namespace，把类和普通函数都放到里面，namespace可以跨越多个文件，但类不能。这正是C++标准库std的风格，std有数十个文件，每个声明std的部分机能，这样能 降低编译的依存性。 

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
// 头文件
namespace test {

class Base
{
public:
    Base();
    ~Base();
    void setA(int a);
    void setB(int b);
private:
    int m_a;
    double m_b;
};
//  这里声明，但不能定义函数体，否则编译不通过
void setAB(Base& obj, int a, int b);
}

// 源文件
test::Base::Base():
    m_a(0),
    m_b(0)
{
    cout<<"base constructor"<<endl;
}

test::Base::~Base()
{
    cout<<"base destructor"<<endl;
}

void test::Base::setA(int a)
{
    m_a = a;
}

void test::Base::setB(int b)
{
    m_b = b;
}
//必须加test:: ，否则编译不通过
void test::setAB(Base& obj, int a, int b)
{
    obj.setA(a);
    obj.setB(b);
}

// 调用
test::Base b;
test::setAB(b, 43, 23);

setAB函数在类Base之外，这样的封装性比作为成员函数要好，它并不增加能访问类private成员的函数数量，有较低的编译相依度。

类放到命名空间后，继承类可以像平常那样继承，不必也放到这个命名空间里。

内联函数|C++C++ 面向对象

内联函数最初的目的:

  1. 代替部分#define 宏定义
  2. 替代普通函数:提高程序的运行效率

如果一些函数被频繁调用，不断地有函数进入函数栈，会造成栈空间的大量消耗。因此引入了内联函数。

函数调用是有时间和空间开销的。执行一个函数之前要将实参、局部变量、返回地址等压入栈中，然后执行函数体中，执行完毕后还要让之前压入栈中的数据都出栈。如果函数体代码比较多，需要较长的执行时间，那么函数调用机制占用的时间可以忽略:如果函数只有一两条语句，那么调用机制的时间开销就就不容忽视。

内联函数是在编译阶段将函数展开，而不用将函数地址压入栈，避免调用的开销，但增大了可执行程序的体积。因此内联函数一般比较小，用于大量调用的场合。如果在类体内定义实现，那么就是内联函数；也可以在类内用inline声明，在体外实现。

其实上面这些还没触及到本质，还有以下几条:

  1. inline只是我们对编译器提出的一个申请，是否真按内联函数处理取决于编译器，比如开启优化选项时才考虑内联函数

  2. 函数不能有循环和递归，不能太大，否则仍是普通函数

  3. 编译器的内联看起来就像是代码的复制与粘贴，这与预处理宏是很不同的：宏是强制的内联展开，可能将会污染所有的命名空间与代码，将为程序的调试带来困难，所以最好用内联函数代替宏


  • 内联函数无法随程序库升级而升级，比如修改了一个内联函数，那么所有用到它的地方都要重新编译，而普通函数做修改后，程序只需要重新链接它，效率比重新编译高

  • 如果一个内联函数会在多个源文件中被用到，那么最好把它定义在头文件中，否则必须在调用该函数的每个文件中定义

  • 构造函数和析构函数不适合做内联函数，编译器在编译期间会给你的构造函数和析构函数额外加入很多的代码，像成员函数的构造和析构等代码，所以通常构造和析构函数比表面上看起来的要多，并不适合作为内联函数

内联函数和宏的区别：

  1. 内联函数展开在编译阶段，宏在预处理阶段

  2. 内联函数会经过编译器安全检查，宏定义的参数没有类型的概念，只有在宏展开以后，才由编译器检查语法，这就存在很多的安全隐患

详细内容看《Effective C++》条款30

总结Effective C++条款|C++C++ 面向对象

最近重读了《Effective C++》，对一些复杂的知识点会专门写文章分析，这里将一些小的知识点总结一下，但不会涵盖书里的所有内容。

条款2

  • 良好的用户自定义类型的特征是它尽量能与内置类型兼容

  • 宁可用编译器替换预处理器

  • 尽量多用const,而不是宏定义.宏属于模块化的设计概念,会破坏封装性

  • 常量可能比#define产生较小量的代码.

  • 如果类需要用到多个常量,不要用const数组,而是改用枚举

  • 对于宏形式的函数,最好用内联函数取代

条款3

  • 希望迭代器指向的东西不可改动,需要const_iterator

条款4

  • 永远在使用对象前初始化.对于int x;,有些编译器会将x初始化为0,有些却不会,所以无论之后有没有用到x,都要先对其初始化,比如int x=0;

  • 成员变量如果是const或引用,它们只能用成员列表初始化的方法进行初始化

条款5

  • 编译器自动生成的析构函数不是virtual,除非这个类的基类的析构函数是virtural

  • 编译器自动生成的4个函数都是public

  • 如果基类的copy构造函数和copy运算符是private,编译器不会为派生类生成它们

  • 派生类的copy构造函数可以尽量不定义

条款6

  • 定义uncopyable类是很好的禁止使用copy构造函数和copy运算符的方法，它的析构函数可以不是virtual,派生类不必以public继承它,也没有成员变量,也可以用于多重继承

  • 类的不可拷贝特性是可以继承的，例如凡是继承自uncopyable的类都不能使用copy构造函数和赋值运算符

条款7

  • 如果基类析构函数不声明为virtual，析构时不会调用派生类的析构，可查看原因分析

  • 如果一个类不做基类，就不要有virtual析构函数或其他虚函数，因为虚指针会增大类的体积。 反过来，只要一个类做基类，就要有virtual函数

  • 当类至少有一个虚函数时，为它声明virtual析构函数，否则编译器有报警

  • 对于抽象类，可以将析构函数声明为pure virtual析构函数

条款8

  • 析构函数里不要抛出异常，否则会导致不确定行为

条款9

  • 构造函数和析构函数中都不要调用virtual函数，因为基类构造时，virtual函数不会下降到派生类阶层，或者说此时的virtual函数还不是virtual函数

  • 对于存在多个构造函数的情况，为避免代码重复，要把同样的代码放到一个函数里，比如init

条款10

  • 赋值运算符(包括+= -= *=)的返回最好是return *this，这符合STL等标准库的风格

条款12

  • 自定义copy构造函数或运算符时，需要复制所有的成员变量，如果少复制了，编译器不会报警或报错

  • 派生类的copy构造函数或运算符，无法像平时那样对基类的private成员变量赋值，因为无法访问private的变量，只能显式调用基类的operator =， 比如： Base::operator=(obj);

条款15

  • 多使用智能指针shared_ptr，它可以返回原始指针，显式方法是get()函数，隐式方法是取指针操作符->

条款16

  • new和delete必须配对，[]必须都有或都没有。千万避免new没有[]，delete有[]，这会导致程序不停运行析构函数

条款18

  • 有时的形参可以用wrapper类型，而不是内置类型

  • 保证接口的一致，比如STL容器都有个size的成员函数

  • 可以让一些返回指针的函数返回智能指针，比如工厂函数

条款21

  • 函数内返回对象时，不要返回其引用。因为引用指向局部变量，而局部变量在函数退出前销毁，所以会出现无定义行为。

条款22

  • 成员变量应该都是private，将它们隐藏在函数接口的背后，如果放到public，直接使用成员变量会降低封装性。如果破坏了成员变量，会破坏太多的客户码

  • 成员变量的封装性和成员变量的内容改变时破坏的代码量成反比

  • protected变量被消灭时，所有用到它的派生类的代码都要破坏，所以封装性也很差

条款24

  • 若函数参数都要进行类型转换，应该使用非成员函数

  • 成员函数的反面是非成员函数，不是friend函数，friend应该尽量避免

条款26

  • 如果某个类的对象没有用到，就不应该声明，否则运行构造和析构函数都会耗费成本

  • 对象初始化用构造函数比=运算符的效率高

  • 对变量定义后应马上初始化，然后马上使用，中间不要隔太远

  • 变量定义在循环内比外面更好，后者造成作用域更大，对程序维护性不好

条款27

  • 代码中尽量避免dynamic_cast，它会降低效率

  • 避免连续的cast转型，尤其是dynamic_cast

条款32

  • public继承是一种is-a的关系，每一个派生类的对象也是基类的对象

  • 程序的错误最好能在编译期检测出来，而不是运行期

条款34

  • public继承涉及到函数接口继承和实现的继承

  • 纯虚函数实际上是可以提供定义的，这个用法比较罕见，知道即可

  • 虚函数使派生类继承了其缺省实现，但这可能造成危险

条款36

  • 不要重新定义继承而来的non-virtual函数，虽然没有错误，但违反了is-a原则，这种情况下干脆不要使用public继承
构造函数的成员初始化列表 (二)|C++C++ 面向对象

根据 Effective C++，类的成员变量最好 都用成员列表初始化 的方法进行初始化，对内置类型没什么不同，但对类对象会提高效率。 成员初始化顺序是其声明的顺序,不是初始化列表中的顺序,二者最好一致

构造函数内赋值成员变量，先调用默认构造函数，再调用赋值运算符。 如果用成员列表初始化， 只调用拷贝构造函数，提高了效率

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
class Derived : public Base
{
public:
    Derived();
    Derived(Base obj)
    {
        m_b = obj;
        cout<<"derived constructor"<<endl;
    }
    ~Derived();
    Derived(const Derived& obj);
    Derived& operator=(const Derived& obj);

private:
    Base m_b;
};

实际调用:

1
2
3
Base b;
cout<<"----------------"<<endl;
Derived d(b);

运行结果:

1
2
3
4
5
6
7
8
9
10
11
12
13
base constructor
----------------
base copy constructor
base constructor
base constructor
base operator =
derived constructor
base destructor
----------------
derived destructor
base destructor
base destructor
base destructor

第一行是Base b的结果，第二行是运行构造函数时，b传递的参数副本，所以调用copy构造函数;
第三行是成员变量m_b的构造函数; 第四行是运行派生类的构造函数前先运行的基类构造函数;
第五行就是=运算符; 析构过程就简单了，不再分析



但实际中一般不这么用，Derived的成员变量会是Base* m_b，此时的调用是这样的:

1
2
3
4
5
Base *b = new Base();
Derived d(b);

delete b;
b = NULL;

结果是:

1
2
3
4
5
6
7
8
base constructor
----------------
base constructor
derived constructor
base destructor
----------------
derived destructor
base destructor

这样简单多了

const成员变量和成员函数|C++C++ 面向对象

  • 在类成员函数后面加const关键字用于声明const成员函数，它不能改变类的成员变量，也不能调用该类中普通的成员函数，最好不要涉及非const的成员变量

  • const成员变量只能通过成员列表初始化进行初始化，任何成员函数均不能改变它的值

  • 只能调用const成员函数访问const对象，那是它的唯一对外接口

  • 若不希望传入的实参对象被修改，应该将形参定义为const的指针变量或者引用

const成员函数的定义是这样形式：

1
int getConst() const;

前面不用加const了，否则会有报警：

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
class Base
{
public:
	void setM(int M)	//setter都不能加const
	{
		m = M;
	}
	int getM() const
	{
		return m;
	}
private:
	int m;
}

const Base b;
b.setM(2);

getM就是常量成员函数，如果给setM加const，那么就会报错，因为它修改m的值。定义的常对象b只能调用getM，调setM会报错。

使用const引用做函数形参类型|C++C++ 面向对象

本问题是Effective C++的条款20

提高代码效率

我们知道函数的实际参数是实参的一个副本，它是由copy构造函数产生的，但这种传const引用的方式可以提高函数调用效率，它不会涉及副本的构造函数、析构函数和copy构造函数

测试代码如下，对基类定义了copy构造函数，但派生类没有定义copy构造函数，信息中会输出this指针的地址

1
2
3
4
5
6
7
8
9
void useDerived(const Derived& obj)
{
    cout<<" use Derived"<<endl;
}

Derived f;
cout<<"-------------"<<endl;
useDerived(f);
cout<<"*************"<<endl;

运行结果：

1
2
3
4
5
6
7
Base constrct  0x75fd20
Derived construct  0x75fd20
-------------
 use Derived
*************
Derived deconstruct  0x75fd20
Base deconstrct  0x75fd20

显然只有f的构造和析构过程，对于副本，只调用了useDerived函数，没有构造和析构，也没有copy构造。

如果改用值传递的方式，结果就变成下面这样：

1
2
3
4
5
6
7
8
9
10
Base constrct  0x75fd10
Derived construct  0x75fd10
-------------
Base copy constrct  ---0x75fd40
 use Derived
Derived deconstruct  0x75fd40
Base deconstrct  0x75fd40
*************
Derived deconstruct  0x75fd10
Base deconstrct  0x75fd10

显然副本的copy构造和析构都有了，但没有副本的构造函数，效率明显降低

避免对象切割问题( object sliced )

如果上面的函数改成这样：

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
void useDerived(Base f)
{
    f.testConst();
    cout<<"value use Derived"<<endl;
}

// testConst声明为虚函数
void Base::testConst() const
{
    cout << "base test const" <<endl;
}
void Derived::testConst() const
{
    cout << "derived test const" <<endl;
}
// 调用
Derived d;
useDerived(d);

结果为：

1
2
3
4
5
6
7
8
9
10
base constructor
derived constructor
# 开始副本
base copy constructor
base test const     // 基类版本
value use Derived
base destructor

derived destructor
base destructor

此时如果传入f，会把它按Base对象处理，而丧失了派生类的特性，调用的还是基类版本的函数，这不是我们的目的。但是如果用const Base& obj，就会根据传入的类型处理，结果会调用派生类版本的函数，也就是体现了多态。 但是注意：形参为常引用时，只能调用const成员函数。

不过对于内置类型和STL的iterator，函数对象，还是用值传递的方式比较好

(一) 论文解读|激光SLAMgmapping

根据概率论知识，SLAM工作可以划分为定位和建图两部分，一般是通过观测值和里程计信息进行定位得到，然后根据位姿 以及观测信息进行建图得到m。

gmapping其实就是优化版的RBPF，先看RBPF的一般过程：

  1. 时间t的粒子群 是 从提议分布中采样获得的，提议分布往常是里程计运动模型
  2. 粒子权重 是目标分布和提议分布的比，用来描述提议分布和目标分布的差别，越接近1说明提议分布越好
  3. 重采样：只有部分粒子用于估计目标分布，重采样之后，所有粒子权重相同。 如果能直接从目标分布采样，所有粒子权重都相同，实际上只用1个粒子就行，或者说没有采样的概念了。因此，提议分布越差，粒子群的权重差别越大， (粒子归一化权重的平方和的倒数)就越大。
  4. 根据轨迹各个位姿 和各个观测 估计地图

  • 目标分布： 真实分布，如果考虑机器人的运动，那么机器人在t时刻位置的真实分布就是目标分布，但是这是不能得到的，只是理论上的分布，我们需要只能通过其他分布来得到该分布

  • 提议分布: 使用提议分布来得到目标分布的真实情况。一般的，使用里程计模型来作为提议分布。但是改进的提议分布发现激光具有单峰的特性，方差更小，更适合作为提议分布，因此考虑激光和里程计一起作为改进的提议分布

gmappping的优化

优化提议分布

不仅考虑机器人的运动，而且考虑了最近的观测，使得提议分布更接近目标分布，从而减少了所需的粒子数。之前的提议分布是 ，优化之后是

如果只用运动模型做提议分布，粒子权重计算比较简单: 。 但是里程计提供位姿的不确定度比激光大得多，导致置信区间太大，似然又太小，各个粒子的权重差别很大，实际上有意义的只有一小部分，结果就需要很多粒子。

相比之下，雷达观测的置信区间小，似然大，所需粒子就少了很多。所以可以把观测加入到提议分布优化之后，结果粒子权重公式为公式 13

因为观测似然函数的不确定形态，提议分布没有闭式解，所以方法是用采样对提议分布做估计。

减少重采样

RBPF中，粒子要覆盖里程计状态的全部空间，但只有一部分粒子是接近目标分布的，粒子权重的差别大，所以需要频繁重采样来丢弃权重小的粒子，保留权重大的粒子，同时又造成了另一个弊端：粒子退化。

选择性重采样，当 小于粒子数一半时，说明粒子分布和真实分布差距很大，某些粒子离真实值近，某些远，执行重采样。

GMapping依靠粒子的多样性， 回环时消除累积的误差。起始位置需要选择特征较为丰富的地方， 这样能在回环时提高正确粒子权重。

  1. 使用scan-matcher估计似然函数的置信区间
  2. 对置信区间采样，对采样点进行评价
  3. 对每个粒子计算，计算K个粒子的期望方差，计算时考虑了里程计运动信息，公式15和16

算法复杂度

主要影响因素是计算提议分布，更新地图，计算权重，检测是否需要重采样，重采样。 结合源码，最大的影响参数是particles, 其次是resampleThreshold, map_update_interval, throttle_scans, lskip

  1. 机器人经过未知区域时， 下降很慢
  2. 机器人经过已知区域时，每个粒子都在各自的地图中保持定位，权重都相差不大
  3. 当closing loop时， 会显著下降，有些粒子能保持定位，有些则丢失了定位，权重相差很大，此时执行重采样

缺陷

从论文和源码上看，gmapping算法有以下缺陷:

  1. scan matcher 计算的是观测似然函数的置信区间，但函数可能是多峰的，比如closing loop的时候，这就造成了问题，粒子群的权重会有很大波动， 会显著下降，当低于resample_Threshold时，就会执行重采样，然后恢复 最大值． 之所以大幅下降，就是因为观测似然出现多峰，使得提议分布与目标分布差别太大，导致粒子权重减小．因此不适用于多闭环的环境

  2. 遇到长走廊或很空旷的环境，雷达数据没有什么特征，比如测距都是最大值，造成位姿在走廊方向的严重不确定，此时里程计精度就很重要了，有利于粒子群的收敛。如果里程计精度不好，增加粒子数也可以解决这个问题。

  3. drawFromMotion函数是一个十分粗糙的运动模型，只是简单的矢量加减运算。相比于《概率机器人》中提到的速度模型和里程计模型，有很多方面都没有考虑，精度上可能有折扣。

  4. 随着场景增大所需的粒子也增加，内存和计算量都会增加，实际中建的地图不到几千平米时就会发生错误

  5. 重采样的粒子索引选取用的是轮盘赌算法，有些论文提出了一些更好的算法

  6. 机器人的轨迹估计与真实的轨迹有一定差异，轨迹出现较大畸变的地方导致了＂假墙＂．这是粒子多样性降低造成的，需要增加粒子数
    假墙.png

  7. 源码中，用户设置的miniScore参数过大时(超过170)，scan match失败，转而使用里程计进行位姿估计．但这样就更不准了，而且gmapping算法里没有提示

综合来看，Gmapping适合的环境有以下特点: 小场景、closing loop比较少、没有长走廊或很空旷的环境、雷达精度高、机器人主机配置较高。此时相比Cartographer，Gmapping不需要太多的粒子并且没有回环检测，因此计算量小于Cartographer，精度没有差太多。

参考：
各种激光slam: 开源代码的比较
2D激光SLAM算法优劣对比

机器人运动模型|ROSROS机器人

用于估计的二轮差速里程计模型

二轮差速模型的航迹推演原理图，后方两个驱动轮，前面两个万向轮

  • IMU只提供yaw
  • 根据航迹推演，两驱车的线速度是两个轮子线速度的平均值，角速度是 差/底盘长度;两个参数作为raw_vel话题发布
  • 机器人切线运动模型
    base_controller节点正确读取到底层（比如嵌入式控制板）传回的速度后进行积分，计算出机器人的估计位置和姿态，并将里程计信息和tf变换发布出去。
  • 机器人中通过运行底盘控制ROS驱动，来实现读取串口的速度反馈，利用航迹推演算法计算得到里程计并发布到/odom这个主题

之所以说这个是粗略的定位，是因为在实际情况中可能会碰到轮子打滑，地面不平整等因素的干扰，里程计的运动增量带有噪声，对速度积分进行航迹推算得到的里程计累积误差会越来越大。当然上层会通过激光信息来匹配校准。

IMU姿态的协方差矩阵代表了姿态测量的不确定度。

用于规划的速度模型

IMU在ROS中的使用 (三) 标定内外参|ROSROS机器人

IMU线加速度噪声积分后根本不能用做线速度

IMU在制造过程中，由于物理因素，导致实际的坐标轴与理想的坐标轴之间会有一定的偏差，同时三轴加速度、三轴角速度、三轴磁力计的原始值会与真实值有一个固定的偏差等。自校准就是要通过给的补偿值来减小坐标轴的偏差及原始值的固定偏差，也就是所谓的IMU内参标定，是为了获得噪声参数。

如果将IMU安装到机器人或摄像头上后，需要知道IMU与机器人或摄像头的相对位置，这个时候进行的标定就是所谓的IMU外参标定