与RRT为固定翼UAV的运动计划
此示例使用快速探索随机树（RRT）算法在三维地图上给出了快速探索的随机树（RRT）算法，演示了固定翼无人驾驶飞行器（UAV）的运动规划。在航点之间移动时，固定翼UAV是非完整的，并且必须遵守最大滚动角度，飞行路径角和空速等空气动力学约束。
在此示例中，您将设置三维地图，提供启动姿势和目标姿势，使用3-D Dubins Motion原语进行RRT的路径，平滑所获得的路径，并模拟UAV的飞行。
％设置RNG种子以获得可重复的结果RNG（1，“旋风”）
加载地图
加载3-D占用映射uavmapcityblock.mat，它包含一组预先生成的障碍。将地图均匀膨胀1米，以增加路径安全性，并考虑到固定翼无人机的翼展。占用地图处于ENU(东-北-上)框架中。
mapData =负载(“uavMapCityBlock.mat”那“OMAP”);omap = mapData.omap;%认为未知空间未被占用Omap.freethreshold = Omap.occupiedthreshold;充气（OMAP，1）图（“名称”那“CityBlock”）显示（OMAP）
设置开始姿势和目标姿势
使用地图进行参考，选择一个未占用的开始姿势和目标姿势。
血统= [12 22 25 pi / 2];守门= [150 180 35 pi / 2];数字（“名称”那“startandgoal”）HMAP = SHOW（OMAP）;抓住在散射3（HMAP，血统（1），纵向（2），纵向（3），30，“红色”那“填充”) scatter3 (hMap goalPose (1) goalPose (2), goalPose(3), 30岁,“绿色”那“填充”） 抓住从查看（[ - 31 63]）
使用3d Dubins运动原语用RRT规划路径
RRT是一个基于树的运动规划器，它从给定状态空间的随机样本中逐步构建搜索树。这棵树最终会扩展搜索空间，并连接开始状态和目标状态。连接两个状态使用UavdubinsConnection.满足空气动力学约束的物体。使用validatorOccupancyMap3D固定翼UAV与环境之间的碰撞检查的对象。
定义状态空间对象
此示例提供了预定义的状态空间，examplehelperuavstatespace.，用于路径规划。状态空间定义为[x y z headingangle],在那里[x y z]指定无人机和的位置headingAngle指定以弧度为单位的航向角。该示例使用UavdubinsConnection.对象作为UAV的运动模型，其受最大滚角，空速和飞行路径角度约束。通过指定最大滚动角度，空速和飞行路径角度将UAV的属性限制为名称值对来创建状态空间对象。使用“界限”名称值对参数指定UAV作为4×2矩阵的位置和方向边界，其中前三行代表X- - - - - -,y-， 和Z.3-D占用图中的XIS边界，最后一行表示该范围内的标题角度[-pi，pi]弧度。
党卫军= ExampleHelperUAVStateSpace (“MaxRollAngle”，pi / 6，......“空速”，6，......“飞行斯坦格林”(-0.1 - 0.1),......“界限”, (-20 220;-20 220;10 100;π-π]);
根据目标目标姿势设置工作空间的阈值界限。该阈值决定了目标姿势周围的目标工作区目标区域的大大是多大的，其用于工作空间目标区域方法的偏置采样。
threshold = [(goalPose-0.5)' (goalPose+0.5)';π-π);
使用SetWorkspaceGoAlRegion.功能更新目标姿势和它周围的区域。
SetWorkspaceGoAlRegion（SS，守门，阈值）
定义状态验证器对象
这validatorOccupancyMap3D对象确定状态无效（如果）xyz- 位置在地图上占用。只有当所有中间状态有效时，两个状态之间的运动才有效，这意味着UAV不会通过地图上的任何占用位置。创建一个validatorOccupancyMap3D对象，通过指定状态空间对象和膨胀的地图。然后设置验证距离(以米为单位)，用于在状态之间进行插值。
sv = validatorOccupancyMap3D(党卫军,“地图”，Omap）;sv.ValidationDistance = 0.1;
设置RRT路径规划器
创建一个Plannerrrt.通过将状态空间和状态验证器指定为输入来实现对象。设定MaxConnectionDistance那球门店,MaxIterationsPlanner对象的属性，然后指定自定义目标函数。如果与目标的欧几里德距离低于5μm的阈值，则该目标函数确定路径已达到目标。
Planner = PlannerRRT（SS，SV）;Planner.MaxConnectionDistance = 50;planner.goalbias = 0.10;Planner.maxIterations = 400;Planner.GoAlReachedFCN = @（〜，x，y）（qu（x（1：3）-y（1：3））<5）;
执行路径规划
在3-D空间中执行基于RRT的路径规划。策划者找到一个无碰撞，适合固定翼飞行的路径。
[pthObj, solnInfo] =计划(规划师、startPose goalPose);
模拟无人机沿着规划的路径飞行
可视化计划的路径。基于UAV Dubins连接内插对计划路径。将内插状态作为绿线绘制。
使用提供的辅助功能模拟UAV飞行，exampleHelperSimulateUAV，这需要航点，空速和时间达到目标（基于空速和路径长度）。辅助功能使用固定翼翼基于航路点产生的控制输入，仿真无人机行为的制导模型。用红线标出模拟状态。
注意，由于控制跟踪误差小，模拟的无人机飞行稍微偏离了计划的路径。此外，三维杜宾路径假设无人机滚转角度的瞬时变化，但实际动力学对滚转命令的响应较慢。补偿这种滞后的一种方法是用更保守的空气动力学约束来规划路径。
如果(solnInfo.IsPathFound)图(“名称”那“OriginalPath”）％可视化3-D图显示(omap)在散射3（纵向（1），血统（2），纵向（3），30，“红色”那“填充”）散射3（守门（1），守门（2），靶向（3），30，“绿色”那“填充”）InterpolatedPathobj = Copy（Pthobj）;插值（InterpolatePathobj，1000）%根据无人机Dubins连接绘制插值路径hreference = plot3（InterpolatePathobj.states（：，1），......interpolatedPathObj.States (:, 2),......interpolatedPathObj.States (:, 3),......“行宽”2,“颜色”那“G”);基于固定翼引导模型的％绘制模拟UAV轨迹％计算航班总时间并添加缓冲区timeToReachGoal = 1.05 *通路长度(pthObj) / ss.AirSpeed;路点= interpolatedPathObj.States;[xENU、射弩、zENU] = exampleHelperSimulateUAV(锚点,ss.AirSpeed timeToReachGoal);hSimulated = plot3 (xENU、射弩、zENU,“行宽”2,“颜色”那“r”);传奇（[hreference，hsimulated]，“参考”那“模拟”那“位置”那“最好”） 抓住从查看（[ - 31 63]）结束
平滑Dubins路径并模拟无人机轨迹
最初计划的路径会在朝着目标前进时产生一些不必要的转弯。利用实例提供的路径平滑算法简化三维Dubins路径，examplehelperuavpathsmoothing.。该函数基于迭代策略删除中间的3d杜宾姿态。有关平滑策略的更多信息，请参见[1]。只要这样做，平滑功能将彼此连接不顺序3-D Dubins姿势不会导致碰撞。该过程产生的光滑路径可提高固定翼仿真模型的跟踪特性。使用这些新的平滑航点模拟固定翼UAV模型。
如果（solninfo.ispathfound）smoothwaypointsobj = examplehelperuavpathsmooth（ss，sv，pthobj）;数字（“名称”那“SmoothedPath”）绘制三维地图显示(omap)在散射3（纵向（1），血统（2），纵向（3），30，“红色”那“填充”）散射3（守门（1），守门（2），靶向（3），30，“绿色”那“填充”）InterpolateDsMoothWayPoints = Copy（SmoothwayPointSobj）;插值（内部插孔，1000）基于UAV DUBINS连接的％绘制平滑路径hReference = plot3 (interpolatedSmoothWaypoints.States (: 1),......interpolatedSmoothWaypoints.States (:, 2),......interpolatedSmoothWaypoints.States (:, 3),......“行宽”2,“颜色”那“G”);基于固定翼制导模型绘制仿真飞行轨迹路点= interpolatedSmoothWaypoints.States;timeToReachGoal = 1.05 *通路长度(smoothWaypointsObj) / ss.AirSpeed;[xENU、射弩、zENU] = exampleHelperSimulateUAV(锚点,ss.AirSpeed timeToReachGoal);hSimulated = plot3 (xENU、射弩、zENU,“行宽”2,“颜色”那“r”);传奇（[hreference，hsimulated]，“SmoothedReference”那“模拟”那“位置”那“最好”） 抓住从查看（[ - 31 63]）结束
平滑的路径更短，显示出改进的跟踪整体。
参考
[1]胡子，兰德尔W.和Timothy W. Mclain。小型无人机:理论与实践。普林斯顿，新泽西州:普林斯顿大学出版社，2012。
[2] Hornung，Armin，Kai M. Wurm，Maren Bennewitz，Cylill Stachniss和Wolfram Burgard。“Octomap：基于Octrees的一个有效的概率3D映射框架。”自治机器人34，不。3（2013年4月）：189-206。https://doi.org/10.1007/S10514-012-9321-0.。
您也可以从以下列表中选择一个网站:
