将算法写入规划路径

创建一个函数，codegenRRTStar，它使用plannerRRTStar对象在映射中规划从开始姿态到目标姿态的路径。

函数[path,treeData] = codegenRRTStar(mapData,startPose,goalPose)% # codegen创建占用地图。omap = occuancymap (mapData);创建一个状态空间对象。stateSpace = stateSpaceSE2;将状态空间边界更新为与映射限制相同。stateSpace。StateBounds = [omap.XWorldLimits;omap.YWorldLimits;[-pi pi]];使用状态空间和映射构造一个状态验证器对象%的对象。stateValidator = validatorOccupancyMap(stateSpace,Map=omap);设置验证器的验证距离。stateValidator。ValidationDistance = 0.01;创建RRT*路径规划器，并允许进一步优化后的目标是%。减少最大迭代次数，增加最大迭代次数%连接距离。planner = plannerRRTStar(stateSpace,stateValidator，.．.ContinueAfterGoalReached = true,.．.MaxIterations = 2500,.．.MaxConnectionDistance = 1);为给定的起始姿势和目标姿势计算一个路径。[pathObj,solnInfo] = plan(planner,startPose,goalPose);从路径对象中提取路径姿态。path = pathObj.States;从解决方案信息中提取树展开数据%的结构。treeData = solnInfo.TreeData;结束

此函数充当标准RRT*路径规划调用的包装器。它接受标准输入，并返回一个无碰撞的路径作为数组。因为不能使用句柄对象作为代码生成支持的函数的输入或输出，所以在函数内部创建planner对象。保存codegenRRTStar函数在当前文件夹中。

在MATLAB中验证路径规划算法

在使用测试文件生成代码之前，在MATLAB中验证路径规划算法，codegenRRTStar_testbench.m。

生成一个随机的二维迷宫地图。map = mapMaze(5,MapSize=[25 25]，MapResolution=1);mapData =占用矩阵(map);定义开始姿势和目标姿势。startPose = [3 3 pi/2];goalPose = [22 22 0];为指定的开始姿势、目标姿势和映射规划路径。[path,treeData] = codegenRRTStar(mapData,startPose,goalPose);可视化计算路径。表演(occupancyMap (mapData))在%启动状态散射(startPose (1, 1), startPose(1、2),“g”，“填充”）%目标状态散射(goalPose (1, 1), goalPose(1、2),“r”，“填充”）%树展开情节(treeData (: 1) treeData (:, 2),“c . - - - - - -”）%的路径情节(路径(:1),路径(:,2),“r -”线宽= 2)传说(“开始姿势”，“目标姿势”，“树扩张”，“MATLAB生成路径”传奇(位置=“西北”）

路径规划算法生成代码

你可以使用codegen(MATLAB编码器)函数或MATLAB编码器(MATLAB编码器)应用程序生成代码。在本例中，使用MATLAB Coder App生成一个MEX文件。

打开MATLAB编码器App，选择源文件

1.在MATLAB工具条上应用程序选项卡,在代码生成，按MATLAB编码器应用程序图标。应用程序打开选择源文件页面。

2.在选择源文件页中，输入或选择入口点函数的名称codegenRRTStar．入口点函数是用于生成代码的顶级MATLAB函数。应用程序用默认名称创建一个项目codegenRRTStar.prj在当前文件夹中。

3.点击下一个去定义输入类型页面。

定义输入类型

1.输入或选择测试文件codegenRRTStar_testbench.m．

2.点击自动定义输入类型．测试文件，codegenRRTStar_testbench.m，调用入口点函数，codegenRRTStar，使用预期的输入类型。应用程序确定输入mapData输入是逻辑的(25x25)吗startPose是double(1x3)和输入goalPose双(1 x3)。

3.点击下一个去检查运行时问题的一步。

检查运行时问题

1.在检查运行时问题对话框中，指定测试文件或输入代码，使用示例输入调用入口点函数。对于本例，使用测试文件codegenRRTStar_testbench用来定义输入类型的。

2.点击检查问题．应用程序生成一个MEX函数。它运行测试脚本codegenRRTStar_testbench替换对codegenRRTStar调用生成的MEX。如果应用程序在生成或执行MEX函数期间检测到问题，它会提供警告和错误消息。单击这些消息可导航到有问题的代码并修复问题。在本例中，应用程序没有检测到问题。

3.点击下一个去生成代码的一步。

生成MEX文件

1.在生成代码对话框，设置构建类型到MEX和语言其他项目生成配置设置使用默认值。您可以选择生成C/ c++构建类型，而不是生成MEX。对于MEX和C/ c++构建类型，可以使用不同的项目设置。在MEX和C/ c++代码生成之间切换时，请验证所选的设置。

2.点击生成．

MATLAB编码器生成一个MEX文件codegenRRTStar_mex在当前文件夹中。MATLAB Coder应用程序表明代码生成成功。它在页面左侧显示MATLAB源文件和生成的输出文件。在Variables选项卡上，它显示了有关MATLAB源变量的信息。在Target Build Log选项卡上，它显示了构建日志，包括C/ c++编译器警告和错误。默认情况下，在代码窗口中，应用程序显示C源代码文件，codegenRRTStar.c．若要查看不同的文件，请在“源代码”或“输出文件”窗格中单击文件名。

3.点击查看报告，在报表查看器中查看报表。如果代码生成器在代码生成过程中检测到错误或警告，则报告将描述问题并提供到有问题的MATLAB代码的链接。有关更多信息，请参见代码生成报告(MATLAB编码器)．

4.点击下一个打开完成工作流程页面。

检查完成工作流页面

的完成工作流程Page表示代码生成成功。它提供了项目摘要和生成输出的链接。

使用生成的MEX函数验证结果

通过为指定的开始姿态、目标姿态和映射调用MEX版本的路径规划算法来规划路径。

mexPath = codegenRRTStar_mex(mapData,startPose,goalPose);

通过MEX版本的路径规划算法计算出的路径可视化。

情节(mexPath (: 1) mexPath (:, 2),”乙:“线宽= 2)传说(“开始姿势”，“目标姿势”，“树扩张”，“MATLAB生成路径”，"MEX生成路径"传奇(位置=“西北”)举行从

检查生成代码的性能

将生成的MEX函数的执行时间与原始函数的执行时间进行比较，使用时间．

time = timeit(@() codegenRRTStar(mapData,startPose,goalPose))

Time = 1.5130424783

mexTime = timeit(@() codegenRRTStar_mex(mapData,startPose,goalPose))

mexTime = 0.1934125783

时间/ mexTime

Ans = 7.82287528349442

本例中，MEX函数的运行速度提高了7倍以上。结果可能因系统而异。

使用生成的MEX函数在新建地图中规划路径

为新的开始规划一条路径，并在新地图中设定目标。新映射的大小必须与生成MEX函数时使用的映射相同。

生成一个随机的二维迷宫地图。

mapNew = mapMaze(5,MapSize=[25 25]，MapResolution=1);mapDataNew =占用矩阵(mapNew);

指定开始姿势和目标姿势。

startposennew = [22 3 pi/2];goalposennew = [3 22 0];

为指定的开始姿势、目标姿势和地图规划路径。

[pathNew, treedatrenew] = codegenRRTStar_mex(mapdatrenew,startPoseNew,goalPoseNew);

可视化由MEX函数计算的新路径。

表演(occupancyMap (mapDataNew))在%启动状态散射(startPoseNew (1, 1), startPoseNew(1、2),“g”，“填充”）%目标状态散射(goalPoseNew (1, 1), goalPoseNew(1、2),“r”，“填充”）%树展开情节(treeDataNew (: 1) treeDataNew (:, 2),“c . - - - - - -”）%的路径情节(pathNew (: 1) pathNew (:, 2),“r -”线宽= 2)传说(“开始姿势”，“目标姿势”，“树扩张”，"MEX生成路径"传奇(位置=“东北”）