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

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表示二次项系数矩阵，b
t
表示一次项系数向量；表示轨迹点处的位移，表示轨迹点处的位移；为规划得到的轨迹点坐标(xi,yi)；表示到轨迹点最近的障碍物坐标点，d
min
表示允许到障碍物的最近距离；lag2(x)表示由轨迹点及其相邻的及经过二阶拉格朗日插值得到的曲线，a表示其二次项系数，ki表示其顶点处的曲率，k
max
为根据车辆最小转弯半径确定的轨迹最大允许曲率。
[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表示二次项系数矩阵，b
t
表示一次项系数向量；表示轨迹点处的位移，表示轨迹点处的位移；为规划得到的轨迹点坐标(xi,yi)；表示到轨迹点最近的障碍物坐标点，d
min
表示允许到障碍物的最近距离；lag2(x)表示由轨迹点及其相邻的及经过二阶拉格朗日插值得到的曲线，a表示其二次项系数，ki表示其顶点处的曲率，k
max
为根据车辆最小转弯半径确定的轨迹最大允许曲率。计算曲率时的拟合处理过程如图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]
以上所述仅是本发明的优选实施方式，应当指出的是，对于本技术领域的普通技术人员来说，在不脱离本发明原理的前提下，还可以做出若干改进和润饰，这些改进和润饰也应视为本发明的保护范围。