自动驾驶（七）---------初探轨迹规划

自动驾驶中轨迹规划是最常见的问题，本文分以下几个场景设计轨迹规划：

1. 带约束的多项式拟合算法。2. 贝赛尔曲线拟合。 3. 三次样条差值。

1. 带约束的多项式拟合算法

    自动驾驶中经常遇到一类问题，要把一条轨迹线（离散点），拟合成一条多项式表达形式：

    首先看不带约束的多项式拟合（最小二乘法）：

                     误差函数：

      为了求最小误差，对每个系数求其偏导，且偏导为0对应为最小值：

                                   

       联立方程简化如下：

                                      

         即X*A=Y，opencv有一个专门用于求解线性方程的函数，即cv::solve()

int cv::solve(
	cv::InputArray X, // 左边矩阵X, nxn
	cv::InputArray Y, // 右边矩阵Y,nx1
	cv::OutputArray A, // 结果，系数矩阵A,nx1
	int method = cv::DECOMP_LU // 估算方法
);

         这个方法最大的问题是不能直接用于控制，为了求解最优解，忽略了车辆的平顺性和运动学模型，为此我们可以加上约束来求解新的参数，保证车辆平稳性和舒适性。

         在x=n的位置一阶导为0：在矩阵方程下加一行：[0+1+2*x_n+3*x_n*x_n+ ...+k*pow(x_n,k-1) ]*A= 0;

         在x=n的位置二阶导为0：在矩阵方程下加一行：[0+0+2+3*2*x_n+ ...+k*（k-1）*pow(x_n,k-2)] *A= 0

         同上采用opencv 解方程，这里附上3阶最小二乘法的matlab代码：
         

function c = pathSmoothing(xPts, yPts, phi0, phiD, curvO):

    matrixC = [yPts(1), yPts(end), phi0, phiD, curvO]';

    matrixK1 = [1, xPts(1),   xPts(1)^2,   xPts(1)^3,     xPts(1)^4; ...
                1, xPts(end), xPts(end)^2, xPts(end)^3,   xPts(end)^4; ...
                0,         1,   2*xPts(1), 3*xPts(1)^2,   4*xPts(1)^3; ...
                0,         1, 2*xPts(end), 3*xPts(end)^2, 4*xPts(end)^3; ...
                0,         0,           2,     6*xPts(1),  12*xPts(1)^2];

    matrixK2 = [xPts(1)^5, xPts(1)^6; ... 
                xPts(end)^5, xPts(end)^6; ...
                5*xPts(1)^4, 6*xPts(1)^5; ...
                5*xPts(end)^4, 6*xPts(end)^5; ...
                20*xPts(1)^3, 30*xPts(1)^4];

    [sizem,sizen] = size(xPts);
    for i=1:1:sizen
        oneMatrix(i) = 1;
    end

    matrixV1 = [oneMatrix;xPts;xPts.^2;xPts.^3;xPts.^4]';
    matrixV2 = [xPts.^5;xPts.^6]';
    matrixX = matrixV2 - matrixV1*(matrixK1\matrixK2);
    matrixY = yPts' - matrixV1*(matrixK1\matrixC);

    matrixA2 = (matrixX'*matrixX)\matrixX'*matrixY;
    matrixA1 = matrixK1\matrixC - (matrixK1\matrixK2)*matrixA2;
    c = [matrixA1;matrixA2]
end

2. 贝赛尔曲线拟合

          贝赛尔曲线应用场景是，已知几个目标关键控制点，求一条光滑的曲线，使得曲线尽量靠近或者经过这些控制点。

         贝塞尔曲线直接看动态图，就能明白（我见过有人看一眼就明白）：

        二阶：三阶：

        四阶：        五阶：

        C++四阶轨迹代码：

void curve4(vector<CvPoint> &p,  
			double x1, double y1,   //Anchor1  
			double x2, double y2,   //Control1  
			double x3, double y3,   //Control2  
			double x4, double y4){  //Anchor2  
  
	CvPoint tmp0(x1,y1);
	p.push_back(tmp0); 
	double dx1 = x2 - x1;  double dy1 = y2 - y1;  
	double dx2 = x3 - x2;  double dy2 = y3 - y2;  
	double dx3 = x4 - x3;  double dy3 = y4 - y3;  

	double subdiv_step  = 1.0 / (NUM_STEPS + 1);  
	double subdiv_step2 = subdiv_step*subdiv_step;  
	double subdiv_step3 = subdiv_step*subdiv_step*subdiv_step;  

	double pre1 = 3.0 * subdiv_step;  
	double pre2 = 3.0 * subdiv_step2;  
	double pre4 = 6.0 * subdiv_step2;  
	double pre5 = 6.0 * subdiv_step3;  
	double tmp1x = x1 - x2 * 2.0 + x3;  
	double tmp1y = y1 - y2 * 2.0 + y3;  
	double tmp2x = (x2 - x3)*3.0 - x1 + x4;  
	double tmp2y = (y2 - y3)*3.0 - y1 + y4;  
	double fx = x1, fy = y1;  
	double dfx = (x2 - x1)*pre1 + tmp1x*pre2 + tmp2x*subdiv_step3;  
	double dfy = (y2 - y1)*pre1 + tmp1y*pre2 + tmp2y*subdiv_step3;  
	double ddfx = tmp1x*pre4 + tmp2x*pre5;  
	double ddfy = tmp1y*pre4 + tmp2y*pre5;  
	double dddfx = tmp2x*pre5;  double dddfy = tmp2y*pre5;  

	int step = NUM_STEPS;  
	while(step--) {  
		fx   += dfx;  fy   += dfy;  
		dfx  += ddfx;  	dfy  += ddfy;  
		ddfx += dddfx; 	ddfy += dddfy;  
		CvPoint tmp1(fx,fy);
		p.push_back(tmp1);  
	}  
	CvPoint tmp2(x4,y4);
	p.push_back(tmp2); 
}

3. 三次样条插值。

          三次样条插值对应的场景为：已知几个控制点，要求计算一条平滑的轨迹，经过这些控制点，与贝赛尔曲线不同的是：

         贝赛尔曲线不要求经过控制点，控制点只是约束作用，

         三次样条插值具体做法，  已知:

              a. n+1个数据点[xi, yi], i = 0, 1, …, n

              b. 每一分段都是三次多项式函数曲线

              c. 节点达到二阶连续

              d. 左右两端点处特性（自然边界，固定边界，非节点边界）

            根据定点，求出每段样条曲线方程中的系数，即可得到每段曲线的具体表达式。

            插值和连续性：, 其中 i = 0, 1, …, n-1

            微分连续性： , 其中 i = 0, 1, …, n-2

             样条曲线的微分式：

 

              将步长 带入样条曲线的条件：

                       a. 由 (i = 0, 1, …, n-1)推出

                       b. 由 (i = 0, 1, …, n-1)推出                     

                       c. 由  (i = 0, 1, …, n-2)推出  

                               

                             由此可得：

                       d. 由  (i = 0, 1, …, n-2)推出

              设 ，则 

                      a.  可写为： ，推出

                      b. 将ci, di带入  可得：

                      c. 将bi, ci, di带入 (i = 0, 1, …, n-2)可得：

                             

             由i的取值范围可知，共有n-1个公式， 但却有n+1个未知量m 。要想求解该方程组，还需另外两个式子。所以需要对两端点x0和xn的微分加些限制。 选择不是唯一的，3种比较常用的限制如下：