基于matlab函数的无人驾驶Frenet和大地坐标系转换

referencePathFrenet

介绍

  这个 referencePathFrenet 对象将一条光滑的、分段的、连续曲线拟合到一组作为 [x y] 或 [x y theta] 输入的路点上。拟合完成后，曲线上的点和路径点将以 [x y theta kappa dkappa s] 的形式表示，如下图所示。

笛卡尔坐标系采点平滑

示例代码

clc
clear
% 加载数据
load 'path_x_y'
%将给定的数据转化为直角坐标系并且进行平滑
global_smooth = referencePathFrenet(r);
%给定一个Frenet上的点，然后基于参考线，转化为笛卡尔坐标系。
globalstate=global_smooth.frenet2global([1 0 0 0 0 0 ]);

效果展示

trajectoryGeneratorFrenet

介绍

  意思就是在给定的直角坐标系的路径上，基于这个路径生成一个基于五次多项式或者四次多项式生成的Frenet坐标系。如下图所示【s ds dds l dl ddl】

示例代码

clc
clear
%% 构造道路
waypoints = [0 0; ...
	50 20; ...
	100 0; ...
	150 10];
%按照上面的函数点生成一直角坐标系的数值道路 
refPath = referencePathFrenet(waypoints);
%%展示这个道路
show(refPath);
%%用基于Frenet的五次多项式连接
connector = trajectoryGeneratorFrenet(refPath);

%% 接着 我们准备生成路径起点到路径30m处的5秒轨迹，Frenet坐标系表示
%设置起始的状态
initState = [0 0 0 0 0 0];  % [S ds ddS L dL ddL]
termState = [30 0 0 0 0 0]; % [S ds ddS L dL ddL]
%调用函数 `connect`，其中 `connector` 是连接器对象，`initState` 是初始状态，`termState` 是终止状态，`5` 是步长。该语句返回一个包含全局轨迹的结构体 `trajGlobal`。
[a,trajGlobal] = connect(connector,initState,termState,5); %这个返回的是全局坐标系  a是Frenet坐标系，trajGlobal是大地坐标系
% a= connect(connector,initState,termState,5);
hold on
axis equal
plot(trajGlobal.Trajectory(:,1),trajGlobal.Trajectory(:,2),'b')
legend(["Waypoints","Reference Path","Trajectory to 30m"])

frenet2global与frenet2global

介绍

  前者是将Frenet坐标系上的点转化为笛卡尔坐标系，后者相反。这两个函数都比较简单，更多的应用细节，可以查看matlab的官方帮助文档。这里需要注意的是这两个函数的调用格式。

示例代码

clc
clear
% 加载数据
% load 'path_x_y'
% %将给定的数据转化为直角坐标系并且进行平滑
% global_smooth = referencePathFrenet(r);
% %给定一个Frenet上的点，然后基于参考线，转化为笛卡尔坐标系。
% globalstate=global_smooth.frenet2global([1 0 0 0 0 0 ]);
% show(global_smooth)

%% 生成全局坐标系
waypoints = [0 0; ...
	50 20; ...
	100 0; ...
	150 10];
refPath = referencePathFrenet(waypoints);
show(refPath);
%在这个笛卡尔坐标系上生成基于Frenet的坐标
connector1 = trajectoryGeneratorFrenet(refPath);

initCartState = refPath.SegmentParameters(1,:);%找到refPath中SegmentParameters的第一行所有列作为初始状态
initFrenetState = global2frenet(refPath,initCartState); %使用这个命令将这个初始状态的点转化为Frenet


a=[30 zeros(1,5)];
%准备生成一条从路径起点到路径下方30米处的五秒轨迹，按照弗伦特（Frenet）坐标系表示。
termFrenetState = initFrenetState + [30 zeros(1,5)]; %先让重终点的S到30 S=30
%规划的时间是5S
[frenetTraj,trajGlobal]= connect(connector1,initFrenetState,termFrenetState,5);
hold on
axis equal

plot(trajGlobal.Trajectory(:,1),trajGlobal.Trajectory(:,2),'b')

%以下这两个式子取得的结果都是一样的，都能够用进行调用
globalTraj1 = refPath.frenet2global(frenetTraj.Trajectory);
% globalTraj = frenet2global(refPath,frenetTraj.Trajectory);

应用扩展

寻找临近点

  首先基于上述函数，生成一条基于笛卡尔坐标系的路径，然后在这个坐标系上进行撒点。如下图：

  接下来利用closestPoint函数找到撒的这些点在参考线上的最近的点——投影点，如下图的蓝色点所示：
  接下来我们选取前两个蓝色的点，然后利用linspace函数进行采样间隔为10的离散化。根据采样的点的S，在参考路径上找到相对应的点。并且获得这些点的[x,y,θ,[…],[…],[…]]，最后绘制的图形如下：