最优轨迹生成（三）—— 无约束BIVP轨迹优化

本系列文章是学习深蓝学院-移动机器人运动规划课程第五章最优轨迹生成过程中所记录的笔记，本系列文章共包含四篇文章，依次介绍了微分平坦特性、无约束BVP轨迹优化、无约束BIVP轨迹优、带约束轨迹优化等内容

本系列文章链接如下：

最优轨迹生成（一）—— 微分平坦

最优轨迹生成（二）—— 无约束BVP轨迹优化

最优轨迹生成（三）—— 无约束BIVP轨迹优化

最优轨迹生成（四）—— 带约束轨迹优化

三、无约束BIVP轨迹优化

在这里插入图片描述

如果用BVP方法来对如下所示的折线路径进行平滑时，需要对每段折线解一个BVP，且需要指定每段折线起始和终末状态，如果指定的状态中的速度过大会不可行，所以BVP的一个缺陷是需要找到合适的指定状态，那么我们能不能仅对状态中的位置进行指定，让其他状态量，比如速度、加速度等自己去进行优化呢？

也就是，对于下面的路径，我们仅给定起始和终末状态（位置、速度、加速度、jerk等）以及中间经过状态点的位置（不对速度、加速度、jerk等其他状态量进行指定），即要求平滑后的路径要经过这些指定的位置点，但这些这些位置点处的速度、加速度、jerk等状态量通过算法优化自行得到，这样也会使得轨迹更加顺滑，这就是边界中间值问题（BIVP）。

在这里插入图片描述

BIVP的解具有超出输入或所优化阶数的连续性，比如当s=3时，状态为位置、速度、加速度、输入为jerk，则优化的目标函数为 $\min_{z(t)}\int_{t_0}^{t_{M}}[p^{\left(3\right)}]^2\mathrm{d}t,$ ，即最小化jerk，最优性条件表明最优解是5次多项式，BIVP的解可以进一步保证snap是连续的。当s=4时，是最小化snap，但BIVP的解可以进一步保证Pop是连续的。

下图给出了s=3时的例子，状态p、v、a是连续的，最小化的目标量jerk也是连续的、更高阶的snap是分段的，但其在中间状态点处（黄色的小球处）也是连续的

在这里插入图片描述

所以，我们可以直接去施加这些轨迹上的连续性条件，得到一个关于多项式系数的等式Mc=b，只需要求一个M的逆就可以得到我们所需要的多项式的系数c，不需要去做优化，也不用去求 $\min_{z(t)}\int_{t_0}^{t_M}v(t)^{\mathrm{T}}\mathbf{W}v(t)\mathrm{d}t,$ 这样一个问题

那么如何构建出上述的Mc=b关系式呢？

首先我们知道最优解一定是2s-1的多项式构成的样条，我们可以把每段多项式都先写出来，当s=3时，下式中N=2s-1=5

$f(t)=\begin{cases}f_1(t)\dot{=}\sum_{i=0}^Np_{1,i}t^i&\quad T_0\le t\le T_1\\f_2(t)\dot{=}\sum_{i=0}^Np_{2,i}t^i&\quad T_1\le t\le T_2\\\vdots&\quad\vdots\\f_M(t)\dot{=}\sum_{i=0}^Np_{M,i}t^i&\quad T_{M-1}\le t\le T_M&\end{cases}$

接下来将给定的信息，以约束的形式写出来,比如将给定的起始状态和终末状态，分别写成第一段和最后一段的等式约束，如下所示：

$\left\{\begin{matrix}{f_{j}^{(k)}(T_{j-1})}&{=x_{0,j}^{(k)}}\\{f_{j}^{(k)}(T_{j})}&{=x_{T,j}^{(k)}}\\\end{matrix}\right.$

中间状态点的位置信息也是给定的，也可以由等式约束的形式写出，此外，由前面的介绍可知相邻两段多项式要经过相同的状态点(位置、速度、加速度、jerk、snap均连续，也就是5个等式)

$f_{j}^{(k)}(T_{j})=f_{j+1}^{(k)}(T_{j})$

通过这些条件就可以得到Mc=b关系式

在这里插入图片描述

我们还需要为每段多项式轨迹分配时间，有两种不同的时间轴给定方法，第一种方法是每段多项式轨迹都独立计时，每段多项式轨迹的起点时间记为0，末端时间记为 $T_i$ ，如下面的第一幅坐标轴所示。另一种方法是记录距离第一段轨迹开始处的时间差，从第一段轨迹的开始处计时为0，每段多项式轨迹的末端时间记为 $T_i$ ，如下面的第二幅坐标轴所示。

从数值稳定性上来看，上面的第一种方法更好一些

在这里插入图片描述

通过上面的介绍，我们可以把BIVP问题，根据最优条件，即给定状态信息，写出每一段多项式系数的方程组Mc=b，其中M矩阵是带状的稀疏矩阵，可以调用稀疏求解器，比如带状的PLU器，来把每段多项式系数构成的矩阵c在线性时间内求解出来，从而得到每段多项式的表达式。

在这里插入图片描述

那么这些中间的位置点如何确定呢？

我们可以使用RRT*等全局规划算法来找到一条全局路径，在这个路径上取一些关键的点，来作为中间位置点，再使用上面介绍的方法生成轨迹。关键点的提取可以采用Douglas-Peukcer等算法。

在这里插入图片描述

道格拉斯普克算法（Douglas-Peukcer）算法是一种简化线状要素的经典算法。其基本思想是对每一条曲线的首末点虚连一条直线，求所有点与直线的距离,并找出最大距离值dmax，用dmax与限差D相比。若dmax<D，这条曲线上的中间点全部舍去；若dmax ≥D，保留dmax对应的坐标点,并以该点为界，把曲线分为两部分，对这两部分重复使用该方法。

算法的详细步骤如下:

(1) 在曲线首尾两点间虚连一条直线,求出其余各点到该直线的距离，如下图1。

(2) 选其最大者与阈值相比较,若大于阈值,则离该直线距离最大的点保留,否则将直线两端点间各点全部舍去，如下图2，第4点保留。

(3) 依据所保留的点,将已知曲线分成两部分处理,重复第1、2步操作，迭代操作，即仍选距离最大者与阈值比较,依次取舍,直到无点可舍去，最后得到满足给定精度限差的曲线点坐标，如图3、图4依次保留第6点、第7点，舍去其他点,即完成线的化简。

在这里插入图片描述

DP算法的实例程序

void BuildTree(DPNode *&root, vector<pcl::PointXYZ> points, pcl::PointXYZ headpoint, pcl::PointXYZ endpoint, double thres_ds)
{
	arrayoperation ArrExample;
	//创建一个新的根节点
	root = new DPNode;
	root->points = points;
	root->HeadPoint = headpoint;
	root->EndPoint = endpoint;
 
	if (points.size() <= 2)//点数少于2个的，不再进行划分
	{
		
		root->Left_node = NULL;
		root->Right_node = NULL;
		root->NodeType = false;//不能再划分
	}
	else
	{
		vector<double> disvec;//计算每个点到首尾两点构成直线的距离
		for (int i = 0; i < points.size(); i++)
		{
			double tempds = Point2Dline(points[i], headpoint, endpoint);
			disvec.push_back(tempds);
		}
		double maxds = ArrExample.getMax_vector(disvec);
		double maxindex = ArrExample.GetIndexOfMax(disvec);//若整个点数为10个，那么maxindex一定是介于 2到9之间，因为不可能取首尾两个点，首尾点到直线的距离为0
 
		if (maxds < thres_ds)//小于阈值的，不再分割
		{
			root->Left_node = NULL;
			root->Right_node = NULL;
			root->NodeType = false;//不能再划分
		}
		else
		{
			root->NodeType = true;//可以继续划分
 
			//将点划分成2部分，左边与右边
			vector<pcl::PointXYZ> Leftpointsvec, Rightpointsvec;
			for (int i = 0; i < points.size(); i++)
			{
				if (i <= maxindex)
				{
					Leftpointsvec.push_back(points[i]);//左边树包含的点
				}
			}
 
			for (int i = 0; i < points.size(); i++)
			{
				if (i >= maxindex)
				{
					Rightpointsvec.push_back(points[i]);//右边树包含的点
				}
			}
			//左边子树的头部点与尾部点
			pcl::PointXYZ left_headpoint = headpoint;
			pcl::PointXYZ left_endpoint = points[maxindex];
 
			//右边子树的头部点与尾部点
			pcl::PointXYZ right_headpoint = points[maxindex];
			pcl::PointXYZ right_endpoint = endpoint;
 
			//创建左、右树
			root->Right_node = new DPNode();
			BuildTree(root->Left_node, Leftpointsvec, left_headpoint, left_endpoint, thres_ds);
			BuildTree(root->Right_node, Rightpointsvec, right_headpoint, right_endpoint, thres_ds);
		}
	}
}