CN117109620A - 基于采样的车辆行为与环境交互的自动驾驶路径规划方法 - Google Patents

Abstract

本发明涉及自动驾驶汽车技术领域，具体公开了一种基于采样的车辆行为与环境交互的自动驾驶路径规划方法，包括：获取车辆当前位置点和参考线上的每个离散点在笛卡尔坐标系下的位置信息，并从参考线上找出车辆当前位置点在参考线上的理想投影点；计算出车辆当前位置点在Frenet坐标系下的位置信息；计算出障碍物在Frenet坐标系下的位置信息；分别计算出车辆在Frenet坐标系下的最优横向运动轨迹和最优纵向运动轨迹，并进行合成，以生成车辆在Frenet坐标系下的最优自动驾驶路径，并对按照最优自动驾驶路径行驶的车辆进行障碍物碰撞检测。本发明提供的基于采样的车辆行为与环境交互的自动驾驶路径规划方法,能够提高自动驾驶路径的可行性和安全性。

Description

基于采样的车辆行为与环境交互的自动驾驶路径规划方法

技术领域

本发明涉及自动驾驶汽车技术领域，尤其涉及一种基于采样的车辆行为与环境交互的自动驾驶路径规划方法。

背景技术

汽车作为一种普遍的交通工具，已经在人类出行方式的变革中发挥了巨大的贡献。然而，由于规模庞大的汽车保有量，导致了交通负荷的增大，经常出现拥堵的交通状况。同时，不规范的驾驶操作也成为了交通事故频发的重要原因。因此，路径规划显得相对重要。自动驾驶汽车作为一种新型交通工具，具有很多优势。首先，自动驾驶汽车可以通过感知周围环境来获取行驶路线需要用到的信息，而这种感知能力比人类更为高效。其次，自动驾驶汽车可以根据预测的情况合理规划行驶路线，在行驶过程中可以遵循最优路径，从而避免拥堵和繁忙路段。最后，自动驾驶汽车可以预测周围车辆未来的行驶轨迹，并通过自身行驶轨迹的调整，提前规避潜在的风险和危险。近年来，对于车辆路径规划的研究主要有基于曲线拟合方法、基于图搜索方法、基于数值优化方法、基于采样方法等。其中基于曲线拟合方法，采用构造多簇曲率连续的曲线，以实现最优路径的选择。在实现过程中，该算法的计算复杂度较低，能够有效地规划路径。同时，该算法具有保证规划路径的曲率连续性和光滑性的优势，这可以使得车辆在行驶过程中更加平稳顺畅，但是这类算法构造的参数化曲线难以满足实际的动态化复杂环境。基于图搜索方法的核心思想是首先将规划空间预处理成包含障碍物信息的栅格地图，栅格地图中的节点表示状态空间中的离散状态，通过节点之间的连接实现状态的跳转。该算法相比于其他路径规划算法，基于图搜索的算法具有较高的求解稳定性，但是该算法找最优解的过程耗时多且无法满足局部路径规划的车辆动力学约束。基于数值优化方法，直接将目标函数和约束条件转化为一个凸优化的格式，利用二次规划解法算出其最优解，最终解的表征形式可以直接为一系列位置点。但是该算法有时不会快速的收敛到最优解，导致在某些帧求解速度缓慢，无法满足自动驾驶场景下的实时性要求，规划速度严重依赖于路点。基于采样方法，是在自动驾驶车辆行进的状态空间中进行密集采样，每一个样本代表一个状态值，状态值是一个向量，表示在某一时刻，轨迹规划输出轨迹的位置、速度、加速度、朝向等信息，然后根据这些参数对状态转移的过程进行插值，得到一个连续平滑过渡的曲线，该曲线连接了自动驾驶车辆的初始状态和末状态，同时也是运动轨迹的表征。基于该方法的算法目前得到了学术界和工业界的认可和深度的研究。

目前学术界对于基于采样的轨迹路径规划的研究已经有很大的进展，一些路径规划方法像RRT、PRM等算法已经成熟，并且可以被应用于复杂场景的处理中。针对交互信息的快速迭代要求算法对数据有较快的处理速度以及较高的稳定性，部分研究者提出在向目标点探索时，根据当前状态空间中节点的密度情况，自适应调整探索分辨率。这样可以使搜索更加细致，提高找到有效路径的几率，也可以避免节点密度不均导致计算复杂度过高的情况。同时借鉴了遗传算法的选择、交叉和变异机制，在沿着树的延伸方向生成节点的同时，结合障碍物信息进行避障。这种机制可以实现智能地绕过障碍物，避免碰撞，提高算法的稳定性。相对于没有考虑交互信息的方法，此规划路径的稳定性有着明显的提高。

已有实现方案如下：

(1)初始化：将起点作为根节点，建立一棵树T＝{V,E}。同时，生成一个随机点Qrand作为初始探索点；

(2)迭代：在每次迭代中，从树T中找到距离随机点Qrand最近的节点P，RRT算法会根据当前生成树中的节点数n，采用自适应调整的方式来确定延伸向量的长度δ，以保证在不同阶段下的探索分辨率得到了最佳的实现并筛选出一条延伸长度为δ的向量，得到延伸后的点Qnew，如果Qnew不在障碍物内，则将其加入树T中作为P的子节点，构建一条从P到Qnew的边；

(3)寻找可行路径：在生成树中寻找从起点到目标点的路径；

(4)优化路径：对于找到的路径进行平滑(如B样条平滑等)和精简(如Ramer-Douglas-Peucker算法等)操作。具体来说，可以设置一个适应性函数，对每一个路径进行评估，并根据评估结果，选取优秀的路径进行交叉和变异，生成新的路径，从而实现路径的优化；

非全局最优性：现有技术其搜索空间只包括了从根节点到目标节点的局部路径，而没有考虑全局信息。因此，搜索结果虽然能够找到一条可行的路径，但是其路径并不一定是全局最优或次优的路径。相对于车辆物理状态与道路结构这两个因素来说，环境交互信息与车辆行为特征都对未来的运动轨迹有较大的影响，但是目前对于这两项信息的研究相对薄弱。因此，如何更好的提取并处理交互信息和行为特征，是当前轨迹预测研究的重点与难点问题。

可能存在较多分支：现有方案在搜索过程中都会以随机的方式进行扩展，因此可能会生成大量无效的路径片段和分支。这会降低搜索效率，极大地增加路径规划的计算量，从而导致规划周期延长不能及时更新。

依赖参数的选择：现有技术中的诸多参数(如延伸向量的长度、树的生长速率等)对算法的效率和精度有着重要的影响。但是，在实际应用中，如何选择这些参数往往需要依据经验和领域知识，比较缺乏可靠的理论指导。

可能存在局部最小值问题：在搜索过程中，现有技术可能陷入到某些局部路径中，无法跳出被障碍物阻挡的局部最佳路径。这时，需要采取一些策略(如重启、增加探索范围、改变采样策略等方法)来改善算法的搜索效果。

对于非光滑、高噪声或动态障碍物的处理不够稳定：如果场景中存在非光滑、高噪声或者动态障碍物，现有技术可能会在概率空间中难以找到有效的最优路径，甚至产生错误的路径。这时需要结合领域专业知识，融合其他的路径规划算法或优化策略，以提高算法的稳定性和可行性。

发明内容

本发明的目的在于克服现有技术中存在的不足，提供了一种基于采样的车辆行为与环境交互的自动驾驶路径规划方法，以解决现实场景中复杂的环境交互信息以及车辆的状态信息由于考虑不足导致的规划轨迹不平滑、不安全、不舒适或者不满足车辆运动学约束与工作空间约束(如障碍物约束)以及规划频率长的问题。

作为本发明的第一个方面，提供一种基于采样的车辆行为与环境交互的自动驾驶路径规划方法，所述基于采样的车辆行为与环境交互的自动驾驶路径规划方法包括：

步骤S1：获取高精度地图中的路径规划参考线，并对所述路径规划参考线进行离散化以得到多个离散点，然后获取所述路径规划参考线上的每个离散点在笛卡尔坐标系下的位置信息；

步骤S2：根据车辆当前位置点在笛卡尔坐标系下的位置信息和所述路径规划参考线上的每个离散点在笛卡尔坐标系下的位置信息，从所述路径规划参考线上的多个离散点中筛选出与所述车辆当前位置点距离最近的最小匹配点，并根据所述路径规划参考线上的最小匹配点在笛卡尔坐标系下的位置信息计算出所述车辆当前位置点在所述路径规划参考线上的理想投影点；

步骤S3：根据所述车辆当前位置点在笛卡尔坐标系下的位置信息和所述路径规划参考线上的理想投影点在笛卡尔坐标系下的位置信息，计算出所述车辆当前位置点在Frenet坐标系下的位置信息；

步骤S4：获取障碍物在笛卡尔坐标系下的位置信息，依据所述障碍物在笛卡尔坐标系下的位置信息计算出所述障碍物在Frenet坐标系下的位置信息；

步骤S5：根据所述车辆当前位置点在Frenet坐标系下的位置信息和所述障碍物在Frenet坐标系下的位置信息，分别计算出车辆在Frenet坐标系下的最优横向运动轨迹和车辆在Frenet坐标系下的最优纵向运动轨迹；

步骤S6：对所述车辆在Frenet坐标系下的最优横向运动轨迹和车辆在Frenet坐标系下的最优纵向运动轨迹进行合成，以生成车辆在Frenet坐标系下的最优自动驾驶路径，并对按照所述最优自动驾驶路径行驶的车辆进行障碍物碰撞检测。

进一步地，所述步骤S1中，还包括：

根据所述路径规划参考线上的每个离散点在笛卡尔坐标系下的位置信息计算出相邻离散点之间的距离；

根据所述路径规划参考线上相邻离散点之间的距离，分别计算出所述路径规划参考线上除起始点之外的其他离散点到所述起始点的距离；

其中，所述每个离散点在笛卡尔坐标系下的位置信息包括横坐标、纵坐标、倾斜角、曲率和曲率导数。

进一步地，所述步骤S2中，还包括：

从所述路径规划参考线上的起始点开始遍历整条所述路径规划参考线上的离散点，根据所述路径规划参考线上的每个离散点到所述车辆当前位置点的距离从所述路径规划参考线上找到一个离所述车辆当前位置点最近的离散点，将该点标记为所述最小匹配点；

判断所述路径规划参考线上的最小匹配点是否为起始点或者末位点；若所述最小匹配点既不是起始点也不是末位点时，将所述最小匹配点前后的两个离散点分别标注为P₀点与P₁点；若所述最小匹配点为起始点时，将所述起始点和所述起始点的后一离散点分别标注为P₀点与P₁点；若所述最小匹配点为末位点时，将所述末位点的前一离散点和所述末位点分别标注为P₀点与P₁点；

分别构造P₀点到车辆当前位置点O的向量和P₀点到P₁点的向量/>计算向量/>的模长，并求出向量/>和向量/>的内积，最后用内积来除向量/>的模长，最终得到所述车辆当前位置点O在所述路径规划参考线上的理想投影点m；

分别计算出理想投影点m到P₀点的距离S_m，以及根据所述理想投影点m到P₀点的距离S_m和所述P₀点到所述起始点的距离计算出所述理想投影点m到所述起始点的距离S_r；

根据所述理想投影点m到所述起始点的距离S_r、P₀点到所述起始点的距离S_p0以及P₁点到所述起始点的距离S_p1计算出权重，权重weight的计算公式如下：

weight＝(S_r-S_p0)/(S_p1-S_p0)

根据权重weight计算出所述路径规划参考线上的理想投影点m在笛卡尔坐标系下的位置信息，其中，所述理想投影点m在笛卡尔坐标系下的位置信息包括横坐标、纵坐标、倾斜角、曲率和曲率导数，所述理想投影点m在笛卡尔坐标系下的横坐标x的计算公式如下：

x＝(1-weight)*P_0.x+weight*P_1.x

其中，P_0.x为P₀点在笛卡尔坐标系下的横坐标，P_1.x为P₁点在笛卡尔坐标系下的横坐标；所述理想投影点在笛卡尔坐标系下的纵坐标、倾斜角、曲率和曲率导数的计算公式同理。

进一步地，所述步骤S3中，还包括：

所述车辆当前位置点在Frenet坐标系下的位置信息包括Frenet纵坐标s、Frenet纵坐标对时间的导数Frenet纵坐标对时间的二阶导/>Frenet横坐标d、Frenet横坐标对纵向坐标的导数d′以及Frenet横坐标对纵向坐标的二阶导d″，计算公式分别如下：

其中，S_r为理想投影点到起始点的距离；为车辆当前位置点在笛卡尔坐标系下的坐标；υ_x为车辆当前位置点在笛卡尔坐标系下的线速度，其中/>θ_x为车辆当前位置点在笛卡尔坐标系下的倾斜角；θ_r为理想投影点在笛卡尔坐标系下的倾斜角；k_r为理想投影点在笛卡尔坐标系下的行驶曲率，其中/>k_x为车辆当前位置点在笛卡尔坐标系下的行驶曲率，其中/>a_x为车辆当前位置点在笛卡尔坐标系下的加速度，其中dx＝x-rx，dy＝y-ry，其中x、y分别为车辆当前位置点在笛卡尔坐标系下的横坐标、纵坐标，rx、ry分别为理想投影点在笛卡尔坐标系下的横坐标、纵坐标；

建立车辆当前位置点在Frenet坐标系下的横坐标d关于时间t的函数d(t)，以及建立车辆当前位置点在Frenet坐标系下的纵坐标s关于时间t的函数s(t)；其中，函数d(t)和函数s(t)分别为车辆行驶过程中的横向运动分量和纵向运动分量。

进一步地，所述步骤S4中，还包括：

获取所述障碍物在笛卡尔坐标系下的位置信息以及所述障碍物的长度和宽度，其中，所述障碍物在笛卡尔坐标系下的位置信息包括横坐标、纵坐标和旋转角；

将所述障碍物的四个顶点在笛卡尔坐标系下的位置信息转换为该四个顶点在Frenet坐标系下的位置信息。

进一步地，所述步骤S5中，还包括：

(1)采用五次多项式对车辆在Frenet坐标系下的横向运动轨迹和纵向运动轨迹进行规划，五次多项式f(t)的公式如下：

f(t)＝c₀+c₁t+c₂t²+c₃t³+c₄t⁴+c₅t⁵

其中，通过确定车辆在Frenet坐标系下的运动初始状态f(t_s)及其一阶微分和二阶微分/>以及车辆在Frenet坐标系下的运动结束状态f(t_e)及其一阶微分/>和二阶微分/>来求解车辆在整个运动过程中的五次多项式模型参数，具体为：

化成矩阵形式为：

令：

得到：C＝M^-10

(2)对于车辆在Frenet坐标系下的横向运动轨迹规划：车辆横向运动的建模本质是对车辆行驶过程中的横向运动分量d(t)进行建模，根据车辆在Frenet坐标系中的运动初始状态和运动结束状态计算出在该时间段内平滑连续的运动轨迹；假设自动驾驶车辆从T_s时刻开始启动自动驾驶功能，并在T_e时刻结束，在[T_s,T_e]这段规划时间内，为了实现安全和高效的行驶，车辆需要根据实时变化的场景和周围动态或静态障碍物进行轨迹规划；所以在采样周期t∈[T_s,T_e]，根据车辆在Frenet坐标系下的运动初始时刻T_s位置信息和车辆在Frenet坐标系下的运动结束时刻T_e位置信息，并结合五次多项式拟合式C＝M^-10，得到车辆在该采样周期t内的横向运动轨迹，即：

其中，所述车辆在Frenet坐标系下的运动初始时刻T_s位置信息包括Frenet横坐标d(T_s)、Frenet横坐标对纵坐标的导数和Frenet横坐标对纵坐标的二阶导/>所述车辆在Frenet坐标系下的运动结束时刻T_e位置信息包括Frenet横坐标d(T_e)、Frenet横坐标纵坐标的导数/>和Frenet横坐标对纵坐标的二阶导/>

根据当前采样时刻T_i车辆所在的初始位置，对下一采样时刻T_i+1车辆可能出现的覆盖区域进行采样，因此在t∈[T_i,T_i+1]时刻，对横向运动分量d(t)的始末状态进行建模得：

因为下一采样时刻T_i+1车辆可能所在的末位置都是互相独立的，所以每个采样周期内由初始位置到末位置有多个候选横向运动轨迹，在横向轨迹规划中将采样频率用f_M进行表示，所以每一次采样都是在1/f_M秒内，每个采样周期都将重新进行一次轨迹规划，每次轨迹规划后均得到一条五次多项式的运动轨迹，在每一次轨迹规划时，以当前采样时刻的车辆所在位置作为初始位置，通过对末位置进行采样，得到多个候选横向运动轨迹，根据横向运动轨迹的评估目标函数在多个候选横向运动轨迹中选择一个最优横向运动轨迹，并根据最优横向运动轨迹得到此次轨迹规划的车辆所在末位置，并将其作为下一次轨迹规划的初始位置，通过不断迭代和计算，最终得到整个规划周期[T_s,T_e]内的完整横向运动轨迹；

(3)对于车辆在Frenet坐标系下的纵向运动轨迹规划：对于车辆的纵向规划也是按照所述五次多项式f(t)进行轨迹表示，则初始时刻T_s的纵向运动状态分别为s(T_s)、结束时刻T_e的纵向运动状态分别为s(T_e)、/> 根据五次多项式的纵向运动模型可得：

D＝P^-1Q

其中D＝(c₀,c₁,c₂,c₃,c₄,c₅)^T

其中，s(T_s)为车辆在初始时刻T_s的Frenet纵坐标，为车辆在初始时刻T_s的Frenet纵坐标对时间的导数，/>为车辆在初始时刻T_s的Frenet纵坐标对时间的二阶导，s(T_e)为车辆在结束时刻T_e的Frenet纵坐标，/>为车辆在结束时刻T_e的Frenet纵坐标对时间的导数，/>为车辆在结束时刻T_e的Frenet纵坐标对时间的二阶导；

将纵向轨迹规划的期望末状态定义为S_a，给出期望的末状态运动参数，自动驾驶车辆在不同场景下运动，影响每一次规划末状态的期望值，因而不同场景下生成的纵向轨迹集合不同，在规划时间内，对车辆纵向运动的始末状态空间采样，即t∈[T_i,T_i+1]，对纵向运动分量s(t)的始末状态进行建模得：

由于纵向运动具有期望的状态值S_a，所以将纵向轨迹规划末状态离散化采样，不同于横向采用时间频率，纵向使用位移大小来确定运动轨迹，因此末状态更新为：

因为下一采样位移车辆可能所在的末位置都是互相独立的，所以由初始位置到下一个采样位移的末位置有多个候选轨迹；在纵向轨迹规划中将位移大小用Δs(T_i+1)进行表示；与横向轨迹规划相同，通过对末状态进行采样，得到多个候选纵向运动轨迹，根据纵向运动轨迹的评估目标函数在多个候选纵向运动轨迹中选择一个最优纵向运动轨迹，通过不断迭代，可以得到整个规划周期[T_s,T_e]内的完整纵向运动轨迹。

进一步地，所述横向运动轨迹的评估目标函数cost_d的计算过程包括：

横向加加速度：由于表示车辆运动过程中的横向加加速度，是描述车辆运动过程中的舒适程度，在规划周期[T_s,T_e]内用/>平方的积分大小来评判所述横向运动轨迹的质量，所述横向运动轨迹质量的评估函数j_t(d(t))的计算公式如下：

横向偏移量：为了让车辆尽可能的靠近车道中心线行驶，设置横向偏移量的评估函数h(de)，用来评判车辆到车道中心线的横向偏移，每一次抽样都计算车辆到车道中心线距离的平方，再求出该规划周期[T_s,T_e]内的平方和：

车辆距离障碍物距离：作为安全保护，在横向轨迹规划途中要距离障碍物位置相对较远，车辆距离障碍物距离的评估函数用C_OBS-T表示；

横向加速度：用来评估车辆换道的平稳程度，所述车辆换道平稳程度的评估函数a_t(d(t))的计算公式如下：

向心加速度：向心加速度的评估函数a(ω(t))的计算公式如下：

以上就是横向运动轨迹的评估目标函数cost_d的五个属性，再分别为其根据不同的应用场景配置适当的权重参数ζ_j、ζ_h、ζ_c、ζ_at、ζ_aω，所以得到所述横向运动轨迹的评估目标函数cost_d为：

cost_d＝ζ_jj_t(d(t))+ζ_hh(de)+ζ_cC_OBS-T+ζ_ata_t(d(t))+ζ_aωa(d(t),ω(t))。

进一步地，所述纵向运动轨迹的评估目标函数cost_s的计算过程包括：

纵向加加速度：由于表示车辆运动过程中的纵向加加速度，是描述车辆运动过程中的舒适程度，在规划周期[T_s,T_e]内用/>的积分大小来评判所述纵向运动轨迹的质量，所述纵向运动轨迹质量的评估函数j_t(s(t))的计算公式如下：

车辆距离障碍物距离：作为安全保护，在纵向轨迹规划途中要距离障碍物位置相对较远，车辆距离障碍物距离的评估函数用C_OBS-S1表示；

车辆距离前车距离：作为安全保护，在纵向轨迹规划途中要距离前车位置相对较远，车辆距离前车距离的评估函数用C_OBS-S2表示；

规划实际末状态S_e与期望末状态S_a的接近程度：用来表示本次规划的完成情况，如果实际与期望相差较大则说明规划失误，用h(S_a,S_e)表示：

h(S_a,S_e)＝(S_a-S_e)²

以上就是所述纵向运动轨迹的评估目标函数cost_s的四个属性，再分别为其根据不同的应用场景配置适当的权重参数所以得到所述纵向运动轨迹的评估目标函数cost_s为：

进一步地，所述对所述车辆在Frenet坐标系下的最优横向运动轨迹和车辆在Frenet坐标系下的最优纵向运动轨迹进行合成，以生成车辆在Frenet坐标系下的最优自动驾驶路径，还包括：

所述完整横向运动轨迹集合d_set如下：

d_set＝ψ(t,d_s,d_e),t＝{t₀,t₁,t₂,...,t_N-1}

式中，ψ(t,d_s,d_e)表示生成的横向运动轨迹集合，共有N个时间采样点；ψ(t,d_s,d_e)表示从初始状态d_s转移到末状态d_e；

同理，所述完整纵向运动轨迹集合s_set为：

s_set＝Ω(t,s_s,s_e),t＝{t₀,t₁,t₂,...,t_N-1}

式中，Ω(t,s_s,s_e)表示生成的纵向运动轨迹集合；共有N个时间采样点；Ω(t,s_s,s_e)表示从初始状态s_s转移到末状态s_e；

在横纵向始末状态采样并生成横纵向运动轨迹集合后，在某一时刻t对两个集合中的取值进行一一对应，在某一时刻t，通过匹配完整横向运动轨迹集合d_set和完整纵向运动轨迹集合s_set中的取值，来确定横纵向轨迹的对应关系，这样可以获得一组相互匹配的横纵向轨迹，形成最终的路径轨迹集合Π如下:

Π＝Traj(t,d_s,d_e,s_s,s_e)。

进一步地，所述对按照所述最优自动驾驶路径行驶的车辆进行障碍物碰撞检测，还包括：

采用包围盒碰撞检测法，通过使用边界框来近似表示物体的包围盒，然后判断这些包围盒是否相交来确定是否发生碰撞，边界框是一个立方体或长方体，其可以由最小点和最大点来定义，分别表示包围盒的最小和最大顶点坐标；

首先用边界框来表示障碍物与行驶环境中的车辆，对于自车车辆和其他物体检测包围盒是否相交；如果自车车辆的包围盒与其他物体的包围盒相交，那么可能会发生碰撞；否则，它们之间不存在碰撞；其中，如果发生相交则进行投影再检查：通过检查每个轴上的投影是否有重叠来实现；

在Frenet坐标系中，比较两个边界框在d轴上的投影是否有重叠；如果自车的d轴投影的最大值小于其他物体的d轴投影的最小值，或者自车d轴投影的最小值大于其他物体d轴投影的最大值，则两个边界框在d轴上没有重叠；然后，对s轴执行相同的检查；如果在所有轴上都存在重叠，那么两个边界框相交，可以判断自车车辆和其他物体可能会发生碰撞；再返回至横纵向采样规划，进行重新规划。

本发明提供的基于采样的车辆行为与环境交互的自动驾驶路径规划方法具有以下优点：

(1)适用于动态环境：可以在动态环境中进行路径规划。通过将路径分解为离散的格点和时间步长，算法可以对动态障碍物进行建模和考虑，从而生成安全可行的路径；

(2)车辆动力学约束：可以考虑车辆的动力学约束，例如车辆的最大加速度、向心加速度等。这样可以确保生成的路径符合车辆的运动特性，提高路径的可行性和安全性；

(3)路径安全性和最优性：使用成本函数对路径进行优化。损失函数可以包括安全性和最优性的考虑，使得生成的路径在满足约束条件的同时，具有良好的避障功能和最优性；

(4)节省计算资源：将三维轨迹分解为两个二维轨迹(纵向和横向)可以降低路径规划的复杂度，从而节省计算资源和计算时间。通过分解问题，可以将复杂的三维路径规划问题简化为两个独立的二维路径规划问题。

附图说明

附图是用来提供对本发明的进一步理解，并且构成说明书的一部分，与下面的具体实施方式一起用于解释本发明，但并不构成对本发明的限制。

图1为本发明提供的基于采样的车辆行为与环境交互的自动驾驶路径规划方法的流程图。

图2为本发明提供的基于采样的车辆行为与环境交互的自动驾驶路径规划方法的具体实施方式流程图。

图3为本发明提供的基于采样的车辆行为与环境交互的自动驾驶路径规划方法的原理图。

图4为本发明提供的离散化参考线的示意图。

图5为本发明提供的以车辆当前位置寻找参考线上的理想投影点的示意图。

图6为本发明提供的车辆位置在Frenet坐标系下的位置信息向量表达示意图。

图7为本发明提供的障碍物检测最小矩阵框模型示意图。

图8为本发明提供的障碍物映射至车辆行驶路线的ST图示意图。

图9为本发明提供的横纵向采样轨迹集合生成最终轨迹的方法流程图。

具体实施方式

需要说明的是，在不冲突的情况下，本发明中的实施例及实施例中的特征可以相互结合。下面将参考附图并结合实施例来详细说明本发明。

为了使本领域技术人员更好地理解本发明方案，下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分的实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都应当属于本发明保护的范围。

需要说明的是，本发明的说明书和权利要求书及上述附图中的术语“第一”、“第二”等是用于区别类似的对象，而不必用于描述特定的顺序或先后次序。应该理解这样使用的数据在适当情况下可以互换，以便这里描述的本发明的实施例。此外，术语“包括”和“具有”以及他们的任何变形，意图在于覆盖不排他的包括，例如，包含了一系列步骤或单元的过程、方法、系统、产品或设备不必限于清楚地列出的那些步骤或单元，而是可包括没有清楚地列出的或对于这些过程、方法、产品或设备固有的其它步骤或单元。

在本实施例中提供了一种基于采样的车辆行为与环境交互的自动驾驶路径规划方法，图1为本发明提供的基于采样的车辆行为与环境交互的自动驾驶路径规划方法的流程图。如图1所示，同时参考图2，所述基于采样的车辆行为与环境交互的自动驾驶路径规划方法包括：

步骤S1：获取高精度地图中的路径规划参考线，并对所述路径规划参考线进行离散化以得到多个离散点，然后获取所述路径规划参考线上的每个离散点在笛卡尔坐标系下的位置信息；

优选地，如图4所示，所述步骤S1中，还包括：

根据所述路径规划参考线上的每个离散点在笛卡尔坐标系下的位置信息计算出相邻离散点之间的距离；

根据所述路径规划参考线上相邻离散点之间的距离，分别计算出所述路径规划参考线上除起始点之外的其他离散点到所述起始点的距离；其中，通过计算每一个离散点距离前一个离散点的距离，做一个距离的累加，最后计算出整一条曲线上的距离以及每一个离散点距离起始点的距离；

其中，所述每个离散点在笛卡尔坐标系下的位置信息包括横坐标、纵坐标、倾斜角、曲率和曲率导数。

需要说明的是，所述每个离散点在笛卡尔坐标系下的位置信息通过高精度地图获得。

步骤S2：根据车辆当前位置点在笛卡尔坐标系下的位置信息和所述路径规划参考线上的每个离散点在笛卡尔坐标系下的位置信息，从所述路径规划参考线上的多个离散点中筛选出与所述车辆当前位置点距离最近的最小匹配点，并根据所述路径规划参考线上的最小匹配点在笛卡尔坐标系下的位置信息计算出所述车辆当前位置点在所述路径规划参考线上的理想投影点；

优选地，如图5所示，所述步骤S2中，还包括：

从所述路径规划参考线上的起始点开始遍历整条所述路径规划参考线上的离散点，根据所述路径规划参考线上的每个离散点到所述车辆当前位置点的距离从所述路径规划参考线上找到一个离所述车辆当前位置点最近的离散点，将该点标记为所述最小匹配点min；

需要说明的是，采用欧式距离的平方进行计算，将起始点作为索引0点，一直标注至最后一个点，以找到一个离所述车辆当前位置点O最近的离散点min：

其中，dx指的是两点之间x轴向的距离，dy指的是两点之间y轴向的距离，point.x()指的是某一离散点的x轴坐标，point.y()指的是某一离散点的y轴坐标，x指的是车辆当前位置点O的x轴坐标，y指的是车辆当前位置点O的y轴坐标；

判断所述路径规划参考线上的最小匹配点min是否为起始点或者末位点，即最小匹配点min的索引值不是0，即不是起始点，且最小匹配点min的索引值加一不是参考线点的总个数，即不是末位点；若所述最小匹配点既不是起始点也不是末位点时，将所述最小匹配点前后的两个离散点分别标注为P₀点与P₁点；若所述最小匹配点为起始点时，将所述起始点和所述起始点的后一离散点分别标注为P₀点与P₁点；若所述最小匹配点为末位点时，将所述末位点的前一离散点和所述末位点分别标注为P₀点与P₁点；

分别构造P₀点到车辆当前位置点O的向量和P₀点到P₁点的向量/>其中/>可以看作此时车辆在参考线上的行驶方向，计算向量/>的模长v1_norm，并求出向量/>和向量的内积，最后用内积来除向量/>的模长，最终得到所述车辆当前位置点O在所述路径规划参考线上的理想投影点m；

分别计算出理想投影点m到P₀点的距离S_m，以及根据所述理想投影点m到P₀点的距离S_m和所述P₀点到所述起始点的距离计算出所述理想投影点m到所述起始点的距离S_r；

根据所述理想投影点m到所述起始点的距离S_r、P₀点到所述起始点的距离S_p0以及P₁点到所述起始点的距离S_p1计算出权重，权重weight的计算公式如下：

weight＝(S_r-S_p0)/(S_p1-S_p0)

根据权重weight计算出所述路径规划参考线上的理想投影点m在笛卡尔坐标系下的位置信息，其中，所述理想投影点m在笛卡尔坐标系下的位置信息包括横坐标、纵坐标、倾斜角、曲率和曲率导数，所述理想投影点m在笛卡尔坐标系下的横坐标x的计算公式如下：

x＝(1-weight)*P_0.x+weight*P_1.x

其中，P_0.x为P₀点在笛卡尔坐标系下的横坐标，P_1.x为P₁点在笛卡尔坐标系下的横坐标；所述理想投影点在笛卡尔坐标系下的纵坐标、倾斜角、曲率和曲率导数的计算公式同理。

步骤S3：根据所述车辆当前位置点在笛卡尔坐标系下的位置信息和所述路径规划参考线上的理想投影点在笛卡尔坐标系下的位置信息，计算出所述车辆当前位置点在Frenet坐标系下的位置信息；

优选地，如图6所示，基于Frenet坐标系进行轨迹规划，输出Frenet坐标系下单独的横、纵向规划轨迹，通过传递两个不同坐标系下的状态信息，计算出对应的车辆当前位置点在Frenet坐标系下的6个状态值d,d',d”,s,其中，

所述步骤S3中，还包括：

所述车辆当前位置点在Frenet坐标系下的位置信息包括Frenet纵坐标s、Frenet纵坐标对时间的导数Frenet纵坐标对时间的二阶导/>Frenet横坐标d、Frenet横坐标对纵向坐标的导数d′以及Frenet横坐标对纵向坐标的二阶导d″，计算公式分别如下：

其中，S_r为理想投影点到起始点的距离；为车辆当前位置点在笛卡尔坐标系下的坐标；υ_x为车辆当前位置点在笛卡尔坐标系下的线速度，其中/>θ_x为车辆当前位置点在笛卡尔坐标系下的倾斜角；θ_r为理想投影点在笛卡尔坐标系下的倾斜角；k_r为理想投影点在笛卡尔坐标系下的行驶曲率，其中/>k_x为车辆当前位置点在笛卡尔坐标系下的行驶曲率，其中/>a_x为车辆当前位置点在笛卡尔坐标系下的加速度，其中dx＝x-rx，dy＝y-ry，其中x、y分别为车辆当前位置点在笛卡尔坐标系下的横坐标、纵坐标，rx、ry分别为理想投影点在笛卡尔坐标系下的横坐标、纵坐标；/>为理想投影点在笛卡尔坐标系下的坐标；/>理想投影点在笛卡尔坐标系下的线速度；理想投影点在笛卡尔坐标系下的加速度。

建立车辆当前位置点在Frenet坐标系下的横坐标d关于时间t的函数d(t)，以及建立车辆当前位置点在Frenet坐标系下的纵坐标s关于时间t的函数s(t)；其中，函数d(t)和函数s(t)分别为车辆行驶过程中的横向运动分量和纵向运动分量。

在本发实施例中，根据上述cartesian_frenet转换公式将点在笛卡尔坐标系下的位置信息转化为在Frenet坐标系下的位置信息，其具体转化过程如下：

(1)首先计算d和s；

d的大小即理想投影点与自车位置点的欧式距离，为： 并且d的方向就是过理想投影点的切向量与自车位置点的叉乘方向。而cosθ_r与sinθ_r就是得到了理想投影点此时的切向量，所以d的方向为:(cosθ_r i,sinθ_rj)×(dx i,dyj)＝cosθ_r*dy-sinθ_r*dx。若这个值小于零则在左侧，若这个值大于零则在右侧。从而得到了d的方向和大小。再理想投影点到起始点的距离s_r给到s，即求出了Frenet中的d和s值。即：

(2)再求出和d'；

求(根据υ_x与/>定义，有/>和/>)，由/>可得： 两边同时对时间t求导可得到：

其中，为Frenet坐标下理想投影点的纵轴单位向量；/>为Frenet坐标下理想投影点的横轴单位向量；/>为Frenet坐标下自车位置点的纵轴单位向量；/>为Frenet坐标下自车位置点的横轴单位向量；

由Frenet公式：于是有/>代入上式可得：

再由与/>正交可得：/>代入上式可得：

/>

求由/>对等式两边同时求导可得：同时左乘/>得到/>整理可得：

求d'，将/>与/>代入得到：

d'＝(1-k_rd)tan(θ_x-θ_r)

(3)最后求出d'‘和

由d'＝(1-k_rd)_tan(θ_x-θ_r)，可得

有曲率：又有/>代入原式得：

由整理得：

以上得到了由cartesian坐标中已知信息转化为Frenet坐标的过程。将车辆自动驾驶车辆当前时刻位置信息经过一系列的计算，求出自车位置点在Frenet坐标系下的d,d',d”,s,在Frenet坐标系下进行运算，为后续的步骤提供便利。

步骤S4：获取障碍物在笛卡尔坐标系下的位置信息，依据所述障碍物在笛卡尔坐标系下的位置信息计算出所述障碍物在Frenet坐标系下的位置信息；

优选地，如图7-8所示，所述步骤S4中，还包括：

对于环境障碍物进行检测:环境目标模型的建立采用了5个参数进行描述：x、y、width、length和θ(旋转角)。其中，x和y表示障碍物在笛卡尔坐标系下相对于车辆的位置坐标，width和length表示障碍物的宽度和长度，θ表示障碍物的旋转角。为了简化模型，障碍物的雷达点云被投影到二维平面上，然后使用最小包络矩形算法计算得到包含旋转角的矩形目标边界。通过将x轴逆时针旋转，直到第一次与矩形的一条边重合，这条边被称为主边，其长度为width，x轴旋转的角度为矩形的旋转角θ。这样，利用这些参数可以准确描述目标物的位置信息、尺寸信息以及姿态信息，如图7所示，指的是障碍物在cartesian上的位置信息。再根据上述cartesian_frenet转换公式将其在cartesian上的信息转化为在Frenet坐标下的信息(将矩形的四个顶点以及中心点去转换，也是得到在Frenet坐标下的6个状态值)，最后将其映射到自车位置以及行驶轨迹上的ST图中，如图8所示。其中，根据汽车位置点在Frenet坐标下的纵坐标S以及时间T建立ST图；根据汽车位置点在Frenet坐标下的纵坐标S以及横坐标d建立Sd图；

步骤S4的具体实施步骤如下：

首先设置纵向规划步长和时间。再通过感知和预测模块获取该步长和时间内出现的障碍物。对此障碍物根据其出现时间顺序进行排列和编号。

用5个参数获取模型，将障碍物的雷达点云被投影到二维平面上，并使用最小包络矩形算法计算出带有旋转角度的矩形目标边界。通过将x轴逆时针旋转，直到与矩形的一条边重合，这条边被定义为主边，其长度为width，旋转的角度为矩形的旋转角度θ。

获取所述障碍物在笛卡尔坐标系下的位置信息以及所述障碍物的长度和宽度，其中，所述障碍物在笛卡尔坐标系下的位置信息包括横坐标、纵坐标和旋转角；

将所述障碍物的四个顶点在笛卡尔坐标系下的位置信息转换为该四个顶点在Frenet坐标系下的位置信息。

步骤S5：根据所述车辆当前位置点在Frenet坐标系下的位置信息和所述障碍物在Frenet坐标系下的位置信息，分别计算出车辆在Frenet坐标系下的最优横向运动轨迹和车辆在Frenet坐标系下的最优纵向运动轨迹；

优选地，如图3所示，所述步骤S5中，还包括：

在Frenet坐标下进行横向采样路径和纵向采样路径规划，横向采样路径可用于规划车辆的横向位置和车道变换，而纵向采样路径可用于规划车辆的速度和加速度。在作用上横纵向采样路径还可以结合车辆的动力学约束进行规划。通过在横向方向上进行采样，可以生成满足车辆转向半径、最大横向加速度等约束条件的路径。在纵向方向上进行采样，则可以生成满足车辆速度、加速度限制等约束条件的路径。这样可以确保生成的路径符合车辆的动力学特性，提高路径的可行性和安全性。根据获取的障碍物列表，通过在横向方向上进行采样，可以避开或绕过道路上的障碍物，确保车辆的安全行驶。在纵向方向上进行采样，则可以根据障碍物的位置和速度进行路径规划，避免与障碍物发生碰撞。具体过程如下：

(1)由于横纵向轨迹规划需要对纵向位移、车速和加速度进行约束。为了在保持计算效率的同时满足这些约束，本发明采用五次多项式对车辆在Frenet坐标系下的横向运动轨迹和纵向运动轨迹进行规划，五次多项式f(t)的公式如下：

f(t)＝c₀+c₁t+c₂t²+c₃t³+c₄t⁴+c₅t⁵

其中，在基于Frenet坐标系描述车辆运动状态时，在使用横向分量d(t)和纵向分量s(t)，它们分别是横坐标d关于时间t的函数和纵坐标S关于时间t的函数，并且必须满足连续平滑的条件。通过确定车辆在Frenet坐标系下的运动初始状态f(t_s)及其一阶微分和二阶微分/>以及车辆在Frenet坐标系下的运动结束状态f(t_e)及其一阶微分和二阶微分/>来求解车辆在整个运动过程中的五次多项式模型参数，具体为：

/>

化成矩阵形式为：

令：

得到：C＝M^-10

(2)对于车辆在Frenet坐标系下的横向运动轨迹规划：车辆横向运动的建模本质是对车辆行驶过程中的横向运动分量d(t)进行建模，它描述了车辆在Frenet坐标系下相对于参考线的横向位置。根据车辆在Frenet坐标系中的运动初始状态和运动结束状态计算出在该时间段内平滑连续的运动轨迹；假设自动驾驶车辆从T_s时刻开始启动自动驾驶功能，并在T_e时刻结束，在[T_s,T_e]这段规划时间内，为了实现安全和高效的行驶，车辆需要根据实时变化的场景和周围动态或静态障碍物进行轨迹规划；所以在采样周期t∈[T_s,T_e]，根据车辆在Frenet坐标系下的运动初始时刻T_s位置信息和车辆在Frenet坐标系下的运动结束时刻T_e位置信息，并结合五次多项式拟合式C＝M^-10，每一次规划结束再采样都解式，得到对应的参数C，便可以描述车辆在该采样周期t内的横向运动轨迹，即：

其中，所述车辆在Frenet坐标系下的运动初始时刻T_s位置信息包括Frenet横坐标d(T_s)、Frenet横坐标对纵坐标的导数和Frenet横坐标对纵坐标的二阶导/>所述车辆在Frenet坐标系下的运动结束时刻T_e位置信息包括Frenet横坐标d(T_e)、Frenet横坐标纵坐标的导数/>和Frenet横坐标对纵坐标的二阶导/>

根据当前采样时刻T_i车辆所在的初始位置，对下一采样时刻T_i+1车辆可能出现的覆盖区域进行采样，因此在t∈[T_i,T_i+1]时刻，对横向运动分量d(t)的始末状态进行建模得：

因为下一采样时刻T_i+1车辆可能所在的末位置都是互相独立的，所以每个采样周期内由初始位置到末位置有多个候选横向运动轨迹，在横向轨迹规划中将采样频率用f_M进行表示，所以每一次采样都是在1/f_M秒内，每个采样周期都将重新进行一次轨迹规划，每次轨迹规划后均得到一条五次多项式的运动轨迹，在每一次轨迹规划时，以当前采样时刻的车辆所在位置作为初始位置，通过对末位置进行采样，得到多个候选横向运动轨迹，根据横向运动轨迹的评估目标函数在多个候选横向运动轨迹中选择一个最优横向运动轨迹，并利用时间窗口1/f_M截取最优横向运动轨迹，并根据最优横向运动轨迹得到此次轨迹规划的车辆所在末位置，并将其作为下一次轨迹规划的初始位置，通过不断迭代和计算，最终得到整个规划周期[T_s,T_e]内的完整横向运动轨迹；

(3)对于车辆在Frenet坐标系下的纵向运动轨迹规划：对于车辆的纵向规划也是按照所述五次多项式f(t)进行轨迹表示，则初始时刻T_s的纵向运动状态分别为s(T_s)、结束时刻T_e的纵向运动状态分别为s(T_e)、/> 根据五次多项式的纵向运动模型可得：

D＝P^-1Q

其中

其中，s(T_s)为车辆在初始时刻T_s的Frenet纵坐标，为车辆在初始时刻T_s的Frenet纵坐标对时间的导数，/>为车辆在初始时刻T_s的Frenet纵坐标对时间的二阶导，s(T_e)为车辆在结束时刻T_e的Frenet纵坐标，/>为车辆在结束时刻T_e的Frenet纵坐标对时间的导数，/>为车辆在结束时刻T_e的Frenet纵坐标对时间的二阶导；

由感知和预测可获取不同的行驶场景，根据不同场景，纵向规划的末端状态也会不同。将纵向轨迹规划的期望末状态定义为S_a，给出期望的末状态运动参数，自动驾驶车辆在不同场景下运动，影响每一次规划末状态的期望值，因而不同场景下生成的纵向轨迹集合不同，在规划时间内，根据实际末状态S_e对车辆纵向运动的始末状态空间采样，即t∈[T_i,T_i+1]，对纵向运动分量s(t)的始末状态进行建模得：

由于纵向运动具有期望的状态值S_a，所以将纵向轨迹规划末状态离散化采样，不同于横向采用时间频率，纵向使用位移大小来确定运动轨迹，因此末状态更新为：

因为下一采样位移车辆可能所在的末位置都是互相独立的，所以由初始位置到下一个采样位移的末位置有多个候选轨迹；在纵向轨迹规划中将位移大小用Δs(T_i+1)进行表示；与横向轨迹规划相同，通过对末状态进行采样，得到多个候选纵向运动轨迹，根据纵向运动轨迹的评估目标函数在多个候选纵向运动轨迹中选择一个最优纵向运动轨迹，通过不断迭代，可以得到整个规划周期[T_s,T_e]内的完整纵向运动轨迹。

具体地，所述横向运动轨迹的评估目标函数cost_d的计算过程包括：

横向加加速度：由于表示车辆运动过程中的横向加加速度，是描述车辆运动过程中的舒适程度，在规划周期[T_s,T_e]内用/>平方的积分大小来评判所述横向运动轨迹的质量，所述横向运动轨迹质量的评估函数j_t(d(t))的计算公式如下：

横向偏移量：为了让车辆尽可能的靠近车道中心线行驶，设置横向偏移量的评估函数h(de)，用来评判车辆到车道中心线的横向偏移，每一次抽样都计算车辆到车道中心线距离的平方，再求出该规划周期[T_s,T_e]内的平方和：

车辆距离障碍物距离：作为安全保护，如果距离较近时会有较高的代价，在横向轨迹规划途中要距离障碍物位置相对较远，车辆距离障碍物距离的评估函数用C_OBS-T表示；

横向加速度：用来评估车辆换道的平稳程度，如果换道速度非常大则会导致行驶过程的不稳定，所述车辆换道平稳程度的评估函数a_t(d(t))的计算公式如下：

向心加速度：为了在转弯或调头的时候能够减速慢行，防止车辆侧倾，向心加速度的评估函数a(ω(t))的计算公式如下：

以上就是横向运动轨迹的评估目标函数cost_d的五个属性，再分别为其根据不同的应用场景配置适当的权重参数ζ_j、ζ_h、ζ_c、ζ_at、ζ_aω，所以得到所述横向运动轨迹的评估目标函数cost_d为：

cost_d＝ζ_jj_t(d(t))+ζ_hh(de)+ζ_cC_OBS-T+ζ_ata_t(d(t))+ζ_aωa(d(t),ω(t))。

具体地，所述纵向运动轨迹的评估目标函数cost_s的计算过程包括：

纵向加加速度：由于表示车辆运动过程中的纵向加加速度，是描述车辆运动过程中的舒适程度，在规划周期[T_s,T_e]内用/>的积分大小来评判所述纵向运动轨迹的质量，所述纵向运动轨迹质量的评估函数j_t(s(t))的计算公式如下：

/>

车辆距离障碍物距离：作为安全保护，在纵向轨迹规划途中要距离障碍物位置相对较远，如果距离较近时会有较高的代价，车辆距离障碍物距离的评估函数用C_OBS-S1表示；

车辆距离前车距离：作为安全保护，在纵向轨迹规划途中要距离前车位置相对较远，如果距离较近时会有较高的代价，车辆距离前车距离的评估函数用C_OBS-S2表示；

规划实际末状态S_e与期望末状态S_a的接近程度：用来表示本次规划的完成情况，如果实际与期望相差较大则说明规划失误，用h(S_a,S_e)表示：

h(S_a,S_e)＝(S_a-S_e)²

以上就是所述纵向运动轨迹的评估目标函数cost_s的四个属性，再分别为其根据不同的应用场景配置适当的权重参数所以得到所述纵向运动轨迹的评估目标函数cost_s为：

步骤S6：对所述车辆在Frenet坐标系下的最优横向运动轨迹和车辆在Frenet坐标系下的最优纵向运动轨迹进行合成，以生成车辆在Frenet坐标系下的最优自动驾驶路径，并对按照所述最优自动驾驶路径行驶的车辆进行障碍物碰撞检测。

优选地，如图9所示，所述对所述车辆在Frenet坐标系下的最优横向运动轨迹和车辆在Frenet坐标系下的最优纵向运动轨迹进行合成，以生成车辆在Frenet坐标系下的最优自动驾驶路径，还包括：

由上一步横纵向始末状态采样，可得到横纵向运动轨迹集合，由于两个轨迹集合中的曲线在时刻点上相互耦合，所以在某一时刻t中，可以对两个集合中的取值一一对应。生成合成的轨迹集合，在该集合内将每一条轨迹进行碰撞检测，挑选出最高效、最安全的轨迹作为该周期的轨迹输出。具体过程如下：

所述完整横向运动轨迹集合d_set如下：

d_set＝ψ(t,d_s,d_e),t＝{t₀,t₁,t₂,...,t_N-1}

式中，ψ(t,d_s,d_e)表示生成的横向运动轨迹集合，共有N个时间采样点；ψ(t,d_s,d_e)表示从初始状态d_s转移到末状态d_e；

同理，所述完整纵向运动轨迹集合s_set为：

s_set＝Ω(t,s_s,s_e),t＝{t₀,t₁,t₂,...,t_N-1}

式中，Ω(t,s_s,s_e)表示生成的纵向运动轨迹集合；共有N个时间采样点；Ω(t,s_s,s_e)表示从初始状态s_s转移到末状态s_e；

在横纵向始末状态采样并生成横纵向运动轨迹集合后，在某一时刻t对两个集合中的取值进行一一对应，在某一时刻t，通过匹配完整横向运动轨迹集合d_set和完整纵向运动轨迹集合s_set中的取值，来确定横纵向轨迹的对应关系，这样可以获得一组相互匹配的横纵向轨迹，形成最终的路径轨迹集合Π如下:

Π＝Traj(t,d_s,d_e,s_s,s_e)。

具体地，所述对按照所述最优自动驾驶路径行驶的车辆进行障碍物碰撞检测，还包括：

采用包围盒碰撞检测法，通过使用边界框来近似表示物体的包围盒，然后判断这些包围盒是否相交来确定是否发生碰撞，边界框是一个立方体或长方体，其可以由最小点和最大点来定义，分别表示包围盒的最小和最大顶点坐标；

首先用边界框来表示障碍物与行驶环境中的车辆，对于自车车辆和其他物体检测包围盒是否相交；如果自车车辆的包围盒与其他物体的包围盒相交，那么可能会发生碰撞；否则，它们之间不存在碰撞；其中，如果发生相交则进行投影再检查：通过检查每个轴上的投影是否有重叠来实现；

在Frenet坐标系中，比较两个边界框在d轴上的投影是否有重叠；如果自车的d轴投影的最大值小于其他物体的d轴投影的最小值，或者自车d轴投影的最小值大于其他物体d轴投影的最大值，则两个边界框在d轴上没有重叠；然后，对s轴执行相同的检查；如果在所有轴上都存在重叠，那么两个边界框相交，可以判断自车车辆和其他物体可能会发生碰撞；再返回至横纵向采样规划，进行重新规划。

在本发明实施例中，在车辆路径规划模块中通过将三维轨迹分解为纵向和横向两个二维轨迹，可以更容易地对轨迹进行建模和分析。纵向轨迹描述了车辆在纵向维度上的行为，包括位置和速度的变化。横向轨迹描述了车辆相对于纵向轨迹的横向位置，可以用来分析车辆的车道偏移和行驶路径。

在本发明实施例中，对轨迹进行评估并选择成本最低的轨迹，根据影响因素和权重创建一个成本函数，对生成的轨迹集合进行考量，来选择成本最低的轨迹。再根据不同场景自动调整比例权重来进行配对，使本发明的规划算法更加全面。

在本发明实施例中，对横纵向采样的迭代方式进行创新：横向通过规划频率，纵向通过迭代步长。在符合该末状态的区域内进行覆盖，从而囊括所有可能的路径以此选择出最佳路径。

在本发明实施例中，基于采样的车辆行为与环境交互的自动驾驶路径规划方法的原理，主要是将三维问题通过建立基于状态空间采样的自动驾驶车辆横向和纵向轨迹规划模型，转化为两个二维问题，以获取车辆运动状态分布。通过求解横向和纵向轨迹规划方程，得到一组可行性运动轨迹。此外，构建了自动驾驶车辆横向和纵向轨迹质量评估模型，用于评估轨迹的优劣。通过该模型，生成了在不同场景下的横向和纵向最优轨迹。并对生成轨迹进行碰撞检测，筛选出合理、安全、符合各种现实约束的轨迹。

需要说明的是，成本函数(Cost Function)是用于衡量路径或决策的质量或效用的函数。在路径规划中，成本函数被用来评估不同路径的优劣，并选择具有最低成本的路径作为最佳路径。成本函数可以考虑多种因素，如路径的长度、时间、安全性、平滑性、能耗等。通过设定不同因素的权重，可以调整成本函数以适应具体需求。

需要说明的是，Frenet坐标(Frenet Coordinates)是一种车辆轨迹描述方式，用于将车辆在道路上的运动分解为纵向和横向两个方向。它是一种局部坐标系，以车辆当前位置为原点，纵向方向表示车辆沿着道路行驶的方向，横向方向表示车辆相对于道路中心线的侧向偏移。通过将三维轨迹分解为纵向和横向分量，可以更容易地对轨迹进行建模和分析，并进行路径规划。

需要说明的是，碰撞检测(Collision Detection)用于确定两个或多个对象是否相交或碰撞。目的是检测并预防物体在运动过程中发生不可接受的重叠或相互穿越。它可以用来防止物体之间的碰撞、避免交通事故、确保合适的物理模拟等。

本发明提供的基于采样的车辆行为与环境交互的自动驾驶路径规划方法，结合了车辆行为特征与环境交互信息进行车辆行驶轨迹规划，以自动驾驶汽车为对象，使用横纵向分开采样，并用五次多项式进行连接生成横纵向偏移距离随时间变化的曲线。将三维S-d-T空间求解最佳路线问题转化为两个二维S-d和S-T空间分别求解轨迹，并且构造cost函数根据不同的属性加以权重得到最优路径。横纵向曲线拟合成一条候选轨迹。最后进行碰撞检测，确保规划路径不会碰撞到障碍物。

可以理解的是，以上实施方式仅仅是为了说明本发明的原理而采用的示例性实施方式，然而本发明并不局限于此。对于本领域内的普通技术人员而言，在不脱离本发明的精神和实质的情况下，可以做出各种变型和改进，这些变型和改进也视为本发明的保护范围。

Claims (10)

1.一种基于采样的车辆行为与环境交互的自动驾驶路径规划方法，其特征在于，所述基于采样的车辆行为与环境交互的自动驾驶路径规划方法包括：

步骤S1：获取高精度地图中的路径规划参考线，并对所述路径规划参考线进行离散化以得到多个离散点，然后获取所述路径规划参考线上的每个离散点在笛卡尔坐标系下的位置信息；

步骤S2：根据车辆当前位置点在笛卡尔坐标系下的位置信息和所述路径规划参考线上的每个离散点在笛卡尔坐标系下的位置信息，从所述路径规划参考线上的多个离散点中筛选出与所述车辆当前位置点距离最近的最小匹配点，并根据所述路径规划参考线上的最小匹配点在笛卡尔坐标系下的位置信息计算出所述车辆当前位置点在所述路径规划参考线上的理想投影点；

步骤S3：根据所述车辆当前位置点在笛卡尔坐标系下的位置信息和所述路径规划参考线上的理想投影点在笛卡尔坐标系下的位置信息，计算出所述车辆当前位置点在Frenet坐标系下的位置信息；

步骤S4：获取障碍物在笛卡尔坐标系下的位置信息，依据所述障碍物在笛卡尔坐标系下的位置信息计算出所述障碍物在Frenet坐标系下的位置信息；

步骤S5：根据所述车辆当前位置点在Frenet坐标系下的位置信息和所述障碍物在Frenet坐标系下的位置信息，分别计算出车辆在Frenet坐标系下的最优横向运动轨迹和车辆在Frenet坐标系下的最优纵向运动轨迹；

步骤S6：对所述车辆在Frenet坐标系下的最优横向运动轨迹和车辆在Frenet坐标系下的最优纵向运动轨迹进行合成，以生成车辆在Frenet坐标系下的最优自动驾驶路径，并对按照所述最优自动驾驶路径行驶的车辆进行障碍物碰撞检测。

2.根据权利要求1所述的基于采样的车辆行为与环境交互的自动驾驶路径规划方法，其特征在于，所述步骤S1中，还包括：

根据所述路径规划参考线上的每个离散点在笛卡尔坐标系下的位置信息计算出相邻离散点之间的距离；

根据所述路径规划参考线上相邻离散点之间的距离，分别计算出所述路径规划参考线上除起始点之外的其他离散点到所述起始点的距离；

其中，所述每个离散点在笛卡尔坐标系下的位置信息包括横坐标、纵坐标、倾斜角、曲率和曲率导数。

3.根据权利要求2所述的基于采样的车辆行为与环境交互的自动驾驶路径规划方法，其特征在于，所述步骤S2中，还包括：

从所述路径规划参考线上的起始点开始遍历整条所述路径规划参考线上的离散点，根据所述路径规划参考线上的每个离散点到所述车辆当前位置点的距离从所述路径规划参考线上找到一个离所述车辆当前位置点最近的离散点，将该点标记为所述最小匹配点；

判断所述路径规划参考线上的最小匹配点是否为起始点或者末位点；若所述最小匹配点既不是起始点也不是末位点时，将所述最小匹配点前后的两个离散点分别标注为P₀点与P₁点；若所述最小匹配点为起始点时，将所述起始点和所述起始点的后一离散点分别标注为P₀点与P₁点；若所述最小匹配点为末位点时，将所述末位点的前一离散点和所述末位点分别标注为P₀点与P₁点；

分别构造P₀点到车辆当前位置点O的向量和P₀点到P₁点的向量/>计算向量/>的模长，并求出向量/>和向量/>的内积，最后用内积来除向量/>的模长，最终得到所述车辆当前位置点O在所述路径规划参考线上的理想投影点m；

分别计算出理想投影点m到P₀点的距离S_m，以及根据所述理想投影点m到P₀点的距离S_m和所述P₀点到所述起始点的距离计算出所述理想投影点m到所述起始点的距离S_r；

根据所述理想投影点m到所述起始点的距离S_r、P₀点到所述起始点的距离S_p0以及P₁点到所述起始点的距离S_p1计算出权重，权重weight的计算公式如下：

weight＝(S_r-S_p0)/(S_p1-S_p0)

根据权重weight计算出所述路径规划参考线上的理想投影点m在笛卡尔坐标系下的位置信息，其中，所述理想投影点m在笛卡尔坐标系下的位置信息包括横坐标、纵坐标、倾斜角、曲率和曲率导数，所述理想投影点m在笛卡尔坐标系下的横坐标x的计算公式如下：

x＝(1-weight)*P_0.x+weight*P_1.x

其中，P_0.x为P₀点在笛卡尔坐标系下的横坐标，P_1.x为P₁点在笛卡尔坐标系下的横坐标；所述理想投影点在笛卡尔坐标系下的纵坐标、倾斜角、曲率和曲率导数的计算公式同理。

4.根据权利要求3所述的基于采样的车辆行为与环境交互的自动驾驶路径规划方法，其特征在于，所述步骤S3中，还包括：

所述车辆当前位置点在Frenet坐标系下的位置信息包括Frenet纵坐标s、Frenet纵坐标对时间的导数Frenet纵坐标对时间的二阶导/>Frenet横坐标d、Frenet横坐标对纵向坐标的导数d′以及Frenet横坐标对纵向坐标的二阶导d″，计算公式分别如下：

其中，S_r为理想投影点到起始点的距离；为车辆当前位置点在笛卡尔坐标系下的坐标；υ_x为车辆当前位置点在笛卡尔坐标系下的线速度，其中/>θ_x为车辆当前位置点在笛卡尔坐标系下的倾斜角；θ_r为理想投影点在笛卡尔坐标系下的倾斜角；k_r为理想投影点在笛卡尔坐标系下的行驶曲率，其中/>k_x为车辆当前位置点在笛卡尔坐标系下的行驶曲率，其中/>a_x为车辆当前位置点在笛卡尔坐标系下的加速度，其中/>dx＝x-rx，dy＝y-ry，其中x、y分别为车辆当前位置点在笛卡尔坐标系下的横坐标、纵坐标，rx、ry分别为理想投影点在笛卡尔坐标系下的横坐标、纵坐标；

建立车辆当前位置点在Frenet坐标系下的横坐标d关于时间t的函数d(t)，以及建立车辆当前位置点在Frenet坐标系下的纵坐标s关于时间t的函数s(t)；其中，函数d(t)和函数s(t)分别为车辆行驶过程中的横向运动分量和纵向运动分量。

5.根据权利要求4所述的基于采样的车辆行为与环境交互的自动驾驶路径规划方法，其特征在于，所述步骤S4中，还包括：

获取所述障碍物在笛卡尔坐标系下的位置信息以及所述障碍物的长度和宽度，其中，所述障碍物在笛卡尔坐标系下的位置信息包括横坐标、纵坐标和旋转角；

将所述障碍物的四个顶点在笛卡尔坐标系下的位置信息转换为该四个顶点在Frenet坐标系下的位置信息。

6.根据权利要求5所述的基于采样的车辆行为与环境交互的自动驾驶路径规划方法，其特征在于，所述步骤S5中，还包括：

(1)采用五次多项式对车辆在Frenet坐标系下的横向运动轨迹和纵向运动轨迹进行规划，五次多项式f(t)的公式如下：

f(t)＝c₀+c₁t+c₂t²+c₃t³+c₄t⁴+c₅t⁵

其中，通过确定车辆在Frenet坐标系下的运动初始状态f(t_s)及其一阶微分和二阶微分/>以及车辆在Frenet坐标系下的运动结束状态f(t_e)及其一阶微分/>和二阶微分/>来求解车辆在整个运动过程中的五次多项式模型参数，具体为：

化成矩阵形式为：

令：

得到：C＝M^-10

(2)对于车辆在Frenet坐标系下的横向运动轨迹规划：车辆横向运动的建模本质是对车辆行驶过程中的横向运动分量d(t)进行建模，根据车辆在Frenet坐标系中的运动初始状态和运动结束状态计算出在该时间段内平滑连续的运动轨迹；假设自动驾驶车辆从T_s时刻开始启动自动驾驶功能，并在T_e时刻结束，在[T_s,T_e]这段规划时间内，为了实现安全和高效的行驶，车辆需要根据实时变化的场景和周围动态或静态障碍物进行轨迹规划；所以在采样周期t∈[T_s,T_e]，根据车辆在Frenet坐标系下的运动初始时刻T_s位置信息和车辆在Frenet坐标系下的运动结束时刻T_e位置信息，并结合五次多项式拟合式C＝M^-10，得到车辆在该采样周期t内的横向运动轨迹，即：

其中，所述车辆在Frenet坐标系下的运动初始时刻T_s位置信息包括Frenet横坐标d(T_s)、Frenet横坐标对纵坐标的导数和Frenet横坐标对纵坐标的二阶导/>所述车辆在Frenet坐标系下的运动结束时刻T_e位置信息包括Frenet横坐标d(T_e)、Frenet横坐标对纵坐标的导数/>和Frenet横坐标对纵坐标的二阶导/>

根据当前采样时刻T_i车辆所在的初始位置，对下一采样时刻T_i+1车辆可能出现的覆盖区域进行采样，因此在t∈[T_i,T_i+1]时刻，对横向运动分量d(t)的始末状态进行建模得：

因为下一采样时刻T_i+1车辆可能所在的末位置都是互相独立的，所以每个采样周期内由初始位置到末位置有多个候选横向运动轨迹，在横向轨迹规划中将采样频率用f_M进行表示，所以每一次采样都是在1/f_M秒内，每个采样周期都将重新进行一次轨迹规划，每次轨迹规划后均得到一条五次多项式的运动轨迹，在每一次轨迹规划时，以当前采样时刻的车辆所在位置作为初始位置，通过对末位置进行采样，得到多个候选横向运动轨迹，根据横向运动轨迹的评估目标函数在多个候选横向运动轨迹中选择一个最优横向运动轨迹，并根据最优横向运动轨迹得到此次轨迹规划的车辆所在末位置，并将其作为下一次轨迹规划的初始位置，通过不断迭代和计算，最终得到整个规划周期[T_s,T_e]内的完整横向运动轨迹；

(3)对于车辆在Frenet坐标系下的纵向运动轨迹规划：对于车辆的纵向规划也是按照所述五次多项式f(t)进行轨迹表示，则初始时刻T_s的纵向运动状态分别为s(T_s)、结束时刻T_e的纵向运动状态分别为s(T_e)、/> 根据五次多项式的纵向运动模型可得：

D＝P^-1Q

其中D＝(c₀,c₁,c₂,c₃,c₄,c₅)^T

其中，s(T_s)为车辆在初始时刻T_s的Frenet纵坐标，为车辆在初始时刻T_s的Frenet纵坐标对时间的导数，/>为车辆在初始时刻T_s的Frenet纵坐标对时间的二阶导，s(T_e)为车辆在结束时刻T_e的Frenet纵坐标，/>为车辆在结束时刻T_e的Frenet纵坐标对时间的导数，/>为车辆在结束时刻T_e的Frenet纵坐标对时间的二阶导；

将纵向轨迹规划的期望末状态定义为S_a，给出期望的末状态运动参数，自动驾驶车辆在不同场景下运动，影响每一次规划末状态的期望值，因而不同场景下生成的纵向轨迹集合不同，在规划时间内，对车辆纵向运动的始末状态空间采样，即t∈[T_i,T_i+1]，对纵向运动分量s(t)的始末状态进行建模得：

由于纵向运动具有期望的状态值S_a，所以将纵向轨迹规划末状态离散化采样，不同于横向采用时间频率，纵向使用位移大小来确定运动轨迹，因此末状态更新为：

T_i+1,S_a(T_i+1)+Δs(T_i+1)，

因为下一采样位移车辆可能所在的末位置都是互相独立的，所以由初始位置到下一个采样位移的末位置有多个候选轨迹；在纵向轨迹规划中将位移大小用Δs(T_i+1)进行表示；与横向轨迹规划相同，通过对末状态进行采样，得到多个候选纵向运动轨迹，根据纵向运动轨迹的评估目标函数在多个候选纵向运动轨迹中选择一个最优纵向运动轨迹，通过不断迭代，可以得到整个规划周期[T_s,T_e]内的完整纵向运动轨迹。

7.根据权利要求6所述的基于采样的车辆行为与环境交互的自动驾驶路径规划方法，其特征在于，所述横向运动轨迹的评估目标函数cost_d的计算过程包括：

横向加加速度：由于表示车辆运动过程中的横向加加速度，是描述车辆运动过程中的舒适程度，在规划周期[T_s,T_e]内用/>平方的积分大小来评判所述横向运动轨迹的质量，所述横向运动轨迹质量的评估函数j_t(d(t))的计算公式如下：

横向偏移量：为了让车辆尽可能的靠近车道中心线行驶，设置横向偏移量的评估函数h(de)，用来评判车辆到车道中心线的横向偏移，每一次抽样都计算车辆到车道中心线距离的平方，再求出该规划周期[T_s,T_e]内的平方和：

车辆距离障碍物距离：作为安全保护，在横向轨迹规划途中要距离障碍物位置相对较远，车辆距离障碍物距离的评估函数用C_OBS-T表示；

横向加速度：用来评估车辆换道的平稳程度，所述车辆换道平稳程度的评估函数a_t(d(t))的计算公式如下：

向心加速度：向心加速度的评估函数a(ω(t))的计算公式如下：

以上就是横向运动轨迹的评估目标函数cost_d的五个属性，再分别为其根据不同的应用场景配置适当的权重参数ζ_j、ζ_h、ζ_c、ζ_at、ζ_aω，所以得到所述横向运动轨迹的评估目标函数cost_d为：

cost_d＝ζ_jj_t(d(t))+ζ_hh(de)+ζ_cC_OBS-T+ζ_ata_t(d(t))+ζ_aωa(d(t),ω(t))。

8.根据权利要求6所述的基于采样的车辆行为与环境交互的自动驾驶路径规划方法，其特征在于，所述纵向运动轨迹的评估目标函数cost_s的计算过程包括：

纵向加加速度：由于表示车辆运动过程中的纵向加加速度，是描述车辆运动过程中的舒适程度，在规划周期[T_s,T_e]内用/>的积分大小来评判所述纵向运动轨迹的质量，所述纵向运动轨迹质量的评估函数j_t(s(t))的计算公式如下：

车辆距离障碍物距离：作为安全保护，在纵向轨迹规划途中要距离障碍物位置相对较远，车辆距离障碍物距离的评估函数用C_OBS-S1表示；

车辆距离前车距离：作为安全保护，在纵向轨迹规划途中要距离前车位置相对较远，车辆距离前车距离的评估函数用C_OBS-S2表示；

规划实际末状态S_e与期望末状态S_a的接近程度：用来表示本次规划的完成情况，如果实际与期望相差较大则说明规划失误，用h(S_a,S_e)表示：

h(S_a,S_e)＝(S_a-S_e)²

以上就是所述纵向运动轨迹的评估目标函数cost_s的四个属性，再分别为其根据不同的应用场景配置适当的权重参数所以得到所述纵向运动轨迹的评估目标函数cost_s为：

9.根据权利要求6所述的基于采样的车辆行为与环境交互的自动驾驶路径规划方法，其特征在于，所述对所述车辆在Frenet坐标系下的最优横向运动轨迹和车辆在Frenet坐标系下的最优纵向运动轨迹进行合成，以生成车辆在Frenet坐标系下的最优自动驾驶路径，还包括：

所述完整横向运动轨迹集合d_set如下：

d_set＝ψ(t,d_s,d_e),t＝{t₀,t₁,t₂,...,t_N-1}

式中，ψ(t,d_s,d_e)表示生成的横向运动轨迹集合，共有N个时间采样点；ψ(t,d_s,d_e)表示从初始状态d_s转移到末状态d_e；

同理，所述完整纵向运动轨迹集合s_set为：

s_set＝Ω(t,s_s,s_e),t＝{t₀,t₁,t₂,...,t_N-1}

式中，Ω(t,s_s,s_e)表示生成的纵向运动轨迹集合；共有N个时间采样点；Ω(t,s_s,s_e)表示从初始状态s_s转移到末状态s_e；

在横纵向始末状态采样并生成横纵向运动轨迹集合后，在某一时刻t对两个集合中的取值进行一一对应，在某一时刻t，通过匹配完整横向运动轨迹集合d_set和完整纵向运动轨迹集合s_set中的取值，来确定横纵向轨迹的对应关系，这样可以获得一组相互匹配的横纵向轨迹，形成最终的路径轨迹集合Π如下:

Π＝Traj(t,d_s,d_e,s_s,s_e)。

10.根据权利要求1所述的基于采样的车辆行为与环境交互的自动驾驶路径规划方法，其特征在于，所述对按照所述最优自动驾驶路径行驶的车辆进行障碍物碰撞检测，还包括：

采用包围盒碰撞检测法，通过使用边界框来近似表示物体的包围盒，然后判断这些包围盒是否相交来确定是否发生碰撞，边界框是一个立方体或长方体，其可以由最小点和最大点来定义，分别表示包围盒的最小和最大顶点坐标；

首先用边界框来表示障碍物与行驶环境中的车辆，对于自车车辆和其他物体检测包围盒是否相交；如果自车车辆的包围盒与其他物体的包围盒相交，那么可能会发生碰撞；否则，它们之间不存在碰撞；其中，如果发生相交则进行投影再检查：通过检查每个轴上的投影是否有重叠来实现；

在Frenet坐标系中，比较两个边界框在d轴上的投影是否有重叠；如果自车的d轴投影的最大值小于其他物体的d轴投影的最小值，或者自车d轴投影的最小值大于其他物体d轴投影的最大值，则两个边界框在d轴上没有重叠；然后，对s轴执行相同的检查；如果在所有轴上都存在重叠，那么两个边界框相交，可以判断自车车辆和其他物体可能会发生碰撞；再返回至横纵向采样规划，进行重新规划。