存在问题
- 路径生成的结果只是一群点列,或数字
- 如果需要验证生成正确与否会很麻烦
- 如果需要调整参数,无法得知调整方向
- 如果需要与实际运行轨迹对比,缺乏参考
- 缺少轨迹的可视化
解决方案
- 将轨迹信息导出文件
- 使用Matlab参考文件中数据的含义绘制路径
-
文件导出
路径信息在生成后直接保存在全局变量中,可以直接调用 ```c /**
- @brief 文件操作,将路径生成所得参数导入到txt文件
@retval 无 / void OUTPUT_Params(void) { FILE fp; int nResult; int i = 0; fp = fopen(“E:/Function_LIB/MATLABWORK/路径规划/Num_Array.txt”, “w+”); //新建文本文件 if (NULL == fp) {
printf("Open file hit.txt error\n");
exit(-1);
}
nResult = fprintf(fp, “%d 0 0 0\n”, Num_path);
for (i = 0; i < Num_path;i++) {
//运动起点
nResult = fprintf(fp, "%d %d 0 0\n", MoveTrack[i].Start.EndPoint.X,
MoveTrack[i].Start.EndPoint.Y);
...
//终止区直线,起点和终点
nResult = fprintf(fp, "%d %d %d %d \n", MoveTrack[i].EndLine.StartPoint.X,
MoveTrack[i].EndLine.StartPoint.Y, MoveTrack[i].EndLine.EndPoint.X,
MoveTrack[i].EndLine.EndPoint.Y);
//分隔符
nResult = fprintf(fp, "0 0 0 0\n");
}
if (nResult < 0){
printf("Write formatted data to hit.txt error\n");
exit(-1);
}
printf(“Write formatted data to file completely\n”); fclose(fp);
}
- 数据由txt文件保存,注意数据对齐,用0补充
![image.png](https://cdn.nlark.com/yuque/0/2020/png/427268/1587648536262-c3174cff-a844-413d-9125-6897619b02ff.png#align=left&display=inline&height=505&margin=%5Bobject%20Object%5D&name=image.png&originHeight=911&originWidth=455&size=47239&status=done&style=none&width=252)
<a name="rxhHD"></a>
## Matlab导入
- Matlab支持txt文本的数据加载<br />
- 需要提取有效信息,并赋予其应有的含义
```matlab
% @brief 路径绘制,绘图并标记数据
load 'E:\Function_LIB\MATLABWORK\路径规划\Num_Array.txt';
StartPoint = Num_Array(2,1:2);
StartVector = Num_Array(3,1:2);
S_barrierPoint = Num_Array(4,1:2);
S_barrierVector = Num_Array(5,1:2);
EndPoint = Num_Array(6,1:2);
EndVector = Num_Array(7,1:2);
E_barrierPoint = Num_Array(8,1:2);
E_barrierVector = Num_Array(9,1:2);
StartLine = Num_Array(10,:);
StartCircle = Num_Array(11,:);
MiddleLine = Num_Array(12,:);
EndCircle = Num_Array(13,:);
EndLine = Num_Array(14,:);
图形绘制
- 依靠提取信息绘制图形
- 使用Matlab提供的绘图函数 ```matlab
hold on; grid on;
%找点 plot(StartPoint(1),StartPoint(2),’g’); plot(S_barrierPoint(1),S_barrierPoint(2),’r’); plot([S_barrierPoint(1) S_barrierPoint(1)+S_barrierVector(1)], [S_barrierPoint(2) S_barrierPoint(2)+S_barrierVector(2)],’-b’,’LineWidth’,3); plot(EndPoint(1),EndPoint(2),’g’); plot(E_barrierPoint(1),E_barrierPoint(2),’r’); plot([E_barrierPoint(1) E_barrierPoint(1)+E_barrierVector(1)], [E_barrierPoint(2) E_barrierPoint(2)+E_barrierVector(2)],’-b’,’LineWidth’,3);
%画图 % LineStart = @(x,y) StartLine(1)x+StartLine(2)y+StartLine(3); % fimplicit(LineStart,’—r’,’LineWidth’,1);
CircleStart = @(x,y) (x-StartCircle(2))^2+(y-StartCircle(3))^2-StartCircle(1)^2; fimplicit(CircleStart,’—g’,’LineWidth’,1);
% LineMiddle = @(x,y) MiddleLine(1)x+MiddleLine(2)y+MiddleLine(3); % fimplicit(LineMiddle,’—b’,’LineWidth’,1);
CircleEnd = @(x,y) (x-EndCircle(2))^2+(y-EndCircle(3))^2-EndCircle(1)^2; fimplicit(CircleEnd,’—y’,’LineWidth’,1);
% LineEnd = @(x,y) EndLine(1)x+EndLine(2)y+EndLine(3); % fimplicit(LineEnd,’—m’,’LineWidth’,1);
%d画端点 StartLinePoint = Num_Array(15,:); StartCirclePoint = Num_Array(16,:); MiddleLinePoint = Num_Array(17,:); EndCirclePoint = Num_Array(18,:); EndLinePoint = Num_Array(19,:);
plot(StartLinePoint(1),StartLinePoint(2),’+g’); plot(StartLinePoint(3),StartLinePoint(4),’+g’); plot([StartLinePoint(1) StartLinePoint(3)],[StartLinePoint(2) StartLinePoint(4)],’—r’); plot(StartCirclePoint(1),StartCirclePoint(2),’+g’); plot(StartCirclePoint(3),StartCirclePoint(4),’+g’); plot(MiddleLinePoint(1),MiddleLinePoint(2),’+g’); plot(MiddleLinePoint(3),MiddleLinePoint(4),’+g’); plot([MiddleLinePoint(1) MiddleLinePoint(3)],[MiddleLinePoint(2) MiddleLinePoint(4)],’—r’); plot(EndCirclePoint(1),EndCirclePoint(2),’+g’); plot(EndCirclePoint(3),EndCirclePoint(4),’+g’); plot(EndLinePoint(1),EndLinePoint(2),’+g’); plot(EndLinePoint(3),EndLinePoint(4),’+g’); plot([EndLinePoint(1) EndLinePoint(3)],[EndLinePoint(2) EndLinePoint(4)],’—r’); axis([-5000 5000 -20000 20000]); axis equal; grid minor; ```