SWC机器人高效信息增强RRT*运动规划

doi:10.11918/202509032

SWC机器人高效信息增强RRT^*运动规划

doi: 10.11918/202509032

孙朝业^1,2 ，孙昊晟^1,2 ，吴庆祥^1,2 ，杨桐^1,2 ，孙宁^1,2

1. 南开大学人工智能学院机器人与信息自动化研究所，天津 300350

2. 南开大学深圳研究院智能技术与机器人系统研究院，深圳 518083

基金项目: 国家重点研发计划（2023YFC3011000）；国家自然科学基金（62533014，62373198）；天津市自然科学基金（24JCZXJC00220）

详细信息

作者简介

孙朝业(2001—)，男，硕士研究生；

孙宁(1988—)，男，教授，博士生导师

通讯作者

孙宁，sunn@nankai.edu.cn

中图分类号: TP242.2

文献标识码: A

文章编号: 0367-6234(2026)01-0047-09

Efficient information enhanced RRT^* motion planning for SWC robots

SUN Chaoye^1,2 ， SUN Haosheng^1,2 ， WU Qingxiang^1,2 ， YANG Tong^1,2 ， SUN Ning^1,2

1. Institute of Robotics and Automatic Information Systems, College of Artificial Intelligence, Nankai University, Tianjin 300350 , China

2. Institute of Intelligence Technology and Robotic Systems, Shenzhen Research Institute of Nankai University, Shenzhen 518083 , China

摘要

自重构波状爬行(self-reconfiguration wave-like crawling,SWC)机器人具有特殊的串/并联连接状态，其在运动规划过程中更需要生成连续可行的轨迹。传统运动规划算法存在效率低下、生成路径不符合运动学约束的问题。本文提出了一种改进的信息增强快速探索随机树运动规划方法。首先以RRT-Connect(rapidly-exploring random tree connect)算法为基础，生成初始可行路径，构建椭圆状态空间采样域，实现随机树的快速生长。其次，基于最小化的加加速度目标函数和Hessian矩阵优化多项式轨迹，生成符合SWC机器人运动学特性的平滑轨迹。最后，基于不同的障碍物场景进行路径规划仿真，以验证优化后算法的效果。仿真结果表明，相较于传统算法，所提方法在多种障碍物环境中可显著提升路径规划效率，缩短全局采样时间和规划路径长度，并可有效地避免SWC机器人运动过程中的急加速转弯，消除路径中的尖锐转折，更符合实际作业需求。

关键词

自重构机器人 / 双向搜索 / 路径规划 / 速度轨迹规划 / 自主避障

Abstract

Self-reconfigurable wave-like crawling (SWC) robots, characterized by their unique serial/parallel connection states, imposes stringent requirements for generating continuous and feasible trajectories during motion planning. Conventional motion planning algorithms suffer from inefficiency and fail to satisfy kinematic constraints. To address these limitations, this paper presents an enhanced motion planning method termed as informed optimal rapid-exploration random tree. First, the rapidly-exploring random tree connect (RRT-Connect) algorithm is employed to generate an initial feasible path and construct an elliptical state-space sampling domain, facilitating rapid expansion of the random tree. Besides, polynomial trajectories are optimized based on a minimized jerk objective function and Hessian matrix to generate smooth motion profiles that conform to the kinematic characteristics of the SWC robot. Finally, simulations conducted in diverse obstacle scenarios validate the effectiveness of the optimized algorithm. The results demonstrate that, compared to traditional algorithms, the proposed method significantly enhances path planning efficiency across various obstacle environments, reducing global sampling time and shortening planned path length. Furthermore, it effectively mitigates abrupt acceleration and sharp turns during the robot’s movement, yielding paths that are better suited for practical operational applications.

Keywords

self-reconfiguration robots / bidirectional search / path planning / trajectory planning / autonomous obstacle avoidance

1 SWC机器人运动规划 2 高效信息增强RRT^*运动规划算法设计 2.1 信息增强RRT^*算法 2.2 基于最小化jerk的轨迹规划 3 仿真结果分析 3.1 高效信息增强RRT^*算法路径规划 3.2 路径规划仿真对比 3.3 基于最小化jerk的轨迹优化仿真 4 结语

移动机器人凭借其独特的自主性和智能性，能够深入人力难以到达的危险作业区域，在国防科技^[1]、民生服务^[2]、工业运输^[3]等多个关键领域得到广泛的应用，其中，自重构波状爬行（SWC）机器人受到较多关注。SWC机器人拥有多个运动模态，可以适应于不同的任务场景，如在并联模态下可以实现灵活的差速转向，从而满足紧急避障任务的高效性。SWC机器人在执行任意的自主对接任务时，均需要在避开障碍物和危险区域的情况下，快速、准确地到达目标位置。因此，充分考虑其运动特性并为SWC机器人规划合理的路径是完成任务的重要基础^[4]。

迄今为止，国内外学者已研发出很多路径规划算法技术，大致分为人造势场局部规划算法、仿生智能算法、基于网格的搜索算法和基于采样的搜索算法。具体而言，人造势场法^[5-6]是假定目标区域在机器人上产生吸引力，而障碍物在其上产生排斥力，通过合力来直接控制机器人的运动。通过这种方法获得的路径通常是平滑可靠的^[7]。但是，该算法在复杂环境中很容易陷入局部最优解^[8]。仿生智能算法的灵感来自于自然动物群落，包括遗传算法^[9]和蚁群算法^[10]等，该类算法适用于求解和优化比较复杂的问题，但其规划效率需要很高的算力条件，通常运行时间比较长，较难达到收敛^[11]。基于网格的搜索算法以A^*算法^[12]为代表。A^*算法在简易障碍物环境下，具有保证最优性和高效性的路径规划效果，但其仅适用于离散的低维空间^[13]，随着路径规划空间所需维度的增加，其计算时间和内存空间也会形成指数性增加，将导致无法生成路径规划结果。基于采样的算法不必需构建配置空间，典型算法为快速探索随机树（rapidly-exploring random trees，RRT）算法^[14-15]，其通过迭代不断添加随机节点绘制树状分支的路径图。RRT的优点是可以在高维空间中迅速找到初始路径，但在狭窄的频道方案中表现不佳^[16-18]。RRT^*被认为是RRT的重要扩展，其通过引入邻居节点扩展搜索和重新布线树以达到最佳路径。由于采样原理的随机性，RRT^*每一次规划效果都相差甚大，致使可能需要迭代足够多的次数才能找到可行的路径，因此算法的鲁棒性较差^[19-25]。

为此，研究人员提出了一系列RRT^*算法的改进方法，如：Tu等^[26]为了改进RRT^*算法在狭窄区域出现的局部最优问题，提出了多树扩展策略来加快算法的收敛速度，提高路径质量。Wu等^[27]提出了Fast-RRT算法，分为两个模块，第1个模块是改进的RRT，目的是找到稳定的初始路径，第2个模块是Fast-Optimal，其在合并多条初始路径后可以得到一条近乎最优的路径。上述改进思路均需要高算力配置。值得注意的是，双向搜索的改进思路是最直接的，且不需要耗费大量算力资源，Wang等^[28]在树扩展过程中实现双向搜索的策略，同时增长前向树和后向树，当这两棵树相遇时，后向树将充当启发式方法，引导前向树不断向目标状态增长。Chen等^[29]使用RRT算法来连接前向树和后向树，足够多的迭代后可以生成复杂障碍物环境下的最优路径。然而，双向路径规划搜索生成的路径由折线组成，并不符合SWC机器人的客观运动情况。

针对SWC机器人运动规划过程中的上述问题，本文提出了一种高效信息增强RRT^*运动规划方法。首先，引入RRT-Connect算法快速搜索从初始状态到目标状态的局部最优路径，并将该路径的长度应用于信息增强的RRT^*算法，作为初始随机树的一部分，从而使随机树能够在已知可行路径的基础上构建，进一步加速树的扩展过程。其中，为避免在初期阶段进行全局采样，本文设计了启发式椭圆状态空间采样域。在此基础上，通过结合最小化jerk的算法为路径添加时间参数，规划出符合SWC机器人运动学要求的运动轨迹。最后，通过仿真验证所提方法的有效性。

1 SWC机器人运动规划

SWC机器人由多个独立的模块组成，能够根据任务和环境的变化，依靠相应的控制方法和所设计的串/并联连接方式自动地组装，从而使机器人具备自重构能力以应对复杂困难的任务。由于SWC机器人模块采用特殊的行波运动机构，单个SWC机器人只能进行直线前进或后退，通过并联连接可以实现差速转向，能够灵活地进行轨迹跟踪，显著提升SWC机器人的可拓展性。

图1介绍了SWC机器人的串/并联连接方式，串联连接机构主要由舵机、直线电机、偏航关节、俯仰关节、公对接器和母对接器组成。舵机安装在母对接器内部，其输出轴与丁字滑块连接，当舵机转动时会带动丁字滑块在母对接器内部滑动。母对接器和公对接器分别设有3个插孔和插销，当两者相互配合时，舵机带动丁字滑块使其与公对接器的插销互锁，实现串联连接。此外，公对接器还通过球形连接器与直线电机连接，这是一个2自由度的机器人关节，能够实现偏航和俯仰角度旋转。如图1（a）所示，当多个SWC机器人串联连接时，可通过所设计的2自由度关节和直线电机改变机器人模块或操作装置的姿态角，从而极大地增强不同模块的协作能力，越过沟壑、台阶等复杂障碍物。如图1（b）所示，当两台SWC机器人左右排列时，可以在操作人员帮助下，使强磁铁相互吸引，实现并联连接。SWC机器人在实现自主对接、轨迹跟踪等复杂任务时，需设计高效可靠的轨迹规划方法，并通过与控制算法的结合来支持其构型在串/并联之间的切换。SWC机器人在并联模态下可通过差速转向机制实现更高自由度的运动控制，具备优越的灵活性与轨迹跟踪能力。因此，本文提出的轨迹规划算法以SWC机器人的并联模态为主要研究对象，充分发挥其机动性能的优势，实现精准、稳定的自主作业任务。SWC机器人灵活的连接方式可适用于多种任务场景，并需要符合其运动学要求的高效准确的运动规划。

SWC机器人运动规划目标是完成在执行任务过程中规划起点位置到目标点位置的一系列运动操作。通常将运动规划分为路径规划和轨迹规划。其中，路径规划要求尽量用最短的时间寻找出一条从起始点到目标点的可行路径；轨迹规划部分则是将得到的可行路径进一步进行平滑性处理，以符合SWC机器人运动学要求和实际运动情况。

图1串/并联连接实物图

Fig.1Design and physical diagrams of series/parallel connection mechanisms

如图2所示，SWC机器人的并联连接机构主要由第1侧板、第2侧板和强磁铁组成。强磁铁按照南、北极分别安装在第1侧板、第2侧板表面的安装槽中，在并联连接下可以通过调整主动对接模块、被动对接模块两个模块的爬行速度差，实现灵活的前进、后退、转向等运动。在有坐标系的运动规划情况下，设立了方形和圆形障碍物，SWC机器人需要生成连续可行的轨迹，进而从起始位置出发抵达目标位置。本文提出的高效信息增强RRT^*运动规划方法，能够确定一条由主动模块到被动模块的对接轨迹，进而实现SWC机器人的自主对接任务。

图2SWC机器人自主对接任务下的运动规划

Fig.2Motion planning for the autonomous docking task of SWC robots

令

X \subset R^{2}

SWC 机器人的运动规划状态空间，

X_{col} ⊊ X

设定为SWC机器人与障碍物发生碰撞的状态集， X_free=X-X_col为自由空间。另外，设立SWC机器人的初始状态为χ_initial∈X_free，目标状态为χ_goal∈X_free。定义从初始位置到目标位置的简易道路为路径

δ \subset X

。基于上述定义，最优路径规划的问题可转化为寻找全局最优路径δ^*，定义最小成本函数为

c : Σ \mapsto R_{⩾ 0} ， Σ

为所有非平凡路径的集合。上述定义需要确保χ_initial与χ_goal通过式（1）表示的自由无障碍空间相连，其表达式为

\begin{matrix} δ^{*} = a r g \underset{δ \in Σ}{m i n} \{c (δ) ∣ δ (0) = χ_{initial}, δ (1) = \\ χ_{goal}, \forall s \in [0,1], δ (s) \in X_{free}\} \end{matrix}

(1)

得到的规划结果δ^*是一条静态空间路径，然而SWC机器人的实际运动轨迹需要引入时间作为参数来满足相应的运动约束。因此，轨迹规划的目标是生成一条时间参数化的连续的轨迹

η : [0 ， T] \mapsto X_{free}

，其中T是SWC机器人的总运动时间。

在对下述n阶多项式轨迹方程的高阶导数进行建模与优化后，SWC机器人在轨迹规划过程中可有效降低运动的不稳定性和震荡，进而实现更符合客观实际运动情况和更加安全高效的运动控制。

对于任意时刻，轨迹以n阶多项式表示如下：

p (t) = [1, t, t^{2}, \dots, t^{n}] p

(2)

式中p=[p₀，p₁，···，p_n]^T∈R^（ⁿ^+1）为n阶多项式的参数向量。进一步定义SWC机器人的速度、加速度、加加速度，具体如式（3）所示：

\{\begin{matrix} v (t) = p^{(1)} (t) = [0,1, 2 t, 3 t^{2}, 4 t^{3}, \dots, n t^{n - 1}] p \\ a (t) = [0,0, 2,6 t, 12 t^{2}, \dots, n (n - 1) t^{n - 2}] p \\ j (t) = p^{(3)} (t) = [0,0, 0,6, 24 t, \dots, \frac{n!}{(n - 3)!} t^{n - 3}] p \end{matrix}

(3)

2 高效信息增强RRT^*运动规划算法设计

2.1 信息增强RRT^*算法

首先针对并联模态下SWC机器人差速圆周运动进行运动学建模，以支持后续理论和算法研究。图3展示了并联SWC机器人的旋转运动。定义世界坐标系{x_w，y_w}（固定在原点）和机器人坐标系{x_r，y_r}（附着在机器人上并随其移动）来描述环境中并联的SWC机器人。虚线框代表并联SWC机器人，机器人框架的原点被定义为两个SWC机器人模块重心的中点。

图3并联模态下SWC机器人差速圆周运动示意图

Fig.3Schematic diagram of differential circular Motion for SWC robots in parallel mode

并联SWC机器人的运动学模型可以表示为

\dot{q} = [\begin{matrix} \dot{x} \\ \dot{y} \\ \dot{θ} \end{matrix}] = [\begin{matrix} 1 / 2 c o s θ & 1 / 2 c o s θ \\ 1 / 2 s i n θ & 1 / 2 s i n θ \\ - 1 / (2 d) & 1 / (2 d) \end{matrix}] [\begin{matrix} v_{s, l} \\ v_{s, r} \end{matrix}]

(4)

式中：q=[x，y，θ]^T被定义为状态变量向量，是指并联SWC机器人在坐标系中的位置p=[x，y]^T与方向θ∈（-π，π]，v=[v_s_，_l，v_s_，_r]^T定义为实际控制输入，参考左右机器人模块的速度。此外，转弯半径表示为R_c，并联SWC机器人系统的线速度定义为v_c，角速度定义为ω=

\dot{θ} ， d

被定义为虚拟轨道。

SWC机器人高效信息增强RRT^*运动规划算法首先利用RRT-Connect算法进行初始路径搜索。为提高搜索效率，RRT-Connect算法采用双向搜索和贪婪扩展的策略。

RRT-Connect算法同时在起点χ_initial与终点χ_goal处初始化两棵随机树，其中前向树Γ_f=（V_f，E_f）从初始位置向目标点生长，反向树Γ_b=（V_b，E_b）则从目标点开始扩展，前向树的初始扩展过程遵循RRT算法的随机采样策略，不同处在于每当生成新节点X_new，f后，算法会将反向树的扩展方向调整为指向该新节点，并持续延伸直至遭遇障碍物。这种双向交替扩展策略使RRT-Connect算法在搜索效率上具有显著优势，通过两端同时逼近的方式，RRT-Connect算法可以高效的完成初始路径的搜索。基于该初始路径，信息增强RRT^*算法可以通过迭代不断优化至最优路径。然而，RRT-Connect算法返回的路径通常是曲折、冗长的，不具备最优性保证。在存在狭窄通道的环境中，其大步长的扩展方式不容易通过狭窄区域，两棵树可能会在通道的入口处反复试探。并且RRT-Connect算法在迭代找到路径后，不会利用已找到的路径信息进行更优的路径筛选。

基于上述RRT-Connect算法快速生成的初始路径，信息增强RRT^*算法在有效利用已生成路径的情况下，可以通过继续迭代保证路径的全局最优性，进一步提高搜索效率。为解决RRT算法和RRT-Connect算法路径非最优、浪费采样资源的问题，信息增强RRT^*算法优先在起点、目标点以及当前最优路径附近进行采样，而不是均匀探索整个空间，整个优化过程体现了采样区域随着搜索过程不断收缩的特点。具体见算法1。

信息增强RRT^*算法通过启发式采样取代随机采样，基于上述RRT-Connect算法，将路径长度c（δ_c）作为启发式信息传递给信息增强RRT^*算法，以此构建椭圆状态空间采样域，进而集中算力资源来搜索最优路径存在可能性更高的区域。另外，启发式椭圆状态空间采样区域的定义对于路径代价具有一定的依赖性，因此该算法具备自适应动态调整采样区域的能力，可以动态收缩搜索范围来更快地收敛到最优解。而后，将RRT-Connect生成的路径δ_c整合到信息增强RRT^*算法的初始随机树Γ_I中，使随机树快速扩展，以保证算法的高效性。这种自适应调整机制持续运行直至达到终止条件，确保算法能够收敛到全局最优路径解。

具体而言，信息增强RRT^*算法通过计算路径代价值不断优化路径，同时利用历史信息c_best引导搜索，从而实现从快速寻找可行解到高效收敛于最优解的跨越，其核心思路如算法1所示，Γ为RRT算法生成的随机树，其为点集V和边集E的二元组。信息增强RRT^*算法首先以RRT-Connect算法给定的初始可行路径δ_c长度作为初始的最优代价值c_best来构建启发式椭圆状态空间采样区域，通过在椭圆形采样区域内随机生成新节点，并采用路径扩展与重连接机制实现路径的渐进优化。在为新生节点X_new选择父节点时，并不是简单地连接最近的节点，而是在其邻域内找到一个从起点到X_new的总代价最小的节点。在迭代过程中，每当发现更优的路径策略时，系统会实时更新最优代价值c_best，并不断动态收缩椭圆采样区域的范围，使后续采样点更加集中于潜在最优解所在区域。

图4为启发式椭圆状态空间采样域，图中以矩形和圆形表示障碍物，黄色虚线椭圆表示不断收缩的椭圆采样区域，椭圆区域的两个焦点分别表示初始状态χ_initial与最终状态χ_goal；椭圆的长轴长度等同于当前迭代下搜索的最优路径的长度，记为c_best，其下界定义为初始状态到目标状态的欧氏距离c_min=‖χ_initial-χ_goal‖₂；并将椭圆区域的短轴长度定义为

\sqrt{c_{best}^{2} - c_{min}^{2}}

。

具体而言，首先从单位圆区域X_circle={（x_c，y_c）∈R²|x²+y²≤1}中进行均匀采样，得到采样点χ_circle，即χ_circle~U（X_circle）。而后，通过线性变换h∶

χ_{circle} \mapsto χ_{ellipse}

将单位圆映射到目标椭圆区域X_ellipse={（x_e，y_e）∈R²∣

4 x_{e}^{2} / c_{b e s t}^{2} + 4 y_{e}^{2} / (c_{b e s t}^{2} - c_{m i n}^{2}) ⩽ 1

}中。已知椭圆方程为

χ_{ellipse}^{T} U_{e} χ_{ellipse} = 1

(5)

图4椭圆状态空间采样域

Fig.4Elliptic state space sampling domain

其中，

U_{e} = [\begin{matrix} \frac{4}{c_{best}^{2}} & 0 \\ 0 & \frac{4}{c_{best}^{2} - c_{min}^{2}} \end{matrix}]

设L_e是线性变换的待求解矩阵，χ_ellipse=L_eχ_circle，将其代入椭圆方程后可得到

χ_{circle}^{T} L_{e}^{T} U_{e} L_{e} χ_{circle} = 1

(6)

根据单位圆方程可知

χ_{circle}^{T} χ_{circle}

=1。因此，

L_{e}^{T} U_{e} L_{e} = I

；通过Cholesky分解可得矩阵L_e的形式为

L_{e} = [\begin{matrix} \frac{c_{best}}{2} & 0 \\ 0 & \frac{\sqrt{c_{best}^{2} - c_{min}^{2}}}{2} \end{matrix}]

(7)

由于上述椭圆中心位于坐标轴的原点，长轴与x轴的夹角为0，需要经过平移变换和旋转后将其转换至焦点分别是χ_initial与χ_goal的椭圆区域内，变换关系如下：

χ_{sample} = R (θ_{e}) L_{e} χ_{circle} + o

(8)

其中，o=（χ_initial+χ_goal）/2，

R (θ_{e}) = [\begin{matrix} c o s θ_{e} & - s i n θ_{e} \\ s i n θ_{e} & c o s θ_{e} \end{matrix}]

其中θ_e为椭圆两个焦点连线与x轴的夹角。

2.2 基于最小化jerk的轨迹规划

上述的信息增强RRT^*算法具有路径最优特性，经过足够迭代后能够收敛到全局最优路径，但其生成的折线路径未考虑SWC机器人的运动学和动力学约束，不符合真实运动情况，无法直接用于SWC机器人的轨迹跟踪控制。为此，需要引入轨迹规划算法对路径进行时间参数化和平滑优化处理。具体实现时，算法对x、y两个坐标轴分别进行平滑处理，以x轴为例，将路径规划输出的最优路径点序列 x_i∈δ^*（i=0，1，2，···，m）作为输入，m是路径点的数量上限。采用5阶多项式函数来描述时间区间t∈[0，T]内的运动轨迹：

f (t) = \{\begin{matrix} p_{1, 0} + p_{1, 1} t + p_{1, 2} t^{2} + p_{1, 3} t^{3} + p_{1, 4} t^{4} + p_{1, 5} t^{5}, 0 ⩽ t < t_{1} \\ p_{2, 0} + p_{2, 1} t + p_{2, 2} t^{2} + p_{2, 3} t^{3} + p_{2, 4} t^{4} + p_{2, 5} t^{5}, t_{1} ⩽ t < t_{2} \\ ⋮ \\ p_{m, 0} + p_{m, 1} t + p_{m, 2} t^{2} + p_{m, 3} t^{3} + p_{m, 4} t^{4} + p_{m, 5} t^{5}, t_{m - 1} ⩽ t ⩽ t_{m} \end{matrix}

(9)

通过优化多项式系数确保轨迹满足连续性、平滑性和动力学约束等要求，使最终生成的轨迹既保持原路径的最优性，又具备良好的可执行性。定义轨迹规划的目标函数为

\begin{matrix} m i n J_{jerk} (t) = m i n \sum_{i = 1}^{m} \int_{t_{i - 1}}^{t_{i}} {(f^{(3)} (t))}^{2} d t = \\ m i n \sum_{i = 1}^{m} p_{i}^{T} Q_{i} p_{i} = m i n ρ^{T} Q ρ \end{matrix}

(10)

式中：

Q = d i a g \{Q_{1} ， Q_{2} ， \dots ， Q_{m}\} ， ρ = {[p_{1}^{T} ， p_{2}^{T} ， \dots ， p_{m}^{T}]}^{T} ， q = [0，0 ， 0，6 ， 24 t ， 60 t^{2}] ， Q_{i} = \int_{t_{i - 1}}^{t_{i}} q^{T} q d t

在轨迹优化过程中，需要建立严格的等式约束条件。首要任务是确保轨迹的初始状态和目标状态满足预设的运动学要求，其中位置约束必须严格对应路径δ^*的起始点和终止点坐标。根据SWC机器人的实际作业情况，通常将运动参数中初始和目标状态的速度、加速度均设为0，以保证SWC机器人在起止时刻处于静止状态。这些约束条件可表述为以下等式方程组：

\{\begin{array}{l} f^{(k)} (0) = x_{0}^{(k)} \\ f^{(k)} (T) = x_{m}^{(k)}, k = 0, 1, 2 \end{array}

(11)

通过该方程组能够有效保证生成的轨迹既符合路径规划结果，又满足实际运动控制的物理可行性要求，为后续轨迹跟踪控制奠定基础。

另外，需要设置一组等式约束条件使得每一段轨迹都经过给定的路径点：

f_{i} (t_{i - 1}) = x_{i - 1}, i = 2,3, \dots, m

(12)

最后，为了确保不同段轨迹的连接处尽可能满足平滑性条件，要求每一段轨迹连接处的位置、速度、加速度都是一致的。基于此，定义如下约束条件：

f_{i}^{(k)} (t_{i}) = f_{i + 1}^{(k)} (t_{i}), k = 0,1, 2; i = 1,2, \dots, m - 1

(13)

综上所述，轨迹规划问题转换为最小化jerk目标函数问题，其Hessian矩阵由多项式导数的积分构成，为半正定矩阵，因此该问题实际为凸二次规划问题，即只包含线性等式约束，可以通过信赖域反射法等方法进行求解。

3 仿真结果分析

3.1 高效信息增强RRT^*算法路径规划

为验证高效信息增强RRT^*算法路径规划的有效性，首先进行典型的障碍物场景的路径规划仿真，SWC机器人的物理尺寸为305 mm×65 mm×65 mm，具体而言，SWC机器人的体积低于128.86 cm³。本文在实际应用时会对障碍物进行膨胀处理，其中的障碍物主要由矩形和圆形构成，并随机分布在地图上，地图尺寸为300 cm×200 cm。主动对接的SWC机器人的初始状态位置和目标状态位置分别设置为χ_initial=[50，90]^T，χ_goal=[250，90]^T。

相应的仿真结果如图5所示，其中黄色和绿色圆点分别代表SWC机器人的初始状态位置和目标状态位置，蓝色圆点与紫色细实线表示算法迭代过程中生成的扩展树，而红色粗实线则表示最终的最优路径，橙色虚线椭圆表示启发式椭圆状态空间采样区域。

图5高效信息增强RRT^*算法路径规划结果

Fig.5Path planning results of the efficient information-enhanced RRT^* algorithm

图5（a）为算法在第0次迭代时的结果，RRT-Connect算法快速生成初始扩展树，路径呈现从初始状态到目标状态的路径，总体较为曲折。而后通过后续信息增强RRT^*算法进一步迭代优化，图5（b）展示了第1 250次迭代的结果，算法采样更加趋向于椭圆区域内，所生成的相较于第0次迭代时的路径更平滑且长度更短。图5（c）为第2 500次迭代的结果，椭圆采样域随着迭代进一步缩小，生成的采样点几乎全部集中于最优路径附近，说明在足够多次数的迭代后，改进后算法成功避免了无效的全局采样，并将计算资源有效地集中获得最优路径。

综上可以认为，本文所提出的高效信息增强RRT^*算法能够规划出连续的最优路径，并可保证完全避障的安全性。

3.2 路径规划仿真对比

为了验证改进后方法在狭窄环境中路径规划搜索效率方面的有效性，本文将所提方法与传统的IRRT^*和RRT^*算法在同一个地图场景下进行仿真对比。对比设计的地图包括一段从初始状态到最终状态必须经过的狭小的可行区域，并且最优路径是唯一的。设置的对比环境地图愈加考验算法的精确性和鲁棒性。所有对比算法仿真设置运行20 s，并记录每50次迭代时所生成的最优路径长度c（δ）_i（i=1，2，3）。

仿真结果如图6所示，其中SWC机器人初始状态和目标状态分别用紫色圆点和橙色圆点表示，红色实线代表改进后方法的仿真结果。图6（a）为3种算法最优路径长度随时间的变化情况，可以看出，所提方法的优化效果很显著，收敛速度最快。值得指出的是，初始即曲线在t=0 s时刻存在一定的缺失值，这是因为算法在运行初期未找到从初始状态到目标状态的有效路径，且此时算法需要进行全局的路径搜索，因此早期计算时间较长，导致计算资源的浪费。对比分析表明：在全局搜索时，IRRT^*算法耗时1.43 s，RRT^*算法耗时3.92 s；而本文所提方法能够在0.2 s内完成该过程，大大提高搜索效率；图6（b）展示了3种路径规划的结果，在引入启发式椭圆区域采样后，所提方法路径最短，IRRT^*算法仅能够收敛至次优路径。与之相比，RRT^*算法的路径不是最优的，还需要进行更多的迭代次数和时间才能够达到最优路径效果。

图6高效信息增强RRT^*算法与传统算法路径规划仿真结果对比

Fig.6Comparison results of path planning between the efficient information-enhanced RRT^* algorithm and traditional algorithms

3.3 基于最小化jerk的轨迹优化仿真

本组仿真结果将用于验证基于最小化jerk方法的轨迹优化算法的有效性。图中起点为黄色圆点，终点为绿色圆点，首先提取所提方法寻找出的随机障碍物地图中从起始状态到目标状态的最优路径节点，将这些节点输入至轨迹规划算法模块。算法分配总时间T设置为26.43 s，经轨迹优化后得到的结果如图7所示。周围黑色矩形实线表示不可碰撞的墙壁障碍物，路径规划的结果是红色实线，轨迹优化后规划的结果是紫色点划线。对比优化前后的轨迹可以看出原始路径规划出的轨迹是由折线组成的，在路径转折点处存在非常明显的尖锐角度。在进行最小化jerk方法优化改进后，路径轨迹更加平滑，转折点的尖锐性得到有效地改善，生成的路径更符合SWC机器人的运动学要求。

图7最小化jerk轨迹规划结果

Fig.7Results of minimizing jerk trajectory planning

图8为经规划后得到的轨迹在引入时间参数后的位置、速度、加速度结果，可以看到轨迹连续且平滑，幅值大小均在合理范围内。因此，说明SWC机器人可以通过结合运动控制方法来跟踪高效信息增强RRT^*算法所规划的运动轨迹，从而完成任务，提升系统整体的任务效率和可实现性。

图8轨迹规划运动学参数结果

Fig.8Kinematic parameter results of trajectory planning

4 结语

本文针对SWC机器人在并联模态下的自主对接运动规划问题，利用RRT-Connect算法搜索得到从初始状态到目标状态的局部最优路径，并将该路径长度直接传递给信息增强RRT^*算法。通过构建启发式椭圆状态空间采样域，避免算法在初始搜索阶段进行全局采样，从而显著提升路径搜索效率。此外，本文充分利用RRT-Connect算法生成的路径信息，将其融入信息增强RRT^*算法的初始随机树，使得随机树可以直接基于路径建立，进一步加快随机树的生长速度。在此基础上，基于最小化jerk算法实现轨迹的平滑处理，为SWC机器人规划出满足运动学平滑特性的最优运动轨迹。通过在不同障碍物环境下的仿真，验证所提出的方法在狭窄环境等条件下也可以有效生成最优路径，能够显著减少路径规划算法全局采样搜索的时长，降低计算资源的消耗，规划出来的路径长度更短。同时，基于最小化jerk方法成功地对所生成的路径进行平滑处理，使路径更符合SWC机器人的运动学要求。为进一步实现SWC机器人的智能化应用，后续的研究中，可考虑设计一种针对多组并联模态下的SWC机器人的运动规划算法，使机器人既可避开固定的障碍物和危险区域，又可以协调各组机器人的运动轨迹的冲突，并实现多目标最优运动规划，从而进一步增强SWC机器人的应用潜力。

图1串/并联连接实物图

Fig.1Design and physical diagrams of series/parallel connection mechanisms

下载: 全尺寸图片

图2SWC机器人自主对接任务下的运动规划

Fig.2Motion planning for the autonomous docking task of SWC robots

下载: 全尺寸图片

图3并联模态下SWC机器人差速圆周运动示意图

Fig.3Schematic diagram of differential circular Motion for SWC robots in parallel mode

下载: 全尺寸图片

图4椭圆状态空间采样域

Fig.4Elliptic state space sampling domain

下载: 全尺寸图片

图5高效信息增强RRT^*算法路径规划结果

Fig.5Path planning results of the efficient information-enhanced RRT^* algorithm

下载: 全尺寸图片

图6高效信息增强RRT^*算法与传统算法路径规划仿真结果对比

Fig.6Comparison results of path planning between the efficient information-enhanced RRT^* algorithm and traditional algorithms

下载: 全尺寸图片

图7最小化jerk轨迹规划结果

Fig.7Results of minimizing jerk trajectory planning

下载: 全尺寸图片

图8轨迹规划运动学参数结果

Fig.8Kinematic parameter results of trajectory planning

下载: 全尺寸图片

图1串/并联连接实物图

Fig.1Design and physical diagrams of series/parallel connection mechanisms

图2SWC机器人自主对接任务下的运动规划

Fig.2Motion planning for the autonomous docking task of SWC robots

图3并联模态下SWC机器人差速圆周运动示意图

Fig.3Schematic diagram of differential circular Motion for SWC robots in parallel mode

图4椭圆状态空间采样域

Fig.4Elliptic state space sampling domain

图5高效信息增强RRT^*算法路径规划结果

Fig.5Path planning results of the efficient information-enhanced RRT^* algorithm

图6高效信息增强RRT^*算法与传统算法路径规划仿真结果对比

Fig.6Comparison results of path planning between the efficient information-enhanced RRT^* algorithm and traditional algorithms

图7最小化jerk轨迹规划结果

Fig.7Results of minimizing jerk trajectory planning

图8轨迹规划运动学参数结果

Fig.8Kinematic parameter results of trajectory planning

图1串/并联连接实物图

Fig.1Design and physical diagrams of series/parallel connection mechanisms

图2SWC机器人自主对接任务下的运动规划

Fig.2Motion planning for the autonomous docking task of SWC robots

图3并联模态下SWC机器人差速圆周运动示意图

Fig.3Schematic diagram of differential circular Motion for SWC robots in parallel mode

图4椭圆状态空间采样域

Fig.4Elliptic state space sampling domain

图5高效信息增强RRT^*算法路径规划结果

Fig.5Path planning results of the efficient information-enhanced RRT^* algorithm

图6高效信息增强RRT^*算法与传统算法路径规划仿真结果对比

Fig.6Comparison results of path planning between the efficient information-enhanced RRT^* algorithm and traditional algorithms

图7最小化jerk轨迹规划结果

Fig.7Results of minimizing jerk trajectory planning

图8轨迹规划运动学参数结果

Fig.8Kinematic parameter results of trajectory planning

MAHMUD M S A, ABIDIN M S Z, MOHAMED Z,et al. Multi-objective path planner for an agricultural mobile robot in a virtual greenhouse environment[J]. Computers and Electronics in Agriculture,2019,157:488. DOI:10.1016/j.compag.2019.01.016

ULRICH J, STEFANEC M, REKABI-BANA F,et al. Autonomous tracking of honey bee behaviors over long-term periods with cooperating robots[J]. Science Robotics,2024,9(95):eadn6848. DOI:10.1126/scirobotics.adn6848

ZHANG Z, WU L, ZHANG W,et al. Energy-efficient path planning for a single-load automated guided vehicle in a manufacturing workshop[J]. Computers & Industrial Engineering,2021,158:107397. DOI:10.1016/j.cie.2021.107397

AJEIL F H, IBRAHEEM I K, SAHIB M A,et al. Multi-objective path planning of an autonomous mobile robot using hybrid PSO-MFB optimization algorithm[J]. Applied Soft Computing,2020,89:106076. DOI:10.1016/j.asoc.2020.106076

SZCZEPANSKI R. Safe artificial potential field-novel local path planning algorithm maintaining safe distance from obstacles[J]. IEEE Robotics and Automation Letters,2023,8(8):4823. DOI:10.1109/LRA.2023.3290819

LI H, LIU W, YANG C,et al. An optimization-based path planning approach for autonomous vehicles using the DynEFWA-artificial potential field[J]. IEEE Transactions on Intelligent Vehicles,2021,7(2):263. DOI:10.1109/TIV.2021.3123341

LI Y, YI H, ZHUO F,et al. Analysis and stabilization of APF systems considering dynamic of nonlinear loads[J]. IEEE Transactions on Power Electronics,2023,39(1):409. DOI:10.1109/TPEL.2023.3324650

SANG H, YOU Y, SUN X,et al. The hybrid path planning algorithm based on improved A^* and artificial potential field for unmanned surface vehicle formations[J]. Ocean Engineering,2021,223:108709. DOI:10.1016/j.oceaneng.2021.108709

QI W, ZHANG N, ZONG G,et al. Event-triggered SMC for networked Markov jumping systems with channel fading and applications: Genetic algorithm[J]. IEEE Transactions on Cybernetics,2023,53(10):6503. DOI:10.1109/TCYB.2023.3253701

LUO Q, WANG H, ZHENG Y,et al. Research on path planning of mobile robot based on improved ant colony algorithm[J]. Neural Computing and Applications,2020,32:1555. DOI:10.1007/s00521-019-04172-2

MIAO C, CHEN G, YAN C,et al. Path planning optimization of indoor mobile robot based on adaptive ant colony algorithm[J]. Computers & Industrial Engineering,2021,156:107230. DOI:10.1016/j.cie.2021.107230

LIN Z, WU K, SHEN R,et al. An efficient and accurate A-star algorithm for autonomous vehicle path planning[J]. IEEE Transactions on Vehicular Technology,2023,73(6):9003. DOI:10.1109/TVT.2023.3348140

DENG X, LI R, ZHAO L,et al. Multi-obstacle path planning and optimization for mobile robot[J]. Expert Systems with Applications,2021,183:115445. DOI:10.1016/j.eswa.2021.115445

WANG J, LI B, MENG M Q H. Kinematic constrained bi-directional RRT with efficient branch pruning for robot path planning[J]. Expert Systems with Applications,2021,170:114541. DOI:10.1016/j.eswa.2020.114541

QI J, YANG H, SUN H. MOD-RRT^*: A sampling-based algorithm for robot path planning in dynamic environment[J]. IEEE Transactions on Industrial Electronics,2020,68(8):7244. DOI:10.1109/TIE.2020.2998740

WANG J, MENG M Q H, KHATIB O. EB-RRT: Optimal motion planning for mobile robots[J]. IEEE Transactions on Automation Science and Engineering,2020,17(4):2063. DOI:10.1109/TASE.2020.2987397

SALEHIZADEH M, DILLER E D. Path planning and tracking for an underactuated two-microrobot system[J]. IEEE Robotics and Automation Letters,2021,6(2):2674. DOI:10.1109/LRA.2021.3062343

SHI Y, LI Q, BU S,et al. Research on intelligent vehicle path planning based on rapidly-exploring random tree[J]. Mathematical Problems in Engineering,2020,2020(1):5910503. DOI:10.1155/2020/5910503

WANG J, CHI W, LI C,et al. Neural RRT^*: Learning-based optimal path planning[J]. IEEE Transactions on Automation Science and Engineering,2020,17(4):1748. DOI:10.1109/TASE.2020.2976560

XU C, ZHU H, ZHU H,et al. Improved RRT^* algorithm for automatic charging robot obstacle avoidance path planning in complex environments[J]. CMES-Computer Modeling in Engineering & Sciences,2023,137(3):2567. DOI:10.32604/cmes.2023.029152

GU C, FENG A, WANG G,et al. Robot path planning of improved adaptive ant colony system algorithm based on dijkstra[J]. Journal of robotics,2022,2022(1):9229155. DOI:10.1155/2022/9229155

KIANI F, SEYYEDABBASI A, ALIYEV R,et al. Adapted-RRT: Novel hybrid method to solve threedimensional path planning problem using sampling and metaheuristic-based algorithms[J]. Neural Computing and Applications,2021,33(22):15569. DOI:10.1007/s00521-021-06179-0

QADIR Z, ULLAH F, MUNAWAR H S,et al. Addressing disasters in smart cities through UAVs path planning and 5G communications: A systematic review[J]. Computer Communications,2021,168:114. DOI:10.1016/j.comcom.2021.01.003

CHAI Q, WANG Y. RJ-RRT: Improved RRT for path planning in narrow passages[J]. Applied Sciences,2022,12(23):12033. DOI:10.3390/app122312033

DONG Y, WANG K, YANG Z,et al. Path planning with modified RRT^* algorithm for lung biopsy[C]//Proceedings of the 8th World Congress on Mechanical, Chemical,and Material Engineering(MCM'22). Prague, Czech Republic: International ASET Inc,2022:1. DOI:10.11159/icmie22.124

TU H, DENG Y, LI Q,et al. Improved RRT global path planning algorithm based on bridge test[J]. Robotics and Autonomous Systems,2024,171:104570. DOI:10.1016/j.robot.2023.104570

WU Z, MENG Z, ZHAO W,et al. Fast-RRT: A RRT-based optimal path finding method[J]. Applied Sciences,2021,11(24):11777. DOI:10.3390/app112411777

WANG J, CHI W, LI C,et al. Efficient robot motion planning using bidirectional-unidirectional RRT extend function[J]. IEEE Transactions on Automation Science and Engineering,2021,19(3):1859. DOI:10.1109/TASE.2021.3130372

CHEN J, YU J. An improved path planning algorithm for UAV based on RRT[C]//2021 4th International Conference on Advanced Electronic Materials, Computers and Software Engineering(AEMCSE). Piscataway, NJ: IEEE,2021:895. DOI:10.1109/AEMCSE51986.2021.00182

出版声明

期刊订阅

1 SWC机器人运动规划

2 高效信息增强RRT^*运动规划算法设计

3 仿真结果分析

4 结语

出版声明

期刊订阅

1 SWC机器人运动规划

2 高效信息增强RRT*运动规划算法设计

3 仿真结果分析

4 结语

2 高效信息增强RRT^*运动规划算法设计