k = size(T.v,2) + 1; T.v(k).x = goal(1)-0.5; T.v(k).y = goal(2)-0.5; T.v(k).xfather = Qnew(1)-0.5; T.v(k).yfather = Qnew(2)-0.5; T.v(k).cost = T.v(k-1).cost + dis2goal; T.v(k).indPre = k - 1; disp('find path!'); break; end
时间: 2023-12-25 11:05:12 浏览: 30
这段代码的作用是将当前节点 `Qnew` 添加到 RRTstar 树中,并检查是否到达目标节点 `goal`。
第一行代码 `k = size(T.v,2) + 1` 是获取 RRTstar 树节点数,并将节点数加一作为当前节点 `Qnew` 的索引号 `k`。
接下来几行代码是将当前节点 `Qnew` 的信息存储到 RRTstar 树的结构体数组 `T.v` 中,包括节点的坐标 `(Qnew(1)-0.5, Qnew(2)-0.5)`、父节点的坐标 `(Qnear(1)-0.5, Qnear(2)-0.5)`、到达当前节点的代价 `T.v(k-1).cost + dis2goal`,以及当前节点的前驱节点索引 `k-1`。
然后代码使用 `disp` 函数输出信息 "find path!",表示已经找到一条从起点到目标点的路径。
最后一行代码 `break` 是跳出循环,结束 RRTstar 算法的运行。
相关问题
优化这行代码: %step8.检查是否到达目标点附近 dis2goal = sqrt((Pnew(1) - goal(1))^2 + (Pnew(2) - goal(2))^2); if dis2goal < threshold %将goal插入树中 k = size(T.v,2) + 1; T.v(k).x = goal(1); T.v(k).y = goal(2); T.v(k).xPre = Pnew(1); T.v(k).yPre = Pnew(2); T.v(k).cost = T.v(k-1).cost + dis2goal; T.v(k).indPre = k - 1; disp('find path!'); break; end end
同样可以使用向量化运算,将if语句改为三目运算符:
dis2goal = sqrt((Pnew(1) - goal(1))^2 + (Pnew(2) - goal(2))^2);
flag = dis2goal < threshold;
k = flag*(size(T.v,2) + 1);
T.v(k).x = flag*goal(1);
T.v(k).y = flag*goal(2);
T.v(k).xPre = flag*Pnew(1);
T.v(k).yPre = flag*Pnew(2);
T.v(k).cost = flag*(T.v(k-1).cost + dis2goal);
T.v(k).indPre = flag*(k - 1);
if flag
disp('find path!');
break;
end
这样可以减少代码行数,使代码更加简洁高效。
path = []; path = [path;goal]; index = T.v(end).indPre; while true path = [path;[T.v(index).x-0.5,T.v(index).y-0.5]]; index = T.v(index).indPre; if index == -1 break; end end
这段代码是用来生成路径的。我们可以看到,首先定义一个空的路径数组 `path`,将终点 `goal` 加入到 `path` 中。然后通过 `T.v(end).indPre` 找到树中最后一个节点的前驱节点的索引,进入 `while` 循环。在循环中,将前驱节点的坐标(`T.v(index).x-0.5,T.v(index).y-0.5`)加入到 `path` 中,然后将当前节点的索引更新为前驱节点的索引,如果当前节点已经是起点,则跳出循环。最终生成的 `path` 数组即为从起点到终点的路径。
相关推荐
![pdf](https://img-home.csdnimg.cn/images/20210720083512.png)
![pdf](https://img-home.csdnimg.cn/images/20210720083512.png)
![whl](https://img-home.csdnimg.cn/images/20210720083646.png)
![](https://csdnimg.cn/download_wenku/file_type_ask_c1.png)
![](https://csdnimg.cn/download_wenku/file_type_ask_c1.png)
![](https://csdnimg.cn/download_wenku/file_type_ask_c1.png)
![](https://csdnimg.cn/download_wenku/file_type_ask_c1.png)
![](https://csdnimg.cn/download_wenku/file_type_ask_c1.png)
![](https://csdnimg.cn/download_wenku/file_type_ask_c1.png)
![](https://csdnimg.cn/download_wenku/file_type_ask_c1.png)
![](https://csdnimg.cn/download_wenku/file_type_ask_c1.png)
![](https://csdnimg.cn/download_wenku/file_type_ask_c1.png)
![](https://csdnimg.cn/download_wenku/file_type_ask_c1.png)
![](https://csdnimg.cn/download_wenku/file_type_ask_c1.png)
![](https://csdnimg.cn/download_wenku/file_type_ask_c1.png)
![](https://csdnimg.cn/download_wenku/file_type_ask_c1.png)
![](https://csdnimg.cn/download_wenku/file_type_ask_c1.png)