基于RRT的固定翼无人机运动规划
本例演示了一架固定翼无人机(UAV)的运动规划，使用快速探索随机树(RRT)算法给出了三维地图上的起始和目标姿态。固定翼无人机本质上是非完整的，在路径点之间移动时必须服从最大滚转角、飞行路径角和空速等气动约束。
在本例中，您将设置一个3-D地图，提供起始姿态和目标姿态，使用3-D杜宾斯运动原语用RRT规划路径，平滑获得的路径，并模拟无人机的飞行。
为可重复的结果设置RNG种子rng (1,“旋风”）;
加载地图
加载三维占用地图uavMapCityBlock.mat，其中包含一组预先生成的障碍。占用图采用ENU(东-北-上)框架。
mapData = load(“uavMapCityBlock.mat”，“核心”）;omap = mapData.omap;%认为未知空间未被占用的核心。FreeThreshold = omap.OccupiedThreshold;
使用地图作为参考，选择一个未占用的开始姿势和目标姿势。
startPose = [12 22 25 pi/2];goalPose = [150 180 35 pi/2];图(“名称”，“StartAndGoal”) hMap = show(omap);持有在scatter3 (hMap startPose (1) startPose (2), startPose(3), 30岁,“红色”，“填充”) scatter3 (hMap goalPose (1) goalPose (2), goalPose(3), 30岁,“绿色”，“填充”)举行从视图(63 [-31])
使用3d Dubins运动原语规划RRT路径
RRT是一种基于树的运动规划器，它从给定状态空间的随机样本中增量地构建搜索树。树最终跨越搜索空间，并连接开始状态和目标状态。使用a连接两个状态uavDubinsConnection满足空气动力学约束的对象。使用validatorOccupancyMap3D用于固定翼无人机与环境之间碰撞检查的对象。
定义状态空间对象
这个例子提供了一个预定义的状态空间，ExampleHelperUavStateSpace，用于路径规划。状态空间定义为[x y z航向角],在那里[x y z]指定无人机的位置和headingAngle以弧度为单位指定标题角度。该示例使用uavDubinsConnection目标作为无人机的运动学模型，该模型受最大滚转角、空速和航迹角的约束。通过指定无人机的最大滚转角度、空速和飞行路径角度限制属性作为名称-值对来创建状态空间对象。使用“界限”参数指定无人机的位置和方向边界为4 × 2矩阵，其中前三行表示x-，y- - - - - -,z-轴边界在3-D占用地图内，最后一行表示范围内的航向角度(π-π,)弧度。
ss = ExampleHelperUAVStateSpace(“MaxRollAngle”π/ 6日,.．.“速度”6.．.“FlightPathAngleLimit”(-0.1 - 0.1),.．.“界限”, (-20 220;-20 220;10 100;π-π]);
根据目标姿态设置工作空间的阈值边界。这个阈值决定了目标姿态周围的目标工作空间目标区域有多大，这用于工作空间目标区域方法的偏差抽样。
threshold = [(goalPose-0.5)' (goalPose+0.5)';π-π);
使用setWorkspaceGoalRegion函数更新目标姿态和它周围的区域。
setWorkspaceGoalRegion (ss、goalPose阈值)
定义状态验证器对象
的validatorOccupancyMap3D对象确定状态无效xyz-location在地图上被占用。只有当所有中间状态都有效时，两个状态之间的运动才有效，这意味着无人机不经过地图上任何已占用的位置。创建一个validatorOccupancyMap3D通过指定状态空间对象和膨胀映射来获取。然后设置验证距离(以米为单位)，用于在状态之间进行插值。
sv = validatoroccuancymap3d (ss，“地图”、omap);sv。ValidationDistance = 0.1;
设置RRT路径规划器
创建一个plannerRRT通过指定状态空间和状态验证器作为输入来获取。设置MaxConnectionDistance，GoalBias,MaxIterations属性，然后指定自定义目标函数。如果到目标的欧几里得距离低于5米的阈值，则该目标函数确定路径已到达目标。
planner = plannerRRT(ss,sv);计划。MaxConnectionDistance = 50;计划。GoalBias = 0.10;计划。MaxIterations = 400;计划。GoalReachedFcn = @ ~, x, y)(规范(x - y (1:3)) (1:3) < 5);
执行路径规划
在三维空间中进行基于rrt的路径规划。规划器找到一条无碰撞且适合固定翼飞行的路径。
[pthObj,solnInfo] = plan(planner,startPose,goalPose);
模拟无人机沿着规划路径飞行
想象计划好的路径。根据无人机Dubins连接插值规划路径。将插值状态绘制为一条绿线。
使用提供的辅助功能模拟无人机飞行，exampleHelperSimulateUAV，它需要航路点、空速和到达目标的时间(基于空速和路径长度)。helper函数使用fixedwing基于航路点产生的控制输入来模拟无人机行为的制导模型。将模拟状态绘制为红线。
注意，由于控制跟踪误差较小，模拟无人机飞行略微偏离计划路径。此外，三维杜宾斯路径假设无人机滚转角度的瞬时变化，但实际动力学对滚转命令的响应较慢。补偿这种滞后的一种方法是用更保守的空气动力学约束来规划路径。
如果(solnInfo.IsPathFound)图(“名称”，“OriginalPath”）可视化3d地图显示(omap)在startPose scatter3 (startPose (1) (2), startPose(3), 30岁,“红色”，“填充”) scatter3 (goalPose (1), goalPose (2), goalPose(3), 30岁,“绿色”，“填充”) interpolatedPathObj = copy(pthObj);插入(interpolatedPathObj, 1000)根据无人机Dubins连接绘制插值路径hReference = plot3(interpolatedPathObj.States(:，1)，.．.interpolatedPathObj.States (:, 2),.．.interpolatedPathObj.States (:, 3),.．.“线宽”2,“颜色”，“g”）;基于固定翼制导模型的无人机仿真轨迹绘制计算总飞行时间并添加缓冲timeToReachGoal = 1.05*pathLength(pthObj)/ss.AirSpeed;路径点= interpolatedPathObj.States;[xENU,yENU,zENU] = exampleHelperSimulateUAV(航路点，ss.AirSpeed,timeToReachGoal);hsimulation = plot3(xENU,yENU,zENU，“线宽”2,“颜色”，“r”）;传奇([hReference, hSimulated],“参考”，“模拟”，“位置”，“最佳”)举行从视图(63 [-31])结束
平滑Dubins路径和模拟无人机轨迹
原计划路径在驶向目标的过程中会出现一些不必要的转弯。通过使用示例中提供的路径平滑算法简化三维Dubins路径，exampleHelperUAVPathSmoothing．该函数基于迭代策略去除中间的3-D Dubins姿态。有关平滑策略的更多信息，请参见[1］．平滑函数将非连续的三维杜宾姿势相互连接起来，只要这样做不会导致碰撞。该过程生成的平滑路径改善了固定翼仿真模型的跟踪特性。用这些新的平滑航路点模拟固定翼无人机模型。
如果(solnInfo.IsPathFound) smoothWaypointsObj = exampleHelperUAVPathSmoothing(ss,sv,pthObj);图(“名称”，“SmoothedPath”）绘制3d地图显示(omap)在startPose scatter3 (startPose (1) (2), startPose(3), 30岁,“红色”，“填充”) scatter3 (goalPose (1), goalPose (2), goalPose(3), 30岁,“绿色”，“填充”) interpolatedSmoothWaypoints = copy(smoothWaypointsObj);插入(interpolatedSmoothWaypoints, 1000)基于无人机Dubins连接绘制平滑路径hReference = plot3(interpolatedSmoothWaypoints.States(:，1)，.．.interpolatedSmoothWaypoints.States (:, 2),.．.interpolatedSmoothWaypoints.States (:, 3),.．.“线宽”2,“颜色”，“g”）;基于固定翼制导模型绘制模拟飞行轨迹waypoints = interpolatedSmoothWaypoints.States;timeToReachGoal = 1.05*pathLength(smoothWaypointsObj)/ss.AirSpeed;[xENU,yENU,zENU] = exampleHelperSimulateUAV(航路点，ss.AirSpeed,timeToReachGoal);hsimulation = plot3(xENU,yENU,zENU，“线宽”2,“颜色”，“r”）;传奇([hReference, hSimulated],“SmoothedReference”，“模拟”，“位置”，“最佳”)举行从视图(63 [-31]);结束
平滑的路径更短，整体上显示出更好的跟踪。
参考文献
[1]比尔德、兰德尔·W和蒂莫西·W·麦克莱恩。小型无人机:理论与实践．普林斯顿，新泽西州:普林斯顿大学出版社，2012。
Hornung, Armin, Kai M. Wurm, Maren Bennewitz, Cyrill Stachniss和Wolfram Burgard。OctoMap:基于八叉树的高效概率三维映射框架自主机器人34岁的没有。3(2013年4月):189-206https://doi.org/10.1007/s10514-012-9321-0．
您也可以从以下列表中选择一个网站: