一种考虑曲率约束的自动驾驶车辆轨迹平滑优化方法与流程

车辆装置的制造及其改造技术1.本发明涉及轨迹平滑优化方法技术领域，尤其涉及一种考虑曲率约束的自动驾驶车辆轨迹平滑优化方法。背景技术：2.自动驾驶中路径规划的目标就是规划出安全与舒适的轨迹供车辆完成行驶任务。它是自动驾驶车辆的核心能力之一，决定了车辆能否顺畅、准确、安全地完成各种驾驶行为。路径规划问题在机器人等领域已经存在相对成熟的解决方案，但对于自动驾驶车辆这些方案是无法满足需求的。传统方法仅考虑了地形空间的几何约束，忽略了车辆的运动学和动力学特性，规划得到的轨迹中某些点可能超出车辆控制系统的极限跟踪能力，行驶中会出现抖动甚至发生碰撞。目前常用的路径规划方法主要分为四类：基于图搜索、基于采样、基于插值曲线以及基于数值优化。其中除了基于数值优化的方式外，其余三类方法无法从原理上保证规划得到的轨迹能够满足车辆运动约束，需要在规划得到的初步轨迹基础上进行平滑优化才能够满足车辆的行驶需求。实际路径规划中也经常采用这种“初步轨迹+优化平滑”的思路来解决问题，在复杂非结构化环境中这种思路经常能更好的解决问题。3.轨迹平滑优化本质上是一种多约束的非线性优化问题。实际解决方案中有两种主流方式，一种是直接进行平滑优化；另一种是作为多约束优化问题进行求解。直接平滑优化这类方法中比较常用的包括采用特定曲线平滑和滑动平均平滑，实现时前者采用如reedsshepp、dubins、bezier、spline等性质较好的平滑曲线来重新生成新的轨迹点；后者从第一个点出发滑动对某个邻域内的数据点做某种平均代替邻域的中心值点，到最后一个点结束即完成对给定轨迹点的平滑。多约束优化问题解决的思路比较一致，一般要明确待优化的目标函数、约束条件、变量这三个要素，然后选取某种优化算法进行求解变量的值。4.然而，上述轨迹平滑优化方法均存在一些缺陷：直接平滑优化的方法未考虑轨迹的碰撞等约束项，优化后的轨迹可能碰到周围的障碍物，且采用的曲线无法保证曲率小于车辆的最大曲率约束。作为多约束优化问题时考虑到了可能的约束项并建立了优化的目标函数，一般直接采用某个轨迹点及其相邻的两个轨迹点计算曲率。计算时直接用三个轨迹点连接的折线结构，使用轨迹点位移变化夹角除以位移的模长，对于离散点这种计算方式会出现轨迹点不断远离相邻两个点但计算出的曲率不断变小导致优化方向错误。出现这种问题的根本原因是曲率是针对光滑曲线定义的，定义中包含位移极限为零这个假设，对于三个离散轨迹点连接成的折线结构这种假设是不成立的。技术实现要素：5.针对上述，本发明的目的是提供了一种考虑曲率约束的自动驾驶车辆轨迹平滑优化方法。6.为实现本发明的目的，本发明提供的一种考虑曲率约束的自动驾驶车辆轨迹平滑优化方法，包括如下步骤：7.首先根据轨迹平滑优化的目标建立与各个轨迹点相关的待优化目标函数；8.然后采用共轭梯度法求解该目标函数的极值；9.最后验证优化后的结果是否满足约束条件；10.其中，计算某个轨迹点处的曲率及其梯度时并非用该点及其前后相邻点直接计算而是拟合通过这三点的二阶拉格朗日插值多项式，然后取拟合曲线的顶点求解曲率及其梯度。11.其中，12.建立目标函数的具体步骤为：13.根据轨迹点与障碍物保持一定距离、该点处曲率满足车辆最大曲率约束、轨迹平顺这三个需求设计目标函数：[0014][0015][0016][0017][0018][0019][0020]ki＝2|a|[0021]其中，wo为到障碍物距离项对应的权重系数，wk为曲率项对应的权重系数，ws为平顺项对应的权重系数，a表示二次项系数矩阵，bt表示一次项系数向量；表示轨迹点处的位移，表示轨迹点处的位移；为规划得到的轨迹点坐标(xi,yi)；表示到轨迹点最近的障碍物坐标点，dmin表示允许到障碍物的最近距离；lag2(x)表示由轨迹点及其相邻的及经过二阶拉格朗日插值得到的曲线，a表示其二次项系数，ki表示其顶点处的曲率，kmax为根据车辆最小转弯半径确定的轨迹最大允许曲率。[0022]其中，[0023]采用共轭梯度法求解具体为：[0024]1)计算梯度[0025][0026]轨迹点平顺项梯度用其前后相邻的5个点来表示：[0027][0028]轨迹点到障碍物距离项的梯度可以表示为：[0029][0030]轨迹点曲率项的梯度转化为计算插值曲线在处的曲率梯度：[0031][0032]2)初值代入[0033]对于每个轨迹点，代入初值与迭代更新思路一致；为了便于描述，这里用代替[0034][0035][0036]k∶＝0[0037]其中为初始迭代方向，k为迭代次数。[0038]3)迭代更新[0039][0040][0041][0042][0043][0044]k∶＝k+1[0045]如果足够小，则提前退出循环。[0046]其中，[0047]所述验证优化后的结果，具体包括得到优化轨迹结果后，计算得到的各个轨迹点的曲率是否小于最大曲率、轨迹点处是否发生碰撞，不满足则重新优化。[0048]与现有技术相比，本发明的有益效果为，。附图说明[0049]图1为本技术的轨迹优化过程示意图；[0050]图2为本技术计算曲率时的处理过程示意图。具体实施方式[0051]需要说明的是，在不冲突的情况下，本技术中的实施例及实施例中的特征可以相互组合。[0052]以下结合附图和具体实施例对本发明作进一步详细说明。应当理解，此处所描述的具体实施例仅仅用以解释本发明，并不用于限定本发明。[0053]本发明提出的考虑曲率约束的自动驾驶车辆轨迹平滑优化方法主要包括建立目标函数、采用共轭梯度法求解、验证轨迹这三部分，整个过程如图1所示。平滑优化的输入包括规划得到的轨迹点坐标，如果能够获得规划区域内的距离变换图，则将与障碍物的距离这一项也纳入目标函数。这里假定轨迹所在区域内各点到最近障碍物点的位置能够获得。[0054]具体步骤如下：[0055]1、建立目标函数[0056]根据轨迹点与障碍物保持一定距离、该点处曲率满足车辆最大曲率约束、轨迹平顺这三个需求设计目标函数：[0057][0058][0059][0060][0061][0062][0063]ki＝2|a|[0064]其中，wo为到障碍物距离项对应的权重系数，wk为曲率项对应的权重系数，ws为平顺项对应的权重系数，a表示二次项系数矩阵，bt表示一次项系数向量；表示轨迹点处的位移，表示轨迹点处的位移；为规划得到的轨迹点坐标(xi,yi)；表示到轨迹点最近的障碍物坐标点，dmin表示允许到障碍物的最近距离；lag2(x)表示由轨迹点及其相邻的及经过二阶拉格朗日插值得到的曲线，a表示其二次项系数，ki表示其顶点处的曲率，kmax为根据车辆最小转弯半径确定的轨迹最大允许曲率。计算曲率时的拟合处理过程如图2所示。[0065]2、采用共轭梯度法求解[0066]2.1计算梯度[0067][0068]轨迹点平顺项梯度用其前后相邻的5个点来表示：[0069][0070]轨迹点到障碍物距离项的梯度可以表示为：[0071][0072]轨迹点曲率项的梯度转化为计算插值曲线在处的曲率梯度：[0073][0074]2.2初值代入[0075]对于每个轨迹点，代入初值与迭代更新思路一致。为了便于描述，这里用代替[0076][0077][0078]k∶＝0[0079]其中为初始迭代方向，k为迭代次数。[0080]3.3迭代更新[0081][0082][0083][0084][0085][0086]k∶＝k+1[0087]如果足够小，则提前退出循环(认为已经找到最优解)。[0088]3、验证轨迹[0089]得到优化轨迹结果后，计算得到的各个轨迹点的曲率是否小于最大曲率、轨迹点处是否发生碰撞，不满足则重新优化。[0090]需要说明的是，本技术中未详述的技术方案，采用公知技术。[0091]以上所述仅是本发明的优选实施方式，应当指出的是，对于本技术领域的普通技术人员来说，在不脱离本发明原理的前提下，还可以做出若干改进和润饰，这些改进和润饰也应视为本发明的保护范围。









图片声明：本站部分配图来自人工智能系统AI生成,觅知网授权图片,PxHere摄影无版权图库。本站只作为美观性配图使用,无任何非法侵犯第三方意图,一切解释权归图片著作权方,本站不承担任何责任。如有恶意碰瓷者,必当奉陪到底严惩不贷!




内容声明：本文中引用的各种信息及资料（包括但不限于文字、数据、图表及超链接等）均来源于该信息及资料的相关主体（包括但不限于公司、媒体、协会等机构）的官方网站或公开发表的信息。部分内容参考包括:(百度百科,百度知道,头条百科,中国民法典,刑法,牛津词典,新华词典,汉语词典,国家院校,科普平台)等数据,内容仅供参考使用,不准确地方联系删除处理！本站为非盈利性质站点,发布内容不收取任何费用也不接任何广告!




免责声明：我们致力于保护作者版权，注重分享，被刊用文章因无法核实真实出处，未能及时与作者取得联系，或有版权异议的，请联系管理员，我们会立即处理，本文部分文字与图片资源来自于网络，部分文章是来自自研大数据AI进行生成,内容摘自(百度百科,百度知道,头条百科,中国民法典,刑法,牛津词典,新华词典,汉语词典,国家院校,科普平台)等数据,内容仅供学习参考,不准确地方联系删除处理!的,若有来源标注错误或侵犯了您的合法权益，请立即通知我们，情况属实，我们会第一时间予以删除，并同时向您表示歉意,谢谢!