自动驾驶(七)---------初探轨迹规划
原标题:自动驾驶(七)---------初探轨迹规划
原文来自:CSDN 原文链接:https://blog.csdn.net/zhouyy858/article/details/96478599
自动驾驶中轨迹规划是最常见的问题,本文分以下几个场景设计轨迹规划:
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*(matrixK1matrixK2); matrixY = yPts' - matrixV1*(matrixK1matrixC); matrixA2 = (matrixX'*matrixX)matrixX'*matrixY; matrixA1 = matrixK1matrixC - (matrixK1matrixK2)*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. 左右两端点处特性(自然边界,固定边界,非节点边界)
根据定点,求出每段样条曲线方程中的系数,即可得到每段曲线的具体表达式。
将步长 带入样条曲线的条件:
c. 将bi, ci, di带入 (i = 0, 1, …, n-2)可得:
由i的取值范围可知,共有n-1个公式, 但却有n+1个未知量m 。要想求解该方程组,还需另外两个式子。所以需要对两端点x0和xn的微分加些限制。 选择不是唯一的,3种比较常用的限制如下:
免责声明:本文来自互联网新闻客户端自媒体,不代表本网的观点和立场。
合作及投稿邮箱:E-mail:editor@tusaishared.com
热门资源
Python 爬虫(二)...
所谓爬虫就是模拟客户端发送网络请求,获取网络响...
TensorFlow从1到2...
原文第四篇中,我们介绍了官方的入门案例MNIST,功...
TensorFlow从1到2...
“回归”这个词,既是Regression算法的名称,也代表...
机器学习中的熵、...
熵 (entropy) 这一词最初来源于热力学。1948年,克...
TensorFlow2.0(10...
前面的博客中我们说过,在加载数据和预处理数据时...
智能在线
400-630-6780
聆听.建议反馈
E-mail: support@tusaishared.com