[发明专利]一种空间大型结构在轨装配序列规划方法

权利要求书

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_c0或n_s0；

(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)的最小值，即找到最优序列，得到最优的装配序列以及机器人的动作与控制序列。