机器人的冗余度使机器人能够在非结构化的复杂环境中运行，能够同时完成多个任务。经典的利用冗余度的方法有梯度投影法^[1-2]、增广雅克比方法^[3-4]和加权最小范数法^[5-9]。多任务优先级算法使低优先级任务在高优先级任务的零空间中运行，保证首先完成高优先级任务，利用冗余度完成低优先级任务。Nakamura等^[10]最先提出了任务优先级概念，采用零空间投影技术，实现了迭代梯度投影，保证低优先级任务在不影响高优先级任务的前提下执行，并成功用于避障^[2]。在之前研究的基础上，Siciliano等^[11]给出了高冗余度系统多任务优先级通用算法，能够迭代求解关节速度。

复杂的非结构化环境中，在不同的时间和位置，机器人需要完成任务不同，需要进行任务的插入或移除，这会引起雅克比矩阵秩的变化，出现关节速度的不连续现象^[12]。解决任务优先级方法中关节速度的不连续问题是非常关键的。Chiaverini^[13]提出奇异鲁棒方法，通过引入阻尼因子，以跟踪误差为代价，解决雅克比矩阵秩的变化引起的奇异问题。在文献[14]中，提出了一种基于连续零空间投影算子的平滑控制方法，以较低的计算成本解决了任务状态切换时的不连续问题，但即使所有任务都处于激活状态或失活状态，该方法系统的稳定性仍然存在问题。Mansard等^[15]提出了一种连续逆，可以保证在任务切换过程中的关节速度连续性，并且当任务完全激活或失效时，其连续逆等于伪逆，之后应用于处理多个优先级任务^[16]。但Mansard等提出的连续逆随着任务数量的增加，计算时间也呈指数增长。之后，Jiang等^[17]和黄水华^[18]通过连续迭代投影每个任务的零空间构建了连续迭代投影算子，有限次迭代能够保证任务切换过程中的连续性，当迭代次数接近无限次时，连续迭代投影算子逐渐接近经典伪逆投影算子。和Mansard等提出的方法相比，此方法可以节省计算时间。

综上所述，需要针对任务优先级算法中任务插入与移除引起的关节速度不连续问题，进行深入研究和分析。笔者基于文献[17]的连续迭代投影原理，提出连续迭代投影多任务优先级方法，依据李雅普诺夫稳定性原理证明所提出方法的稳定性。通过仿真验证该方法的有效性和稳定性，并与增广投影多任务优先级方法进行对比。

1 机器人运动学控制 1.1 机器人运动学控制

冗余串联机器人系统的自由度大于其完成期望任务的自由度，即关节空间的维数n超过任务空间的维数m, n与m的差值为冗余自由度。定义关节空间关节位置向量为q∈Rⁿ，任务空间末端位置向量为x∈R^m，则机器人正向运动学方程为：

$ \boldsymbol{x}=f(\boldsymbol{q}), $

(1)

式中：f为任务函数，为从关节空间向量到任务空间向量的映射。对式(1)求导可以得到关节速度和末端执行器速度的映射关系，如下：

$ \dot{\boldsymbol{x}}=\frac{\partial f(\boldsymbol{q})}{\partial \boldsymbol{q}} \dot{\boldsymbol{q}}=\boldsymbol{J} \dot{\boldsymbol{q}}, $

(2)

式中：J∈R^m×n为雅克比矩阵。对于冗余机器人，式(2)的一般解可表示为：

$ \mathit{\boldsymbol{\dot q}}={\mathit{\boldsymbol{J}}^\dagger }{\mathit{\boldsymbol{\dot x}}_{\rm{u}}} + \mathit{\boldsymbol{Pz}}, $

(3)

式中：J^†=J^T(JJ^T)^-1为雅克比矩阵J的Moore-Penrose伪逆，$ {\mathit{\boldsymbol{\dot x}}_{\rm{u}}}$为任务输入控制指令，z∈Rⁿ为任意关节速度向量，P∈R^n×n为经典雅克比零空间投影算子。P的表达式如下：

$ \boldsymbol{P}=\boldsymbol{I}_n-\boldsymbol{J}^{\dagger} \boldsymbol{J}, $

(4)

式中I_n为n维的单位方阵。

冗余机器人可以同时执行多个任务。如跟踪末端轨迹的同时，进行避障、保证关节限位等。但需要对多个任务的优先级进行排序，来保证首要任务在高优先级层次被执行，利用冗余的自由度来完成次要任务。为了简单起见，先考虑T₁和T₂两个任务，这两个任务定义为：

$ \boldsymbol{x}_1=f_1(\boldsymbol{q}), $

(5)

$ \boldsymbol{x}_2=f_2(\boldsymbol{q}) 。$

(6)

假设T₁是首要任务，任务T₂在任务T₁的零空间中执行。任务T₁和T₂对应的雅克比矩阵分别为J₁和J₂。根据式(3)，则关节速度的解为：

$ \dot{\boldsymbol{q}}_2=\boldsymbol{J}_1^{\dagger} \dot{\boldsymbol{x}}_{1, \mathrm{u}}+\boldsymbol{P}_1\left(\boldsymbol{J}_2 \boldsymbol{P}_1\right)^{\dagger} \dot{\boldsymbol{x}}_{2, \mathrm{u} }。$

(7)

式中：$ \dot{\boldsymbol{x}}_{1, \mathrm{u}}$和$ \dot{\boldsymbol{x}}_{2, \mathrm{u}}$分别为任务1和任务2的输入指令，P₁为雅克比矩阵J₁的经典零空间，P₁=I_n－J₁^†J₁。当需要执行两个以上的任务时，可以将上式推广到任意的k个任务，对应的任务优先级解可表示为

$ \dot{\boldsymbol{q}}_i=\dot{\boldsymbol{q}}_{i-1}+\boldsymbol{P}_{i-1}\left(\boldsymbol{J}_i \boldsymbol{P}_{i-1}\right)^{\dagger} \dot{\boldsymbol{x}}_{i, \mathrm{u}}, i=1, 2, \cdots, k, $

(8)

式中：$ \dot{\boldsymbol{q}}_0=0, \boldsymbol{P}_0=\boldsymbol{I}_n, \dot{\boldsymbol{x}}_{i, \mathrm{u}}$为第i个任务的输入指令，P_i为增广雅克比J_i^A的零空间投影。J_i^A为前i个任务的增广雅克比矩阵，表达式如下：

$ \boldsymbol{J}_i^{\mathrm{A}}=\left[\begin{array}{llll} \boldsymbol{J}_1^{\mathrm{T}} & \boldsymbol{J}_2^{\mathrm{T}} & \cdots & \boldsymbol{J}_i^{\mathrm{T}} \end{array}\right]^{\mathrm{T}}, i=1, 2, \cdots, k 。$

(9)

低优先级任务的关节速度投影在高优先级任务的增广零空间中，这种方法为增广投影法。

在不同的时间点以及当机器人关节移动到不同位置时，需要进行任务的插入与移除。如：在进行末端期望轨迹跟踪的同时，当机器人靠近障碍物时，需要考虑避障任务，且需要将避障任务插入优先级层次中；当机器人远离障碍物时，则不需要考虑避障任务，可以将避障任务移除，将冗余度用于完成其他任务。

式(8)在任务插入与移除时，会出现关节速度不连续的问题，不能满足需求。因此，将根据连续迭代投影原理，构建一种新型连续迭代任务优先级的控制方法。

2 连续迭代投影任务优先级方程 2.1 连续迭代投影算子

引入激活因子，由激活因子构成的激活矩阵，来对任务的插入以及移除进行控制。激活矩阵定义如下：

$ \boldsymbol{H}_k=\left[\begin{array}{cccc} h_1 \boldsymbol{I}_{m 1} & 0 & \cdots & 0 \\ 0 & h_2 \boldsymbol{I}_{m 2} & \cdots & 0 \\ 0 & 0 & & \vdots \\ 0 & 0 & \cdots & h_k \boldsymbol{I}_{m k} \end{array}\right], $

(10)

式中：h_i∈[0, 1] (i=1, 2, …, k)是第i个任务的激活因子，用于表示第i个任务的激活状态。当h_i=0，第i个任务完全失活，被移除；当h_i=1，第i个任务完全激活，被执行；当h_i∈(0, 1)，任务i被部分激活。

基于激活因子和激活矩阵，在文献[16]中，构建了连续迭代投影算子如下：

$ \boldsymbol{P}_{\mathrm{NH}}^k=\left(\prod _{i=1}^k\left(\boldsymbol{I}_n-h_i \boldsymbol{J}_i^{\dagger} \boldsymbol{J}_i\right)\right)^N, $

(11)

式中：N为迭代次数，右上角上标k表示P_NH^k为前k个任务的连续迭代投影算子。在下文，右上角没有小标则表示为所有任务的连续迭代投影算子。

连续迭代投影算子P_NH的连续性与激活因子h_i和迭代次数N有关。激活因子h_i在0, 1之间，h_i平滑连续变化，直接影响连续迭代投影算子P_NH的连续性。此外，迭代次数N也会对连续迭代投影算子P_NH的连续性产生影响。P_NH可以写作关于h_i的多项式形式，对于有限的迭代次数N，P_NH相对于激活因子的连续性可以得到保证。当迭代次数趋于无穷时，P_NH相对于激活因子是不连续的。

假设所有任务处于完全激活与失活状态，即h_i=0或h_j=1(i, j=1, 2, …, k)。定义所有任务增广雅克比矩阵为J=[J₁^T J₂^T …J_k^T] ^T，加入所有任务的激活矩阵H，可以得到增广零空间(I_n－(HJ)^†HJ)。当所有任务处于失活状态，即h_i=0(i=1, 2, …, k)，则HJ=0。根据式(11)，对于迭代次数N都有P_NH=(I_n－(HJ)^†HJ)。接下来对当部分或所有任务处于完全激活状态时连续迭代投影算子P_NH与增广零空间(I_n－(HJ)^†HJ) 的关系进行讨论。

增广雅克比矩阵的零空间为各个任务雅克比零空间的子空间，即N(HJ)⊆N(h_iJ_i) (i=1, 2, …, k)。又因为，J_i(I_n－J_i^†J_i)=0。因此，对于雅克比矩阵J_i有h_iJ_i^†J_i(I_n－(HJ)^†HJ)=0。将(I_n－(HJ)^†HJ)相加在等式h_iJ_i^†J_i(I_n－(HJ)^†HJ)=0的两边可得，(I_n－h_iJ_i^†J_i)(I_n－(HJ)^†HJ)=(I_n－(HJ)^†HJ)。

由(I_n－(HJ)^†HJ)矩阵的对称性质可知：

$ \begin{gathered} \left(\boldsymbol{I}_n-(\boldsymbol{H} \boldsymbol{J})^{\dagger} \boldsymbol{H} \boldsymbol{J}\right)=\left(\boldsymbol{I}_n-(\boldsymbol{H} \boldsymbol{J})^{\dagger} \boldsymbol{H} \boldsymbol{J}\right)^{\mathrm{T}}=\\ \left(\left(\boldsymbol{I}_n-h_i \boldsymbol{J}_i^{\dagger} \boldsymbol{J}_i\right)\left(\boldsymbol{I}_n-(\boldsymbol{H} \boldsymbol{J})^{\dagger} \boldsymbol{H} \boldsymbol{J}\right)\right)^{\mathrm{T}}=\\ \left(\boldsymbol{I}_n-(\boldsymbol{H} \boldsymbol{J})^{\dagger} \boldsymbol{H} \boldsymbol{J}\right)\left(\boldsymbol{I}_n-h_i \boldsymbol{J}_i^{\dagger} \boldsymbol{J}_i\right) 。\end{gathered} $

(12)

进而可以得到(I_n－h_iJ_i^†J_i)(I_n－(HJ)^†HJ)=(I_n－(HJ)^†HJ)(I_n－h_iJ_i^†J_i)，将等式的两边展开可以得到h_iJ_i^†J_i(HJ)^†HJ=h_i(HJ)^†HJJ_i^†J_i，将等式两边同时减去(HJ)^†HJ可以得到：

$ \left(\boldsymbol{I}_n-h_i \boldsymbol{J}_i^{\dagger} \boldsymbol{J}_i\right)(\boldsymbol{H} \boldsymbol{J})^{\dagger} \boldsymbol{H} \boldsymbol{J}=(\boldsymbol{H} \boldsymbol{J})^{\dagger} \boldsymbol{H} \boldsymbol{J}\left(\boldsymbol{I}_n-h_i \boldsymbol{J}_i^{\dagger} \boldsymbol{J}_i\right)。$

(13)

$ \prod_{i=1}^k\left(\boldsymbol{I}_n-h_i \boldsymbol{J}_i^{\dagger} \boldsymbol{J}_i\right)(\boldsymbol{H} \boldsymbol{J})^{\dagger} \boldsymbol{H} \boldsymbol{J}$的二范数为‖$ \prod_{i=1}^k\left(\boldsymbol{I}_n-h_i \boldsymbol{J}_i^{\dagger} \boldsymbol{J}_i\right)(\boldsymbol{H} \boldsymbol{J})^{\dagger} \boldsymbol{H} \boldsymbol{J}$‖，根据‖I_n－h_iJ_i^†J_i‖=1和‖(HJ)^†HJ‖=1可知，‖$ \prod_{i=1}^k\left(\boldsymbol{I}_n-h_i \boldsymbol{J}_i^{\dagger} \boldsymbol{J}_i\right)(\boldsymbol{H} \boldsymbol{J})^{\dagger} \boldsymbol{H} \boldsymbol{J}$‖≤ $ \prod\nolimits_{i=1}^k {\left\| {\left( {{\mathit{\boldsymbol{I}}_n} - {h_i}\mathit{\boldsymbol{J}}_i^\dagger {\mathit{\boldsymbol{J}}_i}} \right)} \right\|} \left\| {{{(\mathit{\boldsymbol{HJ}})}^\dagger }\mathit{\boldsymbol{HJ}}} \right\|$=1。

采用反证法证明‖$ \prod_{i=1}^k\left(\boldsymbol{I}_n-h_i \boldsymbol{J}_i^{\dagger} \boldsymbol{J}_i\right)(\boldsymbol{H} \boldsymbol{J})^{\dagger} \boldsymbol{H} \boldsymbol{J}$‖≠1。假设‖$ \prod_{i=1}^k\left(\boldsymbol{I}_n-h_i \boldsymbol{J}_i^{\dagger} \boldsymbol{J}_i\right)(\boldsymbol{H} \boldsymbol{J})^{\dagger} \boldsymbol{H} \boldsymbol{J}$‖=1成立，则对于任意非零向量z有$|| \prod_{i=1}^k\left(\boldsymbol{I}_n-h_i \boldsymbol{J}_i^{\dagger} \boldsymbol{J}_i\right)(\boldsymbol{H} \boldsymbol{J})^{\dagger} \boldsymbol{H} \boldsymbol{Jz}||$=‖z‖成立。

对于任意非零向量z，有‖z‖²=‖(I_n－h_iJ_i^†J_i)z +h_iJ_i^†J_iz‖²，又因为(I_n－J_i^†J_i)J_i^†J_i=0。所以‖z‖²=‖(I_n－h_iJ_i^†J_i)z‖²+‖h_iJ_i^†J_iz‖²。当且仅当z∈N(h_iJ_i)时，有‖(I_n－h_iJ_i^†J_i)z‖=‖z‖。当且仅当z∈N(h_iJ_i)^⊥时，有‖h_iJ_i^†J_iz‖=‖z‖。由于(I_n－h_iJ_i^†J_i)(HJ)^†HJ=(HJ)^†HJ(I_n－h_iJ_i^†J_i)，那么对于‖(I_n－h_iJ_i^†J_i)(HJ)^†HJz‖，当且仅当z∈N(h_iJ_i)∩N(HJ)^⊥时，有‖(I_n－h_iJ_i^†J_i)(HJ)^†HJz‖=‖z‖。同理，当且仅当z∈N(h₁J₁) ∩N(h₂J₂)∩…∩N(h_kJ_k)∩N(HJ)^⊥时，有$ \left\|\prod_{i=1}^k\left(\boldsymbol{I}_n-h_i \boldsymbol{J}_i^{\dagger} \boldsymbol{J}_i\right)(\boldsymbol{H} \boldsymbol{J})^{\dagger} \boldsymbol{H} \boldsymbol{J} \boldsymbol{z}\right\|=\|\boldsymbol{z}\|$。又由于，N(h₁J₁) ∩N(h₂J₂)∩…∩ N(h_kJ_k)=N(HJ)，而N(HJ) ∩ N(HJ)^⊥=$ \emptyset $。因此，不存在非零向量z使‖$ \prod_{i=1}^k\left(\boldsymbol{I}_n-h_i \boldsymbol{J}_i^{\dagger} \boldsymbol{J}_i\right)(\boldsymbol{H} \boldsymbol{J})^{\dagger} \boldsymbol{H} \boldsymbol{Jz}$‖=‖z‖成立。$ \prod_{i=1}^k\left(\boldsymbol{I}_n-h_i \boldsymbol{J}_i^{\dagger} \boldsymbol{J}_i\right)(\boldsymbol{H} \boldsymbol{J})^{\dagger} \boldsymbol{H} \boldsymbol{J}$‖=1不成立，假设为假。最终得到：

$ \left\|\prod\nolimits_{i=1}^k\left(\boldsymbol{I}_n-h_i \boldsymbol{J}_i^{\dagger} \boldsymbol{J}_i\right)(\boldsymbol{H} \boldsymbol{J})^{\dagger} \boldsymbol{H} \boldsymbol{J}\right\|<1 。$

(14)

根据式(12)可知(I_n－(HJ)^†HJ)(I_n－h_iJ_i^†J_i)=(I_n－(HJ)^†HJ)成立。P_NH可以变换如下：

$ \begin{aligned} \boldsymbol{P}_{\mathrm{NH}}=&\left(\prod\nolimits_{i=1}^k\left(\boldsymbol{I}_n-h_i \boldsymbol{J}_{\boldsymbol{i}}^{\dagger} \boldsymbol{J}_i\right)\right)^{\mathrm{N}}=\\ &\left(\boldsymbol{I}_n-(\boldsymbol{H} \boldsymbol{J})^{\dagger} \boldsymbol{H} \boldsymbol{J}\right)\left(\prod\nolimits_{i=1}^k\left(\boldsymbol{I}_n-h_i \boldsymbol{J}_i^{\dagger} \boldsymbol{J}_i\right)\right)^{\mathrm{N}}+(\boldsymbol{H} \boldsymbol{J})^{\dagger} \boldsymbol{H} \boldsymbol{J}\left(\prod\nolimits_{i=1}^k\left(\boldsymbol{I}_n-h_i \boldsymbol{J}_i^{\dagger} \boldsymbol{J}_i\right)\right)^N=\\ &\left(\boldsymbol{I}_n-(\boldsymbol{H} \boldsymbol{J})^{\dagger} \boldsymbol{H} \boldsymbol{J}\right)+(\boldsymbol{H} \boldsymbol{J})^{\dagger} \boldsymbol{H} \boldsymbol{J}\left(\prod\nolimits_{i=1}^k\left(\boldsymbol{I}_n-h_i \boldsymbol{J}_i^{\dagger} \boldsymbol{J}_i\right)\right)^N 。\end{aligned} $

(15)

根据式(13)，又由于(HJ)^†HJ(HJ)^†=(HJ)^†，可以得到：

$ \begin{aligned} \boldsymbol{P}_{\mathrm{NH}}=&\left(\prod\nolimits_{i=1}^k\left(\boldsymbol{I}_n-h_i \boldsymbol{J}_i^{\dagger} \boldsymbol{J}_i\right)\right)^N=\\ &\left(\boldsymbol{I}_n-(\boldsymbol{H} \boldsymbol{J})^{\dagger} \boldsymbol{H} \boldsymbol{J}\right)+\left(\prod\nolimits_{i=1}^k\left(\boldsymbol{I}_n-h_i \boldsymbol{J}_i^{\dagger} \boldsymbol{J}_i\right)\right)^N(\boldsymbol{H} \boldsymbol{J})^{\dagger} \boldsymbol{H} \boldsymbol{J}=\\ &\left(\boldsymbol{I}_n-(\boldsymbol{H} \boldsymbol{J})^{\dagger} \boldsymbol{H} \boldsymbol{J}\right)+\left(\prod\nolimits_{i=1}^k\left(\boldsymbol{I}_n-h_i \boldsymbol{J}_i^{\dagger} \boldsymbol{J}_i\right)(\boldsymbol{H} \boldsymbol{J})^{\dagger} \boldsymbol{H} \boldsymbol{J}\right)^N 。\end{aligned} $

(16)

根据$ \left\|\prod_{i=1}^k\left(\boldsymbol{I}_n-h_i \boldsymbol{J}_i^{\dagger} \boldsymbol{J}_i\right)(\boldsymbol{H} \boldsymbol{J})^{\dagger} \boldsymbol{H} \boldsymbol{J}\right\|<1$可知，当迭代次数逐渐增大时，连续迭代投影算子P_NH趋近于(I－(HJ)^†HJ)。

综上所述，连续迭代投影算子P_NH的连续性和激活因子h_i和迭代次数N有关。迭代次数N可以保证连续迭代投影算子的连续性。然而，当迭代次数趋于无穷大时，逐渐接近(I－(HJ)^†HJ)，连续迭代投影算子出现不连续现象。

2.2 多任务连续迭代投影优先级方程

基于连续迭代投影算子，类似于式(8)，构建连续迭代投影多任务优先级方程如下：

$ \boldsymbol{P}_{\mathrm{NH}}^0=\boldsymbol{I}_n, \dot{\boldsymbol{q}}_0=0, \dot{\boldsymbol{q}}_i=\dot{\boldsymbol{q}}_{i-1}+\boldsymbol{P}_{\mathrm{NH}}^{i-1}\left(\boldsymbol{I}_n-\boldsymbol{P}_{\mathrm{NH}}^i\right)\left(\boldsymbol{J}_i \boldsymbol{P}_{\mathrm{NH}}^{i-1}\right)^{\dagger} \dot{\boldsymbol{x}}_{i, \mathrm{u}} 。$

(17)

为了说明该方法保证任务优先级层次有效性的条件，需要对单个任务的雅克比矩阵J_i与多任务优先级层次中连续迭代投影算子P_NH，以及单个任务的雅克比矩阵J_i与前i层次任务连续迭代投影算子P_NHⁱ的关系进行讨论。

当任务i的激活因子h_i=1时，根据式(16)可知：

$ \boldsymbol{J}_i \boldsymbol{P}_{\mathrm{NH}}=\boldsymbol{J}_i\left(\left(\boldsymbol{I}_n-(\boldsymbol{H} \boldsymbol{J})^{\dagger} \boldsymbol{H} \boldsymbol{J}\right)+\left(\prod\nolimits_{i=1}^k\left(\boldsymbol{I}_n-h_i \boldsymbol{J}_i^{\dagger} \boldsymbol{J}_i\right)(\boldsymbol{H} \boldsymbol{J})^{\dagger} \boldsymbol{H} \boldsymbol{J}\right)^N\right) 。$

(18)

对于雅克比矩阵J_i有J_i(I_n－(HJ)^†HJ)=0，式(18)可以变换为：

$ \boldsymbol{J}_i \boldsymbol{P}_{\mathrm{NH}}=\boldsymbol{J}_i\left(\prod\nolimits_{i=1}^k\left(\boldsymbol{I}_n-h_i \boldsymbol{J}_i^{\dagger} \boldsymbol{J}_i\right)(\boldsymbol{H} \boldsymbol{J})^{\dagger} \boldsymbol{H} \boldsymbol{J}\right)^N 。$

(19)

因此，

$ \begin{gathered} \left\|\boldsymbol{J}_i \boldsymbol{P}_{\mathrm{NH}}\right\|=\left\|\boldsymbol{J}_i\left(\prod\nolimits_{i=1}^k\left(\boldsymbol{I}_n-h_i \boldsymbol{J}_i^{\dagger} \boldsymbol{J}_i\right)(\boldsymbol{H})^{\dagger} \boldsymbol{H}\right)^N\right\| \leqslant \\ \left\|\boldsymbol{J}_i\right\| \cdot\left\|\left(\prod\nolimits_{i=1}^k\left(\boldsymbol{I}_n-h_i \boldsymbol{J}_i^{\dagger} \boldsymbol{J}_i\right)(\boldsymbol{H} \boldsymbol{J})^{\dagger} \boldsymbol{H} \boldsymbol{J}\right)^N\right\|=\\ \left\|\boldsymbol{J}_i\right\| \cdot\left\|\left(\prod\nolimits_{i=1}^k\left(\boldsymbol{I}_n-h_i \boldsymbol{J}_i^{\dagger} \boldsymbol{J}_i\right)(\boldsymbol{H} \boldsymbol{J})^{\dagger} \boldsymbol{H} \boldsymbol{J}\right)\right\|^N。\end{gathered} $

(20)

根据$ \left\|\left(\prod_{i=1}^k\left(\boldsymbol{I}_n-h_i \boldsymbol{J}_i^{\dagger} \boldsymbol{J}_i\right)(\tilde{\boldsymbol{H}} \boldsymbol{J})^{\dagger} \tilde{\boldsymbol{H}} \boldsymbol{J}\right)\right\|<1$。所以有$ \mathop {\lim }\limits_{N \to \infty } {\mathit{\boldsymbol{J}}_i}{\mathit{\boldsymbol{P}}_{{\rm{NH}}}}$=0。

可以得到，对于任务i，如果h_i=1，当N→∞，有$ \mathop {\lim }\limits_{N \to \infty } {\mathit{\boldsymbol{J}}_i}{\mathit{\boldsymbol{P}}_{{\rm{NH}}}}$=0。同样地，对于任务i≤j，如果h_i=1，当N→∞，有$ \mathop {\lim }\limits_{N \to \infty } {\mathit{\boldsymbol{J}}_i}\mathit{\boldsymbol{P}}_{{\rm{NH}}}^i$=0。

当N→∞时，有P_NHⁱ=(I_n－(H_iJ_i^A)^†H_iJ_i^A)，从而J_iP_NH^i－1P_NHⁱ=0，P_NHⁱ是任务J_iP_NH^i－1的连续零空间投影算子。当N→∞时，

$ \boldsymbol{J}_i \boldsymbol{P}_{\mathrm{NH}}^{i-1}\left(\boldsymbol{I}_n-\boldsymbol{P}_{\mathrm{NH}}^i\right)\left(\boldsymbol{J}_i \boldsymbol{P}_{\mathrm{NH}}^{i-1}\right)^{\dagger}=\boldsymbol{J}_i \boldsymbol{P}_{\mathrm{NH}}^{i-1}\left(\boldsymbol{J}_i \boldsymbol{P}_{\mathrm{NH}}^{i-1}\right)^{\dagger}-\boldsymbol{J}_i \boldsymbol{P}_{\mathrm{NH}}^{i-1} \boldsymbol{P}_{\mathrm{NH}}^i\left(\boldsymbol{J}_i \boldsymbol{P}_{\mathrm{NH}}^{i-1}\right)^{\dagger}=\boldsymbol{I}_n 。$

(21)

当式(17)中$ \dot{\boldsymbol{x}}_{i, \mathrm{u}}=\dot{\boldsymbol{x}}_i-\boldsymbol{J}_i \dot{\boldsymbol{q}}_{i-1}$，迭代次数N趋于无穷时，根据$ \mathop {\lim }\limits_{N \to \infty } {\mathit{\boldsymbol{J}}_i}\mathit{\boldsymbol{P}}_{{\rm{NH}}}^i$=0和$ \mathop {\lim }\limits_{N \to \infty } {\mathit{\boldsymbol{J}}_i}\mathit{\boldsymbol{P}}_{{\rm{NH}}}^{i - 1}$(I_n－P_NHⁱ)(J_iP_NH^i－1)^†=I_n，对于处于完全激活状态的任务i－1和任务i分别有：

$ \boldsymbol{J}_{i-1} \dot{\boldsymbol{q}}_i=\boldsymbol{J}_{i-1} \dot{\boldsymbol{q}}_{i-1}+\boldsymbol{J}_{i-1} \boldsymbol{P}_{\mathrm{NH}}^{i-1}\left(\boldsymbol{I}_n-\boldsymbol{P}_{\mathrm{NH}}^i\right)\left(\boldsymbol{J}_i \boldsymbol{P}_{\mathrm{NH}}^{i-1}\right)^{\dagger}\left(\dot{\boldsymbol{x}}_i-\boldsymbol{J}_i \dot{\boldsymbol{q}}_{i-1}\right)=\dot{\boldsymbol{x}}_{i-1}, $

(22)

$ \boldsymbol{J}_i \dot{\boldsymbol{q}}_i=\boldsymbol{J}_i \dot{\boldsymbol{q}}_{i-1}+\boldsymbol{J}_i \boldsymbol{P}_{\mathrm{NH}}^{i-1}\left(\boldsymbol{I}_n-\boldsymbol{P}_{\mathrm{NH}}^i\right)\left(\boldsymbol{J}_i \boldsymbol{P}_{\mathrm{NH}}^{i-1}\right)^{\dagger}\left(\dot{\boldsymbol{x}}_i-\boldsymbol{J}_i \dot{\boldsymbol{q}}_{i-1}\right)=\dot{\boldsymbol{x}}_i。$

(23)

由式(22)(23)可得，对于处于完全激活状态的任务，即h_i=1(i=1, 2, …, k)，迭代次数N趋于无穷时，任务优先级层次有效。

3 李雅普诺夫稳定性分析

基于李雅普诺夫方法，对连续迭代投影算法的稳定性进行分析。为了简单起见，先考虑T₁和T₂两个任务，且选择$ \dot{\boldsymbol{x}}_{1, \mathrm{u}}$为简单的比例控制器如下：

$ \dot{\boldsymbol{x}}_{1, \mathrm{u}}=\boldsymbol{K}_1\left(\boldsymbol{x}_{1, \mathrm{~d}}-\boldsymbol{x}_1\right)=\boldsymbol{K}_1 \boldsymbol{e}_1, $

(24)

式中：x_{1, d}是期望的末端轨迹，K₁是m₁×m₁的对角正定矩阵，e₁是末端位置误差。假设期望的末端执行器速度是零，即$ \dot{\boldsymbol{x}}_{1, \mathrm{~d}}=0$，则有：

$ \dot{\boldsymbol{e}}_1=-\dot{\boldsymbol{x}}_1=-\boldsymbol{J}_1 \dot{\boldsymbol{q}}_{2 }。$

(25)

同样对于任务T₂有：

$ \dot{\boldsymbol{e}}_2=-\dot{\boldsymbol{x}}_2=-\boldsymbol{J}_2 \dot{\boldsymbol{q}}_{2 }。$

(26)

对于任务误差

$ \boldsymbol{e}=\left[\begin{array}{l} \boldsymbol{e}_1 \\ \boldsymbol{e}_2 \end{array}\right], $

(27)

可以得到：

$ \dot{\boldsymbol{e}}=\left[\begin{array}{c} \dot{\boldsymbol{e}}_1 \\ \dot{\boldsymbol{e}}_2 \end{array}\right]=-\left[\begin{array}{l} \boldsymbol{J}_1 \\ \boldsymbol{J}_2 \end{array}\right] \dot{\boldsymbol{q}}_{2 }。$

(28)

根据李雅普诺夫稳定性定理，定义一个关于任务误差e的严格正定连续可微李雅普诺夫函数V(e)，当其时间导数$ \dot{V}(\boldsymbol{e})$是负定时，任务误差e逐渐收敛到零。在此，李雅普诺夫函数V(e)定义如下：

$ V(\boldsymbol{e})=\frac{1}{2} \boldsymbol{e}^{\mathrm{T}} \boldsymbol{e}。$

(29)

V(e)对时间求导可得：

$ \dot{V}=\boldsymbol{e}^{\mathrm{T}} \dot{\boldsymbol{e}}=-\boldsymbol{e}^{\mathrm{T}}\left[\begin{array}{l} \boldsymbol{J}_1 \\ \boldsymbol{J}_2 \end{array}\right] \dot{\boldsymbol{q}}_2 。$

(30)

将式(17)带入式(30)可得：

$ \begin{aligned} \dot{V}=&-\boldsymbol{e}^{\mathrm{T}}\left[\begin{array}{l} \boldsymbol{J}_1 \\ \boldsymbol{J}_2 \end{array}\right]\left(\left(\boldsymbol{I}_n-\boldsymbol{P}_{\mathrm{NH}}^1\right)\left(\boldsymbol{J}_1\right)^{\dagger} \dot{\boldsymbol{x}}_{1, \mathrm{u}}+\boldsymbol{P}_{\mathrm{NH}}^1\left(\boldsymbol{I}_n-\boldsymbol{P}_{\mathrm{NH}}^2\right)\left(\boldsymbol{J}_2 \boldsymbol{P}_{\mathrm{NH}}^1\right)^{\dagger} \dot{\boldsymbol{x}}_{2, \mathrm{u}}\right)=\\ &-\boldsymbol{e}^{\mathrm{T}}\left[\begin{array}{l} \boldsymbol{J}_1 \\ \boldsymbol{J}_2 \end{array}\right]\left(\left(\boldsymbol{I}_n-\boldsymbol{P}_{\mathrm{NH}}^1\right)\left(\boldsymbol{J}_1\right)^{\dagger} \boldsymbol{K}_1 \boldsymbol{e}_1+\boldsymbol{P}_{\mathrm{NH}}^1\left(\boldsymbol{I}_n-\boldsymbol{P}_{\mathrm{NH}}^2\right)\left(\boldsymbol{J}_2 \boldsymbol{P}_{\mathrm{NH}}^1\right)^{\dagger} \boldsymbol{K}_2 \boldsymbol{e}_2\right) 。\end{aligned} $

(31)

可以整合为：

$ \begin{aligned} \dot{V}=&-\boldsymbol{e}^{\mathrm{T}}\left[\begin{array}{llll} \boldsymbol{J}_1\left(\boldsymbol{I}_n-\boldsymbol{P}_{\mathrm{NH}}^1\right)\left(\boldsymbol{J}_1\right)^{\dagger} \boldsymbol{K}_1 & \boldsymbol{J}_1 \boldsymbol{P}_{\mathrm{NH}}^1\left(\boldsymbol{I}_n-\boldsymbol{P}_{\mathrm{NH}}^2\right)\left(\boldsymbol{J}_2 \boldsymbol{P}_{\mathrm{NH}}^1\right)^{\dagger} \boldsymbol{K}_2 \\ \boldsymbol{J}_2\left(\boldsymbol{I}_n-\boldsymbol{P}_{\mathrm{NH}}^1\right)\left(\boldsymbol{J}_1\right)^{\dagger} \boldsymbol{K}_1 & \boldsymbol{J}_2 \boldsymbol{P}_{\mathrm{NH}}^1\left(\boldsymbol{I}_n-\boldsymbol{P}_{\mathrm{NH}}^2\right)\left(\boldsymbol{J}_2 \boldsymbol{P}_{\mathrm{NH}}^1\right)^{\dagger} \boldsymbol{K}_2 \end{array}\right] \boldsymbol{e}=\\ &-\boldsymbol{e}^{\mathrm{T}} \boldsymbol{M} \boldsymbol{e}=\\ &-\boldsymbol{e}^{\mathrm{T}}\left[\begin{array}{ll} \boldsymbol{M}_{11} & \boldsymbol{M}_{12} \\ \boldsymbol{M}_{21} & \boldsymbol{M}_{22} \end{array}\right] \boldsymbol{e} 。\end{aligned} $

(32)

对于M的子矩阵M₁₁有：

$ \boldsymbol{M}_{11}=\boldsymbol{J}_1\left(\boldsymbol{I}_n-\boldsymbol{P}_{\mathrm{NH}}^1\right)\left(\boldsymbol{J}_1\right)^{\dagger} \boldsymbol{K}_1=\boldsymbol{J}_1\left(\boldsymbol{J}_1\right)^{\dagger} \boldsymbol{K}_1-\boldsymbol{J}_1 \boldsymbol{P}_{\mathrm{NH}}^1\left(\boldsymbol{J}_1\right)^{\dagger} \boldsymbol{K}_{1 }。$

(33)

根据i≤j时，有$ \mathop {\lim }\limits_{N \to \infty } {\mathit{\boldsymbol{J}}_i}\mathit{\boldsymbol{P}}_{{\rm{NH}}}^i$=0。可得当连续迭代次数N趋于无穷时，有J₁P_NH¹=0，进一步可得：

$ \boldsymbol{M}_{11}=\boldsymbol{J}_1\left(\boldsymbol{J}_1\right)^{\dagger} \boldsymbol{K}_1=\boldsymbol{K}_{1 }。$

(34)

K₁是对角正定矩阵，因此，子矩阵M₁₁也是正定的。当连续迭代次数N趋于无穷时，对于子矩阵Μ₁₂有：

$ \boldsymbol{M}_{12}=\boldsymbol{J}_1 \boldsymbol{P}_{\mathrm{NH}}^1\left(\boldsymbol{I}_n-\boldsymbol{P}_{\mathrm{NH}}^2\right)\left(\boldsymbol{J}_2 \boldsymbol{P}_{\mathrm{NH}}^1\right)^{\dagger} \boldsymbol{K}_2=0 。$

(35)

对于子矩阵M₂₂有：

$ \begin{aligned} \boldsymbol{M}_{22}=& \boldsymbol{J}_2 \boldsymbol{P}_{\mathrm{NH}}^1\left(\boldsymbol{I}_n-\boldsymbol{P}_{\mathrm{NH}}^2\right)\left(\boldsymbol{J}_2 \boldsymbol{P}_{\mathrm{NH}}^1\right)^{\dagger} \boldsymbol{K}_2=\\ & \boldsymbol{J}_2 \boldsymbol{P}_{\mathrm{NH}}^1\left(\boldsymbol{J}_2 \boldsymbol{P}_{\mathrm{NH}}^1\right)^{\dagger} \boldsymbol{K}_2-\boldsymbol{J}_2 \boldsymbol{P}_{\mathrm{NH}}^1 \boldsymbol{P}_{\mathrm{NH}}^2\left(\boldsymbol{J}_2 \boldsymbol{P}_{\mathrm{NH}}^1\right)^{\dagger} \boldsymbol{K}_2 。\end{aligned} $

(36)

当连续迭代次数N趋于无穷时，有J₂P_NH¹P_NH²=0，因此可得：

$ \boldsymbol{M}_{22}=\boldsymbol{J}_2 \boldsymbol{P}_{\mathrm{NH}}^1\left(\boldsymbol{J}_2 \boldsymbol{P}_{\mathrm{NH}}^1\right)^{\dagger} \boldsymbol{K}_2=\boldsymbol{K}_2, $

(37)

K₂是对角正定矩阵，因此，子矩阵M₂₂也是正定的。

综上所述，只考虑任务T₁和T₂，当连续迭代次数N趋于无穷时，M为下上三角矩阵，且主对角上矩阵为正定矩阵。因此，M为正定矩阵，李雅普诺夫函数对时间的求导$ \dot{V}(\boldsymbol{e})$是严格负定的，任务误差逐渐趋近于零，保证了系统的稳定性。扩展到k个任务，可以得到同样类似的结论。

4 仿真分析 4.1 仿真案例

通过对六自由度平面机械臂进行MATLAB仿真，完成有障碍物的轨迹跟踪任务，从而验证上一节提出的控制方法的有效性。在仿真过程中，设置目标点和障碍物，通过高次多项式对连杆末端轨迹进行规划，并完成对轨迹的跟踪，当连杆靠近障碍物时，激活避障任务。通过仿真，来验证所提出的控制算法的优点及有效性。

六自由度平面机械臂的每个杆长都为10 cm，连杆初始角度设置为q₀=[0° 20° 30° 30° 30° 30°]^T。连杆末端终点在笛卡尔空间的坐标为x_f=[3.5 2.0]^T cm。柱形障碍物的中心点O的坐标为(10, 10) cm，半径为r_o=6.5 cm。在连杆末端对轨迹跟踪的过程中，所有连杆都有与障碍物碰撞的危险，第i个连杆与障碍物的最近距离为：

$ d_i=\sqrt{\left(C_{i x}-O_x\right)^2+\left(C_{i y}-O_y\right)^2}-r_{\mathrm{o}}, $

(38)

式中: C_ix和C_iy为连杆i上距离障碍物最近点C_i的坐标，O_x和O_y为障碍物中心点O的坐标。为了保证连杆i与障碍物不发生碰撞，则连杆i与障碍物的距离d_i大于0。为了避免碰撞，连杆i的避障任务可用公式表示为：

$ \boldsymbol{J}_{{ }_{\mathrm{o}}}^i \dot{\boldsymbol{q}}=\frac{\boldsymbol{D}_i^{\mathrm{T}}}{\left\|\boldsymbol{D}_i\right\|} \cdot \boldsymbol{J}_{\mathrm{c} i} \dot{\boldsymbol{q}}=v_i, $

(39)

式中: i=1, 2, …, 6，J_oⁱ为连杆i的避障任务雅克比矩阵，J_ci为连杆i上最靠近障碍物点C_i的雅可比矩阵，D_i为障碍物中心点O到点C_i的向量。避障任务当连杆远离障碍物时不激活，当连杆靠近障碍物时激活，激活因子设定如下：

$ h_i=\left\{\begin{array}{cc} 0 & d_i>\tilde{d}, \\ f\left(\frac{d_i}{\tilde{d}}\right) & 0 \leqslant d_i \leqslant \tilde{d}, \\ 1 & d_i<0 。\end{array}\right. $

(40)

其中，$ f\left(d_i / \tilde{d}\right)=3\left(d_i / \tilde{d}\right)^2-2\left(d_i / \tilde{d}\right)^3$，当连杆与障碍物的距离小于$ \tilde{d}$时，避障任务被逐渐激活。避障速度v_i为:

$ v_i=h_i v_{\rm{o}} 。$

(41)

在仿真过程中，$ \tilde{d}$=1.5 cm，v_o=9。

仿真过程中，采用连续迭代投影多任务优先级方法式(17)，以避障任务为首要任务，末端轨迹跟踪任务在避障任务的零空间中运行，可以表示为：

$ \dot{\boldsymbol{q}}_{\mathrm{IS}}=\left(\boldsymbol{I}_n-\boldsymbol{P}_{\mathrm{NH}}^1\right)\left(\boldsymbol{J}_{\mathrm{o}}\right)^{\dagger} \boldsymbol{v}+\boldsymbol{P}_{\mathrm{NH}}^1\left(\boldsymbol{I}_n-\boldsymbol{P}_{\mathrm{NH}}^2\right)\left(\boldsymbol{J}_{\mathrm{e}} \boldsymbol{P}_{\mathrm{NH}}^1\right)^{\dagger} \dot{\boldsymbol{x}}_{\mathrm{e}}, $

(42)

式中六个连杆的避障任务的雅克比J_o为：

$ {\mathit{\boldsymbol{J}}_{\rm{o}}}=\left[ {{{\left( {\mathit{\boldsymbol{J}}_{\rm{o}}^1} \right)}^{\rm{T}}}} \right.\;{\left( {\mathit{\boldsymbol{J}}_{\rm{o}}^2} \right)^{\rm{T}}}\;{\left( {\mathit{\boldsymbol{J}}_{\rm{o}}^3} \right)^{\rm{T}}}\;{\left( {\mathit{\boldsymbol{J}}_{\rm{o}}^4} \right)^{\rm{T}}}\;{\left( {\mathit{\boldsymbol{J}}_{\rm{o}}^5} \right)^{\rm{T}}}\;{\left( {\mathit{\boldsymbol{J}}_{\rm{o}}^6} \right)^{\rm{T}}}{]^{\rm{T}}}。$

(43)

避障任务的激活矩阵为Η_o=diag{h₁, …, h₆}，$ \boldsymbol{P}_{\mathrm{NH}}^1=\left(\prod_{i=1}^6\left(\boldsymbol{I}_n-h_i\left(\boldsymbol{J}_0^i\right)^{\dagger} \boldsymbol{J}_0^i\right)\right)^N$，六个连杆的避障速度为v=[v₁, …, v₆]^T。末端轨迹的跟踪任务为：

$ \dot{\boldsymbol{x}}_{\mathrm{e}}=\boldsymbol{K}_{\mathrm{e}} \boldsymbol{e}=\boldsymbol{K}_{\mathrm{e}}\left(\boldsymbol{x}_{\mathrm{d}}-x\right) 。$

(44)

末端轨迹跟踪任务的激活矩阵为H_e=diag{1, 1}，末端轨迹的跟踪任务始终激活，在避障任务的零空间中运行，J_eP_NH¹的连续迭代零空间为：

$ \boldsymbol{P}_{\mathrm{NH}}^2=\left(\prod\nolimits_{i=1}^6\left(\boldsymbol{I}_n-h_i\left(\boldsymbol{J}_{\mathrm{o}}^i\right)^{\dagger} \boldsymbol{J}_{\mathrm{o}}^i\right) \prod\nolimits_{i=1}^2\left(\boldsymbol{I}_n-\left(\boldsymbol{J}_{\mathrm{e}}^i\right)^{\dagger} \boldsymbol{J}_{\mathrm{e}}^i\right)\right)^N。$

(45)

避障任务为首要任务，末端轨迹跟踪任务为次要任务，引入激活矩阵，根据式(8)采用增广雅克比零空间投影多任务优先级方法可以表示为：

$ \dot{\boldsymbol{q}}_{\mathrm{AP}}=\left(\boldsymbol{H}_{\mathrm{o}} \boldsymbol{J}_{\mathrm{o}}\right)^{\dagger} \boldsymbol{H}_{\mathrm{o}} \boldsymbol{v}+\boldsymbol{P}_1\left(\boldsymbol{H}_{\mathrm{e}} \boldsymbol{J}_{\mathrm{e}} \boldsymbol{P}_1\right)^{\dagger} \boldsymbol{H}_{\mathrm{e}} \dot{\boldsymbol{x}}_{\mathrm{e}}, $

(46)

式中：六个连杆避障任务的增广零空间P₁=I_n－(H_oJ_o)^†H_oJ_o。

对于仿真中可能出现的奇异问题，引入了阻尼因子来避免奇异带来的关节速度不连续。对于矩阵A，引入阻尼因子λ时，其伪逆的形式如下：

$ \boldsymbol{A}_{\mathrm{D}}^{\dagger}=\boldsymbol{A}^{\mathrm{T}}\left(\boldsymbol{A} \boldsymbol{A}^{\mathrm{T}}+\lambda^2 \boldsymbol{I}_n\right)^{-1} 。$

(47)

式中阻尼因子λ的设置如下：

$ \lambda^2=\begin{cases}0 & \sigma_{\min } \geqslant \varepsilon, \\ \left(1-\left(\frac{\sigma_{\min }}{\varepsilon}\right)^2\right) \lambda_{\max }^2 & \text { 其他}。\end{cases} $

(48)

式中，ε为避免奇异区域的宽度，λ_max为允许的最大阻尼因子，σ_min表示为矩阵A的最小非零奇异值。式(42)中，对雅克比矩阵J_o和矩阵J_eP_NH¹，取其对应的加入阻尼因子的伪逆。式(46)中，对雅克比矩阵H_oJ_o和矩阵H_eJ_eP₁，取其对应的加入阻尼因子的伪逆。在仿真中，取λ_max²=5×10^-4，ε=$ \sqrt{3}$λ_max²。

在仿真过程中，迭代次数N=10，末端跟踪反馈增益矩阵为K_e=10I_n，控制周期T=5 ms。

4.2 仿真结果

连续迭代投影多任务优先级方法仿真结果如图 1所示。图 1为连续迭代投影多任务优先级方法式(42)仿真结果。图 1(a)展示了连续迭代投影任务优先级算法仿真的运动过程。在初始运动阶段t=[0, 2.9)s时，所有连杆远离柱形障碍物，避障任务的激活因子都为零，自由度用于执行轨迹跟踪任务，连杆末端以直线形式朝着目标点靠近。之后连杆靠近障碍物，连杆6、5、3的避障任务依次激活，激活因子从零连续平滑增大，避障任务具有较高的优先级，得到保证，关节速度的变化也为连续的。由于末端轨迹任务处于次优先级，跟踪任务出现了误差，跟踪任务的误差在运动结束时收敛到了零。

图 2为增广投影多任务优先级方法式(46)的仿真结果。在t=2.9 s之前，连杆末端以直线形式朝着目标点前进，逐渐靠近圆柱形障碍物。在t=2.9 s之后，避障任务逐渐激活，关节速度发生突变震荡，由于阻尼因子的作用，关节速度没有出现不连续。由于避障任务处于较高优先级，在运行过程中避障任务始终得到了保证。但需要注意到的是，增广投影多任务优先级方法采用阻尼因子来保证关节速度和投影算子的连续性，在任务结束时，避障任务不需要执行之后，高优先级的自由度没有得到释放，跟踪任务处于次优先级，跟踪任务误差没有收敛到零。

对比两种方法的仿真结果，可知相比于增广投影多任务优先级方法，连续迭代投影多任务优先级方法能够很好地保证关节速度的连续性。且可以在任务移除后，释放自由度，在冗余度的范围内尽可能地保证低优先级任务。

5 结语

1) 基于连续迭代投影原理，提出了连续迭代投影多任务优先级方法。当任务处于完全激活状态，迭代次数趋于无穷大时，可以保证任务优先级层次的有效性。

2) 根据李雅普诺夫稳定性理论，当连续迭代投影算子的迭代次数趋于无穷大时，证明了所提出的连续迭代投影多任务优先级方法的稳定性。

3) 设置柱形避障物，以避障任务为首要任务，末端轨迹跟踪任务为次要任务。对比增广投影多任务优先级方法，在六连杆的平面机械臂上仿真验证了所提出方法的有效性和稳定性。在仿真过程中，当避障任务不需要考虑时，自由度用于完成末端轨迹跟踪任务；当连杆靠近障碍物时，避障任务分别被激活，避障任务得到保证；在运动结束时，末端轨迹跟踪任务的误差收敛到了零。

4) 在推导过程中多以连续迭代投影算子的迭代次数趋于无穷大为条件。然而，迭代次数较小时，可以保证关节速度的连续性；迭代次数增大时，连续迭代投影多任务优先级方法闭环系统的稳定性更好，关节速度的连续性难以得到保证。迭代次数的折中选取仍是需要深入探讨的问题。