[发明专利]一种用于车辆自动驾驶机器人的轨迹跟踪控制方法

权利要求书

1.一种用于车辆自动驾驶机器人的轨迹跟踪控制方法，其特征在于，该方法具有如下步 骤：

步骤1：以数据点的形式描述车辆的期望轨迹并存储在轨迹跟踪和速度控制器中，所述 的车辆的期望轨迹包括期望的位置坐标、道路切向角和期望车速，所述期望的位置坐标是位于 绝对坐标系中的坐标；

步骤2：实时获取车辆当前的位置信息，包括绝对坐标系下的车辆纵向、横向坐标，行驶 方向和行驶速度，并将车辆当前的位置信息发送给轨迹跟踪和速度控制器；

步骤3：在车辆当前位置，按照车辆当前的行驶方向进行仿人驾驶，向前方预瞄距离L， 并确定预瞄得到的位置坐标(X_p，Y_p)：

Xp=Xn0+L·cosθn0Yp=Yn0+L·sinθn0]]>

其中，(X_n0，Y_n0)为在绝对坐标系下的车辆当前位置坐标，θ_n0为在绝对坐标系下的车辆当 前的道路切向角；

步骤4：根据预瞄得到的位置坐标(X_p，Y_p)与步骤1中描述的期望轨迹进行比较，进行预 瞄点搜索，确定预瞄得到的坐标位置相对于期望轨迹的侧向位置偏差、当前车辆行驶方向与 期望轨迹的角度偏差以及当前车辆行使速度与期望轨迹的车速偏差；

步骤5：轨迹跟踪和速度控制器采用PID控制器，轨迹跟踪和速度控制器根据位置偏差 和方向偏差联合控制车辆方向，速度控制采用模糊控制方法确定油门踏板和制动踏板的控制 量；

步骤4中所述的进行预瞄点搜索的方法如下：

第一步：以期望轨迹中的第i个期望位置点s_i作为初始搜索点，所述的点s_i为与车辆当前 位置最近的坐标位置点，在首次搜索过程中，点s_i为期望轨迹中的第1个位置点s₁；

第二步：在期望轨迹中，从初始搜索点s_i开始，顺序读取后面所要搜索的道路段的位置 点的信息；具体所要搜索的道路段的位置点的个数通过车辆当前车速、车辆位置更新时间间 隔和预瞄距离确定搜索长度，再结合期望轨迹中位置点的间隔确定；

第三步：计算预瞄得到的位置(X_p，Y_p)到第二步中所要搜索的道路段中的各期望位置点的 距离，筛选出距离最短的期望位置点，设筛选出来的位置点为第k个期望位置点m_k，期望位 置点m_k的坐标位置为(X_pk，Y_pk)，期望道路切向角为θ_pk，期望车速为u_pk；

第四步：取期望位置点m_k左右两侧相邻的期望位置点m_k-1和m_k+1，利用线性插值的方法 获取夹于m_k-1和m_k+1两点之间的预瞄点在绝对坐标系下的坐标，然后将绝对坐标系下的预瞄 点的坐标转换为车辆坐标系下的坐标，最后获得预瞄得到的坐标位置相对于期望轨迹的侧向 位置偏差e_pk，近似得到当前车辆行驶方向与期望车道线方向的角度偏差e_θk＝θ_pk-θ_n0，近似 得到当前车速偏差e_puk＝u_pk-u；

第五步：同第三步过程搜索车辆当前位置与所要搜索道路内的最近的期望位置点s_k，将 得到的期望位置点s_k作为下次搜索的初始搜索点。