cartographer_grpc_server的源码|激光SLAMCartographer原理和配置

node_grpc_main.cc中的关键一句:

1
2
auto map_builder = absl::make_unique<::cartographer::cloud::MapBuilderStub>(
    FLAGS_server_address, FLAGS_client_id);

接着看map_builder_stub.cc的构造函数

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
MapBuilderStub::MapBuilderStub(const std::string& server_address,
                               const std::string& client_id)
    : client_channel_(::grpc::CreateChannel(
          server_address, ::grpc::InsecureChannelCredentials())),
      pose_graph_stub_(make_unique<PoseGraphStub>(client_channel_, client_id)),
      client_id_(client_id)
{
  LOG(INFO) << "Connecting to SLAM process at " << server_address
            << " with client_id " << client_id;
  std::chrono::system_clock::time_point deadline(
      std::chrono::system_clock::now() +
      std::chrono::seconds(kChannelTimeoutSeconds));
  if (!client_channel_->WaitForConnected(deadline)) {
    LOG(FATAL) << "Failed to connect to " << server_address;
  }
}

二分查找法|算法推导

不断分段找中点，判断中点是否是要寻找的num

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
// 获取 num 在 vector 中的 index
int findIndex(const vector<int>& vector, int num);

int main(int argc, char *argv[])
{
    vector<int> v;
    for(int i=0; i<9; i++)
    {
        v.emplace_back();
        auto& num = v.back();
        num = i*2;
        cout << "  v.back: "<< v.back();
    }
    int id = findIndex(v, 6) ;
    if(id < 0 )
        cout << "no such number !" <<endl;
    else
        cout << "index of number is : "<< id <<endl;
    return 0;
}

int findIndex(const vector<int>& vector, int num)
{
    Q_ASSERT(vector.size() >0 );
    int size = vector.size();
    if(vector.at(0) ==num )    return 0;
    if(size==1 && vector.at(0) !=num )    return -1;
    if(num == vector.back() )   return size-1;

    int begin, mid, end;
    begin =0; end = vector.size()-1;
    mid = (begin + end) / 2;
    int count = 0;
    while(num != vector.at(mid) )
    {
        count++;
        if(count > log2(size) )
        {
            return -1;
            break;
        }
        // 比较mid和num, 按情况只取一段进行递归比较，这样才降低复杂度
        if( num > vector.at(mid) )
        {
            begin = mid; end = end;
            mid = (begin + end) / 2;
        }
        else
        {
            begin = begin; end = mid;
            mid = (begin + end) / 2;
        }
    }
    return mid;
}
Apollo的平滑路径模块|路径规划路径平滑

无人车的轨迹优化，都是通过建立优化问题，是非线性规划问题，通过IPopt来求解，

  1. 这个过程有个重要的前提，那就是f(x)是连续可导。如果f(x)这个函数不可导，或者不可微呢？比如说，在SLAM中需要处理姿态估计的问题。而姿态是通过旋转矩阵和平移向量表示的，这显然是不连续的。但是，由于旋转矩阵是一个李群，那么我们可以将其映射到其李代数上。而且，李代数是可以求导的。所以我们就还可以利用非线性优化的方法对姿态进行估计。

  2. 上面的解法求得的只是局部最优解，不一定是全局最优解。因为它只看到下一时刻，在邻域内的最小值，很容易陷入到局部最优，或者说求出的最优值是跟初始位置相关的。即使初始值在同一个位置，也很有可能落入到两个不同的局部最优值，也就是说这个解是很不稳定的。如何解决这个问题，至今仍然存在很大的挑战。

Apollo高精地图给出的道路中心线的平滑性往往都不能满足规划模块的需求，因此规划中是不能拿来直接用的，需要对其进行平滑操作。Apollo目前有三种平滑算法如下，默认为离散点平滑。

离散点平滑三种求解方式，分别是利用不同求解器。如果考虑参考线的曲率约束，其优化问题是非线性的，可以使用ipopt非线性求解器求解（内点法），也可以将曲率约束线性化后使用osqp求解器来用SQP方法求解；如果不考虑曲率约束，则直接用osqp求解二次规划问题。目前Apollo 6.0中默认的参数为不考虑曲率约束-

获取 anchor_s

设置中间点 anchor points

从原始的reference_line来选取中间点，即根据reference_line的anchor_s值进行采样，采样的间隔为0.25m。采样完毕后，就能得到一系列的ref_points，而每个anchor_point就包含了一个ref_point以及横纵向的裕度。

横纵向裕度的默认参数为：

longitudinal_boundary_bound : 2.0

max_lateral_boundary_bound : 0.5

min_lateral_boundary_bound : 0.1

然后再根据自车宽度，车道边界类型（是否为curb）等再对横向裕度进行收缩。AnchorPoint结构中的enforced变量表示该点是否为强约束，在参考线平滑中只有首尾两点为强约束。

计算完anchor points后，将其赋值到smoother对象中。

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
// 省略不重要部分
class ReferencePoint : public hdmap::MapPathPoint
{
     public:
      common::PathPoint ToPathPoint(double s) const;

      double kappa() const;
      double dkappa() const;

      static void RemoveDuplicates(std::vector<ReferencePoint>* points);

     private:
      double kappa_ = 0.0;
      double dkappa_ = 0.0;
};

struct LaneWaypoint {
  LaneWaypoint() = default;
  LaneWaypoint(LaneInfoConstPtr lane, const double s)
      : lane(CHECK_NOTNULL(lane)), s(s) {}
  LaneWaypoint(LaneInfoConstPtr lane, const double s, const double l)
      : lane(CHECK_NOTNULL(lane)), s(s), l(l) {}

  LaneInfoConstPtr lane = nullptr;
  double s = 0.0;
  double l = 0.0;

  std::string DebugString() const;
};
1
2
3
4
5
6
7
8
// lane_info_ptr->accumulate_s(); 的来源
for (size_t i = 0; i + 1 < points_.size(); ++i)
{
    segments_.emplace_back(points_[i], points_[i + 1]);
    accumulated_s_.push_back(s);
    unit_directions_.push_back(segments_.back().unit_direction());
    s += segments_.back().length();
}
路径平滑常用代码|路径规划路径平滑

平均采样，获得anchor的index

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
/**
 * 平均分割区间 [start, end] 为num段，num+1 个点，存入sliced
 * start and end 为sliced的首尾元素
 */
template <typename T>
void uniform_slice(const T start, const T end, uint32_t num,
                   std::vector<T>* sliced)
{
  if (!sliced || num == 0)
        return;
  const T delta = (end - start) / num;
  sliced->resize(num + 1);
  T  s = start;
  for (uint32_t i = 0; i < num; ++i, s += delta)
  {
      sliced->at(i) = s;
  }
  sliced->at(num) = end;
}

补充

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
vector<Point2D> samplePoints(const vector<Point2D>& input_points, int interval)
{
    vector<Point2D> anchor_points;

    int sampling_num = std::max(2, static_cast<int>(input_points.size() / interval + 0.5)  );
    
    vector<double> anchor_id;
    Point2D temp_point;
    uniform_slice(0, input_points.size(), sampling_num - 1, &anchor_id);
    
    for(int i=0; i< anchor_id.size(); i++)
    {
        // cout << anchor_id.at(i) << "   ";
        temp_point = input_points.at(anchor_id.at(i) );
        // cout << "anchor X: " << temp_point.first <<endl;
        // cout << "anchor Y: " << temp_point.second <<endl;
        anchor_points.push_back(temp_point);
    }
    return anchor_points;
}

1
2
3
4
5
6
7
8
9
10
11
12
13
14
/**
 * Check if two points u and v are the same point on XY dimension.
 * @param u one point that has member function x() and y().
 * @param v one point that has member function x() and y().
 * @return sqrt((u.x-v.x)^2 + (u.y-v.y)^2) < epsilon, i.e., the Euclid distance
 * on XY dimension.
 */
template <typename U, typename V>
bool SamePointXY(const U& u, const V& v)
{
    static constexpr double kMathEpsilonSqr = 1e-8 * 1e-8;
    return (u.x() - v.x()) * (u.x() - v.x()) < kMathEpsilonSqr &&
         (u.y() - v.y()) * (u.y() - v.y()) < kMathEpsilonSqr;
}
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
PathPoint GetWeightedAverageOfTwoPathPoints(const PathPoint& p1,
                                            const PathPoint& p2,
                                            const double w1, const double w2)
{
    PathPoint p;
    p.set_x(p1.x() * w1 + p2.x() * w2);
    p.set_y(p1.y() * w1 + p2.y() * w2);
    p.set_z(p1.z() * w1 + p2.z() * w2);
    p.set_theta(p1.theta() * w1 + p2.theta() * w2);
    p.set_kappa(p1.kappa() * w1 + p2.kappa() * w2);
    p.set_dkappa(p1.dkappa() * w1 + p2.dkappa() * w2);
    p.set_ddkappa(p1.ddkappa() * w1 + p2.ddkappa() * w2);
    p.set_s(p1.s() * w1 + p2.s() * w2);
    return p;
}
1
2
3
4
5
6
7
8
9
10
11
12
// Test whether two float or double numbers are equal.
template <typename T>
typename std::enable_if<!std::numeric_limits<T>::is_integer, bool>::type
IsFloatEqual(T x, T y, int ulp = 2)
{
  // the machine epsilon has to be scaled to the magnitude of the values used
  // and multiplied by the desired precision in ULPs (units in the last place)
  return std::fabs(x - y) <
             std::numeric_limits<T>::epsilon() * std::fabs(x + y) * ulp
         // unless the result is subnormal
         || std::fabs(x - y) < std::numeric_limits<T>::min();
}
Apollo的离散点平滑算法|路径规划路径平滑

reference_line是在笛卡尔坐标系下进行优化，主要考虑的在不偏离原始道路中心线太多的情况下（即约束条件）保证相邻点之间的连续性(即代价函数)。

Apollo的二次规划问题都使用OSQP优化

曲率约束的假设：

a. 三点共圆
b. 向量，即前后两段的长度近似相等
c. 向量 和向量 夹角很小

曲率约束用到了最小转弯半径，而差速机器人的这一项是0，所以只能用于car-like机器人。

如果只取cost2和cost3，二次规划问题的约束其实就是限定 的上下界，仍然可以优化出结果。但是看不出有什么意义，只是在每个路径点位置误差内，让路径更紧凑了而已。

试验theta和sin(theta)， tan(theta)|数学基础

试验theta在多小的时候，约等于sin(theta)和tan(theta)

Matlab结果如下



经过换算，暂时取16°或者说0.28弧度

判断点是否在多边形内|路径规划其他

这个问题的来源是如何判断机器人在某个多边形的区域内。

使用面积法，这是显而易见的，对于不规则的多边形面积计算，参考这里，需要知道每个顶点的坐标。连续取两个顶点和点O构成三角形，求出所有三角形的面积和。

如果知道了每个点的坐标，三角形面积可以用海伦公式计算，所以需要求出所有的边长，这样计算量就大了。


机器人是在运动的，如果一直判断是否在某多边形内，计算量太大，先判断机器人坐标到多边形重心的距离是否小于一定阈值，若小于再进行判断。

对于矩形重心，求四个点坐标均值即可，对于不规则多边形，重心求解比较复杂，参考求简单多边形的重心可视化实现

其他还有射线法、求内角法，前者的实现比较复杂，后者用反三角函数，也会增加计算量。

IPOPT的使用|SLAM工具

IPOPT是一种常用的非线性优化求解器，使用内点法进行求解。对于复杂问题，需要借助自动微分工具，帮助求解梯度、雅各比矩阵、Hessian矩阵，如ADOL-C，CppAD

安装

参考非线性优化求解器IPOPT 和 ubuntu 环境下 IPOPT 安装与使用

我安装完后，部分文件和官方说明的不同，目录coin在我的电脑上是coin-or，sudo vim /usr/include/cppad/ipopt/solve_callback.hpp，修改如下

1
2
# include <coin-or/IpIpoptApplication.hpp>
# include <coin-or/IpTNLP.hpp>

配置

CMake配置

1
2
3
4
5
6
7
8
9
message(status "IPOPT_CFLAGS: " ${IPOPT_CFLAGS})
message(status "IPOPT_CFLAGS_OTHER: " ${IPOPT_CFLAGS_OTHER})
set(CMAKE_CXX_FLAGS "-DHAVE_CSTDDEF -DHAVE_MPI_INITIALIZED")

include_directories(/usr/local/include)
link_directories(/usr/local/lib)

add_executable(solver example.cpp)
target_link_libraries(solver ipopt)

举例

状态变量 $ x = [x_1,x_2,x_3,x_4]^\mathrm{T}$

目标函数

约束条件:

起始点

优化结果：

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
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
#include <cppad/ipopt/solve.hpp>
#include <iostream>
//#include <mpi.h>
namespace
{
    using CppAD::AD;
    class FG_eval {
    public:
        typedef CPPAD_TESTVECTOR( AD<double> ) ADvector;
        void operator()(ADvector& fg, const ADvector& x)
        {     assert( fg.size() == 3 );
            assert( x.size()  == 4 );

            // Fortran style indexing
            AD<double> x1 = x[0];
            AD<double> x2 = x[1];
            AD<double> x3 = x[2];
            AD<double> x4 = x[3];
            // f(x)
            fg[0] = x1 * x4 * (x1 + x2 + x3) + x3;
            // g_1 (x)
            fg[1] = x1 * x2 * x3 * x4;
            // g_2 (x)
            fg[2] = x1 * x1 + x2 * x2 + x3 * x3 + x4 * x4;
            return;
        }
    };
}

bool get_started(void)
{     
    bool ok = true;
    size_t i;
    typedef CPPAD_TESTVECTOR( double ) Dvector;
    // number of independent variables (domain dimension for f and g)
    size_t nx = 4;
    // number of constraints (range dimension for g)
    size_t ng = 2;
    // x[i] 设置第i个变量的初始迭代值
    Dvector xi(nx);
    xi[0] = 1.0;
    xi[1] = 1.0;
    xi[2] = 1.0;
    xi[3] = 1.0;

    // x_l[i]设置xi的下界值, x_u[i]设置xi的上界值
    Dvector xl(nx), xu(nx);
    for(i = 0; i < nx; i++)
    {     
        xl[i] = 1.0;
        xu[i] = 5.0;
    }
    // g_l[i]设置约束i的下界值, g_u[i]设置约束i的上界值
    Dvector gl(ng), gu(ng);
    gl[0] = 25.0;     gu[0] = 1.0e19;
    gl[1] = 40.0;     gu[1] = 40.0;

    // object that computes objective and constraints
    FG_eval fg_eval;

    // options
    std::string options;
    // turn off any printing
    options += "Integer print_level  0\n";
    options += "String  sb           yes\n";
    // maximum number of iterations
    options += "Integer max_iter     100\n";
    // approximate accuracy in first order necessary conditions;
    // see Mathematical Programming, Volume 106, Number 1,
    // Pages 25-57, Equation (6)
    options += "Numeric tol          1e-6\n";
    // derivative testing
    options += "String  derivative_test            second-order\n";
    // maximum amount of random pertubation; e.g.,
    // when evaluation finite diff
    options += "Numeric point_perturbation_radius  0.\n";

    // place to return solution
    CppAD::ipopt::solve_result<Dvector> solution;

    // solve the problem
    CppAD::ipopt::solve<Dvector, FG_eval>(
            options, xi, xl, xu, gl, gu, fg_eval, solution
    );

    // Check some of the solution values
    ok &= solution.status == CppAD::ipopt::solve_result<Dvector>::success;
    double rel_tol = 1e-6;  // relative tolerance
    double abs_tol = 1e-6;  // absolute tolerance
    std::cout << "solution x: "<< std::endl;
    for(i = 0; i < nx; i++)
    {     
        std::cout << solution.x[i] << std::endl;
    }
    return ok;
}

int main(int argc, const char** argv) {
    //MPI_Init();
    get_started();
    return 0;
}
minimum snap的闭式解推导|路径规划Minimum Snap

对于QP问题，可以通过数学的带入方法将约束条件转换到代价函数中，那么就变成了无约束的优化问题，就可以进行闭式求解。闭式求解的好处是求解速度更快，数值稳定性更高。

多项式的系数有实际的物理意义，所以在优化的时候可以会出现一些特别小的值，此时可能就会计算精度的影响，导致数值不稳定。因此Bry的论文提出了将系数通过一个映射矩阵转换成轨迹端点的导数，也就是说我们不再优化轨迹的系数，而是对轨迹在端点处的位置、速度、加速度、Jerk等进行优化，由于这些量都是有实际物理含义，不会出现太离谱的数值，所以在一定程度上是比较稳定的。

以最小化Jerk为例，那么根据前面的结论，我们需要构建次数为5次的多项式，那么它将有6个未知的系数，我们需要提供。

置换矩阵就是重新排列后的单位矩阵。实际上，最简单的置换矩阵P就是单位矩阵I本身。对一个矩阵进行行交换，需要通过置换矩阵完成。

用分块矩阵表示J时，需要注意J是标量，这样才能得到中间两项相等，高斯牛顿法中的推导也用到标量的技巧。

非线性优化|算法推导

非线性优化问题：优化函数或约束条件至少有一个是非线性函数的最优化问题。

主要有三类：

  1. 无约束优化问题
  2. 含等式约束的优化问题
  3. 含不等式约束的优化问题

针对上述三类优化问题主要有三种不同的处理策略，对于无约束的优化问题，可直接对其求导，并使其为0，这样便能得到最终的最优解。一般有梯度法、牛顿法、拟牛顿法；对于含等式约束的优化问题，主要通过拉格朗日乘数法、消元法转换成为无约束优化问题求解；对于含有不等式约束的优化问题，主要通过KKT条件( Karush-Kuhn-Tucker Condition )将其转化成无约束优化问题求解。

最小二乘问题是无约束的优化问题。

海森矩阵和局部极值

对于多元函数，通过Hessian矩阵的正定性判断，如果Hessian矩阵是半正定矩阵，那就是凸函数。如果优化函数是凸函数，那么局部最小就是全局最小，不会陷入局部最优里。

基于Hessian矩阵，可以推断多元函数的极值情况了。结论如下：

  • 假设是正定矩阵，则临界点处是一个局部极小值
  • 假设是负定矩阵，则临界点处是一个局部极大值
  • 假设是不定矩阵，则临界点处不是极值