买专利卖专利找龙图腾，真高效！ 查专利查商标用IPTOP,全免费！专利年费监控用IP管家,真方便！

申请/专利权人：常州工学院

摘要：本发明公开了一种面向非结构化场景的自动驾驶拖拉机路径规划方法。该方法首先建立环境地图模型；确定静态障碍物所处的具体位置；利用Reeds‑Shepp曲线模型对启发式函数进行改进，得到混合A*算法的代价函数，再确定系统的约束条件，然后根据代价函数与步骤系统约束进行子节点拓展，获取全局参考路径；进行基于动态规划算法的局部避障优化之前，将拖拉机位姿信息进行Frenet坐标与Cartesian坐标转换；根据全局参考路径以及坐标转换方法，利用S‑L图对全局参考路径进行优化，并获得连续、平滑的期望路径；最后的坐标转换方法以及期望路径规划结果，利用S‑T图进行速度规划，确定有效规避动态障碍物的期望速度。

主权项：1.一种面向非结构化场景的自动驾驶拖拉机路径规划方法，其特征在于，该方法包括以下步骤：S1.建立环境地图模型；确定静态障碍物所处的具体位置；S2.利用Reeds-Shepp曲线模型对启发式函数进行改进，得到混合A*算法的代价函数；S3.确定系统的约束条件，保证避障安全性，并适应拖拉机运动特性；S4.根据步骤S2代价函数与步骤S3的系统的约束条件进行子节点拓展，获取全局参考路径；S5.进行基于动态规划算法的局部避障优化之前，将拖拉机位姿信息进行Frenet坐标与Cartesian坐标转换；S6.根据步骤S4获得的全局参考路径以及步骤S5的坐标转换方法，利用S-L图对全局参考路径进行优化，获得连续、平滑的期望路径；S7.由步骤S5的坐标转换方法以及步骤S6的期望路径规划结果，利用S-T图进行速度规划，确定有效规避动态障碍物的期望速度；所述步骤S3的具体方法是：选择矩形和多边形作为拖拉机与障碍物的轮廓，将拖拉机视为矩形，利用拖拉机边界函数来描述拖拉机尺寸和形状，相应的四个顶点M1～M4横纵坐标表示为： 其中，x,y为拖拉机位置坐标，f为拖拉机前悬距离、r为拖拉机后悬距离、L为拖拉机轴距、w为车身宽度拖拉机尺寸参数、Ψ为拖拉机航向角；判断拖拉机与障碍物是否发生碰撞，需要确定拖拉机边界和障碍物轮廓是否重合，采用三角形面积法检验拖拉机顶点坐标与凸多边形N1N2…Nq的位置关系，令拖拉机其中一个顶点坐标为Mi，连接点Mi与凸多边形的每两个相邻顶点构成三角形，并将这些三角形的面积进行累加，若累加得到的面积之和大于凸多边形的总面积，则确定点Mi位于凸多边形的外部；如式7所示： 其中，S代表各个多边形的面积；凸多边形面积通过若干个三角形进行求解，在二维平面上，碰撞一定会发生在顶点处，将车身矩形的每个顶点始终约束在障碍物多边形的外部，同时限制多边形障碍物的每个顶点也位于车身矩形的外部，利用式7，得到点Mi位于凸多边形N1N2…Nn外部的约束条件f1与f2，并建立如下的避障约束： 其中，γN1,…Nn为凸多边形顶点的集合，χM1,…M4为车身矩形四个顶点的集合；根据拖拉机的运动学特性，可知拖拉机行驶的路径曲率κ仅与前轮转角δf有关： 过大的前轮转角会使路径曲率增大，在降低行驶舒适性同时会加剧拖拉机的机械磨损，因此通过限制拖拉机前轮转角，以约束规划路径的曲率：|δf|≤arctanLκmax10最终确定混合A*算法求解的避障约束与前轮转角约束，保证避障安全性，并适应拖拉机运动特性；所述步骤S5的具体方法是：将步骤S1-S5中采用Cartesian坐标系进行道路描述转换为采用Frenet坐标系进行描述，Cartesian坐标系下，某一点的拖拉机运动状态表示为[x,y,v,Ψ,a,κ]；a是cartesian坐标系下的车辆加速度，而在Frenet坐标系下，拖拉机运动状态表示为其中，s为纵向位移，为纵向车速，为纵向加速度，l为横向位移，为横向车速，为横向加速度，l′为l对s的导数，l″为对的导数；Cartesian坐标系转Frenet坐标系的公式为： Frenet坐标系转Cartesian坐标系的公式为： 其中，sr为期望纵向位移，xr和yr为Cartesian坐标系下期望横向位置和纵向位置，κr为参考路径曲率，κr′为参考路径曲率对s的导数，ΔΨ＝Ψ-Ψr，Ψr为参考航向角；所述步骤S6的具体方法是：首先，根据场景中的障碍物信息构建Frenet坐标系下的表示道路与障碍物之间的关系图，简称S-L图，通过在S-L图中撒点并初始化将空间离散化；其次，设置成本函数，为每个节点分配一个代价值以描述相邻单元之间的路径选择关系；考虑障碍物的位置、拖拉机的当前位置和目标位置，采用动态规划算法对参考路径进行优化，确保在避免碰撞的同时能够找到最小代价路径，代价函数J由路径与静态障碍物的安全距离代价函数Jobs、平滑性代价函数Js以及偏离参考路径程度代价函数Jl描述：J＝Jobs+Js+Jl13路径与静态障碍物的安全距离代价函数Jobs表示为： 其中，dmax为最大碰撞距离，dmin为最小碰撞距离，[dmin,dmax]为碰撞缓冲区间，frisk为单调递减的碰撞风险函数，越靠近障碍物，frisk值越大，即越容易发生碰撞风险；平滑性代价函数Js表示为： 其中，t0为路径规划开始时间，ts为路径规划结束时间，ω1、ω2、ω3为平滑性权重系数，分别代表路径横向位移、路径曲率以及路径曲率的变化率，l″′为对的导数；偏离参考路径程度的代价函数Jl表示为： 其中，ωl为偏离参考路径程度权重系数，lr为参考线函数；采用递推的方法从起点到终点进行搜索，通过计算每个节点到终点的最小代价值来更新当前节点的代价值，并保存路径信息以供回溯，在所有节点计算完毕之后，筛选得到所有路径采样节点的最小连接代价；然后利用五次多项式对给定的节点进行插值和拟合，将离散节点连续化，得到符合要求的平滑路径曲线，五次多项式曲线通过以下公式表示： 其中，t代表时间，a10～aN-15代表五次多项式的系数，τs代表多段五次多项式的集合，s5代表s的五次方，假设起点状态信息为[sj,lj,l′j′,l″j]，终点状态信息为[sk,lk,l′k,l″k]，将起点与终点状态信息带入式17，得： 其中，ak0～ak5代表每一段五次多项式系数；求解式18的线性方程组，确定五次多项式曲线的所有系数，实现对参考路径的优化；最后，对该路径的节点信息进行坐标转换，将相关位置坐标信息由Frenet坐标系的S-L图变换到Cartesian坐标系下的X-Y图，获取用于跟踪控制的期望路径。

全文数据：

权利要求：

百度查询： 常州工学院 一种面向非结构化场景的自动驾驶拖拉机路径规划方法