存在问题

  • 路径生成的结果只是一群点列，或数字
  • 如果需要验证生成正确与否会很麻烦
  • 如果需要调整参数，无法得知调整方向
  • 如果需要与实际运行轨迹对比，缺乏参考

image.png

  • 缺少轨迹的可视化

解决方案

  • 将轨迹信息导出文件
  • 使用Matlab参考文件中数据的含义绘制路径

  • 实现轨迹的可视化

    文件导出

  • 参考【开发与编程】-【C/C++】-【文件操作】

  • 路径信息在生成后直接保存在全局变量中，可以直接调用 ```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) {

      1. printf("Open file hit.txt error\n");
      2. exit(-1);

      }

      nResult = fprintf(fp, “%d 0 0 0\n”, Num_path);

      for (i = 0; i < Num_path;i++) {

      1. //运动起点
      2. nResult = fprintf(fp, "%d %d 0 0\n", MoveTrack[i].Start.EndPoint.X, 
      3.                   MoveTrack[i].Start.EndPoint.Y);
      5. ...
      7. //终止区直线,起点和终点
      8. nResult = fprintf(fp, "%d %d %d %d \n", MoveTrack[i].EndLine.StartPoint.X, 
      9.                   MoveTrack[i].EndLine.StartPoint.Y, MoveTrack[i].EndLine.EndPoint.X, 
      10.                   MoveTrack[i].EndLine.EndPoint.Y);
      11. //分隔符
      12. nResult = fprintf(fp, "0 0 0 0\n");

      }

      if (nResult < 0){

      1. printf("Write formatted data to hit.txt error\n");
      2. exit(-1);

      }

      printf(“Write formatted data to file completely\n”); fclose(fp);

}

  2. - 数据由txt文件保存，注意数据对齐，用0补充
  4. ![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)
  5. <a name="rxhHD"></a>
  6. ## Matlab导入
  8. - Matlab支持txt文本的数据加载<br />
  9. - 需要提取有效信息，并赋予其应有的含义
  10. ```matlab
  11. %   @brief 路径绘制，绘图并标记数据
  12. load 'E:\Function_LIB\MATLABWORK\路径规划\Num_Array.txt';
  14. StartPoint = Num_Array(2,1:2);
  15. StartVector = Num_Array(3,1:2);
  16. S_barrierPoint = Num_Array(4,1:2);
  17. S_barrierVector = Num_Array(5,1:2);
  18. EndPoint = Num_Array(6,1:2);
  19. EndVector = Num_Array(7,1:2);
  20. E_barrierPoint = Num_Array(8,1:2);
  21. E_barrierVector = Num_Array(9,1:2);
  23. StartLine = Num_Array(10,:);
  24. StartCircle = Num_Array(11,:);
  25. MiddleLine = Num_Array(12,:);
  26. EndCircle = Num_Array(13,:);
  27. 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; ```

实现效果

image.png