本发明涉及一种局部路径规划方法、装置和介质，方法包括以下步骤：S1、生成全局参考路径；S2、获取弗勒内坐标系下的自车位置坐标；S3、对自车参考速度、路面附着系数和路面曲率系数进行数学关系建模；S4、以S2得到的弗勒内坐标系下的自车位置坐标和预瞄距离为边界条件进行局部轨迹全采样；S5、对S4采样的局部轨迹进行碰撞判定，筛选出通过碰撞判定的局部轨迹作为候选轨迹；S6、建立成本函数对通过S5的碰撞检测的候选轨迹进行成本估算，以最小成本筛选候选轨迹，得到用于规划的预测轨迹。与现有技术相比，本发明具有适应不同地区路面材质较为多变，复杂且不规整的地形等优点。
1.一种局部路径规划方法，其特征在于，方法包括以下步骤：
S1、获取离散路径采样点数据，生成全局参考路径和路径参数，全局参考路径由各个路径点采样和点之间的弧线组成，所述路径参数包括各点间弧长和各段弧长的轴对齐矩形边界框顶点；
S2、获取笛卡尔坐标系下自车位置坐标，将位置坐标转换到弗勒内坐标系，获得弗勒内坐标系下的自车位置坐标；
S3、基于向心加速度公式，对自车参考速度、路面附着系数和路面曲率系数进行数学关系建模；
S4、根据S3得到的自车参考速度模型和预配置的预瞄时间确定预瞄距离，以S2得到的弗勒内坐标系下的自车位置坐标和预瞄距离为边界条件进行局部轨迹全采样；
S5、使用有向胶囊体边界框来近似自车和障碍物，并对S4采样的局部轨迹进行碰撞判定，筛选出通过碰撞判定的局部轨迹作为候选轨迹；
S6、建立成本函数对通过S5的碰撞检测的候选轨迹进行成本估算，以最小成本筛选候选轨迹，得到用于规划的预测轨迹；
S4的局部轨迹全采样使用曲线拟合的方法，曲线拟合共有6个边界条件，分别为已知起点位置、起点位置对应的第一速度和第一加速度以及已知终点位置、终点位置对应的第二速度和第二加速度，因此采用五次多项式进行参数求解；
设起点时刻 为0，终点时刻 为预瞄时间，预瞄时间为预配置的超参数，曲线拟合拆解为纵、横两个方向，建立纵方向的纵向位置、速度和加速度，以及横方向的横向位置、速度和加速度对应的6个五次多项式方程，其中起点时刻 时的纵方向的纵向位置、速度和加速度，以及横方向的横向位置、速度和加速度为已知量；
基于已知量分析自车处于三种驾驶状态中情况下，分别对应的终点时刻 时的纵方向的纵向位置、速度和加速度，以及横方向的横向位置、速度和加速度，再将三种驾驶状态分别对应的 时的纵方向的纵向位置、速度和加速度，以及横方向的横向位置、速度和加速度代入6个五次多项式方程，求解五次多项式方程的参数，根据6个五次多项式方程分别求得‑ 时间范围内各采样周期的弗勒内坐标系下的自车位置坐标，描点生成局部轨迹，其中三种驾驶状态分别为自主巡航、变道/借道和跟随前车；
其中， 为纵方向的纵向位置， 为纵方向的速度， 为纵方向的加速度，为横方向的横向位置， 为横方向的速度， 为横方向的加速度， 、 、 、 、 和为纵向轨迹五次多项式的6个待解参数； 、 、 、 、 和 为横向轨迹五次多项式的6个待解参数，表示时间，起点时刻 时，对应的纵方向的纵向位置、速度和加速度，以及横方向的横向位置、速度和加速度为已知量；
时刻的其他5个五次多项式方程的参数与 时刻保持一致；
时刻的其他4个五次多项式方程的参数与 时刻保持一致；
跟随前车状态下， 时刻的横方向速度和纵方向的速度与前车车速保持一致，同时纵方向的纵向位置更新为：；
时刻的其他3个五次多项式方程的参数与 时刻保持一致；
将上述的三种驾驶状态分别对应的 时的纵方向的纵向位置、速度和加速度，以及横方向的横向位置、速度和加速度代入6个五次多项式方程，求解待解参数，根据确定了待解参数的五次多项式方程确定局部轨迹；
S5的筛选出通过碰撞判定的局部轨迹作为候选轨迹的具体步骤为：获取障碍物实时信息，基于有向胶囊体边界框近似障碍物实时信息中的坐标、朝向及尺寸，同时也近似自车的坐标、朝向及尺寸，检测自车胶囊体在S4中的局部轨迹上是否与障碍有向胶囊体边界框碰撞，检测是否碰撞具体为：检测自车胶囊体和障碍有向胶囊体的两个有向线段间的最短距离 是否大于两个胶囊体的半径 、之和，若是，则说明未发生碰撞，通过碰撞判定，反之发生碰撞，未通过碰撞判定，对所有的局部轨迹进行判断，最终筛选出通过碰撞判定的所有局部轨迹作为候选轨迹；
其中，胶囊体的两个有向线段为 和 ， 和 为自车胶囊体的两个顶点， 和为障碍有向胶囊体的两个顶点，两个有向线段间的最短距离 通过矢量算法计算；
其中， 和 为自车胶囊体的两个顶点， 和 为障碍有向胶囊体的两个顶点，为条件判别系数，条件判别系数为：；
其中， 、 和 分别为 点到 点、 点到 点和 点到 点的向量；
S6中，成本函数基于自车的在预瞄 时刻横向位置偏离车道中心距离，以及车在预瞄 时刻纵向速度和参考速度差值，对碰撞检测的候选轨迹进行成本估算，成本函数具体为：；
其中，为候选轨迹成本； 为自车在预瞄 时刻横向位置； 为自车在预瞄 时刻所处车道中心线位置，所述车道中心线即为全局参考路径； 为自车在 时刻的纵向速度；
为步骤S3中根据当前路面曲率系数和路面附着系数计算出的自车参考速度；轨迹规划的起始时间是 ，结束时间是 ，其中，起点时刻 为0，终点时刻 为预瞄时间。
2.根据权利要求1所述的一种局部路径规划方法，其特征在于，S1的全局参考路径的生成过程具体为：采用几何连续插值方法，对离散路径采样点数据中的每一对相邻点之间进行插值并进行迭代，生成由密实点集构成的全局参考路径，其中每个插值点满足以下条件：插值点的内角和对应的一对相邻点的内角的算数平均值相等，同时插值点位于对应的一对相邻点连线的垂直平分线上。
3.根据权利要求2所述的一种局部路径规划方法，其特征在于，迭代过程中，根据插值点需要满足的条件，对于一对相邻点A和B，首先确定上一次迭代插值得到的路径采样点集序列，其中第一次迭代插值得到的路径采样点集序列即为初始的离散路径采样点数据，然后计算相邻点A和B连线中点C，再计算点A和点A的另一个相邻点 的连线，与A和B连线这两条连线之间的夹角 ，同时计算点B和点B的另一个相邻点 的连线，与A和B连线这两条连线之间的夹角 ；
根据两个夹角 和 ，计算点 、和插值点的内角，再根据插值点的内角计算点A和插值点连线，与A和B连线这两条连线之间的夹角 ；
根据点A的笛卡尔坐标系下的坐标值、连线中点C的笛卡尔坐标系下的坐标值，以及夹角 确定插值点的笛卡尔坐标系下的坐标值，将插值点作为新的路径采样点存入路径采样点集序列中，循环上述步骤直至达到迭代次数，得到全局参考路径各点坐标点集 ，坐标点集 组成全局参考路径。
4.根据权利要求3所述的一种局部路径规划方法，其特征在于，插值点的笛卡尔坐标系下的坐标值为：；
其中， 为插值点 的x轴和y轴坐标， 为点A的笛卡尔坐标系下的y轴坐标， 为连线中点C的笛卡尔坐标系下的y轴坐标，为两条连线之间的夹角。
5.根据权利要求4所述的一种局部路径规划方法，其特征在于，迭代过程中，每次获得新的插值点时，路径采样点集序列中除路径首尾点外的各中间点作为上端弧长末尾和下端弧长开端，计算中间点中相邻点的曲率，基于非线性最小二乘法对各个点的曲率残差平方和进行优化，对插值点的笛卡尔坐标系下的坐标值进行进一步优化，再将优化后的插值点放入路径采样点集序列中；
得到最后的坐标点集 时，同时输出坐标点集 中各个中间点的各点间弧长、各段弧长起点参数、路径总长和各段弧长的轴对齐矩形边界框顶点。
6.根据权利要求1所述的一种局部路径规划方法，其特征在于，S2的具体步骤为：将各点间弧长根据几何特征归类为羊角螺线、直线和弧线，通过检索各段弧长的轴对齐矩形边界框顶点，确定与笛卡尔坐标系下自车位置坐标最近的弧长，并在该弧长上检索离自车位置坐标最近的路径点采样点，将该点作为弗勒内坐标系的空间映射锚定点，将空间映射锚定点作为终点进行积分，得到自车位置坐标在弗勒内坐标系的第一个坐标系数 ，所述第一个坐标系数为沿道路的行驶距离；
计算笛卡尔坐标系下自车位置坐标与空间映射锚定点之间的欧氏距离，该欧氏距离作为自车位置坐标在弗勒内坐标系的第二个坐标系数 ，所述第二个坐标系数为自车相对于车道中心线的偏移距离，得到弗勒内坐标系下的自车位置坐标 ，其中，所述车道中心线即为全局参考路径；
其中，检索各段弧长的轴对齐矩形边界框顶点的具体步骤为：在各段弧长点集中获取距离最远的两个x轴坐标 和 以及距离最远的两个y轴坐标以及距离最远的两个y轴坐标 和 ，然后用他们来构建一个轴对齐矩形边界框，边界框四角顶点坐标即为：；
7.根据权利要求6所述的一种局部路径规划方法，其特征在于，羊角螺线、直线和弧线的归类依据为：直线的起始曲率 为0，曲率变化率 为0；
8.根据权利要求1所述的一种局部路径规划方法，其特征在于，对自车参考速度、路面附着系数和路面曲率系数进行数学关系建模的具体步骤为：对车辆对应的物体向心运动加速度进行建模，确定车辆过弯时最大向心加速度，根据最大向心加速度和物体向心运动加速度之间的关系，分为车辆对驾驶控制输入的响应近似线性和车辆前后轮的横向轮胎力接近轮胎非线性阈值两种情况，对两种不同的情况进行自车参考速度 、路面附着系数 和路面曲率 三者之间的数学关系进行建模。
9.根据权利要求8所述的一种局部路径规划方法，其特征在于，自车参考速度 、路面附着系数 和路面曲率 三者之间的数学关系模型为：；
其中， 为弗勒内坐标系下，车辆沿道路行驶至距离 处参考速度， 为路段预设速度； 为弗勒内坐标系下，车辆沿道路行驶至距离 处路面曲率， 为路面附着系数， 为重力加速度。
10.一种局部路径规划装置，包括存储器和一个或多个处理器，以及存储于所述存储器中的可执行代码，其特征在于，所述一个或多个处理器执行所述可执行代码时实现如权利要求1‑9中任一所述的方法。
11.一种计算机可读存储介质，其上存储有程序，其特征在于，所述程序被执行时实现如权利要求1‑9中任一所述的方法。
一种局部路径规划方法、装置和介质
技术领域
[0001] 本发明涉及自动驾驶的路面规划技术领域，尤其是涉及一种局部路径规划方法、装置和介质。
背景技术
[0002] 现有技术中，局部路径规划方法中城市道路行驶类多关注速度‑路面曲率系数关系建模，越野行驶类多关注速度‑路面附着系数关系建模，但是，对于区路面材质较为多变，复杂且不规整的地形（如，沥青，混凝土，草地，雪地等路面材质交错变化的路段），具有路面附着系数多变的特点的区域，目前的自动驾驶的路面规划方法对这类区域的泛化能力具有一定局限性，往往会引起车速与安全协调性不贴合等问题，导致车辆在转向操纵时侧翻或冲出车道。这是由于目前的自动驾驶的路面规划方法在一段路程内只会采用相同的附着系数和路面曲率等系数，在这种情况下，面对复杂且不规整的地形时的规划路径无法和实际情况匹配，也就无法在保证车速的情况下保证自动驾驶的安全性。
发明内容
[0003] 本发明的目的就是为了克服上述问题提供的一种局部路径规划方法、装置和介质。
[0004] 本发明的目的可以通过以下技术方案来实现：
[0005] 一种局部路径规划方法，其特征在于，方法包括以下步骤：
[0006] S1、获取离散路径采样点数据，生成全局参考路径和路径参数，全局参考路径由各个路径点采样和点之间的弧线组成，所述路径参数包括各点间弧长和各段弧长的轴对齐矩形边界框顶点；
[0007] S2、获取笛卡尔坐标系下自车位置坐标，将位置坐标转换到弗勒内坐标系，获得弗勒内坐标系下的自车位置坐标；
[0008] S3、基于向心加速度公式，对自车参考速度、路面附着系数和路面曲率系数进行数学关系建模；
[0009] S4、根据S3得到的自车参考速度模型和预配置的预瞄时间确定预瞄距离，以S2得到的弗勒内坐标系下的自车位置坐标和预瞄距离为边界条件进行局部轨迹全采样；
[0010] S5、使用有向胶囊体边界框来近似自车和障碍物，并对S4采样的局部轨迹进行碰撞判定，筛选出通过碰撞判定的局部轨迹作为候选轨迹；
[0011] S6、建立成本函数对通过S5的碰撞检测的候选轨迹进行成本估算，以最小成本筛选候选轨迹，得到用于规划的预测轨迹。
[0012] 进一步地，S1的全局参考路径的生成过程具体为：
[0013] 采用几何连续插值方法，对离散路径采样点数据中的每一对相邻点之间进行插值并进行迭代，生成由密实点集构成的全局参考路径，其中每个插值点满足以下条件：插值点的内角和对应的一对相邻点的内角的算数平均值相等，同时插值点位于对应的一对相邻点连线的垂直平分线上。
[0014] 进一步地，迭代过程中，根据插值点需要满足的条件，对于一对相邻点A和B，首先确定上一次迭代插值得到的路径采样点集序列，其中第一次迭代插值得到的路径采样点集序列即为初始的离散路径采样点数据，
[0015] 然后计算相邻点A和B连线中点C，再计算点A和点A的另一个相邻点 的连线，与A和B连线这两条连线之间的夹角 ，同时计算点B和点B的另一个相邻点 的连线，与A和B连线这两条连线之间的夹角 ；
[0016] 根据两个夹角 和 ，计算点 、和插值点的内角，再根据插值点的内角计算点A和插值点连线，与A和B连线这两条连线之间的夹角 ；
[0017] 根据点A的笛卡尔坐标系下的坐标值、连线中点C的笛卡尔坐标系下的坐标值，以及夹角 确定插值点的笛卡尔坐标系下的坐标值，将插值点作为新的路径采样点存入路径采样点集序列中，循环上述步骤直至达到迭代次数，得到全局参考路径各点坐标点集 ，坐标点集 组成全局参考路径。
[0018] 进一步地，插值点的笛卡尔坐标系下的坐标值为：
[0019] ；
[0020] 其中， 为插值点 的x轴和y轴坐标， 为点A的笛卡尔坐标系下的y轴坐标， 为连线中点C的笛卡尔坐标系下的y轴坐标，为两条连线之间的夹角。
[0021] 进一步地，迭代过程中，每次获得新的插值点时，路径采样点集序列中除路径首尾点外的各中间点作为上端弧长末尾和下端弧长开端，计算中间点中相邻点的曲率，基于非线性最小二乘法对各个点的曲率残差平方和进行优化，对插值点的笛卡尔坐标系下的坐标值进行进一步优化，再将优化后的插值点放入路径采样点集序列中；
[0022] 得到最后的坐标点集 时，同时输出坐标点集 中各个中间点的各点间弧长、各段弧长起点参数、路径总长和各段弧长的轴对齐矩形边界框顶点。
[0023] 进一步地，S2的具体步骤为：将各点间弧长根据几何特征归类为羊角螺线、直线和弧线，通过检索各段弧长的轴对齐矩形边界框顶点，确定与笛卡尔坐标系下自车位置坐标最近的弧长，并在该弧长上检索离自车位置坐标最近的路径点采样点，将该点作为弗勒内坐标系的空间映射锚定点，将空间映射锚定点作为终点进行积分，得到自车位置坐标在弗勒内坐标系的第一个坐标系数 ，所述第一个坐标系数为沿道路的行驶距离；
[0024] 计算笛卡尔坐标系下自车位置坐标与空间映射锚定点之间的欧氏距离，该欧氏距离作为自车位置坐标在弗勒内坐标系的第二个坐标系数 ，所述第二个坐标系数为自车相对于车道中心线的偏移距离，得到弗勒内坐标系下的自车位置坐标 ，其中，所述车道中心线即为全局参考路径；
[0025] 其中，检索各段弧长的轴对齐矩形边界框顶点的具体步骤为：在各段弧长点集中获取距离最远的两个x轴坐标 和 以及距离最远的两个y轴坐标以及距离最远的两个y轴坐标 和 ，然后用他们来构建一个轴对齐矩形边界框，边界框四角顶点坐标即为：
[0026] ；
[0027] 依次检索上述顶点。
[0028] 进一步地，羊角螺线、直线和弧线的归类依据为：
[0029] 直线的起始曲率 为0，曲率变化率 为0；
[0030] 弧线的起始曲率 不为0，曲率变化率 为0；
[0031] 羊角螺线的起始曲率 不为0，曲率变化率 不为0。
[0032] 进一步地，对自车参考速度、路面附着系数和路面曲率系数进行数学关系建模的具体步骤为：
[0033] 对车辆对应的物体向心运动加速度进行建模，确定车辆过弯时最大向心加速度，根据最大向心加速度和物体向心运动加速度之间的关系，分为车辆对驾驶控制输入的响应近似线性和车辆前后轮的横向轮胎力接近轮胎非线性阈值两种情况，对两种不同的情况进行自车参考速度 、路面附着系数 和路面曲率 三者之间的数学关系进行建模。
[0034] 进一步地，自车参考速度 、路面附着系数 和路面曲率 三者之间的数学关系模型为：
[0035] ；
[0036] 其中， 为弗勒内坐标系下，车辆沿道路行驶至距离 处参考速度， 为路段预设速度； 为弗勒内坐标系下，车辆沿道路行驶至距离 处路面曲率， 为路面附着系数， 为重力加速度。
[0037] 进一步地，S4的局部轨迹全采样使用曲线拟合的方法，曲线拟合共有6个边界条件，分别为已知起点位置、起点位置对应的第一速度和第一加速度以及已知终点位置、终点位置对应的第二速度和第二加速度，因此采用五次多项式进行参数求解；
[0038] 设起点时刻 为0，终点时刻 为预瞄时间，预瞄时间为预配置的超参数，曲线拟合拆解为纵、横两个方向，建立纵方向的纵向位置、速度和加速度，以及横方向的横向位置、速度和加速度对应的6个五次多项式方程，其中起点时刻 时的纵方向的纵向位置、速度和加速度，以及横方向的横向位置、速度和加速度为已知量；
[0039] 基于已知量分析自车处于三种驾驶状态中情况下，分别对应的终点时刻 时的纵方向的纵向位置、速度和加速度，以及横方向的横向位置、速度和加速度，再将三种驾驶状态分别对应的 时的纵方向的纵向位置、速度和加速度，以及横方向的横向位置、速度和加速度代入6个五次多项式方程，求解五次多项式方程的参数，根据6个五次多项式方程分别求得 ‑ 时间范围内各采样周期的弗勒内坐标系下的自车位置坐标，描点生成局部轨迹，其中三种驾驶状态分别为自主巡航、变道/借道和跟随前车。
[0040] 进一步地，6个五次多项式方程分别为：
[0041] ；
[0042] ；
[0043] ；
[0044] ；
[0045] ；
[0046] ；
[0047] 其中， 为纵方向的纵向位置， 为纵方向的速度， 为纵方向的加速度，为横方向的横向位置， 为横方向的速度， 为横方向的加速度， 、 、 、 、和 为纵向轨迹五次多项式的6个待解参数； 、 、 、 、 和 为横向轨迹五次多项式的6个待解参数，表示时间，起点时刻 时，对应的纵方向的纵向位置、速度和加速度，以及横方向的横向位置、速度和加速度为已知量；
[0048] 自主巡航状态下，时刻的纵方向的纵向位置更新为：
[0049] ；
[0050] 时刻的其他5个五次多项式方程的参数与 时刻保持一致；
[0051] 变道/借道状态下，时刻的纵方向的纵向位置更新为：
[0052] ；
[0053] 时刻的横方向的横向位置更新为：
[0054] ；
[0055] 时刻的其他4个五次多项式方程的参数与 时刻保持一致；
[0056] 跟随前车状态下， 时刻的横方向速度和纵方向的速度与前车车速保持一致，同时纵方向的纵向位置更新为：
[0057] ；
[0058] 时刻的其他3个五次多项式方程的参数与 时刻保持一致；
[0059] 将上述的三种驾驶状态分别对应的 时的纵方向的纵向位置、速度和加速度，以及横方向的横向位置、速度和加速度代入6个五次多项式方程，求解待解参数，根据确定了待解参数的五次多项式方程确定局部轨迹。
[0060] 进一步地，S5的筛选出通过碰撞判定的局部轨迹作为候选轨迹的具体步骤为：
[0061] 获取障碍物实时信息，基于有向胶囊体边界框近似障碍物实时信息中的坐标、朝向及尺寸，同时也近似自车的坐标、朝向及尺寸，检测自车胶囊体在S4中的局部轨迹上是否与障碍有向胶囊体边界框碰撞，检测是否碰撞具体为：
[0062] 检测自车胶囊体和障碍有向胶囊体的两个有向线段间的最短距离 是否大于两个胶囊体的半径 、之和，若是，则说明未发生碰撞，通过碰撞判定，反之发生碰撞，未通过碰撞判定，对所有的局部轨迹进行判断，最终筛选出通过碰撞判定的所有局部轨迹作为候选轨迹；
[0063] 其中，胶囊体的两个有向线段为 和 ， 和 为自车胶囊体的两个顶点，和 为障碍有向胶囊体的两个顶点，两个有向线段间的最短距离 通过矢量算法计算。
[0064] 进一步地，最短距离 为：
[0065] ；
[0066] 其中， 和 为自车胶囊体的两个顶点， 和 为障碍有向胶囊体的两个顶点，为条件判别系数，条件判别系数为：
[0067] ；
[0068] 其中， 、 和 分别为 点到 点、 点到 点和 点到 点的向量。
[0069] 进一步地，S6中，成本函数基于自车的在预瞄 时刻横向位置偏离车道中心距离，以及车在预瞄 时刻纵向速度和参考速度差值，对碰撞检测的候选轨迹进行成本估算，成本函数具体为：
[0070] ；
[0071] 其中，为候选轨迹成本； 为自车在预瞄 时刻横向位置； 为自车在预瞄 时刻所处车道中心线位置，所述车道中心线即为全局参考路径； 为自车在 时刻的纵向速度； 为步骤S3中根据当前路面曲率系数和路面附着系数计算出的自车参考速度；轨迹规划的起始时间是 ，结束时间是 ，其中，起点时刻 为0，终点时刻 为预瞄时间。
[0072] 本发明的另一方面，提出一种局部路径规划装置，包括存储器和一个或多个处理器，以及存储于所述存储器中的可执行代码，所述一个或多个处理器执行所述可执行代码时实现上所述的方法。
[0073] 本发明的另一方面，提出一种计算机可读存储介质，其上存储有程序，所述程序被执行时实现上述的方法。
[0074] 与现有技术相比，本发明具有以下有益效果：
[0075] （1）本发明能够根据路面曲率系数和路面附着系数求解自车在不同路面曲率和附着能力下的安全通过速度作为参考车速，然后根据不同情况的参考车速确定预瞄距离，确定起点和起点经过预瞄距离后的终点之间的局部轨迹，这样使得局部轨迹能够根据不同的附着系数、路面曲率而变化，具有更强的地形泛化性这一优点，以适应不同地区路面材质较为多变，复杂且不规整的地形
[0076] （2）本发明的预瞄时间为预配置的超参数，能够根据驾驶行为对规划决策模式进行调整，较小的预瞄时间为短视轨迹规划模式，较大的预瞄时间为非短视轨迹规划模式。其中，非短视轨迹规划模式适用于超车场景中自车变道领先后是否立即返回原车道判断（如，变道超车后原车道前方还有其他交通参与车），短视轨迹规划模式则适用于跟车及车道保持场景。因此，本发明具有在超车、车道保持等场景泛化的优点。
附图说明
[0077] 图1为本发明的流程图；
[0078] 图2为本发明的整体流程框图；
[0079] 图3为本发明的几何连续插值方法示意图；
[0080] 图4是本发明的全局参考路径上离自车位置最近点检索流程示意图；
[0081] 图5是本发明的局部轨迹全采样示意图；
[0082] 图6是有向胶囊体几何形状及参数示意图；
[0083] 图7是有向胶囊体边界框近似自车和视觉检测到障碍物示意图；
[0084] 图8是自车采样轨迹基于有向胶囊体边界框的碰撞检测示意图。
具体实施方式
[0085] 下面结合附图和具体实施例对本发明进行详细说明。本实施例以本发明技术方案为前提进行实施，给出了详细的实施方式和具体的操作过程，但本发明的保护范围不限于下述的实施例。
[0086] 实施例1
[0087] 本发明提出一种局部路径规划方法，方法的流程图如图1所示。方法的流程框图如图2所示。本发明的方法包括以下步骤：
[0088] S1、全局参考路径生成：根据离散路径采样点做插值计算生成G2连续曲线作为自车全局参考路径。
[0089] 现有技术中的轨迹插值方法主要分为参数连续和几何连续两类。参数连续的插值方法是一种基于多项式方程的插值法（如，基于贝塞尔曲线的拉格朗日插值、牛顿插值和埃尔米特插值）。在已知某多项式形式在若干离散点上的函数值或者导数信息的情况下，通过求解该多项式中待定系数，使得该多项式在给定离散点上满足约束。几何连续的插值方法是通过在初始路径采样点集的每两个点之间插入与前后曲率连续的点，并迭代这一过程，生成由密实点集构成的全局参考路径。不规则、多崎岖等复杂且不规整的地形及路面几何条件对于基于多项式方程的参数求解插值法是不一定有闭式解的。因此采用参数连续插值的方法，会有可能导致插值曲线与实际路网有偏差，影响轨迹规划的准确性。本发明采用几何连续插值的方法，在接收到离散路径采样点相关数据后，基于几何连续的G2级次要求对路径采样点做插值计算生成自车全局参考路径。其中，G2连续是指符合2类几何连续的要求，包含各点处切线角连续和曲率连续两项要求。离散路径采样点可以是由车载视觉感知单元获取到的可通行区域/道路中线特征点或由高精度地图提供的可通行区域/道路中线特征点先验信息。
[0090] 本发明采用的几何连续插值方法通过在初始路径采样点集的相邻点之间进行插值，并迭代这一过程，生成由密实点集构成的全局参考路径。其中，几何连续插值的迭代次数为超参数，可根据实际需求进行设置。为符合G2级次插值的各点处切线角和曲率连续两项技术要求，每个迭代中，插值点的定位计算有以下约束条件：1）插值点 位于路径采样点集相邻点 、连线的垂直平分线上；2）插值点 的内角 是相邻点 、内角 、 的算数平均值（参见图3）。根据相关约束条件，插值点 的定位 可由下式计算：
[0091] (1)
[0092] (2)
[0093] (3)
[0094] (4)
[0095] (5)
[0096] (6)
[0097] (7)
[0098] (8)
[0099] (9)
[0100] 式中 指代第 次迭代， 为第 次迭代插值得到的路径采样点集序列，、 、 、 分别为 、、‑ 连线中点及插值点 在笛卡尔坐标系下的坐标值， 、 分别为 、在路径采样点集 的左邻
和右邻 在笛卡尔坐标系下的坐标值，、分别为 ‑ 、‑ 连线与 ‑ 连线的夹角，、 和 分别是点 、和插值点 的内角，是 ‑ 连线与 ‑ 连线的夹角。
[0101] 在采用几何连续的方法进行插值过程中，除路径首尾点外的各中间点作为上端弧长末尾和下端弧长开端，其曲率被重复计算。因此方法还采用列文伯格‑马夸尔特算法在最小二乘意义上对各点的曲率残差平方和进行优化：通过进一步调整各插值点位置，对已完成插值的全局参考路径 进行优化。
[0102] 完成插值优化后，此步骤的主要计算输出为全局参考路径各点坐标点集 、各点间弧长和各段弧长的轴对齐矩形边界框顶点。这几项输出是为后面步骤中相关计算做准备。
[0103] S2、自车位置在弗勒内（Frenet）坐标系空间映射：通过搜索步骤S1生成的全局参考路径上距离自车当前位置的最近点，将获取到的自车在笛卡尔坐标系下的当前位置坐标转换为在Frenet坐标系下的位置坐标 。
[0104] 基于笛卡尔坐标系的路径规划方法在计算车辆已行驶距离时更适用于笔直路段，在多崎岖路段的计算成本明显提升。相较笛卡尔坐标系，Frenet坐标系能更直观地描述车辆与车道的关系，通过自车向参考线（即，步骤S1中生成的全局参考路径）投影即可得到自车已行驶距离。多崎岖路段更适合Frenet的路径规划框架。
[0105] 借助车载导航定位模块，可以获取自车在笛卡尔坐标系下位置坐标，并将自车位置由笛卡尔坐标系空间映射至Frenet坐标系空间。这一过程的实现需要先检索全局参考路径上离自车位置最近的点作为映射锚定点。为减少计算量，该方法将全局参考路径的各段弧长根据几何特征归类为羊角螺线、直线和弧线（参考图4）。羊角螺线、直线和弧线的主要区别在于：1）直线的起始曲率 为0，曲率变化率 为0；2）弧线的起始曲率 不为0，曲率变化率 为0；3）羊角螺线的起始曲率 不为0，曲率变化率 不为0。归类后，通过检索各路径分段轴对齐矩形边界框顶点：
[0106]
[0107] 确定与自车位置最近的路径分段，并在该分段上检索离自车位置最近的点 作为Frenet坐标系空间映射锚定点。将该最近点作为终点进行积分，可
得到自车在Frenet坐标系下的 参数，即沿道路的行驶距离。自车位置 与该最近点 之间的欧氏距离 ：
[0108]
[0109] 为自车相对于车道中心线的偏移距离，即自车在Frenet坐标系下的 参数。
[0110] S3、自车参考速度生成：基于向心加速度公式，对自车参考速度、路面附着系数和路面曲率系数进行数学关系建模。
[0111] 在确定自车在Frenet坐标系下的坐标参数 后，对自车目标速度、路面附着系数和路面曲率三者之间的数学关系进行建模。基于该数学关系模型，路径规划方法能够根据车载视觉感知单元获取到的路面附着系数和路面曲率生成自车能够安全通过当前路段的目标车速。由物体向心运动公式可得：
[0112] （10）
[0113] 式中 为物体向心加速度， 为物体切向速度（即，自车目标速度）， 为路面曲率。
[0114] 车辆过弯时，摩擦力是作用在车上的唯一水平力，为车辆提供向心力，防止其打滑产生离心运动。当物体处于临界状态时，静摩擦力达到最大值，即为最大静滑动摩擦力。当物体所需的向心力大于最大静滑动摩擦力提供的向心力时，车辆将做离心运动。静摩擦力能够提供的最大向心力 和最大向心加速度 为：
[0115] （11）
[0116] （12）
[0117] 式中 为静摩擦力所能提供的最大向心力， 为最大向心加速度， 为路面附着系数， 为车辆的质量， 为重力加速度。
[0118] 当车辆向心加速度 小于等于 时，车辆状态对驾驶控制输入的响应近似线性，车辆通常是稳定的。当车辆向心加速度 大于 时，车辆前后轮的横向轮胎力接近轮胎非线性阈值。基于以上，对自车目标速度 、路面附着系数 和路面曲率 三者之间的数学关系进行建模如下：
[0119] （13）
[0120] 式中 为Frenet坐标系下，车辆沿道路行驶至距离 处参考速度， 为路段预设速度； 为Frenet坐标系下，车辆沿道路行驶至距离 处路面曲率，该值可从步骤S1的输出中检索，或由高精度地图提供该先验信息；路面附着系数 由车载视觉感知单元识别路面材质和干湿程度后于表1各值中选取。
[0121] 表1 材质、干湿程度及路面附着系数
[0122]
[0123] S4、自车局部轨迹全采样：根据步骤S3生成的自车参考速度及预瞄时间确定预瞄距离，并以自车当前位置Frenet坐标和预瞄距离为边界条件进行局部轨迹全采样。
[0124] 轨迹采样是使用曲线拟合的方法，由于共有六个边界条件，分别为已知起点位置、速度和加速度以及已知终点位置、速度和加速度，因此采用五次多项式进行参数求解。轨迹规划的起始时间是 ，结束时间是 。其中，起点时刻 为0，终点时刻 为预瞄时间。预瞄时间为超参数，用户可以自行设置。较小的预瞄时间为短视轨迹规划模式，较大的预瞄时间为非短视轨迹规划模式。在Frenet坐标系中，为纵坐标，代表车辆沿道路的行驶距离， 为横坐标，表示车辆相对于车道中心线的偏移距离。基于Frenet坐标系，将轨迹拟合拆解为纵、横两个方向，五次多项式方程分别为：
[0125] (14)
[0126] (15)
[0127] (16)
[0128] (17)
[0129] (18)
[0130] (19)
[0131] 式中 、 和 分别为自车在 时刻的纵向位置、速度和加速度； 、和 分别为自车在 时刻的横向位置、速度和加速度； 、 、 、 、 和 为纵向轨迹五次多项式的6个待解参数； 、 、 、 、 和 为横向轨迹五次多项式的6个待解参数。
[0132] 自车在 时刻的横纵向位置、速度和加速度：
[0133]
[0134] 为已知参数，基于“自主巡航”、“变道/借道”和“跟随前车”三种假设对自车在 时刻的横纵向位置、速度和加速度 进行计算。其中，“自主巡航”是指假设自车前方无其他交通参与者，可按照当前参考速度沿目前车道继续驾驶。此假设下，时刻 ，其余五项参数均与 时刻保持一致。“变
道/借道”是指假设自车前方有其他交通参与者或静态障碍物，需变道/借道驾驶。此假设下， 时刻 ， ，其余四项参数均与
时刻保持一致。“跟随前车”是指假设自车前方有其他交通参与者，且无法变道/借道（例如，有对方来车），需与前车保持安全距离并按照前车速度跟随驾驶。此假设下， 和与前车车速保持一致， ，其余三项参数均与 时刻保持一
致。基于三种假设的自车在 和 时刻的横纵向位置、速度和加速度带入式（14）‑（19），即可求得纵横向轨迹五次多项式的待解参数。基于纵横向多项式方程分别求得 ‑ 时间范围内各采样周期的 、坐标值，描点生成局部轨迹（参见图5）。
[0135] S5、采样轨迹碰撞检测：使用有向胶囊体边界框近似自车和视觉检测到障碍物（静态障碍物及其他交通参与者）的几何形态与朝向，并沿步骤S4采样的候选轨迹对生成的胶囊体边界框进行相交检测（即，碰撞判定）。
[0136] 有向胶囊体是由长度 、半径 、与坐标轴夹角 和顶点 、标定的数据结构（参见图6）。借助车载导航定位模块，可以获取
障碍物实时的坐标、朝向及长宽尺寸信息，并采用有向胶囊体边界框近似自车和视觉检测到障碍物的坐标、朝向及尺寸（参见图7）。由此，自车在采样轨迹上是否与环境障碍物发生碰撞可由自车胶囊体是否与场景中其他胶囊体发生相交进行判定，即检测自车和障碍物胶囊体的有向线段 、 间的最短距离 是否大于两者半径 、之和。若 ，
则未碰撞，否则发生碰撞。其中，最短距离 可由分别计算 点和线段 、 和 、和 和 和 之间的最小距离，并选取四个最小距离中的最小值进行获取。最短距离的计算采用矢量算法，以 点和线段 之间最短距离 为示例，该值可由下式计算：
[0137] （20）
[0138] （21）
[0139] 式中 为式（21）的条件判别系数； 、 和 分别为 点到 点、 点到点和 点到 点的向量。自车采样轨迹基于有向胶囊体边界框的碰撞检测示意图如图8所示。
[0140] S6、采样轨迹成本估算：建立成本函数对通过步骤S5碰撞检测的候选轨迹进行成本估算，以最小成本选择候选轨迹作为预测轨迹。
[0141] 对通过碰撞检测的候选轨迹进行成本估算和排序，选取最小成本选择候选轨迹作为预测轨迹。其中，成本估计函数 从自车“自车在预瞄 时刻横向位置偏离车道中心距离”和“自车在预瞄 时刻纵向速度和参考速度差值”两方面对候选轨迹的成本进行评价，如下：
[0142] （22）
[0143] 式中 为候选轨迹成本； 为自车在预瞄 时刻横向位置； 为自车在预瞄 时刻所处车道中心线位置； 为自车在 时刻的纵向速度； 为步骤S2中根据当前路面曲率系数和路面附着系数计算出的自车安全通过速度（即，参考车速）；轨迹规划的起始时间是，结束时间是 。其中，起点时刻 为0，终点时刻 为预瞄时间。
[0144] 实施例2
[0145] 本发明还提供了一种对应于图1的局部路径规划装置，包括存储器和一个或多个处理器，以及存储于所述存储器中的可执行代码，一个或多个处理器执行所述可执行代码时实现上述的局部路径规划方法。装置还包括内部总线、网络接口、内存。当然，除了软件实现方式之外，本发明并不排除其他实现方式，比如逻辑器件抑或软硬件结合的方式等等，也就是说以下处理流程的执行主体并不限定于各个逻辑单元，也可以是硬件或逻辑器件。
[0146] 对于一个技术的改进可以很明显地区分是硬件上的改进（例如，对二极管、晶体管、开关等电路结构的改进）还是软件上的改进（对于方法流程的改进）。然而，随着技术的发展，当今的很多方法流程的改进已经可以视为硬件电路结构的直接改进。设计人员几乎都通过将改进的方法流程编程到硬件电路中来得到相应的硬件电路结构。因此，不能说一个方法流程的改进就不能用硬件实体模块来实现。例如，可编程逻辑器件（Programmable Logic Device, PLD）（例如现场可编程门阵列（Field Programmable Gate Array，FPGA））就是这样一种集成电路，其逻辑功能由用户对器件编程来确定。由设计人员自行编程来把一个数字系统“集成”在一片PLD上，而不需要请芯片制造厂商来设计和制作专用的集成电路芯片。而且，如今，取代手工地制作集成电路芯片，这种编程也多半改用“逻辑编译器（logic compiler）”软件来实现，它与程序开发撰写时所用的软件编译器相类似，而要编译之前的原始代码也得用特定的编程语言来撰写，此称之为硬件描述语言（Hardware Description Language，HDL），而HDL也并非仅有一种，而是有许多种，如ABEL（Advanced Boolean Expression Language）、AHDL（Altera Hardware Description Language）、Confluence、CUPL（Cornell University Programming Language）、HDCal、JHDL（Java Hardware Description Language）、Lava、Lola、MyHDL、PALASM、RHDL（Ruby Hardware Description Language）等，目前最普遍使用的是VHDL（Very‑High‑Speed Integrated Circuit Hardware Description Language）与Verilog。本领域技术人员也应该清楚，只需要将方法流程用上述几种硬件描述语言稍作逻辑编程并编程到集成电路中，就可以很容易得到实现该逻辑方法流程的硬件电路。
[0147] 控制器可以按任何适当的方式实现，例如，控制器可以采取例如微处理器或处理器以及存储可由该（微）处理器执行的计算机可读程序代码（例如软件或固件）的计算机可读介质、逻辑门、开关、专用集成电路（Application Specific Integrated Circuit，ASIC）、可编程逻辑控制器和嵌入微控制器的形式，控制器的例子包括但不限于以下微控制器：ARC 625D、Atmel AT91SAM、Microchip PIC18F26K20 以及Silicone Labs C8051F320，存储器控制器还可以被实现为存储器的控制逻辑的一部分。本领域技术人员也知道，除了以纯计算机可读程序代码方式实现控制器以外，完全可以通过将方法步骤进行逻辑编程来使得控制器以逻辑门、开关、专用集成电路、可编程逻辑控制器和嵌入微控制器等的形式来实现相同功能。因此这种控制器可以被认为是一种硬件部件，而对其内包括的用于实现各种功能的装置也可以视为硬件部件内的结构。或者甚至，可以将用于实现各种功能的装置视为既可以是实现方法的软件模块又可以是硬件部件内的结构。
[0148] 实施例3
[0149] 本发明还提供了一种对应于图1的存储介质，其上存储有程序，所述程序被执行时实现上述的局部路径规划方法。存储介质包括永久性和非永久性、可移动和非可移动媒体可以由任何方法或技术来实现信息存储。信息可以是计算机可读指令、数据结构、程序的模块或其他数据。计算机的存储介质的例子包括，但不限于相变内存(PRAM)、静态随机存取存储器(SRAM)、动态随机存取存储器(DRAM)、其他类型的随机存取存储器(RAM)、只读存储器(ROM)、电可擦除可编程只读存储器(EEPROM)、快闪记忆体或其他内存技术、只读光盘只读存储器(CD‑ROM)、数字多功能光盘(DVD)或其他光学存储、磁盒式磁带，磁带磁磁盘存储或其他磁性存储设备或任何其他非传输介质，可用于存储可以被计算设备访问的信息。按照本文中的界定，计算机可读介质不包括暂存电脑可读媒体(transitory media)，如调制的数据信号和载波。
[0150] 还需要说明的是，术语“包括”、“包含”或者其任何其他变体意在涵盖非排他性的包含，从而使得包括一系列要素的过程、方法、商品或者设备不仅包括那些要素，而且还包括没有明确列出的其他要素，或者是还包括为这种过程、方法、商品或者设备所固有的要素。在没有更多限制的情况下，由语句“包括一个……”限定的要素，并不排除在包括要素的过程、方法、商品或者设备中还存在另外的相同要素。
[0151] 本领域技术人员应明白，本发明的实施例可提供为方法、系统或计算机程序产品。因此，本发明可采用完全硬件实施例、完全软件实施例或结合软件和硬件方面的实施例的形式。而且，本发明可采用在一个或多个其中包含有计算机可用程序代码的计算机可用存储介质（包括但不限于磁盘存储器、CD‑ROM、光学存储器等）上实施的计算机程序产品的形式。
[0152] 本发明可以在由计算机执行的计算机可执行指令的一般上下文中描述，例如程序模块。一般地，程序模块包括执行特定任务或实现特定抽象数据类型的例程、程序、对象、组件、数据结构等等。也可以在分布式计算环境中实践本发明，在这些分布式计算环境中，由通过通信网络而被连接的远程处理设备来执行任务。在分布式计算环境中，程序模块可以位于包括存储设备在内的本地和远程计算机存储介质中。
[0153] 本发明中的各个实施例均采用递进的方式描述，各个实施例之间相同相似的部分互相参见即可，每个实施例重点说明的都是与其他实施例的不同之处。尤其，对于系统实施例而言，由于其基本相似于方法实施例，所以描述的比较简单，相关之处参见方法实施例的部分说明即可。
[0154] 以上详细描述了本发明的较佳具体实施例。应当理解，本领域的普通技术人员无需创造性劳动就可以根据本发明的构思做出诸多修改和变化。因此，凡本技术领域中技术人员依本发明的构思在现有技术的基础上通过逻辑分析、推理或者有限的实验可以得到的技术方案，皆应在由权利要求书所确定的保护范围内。
