规划四旋翼飞行器的最小弹跳轨迹
该示例演示了如何使用优化的快速探索随机树(RRT*)路径规划器，在3D地图上规划多旋翼无人机(UAV)从起始位姿到目标位姿的最小快速轨迹(最小控制努力)。
在此示例中，您设置了3D映射，提供了一个启动姿势和目标姿势，使用3D直线连接来平铺与RRT *，并通过所获得的航点拟合最小的快速轨迹。
最初设定
配置可重复结果的随机数生成器。
RNG（100，“扭转”）
加载地图
加载3D占用地图uavMapCityBlock.mat，其中包含一组预先生成的障碍，并将其添加到工作区中。占领地图是在一个东-北-上(ENU)框架。
mapdata = load（“uavmapcityblock.mat”）;OMAP = mapdata.omap;％考虑未取消的空间的核心。FreeThreshold = omap.OccupiedThreshold;显示(omap)
设定开始姿势和目标姿势
使用地图进行引用，选择轨迹的未占用的开始姿势和目标姿势。
startPose = [12 22 25 0.7 0.2 0 0.1];％[x y z qw qx qy qz]goalPose = [150 180 35 0.3 0 0.1 0.6];绘制开始和目标姿势持有在散射3（血统（1），血统（2），纵向（3），100，“.r”) scatter3 (goalPose (1), goalPose (2), goalPose(3), 100年,“.g”)视图([-31 63])图例(＂＂，“起始位置”，“目标位置”） 抓住离开
使用SE(3)状态空间使用RRT*规划路径
RRT*是一个基于树的运动规划器，它从给定状态空间的随机样本增量地构建搜索树。树最终跨越搜索空间并连接开始状态和目标状态。将这两个状态用直线连接SolultSpacese3.目的。使用validatoroccupancymap3d.目标，用于多旋翼无人机与环境的碰撞检测。
定义状态空间对象
的SolultSpacese3.对象将状态空间定义为[x y z qw qx qy qz]， 在哪里[x y z]指定无人机的位置，单位为米和[qw qx qy qz]指定方向为四元数。指定位置和方向边界的四旋翼作为一个7 × 2矩阵。方向边界是可选的，可以设置为负和inf如果他们不适用。
ss = stateSpaceSE3([-20 220;-20 220;-10 100;正正;正正;正正;正正]);
定义状态验证器对象
的validatoroccupancymap3d.对象确定状态无效时XYZ.-地图上的位置已被占用。只有当所有中间状态都有效时，两个状态之间的运动才有效，这意味着无人机没有通过地图上任何被占用的位置。创建一个validatoroccupancymap3d.通过指定状态空间对象和膨胀的地图来指定对象。然后将验证距离设置为以米为单位，以便在状态之间插值。
sv = validatorOccupancyMap3D (ss,地图= omap);sv。ValidationDistance = 0.1;
设置RRT * Path Planner
创建一个plannerRRTStar通过将状态空间和状态验证器指定为输入来指定对象。设定maxconnectiondistance.，GoalBias，MaxIterations，ContinueaftergoalReached.， 和MaxNumTreeNodes属性来优化返回的路径点。
Planner = PlannerRrtstar（SS，SV）;Planner.MaxConnectionDistance = 50;Planner.GoalBias = 0.8;Planner.maxIterations = 1000;planner.continuaftergoalReached = True;Planner.maxnumtreenodes = 10000;
执行路径规划
在三维空间中执行基于RRT*的路径规划，获取路径点。规划师找到了一条无碰撞且适合四旋翼飞行器的飞行路径。解决方案信息对于调整计划器很有帮助。
[Pthobj，Solninfo] =计划（策划者，血统，守纵向）;
可视化路径
检查RRT*规划器是否找到了路径。如果规划者找到了路径，绘制路径点。否则，终止脚本。
如果（〜solninfo.ispathfound）disp（“RRT找不到路径，终止示例”）返回结尾％绘图地图，开始姿势和目标姿势显示（OMAP）持有在散射3（血统（1），血统（2），纵向（3），100，“.r”) scatter3 (goalPose (1), goalPose (2), goalPose(3), 100年,“.g”）%绘制路径规划器计算的路径plot3 (pthObj.States (: 1) pthObj.States (:, 2), pthObj.States (:, 3),“g”)视图([-31 63])图例(＂＂，“起始位置”，“目标”位置，“计划路径”） 抓住离开
生成最小快照UAV轨迹
最初规划的路径在朝着目标前进的过程中有一些尖角。将得到的路径点拟合到最小突点轨迹，生成光滑的轨迹minsnappolytraj.函数。为了初始估计达到每个航点所需的时间，假设UAV以恒定的速度移动。
调整轨迹和飞行时间，指定numsamples.，timeallocation.， 和延时争论的论点minsnappolytraj.函数。
路点= pthObj.States;nWayPoints = pthObj.NumStates;％计算航点之间的距离距离= 0(1、nWayPoints);为i = 2：nwaypoints距离（i）= norm（航点（i，1：3） - 航点（i-1,1：3））;结尾％假设UAV速度为3米/秒并计算所花费的时间以达到每个航点UAVspeed = 3;时间点= cumsum(距离/ UAVspeed);nSamples = 100;％沿轨迹计算状态initialStates = minsnappolytraj(锚点的时间点,nSamples MinSegmentTime = 4, MaxSegmentTime = 20, TimeAllocation = true, TimeWeight = 100) ';
想象的轨迹
可视化获得的最小快速轨迹。
％绘图地图，开始姿势和目标姿势显示（OMAP）持有在startPose scatter3 (startPose (1) (2), startPose(3), 30岁,“.r”) scatter3 (goalPose (1), goalPose (2), goalPose(3), 30岁,“.g”）％绘制航路点plot3 (pthObj.States (: 1) pthObj.States (:, 2), pthObj.States (:, 3),“g”）％绘制最小的快照轨迹Plot3（InitialStates（：，1），InitialStates（:,2），InitialStates（:,3），“可能是”)视图([-31 63])图例(＂＂，“起始位置”，“目标”位置，“计划路径”，“最初的轨迹”） 抓住离开
生成有效的最小快照轨迹
注意，生成的飞行轨迹有一些无效状态，这些状态不是无障碍的。你必须修改路径点以便生成的轨迹是无障碍的。为了避免无效状态，可以在无效轨迹的一对路径点之间的线段上添加中间路径点。迭代插入路径点，直到整个轨迹有效为止。
检查弹道是否有效= initialstates;有效=全部（IsStateValid（SV，状态））;尽管(~有效)％检查各州的有效性有效性= IsStateValid（SV，状态）;%将状态映射到相应的路点段semmentindices = examplehelpermapstateTestopathsegments（航点，状态）;获取无效状态的段%使用unique，因为同一段中的多个状态可能无效InvalidSegments =唯一（eymentIndices（〜有效性））;％在无效段上添加中间航点为i = 1：大小（InvalidSegments）段= InvalidSegments（i）;％采取位置的中点以获得中间位置中点（1：3）=（航点（段，1：3）+航点（段+ 1,1：3））/ 2;球面插值四元数得到中间四元数中点（4：7）= SLERP（四元度（航点（段，4：7）），四元度（航点（段（段+ 1,4：7）），.5）.compact;waypoints = [waypoints（1：段，:);中点;航点（段+ 1：结束，:)];结尾nWayPoints =大小(锚点,1);距离= 0(1、nWayPoints);为i = 2：nwaypoints距离（i）= norm（航点（i，1：3） - 航点（i-1,1：3））;结尾计算到达每个路径点所花费的时间时间点= cumsum(距离/ UAVspeed);nSamples = 100;州= minsnappolytraj(锚点、时间点、nSamples MinSegmentTime = 2, MaxSegmentTime = 20, TimeAllocation = true, TimeWeight = 5000) ';％检查新轨迹是否有效有效=全部（IsStateValid（SV，状态））;结尾
可视化最终有效轨迹
可视化最终有效的最小快速轨迹。
％绘图地图，开始和目标姿势显示（OMAP）持有在startPose scatter3 (startPose (1) (2), startPose(3), 30岁,“.r”) scatter3 (goalPose (1), goalPose (2), goalPose(3), 30岁,“.g”）％绘制航路点plot3 (pthObj.States (: 1) pthObj.States (:, 2), pthObj.States (:, 3),“g”）绘制初始轨迹Plot3（InitialStates（：，1），InitialStates（:,2），InitialStates（:,3），“可能是”）％绘制最终有效的轨迹plot3(状态(:1),状态(:,2),状态(:,3),“-C”)视图([-31 63])图例(＂＂，“起始位置”，“目标”位置，“计划路径”，“最初的轨迹”，“有效的轨迹”） 抓住离开
您还可以从以下列表中选择一个网站：
