计划四旋翼飞行器的最小瞬间轨迹
该示例展示了如何使用优化的快速探索随机树(RRT*)路径规划器，为多旋翼无人机(UAV)在3D地图上从起始姿态到目标姿态规划最小抓拍轨迹(最小控制努力)。
在这个示例中，您设置了一个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];画出开始姿势和目标姿势持有在startPose scatter3 (startPose (1) (2), startPose(3), 100年,“r”) scatter3 (goalPose (1), goalPose (2), goalPose(3), 100年,“.g”view([-31 63])＂＂，“起始位置”，“目标位置”)举行从
规划路径与RRT*使用SE(3)状态空间
RRT*是一个基于树的运动规划器，它从给定状态空间的随机样本中增量地构建搜索树。树最终跨越搜索空间，并连接开始状态和目标状态。使用a将两个状态用直线连接连接stateSpaceSE3对象。使用validatorOccupancyMap3D用于多旋翼无人机与环境之间碰撞检查的对象。
定义状态空间对象
的stateSpaceSE3对象将状态空间定义为[x y z qw qx qy qz],在那里[x y z]以米和为单位指定无人机的位置[qw qx qy qz]将方向指定为四元数。指定四旋翼的位置和方向边界为7 × 2矩阵。方向边界是可选的，可以设置为负而且正如果它们不适用。
ss = stateSpaceSE3([-20 220;-20 220;-10 100;正正;正正;正正;正正]);
定义状态验证器对象
的validatorOccupancyMap3D对象确定状态无效xyz-location在地图上被占用。只有当所有中间状态都有效时，两个状态之间的运动才有效，这意味着无人机不经过地图上任何已占用的位置。创建一个validatorOccupancyMap3D通过指定状态空间对象和膨胀映射来获取。然后设置验证距离(以米为单位)，用于在状态之间进行插值。
sv = validatoroccuancymap3d (ss,Map=omap);sv。ValidationDistance = 0.1;
设置RRT*路径规划器
创建一个plannerRRTStar通过指定状态空间和状态验证器作为输入来获取。设置MaxConnectionDistance，GoalBias，MaxIterations，ContinueAfterGoalReached,MaxNumTreeNodes属性来优化返回的路径点。
planner = plannerRRTStar(ss,sv);计划。MaxConnectionDistance = 50;计划。GoalBias = 0.8;计划。MaxIterations = 1000;计划。ContinueAfterGoalReached = true;计划。MaxNumTreeNodes = 10000;
执行路径规划
在3D空间中进行基于RRT*的路径规划，获取路径点。规划器会找到一条无碰撞且适合四旋翼的飞行路线。解决方案信息有助于调整计划。
[pthObj,solnInfo] = plan(planner,startPose,goalPose);
可视化的路径
检查RRT*规划器是否找到了路径。如果规划者找到了一条路径，就把路径点画出来。否则，请终止脚本。
如果(~ solnInfo.IsPathFound) disp (" RRT未找到路径，终止示例"）返回结束绘制地图，开始姿势和目标姿势显示(omap)在startPose scatter3 (startPose (1) (2), startPose(3), 100年,“r”) scatter3 (goalPose (1), goalPose (2), goalPose(3), 100年,“.g”）绘制由路径规划器计算的路径plot3 (pthObj.States (: 1) pthObj.States (:, 2), pthObj.States (:, 3),“g”view([-31 63])＂＂，“起始位置”，“目标”位置，“计划路径”)举行从
生成最小抓拍无人机轨迹
原计划的路径在驶向目标的过程中会出现一些尖角。方法将获得的路径点拟合到最小抓拍轨迹，从而生成平滑轨迹minsnappolytraj函数。对于到达每个航路点所需时间的初始估计，假设无人机以恒定速度移动。
要调整轨迹和飞行时间，请指定numSamples，TimeAllocation,TimeWeight的论点minsnappolytraj函数。
路径点= pthObj.States;nWayPoints = pthObj.NumStates;计算路径点之间的距离距离= 0 (1,nWayPoints);为i = 2:nWayPoints距离(i) = norm(waypoints(i,1:3) - waypoints(i-1,1:3));结束%假设无人机速度为3米/秒，并计算到达每个航路点所需的时间UAVspeed = 3;时间点= cumsum(距离/航速);nSamples = 100;计算沿轨迹的状态initialStates = minsnappolytraj(waypoints'，timepoints,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),“可能是”view([-31 63])＂＂，“起始位置”，“目标”位置，“计划路径”，“最初的轨迹”)举行从
生成有效的最小抓拍轨迹
注意，生成的飞行轨迹有一些无效状态，它们不是无障碍的。您必须修改路径点，以使生成的轨迹无障碍。为了避免无效状态，在轨迹无效的路径点对之间的段上添加中间路径点。迭代地插入路径点，直到整个轨迹有效为止。
检查弹道是否有效states = initialStates;valid = all(isStateValid(sv,states));而(~有效)检查状态的有效性validity = isStateValid(sv,states);将状态映射到相应的路点段segmentIndices = examplehelpermapstatestopasegments(路径点，状态);获取无效状态的段使用唯一，因为同一段中的多个状态可能无效invalidSegments = unique(segmentIndices(~validity));在无效段上添加中间路径点为i = 1:size(invalidSegments) segment = invalidSegments(i);取位置的中点，得到中间位置中点(1:3)=(路点(线段，1:3)+路点(线段+1,1:3))/2;球面插值四元数得到中间四元数中点(4:7)= slerp(四元数(路点(段，4:7))，四元数(路点(段+1,4:7))，.5).compact;路点=[路点(1:段，:);中点;锚点(段+ 1:,:)];结束nWayPoints = size(waypoints,1);距离= 0 (1,nWayPoints);为i = 2:nWayPoints距离(i) = norm(waypoints(i,1:3) - waypoints(i-1,1:3));结束计算到达每个航路点所花费的时间时间点= cumsum(距离/航速);nSamples = 100;states = minsnappolytraj(waypoints'，timepoints,nSamples,MinSegmentTime=2,MaxSegmentTime=20,TimeAllocation=true,TimeWeight=5000)';检查新轨迹是否有效valid = all(isStateValid(sv,states));结束
可视化最终有效轨迹
可视化最终有效的最小抓拍轨迹。
绘制地图，开始和目标姿势显示(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”view([-31 63])＂＂，“起始位置”，“目标”位置，“计划路径”，“最初的轨迹”，“有效的轨迹”)举行从
您也可以从以下列表中选择一个网站: