自动驾驶计算本车离期望轨迹距离的方法

推荐使用点到直线距离进行计算：
首先找到离本车最近点，在取改点的前后点最为直线的端点，计算直线到本车的质点的距离：

function [dist]  =  FindDistance (p1, p2, currentP) % find the distance between the current points to the curve line

A = p1(2)- p2(2);
B = p2(1) - p1(1);
C = p1(1) * p2(2) - p1(2)* p2(1);

dist = abs((A* currentP(1) + B* currentP(2) + C)/(sqrt( (A)^2 + (B)^2)));

end