function iscol = iscollision1(Qnear,Qnew,Pvec,map) iscol = false; step = 1; for n = 0:step p = Qnear + n*Pvec; if unfeasible(p,map) iscol = true; break; end end if unfeasible(Qnew,map) iscol = true; end end
时间: 2024-02-14 11:08:28 浏览: 20
这段代码也是RRT算法的一部分,作用是检测从新节点Qnew到树中已有节点的路径上是否与障碍物相交。具体来说,iscollision1函数输入参数包括:Qnear表示已有节点,Qnew表示新节点,Pvec表示从已有节点到新节点的向量,map表示地图信息。首先将iscol变量初始化为false,表示当前路径不与障碍物相交。然后,从Qnear到Qnew的路径上按照固定步长(step)逐个计算线段端点p,判断线段是否与障碍物相交。具体来说,unfeasible函数用于检测当前点p是否在障碍物内部,如果在,则说明当前路径与障碍物相交,将iscol变量置为true,并跳出循环。最后,再检测新节点Qnew是否在障碍物内部,如果是,则同样将iscol变量置为true。
相关问题
function iscol = iscollision2(this_p,Qnew,dist,map) iscol = false; Pvec = [Qnew(1) - this_p(1), Qnew(2) - this_p(2)]; Pvec = Pvec/norm(Pvec); for n = 0:dist p = this_p + n*Pvec; if unfeasible(p,map) iscol = true; end end end
这段代码也是RRT算法的一部分,作用是检测从新节点Qnew到当前节点this_p的路径上是否与障碍物相交。具体来说,输入参数包括:this_p表示当前节点,Qnew表示新节点,dist表示当前节点到新节点的距离,map表示地图信息。首先将iscol变量初始化为false,表示当前路径不与障碍物相交。然后,计算从当前节点this_p到新节点Qnew的方向向量Pvec,并将其归一化。接着,从当前节点this_p到新节点Qnew的路径上按照固定步长(dist)逐个计算线段端点p,判断线段是否与障碍物相交。具体来说,unfeasible函数用于检测当前点p是否在障碍物内部,如果在,则说明当前路径与障碍物相交,将iscol变量置为true。最后,返回iscol变量表示是否与障碍物相交。
Qnew = Qnear+step*Pvec;
这一行代码的作用是计算新节点Qnew的坐标,它的位置是Qnear沿着向量Pvec前进一定步长(step)后得到的点。具体来说,Qnear是已知的RRT树中距离随机采样点Prand最近的节点,而Pvec是从Qnear指向Prand的向量,并且经过归一化处理。因此,Qnew的坐标可以通过以下公式计算得到:
Qnew = Qnear + step * Pvec
其中,step是一个常数,表示从Qnear到Qnew的距离,它可以根据具体问题进行调整。这个公式的物理意义是,以Qnear为起点,按照向量Pvec的方向前进一段距离step,就可以到达新节点Qnew的位置。因此,这个公式可以帮助我们在RRT树中不断扩展新节点,直到找到一条从起点到目标点的路径。