最优轨迹生成（三）—— 无约束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{array}{llc}{f}_1(t)\dot{=}{\sum }_{i=0}^N{p}_{1,i}{t}^i& T_0\le t\le T_1& \\ f_2(t)\dot{=}{\sum }_{i=0}^N{p}_{2,i}{t}^i& T_1\le t\le T_2& \\ ⋮& ⋮& \\ f_M(t)\dot{=}{\sum }_{i=0}^N{p}_{M,i}{t}^i& T_{M-1}\le t\le T_M& \end{array}$$

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

   $$\{\begin{array}{cc}{f}_j^{(k)}(T_{j-1})& =x_{0,j}^{(k)}\\ f_j^{(k)}(T_j)& =x_{T,j}^{(k)}\end{array}$$

   中间状态点的位置信息也是给定的，也可以由等式约束的形式写出，此外，由前面的介绍可知相邻两段多项式要经过相同的状态点(位置、速度、加速度、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);
		}
	}
}

---

   但前面介绍的通过Mc=b方法解得的多项式轨迹只能保证在中间位置点处是不碰撞的，无法保证整个轨迹是不与障碍物相交的，轨迹可能会与障碍物相交，如下图所示：

   一种解决方法是，首先保证全局路径规划算法找到的初始路径是无碰撞的，一但生成的多项式轨迹与障碍物相交了，则可以在发生碰撞的位置附近再插入新的中间位置点，来使生成的多项式轨迹更加贴合最初的全局路径，如下图所示：

   通过上面介绍的RRT* +BIVP的方案，我们可以把RRT在低维空间找到的可行路径，拓展到高维的空间，而且比Kinodynamic RRT*算法更高效可靠。

   上述方法也存在一些缺陷，比如在障碍物比较多的时候，可能需要加入很多中间位置点，轨迹要很贴合RRT*找到的原始路径才能保证安全性，无人机的飞行可能不顺滑。

---

---

   参考资料：

   1、深蓝学院-移动机器人运动规划

   2、道格拉斯普克算法（简化线段点）

---