[发明专利]一种自动泊车控制系统中的轨迹预瞄计算方法和系统

权利要求书

1.一种自动泊车控制系统中的轨迹预瞄计算方法，其特征在于，包括以下步骤：

S1、根据泊车轨迹信息生成轨迹点集，将所述轨迹点集存放至轨迹点集数组中，对所述轨迹点集数组中的轨迹点依次输出下标，将所述轨迹点集数组分为前进点集数组和后退点集数组，并初始设置折返标志Turnback_flag＝0；

所述将所述轨迹点集数组分为前进点集数组和后退点集数组的具体步骤为：

S11、将所述轨迹点集数组中第一个轨迹点下标输出记为0；

S12、遍历所述轨迹点集数组，找到首次数据符号相反的轨迹点，将所述首次数据符号相反的轨迹点下标输出记为turn_back_index；

S13、将所述轨迹点集数组中最后一个轨迹点下标输出记为final_index；

S14、所述前进点集数组的遍历范围为0至turn_back_index，所述后退点集数组的遍历范围为turn_back_index+1至final_index；

S2、根据当前自车位置在所述轨迹点集数组中遍历计算最近的轨迹点作为定位点，选取所述定位点的下一个轨迹点作为预瞄点，车辆向所述预瞄点移动；

S3、判断车辆的位置，若车辆到达最终目标框则结束自动泊车。

2.根据权利要求1所述的一种自动泊车控制系统中的轨迹预瞄计算方法，其特征在于，步骤S1中所述下标为所述轨迹点在所述轨迹点集数组中存放的位置；所述前进点集数组中的轨迹点数据为正值，所述后退点集数组中的轨迹点数据为负值。

3.根据权利要求1所述的一种自动泊车控制系统中的轨迹预瞄计算方法，其特征在于，步骤S2中所述遍历计算最近的轨迹点的具体公式为：

min(k)＝Min{min_distance(i)}

其中，i表示轨迹上某一轨迹点的下标，traj.x(i)表示轨迹点i的横坐标，traj.y(i)表示轨迹点i的纵坐标，current.x表示当前自车位置的横坐标，current.y表示当前自车位置纵坐标；遍历所有轨迹点后取最小值min(k)，下标为k+1的轨迹点即为所述预瞄点。

4.根据权利要求1所述的一种自动泊车控制系统中的轨迹预瞄计算方法，其特征在于，步骤S3中所述判断车辆的位置，若车辆到达最终目标框则结束自动泊车的具体步骤为：

S31、判断所述折返标志Turnback_flag的值，若Turnback_flag大于0，则选择所述后退点集数组遍历轨迹点，否则选择所述前进点集数组遍历轨迹点；

S32、判断所述预瞄点的下标preview_index和当前车速RealSpeed的值，若所述预瞄点的下标preview_index大于或等于turn_back_index，且当前车速RealSpeed为0，则将所述折返标志Turnback_flag的值置为1，表明车辆当前到达中间目标框，进入步骤S33，否则返回步骤S2；

S33、判断所述预瞄点的下标preview_index和所述当前车速RealSpeed的值，若所述预瞄点的下标preview_index大于或等于final_index，且所述当前车速RealSpeed为0，则将所述折返标志Turnback_flag的值置为2，表明车辆当前到达最终目标框，结束自动泊车，否则返回步骤S2。