1 引言（Introduction）

随着空间技术的快速发展，通过多次发射并在轨道上装配组件来建造大型空间设施（如空间站、大型通讯卫星等）的需求日益迫切^[1-2]。其中运动规划技术作为利用空间机械臂进行在轨装配的关键技术在近十年受到国内外学者的广泛研究^[3-4]。

运动规划算法通常可以分为局部算法和全局算法。局部规划算法，如人工势场法^[5]，利用传感器实时获取机器人周边环境信息，动态调整运动轨迹，可灵活应对突发情况。但是此类算法不是完备的，在拥挤杂乱的环境下容易陷入局部极值。相对应地，全局规划算法，尤其是基于采样的方法，利用事先获取的完整环境信息，可以在复杂环境下高效求解高自由度机器人的运动规划问题^[6]。其中，RRT（快速扩展随机树）及其改进算法^[7]通过随机采样逐步构造一棵随机树，寻找连接初始和目标构形的运动轨迹。此类算法可以较为方便地处理机器人运动学甚至动力学约束，已得到了广泛应用^[8]。

然而，基于采样的规划算法在处理狭窄通道问题时性能急剧下降，但此类问题在装配中却很常见。窄通道是指占整个构形空间比重很小但包含部分解的无碰撞区域，因此难以通过常规方式对其进行采样以及拓展。目前常见的解决方法主要包括改进采样策略和改进拓展方式两种。其中，文[9] 利用高斯采样增加示教区域的样本点密度，从而在一定程度上增加窄通道中的样本点。文[10] 则利用桥测试方法更为高效地对窄通道直接进行采样。但是这两种算法都需要额外的碰撞检测，耗费大量时间，且在高维环境下效率降低。而ADD-RRT（adaptive dynamic domain RRT）^[11]根据节点的可拓展能力，即动态域，对随机采样点进行筛选，只对有潜力的样本点进行拓展。另一方面，文[12] 利用回撤技术，将发生碰撞的构形沿着障碍物边界回撤到无碰撞区域，但是需要进行耗时的穿透深度计算。文[13] 提出RRV算法，通过额外的随机采样和主成分分析（PCA），随机树可以像藤蔓一样攀附着障碍物生长，以快速通过复杂区域。此外，多棵树同时生长也能在一定程度上提高复杂环境下的规划效率^[14-15]。

运动规划一般都是在构形空间中进行，但是机械臂在执行装配操作时任务指令通常在工作空间中下达，且通常工作空间中同一个目标末端状态可能对应多种不同的机械臂构形。如果采取“先求逆运动学，再规划运动”的框架，则可能导致初始和目标构形不在同一连通域内^[8]。为此，文[16] 直接在工作空间中采样，同时考虑了任务对末端施加的约束，再利用雅可比矩阵的伪逆进行拓展。考虑到在整个工作空间中采样会产生大量无用节点，文[17] 提出EET（exploring/exploiting tree）算法，以牺牲算法完备性为代价，平衡算法对工作空间信息的利用和探索行为，减小工作空间采样范围，提高规划效率和路径质量。文[18] 结合EET算法进一步考虑了机械臂运动学约束和末端任务约束，同时并行搜索不同拓扑特性的工作空间路径以提升路径最优性。然而，单纯基于工作空间采样的规划算法是不完备的，并且算法所利用的工作空间信息通常只包含位置信息而缺乏姿态信息，因此在杂乱的环境下算法可能会因陷入局部极值而依然难以求解。文[19] 提出根据算法进程动态切换采样、拓展策略，能够在保证算法完备性的同时提高算法效率和路径质量。

在之前的工作^[10]中，提出了用于复杂环境尤其是窄通道环境的基于构形空间采样的运动规划算法，利用桥测试以及PCA等技术分析局部空间类型，提高了算法在复杂环境下的规划效率和成功率。本文拓展了此算法，用于解决工作空间中的装配运动规划问题。新算法结合混合采样方法，既充分利用工作空间信息进一步提高算法效率，又保证了算法的完备性。此外，在局部拓展中充分考虑了机械臂的运动学与动力学特性，增强了所规划的运动轨迹的可执行性，也提高了整体规划效率。

2 问题描述（Problem description）

本节将对3维工作空间中串联机械臂的运动规划问题与其所涉及的符号作相关定义。

将机械臂所在的工作空间表示为$ {\mathcal{W}}\subset \mathbb{R}^{3} $，工作空间中的障碍物集合表示为$ {\mathcal{O}}\subset {\mathcal{W}} $。$ \mathit{\boldsymbol{q}}=[q_{1}, $ $ q_{2}, \cdots, q_{n} ]^{\rm T} $为$ n $关节机械臂的关节构形，将包含所有可行构形的机械臂构形空间表示为$ {\mathcal{C}} $。设机械臂在构形$ \mathit{\boldsymbol{q}} $下的几何形状可以用一系列刚体$ \mathcal{M}(\mathit{\boldsymbol{q}})\subset {\mathcal{W}} $表示，那么所有会导致机械臂与工作空间中的障碍物发生碰撞的关节构形的集合可表示为$ \mathcal{C}_{\text{obs}} =\{\mathit{\boldsymbol{q}}\in \mathcal{C}:\mathcal{M}(\mathit{\boldsymbol{q}})\cap {\mathcal{O}}\ne \emptyset \} $，相应的无碰撞构形可表示为$ \mathcal{C}_{\text{free}} =\{{\mathit{\boldsymbol{q}}\in \mathcal{C}\backslash \mathcal{C}_{\text{obs}}} \} $。机械臂末端执行器（待装配部件）的位置－姿态矢量表示为$ \mathit{\boldsymbol{x}}_{\rm e} =[{\mathit{\boldsymbol{r}}_{\rm e}^{\rm T}, \mathit{\boldsymbol{\psi}}_{\rm e}^{\rm T}} ]^{\rm T} $。对于给定的关节构形$ \mathit{\boldsymbol{q}} $，利用正运动学即可求得其对应的末端位姿$ \mathit{\boldsymbol{x}}_{\rm e} (\mathit{\boldsymbol{q}}) $。

本文主要研究面向在轨装配的点到点运动规划问题。对于给定的机械臂几何与运动学模型、工作空间障碍物$ {\mathcal{O}} $、机械臂初始构形$ \mathit{\boldsymbol{q}}_{\text{init}} $与相应的初始末端位姿$ \mathit{\boldsymbol{x}}_{\rm e, \text{init}} $，以及末端部件期望装配状态$ \mathit{\boldsymbol{x}}_{\rm e, \text{goal}} $，寻找一条无碰撞运动轨迹$ \mathcal{Q}=\{\mathit{\boldsymbol{q}}(t_{k})\}_{k=0}^{K} \subset \mathcal{C}_{\text{free}} $，使得$ \mathit{\boldsymbol{q}}(t_{0})=\mathit{\boldsymbol{q}}_{\text{init}} $，$ \mathit{\boldsymbol{x}}_{\rm e} (\mathit{\boldsymbol{q}}(t_{K}))=\mathit{\boldsymbol{x}}_{\rm e, \text{goal}} $，且始终满足机械臂关节约束。

3 HS-RRV算法（HS-RRV algorithm）

为了进一步提高空间机械臂在拥挤、狭小环境下进行装配运动规划的效率，结合EET算法^[17]和ADD-RRV算法^[10]的优势，提出基于混合采样的RRV算法（HS-RRV），其流程框架如算法1所示。

与经典RRT算法的基本流程类似，本算法通过随机采样（Rand_Sample函数）寻找最近的节点（Nearest_Node函数），利用局部规划器连接相邻节点（Extend函数），从而逐渐在构形空间构造一棵随机树（Tree_Grow函数）。但是，不同于经典RRT算法，本算法不仅在构形空间中进行搜索，还利用了工作空间中的信息，并且根据随机树的拓展进程对两者的偏向进行动态调节。因此，本算法中随机树的每一个节点都包含2种子节点，即机械臂的关节构形子节点$ \mathit{\boldsymbol{q}} $和对应的末端位姿子节点$ \mathit{\boldsymbol{x}}_{\rm e} $。

算法1 HS-RRV

1: $ {\mathcal{T}}\text{.Init}(\mathit{\boldsymbol{q}}_{\text{init}}, \mathit{\boldsymbol{x}}_{\rm e, \text{init}}) $ 2: $ \mathcal{S}\leftarrow \text{Wavefront}(\mathit{\boldsymbol{r}}_{\rm e, \text{init}} , \mathit{\boldsymbol{r}}_{\rm e, \text{goal}}) $ 3: $ s_{\text{current}} \leftarrow \mathcal{S}.\text{First_Sphere}() $ 4: $ (\sigma , p_{\rm HS })\leftarrow \text{Parameter_Init}() $ 5: repeat 6:    $ (\mathit{\boldsymbol{n}}_{\text{rand}}, l)\leftarrow \text{Rand_Sample}( s_{\text{current}}, \sigma , \mathit{\boldsymbol{x}}_{\rm e, \text{goal}}, p_{\rm HS }) $ 7:    $ (\mathit{\boldsymbol{q}}_{\text{near}}, \mathit{\boldsymbol{x}}_{\rm e, \text{near}})\leftarrow \text{Nearest_Node}(\mathcal{T}, \mathit{\boldsymbol{n}}_{\text{rand}}, l) $ 8:    $ (\mathit{\boldsymbol{q}}_{\text{new}}, \mathit{\boldsymbol{x}}_{\rm e, \text{new}}, \mathit{\boldsymbol{q}}_{\text{obs}})\leftarrow \text{Extend}(\mathit{\boldsymbol{q}}_{\text{near}}, \mathit{\boldsymbol{x}}_{\rm e, \text{near}}, \mathit{\boldsymbol{n}}_{\text{rand}}, l) $ 9:    $ {\mathcal{T}}\leftarrow \text{Tree_Grow}(\mathit{\boldsymbol{q}}_{\text{new}}, \mathit{\boldsymbol{x}}_{\rm e, \text{new}}) $ 10:    $ ( s_{\text{current}}, \sigma , p_{\rm HS })\leftarrow \text{Parameter_Update}( s_{\text{current}}, \sigma , p_{\rm HS }, l, \mathit{\boldsymbol{q}}_{\text{obs}}) $ 11:    $ {\mathcal{T}}\leftarrow \text{Extend_further}(\mathit{\boldsymbol{q}}_{\text{near}}, \mathit{\boldsymbol{x}}_{\rm e, \text{near}}, \mathit{\boldsymbol{q}}_{\text{obs}}, {\mathcal{T}}) $ 12: until $ k_{1} \| \mathit{\boldsymbol{r}}_{\rm e, \text{new}} -\mathit{\boldsymbol{r}}_{\rm e, \text{goal}} \|+ k_{2} \| \mathit{\boldsymbol{\psi}}_{\rm e, \text{new}} -\mathit{\boldsymbol{\psi}}_{\rm e, \text{goal}} \|\leqslant \varepsilon $ 13: return $ {\mathcal{T}} $

3.1 获取工作空间特征

为了有效利用工作空间信息引导随机树的拓展，在算法初始化后，首先利用基于圆球的Wavefront算法^[17]捕获工作空间的连通性特征。如图 1所示，圆球的直径表明了无碰撞工作空间区域的几何特征，一系列相互交叠的圆球构成了连接初始和目标末端位置的无碰撞通道，可用于引导机械臂末端在工作空间中的拓展。Wavefront算法的工作流程如下：

(1) 将初始末端位置作为圆心，将其与最近障碍物之间的距离作为半径，构造第1个圆球；

(2) 在第1个圆球的表面随机均匀采样$ k $个点，将距离目标位置最近的样本点作为新圆心，用与上一步类似的方式确定半径，构造新圆球；

(3) 重复上述过程，直到目标点被圆球覆盖。

3.2 平衡工作/构形空间拓展

在算法1中，随机采样同时在工作空间和构形空间中进行，利用采样权重$ p_{\rm HS} $确定在2种空间中采样的概率。随后，根据样本点的类型，利用合适的度量，选择相邻的节点进行拓展。接着，为不同采样空间下的拓展建立统一的局部规划框架。此外，当初次拓展失败后，利用全局规划器的在线规划结果，对局部构形空间进行分析，确定再次拓展的方向。

3.2.1 混合采样与相邻节点选取

混合采样过程如算法2所示，算法以概率$ p_{\rm HS } $在工作空间中采样，以概率$ 1-p_{\rm HS } $在构形空间中采样。其中，概率$ p_{\rm HS } $根据算法的进程自主调节，以动态平衡两种采样方式（详见3.2.2节），$ l $用于标记区分这两种采样类型。

算法2 $\text{Rand_Sample}( s_{\text{current}}, \sigma , \mathit{\boldsymbol{x}}_{\rm e, \text{goal}}, p_{\rm HS })$

1: if $\text{Random}(0, 1) < p_{\rm HS }$ then 2:      $\mathit{\boldsymbol{n}}_{\text{rand}} \leftarrow \text{Sample_WS}( s_{\text{current}}, \sigma , \mathit{\boldsymbol{x}}_{\rm e, \text{goal}}, p_{\rm HS }$) 3:      $ l\leftarrow$ 'W' 4: else 5:      if $\text{Random}(0, 1) > p_{\text{cs}}$ then 6:           $\mathit{\boldsymbol{x}}_{\text{rand}}^{\rm t} \leftarrow \text{Sample_WS}( s_{\text{current}}, \sigma , \mathit{\boldsymbol{x}}_{\rm e, \text{goal}}, p_{\rm HS }$) 7:           $(\mathit{\boldsymbol{q}}_{\text{near}}^{\rm t}, \mathit{\boldsymbol{x}}_{\text{near}}^{\rm t})\leftarrow \text{Nearest_Node}(\mathcal{T}, \mathit{\boldsymbol{x}}_{\text{rand}}^{\rm t}, $'W'$)$ 8:           $\mathit{\boldsymbol{n}}_{\text{rand}} \leftarrow \text{Sample_CS_Local}(\mathit{\boldsymbol{q}}_{\text{near}}^{\rm t})$ 9:      else 10:           $\mathit{\boldsymbol{n}}_{\text{rand}} \leftarrow \text{Sample_CS_Global}()$ 11:      end if 12:      $ l\leftarrow$ 'C' 13: end if 14: return $(\mathit{\boldsymbol{n}}_{\text{rand}}, l)$

工作空间中的采样利用Sample_WS函数在Wavefront算法生成的末端无碰撞圆球中进行（算法2的第2行）。具体而言，以当前圆球的中心位置为采样均值，以$ \sigma R_{\text{current}} $为标准差，按正态分布对末端位置进行采样，同时对末端姿态角进行随机均匀采样。其中，$ R_{\text{current}} $为当前圆球的半径，使用正态分布可以使机械臂末端尽可能在圆球中心区域附近运动，降低与障碍物发生碰撞的概率。此外，为了加快算法的收敛速度，可以概率$ 1-p_{\rm g} $利用上述方式在当前圆球中采样，以概率$ p_{\rm g} $直接选取目标位姿作为待拓展方向，通常$ p_{\rm g} $取0.1~0.3。此时，$ \mathit{\boldsymbol{n}}_{\text{rand}} $为工作空间中的随机样本点（算法2的第2行）。

上述工作空间采样中利用工作空间连通性信息引导随机树在构形空间探索拓展，排除了与当前规划问题无关的部分构形空间，避免随机树在整个构形空间中盲目地拓展，缩小了采样、搜索范围，提高了规划效率。但是，搜索区域的减小将导致算法不完备。当工作空间信息不准确（如被排除的构形空间也包含部分解）或不能提供有效拓展方向（如机械臂因几何或者运动学等约束而陷入局部极值）时，单一的工作空间采样则因无法搜索整个构形空间而导致规划失败。

因此，当工作空间中的样本点难以被有效拓展时，采样逐渐偏向整个构形空间。为了跳出局部极值，首先以前文描述的方式利用Sample_WS函数获取工作空间中的随机样本点$ \mathit{\boldsymbol{x}}_{\text{rand}}^{\rm t} $（算法2的第6行）。此时，样本点$ \mathit{\boldsymbol{x}}_{\text{rand}}^{\rm t} $并不直接用于拓展，而是利用Nearest_Node函数在随机树中寻找此样本点的邻近末端位姿节点$ \mathit{\boldsymbol{x}}_{\text{near}}^{\rm t} $，并得到对应的构形节点$ \mathit{\boldsymbol{q}}_{\text{near}}^{\rm t} $。然后，在以$ \mathit{\boldsymbol{q}}_{\text{near}}^{\rm t} $为圆心、$ R_{\text{cs}} $为半径的局部构形空间内随机采样（Sample_CS_Local函数），用于后续拓展。本文并没有深入研究参数$ R_{\text{cs}} $的最优取值，但多次仿真试验表明$ R_{\text{cs}} $取$ \pi $/3时算法的性能良好。此外，算法有一定概率$ p_{\text{cs}} $利用Sample_CS_Global函数在整个构形空间中随机均匀采样，使得所有构形均有可能被获取并被拓展，保证了算法的完备性。文中，2种构形空间采样方式具有相同的权重，即$ p_{\text{cs}} =0.5 $。此时，$ \mathit{\boldsymbol{n}}_{\text{rand}} $为构形空间中的随机样本点（算法2的第8、10行）。

在获取随机样本点后，需要利用Nearest_Node函数分别为不同类型（关节构形或末端位姿）的样本点依据各自的距离度量搜索最近节点进行拓展（算法1的第7行）。尽管随机树中的每个节点均包含关节构形和末端位姿2种子节点，但搜索仅利用与随机样本点类型相同的子节点。通常，合适的度量需要反映连接2个样本点的代价。考虑到工作空间度量难以精确获取，因此，对于工作空间中的任意2个样本点$ \mathit{\boldsymbol{x}}_{i} =[\mathit{\boldsymbol{r}}_{i}^{\rm T}, \mathit{\boldsymbol{\psi}}_{i}^{\rm T} ]^{\rm T} $和$ \mathit{\boldsymbol{x}}_{j} =[\mathit{\boldsymbol{r}}_{j}^{\rm T}, \mathit{\boldsymbol{\psi}}_{j}^{\rm T}]^{\rm T} $，选取如下伪度量：

$ \begin{align} {\rm Dist}(\mathit{\boldsymbol{x}}_{i}, \mathit{\boldsymbol{x}}_{j})=k_{1} \| \mathit{\boldsymbol{r}}_{i} -\mathit{\boldsymbol{r}}_{j} \|+k_{2} \| \mathit{\boldsymbol{\psi}}_{i} -\mathit{\boldsymbol{\psi}}_{j} \| \end{align} $

(1)

其中$ \| \cdot \| $表示欧氏距离，$ k_{1}, k_{2} >0 $表示2种分量的权重，取$ k_{1} =0.77 $、$ k_{2} =0.23 $。对于构形空间中的样本点，直接使用欧氏距离来度量。

3.2.2 考虑关节约束的局部规划器设计

局部规划器的工作流程如算法3所示。

对于工作空间中的采样，在利用Nearest_Node函数获取最近的末端位姿节点$ \mathit{\boldsymbol{x}}_{\rm e, \text{near}} $后，可确定末端拓展方向$ \Delta {\mathit{\boldsymbol{x}}}=\mathit{\boldsymbol{n}}_{\text{rand}} -\mathit{\boldsymbol{x}}_{\rm e, \text{near}} $。考虑机械臂的微分运动学$ \mathit{\boldsymbol{J}}(\mathit{\boldsymbol{q}})\mathit{\boldsymbol{\dot{q}}}=\mathit{\boldsymbol{\dot{x}}}_{\rm e} $，那么局部规划问题可以表示成一个最小二乘优化问题：

$ \begin{align} \min \mathit{\boldsymbol{\dot{q}}} \frac{1}{2}\| \mathit{\boldsymbol{w}}_{1} \|^{2}, \; \; \; \text{ s.t. } \mathit{\boldsymbol{J}}(\mathit{\boldsymbol{q}}_{\text{near}})\mathit{\boldsymbol{\dot{q}}}-\alpha_{1} \cdot \Delta \mathit{\boldsymbol{x}}=\mathit{\boldsymbol{w}}_{1} \end{align} $

(2)

算法3 $\text{Extend}(\mathit{\boldsymbol{q}}_{\text{near}}, \mathit{\boldsymbol{x}}_{\rm e, \text{near}}, \mathit{\boldsymbol{n}}_{\text{rand}}, l)$

1: $\mathit{\boldsymbol{\dot{q}}}_{\rm r} \leftarrow \text{HQLP_solver}(\mathit{\boldsymbol{q}}_{\text{near}}, \mathit{\boldsymbol{x}}_{\rm e, \text{near}}, \mathit{\boldsymbol{n}}_{\text{rand}}, l)$ 2: $\mathit{\boldsymbol{q}}'_{\text{new}} \leftarrow \mathit{\boldsymbol{q}}_{\text{near}} +\mathit{\boldsymbol{\dot{q}}}_{\rm r} {\delta} t $ 3: $\mathit{\boldsymbol{x}}'_{\rm e, \text{new}} \leftarrow \text{Forward_Kinematic}(\mathit{\boldsymbol{q}}'_{\text{new}})$ 4: if $\mathit{\boldsymbol{q}}'_{\text{new}} \in \mathcal{C}_{\text{free}}$ then 5:      return $(\mathit{\boldsymbol{q}}'_{\text{new}}, \mathit{\boldsymbol{x}}'_{\rm e, \text{new}}, \emptyset )$ 6: else 7:      return $(\emptyset , \emptyset , \mathit{\boldsymbol{q}}'_{\text{new}})$ 8: end if

其中$ \mathit{\boldsymbol{w}}_{1} $为松弛变量，用于规划任务不可行时对约束进行缩放，$ \alpha_{1} >0 $是常数。冗余机械臂除了可以完成主要的末端拓展任务，还可以利用剩余自由度完成其他次要任务。为了使机械臂能够通过自运动尽可能避开奇异构形，将次要任务设置为机械臂向任意构形方向随机拓展，即

$ \begin{align} \min _{\mathit{\boldsymbol{\dot{q}}}} \frac{1}{2}\| \mathit{\boldsymbol{w}}_{2} \|^{2}, \; \; \; \text{ s.t. } \mathit{\boldsymbol{\dot{q}}}-\mathit{\boldsymbol{\xi}}_{\rm a} =\mathit{\boldsymbol{w}}_{2} \end{align} $

(3)

其中$ \mathit{\boldsymbol{w}}_{2} $为松弛变量，$ \mathit{\boldsymbol{\xi}}_{\rm a} $为随机向量。每次在工作空间中进行局部拓展时，$ \mathit{\boldsymbol{\xi}}_{\rm a} $值都需要重新在容许的关节运动速度范围内随机均匀采样。此外，为了提升所规划轨迹的可执行性，还需要考虑机械臂的关节位置和关节速度限制（参考文[20]），可表示成统一的形式：

$ \begin{align} \min _{\mathit{\boldsymbol{\dot{q}}}} \frac{1}{2}\| \mathit{\boldsymbol{w}}_{\mathit{\boldsymbol{J}}} \|^{2}, \; \; \; \text{ s.t. } \begin{bmatrix} -\mathit{\boldsymbol{E}} \\ \mathit{\boldsymbol{E}} \end{bmatrix} \mathit{\boldsymbol{\dot{q}}}-\begin{bmatrix} -\mathit{\boldsymbol{\dot{Q}}}_{\min} \\ \mathit{\boldsymbol{\dot{Q}}}_{\max} \end{bmatrix}\leqslant \mathit{\boldsymbol{w}}_{\mathit{\boldsymbol{J}}} \end{align} $

(4)

最终，任务空间中的局部规划可以描述为考虑任务优先级的分层二次最小二乘规划（HQLP）问题：

$ \begin{align} \text{lex min} \left\{ \frac{1}{2}\| \mathit{\boldsymbol{w}}_{J} \|^{2}, \; \frac{1}{2}\| \mathit{\boldsymbol{w}}_{1} \|^{2}, \; \frac{1}{2}\| \mathit{\boldsymbol{w}}_{2} \|^{2} \right\} \end{align} $

(5)

对于构形空间中的采样，获取最近的构形节点$ \mathit{\boldsymbol{q}}_{\text{near}} $后，确定拓展方向为$ \Delta \mathit{\boldsymbol{q}}=\mathit{\boldsymbol{n}}_{\text{rand}} -\mathit{\boldsymbol{q}}_{\text{near}} $，则局部规划问题同样可以写成统一的优化问题：

$ \begin{align} \min _{\mathit{\boldsymbol{\dot{q}}}} \frac{1}{2}\| \mathit{\boldsymbol{w}}_{3} \|^{2}, \; \; \text{ s.t. } \mathit{\boldsymbol{\dot{q}}}-\alpha_{2} \cdot \Delta \mathit{\boldsymbol{q}}=\mathit{\boldsymbol{w}}_{3} \end{align} $

(6)

其中$ \mathit{\boldsymbol{w}}_{3} $为松弛变量，$ \alpha_{2} >0 $是常数。在考虑关节运动学约束后，可写成类似的分层规划问题：

$ \begin{align} \text{lex min} \left\{\frac{1}{2}\| \mathit{\boldsymbol{w}}_{J} \|^{2}, \; \frac{1}{2}\| {\mathit{\boldsymbol{w}}_{3}} \|^{2} \right\} \end{align} $

(7)

上述2类规划问题可以用文[20] 所提出的求解器求解，得到满足多优先级任务要求的参考速度$ \mathit{\boldsymbol{\dot{q}}}_{\rm r} $。接着，根据给定的计算时间步长$ {\rm{ \mathsf{ δ} }} t $，即得到新的构形$ \mathit{\boldsymbol{q}}'_{\text{new}} $，再利用正运动学函数求解对应的末端位姿$ \mathit{\boldsymbol{x}}'_{\rm e, \text{new}} $。

如果在新构形$ \mathit{\boldsymbol{q}}'_{\text{new}} $下的机械臂未与工作空间中的障碍物发生碰撞，则返回此构形及对应的末端位姿$ \mathit{\boldsymbol{x}}'_{\rm e, \text{new}} $，再将它们都添加到随机树中。反之，则将$ \mathit{\boldsymbol{q}}'_{\text{new}} $标记为碰撞构形，用于后续分析，同时将其邻近节点标记为受限节点。

此外，需要注意，参数$ \alpha_{1} $、$ \alpha_{2} $的取值对算法整体的求解效率有一定影响。$ \alpha_{1} $、$ \alpha_{2} $较小的取值将产生较小的关节速度$ \mathit{\boldsymbol{\dot{q}}}_{\rm r} $，在给定的计算时间步长$ {\rm{ \mathsf{ δ} }} t $下，随机树探索构形/工作空间的步长也较小，使得算法通常需要更长的运行时间才能找到解。反之，$ \alpha_{1} $、$ \alpha_{2} $较大的取值会加快算法的搜索，但由于存在最大关节运动速度的限制，单次拓展的最大步长也有限。从算法求解效率角度考虑，取$ \alpha_{1} =\alpha_{2} =10 $。

3.2.3 参数自适应调节

正如前文所述，算法中部分参数可自主调节，调节流程如算法4所示。

算法4 $\text{Parameter_Update}( s_{\text{current}}, \sigma , p_{\rm HS }, l , \mathit{\boldsymbol{q}}_{\text{obs}})$

1: if l='W' then 2:      if $\mathit{\boldsymbol{q}}_{\text{obs}} =\emptyset $ then 3:           $\sigma \leftarrow (1-\lambda)\sigma $ 4:           if $\sigma <\underline{\sigma}$ then 5:                $ s_{\text{current}} \leftarrow s_{\text{child}}$ 6:                $\sigma \leftarrow \sigma_{0}$ 7:           end if 8:      else 9:           $\sigma \leftarrow (1+\lambda)\sigma $ 10:           if {$\sigma >\overline{\sigma}$ }then 11:                $ s_{\text{current}} \leftarrow s_{\text{parent}}$ 12:                $\sigma \leftarrow \sigma_{0}$ 13:           end if 14:      end if 15:      $ p_{\rm HS } \leftarrow \min \left(\overline{p}_{\rm HS}, \max \left(\underline{p}_{\rm HS}, p_{\rm HS} -d+p_{\text{prog}}\right)\right)$ 16: end if 17: return $( s_{\text{current}}$, $\sigma $, $ p_{\rm HS })$

如果随机采样与拓展在工作空间中进行，那么采样所在的圆球$ s_{\text{current}} $、样本点分布标准差$ \sigma $将采取和EET算法^[17]类似的方式更新，以权衡对工作空间的探索和相关信息的利用（算法4的第2~14行）。具体而言，如果工作空间中的样本点被成功拓展，则$ \sigma $减小，采样更加集中在圆球内部，以加强对当前工作空间信息的利用。如果$ \sigma $小于给定值$ \underline{\sigma} =0.1 $，表明当前圆球信息已经被充分利用，则转向对下一个圆球的探索利用。反之，如果拓展失败，表明当前工作空间圆球区域难以提供有效的信息，则$ \sigma $增大，从而加强对更大范围的工作空间的探索。如果$ \sigma $大于给定值$ \overline{\sigma}=1 $，表明拓展失败次数过多，随机树难以在当前区域生长，那么将返回前一个圆球，尝试重新探索。每次切换需要探索利用的圆球时，都要重置$ \sigma $值为$ \sigma_{0} =0.5 $。

此外，算法还根据工作空间中的拓展是否有进展来调节采样概率$ p_{\rm HS } $，以平衡工作空间与构形空间中的采样（算法4的第15行）。每一次迭代后，选择在工作空间中采样的概率都会按$ d=0.001 $衰减，即在对工作空间探索没有进展的情况下会逐渐转向对构形空间的探索。不仅如此，还用$ p_{\text{prog}} $指示沿工作空间圆球的探索是否有进展。如果当前迭代下，成功拓展进入到离目标区域更近的圆球中，则概率$ p_{\rm HS } $按$ p_{\text{prog}} =0.3 $增加，以增加工作空间采样的权重；反之，若当前迭代下拓展未能进入新圆球，则$ p_{\text{prog}} =0 $。概率$ p_{\rm HS } $被限制在$ [\overline{p}_{\rm HS}, \underline{p}_{\rm HS} ] $内，本文设置为$ [0.2, 1] $，使得在最差的情况下仍能有一定概率在工作空间中采样。

3.3 辨识局部空间再拓展

对于装配问题，构形空间中通常会存在诸多拥挤复杂的区域（例如窄通道），导致随机拓展更容易因为发生碰撞而失败。因此，当初次拓展失败后，需要对局部构形空间进行分析，以获取更加高效的拓展方向，从而使随机树能够快速穿越复杂区域，到达目标点。在前期工作^[10]基础上，为降低应用复杂度，修改并简化了再次拓展的Extend_further算法，其流程如算法5所示。

算法5 $\text{Extend_further}(\mathit{\boldsymbol{q}}_{\text{near}}, \mathit{\boldsymbol{x}}_{\rm e, \text{near}}, \mathit{\boldsymbol{q}}_{\text{obs}}, \mathcal{T})$

1: $\mathcal{Q}_{\text{free}}^{\rm c} \leftarrow \text{Near_Constrained_Config}(\mathit{\boldsymbol{q}}_{\text{near}}, \mathit{\boldsymbol{x}}_{\rm e, \text{near}}, {\mathcal{T}})$ 2: if $\text{Not_Sufficient}(\mathcal{Q}_{\text{free}}^{\rm c})$ then 3:      $\mathit{\boldsymbol{q}}'_{\text{rand}} \leftarrow \text{Bridge_Test}( \mathit{\boldsymbol{q}}_{\text{obs}})$ 4: else 5:      $\mathit{\boldsymbol{q}}'_{\text{rand}} \leftarrow \text{PCA_Direction}( \mathit{\boldsymbol{q}}_{\text{near}} , \mathit{\boldsymbol{q}}_{\text{obs}} , \mathcal{Q}_{\text{free}}^{\rm c})$ 6: end if 7: $(\mathit{\boldsymbol{q}}'_{\text{new}}, \mathit{\boldsymbol{x}}'_{\rm e, \text{new}}, \mathit{\boldsymbol{q}}'_{\text{obs}})\leftarrow \text{Extend}(\mathit{\boldsymbol{q}}_{\text{near}}, \mathit{\boldsymbol{x}}_{\rm e, \text{near}}, \mathit{\boldsymbol{q}}'_{\text{rand}}, 'C')$ 8: $ \mathcal{T} \leftarrow \text{Tree_Grow}(\mathit{\boldsymbol{q}}'_{\text{new}}, \mathit{\boldsymbol{x}}'_{\rm e, \text{new}})$ 9: return ${\mathcal{T}}$

为了分析局部空间，首先利用Near_Constrained _Config函数搜集以邻近节点$ \mathit{\boldsymbol{q}}_{\text{near}} $为球心、半径$ R_{\rm a} $范围内的受限节点（如图 2中红色节点所示），存储在集合$ \mathcal{Q}_{\text{free}}^{\rm c} $中。其中$ R_{\rm a} $值与窄通道宽度有关，取值过大或过小均会影响算法的效率，本文取$ R_{\rm a} =0.5 $rad。由于受限节点是位于障碍物附近的无碰撞构形，可认为足够丰富的受限节点能够反映局部无碰撞区域的形状。所以，可以对集合$ \mathcal{Q}_{\text{free}}^{\rm c} $进行主成分分析（PCA），计算其协方差矩阵，并寻找对应的特征值$ \lambda_{\text{const}}^{i} $和特征向量$ \mathit{\boldsymbol{v}}_{\text{const}}^{i} $。其中特征向量代表集合的典型变化方向，对应的特征值代表集合在各方向上的方差。据此，还可以做出反映局部空间特征的椭球，椭球中心位于集合$ \mathcal{Q}_{\text{free}}^{\rm c} $各元素的均值点，椭球轴的方向与特征向量重合，轴的长度与对应特征值有关（如图 2所示）。通常复杂区域（尤其是窄通道区域）呈现细长的形状，代表可拓展方向，同时也对应着方差最大的方向。因此，可以将原始发生碰撞的拓展方向$ \mathit{\boldsymbol{q}}_{\text{obs}} $投影到此方向上，产生新的拓展方向（算法5的第5行）：

$ \begin{align} \mathit{\boldsymbol{q}}'_{\text{rand}} =\mathit{\boldsymbol{q}}_{\text{near}} +\sum _{k=1}^n \left( \frac{\lambda_{\text{const}}^{i}} {\lambda_{\text{const}}^{1}} \left(\mathit{\boldsymbol{q}}_{\text{obs}} -\mathit{\boldsymbol{q}}_{\text{near}}\right)\cdot \mathit{\boldsymbol{v}}_{\text{const}}^{i} \right) \mathit{\boldsymbol{v}}_{\text{const}}^{i} \end{align} $

(8)

其中$ \lambda_{\text{const}}^{1} $是$ \lambda_{\text{const}}^{i} $中最大的特征值。

注意到本文并没有像文[10] 那样严格区分局部空间是窄通道环境还是位于凸障碍物边界。但是考虑到受限节点已经位于障碍物附近，能够反映出部分障碍物边界特征，因此简化后的算法在2种情形下均有良好性能。

此外，当节点数量不足以进行主成分分析时（需要节点数至少大于$ n+1 $），将无法获得有效的再次拓展方向。可以利用文[10] 中的桥测试算法，寻找局部空间中是否存在窄通道的入口（算法5的第3行，如图 2所示），其流程为：

(1) 将$ \mathit{\boldsymbol{q}}_{\text{obs}} $作为桥的一个端点，通过随机均匀采样获取桥的方向$ \mathit{\boldsymbol{d}}_{\rm b} $；

(2) 沿方向$ \mathit{\boldsymbol{d}}_{\rm b} $按高斯分布计算随机距离，得到桥的另一个端点$ \mathit{\boldsymbol{q}}_{\text{end}} $；

(3) 如果$ \mathit{\boldsymbol{q}}_{\text{end}} $是碰撞构形，则计算桥的中点；

(4) 如果中点是无碰撞构形，则可以作为再次拓展的方向$ \mathit{\boldsymbol{q}}'_{\text{rand}} $。

考虑到随机树在复杂区域内拓展效率低的特点，当获取到更高效的拓展方向后，可采取多次连续拓展的策略，以更快速地通过复杂区域。

4 仿真与分析（Simulation and analysis）

为了验证算法的有效性与优越性，将HS-RRV算法和经典RRT^[6]、ADD-RRT^[11]、Bi-RRT^[14]、EET^[17]以及HS-RRT算法进行对比。其中HS-RRT算法是HS-RRV算法的简化版本，即未考虑局部空间辨识后再拓展（算法5）部分。所有仿真均利用Matlab软件在内存为16GB，CPU为2.60GHz、I7-9750的笔记本电脑中进行。

如图 3所示，仿真包含4个空间大小均为8m $ \times $8m$ \times $8m的典型场景，其中图 3(a)~(c)是固定基座机械臂，图 3(d)是自由漂浮基座机械臂。机械臂都包含7个关节（相对于6自由度的末端任务是冗余的），且拥有相同的关节排布、D-H（Denavit-Hartenberg）参数以及惯性参数，参数设置如表 1所示。其中，基座（0号刚体）参数与惯性参数仅用于自由漂浮基座机械臂。图 3中，末端部件为黄色表示初始状态，绿色表示目标状态，由于同一目标末端状态可能对应着多种机械臂构形，图中仅给出了其中一种。

仿真中，所有机械臂的关节角运动范围为$ [ -3, 3 ] $，单位为rad；关节速度范围为$ [ -1.5, 1.5 ] $，单位为rad/s；单次规划时间步长为$ {\rm{ \mathsf{ δ} }} t=0.04 $s。仿真场景均用凸多边形建模，采用GJK（Gilbert-Johnson-Keerthi）算法^[21]进行碰撞检测。仿真实验分别统计了迭代次数、生成的节点数、碰撞检测次数、规划时间（即规划效率）、关节和末端的运动轨迹长度、成功率。所有算法都运行了30次，一旦算法搜索到解，则停止运算并记录相关数据，如果算法无法在20min内找到解，则认为求解失败，同样记录此时的数据，最终统计30次的平均值。需要注意的是，关节和末端轨迹长度仅在算法成功规划时进行统计，因为此时的轨迹长度才有意义。统计结果如表 2所示，其中由于RRT和ADD-RRT算法在4种场景下执行30次均无法在给定时间内找到解，规划成功率为0，简洁起见表中省略了相关数据。

4.1 仿真场景1

此场景下，机械臂需要将1.2m$ \times $1.6m$ \times $0.2m长方体部件从狭缝中取出，并安装到另一侧的狭缝中，这两处狭缝都是“窄通道”环境。其中，机械臂初始关节构形（单位rad）为

$ \begin{align*} \mathit{\boldsymbol{q}}_{\text{init}} =[ 0 , \pi /3, \pi /4, -\pi /4, -\pi /2, -\pi /2, -\pi /3 ]^{\rm T} \end{align*} $

对应初始末端位姿（位置单位m，姿态单位rad）为

$ \begin{align*} \mathit{\boldsymbol{x}}_{\rm e, \text{init}} =[ 4.011, -0.210, 1.267, \pi /2, 0, \pi /2 ]^{\rm T} \end{align*} $

部件目标位姿为

$ \begin{align*} \mathit{\boldsymbol{x}}_{\rm e, \text{goal}} =[ 0.210, 4.011, 1.267, -\pi /2, -\pi / 2, 0 ]^{\rm T} \end{align*} $

4种算法的单次规划结果如图 4所示，为清晰起见，图中仅保留了工作空间中的障碍物以及末端位置的随机拓展轨迹（绿色和蓝色）与最终轨迹（红色和紫色）。其中，HS-RRV算法所规划出的关节轨迹如图 5所示，可见关节运动始终满足关节位置和关节速度约束。

由于部件最初已经在狭缝中，EET、HS-RRT和HS-RRV算法利用工作空间信息，均能够较快找到从狭缝中取出部件的轨迹。此后，部件需要经过旋转、对准才能被安装到另一侧狭缝，但上述算法均没有有效的姿态信息可以利用。由于EET算法限制了采样与搜索范围导致算法不完备，因而搜索容易陷在第2个狭缝前，难以找到解，算法成功率较低。与此相对应地，HS-RRT算法相较于EET算法仅增加了在整个构形空间中进行采样与拓展的环节，保证了算法的完备性，从而在同样的运行时间下显著提升了算法的成功率。HS-RRV算法结合了对局部复杂区域的分析，进一步提高了算法的成功率，其成功率约是EET算法的1.7倍，是HS-RRT算法的1.2倍。与此同时，Bi-RRT算法没有利用工作空间信息缩小采样搜索范围，而是始终在整个构形空间中随机均匀采样，尽管从初始和目标状态同时构造了2棵随机树，但在大部分情况下均难以在狭缝中拓展，如图 4(d)所示。Bi-RRT算法在极少数情况下其中一棵随机树能够穿越部分狭缝，但在给定时间内的整体求解成功率为0。注意到，由于HS-RRT、HS-RRV以及Bi-RRT等算法是完备的，因此在足够长的运行时间下，算法总能够找到解，这是EET算法所不能保证的。

从表 2也可以看出，HS-RRV算法能够以更少的迭代次数找到解，相应地，生成的节点也更少，从而所需的存储空间更小。此外，节点数的减少也会提高算法寻找相邻节点的效率，从而提高算法整体效率。碰撞检测次数是另一个影响规划效率的重要因素，在HS-RRV算法中，随机树的拓展、桥测试均需要进行碰撞检测，但在较少的迭代次数下所需的总碰撞检测次数也较少。综合而言，HS-RRV算法的规划效率更高，约是EET算法的2.4倍，是HS-RRT算法的1.6倍。需要注意的是，算法的成功率越低，其统计的规划时间越是低估其实际所需时间。

注意到，此场景下利用工作空间信息进行拓展的3种算法的轨迹长度相当。

4.2 仿真场景2

此场景下，需要将2m$ \times $0.2m$ \times $0.2m杆件从桁架的一侧搬运到另一侧，机械臂初始关节构形为

$ \begin{align*} \mathit{\boldsymbol{q}}_{\text{init}} & =[ 0 , 5\pi /12, 5\pi /12, -\pi /4, -\pi /2, \\ &\quad -2\pi /3, -\pi /3]^{\rm T} \end{align*} $

对应的初始末端位姿为

$ \begin{align*} \mathit{\boldsymbol{x}}_{\rm e, \text{init}} =[2.937, 0.625, 2.453, 2.25, -0.02, -1.28]^{\rm T} \end{align*} $

杆件目标位姿为

$ \begin{align*} \mathit{\boldsymbol{x}}_{\rm e, \text{goal}} =[0.625, 2.937, 2.4530, -0.66, -0.02, -1.28]^{\rm T} \end{align*} $

4种算法的单次规划结果如图 6所示，其中HS-RRV算法规划出的关节轨迹如图 7所示。此时，利用工作空间信息的单树算法可以较为轻松地将杆件搬运至目标区域附近，但是难以精确到达目标点。这是由于较大的杆件、目标区域较为复杂的环境以及机械臂几何构形与运动极限等约束，使得算法需要花费更大的计算代价才能精确到达目标点。如图 8所示，随着规划精度$ \varepsilon $的提升，3种单棵树算法的平均运行时间均增加，且规划成功率均下降。当规划精度从0.1提升到0.05时，EET算法规划效率衰减最显著。相对应地，HS-RRV算法所受影响相对较小。

除此以外，HS-RRV算法的成功率始终高于EET和HS-RRT算法，如表 2所示（对应0.05精度），分别是它们的1.7倍和1.2倍。此时，HS-RRV算法在迭代次数和节点数上相对于前2种算法同样具有明显优势。但是，为了能够以更高的精度接近目标点，HS-RRV算法需要更多的局部空间分析以及再拓展，因此碰撞检测次数有所上升。综合而言，HS-RRV算法的规划效率依然优于EET算法，是其1.4倍；但是仅稍优于HS-RRT算法。

然而，值得关注的是，Bi-RRT算法在此场景下有着令人瞩目的表现，算法成功率达到100%，且规划效率也明显高于其他3种单树算法。但是，Bi-RRT算法需要预先获取末端目标状态下对应的机械臂构形，其本身也是一个复杂的逆运动学求解问题，需要耗费较大的计算代价，本文并没有重点研究此问题。同时，利用Bi-RRT算法得到的关节和末端轨迹都是最长的，其中末端轨迹长度更是其他3种算法的2倍多。但这也为规划算法提供了可参考的改进方向：如果有高效的逆运动学求解方法，则可将HS-RRV与Bi-RRT算法相结合，在保证轨迹质量的同时提高算法的效率和规划精度。

4.3 仿真场景3

此场景下，机械臂需要将2m$ \times $2m$ \times $0.2m大型板件从平台上取出，穿过墙上窗口，转移至指定位置，其中窗口尺寸仅略大于平板。机械臂初始关节构形为

$ \begin{align*} \mathit{\boldsymbol{q}}_{\text{init}} =[\pi /2, \pi /2, \pi /4, -\pi /2, -\pi /4, -\pi /2, \pi /2 ]^{\rm T} \end{align*} $

对应的初始末端位姿为

$ \begin{align*} \mathit{\boldsymbol{x}}_{\rm e, \text{init}} =[-3.771, 0.710, -0.040, \pi /2, 0, -\pi /2 ]^{\rm T} \end{align*} $

杆件目标位姿为

$ \begin{align*} \mathit{\boldsymbol{x}}_{\rm e, \text{goal}} =[3.700, 0.7101, 0.509, -\pi /2, 0, -\pi /2]^{\rm T} \end{align*} $

4种算法的单次规划结果如图 9所示，HS-RRV算法规划出的关节轨迹如图 10所示。此时，由于板件初始状态靠近障碍物，且窗口可行空间有限，EET算法很难成功。Bi-RRT算法虽然有较高的成功率，但是为了寻找穿越窗口的轨迹而耗费了大量时间。然而，同时搜索工作空间和构形空间的HS-RRT算法因为具有完备性，因而既保证了一定的成功率，又提高了规划效率。HS-RRV算法进一步提升了性能，如表 2所示，其成功率约是EET算法的5.3倍，HS-RRT算法的1.7倍，Bi-RRT算法的1.3倍；其规划效率分别是上述3种算法的2.7倍、1.6倍和2.5倍；其轨迹长度也是远小于EET和Bi-RRT算法，与HS-RRT算法相当。

4.4 仿真场景4

此场景下，利用自由漂浮基座空间机械臂系统将2m$ \times $0.2m$ \times $0.2m杆件从墙上的孔中取出并转移到另一个孔内，工作空间中存在2个窄通道。基座初始位姿均为0，机械臂初始关节构形为

$ \begin{align*} \mathit{\boldsymbol{q}}_{\text{init}} =[ \pi /6, \pi /3, 0, \pi /2, 0, -\pi /6, 0 ]^{\rm T} \end{align*} $

对应的初始末端位姿为

$ \begin{align*} \mathit{\boldsymbol{x}}_{\rm e, \text{init}} =[ 2.658, 1.948, 1.249, -\pi /3, -\pi /2, -\pi /2 ]^{\rm T} \end{align*} $

杆件目标位姿为

$ \begin{align*} \mathit{\boldsymbol{x}}_{\rm e, \text{goal}} &=[ 2.856, -0.562, 1.222, -\pi /2, \\ &\quad -\pi /12, -7\pi /12]^{\rm T} \end{align*} $

此时，规划不仅需要考虑系统运动学，还需要考虑其动力学特性。假设基座不受控且不考虑重力以及其他外力的影响，机械臂和基座之间存在动力学耦合，机械臂运动会对基座位姿产生干扰，同时基座位姿扰动又会影响机械臂运动（其动力学建模可参考文[8]）。此场景下，基座位姿也作为子节点被添加到随机树中。值得关注的是，单一的工作空间中的采样可能会因机械臂与基座运动的相互干扰而使拓展陷入局部极值，导致拓展难以进行。但本文算法结合了工作空间－构形空间混合采样，使得当工作空间中的样本点难以被有效拓展时，算法尝试在整个构形空间随机采样，并根据度量选取关节构形距离最近且基座姿态相对于初始姿态扰动最小的节点作为最近节点进行拓展，选取度量如下：

$ \begin{align} {\rm Dist} \left(\mathit{\boldsymbol{q}}_{i}, \mathit{\boldsymbol{\psi}}_{i}, \mathit{\boldsymbol{q}}_{\text{rand}}, \mathit{\boldsymbol{\psi}}_{\rm b, \text{init}}\right) = \left\| \mathit{\boldsymbol{q}}_{i} -\mathit{\boldsymbol{q}}_{\text{rand}} \right\|+\left\| \mathit{\boldsymbol{\psi}}_{i} -\mathit{\boldsymbol{\psi}}_{\rm b, \text{init}} \right\| \end{align} $

(9)

从而尽可能减小基座姿态扰动带来的不利影响。

4种算法的单次规划结果如图 11所示。其中，HS-RRV算法规划所得关节轨迹以及机械臂运动导致的基座扰动如图 12所示。

如表 2所示，此场景下HS-RRV和HS-RRT算法两者性能相当，但HS-RRV算法在规划成功率、效率等方面略优。同时，HS-RRV算法相对于EET算法极大地提升了成功率（约是其2.4倍），但HS-RRV算法所需规划时间却不足EET算法的一半。此外，Bi-RRT算法虽然也有着较高的成功率，但是规划耗时约是HS-RRV算法的2倍。可见，HS-RRV算法在处理复杂非线性动力学系统的规划问题时依然有优异的表现，表明算法具有广泛的适用性。

4.5 局部规划器性能分析

正如3.2.2节中所提，当利用局部规划器在工作空间中拓展时，对于冗余机械臂，除了可以完成末端拓展这一主要任务外，还可以利用剩余自由度使机械臂关节向随机方向拓展，以尽可能避开运动学奇异构形，也能降低机械臂陷入几何结构局部极值的可能性。以场景3为例，与HS-RRV算法规划结果相对应的雅可比矩阵$ \mathit{\boldsymbol{J}} $的最小奇异值典型变化情况如图 13所示，可见机械臂在整个运动过程中最小奇异值都在合理范围内。

为进一步凸显3.2.2节所提局部规划器的优势，还以4种场景下不包含关节随机拓展次要任务的HS-RRV算法（记作HS-RRV-1）作为对比，每个场景均运行了30次，统计结果如表 3所示。此时，HS-RRV-1算法需要经历更多随机采样和碰撞检测才能找到解，因此所需规划时间也更长，平均规划时间至少增加了10%，最多可达70%；相应地，规划成功率降低了至少10%。此外，虽然HS-RRV-1算法在工作空间拓展时由于没有次要任务的干扰，规划所得的关节轨迹长度更短，但为了绕开奇异点和局部极值，末端轨迹长度增加。

另一方面，3.2.2节所提的局部规划器可以直接考虑机械臂关节位置、速度甚至加速度等运动学限制，并整合为统一的速度约束，使得规划出的参考速度在主动满足运动学限制的前提下尽可能减小与局部规划目标的误差。然而，传统算法通常没有直接考虑关节约束，而是先根据局部目标规划出参考速度，再对超出限制的参考速度进行饱和处理以满足约束，此时得到的运动速度可能会偏离期望拓展方向。此后，传统算法还需对积分得到的新关节构形进行筛选，舍去不满足关节位置限制的构形，但这将降低样本点的利用率。4种场景下未直接考虑关节约束的HS-RRV算法的规划结果如表 4所示，可见其运行效率、规划成功率相对于3.2.2节所提算法下降约10%。除此以外，传统局部规划器还难以考虑加速度限制以及其他不等式约束。

综上，本文所提出的局部规划器，相对于不考虑随机拓展次要任务、不直接考虑关节限制的传统算法，运行效率、规划成功率更高，有更广泛的适用性。

5 结论（Conclusion）

为应对利用空间机械臂进行在轨装配时因工作空间狭小、复杂而导致运动规划效率低下的问题，提出了HS-RRV规划算法。该算法结合了工作空间和构形空间混合采样的策略，采样权重随算法进程自主调节，既利用工作空间信息提高规划效率，又保证了算法的完备性，防止陷入局部极值。此外，在局部规划器设计中，充分利用了机械臂的冗余性，以同时完成多种不同优先级的任务；还考虑了机械臂的运动学与动力学约束，以提高所规划轨迹的可执行性。当算法随机拓展陷入复杂区域而难以推进时，利用桥测试和PCA方法对局部空间进行分析，以获取更加高效的拓展方向。仿真结果表明，HS-RRV算法相比其他测试算法在迭代次数、生成的节点数、规划效率以及规划成功率等方面均有明显优势，且对多种复杂系统与工作环境有广泛的适用性。未来，将考虑测量的不确定性、装配时的接触力约束等因素，对机械臂运动规划算法作进一步研究。