CN111168684B - 一种空间大型结构在轨装配序列规划方法 - Google Patents

Abstract

本发明提供了一种空间大型结构在轨装配序列规划方法，建立装配机器人的动力学方程，建立多机器人路径协调规划方法，通过分析机器人运动并计算能量消耗，并建立评价函数，采用粒子群算法对装配序列进行寻优。本发明装配过程中，保证各机器人之间不会发生碰撞且能量最优，在装配序列规划完成后，即可给出机器人的运动规划与控制方法，有利于后续任务的进行；约束指标保证了装配的可行性，优化指标考虑到装配过程中机器人移动的路径最短与能耗最低。

Description

一种空间大型结构在轨装配序列规划方法

技术领域

本发明涉及空间大型结构在轨装配领域，尤其是一种装配序列规划的方法。

背景技术

随着航天技术的进步与应用需求的拓展，代表国家科技实力的空间站、空间反射镜、通信天线、太阳能电站、地外基地等空间大型平台和基础设施的建设需求日益迫切，空间飞行器在轨服务和维护的技术与***已列入国家重大发展计划。空间在轨服务可以拓展人类的太空活动，帮助人类进行更广泛，更深入，更创新的空间探索活动。

大型空间结构与我们目前熟悉的卫星、飞船、空间站相比有很大的差别。大型空间结构体积巨大，目前的运载工具都无法满足其运载需求。所以，大型空间结构无法从地面上整体发射，只能各部件运输到太空进行组装。而过去，在太空中都是由空间机械臂配合宇航员出舱完成装配，但是宇航员出舱存在着一定的危险性。随着大型空间结构的体积与复杂性不断增大，机械臂配合宇航员的装配方案会受到局限，并且危险性不断上升，甚至无法完成任务。

随着机器人技术的发展，用空间机器人代替人类完成危险的太空任务将会成为未来的趋势。通过多自主空间机器人协调作业，完成大型空间结构的装配将成为未来构建大型空间桁架结构的主要方法。但是，由于大型空间桁架结构尺寸大，复杂性高，并且在装配过程中需要多机器人协调工作。

目前现有在轨装配序列规划方法有很多。Bonneville使用了遗传算法进行求解，但是其对新空间探索能力有限，容易陷入局部最优。Hong提出了一种具有启发式工作规则的三阶段集成方法，以帮助计划者生成最佳和最有效的装配序列，最后使用反向神经网络方法进行求解。但是当数据不充分时，神经网络就无法进行工作。Cao使用了一种新的免疫算法并应用在装配序列优化中，缩短了计算时间，但是缩短时间影响了求解的精度。Motavall使用模拟退火算法给出了用于序列优化的评价函数，但是算法性能对初始值等参数很敏感，对初值及参数设置提出了很高要求。Yi提出利用萤火虫算法进行序列规划，但是其存在惯性权重没有充分利用目标函数信息、不能更好地控制和约束萤火虫的移动距离的缺点。Guo等人采用分层规划的方法，用蚁群算法进行求解，但是蚁群算法中信息素会随着时间的推移而逐渐挥发，也存在敛速度慢，容易陷入局部最优的缺点。

为了应对机器人装配的情况，Weiwei Wan考虑了机器人的可抓捕性与装配方向，并对可装配性进行定义。但是该方法只考虑了机器人的约束，保证可行性，并没有考虑机器人装配过程中的最优性。Rodríguez I将机器人装配序列规划问题分为两层，逻辑层与物理层。在逻辑层中规划序列，并通过物理层进行检测并反馈，对逻辑层规划结果进行改进，提高机器人装配序列规划效率。但是该方法需要实时反馈状态，对装配序列进行实时优化，计算量较大，对星载计算机提出较高要求。

发明内容

为了克服现有技术的不足，本发明提供一种空间大型结构在轨装配序列规划的方法。本发明考虑了机器人在装配过程中的路径最短与能耗最小，对装配序列评价函数进行了重新定义。本发明考虑更全面，更切合实际。并且具有节约在轨装配能耗，可在规划装配序列的同时，规划出机器人运动的特点。

本发明解决其技术问题所采用的技术方案的步骤如下：

步骤一、首先建立装配机器人的动力学方程；

根据拉格朗日函数推导装配机器人动力学方程，经计算得到n连杆的机械臂动力学方程为：

其中，M(q)为惯量矩阵，q为关节角，

为非线性项，τ为关节力矩，J为雅可比矩阵，F_e为机器人末端力；

对于逆动力学问题，根据每一时刻的q与

求解

在通过积分迭代求得下一时刻的q与

通过正动力学变换求出：

q与

迭代方法为：

其中，t代表t时刻，step代表步长；

攀爬机器人采用对称设计，当末端执行器交换时，只需令q₇＝q₁，q₆＝q₂，q₅＝q₃，q₃＝q₅，q₂＝q₆，q₁＝q₇并带入所建模型中即可；

步骤二、建立多机器人路径协调规划方法；

首先采用图论的方法将机器人运动路径规划建立为无向图的形式，采用A*算法对单个机器人运动路径进行规划，随后进行多机器人协调路径规划，根据实际装配任务要求，采用基于优先级与交通规则约束相结合的协调方法；首先根据装配任务、机器人能力、机器人可靠性等确定各机器人优先级，并根据优先级对机器人进行排序，每个机器人在运动时只需考虑优先级排序在自己之前的机器人；再根据当前各机器人位置，生成当前的连通图，确定机器人下一步可以运动的顶点，规划各机器人路径；若规划得到的机器人在下一时刻经过的路径点与优先级较高的机器人所经过的路径点发生冲突时，则机器人之间就会发生碰撞；此时引入制定的交通规则，即等待策略，即优先级低的机器人在原地等待，优先级高的机器人通过冲突点后，优先级低的机器人再继续运动，通过该点；在下一时刻，更新各机器人位置，并以此更新连通图，重复每个时刻更新机器人的位置，每个时刻机器人运动一步，也就是机器人每运动一步都会更新；

步骤三、分析机器人运动并计算能量消耗；

将装配过程中机器人的运动分为攀爬运动与装配运动，攀爬运动使机器人在大型空间结构上进行移动，装配运动使机器人完成装配单元间的装配；

在规划出关节运动后，将关节空间内的轨迹作为标称轨迹，控制机器人关节角，以达到跟踪标称轨迹的效果；控制的目标是设计反馈控制器，使关节运动q(t)∈Rⁿ跟踪上所规划出的机器人关节运动q_d；采用逆动力学控制，由公式(1)为动力学模型，在进行轨迹规划时，机器人末端无力与力矩，即F_e＝0，设计控制器为：

其中，ν为辅助控制输入：

其中，τ_m为关节控制力矩，

为期望关节角加速度，

为期望关节角速度，q_d为期望关节角，K_VT与K_PT为控制增益矩阵；

求得误差动力学为：

选取满足控制目标的增益矩阵K_VT与K_PT即可保证控制***稳定；

对于装配操作，采用基于力控制的控制方法，使接触力保持稳定，最后根据控制力矩，对能量消耗进行计算，控制方法如下：

将机器人建模为刚体，将机器人接触的环境，即桁架结构的零件建模为柔性体，刚度用K_p表示，当机器人末端与环境接触产生微小位移δx时，所产生的弹性恢复力为：

F＝-K_pδx (7)

其中，F与δx均表示在任务空间坐标系上，刚度矩阵K_p为对角矩阵，表示零件三个方向上的刚度，如要若要维持弹性恢复力，机器人关节力矩应为：

τ＝J^TF (8)

其中，J为机器人雅可比矩阵，表示机器人末端微小位移与关节微小位移的关系：

δx＝Jδq (9)

当机器人末端与环境接触时，由接触引起的环境局部变形由矢量

表示：

当末端与环境接触时；

当末端与环境不接触时；

其中，x为机器人末端位置，x_E为环境位置；

设计机器人x-y平面的位置控制律为：

设计机器人z方向上的柔顺控制律为：

其中，τ_x-y为x-y平面上的控制力矩，τ_z为z方向上的控制力矩，J(q)为雅可比矩阵，K_P与K_D为控制增益矩阵；

机器人的能量消耗为关节驱动能量与电子设备能量之和，而关节驱动能量消耗远远大于电子设备，因此假设机器人消耗的能量只有关节驱动部分；

对于机器人能量计算，采取绝对值形式：

其中，n为机器人关节数，T为机器人运动总时间；

步骤四：建立评价函数，采用粒子群算法对装配序列进行寻优；

与传统的装配序列规划相同，首先衡量装配单元之间的关系：

(1)干涉矩阵

干涉矩阵衡量待装配的装配单元与已经装配完成的装配体部分的冲突情况，假设装配体有n个装配单元，定义干涉矩阵以IM表示，则I_ijk表示装配单元P_j沿着方向k与装配单元P_i发生的干涉情况：

三维空间中定义装配方向为三个线性无关的矢量轴，因此，沿三轴干涉矩阵有六种，分别为IM_+x、IM_-x、IM_+y、IM_-y、IM_+z、IM_-z，干涉的特点已知I_ij(+k)＝I_ji(-k)，即装配单元P_j沿着方向k与装配单元P_i发生的干涉情况与装配单元P_i沿着方向-k与装配单元P_j发生的干涉情况相同；因此，定义集成干涉矩阵描述在整个装配过程中，是否存在装配单元之间的干涉情况：

矩阵IM描述两两零件之间装配时是否会发生干涉的情况；

(2)连接矩阵

不同装配单元之间的装配方法不同，连接矩阵是描述不同装配单元间的连接关系，对于一个由n个装配单元组成的装配体，定义连接矩阵C为：

C＝(c_ij)_n×n (15)

其中，c_ij代表装配单元P_i与装配单元P_j的连接情况：

其中，“不存在连接关系”表示两装配单元之间无任何连接结构；“存在接触连接”表示两装配单元之间有连接关系，但是在连接后无法保持稳定状态，也就是在连接时不施加外力，装配单元之间的贴合，没有紧固的铰接都包含在此；“存在稳定连接”表示装配单元连接后可保持稳定；螺纹连接、焊接等都属于稳定连接；

(3)支撑矩阵

对于空间桁架结构，工作场景为空间或外星球；不同场景的环境不同，重力特性也不同，支撑矩阵衡量在不同环境与重力特性下，装配过程中各装配单元之间是否具有稳定的支撑关系，定义支撑矩阵S为：

S＝(s_ij)_n×n (17)

其中，s_ij代表装配单元P_j对装配单元P_i的支撑关系：

装配的评价指标有装配几何可行性、装配稳定性、装配重定向性、装配聚合性和装配并行度，针对多机器人装配空间桁架任务，提出以下四个装配指标：

(a)装配几何可行性；

装配几何可行性表示装配单元装配时，与已经装配好的装配体不发生干涉的性质；在装配前已经定义了装配方向并求解出不同装配单元的可行装配方向，装配几何性指根据装配序列装配各装配单元时，每一步的装配单元都有可行的装配序列；

装配几何性通过干涉矩阵求解，定义G_k(P_i)为装配单元P_i沿着k方向进行装配时，和已经装配好的装配体之间的干涉值之和；G_k(P_i)决定了待装配单元P_i的可行装配方向，若装配单元P_i与已装配好的装配体在k方向不发生干涉，则G_k(P_i)＝0；若装配单元P_i与已装配好的装配体在k方向发生干涉，则G_k(P_i)≠0，G_k(P_i)具体表达式为：

对于装配序列{P₁,P₂,…,P_n}，设n_g表示在此序列中无法被装配的装配单元，在计算n_g时，对其初始赋值0，对于每一个装配单元计算G_k(P_i)，当G_k(P_i)＝0时，n_g＝n_g；当G_k(P_i)≠0时，n_g＝n_g+1；如果最终计算得到n_g＝0，则装配序列可行；n_g≠0，装配序列不可行；

(b)装配稳定性

装配稳定性表示在装配单元装配上之后，各装配单元保持装配位置与装配状态的能力；装配稳定性与装配连接关系与重力有关，因此通过连接矩阵C与支撑矩阵S求解，c_ij＝1表示装配单元与装配体为接触连接，c_ij＝2表示装配单元与装配体为稳定连接，s_ij＝1表示装配单元P_j对装配单元P_i具有支撑作用；定义连接关系n_c与n_s，对n_c与n_s初始赋值为0，装配序列{P₁,P₂,…,P_n}中，对于每个装配单元计算c_ij与s_ij，更新n_c＝n_c+c_ij，n_s＝n_s+s_ij；n_c越大，装配稳定性越好；n_s越大，装配支撑性越好；并且要保证每一步装配都是稳定的，需要求每一步n_c＞0或n_s＞0；

(c)装配路径

装配路径表示在装配过程中，各机器人所运动的路径长度之和，通过步骤二计算得到；定义装配体的顶点V与边E，对于装配序列{P₁,P₂,…,P_n}，设n_d表示在此装配序列中机器人运动的总路程，首先初始无向图G₀＝(V₀,E₀)，根据多机器人协同路径规划算法计算各机器人运动路径

与路径长度n_d0，装配单元计算完后，根据装配序列计算下一装配单元，更新无向图G＝(V,E)，并计算

n_d＝n_d+n_d0；所有装配单元检测完成后，输出各机器人路径

与机器人运动路径之和n_d；

(d)装配能耗

装配能耗表示在装配过程中，各机器人消耗能量之和，定义为n_e，通过步骤三式(12)计算得到；对于装配序列{P₁,P₂,…,P_n}，在计算完机器人路径

后，根据式(12)能耗计算方法，求解机器人能耗fuel，令n_e＝fule，并输出各机器人运动规划q_d与控制律τ；

对于以上评价指标，进行量化计算，并将不同指标之间使用式(20)进行归一化处理，保证其对整个指标函数影响相同；建立函数模型，不同装配序列求得不同的函数值，准确评价装配序列的优劣；

对于序列{P₁,P₂,…,P_n}，评价函数为：

满足装配序列可行的约束条件为：

其中，n_c代表装配稳定性，n_s代表装配支撑性，n_l代表装配路径，n_e代表装配能耗。ω₁、ω₂、ω₃、ω₄分别代表各指标权重系数，且ω₁+ω₂+ω₃+ω₄＝1，各权重系数可根据不同装配的不同需求制定，而约束条件代表装配序满足的前提条件，即在装配每一步都需要是稳定的且每一步都有可进行装配的方向，即与之间的装配不发生干涉；

在保证公式(21)成立的条件下，找到公式(20)的最小值，即找到最优序列，得到最优的装配序列以及机器人的动作与控制序列。

本发明的有益效果在于：

(1)本发明在装配过程中，考虑到多机器人协调运动的路径规划，给出基于设计优先级与交通规则的多机器人协调路径规划方法，装配过程中，保证各机器人之间不会发生碰撞且能量最优。

(2)本发明对机器人装配过程中的运动进行了分析，将装配过程中的动作分为基于位置控制的攀爬运动与基于力控制的装配运动。这样可在装配序列规划完成后，即可给出机器人的运动规划与控制方法，有利于后续任务的进行。

(3)将装配序列的评价指标分为约束指标与优化指标两类，约束指标保证了装配的可行性，优化指标考虑到装配过程中机器人移动的路径最短与能耗最低。

附图说明

图1是本发明装配机器人的一般模型示意图。

图2是本发明装配的空间结构示意图，图中数字代表装配单元编号。

图3是本发明攀爬运动关节角变化图。

图4是本发明攀爬运动关节控制力矩变化图。

图5是本发明装配运动关节角变化图。

图6是本发明装配运动关节控制力矩变化图。

图7是本发明装配z轴方向接触力变化图。

图8是本发明装配序列寻优适应度函数变化图。

图9是本发明整个装配过程机器人关节角变化图。

图10是本发明整个装配过程机器人关节控制力矩变化图。

图11是本发明整个装配过程机器人能量消耗变化图。

图12是本发明空间大型结构在轨装配序列规划流程图。

具体实施方式

下面结合附图和实施例对本发明进一步说明。

本发明的目的在于解决上述现有技术中的问题，提供一种全新的空间大型结构在轨装配序列规划的方法。在保证装配可行性的前提下，规划装配序列，实现装配序列的最优。

本发明解决其技术问题所采用的技术方案的步骤如下：

步骤一、首先建立装配机器人的动力学方程；

根据拉格朗日函数推导装配机器人动力学方程，经计算得到n连杆的机械臂动力学方程为：

其中，M(q)为惯量矩阵，q为关节角，

为非线性项，τ为关节力矩，J为雅可比矩阵，F_e为机器人末端力；

对于逆动力学问题，根据每一时刻的q与

求解

在通过积分迭代求得下一时刻的q与

通过正动力学变换求出：

q与

迭代方法为：

其中，t代表t时刻，step代表步长；

攀爬机器人采用对称设计，当末端执行器交换时，只需令q₇＝q₁，q₆＝q₂，q₅＝q₃，q₃＝q₅，q₂＝q₆，q₁＝q₇并带入所建模型中即可。

步骤二、建立多机器人路径协调规划方法；

首先采用图论的方法将机器人运动路径规划建立为无向图的形式，采用A*算法对单个机器人运动路径进行规划，随后进行多机器人协调路径规划，根据实际装配任务要求，采用基于优先级与交通规则约束相结合的协调方法；首先根据装配任务、机器人能力、机器人可靠性等确定各机器人优先级，并根据优先级对机器人进行排序，每个机器人在运动时只需考虑优先级排序在自己之前的机器人；再根据当前各机器人位置，生成当前的连通图，确定机器人下一步可以运动的顶点，规划各机器人路径；若规划得到的机器人在下一时刻经过的路径点与优先级较高的机器人所经过的路径点发生冲突时，则机器人之间就会发生碰撞；此时引入制定的交通规则，即等待策略，即优先级低的机器人在原地等待，优先级高的机器人通过冲突点后，优先级低的机器人再继续运动，通过该点；等待策略有利于节省机器人能耗，针对空间任务更适合；在下一时刻，更新各机器人位置，并以此更新连通图，重复每个时刻更新机器人的位置，每个时刻机器人运动一步，也就是机器人每运动一步都会更新。

步骤三、分析机器人运动并计算能量消耗；

将装配过程中机器人的运动分为攀爬运动与装配运动，攀爬运动使机器人在大型空间结构上进行移动，装配运动使机器人完成装配单元间的装配；

在规划出关节运动后，将关节空间内的轨迹作为标称轨迹，控制机器人关节角，以达到跟踪标称轨迹的效果；控制的目标是设计反馈控制器，使关节运动q(t)∈Rⁿ跟踪上所规划出的机器人关节运动q_d；采用逆动力学控制，由公式(1)为动力学模型，在进行轨迹规划时，机器人末端无力与力矩，即F_e＝0，因此设计控制器为：

其中，ν为辅助控制输入：

其中，τ_m为关节控制力矩，

为期望关节角加速度，

为期望关节角速度，q_d为期望关节角，K_VT与K_PT为控制增益矩阵；

求得误差动力学为：

选取满足控制目标的增益矩阵K_VT与K_PT即可保证控制***稳定；

对于装配操作，采用基于力控制的控制方法，使接触力保持稳定，最后根据控制力矩，对能量消耗进行计算，控制方法如下：

将机器人建模为刚体，将机器人接触的环境，即桁架结构的零件建模为柔性体，刚度用K_p表示，当机器人末端与环境接触产生微小位移δx时，所产生的弹性恢复力为：

F＝-K_pδx (7)

其中，F与δx均表示在任务空间坐标系上，刚度矩阵K_p为对角矩阵，表示零件三个方向上的刚度，如要若要维持弹性恢复力，机器人关节力矩应为：

τ＝J^TF (8)

其中，J为机器人雅可比矩阵，表示机器人末端微小位移与关节微小位移的关系：

δx＝Jδq (9)

当机器人末端与环境接触时，由接触引起的环境局部变形由矢量

表示：

当末端与环境接触时；

当末端与环境不接触时；

其中，x为机器人末端位置，x_E为环境位置；

设计机器人x-y平面的位置控制律为：

设计机器人z方向上的柔顺控制律为：

其中，τ_x-y为x-y平面上的控制力矩，τ_z为z方向上的控制力矩，J(q)为雅可比矩阵，K_P与K_D为控制增益矩阵；

机器人的能量消耗为关节驱动能量与电子设备能量之和，而关节驱动能量消耗远远大于电子设备，因此假设机器人消耗的能量只有关节驱动部分；

对于机器人能量计算，采取绝对值形式：

其中，n为机器人关节数，T为机器人运动总时间；

步骤四：建立评价函数，采用粒子群算法对装配序列进行寻优；

与传统的装配序列规划相同，首先衡量装配单元之间的关系：

(1)干涉矩阵

干涉矩阵衡量待装配的装配单元与已经装配完成的装配体部分的冲突情况，假设装配体有n个装配单元，定义干涉矩阵以IM表示，则I_ijk表示装配单元P_j沿着方向k与装配单元P_i发生的干涉情况：

三维空间中定义装配方向为三个线性无关的矢量轴，因此，沿三轴干涉矩阵有六种，分别为IM_+x、IM_-x、IM_+y、IM_-y、IM_+z、IM_-z，干涉的特点已知I_ij(+k)＝I_ji(-k)，即装配单元P_j沿着方向k与装配单元P_i发生的干涉情况与装配单元P_i沿着方向-k与装配单元P_j发生的干涉情况相同；因此，定义集成干涉矩阵描述在整个装配过程中，是否存在装配单元之间的干涉情况：

矩阵IM描述两两零件之间装配时是否会发生干涉的情况；

(2)连接矩阵

不同装配单元之间的装配方法不同，比如：螺纹连接、铆接、焊接、胶接、销连接、键连接、成型连接；连接矩阵是描述不同装配单元间的连接关系，对于一个由n个装配单元组成的装配体，定义连接矩阵C为：

C＝(c_ij)_n×n (15)

其中，c_ij代表装配单元P_i与装配单元P_j的连接情况：

其中，“不存在连接关系”表示两装配单元之间无任何连接结构；“存在接触连接”表示两装配单元之间有连接关系，但是在连接后无法保持稳定状态，也就是在连接时不施加外力，装配单元之间的贴合，没有紧固的铰接都包含在此；“存在稳定连接”表示装配单元连接后可保持稳定；螺纹连接、焊接等都属于稳定连接。

(3)支撑矩阵

对于空间桁架结构，工作场景为空间或外星球；不同场景的环境不同，重力特性也不同，支撑矩阵衡量在不同环境与重力特性下，装配过程中各装配单元之间是否具有稳定的支撑关系，定义支撑矩阵S为：

S＝(s_ij)_n×n (17)

其中，s_ij代表装配单元P_j对装配单元P_i的支撑关系：

本发明装配的评价指标有装配几何可行性、装配稳定性、装配重定向性、装配聚合性和装配并行度，针对多机器人装配空间桁架任务，提出以下四个装配指标：

(a)装配几何可行性；

装配几何可行性表示装配单元装配时，与已经装配好的装配体不发生干涉的性质；在装配前已经定义了装配方向并求解出不同装配单元的可行装配方向，装配几何性指根据装配序列装配各装配单元时，每一步的装配单元都有可行的装配序列；

装配几何性通过干涉矩阵求解，定义G_k(P_i)为装配单元P_i沿着k方向进行装配时，和已经装配好的装配体之间的干涉值之和；G_k(P_i)决定了待装配单元P_i的可行装配方向，若装配单元P_i与已装配好的装配体在k方向不发生干涉，则G_k(P_i)＝0；若装配单元P_i与已装配好的装配体在k方向发生干涉，则G_k(P_i)≠0，G_k(P_i)具体表达式为：

对于装配序列{P₁,P₂,…,P_n}，设n_g表示在此序列中无法被装配的装配单元，在计算n_g时，对其初始赋值0，对于每一个装配单元计算G_k(P_i)，当G_k(P_i)＝0时，n_g＝n_g；当G_k(P_i)≠0时，n_g＝n_g+1；如果最终计算得到n_g＝0，则装配序列可行；n_g≠0，装配序列不可行；

(b)装配稳定性

装配稳定性表示在装配单元装配上之后，各装配单元保持装配位置与装配状态的能力；装配稳定性与装配连接关系与重力有关，因此通过连接矩阵C与支撑矩阵S求解，c_ij＝1表示装配单元与装配体为接触连接，c_ij＝2表示装配单元与装配体为稳定连接，s_ij＝1表示装配单元P_j对装配单元P_i具有支撑作用；定义连接关系n_c与n_s，对n_c与n_s初始赋值为0，装配序列{P₁,P₂,…,P_n}中，对于每个装配单元计算c_ij与s_ij，更新n_c＝n_c+c_ij，n_s＝n_s+s_ij；n_c越大，装配稳定性越好；n_s越大，装配支撑性越好；并且要保证每一步装配都是稳定的，需要求每一步n_c＞0或n_s＞0；

(c)装配路径

装配路径表示在装配过程中，各机器人所运动的路径长度之和，通过步骤二计算得到；定义装配体的顶点V与边E，对于装配序列{P₁,P₂,…,P_n}，设n_d表示在此装配序列中机器人运动的总路程，首先初始无向图G₀＝(V₀,E₀)，根据多机器人协同路径规划算法计算各机器人运动路径

与路径长度n_d0，装配单元计算完后，根据装配序列计算下一装配单元，更新无向图G＝(V,E)，并计算

n_d＝n_d+n_d0；所有装配单元检测完成后，输出各机器人路径

与机器人运动路径之和n_d；显然，n_d越小，机器人行驶总路径越短，装配越优。

(d)装配能耗

装配能耗表示在装配过程中，各机器人消耗能量之和，定义为n_e，通过步骤三式(12)计算得到；对于装配序列{P₁,P₂,…,P_n}，在计算完机器人路径

后，根据式(12)能耗计算方法，求解机器人能耗fuel，令n_e＝fule，并输出各机器人运动规划q_d与控制律τ。显然，n_e越小，机器人消耗总能量越小，对装配有利。

其中装配几何可行性与装配稳定性为约束指标，不满足前两个指标的装配序列无法完成装配，装配路径与装配能耗，指标越小装配序列越优。

对于以上评价指标，进行量化计算，并将不同指标之间使用式(20)进行归一化处理，保证其对整个指标函数影响相同；建立函数模型，不同装配序列求得不同的函数值，准确评价装配序列的优劣。

对于序列{P₁,P₂,…,P_n}，评价函数为：

满足装配序列可行的约束条件为：

其中，n_c代表装配稳定性，n_s代表装配支撑性，n_l代表装配路径，n_e代表装配能耗。ω₁、ω₂、ω₃、ω₄分别代表各指标权重系数，且ω₁+ω₂+ω₃+ω₄＝1，各权重系数可根据不同装配的不同需求制定，而约束条件代表装配序满足的前提条件，即在装配每一步都需要是稳定的且每一步都有可进行装配的方向，即与之间的装配不发生干涉。

在保证公式(21)成立的条件下，找到公式(20)的最小值，即找到最优序列，得到最优的装配序列以及机器人的动作与控制序列。

实施例包括以下四个步骤：

步骤一、首先建立装配机器人的动力学方程。

根据拉格朗日函数推导装配机器人动力学方程，经计算得到n连杆的机械臂动力学方程为：

其中，τ为机械臂末端力矩，T为变换矩阵，n为连杆数，g为重力常数，I_ai为传动装置的等效转动惯量，所述机器人中，n＝6。且由于机器人在空间进行操作，因此g可视为0，简化为：

式中：

写为矩阵形式：

其中，M(θ)为惯量矩阵，θ为关节角，

为非线性项，τ为关节力矩，J为雅可比矩阵，F_e为机器人末端力。

对于逆动力学问题，根据每一时刻的q与

求解

在通过积分迭代求得下一时刻的q与

可通过正动力学变换求出：

q与

迭代方法为：

攀爬机器人采用对称设计，当末端执行器交换时，只需令θ₆＝θ₁，θ₅＝θ₂，θ₄＝θ₃，θ₃＝θ₄，θ₂＝θ₅，θ₁＝θ₆，并带入所建模型中即可。

步骤二、建立多机器人路径协调规划方法。

首先采用图论的方法将机器人运动路径规划建立为无向图的形式，采用A*算法对单个机器人运动路径进行规划。随后进行多机器人协调路径规划。根据实际装配任务要求，采用优先级准则与交通规则相结合的协调方法。首先根据装配任务、机器人能力、机器人可靠性等准确定各机器人优先级，并对机器人优先级进行排序，每个机器人在运动时只需考虑优先级排序在自己之前的机器人。再根据当前各机器人位置，生成当前的连通图，确定机器人下一步可以运动的顶点，规划各机器人路径。若规划得到的机器人路径在下一时刻会经过同一点，则机器人之间就会发生碰撞。此时引入制定的交通规则，即等待策略。等待策略有利于节省机器人能耗，针对空间任务更适合。在此，让优先级高的机器人首先进行运动，而优先级低的机器人保持当前位置，不进行运动。在下一时刻，更新各机器人位置，并以此更新连通图，重复上述过程。

步骤三、分析机器人运动并计算能量消耗

将装配过程中机器人的运动分为攀爬运动与装配运动。攀爬运动使机器人在大型空间结构上进行移动，装配运动使机器人完成结构间的装配。对于攀爬运动，分析了三种运动方式，分别是翻转步态、蠕动步态、滑行步态。对这三种步态进行了运动规划，采用基于位置的控制方法，最后根据控制力拒，对能量消耗的分析，选择能量消耗最小的运动方式。其控制方法如下：

在规划出关节运动后，将关节空间内的轨迹作为标称轨迹，控制机器人关节角，以达到跟踪标称轨迹的效果。控制的主要目标是设计反馈控制器，使关节运动q(t)∈Rⁿ可以跟踪上我们所规划出的机器人关节运动q_d。机器人控制的一种简单设计方法是利用基于***对操作点线性化的线性控制方案，在此控制器采用PD控制的方式，其具体控制律为：

对于装配操作，采用基于力控制的控制方法，使接触力保持稳定，最后根据控制力矩，对能量消耗进行计算。其控制方法如下：

将机器人建模为刚体，将机器人接触的环境，也就是桁架结构的零件，建模为柔性体，其刚度用K_p表示。当机器人末端与环境接触产生微小位移δx时，所产生的弹性恢复力为：

F＝-K_pδx (6)

其中，F与δx均表示在任务空间坐标系上。刚度矩阵K_p常选为对角矩阵，表示零件三个方向上的刚度。如要若要维持弹性恢复力，机器人关节力矩应为：

τ＝J^TF (7)

其中，J为机器人雅可比矩阵，其也可表示机器人末端微小位移与关节微小位移的关系：

δx＝Jδq (8)

当机器人末端与环境接触时，由接触引起的环境局部变形可由矢量

表示：

当末端与环境接触时

当末端与环境不接触时

综上，设计机器人x-y平面的位置控制律为：

设计机器人z方向上的柔顺控制律为：

步骤四：建立评价函数，采用粒子群算法对装配序列进行寻优。

建立如下四个评价指标：

(1)几何可行性：装配几何可行性表示零件装配时，与已经装配好的装配体不发生干涉的性质。

(2)稳定性：装配稳定性表示在零件装配上之后，各零件保持装配位置与装配状态的能力。

(3)装配路径：装配路径表示在装配过程中，各机器人所运动的路径长度之和。

(4)装配能耗：装配能耗表示在装配过程中，各机器人消耗能量之和。

对于以上评价指标，对其进行量化计算。并将不同指标之间归一处理，保证其对整个指标函数影响相同。建立函数模型，不同装配序列求得不同的函数值，准确评价装配序列的优劣。

对于序列{P₁,P₂,…,P_n}，其评价函数为：

满足装配序列可行的约束条件为：

其中，n_c代表装配稳定性，n_s代表装配支撑性，n_l代表装配路径，n_e代表装配能耗。ω₁、ω₂、ω₃、ω₄分别代表各指标权重系数，且ω₁+ω₂+ω₃+ω₄＝1。各权重系数可根据不同装配的不同需求制定。而约束条件代表装配序满足的前提条件，即在装配每一步都需要是稳定的且每一步都有可进行装配的方向，即与之间的装配不发生干涉。

表1装配机器人的运动学与动力学参数

表1为实例中使用的装配机器人***的运动学和动力学参数，图1为装配机器人的一般模型。图2为装配的空间结构示意图。图3与图4展示了攀爬运动下的关节角与关节控制力矩变化，其关节角变化平稳，控制器跟踪误差小。图5与图6展示了装配运动下的关节角与关节控制力矩变化。可看出关节角与关节控制力矩最终趋于稳定。但是关节控制力矩不都为零，这是因为此部分关节力矩用来保持接触力稳定。图7为装配z轴方向接触力变化，图中可看出接触力稳定在2N，完成了装配控制的目标。图8为装配序列寻优适应度函数变化，图中可看出适应度函数不断下降，最后稳定在最小值，寻优过程结束。图9到图11展示了整个装配过程中的关节角、关节控制力矩以及能耗变化。横坐标的一个刻度代表机器人的一个动作，棕色虚线代表末端执行器切换时刻。图中可看出机器人关节角运动曲线平滑，所设计控制律跟踪误差小，每个阶段机器人运动后都回到初始状态。关节控制力矩可用来指导各关节舵机的选择。机器人能耗可用来指导机器人能量储备规划。图12为序列规划整个过程的流程图。

本发明实例装配零件为空间桁架结构，其由9个零件组成，其中包括3个四面体桁架单元与6根杆，零件编号如图2所示。装配机器人为双末端执行器的6自由度机械臂，***的运动学/动力学参数如表1所示。本发明以6自由度机器人装配9零件空间桁架为例，验证提出的空间大型结构在轨装配序列规划的方法。仿真对比了翻滚运动、滑动运动、蠕动运动三种攀爬运动的燃料消耗，选出最优攀爬运动方式。给出了装配运动的控制方法，并计算了燃料消耗。最后采用粒子群算法进行寻优，得到该空间结构的最优装配序列为[1,2,3,6,7,5,4,8,9]。仿真结果验证此技术是可行的。

Claims (1)

1.一种空间大型结构在轨装配序列规划方法，其特征在于包括下述步骤：

步骤一、首先建立装配机器人的动力学方程；

根据拉格朗日函数推导装配机器人动力学方程，经计算得到n连杆的机械臂动力学方程为：

其中，M(q)为惯量矩阵，q为关节角，

为非线性项，τ为关节力矩，J为雅可比矩阵，F_e为机器人末端力；

对于逆动力学问题，根据每一时刻的q与

求解

在通过积分迭代求得下一时刻的q与

通过正动力学变换求出：

q与

迭代方法为：

其中，t代表t时刻，step代表步长；

攀爬机器人采用对称设计，当末端执行器交换时，只需令q₇＝q₁，q₆＝q₂，q₅＝q₃，q₃＝q₅，q₂＝q₆，q₁＝q₇并带入所建模型中即可；

步骤二、建立多机器人路径协调规划方法；

首先采用图论的方法将机器人运动路径规划建立为无向图的形式，采用A*算法对单个机器人运动路径进行规划，随后进行多机器人协调路径规划，根据实际装配任务要求，采用基于优先级与交通规则约束相结合的协调方法；首先根据装配任务、机器人能力、机器人可靠性确定各机器人优先级，并根据优先级对机器人进行排序，每个机器人在运动时只需考虑优先级排序在自己之前的机器人；再根据当前各机器人位置，生成当前的连通图，确定机器人下一步可以运动的顶点，规划各机器人路径；若规划得到的机器人在下一时刻经过的路径点与优先级较高的机器人所经过的路径点发生冲突时，则机器人之间就会发生碰撞；此时引入制定的交通规则，即等待策略，即优先级低的机器人在原地等待，优先级高的机器人通过冲突点后，优先级低的机器人再继续运动，通过该点；在下一时刻，更新各机器人位置，并以此更新连通图，重复每个时刻更新机器人的位置，每个时刻机器人运动一步，也就是机器人每运动一步都会更新；

步骤三、分析机器人运动并计算能量消耗；

将装配过程中机器人的运动分为攀爬运动与装配运动，攀爬运动使机器人在大型空间结构上进行移动，装配运动使机器人完成装配单元间的装配；

在规划出关节运动后，将关节空间内的轨迹作为标称轨迹，控制机器人关节角，以达到跟踪标称轨迹的效果；控制的目标是设计反馈控制器，使关节运动q(t)∈Rⁿ跟踪上所规划出的机器人关节运动q_d；采用逆动力学控制，由公式(1)为动力学模型，在进行轨迹规划时，机器人末端无力与力矩，即F_e＝0，设计控制器为：

其中，ν为辅助控制输入：

其中，τ_m为关节控制力矩，

为期望关节角加速度，

为期望关节角速度，q_d为期望关节角，K_VT与K_PT为控制增益矩阵；

求得误差动力学为：

选取满足控制目标的增益矩阵K_VT与K_PT即可保证控制***稳定；

对于装配操作，采用基于力控制的控制方法，使接触力保持稳定，最后根据控制力矩，对能量消耗进行计算，控制方法如下：

将机器人建模为刚体，将机器人接触的环境，即桁架结构的零件建模为柔性体，刚度用K_p表示，当机器人末端与环境接触产生微小位移δx时，所产生的弹性恢复力为：

F＝-K_pδx (7)

其中，F与δx均表示在任务空间坐标系上，刚度矩阵K_p为对角矩阵，表示零件三个方向上的刚度，如要若要维持弹性恢复力，机器人关节力矩应为：

τ＝J^TF (8)

其中，J为机器人雅可比矩阵，表示机器人末端微小位移与关节微小位移的关系：

δx＝Jδq (9)

当机器人末端与环境接触时，由接触引起的环境局部变形由矢量

表示：

当末端与环境接触时；

当末端与环境不接触时；

其中，x为机器人末端位置，x_E为环境位置；

设计机器人x-y平面的位置控制律为：

设计机器人z方向上的柔顺控制律为：

其中，τ_x-y为x-y平面上的控制力矩，τ_z为z方向上的控制力矩，J(q)为雅可比矩阵，K_P与K_D为控制增益矩阵；

机器人的能量消耗为关节驱动能量与电子设备能量之和，而关节驱动能量消耗远远大于电子设备，因此假设机器人消耗的能量只有关节驱动部分；

对于机器人能量计算，采取绝对值形式：

其中，n为机器人关节数，T为机器人运动总时间；

步骤四：建立评价函数，采用粒子群算法对装配序列进行寻优；

与传统的装配序列规划相同，首先衡量装配单元之间的关系：

(1)干涉矩阵

干涉矩阵衡量待装配的装配单元与已经装配完成的装配体部分的冲突情况，假设装配体有n个装配单元，定义干涉矩阵以IM表示，则I_ijk表示装配单元P_j沿着方向k与装配单元P_i发生的干涉情况：

三维空间中定义装配方向为三个线性无关的矢量轴，因此，沿三轴干涉矩阵有六种，分别为IM_+x、IM_-x、IM_+y、IM_-y、IM_+z、IM_-z，干涉的特点已知I_ij(+k)＝I_ji(-k)，即装配单元P_j沿着方向k与装配单元P_i发生的干涉情况与装配单元P_i沿着方向-k与装配单元P_j发生的干涉情况相同；因此，定义集成干涉矩阵描述在整个装配过程中，是否存在装配单元之间的干涉情况：

矩阵IM描述两两零件之间装配时是否会发生干涉的情况；

(2)连接矩阵

不同装配单元之间的装配方法不同，连接矩阵是描述不同装配单元间的连接关系，对于一个由n个装配单元组成的装配体，定义连接矩阵C为：

C＝(c_ij)_n×n (15)

其中，c_ij代表装配单元P_i与装配单元P_j的连接情况：

其中，“不存在连接关系”表示两装配单元之间无任何连接结构；“存在接触连接”表示两装配单元之间有连接关系，但是在连接后无法保持稳定状态，也就是在连接时不施加外力，装配单元之间的贴合，没有紧固的铰接都包含在此；“存在稳定连接”表示装配单元连接后可保持稳定；螺纹连接、焊接都属于稳定连接；

(3)支撑矩阵

对于空间桁架结构，工作场景为空间或外星球；不同场景的环境不同，重力特性也不同，支撑矩阵衡量在不同环境与重力特性下，装配过程中各装配单元之间是否具有稳定的支撑关系，定义支撑矩阵S为：

S＝(s_ij)_n×n (17)

其中，s_ij代表装配单元P_j对装配单元P_i的支撑关系：

装配的评价指标有装配几何可行性、装配稳定性、装配重定向性、装配聚合性和装配并行度，针对多机器人装配空间桁架任务，提出以下四个装配指标：

(a)装配几何可行性；

装配几何可行性表示装配单元装配时，与已经装配好的装配体不发生干涉的性质；在装配前已经定义了装配方向并求解出不同装配单元的可行装配方向，装配几何性指根据装配序列装配各装配单元时，每一步的装配单元都有可行的装配序列；

装配几何性通过干涉矩阵求解，定义G_k(P_i)为装配单元P_i沿着k方向进行装配时，和已经装配好的装配体之间的干涉值之和；G_k(P_i)决定了待装配单元P_i的可行装配方向，若装配单元P_i与已装配好的装配体在k方向不发生干涉，则G_k(P_i)＝0；若装配单元P_i与已装配好的装配体在k方向发生干涉，则G_k(P_i)≠0，G_k(P_i)具体表达式为：

对于装配序列{P₁,P₂,…,P_n}，设n_g表示在此序列中无法被装配的装配单元，在计算n_g时，对其初始赋值0，对于每一个装配单元计算G_k(P_i)，当G_k(P_i)＝0时，n_g＝n_g；当G_k(P_i)≠0时，n_g＝n_g+1；如果最终计算得到n_g＝0，则装配序列可行；n_g≠0，装配序列不可行；

(b)装配稳定性

装配稳定性表示在装配单元装配上之后，各装配单元保持装配位置与装配状态的能力；装配稳定性与装配连接关系与重力有关，因此通过连接矩阵C与支撑矩阵S求解，c_ij＝1表示装配单元与装配体为接触连接，c_ij＝2表示装配单元与装配体为稳定连接，s_ij＝1表示装配单元P_j对装配单元P_i具有支撑作用；定义连接关系n_c与n_s，对n_c与n_s初始赋值为0，装配序列{P₁,P₂,…,P_n}中，对于每个装配单元计算c_ij与s_ij，更新n_c＝n_c+c_ij，n_s＝n_s+s_ij；n_c越大，装配稳定性越好；n_s越大，装配支撑性越好；并且要保证每一步装配都是稳定的，需要求每一步n_c>0或n_s>0；

(c)装配路径

装配路径表示在装配过程中，各机器人所运动的路径长度之和，通过步骤二计算得到；定义装配体的顶点V与边E，对于装配序列{P₁,P₂,…,P_n}，设n_d表示在此装配序列中机器人运动的总路程，首先初始无向图G₀＝(V₀,E₀)，根据多机器人协同路径规划算法计算各机器人运动路径

与路径长度n_d0，装配单元计算完后，根据装配序列计算下一装配单元，更新无向图G＝(V,E)，并计算

n_d＝n_d+n_d0；

所有装配单元检测完成后，输出各机器人路径

与机器人运动路径之和n_d；

(d)装配能耗

装配能耗表示在装配过程中，各机器人消耗能量之和，定义为n_e，通过步骤三式(12)计算得到；对于装配序列{P₁,P₂,…,P_n}，在计算完机器人路径

后，根据式(12)能耗计算方法，求解机器人能耗fuel，令n_e＝fule，并输出各机器人运动规划q_d与控制律τ；

对于以上评价指标，进行量化计算，并将不同指标之间使用式(20)进行归一化处理，保证其对整个指标函数影响相同；建立函数模型，不同装配序列求得不同的函数值，准确评价装配序列的优劣；

对于序列{P₁,P₂,…,P_n}，评价函数为：

满足装配序列可行的约束条件为：

其中，n_c代表装配稳定性，n_s代表装配支撑性，n_l代表装配路径，n_e代表装配能耗，ω₁、ω₂、ω₃、ω₄分别代表各指标权重系数，且ω₁+ω₂+ω₃+ω₄＝1，各权重系数可根据不同装配的不同需求制定，而约束条件代表装配序满足的前提条件，即在装配每一步都需要是稳定的且每一步都有可进行装配的方向，即与之间的装配不发生干涉；

在保证公式(21)成立的条件下，找到公式(20)的最小值，即找到最优序列，得到最优的装配序列以及机器人的动作与控制序列。

Images