Informed RRT*实现椭圆启发式采样

我来为您添加Informed RRT*功能，在找到第一条路径后使用椭圆采样来加速收敛。以下是需要新增的函数和修改：
以下代码只含新增的部分

classRRTStar{private:// 在私有成员变量中添加boolpathFound;doublebestPathCost;std::shared_ptr<Node>goalNode;// ... 其他现有成员public:// 在构造函数中初始化新变量RRTStar(constPoint&start,constPoint&goal,doublestepSize=0.5,doublegoalRadius=0.5,doublesearchRadius=1.0,intmaxIterations=5000):start(start),goal(goal),stepSize(stepSize),goalRadius(goalRadius),searchRadius(searchRadius),maxIterations(maxIterations),nodeCounter(0),gen(rd()),pathFound(false),bestPathCost(std::numeric_limits<double>::max()),goalNode(nullptr){minX=std::min(start.x,goal.x)-5;maxX=std::max(start.x,goal.x)+5;minY=std::min(start.y,goal.y)-5;maxY=std::max(start.y,goal.y)+5;xDist=std::uniform_real_distribution<>(minX,maxX);yDist=std::uniform_real_distribution<>(minY,maxY);addNode(std::make_shared<Node>(0,start,-1,0.0));}// 新增函数：生成椭圆内的随机点PointgenerateEllipseRandomPoint(doublecBest){// cBest是当前最佳路径长度// cMin是起点到终点的直线距离（椭圆焦距距离）doublecMin=start.distanceTo(goal);// 如果cBest小于cMin或未找到路径，返回整个空间的随机点if(cBest<cMin||!pathFound){returngenerateRandomPoint(0.1);}// 计算椭圆中心、旋转角和半轴长度Pointcenter((start.x+goal.x)/2.0,(start.y+goal.y)/2.0);// 椭圆的长轴长度doublea=cBest/2.0;// 计算短轴长度：b = sqrt(cBest² - cMin²) / 2doubleb=std::sqrt(std::pow(cBest,2)-std::pow(cMin,2))/2.0;// 计算旋转角（从起点指向终点）doubletheta=std::atan2(goal.y-start.y,goal.x-start.x);// 在单位圆内生成随机点std::uniform_real_distribution<>angleDist(0.0,2.0*M_PI);std::uniform_real_distribution<>radiusDist(0.0,1.0);doubler=std::sqrt(radiusDist(gen));// 均匀分布在圆内doublephi=angleDist(gen);// 将单位圆内的点映射到椭圆doublex=r*std::cos(phi);doubley=r*std::sin(phi);// 应用椭圆变换：缩放、旋转、平移doubletransformedX=center.x+a*x*std::cos(theta)-b*y*std::sin(theta);doubletransformedY=center.y+a*x*std::sin(theta)+b*y*std::cos(theta);returnPoint(transformedX,transformedY);}// 修改后的generateRandomPoint函数，添加椭圆采样逻辑PointgenerateRandomPoint(doublegoalBias=0.1){// 如果已经找到路径，使用椭圆采样if(pathFound){// 10%的概率使用原始采样策略，90%的概率使用椭圆采样std::uniform_real_distribution<>strategyDist(0.0,1.0);if(strategyDist(gen)<0.9){returngenerateEllipseRandomPoint(bestPathCost);}}// 原始采样策略std::uniform_real_distribution<>biasDist(0.0,1.0);if(biasDist(gen)<goalBias){returngoal;}returnPoint(xDist(gen),yDist(gen));}// 新增函数：更新最佳路径voidupdateBestPath(std::shared_ptr<Node>newGoalNode){if(newGoalNode){doublenewCost=newGoalNode->getCost();if(newCost<bestPathCost){bestPathCost=newCost;goalNode=newGoalNode;pathFound=true;std::cout<<"更新最佳路径，成本: "<<bestPathCost<<std::endl;// 动态调整搜索半径（可选）// searchRadius = std::min(10.0 * stepSize,// std::max(stepSize, bestPathCost / 10.0));}}}// 修改后的plan函数，包含Informed RRT*逻辑boolplan(){for(inti=0;i<maxIterations;i++){// 1. 生成随机点（现在会根据是否找到路径选择采样策略）Point randPoint=generateRandomPoint();// 2. 找到最近的节点autonearestNode=findNearestNode(randPoint);// 3. 生成新节点Point newPoint=steer(nearestNode->getPosition(),randPoint);// 4. 检查路径是否无碰撞if(!isPathCollisionFree(nearestNode->getPosition(),newPoint)){continue;}// 5. 查找附近的节点autonearNodes=findNearNodes(newPoint,searchRadius);// 6. 选择最佳父节点std::shared_ptr<Node>bestParent=nearestNode;doubleminCost=nearestNode->getCost()+nearestNode->getPosition().distanceTo(newPoint);for(auto&node:nearNodes){doublenewCost=node->getCost()+node->getPosition().distanceTo(newPoint);if(newCost<minCost&&isPathCollisionFree(node->getPosition(),newPoint)){minCost=newCost;bestParent=node;}}// 7. 创建新节点intnewNodeId=++nodeCounter;autonewNode=std::make_shared<Node>(newNodeId,newPoint,bestParent->getId(),minCost);addNode(newNode);// 8. 重新连接for(auto&node:nearNodes){if(node->getId()==bestParent->getId()){continue;}doublepotentialCost=newNode->getCost()+newNode->getPosition().distanceTo(node->getPosition());if(potentialCost<node->getCost()&&isPathCollisionFree(newNode->getPosition(),node->getPosition())){node->setParentId(newNode->getId());node->setCost(potentialCost);// 如果被重新连接的节点是目标节点的祖先，需要更新目标节点成本if(goalNode&&isDescendantOf(node,goalNode)){updateGoalNodeCost();}}}// 9. 检查是否到达目标if(newPoint.distanceTo(goal)<=goalRadius){if(isPathCollisionFree(newPoint,goal)){// 添加目标节点intgoalNodeId=++nodeCounter;doublegoalCost=newNode->getCost()+newPoint.distanceTo(goal);autonewGoalNode=std::make_shared<Node>(goalNodeId,goal,newNode->getId(),goalCost);addNode(newGoalNode);// 更新最佳路径updateBestPath(newGoalNode);if(!pathFound){std::cout<<"找到第一条路径！迭代次数: "<<i+1<<"，成本: "<<goalCost<<std::endl;pathFound=true;bestPathCost=goalCost;goalNode=newGoalNode;}}}// 每100次迭代打印进度if((i+1)%1000==0){std::cout<<"迭代 "<<i+1;if(pathFound){std::cout<<"，当前最佳路径成本: "<<bestPathCost;}std::cout<<std::endl;}}if(pathFound){std::cout<<"\n最终最佳路径成本: "<<bestPathCost<<std::endl;returntrue;}else{std::cout<<"未找到路径，达到最大迭代次数"<<std::endl;returnfalse;}}// 新增函数：检查一个节点是否是另一个节点的后代boolisDescendantOf(std::shared_ptr<Node>ancestor,std::shared_ptr<Node>descendant){autocurrent=descendant;while(current){if(current->getId()==ancestor->getId()){returntrue;}if(current->getParentId()==-1){break;}current=tree[current->getParentId()];}returnfalse;}// 新增函数：更新目标节点成本（当重新连接影响目标节点路径时）voidupdateGoalNodeCost(){if(!goalNode)return;doublenewCost=0.0;autocurrent=goalNode;std::vector<std::shared_ptr<Node>>pathNodes;// 收集路径上的所有节点while(current){pathNodes.push_back(current);if(current->getParentId()==-1){break;}current=tree[current->getParentId()];}// 反向计算累积成本std::reverse(pathNodes.begin(),pathNodes.end());newCost=0.0;for(size_t i=0;i<pathNodes.size();i++){if(i>0){newCost+=pathNodes[i-1]->getPosition().distanceTo(pathNodes[i]->getPosition());pathNodes[i]->setCost(newCost);}else{pathNodes[i]->setCost(0.0);}}// 如果成本有改善，更新最佳路径if(newCost<bestPathCost){bestPathCost=newCost;std::cout<<"通过重新连接优化路径，新成本: "<<bestPathCost<<std::endl;}}// 修改后的getPath函数std::vector<Point>getPath(){std::vector<Point>path;if(!goalNode){// 如果没有设置goalNode，尝试查找for(constauto&pair:tree){if(pair.second->getPosition().distanceTo(goal)<=goalRadius){goalNode=pair.second;break;}}}if(!goalNode){returnpath;}// 反向追踪路径autocurrent=goalNode;while(current){path.push_back(current->getPosition());if(current->getParentId()==-1){break;}current=tree[current->getParentId()];}std::reverse(path.begin(),path.end());returnpath;}// 新增函数：获取当前最佳路径成本doublegetBestPathCost()const{returnbestPathCost;}// 新增函数：检查是否已找到路径boolisPathFound()const{returnpathFound;}// 新增函数：获取椭圆采样区域信息（用于可视化）voidgetEllipseInfo(double&a,double&b,double&theta,Point&center)const{doublecMin=start.distanceTo(goal);center=Point((start.x+goal.x)/2.0,(start.y+goal.y)/2.0);a=bestPathCost/2.0;b=std::sqrt(std::pow(bestPathCost,2)-std::pow(cMin,2))/2.0;theta=std::atan2(goal.y-start.y,goal.x-start.x);}};

主要新增功能和修改：

1. 新增成员变量

· pathFound: 标记是否已找到路径
· bestPathCost: 当前最佳路径成本
· goalNode: 指向目标节点的指针

2. 椭圆采样函数

· generateEllipseRandomPoint(): 在连接起点和终点的椭圆内生成随机点
· 椭圆焦点：起点和终点
· 椭圆长轴长度：当前最佳路径长度
· 短轴长度：根据勾股定理计算

3. 改进的采样策略

· generateRandomPoint(): 在找到路径后，90%的概率使用椭圆采样，10%的概率使用原始采样
· 这确保了在找到路径后，搜索集中在有希望找到更好路径的区域

4. 路径更新机制

· updateBestPath(): 当找到新的更好路径时更新状态
· updateGoalNodeCost(): 当重新连接影响目标节点路径时更新成本

5. 进度监控

· 每1000次迭代打印当前状态
· 显示当前最佳路径成本

6. 辅助函数

· isDescendantOf(): 检查节点间的祖孙关系
· getEllipseInfo(): 获取椭圆采样区域信息（可用于可视化）

使用示例：

intmain(){Pointstart(0,0);Pointgoal(10,10);RRTStarplanner(start,goal,0.5,0.5,1.5,20000);// ... 添加障碍物和设置边界boolsuccess=planner.plan();if(success){std::vector<Point>path=planner.getPath();std::cout<<"最终路径成本: "<<planner.getBestPathCost()<<std::endl;// 获取椭圆信息（可用于可视化）doublea,b,theta;Point center;if(planner.isPathFound()){planner.getEllipseInfo(a,b,theta,center);std::cout<<"椭圆采样区域 - 中心: ("<<center.x<<", "<<center.y<<"), 长轴: "<<a<<", 短轴: "<<b<<", 旋转角: "<<theta<<" rad"<<std::endl;}}return0;}

这个Informed RRT实现能够在找到第一条路径后显著加速优化过程，通过椭圆采样将搜索集中在有希望找到更好路径的区域，从而比标准RRT更快收敛到最优路径。