问题具体描述
求traj上当前位置在reference_line上的match_point(匹配点)
匹配点定义
位于reference_line上，且在该点处的切线与车辆当前位置和匹配点连成的线相垂直
步骤一、遍历reference_line，取与车辆当前位置点A最近的点A’
步骤二、取A’前后点A_f,A_b,，并将两点的连线看作A’点的切线(也可直接取A’点处的切线)
步骤三、计算车辆当前位置A点到A’切线的投影点的s值
方法1(apollo).计算AA’到A’切线的投影，记投影长度为Δs，s=s_A_f+Δs
方法2.计算AA_f到A’切线的投影，记投影长度为Δs，s=s_A’+Δs
[备注]：方法2的计算相对复杂，需要考虑A’切线与AA_f夹角是钝角还是锐角，这会影响到Δs的正负，而方法1巧妙的规避了这个问题
步骤四、根据一阶插值，计算match_point的剩余信息，包括笛卡尔坐标系下的x，y；frenet坐标系下的s,l,s’,l’,s’‘,l’',曲率以及航向角。
[备注]：航向角的计算需要注意角度的归一化(-pi,pi)
