一种基于双RRT（Bidirectional RRT）和APF（Advanced Path Finding）集成技术的核电站检测与维护机器人路径优化仿真方法 吴彤、 朱美豪、 刘展生、 张晓峰、 陈凤娟、 朱晓青、 孙浩文、 张川、 吴家豪

《Algorithms》：A Path Optimization Simulation Method for Nuclear Power Plant Inspection and Maintenance Robots Based on the Integration of Bi-RRT and APF Tong Wu, Meihao Zhu, Zhansheng Liu, Xiaofeng Zhang, Fengjuan Chen, Xiaoqing Zhu, Haowen Sun, Chuan Zhang and Jiahao Wu

　　摘要 在核电站中，用于检测和维护机器人的路径规划经常面临适应性有限、计算成本高以及在障碍物密集的受限环境中收敛不稳定等问题。为了解决这些问题，本文提出了一种改进的双RRT-APF路径优化框架，适用于复杂的工业场景。该方法结合了（1）混合采样策略，包括随机采样、目标导向采样和势场引导采样，以提高全局探索和收敛效率；（2）势场引导的扰动和停滞检测机制，以增强从局部最小值中逃脱的能力；以及（3）动态目标切换和受限的分段连接策略，以提高路径的可行性和安全性。此外，还开发了一个基于数字孪生的仿真平台来验证所提出方法的工程适用性。仿真结果表明，与基线方法相比，该方法在定量上有了显著改进。与传统RRT和Bi-RRT相比，所提出的方法分别减少了65.3%和43.8%的迭代次数，计算时间分别减少了76.1%和48.4%，同时将成功率提高到95%（从82%和93%），并改善了路径的平滑度（从5.3和3.3降低到2.9）。与高级变体（Quad-RRT和KB-RRT*）相比，该方法进一步将计算时间分别减少了25.2%和10.3%，迭代次数分别减少了29.3%和8.4%。这些结果表明，所提出的方法在效率、鲁棒性和路径质量方面取得了平衡的改进。这项工作为核电站环境中机器人的自主路径规划提供了一种高效可靠的解决方案。

1. 引言
随着核能行业智能运营和维护的不断进步，机器人越来越多地被期望执行检测、维护辅助、碎片回收等任务，尤其是在危险或难以到达的区域。然而，在核电站中，机器人运动规划通常在设备布局密集、通道狭窄、几何边界复杂且安全要求严格的高度受限的工作空间中进行。这些特点使得传统的路径规划方法难以同时满足效率、鲁棒性、安全性和工程可部署性的要求。最近关于核电站检测和退役机器人的研究证实了在这种环境中对可靠自主导航的需求日益增长，同时也指出路径规划仍然是实际部署中的一个主要瓶颈[1,2,3,4]。经典的图搜索方法，如A*，具有很强的启发式解释能力，并且可以在离散空间中生成有效的路径[5]。然而，当扩展到具有不规则障碍物和严格在线响应要求的高维连续空间时，它们的计算负担和对地图离散化的依赖性变得越来越受限。相比之下，人工势场（APF）方法因其简单的公式、低计算成本和良好的局部响应能力而具有吸引力[6]。尽管如此，传统的APF方法在混乱环境中仍存在局部最小值、接近障碍物时的振荡以及路径合理性差的问题。尽管改进的APF变体，如确定性退火APF和基于子区域的APF，在一定程度上缓解了这些问题，但它们仍然难以在狭窄和障碍物密集的环境中保证全局的可行性[7,8]。因此，基于采样的规划器，特别是快速探索随机树（RRT）及其变体，由于其概率完备性、在复杂空间中的灵活性以及对显式离散化的相对较低依赖性，已成为机器人路径规划的重要技术途径[9]。双向扩展通过从起始点和目标点同时生成树来进一步提高搜索效率[10]。然而，现有研究也表明，传统的RRT类型方法仍然存在过度随机探索、在狭窄通道中收敛缓慢以及在混乱环境中初始路径质量不令人满意的问题。为此，最近的研究集中在改进采样机制上，包括自适应采样、信息性或混合采样、密度引导扩展和区域约束搜索[11,12,13,14,15,16,17,18]。尽管这些研究表明采样优化是提高搜索效率的有效方法，但大多数研究都是针对一般移动机器人场景的，而很少有研究关注核电站维护环境中的狭窄通道、密集障碍物、双向快速连接和严格安全约束等耦合要求。为了增强树扩展的方向性，许多研究人员尝试将APF与基于RRT的框架相结合。代表性的研究，包括APF-IRRT*、改进的双向RRT*、BPFPS-RRT和APF引导的RRT*变体，表明将势场信息引入采样或扩展可以加速收敛并提高路径质量[19]。然而，尽管有这些优势，大多数现有方法中使用的势场引导仍然主要是静态的。当合力变弱、抵消或在狭窄或障碍物耦合区域振荡时，树扩展仍可能停滞或陷入局部无效的搜索状态。换句话说，尽管APF引导的RRT方法提高了方向搜索效率，但它们在高度混乱的工业环境中明确检测停滞和自适应逃避局部陷阱的能力仍然不足[20]。在双向连接阶段也存在类似的限制。尽管双向树生长可以显著提高找到可行路径的概率，但许多现有方法仍然将树相遇视为主要是几何事件，对连接安全裕度、无效的长距离连接尝试或势场吸引力与当前扩展树之间的一致性考虑不足[21]。因此，在复杂环境中仍可能发生冗余生长、连接尝试效率低下和路径质量不稳定等问题。在核电站检测和维护任务中，这一点尤其关键，因为局部 clearance、碰撞可靠性和路径可执行性比仅仅获得名义上的可行连接更为重要。因此，仍然需要一个能够同时协调双向探索、局部势场引导、安全连接控制和在障碍物密集的狭窄空间中稳定性能的规划框架。除了算法性能外，工程验证对于核能机器人技术也至关重要。数字孪生技术已被广泛认为是支持复杂工程系统中的仿真、验证、可视化和决策的有效手段[22,23,24]。最近的研究已将数字孪生框架扩展到核电站检测软件堆栈、机器人路径规划和智能装配系统，证明了它们在性能评估和面向部署的测试中的价值[25,26,27]。然而，现有研究往往侧重于算法设计或平台构建，而很少有研究将专门针对障碍物密集和狭窄的核电站维护场景的路径规划器与基于数字孪生的验证框架相结合。因此，在开发一种结合复杂环境适应性、高规划效率、局部逃脱能力、安全双向连接和面向工程的验证性的统一方法方面，仍然存在明显的研究空白。在这种背景下，进一步研究APF引导的双向RRT框架既是必要的，也是有意义的。本研究旨在为核电站检测和维护机器人开发一种路径规划方法，该方法结合了对复杂环境的适应性、高效率和工程实用性。通过将双向快速探索随机树（Bi-RRT）与人工势场（APF）方法相结合，并结合混合采样策略、势场引导的扰动机制、停滞检测与动态目标切换以及受限的分段连接策略，该方法能够在障碍物密集的狭窄空间中实现高效、安全和稳定的路径规划。此外，还开发了一个基于数字孪生技术的仿真和验证平台，以提高所提算法的工程适用性和可验证性。本研究的主要贡献可以归类为方法创新和应用特定开发，如下：
(i) 开发了一种结合随机采样、目标导向采样和势场引导采样的混合采样机制。引入了一种概率调度方案，以提高搜索效率和双向树的连通性，缓解了传统单一采样策略导致的探索不足和冗余扩展问题。
(ii) 提出了一种结合势场引导的扰动机制和停滞检测-自适应逃避策略。引入随机扰动，并在合力接近零或振荡时动态调整采样和势场参数，从而增强从局部最小值中逃脱的能力并避免扩展停滞。
(iii) 设计了一种动态目标切换机制和受限的分段连接策略，以提高双向树扩展和势场引导之间的协调性。距离约束和分段扩展增强了连接的可行性和安全性，减少了无效的连接尝试，并提高了整体路径质量和规划稳定性。
(iv) 该方法论框架针对以狭窄空间和密集障碍物为特征的核电站环境中的检测和维护机器人的路径规划进行了定制和验证。进一步开发了一个基于数字孪生的仿真平台，以支持场景构建、算法验证和性能评估，从而提高了所提方法的工程适用性。

本文的其余部分组织如下：第2节介绍了路径规划的基本理论，包括RRT、Bi-RRT和APF的原理。第3节介绍了Bi-RRT和APF的集成和改进，包括混合采样、扰动机制和连接策略等关键技术。第4节构建了数字孪生模型并实现了仿真平台。第5节通过2D和3D场景中的仿真验证和分析了算法的性能。最后，第6节总结了本文并概述了未来的研究方向。

2. 方法论
2.1. 快速探索随机树（RRT）算法
快速探索随机树（RRT）算法[9]是解决高维空间路径规划问题的重要方法。其基本思想是通过随机采样逐步构建一棵树，以快速找到可行路径。由于其强大的适应性和概率完备性，RRT算法已被广泛应用于机器人路径规划。RRT的基本思想是从起始点xstar（路径规划的起点）开始构建一棵树T。通过随机采样扩展树T，每次选择一个随机点xrand（路径规划中的随机点），然后控制器将其连接到树中最近的节点xnear（当前点的邻近节点）。树不断扩展，直到找到目标点或达到最大扩展限制。
(1) 树的生长
树T是一组节点，每个节点xi包含其状态空间中的坐标。树的根节点是起始点xstar，每个新节点xnew使用以下公式（1）连接到树中的现有节点xnear。这里，xnew是新节点的状态，Δt是每次扩展的步长，v(xnear, xrand)是从xnear到xrand的控制输入，通常是xrand方向的单位向量。该参数通过规范化两个节点之间的方向差异来获得，以引导树向xrand扩展。为了明确机器人运动模型，机器人状态定义为x ∈ χ，表示其在配置空间（2D或3D）中的位置。树扩展受到运动约束的限制，包括有界的步长和无碰撞要求，确保所有生成的节点都位于可行的无障碍空间内。

(2) 节点采样
在RRT算法中，每次迭代时，从状态空间中随机选择一个点xrand，并使用公式（2）计算xrand到树中最接近节点的距离dnear。然后选择xrand最近的最接近节点xnear来扩展树。

(3) 树的扩展
生成新节点xnew的公式是（3）。这里，α是用于控制树扩展速度的步长因子。步长因子α是一个显式的规划超参数，用于控制扩展粒度。根据环境规模、障碍物密度以及收敛速度和碰撞检测分辨率之间的期望平衡来选择α。在我们的实验中，2D情况下α = 0.5，3D情况下α = 0.3。

2.2. 双向快速探索随机树（Bi-RRT）算法
双向快速探索随机树（Bi-RRT）算法[10]是RRT算法的改进版本，旨在通过从起始点和目标点同时扩展两棵树来提高路径规划效率。与传统的RRT算法不同，Bi-RRT通过从起始点和目标点同时扩展树来缩短路径搜索时间，特别是在大型搜索空间中。Bi-RRT算法的基本思想是从起点xstar和终点xgoal同时开始，分别构建两个RRT树Tstar和Tgoal，直到这两棵树相遇。

(1) 起点和终点树的初始化
Bi-RRT算法首先初始化两棵树，其中Tstar是从起点xstar开始的树，Tgoal是从终点xgoal开始的树。这两棵树的根节点如方程(4)所示。方程(4)初始化了两棵树的根节点，分别对应于起点和终点的配置。这种初始化确保了搜索过程中的双向扩展。

(2) 树的扩展
与单向RRT算法类似，Bi-RRT中的每棵树都需要通过随机采样和树扩展来添加新节点。假设我们在起点树Tstar中选择一个随机点xrand，并选择距离最近的节点xnear进行扩展。新节点xnew(Tstar)使用方程(5)计算得出。同样地，对于终点树Tgoal，也选择一个随机点xrand进行扩展，得到新节点xnew(Tgoal)，如方程(6)所示。

(3) 两棵树的相遇
在Bi-RRT算法中，当起点树和终点树都扩展到一定程度时，算法会检查这两棵树是否已经相遇。假设当前的两个节点分别是xnew_start（起点树中的新节点）和xnew_goal（终点树中的新节点）。如果两棵树中新扩展的节点之间的欧几里得距离满足方程(7)，其中ε > 0是预设的树连接阈值，则认为这两棵树已经相遇。在模拟中，ε = 0.5。此时，两棵树之间的连接形成了从起点到终点的完整路径。

(4) 树的连接过程
如果两棵树相遇，算法会将它们连接起来形成一条路径。假设起点树中的一个节点是xstar，终点树中的一个节点是xgoal；通过连接这两个节点可以得到完整的路径。起点树的路径部分如方程(8)所示，终点树的路径部分如方程(9)所示。最终路径是这两部分的连接，如方程(10)所示。

2.3. 人工势场（APF）算法
尽管本工作中采用了改进的APF方法，但本节简要介绍了APF的基本原理，以便在介绍改进的集成方法之前澄清其机制。人工势场（APF）方法[7]是一种基于力场模型的路径规划方法，用于引导机器人到达环境中的目标点。APF算法的基本思想是通过在环境中为目标点、障碍物和机器人本身定义势场来控制机器人的运动。目标点产生吸引力，障碍物产生排斥力，机器人沿着合力方向移动，最终到达目标。在APF方法中，势场可以分为两部分：(1) 目标点的吸引力势场，使机器人朝向目标移动；(2) 障碍物的排斥力势场，防止机器人与障碍物发生碰撞。

(1) 吸引力势场
目标点xgoal产生一种吸引力，使机器人朝向目标点移动。目标的吸引力可以用梯度下降场来表示，其势能函数如方程(11)所示。吸引力和排斥力系数katt和krep并不是普遍最优的常数，而是可调的引导参数，用于调节APF辅助扩展过程中目标吸引力和障碍物排斥力的相对影响。在本研究中，通过初步的敏感性测试确定了这些参数的值（katt = 1.00, krep = 100），以避免两种不理想的极端情况：引导力过弱导致收敛缓慢，或者排斥力过强导致在杂乱区域附近振荡。这些值在考虑的模拟场景中提供了稳定的行为。为了提高透明度，实验中使用的所有参数值都在修订后的手稿中明确列出。这里，Uatt(x)是目标点的吸引力势；x是机器人的当前位置；xgoal是目标点的位置；katt是吸引力势场的系数，控制吸引力的强度。这个势能函数的梯度表示机器人的移动方向，如方程(12)所示。

(2) 排斥力势场
障碍物对机器人施加排斥力以防止碰撞。排斥力势场通常只在障碍物附近有效，其势能函数的形式如方程(13)所示，其中Urep(x)是障碍物的排斥力势；xobstacle是障碍物的位置；krep是排斥力势场的系数，控制排斥力的强度；d0是表示障碍物影响范围的阈值。这个势能函数的梯度表示排斥力的方向，如方程(14)所示。

(3) 作用在机器人上的合力
当机器人在环境中移动时，合力是由目标的吸引力和障碍物的排斥力矢量和决定的。作用在机器人上的总力如方程(15)所示。这里，Fatt(x)表示目标的吸引力，Frep,i(x)表示第i个障碍物产生的排斥力。机器人根据合力的方向移动，如方程(16)所示。机器人沿这个方向移动，直到到达目标或找到无障碍物的路径。在这些方程中，‖?‖表示欧几里得范数，εa > 0是一个小常数，用于避免‖Ftotal‖ = 0的情况。参数vmax表示当前模拟中用于运动更新的固定期望速度大小，而v(x)表示速度向量，其方向由合力Ftotal(x)决定。换句话说，在当前研究中，vmax被视为一个可调的常数，而不是状态依赖的控制变量。

(4) 运动更新
基于合力，机器人更新其位置。设机器人的当前位置为x；那么，下一个位置xnew可以使用方程(17)更新，其中Δt是时间步长。

(5) 复合势场
在有多个障碍物的情况下，APF方法的总势能是所有障碍物和目标势能的总和，如方程(18)所示。机器人根据总势能的方向移动。

3. APF和Bi-RRT算法的集成与改进
APF和Bi-RRT的集成结合了Bi-RRT的全局路径规划能力和APF的局部路径优化能力。Bi-RRT可以通过从起点和终点同时扩展来减少搜索空间并快速找到路径，而APF则优化路径，特别是在避障和局部调整方面，确保机器人避开障碍物周围的死胡同，并保持路径的平滑性和可执行性。Bi-RRT算法的目标是通过从起点和终点分别扩展两棵树来找到路径。然而，Bi-RRT算法生成的路径可能不够平滑，且可能受到障碍物的干扰。APF算法用于对每条扩展后的路径进行局部优化，特别是在障碍物密集或路径弯曲的区域。通过结合这两种方法，可以同时解决全局规划和局部避障问题，从而提高路径规划的效率和质量。

(1) 双向RRT树的初始化
从起点xstar和终点xgoal初始化两棵树Tstart和Tgoal，并设置最大迭代次数max_iter、步长α以及势场参数（吸引力系数katt和排斥力系数krep）。

(2) 双向RRT扩展
在每次迭代中，扩展从两棵树的末端节点开始。对于每个节点，生成一个随机采样点xrand，然后根据Bi-RRT规则选择最近的节点xnear，并根据方程(19)中的公式扩展树。

(3) 合力的计算和APF优化
在扩展过程中，使用人工势场方法计算当前位置的合力。每个节点受到目标的吸引力和障碍物的排斥力的影响。计算目标吸引力和障碍物排斥力的公式分别如方程(20)和(21)所示。这里，Urep(x)是障碍物的排斥力势；xobstacle是障碍物的位置；krep是排斥力势场的系数，控制排斥力的强度；d0是表示障碍物影响范围的阈值。机器人沿着合力的方向扩展路径。总合力Ftotal(x)是目标吸引力和障碍物排斥力的总和，如方程(22)所示。机器人的位置更新公式如方程(23)所示。这确保了机器人朝向目标点移动的同时避开障碍物。

(4) 路径连接
在扩展每个节点后，检查两棵树的末端节点是否相遇。如果相遇，则将两棵树连接起来形成完整路径，如方程(24)所示，其中ε是定义树相遇的阈值。

(5) 路径平滑处理
本研究使用三次B样条插值进行路径后处理平滑。控制点从规划器生成的有序路径节点中选取，在转弯角度超过预设阈值的地方插入额外的局部细化点。为了实现简单性，采用均匀的节点间距。在平滑过程中，起点和终点保持固定，而中间控制点在曲率变化约束下进行调整，以减少急转弯，同时不改变全局路径拓扑或违反避障可行性。通过应用基于B样条的平滑处理，整体路径形状保持不变，只进行局部调整。限制曲率变化Δ?可以防止速度峰值过大。如方程(25)所示，P(s)表示平滑后的连续路径，Ni,k(s)是k阶B样条基函数，Qi表示用于局部细化的插入或调整的控制点，而不改变全局拓扑。路径曲率κ(s)通过其平方值最小化以减少急转弯，Pj表示原始路径的关键节点以保持全局形状，Δ?(s)表示相邻段落之间的方向角度变化，以抑制速度峰值和运动不连续性。

基于上述的Bi-RRT–APF融合框架，本研究提出了一种改进的APF引导的双向RRT融合算法，以进一步提高在设备密集和狭窄环境中运行的核电站检测和维护机器人的搜索效率和规划稳定性。为了解决采样效率不稳定、局部停滞以及复杂环境中频繁出现无效双向连接尝试等问题，所提出的方法结合了混合采样策略、势场扰动机制、停滞检测与自适应逃逸策略以及受限的分段扩展连接方案，以增强路径的可行性和安全性。

(6) 混合采样策略
传统的Bi-RRT–APF方法通常采用固定的随机采样和势场引导采样比例，这可能导致在狭窄通道或障碍物密集区域探索不足或扩展冗余。为了克服这一限制，采样策略扩展为三类，采用概率调度，使算法能够在全局探索和快速连通性之间取得平衡。设采样点为xrand，其生成机制如方程(26)所定义。这里，χ表示可行状态空间，ξ1,ξ2～U(0,1)是均匀分布的随机变量。prand和ptarget分别表示随机采样和目标偏置采样的概率。在模拟中，prand = 0.5 和 ptarget = 0.3，其余的概率质量被分配给势场引导的采样。TargetBiasSample(Toother) 从另一棵树 Tother 中选择一个节点作为引导目标，以增加双向树快速收敛的概率。α 是扩展步长， 是归一化的合力方向，δ 是引入的一个扰动项，用于增强局部逃逸能力（见下一小节）。这种混合采样机制在典型的核电站检查和维护环境中提高了连通性：随机采样确保了全局探索，目标导向采样加速了双向树的连接，而势场引导采样则在障碍物密集的区域增强了局部扩展的效率和方向性。

在核电站中，设备密集的区域通常包含狭窄的通道，在这些通道中，势场引导可能会因为合力抵消而出现停滞或振荡。为了在复杂的障碍物配置下提高算法的稳定性，在势场引导采样中引入了一个扰动项 δ，如方程（27）所定义。在实现中，δ 从一个均值为零的高斯分布中采样，其中 σ 控制扰动的幅度。扰动仅在势场引导采样期间激活，并在每个符合条件的扩展步骤中更新。在模拟中，σ = 0.3。因此，采样点沿着合力方向扩展，同时保留随机扰动，防止在相同的势场轨迹附近长时间停留，并增加逃离局部最小值的概率。

由于双向树的交替扩展，使用 xgoal 作为从目标树扩展时的吸引中心可能会导致目标附近的冗余生长。为了确保势场引导和双向扩展之间的一致性，采用了一种动态目标切换机制：在扩展起始树时，引导目标是 xgoal；在扩展目标树时，目标切换到 xstar。目标定义在方程（28）中。

在核电站的检查和维护场景中，狭窄的通道加上多个障碍物很容易导致树扩展过程中的局部停滞。为了提高规划稳定性，引入了一种停滞检测机制，通过记录最近的 M 个扩展节点 {xk?M+1,…,xk} 并计算最大位移，如方程（29）所定义。在本研究中，M 被选为一个短滑动窗口长度，以平衡灵敏度和鲁棒性：如果 M 太小，短暂的振荡或短期的不成功扩展可能会被误判为停滞；如果 M 太大，触发器会变得不敏感，可能会延迟从真正的局部陷阱中逃逸。考虑到树以固定的步长 α 扩展，M 被设置为使得缓冲轨迹覆盖有限的局部邻域，而不是长范围的运动历史。在模拟中，经过在典型的 2D 和 3D 杂乱场景中的经验敏感性测试后，采用了 M = 8，它提供了稳定的停滞识别，而不会频繁地错误触发。因此，当 Δmax < α 时，扩展被认为是陷入了局部停滞区域。其理由是 α 是树的名义单步扩展长度。因此，如果最近 M 个节点覆盖的最大位移仍然小于一名义扩展步长，那么缓冲轨迹未能产生有效的向外进展，很可能在受限的局部区域内振荡或漂移。这个标准提供了一个与规划器步长直接相关的尺度一致的停滞指标。

为了避免当双向树端点之间的距离过大时进行盲目的连接尝试，在连接阶段施加了距离约束，并采用了分段扩展连接方案。设候选连接点为 xnew，目标树的终端节点为 xend。只有当满足方程（30）中的条件时才允许连接，其中 η 表示连接距离阈值。在连接过程中，应用步长为 α 的分段扩展，如方程（31）所示。在每个扩展段中以离散的方式实施碰撞检测。具体来说，每个段在固定的空间间隔内均匀采样，并在所有采样点对障碍物模型进行碰撞测试。只有当所有采样点都没有碰撞时，才接受该段。因此，本研究采用了离散的近似方法来进行连续的碰撞检测，采样间隔相对于障碍物大小和扩展步长来说足够小。这种策略减少了无效的连接尝试，并提高了生成路径的可行性和安全裕度，更好地满足了核电站检查和维护任务对障碍物避免的严格可靠性要求。改进的 APF–Bi-RRT 融合算法的伪代码在算法 1 中给出。

算法 1：改进的 APF–Bi-RRT 融合算法
输入：xstart, xgoal, α, max_iter, katt, krep, d0, ε, prand, ptarget, M, η, σ
输出：优化路径 Popt

1. 初始化 Tstart←{xstart}, Tgoal←{xgoal}
2. 初始化位置缓冲区 B←?
3. 对于 i = 1 到 max_iter 执行以下操作：
4. 对于每个树 T∈{Tstart,Tgoal} 执行以下操作：
5. 通过方程（28）确定吸引目标 xatt
6. 使用方程（26）通过混合采样生成 xrand
7. 如果 xrand 在障碍物内部，则继续
8. xnear←NearestNode(T, xrand)
9. xnew←xnear + α?Normalize(xrand ? xnear)
10. 通过方程（20）和（21）计算 Fatt(xnear), Frep(xnear)
11. 通过方程（22）计算 Ftotal(xnear)
12. 通过方程（23）计算 xnew
13. 如果 CollisionFree(xnear, xnew) 则
14. 在 T 中添加节点 xnew
15. 更新缓冲区 B←B∪{xnew}
16. 如果 ∣B∣≥M| 则
17. 通过方程（29）计算停滞度量 Δmax
18. 如果 Δmax < α 则
19. 触发自适应逃逸：增加 prand，调整 (katt, krep)，重置 B←?
20. 结束 if
21. 结束 if
22. 结束 if
23. 结束 for
24. 如果 Distance(End(Tstart), End(Tgoal) < ε 则
25. 使用方程（30）和（31）尝试受限的分段连接
26. P←ConnectPaths(Tstart, Tgoal)
27. 结束 if
28. 结束 for
29. 通过方程（25）平滑路径 Popt
30. 返回 Popt

4. 算法的数字孪生实现
本节通过数字孪生技术实现了上述算法。首先，基于智能制造的五维数字孪生模型，建立了一个适用于核电站机器人维护过程中路径规划的五维数字孪生模型，如方程（32）所示。

在这个方程中，CP 代表维护场景中核电站机器人的物理实体；CV 代表虚拟模型，包括场景建模（Scene Model）、移动底盘建模（Mobile Chassis）和机器人臂建模（Robot Arm）；CS 指的是路径规划的服务系统，包括移动和操作过程中的服务；DD 代表孪生数据，包括全局和局部路径及运动规划；CN 代表组件之间的连接，不同元素之间的连接有不同的含义。核电站机器人路径规划的物理空间以建筑机器人的物理实体（CM）为中心。在这个场景中，物理实体层中的移动底盘和机器人臂至关重要。这一层构成了数字孪生框架的基础，并为物理-虚拟交互提供了必要的数据。虚拟模型（CV）是物理实体的虚拟表示，准确映射的关键在于多学科知识的整合。基于物理实体构建了一个高精度的数字虚拟模型，这是数字孪生概念的核心。模型构建工作可以与 SolidWorks（2024）、3D Max、Unity、MATLAB、ROS 等工具协作完成。在这个场景的虚拟模型层中，场景建模（Scene Model）、移动底盘建模（Mobile Chassis）和机器人臂建模（Robot Arm）非常重要。模型必须整合几何、物理、行为和规则，以及在功能性和结构上的要求，例如考虑移动底盘的转向几何原理和机器人臂的运动学模型。使用面向对象的特征来修改模型属性，使模型与物理实体的运动保持一致，为机器人的仿真、建模、预测和推理工作提供了 3D 可视化基础。数据（DD）是物理实体（CM）和虚拟模型（CV）之间交互的基础，驱动数字孪生系统的操作。在这个场景中，主要的孪生数据包括全局规划数据和局部规划数据。全局规划数据包括 Bi-RRT 算法中的起始树、目标树、最近邻节点和扩展步长坐标，而局部规划数据包括 APF 算法中的障碍物坐标以及吸引力和排斥力值。在本研究中，这些数据在基于 MATLAB 的仿真循环中在线更新，并与虚拟场景同步，用于算法执行、状态显示和轨迹回放。因此，“全局规划数据”不仅用于仿真后的可视化；相反，它们还参与仿真环境内的迭代规划过程。然而，由于当前框架尚未连接到物理机器人控制器或现场传感器，这里描述的实时更新应理解为虚拟测试平台内的仿真时间同步，而不是硬件级别的物理-虚拟闭环同步。核电站维护机器人路径规划数字孪生系统的应用服务（CS）侧重于其功能能力，包括机器人状态的可视化、路径和运动规划的准确反馈、实时监控以及为操作员提供维护决策支持。核电站维护机器人路径规划的数字孪生在图 1 中显示。在本研究中，数字孪生作为 Bi-RRT-APF 算法的虚拟测试和验证平台，而不是物理机器人系统。它实时反映了机器人的运动学参数、工作空间几何和障碍物环境，以在部署前验证算法的性能。在本研究中，“数字孪生”一词用于仿真导向的意义，即作为一个在仿真循环内同步机器人运动学、环境几何和规划状态数据的虚拟测试和验证环境。它尚未代表一个连接到在线传感器、控制器或已部署机器人平台的完整物理-虚拟闭环孪生。图 1. 核电站维护机器人路径规划的数字孪生。工作流程图如图 1 所示。工作流程包括：(i) 环境和机器人模型初始化；(ii) 全局和局部规划数据的生成；(iii) 使用 APF 引导的采样和停滞监控的双向树扩展；(iv) 碰撞检测和受限连接；(v) 路径平滑和性能评估；(vi) 虚拟场景中的同步可视化和轨迹回放。该图有助于区分数字孪生平台与核心路径规划算法的功能角色。

基于提出的算法框架并结合数字孪生技术，实现了核电站检查和维护机器人的路径规划。将提出的 Bi-RRT–APF 融合算法与现有算法进行比较，以评估其在路径规划性能上的优势。选择压力容器底部的碎片回收机器人作为代表性示例。这种机器人通常用于反应堆压力容器（RPV）主回路和底部区域的检查和异物清除任务，特别是在换料停机期间。在压力容器底部进行碎片回收时，机器人需要对移动底盘进行二维路径规划，对末端执行器在密集且受限的控制棒驱动机构（CRDM）结构内进行三维路径规划。环境还包含静态和动态障碍物，如螺栓、垫圈、金属碎片和焊接渣。为了降低路径规划和碰撞检测的计算复杂性，采用了保守的安全裕度。复杂的障碍物使用六面体边界模型进行简化，如图 2 和图 3 所示。应当注意的是，六面体表示在这里用于碰撞检测效率，而不是对所提出规划器的理论限制。原则上，Bi-RRT–APF 框架也可以处理非凸障碍物，前提是它们由凸边界体积的并集或足够细的碰撞检测原语表示。在这种表示下，非凸几何被转换为多个局部障碍物约束，规划器仍然以相同的方式执行扩展、碰撞检测和连接。然而，像其他基于采样的规划器一样，该方法无法理论上保证完全避免在极窄的非凸陷阱中的局部困难；相反，提出的扰动和停滞逃逸机制旨在减少陷入的概率并提高在这种环境中的实际逃逸能力。所有实验都在 MATLAB R2023b 仿真平台上进行。选定的性能评估指标如下：(1)平均总路径长度：移动底盘或操作臂从起点到目标点所行驶的距离。较短的路径长度表明规划结果更为直接和高效，有效减少了不必要的绕行。(2)计算时间：算法计算完整路径所需的时间。较短的计算时间表明计算效率更高。(3)迭代次数：算法从初始状态生成可行路径所需的迭代次数。该指标反映了收敛速度和搜索效率。(4)成功率：在相同场景设置下，成功规划尝试次数与总尝试次数的比率。该指标反映了算法在杂乱环境中的鲁棒性。(5)路径平滑度：通过离散化后生成路径上的累积转向角度变化来量化。较低的值表示较少的突然方向变化和更好的运动连续性。图2. 压力容器底部的碎片回收机器人。图3. 三维环境中压力容器的简化障碍物建模。为了验证算法的可行性，使用MATLAB (R2023b)构建了包含静态和动态障碍物的环境，这些障碍物是通过随机生成与手动细化的混合策略配置的。在不同仿真案例中改变障碍物的位置和分布，以评估路径规划算法对环境变化的适应性。需要强调的是，这些测试对应于变化场景的障碍物设置，而不是在单次规划运行中的完全在线动态障碍物避让。RRT、Bi-RRT和Bi-RRT–APF算法是使用基于数字孪生的框架与MATLAB集成的。在二维环境中，起点设置为(0, 0)，终点设置为(100, 100)；在三维环境中，起点设置为(0, 0, 0)，终点设置为(100, 100, 100)。所有算法都在相同的环境中进行了测试，每种算法重复了50次。最大迭代次数设置为1000次，初始参数katt = 1.0和krep = 100。代表性的路径比较结果和步进时间比较分别显示在图4、图5、图6、图7、图8和图9中。图4、图5、图6、图7、图8和图9是根据MATLAB仿真结果生成的。每个案例对应于一个随机采样实验，其中环境条件（起点、终点和参数设置）保持不变，只有障碍物分布的随机种子不同。仿真完成后，使用MATLAB可视化模块导出路径轨迹，以确保结果的一致性和可重复性。图4. 二维环境中核电站检查和维护机器人的路径与传统算法的比较。图5. 三维环境中核电站检查和维护机器人的路径与传统算法的比较。图6. 与传统算法的路径计算效率比较。图7. 二维环境中核电站检查和维护机器人搜索路径的基线算法比较：(a)均匀随机采样；(b)标准APF引导方法；(c)固定目标引导和直接连接；(d)Bi-RRT-APF。图8. 三维环境中核电站检查和维护机器人搜索路径的基线算法比较：(a)均匀随机采样；(b)标准APF引导方法；(c)固定目标引导和直接连接；(d)Bi-RRT-APF。图9. 与基线算法的路径计算效率比较。如图4和图5所示，与RRT和Bi-RRT相比，Bi-RRT–APF在二维和三维场景中都表现出更强的搜索聚焦和更稳定的连通性趋势。树扩展倾向于沿着可行走廊向目标前进，而在障碍物密集区域的绕行更加连贯，冗余分支减少。这一观察结果得到了图4a中RRT的“向外发散扩展”行为和图5a中3D RRT的低效探索甚至停滞的支持。如图4b和图5b所示，Bi-RRT加速了起点到终点的连接，但在随机分布的障碍物下仍然存在局部冗余增长和路径平滑度不足的问题。相比之下，图4c和图5c中的Bi-RRT–APF轨迹更紧密地遵循走廊结构，从而产生了更连续的障碍物避让轨迹和更少的无效探索。这种改进主要源于混合采样策略，它在同一迭代框架内同时保持了全局探索和快速的双向连接。随机采样确保了空间覆盖，目标偏向采样增加了树相遇的概率，而势场引导采样增强了障碍物附近的局部扩展方向性。因此，由于固定比例采样在受限空间中导致的探索不足或冗余扩展得到了缓解，收敛过程变得更加稳定。此外，图5中的3D比较结果进一步表明，在设备密集和通道狭窄的条件下，Bi-RRT–APF的停滞风险较低，扩展过程表现出较少的来回探测行为或振荡模式。这一趋势体现在3D RRT在某些情况下倾向于表现出局部效率低下，而Bi-RRT可能由于障碍物耦合仍然会产生冗余增长。相比之下，Bi-RRT–APF更容易形成向目标连续前进的可行路径。这种行为可以归因于势场扰动机制和停滞检测-逃避策略之间的协同作用。当在障碍物密集区域取消结果势场力时，导致振荡或结果力接近零时，扰动项δ引入小的随机干扰，以防止采样点锁定在相同的势场轨迹附近。同时，停滞检测根据最近M次扩展的位移阈值触发逃避策略，增加随机采样的比例并调整势场权重。这种设计增强了逃离局部最小值或狭窄“死区”的能力，从而在复杂操作条件下提高了规划稳定性。从图6可以看出，与RRT和Bi-RRT相比，Bi-RRT–APF的计算效率显著提高。这通过平均迭代次数从533（RRT）和329（Bi-RRT）减少到185，以及计算时间从20.5和9.5分别减少到4.9得到证明。特别是，迭代次数减少了65.3%和43.8%，计算时间减少了76.1%和48.4%。这是因为所提出的算法通过整合势场引导和双向扩展增强了搜索方向性，从而减少了冗余探索并加速了向可行路径的收敛。此外，从图6还可以观察到，Bi-RRT–APF算法的成功率更高，路径平滑度也得到了改善。这一点通过成功率提高到95%（RRT为82%，Bi-RRT为93%）以及路径平滑度指标从5.3和3.3减少到2.9得到证实。这归因于混合采样策略和停滞检测-逃避机制，它们提高了障碍物密集环境中路径规划的鲁棒性，有效缓解了不稳定的扩展和局部振荡。图6中的一个显著观察结果是，效率提升趋势在不同的评估指标中保持一致，表明所提出的方法在计算成本和路径质量之间实现了平衡优化。这可以通过迭代次数和计算时间的同步减少而无需牺牲成功率或平滑度来证明。这种现象的原因是动态目标切换机制确保了吸引势场与当前扩展树之间的一致性，从而避免了冗余增长并改善了双向协调。考虑到这些发现，可以得出结论，所提出的Bi-RRT–APF融合算法在复杂环境中显著提高了路径规划效率，同时保持了高解决方案质量和鲁棒性。为了全面评估改进后的Bi-RRT-APF算法中每个关键模块对整体性能的影响，在本节中设计并进行了一项消融研究，研究环境为典型的核维护环境，其中障碍物密集且通道狭窄。通过逐步用基线方法替换关键模块，分析了每个模块对路径规划性能的贡献。由于之前已经验证了Bi-RRT和APF混合框架与单一算法相比的有效性，因此本节重点关注三种改进模块的消融分析。首先，将混合采样策略（随机采样、目标偏向采样和势场引导采样）替换为传统的均匀随机采样机制。其次，移除势场引导扰动机制、停滞检测机制和自适应逃避机制（扰动和停滞避免机制），采用标准APF引导方法。第三，移除动态目标切换机制和受限分段连接机制，恢复到固定目标引导和直接连接。消融实验在相同的2D和3D仿真环境中进行，起点和终点、迭代限制和算法参数相同。不同模块组合下的路径规划结果分别显示在图7、图8和图9中。如图7、图8和图9所示，移除混合采样策略导致搜索过程明显分化，冗余扩展显著增加。计算时间增加了约15.9%，而收敛速度减少了34.7%。这是因为均匀随机采样缺乏目标引导和局部方向性，导致大量无效探索。此外，移除扰动和停滞检测机制使得算法在障碍物密集区域更容易陷入局部振荡或停滞，导致路径生成中断或重复搜索，成功率降低。这是因为传统APF缺乏在结果力接近零的区域逃避的能力。进一步地，移除动态目标切换和受限分段连接机制会导致双向树链接阶段更多的无效连接尝试，降低路径安全裕度和连接效率，并增加路径不连续性和局部碰撞风险。总体而言，每个改进模块都在提高搜索效率、改善局部逃避能力和确保路径可行性方面发挥了关键作用。在与代表性高效算法的进一步比较中，图10和图11显示Bi-RRT–APF在关键效率指标上保持了优越的性能。与Quad-RRT和KB-RRT*相比，平均迭代次数分别减少了29.3%和8.4%，平均计算时间分别减少了25.2%和10.3%。这种改进主要归因于采样节点的力引导扩展，使机器人能够更好地适应复杂的核检查和维护环境，并促进生成更高质量的路径。受限分段连接策略减少了由于盲目长距离连接和双向树相遇时的碰撞失败导致的无效尝试。具体来说，只有当端点距离满足阈值η时才触发连接，并且每个分段都进行步长为α的分段扩展，并进行碰撞检查。这种设计加强了路径生成期间的可行性评估和安全裕度，减少了“失败连接–回滚–重试”的额外成本，并使计算时间和迭代次数更加可控。总体而言，基于图4、图5、图6、图7、图8、图9、图10、图11和图12的结果，本节表明所提出的改进后的Bi-RRT–APF方法增强了典型2D/3D复杂核检查和维护环境中树扩展的方向性和生成路径的连续性。此外，混合采样、带停滞逃避的扰动、动态目标切换和受限分段连接的组合效应显著提高了收敛稳定性，降低了局部停滞的风险，并在连接阶段提高了安全性和可行性。这些改进体现在更少的迭代次数和更短的计算时间上。图10. 二维环境中核电站检查和维护机器人搜索路径的外部算法比较。图11. 三维环境中核电站检查和维护机器人搜索路径的外部算法比较。图12. 与外部算法的路径计算效率比较。结论

为了解决核电站检测与维护机器人在空间受限且障碍物密集的环境中规划效率低、容易陷入局部最优解以及路径可行性受限的问题，本研究提出了一种路径优化仿真方法，该方法将双向快速探索随机树（Bi-RRT）算法与人工势场（APF）方法相结合。在数字孪生框架内系统地评估了所提方法的性能。主要结论如下：

(1) 通过构建一种结合随机采样、目标导向采样和势场引导采样的混合采样机制，并引入势场扰动、停滞检测和动态目标切换策略，改进后的Bi-RRT–APF算法显著提升了在复杂核电站检测与维护环境中的全局搜索能力和局部引导效率。该方法有效降低了陷入局部最小值和搜索停滞的风险，从而提高了规划过程的稳定性。

(2) 为减少双向树连接阶段中的无效扩展和安全性裕度不足问题，设计了一种受限的分段连接策略。该策略在保持避障安全性的同时减少了盲目连接尝试。仿真结果表明，与传统RRT、Bi-RRT及其多种改进版本相比，所提方法在2D和3D检测与维护场景中实现了更少的迭代次数和更短的计算时间。生成的路径更加连续和平滑，从而显著提升了整体规划效率和路径质量。

(3) 开发了一个基于数字孪生的核电站检测与维护机器人路径规划仿真平台，实现了算法、机器人运动学模型和操作环境模型之间的协调映射。该平台为性能评估和工程验证提供了有效工具。结果表明，所提方法在仿真验证层面具有较好的工程实用性。然而，由于本研究仅限于基于MATLAB的数字孪生仿真平台，尚未包含硬件在环或真实机器人实验，因此该方法的实时部署能力仍需进一步验证。目前的结果证明了该方法在仿真案例层面对变化环境的适应性，而如何在不断移动的障碍物下进行真正的在线重新规划则是未来工作的重点。