为什么需要平滑轨迹？

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

目标：

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

要素：

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

注意:

• 平滑直线段的角。
• 首选恒速运动,零加速度
• 需要特殊处理短段

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

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

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

1.1 问题和方案

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

论文的框架：

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

3. 多项式路径

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

3.5 保证轨迹是无碰撞的

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

a图的直线部分本来是无碰撞的，经过优化反而碰撞了，此时就需要增加b中的两个中点

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

原理补充

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

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

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

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

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

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

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

缺点

规划最小snap时，占用的CPU也比较大

矩阵求导求$d_p$时，矩阵$R_{pp}^{-1}$可以尝试用LLT分解等方法绕开。 最后求向量P 时的$A^{-1}$也可以尝试绕开。但是$CA^{-T}QA^{-1}C^{T}$中的$A^{-1}$必须计算，这会非常耗时间。因此要看近年的论文。

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

二次规划的一般形式可以表示如下

其中$x$是n维的输入变量，$f$是n维向量，$H$是$n \times n$的对称矩阵，$A$是$m \times n$的矩阵，$b$是m维向量。

$A_{eq}$是矩阵，$b_{eq}, l_b, u_b$是向量。

二次规划的要求就是在约束条件下找到一个n维的向量X，使得$f(X)$的值最小。

二次规划问题的求解速度和怎么构造优化问题有一定的联系, 要想提升整体的求解速度和求解成功率, 应该尽量少的使用等式约束。

另一种形式如下，是OsqpEigen所用的形式，以后主要用这种

$H$ 就是Hessian矩阵，为方阵，大小和变量的个数相同。 $f$ 就是Gradient，如果没有线性部分，就设置为零向量。$A$ 就是约束矩阵，矩阵尺寸为约束个数 x 变量个数。 OSQP把等式和不等式约束放在一起了， 这里有时就需要自己推导一下约束矩阵了。如果是等式约束，就把上下限写成一个数就行

上下限向量的大小是约束的大小

• 如果H是半正定矩阵，那么f(X)是一个凸函数。

• 如果H是正定矩阵，那么全局最小值就是唯一的。

• 如果H=0，那么f(x)只剩线性部分，二次规划问题就变成线性规划问题。

• 如果至少有一个向量x满足约束而且f(x)在可行域有下限，二次规划问题就有一个全局最小值X。

对于维数较低情况，把二次型表达式转为矩阵形式比较简单，矩阵H的元素是二次型中的矩阵元素大小的两倍。 设矩阵H第i行第j列的元素大小为H(i,j)，二次项 $x_i x_j$ 的系数为a(i,j)，则

也就是平方项的系数对应矩阵H的对角元素，可以直接写出。

如果表达式太复杂，无法直接手写，使用Matlab求海森矩阵获得

1
2
3
4
5
6
7

syms x1 x2;
f = 0.5*x1.^2+x2.^2-x1*x2;
H = hessian(f, [x1, x2]);
% 转换为double类型
H = double(H);
% 输出矩阵
disp(h) 

c++

C++的求解使用osqp-eigen库，使用ospq感觉有点繁琐

安装 osqp(必需) 和 osqp-eigen

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17

git clone --recursive https://github.com/osqp/osqp
cd osqp
mkdir build
cd build
cmake -G "Unix Makefiles" ..
cmake --build .
cmake --build . --target install

cd ../..
git clone https://github.com/robotology/osqp-eigen.git
cd osqp-eigen
mkdir build
cd build
安装到 /usr/local/lib/
cmake -DCMAKE_INSTALL_PREFIX:PATH=/usr/local ../
make
sudo make install

CMake配置

1
2
3
4
5

cmake_minimum_required(VERSION 3.0)
project(myproject)
find_package(OsqpEigen REQUIRED)
add_executable(example example.cpp)
target_link_libraries(example OsqpEigen::OsqpEigen)

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

#include "OsqpEigen/OsqpEigen.h"
#include <Eigen/Dense>
#include <iostream>

int main()
{
    // allocate QP problem matrices and vectores
    Eigen::SparseMatrix<double> hessian(2, 2);      //P: n*n正定矩阵,必须为稀疏矩阵SparseMatrix
    Eigen::VectorXd gradient(2);                    //Q: n*1向量
    Eigen::SparseMatrix<double> linearMatrix(2, 2); //A: m*n 矩阵,必须为稀疏矩阵SparseMatrix
    Eigen::VectorXd lowerBound(2);                  //L: m*1下限向量
    Eigen::VectorXd upperBound(2);                  //U: m*1上限向量
    // 注意稀疏矩阵的初始化方式,无法使用<<初始化
    hessian.insert(0, 0) = 2.0; 
    hessian.insert(1, 1) = 2.0;
    std::cout << "hessian:" << std::endl << hessian << std::endl;
    gradient << -2, -2;
    // 注意稀疏矩阵的初始化方式,无法使用<<初始化
    linearMatrix.insert(0, 0) = 1.0; 
    linearMatrix.insert(1, 1) = 1.0;
    std::cout << "linearMatrix:" << std::endl << linearMatrix << std::endl;
    lowerBound << 1, 1;
    upperBound << 1.5, 1.5;
    // instantiate the solver
    OsqpEigen::Solver solver;
    // settings
    solver.settings()->setVerbosity(false);
    solver.settings()->setWarmStart(true);
    // set the initial data of the QP solver
    // NumberOfVariables 与 NumberOfConstraints 跟 Hessian 矩阵的维数对应上
    solver.data()->setNumberOfVariables(2);   //变量数n
    solver.data()->setNumberOfConstraints(2); //约束数m
    if (!solver.data()->setHessianMatrix(hessian) )
        return 1;
    if (!solver.data()->setGradient(gradient))   // 二次规划的线性表达式，有时为空，此时用零向量
        return 2;
    if (!solver.data()->setLinearConstraintsMatrix(A))  // 约束中的矩阵A
        return 3;
    if (!solver.data()->setLowerBound(lowerBound))
        return 4;
    if (!solver.data()->setUpperBound(upperBound))
        return 5;
    // instantiate the solver
    if (!solver.initSolver())
        return 6;

    Eigen::VectorXd QPSolution;
    // solve the QP problem
    if (!solver.solve())
    {
        return 7;
    }
    QPSolution = solver.getSolution();
    std::cout << "QPSolution" << std::endl
              << QPSolution << std::endl; //输出为m*1的向量
    return 0;
}

结果为 1.0003, 1.0003

Matlab

x = quadprog(H,f,A,b,Aeq,beq,lb,ub) 在满足 lb ≤ x ≤ ub 的限制条件下求解上述问题。输入 lb 和 ub 是由双精度值组成的向量，这些限制适用于每个 x 分量。如果不存在等式约束，请设置 Aeq = [] 和 beq = []

1
2
3
4
5
6
7
8

H = [2 0; 0 2];
g = [-2; -2]

A = [1 0; 0 1];
b = [1.5; 1.5];
lb = [1; 1];

[x,fval,exitflag,output,lambda] = quadprog(H,g,A,b,[],[],lb);

结果直接为 1.0000, 1.0000. 比osqp-eigen更精确。

x是解。 fval是解处的目标函数值

python

OSQP的python的使用很简单，如果只是求解OSQP，不用C++，可以优先用python。 安装步骤

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16

import osqp
import numpy as np
from scipy import sparse

# Define problem data
P = sparse.csc_matrix([[4, 1], [1, 2]])
q = np.array([1, 1])
A = sparse.csc_matrix([[1, 1], [1, 0], [0, 1]])
l = np.array([1, 0, 0])
u = np.array([1, 0.7, 0.7])
# Create an OSQP object
prob = osqp.OSQP()
# Setup workspace and change alpha parameter
prob.setup(P, q, A, l, u, alpha=1.0)
# Solve problem
res = prob.solve()

参考:
Matlab中的quadprog函数
OSQP库计算标准二次规划
Matlab计算二次规划
利用OSQP库计算标准二次规划(QP)问题的实例
如何使用OSQP-Eigen

同一个数据集，最简单的比较不同算法结果的方式是计算所建地图和真值地图的差别。但真值地图通常难以获得，需要有高精度传感器，比如 FARO Vantage Laser Tracker

找出最好的地图就是看哪个地图的噪音少，墙最准确，artifacts(假障碍)最少。

最常见和准确的评价地图的方法就是计算地图和真值地图的差别。常见的计算尺度是K近邻的归一化距离。真值地图难以获得时，可以比较估计的轨迹和真值轨迹的差距，衡量尺度是均方根差(RMSE)。其他的衡量参数还有轨迹的全局和局部误差，收敛速度，对噪音的健壮性。

评价SLAM算法质量的标准

1. 占据和空闲栅格的比例，检查是否有模糊的墙。模糊的障碍越多，地图质量越差。

2. 地图中角落的数量，低质量的地图的角落更多。真实的角落当然不能缺少，多余的角落必是错误的：artifacts, 轨迹匹配错误导致的双墙现象, 不准确的圆弧区域。

3. 封闭区域的数量。

对比的算法有gmapping, cartographer, tinySLAM, vinySLAM. 所用数据集是MIT stata center. 这个数据集提供了真值轨迹，所以可以用RMSE评价算法质量。

测试方案包括SLAM算法、数据集、iteration次数，测试是在远程server上运行的。

看完感觉论文讲的不太实用，而且定量的分析比较费时，实际中恐怕用不上，暂时用于定性判断地图质量的标准：

1. 模糊化的部分要尽量少
2. 假墙, artifact应当不存在
3. 不得有重影，区域形状不能有明显偏差

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

#include "ros/ros.h"
#include <tf/transform_listener.h>
#include <tf/tf.h>

int main( int argc, char* argv[] )
{
    ros::init(argc, argv, "test_tf");
    ros::NodeHandle nh;

    tf::TransformListener listener;
    geometry_msgs::PoseStamped in_pose;
    in_pose.header.frame_id = "parent";
    in_pose.header.stamp = ros::Time(0);
    in_pose.pose.position.x = 1;
    in_pose.pose.position.y = 2;
    in_pose.pose.position.z = 0;

    in_pose.pose.orientation.x = 0;
    in_pose.pose.orientation.y = 0;
    in_pose.pose.orientation.z = 0.344215153;
    in_pose.pose.orientation.w = 0.93889079678;
    ros::Rate r(10);
    while(nh.ok() )
    {
        out_pose.header.frame_id = "child";
        // listener->waitForTransform("child", "parent", ros::Time(0),  ros::Duration(1) );
        listener->transformPose("child", in_pose, out_pose);
        r.sleep();
    }
}

如果没有while循环，只是一个transformPose，运行就会报错，找不到child，即使tf关系是正常的。如果还是报错，需要加上waitForTransform一行

问题

tf使用transformPose报错: Exception in transformPose: Quaternion malformed, magnitude: 0 should be 1.0

源码在此，原因是四元数平方和偏离1太多

1
2
3
4
5
6
7
8
9

inline void assertQuaternionValid(const geometry_msgs::Quaternion & q)
{
   if(std::fabs(q.x*q.x + q.y*q.y + q.z*q.z + q.w*q.w - 1) > 0.01)
   {
     std::stringstream ss;
     ss << "Quaternion malformed, magnitude: " << q.x*q.x + q.y*q.y + q.z*q.z + q.w*q.w << " should be 1.0" <<std::endl;
     throw tf::InvalidArgument(ss.str());
   }
};

python中使用transformPose

1
2
3
4
5
6
7
8
9
10
11
12
13
14

import rospy
import tf
from tf import TransformListener

quaternion = tf.transformations.quaternion_from_euler(0, 0, self.ang, axes='sxyz')
self.temp_pose.pose = Pose(Point(self.goal_x, self.goal_y, 0.000), Quaternion(quaternion[0],quaternion[1],quaternion[2],quaternion[3]))
self.temp_pose.header.frame_id = 'base_link'
# 解决时间戳的问题
self.temp_pose.header.stamp = rospy.Time(0)
try:
    self.tf_listener_.waitForTransform("odom", "base_link", rospy.Time(0), rospy.Duration(2.0) )
    self.goal = self.tf_listener_.transformPose('odom', self.temp_pose)
except (tf.LookupException, tf.ConnectivityException):
    rospywarn('tranform frame failed !')

开始总是出现target_frame does not exist的错误，但是用tf_echo明明可以看到两个坐标系，于是加上了waitForTransform，没有问题了。但是又出现了那个时间戳范围不足的问题，还是Time(0)解决

参考：
tf listener cant look up transform frame

单一多项式插值会有两个问题：

1. 随着多项式的阶数越来越高，计算量也越来越大。
2. 随着多项式的阶数越来越高，插值精度并不会越来越高，恰恰相反，函数曲线会出现剧烈的振荡，即龙格现象。

既能穿过所有已知点又能避免龙格现象（剧烈的震荡），就要用分段函数插值。就是把所有的已知数据分割成若干段，每段都对应一个插值函数，最终得到一个插值函数序列。

为了保证分段函数之间彼此衔接，每个分段函数都采用高次函数形式来构造（三次样条差值 就是用x的三次方形式构造）这就保证了多个函数之间的衔接光滑。不能用过高阶的函数，否则抖动太剧烈。

三次样条插值就是把已知数据分割成若干段，每段构造一个三次函数，并且保证分段函数的衔接处具有0阶连续，一阶导数连续，二阶导数连续的性质（也就是光滑衔接）。

一阶连续意味着曲线y=S(x)没有急转弯，没有特别剧烈的跳变。二阶连续意味着每个点的曲率半径有定义。

给定n+1个数据点，共有n个区间，推导如下

自由边界：首尾两端没有受到任何让它们弯曲的力。让端点的斜率自由的在某一位置保持平衡，使得曲线的摇摆最小。
固定边界：首尾两端点的微分值是被指定的。用外力使柔软而有弹性的曲线经过数据点，并在端点处使其具有固定斜率。
非节点边界：指定样条曲线的三次微分匹配

文章QP方法解基于五次多项式形式的cost function minimization问题不是分段的多项式函数，问题相当简单。

参考：
原理
如何推导

安装依赖库

1
2

sudo apt-get install gnuplot
sudo apt-get install gnuplot-x11  

这样还没完，还需下载gnuplot，解压之后，输入 make 进行编译。如果在自己的项目需要使用gnuplot，将gnuplot_i.hpp复制到对应项目，在程序中加入 #include "gnuplot_i.hpp"， 无需链接动态库， 这也是我使用它的主要原因，懒得改CMake

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

Gnuplot g1;

void plot(const vector<double>& Xs, const vector<double>& Ys, const string& comment, const string& style)
{
    g1.set_grid();
    g1.set_pointsize(3);
    g1.set_style(style).plot_xy(Xs, Ys, comment);
}

void wait_for_key()
{
    cout << "Press Enter to continue ..." <<endl;
    cin.clear();
    cin.ignore(cin.rdbuf()->in_avail() );
    cin.get();
    return;
}

            /*以下为main函数部分*/
std::vector<double> X_t, Y_t;
/*对所有 vector 插入数据*/

g1.set_grid();
g1.set_xlabel("X");  //设置x轴说明
g1.set_ylabel("Y");  //设置y轴说明
// 图例文字
g1.set_style("lines").plot_xy(X_t, Y_t, "left");
plot(X_t, Y_t, "left", "points lc rgb 'red' pt 7");

wait_for_key();

图样(style): gnuplot 描绘数据数据图形是以读入档案中的坐标值后，以图样绘上。gnuplot可提供9种图样，我常用的是:

1. lines: 将相邻的点以线条连接
2. points: 将每一点以一符号绘上
3. linespoints: 同时具有lines 及 points 的功能。

其他还包括

• linestyle 连线风格（包括linetype，linewidth等）
• linetype 连线种类
• linewidth 连线粗细
• linecolor 连线颜色
• pointtype 点的种类
• pointsize 点的大小

参考:C++ 调用Gnuplot实现图形绘制的过程