:本发明属于,特别涉及一种面向港口结构化狭窄道路的轨迹规划方法。
背景技术
0、
背景技术:
1、随着汽车技术的飞速发展,越来越多的汽车公司及科技创新公司不满足于汽车传统底盘、座椅、动力总成以及悬架等技术的研发,从而投向了汽车智能驾驶领域的研发。其中港口道路结构规整,运输量大是无人驾驶高度应用的一个场景。其中不乏直角弯,狭窄作业车道等计算作业场景,对无人驾驶规划技术提出了很高的挑战。
2、在道路边界不规则且自由空间严格受限的场景下,如何生成一条可跟踪、平稳的行驶轨迹,是自动驾驶轨迹规划面临的难题。
3、公开于该背景技术部分的信息仅仅旨在增加对本发明的总体背景的理解,而不应当被视为承认或以任何形式暗示该信息构成已为本领域一般技术人员所公知的现有技术。
技术实现思路
0、
技术实现要素:
1、本发明的目的在于针对目前有关无人驾驶狭窄路况路径规划技术研究较少,本专利提供了一种基于周围道路边界信息,障碍物信息,实现规划出通过狭窄道路的方法,可应用于智能车路劲规划的开发,减少狭窄极端路况人工接管。
2、为实现上述目的,本发明提供了一种面向港口结构化狭窄道路的轨迹规划方法,方法包括以下步骤:
3、步骤1,狭窄通道环境建模,采用结构化道路的分段边界拟合方式,能用较少的参数保证道路特征,提高计算效率。每段边界用两个参数表示,n个连续边界可以用n+1个参数表示;
4、步骤2,边界条件的计算和处理,匹配当前的路段id,对应上该id路段的端点参数,将断层边界用直线连上,保证边界的连续性。
5、步骤3,初始b样条构造,初始化时将约束边界外扩,每次迭代优化使用上一次优化结果重新计算边界,直到边界缩至初值或者优化失败。当优化迭代n次失败时,可以显示的表达出当前边界的局限性,给边界调整提供依据。
6、步骤4,约束设置,该问题求解的约束为步骤三的边界约束,车辆的曲率约束,起点终点姿态约束。
7、步骤5,代价函数设计,代价函包含b样条的零一二阶倒数和距离障碍物距离。
8、步骤6,优化问题求解。
9、本申请综合背景技术的分析,面向港口结构化狭窄道路,首先对狭窄环境进行建模,地图引导线作为轨迹规划的初解,并采用基于b样条的优化方法构造轨迹规划问题,最终能求解出安全、平滑的最优路径。
10、优选地,上述技术方案中,由于港口道路多为方块结构化道路:
11、1:可以将边界以多条直线的形式表示,每一段只需要首尾两个端点坐标即可。
12、2:在查询边界的时候,只需要匹配当前的路段id,对应上该id路段的端点参数,即可快速算出左右边界。
13、3:即使当前路段边界出现断层,也可以用直线连上,保证边界的连续性。
14、优选地,上述技术方案中,步骤1,计算bound_left、bound_right中每个线段在b样条的投影,得到每条线段b样条长度s方向的投影范围start_s、end_s,并保存在直线段上;
15、步骤2,根据当前轨迹点的s值,取bound_left、bound_right位于s前后一定范围的直线段;
16、步骤3,设置车辆后轴中心,得到行驶到轨迹点处的车体轮廓,并计算车体轮廓与6.2中直线段的最小距离,作为该轨迹点的左右硬约束b_l、b_r;遍历每个轨迹点,得到所有待优化轨迹点的左右边界硬约束b_ls、b_rs;
17、步骤4,设置起点和终点的限制条件;起点
18、p0=[x0,y0,theta0,wheel_angle0]和pend=[xend,yend,thetaend,wheel_angleend];
19、步骤5,代价函数,考虑b样条的一阶导数bs′,二阶导数bs”,三阶导数bs”',与障碍物距离,曲率约束,对应权重系数w1,w2,w3,w4;cost=w1∫bs′2+w2∫bs″2+w3∫bs″′2+w4∫dis_obs;
20、步骤6,将约束条件和代价函数带入求解器,便得到优化后的b样条函数在新的b样条函数上按照等间隔进行采样,得到一系列的位姿点即为最终的优化求解结果。
21、优选地,上述技术方案中,包含的动态权重调整方法,能根据自由空间的大小,动态对代价函数进行调整,在最大程度搜索到可行解的同时,以最快的速度逼近目标点。
22、优选地,上述技术方案中,在求解到最优解的同时,确保估计曲率连续。
23、与现有技术相比,本发明具有如下有益效果:基于b样条的优化方法构造轨迹规划问题,最终能求解出安全、平滑的最优路径。
1.一种面向港口结构化狭窄道路的轨迹规划方法,其特征在于,方法包括以下步骤:
2.根据权利要求1所述的面向港口结构化狭窄道路的轨迹规划方法,其特征在于:
3.根据权利要求1所述的面向港口结构化狭窄道路的轨迹规划方法,其特征在于:
4.根据权利要求2所述的面向港口结构化狭窄道路的轨迹规划方法,其特征在于:包含的动态权重调整方法,能根据自由空间的大小,动态对代价函数进行调整,在最大程度搜索到可行解的同时,以最快的速度逼近目标点。
5.根据权利要求2所述的面向港口结构化狭窄道路的轨迹规划方法,其特征在于:在求解到最优解的同时,确保估计曲率连续。