[发明专利]一种基于带挂车模型的B样条轨迹规划方法

权利要求书

1.种基于带挂车模型的B样条轨迹规划方法，其特征在于，该方法包括：

步骤1：根据起始点和障碍物信息，用搜索的方法得到初始轨迹；

步骤2：用B样条曲线对初始轨迹进行插值，得到控制点作为优化变量；

步骤3：在B样条曲线上进行均匀采样得到采样点；

步骤4：以挂车后轴中心为基准，建立挂车碰撞模型，并根据障碍物信息计算采样点的横向移动范围，将横向移动范围作为限制条件；

步骤5：构建起始点的限制条件；

步骤6：建立目标函数；

步骤7：将限制条件和目标函数放入优化求解器求解，得到优化后的B样条曲线，在优化后的B样条曲线上采样，得到采样点组成的轨迹即为挂车后轴中心的轨迹；

步骤8：根据车头与挂车的几何关系，推算车头的后轴中心轨迹。

2.根据权利要求1所述的一种基于带挂车模型的B样条轨迹规划方法，其特征在于：步骤1中，所述的初始轨迹以轨迹点的形式进行保存PT_i，i＝1…N_p；其中N_p为轨迹点的个数；

所述初始轨迹为连接起点和终点，且将挂车碰撞模型放在起点和终点都不与障碍物发生碰撞的曲线，表示为：

PT＝[x y θ]^T

其中，x，y，θ，为一个轨迹点中包含的三个状态，θ是航向角，为曲线在轨迹点的切线方向。

3.根据权利要求2所述的一种基于带挂车模型的B样条轨迹规划方法，其特征在于：步骤2中，对初始轨迹PT_i进行插值，得到B样条曲线：BS(u)＝∑_iN_iP_i；

其中，u是节点向量，P_i是控制点，N_i是转化矩阵。

4.根据权利要求3所述的一种基于带挂车模型的B样条轨迹规划方法，其特征在于：步骤3中，对B样条曲线进行采样，得到采样点PB_i，i＝1…N_b，N_b为采样点的个数；获取所有在所述B样条曲线左边的障碍物和道路边界，以线段的形式保存SL_i，i＝1…N_l，N_l为左边线段的个数；

获取所有在所述B样条曲线右边的障碍物和道路边界，以线段的形式保存SR_i，i＝1…N_r，N_r为左边线段的个数；

每条线段都包含一个起点P_start，一个终点P_end，SL/SR＝[P_startP_end]^T。

5.根据权利要求4所述的一种基于带挂车模型的B样条轨迹规划方法，其特征在于：步骤4中，所述的挂车碰撞模型为用几何线条表达出的挂车轮廓；所述的挂车轮廓为以挂车后轴中心为基准点，整车长作为边长，整车宽作为边宽；

将挂车碰撞模型放在采样点上，在采样点处的法向上左右平移挂车碰撞模型，针对挂车碰撞模型与所有SL/SR求交，得出向左最大不碰撞的距离L_left和向右最大不碰撞的距离L_right；

则，限制条件的公式为L_right≤L≤L_left；其中，L为采样点处的横向距离。