基于扩展雅克比矩阵的空间机器人星臂协调控制方法，解决在轨工作的空间机械臂与基座卫星整体协同控制问题。包括计算空间机器人的运动学和动力学参数；建立基于扩展雅可比矩阵的空间机器人数学模型；设计空间机器人星臂协调控制器；对机械臂末端轨迹进行参数化；对机械臂轨迹进行优化；计算空间机器人单框架控制力矩陀螺系统的角速度指令。本发明不需要卫星根据姿态测量进行滞后的反馈控制，通过将臂星的耦合运动进行整体数学建模，根据输入的机械臂末端轨迹，直接计算出卫星需补偿机械臂运动的单框架控制力矩陀螺系统的角速度指令，实现臂星的整体协调控制；机械臂末端轨迹经过优化使卫星态控制系统补偿机械臂反作用力矩所消耗的能量较小。
1.基于扩展雅克比矩阵的空间机器人星臂协调控制方法，其特征在于它由以下步骤完成：步骤一、通过ProE建模计算出包括机械臂和基座卫星的空间机器人的运动学和动力学参数；
步骤二、建立基于扩展雅克比矩阵的空间机器人整体数学模型；
其中JK是扩展雅克比矩阵，ve为空间机器人末端线速度，ωe为空间机器人末端角速度，ω0为载体卫星转动角速度，为空间机器人关节角速度指令，Lcmg为单框架控制力矩陀螺系统的角动量；扩展雅克比矩阵JK可用中间变量Jg_v、Jg_ω、Kl_v、Kl_ω、Jbm_ω和Kbl_ω表示为分块矩阵，令其中
ki为第i个臂杆转轴的方向向量，pe-pi表示空间机械臂末端相对于第i个臂杆末端的位置向量；
E为单位矩阵， 为p0e的斜对称矩阵，p0e为pe-r0，表示空间机械臂末端相对于基座质心的位置向量；
JTi＝[k1×(ri-p1),k2×(ri-p2),...,ki×(ri-pi),0,...,0]JRi＝[k1,k2,...,ki,0,...,0]式中：mi为第i个臂杆的质量，ki为第i个臂杆转轴的方向向量，ri-pm表示第i个臂杆质心相对于第m个臂杆末端的位置向量，M为空间机器人总质量， 为载体航天器质心相对于空间机器人系统质心的位置向量的斜对称矩阵，Ii为第i个臂杆的惯性张量矩阵，I0为载体卫星的惯性张量矩阵；
步骤三、利用Moore-Penrose伪逆求解方法设计空间机器人星臂协调控制器；
步骤五、基于粒子群优化方法，根据空间机器人协调控制器得到的单框架控制力矩陀螺系统角动量的表达式构建优化目标函数，利用构建的优化目标函数对经过参数化的空间机械臂末端轨迹进行优化；
步骤六、将空间机械臂优化轨迹数据输入给空间机器人星臂协调控制器，得到单框架控制力矩陀螺系统的角动量，利用得到的角动量计算出单框架控制力矩陀螺系统的角速度指令。
2.根据权利要求1所述的基于扩展雅克比矩阵的空间机器人星臂协调控制方法，其特征在于在步骤三所述的利用Moore-Penrose伪逆求解方法设计空间机器人的星臂协调控制器为：其中 是扩展雅克比矩阵的伪逆：
3.根据权利要求2所述的基于扩展雅克比矩阵的空间机器人星臂协调控制方法，其特征在于在步骤四所述的空间机械臂末端轨迹参数化的过程为：其中Pe(t)和Ψe(t)为机械臂末端位置和姿态，t表示时间；api和aΨi均为待定的空间机械臂轨迹方程系数；
机械臂末端位置和姿态Pe(t)和Ψe(t)可表示为：Pex、Pey和Pez为机械臂末端位置在x、y和z轴的分量，α、β和γ为描述机械臂末端姿态的欧拉角，机械臂末端角速度与欧拉角间数学关系为：ωex、ωey和ωez为机械臂末端角速度在三轴的分量， 和 为机械臂末端姿态欧拉角的微分；
机械臂末端位置和姿态的轨迹函数形式上完全相同，先确定机械臂末端位置轨迹函数Pe(t)，而姿态函数Ψe(t)的确定方法与位置函数完全相同；
Pe0为机械臂末端初始位置，Ped为机械臂末端目标位置，t0为轨迹的初始时刻，tf为轨迹的终止时刻；
则机械臂末端位置轨迹函数Pe(t)退化为待定系数ap6的函数；通过同样的计算过程，Ψe(t)也退化为aΨ6的函数。
4.根据权利要求3所述的基于扩展雅克比矩阵的空间机器人星臂协调控制方法，其特征在于在步骤五所述的基于粒子群优化方法，根据空间机器人协调控制器得到的单框架控制力矩陀螺系统角动量的表达式构建优化目标函数，利用构建的优化目标函数对经过参数化的空间机械臂末端轨迹进行优化，其实现过程如下：为了利用粒子群方法对空间机器人末端轨迹进行优化，设计优化目标函数为：其中Lcmg为单框架控制力矩陀螺系统的角动量，是基于扩展雅克比矩阵的空间机器人协调控制器的输出，||Lcmg||max和 分别为单框架控制力矩陀螺系统在整个协调控制过程中角动量和角动量微分的极值，δ1、δ2为可根据具体任务要求调整的权值；具体优化过程为：a)初始化：将优化步数N初始化为1，建立用于优化的粒子群，其中每一个粒子代表机械臂末端轨迹函数待定系数的一个可能解，此时第i个粒子定义为：b)如果优化步数N大于最大步数Nmax，则转步骤e，否则计算空间机器人按照每一个粒子所代表的轨迹进行运动时的性能指标函数R：c)确定每一个粒子的最优解和所有粒子目前的最优解，第i个粒子所记录的最优解为：所有粒子目前所能确定的最优解为：
d)根据每一个粒子所记录的最优解和所有粒子目前所能确定的最优解计算此时第i个粒子的飞行速度：其中
c1,c2,r1,r1和w是用来调整优化速度和精度的加权系数，而后对所有粒子按照此时的飞行速度进行更新，令优化步数N等于N+1，然后转步骤b；
e)将所有粒子目前所能确定的最优解带入式(11)求得轨迹参数，将轨迹参数带入式(7)，所得到的轨迹就是经过反作用力优化的空间机械臂轨迹。
5.根据权利要求4所述的基于扩展雅克比矩阵的空间机器人星臂协调控制方法，其特征在于在步骤六所述的将空间机械臂优化轨迹数据输入给空间机器人星臂协调控制器，得到单框架控制力矩陀螺系统的角动量，利用得到的角动量计算出单框架控制力矩陀螺系统的角速度指令；其过程为：根据式(6)中空间机器人星臂协调控制器输出的单框架控制力矩陀螺系统的角动量，求解单框架控制力矩陀螺姿态控制系统的框架角速度指令为：其中Jcmg是单框架控制力矩陀螺系统的雅可比矩阵，
Jcmg＝h(A cosσ-B sinσ) (20)h为单个单框架控制力矩陀螺的飞轮角动量，A、B为确定构型下的结构参数矩阵，sinσ、cosσ为：σ为单框架控制力矩陀螺粒子群系统当前的框架角位置。
基于扩展雅克比矩阵的空间机器人星臂协调控制方法
技术领域
[0001] 本发明涉及一种空间机器人星臂协调控制方法。
背景技术
[0002] 空间机器人执行空间任务时面临的一个实际问题是任何空间机械臂的运动都会改变载体卫星的位置和姿态，而载体卫星位置姿态的改变也会反过来影响空间机械臂的定位，同时也会放大空间机械臂末端操作器的定位误差，不利于空间机械臂的高精度控制。为了满足空间机器人工作过程中对地通信和对太阳能帆板朝向等条件的实际需要，往往要求机械臂运动的同时载体卫星的姿态应保持稳定。如今，随着空间在轨维护和深空探索等科学应用的开展，越来越需要空间机器人替代人类实现在轨操作。空间机械臂与基座卫星的整体协调控制是其在轨应用首要考虑的问题。
[0003] 一些学者基于空间机械臂建模中的VM（Virtual Manipulator）方法，规划了一系列连续的螺旋空间机械臂轨迹，希望通过这种方式能够最小化空间机械臂运动时对载体卫星姿态的扰动，但是这样一来就使机械臂的运动方式受到了严重的限制，运动路线也很复杂，不利于直接完成空间任务。通过空间机器人结构上的改变也可以实现载体卫星姿态保持不变，一些研究认为可以引入一条额外的空间机械臂专门用于补偿空间机器人执行任务时产生的载体卫星扰动力矩，但是这使整个空间机器人系统更复杂，从概率上讲也更容易陷入故障状态，并且很不经济。通过动力学计算获得空间机械臂运动对载体卫星姿态的扰动力矩，并通过反作用飞轮对扰动力矩进行补偿是一种很好的方法，但是这种方法往往需要借助复杂的动力学计算，算法结构复杂，实时性不强，而且存在反作用飞轮输出力矩小，响应较慢的问题。因此，提出一种简单的空间机器人协调控制的方法，是非常迫切和必要的。
发明内容
[0004] 本发明提供一种基于扩展雅克比矩阵的空间机器人星臂协调控制方法，以解决空间机械臂在轨工作时与基座卫星整体协同控制问题，进而实现在机械臂完成空间任务的同时保证载体卫星姿态基本不变；同时通过优化机械臂的运动轨迹，使基座卫星在补偿机械臂产生的反作用力矩时消耗的能量最小。
[0005] 基于扩展雅可比矩阵的空间机器人整体协调的优化控制方法由以下步骤完成：
[0006] 步骤一、通过ProE建模计算出包括机械臂和基座卫星的空间机器人的运动学和动力学参数；
[0007] 步骤二、建立基于扩展雅克比矩阵的空间机器人整体数学模型；
[0008] 步骤三、利用Moore-Penrose伪逆求解方法设计空间机器人星臂协调控制器（见式（6））；
[0009] 步骤四、空间机械臂末端轨迹参数化；
[0010] 步骤五、基于粒子群优化方法，根据空间机器人协调控制器得到的单框架控制力矩陀螺系统角动量的表达式构建的优化目标函数（见式（12）），利用构建的优化目标函数对经过参数化的空间机械臂末端轨迹进行优化；
[0011] 步骤六、将空间机械臂优化轨迹数据输入给空间机器人星臂协调控制器，得到单框架控制力矩陀螺系统的角动量，利用得到的角动量计算出单框架控制力矩陀螺系统的角速度指令（利用式（19）计算）。
[0012] 本发明具有以下有益效果：本发明通过建立的空间机器人（空间机器人包括机械臂和基座卫星）的整体数学模型，根据输入的机械臂末端轨迹，可直接计算出卫星需补偿机械臂运动的单框架控制力矩陀螺系统的角速度指令，实现了空间机械臂与卫星基座的整体协调控制；通过对执行同样任务的空间机械臂轨迹进行优化，可使卫星姿态控制系统补偿机械臂反作用力矩消耗的能量较小。
[0013] 本发明方法不需要卫星根据姿态测量进行滞后的反馈控制，而是通过将臂星的耦合运动进行整体数学建模，根据输入的机械臂末端轨迹，可直接计算出卫星需补偿机械臂运动的单框架控制力矩陀螺系统的角速度指令，实现臂星的整体协调控制；同时机械臂末端轨迹经过优化，可使卫星态控制系统补偿机械臂反作用力矩所消耗的能量较小；该方法避免了复杂的动力学计算，方法结构简单，便于实现载体卫星姿态的高精度控制，可用于空间机器人在轨维护、空间碎片清理、深空探测操做等空间应用领域。
附图说明
[0014] 图1为空间机器人星臂协调控制流程图；
[0015] 图2为空间机器人系统各坐标系示意图；
[0016] 图3为利用粒子群方法对机械臂轨迹进行优化的流程图；
[0017] 图4为空间机器人机械臂关节轨迹图，图4中：图4a为空间机器人第一关节角轨迹图，图4b为第二关节轨迹图，图4c为第三关节轨迹图，图4d为第四关节轨迹图，图4e为第五关节轨迹图，图4f为第六关节轨迹图；
[0018] 图5为单框架控制力矩陀螺系统框架角轨迹图；图5中：图5a为第1个单框架控制力矩陀螺的框架角轨迹图，图5b为第2个单框架控制力矩陀螺的框架角轨迹图，图5c为第3 个单框架控制力矩陀螺的框架角轨迹图,图5d为第4个单框架控制力矩陀螺的框架角轨迹图；
[0019] 图6为单框架控制力矩陀螺系统角动量变化图，图6中：图6a为单框架控制力矩陀螺系统角动量在x轴的分量的变化图，图6b为单框架控制力矩陀螺系统角动量在y轴的分量的变化图，图6c为单框架控制力矩陀螺系统角动量在z轴的分量的变化图；
[0020] 图7为空间机器人载体卫星姿态图，图7中：图7a为空间机器人载体卫星姿态用欧拉角表述时的转角α0，图7b为欧拉角表述时的转角β0，图7c为欧拉角表述时的转角γ0。
具体实施方式
[0021] 具体实施方式一：结合图1、图2和图3说明本实施方式，本实施方式由以下步骤完成：
[0022] 步骤一、通过ProE建模计算出包括机械臂和基座卫星的空间机器人的运动学和动力学参数；
[0023] 步骤二、建立基于扩展雅克比矩阵的空间机器人整体数学模型；
[0024] 步骤三、利用Moore-Penrose伪逆求解方法设计空间机器人星臂协调控制器（见式（6））；
[0025] 步骤四、空间机械臂末端轨迹参数化；
[0026] 步骤五、基于粒子群优化方法，根据空间机器人协调控制器得到的单框架控制力矩陀螺系统角动量的表达式构建优化目标函数（见式（12）），利用构建的优化目标函数对经过参数化的空间机械臂末端轨迹进行优化；
[0027] 步骤六、将空间机械臂优化轨迹数据输入给空间机器人星臂协调控制器，得到单框架控制力矩陀螺系统的角动量，利用得到的角动量计算出单框架控制力矩陀螺系统的角速度指令（利用式（19）计算）。
[0028] 具体实施方式二：本实施方式与具体实施方式一的不同点是：本实施方式在步骤二所述的建立基于扩展雅克比矩阵的空间机器人整体数学模型的公式为：
[0029]
[0030] 其中JK就是扩展雅克比矩阵，ve为空间机器人末端线速度，ωe为空间机器人末端角速度，ω0为载体卫星转动角速度，为空间机器人关节角速度指令，Lcmg为单框架控制力矩陀螺系统的角动量；扩展雅克比矩阵JK可用中间变量Jg_v、Jg_ω、Kl_v、Kl_ω、Jbm_ω和Kbl_ω表示为分块矩阵，令
[0031]
[0032] 其中
[0033]
[0034]
[0035] ki为第i个臂杆转轴的方向向量，pe-pi表示空间机械臂末端相对于第i个臂杆末端的位置向量；
[0036]
[0037] p0e=pe-r0
[0038] E为单位矩阵， 为p0e的斜对称矩阵，p0e为pe-r0，表示空间机械臂末端相对于基座质心的位置向量；
[0039]
[0040]
[0041] JTi=[k1×(ri-p1)，k2×(ri-p2)，...，ki×(ri-pi)，0，...，0]
[0042]
[0043]
[0044]
[0045]
[0046] JRi=[kl,k2,...,k,,O,...,0]
[0047] mi为第i个臂杆的质量，ki为第i个臂杆转轴的方向向量，ri-pm表示第i个臂杆质心相对于第m个臂杆末端的位置向量，M为空间机器人总质量， 为载体卫星质心相对于空间机器人系统质心的位置向量的斜对称矩阵，Ii为第i个臂杆的惯性张量矩阵，I0为载体卫星的惯性张量矩阵；
[0048]
[0049]
[0050] 具体实施方式三：本实施方式与具体实施方式一或二的不同点是：本实施方式在步骤三所述的利用Moore-Penrose伪逆求解方法设计空间机器人的星臂协调控制器为：
[0051]
[0052] 其中 是扩展雅克比矩阵的伪逆
[0053]
[0054] 为JK的转置。
[0055] 具体实施方式四：本实施方式与具体实施方式三的不同点是：本实施方式在步骤四所述的空间机械臂末端轨迹参数化方法为：
[0056]
[0057] 其中Pe(t)和Ψe(t)为机械臂末端位置和姿态，api和aΨi均为待定的空间机械臂轨迹方程系数。机械臂末端位置和姿态Pe(t)和Ψe(t)又可表示为
[0058]
[0059] Pex、Pey和Pez为机械臂末端位置在x、y和z轴的分量，α、β和γ为描述机械臂末端姿态的欧 拉角，机械臂末端角速度与欧拉角间数学关系为：
[0060]
[0061] ωex、ωey和ωez为机械臂末端角速度在三轴的分量， 和 为机械臂末端姿态欧拉角的微分。由于机械臂末端位置和姿态的轨迹函数形式上完全相同，先确定机械臂末端位置轨迹函数Pe(t)，而姿态函数Ψe(t)的确定方法与位置函数完全相同。由于机械臂末端轨迹函数需要满足边界条件
[0062]
[0063] Pe0为机械臂末端初始位置，Ped为机械臂末端目标位置，t0为轨迹的初始时刻，tf为轨迹的终止时刻。
[0064] 可以确定轨迹参数为
[0065]
[0066] 则机械臂末端位置轨迹函数Pe(t)退化为待定系数ap6的函数。通过同样的计算过程，Ψe(t)也退化为aΨ6的函数。
[0067] 具体实施方式五：结合图2说明本实施方式，本实施方式与具体实施方式四的不同点是：本实施方式在步骤五所述的基于粒子群优化方法，根据空间机器人协调控制器得到的单框架控制力矩陀螺系统角动量的表达式构建优化目标函数，利用构建的优化目标函数对经过参数化的空间机械臂末端轨迹进行优化，实现过程如下：
[0068] 为了利用粒子群方法对空间机器人末端轨迹进行优化，设计优化目标函数为[0069]
[0070] 其中Lcmg为单框架控制力矩陀螺系统的角动量，是基于扩展雅克比矩阵的空间机器人协调控制器的输出，||Lcmg||max和 分别为单框架控制力矩陀螺系统在整个协调控制过程中角动量和角动量微分的极值，δ1、δ2为可根据具体任务要求调整的权值；具体优化过程为：
[0071] a）初始化。将优化步数N初始化为1，建立用于优化的粒子群，其中每一个粒子代表机械臂末端轨迹函数待定系数的一个可能解，此时第i个粒子定义为
[0072]
[0073] b）如果优化步数N大于最大步数Nmax，则转步骤，否则计算空间机器人按照每一个粒子所代表的轨迹进行运动时的性能指标函数R。
[0074] c）记录每一个粒子所记录的最优解和所有粒子目前所能确定的最优解。第i个粒子所记录的最优解为
[0075]
[0076] 所有粒子目前所能确定的最优解为
[0077]
[0078] d）根据每一个粒子所记录的最优解和所有粒子目前所能确定的最优解计算此时第i个粒子的飞行速度
[0079]
[0080] 其中
[0081]
[0082] c1,c2,r1,r1和w是用来调整优化速度和精度的加权系数。而后对所有粒子按照此时的飞行速度进行更新。
[0083] ap6i(t+1)=ap6i(t)+Δap6i (18)
[0084] 令优化步数N等于N+1，然后转步骤b。
[0085] e）将所有粒子目前所能确定的最优解带入式(11)求得轨迹参数，将轨迹参数带入式（7），所得到的轨迹就是经过反作用力优化的空间机械臂轨迹。
[0086] 具体实施方式六：本实施方式与具体实施方式三的不同点是：本实施方式在步骤六所述的将空间机械臂优化轨迹数据输入给空间机器人星臂协调控制器，得到单框架控制力矩陀螺系统的角动量，利用得到的角动量计算出单框架控制力矩陀螺系统的角速度指令，其过程为：
[0087] 根据式（6）中空间机器人星臂协调控制器输出的单框架控制力矩陀螺系统的角动量，求解单框架控制力矩陀螺姿态控制系统的框架角速度指令为：
[0088]
[0089] 其中Jcmg是单框架控制力矩陀螺系统的雅可比矩阵，
[0090] Jcmg=h(Acosσ-Bsinσ) (20)
[0091] h为单个单框架控制力矩陀螺的飞轮角动量，A、B为确定构型下的结构参数矩阵，sinσ、cosσ为
[0092]
[0093] σ为单框架控制力矩陀螺系统当前的框架角位置。
[0094] 本发明说明书中未做详细描述的内容属于本领域专业技术人员公知的现有技术。
[0095] 实施例
[0096] 结合图1、图2和图3说明本实施例，空间机器人由六自由度机械臂和卫星组成。
[0097] 空间机器人的协调控制步骤为：
[0098] 步骤一、通过ProE建模计算出空间机器人的运动学和动力学参数，以六自由度机械臂为例。空间机器人运动学和动力学参数如表1所示：
[0099] 表1.空间机器人参数
[0100]
[0101] 其中杆件0表示载体卫星，1-6表示机械臂各关节。
[0102] 空间机械臂末端运动轨迹的位置起点Pe0=[-1.26m00]T，位置终点为Ped=[1.12m0.53m-0.13m]T；械臂末端运动轨迹姿态起点为Ψe0=[-0.52rad-1.57rad-2.62rad]T，姿态终点为Ψed=[-1.32rad1.23rad-4.95rad]T，单框架控制力矩陀螺系统的初始框架角位置为 σ0=[0rad0rad0rad0rad]T，基座初始姿态为[0rad0rad0rad]T。
[0103] 步骤二、根据公式(1)求解扩展雅克比矩阵，建立空间机器人数学模型。
[0104] 步骤三、利用公式（6）设计空间机器人星臂协调控制器。
[0105] 步骤四、根据公式(7)空间机械臂末端轨迹参数化。
[0106] 步骤五、基于公式(13）～(17)优化方法，求得所有粒子目前所能确定的最优解，并带入式(11)求得轨迹参数，将轨迹参数带入式（7），得到优化的空间机械臂轨迹。
[0107] 步骤六、将得到优化的空间机械臂轨迹数据带入公式（6），得到单框架控制力矩陀螺系统的角动量，将得到的角动量带入公式(19)，输出单框架控制力矩陀螺系统的角速度指令。
[0108] 所设计空间机器人协调控制器和机械臂轨迹优化方法的实际效果如图4、图5、图6和图7所示。在空间机器人按照预定轨迹大范围快速运动的过程中，由于机械臂末端轨迹经过优化，从图6可以看出，基座卫星在补偿机械臂产生的反作用力矩时，单框架控制力矩陀螺系统输出的角动量最大只有13Nms，使卫星姿态控制系统补偿机械臂反作用力矩时消耗的能量较小；从图7可以看出，通过空间机器人星臂协调控制器协调控制，载体卫星的姿态能够保持基本不变，载体卫星姿态变化小于2°，实现了臂星的整体协调控制；该方法实现简单，可以满足工程系统需要。
