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

(19)国家知识产权局(12)发明专利申请(10)申请公布号CN114987492A(43)申请公布日2022.09.02(21)申请号202110224707.0(22)申请日2021.03.01(71)申请人武汉智行者科技有限公司地址430056湖北省武汉市经济技术开发区军山街小军山社区商业楼4楼106-6(72)发明人张富强徐成颜波张放李晓飞张德兆王肖霍舒豪(74)专利代理机构天津市三利专利商标代理有限公司12107专利代理师张义(51)Int.Cl.B60W40/00(2006.01)B60W60/00(2020.01)B60W30/08(2012.01)权利要求书3页说明书6页附图1页(54)发明名称一种考虑曲率约束的自动驾驶车辆轨迹平滑优化方法(57)摘要本发明公开了一种考虑曲率约束的自动驾驶车辆轨迹平滑优化方法。该方法沿用多约束优化问题的通用思路，首先根据轨迹平滑优化的目标建立与各个轨迹点相关的待优化目标函数，然后采用共轭梯度法求解该目标函数的极值，最后验证优化后的结果是否满足约束条件。该方法的关键点在于计算某个轨迹点处的曲率及其梯度时并非用该点及其前后相邻点直接计算而是拟合通过这三点的二阶拉格朗日插值多项式，然后取拟合曲线的顶点求解曲率及其梯度。CN114987492ACN114987492A权利要求书1/3页1.一种考虑曲率约束的自动驾驶车辆轨迹平滑优化方法，其特征在于，包括如下步骤：首先，根据轨迹平滑优化的目标建立与各个轨迹点相关的待优化目标函数；然后，采用共轭梯度法求解该目标函数的极值；最后，验证优化后的结果是否满足约束条件；其中，计算某个轨迹点处的曲率及其梯度时并非用该点及其前后相邻点直接计算而是拟合通过这三点的二阶拉格朗日插值多项式，然后取拟合曲线的顶点求解曲率及其梯度。2.根据权利要求1所述的一种考虑曲率约束的自动驾驶车辆轨迹平滑优化方法，其特征在于，建立目标函数的具体步骤为：根据轨迹点与障碍物保持一定距离、该点处曲率满足车辆最大曲率约束、轨迹平顺这三个需求设计目标函数：ki＝2|a|其中，wo为到障碍物距离项对应的权重系数，wk为曲率项对应的权重系数，ws为平顺项对应的权重系数，A表示二次项系数矩阵，bT表示一次项系数向量；表示轨迹点处的位移，表示轨迹点处的位移；为规划得到的轨迹点坐标(xi，yi)；表示到轨迹点最近的障碍物坐标点，dmin表示允许到障碍物的最近距离；lag2(x)表示由轨迹点及其相邻的及经过二阶拉格朗日插值得到的曲线，a表示其二次项系数，ki表示其顶点处的曲率，kmax为根据车辆最小转弯半径确定的轨迹最大允许曲2CN114987492A权利要求书2/3页率。3.根据权利要求1或2所述的一种考虑曲率约束的自动驾驶车辆轨迹平滑优化方法，其特征在于，采用共轭梯度法求解具体为：1)计算梯度轨迹点平顺项梯度用其前后相邻的5个点来表示：轨迹点到障碍物距离项的梯度可以表示为：轨迹点曲率项的梯度转化为计算插值曲线在处的曲率梯度：2)初值代入对于每个轨迹点，代入初值与迭代更新思路一致；为了便于描述，这里用代替k：＝0其中为初始迭代方向，k为迭代次数。3)迭代更新k：＝k+13CN114987492A权利要求书3/3页如果足够小，则提前退出循环。4.根据权利要求3所述的一种考虑曲率约束的自动驾驶车辆轨迹平滑优化方法，其特征在于，所述验证优化后的结果，具体包括得到优化轨迹结果后，计算得到的各个轨迹点的曲率是否小于最大曲率、轨迹点处是否发生碰撞，不满足则重新优化。4CN114987492A说明书1/6页一种考虑曲率约束的自动驾驶车辆轨迹平滑优化方法技术领域[0001]本发明涉及轨迹平滑优化方法技术领域，尤其涉及一种考虑曲率约束的自动驾驶车辆轨迹平滑优化方法。背景技术[0002]自动驾驶中路径规划的目标就是规划出安全与舒适的轨迹供车辆完成行驶任务。它是自动驾驶车辆的核心能力之一，决定了车辆能否顺畅、准确、安全地完成各种驾驶行为。路径规划问题在机器人等领域已经存在相对成熟的解决方案，但对于自动驾驶车辆这些方案是无法满足需求的。传统方法仅考虑了地形空间的几何约束，忽略了车辆的运动学和动力学特性，规划得到的轨迹中某些点可能超出车辆控制系统的极限跟踪能力，行驶中会出现抖动甚至发生碰撞。目前常用的路径规划方法主要分为四类：基于图搜索、基于采样、基于插值曲线以及基于数值优化。其中除了基于数值优化的方式外，其余三类方法无法从原理上保证规划得到的轨迹能够满足车辆运动约束，需要在规划得到的初步轨迹基础上进行平滑优化才能够满足车辆的行驶需求。实际路径规划中也经常采用这种“初步轨迹+优化平滑”的思路来解决问题，在复杂非结构化环境中这种思路经常能更好的解决问题。[0003]轨迹平滑优化本质上是一种多约束的非线性优化问题。实际解决