技术摘要：
本发明提供了一种避障轨迹规划方法、系统及车辆，应用于车辆，其中，所述方法综合考虑了动态障碍物与车辆第一候选轨迹上预设轨迹点之间的两个距离值、以及动态障碍物与车辆之间的合成速度因素的三元素数据，输入预设规避模型计算得到的威胁值，可以准确判断车辆是否会  全部
背景技术：
当前，自动驾驶技术已成为研究热点。 现阶段，自动驾驶技术主要包括感知层、规划层和控制层这三个组成部分。其中， 感知层用于感知车辆及车辆周边环境信息；规划层用于根据自车状况、周边环境信息以及 车辆所受约束、障碍物干扰、舒适性和轨迹长度等因素，通过优化所设计的权重函数，生成 一条由当前状态到期望状态的安全、舒适且可行驶的局部最优轨迹，作为参考信号输入到 控制层中；而控制层则用于根据规划层所提供的局部最优轨迹，控制车辆行驶。 但是，现有自动驾驶技术的轨迹规划中，将轨迹与速度分开规划，且其他车辆等障 碍物的运动状态可能随时会发生变化，从而导致所规划的轨迹变得不合理，无法有效适应 不断变化的实际驾驶环境，进而影响乘坐舒适性，甚至发生行车危险。
技术实现要素：
有鉴于此，本发明旨在提出一种避障轨迹规划方法、系统及车辆，以解决现有自动 驾驶技术的轨迹规划方法不够合理，造成所规划的轨迹无法有效适应不断变化的实际驾驶 环境的问题。 为达到上述目的，本发明的技术方案是这样实现的： 一种避障轨迹规划方法，应用于车辆，其中，所述方法包括： 基于车辆的当前行驶状态数据构建候选轨迹集，所述当前行驶状态数据包括车辆 当前质心位置坐标、车辆当前质心速度大小和车辆当前质心运行方向，所述候选轨迹集包 括多个第一候选轨迹； 将所述候选轨迹集输入预设规避模型，得到预设规避模型输出的目标轨迹集；其 中，所述目标轨迹集包括多个第二候选轨迹，所述预设规避模型用于根据三元素数据，计算 所述各个第一候选轨迹的威胁值，并将所述候选轨迹集中威胁值大于预设值的第一候选轨 迹筛除，得到目标轨迹集； 从所述目标轨迹集中选取一条第二候选轨迹作为目标轨迹； 按照所述目标轨迹控制所述车辆运行； 其中，所述三元素包括第一候选轨迹上预设轨迹点到距离最近点之间的第一直线 距离、潜在碰撞点到所述距离最近点的第二直线距离、动态障碍物的质心速度与所述预设 轨迹点处对应的车辆规划质心速度的速度矢量和。 进一步地，所述的方法中，所述动态障碍物为车辆通过车辆传感器当前监测到的 障碍物；所述距离最近点为合成轮廓按所述速度矢量和运行时，所生成的合成轨迹上距离 所述预设轨迹点最近的点；所述合成轮廓为将车辆外径叠加到所述动态障碍物上所形成的 轮廓；所述潜在碰撞点为参考直线与所述合成轮廓的交点中靠近所述最近碰撞点的点，所 5 CN 111591307 A 说　明　书 2/23 页 述参考直线为经过所述最近碰撞点且与所述速度矢量和的方向平行的直线。 进一步地，所述的方法中，所述预设规避模型包括威胁值确定子模型；所述将所述 候选轨迹集输入预设规避模型，得到预设规避模型输出的目标轨迹集，包括： 获取所述三元素数据； 将所述三元素数据输入所述威胁值确定子模型，得到所述威胁值确定子模型输出 的所述各个第一候选轨迹的威胁值； 将所述威胁值大于预设值的第一候选轨迹从候选轨迹集中筛除，得到目标轨迹 集。 进一步地，所述的方法中，所述威胁值确定子模型包括： 公式： 其中，f为威胁值，w为所述第一直线距离，l为所述第二直线距离，vc为所述速度矢 量和，a、b为常数，且a<0，b>0。 进一步地，所述的方法中，所述威胁值确定子模型包括：公式： 其中，fm为威胁值，w为所述第一直线距离，l为所述第二直线距离，vc为所述速度矢 量和，a、α、b及γ均为常数，且[vc-aα≠0m/s]且-a(γ bα)>0。 进一步地，所述的方法中，在将所述候选轨迹集输入预设规避模型之前，预先对所 述动态障碍物进行威胁性判断，并忽略无碰撞威胁的障碍物。 进一步地，所述的方法中，所述预先对所述动态障碍物威胁性判断，并忽略无碰撞 威胁的障碍物，具体包括： 若针对动态障碍物的所述速度矢量和，与预设轨迹点相对于所述动态障碍物质心 的位置矢量之间的夹角大于90°，则确定所述动态障碍物为无碰撞威胁的障碍物，并将所述 动态障碍物忽略。 进一步地，所述的方法中，所述基于车辆的当前行驶状态数据构建候选轨迹集，包 括： 将所述当前行驶状态数据输入通过预设运动学模型，得到所述预设运动学模型输 出的预设时刻范围内各个时刻的轨迹点状态值； 根据所述预设时刻范围内各个时刻的轨迹点状态值，确定所述第一候选轨迹，并 得到候选轨迹集。 进一步地，所述的方法中，所述预设运动学模型包括： 公式Xh(t k)＝F(Xh(t k-1)) G·u(t k-1)、状态矩阵 控制矩阵 6 CN 111591307 A 说　明　书 3/23 页 控制变量u(t k-1)＝[ω(t k-1)a(t k-1)]T，其中，0≤k≤Np，在t时刻时， 为车辆当前位置的轨迹点状态值，且 其中，xh、yh分别为各个时刻的轨迹点在世界坐标系中的X轴坐标及Y轴坐标，Vh(t) ＝[xh(t)  yh(t)]T为车辆当前质心位置坐标，ω为车辆在对应时刻的质心角速度，a为车辆 在对应时刻的质心线加速度，vh为车辆在对应时刻的质心线速度，θh为车辆在对应时刻的质 心线速度方向与世界坐标系X轴正方向的夹角，Np为所述预设时刻范围的最大值，Δt为两 个相邻时刻之间的时间差，ωL及ωU分别表示车辆质心角速度的下限值与上限值，aL及aU分 别表示车辆质心线加速度的下限值与上限值， 及 分别表示车辆质心线速度的下限 值与上限值。 进一步地，所述的方法中，所述从所述目标轨迹集中选取一条第二候选轨迹作为 目标轨迹，包括： 将所述目标轨迹集中各个第二候选轨迹输入预设筛选模型，得到预设筛选模型输 出的目标轨迹；其中，所述预设筛选模型用于根据第二候选轨迹上的各个轨迹点的所述轨 迹点状态值及所述三元素数据，计算各个第二候选轨迹的筛选值，并将所述目标轨迹集中 筛选值符合预设筛选条件的第二候选轨迹，确定为所述目标轨迹。 进一步地，所述的方法中，所述预设筛选模型包括三元素分布子模型、舒适性筛选 子模型、有效行驶路径筛选子模型及参考偏离量筛选子模型中至少一种子模型，并将使所 述至少一种子模型的输出值之和达到最小的第二候选轨迹，确定为所述目标轨迹； 所述三元素分布子模型，用于根据所述第二候选轨迹上的各个轨迹点的所述三元 素数据，计算各个轨迹点所对应的所述第一直线距离的第一离散度、计算各个轨迹点所对 应的所述第二直线距离的第二离散度、以及计算各个轨迹点所对应的所述速度矢量和的第 三离散度，并将所述第一离散度按第一预设权重、所述第二离散度按第二预设权重与所述 第三离散度按第三预设权重求和，确定所述第二候选轨迹的总离散度； 所述舒适性筛选子模型，用于根据所述第二候选轨迹上的任意两个相邻时刻轨迹 点之间的路径矢量及车辆质心运行距离，计算各个时刻的轨迹点的舒适度，并将所述各个 时刻的轨迹点的舒适度按第四预设权重求和，确定所述第二候选轨迹的总舒适度； 所述有效行驶路径筛选子模型，用于根据所述第二候选轨迹上任意两相邻时刻轨 迹点之间的车辆质心运行距离及相对行驶轨迹，计算所述任意两相邻时刻轨迹点之间的有 效行驶路径评价值，并将所述任意两相邻时刻轨迹点之间的有效行驶路径评价值按第五预 设权重求和，确定所述第二候选轨迹的总有效行驶路径评价值；其中，所述相对行驶轨迹长 度为所车辆质心运行距离投影到道路坐标系下的长度； 所述参考偏离量筛选子模型，用于根据所述第二候选轨迹上各个时刻的轨迹点状 7 CN 111591307 A 说　明　书 4/23 页 态值及预设参考轨迹，计算所述各个时刻轨迹点与预设参考轨迹之间的偏离量评价值，并 将所述各个时刻轨迹点的偏离量评价值按第六预设权重求和，确定所述第二候选轨迹的总 偏离量评价值。 进一步地，所述的方法中，所述预设值为0。 本发明的另一目的在于提出一种避障轨迹规划系统，应用于车辆，其中，所述系统 包括： 轨迹集构建模块，用于基于车辆的当前行驶状态数据构建候选轨迹集，所述当前 行驶状态数据包括车辆当前质心位置坐标、车辆当前质心速度大小和车辆当前质心运行方 向，所述候选轨迹集包括多个第一候选轨迹； 规避模块，用于将所述候选轨迹集输入预设规避模型，得到预设规避模型输出的 目标轨迹集；其中，所述目标轨迹集包括多个第二候选轨迹，所述预设规避模型用于根据三 元素数据，计算所述各个第一候选轨迹的威胁值，并将所述候选轨迹集中威胁值大于预设 值的第一候选轨迹筛除，得到目标轨迹集； 选取模块，用于从所述目标轨迹集中选取一条第二候选轨迹作为目标轨迹； 控制模块，用于按照所述目标轨迹控制所述车辆运行； 其中，所述三元素包括第一候选轨迹上预设轨迹点到距离最近点之间的第一直线 距离、潜在碰撞点到所述距离最近点的第二直线距离、动态障碍物的质心速度与所述预设 轨迹点处对应的车辆规划质心速度的速度矢量和； 所述动态障碍物为车辆通过车辆传感器当前监测到的障碍物；所述距离最近点为 合成轮廓按所述速度矢量和运行时，所生成的合成轨迹上距离所述预设轨迹点最近的点； 所述合成轮廓为将车辆外径叠加到所述动态障碍物上所形成的轮廓；所述潜在碰撞点为参 考直线与所述合成轮廓的交点中靠近所述最近碰撞点的点，所述参考直线为经过所述最近 碰撞点且与所述速度矢量和的方向平行的直线。 进一步地，所述的系统中，所述预设规避模型包括威胁值确定子模型； 所述规避模块包括： 获取单元，用于获取所述三元素数据； 威胁值确定单元，用于将所述三元素数据输入所述威胁值确定子模型，得到所述 威胁值确定子模型输出的所述各个第一候选轨迹的威胁值； 第一筛选单元，用于将所述威胁值大于预设值的第一候选轨迹从候选轨迹集中筛 除，得到目标轨迹集。 进一步地，所述的系统中，所述威胁值确定子模型包括： 公式： 其中，f为威胁值，w为所述第一直线距离，l为所述第二直线距离，vc为所述速度矢 量和，a、b为常数，且a<0，b>0。 进一步地，所述的系统中，所述威胁值确定子模型包括： 公式： 其中，fm为威胁值，w为所述第一直线距离，l为所述第二直线距离，vc为所述速度矢 8 CN 111591307 A 说　明　书 5/23 页 量和，a、α、b及γ均为常数，且[vc-aα≠0m/s]且-a(γ bα)>0。 进一步地，所述系统还包括： 忽略模块，用于在将所述候选轨迹集输入预设规避模型之前，预先对所述动态障 碍物进行威胁性判断，并忽略无碰撞威胁的障碍物。 进一步地，所述的系统中，所述忽略模块，具体用于若针对动态障碍物的所述速度 矢量和，与预设轨迹点相对于所述动态障碍物质心的位置矢量之间的夹角大于90°，则确定 所述动态障碍物为无碰撞威胁的障碍物，并将所述动态障碍物忽略。 进一步地，所述的系统中，所述构建模块包括： 轨迹点状态值确定单元，用于将所述当前行驶状态数据输入通过预设运动学模 型，得到所述预设运动学模型输出的预设时刻范围内各个时刻的轨迹点状态值； 候选轨迹集确定单元，用于根据所述预设时刻范围内各个时刻的轨迹点状态值， 确定所述第一候选轨迹，并得到候选轨迹集。 进一步地，所述的系统中，所述预设运动学模型包括： 公式Xh(t k)＝F(Xh(t k-1)) G·u(t k-1)、状态矩阵 控制矩阵 控制变量u(t k-1)＝[ω(t k-1)a(t k-1)]T，其中，0≤k≤Np，在t时刻时， 为车辆当前位置的轨迹点状态值，且 其中，xh、yh分别为各个时刻的轨迹点在世界坐标系中的X轴坐标及Y轴坐标，Vh(t) ＝[xh(t)  yh(t)]T为车辆当前质心位置坐标，θh(t)为车辆当前质心线速度方向与世界坐标 系X轴正方向的夹角，vh(t)表示车辆当前质心线速度，ω为车辆在对应时刻的质心角速度， a为车辆在对应时刻的质心线加速度，vh为车辆在对应时刻的质心线速度，θh为车辆在对应 时刻的质心线速度方向与世界坐标系X轴正方向的夹角，Np为所述预设时刻范围的最大值， Δt为两个相邻时刻之间的时间差，ωL及ωU分别表示车辆质心角速度的下限值与上限值， aL及aU分别表示车辆质心线加速度的下限值与上限值， 及 分别表示车辆质心线速 度的下限值与上限值。 进一步地，所述的系统中，所述选取模块包括： 第二筛选单元，用于将所述目标轨迹集中各个第二候选轨迹输入预设筛选模型， 得到预设筛选模型输出的目标轨迹；其中，所述预设筛选模型用于根据第二候选轨迹上的 各个轨迹点的所述轨迹点状态值及所述三元素数据，计算各个第二候选轨迹的筛选值，并 9 CN 111591307 A 说　明　书 6/23 页 将所述目标轨迹集中筛选值符合预设筛选条件的第二候选轨迹，确定为所述目标轨迹。 进一步地，所述的系统中，所述预设筛选模型包括三元素分布子模型、舒适性筛选 子模型、有效行驶路径筛选子模型及参考偏离量筛选子模型中至少一种子模型，并将使所 述至少一种子模型的输出值之和达到最小的第二候选轨迹，确定为所述目标轨迹； 所述三元素分布子模型，用于根据所述第二候选轨迹上的各个轨迹点的所述三元 素数据，计算各个轨迹点所对应的所述第一直线距离的第一离散度、计算各个轨迹点所对 应的所述第二直线距离的第二离散度、以及计算各个轨迹点所对应的所述速度矢量和的第 三离散度，并将所述第一离散度按第一预设权重、所述第二离散度按第二预设权重与所述 第三离散度按第三预设权重求和，确定所述第二候选轨迹的总离散度； 所述舒适性筛选子模型，用于根据所述第二候选轨迹上的任意两个相邻时刻轨迹 点之间的路径矢量及车辆质心运行距离，计算各个时刻的轨迹点的舒适度，并将所述各个 时刻的轨迹点的舒适度按第四预设权重求和，确定所述第二候选轨迹的总舒适度； 所述有效行驶路径筛选子模型，用于根据所述第二候选轨迹上任意两相邻时刻轨 迹点之间的车辆质心运行距离及相对行驶轨迹，计算所述任意两相邻时刻轨迹点之间的有 效行驶路径评价值，并将所述任意两相邻时刻轨迹点之间的有效行驶路径评价值按第五预 设权重求和，确定所述第二候选轨迹的总有效行驶路径评价值；其中，所述相对行驶轨迹长 度为所车辆质心运行距离投影到道路坐标系下的长度； 所述参考偏离量筛选子模型，用于根据所述第二候选轨迹上各个时刻的轨迹点状 态值及预设参考轨迹，计算所述各个时刻轨迹点与预设参考轨迹之间的偏离量评价值，并 将所述各个时刻轨迹点的偏离量评价值按第六预设权重求和，确定所述第二候选轨迹的总 偏离量评价值。 进一步地，所述的系统中，所述预设值为0。 相对于在先技术，本发明所述的避障轨迹规划方法及系统具有以下优势： 在基于车辆的当前行驶状态数据构建候选轨迹集后，将所述候选轨迹集输入预设 规避模型，得到预设规避模型输出的目标轨迹集，然后从所述目标轨迹集中选取一条第二 候选轨迹作为目标轨迹，并按照所述目标轨迹控制所述车辆运行；其中，预设规避模型根据 三元素数据，计算所述各个第一候选轨迹的威胁值，并将所述候选轨迹集中威胁值大于预 设值的第一候选轨迹筛除，得到目标轨迹集。因为三元素综合了动态障碍物与第一候选轨 迹上预设轨迹点之间的距离、以及动态障碍物与车辆之间的速度因素，因而通过由上述三 元素数据输入预设规避模型计算得到的威胁值，可以准确判断车辆是否会在上述预设轨迹 点处与动态障碍物发生碰撞，也即可以将会发生碰撞的预设轨迹点对应的第一候选轨迹筛 除，从而得到安全、合理且可以适应不断变化的实际驾驶环境的第二候选轨迹。 本发明的再一目的在于提出一种车辆，其中，所述车辆包括所述的避障轨迹规划 系统。 所述车辆与上述一种避障轨迹规划方法、系统相对于现有技术所具有的优势相 同，在此不再赘述。 附图说明 构成本发明的一部分的附图用来提供对本发明的进一步理解，本发明的示意性实 10 CN 111591307 A 说　明　书 7/23 页 施例及其说明用于解释本发明，并不构成对本发明的不当限定。在附图中： 图1为本发明实施例所提出的避障轨迹规划方法的流程示意图； 图2为本发明实施例中将车辆及动态障碍物均简化为质点时的三元素分析示意 图； 图3为本发明实施例中考虑车辆及动态障碍物外部轮廓时的三元素分析示意图； 图4为本发明实施例中w与 的关系示意图； 图5为本发明实施例中w、l、vc三者的关系曲面图； 图6为本发明实施例中w-vc二者的关系曲线图； 图7为本发明实施例中w-l二者的关系曲线图； 图8为本发明实施例中l-vc二者的关系曲线图； 图9为本发明实施例中三元素分布示意图； 图10为本发明实施例中第二候选轨迹在道路坐标系上的车道偏离量计算原理图； 图11为本发明实施例所提出的避障轨迹规划系统的结构示意图。