自动驾驶(五十五)---------四解轨迹规划

2024-02-25 18:50

本文主要是介绍自动驾驶(五十五)---------四解轨迹规划,希望对大家解决编程问题提供一定的参考价值,需要的开发者们随着小编来一起学习吧!

      前面已经三次讲过轨迹规划,还是如隔靴搔痒,诗不着题。所以想少一些理论,多一些实践的角度写轨迹规划,就从常见的场景展开:

1. 问题引入:

       已知条件 车身传感器:车速、Yawrate、方向盘转角;车身周围车辆信息;车道线信息;一个模糊的目标点。求 一条轨迹能到达目标点的位置和角度,轨迹要满足车辆运动学,舒适性,避障等要求。

       怎么理解模糊的目标点?目标位置和角度不是很精确,目标状态的位置和角度随着不断地靠近,可能会不断的收敛,例如十字路口转弯,在转弯过程中摄像头看不到入口的车道线,高精地图只能给出大概的位置和角度,在这样模糊的信息下,需要先规划出一条车道线,引导车辆转弯,在过程中看到车道线,不断收敛目标点的位置和方向。

2. 解决思路:

  1. 初始化:初始轨迹规划采用选取多个关键点,利用三次样条曲线生成轨迹。
  2. 更新:车身定位采用Klaman滤波收敛,考虑车身的状态和舒适性,结合收敛的目标位置和角度,重新生成三次样条曲线。
  3. 输出:利用生成的三次样条曲线,选取一定长度的轨迹,生成轨迹多项式(三次),满足舒适性;
  4. 错误处理:一旦车身偏离轨道,或者目标的位置和状态达不到,需要一套逻辑处理,或者直接退出轨迹规划模式。

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)}//指定常数Dfor (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]);}//求解三对角矩阵,结果赋值给ETDMA(E, n - 3, A, B, C, D);M[0] = 0;     //自然边界的首端M为0M[n - 1] = 0; //自然边界的末端M为0for (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];//逆向迭代, 求出Xfor (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. 优化更新

        上面生成的轨迹,没有考虑舒适性和车辆运动学模型。在保证舒适性的方法中,窗口滤波是最直接和简单的方法,但是有明显的确定,产生延迟。这里介绍一种新的思路,供大家思考。

  1.  我们把上一帧计算的轨迹离散成一串点。
  2.  通过本次车身姿态和位置变化,仿射变换计算出上一帧轨迹离散点。
  3.  对新观察的离散点,和上一帧推算的离散点进行Kalman滤波。
  4.  对滤波结果安装第4步重新计算新的轨迹。

6. 错误检测

       对输出的轨迹进行检测,对计算的C0、C1、C2、C3进行数值约束。对不满足要求的结果进行退出操作

这篇关于自动驾驶(五十五)---------四解轨迹规划的文章就介绍到这儿,希望我们推荐的文章对编程师们有所帮助!



http://www.chinasem.cn/article/746383

相关文章

动态规划---打家劫舍

题目: 你是一个专业的小偷,计划偷窃沿街的房屋。每间房内都藏有一定的现金,影响你偷窃的唯一制约因素就是相邻的房屋装有相互连通的防盗系统,如果两间相邻的房屋在同一晚上被小偷闯入,系统会自动报警。 给定一个代表每个房屋存放金额的非负整数数组,计算你 不触动警报装置的情况下 ,一夜之内能够偷窃到的最高金额。 思路: 动态规划五部曲: 1.确定dp数组及含义 dp数组是一维数组,dp[i]代表

软考系统规划与管理师考试证书含金量高吗?

2024年软考系统规划与管理师考试报名时间节点: 报名时间:2024年上半年软考将于3月中旬陆续开始报名 考试时间:上半年5月25日到28日,下半年11月9日到12日 分数线:所有科目成绩均须达到45分以上(包括45分)方可通过考试 成绩查询:可在“中国计算机技术职业资格网”上查询软考成绩 出成绩时间:预计在11月左右 证书领取时间:一般在考试成绩公布后3~4个月,各地领取时间有所不同

poj 2976 分数规划二分贪心(部分对总体的贡献度) poj 3111

poj 2976: 题意: 在n场考试中,每场考试共有b题,答对的题目有a题。 允许去掉k场考试,求能达到的最高正确率是多少。 解析: 假设已知准确率为x,则每场考试对于准确率的贡献值为: a - b * x,将贡献值大的排序排在前面舍弃掉后k个。 然后二分x就行了。 代码: #include <iostream>#include <cstdio>#incl

代码随想录冲冲冲 Day39 动态规划Part7

198. 打家劫舍 dp数组的意义是在第i位的时候偷的最大钱数是多少 如果nums的size为0 总价值当然就是0 如果nums的size为1 总价值是nums[0] 遍历顺序就是从小到大遍历 之后是递推公式 对于dp[i]的最大价值来说有两种可能 1.偷第i个 那么最大价值就是dp[i-2]+nums[i] 2.不偷第i个 那么价值就是dp[i-1] 之后取这两个的最大值就是d

基于51单片机的自动转向修复系统的设计与实现

文章目录 前言资料获取设计介绍功能介绍设计清单具体实现截图参考文献设计获取 前言 💗博主介绍:✌全网粉丝10W+,CSDN特邀作者、博客专家、CSDN新星计划导师,一名热衷于单片机技术探索与分享的博主、专注于 精通51/STM32/MSP430/AVR等单片机设计 主要对象是咱们电子相关专业的大学生,希望您们都共创辉煌!✌💗 👇🏻 精彩专栏 推荐订阅👇🏻 单片机

Python3 BeautifulSoup爬虫 POJ自动提交

POJ 提交代码采用Base64加密方式 import http.cookiejarimport loggingimport urllib.parseimport urllib.requestimport base64from bs4 import BeautifulSoupfrom submitcode import SubmitCodeclass SubmitPoj():de

数学建模笔记—— 非线性规划

数学建模笔记—— 非线性规划 非线性规划1. 模型原理1.1 非线性规划的标准型1.2 非线性规划求解的Matlab函数 2. 典型例题3. matlab代码求解3.1 例1 一个简单示例3.2 例2 选址问题1. 第一问 线性规划2. 第二问 非线性规划 非线性规划 非线性规划是一种求解目标函数或约束条件中有一个或几个非线性函数的最优化问题的方法。运筹学的一个重要分支。2

轨迹规划-B样条

B样条究竟是干啥的?白话就是给出一堆点,用样条的方式,给这些点连接起来,并保证丝滑的。 同时B样条分为准均匀和非均匀,以下为准均匀为例。 参考链接1:https://zhuanlan.zhihu.com/p/50626506https://zhuanlan.zhihu.com/p/50626506 参考链接2: https://zhuanlan.zhihu.com/p/536470972h

PMBOK® 第六版 规划进度管理

目录 读后感—PMBOK第六版 目录 规划进度管理主要关注为整个项目期间的进度管理提供指南和方向。以下是两个案例,展示了进度管理中的复杂性和潜在的冲突: 案例一:近期,一个长期合作的客户因政策要求,急需我们为多家医院升级一个小功能。在这个过程中出现了三个主要问题: 在双方确认接口协议后,客户私自修改接口并未通知我们,直到催进度时才发现这个问题关于UI设计的部分,后台开发人员未将其传递给

LeetCode:64. 最大正方形 动态规划 时间复杂度O(nm)

64. 最大正方形 题目链接 题目描述 给定一个由 0 和 1 组成的二维矩阵,找出只包含 1 的最大正方形,并返回其面积。 示例1: 输入: 1 0 1 0 01 0 1 1 11 1 1 1 11 0 0 1 0输出: 4 示例2: 输入: 0 1 1 0 01 1 1 1 11 1 1 1 11 1 1 1 1输出: 9 解题思路 这道题的思路是使用动态规划