MATLAB 车辆状态笛卡尔坐标系转换 Frenet 坐标系函数

function [s, d] = cartesian2frenet(x, y, psi, xc, yc) % x: 车辆当前位置的x坐标 % y: 车辆当前位置的y坐标 % psi: 车辆当前朝向角度 % xc: 参考线x坐标向量 % yc: 参考线y坐标向量

% 计算车辆当前位置在参考线上的投影点 d = inf; s = 0; for i = 1:length(xc) x0 = xc(i); y0 = yc(i); dx = x - x0; dy = y - y0; dist = sqrt(dx^2 + dy^2); if dist < d d = dist; s = i; end end

% 计算车辆当前位置在参考线上的切向角度 psi_r = atan2(yc(s+1)-yc(s), xc(s+1)-xc(s));

% 计算车辆在frenet坐标系下的坐标 d = sign((yc(s+1)-yc(s))(x-xc(s)) - (xc(s+1)-xc(s))(y-yc(s)))*d; psi_d = psi - psi_r; s = s - 1; % 将索引从1开始变为从0开始

end