RRT*路径规划算法的rewie的理解

最新推荐文章于 2024-10-12 21:48:40 发布

我是你肚子里的蛔虫

最新推荐文章于 2024-10-12 21:48:40 发布

阅读量325

点赞数 5

文章标签：算法 windows

本文链接：https://blog.csdn.net/weixin_45669246/article/details/141966058

版权

SourceURL:file:///home/yannick/myCode/PathPlanning/rrt调试记录.doc

功能：优化新采样点中就近点的父节点

输入：newNode， near_idx；输出：void

步骤：

遍历所有的_node_list[near_idx]中的采样点；
设置origenNodeParentCost变量（double类型），用以存储采样点的消耗，即cost

double origenNodeParentCost = _node_list[near_idx[i]]->cost;

设置newNodeAsParentNodeCost（double类型），用以存储以newNode作为父节点的消耗，即cost

double newNodeAsParentNodeCost = newNode->cost + alNodeDis(_node_list[near_idx[i]], newNode);

比较newNodeAsParentNodeCost和origenNodeParentCost，若origenNodeParentCost的消耗小，则不修改父节点；newNodeAsParentNodeCost的消耗小，若则替换父节点，如下：

if (newNodeAsParentNodeCost < origenNodeParentCost)
    {
      _node_list[near_idx[i]]->parent = newNode;
      _node_list[near_idx[i]]->cost = newNodeAsParentNodeCost;
    }

rewire函数代码如下：

void RRTSTAR::rewire(Node* newNode, std::vector<int> near_idx)
{
  for (short i = 0; i < near_idx.size(); i++)
  {
    if (_node_list[near_idx[i]]->parent == nullptr)
    continue;
    double newNodeAsParentNodeCost = newNode->cost + calNodeDis(_node_list[near_idx[i]], newNode);
    double origenNodeParentCost = _node_list[near_idx[i]]->cost;
    std::cout << LOG << "newNodeAsParentNodeCost = " << newNodeAsParentNodeCost << std::endl;
    std::cout << LOG << "origenNodeParentCost = " << origenNodeParentCost << std::endl;
    if (newNodeAsParentNodeCost < origenNodeParentCost)
    {
      _node_list[near_idx[i]]->parent = newNode;
      _node_list[near_idx[i]]->cost = newNodeAsParentNodeCost;
    }
  }
}