基于Carla的EMplanner教程四：匹配点和投影算法

匹配点算法和投影

理论方面可以参考这篇博客讲的十分细致：如何理解Apollo中的横向误差和纵向误差

参考线作为后序规划和控制得基础，必须要生成的足够快。提高速度方法可以：

• 减少规划频率，规划算法每100ms执行一次，控制算法每10ms执行一次
• 充分利用上一周期的规划结果

找匹配点

找到离散轨迹规划点中与车辆真实位置(x,y)最近的点，在Apollo中称之为match_point匹配点。将车辆投影到规划轨迹最近的位置的点称之为投影点。匹配点并不等于投影点，因为匹配点属于离散点，而实际投影是对轨迹做垂线找的最近点。但是可以通过匹配点来估算出投影点。只有当规划的离散轨迹点足够密集的时候，才可以近似将匹配点当作投影点。

由匹配点计算出投影点：

假设：匹配点----> 投影的曲率不变 -----》 匹配------>投影的轨迹近似用圆弧代替。

首先计算出匹配点的切向方向单位向量和垂线方向法向量：
$$\begin{gathered} {\vec{\tau }}_m=\left(\cos {\theta }_m,\sin {\theta }_m\right) \\ {\vec{n}}_m=\left(-\sin {\theta }_m,\cos {\theta }_m\right) \end{gathered}$$
其中$\tau$是切向方向向量，$n$是法向向量。接着还要计算出匹配点指向车辆真实距离(x, y)的矢量：
$$d\_v=\left(\vec{x}-{\vec{x}}_m,\vec{y}-{\vec{y}}_m\right)$$
通过上述已知量可以得到投影点方向的s速度: $\dot{s}=d_v\ast \vec{\tau }$， 由此用下方公式计算出投影点的x,y 坐标：
$$\left(x_r,y_r\right)=\left(x_m,y_m\right)+\dot{s}\cdot \tau$$
接着计算投影点的航向角度：
$${\theta }_r={\theta }_m+k_m\cdot \dot{s}$$
之前假设曲率不变，因此：
$$k_r=k_m$$
如何快速找到匹配点，将整个路径点进行遍历找最近点太时间。因此可以用上一个周期的匹配点作为起点做遍历，一旦有$l_{i+1}>l_i$ ， 立刻退出遍历， $l_i$的点作为匹配点。同时，还要考虑到遍历的方向，因为车会向前走和向后走。

但是这个方法只适用于上一个匹配点附近只有一个极小值点，随便按照100ms规划周期，即使车速很快，车也走不了太远，因此通常上一个规划周期的匹配点附近应该都只有一个极小值点。但是为了保险起见，可以设计一个变量increase count表示$l$连续新增的次数，当连续新增n次以后，如果还是新增，说明附近只有一个极小值点。

实际代码中，需要区分是否是第一次运行，如果是第一次运行就需要从头开始遍历找匹配点，否则就从上一个规划周期作为起点开始遍历。

	i = -1  # 提前定义一个变量用于遍历曲线上的点
    input_xy_length = len(xy_list)
    frenet_path_length = len(frenet_path_node_list)

    match_point_index_list = np.zeros(input_xy_length, dtype="int32")
    project_node_list = []  # 最终长度和input_xy_length应该相同

    if is_first_run is True:
        for index_xy in range(input_xy_length):  # 为每一个点寻找匹配点
            x, y = xy_list[index_xy]
            start_index = 0
            # 用increase_count记录distance连续增大的次数，避免多个局部最小值的干扰
            increase_count = 0
            min_distance = float("inf")
            # 确定匹配点
            for i in range(start_index, frenet_path_length):
                frenet_node_x, frenet_node_y, _, _ = frenet_path_node_list[i]
                # 计算（x,y) 与 （frenet_node_x, frenet_node_y） 之间的距离
                distance = math.sqrt((frenet_node_x - x) ** 2 + (frenet_node_y - y) ** 2)
                if distance < min_distance:
                    min_distance = distance  # 保留最小值
                    match_point_index_list[index_xy] = i
                    increase_count = 0
                else:
                    increase_count += 1
                    if increase_count >= 50:  # 向后50个点还没找到的话，说明当前最小值点及时最优的
                        # 第一次运行阈值较大，是为了保证起始点匹配的精确性
                        break
            # 通过匹配点确定投影点
            # xy_list中取出第一个(x,y)的匹配点目标索引
            match_point_index = match_point_index_list[0]
            x_m, y_m, theta_m, k_m = frenet_path_node_list[match_point_index]  # 获取匹配点的信息
            d_v = np.array([x - x_m, y - y_m])  # 由匹配点指向(x,y)的矢量。
            # tou是指匹配点位置的切向方向向量。
            tou_v = np.array([np.cos(theta_m), np.sin(theta_m)])
            # 投影点方向的s速度
            ds = np.dot(d_v, tou_v)
            r_m_v = np.array([x_m, y_m])  # 参考点坐标
            # 根据公式计算投影点的位置信息
            # 计算投影点坐标，匹配点切向方向的单位向量*d_s
            x_r, y_r = r_m_v + ds * tou_v
            theta_r = theta_m + k_m * ds  # 计算投影点在frenet曲线上切线与X轴的夹角
            k_r = k_m  # 投影点在frenet曲线处的曲率
            # 将结果打包放入缓存区
            project_node_list.append((x_r, y_r, theta_r, k_r))

如果不是第一次运算的话，需要计算遍历方向：

start_index = pre_match_index
# 用increase_count记录distance连续增大的次数，避免多个局部最小值的干扰
increase_count = 0
# 上个周期匹配点坐标
pre_match_point_xy = [frenet_path_node_list[start_index][0], frenet_path_node_list[start_index][1]]
pre_match_point_theta_m = frenet_path_node_list[start_index][2]
# 上个匹配点在曲线上的切向向量 ， 用来判断方向
pre_match_point_direction = np.array([np.cos(pre_match_point_theta_m), np.sin(pre_match_point_theta_m)])
# 计算上个匹配点指向当前(x, y）的向量
pre_match_to_xy_v = np.array([x - pre_match_point_xy[0], y - pre_match_point_xy[1]])
# 计算pre_match_to_xy_v在pre_match_point_direction上的投影，用于判断遍历方向
# 大于零，说明往前走，小于零就是后退了。
flag = np.dot(pre_match_to_xy_v, pre_match_point_direction)  # 大于零正反向遍历，反之，反方向遍历

判断完方向就是正常找匹配点的步骤：

			min_distance = float("inf")
            if flag > 0:
                for i in range(start_index, frenet_path_length):
                    frenet_node_x, frenet_node_y, _, _ = frenet_path_node_list[i]
                    # 计算（x,y) 与 （frenet_node_x, frenet_node_y） 之间的距离
                    distance = math.sqrt((frenet_node_x - x) ** 2 + (frenet_node_y - y) ** 2)
                    if distance < min_distance:
                        min_distance = distance  # 保留最小值
                        match_point_index_list[index_xy] = i
                        increase_count = 0
                    else:
                        increase_count += 1
                        if increase_count >= 5:  # 为了加快速度，这里阈值为5，第一个周期是不同的，向后5个点还没找到的话，说明当前最小值点及时最优的
                            break
            else:
                for i in range(start_index, -1, -1):
                    frenet_node_x, frenet_node_y, _, _ = frenet_path_node_list[i]
                    # 计算（x,y) 与 （frenet_node_x, frenet_node_y） 之间的距离
                    distance = math.sqrt((frenet_node_x - x) ** 2 + (frenet_node_y - y) ** 2)
                    if distance < min_distance:
                        min_distance = distance  # 保留最小值
                        match_point_index_list[index_xy] = i
                        increase_count = 0
                    else:
                        increase_count += 1
                        if increase_count >= 5:
                            break

最后是通过匹配点来计算出投影点的(x,y)和${\theta }_r$以及k。

# 通过匹配点确定投影点
match_point_index = match_point_index_list[0]
x_m, y_m, theta_m, k_m = frenet_path_node_list[match_point_index]
d_v = np.array([x - x_m, y - y_m])
tou_v = np.array([np.cos(theta_m), np.sin(theta_m)])
ds = np.dot(d_v, tou_v)
r_m_v = np.array([x_m, y_m])
# 根据公式计算投影点的坐标信息
x_r, y_r = r_m_v + ds * tou_v
theta_r = theta_m + k_m * ds
k_r = k_m
# 将结果打包放入缓存区
project_node_list.append((x_r, y_r, theta_r, k_r))