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

说明书

本发明涉及一种自动泊车控制系统中的轨迹预瞄计算方法和系统，包括以下步骤：将所述轨迹点集存放至轨迹点集数组中，对所述轨迹点集数组中的轨迹点依次输出下标，将所述轨迹点集数组分为前进点集数组和后退点集数组，并初始设置折返标志Turnback_flag＝0；根据当前自车位置在所述轨迹点集数组中遍历计算最近的轨迹点作为定位点，选取所述定位点的下一个轨迹点作为预瞄点，车辆向所述预瞄点移动；判断车辆的位置，若车辆到达最终目标框则结束自动泊车。本发明提出的预瞄点选取方案，不会引入累积误差,而且能够精确根据轨迹点的关系来判断行车方向，极大改善了横向控制的精确性问题。

技术领域

本发明涉及自动泊车领域控制领域，尤其涉及一种自动泊车控制系统中的轨迹预瞄计算方法和系统。

背景技术

自动泊车系统在确定了泊车路径后，需要保证车辆按照泊车路径行进，这个工作通常是由车辆控制模块实现的。车辆的行进，除了速度上的要求，还有方向上的要求。对应地，车辆控制模块分为纵向控制和横向控制的处理。其中，纵向控制主要针对速度的要求，而横向控制主要针对方向的要求。纵向控制的处理相对简单，而横向控制处理的复杂度和精确度通常决定了整个车辆控制模块的性能。

横向控制的输入通常是泊车路径上的轨迹点集。这些轨迹点具有位姿属性，且彼此等间距。横向控制要解决的是如何从当前位置点移动到轨迹点集中的下一个点。现有的计算方法通常是：根据车辆自身的移动距离来匹配轨迹点。匹配方式为：将轨迹点从起点开始，逐段累加相邻点之间的距离，这样前后两个轨迹点对应于起点的距离范围如果覆盖到了车辆的移动距离，那么就选择前一个点为参考位置，根据移动距离的量用线性差值法求得轨迹上目标位置的曲率，继而由参考位置和目标位置直间的几何关系求出目标位置的横纵坐标。这种方法会引入移动距离的累积误差，最终导致控制性能变差。

发明内容

本发明提供了一种自动泊车控制系统中的轨迹预瞄计算方法和系统，解决了传统横向控制算法会引入移动距离的累积误差，最终导致控制性能变差的问题。

本发明为解决其技术问题，提供了一种自动泊车控制系统中的轨迹预瞄计算方法，包括以下步骤：

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

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

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

进一步地，本发明的一种自动泊车控制系统中的轨迹预瞄计算方法，步骤S1中所述下标为所述轨迹点在所述轨迹点集数组中存放的位置；所述前进点集数组中的轨迹点数据为正值，所述后退点集数组中的轨迹点数据为负值。

进一步地，本发明的一种自动泊车控制系统中的轨迹预瞄计算方法，步骤S1中所述将所述轨迹点集数组分为前进点集数组和后退点集数组的具体步骤为：

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

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

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

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

进一步地，本发明的一种自动泊车控制系统中的轨迹预瞄计算方法，步骤S2中所述遍历计算最近的轨迹点的具体公式为：