二分查找法|算法推导

不断分段找中点，判断中点是否是要寻找的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();
}
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弧度

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矩阵，可以推断多元函数的极值情况了。结论如下：

  • 假设是正定矩阵，则临界点处是一个局部极小值
  • 假设是负定矩阵，则临界点处是一个局部极大值
  • 假设是不定矩阵，则临界点处不是极值
路径平滑的概述|路径规划路径平滑

为什么需要平滑轨迹？

A star、RRT star等算法，找到了一段轨迹路径，也就是一系列的点，但没有指明点的连接方式，可能是平滑的曲线，也可能是折线。这些算法是不考虑机器人运动学约束的，因此轨迹上会出现明显的不光滑点，可以想象，机器人不可能在某一点出现运动突变，如果是万向轮的机器人要想按照这个轨迹走，它必须每走完一段路，就要停下来，然后旋转一个角度对着路径然后再加速往前走。这样很明显浪费很多时间和效率。如果是阿克曼的机器人那么它就无法按照这个路径进行运动。

虽然有的局部路径规划算法也可以对路径做一定平滑，但是先把全局做平滑相当于提供一个好的初值

目标：

  • 动力学状态不能突变。满足微分约束
  • 机器人不能在转弯时停下来，要连续流畅地走。拐弯时的速度曲线，越平滑越好
  • 节约能源。轨迹生成的目标函数可以是最小关于状态和输入的能量泛函

要素：

  • 边界条件：起点、目标的位姿
  • 中间条件：中间路径点的位姿。通过路径规划(A*， RRT*等)可以找到
  • 平滑度评价函数
  • 通常转化为输入变化率的最小化问题

注意:

  • 平滑直线段的角。
  • 首选恒速运动,零加速度
  • 需要特殊处理短段
论文 Polynomial Trajectory Planning for Aggressive Quadrotor Flight in Dense Indoor Environments|路径规划Minimum Snap

论文实际是关于无人机的，这里只记录有参考价值的部分。

论文提出了一种方法，即构建一个无约束的二次规划问题，对分段的多项式路径进行联合优化，这个二次规划对高次多项式和大量分段数值稳定。另一项contribution是自动选择每段的时间。使用多项式路径，不必进行大量的采样和机器人在高维状态空间的模拟。本算法不能保证渐进收敛到全局最优。

算法将搜索和优化结合起来，比纯粹基于搜索的规划算法在计算上的表现好得多。对每段时间的分配使planner能自动适应变化范围很大的size scales，只使用一个用户设置的参数

1.1 问题和方案

使用RRT*算法找到无碰撞的路径，(开始运动后可以有动态障碍)开始只考虑运动学，不考虑动力学。路径被裁剪为极少的路径点，对路径点的选取是根据line-of-sight技巧。使用分段的多项式路径插入到各路径点，生成光滑的minimum-snap路径。二次规划将路径导数(Path Derivatives)的代价函数最小化，路径的终点可以是固定值，也可以是free.(我的理解是类似三次样条曲线的自由边界) 多项式的导数连续性保证了移动的平滑性。

论文的框架：

  • 先讨论无人机的模型
  • 提出二次规划问题的闭式解

3. 多项式路径

要求每段的时间(vector of times)是固定的，也要预先确定。大致可以根据车的期望平均速度确定，但这样不会生成最低代价的方案。因此我们将遍历地refine时间向量。

3.5 保证轨迹是无碰撞的

如果轨迹中某一段和障碍相交了，就在这段轨迹中添加中点，将其一分为二。这个中点是无碰撞的，因为它取决于搜索算法返回的分段路径。我的理解是全局路径返回的一定是无碰撞的，在它上面取一点，肯定还是无碰撞。然后多项式路径添加中点后再次二次规划，而且这个再规划过程可能会重复，直到多项式路径无碰撞(递归取中点)。图4表示了这个过程
image.png
a图的直线部分本来是无碰撞的，经过优化反而碰撞了，此时就需要增加b中的两个中点

在稠密环境中，轨迹需要增加很多额外路径点以处理碰撞情况，因此二次规划会进行很多次，增加了计算量。但是根据我们在室内环境的经验，增加的路径点不会超过原有路径点的一般，所以计算量增加很少。

原理补充

设置一个目标代价函数，目的在于找到这几个变量在取一定值时使得我们设计的代价函数取得最小值。更通俗的说：就是这几个变量取一定值的时候可以使机器人能量耗费最少，或者机器人运动的转动更少等等。这种思想不仅适用于minimum snap算法。

机器人在起点和终点的速度或者加速度，我们都是可以设置的。机器人中间航迹点的位置也是需要事先给定的，但是机器人中间点的速度和加速度 Jerk等信息，我们没法去约束它，因为很难去确定机器人到达某一个点的速度或者加速度。这正是我们研究算法的目的。如果中间点的速度加速度事先设置，机器人会忽快忽慢或者速度不变，很难实现，即使能实现，也十分不美观。

因此我们认为中间点的速度、加速度等信息是一个可以自由变化的状态，那么对于这种中间状态不确定的问题，我们可以构建一个优化问题，使得中间状态取得某一个值时，我们制定的代价函数达到最小值，因此通过优化代价函数得到一个最优的轨迹。根据上面的分析，对于中间点的速度和加速度，或者更高阶的Jerk甚至Snap等，我们不对其进行微分限制，只进行连续性约束，然后让优化器给它们分配最优的状态。

minimum snap 可以理解为最小化加加加速度。
Jerk: 所受力的变化率。 (如每秒增加一牛顿)
snap: 所受力的变化率的变化。(如前一秒增加一牛顿，接下来一秒增加两N，第二秒受力与最初相比增加了三个N)
最小snap，就让jerk变化比较小。如果snap为0，就代表每秒加速度稳定增加 (受力稳定增加)。

实际工程中，对于高于加速度之后的导数，都将其设置为0，也就是最小化Snap，只需给出位置、速度、加速度的设置值，而Jerk就默认为0。

时间对于最终生成轨迹的好坏有很大影响。如果把每段时间也加入到优化的代价函数中，这样固然是好，但是势必会增加优化的时间，并且无法再闭式的求解这个问题。

梯形时间分配：先假设机器人以最大的加速度加速到最大速度，然后运行一段时间，再以最大的减速度到终点，到达是速度为0。

缺点

A星和最小snap路径
A星和最小snap算法所花费时间
规划最小snap时，占用的CPU也比较大

矩阵求导求时，矩阵可以尝试用LLT分解等方法绕开。 最后求向量P 时的也可以尝试绕开。但是中的必须计算，这会非常耗时间。因此要看近年的论文。

Matlab判断矩阵的正定性|Matlab

Matlab判断矩阵的正定性，实际是判断特征值是否全为正

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
% 判断矩阵m是正定、半正定还是负定
m = [2 -1; -1 2]; 

if issymmetric(m) % 检查矩阵是否对称
    % disp('矩阵对称');
    d = eig(m); % 计算矩阵特征值
    if all(d > 0)
        disp('矩阵正定');
    elseif all(d >= 0)
        disp('矩阵半正定');
    else
        disp('矩阵负定');
    end
else
    disp('矩阵不对称');
end