If you wish for peace, prepare for war

路径平滑的概述 2022-08-01|路径规划路径平滑

为什么需要平滑轨迹？

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

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

目标：

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

要素：

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

注意:

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

论文 Polynomial Trajectory Planning for Aggressive Quadrotor Flight in Dense Indoor Environments 2022-07-26|路径规划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表示了这个过程

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也比较大

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

Matlab判断矩阵的正定性 2022-07-20|Matlab

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

% 判断矩阵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

二次型问题的求解 C++和Matlab 2022-07-20|SLAM工具

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

$min\frac{1}{2}x^THx\ +\ f^Tx$ $约束\left\{ \begin{array}{**lr**} Ax \le b \\ A_{eq}x = b_{eq} \\ l_b \le x \le u_b \end{array} \right.$

其中 $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求海森矩阵获得

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

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配置

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

#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 = []

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。安装步骤

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

纯跟踪的小问题累计 2022-07-16|路径规划纯跟踪算法

Welcome to my blog, enter password to read.

使用GDB检查move_base进程终止的问题 2022-07-14|那些卡我很久的bug|疑难bug

abstract Welcome to my blog, enter password to read.

tf(二) transformPose 2022-07-08|ROSROS程序和源码分析

#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太多

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

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

三次样条曲线 2022-07-08|路径规划路径平滑

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

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

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

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

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

三次样条曲线不算基于优化的算法，没有目标函数，不过有约束，而且边界约束有多种情况。

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

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

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

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

参考：
原理
 如何推导

gnuplot安装使用 2022-07-07|第三方库

安装依赖库

1 2	sudo apt-get install gnuplot sudo apt-get install gnuplot-x11

输入 gnuplot，出现命令行，输入 plot sin(x)，如果出现正弦曲线说明安装成功。

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

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种图样，我常用的是:

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

其他还包括

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

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

laser_line_extraction 2022-07-03|激光SLAM雷达

论文

论文A comparison of line extraction algorithms using 2D range data for indoor mobile robotics对比了6种提取雷达数据线条算法的效果。结论如下：

Split-and-Merge 和 Incremental由于处理速度和准确性而表现最好。对于实时的应用，Split-and-Merge是最佳选择，因为处理速度很快。它也是地图定位问题的第一选择，此时FalsePos不太重要。但是，算法实现的细节对应用是很重要的。

对于laser_line_extraction对应的03年论文，这篇论文在介绍Split-and-Merge时没有提到，只在Related Work里提了一下。但是laser_line_extraction的介绍里声称是使用了Split-and-Merge算法，还有待观察。

以及2017年的论文 A line segment extraction algorithm using laser data based on seeded region growing，效果在Line Segment Extraction

原理

对雷达扫描的结果，如果形状接近直线，就进行连线。

先对离群点进行过滤，再执行split-and-merge算法决定哪些点属于线，然后使用weighted line fitting算法找到最合适的线和相应的协方差矩阵。

使用roslaunch laser_line_extraction example.launch，修改参数以符合自己的需求。

参数

bearing_std_dev (default: 0.001): The standard deviation of bearing uncertainty in the laser scans (rad).
min_split_dist (default: 0.05): When performing “split” step of split and merge, a split between two points results when the two points are at least this far apart (m) 线段split的阈值，过大时很多线段被合并成一条，过小时，出现很多碎短的线段
outlier_dist (default: 0.05): Points who are at least this distance from all their neighbours are considered outliers (m).
range_std_dev (default: 0.02): The standard deviation of range uncertainty in the laser scans (m).

float32 radius
float32 angle
float32[4] covariance
float32[2] start
float32[2] end

start和end分别是这个线段的起点坐标和终点坐标。
angle: 原点到直线的垂线的角度，角度以机器人为坐标系计算，范围是-PI~PI
radius: 原点到直线的距离

这两个参数完全能够根据start和end点计算出来。不理解这两个参数存在的意义。

covariance是2X2的矩阵，是radius和angle的协方差

line_extractor

line_extractor发布:

camera_line_markers     [visualization_msgs/Marker]
camera_line_segments    [laser_line_extraction/LineSegmentList]
line_markers            [visualization_msgs/Marker]
line_segments           [laser_line_extraction/LineSegmentList]

taimi/line_center_point       [nav_msgs/Odometry]
taimi/line_wall_position      [geometry_msgs/PoseStamped]
taimi/waste_aruco_position    [geometry_msgs/PoseStamped]

camera1/scan [sensor_msgs/LaserScan]
shelf_lifter/cmd [std_msgs/String]
sick_tim551_scan [sensor_msgs/LaserScan]
tf [tf2_msgs/TFMessage]
tf_static

参考：室内移动机器人二维激光数据线特征提取算法的总结与开源算法分享