(10)申请公布号 (43)申请公布日 2014.06.18
C N 103869704
A (21)申请号 201410138354.2
(22)申请日 2014.04.08
G05B 13/04(2006.01)
(71)申请人哈尔滨工业大学
地址150001 黑龙江省哈尔滨市南岗区西大
直街92号
(72)发明人刘宏 王滨 李振宇 夏进军
王志超
(74)专利代理机构哈尔滨市松花江专利商标事
务所 23109
代理人
杨立超
(54)发明名称
控制方法
(57)摘要
基于扩展雅克比矩阵的空间机器人星臂协调
星整体协同控制问题。包括计算空间机器人的运
动学和动力学参数；建立基于扩展雅可比矩阵的
空间机器人数学模型；设计空间机器人星臂协调
臂轨迹进行优化；计算空间机器人单框架控制力
矩陀螺系统的角速度指令。本发明不需要卫星根
据姿态测量进行滞后的反馈控制，通过将臂星的
耦合运动进行整体数学建模，根据输入的机械臂
末端轨迹，直接计算出卫星需补偿机械臂运动的
单框架控制力矩陀螺系统的角速度指令，实现臂
星的整体协调控制；机械臂末端轨迹经过优化使
卫星态控制系统补偿机械臂反作用力矩所消耗的
能量较小。
(51)Int.Cl.
权利要求书4页 说明书7页 附图5页
(19)中华人民共和国国家知识产权局(12)发明专利申请权利要求书4页 说明书7页 附图5页(10)申请公布号CN 103869704 A
1.基于扩展雅克比矩阵的空间机器人星臂协调控制方法，其特征在于它由以下步骤完成：
步骤一、通过ProE 建模计算出包括机械臂和基座卫星的空间机器人的运动学和动力学参数；
步骤二、建立基于扩展雅克比矩阵的空间机器人整体数学模型；
步骤三、利用Moore-Penrose 伪逆求解方法设计空间机器人星臂协调控制器；
步骤四、空间机械臂末端轨迹参数化；
步骤五、基于粒子优化方法，根据空间机器人协调控制器得到的单框架控制力矩陀螺系统角动量的表达式构建优化目标函数，利用构建的优化目标函数对经过参数化的空间机械臂末端轨迹进行优化；
步骤六、将空间机械臂优化轨迹数据输入给空间机器人星臂协调控制器，得到单框架控制力矩陀螺系统的角动量，利用得到的角动量计算出单框架控制力矩陀螺系统的角速度指令。
2.根据权利要求1所述的基于扩展雅克比矩阵的空间机器人星臂协调控制方法，其特征在于在步骤二所述的建立基于扩展雅克比矩阵的空间机器人整体数学模型的公式为：
其中J K 是扩展雅克比矩阵，v e 为空间机器人末端线速度，ωe 为空间机器人末端角速度，ω0为载体卫星转动角速度，为空间机器人关节角速度指令，L cmg 为单框架控制力矩陀螺系统的角动量；扩展雅克比矩阵J K 可用中间变量J g_v 、J g_ω、K l_v 、K l_ω、J bm_ω和K bl_ω表示为分块矩阵，令
其中
k i 为第i 个臂杆转轴的方向向量，p e -p i 表示空间机械臂末端相对于第i 个臂杆末端的位置向量；
p 0e =p e -r 0
E 为单位矩阵，为p 0e 的斜对称矩阵，p 0e 为p e -r 0，表示空间机械臂末端相对于基座质心的位置向量；
J bm 的求解过程如下：
J Ti =[k 1×(r i -p 1)，k 2×(r i -p 2)，...，k i ×(r i -p i )，0， 0
J Ri =[k l ,k 2,...,k ,,O, 0
式中：m i 为第i 个臂杆的质量，k i 为第i 个臂杆转轴的方向向量，r i -p m 表示第i 个臂杆质心相对于第m 个臂杆末端的位置向量，M 为空间机器人总质量，为载体航天器质心相对于空间机器人系统质心的位置向量的斜对称矩阵，I i 为第i 个臂杆的惯性张量矩阵，I 0为载体卫星的惯性张量矩阵；
3.根据权利要求2所述的基于扩展雅克比矩阵的空间机器人星臂协调控制方法，其特征在于在步骤三所述的利用Moore-Penrose 伪逆求解方法设计空间机器人的星臂协调控制器为：
其中是扩展雅克比矩阵的伪逆：
为J K 的转置。
4.根据权利要求3所述的基于扩展雅克比矩阵的空间机器人星臂协调控制方法，其特征在于在步骤四所述的空间机械臂末端轨迹参数化的过程为：
其中P e (t)和Ψe (t)为机械臂末端位置和姿态，t 表示时间；a pi 和a Ψi 均为待定的空间机械臂轨迹方程系数；
机械臂末端位置和姿态P e (t)和Ψe (t)可表示为：
P ex 、P ey 和P ez 为机械臂末端位置在x 、y 和z 轴的分量，α、β和γ为描述机械臂末端姿态的欧拉角，机械臂末端角速度与欧拉角间数学关系为：
ωex 、ωey 和ωez 为机械臂末端角速度在三轴的分量，和为机械臂末端姿态欧拉角的微分；
机械臂末端位置和姿态的轨迹函数形式上完全相同，先确定机械臂末端位置轨迹函数P e (t)，而姿态函数Ψe (t)的确定方法与位置函数完全相同；
机械臂末端轨迹函数需要满足如下边界条件：
P e0为机械臂末端初始位置，P ed 为机械臂末端目标位置，t 0为轨迹的初始时刻，t f 为轨迹的终止时刻；
确定轨迹参数为：
则机械臂末端位置轨迹函数P e (t)退化为待定系数a p6的函数；通过同样的计算过程，Ψe (t)也退化为a Ψ6的函数。
5.根据权利要求4所述的基于扩展雅克比矩阵的空间机器人星臂协调控制方法，其特征在于在步骤五所述的基于粒子优化方法，根据空间机器人协调控制器得到的单框架控制力矩陀螺系统角动量的表达式构建优化目标函数，利用构建的优化目标函数对经过参数化的空间机械臂末端轨迹进行优化，其实现过程如下：
为了利用粒子方法对空间机器人末端轨迹进行优化，设计优化目标函数为：
其中L cmg 为单框架控制力矩陀螺系统的角动量，是基于扩展雅克比矩阵的空间机器人协调控制器的输出||L cmg ||max 和分别为单框架控制力矩陀螺系统在整个协调控制过程中角动量和角动量微分的极值，δ1、δ2为可根据具体任务要求调整的权值；具体优化过程为：
a ）初始化：将优化步数N 初始化为1，建立用于优化的粒子，其中每一个粒子代表机械臂末端轨迹函数待定系数的一个可能解，此时第i 个粒子定义为：
b ）如果优化步数N 大于最大步数N max ，则转步骤，否则计算空间机器人按照每一个粒子所代表的轨迹进行运动时的性能指标函数R ：
c ）确定每一个粒子的最优解和所有粒子目前的最优解，第i 个粒子所记录的最优解为：
所有粒子目前所能确定的最优解为：
d ）根据每一个粒子所记录的最优解和所有粒子目前所能确定的最优解计算此时第i 个粒子的飞行速度：
其中
c 1,c 2,r 1,r 1和w 是用来调整优化速度和精度的加权系数，而后对所有粒子按照此时的飞行速度进行更新，
a p6i (t+1)=a p6i (t)+Δa p6i (18)
令优化步数N 等于N+1，然后转步骤b ；
e ）将所有粒子目前所能确定的最优解带入式(11)求得轨迹参数，将轨迹参数带入式
（7），所得到的轨迹就是经过反作用力优化的空间机械臂轨迹。
6.根据权利要求5所述的基于扩展雅克比矩阵的空间机器人星臂协调控制方法，其特征在于在步骤六所述
的将空间机械臂优化轨迹数据输入给空间机器人星臂协调控制器，得到单框架控制力矩陀螺系统的角动量，利用得到的角动量计算出单框架控制力矩陀螺系统的角速度指令；其过程为：
根据式（6）中空间机器人星臂协调控制器输出的单框架控制力矩陀螺系统的角动量，求解单框架控制力矩陀螺姿态控制系统的框架角速度指令为：
其中J cmg 是单框架控制力矩陀螺系统的雅可比矩阵，
J cmg =h(Acos σ-Bsin σ) (20)
h 为单个单框架控制力矩陀螺的飞轮角动量，A 、B 为确定构型下的结构参数矩阵，sin σ、cos σ为：
σ为单框架控制力矩陀螺粒子系统当前的框架角位置。
豫公网安备41160202000603
站长QQ:729038198
关于我们
投诉建议