自动驾驶(五十五)---------四解轨迹规划
原标题:自动驾驶(五十五)---------四解轨迹规划
原文来自:CSDN 原文链接:https://blog.csdn.net/zhouyy858/article/details/103374435
前面已经三次讲过轨迹规划,还是如隔靴搔痒,诗不着题。所以想少一些理论,多一些实践的角度写轨迹规划,就从常见的场景展开:
1. 问题引入:
已知条件 车身传感器:车速、Yawrate、方向盘转角;车身周围车辆信息;车道线信息;一个模糊的目标点。求 一条轨迹能到达目标点的位置和角度,轨迹要满足车辆运动学,舒适性,避障等要求。
怎么理解模糊的目标点?目标位置和角度不是很精确,目标状态的位置和角度随着不断地靠近,可能会不断的收敛,例如十字路口转弯,在转弯过程中摄像头看不到入口的车道线,高精地图只能给出大概的位置和角度,在这样模糊的信息下,需要先规划出一条车道线,引导车辆转弯,在过程中看到车道线,不断收敛目标点的位置和方向。
2. 解决思路:
初始化:初始轨迹规划采用选取多个关键点,利用三次样条曲线生成轨迹。
更新:车身定位采用Klaman滤波收敛,考虑车身的状态和舒适性,结合收敛的目标位置和角度,重新生成三次样条曲线。
输出:利用生成的三次样条曲线,选取一定长度的轨迹,生成轨迹多项式(三次),满足舒适性;
错误处理:一旦车身偏离轨道,或者目标的位置和状态达不到,需要一套逻辑处理,或者直接退出轨迹规划模式。
3. 三次样条差值:
在车辆控制中,要通过n个点,为了满足舒适性要求,一般是当前点角度为零作为约束,即b0=0,,最后一个点一般也有角度要求:bn = p;输出为同一坐标系下的分段三次函数。具体推导过程省略,主要讲实现思路:
1. 已知n个点:
2. 分段三次函数表示为:;
假设: ;
3. 通过对分段函数本身、一阶导、二阶导在分段点上的连续性,联立方程组,解方程组从而求出分段函数的系数:
4. 首位一般有约束,即开始点一阶导为车身yawrate下对应的横摆角,即保持当前车身的变化姿势,即使归0也可能不舒适,也可以描述为开始点的二阶导为0。
5. 末位也有类似的约束,二阶导为0。
6. 则通过二阶导可以组成以下方程组:
7. 最后通过以下关系求出其他系数:
8. 封装好的c++代码:
struct poly_coef { float c0; float c1; float c2; float c3; /* data */ }; //自然边界的三次样条曲线函数 void cubic_getval(vector<Point2f> pxy, vector<poly_coef> opcs) { int n = pxy.size(); float* ai = (float*)malloc(sizeof(float) * (n-1)); float* bi = (float*)malloc(sizeof(float) * (n-1)); float* ci = (float*)malloc(sizeof(float) * (n-1)); float* di = (float*)malloc(sizeof(float) * (n-1)); float* h = (float*)malloc(sizeof(float) * (n-1)); //x的?? /* M矩阵的系数 *[B0, C0, ... *[A1, B1, C1, ... *[0, A2, B2, C2, ... *[0, ... An-1, Bn-1] */ float *A = (float *)malloc(sizeof(float) * (n - 2)); float *B = (float *)malloc(sizeof(float) * (n - 2)); float *C = (float *)malloc(sizeof(float) * (n - 2)); float *D = (float *)malloc(sizeof(float) * (n - 2)); //等号右边的常数矩阵 float *E = (float *)malloc(sizeof(float) * (n - 2)); //M矩阵 float *M = (float *)malloc(sizeof(float) * (n)); //包含端点的M矩阵 //计算x的步长 for (int i = 0; i < n - 1; i++){ h[i] = pxy[i + 1].x - pxy[i].x; } //指定系数 for (int i = 0; i < n - 3; i++){ A[i] = h[i]; //忽略A[0] B[i] = 2 * (h[i] + h[i + 1]); C[i] = h[i + 1]; //忽略C(n-1) } //指定常数D for (int i = 0; i < n - 3; i++) { D[i] = 6 * ((pxy[i + 2].y - pxy[i + 1].y) / h[i + 1] - (pxy[i + 1].y - pxy[i].y) / h[i]); } //求解三对角矩阵,结果赋值给E TDMA(E, n - 3, A, B, C, D); M[0] = 0; //自然边界的首端M为0 M[n - 1] = 0; //自然边界的末端M为0 for (int i = 1; i < n - 1; i++) { M[i] = E[i - 1]; //其它的M值 } //?算三次?条曲?的系数 for (int i = 0; i < n - 1; i++) { opcs[i].c0 = map[n + i]; opcs[i].c1 = (map[n + i + 1] - map[n + i]) / h[i] - (2 * h[i] * M[i] + h[i] * M[i + 1]) / 6; opcs[i].c2 = M[i] / 2; opcs[i].c3 = (M[i + 1] - M[i]) / (6 * h[i]); } free(h); free(A); free(B); free(C); free(D); free(E); free(M); free(ai); free(bi); free(ci); free(di); } void TDMA(real_T *X, const int_T n, real_T *A, real_T *B, real_T *C, real_T *D){ float tmp; //上三角矩阵 C[0] = C[0] / B[0]; D[0] = D[0] / B[0]; for (int i = 1; i < n; i++) { tmp = (B[i] - A[i] * C[i - 1]); C[i] = C[i] / tmp; D[i] = (D[i] - A[i] * D[i - 1]) / tmp; } //直接求出X的最后一个值 X[n - 1] = D[n - 1]; //逆向迭代, 求出X for (int i = n - 2; i >= 0; i--){ X[i] = D[i] - C[i] * X[i + 1]; } }
4. 生成轨迹
由于上一步选点,生成分段三次曲线,能不能直接把0点对应的三次曲线作为轨迹线输出呢?我认为不行,首先,生成的分段函数长度是由分段点决定的,不一定满足要求;再次,两帧之间是强调拟合,没有考虑其舒适性和车辆运动学模型。因此我们需要重新拟合当前场景下合适的轨迹线。
传统方法多项式拟合是最小二乘法拟合,前面有专门的的文章介绍带约束的最小二乘法,这里我也附上一般的最小二乘法opencv代码:
bool curveFitting(CvSeq* inDataSet, float curveParam[4]) { if(!inDataSet) return false; int dataSetSize = inDataSet->total; //系数矩阵存储位置 CvMat* cMatrix = cvCreateMat(4, 4, CV_32FC1); cvZero(cMatrix); //常量系数矩阵 CvMat* cstMatrix = cvCreateMat(4, 1, CV_32FC1); cvZero(cstMatrix); //(0, 0) *((float*)CV_MAT_ELEM_PTR(*cMatrix, 0, 0)) = static_cast<float>(dataSetSize); CvPoint2D32f* sample = 0; float x = x2 = x3 = x4 = x5 = x6 = y = 0.f; for(int i = 0; i < dataSetSize; ++i){ sample = (CvPoint2D32f*)cvGetSeqElem(inDataSet, i); x = sample->x; x2 = x * x; x3 = x2 * x; x4 = x3 * x; x5 = x4 * x; x6 = x5 * x; *((float*)CV_MAT_ELEM_PTR(*cMatrix, 0, 1)) += x; *((float*)CV_MAT_ELEM_PTR(*cMatrix, 0, 2)) += x2; *((float*)CV_MAT_ELEM_PTR(*cMatrix, 0, 3)) += x3; *((float*)CV_MAT_ELEM_PTR(*cMatrix, 1, 3)) += x4; *((float*)CV_MAT_ELEM_PTR(*cMatrix, 2, 3)) += x5; *((float*)CV_MAT_ELEM_PTR(*cMatrix, 3, 3)) += x6; y = sample->y; *((float*)CV_MAT_ELEM_PTR(*cstMatrix, 0, 0)) += y; *((float*)CV_MAT_ELEM_PTR(*cstMatrix, 1, 0)) += y * x; *((float*)CV_MAT_ELEM_PTR(*cstMatrix, 2, 0)) += y * x2; *((float*)CV_MAT_ELEM_PTR(*cstMatrix, 3, 0)) += y * x3; } *((float*)CV_MAT_ELEM_PTR(*cMatrix, 1, 0)) = *((float*)CV_MAT_ELEM_PTR(*cMatrix, 0, 1)); *((float*)CV_MAT_ELEM_PTR(*cMatrix, 1, 1)) = *((float*)CV_MAT_ELEM_PTR(*cMatrix, 0, 2)); *((float*)CV_MAT_ELEM_PTR(*cMatrix, 1, 2)) = *((float*)CV_MAT_ELEM_PTR(*cMatrix, 0, 3)); *((float*)CV_MAT_ELEM_PTR(*cMatrix, 2, 0)) = *((float*)CV_MAT_ELEM_PTR(*cMatrix, 0, 2)); *((float*)CV_MAT_ELEM_PTR(*cMatrix, 2, 1)) = *((float*)CV_MAT_ELEM_PTR(*cMatrix, 1, 2)); *((float*)CV_MAT_ELEM_PTR(*cMatrix, 2, 2)) = *((float*)CV_MAT_ELEM_PTR(*cMatrix, 1, 3)); *((float*)CV_MAT_ELEM_PTR(*cMatrix, 3, 0)) = *((float*)CV_MAT_ELEM_PTR(*cMatrix, 0, 3)); *((float*)CV_MAT_ELEM_PTR(*cMatrix, 3, 1)) = *((float*)CV_MAT_ELEM_PTR(*cMatrix, 1, 3)); *((float*)CV_MAT_ELEM_PTR(*cMatrix, 3, 2)) = *((float*)CV_MAT_ELEM_PTR(*cMatrix, 2, 3)); CvMat* pcMatrix = cvCreateMat(4, 4, CV_32FC1); cvZero(pcMatrix); CvMat result = cvMat(4, 1, CV_32FC1, curveParam); cvZero(&result); cvInvert(cMatrix, pcMatrix, CV_LU); cvMatMul(pcMatrix, cstMatrix, &result); cvReleaseMat(&pcMatrix); cvReleaseMat(&cstMatrix); return true; }
同样的多项式拟合没有考虑帧间的舒适性,也没有考虑车辆运动学,所以我们需要新的思路解决问题。这里我介绍一种物理含义解决以上问题,如图:
图示:L1为前面几个点拟合的直线,C0为车中心到L1的距离,L2为最后几个点拟合的直线,L3为L2的法线,L4为L1的法线,计算L3和L4的交点。
对于以上点得到的轨迹多项式 y=a+bx+cx^2+dx^3,其中c直接为C0,b是L1的角度,c是 L3和L4的圆的半径倒数,d是c的变换率,一般在中间在选一组点,计算c的变化率。
5. 优化更新
上面生成的轨迹,没有考虑舒适性和车辆运动学模型。在保证舒适性的方法中,窗口滤波是最直接和简单的方法,但是有明显的确定,产生延迟。这里介绍一种新的思路,供大家思考。
我们把上一帧计算的轨迹离散成一串点。
通过本次车身姿态和位置变化,仿射变换计算出上一帧轨迹离散点。
对新观察的离散点,和上一帧推算的离散点进行Kalman滤波。
对滤波结果安装第4步重新计算新的轨迹。
6. 错误检测
对输出的轨迹进行检测,对计算的C0、C1、C2、C3进行数值约束。对不满足要求的结果进行退出操作
免责声明:本文来自互联网新闻客户端自媒体,不代表本网的观点和立场。
合作及投稿邮箱:E-mail:editor@tusaishared.com
热门资源
Python 爬虫(二)...
所谓爬虫就是模拟客户端发送网络请求,获取网络响...
TensorFlow从1到2...
原文第四篇中,我们介绍了官方的入门案例MNIST,功...
TensorFlow从1到2...
“回归”这个词,既是Regression算法的名称,也代表...
TensorFlow2.0(10...
前面的博客中我们说过,在加载数据和预处理数据时...
机器学习中的熵、...
熵 (entropy) 这一词最初来源于热力学。1948年,克...
智能在线
400-630-6780
聆听.建议反馈
E-mail: support@tusaishared.com