CN111633656B - 一种空间多机器人协同操作的分布式能量均衡方法 - Google Patents

Abstract

本发明涉及一种空间多机器人协同操作的分布式能量均衡方法，在多控制单元控制的航天系统中，执行大角度姿态机动或长时间受到非零干扰力矩的累积作用等情况下，仅采用力矩控制单元可能导致飞轮角动量的饱和，因此需要借助推力器等提供外力矩，实现飞轮角动量卸载。传统方法采用集中式方法，利用中心控制器同时控制反作用飞轮和推力器实现飞轮去饱和控制。分布式多控制单元航天器系统中，控制单元均为分布式连接，采用集中式方法会导致中心控制单元的通信量的增加。本发明采用去中心化的分布式方法，可以避免对中心控制单元的依赖，提升了系统灵活性。

Description

一种空间多机器人协同操作的分布式能量均衡方法

技术领域

本发明属于航天器控制领域，涉及一种空间多机器人协同操作的分布式能量均衡方法，具体涉及多个控制单元，其中包含推力控制单元和力矩控制单元两类，各控制单元利用分布式信息交互实现其中的反作用飞轮的角动量卸载。

背景技术

在大型失效航天器在轨服务和多细胞航天器协同控制中，需要用到多个控制单元，这些控制单元可以是执行空间在轨服务的不同空间机器人，也可以是空间细胞机器人系统中的细胞。当多个控制单元组成的组合系统受到长时间的单向干扰力矩或者大角度姿态机动的时候，可能会导致系统中反作用飞轮长时间/快速单向加速达到动量饱和状态，为了恢复反作用飞轮的功能，需要借助推力器、系绳等其他外力控制机构，将反作用飞轮的动量进行卸载。不同与传统航天器集中式的控制结构，多控制单元对同一目标/组合体进行协同空间操作任务中，控制单元之间是分布式信息交互。为提升分布式系统的灵活性，需要适应系统的分布式特性，设计分布式角动量卸载方法。

为了解决分布式多控制单元中反作用飞轮角动量卸载问题，本发明给出了一种分布式协商飞轮角动量卸载方法，利用控制单元之间的信息交互，实现系统中反作用飞轮的角动量卸载。该方法适用于控制单元之间不同的网络状连接拓扑，避免了中心节点的需求，可以灵活增加或减少系统中的控制单元数量及其连接构型。

发明内容

要解决的技术问题

为了避免现有技术的不足之处，本发明提出一种空间多机器人协同操作的分布式能量均衡方法，实现空间分布式控制系统中反作用飞轮的角动量卸载，即通过空间分布式控制系统中多个控制单元的分布式信息交互，实现控制单元角动量卸载需求和外力控制单元执行能力的匹配。

技术方案

一种空间多机器人协同操作的分布式能量均衡方法，其特征在于：系统中的控制单元分为推力控制单元和力矩控制单元，其中，系统中推力控制单元数量为m，每个单元包含推力器N个；力矩控制单元数量为n，每个力矩控制单元数量包含3个正交分布的反作用飞轮；采用列表L_f＝[ID_f1,ID_f2,...,ID_fm]，L_t＝[ID_t1,ID_t2,...,ID_tn]表示推力控制单元和力矩控制单元的ID值；方法步骤如下：

步骤1、初始化下列数据：

控制单元ID_i∈L_f或ID_i∈L_t，标记列表x_i＝0_n×1；任务列表T_i＝0_4×n；任务队列p_i为空队列；评估列表y_i＝0_n×1，归属列表z_i＝0_n×1；任务反馈数据a_i＝0_3×n；本地输出数据b_i＝0_N×n；数据转发记录c_i＝0_n×1，信息交互计数u_i＝0；

同步开始记时s_i＝0；

步骤2：力矩控制单元角动量饱和检测：

设去饱和控制阈值为r_lim，其中r_lim∈(0 1)

力矩控制单元ID_j∈L_t，读取其中所有反作用飞轮的转速r_j＝[r_jx r_jy r_jz]^T，其中r_jx,r_jy,r_jz分别为其x、y、z轴反作用飞轮的转速；

若|r_jz|，|r_jy|，|r_jz|三项中任意一项大于r_lim，则令力矩控制单元ID_j的角动量饱和信息为：

反之，则令

t_j的第一个元素ID_j表示t_j所对应的信息来源

然后，将角动量饱和信息t_j赋值给T_j第j列,即令T_j(:,j)＝t_j，然后将t_j发送到所有相邻控制单元，并令c_jj＝1；

步骤3、角动量饱和信息交互：

所有控制单元均参与角动量饱和信息的交互，根据控制单元ID_k的类型和其收到的角动量饱和信息的不同可以分为以下两种情况：

情况1：若控制单元的标识ID_k∈L_t，则其为力矩控制单元，所处理的信息为相邻控制单元α发送的角动量饱和信息t_β，则进行下述操作：

读取t_β中的第一个元素，确定角动量饱和信息t_β的来源执行单元的ID_β＝t_β1，根据ID_β检查对应的c_kβ是否为0，若c_kβ＝0则将t_β赋值给T_k第β列,即令T_k(:,β)＝t_β，并将t_β转发到所有相邻控制单元，并令c_kβ＝1；若c_kβ＝1则忽略；

情况2：若控制单元的标识ID_k∈L_f，则其为推力控制单元，所处理的信息为相邻控制单元γ发送的角动量饱和信息t_δ，则进行下述操作：

读取t_δ中的第一个元素，确定角动量饱和信息t_δ的来源执行单元的ID_δ＝t_δ1，根据读取到的ID_δ检查对应的c_kδ是否为0。若c_kδ＝0则将t_δ赋值给T_k的第δ列，即令T_k(:,δ)＝t_δ，将t_δ转发到所有相邻控制单元，并令c_kδ＝1；若c_kδ＝1则忽略；

循环本步骤，直至记时s_k≥s_lim，其中s_lim为算法设置值；

步骤4、推力控制单元计算评估数据：

对于以推力控制单元ID_p∈L_f：

(1)令q＝1

(2)判断x_pq值，若x_pq＝1，则进行步骤(3),反之继续；

设推力控制单元ID_p的力矩控制效率矩阵为B_p，由推力器安装位置和方向相关，为航天器控制领域通用技术；

读取T_p的第q列，得到数据t_q,t_q的第2至4个元素为执行单元ID_q的反作用飞轮转速r_q；

记r_q的3个元素中的绝对值最大的为第v个元素，通过求解下式，得到推力控制单元p对于r_q的最优推力输出f_pq

其中τ_max为力矩执行单元的最大输出力矩，由其反作用飞轮选型参数决定；τ_pq＝B_pf_pq；

若无满足约束的解，则令x_pq＝0，y_pq＝-1，并转向步骤(3)，反之继续；

计算评估数据

函数g(x)为任意单调递增函数，且g(0)＝0；

所述系数

对比记录的y_pq与计算得到的

若

则令x_pq＝1，

z_pq＝ID_p，

b_pq＝f_pq；若

则令x_pq＝0；若

则对比z_pq与ID_p，若ID_p＜z_pq，则令x_pq＝1，

z_pq＝ID_p，

b_pq＝f_pq，反之则令x_pq＝0；

其中

表示队列尾部添加操作；

(3)若q＜n，则令q＝q+1，返回步骤(2)；反之继续步骤5；

步骤5、评估数据协调：

所有控制单元之间进行数据交互,当控制单元ID_ε，接收到控制单元的ID_η的z_η和y_η，a_η，然后进行下述处理：

(a)令ζ＝1

(b)对比z_ηζ和z_εζ，若z_ηζ＝ID_η，进行步骤(c1)；若z_ηζ＝ID_ε则进行步骤(d1)；若z_ηζ＝ID_θ，

则进行步骤(e1)；若z_ηζ＝0，则进行步骤(f1)；

(c1)判断z_εζ，若z_εζ＝ID_η或z_εζ＝0，则进行步骤(g)；反之进行步骤(c2)

(c2)判断z_εζ，若z_εζ＝ID_ε或z_εζ＝ID_p,

则进一步判断y_ηζ和y_εζ，若y_ηζ＞y_εζ，则进行步骤(g)；反之跳转步骤(i)；

(d1)判断z_εζ，若z_εζ＝ID_ε或z_εζ＝0，则进行步骤(i)，反之进行步骤(d2)；

(d2)判断z_εζ，若z_εζ＝ID_η，则进行步骤(h)，否则进行步骤(d3)

(d3)判断z_εζ，若z_εζ＝ID_ι，

则进一步判断y_ηζ和y_εζ，若y_ηζ＞y_εζ，则进行步骤(h)，否则直接进行步骤(i)

(e1)判断z_εζ，若z_εζ＝ID_η或z_εζ＝0，则进行步骤(g)，否则进行步骤(e2)

(e2)判断z_εζ，若z_εζ＝ID_ε或z_εζ＝ID_κ,

或z_εζ＝ID_λ,

则进一步判断y_ηζ和y_εζ，若y_ηζ＞y_εζ,则进行步骤(g)，否则直接进行步骤(i)

(f1)判断z_εζ，若z_εζ＝ID_η，则进行更新，否则进行步骤(i)

(g)执行赋值操作x_εζ＝0，y_εζ＝y_ηζ，z_εζ＝z_ηζ，a_εζ＝a_ηζ，查找p_ε，找到元素ID_ζ，删除元素ID_ζ,读取p_ε中ID_ζ之后的所有ID_π，删除ID_π并令对应x_επ＝0，y_επ＝0，z_επ＝0，a_επ＝0，然后跳转步骤(i)；

(h)执行赋值操作y_εζ＝0，z_εζ＝0，a_εζ＝0，查找p_ε，找到元素ID_ζ，删除元素ID_ζ,读取p_ε中ID_ζ之后的所有

删除

并令对应

然后跳转步骤(i)；

(i)若ζ＝N则进行步骤(j)，反之令ζ＝ζ+1，返回步骤(b)；

(j)令计数u_ε＝u_ε+1，若u_ε＜u_lim则进行步骤五，反之进行步骤六；其中u_lim为交互数限制，设置u_lim∈[10 1000]；

步骤6、控制输出：

所有力矩控制单元输出力矩，ID_ξ∈L_t,ξ＝1,2,...,n，输出力矩τ_ξ为a_ξ的第ξ列；

所有推力控制单元输出推力，ID_ψ∈L_f,ψ＝1,2,...,m，输出推力为

所述s_lim取值范围为s_lim＞DΔt，其中D为控制单元连接拓扑无向图的直径，Δt为控制单元之间的信息交互间隔，二者都有系统设计时即确定。

有益效果

本发明提出的一种空间多机器人协同操作的分布式能量均衡方法，在多控制单元控制的航天系统中，执行大角度姿态机动或长时间受到非零干扰力矩的累积作用等情况下，仅采用力矩控制单元可能导致飞轮角动量的饱和，因此需要借助推力器等提供外力矩，实现飞轮角动量卸载。传统方法采用集中式方法，利用中心控制器同时控制反作用飞轮和推力器实现飞轮去饱和控制。分布式多控制单元航天器系统中，控制单元均为分布式连接，采用集中式方法会导致中心控制单元的通信量的增加。本发明采用去中心化的分布式方法，可以避免对中心控制单元的依赖，提升了系统灵活性。

附图说明

图1：角动量饱和信息计算过程流程图

具体实施方式

现结合实施例、附图对本发明作进一步描述：

本发明实现空间分布式控制系统中反作用飞轮的角动量卸载，即通过空间分布式控制系统中多个控制单元的分布式信息交互，实现控制单元角动量卸载需求和外力控制单元执行能力的匹配。

系统中的控制单元分为两类，推力控制单元和力矩控制单元。其中，系统中推力控制单元数量为m，每个单元包含推力器N个；力矩控制单元数量为n，每个力矩控制单元数量包含3个正交分布的反作用飞轮。所有控制单元在网络中都有唯一标识ID值，其中，分别用列表L_f＝[ID_f1,ID_f2,...,ID_fm]，L_t＝[ID_t1,ID_t2,...,ID_tn]表示推力控制单元和力矩控制单元的ID值。

为实现上述目的，本发明所采用的技术方案包括以下步骤：

步骤一：本地参数初始化

各个控制单元分别初始化数据，控制单元的ID_i∈L_t或者ID_i∈L_f，初始化下列数据:

标记列表x_i＝0_n×1；任务列表T_i＝0_4×n；任务队列p_i为空队列；评估列表y_i＝0_n×1，归属列表z_i＝0_n×1；任务反馈数据a_i＝0_3×n；本地输出数据b_i＝0_N×n；数据转发记录c_i＝0_n×1，信息交互计数u_i＝0；

同步开始记时s_i＝0。

步骤二：力矩控制单元角动量饱和检测

各个力矩控制单元监控自身每个飞轮的角动量，设去饱和控制阈值为r_lim，其中r_lim∈(0 1)。

力矩控制单元ID_j∈L_t，读取其中所有反作用飞轮的转速r_j＝[r_jx r_jy r_jz]^T，其中r_jx,r_jy,r_jz分别为其x、y、z轴反作用飞轮的转速。

若|r_jz|，|r_jy|，|r_jz|三项中任意一项大于r_lim，则令力矩控制单元ID_j的角动量饱和信息为

反之，则令

t_j的第一个元素ID_j表示t_j所对应的信息来源。

然后，将角动量饱和信息t_j赋值给T_j第j列,即令T_j(:,j)＝t_j，然后将t_j发送到所有相邻控制单元，并令c_jj＝1。

步骤三：角动量饱和信息交互

所有控制单元均参与角动量饱和信息的交互，根据控制单元ID_k的类型和其收到的角动量饱和信息的不同可以分为以下两种情况

情况1：若控制单元的标识ID_k∈L_t，则其为力矩控制单元，所处理的信息为相邻控制单元α发送的角动量饱和信息t_β，则进行下述操作

读取t_β中的第一个元素，确定角动量饱和信息t_β的来源执行单元的ID_β＝t_β1，根据ID_β检查对应的c_kβ是否为0，若c_kβ＝0则将t_β赋值给T_k第β列,即令T_k(:,β)＝t_β，并将t_β转发到所有相邻控制单元，并令c_kβ＝1；若c_kβ＝1则忽略。

情况2：若控制单元的标识ID_k∈L_f，则其为推力控制单元，所处理的信息为相邻控制单元γ发送的角动量饱和信息t_δ，则进行下述操作

读取t_δ中的第一个元素，确定角动量饱和信息t_δ的来源执行单元的ID_δ＝t_δ1，根据读取到的ID_δ检查对应的c_kδ是否为0。若c_kδ＝0则将t_δ赋值给T_k的第δ列，即令T_k(:,δ)＝t_δ，将t_δ转发到所有相邻控制单元，并令c_kδ＝1；若c_kδ＝1则忽略。

继续步骤三，直至记时s_k≥s_lim，其中s_lim为算法设置值，取值范围为s_lim＞DΔt，其中D为控制单元连接拓扑无向图的直径，Δt为控制单元之间的信息交互间隔，二者都有系统设计时即确定。

步骤四：推力控制单元计算评估数据

各推力控制单元进行评估数据计算，对于以推力控制单元ID_p∈L_f：

(1)令q＝1

(2)判断x_pq值，若x_pq＝1，则进行步骤(3),反之继续

设推力控制单元ID_p的力矩控制效率矩阵为B_p，由推力器安装位置和方向相关，为航天器控制领域通用技术。

读取T_p的第q列，得到数据t_q,t_q的第2至4个元素为执行单元ID_q的反作用飞轮转速r_q。

记r_q的3个元素中的绝对值最大的为第v个元素，通过求解下式，得到推力控制单元p对于r_q的最优推力输出f_pq

其中τ_max为力矩执行单元的最大输出力矩，由其反作用飞轮选型参数决定；τ_pq＝B_pf_pq。

若无满足约束的解，则令x_pq＝0，y_pq＝-1，并转向步骤(3)，反之继续。

用下式求解系数μ_pq

根据μ_pq，计算评估数据

函数g(x)为任意单调递增函数，且g(0)＝0。

对比记录的y_pq与计算得到的

若

则令x_pq＝1，

z_pq＝ID_p，

b_pq＝f_pq；若

则令x_pq＝0；若

则对比z_pq与ID_p，若ID_p＜z_pq，则令x_pq＝1，

z_pq＝ID_p，

b_pq＝f_pq，反之则令x_pq＝0。

其中

表示队列尾部添加操作。

(3)若q＜n，则令q＝q+1，返回步骤(2)；反之继续步骤五。

步骤五：评估数据协调

所有控制单元之间进行数据交互,当控制单元ID_ε，接收到控制单元的ID_η的z_η和y_η，a_η。然后进行下述处理

(a)令ζ＝1

(b)对比z_ηζ和z_εζ，若z_ηζ＝ID_η，进行步骤(c1)；若z_ηζ＝ID_ε则进行步骤(d1)；若z_ηζ＝ID_θ，

则进行步骤(e1)；若z_ηζ＝0，则进行步骤(f1)；

(c1)判断z_εζ，若z_εζ＝ID_η或z_εζ＝0，则进行步骤(g)；反之进行步骤(c2)

(c2)判断z_εζ，若z_εζ＝ID_ε或z_εζ＝ID_p,

则进一步判断y_ηζ和y_εζ，若y_ηζ＞y_εζ，则进行步骤(g)；反之跳转步骤(i)；

(d1)判断z_εζ，若z_εζ＝ID_ε或z_εζ＝0，则进行步骤(i)，反之进行步骤(d2)；

(d2)判断z_εζ，若z_εζ＝ID_η，则进行步骤(h)，否则进行步骤(d3)

(d3)判断z_εζ，若z_εζ＝ID_ι，

则进一步判断y_ηζ和y_εζ，若y_ηζ＞y_εζ，则进行步骤(h)，否则直接进行步骤(i)

(e1)判断z_εζ，若z_εζ＝ID_η或z_εζ＝0，则进行步骤(g)，否则进行步骤(e2)

(e2)判断z_εζ，若z_εζ＝ID_ε或z_εζ＝ID_κ,

或z_εζ＝ID_λ,

则进一步判断y_ηζ和y_εζ，若y_ηζ＞y_εζ,则进行步骤(g)，否则直接进行步骤(i)

(f1)判断z_εζ，若z_εζ＝ID_η，则进行更新，否则进行步骤(i)

(g)执行赋值操作x_εζ＝0，y_εζ＝y_ηζ，z_εζ＝z_ηζ，a_εζ＝a_ηζ，查找p_ε，找到元素ID_ζ，删除元素ID_ζ,读取p_ε中ID_ζ之后的所有ID_π，删除ID_π并令对应x_επ＝0，y_επ＝0，z_επ＝0，a_επ＝0，然后跳转步骤(i)；

(h)执行赋值操作y_εζ＝0，z_εζ＝0，a_εζ＝0，查找p_ε，找到元素ID_ζ，删除元素ID_ζ,读取p_ε中ID_ζ之后的所有

删除

并令对应

然后跳转步骤(i)；

(i)若ζ＝N则进行步骤(j)，反之令ζ＝ζ+1，返回步骤(b)；

(j)令计数u_ε＝u_ε+1，若u_ε＜u_lim则进行步骤五，反之进行步骤六；其中u_lim为交互数限制，通常设置u_lim∈[10 1000]。

步骤六：控制输出

所有力矩控制单元输出力矩，ID_ξ∈L_t,ξ＝1,2,...,n，输出力矩τ_ξ为a_ξ的第ξ列；

所有推力控制单元输出推力，ID_ψ∈L_f,ψ＝1,2,...,m，输出推力为

Claims (2)

1.一种空间多机器人协同操作的分布式能量均衡方法，其特征在于：系统中的控制单元分为推力控制单元和力矩控制单元，其中，系统中推力控制单元数量为m，每个单元包含推力器N个；力矩控制单元数量为n，每个力矩控制单元数量包含3个正交分布的反作用飞轮；采用列表L_f＝[ID_f1,ID_f2,...,ID_fm]，L_t＝[ID_t1,ID_t2,...,ID_tn]表示推力控制单元和力矩控制单元的ID值；方法步骤如下：

步骤1、初始化下列数据：

控制单元ID_i∈L_f或ID_i∈L_t，初始化标记列表x_i＝0_n×1；任务列表T_i＝0_4×n；任务队列p_i为空队列；评估列表y_i＝0_n×1，归属列表z_i＝0_n×1；任务反馈数据a_i＝0_3×n；本地输出数据b_i＝0_N×n；数据转发记录c_i＝0_n×1，信息交互计数u_i＝0；

同步开始记时s_i＝0；

步骤2、力矩控制单元角动量饱和检测：

设去饱和控制阈值为r_lim，其中r_lim∈(01)

力矩控制单元ID_j∈L_t，读取其中所有反作用飞轮的转速r_j＝[r_jx r_jy r_jz]^T，其中r_jx,r_jy,r_jz分别为其x、y、z轴反作用飞轮的转速；

若|r_jz|，|r_jy|，|r_jz|三项中任意一项大于r_lim，则令力矩控制单元ID_j的角动量饱和信息为：

反之，则令

t_j的第一个元素ID_j表示t_j所对应的信息来源

然后，将角动量饱和信息t_j赋值给T_j第j列,即令T_j(:,j)＝t_j，然后将t_j发送到所有相邻控制单元，并令c_jj＝1；

步骤3、角动量饱和信息交互：

所有控制单元均参与角动量饱和信息的交互，根据控制单元ID_k的类型和其收到的角动量饱和信息的不同可以分为以下两种情况：

情况1：若控制单元的标识ID_k∈L_t，则其为力矩控制单元，所处理的信息为相邻控制单元α发送的角动量饱和信息t_β，则进行下述操作：

读取t_β中的第一个元素，确定角动量饱和信息t_β的来源执行单元的ID_β＝t_β1，根据ID_β检查对应的c_kβ是否为0，若c_kβ＝0则将t_β赋值给T_k第β列,即令T_k(:,β)＝t_β，并将t_β转发到所有相邻控制单元，并令c_kβ＝1；若c_kβ＝1则忽略；

情况2：若控制单元的标识ID_k∈L_f，则其为推力控制单元，所处理的信息为相邻控制单元γ发送的角动量饱和信息t_δ，则进行下述操作：

读取t_δ中的第一个元素，确定角动量饱和信息t_δ的来源执行单元的ID_δ＝t_δ1，根据读取到的ID_δ检查对应的c_kδ是否为0， 若c_kδ＝0则将t_δ赋值给T_k的第δ列，即令T_k(:,δ)＝t_δ，将t_δ转发到所有相邻控制单元，并令c_kδ＝1；若c_kδ＝1则忽略；

循环本步骤，直至记时s_k≥s_lim，其中s_lim为算法设置值；

步骤4、各推力控制单元计算评估数据：

对于以推力控制单元ID_p∈L_f：

(1)令q＝1

(2)判断x_pq值，若x_pq＝1，则进行步骤(3),反之继续；

设推力控制单元ID_p的力矩控制效率矩阵为B_p，由推力器安装位置和方向相关，为航天器控制领域通用技术；

读取T_p的第q列，得到数据t_q,t_q的第2至4个元素为执行单元ID_q的反作用飞轮转速r_q；

记r_q的3个元素中的绝对值最大的为第v个元素，通过求解下式，得到推力控制单元p对于r_q的最优推力输出f_pq

其中τ_max为力矩执行单元的最大输出力矩，由其反作用飞轮选型参数决定；τ_pq＝B_pf_pq；

若无满足约束的解，则令x_pq＝0，y_pq＝-1，并转向步骤(3)，反之继续；

计算评估数据

函数g(x)为任意单调递增函数，且g(0)＝0；

所述系数

对比记录的y_pq与计算得到的

若

则令x_pq＝1，

z_pq＝ID_p，

b_pq＝f_pq；若

则令x_pq＝0；若

则对比z_pq与ID_p，若ID_p＜z_pq，则令x_pq＝1，

z_pq＝ID_p，

b_pq＝f_pq，反之则令x_pq＝0；

其中

表示队列尾部添加操作；

(3)若q＜n，则令q＝q+1，返回步骤(2)；反之继续步骤5；

步骤5、评估数据协调：

所有控制单元之间进行数据交互,当控制单元ID_ε，接收到控制单元的ID_η的z_η和y_η，a_η，然后进行下述处理：

(a)令ζ＝1

(b)对比z_ηζ和z_εζ，若z_ηζ＝ID_η，进行步骤(c1)；若z_ηζ＝ID_ε则进行步骤(d1)；若z_ηζ＝ID_θ，

则进行步骤(e1)；若z_ηζ＝0，则进行步骤(f1)；

(c1)判断z_εζ，若z_εζ＝ID_η或z_εζ＝0，则进行步骤(g)；反之进行步骤(c2)

(c2)判断z_εζ，若z_εζ＝ID_ε或z_εζ＝ID_p,

则进一步判断y_ηζ和y_εζ，若y_ηζ＞y_εζ，则进行步骤(g)；反之跳转步骤(i)；

(d1)判断z_εζ，若z_εζ＝ID_ε或z_εζ＝0，则进行步骤(i)，反之进行步骤(d2)；

(d2)判断z_εζ，若z_εζ＝ID_η，则进行步骤(h)，否则进行步骤(d3)

(d3)判断z_εζ，若z_εζ＝ID_ι，

则进一步判断y_ηζ和y_εζ，若y_ηζ＞y_εζ，则进行步骤(h)，否则直接进行步骤(i)

(e1)判断z_εζ，若z_εζ＝ID_η或z_εζ＝0，则进行步骤(g)，否则进行步骤(e2)

(e2)判断z_εζ，若z_εζ＝ID_ε或z_εζ＝ID_κ,

或z_εζ＝ID_λ,

则进一步判断y_ηζ和y_εζ，若y_ηζ＞y_εζ,则进行步骤(g)，否则直接进行步骤(i)

(f1)判断z_εζ，若z_εζ＝ID_η，则进行更新，否则进行步骤(i)

(g)执行赋值操作x_εζ＝0，y_εζ＝y_ηζ，z_εζ＝z_ηζ，a_εζ＝a_ηζ，查找p_ε，找到元素ID_ζ，删除元素ID_ζ,读取p_ε中ID_ζ之后的所有ID_π，删除ID_π并令对应x_επ＝0，y_επ＝0，z_επ＝0，a_επ＝0，然后跳转步骤(i)；

(h)执行赋值操作y_εζ＝0，z_εζ＝0，a_εζ＝0，查找p_ε，找到元素ID_ζ，删除元素ID_ζ,读取p_ε中ID_ζ之后的所有

删除

并令对应

然后跳转步骤(i)；

(i)若ζ＝N则进行步骤(j)，反之令ζ＝ζ+1，返回步骤(b)；

(j)令计数u_ε＝u_ε+1，若u_ε＜u_lim则进行步骤五，反之进行步骤六；其中u_lim为交互数限制，设置u_lim∈[10 1000]；

步骤6、控制输出：

所有力矩控制单元输出力矩，ID_ξ∈L_t,ξ＝1,2,...,n，输出力矩τ_ξ为a_ξ的第ξ列；

所有推力控制单元输出推力，ID_ψ∈L_f,ψ＝1,2,...,m，输出推力为

2.根据权利要求1所述空间多机器人协同操作的分布式能量均衡方法，其特征在于：所述s_lim取值范围为s_lim＞DΔt，其中D为控制单元连接拓扑无向图的直径，Δt为控制单元之间的信息交互间隔，二者都有系统设计时即确定。

Images