1.一种分阶段的自动驾驶车辆轨迹规划方法,其特征在于,所述方法具体包括以下步骤:搜索阶段,在Frenet坐标系下进行采样,使用五次多项式连结采样点,形成有向无环图,动态规划搜索得到参考轨迹,构建可行驶通道;
优化阶段,建立车辆运动学模型,根据所述可行驶通道,构建代价函数,建立边界约束,使用ILQR算法进行时空联合优化,输出最优轨迹。
2.根据权利要求1所述的分阶段的自动驾驶车辆轨迹规划方法,其特征在于,所述搜索阶段,在Frenet坐标系下进行采样,使用五次多项式连结采样点,形成有向无环图,动态规划搜索得到参考轨迹,构建可行驶通道具体包括以下步骤:获取自动驾驶车辆的处地图信息,构建Frenet坐标系,获取障碍物信息和自动驾驶车辆信息,并投影至所述Frenet坐标系;
在所述Frenet坐标系进行采样,得到采样点,使用五次多项式连结采样点,并根据预设的代价函数,计算相应的代价;
进行动态规划搜索,得到搜索阶段的最优路径,进行膨胀得到可行驶通道,并转化为笛卡尔坐标系。
3.根据权利要求2所述的分阶段的自动驾驶车辆轨迹规划方法,其特征在于,所述使用五次多项式连结采样点的公式为:其中, 是五次多项式的参数,
是五次多项式的自变量。
4.根据权利要求2所述的分阶段的自动驾驶车辆轨迹规划方法,其特征在于,所述代价函数为:cpath=cref+csmooth+cobs其中,cpath为路径代价,cref为参考线偏移代价,csmooth为平滑性代价,cobs为障碍物代价。
5.根据权利要求4所述的分阶段的自动驾驶车辆轨迹规划方法,其特征在于,所述参考线偏移代价的公式为:其中,w0为权重系数,l(s)为偏移程度。
6.根据权利要求5所述的分阶段的自动驾驶车辆轨迹规划方法,其特征在于,所述平滑性代价的公式为:其中,w1,w2,w3均为权重系数, 分别是l(s)的一阶,二阶,三阶导数。
7.根据权利要求6所述的分阶段的自动驾驶车辆轨迹规划方法,其特征在于,所述障碍物代价的公式为:
8.根据权利要求1所述的分阶段的自动驾驶车辆轨迹规划方法,其特征在于,所述车辆运动学模型中车辆的运动状态方程为:vi+1=vi+aiδt
θi+1=θi+ωiδt
其中, 为ti时刻的状态量, 为笛卡尔坐标系下的坐标点,vi和θi分别为车速和航向角,ai为加速度。
9.根据权利要求8所述的分阶段的自动驾驶车辆轨迹规划方法,其特征在于,所述代价函数包括控制代价函数和状态代价函数,具体的:控制代价函数为:
其中,Ju为时控制代价,ωacc,ωω均为权重系数,ai,ωi分别为加速度和横摆角速度;
状态代价函数为:
其中, 为点 在参考路径上的投影点,vr为自动驾驶车辆的参考速度,wref,wvel均为权重系数,Jx为状态代价。
10.根据权利要求9所述的分阶段的自动驾驶车辆轨迹规划方法,其特征在于,所述边界约束的表达式为: