SHENLAN_Chapter6_基于安全走廊和Bezier曲线的硬约束轨迹生成方法_安全走廊约束

原理

Bezier曲线理解的参考链接: https://www.bilibili.com/read/cv2939222/.

Bezier曲线是通过控制点（P0、P1、P2、P3…）生成的。所以控制点对Bezier曲线生成至关重要。
那么怎么把轨迹曲线多项式形式转换成Bezier形式呢？
若多项式表达式是7阶的话，那么Bezier需要有8个控制点？为什么？

M的求法？？？
Bezier曲线和多项式曲线的转换关系参考：https://blog.csdn.net/xuehuafeiwu123/article/details/54584339
$$B(t)=\sum _{i=0}^n{c}_i\frac{n!}{(n-i)!i!}(1-t{)}^{n-i}{t}^i=p_0+p_{i}t+\cdots +p_n{t}^n$$
通过M矩阵进行p和c之间的转换：
$$p=M\ast c$$
M可以通过推导得出，并且M的和多项式的阶数n唯一确定，不用自己再重新推导一遍，用下方的各阶的矩阵M即可：


Cost funciton J的推导：
由
$$J={\left[\begin{array}{c}{p}_1\\ ⋮\\ p_M\end{array}\right]}^T\left[\begin{array}{ccc}{Q}_1& 0& 0\\ 0& \ddots & 0\\ 0& 0& Q_M\end{array}\right]\left[\begin{array}{c}{p}_1\\ ⋮\\ p_M\end{array}\right]$$
和
$$\left[\begin{array}{c}{p}_1\\ ⋮\\ p_M\end{array}\right]=\left[\begin{array}{ccc}{M}_1& 0& 0\\ 0& \ddots & 0\\ 0& 0& M_M\end{array}\right]\left[\begin{array}{c}{c}_1\\ ⋮\\ c_M\end{array}\right]$$
推导出：
$$J={\left[\begin{array}{c}{c}_1\\ ⋮\\ c_M\end{array}\right]}^T{\left[\begin{array}{ccc}{M}_1& 0& 0\\ 0& \ddots & 0\\ 0& 0& M_M\end{array}\right]}^T\left[\begin{array}{ccc}{Q}_1& 0& 0\\ 0& \ddots & 0\\ 0& 0& Q_M\end{array}\right]\left[\begin{array}{ccc}{M}_1& 0& 0\\ 0& \ddots & 0\\ 0& 0& M_M\end{array}\right]\left[\begin{array}{c}{c}_1\\ ⋮\\ c_M\end{array}\right]$$
其中，
$$Q_0=M^{T}QM$$
Q的求法见我写的另一篇文章，见链接: 求解Q的方法.
通过Q’=NearestSPD(Q)；返回和Q矩阵距离最近的一个对称正定（Symmetric Positive Definite）矩阵Q’。目的是把目标函数微调为一个凸函数，保证得到的解为全局最优解。
首先我们要理解c是什么，根据Bezier曲线的定义曲线是由控制点定义而成的，并且每段轨迹的多项式系数个数和控制点个数是相同的。
即每段轨迹的$$p_0\cdots p_n多项系数和c_0\cdots c_n控制点的个数相同$$

等式约束Aeq和beq的求法

我们可以理解成c0、c1…cn是p的控制点,c’0、c’1…c’n是v的控制点,c’‘0、c’‘1…c’‘n是a的控制点,c’’‘0、c’’‘1…c’’'n是j的控制点。
Bezier曲线的各阶导数都是由各阶控制点控制的。
关于Bezier在曲线首末端点的导数性质参见链接3.bezier首尾点的导数:
bezier曲线，因此在u=0时的导数为n(P1−P0)，在u=1时的导数为n(Pn−P(n−1))，也就是说，曲线在首尾处的切线与其首个、末尾处的控制点的方向是一致的！
所以第一段Bezier曲线的起始点的各阶导数的控制点为
$$c_{start}=c_0$$
$$c_{start}^{(1)}=n(c_1-c_0)$$
$$c_{start}^{(2)}=n(n-1)(c2-c1-(c_1-c_0))=n(n-1)(c_2-2c_1+c_0)$$
$$c_{start}^{(3)}=n(n-1)(n-2)(c_3-2c_2+c_1-(c_2-2c_1+c_0))=n(n-1)(n-2)(c_3-c_2+3c_1-c_0)$$
所以：
$$\left[\begin{array}{c}{c}_{start}\\ c_{start}^{(1)}\\ c_{start}^{(2)}\\ c_{start}^{(3)}\end{array}\right]=\left[\begin{array}{c}{p}_{start}\\ 0\\ 0\\ 0\end{array}\right]=\left[\begin{array}{cccccc}1& 0& 0& 0& 0& \cdots \\ -n& n& 0& 0& 0& \cdots \\ n(n-1)& -2n(n-1)& n(n-1)& 0& 0& \cdots \\ -n(n-1)(n-2)& 3n(n-1)(n-2)& -n(n-1)(n-2)& n(n-1)(n-2)& 0& \cdots \end{array}\right]\left[\begin{array}{c}{c}_0\\ c_1\\ c_2\\ c_3\\ c_4\\ c_5\\ c_6\\ c_7\end{array}\right]$$
起始点的v,a,j=0,其中C0=point_start，由Bezier曲线的性质：Bezier曲线一定经过起始点和终止点。
和起始点类似，终止点的Aeq*c=beq方程写成如下形式：
$$c_{end}=c_7$$
end=7,由于c的最多项为7，那么列等式时向前找控制点的导数。。。？？？有啥依据吗？
$$c_{end}^{(1)}=n(c_7-c_6)$$
$$c_{end}^{(2)}=n(n-1)(c7-c6-(c_6-c_5))=n(n-1)(c_7-2c_6+c_5)$$
$$c_{end}^{(3)}=n(n-1)(n-2)(c_7-2c_6+c_5-(c_6-2c_5+c_4))=n(n-1)(n-2)(c_7-c_6+3c_5-c_4)$$
$$\left[\begin{array}{c}{c}_{end}\\ c_{end}^{(1)}\\ c_{end}^{(2)}\\ c_{end}^{(3)}\end{array}\right]=\left[\begin{array}{ccccc}0& 0& 0& \cdots & 1\\ 0& 0& \cdots & -n& n\\ 0& \cdots & n(n-1)& -2n(n-1)& n(n-1)\\ 0& \cdots -n(n-1)(n-2)& 3n(n-1)(n-2)& -n(n-1)(n-2)& n(n-1)(n-2)\end{array}\right]\left[\begin{array}{c}{c}_0\\ c_1\\ c_2\\ c_3\\ c_4\\ c_5\\ c_6\\ c_7\end{array}\right]$$
两段Bezier曲线需要连续，即：前一段曲线的末控制点p,v,a,j和后一段曲线的首控制点的p,v,a,j分别相等，所以(i代表第i段Bezier曲线)：
$$c_{7,i}=c_{0,i+1}=>c_{7,i}-c_{0,i+1}=0$$
$$c_{7,i}^{(1)}=c_{0,i+1}^{(1)}=>n(c_{7,i}-c_{6,i})-n(c_{1,i+1}-c_{0,i+1})=0$$
$$c_{7,i}^{(2)}=c_{0,i+1}^{(2)}=>n(n-1)(c_{7,i}-2c_{6,i}+c_{5,i})-n(n-1)(c_{2,i+1}-2c_{1,i+1}+c_{0,i+1})=0$$
$$c_{7,i}^{(3)}=c_{0,i+1}^{(3)}=>n(n-1)(n-2)(c_{7,i}-c_{6,i}+3c_{5,i}-c_{4,i})-n(n-1)(n-2)(c_{3,i+1}-c_{2,i+1}+3c_{1,i+1}-c_{0,i+1})=0$$
得到下表中的Aeq，beq结果：

不等式约束Aieq和bieq的求法

下图中的蓝色方框为中间安全走廊的范围，需要限制

每条Bezier轨迹有8个控制点，对于一个维度，例如x-t，每个控制点的p需要限制上限和下限，且对于优化问题，都需要转换成ax<=b这种形式，对于上图中5段Bezier轨迹，那么需要限制5x8x2=80组关于p的不等式约束。
$$\begin{gathered} c_{i,j}<=p_{max} \\ -c_{i,j}<=-p_{min} \end{gathered}$$
Bezier经过一阶求导之后最高阶为n-1了，对于七阶方程来说，Bezier一阶求导后的最高阶为6阶，即B’(t)的控制点为7个了，所以需要限制5x7x2=70组关于v的不等式约束。
$$c_{end}^{(1)}=n(c_7-c_6)<=vmax$$